digipeater: enqueue packets in the correct connection queue

So far, only IPv6 is supported.

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;
+}

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

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) {

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.

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;
+			}
 		}
 	}