net/dsa/tag_ocelot.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright 2019 NXP Semiconductors
   3  */
   4 #include <soc/mscc/ocelot.h>
   5 #include <linux/packing.h>
   6 #include "dsa_priv.h"
   7
   8 /* The CPU injection header and the CPU extraction header can have 3 types of
   9  * prefixes: long, short and no prefix. The format of the header itself is the
  10  * same in all 3 cases.
  11  *
  12  * Extraction with long prefix:
  13  *
  14  * +-------------------+-------------------+------+------+------------+-------+
  15  * | ff:ff:ff:ff:ff:ff | ff:ff:ff:ff:ff:ff | 8880 | 000a | extraction | frame |
  16  * |                   |                   |      |      |   header   |       |
  17  * +-------------------+-------------------+------+------+------------+-------+
  18  *        48 bits             48 bits      16 bits 16 bits  128 bits
  19  *
  20  * Extraction with short prefix:
  21  *
  22  *                                         +------+------+------------+-------+
  23  *                                         | 8880 | 000a | extraction | frame |
  24  *                                         |      |      |   header   |       |
  25  *                                         +------+------+------------+-------+
  26  *                                         16 bits 16 bits  128 bits
  27  *
  28  * Extraction with no prefix:
  29  *
  30  *                                                       +------------+-------+
  31  *                                                       | extraction | frame |
  32  *                                                       |   header   |       |
  33  *                                                       +------------+-------+
  34  *                                                          128 bits
  35  *
  36  *
  37  * Injection with long prefix:
  38  *
  39  * +-------------------+-------------------+------+------+------------+-------+
  40  * |      any dmac     |      any smac     | 8880 | 000a | injection  | frame |
  41  * |                   |                   |      |      |   header   |       |
  42  * +-------------------+-------------------+------+------+------------+-------+
  43  *        48 bits             48 bits      16 bits 16 bits  128 bits
  44  *
  45  * Injection with short prefix:
  46  *
  47  *                                         +------+------+------------+-------+
  48  *                                         | 8880 | 000a | injection  | frame |
  49  *                                         |      |      |   header   |       |
  50  *                                         +------+------+------------+-------+
  51  *                                         16 bits 16 bits  128 bits
  52  *
  53  * Injection with no prefix:
  54  *
  55  *                                                       +------------+-------+
  56  *                                                       | injection  | frame |
  57  *                                                       |   header   |       |
  58  *                                                       +------------+-------+
  59  *                                                          128 bits
  60  *
  61  * The injection header looks like this (network byte order, bit 127
  62  * is part of lowest address byte in memory, bit 0 is part of highest
  63  * address byte):
  64  *
  65  *         +------+------+------+------+------+------+------+------+
  66  * 127:120 |BYPASS| MASQ |          MASQ_PORT        |REW_OP|REW_OP|
  67  *         +------+------+------+------+------+------+------+------+
  68  * 119:112 |                         REW_OP                        |
  69  *         +------+------+------+------+------+------+------+------+
  70  * 111:104 |                         REW_VAL                       |
  71  *         +------+------+------+------+------+------+------+------+
  72  * 103: 96 |                         REW_VAL                       |
  73  *         +------+------+------+------+------+------+------+------+
  74  *  95: 88 |                         REW_VAL                       |
  75  *         +------+------+------+------+------+------+------+------+
  76  *  87: 80 |                         REW_VAL                       |
  77  *         +------+------+------+------+------+------+------+------+
  78  *  79: 72 |                          RSV                          |
  79  *         +------+------+------+------+------+------+------+------+
  80  *  71: 64 |            RSV            |           DEST            |
  81  *         +------+------+------+------+------+------+------+------+
  82  *  63: 56 |                         DEST                          |
  83  *         +------+------+------+------+------+------+------+------+
  84  *  55: 48 |                          RSV                          |
  85  *         +------+------+------+------+------+------+------+------+
  86  *  47: 40 |  RSV |         SRC_PORT          |     RSV     |TFRM_TIMER|
  87  *         +------+------+------+------+------+------+------+------+
  88  *  39: 32 |     TFRM_TIMER     |               RSV                |
  89  *         +------+------+------+------+------+------+------+------+
  90  *  31: 24 |  RSV |  DP  |   POP_CNT   |           CPUQ            |
  91  *         +------+------+------+------+------+------+------+------+
  92  *  23: 16 |           CPUQ            |      QOS_CLASS     |TAG_TYPE|
  93  *         +------+------+------+------+------+------+------+------+
  94  *  15:  8 |         PCP        |  DEI |            VID            |
  95  *         +------+------+------+------+------+------+------+------+
  96  *   7:  0 |                          VID                          |
  97  *         +------+------+------+------+------+------+------+------+
  98  *
  99  * And the extraction header looks like this:
 100  *
 101  *         +------+------+------+------+------+------+------+------+
 102  * 127:120 |  RSV |                  REW_OP                        |
 103  *         +------+------+------+------+------+------+------+------+
 104  * 119:112 |       REW_OP       |              REW_VAL             |
 105  *         +------+------+------+------+------+------+------+------+
 106  * 111:104 |                         REW_VAL                       |
 107  *         +------+------+------+------+------+------+------+------+
 108  * 103: 96 |                         REW_VAL                       |
 109  *         +------+------+------+------+------+------+------+------+
 110  *  95: 88 |                         REW_VAL                       |
 111  *         +------+------+------+------+------+------+------+------+
 112  *  87: 80 |       REW_VAL      |               LLEN               |
 113  *         +------+------+------+------+------+------+------+------+
 114  *  79: 72 | LLEN |                      WLEN                      |
 115  *         +------+------+------+------+------+------+------+------+
 116  *  71: 64 | WLEN |                      RSV                       |
 117  *         +------+------+------+------+------+------+------+------+
 118  *  63: 56 |                          RSV                          |
 119  *         +------+------+------+------+------+------+------+------+
 120  *  55: 48 |                          RSV                          |
 121  *         +------+------+------+------+------+------+------+------+
 122  *  47: 40 | RSV  |          SRC_PORT         |       ACL_ID       |
 123  *         +------+------+------+------+------+------+------+------+
 124  *  39: 32 |       ACL_ID       |  RSV |         SFLOW_ID          |
 125  *         +------+------+------+------+------+------+------+------+
 126  *  31: 24 |ACL_HIT| DP  |  LRN_FLAGS  |           CPUQ            |
 127  *         +------+------+------+------+------+------+------+------+
 128  *  23: 16 |           CPUQ            |      QOS_CLASS     |TAG_TYPE|
 129  *         +------+------+------+------+------+------+------+------+
 130  *  15:  8 |         PCP        |  DEI |            VID            |
 131  *         +------+------+------+------+------+------+------+------+
 132  *   7:  0 |                          VID                          |
 133  *         +------+------+------+------+------+------+------+------+
 134  */
 135
 136 static struct sk_buff *ocelot_xmit(struct sk_buff *skb,
 137                                    struct net_device *netdev)
 138 {
 139         struct dsa_port *dp = dsa_slave_to_port(netdev);
 140         u64 bypass, dest, src, qos_class, rew_op;
 141         struct dsa_switch *ds = dp->ds;
 142         int port = dp->index;
 143         struct ocelot *ocelot = ds->priv;
 144         struct ocelot_port *ocelot_port = ocelot->ports[port];
 145         u8 *injection;
 146
 147         if (unlikely(skb_cow_head(skb, OCELOT_TAG_LEN) < 0)) {
 148                 netdev_err(netdev, "Cannot make room for tag.\n");
 149                 return NULL;
 150         }
 151
 152         injection = skb_push(skb, OCELOT_TAG_LEN);
 153
 154         memset(injection, 0, OCELOT_TAG_LEN);
 155
 156         src = dsa_upstream_port(ds, port);
 157         dest = BIT(port);
 158         bypass = true;
 159         qos_class = skb->priority;
 160
 161         packing(injection, &bypass,   127, 127, OCELOT_TAG_LEN, PACK, 0);
 162         packing(injection, &dest,      68,  56, OCELOT_TAG_LEN, PACK, 0);
 163         packing(injection, &src,       46,  43, OCELOT_TAG_LEN, PACK, 0);
 164         packing(injection, &qos_class, 19,  17, OCELOT_TAG_LEN, PACK, 0);
 165
 166         if (ocelot->ptp && (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
 167                 rew_op = ocelot_port->ptp_cmd;
 168                 if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
 169                         rew_op |= (ocelot_port->ts_id  % 4) << 3;
 170                         ocelot_port->ts_id++;
 171                 }
 172
 173                 packing(injection, &rew_op, 125, 117, OCELOT_TAG_LEN, PACK, 0);
 174         }
 175
 176         return skb;
 177 }
 178
 179 static struct sk_buff *ocelot_rcv(struct sk_buff *skb,
 180                                   struct net_device *netdev,
 181                                   struct packet_type *pt)
 182 {
 183         u64 src_port, qos_class;
 184         u8 *start = skb->data;
 185         u8 *extraction;
 186
 187         /* Revert skb->data by the amount consumed by the DSA master,
 188          * so it points to the beginning of the frame.
 189          */
 190         skb_push(skb, ETH_HLEN);
 191         /* We don't care about the long prefix, it is just for easy entrance
 192          * into the DSA master's RX filter. Discard it now by moving it into
 193          * the headroom.
 194          */
 195         skb_pull(skb, OCELOT_LONG_PREFIX_LEN);
 196         /* And skb->data now points to the extraction frame header.
 197          * Keep a pointer to it.
 198          */
 199         extraction = skb->data;
 200         /* Now the EFH is part of the headroom as well */
 201         skb_pull(skb, OCELOT_TAG_LEN);
 202         /* Reset the pointer to the real MAC header */
 203         skb_reset_mac_header(skb);
 204         skb_reset_mac_len(skb);
 205         /* And move skb->data to the correct location again */
 206         skb_pull(skb, ETH_HLEN);
 207
 208         /* Remove from inet csum the extraction header */
 209         skb_postpull_rcsum(skb, start, OCELOT_LONG_PREFIX_LEN + OCELOT_TAG_LEN);
 210
 211         packing(extraction, &src_port,  46, 43, OCELOT_TAG_LEN, UNPACK, 0);
 212         packing(extraction, &qos_class, 19, 17, OCELOT_TAG_LEN, UNPACK, 0);
 213
 214         skb->dev = dsa_master_find_slave(netdev, 0, src_port);
 215         if (!skb->dev)
 216                 /* The switch will reflect back some frames sent through
 217                  * sockets opened on the bare DSA master. These will come back
 218                  * with src_port equal to the index of the CPU port, for which
 219                  * there is no slave registered. So don't print any error
 220                  * message here (ignore and drop those frames).
 221                  */
 222                 return NULL;
 223
 224         skb->offload_fwd_mark = 1;
 225         skb->priority = qos_class;
 226
 227         return skb;
 228 }
 229
 230 static struct dsa_device_ops ocelot_netdev_ops = {
 231         .name                   = "ocelot",
 232         .proto                  = DSA_TAG_PROTO_OCELOT,
 233         .xmit                   = ocelot_xmit,
 234         .rcv                    = ocelot_rcv,
 235         .overhead               = OCELOT_TAG_LEN + OCELOT_LONG_PREFIX_LEN,
 236 };
 237
 238 MODULE_LICENSE("GPL v2");
 239 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_OCELOT);
 240
 241 module_dsa_tag_driver(ocelot_netdev_ops);