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