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