475 lines · c
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 Offenburg7 * Copyright (C) 2019,2020 Linutronix GmbH8 * 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 18int 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 is39 * not available in the switch. Thus, this function only serves as a check if40 * the user requested what is actually available or not41 */42static 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 is51 * enabled when this config function ends successfully52 */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 is104 * compatible with the hwtstamp offered by the switch. Therefore,105 * enable the interaction with the HW timestamping106 */107 set_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state);108 109 return 0;110}111 112int 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 136int 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 NULL151 * if the caller should not.152 */153static 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 171static u64 hellcreek_get_reserved_field(const struct ptp_header *hdr)172{173 return be32_to_cpu(hdr->reserved2);174}175 176static void hellcreek_clear_reserved_field(struct ptp_header *hdr)177{178 hdr->reserved2 = 0;179}180 181static 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 was193 * successfully read, and the one we need is not yet available194 */195 return (status & PR_TS_STATUS_TS_AVAIL) ? 1 : 0;196}197 198/* Get nanoseconds timestamp from timestamping unit */199static 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 213static 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 has244 * exceeded its allowed period245 */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 it254 * in high priority. Restart the work255 */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 correct265 * structure in order to send it to the user266 */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 another274 * timestampable transmit. We have to be ready for it by clearing the275 * ps->tx_skb "flag" beforehand276 */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 284free_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 292static 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 331static 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 342long 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 365void 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 timestamped381 * and the interaction with the HW timestamping is enabled. If not, stop382 * here383 */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 this401 * moment402 */403 ps->tx_tstamp_start = jiffies;404 405 ptp_schedule_worker(hellcreek->ptp_clock, 0);406}407 408bool 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 hardware418 * 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 timestamped424 * and the interaction with the HW timestamping is enabled. If not, stop425 * here426 */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 440static 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 448int 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 the462 * 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 471void hellcreek_hwtstamp_free(struct hellcreek *hellcreek)472{473 /* Nothing todo */474}475