1a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy/* 2a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy * STP SAP demux 3a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy * 4a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy * Copyright (c) 2008 Patrick McHardy <kaber@trash.net> 5a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy * 6a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy * This program is free software; you can redistribute it and/or 7a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy * modify it under the terms of the GNU General Public License 8a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy * version 2 as published by the Free Software Foundation. 9a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy */ 10a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy#include <linux/mutex.h> 11a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy#include <linux/skbuff.h> 12a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy#include <linux/etherdevice.h> 13a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy#include <linux/llc.h> 145a0e3ad6af8660be21ca98a971cd00f331318c05Tejun Heo#include <linux/slab.h> 153a9a231d977222eea36eae091df2c358e03ac839Paul Gortmaker#include <linux/module.h> 16a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy#include <net/llc.h> 17a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy#include <net/llc_pdu.h> 18a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy#include <net/stp.h> 19a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 20a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy/* 01:80:c2:00:00:20 - 01:80:c2:00:00:2F */ 21a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy#define GARP_ADDR_MIN 0x20 22a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy#define GARP_ADDR_MAX 0x2F 23a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy#define GARP_ADDR_RANGE (GARP_ADDR_MAX - GARP_ADDR_MIN) 24a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 253cc77ec74e1583b50b8405114cdbd6b8ebb8c474Eric Dumazetstatic const struct stp_proto __rcu *garp_protos[GARP_ADDR_RANGE + 1] __read_mostly; 263cc77ec74e1583b50b8405114cdbd6b8ebb8c474Eric Dumazetstatic const struct stp_proto __rcu *stp_proto __read_mostly; 27a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 28a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardystatic struct llc_sap *sap __read_mostly; 29a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardystatic unsigned int sap_registered; 30a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardystatic DEFINE_MUTEX(stp_proto_mutex); 31a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 32a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy/* Called under rcu_read_lock from LLC */ 33a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardystatic int stp_pdu_rcv(struct sk_buff *skb, struct net_device *dev, 34a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy struct packet_type *pt, struct net_device *orig_dev) 35a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy{ 36a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy const struct ethhdr *eh = eth_hdr(skb); 37a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy const struct llc_pdu_un *pdu = llc_pdu_un_hdr(skb); 38a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy const struct stp_proto *proto; 39a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 40a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy if (pdu->ssap != LLC_SAP_BSPAN || 41a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy pdu->dsap != LLC_SAP_BSPAN || 42a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy pdu->ctrl_1 != LLC_PDU_TYPE_U) 43a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy goto err; 44a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 45a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy if (eh->h_dest[5] >= GARP_ADDR_MIN && eh->h_dest[5] <= GARP_ADDR_MAX) { 46a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy proto = rcu_dereference(garp_protos[eh->h_dest[5] - 47a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy GARP_ADDR_MIN]); 48a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy if (proto && 4928b29801b95de20405b2a11945cb6dfc7c6c84f9Joe Perches !ether_addr_equal(eh->h_dest, proto->group_address)) 50a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy goto err; 51a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy } else 52a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy proto = rcu_dereference(stp_proto); 53a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 54a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy if (!proto) 55a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy goto err; 56a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 57a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy proto->rcv(proto, skb, dev); 58a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy return 0; 59a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 60a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardyerr: 61a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy kfree_skb(skb); 62a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy return 0; 63a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy} 64a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 65a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardyint stp_proto_register(const struct stp_proto *proto) 66a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy{ 67a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy int err = 0; 68a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 69a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy mutex_lock(&stp_proto_mutex); 70a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy if (sap_registered++ == 0) { 71a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy sap = llc_sap_open(LLC_SAP_BSPAN, stp_pdu_rcv); 72a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy if (!sap) { 73a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy err = -ENOMEM; 74a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy goto out; 75a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy } 76a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy } 77a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy if (is_zero_ether_addr(proto->group_address)) 78a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy rcu_assign_pointer(stp_proto, proto); 79a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy else 80a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy rcu_assign_pointer(garp_protos[proto->group_address[5] - 81a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy GARP_ADDR_MIN], proto); 82a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardyout: 83a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy mutex_unlock(&stp_proto_mutex); 84a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy return err; 85a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy} 86a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardyEXPORT_SYMBOL_GPL(stp_proto_register); 87a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 88a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardyvoid stp_proto_unregister(const struct stp_proto *proto) 89a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy{ 90a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy mutex_lock(&stp_proto_mutex); 91a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy if (is_zero_ether_addr(proto->group_address)) 92a9b3cd7f323b2e57593e7215362a7b02fc933e3aStephen Hemminger RCU_INIT_POINTER(stp_proto, NULL); 93a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy else 94a9b3cd7f323b2e57593e7215362a7b02fc933e3aStephen Hemminger RCU_INIT_POINTER(garp_protos[proto->group_address[5] - 95a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy GARP_ADDR_MIN], NULL); 96a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy synchronize_rcu(); 97a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 98a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy if (--sap_registered == 0) 99a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy llc_sap_put(sap); 100a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy mutex_unlock(&stp_proto_mutex); 101a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy} 102a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardyEXPORT_SYMBOL_GPL(stp_proto_unregister); 103a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardy 104a19800d704177caaa5874baf5819307c5b7d5e4fPatrick McHardyMODULE_LICENSE("GPL"); 105