diff options
Diffstat (limited to 'nhrp/nhrp_packet.c')
| -rw-r--r-- | nhrp/nhrp_packet.c | 1331 |
1 files changed, 1331 insertions, 0 deletions
diff --git a/nhrp/nhrp_packet.c b/nhrp/nhrp_packet.c new file mode 100644 index 0000000..f46b481 --- /dev/null +++ b/nhrp/nhrp_packet.c @@ -0,0 +1,1331 @@ +/* nhrp_packet.c - NHRP packet marshalling and tranceiving + * + * Copyright (C) 2007-2009 Timo Teräs <timo.teras@iki.fi> + * All rights reserved. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 or later as + * published by the Free Software Foundation. + * + * See http://www.gnu.org/ for details. + */ + +#include <malloc.h> +#include <string.h> +#include <stdio.h> +#include <ctype.h> +#include <netinet/in.h> + +#include "libev.h" +#include "nhrp_common.h" +#include "nhrp_packet.h" +#include "nhrp_peer.h" +#include "nhrp_interface.h" + +#define PACKET_RETRIES 6 +#define PACKET_RETRY_INTERVAL 5.0 + +#define RATE_LIMIT_HASH_SIZE 256 +#define RATE_LIMIT_MAX_TOKENS 4 +#define RATE_LIMIT_SEND_INTERVAL 5.0 +#define RATE_LIMIT_SILENCE 360.0 +#define RATE_LIMIT_PURGE_INTERVAL 600.0 + +#define MAX_PDU_SIZE 1500 + +struct nhrp_rate_limit { + struct hlist_node hash_entry; + struct nhrp_address src; + struct nhrp_address dst; + ev_tstamp rate_last; + int rate_tokens; +}; + +static uint32_t request_id = 0; +static struct list_head pending_requests = LIST_INITIALIZER(pending_requests); +static struct hlist_head rate_limit_hash[RATE_LIMIT_HASH_SIZE]; +static ev_timer rate_limit_timer; +static int num_rate_limit_entries = 0; + +static void nhrp_packet_xmit_timeout_cb(struct ev_timer *w, int revents); +static int unmarshall_packet_header(uint8_t **pdu, size_t *pdusize, + struct nhrp_packet *packet); + +static void nhrp_rate_limit_delete(struct nhrp_rate_limit *rl) +{ + hlist_del(&rl->hash_entry); + free(rl); + num_rate_limit_entries--; +} + +int nhrp_rate_limit_clear(struct nhrp_address *a, int pref) +{ + struct nhrp_rate_limit *rl; + struct hlist_node *n, *c; + int i, ret = 0; + + for (i = 0; i < RATE_LIMIT_HASH_SIZE; i++) { + hlist_for_each_entry_safe(rl, c, n, &rate_limit_hash[i], + hash_entry) { + if (a->type == AF_UNSPEC || + nhrp_address_prefix_cmp(a, &rl->src, pref) == 0 || + nhrp_address_prefix_cmp(a, &rl->dst, pref) == 0) { + nhrp_rate_limit_delete(rl); + ret++; + } + } + } + + if (num_rate_limit_entries == 0) + ev_timer_stop(&rate_limit_timer); + + return ret; +} + +static void prune_rate_limit_entries_cb(struct ev_timer *w, int revents) +{ + struct nhrp_rate_limit *rl; + struct hlist_node *c, *n; + int i; + + for (i = 0; i < RATE_LIMIT_HASH_SIZE; i++) { + hlist_for_each_entry_safe(rl, c, n, &rate_limit_hash[i], + hash_entry) { + + if (ev_now() > rl->rate_last + 2 * RATE_LIMIT_SILENCE) + nhrp_rate_limit_delete(rl); + } + } + + if (num_rate_limit_entries == 0) + ev_timer_stop(&rate_limit_timer); +} + +static struct nhrp_rate_limit *get_rate_limit(struct nhrp_address *src, + struct nhrp_address *dst) +{ + unsigned int key; + struct nhrp_rate_limit *e; + struct hlist_node *n; + + key = nhrp_address_hash(src) ^ nhrp_address_hash(dst); + key %= RATE_LIMIT_HASH_SIZE; + + hlist_for_each_entry(e, n, &rate_limit_hash[key], hash_entry) { + if (nhrp_address_cmp(&e->src, src) == 0 && + nhrp_address_cmp(&e->dst, dst) == 0) + return e; + } + + e = calloc(1, sizeof(struct nhrp_rate_limit)); + e->src = *src; + e->dst = *dst; + hlist_add_head(&e->hash_entry, &rate_limit_hash[key]); + + if (num_rate_limit_entries == 0) { + ev_timer_init(&rate_limit_timer, prune_rate_limit_entries_cb, + RATE_LIMIT_PURGE_INTERVAL, + RATE_LIMIT_PURGE_INTERVAL); + ev_timer_start(&rate_limit_timer); + } + + num_rate_limit_entries++; + + return e; +} + +static uint16_t nhrp_calculate_checksum(uint8_t *pdu, uint16_t len) +{ + uint16_t *pdu16 = (uint16_t *) pdu; + uint32_t csum = 0; + int i; + + for (i = 0; i < len / 2; i++) + csum += pdu16[i]; + if (len & 1) + csum += htons(pdu[len - 1]); + + while (csum & 0xffff0000) + csum = (csum & 0xffff) + (csum >> 16); + + return (~csum) & 0xffff; +} + +struct nhrp_buffer *nhrp_buffer_alloc(uint32_t size) +{ + struct nhrp_buffer *buf; + + buf = malloc(sizeof(struct nhrp_buffer) + size); + buf->length = size; + + return buf; +} + +struct nhrp_buffer *nhrp_buffer_copy(struct nhrp_buffer *buffer) +{ + struct nhrp_buffer *copy; + + copy = nhrp_buffer_alloc(buffer->length); + memcpy(copy->data, buffer->data, buffer->length); + return copy; +} + +int nhrp_buffer_cmp(struct nhrp_buffer *a, struct nhrp_buffer *b) +{ + if (a->length > b->length) + return 1; + if (a->length < b->length) + return -1; + return memcmp(a->data, b->data, a->length); +} + +void nhrp_buffer_free(struct nhrp_buffer *buffer) +{ + free(buffer); +} + +struct nhrp_cie *nhrp_cie_alloc(void) +{ + return calloc(1, sizeof(struct nhrp_cie)); +} + +void nhrp_cie_free(struct nhrp_cie *cie) +{ + free(cie); +} + +void nhrp_cie_reset(struct nhrp_cie *cie) +{ + memset(&cie->cie_list_entry, 0, sizeof(cie->cie_list_entry)); +} + +void nhrp_payload_free(struct nhrp_payload *payload) +{ + struct nhrp_cie *cie, *n; + + switch (payload->payload_type) { + case NHRP_PAYLOAD_TYPE_RAW: + nhrp_buffer_free(payload->u.raw); + break; + case NHRP_PAYLOAD_TYPE_CIE_LIST: + list_for_each_entry_safe(cie, n, &payload->u.cie_list, cie_list_entry) { + list_del(&cie->cie_list_entry); + nhrp_cie_free(cie); + } + break; + } + payload->payload_type = NHRP_PAYLOAD_TYPE_NONE; +} + +void nhrp_payload_set_type(struct nhrp_payload *payload, int type) +{ + if (payload->payload_type == type) + return; + + nhrp_payload_free(payload); + payload->payload_type = type; + switch (type) { + case NHRP_PAYLOAD_TYPE_CIE_LIST: + list_init(&payload->u.cie_list); + break; + default: + payload->u.raw = NULL; + break; + } +} + +void nhrp_payload_set_raw(struct nhrp_payload *payload, struct nhrp_buffer *raw) +{ + nhrp_payload_set_type(payload, NHRP_PAYLOAD_TYPE_RAW); + payload->u.raw = raw; +} + +void nhrp_payload_add_cie(struct nhrp_payload *payload, struct nhrp_cie *cie) +{ + if (payload->payload_type != NHRP_PAYLOAD_TYPE_CIE_LIST) { + nhrp_cie_free(cie); + nhrp_info("Trying to add CIE payload to non-CIE payload %d\n", + payload->payload_type); + return; + } + + list_add_tail(&cie->cie_list_entry, &payload->u.cie_list); +} + +struct nhrp_cie *nhrp_payload_get_cie(struct nhrp_payload *payload, int index) +{ + struct nhrp_cie *cie; + + if (payload->payload_type != NHRP_PAYLOAD_TYPE_CIE_LIST) + return NULL; + + list_for_each_entry(cie, &payload->u.cie_list, cie_list_entry) { + index--; + if (index == 0) + return cie; + } + + return NULL; +} + +struct nhrp_packet *nhrp_packet_alloc(void) +{ + struct nhrp_packet *packet; + packet = calloc(1, sizeof(struct nhrp_packet)); + packet->ref = 1; + packet->hdr.hop_count = NHRP_PACKET_DEFAULT_HOP_COUNT; + list_init(&packet->request_list_entry); + ev_timer_init(&packet->timeout, nhrp_packet_xmit_timeout_cb, + PACKET_RETRY_INTERVAL, PACKET_RETRY_INTERVAL); + return packet; +} + +struct nhrp_packet *nhrp_packet_get(struct nhrp_packet *packet) +{ + packet->ref++; + return packet; +} + +struct nhrp_payload *nhrp_packet_payload(struct nhrp_packet *packet, int payload_type) +{ + return nhrp_packet_extension(packet, NHRP_EXTENSION_PAYLOAD, payload_type); +} + +struct nhrp_payload *nhrp_packet_extension(struct nhrp_packet *packet, + uint32_t extension, int payload_type) +{ + struct nhrp_payload *p; + + p = packet->extension_by_type[extension & 0x7fff]; + if (p != NULL) { + if (payload_type == NHRP_PAYLOAD_TYPE_ANY || + payload_type == p->payload_type) + return p; + if (extension & NHRP_EXTENSION_FLAG_NOCREATE) + return NULL; + nhrp_payload_set_type(p, payload_type); + return p; + } + + if (extension & NHRP_EXTENSION_FLAG_NOCREATE) + return NULL; + + p = &packet->extension_by_order[packet->num_extensions++]; + p->extension_type = extension & 0xffff; + packet->extension_by_type[extension & 0x7fff] = p; + if (payload_type != NHRP_PAYLOAD_TYPE_ANY) + nhrp_payload_set_type(p, payload_type); + + return p; +} + +static void nhrp_packet_release(struct nhrp_packet *packet) +{ + int i; + + if (packet->dst_peer != NULL) + nhrp_peer_put(packet->dst_peer); + for (i = 0; i < packet->num_extensions; i++) + nhrp_payload_free(&packet->extension_by_order[i]); + free(packet); +} + +void nhrp_packet_put(struct nhrp_packet *packet) +{ + NHRP_BUG_ON(packet->ref == 0); + + packet->ref--; + if (packet->ref == 0) + nhrp_packet_release(packet); +} + +int nhrp_packet_reroute(struct nhrp_packet *packet, struct nhrp_peer *dst_peer) +{ + packet->dst_iface = packet->src_iface; + if (packet->dst_peer != NULL) + nhrp_peer_put(packet->dst_peer); + packet->dst_peer = nhrp_peer_get(dst_peer); + return nhrp_packet_route(packet); +} + +static void nhrp_packet_dequeue(struct nhrp_packet *packet) +{ + ev_timer_stop(&packet->timeout); + if (list_hashed(&packet->request_list_entry)) + list_del(&packet->request_list_entry); + nhrp_packet_put(packet); +} + +static int nhrp_do_handle_error_indication(struct nhrp_packet *error_pkt, + struct nhrp_packet *orig_pkt) +{ + struct nhrp_packet *req; + + list_for_each_entry(req, &pending_requests, request_list_entry) { + if (orig_pkt->hdr.u.request_id != req->hdr.u.request_id) + continue; + + if (nhrp_address_cmp(&orig_pkt->src_nbma_address, + &req->src_nbma_address)) + continue; + if (nhrp_address_cmp(&orig_pkt->src_protocol_address, + &req->src_protocol_address)) + continue; + + if (req->handler != NULL) + req->handler(req->handler_ctx, error_pkt); + nhrp_packet_dequeue(req); + + return TRUE; + } + + return FALSE; +} + +static int nhrp_handle_error_indication(struct nhrp_packet *error_packet) +{ + struct nhrp_packet *packet; + struct nhrp_payload *payload; + uint8_t *pdu; + size_t pduleft; + int r; + + packet = nhrp_packet_alloc(); + if (packet == NULL) + return FALSE; + + payload = nhrp_packet_payload(error_packet, NHRP_PAYLOAD_TYPE_RAW); + pdu = payload->u.raw->data; + pduleft = payload->u.raw->length; + + if (!unmarshall_packet_header(&pdu, &pduleft, packet)) { + nhrp_packet_put(packet); + return FALSE; + } + + r = nhrp_do_handle_error_indication(error_packet, packet); + nhrp_packet_put(packet); + + return r; +} + +#define NHRP_TYPE_REQUEST 0 +#define NHRP_TYPE_REPLY 1 +#define NHRP_TYPE_INDICATION 2 + +static struct { + int type; + uint16_t payload_type; + int (*handler)(struct nhrp_packet *packet); +} packet_types[] = { + [NHRP_PACKET_RESOLUTION_REQUEST] = { + .type = NHRP_TYPE_REQUEST, + .payload_type = NHRP_PAYLOAD_TYPE_CIE_LIST, + }, + [NHRP_PACKET_RESOLUTION_REPLY] = { + .type = NHRP_TYPE_REPLY, + .payload_type = NHRP_PAYLOAD_TYPE_CIE_LIST, + }, + [NHRP_PACKET_REGISTRATION_REQUEST] = { + .type = NHRP_TYPE_REQUEST, + .payload_type = NHRP_PAYLOAD_TYPE_CIE_LIST, + }, + [NHRP_PACKET_REGISTRATION_REPLY] = { + .type = NHRP_TYPE_REPLY, + .payload_type = NHRP_PAYLOAD_TYPE_CIE_LIST, + }, + [NHRP_PACKET_PURGE_REQUEST] = { + .type = NHRP_TYPE_REQUEST, + .payload_type = NHRP_PAYLOAD_TYPE_CIE_LIST, + }, + [NHRP_PACKET_PURGE_REPLY] = { + .type = NHRP_TYPE_REPLY, + .payload_type = NHRP_PAYLOAD_TYPE_CIE_LIST, + }, + [NHRP_PACKET_ERROR_INDICATION] = { + .type = NHRP_TYPE_INDICATION, + .payload_type = NHRP_PAYLOAD_TYPE_RAW, + .handler = nhrp_handle_error_indication, + }, + [NHRP_PACKET_TRAFFIC_INDICATION] = { + .type = NHRP_TYPE_INDICATION, + .payload_type = NHRP_PAYLOAD_TYPE_RAW, + } +}; +static int extension_types[] = { + [NHRP_EXTENSION_RESPONDER_ADDRESS] = NHRP_PAYLOAD_TYPE_CIE_LIST, + [NHRP_EXTENSION_FORWARD_TRANSIT_NHS] = NHRP_PAYLOAD_TYPE_CIE_LIST, + [NHRP_EXTENSION_REVERSE_TRANSIT_NHS] = NHRP_PAYLOAD_TYPE_CIE_LIST, + [NHRP_EXTENSION_NAT_ADDRESS] = NHRP_PAYLOAD_TYPE_CIE_LIST +}; + +static int unmarshall_binary(uint8_t **pdu, size_t *pduleft, size_t size, void *raw) +{ + if (*pduleft < size) + return FALSE; + + memcpy(raw, *pdu, size); + *pdu += size; + *pduleft -= size; + return TRUE; +} + +static inline int unmarshall_protocol_address(uint8_t **pdu, size_t *pduleft, struct nhrp_address *pa) +{ + if (*pduleft < pa->addr_len) + return FALSE; + + if (pa->addr_len) { + if (!nhrp_address_set(pa, pa->type, pa->addr_len, *pdu)) + return FALSE; + } else { + nhrp_address_set_type(pa, PF_UNSPEC); + } + + *pdu += pa->addr_len; + *pduleft -= pa->addr_len; + return TRUE; +} + +static inline int unmarshall_nbma_address(uint8_t **pdu, size_t *pduleft, struct nhrp_address *na) +{ + if (*pduleft < na->addr_len + na->subaddr_len) + return FALSE; + + if (na->addr_len || na->subaddr_len) { + if (!nhrp_address_set_full(na, na->type, + na->addr_len, *pdu, + na->subaddr_len, *pdu + na->addr_len)) + return FALSE; + } else { + nhrp_address_set_type(na, PF_UNSPEC); + } + + *pdu += na->addr_len + na->subaddr_len; + *pduleft -= na->addr_len + na->subaddr_len; + return TRUE; +} + +static int unmarshall_cie(uint8_t **pdu, size_t *pduleft, struct nhrp_packet *p, struct nhrp_cie *cie) +{ + if (!unmarshall_binary(pdu, pduleft, sizeof(struct nhrp_cie_header), &cie->hdr)) + return FALSE; + + cie->nbma_address.type = nhrp_pf_from_afnum(p->hdr.afnum); + cie->nbma_address.addr_len = cie->hdr.nbma_address_len; + cie->nbma_address.subaddr_len = cie->hdr.nbma_subaddress_len; + cie->protocol_address.type = nhrp_pf_from_protocol(p->hdr.protocol_type); + cie->protocol_address.addr_len = cie->hdr.protocol_address_len; + + if (!unmarshall_nbma_address(pdu, pduleft, &cie->nbma_address)) + return FALSE; + return unmarshall_protocol_address(pdu, pduleft, &cie->protocol_address); +} + +static int unmarshall_payload(uint8_t **pdu, size_t *pduleft, + struct nhrp_packet *packet, + int type, size_t size, + struct nhrp_payload *p) +{ + struct nhrp_cie *cie; + size_t cieleft; + + if (*pduleft < size) + return FALSE; + + nhrp_payload_set_type(p, type); + switch (p->payload_type) { + case NHRP_PAYLOAD_TYPE_NONE: + *pdu += size; + *pduleft -= size; + return TRUE; + case NHRP_PAYLOAD_TYPE_RAW: + p->u.raw = nhrp_buffer_alloc(size); + return unmarshall_binary(pdu, pduleft, size, p->u.raw->data); + case NHRP_PAYLOAD_TYPE_CIE_LIST: + cieleft = size; + while (cieleft) { + cie = nhrp_cie_alloc(); + list_add_tail(&cie->cie_list_entry, &p->u.cie_list); + if (!unmarshall_cie(pdu, &cieleft, packet, cie)) + return FALSE; + } + *pduleft -= size; + return TRUE; + default: + return FALSE; + } +} + +static int unmarshall_packet_header(uint8_t **pdu, size_t *pduleft, struct nhrp_packet *packet) +{ + struct nhrp_packet_header *phdr = (struct nhrp_packet_header *) *pdu; + + if (!unmarshall_binary(pdu, pduleft, sizeof(packet->hdr), &packet->hdr)) + return FALSE; + + if (packet->hdr.type >= ARRAY_SIZE(packet_types)) + return FALSE; + + packet->src_nbma_address.type = nhrp_pf_from_afnum(packet->hdr.afnum); + packet->src_nbma_address.addr_len = phdr->src_nbma_address_len; + packet->src_nbma_address.subaddr_len = phdr->src_nbma_subaddress_len; + packet->src_protocol_address.type = nhrp_pf_from_protocol(packet->hdr.protocol_type); + packet->src_protocol_address.addr_len = phdr->src_protocol_address_len; + packet->dst_protocol_address.type = nhrp_pf_from_protocol(packet->hdr.protocol_type); + packet->dst_protocol_address.addr_len = phdr->dst_protocol_address_len; + + if (!unmarshall_nbma_address(pdu, pduleft, &packet->src_nbma_address)) + return FALSE; + if (!unmarshall_protocol_address(pdu, pduleft, &packet->src_protocol_address)) + return FALSE; + return unmarshall_protocol_address(pdu, pduleft, &packet->dst_protocol_address); +} + +static int unmarshall_packet(uint8_t *pdu, size_t pdusize, struct nhrp_packet *packet) +{ + size_t pduleft = pdusize; + uint8_t *pos = pdu; + int size, extension_offset; + + if (!unmarshall_packet_header(&pos, &pduleft, packet)) + return FALSE; + + extension_offset = ntohs(packet->hdr.extension_offset); + if (extension_offset == 0) { + /* No extensions; rest of data is payload */ + size = pduleft; + } else { + /* Extensions present; exclude those from payload */ + size = extension_offset - (pos - pdu); + if (size < 0 || size > pduleft) { + nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ERROR, pos - pdu); + return FALSE; + } + } + + if (!unmarshall_payload(&pos, &pduleft, packet, + packet_types[packet->hdr.type].payload_type, + size, nhrp_packet_payload(packet, NHRP_PAYLOAD_TYPE_ANY))) { + nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ERROR, pos - pdu); + return FALSE; + } + + if (extension_offset == 0) + return TRUE; + + pos = &pdu[extension_offset]; + pduleft = pdusize - extension_offset; + do { + struct nhrp_extension_header eh; + int extension_type, payload_type; + + if (!unmarshall_binary(&pos, &pduleft, sizeof(eh), &eh)) { + nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ERROR, pos - pdu); + return FALSE; + } + + extension_type = ntohs(eh.type) & ~NHRP_EXTENSION_FLAG_COMPULSORY; + if (extension_type == NHRP_EXTENSION_END) + break; + + payload_type = NHRP_PAYLOAD_TYPE_NONE; + if (extension_type < ARRAY_SIZE(extension_types)) + payload_type = extension_types[extension_type]; + if (payload_type == NHRP_PAYLOAD_TYPE_NONE) + payload_type = NHRP_PAYLOAD_TYPE_RAW; + if (payload_type == NHRP_PAYLOAD_TYPE_RAW && + ntohs(eh.length) == 0) + payload_type = NHRP_PAYLOAD_TYPE_NONE; + + if (!unmarshall_payload(&pos, &pduleft, packet, + payload_type, ntohs(eh.length), + nhrp_packet_extension(packet, ntohs(eh.type), NHRP_PAYLOAD_TYPE_ANY))) { + nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ERROR, pos - pdu); + return FALSE; + } + } while (1); + + return TRUE; +} + +static int nhrp_packet_forward(struct nhrp_packet *packet) +{ + char tmp[64], tmp2[64], tmp3[64]; + struct nhrp_payload *p = NULL; + + nhrp_info("Forwarding packet from nbma src %s, proto src %s to proto dst %s, hop count %d", + nhrp_address_format(&packet->src_nbma_address, + sizeof(tmp), tmp), + nhrp_address_format(&packet->src_protocol_address, + sizeof(tmp2), tmp2), + nhrp_address_format(&packet->dst_protocol_address, + sizeof(tmp3), tmp3), + packet->hdr.hop_count); + + if (packet->hdr.hop_count == 0) { + nhrp_packet_send_error(packet, NHRP_ERROR_HOP_COUNT_EXCEEDED, 0); + return TRUE; + } + packet->hdr.hop_count--; + + if (!nhrp_packet_reroute(packet, NULL)) { + nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ADDRESS_UNREACHABLE, 0); + return FALSE; + } + + switch (packet_types[packet->hdr.type].type) { + case NHRP_TYPE_REQUEST: + case NHRP_TYPE_INDICATION: + p = nhrp_packet_extension(packet, + NHRP_EXTENSION_FORWARD_TRANSIT_NHS | + NHRP_EXTENSION_FLAG_NOCREATE, + NHRP_PAYLOAD_TYPE_CIE_LIST); + break; + case NHRP_TYPE_REPLY: + p = nhrp_packet_extension(packet, + NHRP_EXTENSION_REVERSE_TRANSIT_NHS | + NHRP_EXTENSION_FLAG_NOCREATE, + NHRP_PAYLOAD_TYPE_CIE_LIST); + break; + } + if (p != NULL) { + struct nhrp_cie *cie; + + if (nhrp_address_match_cie_list(&packet->dst_peer->my_nbma_address, + &packet->dst_iface->protocol_address, + &p->u.cie_list)) { + nhrp_packet_send_error(packet, NHRP_ERROR_LOOP_DETECTED, 0); + return FALSE; + } + + cie = nhrp_cie_alloc(); + if (cie != NULL) { + cie->hdr = (struct nhrp_cie_header) { + .code = NHRP_CODE_SUCCESS, + .holding_time = htons(packet->dst_iface->holding_time), + }; + cie->nbma_address = packet->dst_peer->my_nbma_address; + cie->protocol_address = packet->dst_iface->protocol_address; + nhrp_payload_add_cie(p, cie); + } + } + + return nhrp_packet_route_and_send(packet); +} + +static int nhrp_packet_receive_local(struct nhrp_packet *packet) +{ + struct nhrp_packet *req; + char tmp[64], tmp2[64], tmp3[64]; + + if (packet_types[packet->hdr.type].type == NHRP_TYPE_REPLY) { + list_for_each_entry(req, &pending_requests, request_list_entry) { + if (packet->hdr.u.request_id != req->hdr.u.request_id) + continue; + if (nhrp_address_cmp(&packet->src_nbma_address, + &req->src_nbma_address)) + continue; + if (nhrp_address_cmp(&packet->src_protocol_address, + &req->src_protocol_address)) + continue; + + if (req->handler != NULL) + req->handler(req->handler_ctx, packet); + nhrp_packet_dequeue(req); + + return TRUE; + } + + /* Reply to unsent request? */ + nhrp_info("Packet type %d from nbma src %s, proto src %s, " + "proto dst %s dropped: no matching request", + packet->hdr.type, + nhrp_address_format(&packet->src_nbma_address, + sizeof(tmp), tmp), + nhrp_address_format(&packet->src_protocol_address, + sizeof(tmp2), tmp2), + nhrp_address_format(&packet->dst_protocol_address, + sizeof(tmp3), tmp3)); + + nhrp_packet_send_error( + packet, NHRP_ERROR_INVALID_RESOLUTION_REPLY, 0); + return TRUE; + } + + if (packet_types[packet->hdr.type].handler == NULL) { + nhrp_info("Packet type %d from nbma src %s, proto src %s, " + "proto dst %s not supported", + packet->hdr.type, + nhrp_address_format(&packet->src_nbma_address, + sizeof(tmp), tmp), + nhrp_address_format(&packet->src_protocol_address, + sizeof(tmp2), tmp2), + nhrp_address_format(&packet->dst_protocol_address, + sizeof(tmp3), tmp3)); + return FALSE; + } + + if (packet->dst_peer->next_hop_address.type != PF_UNSPEC) { + /* Broadcast destinations gets rewritten as if destinied to + * our local address */ + packet->dst_protocol_address = + packet->dst_peer->next_hop_address; + } + + return packet_types[packet->hdr.type].handler(packet); +} + +int nhrp_packet_receive(uint8_t *pdu, size_t pdulen, + struct nhrp_interface *iface, + struct nhrp_address *from) +{ + char tmp[64]; + struct nhrp_packet *packet; + struct nhrp_address *dest; + struct nhrp_peer *peer; + int ret = FALSE; + + if (nhrp_calculate_checksum(pdu, pdulen) != 0) { + nhrp_error("Bad checksum in packet from %s", + nhrp_address_format(from, sizeof(tmp), tmp)); + return FALSE; + } + + packet = nhrp_packet_alloc(); + if (packet == NULL) + return FALSE; + + if (!unmarshall_packet(pdu, pdulen, packet)) { + nhrp_error("Failed to unmarshall packet from %s", + nhrp_address_format(from, sizeof(tmp), tmp)); + goto error; + } + + packet->req_pdu = pdu; + packet->req_pdulen = pdulen; + + if (packet_types[packet->hdr.type].type == NHRP_TYPE_REPLY) + dest = &packet->src_protocol_address; + else + dest = &packet->dst_protocol_address; + + peer = nhrp_peer_route(iface, dest, 0, BIT(NHRP_PEER_TYPE_LOCAL_ADDR)); + packet->src_linklayer_address = *from; + packet->src_iface = iface; + packet->dst_peer = nhrp_peer_get(peer); + + /* RFC2332 5.3.4 - Authentication is always done pairwise on an NHRP + * hop-by-hop basis; i.e. regenerated at each hop. */ + if (packet->src_iface->auth_token && + (packet->hdr.type != NHRP_PACKET_ERROR_INDICATION || + packet->hdr.u.error.code != NHRP_ERROR_AUTHENTICATION_FAILURE)) { + struct nhrp_payload *p; + p = nhrp_packet_extension(packet, + NHRP_EXTENSION_AUTHENTICATION | + NHRP_EXTENSION_FLAG_NOCREATE, + NHRP_PAYLOAD_TYPE_RAW); + if (p == NULL || + nhrp_buffer_cmp(packet->src_iface->auth_token, p->u.raw) != 0) { + nhrp_error("Dropping packet from %s with bad authentication", + nhrp_address_format(from, sizeof(tmp), tmp)); + nhrp_packet_send_error(packet, NHRP_ERROR_AUTHENTICATION_FAILURE, 0); + goto error; + } + } + + if (peer != NULL && + peer->type == NHRP_PEER_TYPE_LOCAL_ADDR) + ret = nhrp_packet_receive_local(packet); + else + ret = nhrp_packet_forward(packet); + + packet->req_pdu = NULL; + packet->req_pdulen = 0; + +error: + nhrp_packet_put(packet); + return ret; +} + +static int marshall_binary(uint8_t **pdu, size_t *pduleft, size_t size, void *raw) +{ + if (*pduleft < size) + return FALSE; + + memcpy(*pdu, raw, size); + *pdu += size; + *pduleft -= size; + + return TRUE; +} + +static inline int marshall_protocol_address(uint8_t **pdu, size_t *pduleft, struct nhrp_address *pa) +{ + if (pa->subaddr_len != 0) + return FALSE; + return marshall_binary(pdu, pduleft, pa->addr_len, pa->addr); +} + +static inline int marshall_nbma_address(uint8_t **pdu, size_t *pduleft, struct nhrp_address *na) +{ + return marshall_binary(pdu, pduleft, na->addr_len + na->subaddr_len, na->addr); +} + +static int marshall_cie(uint8_t **pdu, size_t *pduleft, struct nhrp_cie *cie) +{ + cie->hdr.nbma_address_len = cie->nbma_address.addr_len; + cie->hdr.nbma_subaddress_len = cie->nbma_address.subaddr_len; + cie->hdr.protocol_address_len = cie->protocol_address.addr_len; + + if (!marshall_binary(pdu, pduleft, sizeof(struct nhrp_cie_header), &cie->hdr)) + return FALSE; + if (!marshall_nbma_address(pdu, pduleft, &cie->nbma_address)) + return FALSE; + return marshall_protocol_address(pdu, pduleft, &cie->protocol_address); +} + +static int marshall_payload(uint8_t **pdu, size_t *pduleft, struct nhrp_payload *p) +{ + struct nhrp_cie *cie; + + switch (p->payload_type) { + case NHRP_PAYLOAD_TYPE_NONE: + return TRUE; + case NHRP_PAYLOAD_TYPE_RAW: + if (p->u.raw->length == 0) + return TRUE; + return marshall_binary(pdu, pduleft, p->u.raw->length, p->u.raw->data); + case NHRP_PAYLOAD_TYPE_CIE_LIST: + list_for_each_entry(cie, &p->u.cie_list, cie_list_entry) { + if (!marshall_cie(pdu, pduleft, cie)) + return FALSE; + } + return TRUE; + default: + return FALSE; + } +} + +static int marshall_packet_header(uint8_t **pdu, size_t *pduleft, struct nhrp_packet *packet) +{ + if (!marshall_binary(pdu, pduleft, sizeof(packet->hdr), &packet->hdr)) + return FALSE; + if (!marshall_nbma_address(pdu, pduleft, &packet->src_nbma_address)) + return FALSE; + if (!marshall_protocol_address(pdu, pduleft, &packet->src_protocol_address)) + return FALSE; + return marshall_protocol_address(pdu, pduleft, &packet->dst_protocol_address); +} + +static int marshall_packet(uint8_t *pdu, size_t pduleft, struct nhrp_packet *packet) +{ + uint8_t *pos = pdu; + struct nhrp_packet_header *phdr = (struct nhrp_packet_header *) pdu; + struct nhrp_extension_header neh; + int i, size; + + if (!marshall_packet_header(&pos, &pduleft, packet)) + return -1; + if (!marshall_payload(&pos, &pduleft, nhrp_packet_payload(packet, NHRP_PAYLOAD_TYPE_ANY))) + return -2; + + phdr->extension_offset = htons((int)(pos - pdu)); + for (i = 1; i < packet->num_extensions; i++) { + struct nhrp_extension_header *eh = (struct nhrp_extension_header *) pos; + + if (packet->extension_by_order[i].payload_type == NHRP_PAYLOAD_TYPE_NONE) + continue; + + neh.type = htons(packet->extension_by_order[i].extension_type); + neh.length = 0; + + if (!marshall_binary(&pos, &pduleft, sizeof(neh), &neh)) + return -3; + if (!marshall_payload(&pos, &pduleft, &packet->extension_by_order[i])) + return -4; + eh->length = htons((pos - (uint8_t *) eh) - sizeof(neh)); + } + neh.type = htons(NHRP_EXTENSION_END | NHRP_EXTENSION_FLAG_COMPULSORY); + neh.length = 0; + if (!marshall_binary(&pos, &pduleft, sizeof(neh), &neh)) + return -5; + + /* Cisco is seriously brain damaged. It needs some extra garbage + * at the end of error indication or it'll barf out spurious errors. */ + if (packet->hdr.type == NHRP_PACKET_ERROR_INDICATION && + pduleft >= 0x10) { + memset(pos, 0, 0x10); + pos += 0x10; + pduleft -= 0x10; + } + + size = (int)(pos - pdu); + phdr->packet_size = htons(size); + phdr->checksum = 0; + phdr->src_nbma_address_len = packet->src_nbma_address.addr_len; + phdr->src_nbma_subaddress_len = packet->src_nbma_address.subaddr_len; + phdr->src_protocol_address_len = packet->src_protocol_address.addr_len; + phdr->dst_protocol_address_len = packet->dst_protocol_address.addr_len; + phdr->checksum = nhrp_calculate_checksum(pdu, size); + + return size; +} + +int nhrp_packet_route(struct nhrp_packet *packet) +{ + struct nhrp_address proto_nexthop, *src, *dst; + struct list_head *cielist = NULL; + struct nhrp_payload *payload; + struct nhrp_peer *peer; + char tmp[64]; + int r; + + if (packet->dst_iface == NULL) { + nhrp_error("nhrp_packet_route called without destination interface"); + return FALSE; + } + + if (packet_types[packet->hdr.type].type == NHRP_TYPE_REPLY) { + dst = &packet->src_protocol_address; + src = &packet->dst_protocol_address; + r = NHRP_EXTENSION_REVERSE_TRANSIT_NHS; + } else { + dst = &packet->dst_protocol_address; + src = &packet->src_protocol_address; + r = NHRP_EXTENSION_FORWARD_TRANSIT_NHS; + } + payload = nhrp_packet_extension(packet, + r | NHRP_EXTENSION_FLAG_NOCREATE, + NHRP_PAYLOAD_TYPE_CIE_LIST); + if (payload != NULL) + cielist = &payload->u.cie_list; + + if (packet->dst_peer != NULL) { + proto_nexthop = packet->dst_peer->next_hop_address; + } else { + proto_nexthop = *dst; + do { + peer = nhrp_peer_route_full( + packet->dst_iface, &proto_nexthop, 0, + NHRP_PEER_TYPEMASK_ROUTE_VIA_NHS, src, cielist); + if (peer == NULL || peer->type == NHRP_PEER_TYPE_NEGATIVE) { + nhrp_error("No peer entry for protocol address %s", + nhrp_address_format(&proto_nexthop, + sizeof(tmp), tmp)); + return FALSE; + } + if (peer->type != NHRP_PEER_TYPE_LOCAL_ROUTE) + break; + if (peer->next_hop_address.type == AF_UNSPEC) + break; + proto_nexthop = peer->next_hop_address; + } while (1); + + packet->dst_peer = nhrp_peer_get(peer); + } + + return TRUE; +} + +int nhrp_packet_marshall_and_send(struct nhrp_packet *packet) +{ + uint8_t pdu[MAX_PDU_SIZE]; + char tmp[4][64]; + int size; + + nhrp_debug("Sending packet %d, from: %s (nbma %s), to: %s (nbma %s)", + packet->hdr.type, + nhrp_address_format(&packet->src_protocol_address, + sizeof(tmp[0]), tmp[0]), + nhrp_address_format(&packet->src_nbma_address, + sizeof(tmp[1]), tmp[1]), + nhrp_address_format(&packet->dst_protocol_address, + sizeof(tmp[2]), tmp[2]), + nhrp_address_format(&packet->dst_peer->next_hop_address, + sizeof(tmp[3]), tmp[3])); + + size = marshall_packet(pdu, sizeof(pdu), packet); + if (size < 0) { + nhrp_error("Packet marshalling failed (r=%d)", size); + return FALSE; + } + + if (!kernel_send(pdu, size, packet->dst_iface, + &packet->dst_peer->next_hop_address)) + return FALSE; + + return TRUE; +} + +int nhrp_packet_route_and_send(struct nhrp_packet *packet) +{ + struct nhrp_payload *payload; + + if (packet->dst_peer == NULL || packet->dst_iface == NULL) { + if (!nhrp_packet_route(packet)) { + nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ADDRESS_UNREACHABLE, 0); + return TRUE; + } + } + + if (packet->src_nbma_address.addr_len == 0) + packet->src_nbma_address = packet->dst_peer->my_nbma_address; + if (packet->src_protocol_address.addr_len == 0) + packet->src_protocol_address = packet->dst_iface->protocol_address; + if (packet->hdr.afnum == AFNUM_RESERVED) + packet->hdr.afnum = packet->dst_peer->afnum; + if (packet->hdr.protocol_type == 0) + packet->hdr.protocol_type = packet->dst_peer->protocol_type; + + /* RFC2332 5.3.1 */ + payload = nhrp_packet_extension( + packet, NHRP_EXTENSION_RESPONDER_ADDRESS | + NHRP_EXTENSION_FLAG_COMPULSORY | NHRP_EXTENSION_FLAG_NOCREATE, + NHRP_PAYLOAD_TYPE_CIE_LIST); + if (packet_types[packet->hdr.type].type == NHRP_TYPE_REPLY && + (payload != NULL && list_empty(&payload->u.cie_list))) { + struct nhrp_cie *cie; + + cie = nhrp_cie_alloc(); + if (cie == NULL) + return FALSE; + + cie->hdr.holding_time = htons(packet->dst_iface->holding_time); + cie->nbma_address = packet->dst_peer->my_nbma_address; + cie->protocol_address = packet->dst_iface->protocol_address; + nhrp_payload_set_type(payload, NHRP_PAYLOAD_TYPE_CIE_LIST); + nhrp_payload_add_cie(payload, cie); + } + + /* RFC2332 5.3.4 - Authentication is always done pairwise on an NHRP + * hop-by-hop basis; i.e. regenerated at each hop. */ + payload = nhrp_packet_extension(packet, + NHRP_EXTENSION_AUTHENTICATION | + NHRP_EXTENSION_FLAG_COMPULSORY, + NHRP_PAYLOAD_TYPE_RAW); + nhrp_payload_free(payload); + if (packet->dst_iface->auth_token != NULL) + nhrp_payload_set_raw(payload, + nhrp_buffer_copy(packet->dst_iface->auth_token)); + + if (packet->dst_peer->type == NHRP_PEER_TYPE_LOCAL_ADDR) { + packet->src_iface = packet->dst_peer->interface; + return nhrp_packet_receive_local(packet); + } + + if (packet->dst_peer->flags & (NHRP_PEER_FLAG_UP | + NHRP_PEER_FLAG_LOWER_UP)) + return nhrp_packet_marshall_and_send(packet); + + if (packet->dst_peer->queued_packet != NULL) + nhrp_packet_put(packet->dst_peer->queued_packet); + packet->dst_peer->queued_packet = nhrp_packet_get(packet); + + return TRUE; +} + +int nhrp_packet_send(struct nhrp_packet *packet) +{ + struct nhrp_payload *payload; + struct nhrp_cie *cie; + + if (packet->dst_iface == NULL) { + if (!nhrp_packet_route(packet)) { + nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ADDRESS_UNREACHABLE, 0); + return TRUE; + } + } + + /* Cisco NAT extension CIE */ + if (packet_types[packet->hdr.type].type != NHRP_TYPE_INDICATION && + (packet->hdr.flags & NHRP_FLAG_REGISTRATION_NAT)) { + payload = nhrp_packet_extension(packet, NHRP_EXTENSION_NAT_ADDRESS, + NHRP_PAYLOAD_TYPE_CIE_LIST); + + if (packet->dst_iface->nat_cie.nbma_address.addr_len && + payload != NULL && list_empty(&payload->u.cie_list)) { + cie = nhrp_cie_alloc(); + if (cie != NULL) { + *cie = packet->dst_iface->nat_cie; + nhrp_cie_reset(cie); + nhrp_payload_add_cie(payload, cie); + } + } + } + + return nhrp_packet_route_and_send(packet); +} + +static void nhrp_packet_xmit_timeout_cb(struct ev_timer *w, int revents) +{ + struct nhrp_packet *packet = + container_of(w, struct nhrp_packet, timeout); + + list_del(&packet->request_list_entry); + + if (packet->dst_peer != NULL && + ++packet->retry < PACKET_RETRIES) { + nhrp_packet_marshall_and_send(packet); + + list_add(&packet->request_list_entry, &pending_requests); + } else { + ev_timer_stop(&packet->timeout); + if (packet->dst_peer == NULL) + nhrp_error("nhrp_packet_xmit_timeout: no destination peer!"); + if (packet->handler != NULL) + packet->handler(packet->handler_ctx, NULL); + nhrp_packet_dequeue(packet); + } +} + +int nhrp_packet_send_request(struct nhrp_packet *pkt, + void (*handler)(void *ctx, struct nhrp_packet *packet), + void *ctx) +{ + struct nhrp_packet *packet; + + packet = nhrp_packet_get(pkt); + + packet->retry = 0; + if (packet->hdr.u.request_id == constant_htonl(0)) { + request_id++; + packet->hdr.u.request_id = htonl(request_id); + } + + packet->handler = handler; + packet->handler_ctx = ctx; + list_add(&packet->request_list_entry, &pending_requests); + ev_timer_again(&packet->timeout); + + return nhrp_packet_send(packet); +} + +int nhrp_packet_send_error(struct nhrp_packet *error_packet, + uint16_t indication_code, uint16_t offset) +{ + struct nhrp_packet *p; + struct nhrp_payload *pl; + int r; + + /* RFC2332 5.2.7 Never generate errors about errors */ + if (error_packet->hdr.type == NHRP_PACKET_ERROR_INDICATION) + return TRUE; + + p = nhrp_packet_alloc(); + p->hdr = error_packet->hdr; + p->hdr.type = NHRP_PACKET_ERROR_INDICATION; + p->hdr.hop_count = 0; + p->hdr.u.error.code = indication_code; + p->hdr.u.error.offset = htons(offset); + p->dst_iface = error_packet->src_iface; + + if (packet_types[error_packet->hdr.type].type == NHRP_TYPE_REPLY) + p->dst_protocol_address = error_packet->dst_protocol_address; + else + p->dst_protocol_address = error_packet->src_protocol_address; + + pl = nhrp_packet_payload(p, NHRP_PAYLOAD_TYPE_RAW); + pl->u.raw = nhrp_buffer_alloc(error_packet->req_pdulen); + memcpy(pl->u.raw->data, error_packet->req_pdu, error_packet->req_pdulen); + + /* Standard extensions */ + nhrp_packet_extension(p, + NHRP_EXTENSION_FORWARD_TRANSIT_NHS | + NHRP_EXTENSION_FLAG_COMPULSORY, + NHRP_PAYLOAD_TYPE_CIE_LIST); + + if (p->dst_protocol_address.type == PF_UNSPEC) + r = nhrp_do_handle_error_indication(p, error_packet); + else + r = nhrp_packet_send(p); + + nhrp_packet_put(p); + + return r; +} + +int nhrp_packet_send_traffic(struct nhrp_interface *iface, + struct nhrp_address *nbma_src, + struct nhrp_address *protocol_src, + struct nhrp_address *protocol_dst, + int protocol_type, uint8_t *pdu, size_t pdulen) +{ + struct nhrp_rate_limit *rl; + struct nhrp_packet *p; + struct nhrp_payload *pl; + struct nhrp_peer *peer; + char tmp1[64], tmp2[64], tmp3[64], tmp4[64]; + int r; + + if (!(iface->flags & NHRP_INTERFACE_FLAG_REDIRECT)) + return FALSE; + + /* Are we serving the NBMA source */ + peer = nhrp_interface_find_peer(iface, nbma_src); + if (peer == NULL || peer->type != NHRP_PEER_TYPE_DYNAMIC) + return FALSE; + + rl = get_rate_limit(protocol_src, protocol_dst); + if (rl == NULL) + return FALSE; + + /* If silence period has elapsed, reset algorithm */ + if (ev_now() > rl->rate_last + RATE_LIMIT_SILENCE) + rl->rate_tokens = 0; + + /* Too many ignored redirects; just update time of last packet */ + if (rl->rate_tokens >= RATE_LIMIT_MAX_TOKENS) { + rl->rate_last = ev_now(); + return FALSE; + } + + /* Check for load limit; set rate_last to last sent redirect */ + if (rl->rate_tokens != 0 && + ev_now() < rl->rate_last + RATE_LIMIT_SEND_INTERVAL) + return FALSE; + + rl->rate_tokens++; + rl->rate_last = ev_now(); + + p = nhrp_packet_alloc(); + p->hdr = (struct nhrp_packet_header) { + .protocol_type = protocol_type, + .version = NHRP_VERSION_RFC2332, + .type = NHRP_PACKET_TRAFFIC_INDICATION, + .hop_count = 0, + }; + p->dst_protocol_address = *protocol_src; + + pl = nhrp_packet_payload(p, NHRP_PAYLOAD_TYPE_RAW); + pl->u.raw = nhrp_buffer_alloc(pdulen); + memcpy(pl->u.raw->data, pdu, pdulen); + + /* Standard extensions */ + nhrp_packet_extension(p, + NHRP_EXTENSION_FORWARD_TRANSIT_NHS | + NHRP_EXTENSION_FLAG_COMPULSORY, + NHRP_PAYLOAD_TYPE_CIE_LIST); + + nhrp_info("Sending Traffic Indication about packet from %s to %s (to %s/%s)", + nhrp_address_format(protocol_src, sizeof(tmp1), tmp1), + nhrp_address_format(protocol_dst, sizeof(tmp2), tmp2), + nhrp_address_format(&peer->protocol_address, sizeof(tmp3), tmp3), + nhrp_address_format(&peer->next_hop_address, sizeof(tmp4), tmp4)); + + p->dst_iface = iface; + p->dst_peer = nhrp_peer_get(peer); + r = nhrp_packet_send(p); + nhrp_packet_put(p); + + return r; +} + +void nhrp_packet_hook_request(int request, + int (*handler)(struct nhrp_packet *packet)) +{ + NHRP_BUG_ON(request < 0 || request >= ARRAY_SIZE(packet_types)); + NHRP_BUG_ON(packet_types[request].handler != NULL); + + packet_types[request].handler = handler; +} |