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 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 } 463