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_classify.h> 13 14 #include "hellcreek.h" 15 #include "hellcreek_hwtstamp.h" 16 #include "hellcreek_ptp.h" 17 18 int hellcreek_get_ts_info(struct dsa_switch *ds, int port, 19 struct ethtool_ts_info *info) 20 { 21 struct hellcreek *hellcreek = ds->priv; 22 23 info->phc_index = hellcreek->ptp_clock ? 24 ptp_clock_index(hellcreek->ptp_clock) : -1; 25 info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE | 26 SOF_TIMESTAMPING_RX_HARDWARE | 27 SOF_TIMESTAMPING_RAW_HARDWARE; 28 29 /* enabled tx timestamping */ 30 info->tx_types = BIT(HWTSTAMP_TX_ON); 31 32 /* L2 & L4 PTPv2 event rx messages are timestamped */ 33 info->rx_filters = BIT(HWTSTAMP_FILTER_PTP_V2_EVENT); 34 35 return 0; 36 } 37 38 /* Enabling/disabling TX and RX HW timestamping for different PTP messages is 39 * not available in the switch. Thus, this function only serves as a check if 40 * the user requested what is actually available or not 41 */ 42 static int hellcreek_set_hwtstamp_config(struct hellcreek *hellcreek, int port, 43 struct hwtstamp_config *config) 44 { 45 struct hellcreek_port_hwtstamp *ps = 46 &hellcreek->ports[port].port_hwtstamp; 47 bool tx_tstamp_enable = false; 48 bool rx_tstamp_enable = false; 49 50 /* Interaction with the timestamp hardware is prevented here. It is 51 * enabled when this config function ends successfully 52 */ 53 clear_bit_unlock(HELLCREEK_HWTSTAMP_ENABLED, &ps->state); 54 55 /* Reserved for future extensions */ 56 if (config->flags) 57 return -EINVAL; 58 59 switch (config->tx_type) { 60 case HWTSTAMP_TX_ON: 61 tx_tstamp_enable = true; 62 break; 63 64 /* TX HW timestamping can't be disabled on the switch */ 65 case HWTSTAMP_TX_OFF: 66 config->tx_type = HWTSTAMP_TX_ON; 67 break; 68 69 default: 70 return -ERANGE; 71 } 72 73 switch (config->rx_filter) { 74 /* RX HW timestamping can't be disabled on the switch */ 75 case HWTSTAMP_FILTER_NONE: 76 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 77 break; 78 79 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 80 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 81 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 82 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 83 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 84 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 85 case HWTSTAMP_FILTER_PTP_V2_EVENT: 86 case HWTSTAMP_FILTER_PTP_V2_SYNC: 87 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 88 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 89 rx_tstamp_enable = true; 90 break; 91 92 /* RX HW timestamping can't be enabled for all messages on the switch */ 93 case HWTSTAMP_FILTER_ALL: 94 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 95 break; 96 97 default: 98 return -ERANGE; 99 } 100 101 if (!tx_tstamp_enable) 102 return -ERANGE; 103 104 if (!rx_tstamp_enable) 105 return -ERANGE; 106 107 /* If this point is reached, then the requested hwtstamp config is 108 * compatible with the hwtstamp offered by the switch. Therefore, 109 * enable the interaction with the HW timestamping 110 */ 111 set_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state); 112 113 return 0; 114 } 115 116 int hellcreek_port_hwtstamp_set(struct dsa_switch *ds, int port, 117 struct ifreq *ifr) 118 { 119 struct hellcreek *hellcreek = ds->priv; 120 struct hellcreek_port_hwtstamp *ps; 121 struct hwtstamp_config config; 122 int err; 123 124 ps = &hellcreek->ports[port].port_hwtstamp; 125 126 if (copy_from_user(&config, ifr->ifr_data, sizeof(config))) 127 return -EFAULT; 128 129 err = hellcreek_set_hwtstamp_config(hellcreek, port, &config); 130 if (err) 131 return err; 132 133 /* Save the chosen configuration to be returned later */ 134 memcpy(&ps->tstamp_config, &config, sizeof(config)); 135 136 return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? 137 -EFAULT : 0; 138 } 139 140 int hellcreek_port_hwtstamp_get(struct dsa_switch *ds, int port, 141 struct ifreq *ifr) 142 { 143 struct hellcreek *hellcreek = ds->priv; 144 struct hellcreek_port_hwtstamp *ps; 145 struct hwtstamp_config *config; 146 147 ps = &hellcreek->ports[port].port_hwtstamp; 148 config = &ps->tstamp_config; 149 150 return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ? 151 -EFAULT : 0; 152 } 153 154 /* Returns a pointer to the PTP header if the caller should time stamp, or NULL 155 * if the caller should not. 156 */ 157 static struct ptp_header *hellcreek_should_tstamp(struct hellcreek *hellcreek, 158 int port, struct sk_buff *skb, 159 unsigned int type) 160 { 161 struct hellcreek_port_hwtstamp *ps = 162 &hellcreek->ports[port].port_hwtstamp; 163 struct ptp_header *hdr; 164 165 hdr = ptp_parse_header(skb, type); 166 if (!hdr) 167 return NULL; 168 169 if (!test_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state)) 170 return NULL; 171 172 return hdr; 173 } 174 175 static u64 hellcreek_get_reserved_field(const struct ptp_header *hdr) 176 { 177 return be32_to_cpu(hdr->reserved2); 178 } 179 180 static void hellcreek_clear_reserved_field(struct ptp_header *hdr) 181 { 182 hdr->reserved2 = 0; 183 } 184 185 static int hellcreek_ptp_hwtstamp_available(struct hellcreek *hellcreek, 186 unsigned int ts_reg) 187 { 188 u16 status; 189 190 status = hellcreek_ptp_read(hellcreek, ts_reg); 191 192 if (status & PR_TS_STATUS_TS_LOST) 193 dev_err(hellcreek->dev, 194 "Tx time stamp lost! This should never happen!\n"); 195 196 /* If hwtstamp is not available, this means the previous hwtstamp was 197 * successfully read, and the one we need is not yet available 198 */ 199 return (status & PR_TS_STATUS_TS_AVAIL) ? 1 : 0; 200 } 201 202 /* Get nanoseconds timestamp from timestamping unit */ 203 static u64 hellcreek_ptp_hwtstamp_read(struct hellcreek *hellcreek, 204 unsigned int ts_reg) 205 { 206 u16 nsl, nsh; 207 208 nsh = hellcreek_ptp_read(hellcreek, ts_reg); 209 nsh = hellcreek_ptp_read(hellcreek, ts_reg); 210 nsh = hellcreek_ptp_read(hellcreek, ts_reg); 211 nsh = hellcreek_ptp_read(hellcreek, ts_reg); 212 nsl = hellcreek_ptp_read(hellcreek, ts_reg); 213 214 return (u64)nsl | ((u64)nsh << 16); 215 } 216 217 static int hellcreek_txtstamp_work(struct hellcreek *hellcreek, 218 struct hellcreek_port_hwtstamp *ps, int port) 219 { 220 struct skb_shared_hwtstamps shhwtstamps; 221 unsigned int status_reg, data_reg; 222 struct sk_buff *tmp_skb; 223 int ts_status; 224 u64 ns = 0; 225 226 if (!ps->tx_skb) 227 return 0; 228 229 switch (port) { 230 case 2: 231 status_reg = PR_TS_TX_P1_STATUS_C; 232 data_reg = PR_TS_TX_P1_DATA_C; 233 break; 234 case 3: 235 status_reg = PR_TS_TX_P2_STATUS_C; 236 data_reg = PR_TS_TX_P2_DATA_C; 237 break; 238 default: 239 dev_err(hellcreek->dev, "Wrong port for timestamping!\n"); 240 return 0; 241 } 242 243 ts_status = hellcreek_ptp_hwtstamp_available(hellcreek, status_reg); 244 245 /* Not available yet? */ 246 if (ts_status == 0) { 247 /* Check whether the operation of reading the tx timestamp has 248 * exceeded its allowed period 249 */ 250 if (time_is_before_jiffies(ps->tx_tstamp_start + 251 TX_TSTAMP_TIMEOUT)) { 252 dev_err(hellcreek->dev, 253 "Timeout while waiting for Tx timestamp!\n"); 254 goto free_and_clear_skb; 255 } 256 257 /* The timestamp should be available quickly, while getting it 258 * in high priority. Restart the work 259 */ 260 return 1; 261 } 262 263 mutex_lock(&hellcreek->ptp_lock); 264 ns = hellcreek_ptp_hwtstamp_read(hellcreek, data_reg); 265 ns += hellcreek_ptp_gettime_seconds(hellcreek, ns); 266 mutex_unlock(&hellcreek->ptp_lock); 267 268 /* Now we have the timestamp in nanoseconds, store it in the correct 269 * structure in order to send it to the user 270 */ 271 memset(&shhwtstamps, 0, sizeof(shhwtstamps)); 272 shhwtstamps.hwtstamp = ns_to_ktime(ns); 273 274 tmp_skb = ps->tx_skb; 275 ps->tx_skb = NULL; 276 277 /* skb_complete_tx_timestamp() frees up the client to make another 278 * timestampable transmit. We have to be ready for it by clearing the 279 * ps->tx_skb "flag" beforehand 280 */ 281 clear_bit_unlock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state); 282 283 /* Deliver a clone of the original outgoing tx_skb with tx hwtstamp */ 284 skb_complete_tx_timestamp(tmp_skb, &shhwtstamps); 285 286 return 0; 287 288 free_and_clear_skb: 289 dev_kfree_skb_any(ps->tx_skb); 290 ps->tx_skb = NULL; 291 clear_bit_unlock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state); 292 293 return 0; 294 } 295 296 static void hellcreek_get_rxts(struct hellcreek *hellcreek, 297 struct hellcreek_port_hwtstamp *ps, 298 struct sk_buff *skb, struct sk_buff_head *rxq, 299 int port) 300 { 301 struct skb_shared_hwtstamps *shwt; 302 struct sk_buff_head received; 303 unsigned long flags; 304 305 /* The latched timestamp belongs to one of the received frames. */ 306 __skb_queue_head_init(&received); 307 308 /* Lock & disable interrupts */ 309 spin_lock_irqsave(&rxq->lock, flags); 310 311 /* Add the reception queue "rxq" to the "received" queue an reintialize 312 * "rxq". From now on, we deal with "received" not with "rxq" 313 */ 314 skb_queue_splice_tail_init(rxq, &received); 315 316 spin_unlock_irqrestore(&rxq->lock, flags); 317 318 for (; skb; skb = __skb_dequeue(&received)) { 319 struct ptp_header *hdr; 320 unsigned int type; 321 u64 ns; 322 323 /* Get nanoseconds from ptp packet */ 324 type = SKB_PTP_TYPE(skb); 325 hdr = ptp_parse_header(skb, type); 326 ns = hellcreek_get_reserved_field(hdr); 327 hellcreek_clear_reserved_field(hdr); 328 329 /* Add seconds part */ 330 mutex_lock(&hellcreek->ptp_lock); 331 ns += hellcreek_ptp_gettime_seconds(hellcreek, ns); 332 mutex_unlock(&hellcreek->ptp_lock); 333 334 /* Save time stamp */ 335 shwt = skb_hwtstamps(skb); 336 memset(shwt, 0, sizeof(*shwt)); 337 shwt->hwtstamp = ns_to_ktime(ns); 338 netif_rx_ni(skb); 339 } 340 } 341 342 static void hellcreek_rxtstamp_work(struct hellcreek *hellcreek, 343 struct hellcreek_port_hwtstamp *ps, 344 int port) 345 { 346 struct sk_buff *skb; 347 348 skb = skb_dequeue(&ps->rx_queue); 349 if (skb) 350 hellcreek_get_rxts(hellcreek, ps, skb, &ps->rx_queue, port); 351 } 352 353 long hellcreek_hwtstamp_work(struct ptp_clock_info *ptp) 354 { 355 struct hellcreek *hellcreek = ptp_to_hellcreek(ptp); 356 struct dsa_switch *ds = hellcreek->ds; 357 int i, restart = 0; 358 359 for (i = 0; i < ds->num_ports; i++) { 360 struct hellcreek_port_hwtstamp *ps; 361 362 if (!dsa_is_user_port(ds, i)) 363 continue; 364 365 ps = &hellcreek->ports[i].port_hwtstamp; 366 367 if (test_bit(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state)) 368 restart |= hellcreek_txtstamp_work(hellcreek, ps, i); 369 370 hellcreek_rxtstamp_work(hellcreek, ps, i); 371 } 372 373 return restart ? 1 : -1; 374 } 375 376 void hellcreek_port_txtstamp(struct dsa_switch *ds, int port, 377 struct sk_buff *skb) 378 { 379 struct hellcreek *hellcreek = ds->priv; 380 struct hellcreek_port_hwtstamp *ps; 381 struct ptp_header *hdr; 382 struct sk_buff *clone; 383 unsigned int type; 384 385 ps = &hellcreek->ports[port].port_hwtstamp; 386 387 type = ptp_classify_raw(skb); 388 if (type == PTP_CLASS_NONE) 389 return; 390 391 /* Make sure the message is a PTP message that needs to be timestamped 392 * and the interaction with the HW timestamping is enabled. If not, stop 393 * here 394 */ 395 hdr = hellcreek_should_tstamp(hellcreek, port, skb, type); 396 if (!hdr) 397 return; 398 399 clone = skb_clone_sk(skb); 400 if (!clone) 401 return; 402 403 if (test_and_set_bit_lock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, 404 &ps->state)) { 405 kfree_skb(clone); 406 return; 407 } 408 409 ps->tx_skb = clone; 410 411 /* store the number of ticks occurred since system start-up till this 412 * moment 413 */ 414 ps->tx_tstamp_start = jiffies; 415 416 ptp_schedule_worker(hellcreek->ptp_clock, 0); 417 } 418 419 bool hellcreek_port_rxtstamp(struct dsa_switch *ds, int port, 420 struct sk_buff *skb, unsigned int type) 421 { 422 struct hellcreek *hellcreek = ds->priv; 423 struct hellcreek_port_hwtstamp *ps; 424 struct ptp_header *hdr; 425 426 ps = &hellcreek->ports[port].port_hwtstamp; 427 428 /* This check only fails if the user did not initialize hardware 429 * timestamping beforehand. 430 */ 431 if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT) 432 return false; 433 434 /* Make sure the message is a PTP message that needs to be timestamped 435 * and the interaction with the HW timestamping is enabled. If not, stop 436 * here 437 */ 438 hdr = hellcreek_should_tstamp(hellcreek, port, skb, type); 439 if (!hdr) 440 return false; 441 442 SKB_PTP_TYPE(skb) = type; 443 444 skb_queue_tail(&ps->rx_queue, skb); 445 446 ptp_schedule_worker(hellcreek->ptp_clock, 0); 447 448 return true; 449 } 450 451 static void hellcreek_hwtstamp_port_setup(struct hellcreek *hellcreek, int port) 452 { 453 struct hellcreek_port_hwtstamp *ps = 454 &hellcreek->ports[port].port_hwtstamp; 455 456 skb_queue_head_init(&ps->rx_queue); 457 } 458 459 int hellcreek_hwtstamp_setup(struct hellcreek *hellcreek) 460 { 461 struct dsa_switch *ds = hellcreek->ds; 462 int i; 463 464 /* Initialize timestamping ports. */ 465 for (i = 0; i < ds->num_ports; ++i) { 466 if (!dsa_is_user_port(ds, i)) 467 continue; 468 469 hellcreek_hwtstamp_port_setup(hellcreek, i); 470 } 471 472 /* Select the synchronized clock as the source timekeeper for the 473 * timestamps and enable inline timestamping. 474 */ 475 hellcreek_ptp_write(hellcreek, PR_SETTINGS_C_TS_SRC_TK_MASK | 476 PR_SETTINGS_C_RES3TS, 477 PR_SETTINGS_C); 478 479 return 0; 480 } 481 482 void hellcreek_hwtstamp_free(struct hellcreek *hellcreek) 483 { 484 /* Nothing todo */ 485 } 486