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