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