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