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 /* Construct Rx timestamps for all received PTP packets. */ 302 __skb_queue_head_init(&received); 303 spin_lock_irqsave(&rxq->lock, flags); 304 skb_queue_splice_tail_init(rxq, &received); 305 spin_unlock_irqrestore(&rxq->lock, flags); 306 307 for (; skb; skb = __skb_dequeue(&received)) { 308 struct ptp_header *hdr; 309 unsigned int type; 310 u64 ns; 311 312 /* Get nanoseconds from ptp packet */ 313 type = SKB_PTP_TYPE(skb); 314 hdr = ptp_parse_header(skb, type); 315 ns = hellcreek_get_reserved_field(hdr); 316 hellcreek_clear_reserved_field(hdr); 317 318 /* Add seconds part */ 319 mutex_lock(&hellcreek->ptp_lock); 320 ns += hellcreek_ptp_gettime_seconds(hellcreek, ns); 321 mutex_unlock(&hellcreek->ptp_lock); 322 323 /* Save time stamp */ 324 shwt = skb_hwtstamps(skb); 325 memset(shwt, 0, sizeof(*shwt)); 326 shwt->hwtstamp = ns_to_ktime(ns); 327 netif_rx(skb); 328 } 329 } 330 331 static void hellcreek_rxtstamp_work(struct hellcreek *hellcreek, 332 struct hellcreek_port_hwtstamp *ps, 333 int port) 334 { 335 struct sk_buff *skb; 336 337 skb = skb_dequeue(&ps->rx_queue); 338 if (skb) 339 hellcreek_get_rxts(hellcreek, ps, skb, &ps->rx_queue, port); 340 } 341 342 long hellcreek_hwtstamp_work(struct ptp_clock_info *ptp) 343 { 344 struct hellcreek *hellcreek = ptp_to_hellcreek(ptp); 345 struct dsa_switch *ds = hellcreek->ds; 346 int i, restart = 0; 347 348 for (i = 0; i < ds->num_ports; i++) { 349 struct hellcreek_port_hwtstamp *ps; 350 351 if (!dsa_is_user_port(ds, i)) 352 continue; 353 354 ps = &hellcreek->ports[i].port_hwtstamp; 355 356 if (test_bit(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state)) 357 restart |= hellcreek_txtstamp_work(hellcreek, ps, i); 358 359 hellcreek_rxtstamp_work(hellcreek, ps, i); 360 } 361 362 return restart ? 1 : -1; 363 } 364 365 void hellcreek_port_txtstamp(struct dsa_switch *ds, int port, 366 struct sk_buff *skb) 367 { 368 struct hellcreek *hellcreek = ds->priv; 369 struct hellcreek_port_hwtstamp *ps; 370 struct ptp_header *hdr; 371 struct sk_buff *clone; 372 unsigned int type; 373 374 ps = &hellcreek->ports[port].port_hwtstamp; 375 376 type = ptp_classify_raw(skb); 377 if (type == PTP_CLASS_NONE) 378 return; 379 380 /* Make sure the message is a PTP message that needs to be timestamped 381 * and the interaction with the HW timestamping is enabled. If not, stop 382 * here 383 */ 384 hdr = hellcreek_should_tstamp(hellcreek, port, skb, type); 385 if (!hdr) 386 return; 387 388 clone = skb_clone_sk(skb); 389 if (!clone) 390 return; 391 392 if (test_and_set_bit_lock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, 393 &ps->state)) { 394 kfree_skb(clone); 395 return; 396 } 397 398 ps->tx_skb = clone; 399 400 /* store the number of ticks occurred since system start-up till this 401 * moment 402 */ 403 ps->tx_tstamp_start = jiffies; 404 405 ptp_schedule_worker(hellcreek->ptp_clock, 0); 406 } 407 408 bool hellcreek_port_rxtstamp(struct dsa_switch *ds, int port, 409 struct sk_buff *skb, unsigned int type) 410 { 411 struct hellcreek *hellcreek = ds->priv; 412 struct hellcreek_port_hwtstamp *ps; 413 struct ptp_header *hdr; 414 415 ps = &hellcreek->ports[port].port_hwtstamp; 416 417 /* This check only fails if the user did not initialize hardware 418 * timestamping beforehand. 419 */ 420 if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT) 421 return false; 422 423 /* Make sure the message is a PTP message that needs to be timestamped 424 * and the interaction with the HW timestamping is enabled. If not, stop 425 * here 426 */ 427 hdr = hellcreek_should_tstamp(hellcreek, port, skb, type); 428 if (!hdr) 429 return false; 430 431 SKB_PTP_TYPE(skb) = type; 432 433 skb_queue_tail(&ps->rx_queue, skb); 434 435 ptp_schedule_worker(hellcreek->ptp_clock, 0); 436 437 return true; 438 } 439 440 static void hellcreek_hwtstamp_port_setup(struct hellcreek *hellcreek, int port) 441 { 442 struct hellcreek_port_hwtstamp *ps = 443 &hellcreek->ports[port].port_hwtstamp; 444 445 skb_queue_head_init(&ps->rx_queue); 446 } 447 448 int hellcreek_hwtstamp_setup(struct hellcreek *hellcreek) 449 { 450 struct dsa_switch *ds = hellcreek->ds; 451 int i; 452 453 /* Initialize timestamping ports. */ 454 for (i = 0; i < ds->num_ports; ++i) { 455 if (!dsa_is_user_port(ds, i)) 456 continue; 457 458 hellcreek_hwtstamp_port_setup(hellcreek, i); 459 } 460 461 /* Select the synchronized clock as the source timekeeper for the 462 * timestamps and enable inline timestamping. 463 */ 464 hellcreek_ptp_write(hellcreek, PR_SETTINGS_C_TS_SRC_TK_MASK | 465 PR_SETTINGS_C_RES3TS, 466 PR_SETTINGS_C); 467 468 return 0; 469 } 470 471 void hellcreek_hwtstamp_free(struct hellcreek *hellcreek) 472 { 473 /* Nothing todo */ 474 } 475