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