garp.c 18 KB

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