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