dsa/hirschmann/hellcreek_hwtstamp.c

0001 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
0002 /*
0003  * DSA driver for:
0004  * Hirschmann Hellcreek TSN switch.
0005  *
0006  * Copyright (C) 2019,2020 Hochschule Offenburg
0007  * Copyright (C) 2019,2020 Linutronix GmbH
0008  * Authors: Kamil Alkhouri <kamil.alkhouri@hs-offenburg.de>
0009  *      Kurt Kanzenbach <kurt@linutronix.de>
0010  */
0011
0012 #include <linux/ptp_classify.h>
0013
0014 #include "hellcreek.h"
0015 #include "hellcreek_hwtstamp.h"
0016 #include "hellcreek_ptp.h"
0017
0018 int hellcreek_get_ts_info(struct dsa_switch *ds, int port,
0019               struct ethtool_ts_info *info)
0020 {
0021     struct hellcreek *hellcreek = ds->priv;
0022
0023     info->phc_index = hellcreek->ptp_clock ?
0024         ptp_clock_index(hellcreek->ptp_clock) : -1;
0025     info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
0026         SOF_TIMESTAMPING_RX_HARDWARE |
0027         SOF_TIMESTAMPING_RAW_HARDWARE;
0028
0029     /* enabled tx timestamping */
0030     info->tx_types = BIT(HWTSTAMP_TX_ON);
0031
0032     /* L2 & L4 PTPv2 event rx messages are timestamped */
0033     info->rx_filters = BIT(HWTSTAMP_FILTER_PTP_V2_EVENT);
0034
0035     return 0;
0036 }
0037
0038 /* Enabling/disabling TX and RX HW timestamping for different PTP messages is
0039  * not available in the switch. Thus, this function only serves as a check if
0040  * the user requested what is actually available or not
0041  */
0042 static int hellcreek_set_hwtstamp_config(struct hellcreek *hellcreek, int port,
0043                      struct hwtstamp_config *config)
0044 {
0045     struct hellcreek_port_hwtstamp *ps =
0046         &hellcreek->ports[port].port_hwtstamp;
0047     bool tx_tstamp_enable = false;
0048     bool rx_tstamp_enable = false;
0049
0050     /* Interaction with the timestamp hardware is prevented here.  It is
0051      * enabled when this config function ends successfully
0052      */
0053     clear_bit_unlock(HELLCREEK_HWTSTAMP_ENABLED, &ps->state);
0054
0055     switch (config->tx_type) {
0056     case HWTSTAMP_TX_ON:
0057         tx_tstamp_enable = true;
0058         break;
0059
0060     /* TX HW timestamping can't be disabled on the switch */
0061     case HWTSTAMP_TX_OFF:
0062         config->tx_type = HWTSTAMP_TX_ON;
0063         break;
0064
0065     default:
0066         return -ERANGE;
0067     }
0068
0069     switch (config->rx_filter) {
0070     /* RX HW timestamping can't be disabled on the switch */
0071     case HWTSTAMP_FILTER_NONE:
0072         config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
0073         break;
0074
0075     case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
0076     case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
0077     case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
0078     case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
0079     case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
0080     case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
0081     case HWTSTAMP_FILTER_PTP_V2_EVENT:
0082     case HWTSTAMP_FILTER_PTP_V2_SYNC:
0083     case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
0084         config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
0085         rx_tstamp_enable = true;
0086         break;
0087
0088     /* RX HW timestamping can't be enabled for all messages on the switch */
0089     case HWTSTAMP_FILTER_ALL:
0090         config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
0091         break;
0092
0093     default:
0094         return -ERANGE;
0095     }
0096
0097     if (!tx_tstamp_enable)
0098         return -ERANGE;
0099
0100     if (!rx_tstamp_enable)
0101         return -ERANGE;
0102
0103     /* If this point is reached, then the requested hwtstamp config is
0104      * compatible with the hwtstamp offered by the switch.  Therefore,
0105      * enable the interaction with the HW timestamping
0106      */
0107     set_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state);
0108
0109     return 0;
0110 }
0111
0112 int hellcreek_port_hwtstamp_set(struct dsa_switch *ds, int port,
0113                 struct ifreq *ifr)
0114 {
0115     struct hellcreek *hellcreek = ds->priv;
0116     struct hellcreek_port_hwtstamp *ps;
0117     struct hwtstamp_config config;
0118     int err;
0119
0120     ps = &hellcreek->ports[port].port_hwtstamp;
0121
0122     if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
0123         return -EFAULT;
0124
0125     err = hellcreek_set_hwtstamp_config(hellcreek, port, &config);
0126     if (err)
0127         return err;
0128
0129     /* Save the chosen configuration to be returned later */
0130     memcpy(&ps->tstamp_config, &config, sizeof(config));
0131
0132     return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
0133         -EFAULT : 0;
0134 }
0135
0136 int hellcreek_port_hwtstamp_get(struct dsa_switch *ds, int port,
0137                 struct ifreq *ifr)
0138 {
0139     struct hellcreek *hellcreek = ds->priv;
0140     struct hellcreek_port_hwtstamp *ps;
0141     struct hwtstamp_config *config;
0142
0143     ps = &hellcreek->ports[port].port_hwtstamp;
0144     config = &ps->tstamp_config;
0145
0146     return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ?
0147         -EFAULT : 0;
0148 }
0149
0150 /* Returns a pointer to the PTP header if the caller should time stamp, or NULL
0151  * if the caller should not.
0152  */
0153 static struct ptp_header *hellcreek_should_tstamp(struct hellcreek *hellcreek,
0154                           int port, struct sk_buff *skb,
0155                           unsigned int type)
0156 {
0157     struct hellcreek_port_hwtstamp *ps =
0158         &hellcreek->ports[port].port_hwtstamp;
0159     struct ptp_header *hdr;
0160
0161     hdr = ptp_parse_header(skb, type);
0162     if (!hdr)
0163         return NULL;
0164
0165     if (!test_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state))
0166         return NULL;
0167
0168     return hdr;
0169 }
0170
0171 static u64 hellcreek_get_reserved_field(const struct ptp_header *hdr)
0172 {
0173     return be32_to_cpu(hdr->reserved2);
0174 }
0175
0176 static void hellcreek_clear_reserved_field(struct ptp_header *hdr)
0177 {
0178     hdr->reserved2 = 0;
0179 }
0180
0181 static int hellcreek_ptp_hwtstamp_available(struct hellcreek *hellcreek,
0182                         unsigned int ts_reg)
0183 {
0184     u16 status;
0185
0186     status = hellcreek_ptp_read(hellcreek, ts_reg);
0187
0188     if (status & PR_TS_STATUS_TS_LOST)
0189         dev_err(hellcreek->dev,
0190             "Tx time stamp lost! This should never happen!\n");
0191
0192     /* If hwtstamp is not available, this means the previous hwtstamp was
0193      * successfully read, and the one we need is not yet available
0194      */
0195     return (status & PR_TS_STATUS_TS_AVAIL) ? 1 : 0;
0196 }
0197
0198 /* Get nanoseconds timestamp from timestamping unit */
0199 static u64 hellcreek_ptp_hwtstamp_read(struct hellcreek *hellcreek,
0200                        unsigned int ts_reg)
0201 {
0202     u16 nsl, nsh;
0203
0204     nsh = hellcreek_ptp_read(hellcreek, ts_reg);
0205     nsh = hellcreek_ptp_read(hellcreek, ts_reg);
0206     nsh = hellcreek_ptp_read(hellcreek, ts_reg);
0207     nsh = hellcreek_ptp_read(hellcreek, ts_reg);
0208     nsl = hellcreek_ptp_read(hellcreek, ts_reg);
0209
0210     return (u64)nsl | ((u64)nsh << 16);
0211 }
0212
0213 static int hellcreek_txtstamp_work(struct hellcreek *hellcreek,
0214                    struct hellcreek_port_hwtstamp *ps, int port)
0215 {
0216     struct skb_shared_hwtstamps shhwtstamps;
0217     unsigned int status_reg, data_reg;
0218     struct sk_buff *tmp_skb;
0219     int ts_status;
0220     u64 ns = 0;
0221
0222     if (!ps->tx_skb)
0223         return 0;
0224
0225     switch (port) {
0226     case 2:
0227         status_reg = PR_TS_TX_P1_STATUS_C;
0228         data_reg   = PR_TS_TX_P1_DATA_C;
0229         break;
0230     case 3:
0231         status_reg = PR_TS_TX_P2_STATUS_C;
0232         data_reg   = PR_TS_TX_P2_DATA_C;
0233         break;
0234     default:
0235         dev_err(hellcreek->dev, "Wrong port for timestamping!\n");
0236         return 0;
0237     }
0238
0239     ts_status = hellcreek_ptp_hwtstamp_available(hellcreek, status_reg);
0240
0241     /* Not available yet? */
0242     if (ts_status == 0) {
0243         /* Check whether the operation of reading the tx timestamp has
0244          * exceeded its allowed period
0245          */
0246         if (time_is_before_jiffies(ps->tx_tstamp_start +
0247                        TX_TSTAMP_TIMEOUT)) {
0248             dev_err(hellcreek->dev,
0249                 "Timeout while waiting for Tx timestamp!\n");
0250             goto free_and_clear_skb;
0251         }
0252
0253         /* The timestamp should be available quickly, while getting it
0254          * in high priority. Restart the work
0255          */
0256         return 1;
0257     }
0258
0259     mutex_lock(&hellcreek->ptp_lock);
0260     ns  = hellcreek_ptp_hwtstamp_read(hellcreek, data_reg);
0261     ns += hellcreek_ptp_gettime_seconds(hellcreek, ns);
0262     mutex_unlock(&hellcreek->ptp_lock);
0263
0264     /* Now we have the timestamp in nanoseconds, store it in the correct
0265      * structure in order to send it to the user
0266      */
0267     memset(&shhwtstamps, 0, sizeof(shhwtstamps));
0268     shhwtstamps.hwtstamp = ns_to_ktime(ns);
0269
0270     tmp_skb = ps->tx_skb;
0271     ps->tx_skb = NULL;
0272
0273     /* skb_complete_tx_timestamp() frees up the client to make another
0274      * timestampable transmit.  We have to be ready for it by clearing the
0275      * ps->tx_skb "flag" beforehand
0276      */
0277     clear_bit_unlock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
0278
0279     /* Deliver a clone of the original outgoing tx_skb with tx hwtstamp */
0280     skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);
0281
0282     return 0;
0283
0284 free_and_clear_skb:
0285     dev_kfree_skb_any(ps->tx_skb);
0286     ps->tx_skb = NULL;
0287     clear_bit_unlock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
0288
0289     return 0;
0290 }
0291
0292 static void hellcreek_get_rxts(struct hellcreek *hellcreek,
0293                    struct hellcreek_port_hwtstamp *ps,
0294                    struct sk_buff *skb, struct sk_buff_head *rxq,
0295                    int port)
0296 {
0297     struct skb_shared_hwtstamps *shwt;
0298     struct sk_buff_head received;
0299     unsigned long flags;
0300
0301     /* The latched timestamp belongs to one of the received frames. */
0302     __skb_queue_head_init(&received);
0303
0304     /* Lock & disable interrupts */
0305     spin_lock_irqsave(&rxq->lock, flags);
0306
0307     /* Add the reception queue "rxq" to the "received" queue an reintialize
0308      * "rxq".  From now on, we deal with "received" not with "rxq"
0309      */
0310     skb_queue_splice_tail_init(rxq, &received);
0311
0312     spin_unlock_irqrestore(&rxq->lock, flags);
0313
0314     for (; skb; skb = __skb_dequeue(&received)) {
0315         struct ptp_header *hdr;
0316         unsigned int type;
0317         u64 ns;
0318
0319         /* Get nanoseconds from ptp packet */
0320         type = SKB_PTP_TYPE(skb);
0321         hdr  = ptp_parse_header(skb, type);
0322         ns   = hellcreek_get_reserved_field(hdr);
0323         hellcreek_clear_reserved_field(hdr);
0324
0325         /* Add seconds part */
0326         mutex_lock(&hellcreek->ptp_lock);
0327         ns += hellcreek_ptp_gettime_seconds(hellcreek, ns);
0328         mutex_unlock(&hellcreek->ptp_lock);
0329
0330         /* Save time stamp */
0331         shwt = skb_hwtstamps(skb);
0332         memset(shwt, 0, sizeof(*shwt));
0333         shwt->hwtstamp = ns_to_ktime(ns);
0334         netif_rx(skb);
0335     }
0336 }
0337
0338 static void hellcreek_rxtstamp_work(struct hellcreek *hellcreek,
0339                     struct hellcreek_port_hwtstamp *ps,
0340                     int port)
0341 {
0342     struct sk_buff *skb;
0343
0344     skb = skb_dequeue(&ps->rx_queue);
0345     if (skb)
0346         hellcreek_get_rxts(hellcreek, ps, skb, &ps->rx_queue, port);
0347 }
0348
0349 long hellcreek_hwtstamp_work(struct ptp_clock_info *ptp)
0350 {
0351     struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
0352     struct dsa_switch *ds = hellcreek->ds;
0353     int i, restart = 0;
0354
0355     for (i = 0; i < ds->num_ports; i++) {
0356         struct hellcreek_port_hwtstamp *ps;
0357
0358         if (!dsa_is_user_port(ds, i))
0359             continue;
0360
0361         ps = &hellcreek->ports[i].port_hwtstamp;
0362
0363         if (test_bit(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
0364             restart |= hellcreek_txtstamp_work(hellcreek, ps, i);
0365
0366         hellcreek_rxtstamp_work(hellcreek, ps, i);
0367     }
0368
0369     return restart ? 1 : -1;
0370 }
0371
0372 void hellcreek_port_txtstamp(struct dsa_switch *ds, int port,
0373                  struct sk_buff *skb)
0374 {
0375     struct hellcreek *hellcreek = ds->priv;
0376     struct hellcreek_port_hwtstamp *ps;
0377     struct ptp_header *hdr;
0378     struct sk_buff *clone;
0379     unsigned int type;
0380
0381     ps = &hellcreek->ports[port].port_hwtstamp;
0382
0383     type = ptp_classify_raw(skb);
0384     if (type == PTP_CLASS_NONE)
0385         return;
0386
0387     /* Make sure the message is a PTP message that needs to be timestamped
0388      * and the interaction with the HW timestamping is enabled. If not, stop
0389      * here
0390      */
0391     hdr = hellcreek_should_tstamp(hellcreek, port, skb, type);
0392     if (!hdr)
0393         return;
0394
0395     clone = skb_clone_sk(skb);
0396     if (!clone)
0397         return;
0398
0399     if (test_and_set_bit_lock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS,
0400                   &ps->state)) {
0401         kfree_skb(clone);
0402         return;
0403     }
0404
0405     ps->tx_skb = clone;
0406
0407     /* store the number of ticks occurred since system start-up till this
0408      * moment
0409      */
0410     ps->tx_tstamp_start = jiffies;
0411
0412     ptp_schedule_worker(hellcreek->ptp_clock, 0);
0413 }
0414
0415 bool hellcreek_port_rxtstamp(struct dsa_switch *ds, int port,
0416                  struct sk_buff *skb, unsigned int type)
0417 {
0418     struct hellcreek *hellcreek = ds->priv;
0419     struct hellcreek_port_hwtstamp *ps;
0420     struct ptp_header *hdr;
0421
0422     ps = &hellcreek->ports[port].port_hwtstamp;
0423
0424     /* This check only fails if the user did not initialize hardware
0425      * timestamping beforehand.
0426      */
0427     if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
0428         return false;
0429
0430     /* Make sure the message is a PTP message that needs to be timestamped
0431      * and the interaction with the HW timestamping is enabled. If not, stop
0432      * here
0433      */
0434     hdr = hellcreek_should_tstamp(hellcreek, port, skb, type);
0435     if (!hdr)
0436         return false;
0437
0438     SKB_PTP_TYPE(skb) = type;
0439
0440     skb_queue_tail(&ps->rx_queue, skb);
0441
0442     ptp_schedule_worker(hellcreek->ptp_clock, 0);
0443
0444     return true;
0445 }
0446
0447 static void hellcreek_hwtstamp_port_setup(struct hellcreek *hellcreek, int port)
0448 {
0449     struct hellcreek_port_hwtstamp *ps =
0450         &hellcreek->ports[port].port_hwtstamp;
0451
0452     skb_queue_head_init(&ps->rx_queue);
0453 }
0454
0455 int hellcreek_hwtstamp_setup(struct hellcreek *hellcreek)
0456 {
0457     struct dsa_switch *ds = hellcreek->ds;
0458     int i;
0459
0460     /* Initialize timestamping ports. */
0461     for (i = 0; i < ds->num_ports; ++i) {
0462         if (!dsa_is_user_port(ds, i))
0463             continue;
0464
0465         hellcreek_hwtstamp_port_setup(hellcreek, i);
0466     }
0467
0468     /* Select the synchronized clock as the source timekeeper for the
0469      * timestamps and enable inline timestamping.
0470      */
0471     hellcreek_ptp_write(hellcreek, PR_SETTINGS_C_TS_SRC_TK_MASK |
0472                 PR_SETTINGS_C_RES3TS,
0473                 PR_SETTINGS_C);
0474
0475     return 0;
0476 }
0477
0478 void hellcreek_hwtstamp_free(struct hellcreek *hellcreek)
0479 {
0480     /* Nothing todo */
0481 }