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 enum led_default_state state; 301 const char *label; 302 int ret = -EINVAL; 303 304 of_node_get(hellcreek->dev->of_node); 305 leds = of_find_node_by_name(hellcreek->dev->of_node, "leds"); 306 if (!leds) { 307 dev_err(hellcreek->dev, "No LEDs specified in device tree!\n"); 308 return ret; 309 } 310 311 hellcreek->status_out = 0; 312 313 led = of_get_next_available_child(leds, led); 314 if (!led) { 315 dev_err(hellcreek->dev, "First LED not specified!\n"); 316 goto out; 317 } 318 319 ret = of_property_read_string(led, "label", &label); 320 hellcreek->led_sync_good.name = ret ? "sync_good" : label; 321 322 state = led_init_default_state_get(of_fwnode_handle(led)); 323 switch (state) { 324 case LEDS_DEFSTATE_ON: 325 hellcreek->led_sync_good.brightness = 1; 326 break; 327 case LEDS_DEFSTATE_KEEP: 328 hellcreek->led_sync_good.brightness = 329 hellcreek_get_brightness(hellcreek, STATUS_OUT_SYNC_GOOD); 330 break; 331 default: 332 hellcreek->led_sync_good.brightness = 0; 333 } 334 335 hellcreek->led_sync_good.max_brightness = 1; 336 hellcreek->led_sync_good.brightness_set = hellcreek_led_sync_good_set; 337 hellcreek->led_sync_good.brightness_get = hellcreek_led_sync_good_get; 338 339 led = of_get_next_available_child(leds, led); 340 if (!led) { 341 dev_err(hellcreek->dev, "Second LED not specified!\n"); 342 ret = -EINVAL; 343 goto out; 344 } 345 346 ret = of_property_read_string(led, "label", &label); 347 hellcreek->led_is_gm.name = ret ? "is_gm" : label; 348 349 state = led_init_default_state_get(of_fwnode_handle(led)); 350 switch (state) { 351 case LEDS_DEFSTATE_ON: 352 hellcreek->led_is_gm.brightness = 1; 353 break; 354 case LEDS_DEFSTATE_KEEP: 355 hellcreek->led_is_gm.brightness = 356 hellcreek_get_brightness(hellcreek, STATUS_OUT_IS_GM); 357 break; 358 default: 359 hellcreek->led_is_gm.brightness = 0; 360 } 361 362 hellcreek->led_is_gm.max_brightness = 1; 363 hellcreek->led_is_gm.brightness_set = hellcreek_led_is_gm_set; 364 hellcreek->led_is_gm.brightness_get = hellcreek_led_is_gm_get; 365 366 /* Set initial state */ 367 if (hellcreek->led_sync_good.brightness == 1) 368 hellcreek_set_brightness(hellcreek, STATUS_OUT_SYNC_GOOD, 1); 369 if (hellcreek->led_is_gm.brightness == 1) 370 hellcreek_set_brightness(hellcreek, STATUS_OUT_IS_GM, 1); 371 372 /* Register both leds */ 373 led_classdev_register(hellcreek->dev, &hellcreek->led_sync_good); 374 led_classdev_register(hellcreek->dev, &hellcreek->led_is_gm); 375 376 ret = 0; 377 378 out: 379 of_node_put(leds); 380 381 return ret; 382 } 383 384 int hellcreek_ptp_setup(struct hellcreek *hellcreek) 385 { 386 u16 status; 387 int ret; 388 389 /* Set up the overflow work */ 390 INIT_DELAYED_WORK(&hellcreek->overflow_work, 391 hellcreek_ptp_overflow_check); 392 393 /* Setup PTP clock */ 394 hellcreek->ptp_clock_info.owner = THIS_MODULE; 395 snprintf(hellcreek->ptp_clock_info.name, 396 sizeof(hellcreek->ptp_clock_info.name), 397 dev_name(hellcreek->dev)); 398 399 /* IP-Core can add up to 0.5 ns per 8 ns cycle, which means 400 * accumulator_overflow_rate shall not exceed 62.5 MHz (which adjusts 401 * the nominal frequency by 6.25%) 402 */ 403 hellcreek->ptp_clock_info.max_adj = 62500000; 404 hellcreek->ptp_clock_info.n_alarm = 0; 405 hellcreek->ptp_clock_info.n_pins = 0; 406 hellcreek->ptp_clock_info.n_ext_ts = 0; 407 hellcreek->ptp_clock_info.n_per_out = 0; 408 hellcreek->ptp_clock_info.pps = 0; 409 hellcreek->ptp_clock_info.adjfine = hellcreek_ptp_adjfine; 410 hellcreek->ptp_clock_info.adjtime = hellcreek_ptp_adjtime; 411 hellcreek->ptp_clock_info.gettime64 = hellcreek_ptp_gettime; 412 hellcreek->ptp_clock_info.settime64 = hellcreek_ptp_settime; 413 hellcreek->ptp_clock_info.enable = hellcreek_ptp_enable; 414 hellcreek->ptp_clock_info.do_aux_work = hellcreek_hwtstamp_work; 415 416 hellcreek->ptp_clock = ptp_clock_register(&hellcreek->ptp_clock_info, 417 hellcreek->dev); 418 if (IS_ERR(hellcreek->ptp_clock)) 419 return PTR_ERR(hellcreek->ptp_clock); 420 421 /* Enable the offset correction process, if no offset correction is 422 * already taking place 423 */ 424 status = hellcreek_ptp_read(hellcreek, PR_CLOCK_STATUS_C); 425 if (!(status & PR_CLOCK_STATUS_C_OFS_ACT)) 426 hellcreek_ptp_write(hellcreek, 427 status | PR_CLOCK_STATUS_C_ENA_OFS, 428 PR_CLOCK_STATUS_C); 429 430 /* Enable the drift correction process */ 431 hellcreek_ptp_write(hellcreek, status | PR_CLOCK_STATUS_C_ENA_DRIFT, 432 PR_CLOCK_STATUS_C); 433 434 /* LED setup */ 435 ret = hellcreek_led_setup(hellcreek); 436 if (ret) { 437 if (hellcreek->ptp_clock) 438 ptp_clock_unregister(hellcreek->ptp_clock); 439 return ret; 440 } 441 442 schedule_delayed_work(&hellcreek->overflow_work, 443 HELLCREEK_OVERFLOW_PERIOD); 444 445 return 0; 446 } 447 448 void hellcreek_ptp_free(struct hellcreek *hellcreek) 449 { 450 led_classdev_unregister(&hellcreek->led_is_gm); 451 led_classdev_unregister(&hellcreek->led_sync_good); 452 cancel_delayed_work_sync(&hellcreek->overflow_work); 453 if (hellcreek->ptp_clock) 454 ptp_clock_unregister(hellcreek->ptp_clock); 455 hellcreek->ptp_clock = NULL; 456 } 457