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