net/802/stp.c

   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 *garp_protos[GARP_ADDR_RANGE + 1] __read_mostly;
  25 static const struct stp_proto *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");