drivers/ptp/ptp_dte.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 // Copyright 2017 Broadcom
0003
0004 #include <linux/err.h>
0005 #include <linux/io.h>
0006 #include <linux/module.h>
0007 #include <linux/mod_devicetable.h>
0008 #include <linux/platform_device.h>
0009 #include <linux/ptp_clock_kernel.h>
0010 #include <linux/types.h>
0011
0012 #define DTE_NCO_LOW_TIME_REG    0x00
0013 #define DTE_NCO_TIME_REG    0x04
0014 #define DTE_NCO_OVERFLOW_REG    0x08
0015 #define DTE_NCO_INC_REG     0x0c
0016
0017 #define DTE_NCO_SUM2_MASK   0xffffffff
0018 #define DTE_NCO_SUM2_SHIFT  4ULL
0019
0020 #define DTE_NCO_SUM3_MASK   0xff
0021 #define DTE_NCO_SUM3_SHIFT  36ULL
0022 #define DTE_NCO_SUM3_WR_SHIFT   8
0023
0024 #define DTE_NCO_TS_WRAP_MASK    0xfff
0025 #define DTE_NCO_TS_WRAP_LSHIFT  32
0026
0027 #define DTE_NCO_INC_DEFAULT 0x80000000
0028 #define DTE_NUM_REGS_TO_RESTORE 4
0029
0030 /* Full wrap around is 44bits in ns (~4.887 hrs) */
0031 #define DTE_WRAP_AROUND_NSEC_SHIFT 44
0032
0033 /* 44 bits NCO */
0034 #define DTE_NCO_MAX_NS  0xFFFFFFFFFFFLL
0035
0036 /* 125MHz with 3.29 reg cfg */
0037 #define DTE_PPB_ADJ(ppb) (u32)(div64_u64((((u64)abs(ppb) * BIT(28)) +\
0038                       62500000ULL), 125000000ULL))
0039
0040 /* ptp dte priv structure */
0041 struct ptp_dte {
0042     void __iomem *regs;
0043     struct ptp_clock *ptp_clk;
0044     struct ptp_clock_info caps;
0045     struct device *dev;
0046     u32 ts_ovf_last;
0047     u32 ts_wrap_cnt;
0048     spinlock_t lock;
0049     u32 reg_val[DTE_NUM_REGS_TO_RESTORE];
0050 };
0051
0052 static void dte_write_nco(void __iomem *regs, s64 ns)
0053 {
0054     u32 sum2, sum3;
0055
0056     sum2 = (u32)((ns >> DTE_NCO_SUM2_SHIFT) & DTE_NCO_SUM2_MASK);
0057     /* compensate for ignoring sum1 */
0058     if (sum2 != DTE_NCO_SUM2_MASK)
0059         sum2++;
0060
0061     /* to write sum3, bits [15:8] needs to be written */
0062     sum3 = (u32)(((ns >> DTE_NCO_SUM3_SHIFT) & DTE_NCO_SUM3_MASK) <<
0063              DTE_NCO_SUM3_WR_SHIFT);
0064
0065     writel(0, (regs + DTE_NCO_LOW_TIME_REG));
0066     writel(sum2, (regs + DTE_NCO_TIME_REG));
0067     writel(sum3, (regs + DTE_NCO_OVERFLOW_REG));
0068 }
0069
0070 static s64 dte_read_nco(void __iomem *regs)
0071 {
0072     u32 sum2, sum3;
0073     s64 ns;
0074
0075     /*
0076      * ignoring sum1 (4 bits) gives a 16ns resolution, which
0077      * works due to the async register read.
0078      */
0079     sum3 = readl(regs + DTE_NCO_OVERFLOW_REG) & DTE_NCO_SUM3_MASK;
0080     sum2 = readl(regs + DTE_NCO_TIME_REG);
0081     ns = ((s64)sum3 << DTE_NCO_SUM3_SHIFT) |
0082          ((s64)sum2 << DTE_NCO_SUM2_SHIFT);
0083
0084     return ns;
0085 }
0086
0087 static void dte_write_nco_delta(struct ptp_dte *ptp_dte, s64 delta)
0088 {
0089     s64 ns;
0090
0091     ns = dte_read_nco(ptp_dte->regs);
0092
0093     /* handle wraparound conditions */
0094     if ((delta < 0) && (abs(delta) > ns)) {
0095         if (ptp_dte->ts_wrap_cnt) {
0096             ns += DTE_NCO_MAX_NS + delta;
0097             ptp_dte->ts_wrap_cnt--;
0098         } else {
0099             ns = 0;
0100         }
0101     } else {
0102         ns += delta;
0103         if (ns > DTE_NCO_MAX_NS) {
0104             ptp_dte->ts_wrap_cnt++;
0105             ns -= DTE_NCO_MAX_NS;
0106         }
0107     }
0108
0109     dte_write_nco(ptp_dte->regs, ns);
0110
0111     ptp_dte->ts_ovf_last = (ns >> DTE_NCO_TS_WRAP_LSHIFT) &
0112             DTE_NCO_TS_WRAP_MASK;
0113 }
0114
0115 static s64 dte_read_nco_with_ovf(struct ptp_dte *ptp_dte)
0116 {
0117     u32 ts_ovf;
0118     s64 ns = 0;
0119
0120     ns = dte_read_nco(ptp_dte->regs);
0121
0122     /*Timestamp overflow: 8 LSB bits of sum3, 4 MSB bits of sum2 */
0123     ts_ovf = (ns >> DTE_NCO_TS_WRAP_LSHIFT) & DTE_NCO_TS_WRAP_MASK;
0124
0125     /* Check for wrap around */
0126     if (ts_ovf < ptp_dte->ts_ovf_last)
0127         ptp_dte->ts_wrap_cnt++;
0128
0129     ptp_dte->ts_ovf_last = ts_ovf;
0130
0131     /* adjust for wraparounds */
0132     ns += (s64)(BIT_ULL(DTE_WRAP_AROUND_NSEC_SHIFT) * ptp_dte->ts_wrap_cnt);
0133
0134     return ns;
0135 }
0136
0137 static int ptp_dte_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
0138 {
0139     u32 nco_incr;
0140     unsigned long flags;
0141     struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);
0142
0143     if (abs(ppb) > ptp_dte->caps.max_adj) {
0144         dev_err(ptp_dte->dev, "ppb adj too big\n");
0145         return -EINVAL;
0146     }
0147
0148     if (ppb < 0)
0149         nco_incr = DTE_NCO_INC_DEFAULT - DTE_PPB_ADJ(ppb);
0150     else
0151         nco_incr = DTE_NCO_INC_DEFAULT + DTE_PPB_ADJ(ppb);
0152
0153     spin_lock_irqsave(&ptp_dte->lock, flags);
0154     writel(nco_incr, ptp_dte->regs + DTE_NCO_INC_REG);
0155     spin_unlock_irqrestore(&ptp_dte->lock, flags);
0156
0157     return 0;
0158 }
0159
0160 static int ptp_dte_adjtime(struct ptp_clock_info *ptp, s64 delta)
0161 {
0162     unsigned long flags;
0163     struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);
0164
0165     spin_lock_irqsave(&ptp_dte->lock, flags);
0166     dte_write_nco_delta(ptp_dte, delta);
0167     spin_unlock_irqrestore(&ptp_dte->lock, flags);
0168
0169     return 0;
0170 }
0171
0172 static int ptp_dte_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
0173 {
0174     unsigned long flags;
0175     struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);
0176
0177     spin_lock_irqsave(&ptp_dte->lock, flags);
0178     *ts = ns_to_timespec64(dte_read_nco_with_ovf(ptp_dte));
0179     spin_unlock_irqrestore(&ptp_dte->lock, flags);
0180
0181     return 0;
0182 }
0183
0184 static int ptp_dte_settime(struct ptp_clock_info *ptp,
0185                  const struct timespec64 *ts)
0186 {
0187     unsigned long flags;
0188     struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);
0189
0190     spin_lock_irqsave(&ptp_dte->lock, flags);
0191
0192     /* Disable nco increment */
0193     writel(0, ptp_dte->regs + DTE_NCO_INC_REG);
0194
0195     dte_write_nco(ptp_dte->regs, timespec64_to_ns(ts));
0196
0197     /* reset overflow and wrap counter */
0198     ptp_dte->ts_ovf_last = 0;
0199     ptp_dte->ts_wrap_cnt = 0;
0200
0201     /* Enable nco increment */
0202     writel(DTE_NCO_INC_DEFAULT, ptp_dte->regs + DTE_NCO_INC_REG);
0203
0204     spin_unlock_irqrestore(&ptp_dte->lock, flags);
0205
0206     return 0;
0207 }
0208
0209 static int ptp_dte_enable(struct ptp_clock_info *ptp,
0210                 struct ptp_clock_request *rq, int on)
0211 {
0212     return -EOPNOTSUPP;
0213 }
0214
0215 static const struct ptp_clock_info ptp_dte_caps = {
0216     .owner      = THIS_MODULE,
0217     .name       = "DTE PTP timer",
0218     .max_adj    = 50000000,
0219     .n_ext_ts   = 0,
0220     .n_pins     = 0,
0221     .pps        = 0,
0222     .adjfreq    = ptp_dte_adjfreq,
0223     .adjtime    = ptp_dte_adjtime,
0224     .gettime64  = ptp_dte_gettime,
0225     .settime64  = ptp_dte_settime,
0226     .enable     = ptp_dte_enable,
0227 };
0228
0229 static int ptp_dte_probe(struct platform_device *pdev)
0230 {
0231     struct ptp_dte *ptp_dte;
0232     struct device *dev = &pdev->dev;
0233
0234     ptp_dte = devm_kzalloc(dev, sizeof(struct ptp_dte), GFP_KERNEL);
0235     if (!ptp_dte)
0236         return -ENOMEM;
0237
0238     ptp_dte->regs = devm_platform_ioremap_resource(pdev, 0);
0239     if (IS_ERR(ptp_dte->regs))
0240         return PTR_ERR(ptp_dte->regs);
0241
0242     spin_lock_init(&ptp_dte->lock);
0243
0244     ptp_dte->dev = dev;
0245     ptp_dte->caps = ptp_dte_caps;
0246     ptp_dte->ptp_clk = ptp_clock_register(&ptp_dte->caps, &pdev->dev);
0247     if (IS_ERR(ptp_dte->ptp_clk)) {
0248         dev_err(dev,
0249             "%s: Failed to register ptp clock\n", __func__);
0250         return PTR_ERR(ptp_dte->ptp_clk);
0251     }
0252
0253     platform_set_drvdata(pdev, ptp_dte);
0254
0255     dev_info(dev, "ptp clk probe done\n");
0256
0257     return 0;
0258 }
0259
0260 static int ptp_dte_remove(struct platform_device *pdev)
0261 {
0262     struct ptp_dte *ptp_dte = platform_get_drvdata(pdev);
0263     u8 i;
0264
0265     ptp_clock_unregister(ptp_dte->ptp_clk);
0266
0267     for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++)
0268         writel(0, ptp_dte->regs + (i * sizeof(u32)));
0269
0270     return 0;
0271 }
0272
0273 #ifdef CONFIG_PM_SLEEP
0274 static int ptp_dte_suspend(struct device *dev)
0275 {
0276     struct ptp_dte *ptp_dte = dev_get_drvdata(dev);
0277     u8 i;
0278
0279     for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) {
0280         ptp_dte->reg_val[i] =
0281             readl(ptp_dte->regs + (i * sizeof(u32)));
0282     }
0283
0284     /* disable the nco */
0285     writel(0, ptp_dte->regs + DTE_NCO_INC_REG);
0286
0287     return 0;
0288 }
0289
0290 static int ptp_dte_resume(struct device *dev)
0291 {
0292     struct ptp_dte *ptp_dte = dev_get_drvdata(dev);
0293     u8 i;
0294
0295     for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) {
0296         if ((i * sizeof(u32)) != DTE_NCO_OVERFLOW_REG)
0297             writel(ptp_dte->reg_val[i],
0298                 (ptp_dte->regs + (i * sizeof(u32))));
0299         else
0300             writel(((ptp_dte->reg_val[i] &
0301                 DTE_NCO_SUM3_MASK) << DTE_NCO_SUM3_WR_SHIFT),
0302                 (ptp_dte->regs + (i * sizeof(u32))));
0303     }
0304
0305     return 0;
0306 }
0307
0308 static const struct dev_pm_ops ptp_dte_pm_ops = {
0309     .suspend = ptp_dte_suspend,
0310     .resume = ptp_dte_resume
0311 };
0312
0313 #define PTP_DTE_PM_OPS  (&ptp_dte_pm_ops)
0314 #else
0315 #define PTP_DTE_PM_OPS  NULL
0316 #endif
0317
0318 static const struct of_device_id ptp_dte_of_match[] = {
0319     { .compatible = "brcm,ptp-dte", },
0320     {},
0321 };
0322 MODULE_DEVICE_TABLE(of, ptp_dte_of_match);
0323
0324 static struct platform_driver ptp_dte_driver = {
0325     .driver = {
0326         .name = "ptp-dte",
0327         .pm = PTP_DTE_PM_OPS,
0328         .of_match_table = ptp_dte_of_match,
0329     },
0330     .probe    = ptp_dte_probe,
0331     .remove   = ptp_dte_remove,
0332 };
0333 module_platform_driver(ptp_dte_driver);
0334
0335 MODULE_AUTHOR("Broadcom");
0336 MODULE_DESCRIPTION("Broadcom DTE PTP Clock driver");
0337 MODULE_LICENSE("GPL v2");