mrp.c 25 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * IEEE 802.1Q Multiple Registration Protocol (MRP)
  4. *
  5. * Copyright (c) 2012 Massachusetts Institute of Technology
  6. *
  7. * Adapted from code in net/802/garp.c
  8. * Copyright (c) 2008 Patrick McHardy <kaber@trash.net>
  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/slab.h>
  17. #include <linux/module.h>
  18. #include <net/mrp.h>
  19. #include <asm/unaligned.h>
  20. static unsigned int mrp_join_time __read_mostly = 200;
  21. module_param(mrp_join_time, uint, 0644);
  22. MODULE_PARM_DESC(mrp_join_time, "Join time in ms (default 200ms)");
  23. static unsigned int mrp_periodic_time __read_mostly = 1000;
  24. module_param(mrp_periodic_time, uint, 0644);
  25. MODULE_PARM_DESC(mrp_periodic_time, "Periodic time in ms (default 1s)");
  26. MODULE_LICENSE("GPL");
  27. static const u8
  28. mrp_applicant_state_table[MRP_APPLICANT_MAX + 1][MRP_EVENT_MAX + 1] = {
  29. [MRP_APPLICANT_VO] = {
  30. [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
  31. [MRP_EVENT_JOIN] = MRP_APPLICANT_VP,
  32. [MRP_EVENT_LV] = MRP_APPLICANT_VO,
  33. [MRP_EVENT_TX] = MRP_APPLICANT_VO,
  34. [MRP_EVENT_R_NEW] = MRP_APPLICANT_VO,
  35. [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_AO,
  36. [MRP_EVENT_R_IN] = MRP_APPLICANT_VO,
  37. [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_VO,
  38. [MRP_EVENT_R_MT] = MRP_APPLICANT_VO,
  39. [MRP_EVENT_R_LV] = MRP_APPLICANT_VO,
  40. [MRP_EVENT_R_LA] = MRP_APPLICANT_VO,
  41. [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VO,
  42. [MRP_EVENT_PERIODIC] = MRP_APPLICANT_VO,
  43. },
  44. [MRP_APPLICANT_VP] = {
  45. [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
  46. [MRP_EVENT_JOIN] = MRP_APPLICANT_VP,
  47. [MRP_EVENT_LV] = MRP_APPLICANT_VO,
  48. [MRP_EVENT_TX] = MRP_APPLICANT_AA,
  49. [MRP_EVENT_R_NEW] = MRP_APPLICANT_VP,
  50. [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_AP,
  51. [MRP_EVENT_R_IN] = MRP_APPLICANT_VP,
  52. [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_VP,
  53. [MRP_EVENT_R_MT] = MRP_APPLICANT_VP,
  54. [MRP_EVENT_R_LV] = MRP_APPLICANT_VP,
  55. [MRP_EVENT_R_LA] = MRP_APPLICANT_VP,
  56. [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP,
  57. [MRP_EVENT_PERIODIC] = MRP_APPLICANT_VP,
  58. },
  59. [MRP_APPLICANT_VN] = {
  60. [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
  61. [MRP_EVENT_JOIN] = MRP_APPLICANT_VN,
  62. [MRP_EVENT_LV] = MRP_APPLICANT_LA,
  63. [MRP_EVENT_TX] = MRP_APPLICANT_AN,
  64. [MRP_EVENT_R_NEW] = MRP_APPLICANT_VN,
  65. [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_VN,
  66. [MRP_EVENT_R_IN] = MRP_APPLICANT_VN,
  67. [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_VN,
  68. [MRP_EVENT_R_MT] = MRP_APPLICANT_VN,
  69. [MRP_EVENT_R_LV] = MRP_APPLICANT_VN,
  70. [MRP_EVENT_R_LA] = MRP_APPLICANT_VN,
  71. [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VN,
  72. [MRP_EVENT_PERIODIC] = MRP_APPLICANT_VN,
  73. },
  74. [MRP_APPLICANT_AN] = {
  75. [MRP_EVENT_NEW] = MRP_APPLICANT_AN,
  76. [MRP_EVENT_JOIN] = MRP_APPLICANT_AN,
  77. [MRP_EVENT_LV] = MRP_APPLICANT_LA,
  78. [MRP_EVENT_TX] = MRP_APPLICANT_QA,
  79. [MRP_EVENT_R_NEW] = MRP_APPLICANT_AN,
  80. [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_AN,
  81. [MRP_EVENT_R_IN] = MRP_APPLICANT_AN,
  82. [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AN,
  83. [MRP_EVENT_R_MT] = MRP_APPLICANT_AN,
  84. [MRP_EVENT_R_LV] = MRP_APPLICANT_VN,
  85. [MRP_EVENT_R_LA] = MRP_APPLICANT_VN,
  86. [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VN,
  87. [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AN,
  88. },
  89. [MRP_APPLICANT_AA] = {
  90. [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
  91. [MRP_EVENT_JOIN] = MRP_APPLICANT_AA,
  92. [MRP_EVENT_LV] = MRP_APPLICANT_LA,
  93. [MRP_EVENT_TX] = MRP_APPLICANT_QA,
  94. [MRP_EVENT_R_NEW] = MRP_APPLICANT_AA,
  95. [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QA,
  96. [MRP_EVENT_R_IN] = MRP_APPLICANT_AA,
  97. [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AA,
  98. [MRP_EVENT_R_MT] = MRP_APPLICANT_AA,
  99. [MRP_EVENT_R_LV] = MRP_APPLICANT_VP,
  100. [MRP_EVENT_R_LA] = MRP_APPLICANT_VP,
  101. [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP,
  102. [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AA,
  103. },
  104. [MRP_APPLICANT_QA] = {
  105. [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
  106. [MRP_EVENT_JOIN] = MRP_APPLICANT_QA,
  107. [MRP_EVENT_LV] = MRP_APPLICANT_LA,
  108. [MRP_EVENT_TX] = MRP_APPLICANT_QA,
  109. [MRP_EVENT_R_NEW] = MRP_APPLICANT_QA,
  110. [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QA,
  111. [MRP_EVENT_R_IN] = MRP_APPLICANT_QA,
  112. [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AA,
  113. [MRP_EVENT_R_MT] = MRP_APPLICANT_AA,
  114. [MRP_EVENT_R_LV] = MRP_APPLICANT_VP,
  115. [MRP_EVENT_R_LA] = MRP_APPLICANT_VP,
  116. [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP,
  117. [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AA,
  118. },
  119. [MRP_APPLICANT_LA] = {
  120. [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
  121. [MRP_EVENT_JOIN] = MRP_APPLICANT_AA,
  122. [MRP_EVENT_LV] = MRP_APPLICANT_LA,
  123. [MRP_EVENT_TX] = MRP_APPLICANT_VO,
  124. [MRP_EVENT_R_NEW] = MRP_APPLICANT_LA,
  125. [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_LA,
  126. [MRP_EVENT_R_IN] = MRP_APPLICANT_LA,
  127. [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_LA,
  128. [MRP_EVENT_R_MT] = MRP_APPLICANT_LA,
  129. [MRP_EVENT_R_LV] = MRP_APPLICANT_LA,
  130. [MRP_EVENT_R_LA] = MRP_APPLICANT_LA,
  131. [MRP_EVENT_REDECLARE] = MRP_APPLICANT_LA,
  132. [MRP_EVENT_PERIODIC] = MRP_APPLICANT_LA,
  133. },
  134. [MRP_APPLICANT_AO] = {
  135. [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
  136. [MRP_EVENT_JOIN] = MRP_APPLICANT_AP,
  137. [MRP_EVENT_LV] = MRP_APPLICANT_AO,
  138. [MRP_EVENT_TX] = MRP_APPLICANT_AO,
  139. [MRP_EVENT_R_NEW] = MRP_APPLICANT_AO,
  140. [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QO,
  141. [MRP_EVENT_R_IN] = MRP_APPLICANT_AO,
  142. [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AO,
  143. [MRP_EVENT_R_MT] = MRP_APPLICANT_AO,
  144. [MRP_EVENT_R_LV] = MRP_APPLICANT_VO,
  145. [MRP_EVENT_R_LA] = MRP_APPLICANT_VO,
  146. [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VO,
  147. [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AO,
  148. },
  149. [MRP_APPLICANT_QO] = {
  150. [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
  151. [MRP_EVENT_JOIN] = MRP_APPLICANT_QP,
  152. [MRP_EVENT_LV] = MRP_APPLICANT_QO,
  153. [MRP_EVENT_TX] = MRP_APPLICANT_QO,
  154. [MRP_EVENT_R_NEW] = MRP_APPLICANT_QO,
  155. [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QO,
  156. [MRP_EVENT_R_IN] = MRP_APPLICANT_QO,
  157. [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AO,
  158. [MRP_EVENT_R_MT] = MRP_APPLICANT_AO,
  159. [MRP_EVENT_R_LV] = MRP_APPLICANT_VO,
  160. [MRP_EVENT_R_LA] = MRP_APPLICANT_VO,
  161. [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VO,
  162. [MRP_EVENT_PERIODIC] = MRP_APPLICANT_QO,
  163. },
  164. [MRP_APPLICANT_AP] = {
  165. [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
  166. [MRP_EVENT_JOIN] = MRP_APPLICANT_AP,
  167. [MRP_EVENT_LV] = MRP_APPLICANT_AO,
  168. [MRP_EVENT_TX] = MRP_APPLICANT_QA,
  169. [MRP_EVENT_R_NEW] = MRP_APPLICANT_AP,
  170. [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QP,
  171. [MRP_EVENT_R_IN] = MRP_APPLICANT_AP,
  172. [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AP,
  173. [MRP_EVENT_R_MT] = MRP_APPLICANT_AP,
  174. [MRP_EVENT_R_LV] = MRP_APPLICANT_VP,
  175. [MRP_EVENT_R_LA] = MRP_APPLICANT_VP,
  176. [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP,
  177. [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AP,
  178. },
  179. [MRP_APPLICANT_QP] = {
  180. [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
  181. [MRP_EVENT_JOIN] = MRP_APPLICANT_QP,
  182. [MRP_EVENT_LV] = MRP_APPLICANT_QO,
  183. [MRP_EVENT_TX] = MRP_APPLICANT_QP,
  184. [MRP_EVENT_R_NEW] = MRP_APPLICANT_QP,
  185. [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QP,
  186. [MRP_EVENT_R_IN] = MRP_APPLICANT_QP,
  187. [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AP,
  188. [MRP_EVENT_R_MT] = MRP_APPLICANT_AP,
  189. [MRP_EVENT_R_LV] = MRP_APPLICANT_VP,
  190. [MRP_EVENT_R_LA] = MRP_APPLICANT_VP,
  191. [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP,
  192. [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AP,
  193. },
  194. };
  195. static const u8
  196. mrp_tx_action_table[MRP_APPLICANT_MAX + 1] = {
  197. [MRP_APPLICANT_VO] = MRP_TX_ACTION_S_IN_OPTIONAL,
  198. [MRP_APPLICANT_VP] = MRP_TX_ACTION_S_JOIN_IN,
  199. [MRP_APPLICANT_VN] = MRP_TX_ACTION_S_NEW,
  200. [MRP_APPLICANT_AN] = MRP_TX_ACTION_S_NEW,
  201. [MRP_APPLICANT_AA] = MRP_TX_ACTION_S_JOIN_IN,
  202. [MRP_APPLICANT_QA] = MRP_TX_ACTION_S_JOIN_IN_OPTIONAL,
  203. [MRP_APPLICANT_LA] = MRP_TX_ACTION_S_LV,
  204. [MRP_APPLICANT_AO] = MRP_TX_ACTION_S_IN_OPTIONAL,
  205. [MRP_APPLICANT_QO] = MRP_TX_ACTION_S_IN_OPTIONAL,
  206. [MRP_APPLICANT_AP] = MRP_TX_ACTION_S_JOIN_IN,
  207. [MRP_APPLICANT_QP] = MRP_TX_ACTION_S_IN_OPTIONAL,
  208. };
  209. static void mrp_attrvalue_inc(void *value, u8 len)
  210. {
  211. u8 *v = (u8 *)value;
  212. /* Add 1 to the last byte. If it becomes zero,
  213. * go to the previous byte and repeat.
  214. */
  215. while (len > 0 && !++v[--len])
  216. ;
  217. }
  218. static int mrp_attr_cmp(const struct mrp_attr *attr,
  219. const void *value, u8 len, u8 type)
  220. {
  221. if (attr->type != type)
  222. return attr->type - type;
  223. if (attr->len != len)
  224. return attr->len - len;
  225. return memcmp(attr->value, value, len);
  226. }
  227. static struct mrp_attr *mrp_attr_lookup(const struct mrp_applicant *app,
  228. const void *value, u8 len, u8 type)
  229. {
  230. struct rb_node *parent = app->mad.rb_node;
  231. struct mrp_attr *attr;
  232. int d;
  233. while (parent) {
  234. attr = rb_entry(parent, struct mrp_attr, node);
  235. d = mrp_attr_cmp(attr, value, len, type);
  236. if (d > 0)
  237. parent = parent->rb_left;
  238. else if (d < 0)
  239. parent = parent->rb_right;
  240. else
  241. return attr;
  242. }
  243. return NULL;
  244. }
  245. static struct mrp_attr *mrp_attr_create(struct mrp_applicant *app,
  246. const void *value, u8 len, u8 type)
  247. {
  248. struct rb_node *parent = NULL, **p = &app->mad.rb_node;
  249. struct mrp_attr *attr;
  250. int d;
  251. while (*p) {
  252. parent = *p;
  253. attr = rb_entry(parent, struct mrp_attr, node);
  254. d = mrp_attr_cmp(attr, value, len, type);
  255. if (d > 0)
  256. p = &parent->rb_left;
  257. else if (d < 0)
  258. p = &parent->rb_right;
  259. else {
  260. /* The attribute already exists; re-use it. */
  261. return attr;
  262. }
  263. }
  264. attr = kmalloc(sizeof(*attr) + len, GFP_ATOMIC);
  265. if (!attr)
  266. return attr;
  267. attr->state = MRP_APPLICANT_VO;
  268. attr->type = type;
  269. attr->len = len;
  270. memcpy(attr->value, value, len);
  271. rb_link_node(&attr->node, parent, p);
  272. rb_insert_color(&attr->node, &app->mad);
  273. return attr;
  274. }
  275. static void mrp_attr_destroy(struct mrp_applicant *app, struct mrp_attr *attr)
  276. {
  277. rb_erase(&attr->node, &app->mad);
  278. kfree(attr);
  279. }
  280. static void mrp_attr_destroy_all(struct mrp_applicant *app)
  281. {
  282. struct rb_node *node, *next;
  283. struct mrp_attr *attr;
  284. for (node = rb_first(&app->mad);
  285. next = node ? rb_next(node) : NULL, node != NULL;
  286. node = next) {
  287. attr = rb_entry(node, struct mrp_attr, node);
  288. mrp_attr_destroy(app, attr);
  289. }
  290. }
  291. static int mrp_pdu_init(struct mrp_applicant *app)
  292. {
  293. struct sk_buff *skb;
  294. struct mrp_pdu_hdr *ph;
  295. skb = alloc_skb(app->dev->mtu + LL_RESERVED_SPACE(app->dev),
  296. GFP_ATOMIC);
  297. if (!skb)
  298. return -ENOMEM;
  299. skb->dev = app->dev;
  300. skb->protocol = app->app->pkttype.type;
  301. skb_reserve(skb, LL_RESERVED_SPACE(app->dev));
  302. skb_reset_network_header(skb);
  303. skb_reset_transport_header(skb);
  304. ph = __skb_put(skb, sizeof(*ph));
  305. ph->version = app->app->version;
  306. app->pdu = skb;
  307. return 0;
  308. }
  309. static int mrp_pdu_append_end_mark(struct mrp_applicant *app)
  310. {
  311. __be16 *endmark;
  312. if (skb_tailroom(app->pdu) < sizeof(*endmark))
  313. return -1;
  314. endmark = __skb_put(app->pdu, sizeof(*endmark));
  315. put_unaligned(MRP_END_MARK, endmark);
  316. return 0;
  317. }
  318. static void mrp_pdu_queue(struct mrp_applicant *app)
  319. {
  320. if (!app->pdu)
  321. return;
  322. if (mrp_cb(app->pdu)->mh)
  323. mrp_pdu_append_end_mark(app);
  324. mrp_pdu_append_end_mark(app);
  325. dev_hard_header(app->pdu, app->dev, ntohs(app->app->pkttype.type),
  326. app->app->group_address, app->dev->dev_addr,
  327. app->pdu->len);
  328. skb_queue_tail(&app->queue, app->pdu);
  329. app->pdu = NULL;
  330. }
  331. static void mrp_queue_xmit(struct mrp_applicant *app)
  332. {
  333. struct sk_buff *skb;
  334. while ((skb = skb_dequeue(&app->queue)))
  335. dev_queue_xmit(skb);
  336. }
  337. static int mrp_pdu_append_msg_hdr(struct mrp_applicant *app,
  338. u8 attrtype, u8 attrlen)
  339. {
  340. struct mrp_msg_hdr *mh;
  341. if (mrp_cb(app->pdu)->mh) {
  342. if (mrp_pdu_append_end_mark(app) < 0)
  343. return -1;
  344. mrp_cb(app->pdu)->mh = NULL;
  345. mrp_cb(app->pdu)->vah = NULL;
  346. }
  347. if (skb_tailroom(app->pdu) < sizeof(*mh))
  348. return -1;
  349. mh = __skb_put(app->pdu, sizeof(*mh));
  350. mh->attrtype = attrtype;
  351. mh->attrlen = attrlen;
  352. mrp_cb(app->pdu)->mh = mh;
  353. return 0;
  354. }
  355. static int mrp_pdu_append_vecattr_hdr(struct mrp_applicant *app,
  356. const void *firstattrvalue, u8 attrlen)
  357. {
  358. struct mrp_vecattr_hdr *vah;
  359. if (skb_tailroom(app->pdu) < sizeof(*vah) + attrlen)
  360. return -1;
  361. vah = __skb_put(app->pdu, sizeof(*vah) + attrlen);
  362. put_unaligned(0, &vah->lenflags);
  363. memcpy(vah->firstattrvalue, firstattrvalue, attrlen);
  364. mrp_cb(app->pdu)->vah = vah;
  365. memcpy(mrp_cb(app->pdu)->attrvalue, firstattrvalue, attrlen);
  366. return 0;
  367. }
  368. static int mrp_pdu_append_vecattr_event(struct mrp_applicant *app,
  369. const struct mrp_attr *attr,
  370. enum mrp_vecattr_event vaevent)
  371. {
  372. u16 len, pos;
  373. u8 *vaevents;
  374. int err;
  375. again:
  376. if (!app->pdu) {
  377. err = mrp_pdu_init(app);
  378. if (err < 0)
  379. return err;
  380. }
  381. /* If there is no Message header in the PDU, or the Message header is
  382. * for a different attribute type, add an EndMark (if necessary) and a
  383. * new Message header to the PDU.
  384. */
  385. if (!mrp_cb(app->pdu)->mh ||
  386. mrp_cb(app->pdu)->mh->attrtype != attr->type ||
  387. mrp_cb(app->pdu)->mh->attrlen != attr->len) {
  388. if (mrp_pdu_append_msg_hdr(app, attr->type, attr->len) < 0)
  389. goto queue;
  390. }
  391. /* If there is no VectorAttribute header for this Message in the PDU,
  392. * or this attribute's value does not sequentially follow the previous
  393. * attribute's value, add a new VectorAttribute header to the PDU.
  394. */
  395. if (!mrp_cb(app->pdu)->vah ||
  396. memcmp(mrp_cb(app->pdu)->attrvalue, attr->value, attr->len)) {
  397. if (mrp_pdu_append_vecattr_hdr(app, attr->value, attr->len) < 0)
  398. goto queue;
  399. }
  400. len = be16_to_cpu(get_unaligned(&mrp_cb(app->pdu)->vah->lenflags));
  401. pos = len % 3;
  402. /* Events are packed into Vectors in the PDU, three to a byte. Add a
  403. * byte to the end of the Vector if necessary.
  404. */
  405. if (!pos) {
  406. if (skb_tailroom(app->pdu) < sizeof(u8))
  407. goto queue;
  408. vaevents = __skb_put(app->pdu, sizeof(u8));
  409. } else {
  410. vaevents = (u8 *)(skb_tail_pointer(app->pdu) - sizeof(u8));
  411. }
  412. switch (pos) {
  413. case 0:
  414. *vaevents = vaevent * (__MRP_VECATTR_EVENT_MAX *
  415. __MRP_VECATTR_EVENT_MAX);
  416. break;
  417. case 1:
  418. *vaevents += vaevent * __MRP_VECATTR_EVENT_MAX;
  419. break;
  420. case 2:
  421. *vaevents += vaevent;
  422. break;
  423. default:
  424. WARN_ON(1);
  425. }
  426. /* Increment the length of the VectorAttribute in the PDU, as well as
  427. * the value of the next attribute that would continue its Vector.
  428. */
  429. put_unaligned(cpu_to_be16(++len), &mrp_cb(app->pdu)->vah->lenflags);
  430. mrp_attrvalue_inc(mrp_cb(app->pdu)->attrvalue, attr->len);
  431. return 0;
  432. queue:
  433. mrp_pdu_queue(app);
  434. goto again;
  435. }
  436. static void mrp_attr_event(struct mrp_applicant *app,
  437. struct mrp_attr *attr, enum mrp_event event)
  438. {
  439. enum mrp_applicant_state state;
  440. state = mrp_applicant_state_table[attr->state][event];
  441. if (state == MRP_APPLICANT_INVALID) {
  442. WARN_ON(1);
  443. return;
  444. }
  445. if (event == MRP_EVENT_TX) {
  446. /* When appending the attribute fails, don't update its state
  447. * in order to retry at the next TX event.
  448. */
  449. switch (mrp_tx_action_table[attr->state]) {
  450. case MRP_TX_ACTION_NONE:
  451. case MRP_TX_ACTION_S_JOIN_IN_OPTIONAL:
  452. case MRP_TX_ACTION_S_IN_OPTIONAL:
  453. break;
  454. case MRP_TX_ACTION_S_NEW:
  455. if (mrp_pdu_append_vecattr_event(
  456. app, attr, MRP_VECATTR_EVENT_NEW) < 0)
  457. return;
  458. break;
  459. case MRP_TX_ACTION_S_JOIN_IN:
  460. if (mrp_pdu_append_vecattr_event(
  461. app, attr, MRP_VECATTR_EVENT_JOIN_IN) < 0)
  462. return;
  463. break;
  464. case MRP_TX_ACTION_S_LV:
  465. if (mrp_pdu_append_vecattr_event(
  466. app, attr, MRP_VECATTR_EVENT_LV) < 0)
  467. return;
  468. /* As a pure applicant, sending a leave message
  469. * implies that the attribute was unregistered and
  470. * can be destroyed.
  471. */
  472. mrp_attr_destroy(app, attr);
  473. return;
  474. default:
  475. WARN_ON(1);
  476. }
  477. }
  478. attr->state = state;
  479. }
  480. int mrp_request_join(const struct net_device *dev,
  481. const struct mrp_application *appl,
  482. const void *value, u8 len, u8 type)
  483. {
  484. struct mrp_port *port = rtnl_dereference(dev->mrp_port);
  485. struct mrp_applicant *app = rtnl_dereference(
  486. port->applicants[appl->type]);
  487. struct mrp_attr *attr;
  488. if (sizeof(struct mrp_skb_cb) + len >
  489. sizeof_field(struct sk_buff, cb))
  490. return -ENOMEM;
  491. spin_lock_bh(&app->lock);
  492. attr = mrp_attr_create(app, value, len, type);
  493. if (!attr) {
  494. spin_unlock_bh(&app->lock);
  495. return -ENOMEM;
  496. }
  497. mrp_attr_event(app, attr, MRP_EVENT_JOIN);
  498. spin_unlock_bh(&app->lock);
  499. return 0;
  500. }
  501. EXPORT_SYMBOL_GPL(mrp_request_join);
  502. void mrp_request_leave(const struct net_device *dev,
  503. const struct mrp_application *appl,
  504. const void *value, u8 len, u8 type)
  505. {
  506. struct mrp_port *port = rtnl_dereference(dev->mrp_port);
  507. struct mrp_applicant *app = rtnl_dereference(
  508. port->applicants[appl->type]);
  509. struct mrp_attr *attr;
  510. if (sizeof(struct mrp_skb_cb) + len >
  511. sizeof_field(struct sk_buff, cb))
  512. return;
  513. spin_lock_bh(&app->lock);
  514. attr = mrp_attr_lookup(app, value, len, type);
  515. if (!attr) {
  516. spin_unlock_bh(&app->lock);
  517. return;
  518. }
  519. mrp_attr_event(app, attr, MRP_EVENT_LV);
  520. spin_unlock_bh(&app->lock);
  521. }
  522. EXPORT_SYMBOL_GPL(mrp_request_leave);
  523. static void mrp_mad_event(struct mrp_applicant *app, enum mrp_event event)
  524. {
  525. struct rb_node *node, *next;
  526. struct mrp_attr *attr;
  527. for (node = rb_first(&app->mad);
  528. next = node ? rb_next(node) : NULL, node != NULL;
  529. node = next) {
  530. attr = rb_entry(node, struct mrp_attr, node);
  531. mrp_attr_event(app, attr, event);
  532. }
  533. }
  534. static void mrp_join_timer_arm(struct mrp_applicant *app)
  535. {
  536. unsigned long delay;
  537. delay = (u64)msecs_to_jiffies(mrp_join_time) * prandom_u32() >> 32;
  538. mod_timer(&app->join_timer, jiffies + delay);
  539. }
  540. static void mrp_join_timer(struct timer_list *t)
  541. {
  542. struct mrp_applicant *app = from_timer(app, t, join_timer);
  543. spin_lock(&app->lock);
  544. mrp_mad_event(app, MRP_EVENT_TX);
  545. mrp_pdu_queue(app);
  546. spin_unlock(&app->lock);
  547. mrp_queue_xmit(app);
  548. mrp_join_timer_arm(app);
  549. }
  550. static void mrp_periodic_timer_arm(struct mrp_applicant *app)
  551. {
  552. mod_timer(&app->periodic_timer,
  553. jiffies + msecs_to_jiffies(mrp_periodic_time));
  554. }
  555. static void mrp_periodic_timer(struct timer_list *t)
  556. {
  557. struct mrp_applicant *app = from_timer(app, t, periodic_timer);
  558. spin_lock(&app->lock);
  559. mrp_mad_event(app, MRP_EVENT_PERIODIC);
  560. mrp_pdu_queue(app);
  561. spin_unlock(&app->lock);
  562. mrp_periodic_timer_arm(app);
  563. }
  564. static int mrp_pdu_parse_end_mark(struct sk_buff *skb, int *offset)
  565. {
  566. __be16 endmark;
  567. if (skb_copy_bits(skb, *offset, &endmark, sizeof(endmark)) < 0)
  568. return -1;
  569. if (endmark == MRP_END_MARK) {
  570. *offset += sizeof(endmark);
  571. return -1;
  572. }
  573. return 0;
  574. }
  575. static void mrp_pdu_parse_vecattr_event(struct mrp_applicant *app,
  576. struct sk_buff *skb,
  577. enum mrp_vecattr_event vaevent)
  578. {
  579. struct mrp_attr *attr;
  580. enum mrp_event event;
  581. attr = mrp_attr_lookup(app, mrp_cb(skb)->attrvalue,
  582. mrp_cb(skb)->mh->attrlen,
  583. mrp_cb(skb)->mh->attrtype);
  584. if (attr == NULL)
  585. return;
  586. switch (vaevent) {
  587. case MRP_VECATTR_EVENT_NEW:
  588. event = MRP_EVENT_R_NEW;
  589. break;
  590. case MRP_VECATTR_EVENT_JOIN_IN:
  591. event = MRP_EVENT_R_JOIN_IN;
  592. break;
  593. case MRP_VECATTR_EVENT_IN:
  594. event = MRP_EVENT_R_IN;
  595. break;
  596. case MRP_VECATTR_EVENT_JOIN_MT:
  597. event = MRP_EVENT_R_JOIN_MT;
  598. break;
  599. case MRP_VECATTR_EVENT_MT:
  600. event = MRP_EVENT_R_MT;
  601. break;
  602. case MRP_VECATTR_EVENT_LV:
  603. event = MRP_EVENT_R_LV;
  604. break;
  605. default:
  606. return;
  607. }
  608. mrp_attr_event(app, attr, event);
  609. }
  610. static int mrp_pdu_parse_vecattr(struct mrp_applicant *app,
  611. struct sk_buff *skb, int *offset)
  612. {
  613. struct mrp_vecattr_hdr _vah;
  614. u16 valen;
  615. u8 vaevents, vaevent;
  616. mrp_cb(skb)->vah = skb_header_pointer(skb, *offset, sizeof(_vah),
  617. &_vah);
  618. if (!mrp_cb(skb)->vah)
  619. return -1;
  620. *offset += sizeof(_vah);
  621. if (get_unaligned(&mrp_cb(skb)->vah->lenflags) &
  622. MRP_VECATTR_HDR_FLAG_LA)
  623. mrp_mad_event(app, MRP_EVENT_R_LA);
  624. valen = be16_to_cpu(get_unaligned(&mrp_cb(skb)->vah->lenflags) &
  625. MRP_VECATTR_HDR_LEN_MASK);
  626. /* The VectorAttribute structure in a PDU carries event information
  627. * about one or more attributes having consecutive values. Only the
  628. * value for the first attribute is contained in the structure. So
  629. * we make a copy of that value, and then increment it each time we
  630. * advance to the next event in its Vector.
  631. */
  632. if (sizeof(struct mrp_skb_cb) + mrp_cb(skb)->mh->attrlen >
  633. sizeof_field(struct sk_buff, cb))
  634. return -1;
  635. if (skb_copy_bits(skb, *offset, mrp_cb(skb)->attrvalue,
  636. mrp_cb(skb)->mh->attrlen) < 0)
  637. return -1;
  638. *offset += mrp_cb(skb)->mh->attrlen;
  639. /* In a VectorAttribute, the Vector contains events which are packed
  640. * three to a byte. We process one byte of the Vector at a time.
  641. */
  642. while (valen > 0) {
  643. if (skb_copy_bits(skb, *offset, &vaevents,
  644. sizeof(vaevents)) < 0)
  645. return -1;
  646. *offset += sizeof(vaevents);
  647. /* Extract and process the first event. */
  648. vaevent = vaevents / (__MRP_VECATTR_EVENT_MAX *
  649. __MRP_VECATTR_EVENT_MAX);
  650. if (vaevent >= __MRP_VECATTR_EVENT_MAX) {
  651. /* The byte is malformed; stop processing. */
  652. return -1;
  653. }
  654. mrp_pdu_parse_vecattr_event(app, skb, vaevent);
  655. /* If present, extract and process the second event. */
  656. if (!--valen)
  657. break;
  658. mrp_attrvalue_inc(mrp_cb(skb)->attrvalue,
  659. mrp_cb(skb)->mh->attrlen);
  660. vaevents %= (__MRP_VECATTR_EVENT_MAX *
  661. __MRP_VECATTR_EVENT_MAX);
  662. vaevent = vaevents / __MRP_VECATTR_EVENT_MAX;
  663. mrp_pdu_parse_vecattr_event(app, skb, vaevent);
  664. /* If present, extract and process the third event. */
  665. if (!--valen)
  666. break;
  667. mrp_attrvalue_inc(mrp_cb(skb)->attrvalue,
  668. mrp_cb(skb)->mh->attrlen);
  669. vaevents %= __MRP_VECATTR_EVENT_MAX;
  670. vaevent = vaevents;
  671. mrp_pdu_parse_vecattr_event(app, skb, vaevent);
  672. }
  673. return 0;
  674. }
  675. static int mrp_pdu_parse_msg(struct mrp_applicant *app, struct sk_buff *skb,
  676. int *offset)
  677. {
  678. struct mrp_msg_hdr _mh;
  679. mrp_cb(skb)->mh = skb_header_pointer(skb, *offset, sizeof(_mh), &_mh);
  680. if (!mrp_cb(skb)->mh)
  681. return -1;
  682. *offset += sizeof(_mh);
  683. if (mrp_cb(skb)->mh->attrtype == 0 ||
  684. mrp_cb(skb)->mh->attrtype > app->app->maxattr ||
  685. mrp_cb(skb)->mh->attrlen == 0)
  686. return -1;
  687. while (skb->len > *offset) {
  688. if (mrp_pdu_parse_end_mark(skb, offset) < 0)
  689. break;
  690. if (mrp_pdu_parse_vecattr(app, skb, offset) < 0)
  691. return -1;
  692. }
  693. return 0;
  694. }
  695. static int mrp_rcv(struct sk_buff *skb, struct net_device *dev,
  696. struct packet_type *pt, struct net_device *orig_dev)
  697. {
  698. struct mrp_application *appl = container_of(pt, struct mrp_application,
  699. pkttype);
  700. struct mrp_port *port;
  701. struct mrp_applicant *app;
  702. struct mrp_pdu_hdr _ph;
  703. const struct mrp_pdu_hdr *ph;
  704. int offset = skb_network_offset(skb);
  705. /* If the interface is in promiscuous mode, drop the packet if
  706. * it was unicast to another host.
  707. */
  708. if (unlikely(skb->pkt_type == PACKET_OTHERHOST))
  709. goto out;
  710. skb = skb_share_check(skb, GFP_ATOMIC);
  711. if (unlikely(!skb))
  712. goto out;
  713. port = rcu_dereference(dev->mrp_port);
  714. if (unlikely(!port))
  715. goto out;
  716. app = rcu_dereference(port->applicants[appl->type]);
  717. if (unlikely(!app))
  718. goto out;
  719. ph = skb_header_pointer(skb, offset, sizeof(_ph), &_ph);
  720. if (!ph)
  721. goto out;
  722. offset += sizeof(_ph);
  723. if (ph->version != app->app->version)
  724. goto out;
  725. spin_lock(&app->lock);
  726. while (skb->len > offset) {
  727. if (mrp_pdu_parse_end_mark(skb, &offset) < 0)
  728. break;
  729. if (mrp_pdu_parse_msg(app, skb, &offset) < 0)
  730. break;
  731. }
  732. spin_unlock(&app->lock);
  733. out:
  734. kfree_skb(skb);
  735. return 0;
  736. }
  737. static int mrp_init_port(struct net_device *dev)
  738. {
  739. struct mrp_port *port;
  740. port = kzalloc(sizeof(*port), GFP_KERNEL);
  741. if (!port)
  742. return -ENOMEM;
  743. rcu_assign_pointer(dev->mrp_port, port);
  744. return 0;
  745. }
  746. static void mrp_release_port(struct net_device *dev)
  747. {
  748. struct mrp_port *port = rtnl_dereference(dev->mrp_port);
  749. unsigned int i;
  750. for (i = 0; i <= MRP_APPLICATION_MAX; i++) {
  751. if (rtnl_dereference(port->applicants[i]))
  752. return;
  753. }
  754. RCU_INIT_POINTER(dev->mrp_port, NULL);
  755. kfree_rcu(port, rcu);
  756. }
  757. int mrp_init_applicant(struct net_device *dev, struct mrp_application *appl)
  758. {
  759. struct mrp_applicant *app;
  760. int err;
  761. ASSERT_RTNL();
  762. if (!rtnl_dereference(dev->mrp_port)) {
  763. err = mrp_init_port(dev);
  764. if (err < 0)
  765. goto err1;
  766. }
  767. err = -ENOMEM;
  768. app = kzalloc(sizeof(*app), GFP_KERNEL);
  769. if (!app)
  770. goto err2;
  771. err = dev_mc_add(dev, appl->group_address);
  772. if (err < 0)
  773. goto err3;
  774. app->dev = dev;
  775. app->app = appl;
  776. app->mad = RB_ROOT;
  777. spin_lock_init(&app->lock);
  778. skb_queue_head_init(&app->queue);
  779. rcu_assign_pointer(dev->mrp_port->applicants[appl->type], app);
  780. timer_setup(&app->join_timer, mrp_join_timer, 0);
  781. mrp_join_timer_arm(app);
  782. timer_setup(&app->periodic_timer, mrp_periodic_timer, 0);
  783. mrp_periodic_timer_arm(app);
  784. return 0;
  785. err3:
  786. kfree(app);
  787. err2:
  788. mrp_release_port(dev);
  789. err1:
  790. return err;
  791. }
  792. EXPORT_SYMBOL_GPL(mrp_init_applicant);
  793. void mrp_uninit_applicant(struct net_device *dev, struct mrp_application *appl)
  794. {
  795. struct mrp_port *port = rtnl_dereference(dev->mrp_port);
  796. struct mrp_applicant *app = rtnl_dereference(
  797. port->applicants[appl->type]);
  798. ASSERT_RTNL();
  799. RCU_INIT_POINTER(port->applicants[appl->type], NULL);
  800. /* Delete timer and generate a final TX event to flush out
  801. * all pending messages before the applicant is gone.
  802. */
  803. del_timer_sync(&app->join_timer);
  804. del_timer_sync(&app->periodic_timer);
  805. spin_lock_bh(&app->lock);
  806. mrp_mad_event(app, MRP_EVENT_TX);
  807. mrp_attr_destroy_all(app);
  808. mrp_pdu_queue(app);
  809. spin_unlock_bh(&app->lock);
  810. mrp_queue_xmit(app);
  811. dev_mc_del(dev, appl->group_address);
  812. kfree_rcu(app, rcu);
  813. mrp_release_port(dev);
  814. }
  815. EXPORT_SYMBOL_GPL(mrp_uninit_applicant);
  816. int mrp_register_application(struct mrp_application *appl)
  817. {
  818. appl->pkttype.func = mrp_rcv;
  819. dev_add_pack(&appl->pkttype);
  820. return 0;
  821. }
  822. EXPORT_SYMBOL_GPL(mrp_register_application);
  823. void mrp_unregister_application(struct mrp_application *appl)
  824. {
  825. dev_remove_pack(&appl->pkttype);
  826. }
  827. EXPORT_SYMBOL_GPL(mrp_unregister_application);