LXR linux/net/dsa/tag_rtl4

   1// SPDX-License-Identifier: GPL-2.0
   2/*
   3 * Handler for Realtek 4 byte DSA switch tags
   4 * Currently only supports protocol "A" found in RTL8366RB
   5 * Copyright (c) 2020 Linus Walleij <linus.walleij@linaro.org>
   6 *
   7 * This "proprietary tag" header looks like so:
   8 *
   9 * -------------------------------------------------
  10 * | MAC DA | MAC SA | 0x8899 | 2 bytes tag | Type |
  11 * -------------------------------------------------
  12 *
  13 * The 2 bytes tag form a 16 bit big endian word. The exact
  14 * meaning has been guessed from packet dumps from ingress
  15 * frames.
  16 */
  17
  18#include <linux/etherdevice.h>
  19#include <linux/bits.h>
  20
  21#include "dsa_priv.h"
  22
  23#define RTL4_A_HDR_LEN          4
  24#define RTL4_A_ETHERTYPE        0x8899
  25#define RTL4_A_PROTOCOL_SHIFT   12
  26/*
  27 * 0x1 = Realtek Remote Control protocol (RRCP)
  28 * 0x2/0x3 seems to be used for loopback testing
  29 * 0x9 = RTL8306 DSA protocol
  30 * 0xa = RTL8366RB DSA protocol
  31 */
  32#define RTL4_A_PROTOCOL_RTL8366RB       0xa
  33
  34static struct sk_buff *rtl4a_tag_xmit(struct sk_buff *skb,
  35                                      struct net_device *dev)
  36{
  37        struct dsa_port *dp = dsa_slave_to_port(dev);
  38        __be16 *p;
  39        u8 *tag;
  40        u16 out;
  41
  42        /* Pad out to at least 60 bytes */
  43        if (unlikely(__skb_put_padto(skb, ETH_ZLEN, false)))
  44                return NULL;
  45
  46        netdev_dbg(dev, "add realtek tag to package to port %d\n",
  47                   dp->index);
  48        skb_push(skb, RTL4_A_HDR_LEN);
  49
  50        dsa_alloc_etype_header(skb, RTL4_A_HDR_LEN);
  51        tag = dsa_etype_header_pos_tx(skb);
  52
  53        /* Set Ethertype */
  54        p = (__be16 *)tag;
  55        *p = htons(RTL4_A_ETHERTYPE);
  56
  57        out = (RTL4_A_PROTOCOL_RTL8366RB << RTL4_A_PROTOCOL_SHIFT);
  58        /* The lower bits indicate the port number */
  59        out |= BIT(dp->index);
  60
  61        p = (__be16 *)(tag + 2);
  62        *p = htons(out);
  63
  64        return skb;
  65}
  66
  67static struct sk_buff *rtl4a_tag_rcv(struct sk_buff *skb,
  68                                     struct net_device *dev)
  69{
  70        u16 protport;
  71        __be16 *p;
  72        u16 etype;
  73        u8 *tag;
  74        u8 prot;
  75        u8 port;
  76
  77        if (unlikely(!pskb_may_pull(skb, RTL4_A_HDR_LEN)))
  78                return NULL;
  79
  80        tag = dsa_etype_header_pos_rx(skb);
  81        p = (__be16 *)tag;
  82        etype = ntohs(*p);
  83        if (etype != RTL4_A_ETHERTYPE) {
  84                /* Not custom, just pass through */
  85                netdev_dbg(dev, "non-realtek ethertype 0x%04x\n", etype);
  86                return skb;
  87        }
  88        p = (__be16 *)(tag + 2);
  89        protport = ntohs(*p);
  90        /* The 4 upper bits are the protocol */
  91        prot = (protport >> RTL4_A_PROTOCOL_SHIFT) & 0x0f;
  92        if (prot != RTL4_A_PROTOCOL_RTL8366RB) {
  93                netdev_err(dev, "unknown realtek protocol 0x%01x\n", prot);
  94                return NULL;
  95        }
  96        port = protport & 0xff;
  97
  98        skb->dev = dsa_master_find_slave(dev, 0, port);
  99        if (!skb->dev) {
 100                netdev_dbg(dev, "could not find slave for port %d\n", port);
 101                return NULL;
 102        }
 103
 104        /* Remove RTL4 tag and recalculate checksum */
 105        skb_pull_rcsum(skb, RTL4_A_HDR_LEN);
 106
 107        dsa_strip_etype_header(skb, RTL4_A_HDR_LEN);
 108
 109        dsa_default_offload_fwd_mark(skb);
 110
 111        return skb;
 112}
 113
 114static const struct dsa_device_ops rtl4a_netdev_ops = {
 115        .name   = "rtl4a",
 116        .proto  = DSA_TAG_PROTO_RTL4_A,
 117        .xmit   = rtl4a_tag_xmit,
 118        .rcv    = rtl4a_tag_rcv,
 119        .needed_headroom = RTL4_A_HDR_LEN,
 120};
 121module_dsa_tag_driver(rtl4a_netdev_ops);
 122
 123MODULE_LICENSE("GPL");
 124MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_RTL4_A);
 125