463 lines · 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 Offenburg7 * Copyright (C) 2019,2020 Linutronix GmbH8 * 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 18u16 hellcreek_ptp_read(struct hellcreek *hellcreek, unsigned int offset)19{20 return readw(hellcreek->ptp_base + offset);21}22 23void 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 */30static 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 hardware39 * limitations the seconds are not or only partly kept in the PTP40 * core. Currently only three bits for the seconds are available. That's41 * why only the nanoseconds are used and the seconds are tracked in42 * software. Anyway due to internal locking all five registers should be43 * 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 56static 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 packet72 * arrival and now. If so use the correct seconds (-1) for calculating the73 * packet arrival time.74 */75u64 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 else83 s = (hellcreek->seconds - 1) * NSEC_PER_SEC;84 85 return s;86}87 88static 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 104static 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 132static 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 ns145 * from the 8 ns (period of the oscillator) every time the accumulator146 * register overflows. The value stored in the addend register is added147 * to the accumulator register every 8 ns.148 *149 * addend value = (2^30 * accumulator_overflow_rate) /150 * oscillator_frequency151 * where:152 *153 * oscillator_frequency = 125 MHz154 * accumulator_overflow_rate = 125 MHz * scaled_ppm * 2^-16 * 10^-6 * 8155 */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 179static 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't186 * consider slow adjustment. Rather, add the offset directly to the187 * current time188 */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 227static int hellcreek_ptp_enable(struct ptp_clock_info *ptp,228 struct ptp_clock_request *rq, int on)229{230 return -EOPNOTSUPP;231}232 233static 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 248static enum led_brightness hellcreek_get_brightness(struct hellcreek *hellcreek,249 int led)250{251 return (hellcreek->status_out & led) ? 1 : 0;252}253 254static 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 else262 hellcreek->status_out &= ~led;263 264 hellcreek_ptp_write(hellcreek, hellcreek->status_out, STATUS_OUT);265 266 mutex_unlock(&hellcreek->ptp_lock);267}268 269static 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 277static 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 284static 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 292static 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, the300 * user might want to use a different label and specify the default state. Take301 * those properties from device tree.302 */303static 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 384out:385 of_node_put(leds);386 387 return ret;388}389 390int 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 means406 * accumulator_overflow_rate shall not exceed 62.5 MHz (which adjusts407 * 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 is428 * already taking place429 */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 454void 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}463