/net/802/stp.c

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