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