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 */ 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 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 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