Return to packets.c CVS log | Up to [ELWIX - Embedded LightWeight unIX -] / embedaddon / bird / proto / babel |
1.1 ! misho 1: /* ! 2: * BIRD -- The Babel protocol ! 3: * ! 4: * Copyright (c) 2015--2016 Toke Hoiland-Jorgensen ! 5: * ! 6: * Can be freely distributed and used under the terms of the GNU GPL. ! 7: * ! 8: * This file contains the packet and TLV handling code for the protocol. ! 9: */ ! 10: ! 11: #include "babel.h" ! 12: ! 13: ! 14: struct babel_pkt_header { ! 15: u8 magic; ! 16: u8 version; ! 17: u16 length; ! 18: } PACKED; ! 19: ! 20: struct babel_tlv { ! 21: u8 type; ! 22: u8 length; ! 23: u8 value[0]; ! 24: } PACKED; ! 25: ! 26: struct babel_tlv_ack_req { ! 27: u8 type; ! 28: u8 length; ! 29: u16 reserved; ! 30: u16 nonce; ! 31: u16 interval; ! 32: } PACKED; ! 33: ! 34: struct babel_tlv_ack { ! 35: u8 type; ! 36: u8 length; ! 37: u16 nonce; ! 38: } PACKED; ! 39: ! 40: struct babel_tlv_hello { ! 41: u8 type; ! 42: u8 length; ! 43: u16 reserved; ! 44: u16 seqno; ! 45: u16 interval; ! 46: } PACKED; ! 47: ! 48: struct babel_tlv_ihu { ! 49: u8 type; ! 50: u8 length; ! 51: u8 ae; ! 52: u8 reserved; ! 53: u16 rxcost; ! 54: u16 interval; ! 55: u8 addr[0]; ! 56: } PACKED; ! 57: ! 58: struct babel_tlv_router_id { ! 59: u8 type; ! 60: u8 length; ! 61: u16 reserved; ! 62: u64 router_id; ! 63: } PACKED; ! 64: ! 65: struct babel_tlv_next_hop { ! 66: u8 type; ! 67: u8 length; ! 68: u8 ae; ! 69: u8 reserved; ! 70: u8 addr[0]; ! 71: } PACKED; ! 72: ! 73: struct babel_tlv_update { ! 74: u8 type; ! 75: u8 length; ! 76: u8 ae; ! 77: u8 flags; ! 78: u8 plen; ! 79: u8 omitted; ! 80: u16 interval; ! 81: u16 seqno; ! 82: u16 metric; ! 83: u8 addr[0]; ! 84: } PACKED; ! 85: ! 86: struct babel_tlv_route_request { ! 87: u8 type; ! 88: u8 length; ! 89: u8 ae; ! 90: u8 plen; ! 91: u8 addr[0]; ! 92: } PACKED; ! 93: ! 94: struct babel_tlv_seqno_request { ! 95: u8 type; ! 96: u8 length; ! 97: u8 ae; ! 98: u8 plen; ! 99: u16 seqno; ! 100: u8 hop_count; ! 101: u8 reserved; ! 102: u64 router_id; ! 103: u8 addr[0]; ! 104: } PACKED; ! 105: ! 106: ! 107: #define BABEL_FLAG_DEF_PREFIX 0x80 ! 108: #define BABEL_FLAG_ROUTER_ID 0x40 ! 109: ! 110: ! 111: struct babel_parse_state { ! 112: struct babel_proto *proto; ! 113: struct babel_iface *ifa; ! 114: ip_addr saddr; ! 115: ip_addr next_hop; ! 116: u64 router_id; /* Router ID used in subsequent updates */ ! 117: u8 def_ip6_prefix[16]; /* Implicit IPv6 prefix in network order */ ! 118: u8 def_ip4_prefix[4]; /* Implicit IPv4 prefix in network order */ ! 119: u8 router_id_seen; /* router_id field is valid */ ! 120: u8 def_ip6_prefix_seen; /* def_ip6_prefix is valid */ ! 121: u8 def_ip4_prefix_seen; /* def_ip4_prefix is valid */ ! 122: }; ! 123: ! 124: enum parse_result { ! 125: PARSE_SUCCESS, ! 126: PARSE_ERROR, ! 127: PARSE_IGNORE, ! 128: }; ! 129: ! 130: struct babel_write_state { ! 131: u64 router_id; ! 132: u8 router_id_seen; ! 133: // ip_addr next_hop; ! 134: }; ! 135: ! 136: ! 137: #define DROP(DSC,VAL) do { err_dsc = DSC; err_val = VAL; goto drop; } while(0) ! 138: #define DROP1(DSC) do { err_dsc = DSC; goto drop; } while(0) ! 139: #define LOG_PKT(msg, args...) \ ! 140: log_rl(&p->log_pkt_tbf, L_REMOTE "%s: " msg, p->p.name, args) ! 141: ! 142: #define FIRST_TLV(p) ((struct babel_tlv *) (((struct babel_pkt_header *) p) + 1)) ! 143: #define NEXT_TLV(t) ((struct babel_tlv *) (((byte *) t) + TLV_LENGTH(t))) ! 144: #define TLV_LENGTH(t) (t->type == BABEL_TLV_PAD1 ? 1 : t->length + sizeof(struct babel_tlv)) ! 145: #define TLV_OPT_LENGTH(t) (t->length + sizeof(struct babel_tlv) - sizeof(*t)) ! 146: #define TLV_HDR(tlv,t,l) ({ tlv->type = t; tlv->length = l - sizeof(struct babel_tlv); }) ! 147: #define TLV_HDR0(tlv,t) TLV_HDR(tlv, t, tlv_data[t].min_length) ! 148: ! 149: #define BYTES(n) ((((uint) n) + 7) / 8) ! 150: ! 151: static inline u16 ! 152: get_time16(const void *p) ! 153: { ! 154: u16 v = get_u16(p) / BABEL_TIME_UNITS; ! 155: return MAX(1, v); ! 156: } ! 157: ! 158: static inline void ! 159: put_time16(void *p, u16 v) ! 160: { ! 161: put_u16(p, v * BABEL_TIME_UNITS); ! 162: } ! 163: ! 164: static inline ip6_addr ! 165: get_ip6_px(const void *p, uint plen) ! 166: { ! 167: ip6_addr addr = IPA_NONE; ! 168: memcpy(&addr, p, BYTES(plen)); ! 169: return ip6_ntoh(addr); ! 170: } ! 171: ! 172: static inline void ! 173: put_ip6_px(void *p, ip6_addr addr, uint plen) ! 174: { ! 175: addr = ip6_hton(addr); ! 176: memcpy(p, &addr, BYTES(plen)); ! 177: } ! 178: ! 179: static inline ip6_addr ! 180: get_ip6_ll(const void *p) ! 181: { ! 182: return ip6_build(0xfe800000, 0, get_u32(p+0), get_u32(p+4)); ! 183: } ! 184: ! 185: static inline void ! 186: put_ip6_ll(void *p, ip6_addr addr) ! 187: { ! 188: put_u32(p+0, _I2(addr)); ! 189: put_u32(p+4, _I3(addr)); ! 190: } ! 191: ! 192: ! 193: /* ! 194: * TLV read/write functions ! 195: */ ! 196: ! 197: static int babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); ! 198: static int babel_read_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); ! 199: static int babel_read_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); ! 200: static int babel_read_router_id(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); ! 201: static int babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); ! 202: static int babel_read_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); ! 203: static int babel_read_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); ! 204: static int babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); ! 205: ! 206: static uint babel_write_ack(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len); ! 207: static uint babel_write_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len); ! 208: static uint babel_write_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len); ! 209: static uint babel_write_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len); ! 210: static uint babel_write_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len); ! 211: static uint babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len); ! 212: ! 213: struct babel_tlv_data { ! 214: u8 min_length; ! 215: int (*read_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_parse_state *state); ! 216: uint (*write_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_write_state *state, uint max_len); ! 217: void (*handle_tlv)(union babel_msg *m, struct babel_iface *ifa); ! 218: }; ! 219: ! 220: static const struct babel_tlv_data tlv_data[BABEL_TLV_MAX] = { ! 221: [BABEL_TLV_ACK_REQ] = { ! 222: sizeof(struct babel_tlv_ack_req), ! 223: babel_read_ack_req, ! 224: NULL, ! 225: babel_handle_ack_req ! 226: }, ! 227: [BABEL_TLV_ACK] = { ! 228: sizeof(struct babel_tlv_ack), ! 229: NULL, ! 230: babel_write_ack, ! 231: NULL ! 232: }, ! 233: [BABEL_TLV_HELLO] = { ! 234: sizeof(struct babel_tlv_hello), ! 235: babel_read_hello, ! 236: babel_write_hello, ! 237: babel_handle_hello ! 238: }, ! 239: [BABEL_TLV_IHU] = { ! 240: sizeof(struct babel_tlv_ihu), ! 241: babel_read_ihu, ! 242: babel_write_ihu, ! 243: babel_handle_ihu ! 244: }, ! 245: [BABEL_TLV_ROUTER_ID] = { ! 246: sizeof(struct babel_tlv_router_id), ! 247: babel_read_router_id, ! 248: NULL, ! 249: NULL ! 250: }, ! 251: [BABEL_TLV_NEXT_HOP] = { ! 252: sizeof(struct babel_tlv_next_hop), ! 253: babel_read_next_hop, ! 254: NULL, ! 255: NULL ! 256: }, ! 257: [BABEL_TLV_UPDATE] = { ! 258: sizeof(struct babel_tlv_update), ! 259: babel_read_update, ! 260: babel_write_update, ! 261: babel_handle_update ! 262: }, ! 263: [BABEL_TLV_ROUTE_REQUEST] = { ! 264: sizeof(struct babel_tlv_route_request), ! 265: babel_read_route_request, ! 266: babel_write_route_request, ! 267: babel_handle_route_request ! 268: }, ! 269: [BABEL_TLV_SEQNO_REQUEST] = { ! 270: sizeof(struct babel_tlv_seqno_request), ! 271: babel_read_seqno_request, ! 272: babel_write_seqno_request, ! 273: babel_handle_seqno_request ! 274: }, ! 275: }; ! 276: ! 277: static int ! 278: babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *m, ! 279: struct babel_parse_state *state) ! 280: { ! 281: struct babel_tlv_ack_req *tlv = (void *) hdr; ! 282: struct babel_msg_ack_req *msg = &m->ack_req; ! 283: ! 284: msg->type = BABEL_TLV_ACK_REQ; ! 285: msg->nonce = get_u16(&tlv->nonce); ! 286: msg->interval = get_time16(&tlv->interval); ! 287: msg->sender = state->saddr; ! 288: ! 289: if (!msg->interval) ! 290: return PARSE_ERROR; ! 291: ! 292: return PARSE_SUCCESS; ! 293: } ! 294: ! 295: static uint ! 296: babel_write_ack(struct babel_tlv *hdr, union babel_msg *m, ! 297: struct babel_write_state *state UNUSED, uint max_len UNUSED) ! 298: { ! 299: struct babel_tlv_ack *tlv = (void *) hdr; ! 300: struct babel_msg_ack *msg = &m->ack; ! 301: ! 302: TLV_HDR0(tlv, BABEL_TLV_ACK); ! 303: put_u16(&tlv->nonce, msg->nonce); ! 304: ! 305: return sizeof(struct babel_tlv_ack); ! 306: } ! 307: ! 308: static int ! 309: babel_read_hello(struct babel_tlv *hdr, union babel_msg *m, ! 310: struct babel_parse_state *state) ! 311: { ! 312: struct babel_tlv_hello *tlv = (void *) hdr; ! 313: struct babel_msg_hello *msg = &m->hello; ! 314: ! 315: msg->type = BABEL_TLV_HELLO; ! 316: msg->seqno = get_u16(&tlv->seqno); ! 317: msg->interval = get_time16(&tlv->interval); ! 318: msg->sender = state->saddr; ! 319: ! 320: return PARSE_SUCCESS; ! 321: } ! 322: ! 323: static uint ! 324: babel_write_hello(struct babel_tlv *hdr, union babel_msg *m, ! 325: struct babel_write_state *state UNUSED, uint max_len UNUSED) ! 326: { ! 327: struct babel_tlv_hello *tlv = (void *) hdr; ! 328: struct babel_msg_hello *msg = &m->hello; ! 329: ! 330: TLV_HDR0(tlv, BABEL_TLV_HELLO); ! 331: put_u16(&tlv->seqno, msg->seqno); ! 332: put_time16(&tlv->interval, msg->interval); ! 333: ! 334: return sizeof(struct babel_tlv_hello); ! 335: } ! 336: ! 337: static int ! 338: babel_read_ihu(struct babel_tlv *hdr, union babel_msg *m, ! 339: struct babel_parse_state *state) ! 340: { ! 341: struct babel_tlv_ihu *tlv = (void *) hdr; ! 342: struct babel_msg_ihu *msg = &m->ihu; ! 343: ! 344: msg->type = BABEL_TLV_IHU; ! 345: msg->ae = tlv->ae; ! 346: msg->rxcost = get_u16(&tlv->rxcost); ! 347: msg->interval = get_time16(&tlv->interval); ! 348: msg->addr = IPA_NONE; ! 349: msg->sender = state->saddr; ! 350: ! 351: if (msg->ae >= BABEL_AE_MAX) ! 352: return PARSE_IGNORE; ! 353: ! 354: // We handle link-local IPs. In every other case, the addr field will be 0 but ! 355: // validation will succeed. The handler takes care of these cases. ! 356: if (msg->ae == BABEL_AE_IP6_LL) ! 357: { ! 358: if (TLV_OPT_LENGTH(tlv) < 8) ! 359: return PARSE_ERROR; ! 360: ! 361: msg->addr = ipa_from_ip6(get_ip6_ll(&tlv->addr)); ! 362: } ! 363: ! 364: return PARSE_SUCCESS; ! 365: } ! 366: ! 367: static uint ! 368: babel_write_ihu(struct babel_tlv *hdr, union babel_msg *m, ! 369: struct babel_write_state *state UNUSED, uint max_len) ! 370: { ! 371: struct babel_tlv_ihu *tlv = (void *) hdr; ! 372: struct babel_msg_ihu *msg = &m->ihu; ! 373: ! 374: if (ipa_is_link_local(msg->addr) && max_len < sizeof(struct babel_tlv_ihu) + 8) ! 375: return 0; ! 376: ! 377: TLV_HDR0(tlv, BABEL_TLV_IHU); ! 378: put_u16(&tlv->rxcost, msg->rxcost); ! 379: put_time16(&tlv->interval, msg->interval); ! 380: ! 381: if (!ipa_is_link_local(msg->addr)) ! 382: { ! 383: tlv->ae = BABEL_AE_WILDCARD; ! 384: return sizeof(struct babel_tlv_ihu); ! 385: } ! 386: put_ip6_ll(&tlv->addr, msg->addr); ! 387: tlv->ae = BABEL_AE_IP6_LL; ! 388: hdr->length += 8; ! 389: return sizeof(struct babel_tlv_ihu) + 8; ! 390: } ! 391: ! 392: static int ! 393: babel_read_router_id(struct babel_tlv *hdr, union babel_msg *m UNUSED, ! 394: struct babel_parse_state *state) ! 395: { ! 396: struct babel_tlv_router_id *tlv = (void *) hdr; ! 397: ! 398: state->router_id = get_u64(&tlv->router_id); ! 399: state->router_id_seen = 1; ! 400: ! 401: return PARSE_IGNORE; ! 402: } ! 403: ! 404: /* This is called directly from babel_write_update() */ ! 405: static uint ! 406: babel_write_router_id(struct babel_tlv *hdr, u64 router_id, ! 407: struct babel_write_state *state, uint max_len UNUSED) ! 408: { ! 409: struct babel_tlv_router_id *tlv = (void *) hdr; ! 410: ! 411: /* We still assume that first min_length bytes are available and zeroed */ ! 412: ! 413: TLV_HDR0(tlv, BABEL_TLV_ROUTER_ID); ! 414: put_u64(&tlv->router_id, router_id); ! 415: ! 416: state->router_id = router_id; ! 417: state->router_id_seen = 1; ! 418: ! 419: return sizeof(struct babel_tlv_router_id); ! 420: } ! 421: ! 422: static int ! 423: babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *m UNUSED, ! 424: struct babel_parse_state *state) ! 425: { ! 426: struct babel_tlv_next_hop *tlv = (void *) hdr; ! 427: ! 428: switch (tlv->ae) ! 429: { ! 430: case BABEL_AE_WILDCARD: ! 431: return PARSE_ERROR; ! 432: ! 433: case BABEL_AE_IP4: ! 434: /* TODO */ ! 435: return PARSE_IGNORE; ! 436: ! 437: case BABEL_AE_IP6: ! 438: if (TLV_OPT_LENGTH(tlv) < sizeof(ip6_addr)) ! 439: return PARSE_ERROR; ! 440: ! 441: state->next_hop = ipa_from_ip6(get_ip6(&tlv->addr)); ! 442: return PARSE_IGNORE; ! 443: ! 444: case BABEL_AE_IP6_LL: ! 445: if (TLV_OPT_LENGTH(tlv) < 8) ! 446: return PARSE_ERROR; ! 447: ! 448: state->next_hop = ipa_from_ip6(get_ip6_ll(&tlv->addr)); ! 449: return PARSE_IGNORE; ! 450: ! 451: default: ! 452: return PARSE_IGNORE; ! 453: } ! 454: ! 455: return PARSE_IGNORE; ! 456: } ! 457: ! 458: static int ! 459: babel_read_update(struct babel_tlv *hdr, union babel_msg *m, ! 460: struct babel_parse_state *state) ! 461: { ! 462: struct babel_tlv_update *tlv = (void *) hdr; ! 463: struct babel_msg_update *msg = &m->update; ! 464: ! 465: msg->type = BABEL_TLV_UPDATE; ! 466: msg->interval = get_time16(&tlv->interval); ! 467: msg->seqno = get_u16(&tlv->seqno); ! 468: msg->metric = get_u16(&tlv->metric); ! 469: ! 470: /* Length of received prefix data without omitted part */ ! 471: int len = BYTES(tlv->plen) - (int) tlv->omitted; ! 472: u8 buf[16] = {}; ! 473: ! 474: if ((len < 0) || ((uint) len > TLV_OPT_LENGTH(tlv))) ! 475: return PARSE_ERROR; ! 476: ! 477: switch (tlv->ae) ! 478: { ! 479: case BABEL_AE_WILDCARD: ! 480: if (tlv->plen > 0) ! 481: return PARSE_ERROR; ! 482: ! 483: msg->wildcard = 1; ! 484: break; ! 485: ! 486: case BABEL_AE_IP4: ! 487: /* TODO */ ! 488: return PARSE_IGNORE; ! 489: ! 490: case BABEL_AE_IP6: ! 491: if (tlv->plen > MAX_PREFIX_LENGTH) ! 492: return PARSE_ERROR; ! 493: ! 494: /* Cannot omit data if there is no saved prefix */ ! 495: if (tlv->omitted && !state->def_ip6_prefix_seen) ! 496: return PARSE_ERROR; ! 497: ! 498: /* Merge saved prefix and received prefix parts */ ! 499: memcpy(buf, state->def_ip6_prefix, tlv->omitted); ! 500: memcpy(buf + tlv->omitted, tlv->addr, len); ! 501: ! 502: msg->plen = tlv->plen; ! 503: msg->prefix = ipa_from_ip6(get_ip6(buf)); ! 504: ! 505: if (tlv->flags & BABEL_FLAG_DEF_PREFIX) ! 506: { ! 507: put_ip6(state->def_ip6_prefix, msg->prefix); ! 508: state->def_ip6_prefix_seen = 1; ! 509: } ! 510: ! 511: if (tlv->flags & BABEL_FLAG_ROUTER_ID) ! 512: { ! 513: state->router_id = ((u64) _I2(msg->prefix)) << 32 | _I3(msg->prefix); ! 514: state->router_id_seen = 1; ! 515: } ! 516: break; ! 517: ! 518: case BABEL_AE_IP6_LL: ! 519: /* ??? */ ! 520: return PARSE_IGNORE; ! 521: ! 522: default: ! 523: return PARSE_IGNORE; ! 524: } ! 525: ! 526: /* Update must have Router ID, unless it is retraction */ ! 527: if (!state->router_id_seen && (msg->metric != BABEL_INFINITY)) ! 528: { ! 529: DBG("Babel: No router ID seen before update\n"); ! 530: return PARSE_ERROR; ! 531: } ! 532: ! 533: msg->router_id = state->router_id; ! 534: msg->next_hop = state->next_hop; ! 535: msg->sender = state->saddr; ! 536: ! 537: return PARSE_SUCCESS; ! 538: } ! 539: ! 540: static uint ! 541: babel_write_update(struct babel_tlv *hdr, union babel_msg *m, ! 542: struct babel_write_state *state, uint max_len) ! 543: { ! 544: struct babel_tlv_update *tlv = (void *) hdr; ! 545: struct babel_msg_update *msg = &m->update; ! 546: uint len0 = 0; ! 547: ! 548: /* ! 549: * When needed, we write Router-ID TLV before Update TLV and return size of ! 550: * both of them. There is enough space for the Router-ID TLV, because ! 551: * sizeof(struct babel_tlv_router_id) == sizeof(struct babel_tlv_update). ! 552: * ! 553: * Router ID is not used for retractions, so do not us it in such case. ! 554: */ ! 555: if ((msg->metric < BABEL_INFINITY) && ! 556: (!state->router_id_seen || (msg->router_id != state->router_id))) ! 557: { ! 558: len0 = babel_write_router_id(hdr, msg->router_id, state, max_len); ! 559: tlv = (struct babel_tlv_update *) NEXT_TLV(tlv); ! 560: } ! 561: ! 562: uint len = sizeof(struct babel_tlv_update) + BYTES(msg->plen); ! 563: ! 564: if (len0 + len > max_len) ! 565: return 0; ! 566: ! 567: memset(tlv, 0, sizeof(struct babel_tlv_update)); ! 568: TLV_HDR(tlv, BABEL_TLV_UPDATE, len); ! 569: ! 570: if (msg->wildcard) ! 571: { ! 572: tlv->ae = BABEL_AE_WILDCARD; ! 573: tlv->plen = 0; ! 574: } ! 575: else ! 576: { ! 577: tlv->ae = BABEL_AE_IP6; ! 578: tlv->plen = msg->plen; ! 579: put_ip6_px(tlv->addr, msg->prefix, msg->plen); ! 580: } ! 581: ! 582: put_time16(&tlv->interval, msg->interval); ! 583: put_u16(&tlv->seqno, msg->seqno); ! 584: put_u16(&tlv->metric, msg->metric); ! 585: ! 586: return len0 + len; ! 587: } ! 588: ! 589: static int ! 590: babel_read_route_request(struct babel_tlv *hdr, union babel_msg *m, ! 591: struct babel_parse_state *state UNUSED) ! 592: { ! 593: struct babel_tlv_route_request *tlv = (void *) hdr; ! 594: struct babel_msg_route_request *msg = &m->route_request; ! 595: ! 596: msg->type = BABEL_TLV_ROUTE_REQUEST; ! 597: ! 598: switch (tlv->ae) ! 599: { ! 600: case BABEL_AE_WILDCARD: ! 601: /* Wildcard requests must have plen 0 */ ! 602: if (tlv->plen > 0) ! 603: return PARSE_ERROR; ! 604: ! 605: msg->full = 1; ! 606: return PARSE_SUCCESS; ! 607: ! 608: case BABEL_AE_IP4: ! 609: /* TODO */ ! 610: return PARSE_IGNORE; ! 611: ! 612: case BABEL_AE_IP6: ! 613: if (tlv->plen > MAX_PREFIX_LENGTH) ! 614: return PARSE_ERROR; ! 615: ! 616: if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen)) ! 617: return PARSE_ERROR; ! 618: ! 619: msg->plen = tlv->plen; ! 620: msg->prefix = get_ip6_px(tlv->addr, tlv->plen); ! 621: return PARSE_SUCCESS; ! 622: ! 623: case BABEL_AE_IP6_LL: ! 624: return PARSE_ERROR; ! 625: ! 626: default: ! 627: return PARSE_IGNORE; ! 628: } ! 629: ! 630: return PARSE_IGNORE; ! 631: } ! 632: ! 633: static uint ! 634: babel_write_route_request(struct babel_tlv *hdr, union babel_msg *m, ! 635: struct babel_write_state *state UNUSED, uint max_len) ! 636: { ! 637: struct babel_tlv_route_request *tlv = (void *) hdr; ! 638: struct babel_msg_route_request *msg = &m->route_request; ! 639: ! 640: uint len = sizeof(struct babel_tlv_route_request) + BYTES(msg->plen); ! 641: ! 642: if (len > max_len) ! 643: return 0; ! 644: ! 645: TLV_HDR(tlv, BABEL_TLV_ROUTE_REQUEST, len); ! 646: ! 647: if (msg->full) ! 648: { ! 649: tlv->ae = BABEL_AE_WILDCARD; ! 650: tlv->plen = 0; ! 651: } ! 652: else ! 653: { ! 654: tlv->ae = BABEL_AE_IP6; ! 655: tlv->plen = msg->plen; ! 656: put_ip6_px(tlv->addr, msg->prefix, msg->plen); ! 657: } ! 658: ! 659: return len; ! 660: } ! 661: ! 662: static int ! 663: babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *m, ! 664: struct babel_parse_state *state) ! 665: { ! 666: struct babel_tlv_seqno_request *tlv = (void *) hdr; ! 667: struct babel_msg_seqno_request *msg = &m->seqno_request; ! 668: ! 669: msg->type = BABEL_TLV_SEQNO_REQUEST; ! 670: msg->seqno = get_u16(&tlv->seqno); ! 671: msg->hop_count = tlv->hop_count; ! 672: msg->router_id = get_u64(&tlv->router_id); ! 673: msg->sender = state->saddr; ! 674: ! 675: if (tlv->hop_count == 0) ! 676: return PARSE_ERROR; ! 677: ! 678: switch (tlv->ae) ! 679: { ! 680: case BABEL_AE_WILDCARD: ! 681: return PARSE_ERROR; ! 682: ! 683: case BABEL_AE_IP4: ! 684: /* TODO */ ! 685: return PARSE_IGNORE; ! 686: ! 687: case BABEL_AE_IP6: ! 688: if (tlv->plen > MAX_PREFIX_LENGTH) ! 689: return PARSE_ERROR; ! 690: ! 691: if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen)) ! 692: return PARSE_ERROR; ! 693: ! 694: msg->plen = tlv->plen; ! 695: msg->prefix = get_ip6_px(tlv->addr, tlv->plen); ! 696: return PARSE_SUCCESS; ! 697: ! 698: case BABEL_AE_IP6_LL: ! 699: return PARSE_ERROR; ! 700: ! 701: default: ! 702: return PARSE_IGNORE; ! 703: } ! 704: ! 705: return PARSE_IGNORE; ! 706: } ! 707: ! 708: static uint ! 709: babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *m, ! 710: struct babel_write_state *state UNUSED, uint max_len) ! 711: { ! 712: struct babel_tlv_seqno_request *tlv = (void *) hdr; ! 713: struct babel_msg_seqno_request *msg = &m->seqno_request; ! 714: ! 715: uint len = sizeof(struct babel_tlv_seqno_request) + BYTES(msg->plen); ! 716: ! 717: if (len > max_len) ! 718: return 0; ! 719: ! 720: TLV_HDR(tlv, BABEL_TLV_SEQNO_REQUEST, len); ! 721: tlv->ae = BABEL_AE_IP6; ! 722: tlv->plen = msg->plen; ! 723: put_u16(&tlv->seqno, msg->seqno); ! 724: tlv->hop_count = msg->hop_count; ! 725: put_u64(&tlv->router_id, msg->router_id); ! 726: put_ip6_px(tlv->addr, msg->prefix, msg->plen); ! 727: ! 728: return len; ! 729: } ! 730: ! 731: static inline int ! 732: babel_read_tlv(struct babel_tlv *hdr, ! 733: union babel_msg *msg, ! 734: struct babel_parse_state *state) ! 735: { ! 736: if ((hdr->type <= BABEL_TLV_PADN) || ! 737: (hdr->type >= BABEL_TLV_MAX) || ! 738: !tlv_data[hdr->type].read_tlv) ! 739: return PARSE_IGNORE; ! 740: ! 741: if (TLV_LENGTH(hdr) < tlv_data[hdr->type].min_length) ! 742: return PARSE_ERROR; ! 743: ! 744: memset(msg, 0, sizeof(*msg)); ! 745: return tlv_data[hdr->type].read_tlv(hdr, msg, state); ! 746: } ! 747: ! 748: static uint ! 749: babel_write_tlv(struct babel_tlv *hdr, ! 750: union babel_msg *msg, ! 751: struct babel_write_state *state, ! 752: uint max_len) ! 753: { ! 754: if ((msg->type <= BABEL_TLV_PADN) || ! 755: (msg->type >= BABEL_TLV_MAX) || ! 756: !tlv_data[msg->type].write_tlv) ! 757: return 0; ! 758: ! 759: if (tlv_data[msg->type].min_length > max_len) ! 760: return 0; ! 761: ! 762: memset(hdr, 0, tlv_data[msg->type].min_length); ! 763: return tlv_data[msg->type].write_tlv(hdr, msg, state, max_len); ! 764: } ! 765: ! 766: ! 767: /* ! 768: * Packet RX/TX functions ! 769: */ ! 770: ! 771: static int ! 772: babel_send_to(struct babel_iface *ifa, ip_addr dest) ! 773: { ! 774: sock *sk = ifa->sk; ! 775: struct babel_pkt_header *hdr = (void *) sk->tbuf; ! 776: int len = get_u16(&hdr->length) + sizeof(struct babel_pkt_header); ! 777: ! 778: DBG("Babel: Sending %d bytes to %I\n", len, dest); ! 779: return sk_send_to(sk, len, dest, 0); ! 780: } ! 781: ! 782: /** ! 783: * babel_write_queue - Write a TLV queue to a transmission buffer ! 784: * @ifa: Interface holding the transmission buffer ! 785: * @queue: TLV queue to write (containing internal-format TLVs) ! 786: * ! 787: * This function writes a packet to the interface transmission buffer with as ! 788: * many TLVs from the &queue as will fit in the buffer. It returns the number of ! 789: * bytes written (NOT counting the packet header). The function is called by ! 790: * babel_send_queue() and babel_send_unicast() to construct packets for ! 791: * transmission, and uses per-TLV helper functions to convert the ! 792: * internal-format TLVs to their wire representations. ! 793: * ! 794: * The TLVs in the queue are freed after they are written to the buffer. ! 795: */ ! 796: static uint ! 797: babel_write_queue(struct babel_iface *ifa, list *queue) ! 798: { ! 799: struct babel_proto *p = ifa->proto; ! 800: struct babel_write_state state = {}; ! 801: ! 802: if (EMPTY_LIST(*queue)) ! 803: return 0; ! 804: ! 805: byte *pos = ifa->sk->tbuf; ! 806: byte *end = pos + ifa->tx_length; ! 807: ! 808: struct babel_pkt_header *pkt = (void *) pos; ! 809: pkt->magic = BABEL_MAGIC; ! 810: pkt->version = BABEL_VERSION; ! 811: pkt->length = 0; ! 812: pos += sizeof(struct babel_pkt_header); ! 813: ! 814: struct babel_msg_node *msg; ! 815: WALK_LIST_FIRST(msg, *queue) ! 816: { ! 817: if (pos >= end) ! 818: break; ! 819: ! 820: int len = babel_write_tlv((struct babel_tlv *) pos, &msg->msg, &state, end - pos); ! 821: ! 822: if (!len) ! 823: break; ! 824: ! 825: pos += len; ! 826: rem_node(NODE msg); ! 827: sl_free(p->msg_slab, msg); ! 828: } ! 829: ! 830: uint plen = pos - (byte *) pkt; ! 831: put_u16(&pkt->length, plen - sizeof(struct babel_pkt_header)); ! 832: ! 833: return plen; ! 834: } ! 835: ! 836: void ! 837: babel_send_queue(void *arg) ! 838: { ! 839: struct babel_iface *ifa = arg; ! 840: while ((babel_write_queue(ifa, &ifa->msg_queue) > 0) && ! 841: (babel_send_to(ifa, IP6_BABEL_ROUTERS) > 0)); ! 842: } ! 843: ! 844: static inline void ! 845: babel_kick_queue(struct babel_iface *ifa) ! 846: { ! 847: /* ! 848: * Only schedule send event if there is not already data in the socket buffer. ! 849: * Otherwise we may overwrite the data already in the buffer. ! 850: */ ! 851: ! 852: if ((ifa->sk->tpos == ifa->sk->tbuf) && !ev_active(ifa->send_event)) ! 853: ev_schedule(ifa->send_event); ! 854: } ! 855: ! 856: /** ! 857: * babel_send_unicast - send a single TLV via unicast to a destination ! 858: * @msg: TLV to send ! 859: * @ifa: Interface to send via ! 860: * @dest: Destination of the TLV ! 861: * ! 862: * This function is used to send a single TLV via unicast to a designated ! 863: * receiver. This is used for replying to certain incoming requests, and for ! 864: * sending unicast requests to refresh routes before they expire. ! 865: */ ! 866: void ! 867: babel_send_unicast(union babel_msg *msg, struct babel_iface *ifa, ip_addr dest) ! 868: { ! 869: struct babel_proto *p = ifa->proto; ! 870: struct babel_msg_node *msgn = sl_alloc(p->msg_slab); ! 871: list queue; ! 872: ! 873: msgn->msg = *msg; ! 874: init_list(&queue); ! 875: add_tail(&queue, NODE msgn); ! 876: babel_write_queue(ifa, &queue); ! 877: babel_send_to(ifa, dest); ! 878: ! 879: /* We could overwrite waiting packet here, we may have to kick TX queue */ ! 880: if (!EMPTY_LIST(ifa->msg_queue)) ! 881: babel_kick_queue(ifa); ! 882: } ! 883: ! 884: /** ! 885: * babel_enqueue - enqueue a TLV for transmission on an interface ! 886: * @msg: TLV to enqueue (in internal TLV format) ! 887: * @ifa: Interface to enqueue to ! 888: * ! 889: * This function is called to enqueue a TLV for subsequent transmission on an ! 890: * interface. The transmission event is triggered whenever a TLV is enqueued; ! 891: * this ensures that TLVs will be transmitted in a timely manner, but that TLVs ! 892: * which are enqueued in rapid succession can be transmitted together in one ! 893: * packet. ! 894: */ ! 895: void ! 896: babel_enqueue(union babel_msg *msg, struct babel_iface *ifa) ! 897: { ! 898: struct babel_proto *p = ifa->proto; ! 899: struct babel_msg_node *msgn = sl_alloc(p->msg_slab); ! 900: msgn->msg = *msg; ! 901: add_tail(&ifa->msg_queue, NODE msgn); ! 902: babel_kick_queue(ifa); ! 903: } ! 904: ! 905: /** ! 906: * babel_process_packet - process incoming data packet ! 907: * @pkt: Pointer to the packet data ! 908: * @len: Length of received packet ! 909: * @saddr: Address of packet sender ! 910: * @ifa: Interface packet was received on. ! 911: * ! 912: * This function is the main processing hook of incoming Babel packets. It ! 913: * checks that the packet header is well-formed, then processes the TLVs ! 914: * contained in the packet. This is done in two passes: First all TLVs are ! 915: * parsed into the internal TLV format. If a TLV parser fails, processing of the ! 916: * rest of the packet is aborted. ! 917: * ! 918: * After the parsing step, the TLV handlers are called for each parsed TLV in ! 919: * order. ! 920: */ ! 921: static void ! 922: babel_process_packet(struct babel_pkt_header *pkt, int len, ! 923: ip_addr saddr, struct babel_iface *ifa) ! 924: { ! 925: struct babel_proto *p = ifa->proto; ! 926: struct babel_tlv *tlv; ! 927: struct babel_msg_node *msg; ! 928: list msgs; ! 929: int res; ! 930: ! 931: int plen = sizeof(struct babel_pkt_header) + get_u16(&pkt->length); ! 932: byte *pos; ! 933: byte *end = (byte *)pkt + plen; ! 934: ! 935: struct babel_parse_state state = { ! 936: .proto = p, ! 937: .ifa = ifa, ! 938: .saddr = saddr, ! 939: .next_hop = saddr, ! 940: }; ! 941: ! 942: if ((pkt->magic != BABEL_MAGIC) || (pkt->version != BABEL_VERSION)) ! 943: { ! 944: TRACE(D_PACKETS, "Strange packet from %I via %s - magic %d version %d", ! 945: saddr, ifa->iface->name, pkt->magic, pkt->version); ! 946: return; ! 947: } ! 948: ! 949: if (plen > len) ! 950: { ! 951: LOG_PKT("Bad packet from %I via %s - %s (%u)", ! 952: saddr, ifa->iface->name, "length mismatch", plen); ! 953: return; ! 954: } ! 955: ! 956: TRACE(D_PACKETS, "Packet received from %I via %s", ! 957: saddr, ifa->iface->name); ! 958: ! 959: init_list(&msgs); ! 960: ! 961: /* First pass through the packet TLV by TLV, parsing each into internal data ! 962: structures. */ ! 963: for (tlv = FIRST_TLV(pkt); ! 964: (byte *)tlv < end; ! 965: tlv = NEXT_TLV(tlv)) ! 966: { ! 967: /* Ugly special case */ ! 968: if (tlv->type == BABEL_TLV_PAD1) ! 969: continue; ! 970: ! 971: /* The end of the common TLV header */ ! 972: pos = (byte *)tlv + sizeof(struct babel_tlv); ! 973: if ((pos > end) || (pos + tlv->length > end)) ! 974: { ! 975: LOG_PKT("Bad TLV from %I via %s type %d pos %d - framing error", ! 976: saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt); ! 977: break; ! 978: } ! 979: ! 980: msg = sl_alloc(p->msg_slab); ! 981: res = babel_read_tlv(tlv, &msg->msg, &state); ! 982: if (res == PARSE_SUCCESS) ! 983: { ! 984: add_tail(&msgs, NODE msg); ! 985: } ! 986: else if (res == PARSE_IGNORE) ! 987: { ! 988: DBG("Babel: Ignoring TLV of type %d\n", tlv->type); ! 989: sl_free(p->msg_slab, msg); ! 990: } ! 991: else /* PARSE_ERROR */ ! 992: { ! 993: LOG_PKT("Bad TLV from %I via %s type %d pos %d - parse error", ! 994: saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt); ! 995: sl_free(p->msg_slab, msg); ! 996: break; ! 997: } ! 998: } ! 999: ! 1000: /* Parsing done, handle all parsed TLVs */ ! 1001: WALK_LIST_FIRST(msg, msgs) ! 1002: { ! 1003: if (tlv_data[msg->msg.type].handle_tlv) ! 1004: tlv_data[msg->msg.type].handle_tlv(&msg->msg, ifa); ! 1005: rem_node(NODE msg); ! 1006: sl_free(p->msg_slab, msg); ! 1007: } ! 1008: } ! 1009: ! 1010: static void ! 1011: babel_err_hook(sock *sk, int err) ! 1012: { ! 1013: struct babel_iface *ifa = sk->data; ! 1014: struct babel_proto *p = ifa->proto; ! 1015: ! 1016: log(L_ERR "%s: Socket error on %s: %M", p->p.name, ifa->iface->name, err); ! 1017: /* FIXME: Drop queued TLVs here? */ ! 1018: } ! 1019: ! 1020: ! 1021: static void ! 1022: babel_tx_hook(sock *sk) ! 1023: { ! 1024: struct babel_iface *ifa = sk->data; ! 1025: ! 1026: DBG("Babel: TX hook called (iface %s, src %I, dst %I)\n", ! 1027: sk->iface->name, sk->saddr, sk->daddr); ! 1028: ! 1029: babel_send_queue(ifa); ! 1030: } ! 1031: ! 1032: ! 1033: static int ! 1034: babel_rx_hook(sock *sk, uint len) ! 1035: { ! 1036: struct babel_iface *ifa = sk->data; ! 1037: struct babel_proto *p = ifa->proto; ! 1038: const char *err_dsc = NULL; ! 1039: uint err_val = 0; ! 1040: ! 1041: if (sk->lifindex != ifa->iface->index) ! 1042: return 1; ! 1043: ! 1044: DBG("Babel: RX hook called (iface %s, src %I, dst %I)\n", ! 1045: sk->iface->name, sk->faddr, sk->laddr); ! 1046: ! 1047: /* Silently ignore my own packets */ ! 1048: if (ipa_equal(ifa->iface->addr->ip, sk->faddr)) ! 1049: return 1; ! 1050: ! 1051: if (!ipa_is_link_local(sk->faddr)) ! 1052: DROP1("wrong src address"); ! 1053: ! 1054: if (sk->fport != ifa->cf->port) ! 1055: DROP("wrong src port", sk->fport); ! 1056: ! 1057: if (len < sizeof(struct babel_pkt_header)) ! 1058: DROP("too short", len); ! 1059: ! 1060: if (sk->flags & SKF_TRUNCATED) ! 1061: DROP("truncated", len); ! 1062: ! 1063: babel_process_packet((struct babel_pkt_header *) sk->rbuf, len, sk->faddr, ifa); ! 1064: return 1; ! 1065: ! 1066: drop: ! 1067: LOG_PKT("Bad packet from %I via %s - %s (%u)", ! 1068: sk->faddr, sk->iface->name, err_dsc, err_val); ! 1069: return 1; ! 1070: } ! 1071: ! 1072: int ! 1073: babel_open_socket(struct babel_iface *ifa) ! 1074: { ! 1075: struct babel_proto *p = ifa->proto; ! 1076: ! 1077: sock *sk; ! 1078: sk = sk_new(ifa->pool); ! 1079: sk->type = SK_UDP; ! 1080: sk->sport = ifa->cf->port; ! 1081: sk->dport = ifa->cf->port; ! 1082: sk->iface = ifa->iface; ! 1083: ! 1084: sk->rx_hook = babel_rx_hook; ! 1085: sk->tx_hook = babel_tx_hook; ! 1086: sk->err_hook = babel_err_hook; ! 1087: sk->data = ifa; ! 1088: ! 1089: sk->tos = ifa->cf->tx_tos; ! 1090: sk->priority = ifa->cf->tx_priority; ! 1091: sk->ttl = 1; ! 1092: sk->flags = SKF_LADDR_RX; ! 1093: ! 1094: if (sk_open(sk) < 0) ! 1095: goto err; ! 1096: ! 1097: if (sk_setup_multicast(sk) < 0) ! 1098: goto err; ! 1099: ! 1100: if (sk_join_group(sk, IP6_BABEL_ROUTERS) < 0) ! 1101: goto err; ! 1102: ! 1103: ifa->sk = sk; ! 1104: return 1; ! 1105: ! 1106: err: ! 1107: sk_log_error(sk, p->p.name); ! 1108: rfree(sk); ! 1109: return 0; ! 1110: }