drivers/net/dsa/hirschmann/hellcreek_ptp.c

   1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
   2 /*
   3  * DSA driver for:
   4  * Hirschmann Hellcreek TSN switch.
   5  *
   6  * Copyright (C) 2019,2020 Hochschule Offenburg
   7  * Copyright (C) 2019,2020 Linutronix GmbH
   8  * Authors: Kamil Alkhouri <kamil.alkhouri@hs-offenburg.de>
   9  *          Kurt Kanzenbach <kurt@linutronix.de>
  10  */
  11
  12 #include <linux/ptp_clock_kernel.h>
  13 #include "hellcreek.h"
  14 #include "hellcreek_ptp.h"
  15 #include "hellcreek_hwtstamp.h"
  16
  17 u16 hellcreek_ptp_read(struct hellcreek *hellcreek, unsigned int offset)
  18 {
  19         return readw(hellcreek->ptp_base + offset);
  20 }
  21
  22 void hellcreek_ptp_write(struct hellcreek *hellcreek, u16 data,
  23                          unsigned int offset)
  24 {
  25         writew(data, hellcreek->ptp_base + offset);
  26 }
  27
  28 /* Get nanoseconds from PTP clock */
  29 static u64 hellcreek_ptp_clock_read(struct hellcreek *hellcreek)
  30 {
  31         u16 nsl, nsh;
  32
  33         /* Take a snapshot */
  34         hellcreek_ptp_write(hellcreek, PR_COMMAND_C_SS, PR_COMMAND_C);
  35
  36         /* The time of the day is saved as 96 bits. However, due to hardware
  37          * limitations the seconds are not or only partly kept in the PTP
  38          * core. Currently only three bits for the seconds are available. That's
  39          * why only the nanoseconds are used and the seconds are tracked in
  40          * software. Anyway due to internal locking all five registers should be
  41          * read.
  42          */
  43         nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C);
  44         nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C);
  45         nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C);
  46         nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C);
  47         nsl = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C);
  48
  49         return (u64)nsl | ((u64)nsh << 16);
  50 }
  51
  52 static u64 __hellcreek_ptp_gettime(struct hellcreek *hellcreek)
  53 {
  54         u64 ns;
  55
  56         ns = hellcreek_ptp_clock_read(hellcreek);
  57         if (ns < hellcreek->last_ts)
  58                 hellcreek->seconds++;
  59         hellcreek->last_ts = ns;
  60         ns += hellcreek->seconds * NSEC_PER_SEC;
  61
  62         return ns;
  63 }
  64
  65 /* Retrieve the seconds parts in nanoseconds for a packet timestamped with @ns.
  66  * There has to be a check whether an overflow occurred between the packet
  67  * arrival and now. If so use the correct seconds (-1) for calculating the
  68  * packet arrival time.
  69  */
  70 u64 hellcreek_ptp_gettime_seconds(struct hellcreek *hellcreek, u64 ns)
  71 {
  72         u64 s;
  73
  74         __hellcreek_ptp_gettime(hellcreek);
  75         if (hellcreek->last_ts > ns)
  76                 s = hellcreek->seconds * NSEC_PER_SEC;
  77         else
  78                 s = (hellcreek->seconds - 1) * NSEC_PER_SEC;
  79
  80         return s;
  81 }
  82
  83 static int hellcreek_ptp_gettime(struct ptp_clock_info *ptp,
  84                                  struct timespec64 *ts)
  85 {
  86         struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
  87         u64 ns;
  88
  89         mutex_lock(&hellcreek->ptp_lock);
  90         ns = __hellcreek_ptp_gettime(hellcreek);
  91         mutex_unlock(&hellcreek->ptp_lock);
  92
  93         *ts = ns_to_timespec64(ns);
  94
  95         return 0;
  96 }
  97
  98 static int hellcreek_ptp_settime(struct ptp_clock_info *ptp,
  99                                  const struct timespec64 *ts)
 100 {
 101         struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
 102         u16 secl, nsh, nsl;
 103
 104         secl = ts->tv_sec & 0xffff;
 105         nsh  = ((u32)ts->tv_nsec & 0xffff0000) >> 16;
 106         nsl  = ts->tv_nsec & 0xffff;
 107
 108         mutex_lock(&hellcreek->ptp_lock);
 109
 110         /* Update overflow data structure */
 111         hellcreek->seconds = ts->tv_sec;
 112         hellcreek->last_ts = ts->tv_nsec;
 113
 114         /* Set time in clock */
 115         hellcreek_ptp_write(hellcreek, 0x00, PR_CLOCK_WRITE_C);
 116         hellcreek_ptp_write(hellcreek, 0x00, PR_CLOCK_WRITE_C);
 117         hellcreek_ptp_write(hellcreek, secl, PR_CLOCK_WRITE_C);
 118         hellcreek_ptp_write(hellcreek, nsh,  PR_CLOCK_WRITE_C);
 119         hellcreek_ptp_write(hellcreek, nsl,  PR_CLOCK_WRITE_C);
 120
 121         mutex_unlock(&hellcreek->ptp_lock);
 122
 123         return 0;
 124 }
 125
 126 static int hellcreek_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 127 {
 128         struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
 129         u16 negative = 0, addendh, addendl;
 130         u32 addend;
 131         u64 adj;
 132
 133         if (scaled_ppm < 0) {
 134                 negative = 1;
 135                 scaled_ppm = -scaled_ppm;
 136         }
 137
 138         /* IP-Core adjusts the nominal frequency by adding or subtracting 1 ns
 139          * from the 8 ns (period of the oscillator) every time the accumulator
 140          * register overflows. The value stored in the addend register is added
 141          * to the accumulator register every 8 ns.
 142          *
 143          * addend value = (2^30 * accumulator_overflow_rate) /
 144          *                oscillator_frequency
 145          * where:
 146          *
 147          * oscillator_frequency = 125 MHz
 148          * accumulator_overflow_rate = 125 MHz * scaled_ppm * 2^-16 * 10^-6 * 8
 149          */
 150         adj = scaled_ppm;
 151         adj <<= 11;
 152         addend = (u32)div_u64(adj, 15625);
 153
 154         addendh = (addend & 0xffff0000) >> 16;
 155         addendl = addend & 0xffff;
 156
 157         negative = (negative << 15) & 0x8000;
 158
 159         mutex_lock(&hellcreek->ptp_lock);
 160
 161         /* Set drift register */
 162         hellcreek_ptp_write(hellcreek, negative, PR_CLOCK_DRIFT_C);
 163         hellcreek_ptp_write(hellcreek, 0x00, PR_CLOCK_DRIFT_C);
 164         hellcreek_ptp_write(hellcreek, 0x00, PR_CLOCK_DRIFT_C);
 165         hellcreek_ptp_write(hellcreek, addendh,  PR_CLOCK_DRIFT_C);
 166         hellcreek_ptp_write(hellcreek, addendl,  PR_CLOCK_DRIFT_C);
 167
 168         mutex_unlock(&hellcreek->ptp_lock);
 169
 170         return 0;
 171 }
 172
 173 static int hellcreek_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 174 {
 175         struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
 176         u16 negative = 0, counth, countl;
 177         u32 count_val;
 178
 179         /* If the offset is larger than IP-Core slow offset resources. Don't
 180          * consider slow adjustment. Rather, add the offset directly to the
 181          * current time
 182          */
 183         if (abs(delta) > MAX_SLOW_OFFSET_ADJ) {
 184                 struct timespec64 now, then = ns_to_timespec64(delta);
 185
 186                 hellcreek_ptp_gettime(ptp, &now);
 187                 now = timespec64_add(now, then);
 188                 hellcreek_ptp_settime(ptp, &now);
 189
 190                 return 0;
 191         }
 192
 193         if (delta < 0) {
 194                 negative = 1;
 195                 delta = -delta;
 196         }
 197
 198         /* 'count_val' does not exceed the maximum register size (2^30) */
 199         count_val = div_s64(delta, MAX_NS_PER_STEP);
 200
 201         counth = (count_val & 0xffff0000) >> 16;
 202         countl = count_val & 0xffff;
 203
 204         negative = (negative << 15) & 0x8000;
 205
 206         mutex_lock(&hellcreek->ptp_lock);
 207
 208         /* Set offset write register */
 209         hellcreek_ptp_write(hellcreek, negative, PR_CLOCK_OFFSET_C);
 210         hellcreek_ptp_write(hellcreek, MAX_NS_PER_STEP, PR_CLOCK_OFFSET_C);
 211         hellcreek_ptp_write(hellcreek, MIN_CLK_CYCLES_BETWEEN_STEPS,
 212                             PR_CLOCK_OFFSET_C);
 213         hellcreek_ptp_write(hellcreek, countl,  PR_CLOCK_OFFSET_C);
 214         hellcreek_ptp_write(hellcreek, counth,  PR_CLOCK_OFFSET_C);
 215
 216         mutex_unlock(&hellcreek->ptp_lock);
 217
 218         return 0;
 219 }
 220
 221 static int hellcreek_ptp_enable(struct ptp_clock_info *ptp,
 222                                 struct ptp_clock_request *rq, int on)
 223 {
 224         return -EOPNOTSUPP;
 225 }
 226
 227 static void hellcreek_ptp_overflow_check(struct work_struct *work)
 228 {
 229         struct delayed_work *dw = to_delayed_work(work);
 230         struct hellcreek *hellcreek;
 231
 232         hellcreek = dw_overflow_to_hellcreek(dw);
 233
 234         mutex_lock(&hellcreek->ptp_lock);
 235         __hellcreek_ptp_gettime(hellcreek);
 236         mutex_unlock(&hellcreek->ptp_lock);
 237
 238         schedule_delayed_work(&hellcreek->overflow_work,
 239                               HELLCREEK_OVERFLOW_PERIOD);
 240 }
 241
 242 static enum led_brightness hellcreek_get_brightness(struct hellcreek *hellcreek,
 243                                                     int led)
 244 {
 245         return (hellcreek->status_out & led) ? 1 : 0;
 246 }
 247
 248 static void hellcreek_set_brightness(struct hellcreek *hellcreek, int led,
 249                                      enum led_brightness b)
 250 {
 251         mutex_lock(&hellcreek->ptp_lock);
 252
 253         if (b)
 254                 hellcreek->status_out |= led;
 255         else
 256                 hellcreek->status_out &= ~led;
 257
 258         hellcreek_ptp_write(hellcreek, hellcreek->status_out, STATUS_OUT);
 259
 260         mutex_unlock(&hellcreek->ptp_lock);
 261 }
 262
 263 static void hellcreek_led_sync_good_set(struct led_classdev *ldev,
 264                                         enum led_brightness b)
 265 {
 266         struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_sync_good);
 267
 268         hellcreek_set_brightness(hellcreek, STATUS_OUT_SYNC_GOOD, b);
 269 }
 270
 271 static enum led_brightness hellcreek_led_sync_good_get(struct led_classdev *ldev)
 272 {
 273         struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_sync_good);
 274
 275         return hellcreek_get_brightness(hellcreek, STATUS_OUT_SYNC_GOOD);
 276 }
 277
 278 static void hellcreek_led_is_gm_set(struct led_classdev *ldev,
 279                                     enum led_brightness b)
 280 {
 281         struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_is_gm);
 282
 283         hellcreek_set_brightness(hellcreek, STATUS_OUT_IS_GM, b);
 284 }
 285
 286 static enum led_brightness hellcreek_led_is_gm_get(struct led_classdev *ldev)
 287 {
 288         struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_is_gm);
 289
 290         return hellcreek_get_brightness(hellcreek, STATUS_OUT_IS_GM);
 291 }
 292
 293 /* There two available LEDs internally called sync_good and is_gm. However, the
 294  * user might want to use a different label and specify the default state. Take
 295  * those properties from device tree.
 296  */
 297 static int hellcreek_led_setup(struct hellcreek *hellcreek)
 298 {
 299         struct device_node *leds, *led = NULL;
 300         const char *label, *state;
 301         int ret = -EINVAL;
 302
 303         of_node_get(hellcreek->dev->of_node);
 304         leds = of_find_node_by_name(hellcreek->dev->of_node, "leds");
 305         if (!leds) {
 306                 dev_err(hellcreek->dev, "No LEDs specified in device tree!\n");
 307                 return ret;
 308         }
 309
 310         hellcreek->status_out = 0;
 311
 312         led = of_get_next_available_child(leds, led);
 313         if (!led) {
 314                 dev_err(hellcreek->dev, "First LED not specified!\n");
 315                 goto out;
 316         }
 317
 318         ret = of_property_read_string(led, "label", &label);
 319         hellcreek->led_sync_good.name = ret ? "sync_good" : label;
 320
 321         ret = of_property_read_string(led, "default-state", &state);
 322         if (!ret) {
 323                 if (!strcmp(state, "on"))
 324                         hellcreek->led_sync_good.brightness = 1;
 325                 else if (!strcmp(state, "off"))
 326                         hellcreek->led_sync_good.brightness = 0;
 327                 else if (!strcmp(state, "keep"))
 328                         hellcreek->led_sync_good.brightness =
 329                                 hellcreek_get_brightness(hellcreek,
 330                                                          STATUS_OUT_SYNC_GOOD);
 331         }
 332
 333         hellcreek->led_sync_good.max_brightness = 1;
 334         hellcreek->led_sync_good.brightness_set = hellcreek_led_sync_good_set;
 335         hellcreek->led_sync_good.brightness_get = hellcreek_led_sync_good_get;
 336
 337         led = of_get_next_available_child(leds, led);
 338         if (!led) {
 339                 dev_err(hellcreek->dev, "Second LED not specified!\n");
 340                 ret = -EINVAL;
 341                 goto out;
 342         }
 343
 344         ret = of_property_read_string(led, "label", &label);
 345         hellcreek->led_is_gm.name = ret ? "is_gm" : label;
 346
 347         ret = of_property_read_string(led, "default-state", &state);
 348         if (!ret) {
 349                 if (!strcmp(state, "on"))
 350                         hellcreek->led_is_gm.brightness = 1;
 351                 else if (!strcmp(state, "off"))
 352                         hellcreek->led_is_gm.brightness = 0;
 353                 else if (!strcmp(state, "keep"))
 354                         hellcreek->led_is_gm.brightness =
 355                                 hellcreek_get_brightness(hellcreek,
 356                                                          STATUS_OUT_IS_GM);
 357         }
 358
 359         hellcreek->led_is_gm.max_brightness = 1;
 360         hellcreek->led_is_gm.brightness_set = hellcreek_led_is_gm_set;
 361         hellcreek->led_is_gm.brightness_get = hellcreek_led_is_gm_get;
 362
 363         /* Set initial state */
 364         if (hellcreek->led_sync_good.brightness == 1)
 365                 hellcreek_set_brightness(hellcreek, STATUS_OUT_SYNC_GOOD, 1);
 366         if (hellcreek->led_is_gm.brightness == 1)
 367                 hellcreek_set_brightness(hellcreek, STATUS_OUT_IS_GM, 1);
 368
 369         /* Register both leds */
 370         led_classdev_register(hellcreek->dev, &hellcreek->led_sync_good);
 371         led_classdev_register(hellcreek->dev, &hellcreek->led_is_gm);
 372
 373         ret = 0;
 374
 375 out:
 376         of_node_put(leds);
 377
 378         return ret;
 379 }
 380
 381 int hellcreek_ptp_setup(struct hellcreek *hellcreek)
 382 {
 383         u16 status;
 384         int ret;
 385
 386         /* Set up the overflow work */
 387         INIT_DELAYED_WORK(&hellcreek->overflow_work,
 388                           hellcreek_ptp_overflow_check);
 389
 390         /* Setup PTP clock */
 391         hellcreek->ptp_clock_info.owner = THIS_MODULE;
 392         snprintf(hellcreek->ptp_clock_info.name,
 393                  sizeof(hellcreek->ptp_clock_info.name),
 394                  dev_name(hellcreek->dev));
 395
 396         /* IP-Core can add up to 0.5 ns per 8 ns cycle, which means
 397          * accumulator_overflow_rate shall not exceed 62.5 MHz (which adjusts
 398          * the nominal frequency by 6.25%)
 399          */
 400         hellcreek->ptp_clock_info.max_adj     = 62500000;
 401         hellcreek->ptp_clock_info.n_alarm     = 0;
 402         hellcreek->ptp_clock_info.n_pins      = 0;
 403         hellcreek->ptp_clock_info.n_ext_ts    = 0;
 404         hellcreek->ptp_clock_info.n_per_out   = 0;
 405         hellcreek->ptp_clock_info.pps         = 0;
 406         hellcreek->ptp_clock_info.adjfine     = hellcreek_ptp_adjfine;
 407         hellcreek->ptp_clock_info.adjtime     = hellcreek_ptp_adjtime;
 408         hellcreek->ptp_clock_info.gettime64   = hellcreek_ptp_gettime;
 409         hellcreek->ptp_clock_info.settime64   = hellcreek_ptp_settime;
 410         hellcreek->ptp_clock_info.enable      = hellcreek_ptp_enable;
 411         hellcreek->ptp_clock_info.do_aux_work = hellcreek_hwtstamp_work;
 412
 413         hellcreek->ptp_clock = ptp_clock_register(&hellcreek->ptp_clock_info,
 414                                                   hellcreek->dev);
 415         if (IS_ERR(hellcreek->ptp_clock))
 416                 return PTR_ERR(hellcreek->ptp_clock);
 417
 418         /* Enable the offset correction process, if no offset correction is
 419          * already taking place
 420          */
 421         status = hellcreek_ptp_read(hellcreek, PR_CLOCK_STATUS_C);
 422         if (!(status & PR_CLOCK_STATUS_C_OFS_ACT))
 423                 hellcreek_ptp_write(hellcreek,
 424                                     status | PR_CLOCK_STATUS_C_ENA_OFS,
 425                                     PR_CLOCK_STATUS_C);
 426
 427         /* Enable the drift correction process */
 428         hellcreek_ptp_write(hellcreek, status | PR_CLOCK_STATUS_C_ENA_DRIFT,
 429                             PR_CLOCK_STATUS_C);
 430
 431         /* LED setup */
 432         ret = hellcreek_led_setup(hellcreek);
 433         if (ret) {
 434                 if (hellcreek->ptp_clock)
 435                         ptp_clock_unregister(hellcreek->ptp_clock);
 436                 return ret;
 437         }
 438
 439         schedule_delayed_work(&hellcreek->overflow_work,
 440                               HELLCREEK_OVERFLOW_PERIOD);
 441
 442         return 0;
 443 }
 444
 445 void hellcreek_ptp_free(struct hellcreek *hellcreek)
 446 {
 447         led_classdev_unregister(&hellcreek->led_is_gm);
 448         led_classdev_unregister(&hellcreek->led_sync_good);
 449         cancel_delayed_work_sync(&hellcreek->overflow_work);
 450         if (hellcreek->ptp_clock)
 451                 ptp_clock_unregister(hellcreek->ptp_clock);
 452         hellcreek->ptp_clock = NULL;
 453 }