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 err; 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 err; 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 err; 73 } 74 75 static int ptp_dpaa2_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts) 76 { 77 struct ptp_dpaa2_priv *ptp_dpaa2 = 78 container_of(ptp, struct ptp_dpaa2_priv, caps); 79 struct fsl_mc_device *mc_dev = ptp_dpaa2->ptp_mc_dev; 80 struct device *dev = &mc_dev->dev; 81 u64 ns; 82 u32 remainder; 83 int err = 0; 84 85 err = dprtc_get_time(mc_dev->mc_io, 0, mc_dev->mc_handle, &ns); 86 if (err) { 87 dev_err(dev, "dprtc_get_time err %d\n", err); 88 return err; 89 } 90 91 ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder); 92 ts->tv_nsec = remainder; 93 return err; 94 } 95 96 static int ptp_dpaa2_settime(struct ptp_clock_info *ptp, 97 const struct timespec64 *ts) 98 { 99 struct ptp_dpaa2_priv *ptp_dpaa2 = 100 container_of(ptp, struct ptp_dpaa2_priv, caps); 101 struct fsl_mc_device *mc_dev = ptp_dpaa2->ptp_mc_dev; 102 struct device *dev = &mc_dev->dev; 103 u64 ns; 104 int err = 0; 105 106 ns = ts->tv_sec * 1000000000ULL; 107 ns += ts->tv_nsec; 108 109 err = dprtc_set_time(mc_dev->mc_io, 0, mc_dev->mc_handle, ns); 110 if (err) 111 dev_err(dev, "dprtc_set_time err %d\n", err); 112 return err; 113 } 114 115 static const struct ptp_clock_info ptp_dpaa2_caps = { 116 .owner = THIS_MODULE, 117 .name = "DPAA2 PTP Clock", 118 .max_adj = 512000, 119 .n_alarm = 2, 120 .n_ext_ts = 2, 121 .n_per_out = 3, 122 .n_pins = 0, 123 .pps = 1, 124 .adjfreq = ptp_dpaa2_adjfreq, 125 .adjtime = ptp_dpaa2_adjtime, 126 .gettime64 = ptp_dpaa2_gettime, 127 .settime64 = ptp_dpaa2_settime, 128 }; 129 130 static int dpaa2_ptp_probe(struct fsl_mc_device *mc_dev) 131 { 132 struct device *dev = &mc_dev->dev; 133 struct ptp_dpaa2_priv *ptp_dpaa2; 134 u32 tmr_add = 0; 135 int err; 136 137 ptp_dpaa2 = devm_kzalloc(dev, sizeof(*ptp_dpaa2), GFP_KERNEL); 138 if (!ptp_dpaa2) 139 return -ENOMEM; 140 141 err = fsl_mc_portal_allocate(mc_dev, 0, &mc_dev->mc_io); 142 if (err) { 143 if (err == -ENXIO) 144 err = -EPROBE_DEFER; 145 else 146 dev_err(dev, "fsl_mc_portal_allocate err %d\n", err); 147 goto err_exit; 148 } 149 150 err = dprtc_open(mc_dev->mc_io, 0, mc_dev->obj_desc.id, 151 &mc_dev->mc_handle); 152 if (err) { 153 dev_err(dev, "dprtc_open err %d\n", err); 154 goto err_free_mcp; 155 } 156 157 ptp_dpaa2->ptp_mc_dev = mc_dev; 158 159 err = dprtc_get_freq_compensation(mc_dev->mc_io, 0, 160 mc_dev->mc_handle, &tmr_add); 161 if (err) { 162 dev_err(dev, "dprtc_get_freq_compensation err %d\n", err); 163 goto err_close; 164 } 165 166 ptp_dpaa2->freq_comp = tmr_add; 167 ptp_dpaa2->caps = ptp_dpaa2_caps; 168 169 ptp_dpaa2->clock = ptp_clock_register(&ptp_dpaa2->caps, dev); 170 if (IS_ERR(ptp_dpaa2->clock)) { 171 err = PTR_ERR(ptp_dpaa2->clock); 172 goto err_close; 173 } 174 175 dpaa2_phc_index = ptp_clock_index(ptp_dpaa2->clock); 176 177 dev_set_drvdata(dev, ptp_dpaa2); 178 179 return 0; 180 181 err_close: 182 dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle); 183 err_free_mcp: 184 fsl_mc_portal_free(mc_dev->mc_io); 185 err_exit: 186 return err; 187 } 188 189 static int dpaa2_ptp_remove(struct fsl_mc_device *mc_dev) 190 { 191 struct ptp_dpaa2_priv *ptp_dpaa2; 192 struct device *dev = &mc_dev->dev; 193 194 ptp_dpaa2 = dev_get_drvdata(dev); 195 ptp_clock_unregister(ptp_dpaa2->clock); 196 197 dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle); 198 fsl_mc_portal_free(mc_dev->mc_io); 199 200 return 0; 201 } 202 203 static const struct fsl_mc_device_id dpaa2_ptp_match_id_table[] = { 204 { 205 .vendor = FSL_MC_VENDOR_FREESCALE, 206 .obj_type = "dprtc", 207 }, 208 {} 209 }; 210 MODULE_DEVICE_TABLE(fslmc, dpaa2_ptp_match_id_table); 211 212 static struct fsl_mc_driver dpaa2_ptp_drv = { 213 .driver = { 214 .name = KBUILD_MODNAME, 215 .owner = THIS_MODULE, 216 }, 217 .probe = dpaa2_ptp_probe, 218 .remove = dpaa2_ptp_remove, 219 .match_id_table = dpaa2_ptp_match_id_table, 220 }; 221 222 module_fsl_mc_driver(dpaa2_ptp_drv); 223 224 MODULE_LICENSE("GPL v2"); 225 MODULE_DESCRIPTION("DPAA2 PTP Clock Driver"); 226