garp.c 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * IEEE 802.1D Generic Attribute Registration Protocol (GARP)
  4. *
  5. * Copyright (c) 2008 Patrick McHardy <kaber@trash.net>
  6. */
  7. #include <linux/kernel.h>
  8. #include <linux/timer.h>
  9. #include <linux/skbuff.h>
  10. #include <linux/netdevice.h>
  11. #include <linux/etherdevice.h>
  12. #include <linux/rtnetlink.h>
  13. #include <linux/llc.h>
  14. #include <linux/slab.h>
  15. #include <linux/module.h>
  16. #include <net/llc.h>
  17. #include <net/llc_pdu.h>
  18. #include <net/garp.h>
  19. #include <linux/unaligned.h>
  20. static unsigned int garp_join_time __read_mostly = 200;
  21. module_param(garp_join_time, uint, 0644);
  22. MODULE_PARM_DESC(garp_join_time, "Join time in ms (default 200ms)");
  23. MODULE_DESCRIPTION("IEEE 802.1D Generic Attribute Registration Protocol (GARP)");
  24. MODULE_LICENSE("GPL");
  25. static const struct garp_state_trans {
  26. u8 state;
  27. u8 action;
  28. } garp_applicant_state_table[GARP_APPLICANT_MAX + 1][GARP_EVENT_MAX + 1] = {
  29. [GARP_APPLICANT_VA] = {
  30. [GARP_EVENT_TRANSMIT_PDU] = { .state = GARP_APPLICANT_AA,
  31. .action = GARP_ACTION_S_JOIN_IN },
  32. [GARP_EVENT_R_JOIN_IN] = { .state = GARP_APPLICANT_AA },
  33. [GARP_EVENT_R_JOIN_EMPTY] = { .state = GARP_APPLICANT_VA },
  34. [GARP_EVENT_R_EMPTY] = { .state = GARP_APPLICANT_VA },
  35. [GARP_EVENT_R_LEAVE_IN] = { .state = GARP_APPLICANT_VA },
  36. [GARP_EVENT_R_LEAVE_EMPTY] = { .state = GARP_APPLICANT_VP },
  37. [GARP_EVENT_REQ_JOIN] = { .state = GARP_APPLICANT_INVALID },
  38. [GARP_EVENT_REQ_LEAVE] = { .state = GARP_APPLICANT_LA },
  39. },
  40. [GARP_APPLICANT_AA] = {
  41. [GARP_EVENT_TRANSMIT_PDU] = { .state = GARP_APPLICANT_QA,
  42. .action = GARP_ACTION_S_JOIN_IN },
  43. [GARP_EVENT_R_JOIN_IN] = { .state = GARP_APPLICANT_QA },
  44. [GARP_EVENT_R_JOIN_EMPTY] = { .state = GARP_APPLICANT_VA },
  45. [GARP_EVENT_R_EMPTY] = { .state = GARP_APPLICANT_VA },
  46. [GARP_EVENT_R_LEAVE_IN] = { .state = GARP_APPLICANT_VA },
  47. [GARP_EVENT_R_LEAVE_EMPTY] = { .state = GARP_APPLICANT_VP },
  48. [GARP_EVENT_REQ_JOIN] = { .state = GARP_APPLICANT_INVALID },
  49. [GARP_EVENT_REQ_LEAVE] = { .state = GARP_APPLICANT_LA },
  50. },
  51. [GARP_APPLICANT_QA] = {
  52. [GARP_EVENT_TRANSMIT_PDU] = { .state = GARP_APPLICANT_INVALID },
  53. [GARP_EVENT_R_JOIN_IN] = { .state = GARP_APPLICANT_QA },
  54. [GARP_EVENT_R_JOIN_EMPTY] = { .state = GARP_APPLICANT_VA },
  55. [GARP_EVENT_R_EMPTY] = { .state = GARP_APPLICANT_VA },
  56. [GARP_EVENT_R_LEAVE_IN] = { .state = GARP_APPLICANT_VP },
  57. [GARP_EVENT_R_LEAVE_EMPTY] = { .state = GARP_APPLICANT_VP },
  58. [GARP_EVENT_REQ_JOIN] = { .state = GARP_APPLICANT_INVALID },
  59. [GARP_EVENT_REQ_LEAVE] = { .state = GARP_APPLICANT_LA },
  60. },
  61. [GARP_APPLICANT_LA] = {
  62. [GARP_EVENT_TRANSMIT_PDU] = { .state = GARP_APPLICANT_VO,
  63. .action = GARP_ACTION_S_LEAVE_EMPTY },
  64. [GARP_EVENT_R_JOIN_IN] = { .state = GARP_APPLICANT_LA },
  65. [GARP_EVENT_R_JOIN_EMPTY] = { .state = GARP_APPLICANT_VO },
  66. [GARP_EVENT_R_EMPTY] = { .state = GARP_APPLICANT_LA },
  67. [GARP_EVENT_R_LEAVE_IN] = { .state = GARP_APPLICANT_LA },
  68. [GARP_EVENT_R_LEAVE_EMPTY] = { .state = GARP_APPLICANT_VO },
  69. [GARP_EVENT_REQ_JOIN] = { .state = GARP_APPLICANT_VA },
  70. [GARP_EVENT_REQ_LEAVE] = { .state = GARP_APPLICANT_INVALID },
  71. },
  72. [GARP_APPLICANT_VP] = {
  73. [GARP_EVENT_TRANSMIT_PDU] = { .state = GARP_APPLICANT_AA,
  74. .action = GARP_ACTION_S_JOIN_IN },
  75. [GARP_EVENT_R_JOIN_IN] = { .state = GARP_APPLICANT_AP },
  76. [GARP_EVENT_R_JOIN_EMPTY] = { .state = GARP_APPLICANT_VP },
  77. [GARP_EVENT_R_EMPTY] = { .state = GARP_APPLICANT_VP },
  78. [GARP_EVENT_R_LEAVE_IN] = { .state = GARP_APPLICANT_VP },
  79. [GARP_EVENT_R_LEAVE_EMPTY] = { .state = GARP_APPLICANT_VP },
  80. [GARP_EVENT_REQ_JOIN] = { .state = GARP_APPLICANT_INVALID },
  81. [GARP_EVENT_REQ_LEAVE] = { .state = GARP_APPLICANT_VO },
  82. },
  83. [GARP_APPLICANT_AP] = {
  84. [GARP_EVENT_TRANSMIT_PDU] = { .state = GARP_APPLICANT_QA,
  85. .action = GARP_ACTION_S_JOIN_IN },
  86. [GARP_EVENT_R_JOIN_IN] = { .state = GARP_APPLICANT_QP },
  87. [GARP_EVENT_R_JOIN_EMPTY] = { .state = GARP_APPLICANT_VP },
  88. [GARP_EVENT_R_EMPTY] = { .state = GARP_APPLICANT_VP },
  89. [GARP_EVENT_R_LEAVE_IN] = { .state = GARP_APPLICANT_VP },
  90. [GARP_EVENT_R_LEAVE_EMPTY] = { .state = GARP_APPLICANT_VP },
  91. [GARP_EVENT_REQ_JOIN] = { .state = GARP_APPLICANT_INVALID },
  92. [GARP_EVENT_REQ_LEAVE] = { .state = GARP_APPLICANT_AO },
  93. },
  94. [GARP_APPLICANT_QP] = {
  95. [GARP_EVENT_TRANSMIT_PDU] = { .state = GARP_APPLICANT_INVALID },
  96. [GARP_EVENT_R_JOIN_IN] = { .state = GARP_APPLICANT_QP },
  97. [GARP_EVENT_R_JOIN_EMPTY] = { .state = GARP_APPLICANT_VP },
  98. [GARP_EVENT_R_EMPTY] = { .state = GARP_APPLICANT_VP },
  99. [GARP_EVENT_R_LEAVE_IN] = { .state = GARP_APPLICANT_VP },
  100. [GARP_EVENT_R_LEAVE_EMPTY] = { .state = GARP_APPLICANT_VP },
  101. [GARP_EVENT_REQ_JOIN] = { .state = GARP_APPLICANT_INVALID },
  102. [GARP_EVENT_REQ_LEAVE] = { .state = GARP_APPLICANT_QO },
  103. },
  104. [GARP_APPLICANT_VO] = {
  105. [GARP_EVENT_TRANSMIT_PDU] = { .state = GARP_APPLICANT_INVALID },
  106. [GARP_EVENT_R_JOIN_IN] = { .state = GARP_APPLICANT_AO },
  107. [GARP_EVENT_R_JOIN_EMPTY] = { .state = GARP_APPLICANT_VO },
  108. [GARP_EVENT_R_EMPTY] = { .state = GARP_APPLICANT_VO },
  109. [GARP_EVENT_R_LEAVE_IN] = { .state = GARP_APPLICANT_VO },
  110. [GARP_EVENT_R_LEAVE_EMPTY] = { .state = GARP_APPLICANT_VO },
  111. [GARP_EVENT_REQ_JOIN] = { .state = GARP_APPLICANT_VP },
  112. [GARP_EVENT_REQ_LEAVE] = { .state = GARP_APPLICANT_INVALID },
  113. },
  114. [GARP_APPLICANT_AO] = {
  115. [GARP_EVENT_TRANSMIT_PDU] = { .state = GARP_APPLICANT_INVALID },
  116. [GARP_EVENT_R_JOIN_IN] = { .state = GARP_APPLICANT_QO },
  117. [GARP_EVENT_R_JOIN_EMPTY] = { .state = GARP_APPLICANT_VO },
  118. [GARP_EVENT_R_EMPTY] = { .state = GARP_APPLICANT_VO },
  119. [GARP_EVENT_R_LEAVE_IN] = { .state = GARP_APPLICANT_VO },
  120. [GARP_EVENT_R_LEAVE_EMPTY] = { .state = GARP_APPLICANT_VO },
  121. [GARP_EVENT_REQ_JOIN] = { .state = GARP_APPLICANT_AP },
  122. [GARP_EVENT_REQ_LEAVE] = { .state = GARP_APPLICANT_INVALID },
  123. },
  124. [GARP_APPLICANT_QO] = {
  125. [GARP_EVENT_TRANSMIT_PDU] = { .state = GARP_APPLICANT_INVALID },
  126. [GARP_EVENT_R_JOIN_IN] = { .state = GARP_APPLICANT_QO },
  127. [GARP_EVENT_R_JOIN_EMPTY] = { .state = GARP_APPLICANT_VO },
  128. [GARP_EVENT_R_EMPTY] = { .state = GARP_APPLICANT_VO },
  129. [GARP_EVENT_R_LEAVE_IN] = { .state = GARP_APPLICANT_VO },
  130. [GARP_EVENT_R_LEAVE_EMPTY] = { .state = GARP_APPLICANT_VO },
  131. [GARP_EVENT_REQ_JOIN] = { .state = GARP_APPLICANT_QP },
  132. [GARP_EVENT_REQ_LEAVE] = { .state = GARP_APPLICANT_INVALID },
  133. },
  134. };
  135. static int garp_attr_cmp(const struct garp_attr *attr,
  136. const void *data, u8 len, u8 type)
  137. {
  138. if (attr->type != type)
  139. return attr->type - type;
  140. if (attr->dlen != len)
  141. return attr->dlen - len;
  142. return memcmp(attr->data, data, len);
  143. }
  144. static struct garp_attr *garp_attr_lookup(const struct garp_applicant *app,
  145. const void *data, u8 len, u8 type)
  146. {
  147. struct rb_node *parent = app->gid.rb_node;
  148. struct garp_attr *attr;
  149. int d;
  150. while (parent) {
  151. attr = rb_entry(parent, struct garp_attr, node);
  152. d = garp_attr_cmp(attr, data, len, type);
  153. if (d > 0)
  154. parent = parent->rb_left;
  155. else if (d < 0)
  156. parent = parent->rb_right;
  157. else
  158. return attr;
  159. }
  160. return NULL;
  161. }
  162. static struct garp_attr *garp_attr_create(struct garp_applicant *app,
  163. const void *data, u8 len, u8 type)
  164. {
  165. struct rb_node *parent = NULL, **p = &app->gid.rb_node;
  166. struct garp_attr *attr;
  167. int d;
  168. while (*p) {
  169. parent = *p;
  170. attr = rb_entry(parent, struct garp_attr, node);
  171. d = garp_attr_cmp(attr, data, len, type);
  172. if (d > 0)
  173. p = &parent->rb_left;
  174. else if (d < 0)
  175. p = &parent->rb_right;
  176. else {
  177. /* The attribute already exists; re-use it. */
  178. return attr;
  179. }
  180. }
  181. attr = kmalloc(sizeof(*attr) + len, GFP_ATOMIC);
  182. if (!attr)
  183. return attr;
  184. attr->state = GARP_APPLICANT_VO;
  185. attr->type = type;
  186. attr->dlen = len;
  187. memcpy(attr->data, data, len);
  188. rb_link_node(&attr->node, parent, p);
  189. rb_insert_color(&attr->node, &app->gid);
  190. return attr;
  191. }
  192. static void garp_attr_destroy(struct garp_applicant *app, struct garp_attr *attr)
  193. {
  194. rb_erase(&attr->node, &app->gid);
  195. kfree(attr);
  196. }
  197. static void garp_attr_destroy_all(struct garp_applicant *app)
  198. {
  199. struct rb_node *node, *next;
  200. struct garp_attr *attr;
  201. for (node = rb_first(&app->gid);
  202. next = node ? rb_next(node) : NULL, node != NULL;
  203. node = next) {
  204. attr = rb_entry(node, struct garp_attr, node);
  205. garp_attr_destroy(app, attr);
  206. }
  207. }
  208. static int garp_pdu_init(struct garp_applicant *app)
  209. {
  210. struct sk_buff *skb;
  211. struct garp_pdu_hdr *gp;
  212. #define LLC_RESERVE sizeof(struct llc_pdu_un)
  213. skb = alloc_skb(app->dev->mtu + LL_RESERVED_SPACE(app->dev),
  214. GFP_ATOMIC);
  215. if (!skb)
  216. return -ENOMEM;
  217. skb->dev = app->dev;
  218. skb->protocol = htons(ETH_P_802_2);
  219. skb_reserve(skb, LL_RESERVED_SPACE(app->dev) + LLC_RESERVE);
  220. gp = __skb_put(skb, sizeof(*gp));
  221. put_unaligned(htons(GARP_PROTOCOL_ID), &gp->protocol);
  222. app->pdu = skb;
  223. return 0;
  224. }
  225. static int garp_pdu_append_end_mark(struct garp_applicant *app)
  226. {
  227. if (skb_tailroom(app->pdu) < sizeof(u8))
  228. return -1;
  229. __skb_put_u8(app->pdu, GARP_END_MARK);
  230. return 0;
  231. }
  232. static void garp_pdu_queue(struct garp_applicant *app)
  233. {
  234. if (!app->pdu)
  235. return;
  236. garp_pdu_append_end_mark(app);
  237. garp_pdu_append_end_mark(app);
  238. llc_pdu_header_init(app->pdu, LLC_PDU_TYPE_U, LLC_SAP_BSPAN,
  239. LLC_SAP_BSPAN, LLC_PDU_CMD);
  240. llc_pdu_init_as_ui_cmd(app->pdu);
  241. llc_mac_hdr_init(app->pdu, app->dev->dev_addr,
  242. app->app->proto.group_address);
  243. skb_queue_tail(&app->queue, app->pdu);
  244. app->pdu = NULL;
  245. }
  246. static void garp_queue_xmit(struct garp_applicant *app)
  247. {
  248. struct sk_buff *skb;
  249. while ((skb = skb_dequeue(&app->queue)))
  250. dev_queue_xmit(skb);
  251. }
  252. static int garp_pdu_append_msg(struct garp_applicant *app, u8 attrtype)
  253. {
  254. struct garp_msg_hdr *gm;
  255. if (skb_tailroom(app->pdu) < sizeof(*gm))
  256. return -1;
  257. gm = __skb_put(app->pdu, sizeof(*gm));
  258. gm->attrtype = attrtype;
  259. garp_cb(app->pdu)->cur_type = attrtype;
  260. return 0;
  261. }
  262. static int garp_pdu_append_attr(struct garp_applicant *app,
  263. const struct garp_attr *attr,
  264. enum garp_attr_event event)
  265. {
  266. struct garp_attr_hdr *ga;
  267. unsigned int len;
  268. int err;
  269. again:
  270. if (!app->pdu) {
  271. err = garp_pdu_init(app);
  272. if (err < 0)
  273. return err;
  274. }
  275. if (garp_cb(app->pdu)->cur_type != attr->type) {
  276. if (garp_cb(app->pdu)->cur_type &&
  277. garp_pdu_append_end_mark(app) < 0)
  278. goto queue;
  279. if (garp_pdu_append_msg(app, attr->type) < 0)
  280. goto queue;
  281. }
  282. len = sizeof(*ga) + attr->dlen;
  283. if (skb_tailroom(app->pdu) < len)
  284. goto queue;
  285. ga = __skb_put(app->pdu, len);
  286. ga->len = len;
  287. ga->event = event;
  288. memcpy(ga->data, attr->data, attr->dlen);
  289. return 0;
  290. queue:
  291. garp_pdu_queue(app);
  292. goto again;
  293. }
  294. static void garp_attr_event(struct garp_applicant *app,
  295. struct garp_attr *attr, enum garp_event event)
  296. {
  297. enum garp_applicant_state state;
  298. state = garp_applicant_state_table[attr->state][event].state;
  299. if (state == GARP_APPLICANT_INVALID)
  300. return;
  301. switch (garp_applicant_state_table[attr->state][event].action) {
  302. case GARP_ACTION_NONE:
  303. break;
  304. case GARP_ACTION_S_JOIN_IN:
  305. /* When appending the attribute fails, don't update state in
  306. * order to retry on next TRANSMIT_PDU event. */
  307. if (garp_pdu_append_attr(app, attr, GARP_JOIN_IN) < 0)
  308. return;
  309. break;
  310. case GARP_ACTION_S_LEAVE_EMPTY:
  311. garp_pdu_append_attr(app, attr, GARP_LEAVE_EMPTY);
  312. /* As a pure applicant, sending a leave message implies that
  313. * the attribute was unregistered and can be destroyed. */
  314. garp_attr_destroy(app, attr);
  315. return;
  316. default:
  317. WARN_ON(1);
  318. }
  319. attr->state = state;
  320. }
  321. int garp_request_join(const struct net_device *dev,
  322. const struct garp_application *appl,
  323. const void *data, u8 len, u8 type)
  324. {
  325. struct garp_port *port = rtnl_dereference(dev->garp_port);
  326. struct garp_applicant *app = rtnl_dereference(port->applicants[appl->type]);
  327. struct garp_attr *attr;
  328. spin_lock_bh(&app->lock);
  329. attr = garp_attr_create(app, data, len, type);
  330. if (!attr) {
  331. spin_unlock_bh(&app->lock);
  332. return -ENOMEM;
  333. }
  334. garp_attr_event(app, attr, GARP_EVENT_REQ_JOIN);
  335. spin_unlock_bh(&app->lock);
  336. return 0;
  337. }
  338. EXPORT_SYMBOL_GPL(garp_request_join);
  339. void garp_request_leave(const struct net_device *dev,
  340. const struct garp_application *appl,
  341. const void *data, u8 len, u8 type)
  342. {
  343. struct garp_port *port = rtnl_dereference(dev->garp_port);
  344. struct garp_applicant *app = rtnl_dereference(port->applicants[appl->type]);
  345. struct garp_attr *attr;
  346. spin_lock_bh(&app->lock);
  347. attr = garp_attr_lookup(app, data, len, type);
  348. if (!attr) {
  349. spin_unlock_bh(&app->lock);
  350. return;
  351. }
  352. garp_attr_event(app, attr, GARP_EVENT_REQ_LEAVE);
  353. spin_unlock_bh(&app->lock);
  354. }
  355. EXPORT_SYMBOL_GPL(garp_request_leave);
  356. static void garp_gid_event(struct garp_applicant *app, enum garp_event event)
  357. {
  358. struct rb_node *node, *next;
  359. struct garp_attr *attr;
  360. for (node = rb_first(&app->gid);
  361. next = node ? rb_next(node) : NULL, node != NULL;
  362. node = next) {
  363. attr = rb_entry(node, struct garp_attr, node);
  364. garp_attr_event(app, attr, event);
  365. }
  366. }
  367. static void garp_join_timer_arm(struct garp_applicant *app)
  368. {
  369. unsigned long delay;
  370. delay = get_random_u32_below(msecs_to_jiffies(garp_join_time));
  371. mod_timer(&app->join_timer, jiffies + delay);
  372. }
  373. static void garp_join_timer(struct timer_list *t)
  374. {
  375. struct garp_applicant *app = from_timer(app, t, join_timer);
  376. spin_lock(&app->lock);
  377. garp_gid_event(app, GARP_EVENT_TRANSMIT_PDU);
  378. garp_pdu_queue(app);
  379. spin_unlock(&app->lock);
  380. garp_queue_xmit(app);
  381. garp_join_timer_arm(app);
  382. }
  383. static int garp_pdu_parse_end_mark(struct sk_buff *skb)
  384. {
  385. if (!pskb_may_pull(skb, sizeof(u8)))
  386. return -1;
  387. if (*skb->data == GARP_END_MARK) {
  388. skb_pull(skb, sizeof(u8));
  389. return -1;
  390. }
  391. return 0;
  392. }
  393. static int garp_pdu_parse_attr(struct garp_applicant *app, struct sk_buff *skb,
  394. u8 attrtype)
  395. {
  396. const struct garp_attr_hdr *ga;
  397. struct garp_attr *attr;
  398. enum garp_event event;
  399. unsigned int dlen;
  400. if (!pskb_may_pull(skb, sizeof(*ga)))
  401. return -1;
  402. ga = (struct garp_attr_hdr *)skb->data;
  403. if (ga->len < sizeof(*ga))
  404. return -1;
  405. if (!pskb_may_pull(skb, ga->len))
  406. return -1;
  407. skb_pull(skb, ga->len);
  408. dlen = sizeof(*ga) - ga->len;
  409. if (attrtype > app->app->maxattr)
  410. return 0;
  411. switch (ga->event) {
  412. case GARP_LEAVE_ALL:
  413. if (dlen != 0)
  414. return -1;
  415. garp_gid_event(app, GARP_EVENT_R_LEAVE_EMPTY);
  416. return 0;
  417. case GARP_JOIN_EMPTY:
  418. event = GARP_EVENT_R_JOIN_EMPTY;
  419. break;
  420. case GARP_JOIN_IN:
  421. event = GARP_EVENT_R_JOIN_IN;
  422. break;
  423. case GARP_LEAVE_EMPTY:
  424. event = GARP_EVENT_R_LEAVE_EMPTY;
  425. break;
  426. case GARP_EMPTY:
  427. event = GARP_EVENT_R_EMPTY;
  428. break;
  429. default:
  430. return 0;
  431. }
  432. if (dlen == 0)
  433. return -1;
  434. attr = garp_attr_lookup(app, ga->data, dlen, attrtype);
  435. if (attr == NULL)
  436. return 0;
  437. garp_attr_event(app, attr, event);
  438. return 0;
  439. }
  440. static int garp_pdu_parse_msg(struct garp_applicant *app, struct sk_buff *skb)
  441. {
  442. const struct garp_msg_hdr *gm;
  443. if (!pskb_may_pull(skb, sizeof(*gm)))
  444. return -1;
  445. gm = (struct garp_msg_hdr *)skb->data;
  446. if (gm->attrtype == 0)
  447. return -1;
  448. skb_pull(skb, sizeof(*gm));
  449. while (skb->len > 0) {
  450. if (garp_pdu_parse_attr(app, skb, gm->attrtype) < 0)
  451. return -1;
  452. if (garp_pdu_parse_end_mark(skb) < 0)
  453. break;
  454. }
  455. return 0;
  456. }
  457. static void garp_pdu_rcv(const struct stp_proto *proto, struct sk_buff *skb,
  458. struct net_device *dev)
  459. {
  460. struct garp_application *appl = proto->data;
  461. struct garp_port *port;
  462. struct garp_applicant *app;
  463. const struct garp_pdu_hdr *gp;
  464. port = rcu_dereference(dev->garp_port);
  465. if (!port)
  466. goto err;
  467. app = rcu_dereference(port->applicants[appl->type]);
  468. if (!app)
  469. goto err;
  470. if (!pskb_may_pull(skb, sizeof(*gp)))
  471. goto err;
  472. gp = (struct garp_pdu_hdr *)skb->data;
  473. if (get_unaligned(&gp->protocol) != htons(GARP_PROTOCOL_ID))
  474. goto err;
  475. skb_pull(skb, sizeof(*gp));
  476. spin_lock(&app->lock);
  477. while (skb->len > 0) {
  478. if (garp_pdu_parse_msg(app, skb) < 0)
  479. break;
  480. if (garp_pdu_parse_end_mark(skb) < 0)
  481. break;
  482. }
  483. spin_unlock(&app->lock);
  484. err:
  485. kfree_skb(skb);
  486. }
  487. static int garp_init_port(struct net_device *dev)
  488. {
  489. struct garp_port *port;
  490. port = kzalloc(sizeof(*port), GFP_KERNEL);
  491. if (!port)
  492. return -ENOMEM;
  493. rcu_assign_pointer(dev->garp_port, port);
  494. return 0;
  495. }
  496. static void garp_release_port(struct net_device *dev)
  497. {
  498. struct garp_port *port = rtnl_dereference(dev->garp_port);
  499. unsigned int i;
  500. for (i = 0; i <= GARP_APPLICATION_MAX; i++) {
  501. if (rtnl_dereference(port->applicants[i]))
  502. return;
  503. }
  504. RCU_INIT_POINTER(dev->garp_port, NULL);
  505. kfree_rcu(port, rcu);
  506. }
  507. int garp_init_applicant(struct net_device *dev, struct garp_application *appl)
  508. {
  509. struct garp_applicant *app;
  510. int err;
  511. ASSERT_RTNL();
  512. if (!rtnl_dereference(dev->garp_port)) {
  513. err = garp_init_port(dev);
  514. if (err < 0)
  515. goto err1;
  516. }
  517. err = -ENOMEM;
  518. app = kzalloc(sizeof(*app), GFP_KERNEL);
  519. if (!app)
  520. goto err2;
  521. err = dev_mc_add(dev, appl->proto.group_address);
  522. if (err < 0)
  523. goto err3;
  524. app->dev = dev;
  525. app->app = appl;
  526. app->gid = RB_ROOT;
  527. spin_lock_init(&app->lock);
  528. skb_queue_head_init(&app->queue);
  529. rcu_assign_pointer(dev->garp_port->applicants[appl->type], app);
  530. timer_setup(&app->join_timer, garp_join_timer, 0);
  531. garp_join_timer_arm(app);
  532. return 0;
  533. err3:
  534. kfree(app);
  535. err2:
  536. garp_release_port(dev);
  537. err1:
  538. return err;
  539. }
  540. EXPORT_SYMBOL_GPL(garp_init_applicant);
  541. void garp_uninit_applicant(struct net_device *dev, struct garp_application *appl)
  542. {
  543. struct garp_port *port = rtnl_dereference(dev->garp_port);
  544. struct garp_applicant *app = rtnl_dereference(port->applicants[appl->type]);
  545. ASSERT_RTNL();
  546. RCU_INIT_POINTER(port->applicants[appl->type], NULL);
  547. /* Delete timer and generate a final TRANSMIT_PDU event to flush out
  548. * all pending messages before the applicant is gone. */
  549. timer_shutdown_sync(&app->join_timer);
  550. spin_lock_bh(&app->lock);
  551. garp_gid_event(app, GARP_EVENT_TRANSMIT_PDU);
  552. garp_attr_destroy_all(app);
  553. garp_pdu_queue(app);
  554. spin_unlock_bh(&app->lock);
  555. garp_queue_xmit(app);
  556. dev_mc_del(dev, appl->proto.group_address);
  557. kfree_rcu(app, rcu);
  558. garp_release_port(dev);
  559. }
  560. EXPORT_SYMBOL_GPL(garp_uninit_applicant);
  561. int garp_register_application(struct garp_application *appl)
  562. {
  563. appl->proto.rcv = garp_pdu_rcv;
  564. appl->proto.data = appl;
  565. return stp_proto_register(&appl->proto);
  566. }
  567. EXPORT_SYMBOL_GPL(garp_register_application);
  568. void garp_unregister_application(struct garp_application *appl)
  569. {
  570. stp_proto_unregister(&appl->proto);
  571. }
  572. EXPORT_SYMBOL_GPL(garp_unregister_application);