net/dsa/tag_ar9331.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (c) 2019 Pengutronix, Oleksij Rempel <kernel@pengutronix.de>
   4  */
   5
   6
   7 #include <linux/bitfield.h>
   8 #include <linux/etherdevice.h>
   9
  10 #include "dsa_priv.h"
  11
  12 #define AR9331_HDR_LEN                  2
  13 #define AR9331_HDR_VERSION              1
  14
  15 #define AR9331_HDR_VERSION_MASK         GENMASK(15, 14)
  16 #define AR9331_HDR_PRIORITY_MASK        GENMASK(13, 12)
  17 #define AR9331_HDR_TYPE_MASK            GENMASK(10, 8)
  18 #define AR9331_HDR_BROADCAST            BIT(7)
  19 #define AR9331_HDR_FROM_CPU             BIT(6)
  20 /* AR9331_HDR_RESERVED - not used or may be version field.
  21  * According to the AR8216 doc it should 0b10. On AR9331 it is 0b11 on RX path
  22  * and should be set to 0b11 to make it work.
  23  */
  24 #define AR9331_HDR_RESERVED_MASK        GENMASK(5, 4)
  25 #define AR9331_HDR_PORT_NUM_MASK        GENMASK(3, 0)
  26
  27 static struct sk_buff *ar9331_tag_xmit(struct sk_buff *skb,
  28                                        struct net_device *dev)
  29 {
  30         struct dsa_port *dp = dsa_slave_to_port(dev);
  31         __le16 *phdr;
  32         u16 hdr;
  33
  34         if (skb_cow_head(skb, AR9331_HDR_LEN) < 0)
  35                 return NULL;
  36
  37         phdr = skb_push(skb, AR9331_HDR_LEN);
  38
  39         hdr = FIELD_PREP(AR9331_HDR_VERSION_MASK, AR9331_HDR_VERSION);
  40         hdr |= AR9331_HDR_FROM_CPU | dp->index;
  41         /* 0b10 for AR8216 and 0b11 for AR9331 */
  42         hdr |= AR9331_HDR_RESERVED_MASK;
  43
  44         phdr[0] = cpu_to_le16(hdr);
  45
  46         return skb;
  47 }
  48
  49 static struct sk_buff *ar9331_tag_rcv(struct sk_buff *skb,
  50                                       struct net_device *ndev,
  51                                       struct packet_type *pt)
  52 {
  53         u8 ver, port;
  54         u16 hdr;
  55
  56         if (unlikely(!pskb_may_pull(skb, AR9331_HDR_LEN)))
  57                 return NULL;
  58
  59         hdr = le16_to_cpu(*(__le16 *)skb_mac_header(skb));
  60
  61         ver = FIELD_GET(AR9331_HDR_VERSION_MASK, hdr);
  62         if (unlikely(ver != AR9331_HDR_VERSION)) {
  63                 netdev_warn_once(ndev, "%s:%i wrong header version 0x%2x\n",
  64                                  __func__, __LINE__, hdr);
  65                 return NULL;
  66         }
  67
  68         if (unlikely(hdr & AR9331_HDR_FROM_CPU)) {
  69                 netdev_warn_once(ndev, "%s:%i packet should not be from cpu 0x%2x\n",
  70                                  __func__, __LINE__, hdr);
  71                 return NULL;
  72         }
  73
  74         skb_pull_rcsum(skb, AR9331_HDR_LEN);
  75
  76         /* Get source port information */
  77         port = FIELD_GET(AR9331_HDR_PORT_NUM_MASK, hdr);
  78
  79         skb->dev = dsa_master_find_slave(ndev, 0, port);
  80         if (!skb->dev)
  81                 return NULL;
  82
  83         return skb;
  84 }
  85
  86 static const struct dsa_device_ops ar9331_netdev_ops = {
  87         .name   = "ar9331",
  88         .proto  = DSA_TAG_PROTO_AR9331,
  89         .xmit   = ar9331_tag_xmit,
  90         .rcv    = ar9331_tag_rcv,
  91         .overhead = AR9331_HDR_LEN,
  92 };
  93
  94 MODULE_LICENSE("GPL v2");
  95 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_AR9331);
  96 module_dsa_tag_driver(ar9331_netdev_ops);