1*4882a593Smuzhiyun // SPDX-License-Identifier: GPL-2.0-only
2*4882a593Smuzhiyun /*
3*4882a593Smuzhiyun * STP SAP demux
4*4882a593Smuzhiyun *
5*4882a593Smuzhiyun * Copyright (c) 2008 Patrick McHardy <kaber@trash.net>
6*4882a593Smuzhiyun */
7*4882a593Smuzhiyun #include <linux/mutex.h>
8*4882a593Smuzhiyun #include <linux/skbuff.h>
9*4882a593Smuzhiyun #include <linux/etherdevice.h>
10*4882a593Smuzhiyun #include <linux/llc.h>
11*4882a593Smuzhiyun #include <linux/slab.h>
12*4882a593Smuzhiyun #include <linux/module.h>
13*4882a593Smuzhiyun #include <net/llc.h>
14*4882a593Smuzhiyun #include <net/llc_pdu.h>
15*4882a593Smuzhiyun #include <net/stp.h>
16*4882a593Smuzhiyun
17*4882a593Smuzhiyun /* 01:80:c2:00:00:20 - 01:80:c2:00:00:2F */
18*4882a593Smuzhiyun #define GARP_ADDR_MIN 0x20
19*4882a593Smuzhiyun #define GARP_ADDR_MAX 0x2F
20*4882a593Smuzhiyun #define GARP_ADDR_RANGE (GARP_ADDR_MAX - GARP_ADDR_MIN)
21*4882a593Smuzhiyun
22*4882a593Smuzhiyun static const struct stp_proto __rcu *garp_protos[GARP_ADDR_RANGE + 1] __read_mostly;
23*4882a593Smuzhiyun static const struct stp_proto __rcu *stp_proto __read_mostly;
24*4882a593Smuzhiyun
25*4882a593Smuzhiyun static struct llc_sap *sap __read_mostly;
26*4882a593Smuzhiyun static unsigned int sap_registered;
27*4882a593Smuzhiyun static DEFINE_MUTEX(stp_proto_mutex);
28*4882a593Smuzhiyun
29*4882a593Smuzhiyun /* Called under rcu_read_lock from LLC */
stp_pdu_rcv(struct sk_buff * skb,struct net_device * dev,struct packet_type * pt,struct net_device * orig_dev)30*4882a593Smuzhiyun static int stp_pdu_rcv(struct sk_buff *skb, struct net_device *dev,
31*4882a593Smuzhiyun struct packet_type *pt, struct net_device *orig_dev)
32*4882a593Smuzhiyun {
33*4882a593Smuzhiyun const struct ethhdr *eh = eth_hdr(skb);
34*4882a593Smuzhiyun const struct llc_pdu_un *pdu = llc_pdu_un_hdr(skb);
35*4882a593Smuzhiyun const struct stp_proto *proto;
36*4882a593Smuzhiyun
37*4882a593Smuzhiyun if (pdu->ssap != LLC_SAP_BSPAN ||
38*4882a593Smuzhiyun pdu->dsap != LLC_SAP_BSPAN ||
39*4882a593Smuzhiyun pdu->ctrl_1 != LLC_PDU_TYPE_U)
40*4882a593Smuzhiyun goto err;
41*4882a593Smuzhiyun
42*4882a593Smuzhiyun if (eh->h_dest[5] >= GARP_ADDR_MIN && eh->h_dest[5] <= GARP_ADDR_MAX) {
43*4882a593Smuzhiyun proto = rcu_dereference(garp_protos[eh->h_dest[5] -
44*4882a593Smuzhiyun GARP_ADDR_MIN]);
45*4882a593Smuzhiyun if (proto &&
46*4882a593Smuzhiyun !ether_addr_equal(eh->h_dest, proto->group_address))
47*4882a593Smuzhiyun goto err;
48*4882a593Smuzhiyun } else
49*4882a593Smuzhiyun proto = rcu_dereference(stp_proto);
50*4882a593Smuzhiyun
51*4882a593Smuzhiyun if (!proto)
52*4882a593Smuzhiyun goto err;
53*4882a593Smuzhiyun
54*4882a593Smuzhiyun proto->rcv(proto, skb, dev);
55*4882a593Smuzhiyun return 0;
56*4882a593Smuzhiyun
57*4882a593Smuzhiyun err:
58*4882a593Smuzhiyun kfree_skb(skb);
59*4882a593Smuzhiyun return 0;
60*4882a593Smuzhiyun }
61*4882a593Smuzhiyun
stp_proto_register(const struct stp_proto * proto)62*4882a593Smuzhiyun int stp_proto_register(const struct stp_proto *proto)
63*4882a593Smuzhiyun {
64*4882a593Smuzhiyun int err = 0;
65*4882a593Smuzhiyun
66*4882a593Smuzhiyun mutex_lock(&stp_proto_mutex);
67*4882a593Smuzhiyun if (sap_registered++ == 0) {
68*4882a593Smuzhiyun sap = llc_sap_open(LLC_SAP_BSPAN, stp_pdu_rcv);
69*4882a593Smuzhiyun if (!sap) {
70*4882a593Smuzhiyun err = -ENOMEM;
71*4882a593Smuzhiyun goto out;
72*4882a593Smuzhiyun }
73*4882a593Smuzhiyun }
74*4882a593Smuzhiyun if (is_zero_ether_addr(proto->group_address))
75*4882a593Smuzhiyun rcu_assign_pointer(stp_proto, proto);
76*4882a593Smuzhiyun else
77*4882a593Smuzhiyun rcu_assign_pointer(garp_protos[proto->group_address[5] -
78*4882a593Smuzhiyun GARP_ADDR_MIN], proto);
79*4882a593Smuzhiyun out:
80*4882a593Smuzhiyun mutex_unlock(&stp_proto_mutex);
81*4882a593Smuzhiyun return err;
82*4882a593Smuzhiyun }
83*4882a593Smuzhiyun EXPORT_SYMBOL_GPL(stp_proto_register);
84*4882a593Smuzhiyun
stp_proto_unregister(const struct stp_proto * proto)85*4882a593Smuzhiyun void stp_proto_unregister(const struct stp_proto *proto)
86*4882a593Smuzhiyun {
87*4882a593Smuzhiyun mutex_lock(&stp_proto_mutex);
88*4882a593Smuzhiyun if (is_zero_ether_addr(proto->group_address))
89*4882a593Smuzhiyun RCU_INIT_POINTER(stp_proto, NULL);
90*4882a593Smuzhiyun else
91*4882a593Smuzhiyun RCU_INIT_POINTER(garp_protos[proto->group_address[5] -
92*4882a593Smuzhiyun GARP_ADDR_MIN], NULL);
93*4882a593Smuzhiyun synchronize_rcu();
94*4882a593Smuzhiyun
95*4882a593Smuzhiyun if (--sap_registered == 0)
96*4882a593Smuzhiyun llc_sap_put(sap);
97*4882a593Smuzhiyun mutex_unlock(&stp_proto_mutex);
98*4882a593Smuzhiyun }
99*4882a593Smuzhiyun EXPORT_SYMBOL_GPL(stp_proto_unregister);
100*4882a593Smuzhiyun
101*4882a593Smuzhiyun MODULE_LICENSE("GPL");
102