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