diff options
Diffstat (limited to 'proto/babel/packets.c')
| -rw-r--r-- | proto/babel/packets.c | 1093 |
1 files changed, 1093 insertions, 0 deletions
diff --git a/proto/babel/packets.c b/proto/babel/packets.c new file mode 100644 index 00000000..be47aa75 --- /dev/null +++ b/proto/babel/packets.c @@ -0,0 +1,1093 @@ +/* + * BIRD -- The Babel protocol + * + * Copyright (c) 2015--2016 Toke Hoiland-Jorgensen + * + * Can be freely distributed and used under the terms of the GNU GPL. + * + * This file contains the packet and TLV handling code for the protocol. + */ + +#include "babel.h" + + +struct babel_pkt_header { + u8 magic; + u8 version; + u16 length; +} PACKED; + +struct babel_tlv { + u8 type; + u8 length; + u8 value[0]; +} PACKED; + +struct babel_tlv_ack_req { + u8 type; + u8 length; + u16 reserved; + u16 nonce; + u16 interval; +} PACKED; + +struct babel_tlv_ack { + u8 type; + u8 length; + u16 nonce; +} PACKED; + +struct babel_tlv_hello { + u8 type; + u8 length; + u16 reserved; + u16 seqno; + u16 interval; +} PACKED; + +struct babel_tlv_ihu { + u8 type; + u8 length; + u8 ae; + u8 reserved; + u16 rxcost; + u16 interval; + u8 addr[0]; +} PACKED; + +struct babel_tlv_router_id { + u8 type; + u8 length; + u16 reserved; + u64 router_id; +} PACKED; + +struct babel_tlv_next_hop { + u8 type; + u8 length; + u8 ae; + u8 reserved; + u8 addr[0]; +} PACKED; + +struct babel_tlv_update { + u8 type; + u8 length; + u8 ae; + u8 flags; + u8 plen; + u8 omitted; + u16 interval; + u16 seqno; + u16 metric; + u8 addr[0]; +} PACKED; + +struct babel_tlv_route_request { + u8 type; + u8 length; + u8 ae; + u8 plen; + u8 addr[0]; +} PACKED; + +struct babel_tlv_seqno_request { + u8 type; + u8 length; + u8 ae; + u8 plen; + u16 seqno; + u8 hop_count; + u8 reserved; + u64 router_id; + u8 addr[0]; +} PACKED; + + +#define BABEL_FLAG_DEF_PREFIX 0x80 +#define BABEL_FLAG_ROUTER_ID 0x40 + + +struct babel_parse_state { + struct babel_proto *proto; + struct babel_iface *ifa; + ip_addr saddr; + ip_addr next_hop; + u64 router_id; /* Router ID used in subsequent updates */ + u8 def_ip6_prefix[16]; /* Implicit IPv6 prefix in network order */ + u8 def_ip4_prefix[4]; /* Implicit IPv4 prefix in network order */ + u8 router_id_seen; /* router_id field is valid */ + u8 def_ip6_prefix_seen; /* def_ip6_prefix is valid */ + u8 def_ip4_prefix_seen; /* def_ip4_prefix is valid */ +}; + +enum parse_result { + PARSE_SUCCESS, + PARSE_ERROR, + PARSE_IGNORE, +}; + +struct babel_write_state { + u64 router_id; + u8 router_id_seen; +// ip_addr next_hop; +}; + + +#define DROP(DSC,VAL) do { err_dsc = DSC; err_val = VAL; goto drop; } while(0) +#define DROP1(DSC) do { err_dsc = DSC; goto drop; } while(0) +#define LOG_PKT(msg, args...) \ + log_rl(&p->log_pkt_tbf, L_REMOTE "%s: " msg, p->p.name, args) + +#define FIRST_TLV(p) ((struct babel_tlv *) (((struct babel_pkt_header *) p) + 1)) +#define NEXT_TLV(t) ((struct babel_tlv *) (((byte *) t) + TLV_LENGTH(t))) +#define TLV_LENGTH(t) (t->type == BABEL_TLV_PAD1 ? 1 : t->length + sizeof(struct babel_tlv)) +#define TLV_OPT_LENGTH(t) (t->length + sizeof(struct babel_tlv) - sizeof(*t)) +#define TLV_HDR(tlv,t,l) ({ tlv->type = t; tlv->length = l - sizeof(struct babel_tlv); }) +#define TLV_HDR0(tlv,t) TLV_HDR(tlv, t, tlv_data[t].min_length) + + +static inline u16 +get_time16(const void *p) +{ + u16 v = get_u16(p) / BABEL_TIME_UNITS; + return MAX(1, v); +} + +static inline void +put_time16(void *p, u16 v) +{ + put_u16(p, v * BABEL_TIME_UNITS); +} + +static inline ip6_addr +get_ip6_px(const void *p, int plen) +{ + ip6_addr addr = IPA_NONE; + memcpy(&addr, p, (plen + 7) / 8); + return ip6_ntoh(addr); +} + +static inline void +put_ip6_px(void *p, ip6_addr addr, int plen) +{ + addr = ip6_hton(addr); + memcpy(p, &addr, (plen + 7) / 8); +} + +static inline ip6_addr +get_ip6_ll(const void *p) +{ + return ip6_build(0xfe800000, 0, get_u32(p+0), get_u32(p+4)); +} + +static inline void +put_ip6_ll(void *p, ip6_addr addr) +{ + put_u32(p+0, _I2(addr)); + put_u32(p+4, _I3(addr)); +} + + +/* + * TLV read/write functions + */ + +static int babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); +static int babel_read_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); +static int babel_read_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); +static int babel_read_router_id(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); +static int babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); +static int babel_read_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); +static int babel_read_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); +static int babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state); + +static int babel_write_ack(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, int max_len); +static int babel_write_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, int max_len); +static int babel_write_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, int max_len); +static int babel_write_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, int max_len); +static int babel_write_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, int max_len); +static int babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, int max_len); + +struct babel_tlv_data { + u8 min_length; + int (*read_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_parse_state *state); + int (*write_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_write_state *state, int max_len); + void (*handle_tlv)(union babel_msg *m, struct babel_iface *ifa); +}; + +const static struct babel_tlv_data tlv_data[BABEL_TLV_MAX] = { + [BABEL_TLV_ACK_REQ] = { + sizeof(struct babel_tlv_ack_req), + babel_read_ack_req, + NULL, + babel_handle_ack_req + }, + [BABEL_TLV_ACK] = { + sizeof(struct babel_tlv_ack), + NULL, + babel_write_ack, + NULL + }, + [BABEL_TLV_HELLO] = { + sizeof(struct babel_tlv_hello), + babel_read_hello, + babel_write_hello, + babel_handle_hello + }, + [BABEL_TLV_IHU] = { + sizeof(struct babel_tlv_ihu), + babel_read_ihu, + babel_write_ihu, + babel_handle_ihu + }, + [BABEL_TLV_ROUTER_ID] = { + sizeof(struct babel_tlv_router_id), + babel_read_router_id, + NULL, + NULL + }, + [BABEL_TLV_NEXT_HOP] = { + sizeof(struct babel_tlv_next_hop), + babel_read_next_hop, + NULL, + NULL + }, + [BABEL_TLV_UPDATE] = { + sizeof(struct babel_tlv_update), + babel_read_update, + babel_write_update, + babel_handle_update + }, + [BABEL_TLV_ROUTE_REQUEST] = { + sizeof(struct babel_tlv_route_request), + babel_read_route_request, + babel_write_route_request, + babel_handle_route_request + }, + [BABEL_TLV_SEQNO_REQUEST] = { + sizeof(struct babel_tlv_seqno_request), + babel_read_seqno_request, + babel_write_seqno_request, + babel_handle_seqno_request + }, +}; + +static int +babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *m, + struct babel_parse_state *state) +{ + struct babel_tlv_ack_req *tlv = (void *) hdr; + struct babel_msg_ack_req *msg = &m->ack_req; + + msg->type = BABEL_TLV_ACK_REQ; + msg->nonce = get_u16(&tlv->nonce); + msg->interval = get_time16(&tlv->interval); + msg->sender = state->saddr; + + if (!msg->interval) + return PARSE_ERROR; + + return PARSE_SUCCESS; +} + +static int +babel_write_ack(struct babel_tlv *hdr, union babel_msg *m, + struct babel_write_state *state, int max_len) +{ + struct babel_tlv_ack *tlv = (void *) hdr; + struct babel_msg_ack *msg = &m->ack; + + TLV_HDR0(tlv, BABEL_TLV_ACK); + put_u16(&tlv->nonce, msg->nonce); + + return sizeof(struct babel_tlv_ack); +} + +static int +babel_read_hello(struct babel_tlv *hdr, union babel_msg *m, + struct babel_parse_state *state) +{ + struct babel_tlv_hello *tlv = (void *) hdr; + struct babel_msg_hello *msg = &m->hello; + + msg->type = BABEL_TLV_HELLO; + msg->seqno = get_u16(&tlv->seqno); + msg->interval = get_time16(&tlv->interval); + msg->sender = state->saddr; + + return PARSE_SUCCESS; +} + +static int +babel_write_hello(struct babel_tlv *hdr, union babel_msg *m, + struct babel_write_state *state, int max_len) +{ + struct babel_tlv_hello *tlv = (void *) hdr; + struct babel_msg_hello *msg = &m->hello; + + TLV_HDR0(tlv, BABEL_TLV_HELLO); + put_u16(&tlv->seqno, msg->seqno); + put_time16(&tlv->interval, msg->interval); + + return sizeof(struct babel_tlv_hello); +} + +static int +babel_read_ihu(struct babel_tlv *hdr, union babel_msg *m, + struct babel_parse_state *state) +{ + struct babel_tlv_ihu *tlv = (void *) hdr; + struct babel_msg_ihu *msg = &m->ihu; + + msg->type = BABEL_TLV_IHU; + msg->ae = tlv->ae; + msg->rxcost = get_u16(&tlv->rxcost); + msg->interval = get_time16(&tlv->interval); + msg->addr = IPA_NONE; + msg->sender = state->saddr; + + if (msg->ae >= BABEL_AE_MAX) + return PARSE_IGNORE; + + // We handle link-local IPs. In every other case, the addr field will be 0 but + // validation will succeed. The handler takes care of these cases. + if (msg->ae == BABEL_AE_IP6_LL) + { + if (TLV_OPT_LENGTH(tlv) < 8) + return PARSE_ERROR; + + msg->addr = ipa_from_ip6(get_ip6_ll(&tlv->addr)); + } + + return PARSE_SUCCESS; +} + +static int +babel_write_ihu(struct babel_tlv *hdr, union babel_msg *m, + struct babel_write_state *state, int max_len) +{ + struct babel_tlv_ihu *tlv = (void *) hdr; + struct babel_msg_ihu *msg = &m->ihu; + + if (ipa_is_link_local(msg->addr) && max_len < sizeof(struct babel_tlv_ihu) + 8) + return 0; + + TLV_HDR0(tlv, BABEL_TLV_IHU); + put_u16(&tlv->rxcost, msg->rxcost); + put_time16(&tlv->interval, msg->interval); + + if (!ipa_is_link_local(msg->addr)) + { + tlv->ae = BABEL_AE_WILDCARD; + return sizeof(struct babel_tlv_ihu); + } + put_ip6_ll(&tlv->addr, msg->addr); + tlv->ae = BABEL_AE_IP6_LL; + hdr->length += 8; + return sizeof(struct babel_tlv_ihu) + 8; +} + +static int +babel_read_router_id(struct babel_tlv *hdr, union babel_msg *m UNUSED, + struct babel_parse_state *state) +{ + struct babel_tlv_router_id *tlv = (void *) hdr; + + state->router_id = get_u64(&tlv->router_id); + state->router_id_seen = 1; + + return PARSE_IGNORE; +} + +/* This is called directly from babel_write_update() */ +static int +babel_write_router_id(struct babel_tlv *hdr, u64 router_id, + struct babel_write_state *state, int max_len UNUSED) +{ + struct babel_tlv_router_id *tlv = (void *) hdr; + + /* We still assume that first min_length bytes are available and zeroed */ + + TLV_HDR0(tlv, BABEL_TLV_ROUTER_ID); + put_u64(&tlv->router_id, router_id); + + state->router_id = router_id; + state->router_id_seen = 1; + + return sizeof(struct babel_tlv_router_id); +} + +static int +babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *m UNUSED, + struct babel_parse_state *state) +{ + struct babel_tlv_next_hop *tlv = (void *) hdr; + + switch (tlv->ae) + { + case BABEL_AE_WILDCARD: + return PARSE_ERROR; + + case BABEL_AE_IP4: + /* TODO */ + return PARSE_IGNORE; + + case BABEL_AE_IP6: + if (TLV_OPT_LENGTH(tlv) < sizeof(ip6_addr)) + return PARSE_ERROR; + + state->next_hop = ipa_from_ip6(get_ip6(&tlv->addr)); + return PARSE_IGNORE; + + case BABEL_AE_IP6_LL: + if (TLV_OPT_LENGTH(tlv) < 8) + return PARSE_ERROR; + + state->next_hop = ipa_from_ip6(get_ip6_ll(&tlv->addr)); + return PARSE_IGNORE; + + default: + return PARSE_IGNORE; + } + + return PARSE_IGNORE; +} + +static int +babel_read_update(struct babel_tlv *hdr, union babel_msg *m, + struct babel_parse_state *state) +{ + struct babel_tlv_update *tlv = (void *) hdr; + struct babel_msg_update *msg = &m->update; + + msg->type = BABEL_TLV_UPDATE; + msg->ae = tlv->ae; + msg->interval = get_time16(&tlv->interval); + msg->seqno = get_u16(&tlv->seqno); + msg->metric = get_u16(&tlv->metric); + + /* Length of received prefix data without omitted part */ + int len = (tlv->plen + 7)/8 - (int) tlv->omitted; + u8 buf[16] = {}; + + if ((len < 0) || (len > TLV_OPT_LENGTH(tlv))) + return PARSE_ERROR; + + switch (tlv->ae) + { + case BABEL_AE_WILDCARD: + if (tlv->plen > 0) + return PARSE_ERROR; + + msg->prefix = IPA_NONE; + break; + + case BABEL_AE_IP4: + /* TODO */ + return PARSE_IGNORE; + + case BABEL_AE_IP6: + if (tlv->plen > MAX_PREFIX_LENGTH) + return PARSE_ERROR; + + /* Cannot omit data if there is no saved prefix */ + if (tlv->omitted && !state->def_ip6_prefix_seen) + return PARSE_ERROR; + + /* Merge saved prefix and received prefix parts */ + memcpy(buf, state->def_ip6_prefix, tlv->omitted); + memcpy(buf + tlv->omitted, tlv->addr, len); + + msg->plen = tlv->plen; + msg->prefix = ipa_from_ip6(get_ip6(buf)); + + if (tlv->flags & BABEL_FLAG_DEF_PREFIX) + { + put_ip6(state->def_ip6_prefix, msg->prefix); + state->def_ip6_prefix_seen = 1; + } + + if (tlv->flags & BABEL_FLAG_ROUTER_ID) + { + state->router_id = ((u64) _I2(msg->prefix)) << 32 | _I3(msg->prefix); + state->router_id_seen = 1; + } + break; + + case BABEL_AE_IP6_LL: + /* ??? */ + return PARSE_IGNORE; + + default: + return PARSE_IGNORE; + } + + if (!state->router_id_seen) + { + DBG("Babel: No router ID seen before update\n"); + return PARSE_ERROR; + } + + msg->router_id = state->router_id; + msg->next_hop = state->next_hop; + msg->sender = state->saddr; + + return PARSE_SUCCESS; +} + +static int +babel_write_update(struct babel_tlv *hdr, union babel_msg *m, + struct babel_write_state *state, int max_len) +{ + struct babel_tlv_update *tlv = (void *) hdr; + struct babel_msg_update *msg = &m->update; + int len0 = 0; + + /* + * When needed, we write Router-ID TLV before Update TLV and return size of + * both of them. There is enough space for the Router-ID TLV, because + * sizeof(struct babel_tlv_router_id) == sizeof(struct babel_tlv_update). + */ + if (!state->router_id_seen || (msg->router_id != state->router_id)) + { + len0 = babel_write_router_id(hdr, msg->router_id, state, max_len); + tlv = (struct babel_tlv_update *) NEXT_TLV(tlv); + } + + int len = sizeof(struct babel_tlv_update) + (msg->plen + 7)/8; + + if (len0 + len > max_len) + return 0; + + memset(tlv, 0, sizeof(struct babel_tlv_update)); + TLV_HDR(tlv, BABEL_TLV_UPDATE, len); + tlv->ae = BABEL_AE_IP6; + tlv->plen = msg->plen; + put_time16(&tlv->interval, msg->interval); + put_u16(&tlv->seqno, msg->seqno); + put_u16(&tlv->metric, msg->metric); + put_ip6_px(tlv->addr, msg->prefix, msg->plen); + + return len0 + len; +} + +static int +babel_read_route_request(struct babel_tlv *hdr, union babel_msg *m, + struct babel_parse_state *state) +{ + struct babel_tlv_route_request *tlv = (void *) hdr; + struct babel_msg_route_request *msg = &m->route_request; + + msg->type = BABEL_TLV_ROUTE_REQUEST; + + switch (tlv->ae) + { + case BABEL_AE_WILDCARD: + /* Wildcard requests must have plen 0 */ + if (tlv->plen > 0) + return PARSE_ERROR; + + msg->full = 1; + return PARSE_SUCCESS; + + case BABEL_AE_IP4: + /* TODO */ + return PARSE_IGNORE; + + case BABEL_AE_IP6: + if (tlv->plen > MAX_PREFIX_LENGTH) + return PARSE_ERROR; + + if (TLV_OPT_LENGTH(tlv) < (tlv->plen + 7)/8) + return PARSE_ERROR; + + msg->plen = tlv->plen; + msg->prefix = get_ip6_px(tlv->addr, tlv->plen); + return PARSE_SUCCESS; + + case BABEL_AE_IP6_LL: + return PARSE_ERROR; + + default: + return PARSE_IGNORE; + } + + return PARSE_IGNORE; +} + +static int +babel_write_route_request(struct babel_tlv *hdr, union babel_msg *m, + struct babel_write_state *state, int max_len) +{ + struct babel_tlv_route_request *tlv = (void *) hdr; + struct babel_msg_route_request *msg = &m->route_request; + + int len = sizeof(struct babel_tlv_route_request) + (msg->plen + 7)/8; + + if (len > max_len) + return 0; + + TLV_HDR(tlv, BABEL_TLV_ROUTE_REQUEST, len); + + if (msg->full) + { + tlv->ae = BABEL_AE_WILDCARD; + tlv->plen = 0; + } + else + { + tlv->ae = BABEL_AE_IP6; + tlv->plen = msg->plen; + put_ip6_px(tlv->addr, msg->prefix, msg->plen); + } + + return len; +} + +static int +babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *m, + struct babel_parse_state *state) +{ + struct babel_tlv_seqno_request *tlv = (void *) hdr; + struct babel_msg_seqno_request *msg = &m->seqno_request; + + msg->type = BABEL_TLV_SEQNO_REQUEST; + msg->seqno = get_u16(&tlv->seqno); + msg->hop_count = tlv->hop_count; + msg->router_id = get_u64(&tlv->router_id); + msg->sender = state->saddr; + + if (tlv->hop_count == 0) + return PARSE_ERROR; + + switch (tlv->ae) + { + case BABEL_AE_WILDCARD: + return PARSE_ERROR; + + case BABEL_AE_IP4: + /* TODO */ + return PARSE_IGNORE; + + case BABEL_AE_IP6: + if (tlv->plen > MAX_PREFIX_LENGTH) + return PARSE_ERROR; + + if (TLV_OPT_LENGTH(tlv) < (tlv->plen + 7)/8) + return PARSE_ERROR; + + msg->plen = tlv->plen; + msg->prefix = get_ip6_px(tlv->addr, tlv->plen); + return PARSE_SUCCESS; + + case BABEL_AE_IP6_LL: + return PARSE_ERROR; + + default: + return PARSE_IGNORE; + } + + return PARSE_IGNORE; +} + +static int +babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *m, + struct babel_write_state *state, int max_len) +{ + struct babel_tlv_seqno_request *tlv = (void *) hdr; + struct babel_msg_seqno_request *msg = &m->seqno_request; + + int len = sizeof(struct babel_tlv_seqno_request) + (msg->plen + 7)/8; + + if (len > max_len) + return 0; + + TLV_HDR(tlv, BABEL_TLV_SEQNO_REQUEST, len); + tlv->ae = BABEL_AE_IP6; + tlv->plen = msg->plen; + put_u16(&tlv->seqno, msg->seqno); + tlv->hop_count = msg->hop_count; + put_u64(&tlv->router_id, msg->router_id); + put_ip6_px(tlv->addr, msg->prefix, msg->plen); + + return len; +} + +static inline int +babel_read_tlv(struct babel_tlv *hdr, + union babel_msg *msg, + struct babel_parse_state *state) +{ + if ((hdr->type <= BABEL_TLV_PADN) || + (hdr->type >= BABEL_TLV_MAX) || + !tlv_data[hdr->type].read_tlv) + return PARSE_IGNORE; + + if (TLV_LENGTH(hdr) < tlv_data[hdr->type].min_length) + return PARSE_ERROR; + + memset(msg, 0, sizeof(*msg)); + return tlv_data[hdr->type].read_tlv(hdr, msg, state); +} + +static int +babel_write_tlv(struct babel_tlv *hdr, + union babel_msg *msg, + struct babel_write_state *state, + int max_len) +{ + if ((msg->type <= BABEL_TLV_PADN) || + (msg->type >= BABEL_TLV_MAX) || + !tlv_data[msg->type].write_tlv) + return 0; + + if (tlv_data[msg->type].min_length > max_len) + return 0; + + memset(hdr, 0, tlv_data[msg->type].min_length); + return tlv_data[msg->type].write_tlv(hdr, msg, state, max_len); +} + + +/* + * Packet RX/TX functions + */ + +static int +babel_send_to(struct babel_iface *ifa, ip_addr dest) +{ + sock *sk = ifa->sk; + struct babel_pkt_header *hdr = (void *) sk->tbuf; + int len = get_u16(&hdr->length) + sizeof(struct babel_pkt_header); + + DBG("Babel: Sending %d bytes to %I\n", len, dest); + return sk_send_to(sk, len, dest, 0); +} + +/** + * babel_write_queue - Write a TLV queue to a transmission buffer + * @ifa: Interface holding the transmission buffer + * @queue: TLV queue to write (containing internal-format TLVs) + * + * This function writes a packet to the interface transmission buffer with as + * many TLVs from the &queue as will fit in the buffer. It returns the number of + * bytes written (NOT counting the packet header). The function is called by + * babel_send_queue() and babel_send_unicast() to construct packets for + * transmission, and uses per-TLV helper functions to convert the + * internal-format TLVs to their wire representations. + * + * The TLVs in the queue are freed after they are written to the buffer. + */ +static int +babel_write_queue(struct babel_iface *ifa, list *queue) +{ + struct babel_proto *p = ifa->proto; + struct babel_write_state state = {}; + + if (EMPTY_LIST(*queue)) + return 0; + + byte *pos = ifa->sk->tbuf; + byte *end = pos + ifa->tx_length; + + struct babel_pkt_header *pkt = (void *) pos; + pkt->magic = BABEL_MAGIC; + pkt->version = BABEL_VERSION; + pkt->length = 0; + pos += sizeof(struct babel_pkt_header); + + struct babel_msg_node *msg; + WALK_LIST_FIRST(msg, *queue) + { + int len = babel_write_tlv((struct babel_tlv *) pos, &msg->msg, &state, end - pos); + + if (!len) + break; + + pos += len; + rem_node(NODE msg); + sl_free(p->msg_slab, msg); + } + + int plen = pos - (byte *) pkt; + put_u16(&pkt->length, plen - sizeof(struct babel_pkt_header)); + + return plen; +} + +void +babel_send_queue(void *arg) +{ + struct babel_iface *ifa = arg; + while ((babel_write_queue(ifa, &ifa->msg_queue) > 0) && + (babel_send_to(ifa, IP6_BABEL_ROUTERS) > 0)); +} + +static inline void +babel_kick_queue(struct babel_iface *ifa) +{ + /* + * Only schedule send event if there is not already data in the socket buffer. + * Otherwise we may overwrite the data already in the buffer. + */ + + if ((ifa->sk->tpos == ifa->sk->tbuf) && !ev_active(ifa->send_event)) + ev_schedule(ifa->send_event); +} + +/** + * babel_send_unicast - send a single TLV via unicast to a destination + * @msg: TLV to send + * @ifa: Interface to send via + * @dest: Destination of the TLV + * + * This function is used to send a single TLV via unicast to a designated + * receiver. This is used for replying to certain incoming requests, and for + * sending unicast requests to refresh routes before they expire. + */ +void +babel_send_unicast(union babel_msg *msg, struct babel_iface *ifa, ip_addr dest) +{ + struct babel_proto *p = ifa->proto; + struct babel_msg_node *msgn = sl_alloc(p->msg_slab); + list queue; + + msgn->msg = *msg; + init_list(&queue); + add_tail(&queue, NODE msgn); + babel_write_queue(ifa, &queue); + babel_send_to(ifa, dest); + + /* We could overwrite waiting packet here, we may have to kick TX queue */ + if (!EMPTY_LIST(ifa->msg_queue)) + babel_kick_queue(ifa); +} + +/** + * babel_enqueue - enqueue a TLV for transmission on an interface + * @msg: TLV to enqueue (in internal TLV format) + * @ifa: Interface to enqueue to + * + * This function is called to enqueue a TLV for subsequent transmission on an + * interface. The transmission event is triggered whenever a TLV is enqueued; + * this ensures that TLVs will be transmitted in a timely manner, but that TLVs + * which are enqueued in rapid succession can be transmitted together in one + * packet. + */ +void +babel_enqueue(union babel_msg *msg, struct babel_iface *ifa) +{ + struct babel_proto *p = ifa->proto; + struct babel_msg_node *msgn = sl_alloc(p->msg_slab); + msgn->msg = *msg; + add_tail(&ifa->msg_queue, NODE msgn); + babel_kick_queue(ifa); +} + +/** + * babel_process_packet - process incoming data packet + * @pkt: Pointer to the packet data + * @len: Length of received packet + * @saddr: Address of packet sender + * @ifa: Interface packet was received on. + * + * This function is the main processing hook of incoming Babel packets. It + * checks that the packet header is well-formed, then processes the TLVs + * contained in the packet. This is done in two passes: First all TLVs are + * parsed into the internal TLV format. If a TLV parser fails, processing of the + * rest of the packet is aborted. + * + * After the parsing step, the TLV handlers are called for each parsed TLV in + * order. + */ +static void +babel_process_packet(struct babel_pkt_header *pkt, int len, + ip_addr saddr, struct babel_iface *ifa) +{ + struct babel_proto *p = ifa->proto; + struct babel_tlv *tlv; + struct babel_msg_node *msg; + list msgs; + int res; + + int plen = sizeof(struct babel_pkt_header) + get_u16(&pkt->length); + byte *pos; + byte *end = (byte *)pkt + plen; + + struct babel_parse_state state = { + .proto = p, + .ifa = ifa, + .saddr = saddr, + .next_hop = saddr, + }; + + if ((pkt->magic != BABEL_MAGIC) || (pkt->version != BABEL_VERSION)) + { + TRACE(D_PACKETS, "Strange packet from %I via %s - magic %d version %d", + saddr, ifa->iface->name, pkt->magic, pkt->version); + return; + } + + if (plen > len) + { + LOG_PKT("Bad packet from %I via %s - %s (%u)", + saddr, ifa->iface->name, "length mismatch", plen); + return; + } + + TRACE(D_PACKETS, "Packet received from %I via %s", + saddr, ifa->iface->name); + + init_list(&msgs); + + /* First pass through the packet TLV by TLV, parsing each into internal data + structures. */ + for (tlv = FIRST_TLV(pkt); + (byte *)tlv < end; + tlv = NEXT_TLV(tlv)) + { + /* Ugly special case */ + if (tlv->type == BABEL_TLV_PAD1) + continue; + + /* The end of the common TLV header */ + pos = (byte *)tlv + sizeof(struct babel_tlv); + if ((pos > end) || (pos + tlv->length > end)) + { + LOG_PKT("Bad TLV from %I via %s type %d pos %d - framing error", + saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt); + break; + } + + msg = sl_alloc(p->msg_slab); + res = babel_read_tlv(tlv, &msg->msg, &state); + if (res == PARSE_SUCCESS) + { + add_tail(&msgs, NODE msg); + } + else if (res == PARSE_IGNORE) + { + DBG("Babel: Ignoring TLV of type %d\n", tlv->type); + sl_free(p->msg_slab, msg); + } + else /* PARSE_ERROR */ + { + LOG_PKT("Bad TLV from %I via %s type %d pos %d - parse error", + saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt); + sl_free(p->msg_slab, msg); + break; + } + } + + /* Parsing done, handle all parsed TLVs */ + WALK_LIST_FIRST(msg, msgs) + { + if (tlv_data[msg->msg.type].handle_tlv) + tlv_data[msg->msg.type].handle_tlv(&msg->msg, ifa); + rem_node(NODE msg); + sl_free(p->msg_slab, msg); + } +} + +static void +babel_err_hook(sock *sk, int err) +{ + struct babel_iface *ifa = sk->data; + struct babel_proto *p = ifa->proto; + + log(L_ERR "%s: Socket error on %s: %M", p->p.name, ifa->iface->name, err); + /* FIXME: Drop queued TLVs here? */ +} + + +static void +babel_tx_hook(sock *sk) +{ + struct babel_iface *ifa = sk->data; + + DBG("Babel: TX hook called (iface %s, src %I, dst %I)\n", + sk->iface->name, sk->saddr, sk->daddr); + + babel_send_queue(ifa); +} + + +static int +babel_rx_hook(sock *sk, int len) +{ + struct babel_iface *ifa = sk->data; + struct babel_proto *p = ifa->proto; + const char *err_dsc = NULL; + uint err_val = 0; + + if (sk->lifindex != ifa->iface->index) + return 1; + + DBG("Babel: RX hook called (iface %s, src %I, dst %I)\n", + sk->iface->name, sk->faddr, sk->laddr); + + /* Silently ignore my own packets */ + if (ipa_equal(ifa->iface->addr->ip, sk->faddr)) + return 1; + + if (!ipa_is_link_local(sk->faddr)) + DROP1("wrong src address"); + + if (sk->fport != ifa->cf->port) + DROP("wrong src port", sk->fport); + + if (len < sizeof(struct babel_pkt_header)) + DROP("too short", len); + + if (sk->flags & SKF_TRUNCATED) + DROP("truncated", len); + + babel_process_packet((struct babel_pkt_header *) sk->rbuf, len, sk->faddr, ifa); + return 1; + +drop: + LOG_PKT("Bad packet from %I via %s - %s (%u)", + sk->faddr, sk->iface->name, err_dsc, err_val); + return 1; +} + +int +babel_open_socket(struct babel_iface *ifa) +{ + struct babel_proto *p = ifa->proto; + + sock *sk; + sk = sk_new(ifa->pool); + sk->type = SK_UDP; + sk->sport = ifa->cf->port; + sk->dport = ifa->cf->port; + sk->iface = ifa->iface; + + sk->rx_hook = babel_rx_hook; + sk->tx_hook = babel_tx_hook; + sk->err_hook = babel_err_hook; + sk->data = ifa; + + sk->tos = ifa->cf->tx_tos; + sk->priority = ifa->cf->tx_priority; + sk->ttl = 1; + sk->flags = SKF_LADDR_RX; + + if (sk_open(sk) < 0) + goto err; + + if (sk_setup_multicast(sk) < 0) + goto err; + + if (sk_join_group(sk, IP6_BABEL_ROUTERS) < 0) + goto err; + + ifa->sk = sk; + return 1; + +err: + sk_log_error(sk, p->p.name); + rfree(sk); + return 0; +} |