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);