diff --git a/impl/src/layer2/connection.c b/impl/src/layer2/connection.c index cadd193..8b3ae33 100644 --- a/impl/src/layer2/connection.c +++ b/impl/src/layer2/connection.c @@ -527,3 +527,15 @@ result_t connection_maintain(connection_ctx_t *ctx) return OK; } + + +bool connection_has_ipv6_peer_address(connection_ctx_t *ctx, const uint8_t *address_to_check) +{ + for(size_t i = 0; i < 16; i++) { + if(address_to_check[i] != ctx->peer_ipv6_addr.s6_addr[i]) { + return false; + } + } + + return true; +} diff --git a/impl/src/layer2/connection.h b/impl/src/layer2/connection.h index 4e27370..99bdd87 100644 --- a/impl/src/layer2/connection.h +++ b/impl/src/layer2/connection.h @@ -191,4 +191,11 @@ bool connection_is_closed(const connection_ctx_t *ctx); */ result_t connection_maintain(connection_ctx_t *ctx); +/*!\brief Check if the given IPv6 peer address belongs to this connection. + * + * \param address_to_check Pointer to the IPv6 address to check. Must point to 16 bytes of data. + * \returns True if this address is handled by this connection, false otherwise. + */ +bool connection_has_ipv6_peer_address(connection_ctx_t *ctx, const uint8_t *address_to_check); + #endif // CONNECTION_H diff --git a/impl/src/layer2/connection_list.c b/impl/src/layer2/connection_list.c index 814a2f1..979cc83 100644 --- a/impl/src/layer2/connection_list.c +++ b/impl/src/layer2/connection_list.c @@ -180,6 +180,43 @@ result_t connection_list_delete_closed(connection_list_t *list) } +result_t connection_list_enqueue_packet(connection_list_t *list, uint8_t *data, size_t data_len) +{ + if(data_len < 40) { + // packet not large enough for an IPv6 header + LOG(LVL_DEBUG, "Packet size too small: %zu bytes given, 40 bytes needed.", data_len); + return ERR_INVALID_PARAM; + } + + uint8_t version = data[0] >> 4; + if(version != 6) { + LOG(LVL_DEBUG, "IP version (%i) is not 6.", version); + return ERR_INVALID_PARAM; + } + + uint8_t *dest_addr = data + 24; + + // search the list for the destination address + connection_list_entry_t *ptr = list->head; + + while(ptr) { + if(connection_has_ipv6_peer_address(&ptr->connection, dest_addr)) { + // found it! + break; + } + + ptr = ptr->next; + } + + if(!ptr) { + // address not found in any connection + return ERR_INVALID_ADDRESS; + } + + return connection_enqueue_packet(&ptr->connection, data, data_len); +} + + bool connection_list_can_enqueue_packet(connection_list_t *list) { if(!list->head) { diff --git a/impl/src/layer2/connection_list.h b/impl/src/layer2/connection_list.h index 5f0154b..451b333 100644 --- a/impl/src/layer2/connection_list.h +++ b/impl/src/layer2/connection_list.h @@ -83,6 +83,23 @@ result_t connection_list_delete_head(connection_list_t *list); */ result_t connection_list_delete_closed(connection_list_t *list); +/*!\brief Insert a packet in the appropriate connection’s queue. + * + * The appropriate connection is selected by extracting the destination IP + * address from the packet and comparing it to the peer address of the + * connection. + * + * \param list Pointer to the connection list to operate on. + * \param packet Pointer to the raw packet data (no TUNTAP header!). + * \param packet_len Length of the packet data. + * + * \retval OK if the packet was inserted. + * \retval ERR_NO_MEM if the target queue was full. + * \retval ERR_INVALID_ADDRESS if the destination address does not match any connection. + * \retval ERR_INVALID_PARAM if the packet format is not supported. + */ +result_t connection_list_enqueue_packet(connection_list_t *list, uint8_t *data, size_t data_len); + /*!\brief Determine if a packet can be enqueued in all queues. * * \returns False if any queue is full, true otherwise. diff --git a/impl/src/layer2/digipeater.c b/impl/src/layer2/digipeater.c index b1c289a..36f2264 100644 --- a/impl/src/layer2/digipeater.c +++ b/impl/src/layer2/digipeater.c @@ -215,11 +215,20 @@ result_t digipeater_fill_packet_queues_from_tundev(digipeater_ctx_t *ctx, int tu break; } - LOG(LVL_DUMP, "TUN Flags: 0x%04x", *(uint16_t*)packetbuf); - LOG(LVL_DUMP, "TUN Proto: 0x%04x", *((uint16_t*)packetbuf + 1)); + uint16_t flags = *(uint16_t*)packetbuf; + uint16_t proto = *((uint16_t*)packetbuf + 1); + LOG(LVL_DUMP, "TUN Flags: 0x%04x", flags); + LOG(LVL_DUMP, "TUN Proto: 0x%04x", proto); - // FIXME: - //ERR_CHECK(connection_list_enqueue_packet(entry->connection, packetbuf, ret)); + switch(proto) { + case 0x86dd: // IPv6 + ERR_CHECK(connection_list_enqueue_packet(&ctx->conn_list, packetbuf, ret)); + break; + + default: + LOG(LVL_WARN, "Unsupported Protocol 0x%04x. Packet dropped.", proto); + continue; + } } }