garp.c 18 KB

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