337 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2// Copyright 2017 Broadcom3 4#include <linux/err.h>5#include <linux/io.h>6#include <linux/module.h>7#include <linux/mod_devicetable.h>8#include <linux/platform_device.h>9#include <linux/ptp_clock_kernel.h>10#include <linux/types.h>11 12#define DTE_NCO_LOW_TIME_REG 0x0013#define DTE_NCO_TIME_REG 0x0414#define DTE_NCO_OVERFLOW_REG 0x0815#define DTE_NCO_INC_REG 0x0c16 17#define DTE_NCO_SUM2_MASK 0xffffffff18#define DTE_NCO_SUM2_SHIFT 4ULL19 20#define DTE_NCO_SUM3_MASK 0xff21#define DTE_NCO_SUM3_SHIFT 36ULL22#define DTE_NCO_SUM3_WR_SHIFT 823 24#define DTE_NCO_TS_WRAP_MASK 0xfff25#define DTE_NCO_TS_WRAP_LSHIFT 3226 27#define DTE_NCO_INC_DEFAULT 0x8000000028#define DTE_NUM_REGS_TO_RESTORE 429 30/* Full wrap around is 44bits in ns (~4.887 hrs) */31#define DTE_WRAP_AROUND_NSEC_SHIFT 4432 33/* 44 bits NCO */34#define DTE_NCO_MAX_NS 0xFFFFFFFFFFFLL35 36/* 125MHz with 3.29 reg cfg */37#define DTE_PPB_ADJ(ppb) (u32)(div64_u64((((u64)abs(ppb) * BIT(28)) +\38 62500000ULL), 125000000ULL))39 40/* ptp dte priv structure */41struct ptp_dte {42 void __iomem *regs;43 struct ptp_clock *ptp_clk;44 struct ptp_clock_info caps;45 struct device *dev;46 u32 ts_ovf_last;47 u32 ts_wrap_cnt;48 spinlock_t lock;49 u32 reg_val[DTE_NUM_REGS_TO_RESTORE];50};51 52static void dte_write_nco(void __iomem *regs, s64 ns)53{54 u32 sum2, sum3;55 56 sum2 = (u32)((ns >> DTE_NCO_SUM2_SHIFT) & DTE_NCO_SUM2_MASK);57 /* compensate for ignoring sum1 */58 if (sum2 != DTE_NCO_SUM2_MASK)59 sum2++;60 61 /* to write sum3, bits [15:8] needs to be written */62 sum3 = (u32)(((ns >> DTE_NCO_SUM3_SHIFT) & DTE_NCO_SUM3_MASK) <<63 DTE_NCO_SUM3_WR_SHIFT);64 65 writel(0, (regs + DTE_NCO_LOW_TIME_REG));66 writel(sum2, (regs + DTE_NCO_TIME_REG));67 writel(sum3, (regs + DTE_NCO_OVERFLOW_REG));68}69 70static s64 dte_read_nco(void __iomem *regs)71{72 u32 sum2, sum3;73 s64 ns;74 75 /*76 * ignoring sum1 (4 bits) gives a 16ns resolution, which77 * works due to the async register read.78 */79 sum3 = readl(regs + DTE_NCO_OVERFLOW_REG) & DTE_NCO_SUM3_MASK;80 sum2 = readl(regs + DTE_NCO_TIME_REG);81 ns = ((s64)sum3 << DTE_NCO_SUM3_SHIFT) |82 ((s64)sum2 << DTE_NCO_SUM2_SHIFT);83 84 return ns;85}86 87static void dte_write_nco_delta(struct ptp_dte *ptp_dte, s64 delta)88{89 s64 ns;90 91 ns = dte_read_nco(ptp_dte->regs);92 93 /* handle wraparound conditions */94 if ((delta < 0) && (abs(delta) > ns)) {95 if (ptp_dte->ts_wrap_cnt) {96 ns += DTE_NCO_MAX_NS + delta;97 ptp_dte->ts_wrap_cnt--;98 } else {99 ns = 0;100 }101 } else {102 ns += delta;103 if (ns > DTE_NCO_MAX_NS) {104 ptp_dte->ts_wrap_cnt++;105 ns -= DTE_NCO_MAX_NS;106 }107 }108 109 dte_write_nco(ptp_dte->regs, ns);110 111 ptp_dte->ts_ovf_last = (ns >> DTE_NCO_TS_WRAP_LSHIFT) &112 DTE_NCO_TS_WRAP_MASK;113}114 115static s64 dte_read_nco_with_ovf(struct ptp_dte *ptp_dte)116{117 u32 ts_ovf;118 s64 ns = 0;119 120 ns = dte_read_nco(ptp_dte->regs);121 122 /*Timestamp overflow: 8 LSB bits of sum3, 4 MSB bits of sum2 */123 ts_ovf = (ns >> DTE_NCO_TS_WRAP_LSHIFT) & DTE_NCO_TS_WRAP_MASK;124 125 /* Check for wrap around */126 if (ts_ovf < ptp_dte->ts_ovf_last)127 ptp_dte->ts_wrap_cnt++;128 129 ptp_dte->ts_ovf_last = ts_ovf;130 131 /* adjust for wraparounds */132 ns += (s64)(BIT_ULL(DTE_WRAP_AROUND_NSEC_SHIFT) * ptp_dte->ts_wrap_cnt);133 134 return ns;135}136 137static int ptp_dte_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)138{139 s32 ppb = scaled_ppm_to_ppb(scaled_ppm);140 u32 nco_incr;141 unsigned long flags;142 struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);143 144 if (abs(ppb) > ptp_dte->caps.max_adj) {145 dev_err(ptp_dte->dev, "ppb adj too big\n");146 return -EINVAL;147 }148 149 if (ppb < 0)150 nco_incr = DTE_NCO_INC_DEFAULT - DTE_PPB_ADJ(ppb);151 else152 nco_incr = DTE_NCO_INC_DEFAULT + DTE_PPB_ADJ(ppb);153 154 spin_lock_irqsave(&ptp_dte->lock, flags);155 writel(nco_incr, ptp_dte->regs + DTE_NCO_INC_REG);156 spin_unlock_irqrestore(&ptp_dte->lock, flags);157 158 return 0;159}160 161static int ptp_dte_adjtime(struct ptp_clock_info *ptp, s64 delta)162{163 unsigned long flags;164 struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);165 166 spin_lock_irqsave(&ptp_dte->lock, flags);167 dte_write_nco_delta(ptp_dte, delta);168 spin_unlock_irqrestore(&ptp_dte->lock, flags);169 170 return 0;171}172 173static int ptp_dte_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)174{175 unsigned long flags;176 struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);177 178 spin_lock_irqsave(&ptp_dte->lock, flags);179 *ts = ns_to_timespec64(dte_read_nco_with_ovf(ptp_dte));180 spin_unlock_irqrestore(&ptp_dte->lock, flags);181 182 return 0;183}184 185static int ptp_dte_settime(struct ptp_clock_info *ptp,186 const struct timespec64 *ts)187{188 unsigned long flags;189 struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);190 191 spin_lock_irqsave(&ptp_dte->lock, flags);192 193 /* Disable nco increment */194 writel(0, ptp_dte->regs + DTE_NCO_INC_REG);195 196 dte_write_nco(ptp_dte->regs, timespec64_to_ns(ts));197 198 /* reset overflow and wrap counter */199 ptp_dte->ts_ovf_last = 0;200 ptp_dte->ts_wrap_cnt = 0;201 202 /* Enable nco increment */203 writel(DTE_NCO_INC_DEFAULT, ptp_dte->regs + DTE_NCO_INC_REG);204 205 spin_unlock_irqrestore(&ptp_dte->lock, flags);206 207 return 0;208}209 210static int ptp_dte_enable(struct ptp_clock_info *ptp,211 struct ptp_clock_request *rq, int on)212{213 return -EOPNOTSUPP;214}215 216static const struct ptp_clock_info ptp_dte_caps = {217 .owner = THIS_MODULE,218 .name = "DTE PTP timer",219 .max_adj = 50000000,220 .n_ext_ts = 0,221 .n_pins = 0,222 .pps = 0,223 .adjfine = ptp_dte_adjfine,224 .adjtime = ptp_dte_adjtime,225 .gettime64 = ptp_dte_gettime,226 .settime64 = ptp_dte_settime,227 .enable = ptp_dte_enable,228};229 230static int ptp_dte_probe(struct platform_device *pdev)231{232 struct ptp_dte *ptp_dte;233 struct device *dev = &pdev->dev;234 235 ptp_dte = devm_kzalloc(dev, sizeof(struct ptp_dte), GFP_KERNEL);236 if (!ptp_dte)237 return -ENOMEM;238 239 ptp_dte->regs = devm_platform_ioremap_resource(pdev, 0);240 if (IS_ERR(ptp_dte->regs))241 return PTR_ERR(ptp_dte->regs);242 243 spin_lock_init(&ptp_dte->lock);244 245 ptp_dte->dev = dev;246 ptp_dte->caps = ptp_dte_caps;247 ptp_dte->ptp_clk = ptp_clock_register(&ptp_dte->caps, &pdev->dev);248 if (IS_ERR(ptp_dte->ptp_clk)) {249 dev_err(dev,250 "%s: Failed to register ptp clock\n", __func__);251 return PTR_ERR(ptp_dte->ptp_clk);252 }253 254 platform_set_drvdata(pdev, ptp_dte);255 256 dev_info(dev, "ptp clk probe done\n");257 258 return 0;259}260 261static void ptp_dte_remove(struct platform_device *pdev)262{263 struct ptp_dte *ptp_dte = platform_get_drvdata(pdev);264 u8 i;265 266 ptp_clock_unregister(ptp_dte->ptp_clk);267 268 for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++)269 writel(0, ptp_dte->regs + (i * sizeof(u32)));270}271 272#ifdef CONFIG_PM_SLEEP273static int ptp_dte_suspend(struct device *dev)274{275 struct ptp_dte *ptp_dte = dev_get_drvdata(dev);276 u8 i;277 278 for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) {279 ptp_dte->reg_val[i] =280 readl(ptp_dte->regs + (i * sizeof(u32)));281 }282 283 /* disable the nco */284 writel(0, ptp_dte->regs + DTE_NCO_INC_REG);285 286 return 0;287}288 289static int ptp_dte_resume(struct device *dev)290{291 struct ptp_dte *ptp_dte = dev_get_drvdata(dev);292 u8 i;293 294 for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) {295 if ((i * sizeof(u32)) != DTE_NCO_OVERFLOW_REG)296 writel(ptp_dte->reg_val[i],297 (ptp_dte->regs + (i * sizeof(u32))));298 else299 writel(((ptp_dte->reg_val[i] &300 DTE_NCO_SUM3_MASK) << DTE_NCO_SUM3_WR_SHIFT),301 (ptp_dte->regs + (i * sizeof(u32))));302 }303 304 return 0;305}306 307static const struct dev_pm_ops ptp_dte_pm_ops = {308 .suspend = ptp_dte_suspend,309 .resume = ptp_dte_resume310};311 312#define PTP_DTE_PM_OPS (&ptp_dte_pm_ops)313#else314#define PTP_DTE_PM_OPS NULL315#endif316 317static const struct of_device_id ptp_dte_of_match[] = {318 { .compatible = "brcm,ptp-dte", },319 {},320};321MODULE_DEVICE_TABLE(of, ptp_dte_of_match);322 323static struct platform_driver ptp_dte_driver = {324 .driver = {325 .name = "ptp-dte",326 .pm = PTP_DTE_PM_OPS,327 .of_match_table = ptp_dte_of_match,328 },329 .probe = ptp_dte_probe,330 .remove_new = ptp_dte_remove,331};332module_platform_driver(ptp_dte_driver);333 334MODULE_AUTHOR("Broadcom");335MODULE_DESCRIPTION("Broadcom DTE PTP Clock driver");336MODULE_LICENSE("GPL v2");337