1
2
3
4
5
6
7
8
9
10
11
12
13#include <linux/kernel.h>
14#include <linux/timer.h>
15#include <linux/skbuff.h>
16#include <linux/netdevice.h>
17#include <linux/etherdevice.h>
18#include <linux/rtnetlink.h>
19#include <linux/slab.h>
20#include <linux/module.h>
21#include <net/mrp.h>
22#include <asm/unaligned.h>
23
24static unsigned int mrp_join_time __read_mostly = 200;
25module_param(mrp_join_time, uint, 0644);
26MODULE_PARM_DESC(mrp_join_time, "Join time in ms (default 200ms)");
27
28static unsigned int mrp_periodic_time __read_mostly = 1000;
29module_param(mrp_periodic_time, uint, 0644);
30MODULE_PARM_DESC(mrp_periodic_time, "Periodic time in ms (default 1s)");
31
32MODULE_LICENSE("GPL");
33
34static const u8
35mrp_applicant_state_table[MRP_APPLICANT_MAX + 1][MRP_EVENT_MAX + 1] = {
36 [MRP_APPLICANT_VO] = {
37 [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
38 [MRP_EVENT_JOIN] = MRP_APPLICANT_VP,
39 [MRP_EVENT_LV] = MRP_APPLICANT_VO,
40 [MRP_EVENT_TX] = MRP_APPLICANT_VO,
41 [MRP_EVENT_R_NEW] = MRP_APPLICANT_VO,
42 [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_AO,
43 [MRP_EVENT_R_IN] = MRP_APPLICANT_VO,
44 [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_VO,
45 [MRP_EVENT_R_MT] = MRP_APPLICANT_VO,
46 [MRP_EVENT_R_LV] = MRP_APPLICANT_VO,
47 [MRP_EVENT_R_LA] = MRP_APPLICANT_VO,
48 [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VO,
49 [MRP_EVENT_PERIODIC] = MRP_APPLICANT_VO,
50 },
51 [MRP_APPLICANT_VP] = {
52 [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
53 [MRP_EVENT_JOIN] = MRP_APPLICANT_VP,
54 [MRP_EVENT_LV] = MRP_APPLICANT_VO,
55 [MRP_EVENT_TX] = MRP_APPLICANT_AA,
56 [MRP_EVENT_R_NEW] = MRP_APPLICANT_VP,
57 [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_AP,
58 [MRP_EVENT_R_IN] = MRP_APPLICANT_VP,
59 [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_VP,
60 [MRP_EVENT_R_MT] = MRP_APPLICANT_VP,
61 [MRP_EVENT_R_LV] = MRP_APPLICANT_VP,
62 [MRP_EVENT_R_LA] = MRP_APPLICANT_VP,
63 [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP,
64 [MRP_EVENT_PERIODIC] = MRP_APPLICANT_VP,
65 },
66 [MRP_APPLICANT_VN] = {
67 [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
68 [MRP_EVENT_JOIN] = MRP_APPLICANT_VN,
69 [MRP_EVENT_LV] = MRP_APPLICANT_LA,
70 [MRP_EVENT_TX] = MRP_APPLICANT_AN,
71 [MRP_EVENT_R_NEW] = MRP_APPLICANT_VN,
72 [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_VN,
73 [MRP_EVENT_R_IN] = MRP_APPLICANT_VN,
74 [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_VN,
75 [MRP_EVENT_R_MT] = MRP_APPLICANT_VN,
76 [MRP_EVENT_R_LV] = MRP_APPLICANT_VN,
77 [MRP_EVENT_R_LA] = MRP_APPLICANT_VN,
78 [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VN,
79 [MRP_EVENT_PERIODIC] = MRP_APPLICANT_VN,
80 },
81 [MRP_APPLICANT_AN] = {
82 [MRP_EVENT_NEW] = MRP_APPLICANT_AN,
83 [MRP_EVENT_JOIN] = MRP_APPLICANT_AN,
84 [MRP_EVENT_LV] = MRP_APPLICANT_LA,
85 [MRP_EVENT_TX] = MRP_APPLICANT_QA,
86 [MRP_EVENT_R_NEW] = MRP_APPLICANT_AN,
87 [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_AN,
88 [MRP_EVENT_R_IN] = MRP_APPLICANT_AN,
89 [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AN,
90 [MRP_EVENT_R_MT] = MRP_APPLICANT_AN,
91 [MRP_EVENT_R_LV] = MRP_APPLICANT_VN,
92 [MRP_EVENT_R_LA] = MRP_APPLICANT_VN,
93 [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VN,
94 [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AN,
95 },
96 [MRP_APPLICANT_AA] = {
97 [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
98 [MRP_EVENT_JOIN] = MRP_APPLICANT_AA,
99 [MRP_EVENT_LV] = MRP_APPLICANT_LA,
100 [MRP_EVENT_TX] = MRP_APPLICANT_QA,
101 [MRP_EVENT_R_NEW] = MRP_APPLICANT_AA,
102 [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QA,
103 [MRP_EVENT_R_IN] = MRP_APPLICANT_AA,
104 [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AA,
105 [MRP_EVENT_R_MT] = MRP_APPLICANT_AA,
106 [MRP_EVENT_R_LV] = MRP_APPLICANT_VP,
107 [MRP_EVENT_R_LA] = MRP_APPLICANT_VP,
108 [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP,
109 [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AA,
110 },
111 [MRP_APPLICANT_QA] = {
112 [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
113 [MRP_EVENT_JOIN] = MRP_APPLICANT_QA,
114 [MRP_EVENT_LV] = MRP_APPLICANT_LA,
115 [MRP_EVENT_TX] = MRP_APPLICANT_QA,
116 [MRP_EVENT_R_NEW] = MRP_APPLICANT_QA,
117 [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QA,
118 [MRP_EVENT_R_IN] = MRP_APPLICANT_QA,
119 [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AA,
120 [MRP_EVENT_R_MT] = MRP_APPLICANT_AA,
121 [MRP_EVENT_R_LV] = MRP_APPLICANT_VP,
122 [MRP_EVENT_R_LA] = MRP_APPLICANT_VP,
123 [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP,
124 [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AA,
125 },
126 [MRP_APPLICANT_LA] = {
127 [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
128 [MRP_EVENT_JOIN] = MRP_APPLICANT_AA,
129 [MRP_EVENT_LV] = MRP_APPLICANT_LA,
130 [MRP_EVENT_TX] = MRP_APPLICANT_VO,
131 [MRP_EVENT_R_NEW] = MRP_APPLICANT_LA,
132 [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_LA,
133 [MRP_EVENT_R_IN] = MRP_APPLICANT_LA,
134 [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_LA,
135 [MRP_EVENT_R_MT] = MRP_APPLICANT_LA,
136 [MRP_EVENT_R_LV] = MRP_APPLICANT_LA,
137 [MRP_EVENT_R_LA] = MRP_APPLICANT_LA,
138 [MRP_EVENT_REDECLARE] = MRP_APPLICANT_LA,
139 [MRP_EVENT_PERIODIC] = MRP_APPLICANT_LA,
140 },
141 [MRP_APPLICANT_AO] = {
142 [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
143 [MRP_EVENT_JOIN] = MRP_APPLICANT_AP,
144 [MRP_EVENT_LV] = MRP_APPLICANT_AO,
145 [MRP_EVENT_TX] = MRP_APPLICANT_AO,
146 [MRP_EVENT_R_NEW] = MRP_APPLICANT_AO,
147 [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QO,
148 [MRP_EVENT_R_IN] = MRP_APPLICANT_AO,
149 [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AO,
150 [MRP_EVENT_R_MT] = MRP_APPLICANT_AO,
151 [MRP_EVENT_R_LV] = MRP_APPLICANT_VO,
152 [MRP_EVENT_R_LA] = MRP_APPLICANT_VO,
153 [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VO,
154 [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AO,
155 },
156 [MRP_APPLICANT_QO] = {
157 [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
158 [MRP_EVENT_JOIN] = MRP_APPLICANT_QP,
159 [MRP_EVENT_LV] = MRP_APPLICANT_QO,
160 [MRP_EVENT_TX] = MRP_APPLICANT_QO,
161 [MRP_EVENT_R_NEW] = MRP_APPLICANT_QO,
162 [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QO,
163 [MRP_EVENT_R_IN] = MRP_APPLICANT_QO,
164 [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AO,
165 [MRP_EVENT_R_MT] = MRP_APPLICANT_AO,
166 [MRP_EVENT_R_LV] = MRP_APPLICANT_VO,
167 [MRP_EVENT_R_LA] = MRP_APPLICANT_VO,
168 [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VO,
169 [MRP_EVENT_PERIODIC] = MRP_APPLICANT_QO,
170 },
171 [MRP_APPLICANT_AP] = {
172 [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
173 [MRP_EVENT_JOIN] = MRP_APPLICANT_AP,
174 [MRP_EVENT_LV] = MRP_APPLICANT_AO,
175 [MRP_EVENT_TX] = MRP_APPLICANT_QA,
176 [MRP_EVENT_R_NEW] = MRP_APPLICANT_AP,
177 [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QP,
178 [MRP_EVENT_R_IN] = MRP_APPLICANT_AP,
179 [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AP,
180 [MRP_EVENT_R_MT] = MRP_APPLICANT_AP,
181 [MRP_EVENT_R_LV] = MRP_APPLICANT_VP,
182 [MRP_EVENT_R_LA] = MRP_APPLICANT_VP,
183 [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP,
184 [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AP,
185 },
186 [MRP_APPLICANT_QP] = {
187 [MRP_EVENT_NEW] = MRP_APPLICANT_VN,
188 [MRP_EVENT_JOIN] = MRP_APPLICANT_QP,
189 [MRP_EVENT_LV] = MRP_APPLICANT_QO,
190 [MRP_EVENT_TX] = MRP_APPLICANT_QP,
191 [MRP_EVENT_R_NEW] = MRP_APPLICANT_QP,
192 [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QP,
193 [MRP_EVENT_R_IN] = MRP_APPLICANT_QP,
194 [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AP,
195 [MRP_EVENT_R_MT] = MRP_APPLICANT_AP,
196 [MRP_EVENT_R_LV] = MRP_APPLICANT_VP,
197 [MRP_EVENT_R_LA] = MRP_APPLICANT_VP,
198 [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP,
199 [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AP,
200 },
201};
202
203static const u8
204mrp_tx_action_table[MRP_APPLICANT_MAX + 1] = {
205 [MRP_APPLICANT_VO] = MRP_TX_ACTION_S_IN_OPTIONAL,
206 [MRP_APPLICANT_VP] = MRP_TX_ACTION_S_JOIN_IN,
207 [MRP_APPLICANT_VN] = MRP_TX_ACTION_S_NEW,
208 [MRP_APPLICANT_AN] = MRP_TX_ACTION_S_NEW,
209 [MRP_APPLICANT_AA] = MRP_TX_ACTION_S_JOIN_IN,
210 [MRP_APPLICANT_QA] = MRP_TX_ACTION_S_JOIN_IN_OPTIONAL,
211 [MRP_APPLICANT_LA] = MRP_TX_ACTION_S_LV,
212 [MRP_APPLICANT_AO] = MRP_TX_ACTION_S_IN_OPTIONAL,
213 [MRP_APPLICANT_QO] = MRP_TX_ACTION_S_IN_OPTIONAL,
214 [MRP_APPLICANT_AP] = MRP_TX_ACTION_S_JOIN_IN,
215 [MRP_APPLICANT_QP] = MRP_TX_ACTION_S_IN_OPTIONAL,
216};
217
218static void mrp_attrvalue_inc(void *value, u8 len)
219{
220 u8 *v = (u8 *)value;
221
222
223
224
225 while (len > 0 && !++v[--len])
226 ;
227}
228
229static int mrp_attr_cmp(const struct mrp_attr *attr,
230 const void *value, u8 len, u8 type)
231{
232 if (attr->type != type)
233 return attr->type - type;
234 if (attr->len != len)
235 return attr->len - len;
236 return memcmp(attr->value, value, len);
237}
238
239static struct mrp_attr *mrp_attr_lookup(const struct mrp_applicant *app,
240 const void *value, u8 len, u8 type)
241{
242 struct rb_node *parent = app->mad.rb_node;
243 struct mrp_attr *attr;
244 int d;
245
246 while (parent) {
247 attr = rb_entry(parent, struct mrp_attr, node);
248 d = mrp_attr_cmp(attr, value, len, type);
249 if (d > 0)
250 parent = parent->rb_left;
251 else if (d < 0)
252 parent = parent->rb_right;
253 else
254 return attr;
255 }
256 return NULL;
257}
258
259static struct mrp_attr *mrp_attr_create(struct mrp_applicant *app,
260 const void *value, u8 len, u8 type)
261{
262 struct rb_node *parent = NULL, **p = &app->mad.rb_node;
263 struct mrp_attr *attr;
264 int d;
265
266 while (*p) {
267 parent = *p;
268 attr = rb_entry(parent, struct mrp_attr, node);
269 d = mrp_attr_cmp(attr, value, len, type);
270 if (d > 0)
271 p = &parent->rb_left;
272 else if (d < 0)
273 p = &parent->rb_right;
274 else {
275
276 return attr;
277 }
278 }
279 attr = kmalloc(sizeof(*attr) + len, GFP_ATOMIC);
280 if (!attr)
281 return attr;
282 attr->state = MRP_APPLICANT_VO;
283 attr->type = type;
284 attr->len = len;
285 memcpy(attr->value, value, len);
286
287 rb_link_node(&attr->node, parent, p);
288 rb_insert_color(&attr->node, &app->mad);
289 return attr;
290}
291
292static void mrp_attr_destroy(struct mrp_applicant *app, struct mrp_attr *attr)
293{
294 rb_erase(&attr->node, &app->mad);
295 kfree(attr);
296}
297
298static int mrp_pdu_init(struct mrp_applicant *app)
299{
300 struct sk_buff *skb;
301 struct mrp_pdu_hdr *ph;
302
303 skb = alloc_skb(app->dev->mtu + LL_RESERVED_SPACE(app->dev),
304 GFP_ATOMIC);
305 if (!skb)
306 return -ENOMEM;
307
308 skb->dev = app->dev;
309 skb->protocol = app->app->pkttype.type;
310 skb_reserve(skb, LL_RESERVED_SPACE(app->dev));
311 skb_reset_network_header(skb);
312 skb_reset_transport_header(skb);
313
314 ph = (struct mrp_pdu_hdr *)__skb_put(skb, sizeof(*ph));
315 ph->version = app->app->version;
316
317 app->pdu = skb;
318 return 0;
319}
320
321static int mrp_pdu_append_end_mark(struct mrp_applicant *app)
322{
323 __be16 *endmark;
324
325 if (skb_tailroom(app->pdu) < sizeof(*endmark))
326 return -1;
327 endmark = (__be16 *)__skb_put(app->pdu, sizeof(*endmark));
328 put_unaligned(MRP_END_MARK, endmark);
329 return 0;
330}
331
332static void mrp_pdu_queue(struct mrp_applicant *app)
333{
334 if (!app->pdu)
335 return;
336
337 if (mrp_cb(app->pdu)->mh)
338 mrp_pdu_append_end_mark(app);
339 mrp_pdu_append_end_mark(app);
340
341 dev_hard_header(app->pdu, app->dev, ntohs(app->app->pkttype.type),
342 app->app->group_address, app->dev->dev_addr,
343 app->pdu->len);
344
345 skb_queue_tail(&app->queue, app->pdu);
346 app->pdu = NULL;
347}
348
349static void mrp_queue_xmit(struct mrp_applicant *app)
350{
351 struct sk_buff *skb;
352
353 while ((skb = skb_dequeue(&app->queue)))
354 dev_queue_xmit(skb);
355}
356
357static int mrp_pdu_append_msg_hdr(struct mrp_applicant *app,
358 u8 attrtype, u8 attrlen)
359{
360 struct mrp_msg_hdr *mh;
361
362 if (mrp_cb(app->pdu)->mh) {
363 if (mrp_pdu_append_end_mark(app) < 0)
364 return -1;
365 mrp_cb(app->pdu)->mh = NULL;
366 mrp_cb(app->pdu)->vah = NULL;
367 }
368
369 if (skb_tailroom(app->pdu) < sizeof(*mh))
370 return -1;
371 mh = (struct mrp_msg_hdr *)__skb_put(app->pdu, sizeof(*mh));
372 mh->attrtype = attrtype;
373 mh->attrlen = attrlen;
374 mrp_cb(app->pdu)->mh = mh;
375 return 0;
376}
377
378static int mrp_pdu_append_vecattr_hdr(struct mrp_applicant *app,
379 const void *firstattrvalue, u8 attrlen)
380{
381 struct mrp_vecattr_hdr *vah;
382
383 if (skb_tailroom(app->pdu) < sizeof(*vah) + attrlen)
384 return -1;
385 vah = (struct mrp_vecattr_hdr *)__skb_put(app->pdu,
386 sizeof(*vah) + attrlen);
387 put_unaligned(0, &vah->lenflags);
388 memcpy(vah->firstattrvalue, firstattrvalue, attrlen);
389 mrp_cb(app->pdu)->vah = vah;
390 memcpy(mrp_cb(app->pdu)->attrvalue, firstattrvalue, attrlen);
391 return 0;
392}
393
394static int mrp_pdu_append_vecattr_event(struct mrp_applicant *app,
395 const struct mrp_attr *attr,
396 enum mrp_vecattr_event vaevent)
397{
398 u16 len, pos;
399 u8 *vaevents;
400 int err;
401again:
402 if (!app->pdu) {
403 err = mrp_pdu_init(app);
404 if (err < 0)
405 return err;
406 }
407
408
409
410
411
412 if (!mrp_cb(app->pdu)->mh ||
413 mrp_cb(app->pdu)->mh->attrtype != attr->type ||
414 mrp_cb(app->pdu)->mh->attrlen != attr->len) {
415 if (mrp_pdu_append_msg_hdr(app, attr->type, attr->len) < 0)
416 goto queue;
417 }
418
419
420
421
422
423 if (!mrp_cb(app->pdu)->vah ||
424 memcmp(mrp_cb(app->pdu)->attrvalue, attr->value, attr->len)) {
425 if (mrp_pdu_append_vecattr_hdr(app, attr->value, attr->len) < 0)
426 goto queue;
427 }
428
429 len = be16_to_cpu(get_unaligned(&mrp_cb(app->pdu)->vah->lenflags));
430 pos = len % 3;
431
432
433
434
435 if (!pos) {
436 if (skb_tailroom(app->pdu) < sizeof(u8))
437 goto queue;
438 vaevents = (u8 *)__skb_put(app->pdu, sizeof(u8));
439 } else {
440 vaevents = (u8 *)(skb_tail_pointer(app->pdu) - sizeof(u8));
441 }
442
443 switch (pos) {
444 case 0:
445 *vaevents = vaevent * (__MRP_VECATTR_EVENT_MAX *
446 __MRP_VECATTR_EVENT_MAX);
447 break;
448 case 1:
449 *vaevents += vaevent * __MRP_VECATTR_EVENT_MAX;
450 break;
451 case 2:
452 *vaevents += vaevent;
453 break;
454 default:
455 WARN_ON(1);
456 }
457
458
459
460
461 put_unaligned(cpu_to_be16(++len), &mrp_cb(app->pdu)->vah->lenflags);
462 mrp_attrvalue_inc(mrp_cb(app->pdu)->attrvalue, attr->len);
463
464 return 0;
465
466queue:
467 mrp_pdu_queue(app);
468 goto again;
469}
470
471static void mrp_attr_event(struct mrp_applicant *app,
472 struct mrp_attr *attr, enum mrp_event event)
473{
474 enum mrp_applicant_state state;
475
476 state = mrp_applicant_state_table[attr->state][event];
477 if (state == MRP_APPLICANT_INVALID) {
478 WARN_ON(1);
479 return;
480 }
481
482 if (event == MRP_EVENT_TX) {
483
484
485
486
487 switch (mrp_tx_action_table[attr->state]) {
488 case MRP_TX_ACTION_NONE:
489 case MRP_TX_ACTION_S_JOIN_IN_OPTIONAL:
490 case MRP_TX_ACTION_S_IN_OPTIONAL:
491 break;
492 case MRP_TX_ACTION_S_NEW:
493 if (mrp_pdu_append_vecattr_event(
494 app, attr, MRP_VECATTR_EVENT_NEW) < 0)
495 return;
496 break;
497 case MRP_TX_ACTION_S_JOIN_IN:
498 if (mrp_pdu_append_vecattr_event(
499 app, attr, MRP_VECATTR_EVENT_JOIN_IN) < 0)
500 return;
501 break;
502 case MRP_TX_ACTION_S_LV:
503 if (mrp_pdu_append_vecattr_event(
504 app, attr, MRP_VECATTR_EVENT_LV) < 0)
505 return;
506
507
508
509
510 mrp_attr_destroy(app, attr);
511 return;
512 default:
513 WARN_ON(1);
514 }
515 }
516
517 attr->state = state;
518}
519
520int mrp_request_join(const struct net_device *dev,
521 const struct mrp_application *appl,
522 const void *value, u8 len, u8 type)
523{
524 struct mrp_port *port = rtnl_dereference(dev->mrp_port);
525 struct mrp_applicant *app = rtnl_dereference(
526 port->applicants[appl->type]);
527 struct mrp_attr *attr;
528
529 if (sizeof(struct mrp_skb_cb) + len >
530 FIELD_SIZEOF(struct sk_buff, cb))
531 return -ENOMEM;
532
533 spin_lock_bh(&app->lock);
534 attr = mrp_attr_create(app, value, len, type);
535 if (!attr) {
536 spin_unlock_bh(&app->lock);
537 return -ENOMEM;
538 }
539 mrp_attr_event(app, attr, MRP_EVENT_JOIN);
540 spin_unlock_bh(&app->lock);
541 return 0;
542}
543EXPORT_SYMBOL_GPL(mrp_request_join);
544
545void mrp_request_leave(const struct net_device *dev,
546 const struct mrp_application *appl,
547 const void *value, u8 len, u8 type)
548{
549 struct mrp_port *port = rtnl_dereference(dev->mrp_port);
550 struct mrp_applicant *app = rtnl_dereference(
551 port->applicants[appl->type]);
552 struct mrp_attr *attr;
553
554 if (sizeof(struct mrp_skb_cb) + len >
555 FIELD_SIZEOF(struct sk_buff, cb))
556 return;
557
558 spin_lock_bh(&app->lock);
559 attr = mrp_attr_lookup(app, value, len, type);
560 if (!attr) {
561 spin_unlock_bh(&app->lock);
562 return;
563 }
564 mrp_attr_event(app, attr, MRP_EVENT_LV);
565 spin_unlock_bh(&app->lock);
566}
567EXPORT_SYMBOL_GPL(mrp_request_leave);
568
569static void mrp_mad_event(struct mrp_applicant *app, enum mrp_event event)
570{
571 struct rb_node *node, *next;
572 struct mrp_attr *attr;
573
574 for (node = rb_first(&app->mad);
575 next = node ? rb_next(node) : NULL, node != NULL;
576 node = next) {
577 attr = rb_entry(node, struct mrp_attr, node);
578 mrp_attr_event(app, attr, event);
579 }
580}
581
582static void mrp_join_timer_arm(struct mrp_applicant *app)
583{
584 unsigned long delay;
585
586 delay = (u64)msecs_to_jiffies(mrp_join_time) * prandom_u32() >> 32;
587 mod_timer(&app->join_timer, jiffies + delay);
588}
589
590static void mrp_join_timer(unsigned long data)
591{
592 struct mrp_applicant *app = (struct mrp_applicant *)data;
593
594 spin_lock(&app->lock);
595 mrp_mad_event(app, MRP_EVENT_TX);
596 mrp_pdu_queue(app);
597 spin_unlock(&app->lock);
598
599 mrp_queue_xmit(app);
600 mrp_join_timer_arm(app);
601}
602
603static void mrp_periodic_timer_arm(struct mrp_applicant *app)
604{
605 mod_timer(&app->periodic_timer,
606 jiffies + msecs_to_jiffies(mrp_periodic_time));
607}
608
609static void mrp_periodic_timer(unsigned long data)
610{
611 struct mrp_applicant *app = (struct mrp_applicant *)data;
612
613 spin_lock(&app->lock);
614 mrp_mad_event(app, MRP_EVENT_PERIODIC);
615 mrp_pdu_queue(app);
616 spin_unlock(&app->lock);
617
618 mrp_periodic_timer_arm(app);
619}
620
621static int mrp_pdu_parse_end_mark(struct sk_buff *skb, int *offset)
622{
623 __be16 endmark;
624
625 if (skb_copy_bits(skb, *offset, &endmark, sizeof(endmark)) < 0)
626 return -1;
627 if (endmark == MRP_END_MARK) {
628 *offset += sizeof(endmark);
629 return -1;
630 }
631 return 0;
632}
633
634static void mrp_pdu_parse_vecattr_event(struct mrp_applicant *app,
635 struct sk_buff *skb,
636 enum mrp_vecattr_event vaevent)
637{
638 struct mrp_attr *attr;
639 enum mrp_event event;
640
641 attr = mrp_attr_lookup(app, mrp_cb(skb)->attrvalue,
642 mrp_cb(skb)->mh->attrlen,
643 mrp_cb(skb)->mh->attrtype);
644 if (attr == NULL)
645 return;
646
647 switch (vaevent) {
648 case MRP_VECATTR_EVENT_NEW:
649 event = MRP_EVENT_R_NEW;
650 break;
651 case MRP_VECATTR_EVENT_JOIN_IN:
652 event = MRP_EVENT_R_JOIN_IN;
653 break;
654 case MRP_VECATTR_EVENT_IN:
655 event = MRP_EVENT_R_IN;
656 break;
657 case MRP_VECATTR_EVENT_JOIN_MT:
658 event = MRP_EVENT_R_JOIN_MT;
659 break;
660 case MRP_VECATTR_EVENT_MT:
661 event = MRP_EVENT_R_MT;
662 break;
663 case MRP_VECATTR_EVENT_LV:
664 event = MRP_EVENT_R_LV;
665 break;
666 default:
667 return;
668 }
669
670 mrp_attr_event(app, attr, event);
671}
672
673static int mrp_pdu_parse_vecattr(struct mrp_applicant *app,
674 struct sk_buff *skb, int *offset)
675{
676 struct mrp_vecattr_hdr _vah;
677 u16 valen;
678 u8 vaevents, vaevent;
679
680 mrp_cb(skb)->vah = skb_header_pointer(skb, *offset, sizeof(_vah),
681 &_vah);
682 if (!mrp_cb(skb)->vah)
683 return -1;
684 *offset += sizeof(_vah);
685
686 if (get_unaligned(&mrp_cb(skb)->vah->lenflags) &
687 MRP_VECATTR_HDR_FLAG_LA)
688 mrp_mad_event(app, MRP_EVENT_R_LA);
689 valen = be16_to_cpu(get_unaligned(&mrp_cb(skb)->vah->lenflags) &
690 MRP_VECATTR_HDR_LEN_MASK);
691
692
693
694
695
696
697
698 if (sizeof(struct mrp_skb_cb) + mrp_cb(skb)->mh->attrlen >
699 FIELD_SIZEOF(struct sk_buff, cb))
700 return -1;
701 if (skb_copy_bits(skb, *offset, mrp_cb(skb)->attrvalue,
702 mrp_cb(skb)->mh->attrlen) < 0)
703 return -1;
704 *offset += mrp_cb(skb)->mh->attrlen;
705
706
707
708
709 while (valen > 0) {
710 if (skb_copy_bits(skb, *offset, &vaevents,
711 sizeof(vaevents)) < 0)
712 return -1;
713 *offset += sizeof(vaevents);
714
715
716 vaevent = vaevents / (__MRP_VECATTR_EVENT_MAX *
717 __MRP_VECATTR_EVENT_MAX);
718 if (vaevent >= __MRP_VECATTR_EVENT_MAX) {
719
720 return -1;
721 }
722 mrp_pdu_parse_vecattr_event(app, skb, vaevent);
723
724
725 if (!--valen)
726 break;
727 mrp_attrvalue_inc(mrp_cb(skb)->attrvalue,
728 mrp_cb(skb)->mh->attrlen);
729 vaevents %= (__MRP_VECATTR_EVENT_MAX *
730 __MRP_VECATTR_EVENT_MAX);
731 vaevent = vaevents / __MRP_VECATTR_EVENT_MAX;
732 mrp_pdu_parse_vecattr_event(app, skb, vaevent);
733
734
735 if (!--valen)
736 break;
737 mrp_attrvalue_inc(mrp_cb(skb)->attrvalue,
738 mrp_cb(skb)->mh->attrlen);
739 vaevents %= __MRP_VECATTR_EVENT_MAX;
740 vaevent = vaevents;
741 mrp_pdu_parse_vecattr_event(app, skb, vaevent);
742 }
743 return 0;
744}
745
746static int mrp_pdu_parse_msg(struct mrp_applicant *app, struct sk_buff *skb,
747 int *offset)
748{
749 struct mrp_msg_hdr _mh;
750
751 mrp_cb(skb)->mh = skb_header_pointer(skb, *offset, sizeof(_mh), &_mh);
752 if (!mrp_cb(skb)->mh)
753 return -1;
754 *offset += sizeof(_mh);
755
756 if (mrp_cb(skb)->mh->attrtype == 0 ||
757 mrp_cb(skb)->mh->attrtype > app->app->maxattr ||
758 mrp_cb(skb)->mh->attrlen == 0)
759 return -1;
760
761 while (skb->len > *offset) {
762 if (mrp_pdu_parse_end_mark(skb, offset) < 0)
763 break;
764 if (mrp_pdu_parse_vecattr(app, skb, offset) < 0)
765 return -1;
766 }
767 return 0;
768}
769
770static int mrp_rcv(struct sk_buff *skb, struct net_device *dev,
771 struct packet_type *pt, struct net_device *orig_dev)
772{
773 struct mrp_application *appl = container_of(pt, struct mrp_application,
774 pkttype);
775 struct mrp_port *port;
776 struct mrp_applicant *app;
777 struct mrp_pdu_hdr _ph;
778 const struct mrp_pdu_hdr *ph;
779 int offset = skb_network_offset(skb);
780
781
782
783
784 if (unlikely(skb->pkt_type == PACKET_OTHERHOST))
785 goto out;
786 skb = skb_share_check(skb, GFP_ATOMIC);
787 if (unlikely(!skb))
788 goto out;
789 port = rcu_dereference(dev->mrp_port);
790 if (unlikely(!port))
791 goto out;
792 app = rcu_dereference(port->applicants[appl->type]);
793 if (unlikely(!app))
794 goto out;
795
796 ph = skb_header_pointer(skb, offset, sizeof(_ph), &_ph);
797 if (!ph)
798 goto out;
799 offset += sizeof(_ph);
800
801 if (ph->version != app->app->version)
802 goto out;
803
804 spin_lock(&app->lock);
805 while (skb->len > offset) {
806 if (mrp_pdu_parse_end_mark(skb, &offset) < 0)
807 break;
808 if (mrp_pdu_parse_msg(app, skb, &offset) < 0)
809 break;
810 }
811 spin_unlock(&app->lock);
812out:
813 kfree_skb(skb);
814 return 0;
815}
816
817static int mrp_init_port(struct net_device *dev)
818{
819 struct mrp_port *port;
820
821 port = kzalloc(sizeof(*port), GFP_KERNEL);
822 if (!port)
823 return -ENOMEM;
824 rcu_assign_pointer(dev->mrp_port, port);
825 return 0;
826}
827
828static void mrp_release_port(struct net_device *dev)
829{
830 struct mrp_port *port = rtnl_dereference(dev->mrp_port);
831 unsigned int i;
832
833 for (i = 0; i <= MRP_APPLICATION_MAX; i++) {
834 if (rtnl_dereference(port->applicants[i]))
835 return;
836 }
837 RCU_INIT_POINTER(dev->mrp_port, NULL);
838 kfree_rcu(port, rcu);
839}
840
841int mrp_init_applicant(struct net_device *dev, struct mrp_application *appl)
842{
843 struct mrp_applicant *app;
844 int err;
845
846 ASSERT_RTNL();
847
848 if (!rtnl_dereference(dev->mrp_port)) {
849 err = mrp_init_port(dev);
850 if (err < 0)
851 goto err1;
852 }
853
854 err = -ENOMEM;
855 app = kzalloc(sizeof(*app), GFP_KERNEL);
856 if (!app)
857 goto err2;
858
859 err = dev_mc_add(dev, appl->group_address);
860 if (err < 0)
861 goto err3;
862
863 app->dev = dev;
864 app->app = appl;
865 app->mad = RB_ROOT;
866 spin_lock_init(&app->lock);
867 skb_queue_head_init(&app->queue);
868 rcu_assign_pointer(dev->mrp_port->applicants[appl->type], app);
869 setup_timer(&app->join_timer, mrp_join_timer, (unsigned long)app);
870 mrp_join_timer_arm(app);
871 setup_timer(&app->periodic_timer, mrp_periodic_timer,
872 (unsigned long)app);
873 mrp_periodic_timer_arm(app);
874 return 0;
875
876err3:
877 kfree(app);
878err2:
879 mrp_release_port(dev);
880err1:
881 return err;
882}
883EXPORT_SYMBOL_GPL(mrp_init_applicant);
884
885void mrp_uninit_applicant(struct net_device *dev, struct mrp_application *appl)
886{
887 struct mrp_port *port = rtnl_dereference(dev->mrp_port);
888 struct mrp_applicant *app = rtnl_dereference(
889 port->applicants[appl->type]);
890
891 ASSERT_RTNL();
892
893 RCU_INIT_POINTER(port->applicants[appl->type], NULL);
894
895
896
897
898 del_timer_sync(&app->join_timer);
899 del_timer_sync(&app->periodic_timer);
900
901 spin_lock_bh(&app->lock);
902 mrp_mad_event(app, MRP_EVENT_TX);
903 mrp_pdu_queue(app);
904 spin_unlock_bh(&app->lock);
905
906 mrp_queue_xmit(app);
907
908 dev_mc_del(dev, appl->group_address);
909 kfree_rcu(app, rcu);
910 mrp_release_port(dev);
911}
912EXPORT_SYMBOL_GPL(mrp_uninit_applicant);
913
914int mrp_register_application(struct mrp_application *appl)
915{
916 appl->pkttype.func = mrp_rcv;
917 dev_add_pack(&appl->pkttype);
918 return 0;
919}
920EXPORT_SYMBOL_GPL(mrp_register_application);
921
922void mrp_unregister_application(struct mrp_application *appl)
923{
924 dev_remove_pack(&appl->pkttype);
925}
926EXPORT_SYMBOL_GPL(mrp_unregister_application);
927
