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