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