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