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