1 files changed, 302 insertions, 0 deletions

diff --git a/VNFs/DPPD-PROX/prox_ipv6.c b/VNFs/DPPD-PROX/prox_ipv6.c
new file mode 100644
index 00000000..9425f4a0
--- /dev/null
+++ b/VNFs/DPPD-PROX/prox_ipv6.c
@@ -0,0 +1,302 @@
+/*
+// Copyright (c) 2020 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+*/
+
+#include "task_base.h"
+#include "handle_master.h"
+#include "prox_cfg.h"
+#include "prox_ipv6.h"
+
+struct ipv6_addr null_addr = {{0}};
+char ip6_str[40]; // 8 blocks of 2 bytes (4 char) + 1x ":" between blocks
+
+void set_mcast_mac_from_ipv6(prox_rte_ether_addr *mac, struct ipv6_addr *ipv6_addr)
+{
+	mac->addr_bytes[0] = 0x33;
+	mac->addr_bytes[1] = 0x33;
+	memcpy(((uint32_t *)&mac->addr_bytes[2]), (uint32_t *)(&ipv6_addr->bytes[12]), sizeof(uint32_t));
+}
+
+// Note that this function is not Mthread safe and would result in garbage if called simultaneously from multiple threads
+// This function is however only used for debugging, printing errors...
+char *IP6_Canonical(struct ipv6_addr *addr)
+{
+	uint8_t *a = (uint8_t *)addr;
+	char *ptr = ip6_str;
+	int field = -1, len = 0, stored_field = 0, stored_len = 0;
+
+	// Find longest run of consecutive 16-bit 0 fields
+	for (int i = 0; i < 8; i++) {
+		if (((int)a[i * 2] == 0) && ((int)a[i * 2 + 1] == 0)) {
+			len++;
+			if (field == -1)
+				field = i;	// Store where the first 0 field started
+		} else {
+			if (len > stored_len) {
+				// the longest run of consecutive 16-bit 0 fields MUST be shortened
+				stored_len = len;
+				stored_field = field;
+			}
+			len = 0;
+			field = -1;
+		}
+	}
+	if (len > stored_len) {
+		// the longest run of consecutive 16-bit 0 fields MUST be shortened
+		stored_len = len;
+		stored_field = field;
+	}
+	if (stored_len <= 1) {
+		// The symbol "::" MUST NOT be used to shorten just one 16-bit 0 field.
+		stored_len = 0;
+		stored_field = -1;
+	}
+	for (int i = 0; i < 8; i++) {
+		if (i == stored_field) {
+			sprintf(ptr, ":");
+			ptr++;
+			if (i == 0) {
+				sprintf(ptr, ":");
+				ptr++;
+			}
+			i +=stored_len - 1;	// ++ done in for loop
+			continue;
+		}
+		if ((int)a[i * 2] & 0xF0) {
+			sprintf(ptr, "%02x%02x", (int)a[i * 2], (int)a[i * 2 + 1]);
+			ptr+=4;
+		} else if ((int)a[i * 2] & 0x0F) {
+			sprintf(ptr, "%x%02x", (int)a[i * 2] >> 4, (int)a[i * 2] + 1);
+			ptr+=3;
+		} else if ((int)a[i * 2 + 1] & 0xF0) {
+			sprintf(ptr, "%02x", (int)a[i * 2 + 1]);
+			ptr+=2;
+		} else {
+			sprintf(ptr, "%x", ((int)a[i * 2 + 1]) & 0xF);
+			ptr++;
+		}
+		if (i != 7) {
+			sprintf(ptr, ":");
+			ptr++;
+		}
+	}
+	return ip6_str;
+}
+
+void set_link_local(struct ipv6_addr *ipv6_addr)
+{
+	ipv6_addr->bytes[0] = 0xfe;
+	ipv6_addr->bytes[1] = 0x80;
+}
+
+// Create Extended Unique Identifier (RFC 2373)
+// Store it in LSB of IPv6 address
+void set_EUI(struct ipv6_addr *ipv6_addr, prox_rte_ether_addr *mac)
+{
+	memcpy(&ipv6_addr->bytes[8], mac, 3);						// Copy first 3 bytes of MAC
+	ipv6_addr->bytes[8] = ipv6_addr->bytes[8] ^ 0x02; 				// Invert Universal/local bit
+	ipv6_addr->bytes[11] = 0xff;							// Next 2 bytes are 0xfffe
+	ipv6_addr->bytes[12] = 0xfe;
+	memcpy(&ipv6_addr->bytes[13], &mac->addr_bytes[3], 3);				// Copy last 3 bytes
+	// plog_info("mac = "MAC_BYTES_FMT", eui = "IPv6_BYTES_FMT"\n", MAC_BYTES(mac->addr_bytes), IPv6_BYTES(ipv6_addr->bytes));
+}
+
+void create_mac_from_EUI(struct ipv6_addr *ipv6_addr, prox_rte_ether_addr *mac)
+{
+	memcpy(mac, &ipv6_addr->bytes[8], 3);
+	mac->addr_bytes[0] = mac->addr_bytes[0] ^ 0x02;
+	memcpy(&mac->addr_bytes[3], &ipv6_addr->bytes[13], 3);
+}
+void build_router_advertisement(struct rte_mbuf *mbuf, prox_rte_ether_addr *s_addr, struct ipv6_addr *ipv6_s_addr, struct ipv6_addr *router_prefix)
+{
+	prox_rte_ether_hdr *peth = rte_pktmbuf_mtod(mbuf, prox_rte_ether_hdr *);
+	init_mbuf_seg(mbuf);
+	mbuf->ol_flags &= ~(PKT_TX_IP_CKSUM|PKT_TX_UDP_CKSUM);  // Software calculates the checksum
+
+	memcpy(peth->d_addr.addr_bytes, &prox_cfg.all_nodes_mac_addr, sizeof(prox_rte_ether_addr));
+	memcpy(peth->s_addr.addr_bytes, s_addr, sizeof(prox_rte_ether_addr));
+	peth->ether_type = ETYPE_IPv6;
+
+	prox_rte_ipv6_hdr *ipv6_hdr = (prox_rte_ipv6_hdr *)(peth + 1);
+	ipv6_hdr->vtc_flow = 0x00000060;
+	ipv6_hdr->payload_len = rte_cpu_to_be_16(sizeof(struct icmpv6_RA) + sizeof(struct icmpv6_prefix_option));
+	ipv6_hdr->proto = ICMPv6;
+	ipv6_hdr->hop_limits = 255;
+	memcpy(ipv6_hdr->src_addr, ipv6_s_addr, sizeof(struct ipv6_addr));	// 0 = "Unspecified address" if unknown
+	memcpy(ipv6_hdr->dst_addr, &prox_cfg.all_nodes_ipv6_mcast_addr, sizeof(struct ipv6_addr));
+
+	struct icmpv6_RA *router_advertisement = (struct icmpv6_RA *)(ipv6_hdr + 1);
+	router_advertisement->type = ICMPv6_RA;
+	router_advertisement->code = 0;
+	router_advertisement->hop_limit = 255;
+	router_advertisement->bits = 0;	// M and O bits set to 0 => no dhcpv6
+	router_advertisement->router_lifespan = rte_cpu_to_be_16(9000);		// 9000 sec
+	router_advertisement->reachable_timeout = rte_cpu_to_be_32(30000);	// 1 sec
+	router_advertisement->retrans_timeout = rte_cpu_to_be_32(1000);       // 30 sec
+
+	struct icmpv6_option *option = &router_advertisement->options;
+	option->type = ICMPv6_source_link_layer_address;
+	option->length = 1;	// 8 bytes
+	memcpy(&option->data, s_addr, sizeof(prox_rte_ether_addr));
+
+	struct icmpv6_prefix_option *prefix_option = (struct icmpv6_prefix_option *)(option + 1);
+	prefix_option->type = ICMPv6_prefix_information;
+	prefix_option->length = 4;		// 32 bytes
+	prefix_option->prefix_length = 64;	// 64 bits in prefix
+	prefix_option->flag = 0xc0;		// on-link flag & autonamous address-configuration flag are set
+	prefix_option->valid_lifetime = rte_cpu_to_be_32(86400);	// 1 day
+	prefix_option->preferred_lifetime = rte_cpu_to_be_32(43200);	// 12 hours
+	prefix_option->reserved = 0;
+	memcpy(&prefix_option->prefix, router_prefix, sizeof(struct ipv6_addr));
+	// Could Add MTU Option
+	router_advertisement->checksum = 0;
+	router_advertisement->checksum = rte_ipv6_udptcp_cksum(ipv6_hdr, router_advertisement);
+
+	uint16_t pktlen = rte_be_to_cpu_16(ipv6_hdr->payload_len) + sizeof(prox_rte_ipv6_hdr) + sizeof(prox_rte_ether_hdr);
+	rte_pktmbuf_pkt_len(mbuf) = pktlen;
+	rte_pktmbuf_data_len(mbuf) = pktlen;
+}
+
+void build_router_sollicitation(struct rte_mbuf *mbuf, prox_rte_ether_addr *s_addr, struct ipv6_addr *ipv6_s_addr)
+{
+	prox_rte_ether_hdr *peth = rte_pktmbuf_mtod(mbuf, prox_rte_ether_hdr *);
+
+	init_mbuf_seg(mbuf);
+	mbuf->ol_flags &= ~(PKT_TX_IP_CKSUM|PKT_TX_UDP_CKSUM);  // Software calculates the checksum
+
+	memcpy(peth->d_addr.addr_bytes, &prox_cfg.all_routers_mac_addr, sizeof(prox_rte_ether_addr));
+	memcpy(peth->s_addr.addr_bytes, s_addr, sizeof(prox_rte_ether_addr));
+	peth->ether_type = ETYPE_IPv6;
+
+	prox_rte_ipv6_hdr *ipv6_hdr = (prox_rte_ipv6_hdr *)(peth + 1);
+	ipv6_hdr->vtc_flow = 0x00000060;
+	ipv6_hdr->payload_len = rte_cpu_to_be_16(sizeof(struct icmpv6_RS));
+	ipv6_hdr->proto = ICMPv6;
+	ipv6_hdr->hop_limits = 255;
+	memcpy(ipv6_hdr->src_addr, ipv6_s_addr, sizeof(struct ipv6_addr));	// 0 = "Unspecified address" if unknown
+	memcpy(ipv6_hdr->dst_addr, &prox_cfg.all_routers_ipv6_mcast_addr, sizeof(struct ipv6_addr));
+
+	struct icmpv6_RS *router_sollicitation = (struct icmpv6_RS *)(ipv6_hdr + 1);
+	router_sollicitation->type = ICMPv6_RS;
+	router_sollicitation->code = 0;
+	router_sollicitation->options.type = ICMPv6_source_link_layer_address;
+	router_sollicitation->options.length = 1;	// 8 bytes
+	memcpy(&router_sollicitation->options.data, s_addr, sizeof(prox_rte_ether_addr));
+
+	router_sollicitation->checksum = 0;
+	router_sollicitation->checksum = rte_ipv6_udptcp_cksum(ipv6_hdr, router_sollicitation);
+	uint16_t pktlen = rte_be_to_cpu_16(ipv6_hdr->payload_len) + sizeof(prox_rte_ipv6_hdr) + sizeof(prox_rte_ether_hdr);
+	rte_pktmbuf_pkt_len(mbuf) = pktlen;
+	rte_pktmbuf_data_len(mbuf) = pktlen;
+}
+
+void build_neighbour_sollicitation(struct rte_mbuf *mbuf, prox_rte_ether_addr *s_addr, struct ipv6_addr *dst, struct ipv6_addr *src)
+{
+	prox_rte_ether_hdr *peth = rte_pktmbuf_mtod(mbuf, prox_rte_ether_hdr *);
+	prox_rte_ether_addr mac_dst;
+	set_mcast_mac_from_ipv6(&mac_dst, dst);
+
+	init_mbuf_seg(mbuf);
+	mbuf->ol_flags &= ~(PKT_TX_IP_CKSUM|PKT_TX_UDP_CKSUM);  // Software calculates the checksum
+
+	memcpy(peth->d_addr.addr_bytes, &mac_dst, sizeof(prox_rte_ether_addr));
+	memcpy(peth->s_addr.addr_bytes, s_addr, sizeof(prox_rte_ether_addr));
+	peth->ether_type = ETYPE_IPv6;
+
+	prox_rte_ipv6_hdr *ipv6_hdr = (prox_rte_ipv6_hdr *)(peth + 1);
+	ipv6_hdr->vtc_flow = 0x00000060;
+	ipv6_hdr->payload_len = rte_cpu_to_be_16(sizeof(struct icmpv6_NS));
+	ipv6_hdr->proto = ICMPv6;
+	ipv6_hdr->hop_limits = 255;
+	memcpy(ipv6_hdr->src_addr, src, 16);
+	memcpy(ipv6_hdr->dst_addr, dst, 16);
+
+	struct icmpv6_NS *neighbour_sollicitation = (struct icmpv6_NS *)(ipv6_hdr + 1);
+	neighbour_sollicitation->type = ICMPv6_NS;
+	neighbour_sollicitation->code = 0;
+	neighbour_sollicitation->reserved = 0;
+	memcpy(&neighbour_sollicitation->target_address, dst, sizeof(struct ipv6_addr));
+	neighbour_sollicitation->options.type = ICMPv6_source_link_layer_address;
+	neighbour_sollicitation->options.length = 1;	// 8 bytes
+	memcpy(&neighbour_sollicitation->options.data, s_addr, sizeof(prox_rte_ether_addr));
+	neighbour_sollicitation->checksum = 0;
+	neighbour_sollicitation->checksum = rte_ipv6_udptcp_cksum(ipv6_hdr, neighbour_sollicitation);
+
+	uint16_t pktlen = rte_be_to_cpu_16(ipv6_hdr->payload_len) + sizeof(prox_rte_ipv6_hdr) + sizeof(prox_rte_ether_hdr);
+	rte_pktmbuf_pkt_len(mbuf) = pktlen;
+	rte_pktmbuf_data_len(mbuf) = pktlen;
+}
+
+void build_neighbour_advertisement(struct task_base *tbase, struct rte_mbuf *mbuf, prox_rte_ether_addr *target, struct ipv6_addr *src_ipv6_addr, int sollicited)
+{
+	struct task_master *task = (struct task_master *)tbase;
+	prox_rte_ether_hdr *peth = rte_pktmbuf_mtod(mbuf, prox_rte_ether_hdr *);
+	prox_rte_ipv6_hdr *ipv6_hdr = (prox_rte_ipv6_hdr *)(peth + 1);
+
+	uint8_t port_id = get_port(mbuf);
+
+	init_mbuf_seg(mbuf);
+	mbuf->ol_flags &= ~(PKT_TX_IP_CKSUM|PKT_TX_UDP_CKSUM);  // Software calculates the checksum
+
+	// If source mac is null, use all_nodes_mac_addr.
+	if ((!sollicited) || (memcmp(peth->s_addr.addr_bytes, &null_addr, sizeof(struct ipv6_addr)) == 0)) {
+		memcpy(peth->d_addr.addr_bytes, &prox_cfg.all_nodes_mac_addr, sizeof(prox_rte_ether_addr));
+		memcpy(ipv6_hdr->dst_addr, &prox_cfg.all_nodes_ipv6_mcast_addr, sizeof(struct ipv6_addr));
+	} else {
+		memcpy(peth->d_addr.addr_bytes, peth->s_addr.addr_bytes, sizeof(prox_rte_ether_addr));
+		memcpy(ipv6_hdr->dst_addr, ipv6_hdr->src_addr, sizeof(struct ipv6_addr));
+	}
+
+	memcpy(peth->s_addr.addr_bytes, &task->internal_port_table[port_id].mac, sizeof(prox_rte_ether_addr));
+	peth->ether_type = ETYPE_IPv6;
+
+	ipv6_hdr->vtc_flow = 0x00000060;
+	ipv6_hdr->payload_len = rte_cpu_to_be_16(sizeof(struct icmpv6_NA));
+	ipv6_hdr->proto = ICMPv6;
+	ipv6_hdr->hop_limits = 255;
+	memcpy(ipv6_hdr->src_addr, src_ipv6_addr, sizeof(struct ipv6_addr));
+
+	struct icmpv6_NA *neighbour_advertisement = (struct icmpv6_NA *)(ipv6_hdr + 1);
+	neighbour_advertisement->type = ICMPv6_NA;
+	neighbour_advertisement->code = 0;
+	neighbour_advertisement->reserved = 0;
+	if (task->internal_port_table[port_id].flags & IPV6_ROUTER)
+		neighbour_advertisement->bits = 0xC0; // R+S bit set
+	else
+		neighbour_advertisement->bits = 0x40; // S bit set
+	if (!sollicited) {
+		memcpy(&neighbour_advertisement->destination_address, src_ipv6_addr, sizeof(struct ipv6_addr));
+		neighbour_advertisement->bits &= 0xBF; // Clear S bit
+		neighbour_advertisement->bits |= 0x20; // Overide bit
+	}
+	// else neighbour_advertisement->destination_address is already set to neighbour_sollicitation->target_address
+
+	struct icmpv6_option *option = &neighbour_advertisement->options;
+	// Do not think this is necessary
+	// option->type = ICMPv6_source_link_layer_address;
+	// option->length = 1;	// 8 bytes
+	// memcpy(&option->data, &task->internal_port_table[port_id].mac, sizeof(prox_rte_ether_addr));
+
+	// option = option + 1;
+	option->type = ICMPv6_target_link_layer_address;
+	option->length = 1;	// 8 bytes
+	memcpy(&option->data, target, sizeof(prox_rte_ether_addr));
+
+	neighbour_advertisement->checksum = 0;
+	neighbour_advertisement->checksum = rte_ipv6_udptcp_cksum(ipv6_hdr, neighbour_advertisement);
+	uint16_t pktlen = rte_be_to_cpu_16(ipv6_hdr->payload_len) + sizeof(prox_rte_ipv6_hdr) + sizeof(prox_rte_ether_hdr);
+	rte_pktmbuf_pkt_len(mbuf) = pktlen;
+	rte_pktmbuf_data_len(mbuf) = pktlen;
+}