cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

tag_rtl4_a.c (3067B)

      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);