target/linux/generic/backport-5.15/766-v5.18-08-net-dsa-tag_qca-add-support-for-handling-mgmt-and-MI.patch

   1 From 31eb6b4386ad91930417e3f5c8157a4b5e31cbd5 Mon Sep 17 00:00:00 2001
   2 From: Ansuel Smith <ansuelsmth@gmail.com>
   3 Date: Wed, 2 Feb 2022 01:03:27 +0100
   4 Subject: [PATCH 08/16] net: dsa: tag_qca: add support for handling mgmt and
   5  MIB Ethernet packet
   6
   7 Add connect/disconnect helper to assign private struct to the DSA switch.
   8 Add support for Ethernet mgmt and MIB if the DSA driver provide an handler
   9 to correctly parse and elaborate the data.
  10
  11 Signed-off-by: Ansuel Smith <ansuelsmth@gmail.com>
  12 Reviewed-by: Vladimir Oltean <olteanv@gmail.com>
  13 Reviewed-by: Florian Fainelli <f.fainelli@gmail.com>
  14 Signed-off-by: David S. Miller <davem@davemloft.net>
  15 ---
  16  include/linux/dsa/tag_qca.h |  7 +++++++
  17  net/dsa/tag_qca.c           | 39 ++++++++++++++++++++++++++++++++++---
  18  2 files changed, 43 insertions(+), 3 deletions(-)
  19
  20 --- a/include/linux/dsa/tag_qca.h
  21 +++ b/include/linux/dsa/tag_qca.h
  22 @@ -72,4 +72,11 @@ struct mib_ethhdr {
  23         __be16 hdr;             /* qca hdr */
  24  } __packed;
  25
  26 +struct qca_tagger_data {
  27 +       void (*rw_reg_ack_handler)(struct dsa_switch *ds,
  28 +                                  struct sk_buff *skb);
  29 +       void (*mib_autocast_handler)(struct dsa_switch *ds,
  30 +                                    struct sk_buff *skb);
  31 +};
  32 +
  33  #endif /* __TAG_QCA_H */
  34 --- a/net/dsa/tag_qca.c
  35 +++ b/net/dsa/tag_qca.c
  36 @@ -5,6 +5,7 @@
  37
  38  #include <linux/etherdevice.h>
  39  #include <linux/bitfield.h>
  40 +#include <net/dsa.h>
  41  #include <linux/dsa/tag_qca.h>
  42
  43  #include "dsa_priv.h"
  44 @@ -32,6 +33,9 @@ static struct sk_buff *qca_tag_xmit(stru
  45
  46  static struct sk_buff *qca_tag_rcv(struct sk_buff *skb, struct net_device *dev)
  47  {
  48 +       struct qca_tagger_data *tagger_data;
  49 +       struct dsa_port *dp = dev->dsa_ptr;
  50 +       struct dsa_switch *ds = dp->ds;
  51         u8 ver, pk_type;
  52         __be16 *phdr;
  53         int port;
  54 @@ -39,6 +43,8 @@ static struct sk_buff *qca_tag_rcv(struc
  55
  56         BUILD_BUG_ON(sizeof(struct qca_mgmt_ethhdr) != QCA_HDR_MGMT_HEADER_LEN + QCA_HDR_LEN);
  57
  58 +       tagger_data = ds->tagger_data;
  59 +
  60         if (unlikely(!pskb_may_pull(skb, QCA_HDR_LEN)))
  61                 return NULL;
  62
  63 @@ -53,13 +59,19 @@ static struct sk_buff *qca_tag_rcv(struc
  64         /* Get pk type */
  65         pk_type = FIELD_GET(QCA_HDR_RECV_TYPE, hdr);
  66
  67 -       /* Ethernet MDIO read/write packet */
  68 -       if (pk_type == QCA_HDR_RECV_TYPE_RW_REG_ACK)
  69 +       /* Ethernet mgmt read/write packet */
  70 +       if (pk_type == QCA_HDR_RECV_TYPE_RW_REG_ACK) {
  71 +               if (likely(tagger_data->rw_reg_ack_handler))
  72 +                       tagger_data->rw_reg_ack_handler(ds, skb);
  73                 return NULL;
  74 +       }
  75
  76         /* Ethernet MIB counter packet */
  77 -       if (pk_type == QCA_HDR_RECV_TYPE_MIB)
  78 +       if (pk_type == QCA_HDR_RECV_TYPE_MIB) {
  79 +               if (likely(tagger_data->mib_autocast_handler))
  80 +                       tagger_data->mib_autocast_handler(ds, skb);
  81                 return NULL;
  82 +       }
  83
  84         /* Remove QCA tag and recalculate checksum */
  85         skb_pull_rcsum(skb, QCA_HDR_LEN);
  86 @@ -75,9 +87,30 @@ static struct sk_buff *qca_tag_rcv(struc
  87         return skb;
  88  }
  89
  90 +static int qca_tag_connect(struct dsa_switch *ds)
  91 +{
  92 +       struct qca_tagger_data *tagger_data;
  93 +
  94 +       tagger_data = kzalloc(sizeof(*tagger_data), GFP_KERNEL);
  95 +       if (!tagger_data)
  96 +               return -ENOMEM;
  97 +
  98 +       ds->tagger_data = tagger_data;
  99 +
 100 +       return 0;
 101 +}
 102 +
 103 +static void qca_tag_disconnect(struct dsa_switch *ds)
 104 +{
 105 +       kfree(ds->tagger_data);
 106 +       ds->tagger_data = NULL;
 107 +}
 108 +
 109  static const struct dsa_device_ops qca_netdev_ops = {
 110         .name   = "qca",
 111         .proto  = DSA_TAG_PROTO_QCA,
 112 +       .connect = qca_tag_connect,
 113 +       .disconnect = qca_tag_disconnect,
 114         .xmit   = qca_tag_xmit,
 115         .rcv    = qca_tag_rcv,
 116         .needed_headroom = QCA_HDR_LEN,