dsa/hirschmann/hellcreek_ptp.c

0001 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
0002 /*
0003  * DSA driver for:
0004  * Hirschmann Hellcreek TSN switch.
0005  *
0006  * Copyright (C) 2019,2020 Hochschule Offenburg
0007  * Copyright (C) 2019,2020 Linutronix GmbH
0008  * Authors: Kamil Alkhouri <kamil.alkhouri@hs-offenburg.de>
0009  *      Kurt Kanzenbach <kurt@linutronix.de>
0010  */
0011
0012 #include <linux/ptp_clock_kernel.h>
0013 #include "hellcreek.h"
0014 #include "hellcreek_ptp.h"
0015 #include "hellcreek_hwtstamp.h"
0016
0017 u16 hellcreek_ptp_read(struct hellcreek *hellcreek, unsigned int offset)
0018 {
0019     return readw(hellcreek->ptp_base + offset);
0020 }
0021
0022 void hellcreek_ptp_write(struct hellcreek *hellcreek, u16 data,
0023              unsigned int offset)
0024 {
0025     writew(data, hellcreek->ptp_base + offset);
0026 }
0027
0028 /* Get nanoseconds from PTP clock */
0029 static u64 hellcreek_ptp_clock_read(struct hellcreek *hellcreek)
0030 {
0031     u16 nsl, nsh;
0032
0033     /* Take a snapshot */
0034     hellcreek_ptp_write(hellcreek, PR_COMMAND_C_SS, PR_COMMAND_C);
0035
0036     /* The time of the day is saved as 96 bits. However, due to hardware
0037      * limitations the seconds are not or only partly kept in the PTP
0038      * core. Currently only three bits for the seconds are available. That's
0039      * why only the nanoseconds are used and the seconds are tracked in
0040      * software. Anyway due to internal locking all five registers should be
0041      * read.
0042      */
0043     nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C);
0044     nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C);
0045     nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C);
0046     nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C);
0047     nsl = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C);
0048
0049     return (u64)nsl | ((u64)nsh << 16);
0050 }
0051
0052 static u64 __hellcreek_ptp_gettime(struct hellcreek *hellcreek)
0053 {
0054     u64 ns;
0055
0056     ns = hellcreek_ptp_clock_read(hellcreek);
0057     if (ns < hellcreek->last_ts)
0058         hellcreek->seconds++;
0059     hellcreek->last_ts = ns;
0060     ns += hellcreek->seconds * NSEC_PER_SEC;
0061
0062     return ns;
0063 }
0064
0065 /* Retrieve the seconds parts in nanoseconds for a packet timestamped with @ns.
0066  * There has to be a check whether an overflow occurred between the packet
0067  * arrival and now. If so use the correct seconds (-1) for calculating the
0068  * packet arrival time.
0069  */
0070 u64 hellcreek_ptp_gettime_seconds(struct hellcreek *hellcreek, u64 ns)
0071 {
0072     u64 s;
0073
0074     __hellcreek_ptp_gettime(hellcreek);
0075     if (hellcreek->last_ts > ns)
0076         s = hellcreek->seconds * NSEC_PER_SEC;
0077     else
0078         s = (hellcreek->seconds - 1) * NSEC_PER_SEC;
0079
0080     return s;
0081 }
0082
0083 static int hellcreek_ptp_gettime(struct ptp_clock_info *ptp,
0084                  struct timespec64 *ts)
0085 {
0086     struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
0087     u64 ns;
0088
0089     mutex_lock(&hellcreek->ptp_lock);
0090     ns = __hellcreek_ptp_gettime(hellcreek);
0091     mutex_unlock(&hellcreek->ptp_lock);
0092
0093     *ts = ns_to_timespec64(ns);
0094
0095     return 0;
0096 }
0097
0098 static int hellcreek_ptp_settime(struct ptp_clock_info *ptp,
0099                  const struct timespec64 *ts)
0100 {
0101     struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
0102     u16 secl, nsh, nsl;
0103
0104     secl = ts->tv_sec & 0xffff;
0105     nsh  = ((u32)ts->tv_nsec & 0xffff0000) >> 16;
0106     nsl  = ts->tv_nsec & 0xffff;
0107
0108     mutex_lock(&hellcreek->ptp_lock);
0109
0110     /* Update overflow data structure */
0111     hellcreek->seconds = ts->tv_sec;
0112     hellcreek->last_ts = ts->tv_nsec;
0113
0114     /* Set time in clock */
0115     hellcreek_ptp_write(hellcreek, 0x00, PR_CLOCK_WRITE_C);
0116     hellcreek_ptp_write(hellcreek, 0x00, PR_CLOCK_WRITE_C);
0117     hellcreek_ptp_write(hellcreek, secl, PR_CLOCK_WRITE_C);
0118     hellcreek_ptp_write(hellcreek, nsh,  PR_CLOCK_WRITE_C);
0119     hellcreek_ptp_write(hellcreek, nsl,  PR_CLOCK_WRITE_C);
0120
0121     mutex_unlock(&hellcreek->ptp_lock);
0122
0123     return 0;
0124 }
0125
0126 static int hellcreek_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
0127 {
0128     struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
0129     u16 negative = 0, addendh, addendl;
0130     u32 addend;
0131     u64 adj;
0132
0133     if (scaled_ppm < 0) {
0134         negative = 1;
0135         scaled_ppm = -scaled_ppm;
0136     }
0137
0138     /* IP-Core adjusts the nominal frequency by adding or subtracting 1 ns
0139      * from the 8 ns (period of the oscillator) every time the accumulator
0140      * register overflows. The value stored in the addend register is added
0141      * to the accumulator register every 8 ns.
0142      *
0143      * addend value = (2^30 * accumulator_overflow_rate) /
0144      *                oscillator_frequency
0145      * where:
0146      *
0147      * oscillator_frequency = 125 MHz
0148      * accumulator_overflow_rate = 125 MHz * scaled_ppm * 2^-16 * 10^-6 * 8
0149      */
0150     adj = scaled_ppm;
0151     adj <<= 11;
0152     addend = (u32)div_u64(adj, 15625);
0153
0154     addendh = (addend & 0xffff0000) >> 16;
0155     addendl = addend & 0xffff;
0156
0157     negative = (negative << 15) & 0x8000;
0158
0159     mutex_lock(&hellcreek->ptp_lock);
0160
0161     /* Set drift register */
0162     hellcreek_ptp_write(hellcreek, negative, PR_CLOCK_DRIFT_C);
0163     hellcreek_ptp_write(hellcreek, 0x00, PR_CLOCK_DRIFT_C);
0164     hellcreek_ptp_write(hellcreek, 0x00, PR_CLOCK_DRIFT_C);
0165     hellcreek_ptp_write(hellcreek, addendh,  PR_CLOCK_DRIFT_C);
0166     hellcreek_ptp_write(hellcreek, addendl,  PR_CLOCK_DRIFT_C);
0167
0168     mutex_unlock(&hellcreek->ptp_lock);
0169
0170     return 0;
0171 }
0172
0173 static int hellcreek_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
0174 {
0175     struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
0176     u16 negative = 0, counth, countl;
0177     u32 count_val;
0178
0179     /* If the offset is larger than IP-Core slow offset resources. Don't
0180      * consider slow adjustment. Rather, add the offset directly to the
0181      * current time
0182      */
0183     if (abs(delta) > MAX_SLOW_OFFSET_ADJ) {
0184         struct timespec64 now, then = ns_to_timespec64(delta);
0185
0186         hellcreek_ptp_gettime(ptp, &now);
0187         now = timespec64_add(now, then);
0188         hellcreek_ptp_settime(ptp, &now);
0189
0190         return 0;
0191     }
0192
0193     if (delta < 0) {
0194         negative = 1;
0195         delta = -delta;
0196     }
0197
0198     /* 'count_val' does not exceed the maximum register size (2^30) */
0199     count_val = div_s64(delta, MAX_NS_PER_STEP);
0200
0201     counth = (count_val & 0xffff0000) >> 16;
0202     countl = count_val & 0xffff;
0203
0204     negative = (negative << 15) & 0x8000;
0205
0206     mutex_lock(&hellcreek->ptp_lock);
0207
0208     /* Set offset write register */
0209     hellcreek_ptp_write(hellcreek, negative, PR_CLOCK_OFFSET_C);
0210     hellcreek_ptp_write(hellcreek, MAX_NS_PER_STEP, PR_CLOCK_OFFSET_C);
0211     hellcreek_ptp_write(hellcreek, MIN_CLK_CYCLES_BETWEEN_STEPS,
0212                 PR_CLOCK_OFFSET_C);
0213     hellcreek_ptp_write(hellcreek, countl,  PR_CLOCK_OFFSET_C);
0214     hellcreek_ptp_write(hellcreek, counth,  PR_CLOCK_OFFSET_C);
0215
0216     mutex_unlock(&hellcreek->ptp_lock);
0217
0218     return 0;
0219 }
0220
0221 static int hellcreek_ptp_enable(struct ptp_clock_info *ptp,
0222                 struct ptp_clock_request *rq, int on)
0223 {
0224     return -EOPNOTSUPP;
0225 }
0226
0227 static void hellcreek_ptp_overflow_check(struct work_struct *work)
0228 {
0229     struct delayed_work *dw = to_delayed_work(work);
0230     struct hellcreek *hellcreek;
0231
0232     hellcreek = dw_overflow_to_hellcreek(dw);
0233
0234     mutex_lock(&hellcreek->ptp_lock);
0235     __hellcreek_ptp_gettime(hellcreek);
0236     mutex_unlock(&hellcreek->ptp_lock);
0237
0238     schedule_delayed_work(&hellcreek->overflow_work,
0239                   HELLCREEK_OVERFLOW_PERIOD);
0240 }
0241
0242 static enum led_brightness hellcreek_get_brightness(struct hellcreek *hellcreek,
0243                             int led)
0244 {
0245     return (hellcreek->status_out & led) ? 1 : 0;
0246 }
0247
0248 static void hellcreek_set_brightness(struct hellcreek *hellcreek, int led,
0249                      enum led_brightness b)
0250 {
0251     mutex_lock(&hellcreek->ptp_lock);
0252
0253     if (b)
0254         hellcreek->status_out |= led;
0255     else
0256         hellcreek->status_out &= ~led;
0257
0258     hellcreek_ptp_write(hellcreek, hellcreek->status_out, STATUS_OUT);
0259
0260     mutex_unlock(&hellcreek->ptp_lock);
0261 }
0262
0263 static void hellcreek_led_sync_good_set(struct led_classdev *ldev,
0264                     enum led_brightness b)
0265 {
0266     struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_sync_good);
0267
0268     hellcreek_set_brightness(hellcreek, STATUS_OUT_SYNC_GOOD, b);
0269 }
0270
0271 static enum led_brightness hellcreek_led_sync_good_get(struct led_classdev *ldev)
0272 {
0273     struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_sync_good);
0274
0275     return hellcreek_get_brightness(hellcreek, STATUS_OUT_SYNC_GOOD);
0276 }
0277
0278 static void hellcreek_led_is_gm_set(struct led_classdev *ldev,
0279                     enum led_brightness b)
0280 {
0281     struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_is_gm);
0282
0283     hellcreek_set_brightness(hellcreek, STATUS_OUT_IS_GM, b);
0284 }
0285
0286 static enum led_brightness hellcreek_led_is_gm_get(struct led_classdev *ldev)
0287 {
0288     struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_is_gm);
0289
0290     return hellcreek_get_brightness(hellcreek, STATUS_OUT_IS_GM);
0291 }
0292
0293 /* There two available LEDs internally called sync_good and is_gm. However, the
0294  * user might want to use a different label and specify the default state. Take
0295  * those properties from device tree.
0296  */
0297 static int hellcreek_led_setup(struct hellcreek *hellcreek)
0298 {
0299     struct device_node *leds, *led = NULL;
0300     const char *label, *state;
0301     int ret = -EINVAL;
0302
0303     of_node_get(hellcreek->dev->of_node);
0304     leds = of_find_node_by_name(hellcreek->dev->of_node, "leds");
0305     if (!leds) {
0306         dev_err(hellcreek->dev, "No LEDs specified in device tree!\n");
0307         return ret;
0308     }
0309
0310     hellcreek->status_out = 0;
0311
0312     led = of_get_next_available_child(leds, led);
0313     if (!led) {
0314         dev_err(hellcreek->dev, "First LED not specified!\n");
0315         goto out;
0316     }
0317
0318     ret = of_property_read_string(led, "label", &label);
0319     hellcreek->led_sync_good.name = ret ? "sync_good" : label;
0320
0321     ret = of_property_read_string(led, "default-state", &state);
0322     if (!ret) {
0323         if (!strcmp(state, "on"))
0324             hellcreek->led_sync_good.brightness = 1;
0325         else if (!strcmp(state, "off"))
0326             hellcreek->led_sync_good.brightness = 0;
0327         else if (!strcmp(state, "keep"))
0328             hellcreek->led_sync_good.brightness =
0329                 hellcreek_get_brightness(hellcreek,
0330                              STATUS_OUT_SYNC_GOOD);
0331     }
0332
0333     hellcreek->led_sync_good.max_brightness = 1;
0334     hellcreek->led_sync_good.brightness_set = hellcreek_led_sync_good_set;
0335     hellcreek->led_sync_good.brightness_get = hellcreek_led_sync_good_get;
0336
0337     led = of_get_next_available_child(leds, led);
0338     if (!led) {
0339         dev_err(hellcreek->dev, "Second LED not specified!\n");
0340         ret = -EINVAL;
0341         goto out;
0342     }
0343
0344     ret = of_property_read_string(led, "label", &label);
0345     hellcreek->led_is_gm.name = ret ? "is_gm" : label;
0346
0347     ret = of_property_read_string(led, "default-state", &state);
0348     if (!ret) {
0349         if (!strcmp(state, "on"))
0350             hellcreek->led_is_gm.brightness = 1;
0351         else if (!strcmp(state, "off"))
0352             hellcreek->led_is_gm.brightness = 0;
0353         else if (!strcmp(state, "keep"))
0354             hellcreek->led_is_gm.brightness =
0355                 hellcreek_get_brightness(hellcreek,
0356                              STATUS_OUT_IS_GM);
0357     }
0358
0359     hellcreek->led_is_gm.max_brightness = 1;
0360     hellcreek->led_is_gm.brightness_set = hellcreek_led_is_gm_set;
0361     hellcreek->led_is_gm.brightness_get = hellcreek_led_is_gm_get;
0362
0363     /* Set initial state */
0364     if (hellcreek->led_sync_good.brightness == 1)
0365         hellcreek_set_brightness(hellcreek, STATUS_OUT_SYNC_GOOD, 1);
0366     if (hellcreek->led_is_gm.brightness == 1)
0367         hellcreek_set_brightness(hellcreek, STATUS_OUT_IS_GM, 1);
0368
0369     /* Register both leds */
0370     led_classdev_register(hellcreek->dev, &hellcreek->led_sync_good);
0371     led_classdev_register(hellcreek->dev, &hellcreek->led_is_gm);
0372
0373     ret = 0;
0374
0375 out:
0376     of_node_put(leds);
0377
0378     return ret;
0379 }
0380
0381 int hellcreek_ptp_setup(struct hellcreek *hellcreek)
0382 {
0383     u16 status;
0384     int ret;
0385
0386     /* Set up the overflow work */
0387     INIT_DELAYED_WORK(&hellcreek->overflow_work,
0388               hellcreek_ptp_overflow_check);
0389
0390     /* Setup PTP clock */
0391     hellcreek->ptp_clock_info.owner = THIS_MODULE;
0392     snprintf(hellcreek->ptp_clock_info.name,
0393          sizeof(hellcreek->ptp_clock_info.name),
0394          dev_name(hellcreek->dev));
0395
0396     /* IP-Core can add up to 0.5 ns per 8 ns cycle, which means
0397      * accumulator_overflow_rate shall not exceed 62.5 MHz (which adjusts
0398      * the nominal frequency by 6.25%)
0399      */
0400     hellcreek->ptp_clock_info.max_adj     = 62500000;
0401     hellcreek->ptp_clock_info.n_alarm     = 0;
0402     hellcreek->ptp_clock_info.n_pins      = 0;
0403     hellcreek->ptp_clock_info.n_ext_ts    = 0;
0404     hellcreek->ptp_clock_info.n_per_out   = 0;
0405     hellcreek->ptp_clock_info.pps         = 0;
0406     hellcreek->ptp_clock_info.adjfine     = hellcreek_ptp_adjfine;
0407     hellcreek->ptp_clock_info.adjtime     = hellcreek_ptp_adjtime;
0408     hellcreek->ptp_clock_info.gettime64   = hellcreek_ptp_gettime;
0409     hellcreek->ptp_clock_info.settime64   = hellcreek_ptp_settime;
0410     hellcreek->ptp_clock_info.enable      = hellcreek_ptp_enable;
0411     hellcreek->ptp_clock_info.do_aux_work = hellcreek_hwtstamp_work;
0412
0413     hellcreek->ptp_clock = ptp_clock_register(&hellcreek->ptp_clock_info,
0414                           hellcreek->dev);
0415     if (IS_ERR(hellcreek->ptp_clock))
0416         return PTR_ERR(hellcreek->ptp_clock);
0417
0418     /* Enable the offset correction process, if no offset correction is
0419      * already taking place
0420      */
0421     status = hellcreek_ptp_read(hellcreek, PR_CLOCK_STATUS_C);
0422     if (!(status & PR_CLOCK_STATUS_C_OFS_ACT))
0423         hellcreek_ptp_write(hellcreek,
0424                     status | PR_CLOCK_STATUS_C_ENA_OFS,
0425                     PR_CLOCK_STATUS_C);
0426
0427     /* Enable the drift correction process */
0428     hellcreek_ptp_write(hellcreek, status | PR_CLOCK_STATUS_C_ENA_DRIFT,
0429                 PR_CLOCK_STATUS_C);
0430
0431     /* LED setup */
0432     ret = hellcreek_led_setup(hellcreek);
0433     if (ret) {
0434         if (hellcreek->ptp_clock)
0435             ptp_clock_unregister(hellcreek->ptp_clock);
0436         return ret;
0437     }
0438
0439     schedule_delayed_work(&hellcreek->overflow_work,
0440                   HELLCREEK_OVERFLOW_PERIOD);
0441
0442     return 0;
0443 }
0444
0445 void hellcreek_ptp_free(struct hellcreek *hellcreek)
0446 {
0447     led_classdev_unregister(&hellcreek->led_is_gm);
0448     led_classdev_unregister(&hellcreek->led_sync_good);
0449     cancel_delayed_work_sync(&hellcreek->overflow_work);
0450     if (hellcreek->ptp_clock)
0451         ptp_clock_unregister(hellcreek->ptp_clock);
0452     hellcreek->ptp_clock = NULL;
0453 }