stp.c 2.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101
  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. /* 01:80:c2:00:00:20 - 01:80:c2:00:00:2F */
  17. #define GARP_ADDR_MIN 0x20
  18. #define GARP_ADDR_MAX 0x2F
  19. #define GARP_ADDR_RANGE (GARP_ADDR_MAX - GARP_ADDR_MIN)
  20. static const struct stp_proto __rcu *garp_protos[GARP_ADDR_RANGE + 1] __read_mostly;
  21. static const struct stp_proto __rcu *stp_proto __read_mostly;
  22. static struct llc_sap *sap __read_mostly;
  23. static unsigned int sap_registered;
  24. static DEFINE_MUTEX(stp_proto_mutex);
  25. /* Called under rcu_read_lock from LLC */
  26. static int stp_pdu_rcv(struct sk_buff *skb, struct net_device *dev,
  27. struct packet_type *pt, struct net_device *orig_dev)
  28. {
  29. const struct ethhdr *eh = eth_hdr(skb);
  30. const struct llc_pdu_un *pdu = llc_pdu_un_hdr(skb);
  31. const struct stp_proto *proto;
  32. if (pdu->ssap != LLC_SAP_BSPAN ||
  33. pdu->dsap != LLC_SAP_BSPAN ||
  34. pdu->ctrl_1 != LLC_PDU_TYPE_U)
  35. goto err;
  36. if (eh->h_dest[5] >= GARP_ADDR_MIN && eh->h_dest[5] <= GARP_ADDR_MAX) {
  37. proto = rcu_dereference(garp_protos[eh->h_dest[5] -
  38. GARP_ADDR_MIN]);
  39. if (proto &&
  40. !ether_addr_equal(eh->h_dest, proto->group_address))
  41. goto err;
  42. } else
  43. proto = rcu_dereference(stp_proto);
  44. if (!proto)
  45. goto err;
  46. proto->rcv(proto, skb, dev);
  47. return 0;
  48. err:
  49. kfree_skb(skb);
  50. return 0;
  51. }
  52. int stp_proto_register(const struct stp_proto *proto)
  53. {
  54. int err = 0;
  55. mutex_lock(&stp_proto_mutex);
  56. if (sap_registered++ == 0) {
  57. sap = llc_sap_open(LLC_SAP_BSPAN, stp_pdu_rcv);
  58. if (!sap) {
  59. err = -ENOMEM;
  60. goto out;
  61. }
  62. }
  63. if (is_zero_ether_addr(proto->group_address))
  64. rcu_assign_pointer(stp_proto, proto);
  65. else
  66. rcu_assign_pointer(garp_protos[proto->group_address[5] -
  67. GARP_ADDR_MIN], proto);
  68. out:
  69. mutex_unlock(&stp_proto_mutex);
  70. return err;
  71. }
  72. EXPORT_SYMBOL_GPL(stp_proto_register);
  73. void stp_proto_unregister(const struct stp_proto *proto)
  74. {
  75. mutex_lock(&stp_proto_mutex);
  76. if (is_zero_ether_addr(proto->group_address))
  77. RCU_INIT_POINTER(stp_proto, NULL);
  78. else
  79. RCU_INIT_POINTER(garp_protos[proto->group_address[5] -
  80. GARP_ADDR_MIN], NULL);
  81. synchronize_rcu();
  82. if (--sap_registered == 0)
  83. llc_sap_put(sap);
  84. mutex_unlock(&stp_proto_mutex);
  85. }
  86. EXPORT_SYMBOL_GPL(stp_proto_unregister);
  87. MODULE_LICENSE("GPL");