drivers/net/ethernet/freescale/dpaa2/dpaa2-ptp.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright 2013-2016 Freescale Semiconductor Inc.
   4  * Copyright 2016-2018 NXP
   5  */
   6
   7 #include <linux/module.h>
   8 #include <linux/slab.h>
   9 #include <linux/ptp_clock_kernel.h>
  10 #include <linux/fsl/mc.h>
  11
  12 #include "dpaa2-ptp.h"
  13
  14 struct ptp_dpaa2_priv {
  15         struct fsl_mc_device *ptp_mc_dev;
  16         struct ptp_clock *clock;
  17         struct ptp_clock_info caps;
  18         u32 freq_comp;
  19 };
  20
  21 /* PTP clock operations */
  22 static int ptp_dpaa2_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
  23 {
  24         struct ptp_dpaa2_priv *ptp_dpaa2 =
  25                 container_of(ptp, struct ptp_dpaa2_priv, caps);
  26         struct fsl_mc_device *mc_dev = ptp_dpaa2->ptp_mc_dev;
  27         struct device *dev = &mc_dev->dev;
  28         u64 adj;
  29         u32 diff, tmr_add;
  30         int neg_adj = 0;
  31         int err = 0;
  32
  33         if (ppb < 0) {
  34                 neg_adj = 1;
  35                 ppb = -ppb;
  36         }
  37
  38         tmr_add = ptp_dpaa2->freq_comp;
  39         adj = tmr_add;
  40         adj *= ppb;
  41         diff = div_u64(adj, 1000000000ULL);
  42
  43         tmr_add = neg_adj ? tmr_add - diff : tmr_add + diff;
  44
  45         err = dprtc_set_freq_compensation(mc_dev->mc_io, 0,
  46                                           mc_dev->mc_handle, tmr_add);
  47         if (err)
  48                 dev_err(dev, "dprtc_set_freq_compensation err %d\n", err);
  49         return 0;
  50 }
  51
  52 static int ptp_dpaa2_adjtime(struct ptp_clock_info *ptp, s64 delta)
  53 {
  54         struct ptp_dpaa2_priv *ptp_dpaa2 =
  55                 container_of(ptp, struct ptp_dpaa2_priv, caps);
  56         struct fsl_mc_device *mc_dev = ptp_dpaa2->ptp_mc_dev;
  57         struct device *dev = &mc_dev->dev;
  58         s64 now;
  59         int err = 0;
  60
  61         err = dprtc_get_time(mc_dev->mc_io, 0, mc_dev->mc_handle, &now);
  62         if (err) {
  63                 dev_err(dev, "dprtc_get_time err %d\n", err);
  64                 return 0;
  65         }
  66
  67         now += delta;
  68
  69         err = dprtc_set_time(mc_dev->mc_io, 0, mc_dev->mc_handle, now);
  70         if (err) {
  71                 dev_err(dev, "dprtc_set_time err %d\n", err);
  72                 return 0;
  73         }
  74         return 0;
  75 }
  76
  77 static int ptp_dpaa2_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
  78 {
  79         struct ptp_dpaa2_priv *ptp_dpaa2 =
  80                 container_of(ptp, struct ptp_dpaa2_priv, caps);
  81         struct fsl_mc_device *mc_dev = ptp_dpaa2->ptp_mc_dev;
  82         struct device *dev = &mc_dev->dev;
  83         u64 ns;
  84         u32 remainder;
  85         int err = 0;
  86
  87         err = dprtc_get_time(mc_dev->mc_io, 0, mc_dev->mc_handle, &ns);
  88         if (err) {
  89                 dev_err(dev, "dprtc_get_time err %d\n", err);
  90                 return 0;
  91         }
  92
  93         ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
  94         ts->tv_nsec = remainder;
  95         return 0;
  96 }
  97
  98 static int ptp_dpaa2_settime(struct ptp_clock_info *ptp,
  99                              const struct timespec64 *ts)
 100 {
 101         struct ptp_dpaa2_priv *ptp_dpaa2 =
 102                 container_of(ptp, struct ptp_dpaa2_priv, caps);
 103         struct fsl_mc_device *mc_dev = ptp_dpaa2->ptp_mc_dev;
 104         struct device *dev = &mc_dev->dev;
 105         u64 ns;
 106         int err = 0;
 107
 108         ns = ts->tv_sec * 1000000000ULL;
 109         ns += ts->tv_nsec;
 110
 111         err = dprtc_set_time(mc_dev->mc_io, 0, mc_dev->mc_handle, ns);
 112         if (err)
 113                 dev_err(dev, "dprtc_set_time err %d\n", err);
 114         return 0;
 115 }
 116
 117 static struct ptp_clock_info ptp_dpaa2_caps = {
 118         .owner          = THIS_MODULE,
 119         .name           = "DPAA2 PTP Clock",
 120         .max_adj        = 512000,
 121         .n_alarm        = 2,
 122         .n_ext_ts       = 2,
 123         .n_per_out      = 3,
 124         .n_pins         = 0,
 125         .pps            = 1,
 126         .adjfreq        = ptp_dpaa2_adjfreq,
 127         .adjtime        = ptp_dpaa2_adjtime,
 128         .gettime64      = ptp_dpaa2_gettime,
 129         .settime64      = ptp_dpaa2_settime,
 130 };
 131
 132 static int dpaa2_ptp_probe(struct fsl_mc_device *mc_dev)
 133 {
 134         struct device *dev = &mc_dev->dev;
 135         struct ptp_dpaa2_priv *ptp_dpaa2;
 136         u32 tmr_add = 0;
 137         int err;
 138
 139         ptp_dpaa2 = kzalloc(sizeof(*ptp_dpaa2), GFP_KERNEL);
 140         if (!ptp_dpaa2)
 141                 return -ENOMEM;
 142
 143         err = fsl_mc_portal_allocate(mc_dev, 0, &mc_dev->mc_io);
 144         if (err) {
 145                 dev_err(dev, "fsl_mc_portal_allocate err %d\n", err);
 146                 goto err_exit;
 147         }
 148
 149         err = dprtc_open(mc_dev->mc_io, 0, mc_dev->obj_desc.id,
 150                          &mc_dev->mc_handle);
 151         if (err) {
 152                 dev_err(dev, "dprtc_open err %d\n", err);
 153                 goto err_free_mcp;
 154         }
 155
 156         ptp_dpaa2->ptp_mc_dev = mc_dev;
 157
 158         err = dprtc_get_freq_compensation(mc_dev->mc_io, 0,
 159                                           mc_dev->mc_handle, &tmr_add);
 160         if (err) {
 161                 dev_err(dev, "dprtc_get_freq_compensation err %d\n", err);
 162                 goto err_close;
 163         }
 164
 165         ptp_dpaa2->freq_comp = tmr_add;
 166         ptp_dpaa2->caps = ptp_dpaa2_caps;
 167
 168         ptp_dpaa2->clock = ptp_clock_register(&ptp_dpaa2->caps, dev);
 169         if (IS_ERR(ptp_dpaa2->clock)) {
 170                 err = PTR_ERR(ptp_dpaa2->clock);
 171                 goto err_close;
 172         }
 173
 174         dpaa2_phc_index = ptp_clock_index(ptp_dpaa2->clock);
 175
 176         dev_set_drvdata(dev, ptp_dpaa2);
 177
 178         return 0;
 179
 180 err_close:
 181         dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle);
 182 err_free_mcp:
 183         fsl_mc_portal_free(mc_dev->mc_io);
 184 err_exit:
 185         kfree(ptp_dpaa2);
 186         dev_set_drvdata(dev, NULL);
 187         return err;
 188 }
 189
 190 static int dpaa2_ptp_remove(struct fsl_mc_device *mc_dev)
 191 {
 192         struct ptp_dpaa2_priv *ptp_dpaa2;
 193         struct device *dev = &mc_dev->dev;
 194
 195         ptp_dpaa2 = dev_get_drvdata(dev);
 196         ptp_clock_unregister(ptp_dpaa2->clock);
 197
 198         dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle);
 199         fsl_mc_portal_free(mc_dev->mc_io);
 200
 201         kfree(ptp_dpaa2);
 202         dev_set_drvdata(dev, NULL);
 203
 204         return 0;
 205 }
 206
 207 static const struct fsl_mc_device_id dpaa2_ptp_match_id_table[] = {
 208         {
 209                 .vendor = FSL_MC_VENDOR_FREESCALE,
 210                 .obj_type = "dprtc",
 211         },
 212         {}
 213 };
 214 MODULE_DEVICE_TABLE(fslmc, dpaa2_ptp_match_id_table);
 215
 216 static struct fsl_mc_driver dpaa2_ptp_drv = {
 217         .driver = {
 218                 .name = KBUILD_MODNAME,
 219                 .owner = THIS_MODULE,
 220         },
 221         .probe = dpaa2_ptp_probe,
 222         .remove = dpaa2_ptp_remove,
 223         .match_id_table = dpaa2_ptp_match_id_table,
 224 };
 225
 226 module_fsl_mc_driver(dpaa2_ptp_drv);
 227
 228 MODULE_LICENSE("GPL v2");
 229 MODULE_DESCRIPTION("DPAA2 PTP Clock Driver");