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