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