GNU Linux-libre 4.19.211-gnu1
[releases.git] / net / 802 / mrp.c
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 void mrp_attr_destroy_all(struct mrp_applicant *app)
299 {
300         struct rb_node *node, *next;
301         struct mrp_attr *attr;
302
303         for (node = rb_first(&app->mad);
304              next = node ? rb_next(node) : NULL, node != NULL;
305              node = next) {
306                 attr = rb_entry(node, struct mrp_attr, node);
307                 mrp_attr_destroy(app, attr);
308         }
309 }
310
311 static int mrp_pdu_init(struct mrp_applicant *app)
312 {
313         struct sk_buff *skb;
314         struct mrp_pdu_hdr *ph;
315
316         skb = alloc_skb(app->dev->mtu + LL_RESERVED_SPACE(app->dev),
317                         GFP_ATOMIC);
318         if (!skb)
319                 return -ENOMEM;
320
321         skb->dev = app->dev;
322         skb->protocol = app->app->pkttype.type;
323         skb_reserve(skb, LL_RESERVED_SPACE(app->dev));
324         skb_reset_network_header(skb);
325         skb_reset_transport_header(skb);
326
327         ph = __skb_put(skb, sizeof(*ph));
328         ph->version = app->app->version;
329
330         app->pdu = skb;
331         return 0;
332 }
333
334 static int mrp_pdu_append_end_mark(struct mrp_applicant *app)
335 {
336         __be16 *endmark;
337
338         if (skb_tailroom(app->pdu) < sizeof(*endmark))
339                 return -1;
340         endmark = __skb_put(app->pdu, sizeof(*endmark));
341         put_unaligned(MRP_END_MARK, endmark);
342         return 0;
343 }
344
345 static void mrp_pdu_queue(struct mrp_applicant *app)
346 {
347         if (!app->pdu)
348                 return;
349
350         if (mrp_cb(app->pdu)->mh)
351                 mrp_pdu_append_end_mark(app);
352         mrp_pdu_append_end_mark(app);
353
354         dev_hard_header(app->pdu, app->dev, ntohs(app->app->pkttype.type),
355                         app->app->group_address, app->dev->dev_addr,
356                         app->pdu->len);
357
358         skb_queue_tail(&app->queue, app->pdu);
359         app->pdu = NULL;
360 }
361
362 static void mrp_queue_xmit(struct mrp_applicant *app)
363 {
364         struct sk_buff *skb;
365
366         while ((skb = skb_dequeue(&app->queue)))
367                 dev_queue_xmit(skb);
368 }
369
370 static int mrp_pdu_append_msg_hdr(struct mrp_applicant *app,
371                                   u8 attrtype, u8 attrlen)
372 {
373         struct mrp_msg_hdr *mh;
374
375         if (mrp_cb(app->pdu)->mh) {
376                 if (mrp_pdu_append_end_mark(app) < 0)
377                         return -1;
378                 mrp_cb(app->pdu)->mh = NULL;
379                 mrp_cb(app->pdu)->vah = NULL;
380         }
381
382         if (skb_tailroom(app->pdu) < sizeof(*mh))
383                 return -1;
384         mh = __skb_put(app->pdu, sizeof(*mh));
385         mh->attrtype = attrtype;
386         mh->attrlen = attrlen;
387         mrp_cb(app->pdu)->mh = mh;
388         return 0;
389 }
390
391 static int mrp_pdu_append_vecattr_hdr(struct mrp_applicant *app,
392                                       const void *firstattrvalue, u8 attrlen)
393 {
394         struct mrp_vecattr_hdr *vah;
395
396         if (skb_tailroom(app->pdu) < sizeof(*vah) + attrlen)
397                 return -1;
398         vah = __skb_put(app->pdu, sizeof(*vah) + attrlen);
399         put_unaligned(0, &vah->lenflags);
400         memcpy(vah->firstattrvalue, firstattrvalue, attrlen);
401         mrp_cb(app->pdu)->vah = vah;
402         memcpy(mrp_cb(app->pdu)->attrvalue, firstattrvalue, attrlen);
403         return 0;
404 }
405
406 static int mrp_pdu_append_vecattr_event(struct mrp_applicant *app,
407                                         const struct mrp_attr *attr,
408                                         enum mrp_vecattr_event vaevent)
409 {
410         u16 len, pos;
411         u8 *vaevents;
412         int err;
413 again:
414         if (!app->pdu) {
415                 err = mrp_pdu_init(app);
416                 if (err < 0)
417                         return err;
418         }
419
420         /* If there is no Message header in the PDU, or the Message header is
421          * for a different attribute type, add an EndMark (if necessary) and a
422          * new Message header to the PDU.
423          */
424         if (!mrp_cb(app->pdu)->mh ||
425             mrp_cb(app->pdu)->mh->attrtype != attr->type ||
426             mrp_cb(app->pdu)->mh->attrlen != attr->len) {
427                 if (mrp_pdu_append_msg_hdr(app, attr->type, attr->len) < 0)
428                         goto queue;
429         }
430
431         /* If there is no VectorAttribute header for this Message in the PDU,
432          * or this attribute's value does not sequentially follow the previous
433          * attribute's value, add a new VectorAttribute header to the PDU.
434          */
435         if (!mrp_cb(app->pdu)->vah ||
436             memcmp(mrp_cb(app->pdu)->attrvalue, attr->value, attr->len)) {
437                 if (mrp_pdu_append_vecattr_hdr(app, attr->value, attr->len) < 0)
438                         goto queue;
439         }
440
441         len = be16_to_cpu(get_unaligned(&mrp_cb(app->pdu)->vah->lenflags));
442         pos = len % 3;
443
444         /* Events are packed into Vectors in the PDU, three to a byte. Add a
445          * byte to the end of the Vector if necessary.
446          */
447         if (!pos) {
448                 if (skb_tailroom(app->pdu) < sizeof(u8))
449                         goto queue;
450                 vaevents = __skb_put(app->pdu, sizeof(u8));
451         } else {
452                 vaevents = (u8 *)(skb_tail_pointer(app->pdu) - sizeof(u8));
453         }
454
455         switch (pos) {
456         case 0:
457                 *vaevents = vaevent * (__MRP_VECATTR_EVENT_MAX *
458                                        __MRP_VECATTR_EVENT_MAX);
459                 break;
460         case 1:
461                 *vaevents += vaevent * __MRP_VECATTR_EVENT_MAX;
462                 break;
463         case 2:
464                 *vaevents += vaevent;
465                 break;
466         default:
467                 WARN_ON(1);
468         }
469
470         /* Increment the length of the VectorAttribute in the PDU, as well as
471          * the value of the next attribute that would continue its Vector.
472          */
473         put_unaligned(cpu_to_be16(++len), &mrp_cb(app->pdu)->vah->lenflags);
474         mrp_attrvalue_inc(mrp_cb(app->pdu)->attrvalue, attr->len);
475
476         return 0;
477
478 queue:
479         mrp_pdu_queue(app);
480         goto again;
481 }
482
483 static void mrp_attr_event(struct mrp_applicant *app,
484                            struct mrp_attr *attr, enum mrp_event event)
485 {
486         enum mrp_applicant_state state;
487
488         state = mrp_applicant_state_table[attr->state][event];
489         if (state == MRP_APPLICANT_INVALID) {
490                 WARN_ON(1);
491                 return;
492         }
493
494         if (event == MRP_EVENT_TX) {
495                 /* When appending the attribute fails, don't update its state
496                  * in order to retry at the next TX event.
497                  */
498
499                 switch (mrp_tx_action_table[attr->state]) {
500                 case MRP_TX_ACTION_NONE:
501                 case MRP_TX_ACTION_S_JOIN_IN_OPTIONAL:
502                 case MRP_TX_ACTION_S_IN_OPTIONAL:
503                         break;
504                 case MRP_TX_ACTION_S_NEW:
505                         if (mrp_pdu_append_vecattr_event(
506                                     app, attr, MRP_VECATTR_EVENT_NEW) < 0)
507                                 return;
508                         break;
509                 case MRP_TX_ACTION_S_JOIN_IN:
510                         if (mrp_pdu_append_vecattr_event(
511                                     app, attr, MRP_VECATTR_EVENT_JOIN_IN) < 0)
512                                 return;
513                         break;
514                 case MRP_TX_ACTION_S_LV:
515                         if (mrp_pdu_append_vecattr_event(
516                                     app, attr, MRP_VECATTR_EVENT_LV) < 0)
517                                 return;
518                         /* As a pure applicant, sending a leave message
519                          * implies that the attribute was unregistered and
520                          * can be destroyed.
521                          */
522                         mrp_attr_destroy(app, attr);
523                         return;
524                 default:
525                         WARN_ON(1);
526                 }
527         }
528
529         attr->state = state;
530 }
531
532 int mrp_request_join(const struct net_device *dev,
533                      const struct mrp_application *appl,
534                      const void *value, u8 len, u8 type)
535 {
536         struct mrp_port *port = rtnl_dereference(dev->mrp_port);
537         struct mrp_applicant *app = rtnl_dereference(
538                 port->applicants[appl->type]);
539         struct mrp_attr *attr;
540
541         if (sizeof(struct mrp_skb_cb) + len >
542             FIELD_SIZEOF(struct sk_buff, cb))
543                 return -ENOMEM;
544
545         spin_lock_bh(&app->lock);
546         attr = mrp_attr_create(app, value, len, type);
547         if (!attr) {
548                 spin_unlock_bh(&app->lock);
549                 return -ENOMEM;
550         }
551         mrp_attr_event(app, attr, MRP_EVENT_JOIN);
552         spin_unlock_bh(&app->lock);
553         return 0;
554 }
555 EXPORT_SYMBOL_GPL(mrp_request_join);
556
557 void mrp_request_leave(const struct net_device *dev,
558                        const struct mrp_application *appl,
559                        const void *value, u8 len, u8 type)
560 {
561         struct mrp_port *port = rtnl_dereference(dev->mrp_port);
562         struct mrp_applicant *app = rtnl_dereference(
563                 port->applicants[appl->type]);
564         struct mrp_attr *attr;
565
566         if (sizeof(struct mrp_skb_cb) + len >
567             FIELD_SIZEOF(struct sk_buff, cb))
568                 return;
569
570         spin_lock_bh(&app->lock);
571         attr = mrp_attr_lookup(app, value, len, type);
572         if (!attr) {
573                 spin_unlock_bh(&app->lock);
574                 return;
575         }
576         mrp_attr_event(app, attr, MRP_EVENT_LV);
577         spin_unlock_bh(&app->lock);
578 }
579 EXPORT_SYMBOL_GPL(mrp_request_leave);
580
581 static void mrp_mad_event(struct mrp_applicant *app, enum mrp_event event)
582 {
583         struct rb_node *node, *next;
584         struct mrp_attr *attr;
585
586         for (node = rb_first(&app->mad);
587              next = node ? rb_next(node) : NULL, node != NULL;
588              node = next) {
589                 attr = rb_entry(node, struct mrp_attr, node);
590                 mrp_attr_event(app, attr, event);
591         }
592 }
593
594 static void mrp_join_timer_arm(struct mrp_applicant *app)
595 {
596         unsigned long delay;
597
598         delay = (u64)msecs_to_jiffies(mrp_join_time) * prandom_u32() >> 32;
599         mod_timer(&app->join_timer, jiffies + delay);
600 }
601
602 static void mrp_join_timer(struct timer_list *t)
603 {
604         struct mrp_applicant *app = from_timer(app, t, join_timer);
605
606         spin_lock(&app->lock);
607         mrp_mad_event(app, MRP_EVENT_TX);
608         mrp_pdu_queue(app);
609         spin_unlock(&app->lock);
610
611         mrp_queue_xmit(app);
612         mrp_join_timer_arm(app);
613 }
614
615 static void mrp_periodic_timer_arm(struct mrp_applicant *app)
616 {
617         mod_timer(&app->periodic_timer,
618                   jiffies + msecs_to_jiffies(mrp_periodic_time));
619 }
620
621 static void mrp_periodic_timer(struct timer_list *t)
622 {
623         struct mrp_applicant *app = from_timer(app, t, periodic_timer);
624
625         spin_lock(&app->lock);
626         mrp_mad_event(app, MRP_EVENT_PERIODIC);
627         mrp_pdu_queue(app);
628         spin_unlock(&app->lock);
629
630         mrp_periodic_timer_arm(app);
631 }
632
633 static int mrp_pdu_parse_end_mark(struct sk_buff *skb, int *offset)
634 {
635         __be16 endmark;
636
637         if (skb_copy_bits(skb, *offset, &endmark, sizeof(endmark)) < 0)
638                 return -1;
639         if (endmark == MRP_END_MARK) {
640                 *offset += sizeof(endmark);
641                 return -1;
642         }
643         return 0;
644 }
645
646 static void mrp_pdu_parse_vecattr_event(struct mrp_applicant *app,
647                                         struct sk_buff *skb,
648                                         enum mrp_vecattr_event vaevent)
649 {
650         struct mrp_attr *attr;
651         enum mrp_event event;
652
653         attr = mrp_attr_lookup(app, mrp_cb(skb)->attrvalue,
654                                mrp_cb(skb)->mh->attrlen,
655                                mrp_cb(skb)->mh->attrtype);
656         if (attr == NULL)
657                 return;
658
659         switch (vaevent) {
660         case MRP_VECATTR_EVENT_NEW:
661                 event = MRP_EVENT_R_NEW;
662                 break;
663         case MRP_VECATTR_EVENT_JOIN_IN:
664                 event = MRP_EVENT_R_JOIN_IN;
665                 break;
666         case MRP_VECATTR_EVENT_IN:
667                 event = MRP_EVENT_R_IN;
668                 break;
669         case MRP_VECATTR_EVENT_JOIN_MT:
670                 event = MRP_EVENT_R_JOIN_MT;
671                 break;
672         case MRP_VECATTR_EVENT_MT:
673                 event = MRP_EVENT_R_MT;
674                 break;
675         case MRP_VECATTR_EVENT_LV:
676                 event = MRP_EVENT_R_LV;
677                 break;
678         default:
679                 return;
680         }
681
682         mrp_attr_event(app, attr, event);
683 }
684
685 static int mrp_pdu_parse_vecattr(struct mrp_applicant *app,
686                                  struct sk_buff *skb, int *offset)
687 {
688         struct mrp_vecattr_hdr _vah;
689         u16 valen;
690         u8 vaevents, vaevent;
691
692         mrp_cb(skb)->vah = skb_header_pointer(skb, *offset, sizeof(_vah),
693                                               &_vah);
694         if (!mrp_cb(skb)->vah)
695                 return -1;
696         *offset += sizeof(_vah);
697
698         if (get_unaligned(&mrp_cb(skb)->vah->lenflags) &
699             MRP_VECATTR_HDR_FLAG_LA)
700                 mrp_mad_event(app, MRP_EVENT_R_LA);
701         valen = be16_to_cpu(get_unaligned(&mrp_cb(skb)->vah->lenflags) &
702                             MRP_VECATTR_HDR_LEN_MASK);
703
704         /* The VectorAttribute structure in a PDU carries event information
705          * about one or more attributes having consecutive values. Only the
706          * value for the first attribute is contained in the structure. So
707          * we make a copy of that value, and then increment it each time we
708          * advance to the next event in its Vector.
709          */
710         if (sizeof(struct mrp_skb_cb) + mrp_cb(skb)->mh->attrlen >
711             FIELD_SIZEOF(struct sk_buff, cb))
712                 return -1;
713         if (skb_copy_bits(skb, *offset, mrp_cb(skb)->attrvalue,
714                           mrp_cb(skb)->mh->attrlen) < 0)
715                 return -1;
716         *offset += mrp_cb(skb)->mh->attrlen;
717
718         /* In a VectorAttribute, the Vector contains events which are packed
719          * three to a byte. We process one byte of the Vector at a time.
720          */
721         while (valen > 0) {
722                 if (skb_copy_bits(skb, *offset, &vaevents,
723                                   sizeof(vaevents)) < 0)
724                         return -1;
725                 *offset += sizeof(vaevents);
726
727                 /* Extract and process the first event. */
728                 vaevent = vaevents / (__MRP_VECATTR_EVENT_MAX *
729                                       __MRP_VECATTR_EVENT_MAX);
730                 if (vaevent >= __MRP_VECATTR_EVENT_MAX) {
731                         /* The byte is malformed; stop processing. */
732                         return -1;
733                 }
734                 mrp_pdu_parse_vecattr_event(app, skb, vaevent);
735
736                 /* If present, extract and process the second event. */
737                 if (!--valen)
738                         break;
739                 mrp_attrvalue_inc(mrp_cb(skb)->attrvalue,
740                                   mrp_cb(skb)->mh->attrlen);
741                 vaevents %= (__MRP_VECATTR_EVENT_MAX *
742                              __MRP_VECATTR_EVENT_MAX);
743                 vaevent = vaevents / __MRP_VECATTR_EVENT_MAX;
744                 mrp_pdu_parse_vecattr_event(app, skb, vaevent);
745
746                 /* If present, extract and process the third event. */
747                 if (!--valen)
748                         break;
749                 mrp_attrvalue_inc(mrp_cb(skb)->attrvalue,
750                                   mrp_cb(skb)->mh->attrlen);
751                 vaevents %= __MRP_VECATTR_EVENT_MAX;
752                 vaevent = vaevents;
753                 mrp_pdu_parse_vecattr_event(app, skb, vaevent);
754         }
755         return 0;
756 }
757
758 static int mrp_pdu_parse_msg(struct mrp_applicant *app, struct sk_buff *skb,
759                              int *offset)
760 {
761         struct mrp_msg_hdr _mh;
762
763         mrp_cb(skb)->mh = skb_header_pointer(skb, *offset, sizeof(_mh), &_mh);
764         if (!mrp_cb(skb)->mh)
765                 return -1;
766         *offset += sizeof(_mh);
767
768         if (mrp_cb(skb)->mh->attrtype == 0 ||
769             mrp_cb(skb)->mh->attrtype > app->app->maxattr ||
770             mrp_cb(skb)->mh->attrlen == 0)
771                 return -1;
772
773         while (skb->len > *offset) {
774                 if (mrp_pdu_parse_end_mark(skb, offset) < 0)
775                         break;
776                 if (mrp_pdu_parse_vecattr(app, skb, offset) < 0)
777                         return -1;
778         }
779         return 0;
780 }
781
782 static int mrp_rcv(struct sk_buff *skb, struct net_device *dev,
783                    struct packet_type *pt, struct net_device *orig_dev)
784 {
785         struct mrp_application *appl = container_of(pt, struct mrp_application,
786                                                     pkttype);
787         struct mrp_port *port;
788         struct mrp_applicant *app;
789         struct mrp_pdu_hdr _ph;
790         const struct mrp_pdu_hdr *ph;
791         int offset = skb_network_offset(skb);
792
793         /* If the interface is in promiscuous mode, drop the packet if
794          * it was unicast to another host.
795          */
796         if (unlikely(skb->pkt_type == PACKET_OTHERHOST))
797                 goto out;
798         skb = skb_share_check(skb, GFP_ATOMIC);
799         if (unlikely(!skb))
800                 goto out;
801         port = rcu_dereference(dev->mrp_port);
802         if (unlikely(!port))
803                 goto out;
804         app = rcu_dereference(port->applicants[appl->type]);
805         if (unlikely(!app))
806                 goto out;
807
808         ph = skb_header_pointer(skb, offset, sizeof(_ph), &_ph);
809         if (!ph)
810                 goto out;
811         offset += sizeof(_ph);
812
813         if (ph->version != app->app->version)
814                 goto out;
815
816         spin_lock(&app->lock);
817         while (skb->len > offset) {
818                 if (mrp_pdu_parse_end_mark(skb, &offset) < 0)
819                         break;
820                 if (mrp_pdu_parse_msg(app, skb, &offset) < 0)
821                         break;
822         }
823         spin_unlock(&app->lock);
824 out:
825         kfree_skb(skb);
826         return 0;
827 }
828
829 static int mrp_init_port(struct net_device *dev)
830 {
831         struct mrp_port *port;
832
833         port = kzalloc(sizeof(*port), GFP_KERNEL);
834         if (!port)
835                 return -ENOMEM;
836         rcu_assign_pointer(dev->mrp_port, port);
837         return 0;
838 }
839
840 static void mrp_release_port(struct net_device *dev)
841 {
842         struct mrp_port *port = rtnl_dereference(dev->mrp_port);
843         unsigned int i;
844
845         for (i = 0; i <= MRP_APPLICATION_MAX; i++) {
846                 if (rtnl_dereference(port->applicants[i]))
847                         return;
848         }
849         RCU_INIT_POINTER(dev->mrp_port, NULL);
850         kfree_rcu(port, rcu);
851 }
852
853 int mrp_init_applicant(struct net_device *dev, struct mrp_application *appl)
854 {
855         struct mrp_applicant *app;
856         int err;
857
858         ASSERT_RTNL();
859
860         if (!rtnl_dereference(dev->mrp_port)) {
861                 err = mrp_init_port(dev);
862                 if (err < 0)
863                         goto err1;
864         }
865
866         err = -ENOMEM;
867         app = kzalloc(sizeof(*app), GFP_KERNEL);
868         if (!app)
869                 goto err2;
870
871         err = dev_mc_add(dev, appl->group_address);
872         if (err < 0)
873                 goto err3;
874
875         app->dev = dev;
876         app->app = appl;
877         app->mad = RB_ROOT;
878         spin_lock_init(&app->lock);
879         skb_queue_head_init(&app->queue);
880         rcu_assign_pointer(dev->mrp_port->applicants[appl->type], app);
881         timer_setup(&app->join_timer, mrp_join_timer, 0);
882         mrp_join_timer_arm(app);
883         timer_setup(&app->periodic_timer, mrp_periodic_timer, 0);
884         mrp_periodic_timer_arm(app);
885         return 0;
886
887 err3:
888         kfree(app);
889 err2:
890         mrp_release_port(dev);
891 err1:
892         return err;
893 }
894 EXPORT_SYMBOL_GPL(mrp_init_applicant);
895
896 void mrp_uninit_applicant(struct net_device *dev, struct mrp_application *appl)
897 {
898         struct mrp_port *port = rtnl_dereference(dev->mrp_port);
899         struct mrp_applicant *app = rtnl_dereference(
900                 port->applicants[appl->type]);
901
902         ASSERT_RTNL();
903
904         RCU_INIT_POINTER(port->applicants[appl->type], NULL);
905
906         /* Delete timer and generate a final TX event to flush out
907          * all pending messages before the applicant is gone.
908          */
909         del_timer_sync(&app->join_timer);
910         del_timer_sync(&app->periodic_timer);
911
912         spin_lock_bh(&app->lock);
913         mrp_mad_event(app, MRP_EVENT_TX);
914         mrp_attr_destroy_all(app);
915         mrp_pdu_queue(app);
916         spin_unlock_bh(&app->lock);
917
918         mrp_queue_xmit(app);
919
920         dev_mc_del(dev, appl->group_address);
921         kfree_rcu(app, rcu);
922         mrp_release_port(dev);
923 }
924 EXPORT_SYMBOL_GPL(mrp_uninit_applicant);
925
926 int mrp_register_application(struct mrp_application *appl)
927 {
928         appl->pkttype.func = mrp_rcv;
929         dev_add_pack(&appl->pkttype);
930         return 0;
931 }
932 EXPORT_SYMBOL_GPL(mrp_register_application);
933
934 void mrp_unregister_application(struct mrp_application *appl)
935 {
936         dev_remove_pack(&appl->pkttype);
937 }
938 EXPORT_SYMBOL_GPL(mrp_unregister_application);