garp.c 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636
  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 void garp_attr_insert(struct garp_applicant *app, struct garp_attr *new)
  165. {
  166. struct rb_node *parent = NULL, **p = &app->gid.rb_node;
  167. struct garp_attr *attr;
  168. int d;
  169. while (*p) {
  170. parent = *p;
  171. attr = rb_entry(parent, struct garp_attr, node);
  172. d = garp_attr_cmp(attr, new->data, new->dlen, new->type);
  173. if (d < 0)
  174. p = &parent->rb_left;
  175. else if (d > 0)
  176. p = &parent->rb_right;
  177. }
  178. rb_link_node(&new->node, parent, p);
  179. rb_insert_color(&new->node, &app->gid);
  180. }
  181. static struct garp_attr *garp_attr_create(struct garp_applicant *app,
  182. const void *data, u8 len, u8 type)
  183. {
  184. struct garp_attr *attr;
  185. attr = kmalloc(sizeof(*attr) + len, GFP_ATOMIC);
  186. if (!attr)
  187. return attr;
  188. attr->state = GARP_APPLICANT_VO;
  189. attr->type = type;
  190. attr->dlen = len;
  191. memcpy(attr->data, data, len);
  192. garp_attr_insert(app, attr);
  193. return attr;
  194. }
  195. static void garp_attr_destroy(struct garp_applicant *app, struct garp_attr *attr)
  196. {
  197. rb_erase(&attr->node, &app->gid);
  198. kfree(attr);
  199. }
  200. static int garp_pdu_init(struct garp_applicant *app)
  201. {
  202. struct sk_buff *skb;
  203. struct garp_pdu_hdr *gp;
  204. #define LLC_RESERVE sizeof(struct llc_pdu_un)
  205. skb = alloc_skb(app->dev->mtu + LL_RESERVED_SPACE(app->dev),
  206. GFP_ATOMIC);
  207. if (!skb)
  208. return -ENOMEM;
  209. skb->dev = app->dev;
  210. skb->protocol = htons(ETH_P_802_2);
  211. skb_reserve(skb, LL_RESERVED_SPACE(app->dev) + LLC_RESERVE);
  212. gp = (struct garp_pdu_hdr *)__skb_put(skb, sizeof(*gp));
  213. put_unaligned(htons(GARP_PROTOCOL_ID), &gp->protocol);
  214. app->pdu = skb;
  215. return 0;
  216. }
  217. static int garp_pdu_append_end_mark(struct garp_applicant *app)
  218. {
  219. if (skb_tailroom(app->pdu) < sizeof(u8))
  220. return -1;
  221. *(u8 *)__skb_put(app->pdu, sizeof(u8)) = GARP_END_MARK;
  222. return 0;
  223. }
  224. static void garp_pdu_queue(struct garp_applicant *app)
  225. {
  226. if (!app->pdu)
  227. return;
  228. garp_pdu_append_end_mark(app);
  229. garp_pdu_append_end_mark(app);
  230. llc_pdu_header_init(app->pdu, LLC_PDU_TYPE_U, LLC_SAP_BSPAN,
  231. LLC_SAP_BSPAN, LLC_PDU_CMD);
  232. llc_pdu_init_as_ui_cmd(app->pdu);
  233. llc_mac_hdr_init(app->pdu, app->dev->dev_addr,
  234. app->app->proto.group_address);
  235. skb_queue_tail(&app->queue, app->pdu);
  236. app->pdu = NULL;
  237. }
  238. static void garp_queue_xmit(struct garp_applicant *app)
  239. {
  240. struct sk_buff *skb;
  241. while ((skb = skb_dequeue(&app->queue)))
  242. dev_queue_xmit(skb);
  243. }
  244. static int garp_pdu_append_msg(struct garp_applicant *app, u8 attrtype)
  245. {
  246. struct garp_msg_hdr *gm;
  247. if (skb_tailroom(app->pdu) < sizeof(*gm))
  248. return -1;
  249. gm = (struct garp_msg_hdr *)__skb_put(app->pdu, sizeof(*gm));
  250. gm->attrtype = attrtype;
  251. garp_cb(app->pdu)->cur_type = attrtype;
  252. return 0;
  253. }
  254. static int garp_pdu_append_attr(struct garp_applicant *app,
  255. const struct garp_attr *attr,
  256. enum garp_attr_event event)
  257. {
  258. struct garp_attr_hdr *ga;
  259. unsigned int len;
  260. int err;
  261. again:
  262. if (!app->pdu) {
  263. err = garp_pdu_init(app);
  264. if (err < 0)
  265. return err;
  266. }
  267. if (garp_cb(app->pdu)->cur_type != attr->type) {
  268. if (garp_cb(app->pdu)->cur_type &&
  269. garp_pdu_append_end_mark(app) < 0)
  270. goto queue;
  271. if (garp_pdu_append_msg(app, attr->type) < 0)
  272. goto queue;
  273. }
  274. len = sizeof(*ga) + attr->dlen;
  275. if (skb_tailroom(app->pdu) < len)
  276. goto queue;
  277. ga = (struct garp_attr_hdr *)__skb_put(app->pdu, len);
  278. ga->len = len;
  279. ga->event = event;
  280. memcpy(ga->data, attr->data, attr->dlen);
  281. return 0;
  282. queue:
  283. garp_pdu_queue(app);
  284. goto again;
  285. }
  286. static void garp_attr_event(struct garp_applicant *app,
  287. struct garp_attr *attr, enum garp_event event)
  288. {
  289. enum garp_applicant_state state;
  290. state = garp_applicant_state_table[attr->state][event].state;
  291. if (state == GARP_APPLICANT_INVALID)
  292. return;
  293. switch (garp_applicant_state_table[attr->state][event].action) {
  294. case GARP_ACTION_NONE:
  295. break;
  296. case GARP_ACTION_S_JOIN_IN:
  297. /* When appending the attribute fails, don't update state in
  298. * order to retry on next TRANSMIT_PDU event. */
  299. if (garp_pdu_append_attr(app, attr, GARP_JOIN_IN) < 0)
  300. return;
  301. break;
  302. case GARP_ACTION_S_LEAVE_EMPTY:
  303. garp_pdu_append_attr(app, attr, GARP_LEAVE_EMPTY);
  304. /* As a pure applicant, sending a leave message implies that
  305. * the attribute was unregistered and can be destroyed. */
  306. garp_attr_destroy(app, attr);
  307. return;
  308. default:
  309. WARN_ON(1);
  310. }
  311. attr->state = state;
  312. }
  313. int garp_request_join(const struct net_device *dev,
  314. const struct garp_application *appl,
  315. const void *data, u8 len, u8 type)
  316. {
  317. struct garp_port *port = rtnl_dereference(dev->garp_port);
  318. struct garp_applicant *app = rtnl_dereference(port->applicants[appl->type]);
  319. struct garp_attr *attr;
  320. spin_lock_bh(&app->lock);
  321. attr = garp_attr_create(app, data, len, type);
  322. if (!attr) {
  323. spin_unlock_bh(&app->lock);
  324. return -ENOMEM;
  325. }
  326. garp_attr_event(app, attr, GARP_EVENT_REQ_JOIN);
  327. spin_unlock_bh(&app->lock);
  328. return 0;
  329. }
  330. EXPORT_SYMBOL_GPL(garp_request_join);
  331. void garp_request_leave(const struct net_device *dev,
  332. const struct garp_application *appl,
  333. const void *data, u8 len, u8 type)
  334. {
  335. struct garp_port *port = rtnl_dereference(dev->garp_port);
  336. struct garp_applicant *app = rtnl_dereference(port->applicants[appl->type]);
  337. struct garp_attr *attr;
  338. spin_lock_bh(&app->lock);
  339. attr = garp_attr_lookup(app, data, len, type);
  340. if (!attr) {
  341. spin_unlock_bh(&app->lock);
  342. return;
  343. }
  344. garp_attr_event(app, attr, GARP_EVENT_REQ_LEAVE);
  345. spin_unlock_bh(&app->lock);
  346. }
  347. EXPORT_SYMBOL_GPL(garp_request_leave);
  348. static void garp_gid_event(struct garp_applicant *app, enum garp_event event)
  349. {
  350. struct rb_node *node, *next;
  351. struct garp_attr *attr;
  352. for (node = rb_first(&app->gid);
  353. next = node ? rb_next(node) : NULL, node != NULL;
  354. node = next) {
  355. attr = rb_entry(node, struct garp_attr, node);
  356. garp_attr_event(app, attr, event);
  357. }
  358. }
  359. static void garp_join_timer_arm(struct garp_applicant *app)
  360. {
  361. unsigned long delay;
  362. delay = (u64)msecs_to_jiffies(garp_join_time) * net_random() >> 32;
  363. mod_timer(&app->join_timer, jiffies + delay);
  364. }
  365. static void garp_join_timer(unsigned long data)
  366. {
  367. struct garp_applicant *app = (struct garp_applicant *)data;
  368. spin_lock(&app->lock);
  369. garp_gid_event(app, GARP_EVENT_TRANSMIT_PDU);
  370. garp_pdu_queue(app);
  371. spin_unlock(&app->lock);
  372. garp_queue_xmit(app);
  373. garp_join_timer_arm(app);
  374. }
  375. static int garp_pdu_parse_end_mark(struct sk_buff *skb)
  376. {
  377. if (!pskb_may_pull(skb, sizeof(u8)))
  378. return -1;
  379. if (*skb->data == GARP_END_MARK) {
  380. skb_pull(skb, sizeof(u8));
  381. return -1;
  382. }
  383. return 0;
  384. }
  385. static int garp_pdu_parse_attr(struct garp_applicant *app, struct sk_buff *skb,
  386. u8 attrtype)
  387. {
  388. const struct garp_attr_hdr *ga;
  389. struct garp_attr *attr;
  390. enum garp_event event;
  391. unsigned int dlen;
  392. if (!pskb_may_pull(skb, sizeof(*ga)))
  393. return -1;
  394. ga = (struct garp_attr_hdr *)skb->data;
  395. if (ga->len < sizeof(*ga))
  396. return -1;
  397. if (!pskb_may_pull(skb, ga->len))
  398. return -1;
  399. skb_pull(skb, ga->len);
  400. dlen = sizeof(*ga) - ga->len;
  401. if (attrtype > app->app->maxattr)
  402. return 0;
  403. switch (ga->event) {
  404. case GARP_LEAVE_ALL:
  405. if (dlen != 0)
  406. return -1;
  407. garp_gid_event(app, GARP_EVENT_R_LEAVE_EMPTY);
  408. return 0;
  409. case GARP_JOIN_EMPTY:
  410. event = GARP_EVENT_R_JOIN_EMPTY;
  411. break;
  412. case GARP_JOIN_IN:
  413. event = GARP_EVENT_R_JOIN_IN;
  414. break;
  415. case GARP_LEAVE_EMPTY:
  416. event = GARP_EVENT_R_LEAVE_EMPTY;
  417. break;
  418. case GARP_EMPTY:
  419. event = GARP_EVENT_R_EMPTY;
  420. break;
  421. default:
  422. return 0;
  423. }
  424. if (dlen == 0)
  425. return -1;
  426. attr = garp_attr_lookup(app, ga->data, dlen, attrtype);
  427. if (attr == NULL)
  428. return 0;
  429. garp_attr_event(app, attr, event);
  430. return 0;
  431. }
  432. static int garp_pdu_parse_msg(struct garp_applicant *app, struct sk_buff *skb)
  433. {
  434. const struct garp_msg_hdr *gm;
  435. if (!pskb_may_pull(skb, sizeof(*gm)))
  436. return -1;
  437. gm = (struct garp_msg_hdr *)skb->data;
  438. if (gm->attrtype == 0)
  439. return -1;
  440. skb_pull(skb, sizeof(*gm));
  441. while (skb->len > 0) {
  442. if (garp_pdu_parse_attr(app, skb, gm->attrtype) < 0)
  443. return -1;
  444. if (garp_pdu_parse_end_mark(skb) < 0)
  445. break;
  446. }
  447. return 0;
  448. }
  449. static void garp_pdu_rcv(const struct stp_proto *proto, struct sk_buff *skb,
  450. struct net_device *dev)
  451. {
  452. struct garp_application *appl = proto->data;
  453. struct garp_port *port;
  454. struct garp_applicant *app;
  455. const struct garp_pdu_hdr *gp;
  456. port = rcu_dereference(dev->garp_port);
  457. if (!port)
  458. goto err;
  459. app = rcu_dereference(port->applicants[appl->type]);
  460. if (!app)
  461. goto err;
  462. if (!pskb_may_pull(skb, sizeof(*gp)))
  463. goto err;
  464. gp = (struct garp_pdu_hdr *)skb->data;
  465. if (get_unaligned(&gp->protocol) != htons(GARP_PROTOCOL_ID))
  466. goto err;
  467. skb_pull(skb, sizeof(*gp));
  468. spin_lock(&app->lock);
  469. while (skb->len > 0) {
  470. if (garp_pdu_parse_msg(app, skb) < 0)
  471. break;
  472. if (garp_pdu_parse_end_mark(skb) < 0)
  473. break;
  474. }
  475. spin_unlock(&app->lock);
  476. err:
  477. kfree_skb(skb);
  478. }
  479. static int garp_init_port(struct net_device *dev)
  480. {
  481. struct garp_port *port;
  482. port = kzalloc(sizeof(*port), GFP_KERNEL);
  483. if (!port)
  484. return -ENOMEM;
  485. rcu_assign_pointer(dev->garp_port, port);
  486. return 0;
  487. }
  488. static void garp_release_port(struct net_device *dev)
  489. {
  490. struct garp_port *port = rtnl_dereference(dev->garp_port);
  491. unsigned int i;
  492. for (i = 0; i <= GARP_APPLICATION_MAX; i++) {
  493. if (rtnl_dereference(port->applicants[i]))
  494. return;
  495. }
  496. RCU_INIT_POINTER(dev->garp_port, NULL);
  497. kfree_rcu(port, rcu);
  498. }
  499. int garp_init_applicant(struct net_device *dev, struct garp_application *appl)
  500. {
  501. struct garp_applicant *app;
  502. int err;
  503. ASSERT_RTNL();
  504. if (!rtnl_dereference(dev->garp_port)) {
  505. err = garp_init_port(dev);
  506. if (err < 0)
  507. goto err1;
  508. }
  509. err = -ENOMEM;
  510. app = kzalloc(sizeof(*app), GFP_KERNEL);
  511. if (!app)
  512. goto err2;
  513. err = dev_mc_add(dev, appl->proto.group_address);
  514. if (err < 0)
  515. goto err3;
  516. app->dev = dev;
  517. app->app = appl;
  518. app->gid = RB_ROOT;
  519. spin_lock_init(&app->lock);
  520. skb_queue_head_init(&app->queue);
  521. rcu_assign_pointer(dev->garp_port->applicants[appl->type], app);
  522. setup_timer(&app->join_timer, garp_join_timer, (unsigned long)app);
  523. garp_join_timer_arm(app);
  524. return 0;
  525. err3:
  526. kfree(app);
  527. err2:
  528. garp_release_port(dev);
  529. err1:
  530. return err;
  531. }
  532. EXPORT_SYMBOL_GPL(garp_init_applicant);
  533. void garp_uninit_applicant(struct net_device *dev, struct garp_application *appl)
  534. {
  535. struct garp_port *port = rtnl_dereference(dev->garp_port);
  536. struct garp_applicant *app = rtnl_dereference(port->applicants[appl->type]);
  537. ASSERT_RTNL();
  538. RCU_INIT_POINTER(port->applicants[appl->type], NULL);
  539. /* Delete timer and generate a final TRANSMIT_PDU event to flush out
  540. * all pending messages before the applicant is gone. */
  541. del_timer_sync(&app->join_timer);
  542. garp_gid_event(app, GARP_EVENT_TRANSMIT_PDU);
  543. garp_pdu_queue(app);
  544. garp_queue_xmit(app);
  545. dev_mc_del(dev, appl->proto.group_address);
  546. kfree_rcu(app, rcu);
  547. garp_release_port(dev);
  548. }
  549. EXPORT_SYMBOL_GPL(garp_uninit_applicant);
  550. int garp_register_application(struct garp_application *appl)
  551. {
  552. appl->proto.rcv = garp_pdu_rcv;
  553. appl->proto.data = appl;
  554. return stp_proto_register(&appl->proto);
  555. }
  556. EXPORT_SYMBOL_GPL(garp_register_application);
  557. void garp_unregister_application(struct garp_application *appl)
  558. {
  559. stp_proto_unregister(&appl->proto);
  560. }
  561. EXPORT_SYMBOL_GPL(garp_unregister_application);