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