From c44c229660e7d870982b60d6fe177c259326f830 Mon Sep 17 00:00:00 2001
From: Jukka Rissanen <jukka.rissanen@linux.intel.com>
Date: Tue, 17 May 2016 13:03:02 +0300
Subject: [PATCH] net: Receive IPv6 packet

Check if we have received IPv6 packet and call a handler
function to process it.

Change-Id: I1226729685ae2a805128b587e2a37e0016d53ecc
Signed-off-by: Jukka Rissanen <jukka.rissanen@linux.intel.com>
---
 include/net/yaip/net_ip.h | 11 ++++++++
 net/yaip/net_core.c       | 55 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 66 insertions(+)

diff --git a/include/net/yaip/net_ip.h b/include/net/yaip/net_ip.h
index 9b6380fd6fe..e4ad3205efa 100644
--- a/include/net/yaip/net_ip.h
+++ b/include/net/yaip/net_ip.h
@@ -130,6 +130,17 @@ enum net_addr_state {
 	NET_ADDR_DEPRECATED,
 };
 
+struct net_ipv6_hdr {
+	uint8_t vtc;
+	uint8_t tcflow;
+	uint16_t flow;
+	uint8_t len[2];
+	uint8_t nexthdr;
+	uint8_t hop_limit;
+	struct in6_addr src;
+	struct in6_addr dst;
+} __attribute__((__packed__));
+
 #define NET_UDPH_LEN	8			/* Size of UDP header */
 #define NET_TCPH_LEN	20			/* Size of TCP header */
 #define NET_ICMPH_LEN	4			/* Size of ICMP header */
diff --git a/net/yaip/net_core.c b/net/yaip/net_core.c
index 8d1b29ca1b6..ddbed21e1a2 100644
--- a/net/yaip/net_core.c
+++ b/net/yaip/net_core.c
@@ -124,8 +124,63 @@ static inline void stats(void)
 #define net_print_statistics()
 #endif /* CONFIG_NET_STATISTICS */
 
+#if defined(CONFIG_NET_IPV6)
+static inline enum net_verdict process_ipv6_pkt(struct net_buf *buf)
+{
+	struct net_ipv6_hdr *hdr = NET_IPV6_BUF(buf);
+	int real_len = net_buf_frags_len(buf);
+	int pkt_len = (hdr->len[0] << 8) + hdr->len[1] + sizeof(*hdr);
+
+	if (real_len > pkt_len) {
+		NET_DBG("IPv6 adjust pkt len to %d (was %d)",
+			pkt_len, real_len);
+		net_buf_frag_last(buf)->len -= real_len - pkt_len;
+		real_len -= pkt_len;
+	} else if (real_len < pkt_len) {
+		NET_DBG("IPv6 packet size %d buf len %d", pkt_len, real_len);
+		NET_STATS(++net_stats.ipv6.drop);
+		return NET_DROP;
+	}
+
+#if NET_DEBUG > 0
+	do {
+		char out[sizeof("xxxx:xxxx:xxxx:xxxx:xxxx:xxxx")];
+		snprintf(out, sizeof(out), net_sprint_ipv6_addr(&hdr->dst));
+		NET_DBG("IPv6 packet len %d received from %s to %s",
+			real_len, net_sprint_ipv6_addr(&hdr->src), out);
+	} while (0);
+#endif /* NET_DEBUG > 0 */
+
+	return NET_DROP;
+}
+#endif /* CONFIG_NET_IPV6 */
+
 static inline enum net_verdict process_data(struct net_buf *buf)
 {
+	/* If there is no data, then drop the packet. Also if
+	 * the buffer is wrong type, then also drop the packet.
+	 * The first buffer needs to have user data part that
+	 * contains user data. The rest of the fragments should
+	 * be data fragments without user data.
+	 */
+	if (!buf->frags || !buf->user_data_size) {
+		NET_STATS(++net_stats.processing_error);
+		return NET_DROP;
+	}
+
+	/* IP version and header length. */
+	switch (NET_IPV6_BUF(buf)->vtc & 0xf0) {
+#if defined(CONFIG_NET_IPV6)
+	case 0x60:
+		NET_STATS(++net_stats.ipv6.recv);
+		net_nbuf_family(buf) = PF_INET6;
+		return process_ipv6_pkt(buf);
+#endif
+	}
+
+drop:
+	NET_STATS(++net_stats.ip_errors.protoerr);
+	NET_STATS(++net_stats.ip_errors.vhlerr);
 	return NET_DROP;
 }