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