target/linux/layerscape/patches-5.4/701-net-0269-net-dsa-ocelot-add-hardware-timestamping-support-for.patch

   1 From 34bfb24b8ff5af09b014ea8530e1e8d89bb2a155 Mon Sep 17 00:00:00 2001
   2 From: Yangbo Lu <yangbo.lu@nxp.com>
   3 Date: Wed, 20 Nov 2019 16:23:18 +0800
   4 Subject: [PATCH] net: dsa: ocelot: add hardware timestamping support for Felix
   5
   6 This patch is to reuse ocelot functions as possible to enable PTP
   7 clock and to support hardware timestamping on Felix.
   8 On TX path, timestamping works on packet which requires timestamp.
   9 The injection header will be configured accordingly, and skb clone
  10 requires timestamp will be added into a list. The TX timestamp
  11 is final handled in threaded interrupt handler when PTP timestamp
  12 FIFO is ready.
  13 On RX path, timestamping is always working. The RX timestamp could
  14 be got from extraction header.
  15
  16 Signed-off-by: Yangbo Lu <yangbo.lu@nxp.com>
  17 Signed-off-by: David S. Miller <davem@davemloft.net>
  18 ---
  19  drivers/net/dsa/ocelot/felix.c | 89 ++++++++++++++++++++++++++++++++++++++++++
  20  net/dsa/tag_ocelot.c           | 14 ++++++-
  21  2 files changed, 102 insertions(+), 1 deletion(-)
  22
  23 --- a/drivers/net/dsa/ocelot/felix.c
  24 +++ b/drivers/net/dsa/ocelot/felix.c
  25 @@ -3,6 +3,7 @@
  26   */
  27  #include <uapi/linux/if_bridge.h>
  28  #include <soc/mscc/ocelot.h>
  29 +#include <linux/packing.h>
  30  #include <linux/module.h>
  31  #include <linux/pci.h>
  32  #include <linux/of.h>
  33 @@ -303,6 +304,62 @@ static void felix_teardown(struct dsa_sw
  34         ocelot_deinit(ocelot);
  35  }
  36
  37 +static int felix_hwtstamp_get(struct dsa_switch *ds, int port,
  38 +                             struct ifreq *ifr)
  39 +{
  40 +       struct ocelot *ocelot = ds->priv;
  41 +
  42 +       return ocelot_hwstamp_get(ocelot, port, ifr);
  43 +}
  44 +
  45 +static int felix_hwtstamp_set(struct dsa_switch *ds, int port,
  46 +                             struct ifreq *ifr)
  47 +{
  48 +       struct ocelot *ocelot = ds->priv;
  49 +
  50 +       return ocelot_hwstamp_set(ocelot, port, ifr);
  51 +}
  52 +
  53 +static bool felix_rxtstamp(struct dsa_switch *ds, int port,
  54 +                          struct sk_buff *skb, unsigned int type)
  55 +{
  56 +       struct skb_shared_hwtstamps *shhwtstamps;
  57 +       struct ocelot *ocelot = ds->priv;
  58 +       u8 *extraction = skb->data - ETH_HLEN - OCELOT_TAG_LEN;
  59 +       u32 tstamp_lo, tstamp_hi;
  60 +       struct timespec64 ts;
  61 +       u64 tstamp, val;
  62 +
  63 +       ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
  64 +       tstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
  65 +
  66 +       packing(extraction, &val,  116, 85, OCELOT_TAG_LEN, UNPACK, 0);
  67 +       tstamp_lo = (u32)val;
  68 +
  69 +       tstamp_hi = tstamp >> 32;
  70 +       if ((tstamp & 0xffffffff) < tstamp_lo)
  71 +               tstamp_hi--;
  72 +
  73 +       tstamp = ((u64)tstamp_hi << 32) | tstamp_lo;
  74 +
  75 +       shhwtstamps = skb_hwtstamps(skb);
  76 +       memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
  77 +       shhwtstamps->hwtstamp = tstamp;
  78 +       return false;
  79 +}
  80 +
  81 +bool felix_txtstamp(struct dsa_switch *ds, int port,
  82 +                   struct sk_buff *clone, unsigned int type)
  83 +{
  84 +       struct ocelot *ocelot = ds->priv;
  85 +       struct ocelot_port *ocelot_port = ocelot->ports[port];
  86 +
  87 +       if (!ocelot_port_add_txtstamp_skb(ocelot_port, clone))
  88 +               return true;
  89 +
  90 +       return false;
  91 +}
  92 +
  93  static const struct dsa_switch_ops felix_switch_ops = {
  94         .get_tag_protocol       = felix_get_tag_protocol,
  95         .setup                  = felix_setup,
  96 @@ -325,12 +382,33 @@ static const struct dsa_switch_ops felix
  97         .port_vlan_filtering    = felix_vlan_filtering,
  98         .port_vlan_add          = felix_vlan_add,
  99         .port_vlan_del          = felix_vlan_del,
 100 +       .port_hwtstamp_get      = felix_hwtstamp_get,
 101 +       .port_hwtstamp_set      = felix_hwtstamp_set,
 102 +       .port_rxtstamp          = felix_rxtstamp,
 103 +       .port_txtstamp          = felix_txtstamp,
 104  };
 105
 106  static struct felix_info *felix_instance_tbl[] = {
 107         [FELIX_INSTANCE_VSC9959] = &felix_info_vsc9959,
 108  };
 109
 110 +static irqreturn_t felix_irq_handler(int irq, void *data)
 111 +{
 112 +       struct ocelot *ocelot = (struct ocelot *)data;
 113 +
 114 +       /* The INTB interrupt is used for both PTP TX timestamp interrupt
 115 +        * and preemption status change interrupt on each port.
 116 +        *
 117 +        * - Get txtstamp if have
 118 +        * - TODO: handle preemption. Without handling it, driver may get
 119 +        *   interrupt storm.
 120 +        */
 121 +
 122 +       ocelot_get_txtstamp(ocelot);
 123 +
 124 +       return IRQ_HANDLED;
 125 +}
 126 +
 127  static int felix_pci_probe(struct pci_dev *pdev,
 128                            const struct pci_device_id *id)
 129  {
 130 @@ -372,6 +450,16 @@ static int felix_pci_probe(struct pci_de
 131
 132         pci_set_master(pdev);
 133
 134 +       err = devm_request_threaded_irq(&pdev->dev, pdev->irq, NULL,
 135 +                                       &felix_irq_handler, IRQF_ONESHOT,
 136 +                                       "felix-intb", ocelot);
 137 +       if (err) {
 138 +               dev_err(&pdev->dev, "Failed to request irq\n");
 139 +               goto err_alloc_irq;
 140 +       }
 141 +
 142 +       ocelot->ptp = 1;
 143 +
 144         ds = kzalloc(sizeof(struct dsa_switch), GFP_KERNEL);
 145         if (!ds) {
 146                 err = -ENOMEM;
 147 @@ -396,6 +484,7 @@ static int felix_pci_probe(struct pci_de
 148  err_register_ds:
 149         kfree(ds);
 150  err_alloc_ds:
 151 +err_alloc_irq:
 152  err_alloc_felix:
 153         kfree(felix);
 154  err_dma:
 155 --- a/net/dsa/tag_ocelot.c
 156 +++ b/net/dsa/tag_ocelot.c
 157 @@ -137,9 +137,11 @@ static struct sk_buff *ocelot_xmit(struc
 158                                    struct net_device *netdev)
 159  {
 160         struct dsa_port *dp = dsa_slave_to_port(netdev);
 161 -       u64 bypass, dest, src, qos_class;
 162 +       u64 bypass, dest, src, qos_class, rew_op;
 163         struct dsa_switch *ds = dp->ds;
 164         int port = dp->index;
 165 +       struct ocelot *ocelot = ds->priv;
 166 +       struct ocelot_port *ocelot_port = ocelot->ports[port];
 167         u8 *injection;
 168
 169         if (unlikely(skb_cow_head(skb, OCELOT_TAG_LEN) < 0)) {
 170 @@ -161,6 +163,16 @@ static struct sk_buff *ocelot_xmit(struc
 171         packing(injection, &src,       46,  43, OCELOT_TAG_LEN, PACK, 0);
 172         packing(injection, &qos_class, 19,  17, OCELOT_TAG_LEN, PACK, 0);
 173
 174 +       if (ocelot->ptp && (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
 175 +               rew_op = ocelot_port->ptp_cmd;
 176 +               if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
 177 +                       rew_op |= (ocelot_port->ts_id  % 4) << 3;
 178 +                       ocelot_port->ts_id++;
 179 +               }
 180 +
 181 +               packing(injection, &rew_op, 125, 117, OCELOT_TAG_LEN, PACK, 0);
 182 +       }
 183 +
 184         return skb;
 185  }
 186