net/802/mrp.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  *  IEEE 802.1Q Multiple Registration Protocol (MRP)
0004  *
0005  *  Copyright (c) 2012 Massachusetts Institute of Technology
0006  *
0007  *  Adapted from code in net/802/garp.c
0008  *  Copyright (c) 2008 Patrick McHardy <kaber@trash.net>
0009  */
0010 #include <linux/kernel.h>
0011 #include <linux/timer.h>
0012 #include <linux/skbuff.h>
0013 #include <linux/netdevice.h>
0014 #include <linux/etherdevice.h>
0015 #include <linux/rtnetlink.h>
0016 #include <linux/slab.h>
0017 #include <linux/module.h>
0018 #include <net/mrp.h>
0019 #include <asm/unaligned.h>
0020
0021 static unsigned int mrp_join_time __read_mostly = 200;
0022 module_param(mrp_join_time, uint, 0644);
0023 MODULE_PARM_DESC(mrp_join_time, "Join time in ms (default 200ms)");
0024
0025 static unsigned int mrp_periodic_time __read_mostly = 1000;
0026 module_param(mrp_periodic_time, uint, 0644);
0027 MODULE_PARM_DESC(mrp_periodic_time, "Periodic time in ms (default 1s)");
0028
0029 MODULE_LICENSE("GPL");
0030
0031 static const u8
0032 mrp_applicant_state_table[MRP_APPLICANT_MAX + 1][MRP_EVENT_MAX + 1] = {
0033     [MRP_APPLICANT_VO] = {
0034         [MRP_EVENT_NEW]     = MRP_APPLICANT_VN,
0035         [MRP_EVENT_JOIN]    = MRP_APPLICANT_VP,
0036         [MRP_EVENT_LV]      = MRP_APPLICANT_VO,
0037         [MRP_EVENT_TX]      = MRP_APPLICANT_VO,
0038         [MRP_EVENT_R_NEW]   = MRP_APPLICANT_VO,
0039         [MRP_EVENT_R_JOIN_IN]   = MRP_APPLICANT_AO,
0040         [MRP_EVENT_R_IN]    = MRP_APPLICANT_VO,
0041         [MRP_EVENT_R_JOIN_MT]   = MRP_APPLICANT_VO,
0042         [MRP_EVENT_R_MT]    = MRP_APPLICANT_VO,
0043         [MRP_EVENT_R_LV]    = MRP_APPLICANT_VO,
0044         [MRP_EVENT_R_LA]    = MRP_APPLICANT_VO,
0045         [MRP_EVENT_REDECLARE]   = MRP_APPLICANT_VO,
0046         [MRP_EVENT_PERIODIC]    = MRP_APPLICANT_VO,
0047     },
0048     [MRP_APPLICANT_VP] = {
0049         [MRP_EVENT_NEW]     = MRP_APPLICANT_VN,
0050         [MRP_EVENT_JOIN]    = MRP_APPLICANT_VP,
0051         [MRP_EVENT_LV]      = MRP_APPLICANT_VO,
0052         [MRP_EVENT_TX]      = MRP_APPLICANT_AA,
0053         [MRP_EVENT_R_NEW]   = MRP_APPLICANT_VP,
0054         [MRP_EVENT_R_JOIN_IN]   = MRP_APPLICANT_AP,
0055         [MRP_EVENT_R_IN]    = MRP_APPLICANT_VP,
0056         [MRP_EVENT_R_JOIN_MT]   = MRP_APPLICANT_VP,
0057         [MRP_EVENT_R_MT]    = MRP_APPLICANT_VP,
0058         [MRP_EVENT_R_LV]    = MRP_APPLICANT_VP,
0059         [MRP_EVENT_R_LA]    = MRP_APPLICANT_VP,
0060         [MRP_EVENT_REDECLARE]   = MRP_APPLICANT_VP,
0061         [MRP_EVENT_PERIODIC]    = MRP_APPLICANT_VP,
0062     },
0063     [MRP_APPLICANT_VN] = {
0064         [MRP_EVENT_NEW]     = MRP_APPLICANT_VN,
0065         [MRP_EVENT_JOIN]    = MRP_APPLICANT_VN,
0066         [MRP_EVENT_LV]      = MRP_APPLICANT_LA,
0067         [MRP_EVENT_TX]      = MRP_APPLICANT_AN,
0068         [MRP_EVENT_R_NEW]   = MRP_APPLICANT_VN,
0069         [MRP_EVENT_R_JOIN_IN]   = MRP_APPLICANT_VN,
0070         [MRP_EVENT_R_IN]    = MRP_APPLICANT_VN,
0071         [MRP_EVENT_R_JOIN_MT]   = MRP_APPLICANT_VN,
0072         [MRP_EVENT_R_MT]    = MRP_APPLICANT_VN,
0073         [MRP_EVENT_R_LV]    = MRP_APPLICANT_VN,
0074         [MRP_EVENT_R_LA]    = MRP_APPLICANT_VN,
0075         [MRP_EVENT_REDECLARE]   = MRP_APPLICANT_VN,
0076         [MRP_EVENT_PERIODIC]    = MRP_APPLICANT_VN,
0077     },
0078     [MRP_APPLICANT_AN] = {
0079         [MRP_EVENT_NEW]     = MRP_APPLICANT_AN,
0080         [MRP_EVENT_JOIN]    = MRP_APPLICANT_AN,
0081         [MRP_EVENT_LV]      = MRP_APPLICANT_LA,
0082         [MRP_EVENT_TX]      = MRP_APPLICANT_QA,
0083         [MRP_EVENT_R_NEW]   = MRP_APPLICANT_AN,
0084         [MRP_EVENT_R_JOIN_IN]   = MRP_APPLICANT_AN,
0085         [MRP_EVENT_R_IN]    = MRP_APPLICANT_AN,
0086         [MRP_EVENT_R_JOIN_MT]   = MRP_APPLICANT_AN,
0087         [MRP_EVENT_R_MT]    = MRP_APPLICANT_AN,
0088         [MRP_EVENT_R_LV]    = MRP_APPLICANT_VN,
0089         [MRP_EVENT_R_LA]    = MRP_APPLICANT_VN,
0090         [MRP_EVENT_REDECLARE]   = MRP_APPLICANT_VN,
0091         [MRP_EVENT_PERIODIC]    = MRP_APPLICANT_AN,
0092     },
0093     [MRP_APPLICANT_AA] = {
0094         [MRP_EVENT_NEW]     = MRP_APPLICANT_VN,
0095         [MRP_EVENT_JOIN]    = MRP_APPLICANT_AA,
0096         [MRP_EVENT_LV]      = MRP_APPLICANT_LA,
0097         [MRP_EVENT_TX]      = MRP_APPLICANT_QA,
0098         [MRP_EVENT_R_NEW]   = MRP_APPLICANT_AA,
0099         [MRP_EVENT_R_JOIN_IN]   = MRP_APPLICANT_QA,
0100         [MRP_EVENT_R_IN]    = MRP_APPLICANT_AA,
0101         [MRP_EVENT_R_JOIN_MT]   = MRP_APPLICANT_AA,
0102         [MRP_EVENT_R_MT]    = MRP_APPLICANT_AA,
0103         [MRP_EVENT_R_LV]    = MRP_APPLICANT_VP,
0104         [MRP_EVENT_R_LA]    = MRP_APPLICANT_VP,
0105         [MRP_EVENT_REDECLARE]   = MRP_APPLICANT_VP,
0106         [MRP_EVENT_PERIODIC]    = MRP_APPLICANT_AA,
0107     },
0108     [MRP_APPLICANT_QA] = {
0109         [MRP_EVENT_NEW]     = MRP_APPLICANT_VN,
0110         [MRP_EVENT_JOIN]    = MRP_APPLICANT_QA,
0111         [MRP_EVENT_LV]      = MRP_APPLICANT_LA,
0112         [MRP_EVENT_TX]      = MRP_APPLICANT_QA,
0113         [MRP_EVENT_R_NEW]   = MRP_APPLICANT_QA,
0114         [MRP_EVENT_R_JOIN_IN]   = MRP_APPLICANT_QA,
0115         [MRP_EVENT_R_IN]    = MRP_APPLICANT_QA,
0116         [MRP_EVENT_R_JOIN_MT]   = MRP_APPLICANT_AA,
0117         [MRP_EVENT_R_MT]    = MRP_APPLICANT_AA,
0118         [MRP_EVENT_R_LV]    = MRP_APPLICANT_VP,
0119         [MRP_EVENT_R_LA]    = MRP_APPLICANT_VP,
0120         [MRP_EVENT_REDECLARE]   = MRP_APPLICANT_VP,
0121         [MRP_EVENT_PERIODIC]    = MRP_APPLICANT_AA,
0122     },
0123     [MRP_APPLICANT_LA] = {
0124         [MRP_EVENT_NEW]     = MRP_APPLICANT_VN,
0125         [MRP_EVENT_JOIN]    = MRP_APPLICANT_AA,
0126         [MRP_EVENT_LV]      = MRP_APPLICANT_LA,
0127         [MRP_EVENT_TX]      = MRP_APPLICANT_VO,
0128         [MRP_EVENT_R_NEW]   = MRP_APPLICANT_LA,
0129         [MRP_EVENT_R_JOIN_IN]   = MRP_APPLICANT_LA,
0130         [MRP_EVENT_R_IN]    = MRP_APPLICANT_LA,
0131         [MRP_EVENT_R_JOIN_MT]   = MRP_APPLICANT_LA,
0132         [MRP_EVENT_R_MT]    = MRP_APPLICANT_LA,
0133         [MRP_EVENT_R_LV]    = MRP_APPLICANT_LA,
0134         [MRP_EVENT_R_LA]    = MRP_APPLICANT_LA,
0135         [MRP_EVENT_REDECLARE]   = MRP_APPLICANT_LA,
0136         [MRP_EVENT_PERIODIC]    = MRP_APPLICANT_LA,
0137     },
0138     [MRP_APPLICANT_AO] = {
0139         [MRP_EVENT_NEW]     = MRP_APPLICANT_VN,
0140         [MRP_EVENT_JOIN]    = MRP_APPLICANT_AP,
0141         [MRP_EVENT_LV]      = MRP_APPLICANT_AO,
0142         [MRP_EVENT_TX]      = MRP_APPLICANT_AO,
0143         [MRP_EVENT_R_NEW]   = MRP_APPLICANT_AO,
0144         [MRP_EVENT_R_JOIN_IN]   = MRP_APPLICANT_QO,
0145         [MRP_EVENT_R_IN]    = MRP_APPLICANT_AO,
0146         [MRP_EVENT_R_JOIN_MT]   = MRP_APPLICANT_AO,
0147         [MRP_EVENT_R_MT]    = MRP_APPLICANT_AO,
0148         [MRP_EVENT_R_LV]    = MRP_APPLICANT_VO,
0149         [MRP_EVENT_R_LA]    = MRP_APPLICANT_VO,
0150         [MRP_EVENT_REDECLARE]   = MRP_APPLICANT_VO,
0151         [MRP_EVENT_PERIODIC]    = MRP_APPLICANT_AO,
0152     },
0153     [MRP_APPLICANT_QO] = {
0154         [MRP_EVENT_NEW]     = MRP_APPLICANT_VN,
0155         [MRP_EVENT_JOIN]    = MRP_APPLICANT_QP,
0156         [MRP_EVENT_LV]      = MRP_APPLICANT_QO,
0157         [MRP_EVENT_TX]      = MRP_APPLICANT_QO,
0158         [MRP_EVENT_R_NEW]   = MRP_APPLICANT_QO,
0159         [MRP_EVENT_R_JOIN_IN]   = MRP_APPLICANT_QO,
0160         [MRP_EVENT_R_IN]    = MRP_APPLICANT_QO,
0161         [MRP_EVENT_R_JOIN_MT]   = MRP_APPLICANT_AO,
0162         [MRP_EVENT_R_MT]    = MRP_APPLICANT_AO,
0163         [MRP_EVENT_R_LV]    = MRP_APPLICANT_VO,
0164         [MRP_EVENT_R_LA]    = MRP_APPLICANT_VO,
0165         [MRP_EVENT_REDECLARE]   = MRP_APPLICANT_VO,
0166         [MRP_EVENT_PERIODIC]    = MRP_APPLICANT_QO,
0167     },
0168     [MRP_APPLICANT_AP] = {
0169         [MRP_EVENT_NEW]     = MRP_APPLICANT_VN,
0170         [MRP_EVENT_JOIN]    = MRP_APPLICANT_AP,
0171         [MRP_EVENT_LV]      = MRP_APPLICANT_AO,
0172         [MRP_EVENT_TX]      = MRP_APPLICANT_QA,
0173         [MRP_EVENT_R_NEW]   = MRP_APPLICANT_AP,
0174         [MRP_EVENT_R_JOIN_IN]   = MRP_APPLICANT_QP,
0175         [MRP_EVENT_R_IN]    = MRP_APPLICANT_AP,
0176         [MRP_EVENT_R_JOIN_MT]   = MRP_APPLICANT_AP,
0177         [MRP_EVENT_R_MT]    = MRP_APPLICANT_AP,
0178         [MRP_EVENT_R_LV]    = MRP_APPLICANT_VP,
0179         [MRP_EVENT_R_LA]    = MRP_APPLICANT_VP,
0180         [MRP_EVENT_REDECLARE]   = MRP_APPLICANT_VP,
0181         [MRP_EVENT_PERIODIC]    = MRP_APPLICANT_AP,
0182     },
0183     [MRP_APPLICANT_QP] = {
0184         [MRP_EVENT_NEW]     = MRP_APPLICANT_VN,
0185         [MRP_EVENT_JOIN]    = MRP_APPLICANT_QP,
0186         [MRP_EVENT_LV]      = MRP_APPLICANT_QO,
0187         [MRP_EVENT_TX]      = MRP_APPLICANT_QP,
0188         [MRP_EVENT_R_NEW]   = MRP_APPLICANT_QP,
0189         [MRP_EVENT_R_JOIN_IN]   = MRP_APPLICANT_QP,
0190         [MRP_EVENT_R_IN]    = MRP_APPLICANT_QP,
0191         [MRP_EVENT_R_JOIN_MT]   = MRP_APPLICANT_AP,
0192         [MRP_EVENT_R_MT]    = MRP_APPLICANT_AP,
0193         [MRP_EVENT_R_LV]    = MRP_APPLICANT_VP,
0194         [MRP_EVENT_R_LA]    = MRP_APPLICANT_VP,
0195         [MRP_EVENT_REDECLARE]   = MRP_APPLICANT_VP,
0196         [MRP_EVENT_PERIODIC]    = MRP_APPLICANT_AP,
0197     },
0198 };
0199
0200 static const u8
0201 mrp_tx_action_table[MRP_APPLICANT_MAX + 1] = {
0202     [MRP_APPLICANT_VO] = MRP_TX_ACTION_S_IN_OPTIONAL,
0203     [MRP_APPLICANT_VP] = MRP_TX_ACTION_S_JOIN_IN,
0204     [MRP_APPLICANT_VN] = MRP_TX_ACTION_S_NEW,
0205     [MRP_APPLICANT_AN] = MRP_TX_ACTION_S_NEW,
0206     [MRP_APPLICANT_AA] = MRP_TX_ACTION_S_JOIN_IN,
0207     [MRP_APPLICANT_QA] = MRP_TX_ACTION_S_JOIN_IN_OPTIONAL,
0208     [MRP_APPLICANT_LA] = MRP_TX_ACTION_S_LV,
0209     [MRP_APPLICANT_AO] = MRP_TX_ACTION_S_IN_OPTIONAL,
0210     [MRP_APPLICANT_QO] = MRP_TX_ACTION_S_IN_OPTIONAL,
0211     [MRP_APPLICANT_AP] = MRP_TX_ACTION_S_JOIN_IN,
0212     [MRP_APPLICANT_QP] = MRP_TX_ACTION_S_IN_OPTIONAL,
0213 };
0214
0215 static void mrp_attrvalue_inc(void *value, u8 len)
0216 {
0217     u8 *v = (u8 *)value;
0218
0219     /* Add 1 to the last byte. If it becomes zero,
0220      * go to the previous byte and repeat.
0221      */
0222     while (len > 0 && !++v[--len])
0223         ;
0224 }
0225
0226 static int mrp_attr_cmp(const struct mrp_attr *attr,
0227              const void *value, u8 len, u8 type)
0228 {
0229     if (attr->type != type)
0230         return attr->type - type;
0231     if (attr->len != len)
0232         return attr->len - len;
0233     return memcmp(attr->value, value, len);
0234 }
0235
0236 static struct mrp_attr *mrp_attr_lookup(const struct mrp_applicant *app,
0237                     const void *value, u8 len, u8 type)
0238 {
0239     struct rb_node *parent = app->mad.rb_node;
0240     struct mrp_attr *attr;
0241     int d;
0242
0243     while (parent) {
0244         attr = rb_entry(parent, struct mrp_attr, node);
0245         d = mrp_attr_cmp(attr, value, len, type);
0246         if (d > 0)
0247             parent = parent->rb_left;
0248         else if (d < 0)
0249             parent = parent->rb_right;
0250         else
0251             return attr;
0252     }
0253     return NULL;
0254 }
0255
0256 static struct mrp_attr *mrp_attr_create(struct mrp_applicant *app,
0257                     const void *value, u8 len, u8 type)
0258 {
0259     struct rb_node *parent = NULL, **p = &app->mad.rb_node;
0260     struct mrp_attr *attr;
0261     int d;
0262
0263     while (*p) {
0264         parent = *p;
0265         attr = rb_entry(parent, struct mrp_attr, node);
0266         d = mrp_attr_cmp(attr, value, len, type);
0267         if (d > 0)
0268             p = &parent->rb_left;
0269         else if (d < 0)
0270             p = &parent->rb_right;
0271         else {
0272             /* The attribute already exists; re-use it. */
0273             return attr;
0274         }
0275     }
0276     attr = kmalloc(sizeof(*attr) + len, GFP_ATOMIC);
0277     if (!attr)
0278         return attr;
0279     attr->state = MRP_APPLICANT_VO;
0280     attr->type  = type;
0281     attr->len   = len;
0282     memcpy(attr->value, value, len);
0283
0284     rb_link_node(&attr->node, parent, p);
0285     rb_insert_color(&attr->node, &app->mad);
0286     return attr;
0287 }
0288
0289 static void mrp_attr_destroy(struct mrp_applicant *app, struct mrp_attr *attr)
0290 {
0291     rb_erase(&attr->node, &app->mad);
0292     kfree(attr);
0293 }
0294
0295 static void mrp_attr_destroy_all(struct mrp_applicant *app)
0296 {
0297     struct rb_node *node, *next;
0298     struct mrp_attr *attr;
0299
0300     for (node = rb_first(&app->mad);
0301          next = node ? rb_next(node) : NULL, node != NULL;
0302          node = next) {
0303         attr = rb_entry(node, struct mrp_attr, node);
0304         mrp_attr_destroy(app, attr);
0305     }
0306 }
0307
0308 static int mrp_pdu_init(struct mrp_applicant *app)
0309 {
0310     struct sk_buff *skb;
0311     struct mrp_pdu_hdr *ph;
0312
0313     skb = alloc_skb(app->dev->mtu + LL_RESERVED_SPACE(app->dev),
0314             GFP_ATOMIC);
0315     if (!skb)
0316         return -ENOMEM;
0317
0318     skb->dev = app->dev;
0319     skb->protocol = app->app->pkttype.type;
0320     skb_reserve(skb, LL_RESERVED_SPACE(app->dev));
0321     skb_reset_network_header(skb);
0322     skb_reset_transport_header(skb);
0323
0324     ph = __skb_put(skb, sizeof(*ph));
0325     ph->version = app->app->version;
0326
0327     app->pdu = skb;
0328     return 0;
0329 }
0330
0331 static int mrp_pdu_append_end_mark(struct mrp_applicant *app)
0332 {
0333     __be16 *endmark;
0334
0335     if (skb_tailroom(app->pdu) < sizeof(*endmark))
0336         return -1;
0337     endmark = __skb_put(app->pdu, sizeof(*endmark));
0338     put_unaligned(MRP_END_MARK, endmark);
0339     return 0;
0340 }
0341
0342 static void mrp_pdu_queue(struct mrp_applicant *app)
0343 {
0344     if (!app->pdu)
0345         return;
0346
0347     if (mrp_cb(app->pdu)->mh)
0348         mrp_pdu_append_end_mark(app);
0349     mrp_pdu_append_end_mark(app);
0350
0351     dev_hard_header(app->pdu, app->dev, ntohs(app->app->pkttype.type),
0352             app->app->group_address, app->dev->dev_addr,
0353             app->pdu->len);
0354
0355     skb_queue_tail(&app->queue, app->pdu);
0356     app->pdu = NULL;
0357 }
0358
0359 static void mrp_queue_xmit(struct mrp_applicant *app)
0360 {
0361     struct sk_buff *skb;
0362
0363     while ((skb = skb_dequeue(&app->queue)))
0364         dev_queue_xmit(skb);
0365 }
0366
0367 static int mrp_pdu_append_msg_hdr(struct mrp_applicant *app,
0368                   u8 attrtype, u8 attrlen)
0369 {
0370     struct mrp_msg_hdr *mh;
0371
0372     if (mrp_cb(app->pdu)->mh) {
0373         if (mrp_pdu_append_end_mark(app) < 0)
0374             return -1;
0375         mrp_cb(app->pdu)->mh = NULL;
0376         mrp_cb(app->pdu)->vah = NULL;
0377     }
0378
0379     if (skb_tailroom(app->pdu) < sizeof(*mh))
0380         return -1;
0381     mh = __skb_put(app->pdu, sizeof(*mh));
0382     mh->attrtype = attrtype;
0383     mh->attrlen = attrlen;
0384     mrp_cb(app->pdu)->mh = mh;
0385     return 0;
0386 }
0387
0388 static int mrp_pdu_append_vecattr_hdr(struct mrp_applicant *app,
0389                       const void *firstattrvalue, u8 attrlen)
0390 {
0391     struct mrp_vecattr_hdr *vah;
0392
0393     if (skb_tailroom(app->pdu) < sizeof(*vah) + attrlen)
0394         return -1;
0395     vah = __skb_put(app->pdu, sizeof(*vah) + attrlen);
0396     put_unaligned(0, &vah->lenflags);
0397     memcpy(vah->firstattrvalue, firstattrvalue, attrlen);
0398     mrp_cb(app->pdu)->vah = vah;
0399     memcpy(mrp_cb(app->pdu)->attrvalue, firstattrvalue, attrlen);
0400     return 0;
0401 }
0402
0403 static int mrp_pdu_append_vecattr_event(struct mrp_applicant *app,
0404                     const struct mrp_attr *attr,
0405                     enum mrp_vecattr_event vaevent)
0406 {
0407     u16 len, pos;
0408     u8 *vaevents;
0409     int err;
0410 again:
0411     if (!app->pdu) {
0412         err = mrp_pdu_init(app);
0413         if (err < 0)
0414             return err;
0415     }
0416
0417     /* If there is no Message header in the PDU, or the Message header is
0418      * for a different attribute type, add an EndMark (if necessary) and a
0419      * new Message header to the PDU.
0420      */
0421     if (!mrp_cb(app->pdu)->mh ||
0422         mrp_cb(app->pdu)->mh->attrtype != attr->type ||
0423         mrp_cb(app->pdu)->mh->attrlen != attr->len) {
0424         if (mrp_pdu_append_msg_hdr(app, attr->type, attr->len) < 0)
0425             goto queue;
0426     }
0427
0428     /* If there is no VectorAttribute header for this Message in the PDU,
0429      * or this attribute's value does not sequentially follow the previous
0430      * attribute's value, add a new VectorAttribute header to the PDU.
0431      */
0432     if (!mrp_cb(app->pdu)->vah ||
0433         memcmp(mrp_cb(app->pdu)->attrvalue, attr->value, attr->len)) {
0434         if (mrp_pdu_append_vecattr_hdr(app, attr->value, attr->len) < 0)
0435             goto queue;
0436     }
0437
0438     len = be16_to_cpu(get_unaligned(&mrp_cb(app->pdu)->vah->lenflags));
0439     pos = len % 3;
0440
0441     /* Events are packed into Vectors in the PDU, three to a byte. Add a
0442      * byte to the end of the Vector if necessary.
0443      */
0444     if (!pos) {
0445         if (skb_tailroom(app->pdu) < sizeof(u8))
0446             goto queue;
0447         vaevents = __skb_put(app->pdu, sizeof(u8));
0448     } else {
0449         vaevents = (u8 *)(skb_tail_pointer(app->pdu) - sizeof(u8));
0450     }
0451
0452     switch (pos) {
0453     case 0:
0454         *vaevents = vaevent * (__MRP_VECATTR_EVENT_MAX *
0455                        __MRP_VECATTR_EVENT_MAX);
0456         break;
0457     case 1:
0458         *vaevents += vaevent * __MRP_VECATTR_EVENT_MAX;
0459         break;
0460     case 2:
0461         *vaevents += vaevent;
0462         break;
0463     default:
0464         WARN_ON(1);
0465     }
0466
0467     /* Increment the length of the VectorAttribute in the PDU, as well as
0468      * the value of the next attribute that would continue its Vector.
0469      */
0470     put_unaligned(cpu_to_be16(++len), &mrp_cb(app->pdu)->vah->lenflags);
0471     mrp_attrvalue_inc(mrp_cb(app->pdu)->attrvalue, attr->len);
0472
0473     return 0;
0474
0475 queue:
0476     mrp_pdu_queue(app);
0477     goto again;
0478 }
0479
0480 static void mrp_attr_event(struct mrp_applicant *app,
0481                struct mrp_attr *attr, enum mrp_event event)
0482 {
0483     enum mrp_applicant_state state;
0484
0485     state = mrp_applicant_state_table[attr->state][event];
0486     if (state == MRP_APPLICANT_INVALID) {
0487         WARN_ON(1);
0488         return;
0489     }
0490
0491     if (event == MRP_EVENT_TX) {
0492         /* When appending the attribute fails, don't update its state
0493          * in order to retry at the next TX event.
0494          */
0495
0496         switch (mrp_tx_action_table[attr->state]) {
0497         case MRP_TX_ACTION_NONE:
0498         case MRP_TX_ACTION_S_JOIN_IN_OPTIONAL:
0499         case MRP_TX_ACTION_S_IN_OPTIONAL:
0500             break;
0501         case MRP_TX_ACTION_S_NEW:
0502             if (mrp_pdu_append_vecattr_event(
0503                     app, attr, MRP_VECATTR_EVENT_NEW) < 0)
0504                 return;
0505             break;
0506         case MRP_TX_ACTION_S_JOIN_IN:
0507             if (mrp_pdu_append_vecattr_event(
0508                     app, attr, MRP_VECATTR_EVENT_JOIN_IN) < 0)
0509                 return;
0510             break;
0511         case MRP_TX_ACTION_S_LV:
0512             if (mrp_pdu_append_vecattr_event(
0513                     app, attr, MRP_VECATTR_EVENT_LV) < 0)
0514                 return;
0515             /* As a pure applicant, sending a leave message
0516              * implies that the attribute was unregistered and
0517              * can be destroyed.
0518              */
0519             mrp_attr_destroy(app, attr);
0520             return;
0521         default:
0522             WARN_ON(1);
0523         }
0524     }
0525
0526     attr->state = state;
0527 }
0528
0529 int mrp_request_join(const struct net_device *dev,
0530              const struct mrp_application *appl,
0531              const void *value, u8 len, u8 type)
0532 {
0533     struct mrp_port *port = rtnl_dereference(dev->mrp_port);
0534     struct mrp_applicant *app = rtnl_dereference(
0535         port->applicants[appl->type]);
0536     struct mrp_attr *attr;
0537
0538     if (sizeof(struct mrp_skb_cb) + len >
0539         sizeof_field(struct sk_buff, cb))
0540         return -ENOMEM;
0541
0542     spin_lock_bh(&app->lock);
0543     attr = mrp_attr_create(app, value, len, type);
0544     if (!attr) {
0545         spin_unlock_bh(&app->lock);
0546         return -ENOMEM;
0547     }
0548     mrp_attr_event(app, attr, MRP_EVENT_JOIN);
0549     spin_unlock_bh(&app->lock);
0550     return 0;
0551 }
0552 EXPORT_SYMBOL_GPL(mrp_request_join);
0553
0554 void mrp_request_leave(const struct net_device *dev,
0555                const struct mrp_application *appl,
0556                const void *value, u8 len, u8 type)
0557 {
0558     struct mrp_port *port = rtnl_dereference(dev->mrp_port);
0559     struct mrp_applicant *app = rtnl_dereference(
0560         port->applicants[appl->type]);
0561     struct mrp_attr *attr;
0562
0563     if (sizeof(struct mrp_skb_cb) + len >
0564         sizeof_field(struct sk_buff, cb))
0565         return;
0566
0567     spin_lock_bh(&app->lock);
0568     attr = mrp_attr_lookup(app, value, len, type);
0569     if (!attr) {
0570         spin_unlock_bh(&app->lock);
0571         return;
0572     }
0573     mrp_attr_event(app, attr, MRP_EVENT_LV);
0574     spin_unlock_bh(&app->lock);
0575 }
0576 EXPORT_SYMBOL_GPL(mrp_request_leave);
0577
0578 static void mrp_mad_event(struct mrp_applicant *app, enum mrp_event event)
0579 {
0580     struct rb_node *node, *next;
0581     struct mrp_attr *attr;
0582
0583     for (node = rb_first(&app->mad);
0584          next = node ? rb_next(node) : NULL, node != NULL;
0585          node = next) {
0586         attr = rb_entry(node, struct mrp_attr, node);
0587         mrp_attr_event(app, attr, event);
0588     }
0589 }
0590
0591 static void mrp_join_timer_arm(struct mrp_applicant *app)
0592 {
0593     unsigned long delay;
0594
0595     delay = (u64)msecs_to_jiffies(mrp_join_time) * prandom_u32() >> 32;
0596     mod_timer(&app->join_timer, jiffies + delay);
0597 }
0598
0599 static void mrp_join_timer(struct timer_list *t)
0600 {
0601     struct mrp_applicant *app = from_timer(app, t, join_timer);
0602
0603     spin_lock(&app->lock);
0604     mrp_mad_event(app, MRP_EVENT_TX);
0605     mrp_pdu_queue(app);
0606     spin_unlock(&app->lock);
0607
0608     mrp_queue_xmit(app);
0609     mrp_join_timer_arm(app);
0610 }
0611
0612 static void mrp_periodic_timer_arm(struct mrp_applicant *app)
0613 {
0614     mod_timer(&app->periodic_timer,
0615           jiffies + msecs_to_jiffies(mrp_periodic_time));
0616 }
0617
0618 static void mrp_periodic_timer(struct timer_list *t)
0619 {
0620     struct mrp_applicant *app = from_timer(app, t, periodic_timer);
0621
0622     spin_lock(&app->lock);
0623     mrp_mad_event(app, MRP_EVENT_PERIODIC);
0624     mrp_pdu_queue(app);
0625     spin_unlock(&app->lock);
0626
0627     mrp_periodic_timer_arm(app);
0628 }
0629
0630 static int mrp_pdu_parse_end_mark(struct sk_buff *skb, int *offset)
0631 {
0632     __be16 endmark;
0633
0634     if (skb_copy_bits(skb, *offset, &endmark, sizeof(endmark)) < 0)
0635         return -1;
0636     if (endmark == MRP_END_MARK) {
0637         *offset += sizeof(endmark);
0638         return -1;
0639     }
0640     return 0;
0641 }
0642
0643 static void mrp_pdu_parse_vecattr_event(struct mrp_applicant *app,
0644                     struct sk_buff *skb,
0645                     enum mrp_vecattr_event vaevent)
0646 {
0647     struct mrp_attr *attr;
0648     enum mrp_event event;
0649
0650     attr = mrp_attr_lookup(app, mrp_cb(skb)->attrvalue,
0651                    mrp_cb(skb)->mh->attrlen,
0652                    mrp_cb(skb)->mh->attrtype);
0653     if (attr == NULL)
0654         return;
0655
0656     switch (vaevent) {
0657     case MRP_VECATTR_EVENT_NEW:
0658         event = MRP_EVENT_R_NEW;
0659         break;
0660     case MRP_VECATTR_EVENT_JOIN_IN:
0661         event = MRP_EVENT_R_JOIN_IN;
0662         break;
0663     case MRP_VECATTR_EVENT_IN:
0664         event = MRP_EVENT_R_IN;
0665         break;
0666     case MRP_VECATTR_EVENT_JOIN_MT:
0667         event = MRP_EVENT_R_JOIN_MT;
0668         break;
0669     case MRP_VECATTR_EVENT_MT:
0670         event = MRP_EVENT_R_MT;
0671         break;
0672     case MRP_VECATTR_EVENT_LV:
0673         event = MRP_EVENT_R_LV;
0674         break;
0675     default:
0676         return;
0677     }
0678
0679     mrp_attr_event(app, attr, event);
0680 }
0681
0682 static int mrp_pdu_parse_vecattr(struct mrp_applicant *app,
0683                  struct sk_buff *skb, int *offset)
0684 {
0685     struct mrp_vecattr_hdr _vah;
0686     u16 valen;
0687     u8 vaevents, vaevent;
0688
0689     mrp_cb(skb)->vah = skb_header_pointer(skb, *offset, sizeof(_vah),
0690                           &_vah);
0691     if (!mrp_cb(skb)->vah)
0692         return -1;
0693     *offset += sizeof(_vah);
0694
0695     if (get_unaligned(&mrp_cb(skb)->vah->lenflags) &
0696         MRP_VECATTR_HDR_FLAG_LA)
0697         mrp_mad_event(app, MRP_EVENT_R_LA);
0698     valen = be16_to_cpu(get_unaligned(&mrp_cb(skb)->vah->lenflags) &
0699                 MRP_VECATTR_HDR_LEN_MASK);
0700
0701     /* The VectorAttribute structure in a PDU carries event information
0702      * about one or more attributes having consecutive values. Only the
0703      * value for the first attribute is contained in the structure. So
0704      * we make a copy of that value, and then increment it each time we
0705      * advance to the next event in its Vector.
0706      */
0707     if (sizeof(struct mrp_skb_cb) + mrp_cb(skb)->mh->attrlen >
0708         sizeof_field(struct sk_buff, cb))
0709         return -1;
0710     if (skb_copy_bits(skb, *offset, mrp_cb(skb)->attrvalue,
0711               mrp_cb(skb)->mh->attrlen) < 0)
0712         return -1;
0713     *offset += mrp_cb(skb)->mh->attrlen;
0714
0715     /* In a VectorAttribute, the Vector contains events which are packed
0716      * three to a byte. We process one byte of the Vector at a time.
0717      */
0718     while (valen > 0) {
0719         if (skb_copy_bits(skb, *offset, &vaevents,
0720                   sizeof(vaevents)) < 0)
0721             return -1;
0722         *offset += sizeof(vaevents);
0723
0724         /* Extract and process the first event. */
0725         vaevent = vaevents / (__MRP_VECATTR_EVENT_MAX *
0726                       __MRP_VECATTR_EVENT_MAX);
0727         if (vaevent >= __MRP_VECATTR_EVENT_MAX) {
0728             /* The byte is malformed; stop processing. */
0729             return -1;
0730         }
0731         mrp_pdu_parse_vecattr_event(app, skb, vaevent);
0732
0733         /* If present, extract and process the second event. */
0734         if (!--valen)
0735             break;
0736         mrp_attrvalue_inc(mrp_cb(skb)->attrvalue,
0737                   mrp_cb(skb)->mh->attrlen);
0738         vaevents %= (__MRP_VECATTR_EVENT_MAX *
0739                  __MRP_VECATTR_EVENT_MAX);
0740         vaevent = vaevents / __MRP_VECATTR_EVENT_MAX;
0741         mrp_pdu_parse_vecattr_event(app, skb, vaevent);
0742
0743         /* If present, extract and process the third event. */
0744         if (!--valen)
0745             break;
0746         mrp_attrvalue_inc(mrp_cb(skb)->attrvalue,
0747                   mrp_cb(skb)->mh->attrlen);
0748         vaevents %= __MRP_VECATTR_EVENT_MAX;
0749         vaevent = vaevents;
0750         mrp_pdu_parse_vecattr_event(app, skb, vaevent);
0751     }
0752     return 0;
0753 }
0754
0755 static int mrp_pdu_parse_msg(struct mrp_applicant *app, struct sk_buff *skb,
0756                  int *offset)
0757 {
0758     struct mrp_msg_hdr _mh;
0759
0760     mrp_cb(skb)->mh = skb_header_pointer(skb, *offset, sizeof(_mh), &_mh);
0761     if (!mrp_cb(skb)->mh)
0762         return -1;
0763     *offset += sizeof(_mh);
0764
0765     if (mrp_cb(skb)->mh->attrtype == 0 ||
0766         mrp_cb(skb)->mh->attrtype > app->app->maxattr ||
0767         mrp_cb(skb)->mh->attrlen == 0)
0768         return -1;
0769
0770     while (skb->len > *offset) {
0771         if (mrp_pdu_parse_end_mark(skb, offset) < 0)
0772             break;
0773         if (mrp_pdu_parse_vecattr(app, skb, offset) < 0)
0774             return -1;
0775     }
0776     return 0;
0777 }
0778
0779 static int mrp_rcv(struct sk_buff *skb, struct net_device *dev,
0780            struct packet_type *pt, struct net_device *orig_dev)
0781 {
0782     struct mrp_application *appl = container_of(pt, struct mrp_application,
0783                             pkttype);
0784     struct mrp_port *port;
0785     struct mrp_applicant *app;
0786     struct mrp_pdu_hdr _ph;
0787     const struct mrp_pdu_hdr *ph;
0788     int offset = skb_network_offset(skb);
0789
0790     /* If the interface is in promiscuous mode, drop the packet if
0791      * it was unicast to another host.
0792      */
0793     if (unlikely(skb->pkt_type == PACKET_OTHERHOST))
0794         goto out;
0795     skb = skb_share_check(skb, GFP_ATOMIC);
0796     if (unlikely(!skb))
0797         goto out;
0798     port = rcu_dereference(dev->mrp_port);
0799     if (unlikely(!port))
0800         goto out;
0801     app = rcu_dereference(port->applicants[appl->type]);
0802     if (unlikely(!app))
0803         goto out;
0804
0805     ph = skb_header_pointer(skb, offset, sizeof(_ph), &_ph);
0806     if (!ph)
0807         goto out;
0808     offset += sizeof(_ph);
0809
0810     if (ph->version != app->app->version)
0811         goto out;
0812
0813     spin_lock(&app->lock);
0814     while (skb->len > offset) {
0815         if (mrp_pdu_parse_end_mark(skb, &offset) < 0)
0816             break;
0817         if (mrp_pdu_parse_msg(app, skb, &offset) < 0)
0818             break;
0819     }
0820     spin_unlock(&app->lock);
0821 out:
0822     kfree_skb(skb);
0823     return 0;
0824 }
0825
0826 static int mrp_init_port(struct net_device *dev)
0827 {
0828     struct mrp_port *port;
0829
0830     port = kzalloc(sizeof(*port), GFP_KERNEL);
0831     if (!port)
0832         return -ENOMEM;
0833     rcu_assign_pointer(dev->mrp_port, port);
0834     return 0;
0835 }
0836
0837 static void mrp_release_port(struct net_device *dev)
0838 {
0839     struct mrp_port *port = rtnl_dereference(dev->mrp_port);
0840     unsigned int i;
0841
0842     for (i = 0; i <= MRP_APPLICATION_MAX; i++) {
0843         if (rtnl_dereference(port->applicants[i]))
0844             return;
0845     }
0846     RCU_INIT_POINTER(dev->mrp_port, NULL);
0847     kfree_rcu(port, rcu);
0848 }
0849
0850 int mrp_init_applicant(struct net_device *dev, struct mrp_application *appl)
0851 {
0852     struct mrp_applicant *app;
0853     int err;
0854
0855     ASSERT_RTNL();
0856
0857     if (!rtnl_dereference(dev->mrp_port)) {
0858         err = mrp_init_port(dev);
0859         if (err < 0)
0860             goto err1;
0861     }
0862
0863     err = -ENOMEM;
0864     app = kzalloc(sizeof(*app), GFP_KERNEL);
0865     if (!app)
0866         goto err2;
0867
0868     err = dev_mc_add(dev, appl->group_address);
0869     if (err < 0)
0870         goto err3;
0871
0872     app->dev = dev;
0873     app->app = appl;
0874     app->mad = RB_ROOT;
0875     spin_lock_init(&app->lock);
0876     skb_queue_head_init(&app->queue);
0877     rcu_assign_pointer(dev->mrp_port->applicants[appl->type], app);
0878     timer_setup(&app->join_timer, mrp_join_timer, 0);
0879     mrp_join_timer_arm(app);
0880     timer_setup(&app->periodic_timer, mrp_periodic_timer, 0);
0881     mrp_periodic_timer_arm(app);
0882     return 0;
0883
0884 err3:
0885     kfree(app);
0886 err2:
0887     mrp_release_port(dev);
0888 err1:
0889     return err;
0890 }
0891 EXPORT_SYMBOL_GPL(mrp_init_applicant);
0892
0893 void mrp_uninit_applicant(struct net_device *dev, struct mrp_application *appl)
0894 {
0895     struct mrp_port *port = rtnl_dereference(dev->mrp_port);
0896     struct mrp_applicant *app = rtnl_dereference(
0897         port->applicants[appl->type]);
0898
0899     ASSERT_RTNL();
0900
0901     RCU_INIT_POINTER(port->applicants[appl->type], NULL);
0902
0903     /* Delete timer and generate a final TX event to flush out
0904      * all pending messages before the applicant is gone.
0905      */
0906     del_timer_sync(&app->join_timer);
0907     del_timer_sync(&app->periodic_timer);
0908
0909     spin_lock_bh(&app->lock);
0910     mrp_mad_event(app, MRP_EVENT_TX);
0911     mrp_attr_destroy_all(app);
0912     mrp_pdu_queue(app);
0913     spin_unlock_bh(&app->lock);
0914
0915     mrp_queue_xmit(app);
0916
0917     dev_mc_del(dev, appl->group_address);
0918     kfree_rcu(app, rcu);
0919     mrp_release_port(dev);
0920 }
0921 EXPORT_SYMBOL_GPL(mrp_uninit_applicant);
0922
0923 int mrp_register_application(struct mrp_application *appl)
0924 {
0925     appl->pkttype.func = mrp_rcv;
0926     dev_add_pack(&appl->pkttype);
0927     return 0;
0928 }
0929 EXPORT_SYMBOL_GPL(mrp_register_application);
0930
0931 void mrp_unregister_application(struct mrp_application *appl)
0932 {
0933     dev_remove_pack(&appl->pkttype);
0934 }
0935 EXPORT_SYMBOL_GPL(mrp_unregister_application);