net/dsa/tag_rzn1_a5psw.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * Copyright (C) 2022 Schneider Electric
0004  *
0005  * Clément Léger <clement.leger@bootlin.com>
0006  */
0007
0008 #include <linux/bitfield.h>
0009 #include <linux/etherdevice.h>
0010 #include <linux/if_ether.h>
0011 #include <net/dsa.h>
0012
0013 #include "dsa_priv.h"
0014
0015 /* To define the outgoing port and to discover the incoming port a TAG is
0016  * inserted after Src MAC :
0017  *
0018  *       Dest MAC       Src MAC           TAG         Type
0019  * ...| 1 2 3 4 5 6 | 1 2 3 4 5 6 | 1 2 3 4 5 6 7 8 | 1 2 |...
0020  *                                |<--------------->|
0021  *
0022  * See struct a5psw_tag for layout
0023  */
0024
0025 #define ETH_P_DSA_A5PSW         0xE001
0026 #define A5PSW_TAG_LEN           8
0027 #define A5PSW_CTRL_DATA_FORCE_FORWARD   BIT(0)
0028 /* This is both used for xmit tag and rcv tagging */
0029 #define A5PSW_CTRL_DATA_PORT        GENMASK(3, 0)
0030
0031 struct a5psw_tag {
0032     __be16 ctrl_tag;
0033     __be16 ctrl_data;
0034     __be16 ctrl_data2_hi;
0035     __be16 ctrl_data2_lo;
0036 };
0037
0038 static struct sk_buff *a5psw_tag_xmit(struct sk_buff *skb, struct net_device *dev)
0039 {
0040     struct dsa_port *dp = dsa_slave_to_port(dev);
0041     struct a5psw_tag *ptag;
0042     u32 data2_val;
0043
0044     BUILD_BUG_ON(sizeof(*ptag) != A5PSW_TAG_LEN);
0045
0046     /* The Ethernet switch we are interfaced with needs packets to be at
0047      * least 60 bytes otherwise they will be discarded when they enter the
0048      * switch port logic.
0049      */
0050     if (__skb_put_padto(skb, ETH_ZLEN, false))
0051         return NULL;
0052
0053     /* provide 'A5PSW_TAG_LEN' bytes additional space */
0054     skb_push(skb, A5PSW_TAG_LEN);
0055
0056     /* make room between MACs and Ether-Type to insert tag */
0057     dsa_alloc_etype_header(skb, A5PSW_TAG_LEN);
0058
0059     ptag = dsa_etype_header_pos_tx(skb);
0060
0061     data2_val = FIELD_PREP(A5PSW_CTRL_DATA_PORT, BIT(dp->index));
0062     ptag->ctrl_tag = htons(ETH_P_DSA_A5PSW);
0063     ptag->ctrl_data = htons(A5PSW_CTRL_DATA_FORCE_FORWARD);
0064     ptag->ctrl_data2_lo = htons(data2_val);
0065     ptag->ctrl_data2_hi = 0;
0066
0067     return skb;
0068 }
0069
0070 static struct sk_buff *a5psw_tag_rcv(struct sk_buff *skb,
0071                      struct net_device *dev)
0072 {
0073     struct a5psw_tag *tag;
0074     int port;
0075
0076     if (unlikely(!pskb_may_pull(skb, A5PSW_TAG_LEN))) {
0077         dev_warn_ratelimited(&dev->dev,
0078                      "Dropping packet, cannot pull\n");
0079         return NULL;
0080     }
0081
0082     tag = dsa_etype_header_pos_rx(skb);
0083
0084     if (tag->ctrl_tag != htons(ETH_P_DSA_A5PSW)) {
0085         dev_warn_ratelimited(&dev->dev, "Dropping packet due to invalid TAG marker\n");
0086         return NULL;
0087     }
0088
0089     port = FIELD_GET(A5PSW_CTRL_DATA_PORT, ntohs(tag->ctrl_data));
0090
0091     skb->dev = dsa_master_find_slave(dev, 0, port);
0092     if (!skb->dev)
0093         return NULL;
0094
0095     skb_pull_rcsum(skb, A5PSW_TAG_LEN);
0096     dsa_strip_etype_header(skb, A5PSW_TAG_LEN);
0097
0098     dsa_default_offload_fwd_mark(skb);
0099
0100     return skb;
0101 }
0102
0103 static const struct dsa_device_ops a5psw_netdev_ops = {
0104     .name   = "a5psw",
0105     .proto  = DSA_TAG_PROTO_RZN1_A5PSW,
0106     .xmit   = a5psw_tag_xmit,
0107     .rcv    = a5psw_tag_rcv,
0108     .needed_headroom = A5PSW_TAG_LEN,
0109 };
0110
0111 MODULE_LICENSE("GPL v2");
0112 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_A5PSW);
0113 module_dsa_tag_driver(a5psw_netdev_ops);