drivers/ptp/ptp_mock.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Copyright 2023 NXP
   4  *
   5  * Mock-up PTP Hardware Clock driver for virtual network devices
   6  *
   7  * Create a PTP clock which offers PTP time manipulation operations
   8  * using a timecounter/cyclecounter on top of CLOCK_MONOTONIC_RAW.
   9  */
  10
  11 #include <linux/ptp_clock_kernel.h>
  12 #include <linux/ptp_mock.h>
  13 #include <linux/timecounter.h>
  14
  15 /* Clamp scaled_ppm between -2,097,152,000 and 2,097,152,000,
  16  * and thus "adj" between -68,719,476 and 68,719,476
  17  */
  18 #define MOCK_PHC_MAX_ADJ_PPB            32000000
  19 /* Timestamps from ktime_get_raw() have 1 ns resolution, so the scale factor
  20  * (MULT >> SHIFT) needs to be 1. Pick SHIFT as 31 bits, which translates
  21  * MULT(freq 0) into 0x80000000.
  22  */
  23 #define MOCK_PHC_CC_SHIFT               31
  24 #define MOCK_PHC_CC_MULT                (1 << MOCK_PHC_CC_SHIFT)
  25 #define MOCK_PHC_FADJ_SHIFT             9
  26 #define MOCK_PHC_FADJ_DENOMINATOR       15625ULL
  27
  28 /* The largest cycle_delta that timecounter_read_delta() can handle without a
  29  * 64-bit overflow during the multiplication with cc->mult, given the max "adj"
  30  * we permit, is ~8.3 seconds. Make sure readouts are more frequent than that.
  31  */
  32 #define MOCK_PHC_REFRESH_INTERVAL       (HZ * 5)
  33
  34 #define info_to_phc(d) container_of((d), struct mock_phc, info)
  35
  36 struct mock_phc {
  37         struct ptp_clock_info info;
  38         struct ptp_clock *clock;
  39         struct timecounter tc;
  40         struct cyclecounter cc;
  41         spinlock_t lock;
  42 };
  43
  44 static u64 mock_phc_cc_read(const struct cyclecounter *cc)
  45 {
  46         return ktime_get_raw_ns();
  47 }
  48
  49 static int mock_phc_adjfine(struct ptp_clock_info *info, long scaled_ppm)
  50 {
  51         struct mock_phc *phc = info_to_phc(info);
  52         s64 adj;
  53
  54         adj = (s64)scaled_ppm << MOCK_PHC_FADJ_SHIFT;
  55         adj = div_s64(adj, MOCK_PHC_FADJ_DENOMINATOR);
  56
  57         spin_lock(&phc->lock);
  58         timecounter_read(&phc->tc);
  59         phc->cc.mult = MOCK_PHC_CC_MULT + adj;
  60         spin_unlock(&phc->lock);
  61
  62         return 0;
  63 }
  64
  65 static int mock_phc_adjtime(struct ptp_clock_info *info, s64 delta)
  66 {
  67         struct mock_phc *phc = info_to_phc(info);
  68
  69         spin_lock(&phc->lock);
  70         timecounter_adjtime(&phc->tc, delta);
  71         spin_unlock(&phc->lock);
  72
  73         return 0;
  74 }
  75
  76 static int mock_phc_settime64(struct ptp_clock_info *info,
  77                               const struct timespec64 *ts)
  78 {
  79         struct mock_phc *phc = info_to_phc(info);
  80         u64 ns = timespec64_to_ns(ts);
  81
  82         spin_lock(&phc->lock);
  83         timecounter_init(&phc->tc, &phc->cc, ns);
  84         spin_unlock(&phc->lock);
  85
  86         return 0;
  87 }
  88
  89 static int mock_phc_gettime64(struct ptp_clock_info *info, struct timespec64 *ts)
  90 {
  91         struct mock_phc *phc = info_to_phc(info);
  92         u64 ns;
  93
  94         spin_lock(&phc->lock);
  95         ns = timecounter_read(&phc->tc);
  96         spin_unlock(&phc->lock);
  97
  98         *ts = ns_to_timespec64(ns);
  99
 100         return 0;
 101 }
 102
 103 static long mock_phc_refresh(struct ptp_clock_info *info)
 104 {
 105         struct timespec64 ts;
 106
 107         mock_phc_gettime64(info, &ts);
 108
 109         return MOCK_PHC_REFRESH_INTERVAL;
 110 }
 111
 112 int mock_phc_index(struct mock_phc *phc)
 113 {
 114         return ptp_clock_index(phc->clock);
 115 }
 116 EXPORT_SYMBOL_GPL(mock_phc_index);
 117
 118 struct mock_phc *mock_phc_create(struct device *dev)
 119 {
 120         struct mock_phc *phc;
 121         int err;
 122
 123         phc = kzalloc(sizeof(*phc), GFP_KERNEL);
 124         if (!phc) {
 125                 err = -ENOMEM;
 126                 goto out;
 127         }
 128
 129         phc->info = (struct ptp_clock_info) {
 130                 .owner          = THIS_MODULE,
 131                 .name           = "Mock-up PTP clock",
 132                 .max_adj        = MOCK_PHC_MAX_ADJ_PPB,
 133                 .adjfine        = mock_phc_adjfine,
 134                 .adjtime        = mock_phc_adjtime,
 135                 .gettime64      = mock_phc_gettime64,
 136                 .settime64      = mock_phc_settime64,
 137                 .do_aux_work    = mock_phc_refresh,
 138         };
 139
 140         phc->cc = (struct cyclecounter) {
 141                 .read   = mock_phc_cc_read,
 142                 .mask   = CYCLECOUNTER_MASK(64),
 143                 .mult   = MOCK_PHC_CC_MULT,
 144                 .shift  = MOCK_PHC_CC_SHIFT,
 145         };
 146
 147         spin_lock_init(&phc->lock);
 148         timecounter_init(&phc->tc, &phc->cc, 0);
 149
 150         phc->clock = ptp_clock_register(&phc->info, dev);
 151         if (IS_ERR(phc->clock)) {
 152                 err = PTR_ERR(phc->clock);
 153                 goto out_free_phc;
 154         }
 155
 156         ptp_schedule_worker(phc->clock, MOCK_PHC_REFRESH_INTERVAL);
 157
 158         return phc;
 159
 160 out_free_phc:
 161         kfree(phc);
 162 out:
 163         return ERR_PTR(err);
 164 }
 165 EXPORT_SYMBOL_GPL(mock_phc_create);
 166
 167 void mock_phc_destroy(struct mock_phc *phc)
 168 {
 169         ptp_clock_unregister(phc->clock);
 170         kfree(phc);
 171 }
 172 EXPORT_SYMBOL_GPL(mock_phc_destroy);
 173
 174 MODULE_DESCRIPTION("Mock-up PTP Hardware Clock driver");
 175 MODULE_LICENSE("GPL");