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
25static const struct stp_proto __rcu *garp_protos[GARP_ADDR_RANGE + 1] __read_mostly;
26static const struct stp_proto __rcu *stp_proto __read_mostly;
27
28static struct llc_sap *sap __read_mostly;
29static unsigned int sap_registered;
30static DEFINE_MUTEX(stp_proto_mutex);
31
32/* Called under rcu_read_lock from LLC */
33static 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
60err:
61	kfree_skb(skb);
62	return 0;
63}
64
65int 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);
82out:
83	mutex_unlock(&stp_proto_mutex);
84	return err;
85}
86EXPORT_SYMBOL_GPL(stp_proto_register);
87
88void 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}
102EXPORT_SYMBOL_GPL(stp_proto_unregister);
103
104MODULE_LICENSE("GPL");
105