drivers/staging/fsl-dpaa2/rtc/rtc.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 "rtc.h"
  13
  14 struct ptp_dpaa2_priv {
  15         struct fsl_mc_device *rtc_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->rtc_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->rtc_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->rtc_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->rtc_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 rtc_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                 if (err == -ENXIO)
 146                         err = -EPROBE_DEFER;
 147                 else
 148                         dev_err(dev, "fsl_mc_portal_allocate err %d\n", err);
 149                 goto err_exit;
 150         }
 151
 152         err = dprtc_open(mc_dev->mc_io, 0, mc_dev->obj_desc.id,
 153                          &mc_dev->mc_handle);
 154         if (err) {
 155                 dev_err(dev, "dprtc_open err %d\n", err);
 156                 goto err_free_mcp;
 157         }
 158
 159         ptp_dpaa2->rtc_mc_dev = mc_dev;
 160
 161         err = dprtc_get_freq_compensation(mc_dev->mc_io, 0,
 162                                           mc_dev->mc_handle, &tmr_add);
 163         if (err) {
 164                 dev_err(dev, "dprtc_get_freq_compensation err %d\n", err);
 165                 goto err_close;
 166         }
 167
 168         ptp_dpaa2->freq_comp = tmr_add;
 169         ptp_dpaa2->caps = ptp_dpaa2_caps;
 170
 171         ptp_dpaa2->clock = ptp_clock_register(&ptp_dpaa2->caps, dev);
 172         if (IS_ERR(ptp_dpaa2->clock)) {
 173                 err = PTR_ERR(ptp_dpaa2->clock);
 174                 goto err_close;
 175         }
 176
 177         dpaa2_phc_index = ptp_clock_index(ptp_dpaa2->clock);
 178
 179         dev_set_drvdata(dev, ptp_dpaa2);
 180
 181         return 0;
 182
 183 err_close:
 184         dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle);
 185 err_free_mcp:
 186         fsl_mc_portal_free(mc_dev->mc_io);
 187 err_exit:
 188         kfree(ptp_dpaa2);
 189         dev_set_drvdata(dev, NULL);
 190         return err;
 191 }
 192
 193 static int rtc_remove(struct fsl_mc_device *mc_dev)
 194 {
 195         struct ptp_dpaa2_priv *ptp_dpaa2;
 196         struct device *dev = &mc_dev->dev;
 197
 198         ptp_dpaa2 = dev_get_drvdata(dev);
 199         ptp_clock_unregister(ptp_dpaa2->clock);
 200
 201         dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle);
 202         fsl_mc_portal_free(mc_dev->mc_io);
 203
 204         kfree(ptp_dpaa2);
 205         dev_set_drvdata(dev, NULL);
 206
 207         return 0;
 208 }
 209
 210 static const struct fsl_mc_device_id rtc_match_id_table[] = {
 211         {
 212                 .vendor = FSL_MC_VENDOR_FREESCALE,
 213                 .obj_type = "dprtc",
 214         },
 215         {}
 216 };
 217 MODULE_DEVICE_TABLE(fslmc, rtc_match_id_table);
 218
 219 static struct fsl_mc_driver rtc_drv = {
 220         .driver = {
 221                 .name = KBUILD_MODNAME,
 222                 .owner = THIS_MODULE,
 223         },
 224         .probe = rtc_probe,
 225         .remove = rtc_remove,
 226         .match_id_table = rtc_match_id_table,
 227 };
 228
 229 module_fsl_mc_driver(rtc_drv);
 230
 231 MODULE_LICENSE("GPL v2");
 232 MODULE_DESCRIPTION("DPAA2 PTP Clock Driver");