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