1 // SPDX-License-Identifier: (LGPL-2.1 OR BSD-2-Clause)
    2 #include <linux/bpf.h>
    3 #include <linux/if_ether.h>
    4 #include <linux/in.h>
    5 
    6 #include <bpf/bpf_helpers.h>
    7 #include <bpf/bpf_endian.h>
    8 
    9 #ifndef AF_INET
   10 #define AF_INET 2
   11 #endif
   12 
   13 /* NOTICE: Re-defining VLAN header levels to parse */
   14 #define VLAN_MAX_DEPTH 10
   15 #include "../common/parsing_helpers.h"
   16 #include "../common/rewrite_helpers.h"
   17 
   18 #define unlikely(e) __builtin_expect((e), 0)
   19 
   20 struct ethhdr_dsa {
   21         unsigned char   h_dest[ETH_ALEN];
   22         unsigned char   h_source[ETH_ALEN];
   23         struct dsa_tag {
   24                 __u8    dev : 5;
   25                 __u8    tagged : 1;
   26                 __u8    cmd : 2;
   27 
   28                 __u8    cfi : 1;
   29                 __u8    res1 : 1;
   30                 __u8    is_trunk : 1;
   31                 __u8    port : 5;
   32 
   33                 __u8    vid_h : 4;
   34                 __u8    res2 : 1;
   35                 __u8    pri : 3;
   36 
   37                 __u8    vid_l;
   38         } __attribute__((packed)) dsa;
   39         __be16          h_proto;
   40 } __attribute__((packed));
   41 
   42 enum dsa_cmd {
   43         DSA_CMD_TO_CPU     = 0,
   44         DSA_CMD_FROM_CPU   = 1,
   45         DSA_CMD_TO_SNIFFER = 2,
   46         DSA_CMD_FORWARD    = 3
   47 };
   48 
   49 static __always_inline int ifindex_to_mv88e6xxx_port(int ifindex)
   50 {
   51         switch (ifindex) {
   52         case 14:
   53         case 24:
   54         case 25:
   55                 return 1;
   56         case 16:
   57         case 26:
   58         case 27:
   59                 return 3;
   60         case 18:
   61         case 28:
   62         case 29:
   63                 return 5;
   64         case 19:
   65         case 34:
   66         case 35:
   67                 return 6;
   68         case 20:
   69         case 30:
   70         case 31:
   71                 return 7;
   72         case 21:
   73         case 32:
   74         case 33:
   75                 return 8;
   76         case 22:
   77         case 36:
   78         case 37:
   79         case 42:
   80         case 43:
   81                 return 9;
   82         default:
   83                 return -1;
   84         }
   85 }
   86 
   87 static __always_inline int ifindex_to_vid(int ifindex)
   88 {
   89         switch (ifindex) {
   90         case 24:
   91                 return 10;
   92         case 25:
   93                 return 11;
   94         case 26:
   95                 return 12;
   96         case 27:
   97                 return 13;
   98         case 28:
   99                 return 14;
  100         case 29:
  101                 return 15;
  102         case 30:
  103                 return 16;
  104         case 31:
  105                 return 17;
  106         case 32:
  107                 return 18;
  108         case 33:
  109                 return 19;
  110         case 34:
  111                 return 20;
  112         case 35:
  113                 return 21;
  114         case 36:
  115                 return 22;
  116         case 37:
  117                 return 23;
  118         case 42:
  119                 return 100;
  120         case 43:
  121                 return 101;
  122         default:
  123                 return -1;
  124         }
  125 }
  126 
  127 static __always_inline int ifindex_to_dsa_tag(int ifindex, struct dsa_tag *dsa)
  128 {
  129         int port, vid;
  130 
  131         port = ifindex_to_mv88e6xxx_port(ifindex);
  132         if (port < 0)
  133                 return -1;
  134 
  135         dsa->port = port;
  136         vid = ifindex_to_vid(ifindex);
  137 
  138         if (vid >= 0) {
  139                 dsa->tagged = 1;
  140                 dsa->vid_h = (vid >> 8) & 0xf;
  141                 dsa->vid_l = vid & 0xff;
  142         } else {
  143                 dsa->tagged = dsa->vid_h = dsa->vid_l = 0;
  144         }
  145 
  146         return 0;
  147 }
  148 
  149 static __always_inline int dsa_tag_to_ifindex(struct dsa_tag *dsa)
  150 {
  151         int vid;
  152 
  153         if (dsa->tagged)
  154                 vid = ((int)dsa->vid_h << 8) | dsa->vid_l;
  155         else
  156                 vid = -1;
  157 
  158         switch (vid) {
  159         case 10:
  160                 return 24;
  161         case 11:
  162                 return 25;
  163         case 12:
  164                 return 26;
  165         case 13:
  166                 return 27;
  167         case 14:
  168                 return 28;
  169         case 15:
  170                 return 29;
  171         case 16:
  172                 return 30;
  173         case 17:
  174                 return 31;
  175         case 18:
  176                 return 32;
  177         case 19:
  178                 return 33;
  179         case 20:
  180                 return 34;
  181         case 21:
  182                 return 35;
  183         case 22:
  184                 return 36;
  185         case 23:
  186                 return 37;
  187         case 100:
  188                 return 42;
  189         case 101:
  190                 return 43;
  191         case -1:
  192                 switch (dsa->port) {
  193                 case 1:
  194                         return 14;
  195                 case 3:
  196                         return 16;
  197                 case 5:
  198                         return 18;
  199                 case 6:
  200                         return 19;
  201                 case 7:
  202                         return 20;
  203                 case 8:
  204                         return 21;
  205                 case 9:
  206                         return 22;
  207                 default:
  208                         return -1;
  209                 }
  210         default:
  211                 return -1;
  212         }
  213 }
  214 
  215 SEC("xdp_dsa")
  216 int xdp_dsa_func(struct xdp_md *ctx)
  217 {
  218         void *data_end = (void *)(long)ctx->data_end;
  219         void *data = (void *)(long)ctx->data;
  220         struct bpf_fib_lookup fib_params;
  221         struct ethhdr_dsa *eth = data;
  222         struct iphdr *iphdr;
  223         struct dsa_tag dsa;
  224         int source_ifindex;
  225         __u64 nh_off;
  226         int pop_vlan = 0;
  227         __u16 h_proto;
  228         int rc;
  229 
  230         nh_off = sizeof(*eth);
  231         if (data + nh_off > data_end)
  232                 return XDP_DROP;
  233 
  234         __builtin_memcpy(&dsa, &eth->dsa, sizeof(dsa));
  235 
  236         if (dsa.cmd != DSA_CMD_FORWARD)
  237                 return XDP_PASS;
  238 
  239         h_proto = eth->h_proto;
  240 
  241         if (!dsa.tagged && h_proto == bpf_htons(ETH_P_8021Q)) {
  242                 struct vlan_hdr *vhdr;
  243                 int vid;
  244 
  245                 vhdr = data + nh_off;
  246                 nh_off += sizeof(struct vlan_hdr);
  247                 if (data + nh_off > data_end)
  248                         return XDP_DROP;
  249 
  250                 dsa.tagged = 1;
  251                 dsa.pri = bpf_ntohs(vhdr->h_vlan_TCI) >> 13;
  252                 vid = bpf_ntohs(vhdr->h_vlan_TCI) & VLAN_VID_MASK;
  253                 dsa.vid_h = (vid >> 8) & 0xf;
  254                 dsa.vid_l = vid & 0xff;
  255 
  256                 h_proto = vhdr->h_vlan_encapsulated_proto;
  257                 pop_vlan = 1;
  258         }
  259 
  260         if (h_proto != bpf_htons(ETH_P_IP))
  261                 return XDP_PASS;
  262 
  263         iphdr = data + nh_off;
  264         nh_off += sizeof(struct iphdr);
  265         if (data + nh_off > data_end)
  266                 return XDP_DROP;
  267 
  268         if (iphdr->ttl <= 1)
  269                 return XDP_PASS;
  270 
  271         source_ifindex = dsa_tag_to_ifindex(&dsa);
  272         if (source_ifindex < 0)
  273                 return XDP_PASS;
  274 
  275         __builtin_memset(&fib_params, 0, sizeof(fib_params));
  276 
  277         fib_params.family = AF_INET;
  278         fib_params.tos = iphdr->tos;
  279         fib_params.l4_protocol = iphdr->protocol;
  280         fib_params.sport = 0;
  281         fib_params.dport = 0;
  282         fib_params.tot_len = bpf_ntohs(iphdr->tot_len);
  283         fib_params.ipv4_src = iphdr->saddr;
  284         fib_params.ipv4_dst = iphdr->daddr;
  285         fib_params.ifindex = source_ifindex;
  286 
  287         rc = bpf_fib_lookup(ctx, &fib_params, sizeof(fib_params), 0);
  288 
  289         if (rc == BPF_FIB_LKUP_RET_SUCCESS) {
  290                 __builtin_memcpy(eth->h_source, fib_params.smac, ETH_ALEN);
  291                 __builtin_memcpy(eth->h_dest, fib_params.dmac, ETH_ALEN);
  292 
  293                 if (ifindex_to_dsa_tag(fib_params.ifindex, &dsa) < 0) {
  294                         /* tu by sa mohlo robit bpf_redirect_map */
  295                         return XDP_PASS;
  296                 }
  297         } else {
  298                 return XDP_PASS;
  299         }
  300 
  301         dsa.cmd = DSA_CMD_FROM_CPU;
  302         dsa.res1 = 0;
  303         dsa.res2 = 0;
  304         dsa.pri = 0;
  305         __builtin_memcpy(&eth->dsa, &dsa, sizeof(dsa));
  306 
  307         if (pop_vlan) {
  308                 eth->h_proto = h_proto;
  309 
  310                 __builtin_memmove(data + sizeof(struct vlan_hdr), data, sizeof(*eth));
  311                 if (bpf_xdp_adjust_head(ctx, (int)sizeof(struct vlan_hdr)))
  312                         return XDP_DROP;
  313 
  314                 data = (void *)(long)ctx->data;
  315                 data_end = (void *)(long)ctx->data_end;
  316                 eth = data;
  317                 nh_off = sizeof(*eth);
  318 
  319                 if (data + nh_off > data_end)
  320                         return XDP_DROP;
  321         }
  322 
  323         return XDP_TX;
  324 }
  325 
  326 char _license[] SEC("license") = "GPL";