?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Linux 4.13.16
[linux/fpc-iii.git] / net / 802 / mrp.c
blobbe4dd31653474fbe806d17458773cc587aa56606
1 /*
2 * IEEE 802.1Q Multiple Registration Protocol (MRP)
3 *
4 * Copyright (c) 2012 Massachusetts Institute of Technology
5 *
6 * Adapted from code in net/802/garp.c
7 * Copyright (c) 2008 Patrick McHardy <kaber@trash.net>
8 *
9 * This program is free software; you can redistribute it and/or
10 * modify it under the terms of the GNU General Public License
11 * version 2 as published by the Free Software Foundation.
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
24 static unsigned int mrp_join_time __read_mostly = 200;
25 module_param(mrp_join_time, uint, 0644);
26 MODULE_PARM_DESC(mrp_join_time, "Join time in ms (default 200ms)");
27
28 static unsigned int mrp_periodic_time __read_mostly = 1000;
29 module_param(mrp_periodic_time, uint, 0644);
30 MODULE_PARM_DESC(mrp_periodic_time, "Periodic time in ms (default 1s)");
31
32 MODULE_LICENSE("GPL");
33
34 static const u8
35 mrp_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
203 static const u8
204 mrp_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
218 static void mrp_attrvalue_inc(void *value, u8 len)
219 {
220 u8 *v = (u8 *)value;
221
222 /* Add 1 to the last byte. If it becomes zero,
223 * go to the previous byte and repeat.
224 */
225 while (len > 0 && !++v[--len])
226 ;
227 }
228
229 static 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
239 static 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
259 static 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 /* The attribute already exists; re-use it. */
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
292 static 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
298 static 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 = __skb_put(skb, sizeof(*ph));
315 ph->version = app->app->version;
316
317 app->pdu = skb;
318 return 0;
319 }
320
321 static 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 = __skb_put(app->pdu, sizeof(*endmark));
328 put_unaligned(MRP_END_MARK, endmark);
329 return 0;
330 }
331
332 static 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
349 static 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
357 static 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 = __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
378 static 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 = __skb_put(app->pdu, sizeof(*vah) + attrlen);
386 put_unaligned(0, &vah->lenflags);
387 memcpy(vah->firstattrvalue, firstattrvalue, attrlen);
388 mrp_cb(app->pdu)->vah = vah;
389 memcpy(mrp_cb(app->pdu)->attrvalue, firstattrvalue, attrlen);
390 return 0;
391 }
392
393 static int mrp_pdu_append_vecattr_event(struct mrp_applicant *app,
394 const struct mrp_attr *attr,
395 enum mrp_vecattr_event vaevent)
396 {
397 u16 len, pos;
398 u8 *vaevents;
399 int err;
400 again:
401 if (!app->pdu) {
402 err = mrp_pdu_init(app);
403 if (err < 0)
404 return err;
405 }
406
407 /* If there is no Message header in the PDU, or the Message header is
408 * for a different attribute type, add an EndMark (if necessary) and a
409 * new Message header to the PDU.
410 */
411 if (!mrp_cb(app->pdu)->mh ||
412 mrp_cb(app->pdu)->mh->attrtype != attr->type ||
413 mrp_cb(app->pdu)->mh->attrlen != attr->len) {
414 if (mrp_pdu_append_msg_hdr(app, attr->type, attr->len) < 0)
415 goto queue;
416 }
417
418 /* If there is no VectorAttribute header for this Message in the PDU,
419 * or this attribute's value does not sequentially follow the previous
420 * attribute's value, add a new VectorAttribute header to the PDU.
421 */
422 if (!mrp_cb(app->pdu)->vah ||
423 memcmp(mrp_cb(app->pdu)->attrvalue, attr->value, attr->len)) {
424 if (mrp_pdu_append_vecattr_hdr(app, attr->value, attr->len) < 0)
425 goto queue;
426 }
427
428 len = be16_to_cpu(get_unaligned(&mrp_cb(app->pdu)->vah->lenflags));
429 pos = len % 3;
430
431 /* Events are packed into Vectors in the PDU, three to a byte. Add a
432 * byte to the end of the Vector if necessary.
433 */
434 if (!pos) {
435 if (skb_tailroom(app->pdu) < sizeof(u8))
436 goto queue;
437 vaevents = __skb_put(app->pdu, sizeof(u8));
438 } else {
439 vaevents = (u8 *)(skb_tail_pointer(app->pdu) - sizeof(u8));
440 }
441
442 switch (pos) {
443 case 0:
444 *vaevents = vaevent * (__MRP_VECATTR_EVENT_MAX *
445 __MRP_VECATTR_EVENT_MAX);
446 break;
447 case 1:
448 *vaevents += vaevent * __MRP_VECATTR_EVENT_MAX;
449 break;
450 case 2:
451 *vaevents += vaevent;
452 break;
453 default:
454 WARN_ON(1);
455 }
456
457 /* Increment the length of the VectorAttribute in the PDU, as well as
458 * the value of the next attribute that would continue its Vector.
459 */
460 put_unaligned(cpu_to_be16(++len), &mrp_cb(app->pdu)->vah->lenflags);
461 mrp_attrvalue_inc(mrp_cb(app->pdu)->attrvalue, attr->len);
462
463 return 0;
464
465 queue:
466 mrp_pdu_queue(app);
467 goto again;
468 }
469
470 static void mrp_attr_event(struct mrp_applicant *app,
471 struct mrp_attr *attr, enum mrp_event event)
472 {
473 enum mrp_applicant_state state;
474
475 state = mrp_applicant_state_table[attr->state][event];
476 if (state == MRP_APPLICANT_INVALID) {
477 WARN_ON(1);
478 return;
479 }
480
481 if (event == MRP_EVENT_TX) {
482 /* When appending the attribute fails, don't update its state
483 * in order to retry at the next TX event.
484 */
485
486 switch (mrp_tx_action_table[attr->state]) {
487 case MRP_TX_ACTION_NONE:
488 case MRP_TX_ACTION_S_JOIN_IN_OPTIONAL:
489 case MRP_TX_ACTION_S_IN_OPTIONAL:
490 break;
491 case MRP_TX_ACTION_S_NEW:
492 if (mrp_pdu_append_vecattr_event(
493 app, attr, MRP_VECATTR_EVENT_NEW) < 0)
494 return;
495 break;
496 case MRP_TX_ACTION_S_JOIN_IN:
497 if (mrp_pdu_append_vecattr_event(
498 app, attr, MRP_VECATTR_EVENT_JOIN_IN) < 0)
499 return;
500 break;
501 case MRP_TX_ACTION_S_LV:
502 if (mrp_pdu_append_vecattr_event(
503 app, attr, MRP_VECATTR_EVENT_LV) < 0)
504 return;
505 /* As a pure applicant, sending a leave message
506 * implies that the attribute was unregistered and
507 * can be destroyed.
508 */
509 mrp_attr_destroy(app, attr);
510 return;
511 default:
512 WARN_ON(1);
513 }
514 }
515
516 attr->state = state;
517 }
518
519 int mrp_request_join(const struct net_device *dev,
520 const struct mrp_application *appl,
521 const void *value, u8 len, u8 type)
522 {
523 struct mrp_port *port = rtnl_dereference(dev->mrp_port);
524 struct mrp_applicant *app = rtnl_dereference(
525 port->applicants[appl->type]);
526 struct mrp_attr *attr;
527
528 if (sizeof(struct mrp_skb_cb) + len >
529 FIELD_SIZEOF(struct sk_buff, cb))
530 return -ENOMEM;
531
532 spin_lock_bh(&app->lock);
533 attr = mrp_attr_create(app, value, len, type);
534 if (!attr) {
535 spin_unlock_bh(&app->lock);
536 return -ENOMEM;
537 }
538 mrp_attr_event(app, attr, MRP_EVENT_JOIN);
539 spin_unlock_bh(&app->lock);
540 return 0;
541 }
542 EXPORT_SYMBOL_GPL(mrp_request_join);
543
544 void mrp_request_leave(const struct net_device *dev,
545 const struct mrp_application *appl,
546 const void *value, u8 len, u8 type)
547 {
548 struct mrp_port *port = rtnl_dereference(dev->mrp_port);
549 struct mrp_applicant *app = rtnl_dereference(
550 port->applicants[appl->type]);
551 struct mrp_attr *attr;
552
553 if (sizeof(struct mrp_skb_cb) + len >
554 FIELD_SIZEOF(struct sk_buff, cb))
555 return;
556
557 spin_lock_bh(&app->lock);
558 attr = mrp_attr_lookup(app, value, len, type);
559 if (!attr) {
560 spin_unlock_bh(&app->lock);
561 return;
562 }
563 mrp_attr_event(app, attr, MRP_EVENT_LV);
564 spin_unlock_bh(&app->lock);
565 }
566 EXPORT_SYMBOL_GPL(mrp_request_leave);
567
568 static void mrp_mad_event(struct mrp_applicant *app, enum mrp_event event)
569 {
570 struct rb_node *node, *next;
571 struct mrp_attr *attr;
572
573 for (node = rb_first(&app->mad);
574 next = node ? rb_next(node) : NULL, node != NULL;
575 node = next) {
576 attr = rb_entry(node, struct mrp_attr, node);
577 mrp_attr_event(app, attr, event);
578 }
579 }
580
581 static void mrp_join_timer_arm(struct mrp_applicant *app)
582 {
583 unsigned long delay;
584
585 delay = (u64)msecs_to_jiffies(mrp_join_time) * prandom_u32() >> 32;
586 mod_timer(&app->join_timer, jiffies + delay);
587 }
588
589 static void mrp_join_timer(unsigned long data)
590 {
591 struct mrp_applicant *app = (struct mrp_applicant *)data;
592
593 spin_lock(&app->lock);
594 mrp_mad_event(app, MRP_EVENT_TX);
595 mrp_pdu_queue(app);
596 spin_unlock(&app->lock);
597
598 mrp_queue_xmit(app);
599 mrp_join_timer_arm(app);
600 }
601
602 static void mrp_periodic_timer_arm(struct mrp_applicant *app)
603 {
604 mod_timer(&app->periodic_timer,
605 jiffies + msecs_to_jiffies(mrp_periodic_time));
606 }
607
608 static void mrp_periodic_timer(unsigned long data)
609 {
610 struct mrp_applicant *app = (struct mrp_applicant *)data;
611
612 spin_lock(&app->lock);
613 mrp_mad_event(app, MRP_EVENT_PERIODIC);
614 mrp_pdu_queue(app);
615 spin_unlock(&app->lock);
616
617 mrp_periodic_timer_arm(app);
618 }
619
620 static int mrp_pdu_parse_end_mark(struct sk_buff *skb, int *offset)
621 {
622 __be16 endmark;
623
624 if (skb_copy_bits(skb, *offset, &endmark, sizeof(endmark)) < 0)
625 return -1;
626 if (endmark == MRP_END_MARK) {
627 *offset += sizeof(endmark);
628 return -1;
629 }
630 return 0;
631 }
632
633 static void mrp_pdu_parse_vecattr_event(struct mrp_applicant *app,
634 struct sk_buff *skb,
635 enum mrp_vecattr_event vaevent)
636 {
637 struct mrp_attr *attr;
638 enum mrp_event event;
639
640 attr = mrp_attr_lookup(app, mrp_cb(skb)->attrvalue,
641 mrp_cb(skb)->mh->attrlen,
642 mrp_cb(skb)->mh->attrtype);
643 if (attr == NULL)
644 return;
645
646 switch (vaevent) {
647 case MRP_VECATTR_EVENT_NEW:
648 event = MRP_EVENT_R_NEW;
649 break;
650 case MRP_VECATTR_EVENT_JOIN_IN:
651 event = MRP_EVENT_R_JOIN_IN;
652 break;
653 case MRP_VECATTR_EVENT_IN:
654 event = MRP_EVENT_R_IN;
655 break;
656 case MRP_VECATTR_EVENT_JOIN_MT:
657 event = MRP_EVENT_R_JOIN_MT;
658 break;
659 case MRP_VECATTR_EVENT_MT:
660 event = MRP_EVENT_R_MT;
661 break;
662 case MRP_VECATTR_EVENT_LV:
663 event = MRP_EVENT_R_LV;
664 break;
665 default:
666 return;
667 }
668
669 mrp_attr_event(app, attr, event);
670 }
671
672 static int mrp_pdu_parse_vecattr(struct mrp_applicant *app,
673 struct sk_buff *skb, int *offset)
674 {
675 struct mrp_vecattr_hdr _vah;
676 u16 valen;
677 u8 vaevents, vaevent;
678
679 mrp_cb(skb)->vah = skb_header_pointer(skb, *offset, sizeof(_vah),
680 &_vah);
681 if (!mrp_cb(skb)->vah)
682 return -1;
683 *offset += sizeof(_vah);
684
685 if (get_unaligned(&mrp_cb(skb)->vah->lenflags) &
686 MRP_VECATTR_HDR_FLAG_LA)
687 mrp_mad_event(app, MRP_EVENT_R_LA);
688 valen = be16_to_cpu(get_unaligned(&mrp_cb(skb)->vah->lenflags) &
689 MRP_VECATTR_HDR_LEN_MASK);
690
691 /* The VectorAttribute structure in a PDU carries event information
692 * about one or more attributes having consecutive values. Only the
693 * value for the first attribute is contained in the structure. So
694 * we make a copy of that value, and then increment it each time we
695 * advance to the next event in its Vector.
696 */
697 if (sizeof(struct mrp_skb_cb) + mrp_cb(skb)->mh->attrlen >
698 FIELD_SIZEOF(struct sk_buff, cb))
699 return -1;
700 if (skb_copy_bits(skb, *offset, mrp_cb(skb)->attrvalue,
701 mrp_cb(skb)->mh->attrlen) < 0)
702 return -1;
703 *offset += mrp_cb(skb)->mh->attrlen;
704
705 /* In a VectorAttribute, the Vector contains events which are packed
706 * three to a byte. We process one byte of the Vector at a time.
707 */
708 while (valen > 0) {
709 if (skb_copy_bits(skb, *offset, &vaevents,
710 sizeof(vaevents)) < 0)
711 return -1;
712 *offset += sizeof(vaevents);
713
714 /* Extract and process the first event. */
715 vaevent = vaevents / (__MRP_VECATTR_EVENT_MAX *
716 __MRP_VECATTR_EVENT_MAX);
717 if (vaevent >= __MRP_VECATTR_EVENT_MAX) {
718 /* The byte is malformed; stop processing. */
719 return -1;
720 }
721 mrp_pdu_parse_vecattr_event(app, skb, vaevent);
722
723 /* If present, extract and process the second event. */
724 if (!--valen)
725 break;
726 mrp_attrvalue_inc(mrp_cb(skb)->attrvalue,
727 mrp_cb(skb)->mh->attrlen);
728 vaevents %= (__MRP_VECATTR_EVENT_MAX *
729 __MRP_VECATTR_EVENT_MAX);
730 vaevent = vaevents / __MRP_VECATTR_EVENT_MAX;
731 mrp_pdu_parse_vecattr_event(app, skb, vaevent);
732
733 /* If present, extract and process the third event. */
734 if (!--valen)
735 break;
736 mrp_attrvalue_inc(mrp_cb(skb)->attrvalue,
737 mrp_cb(skb)->mh->attrlen);
738 vaevents %= __MRP_VECATTR_EVENT_MAX;
739 vaevent = vaevents;
740 mrp_pdu_parse_vecattr_event(app, skb, vaevent);
741 }
742 return 0;
743 }
744
745 static int mrp_pdu_parse_msg(struct mrp_applicant *app, struct sk_buff *skb,
746 int *offset)
747 {
748 struct mrp_msg_hdr _mh;
749
750 mrp_cb(skb)->mh = skb_header_pointer(skb, *offset, sizeof(_mh), &_mh);
751 if (!mrp_cb(skb)->mh)
752 return -1;
753 *offset += sizeof(_mh);
754
755 if (mrp_cb(skb)->mh->attrtype == 0 ||
756 mrp_cb(skb)->mh->attrtype > app->app->maxattr ||
757 mrp_cb(skb)->mh->attrlen == 0)
758 return -1;
759
760 while (skb->len > *offset) {
761 if (mrp_pdu_parse_end_mark(skb, offset) < 0)
762 break;
763 if (mrp_pdu_parse_vecattr(app, skb, offset) < 0)
764 return -1;
765 }
766 return 0;
767 }
768
769 static int mrp_rcv(struct sk_buff *skb, struct net_device *dev,
770 struct packet_type *pt, struct net_device *orig_dev)
771 {
772 struct mrp_application *appl = container_of(pt, struct mrp_application,
773 pkttype);
774 struct mrp_port *port;
775 struct mrp_applicant *app;
776 struct mrp_pdu_hdr _ph;
777 const struct mrp_pdu_hdr *ph;
778 int offset = skb_network_offset(skb);
779
780 /* If the interface is in promiscuous mode, drop the packet if
781 * it was unicast to another host.
782 */
783 if (unlikely(skb->pkt_type == PACKET_OTHERHOST))
784 goto out;
785 skb = skb_share_check(skb, GFP_ATOMIC);
786 if (unlikely(!skb))
787 goto out;
788 port = rcu_dereference(dev->mrp_port);
789 if (unlikely(!port))
790 goto out;
791 app = rcu_dereference(port->applicants[appl->type]);
792 if (unlikely(!app))
793 goto out;
794
795 ph = skb_header_pointer(skb, offset, sizeof(_ph), &_ph);
796 if (!ph)
797 goto out;
798 offset += sizeof(_ph);
799
800 if (ph->version != app->app->version)
801 goto out;
802
803 spin_lock(&app->lock);
804 while (skb->len > offset) {
805 if (mrp_pdu_parse_end_mark(skb, &offset) < 0)
806 break;
807 if (mrp_pdu_parse_msg(app, skb, &offset) < 0)
808 break;
809 }
810 spin_unlock(&app->lock);
811 out:
812 kfree_skb(skb);
813 return 0;
814 }
815
816 static int mrp_init_port(struct net_device *dev)
817 {
818 struct mrp_port *port;
819
820 port = kzalloc(sizeof(*port), GFP_KERNEL);
821 if (!port)
822 return -ENOMEM;
823 rcu_assign_pointer(dev->mrp_port, port);
824 return 0;
825 }
826
827 static void mrp_release_port(struct net_device *dev)
828 {
829 struct mrp_port *port = rtnl_dereference(dev->mrp_port);
830 unsigned int i;
831
832 for (i = 0; i <= MRP_APPLICATION_MAX; i++) {
833 if (rtnl_dereference(port->applicants[i]))
834 return;
835 }
836 RCU_INIT_POINTER(dev->mrp_port, NULL);
837 kfree_rcu(port, rcu);
838 }
839
840 int mrp_init_applicant(struct net_device *dev, struct mrp_application *appl)
841 {
842 struct mrp_applicant *app;
843 int err;
844
845 ASSERT_RTNL();
846
847 if (!rtnl_dereference(dev->mrp_port)) {
848 err = mrp_init_port(dev);
849 if (err < 0)
850 goto err1;
851 }
852
853 err = -ENOMEM;
854 app = kzalloc(sizeof(*app), GFP_KERNEL);
855 if (!app)
856 goto err2;
857
858 err = dev_mc_add(dev, appl->group_address);
859 if (err < 0)
860 goto err3;
861
862 app->dev = dev;
863 app->app = appl;
864 app->mad = RB_ROOT;
865 spin_lock_init(&app->lock);
866 skb_queue_head_init(&app->queue);
867 rcu_assign_pointer(dev->mrp_port->applicants[appl->type], app);
868 setup_timer(&app->join_timer, mrp_join_timer, (unsigned long)app);
869 mrp_join_timer_arm(app);
870 setup_timer(&app->periodic_timer, mrp_periodic_timer,
871 (unsigned long)app);
872 mrp_periodic_timer_arm(app);
873 return 0;
874
875 err3:
876 kfree(app);
877 err2:
878 mrp_release_port(dev);
879 err1:
880 return err;
881 }
882 EXPORT_SYMBOL_GPL(mrp_init_applicant);
883
884 void mrp_uninit_applicant(struct net_device *dev, struct mrp_application *appl)
885 {
886 struct mrp_port *port = rtnl_dereference(dev->mrp_port);
887 struct mrp_applicant *app = rtnl_dereference(
888 port->applicants[appl->type]);
889
890 ASSERT_RTNL();
891
892 RCU_INIT_POINTER(port->applicants[appl->type], NULL);
893
894 /* Delete timer and generate a final TX event to flush out
895 * all pending messages before the applicant is gone.
896 */
897 del_timer_sync(&app->join_timer);
898 del_timer_sync(&app->periodic_timer);
899
900 spin_lock_bh(&app->lock);
901 mrp_mad_event(app, MRP_EVENT_TX);
902 mrp_pdu_queue(app);
903 spin_unlock_bh(&app->lock);
904
905 mrp_queue_xmit(app);
906
907 dev_mc_del(dev, appl->group_address);
908 kfree_rcu(app, rcu);
909 mrp_release_port(dev);
910 }
911 EXPORT_SYMBOL_GPL(mrp_uninit_applicant);
912
913 int mrp_register_application(struct mrp_application *appl)
914 {
915 appl->pkttype.func = mrp_rcv;
916 dev_add_pack(&appl->pkttype);
917 return 0;
918 }
919 EXPORT_SYMBOL_GPL(mrp_register_application);
920
921 void mrp_unregister_application(struct mrp_application *appl)
922 {
923 dev_remove_pack(&appl->pkttype);
924 }
925 EXPORT_SYMBOL_GPL(mrp_unregister_application);
Linux with added FPC-III support