drivers/net/dsa/hirschmann/hellcreek_hwtstamp.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 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 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 }