Diff: ip6/ip6.cpp

Revision:

14:e75a59c1123d

Parent:

13:9cd54f7db57a

Child:

15:6ca6778168b1

--- a/ip6/ip6.cpp	Mon May 01 18:20:55 2017 +0000
+++ b/ip6/ip6.cpp	Fri May 05 17:44:16 2017 +0000
@@ -6,12 +6,51 @@
 #include      "ar.h"
 #include   "slaac.h"
 #include     "eth.h"
+#include      "ip.h"
 #include     "ip6.h"
 #include     "ndp.h"
 #include      "io.h"
 
 #define DEBUG false
 
+
+static void addHexNibble(bool* pAdded, int number, int index, char** pp)
+{
+    int nibble = number;
+    if (index) nibble >>= 4;
+    nibble &= 0xF;
+    
+    if (nibble || *pAdded)
+    {
+        **pp = nibble < 10 ? nibble + '0' : nibble - 10 + 'a';
+        *pp += 1;
+        *pAdded = true;
+    }
+}
+int Ip6AddressToString(char* pIp, int size, char* pText)
+{
+    char* pIpE = pIp + 16;
+    char* p = pText;
+    while (true)
+    {
+        bool added = false;
+        if (*pIp || *(pIp + 1))
+        {
+            if (p > pText + size - 2) break;  addHexNibble(&added, *(pIp + 0), 1, &p);
+            if (p > pText + size - 2) break;  addHexNibble(&added, *(pIp + 0), 0, &p);
+            if (p > pText + size - 2) break;  addHexNibble(&added, *(pIp + 1), 1, &p);
+            if (p > pText + size - 2) break;  addHexNibble(&added, *(pIp + 1), 0, &p);
+        }
+        
+        pIp += 2;
+        if (pIp >= pIpE) break;
+        
+        if (p > pText + size - 2) break; *p++ = ':';
+    }
+    *p = 0;
+    return p - pText;
+}
+
 char Ip6AllNodes  [] = {0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01};
 char Ip6AllRouters[] = {0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02};
 char Ip6Mdns      [] = {0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfb};
@@ -48,8 +87,8 @@
 static int      dataLength;
 static uint8_t    protocol;
 static uint8_t    hoplimit;
-static char     pSrcIp[16];
-static char     pDstIp[16];
+char     Ip6Src[16];
+char     Ip6Dst[16];
 static void*         pData;
 
 static void readHeader(struct header * pHeader)
@@ -58,8 +97,8 @@
     dataLength = NetToHost16(pHeader->dataLength);
     protocol   =             pHeader->protocol;
     hoplimit   =             pHeader->hoplimit;
-              memcpy(pSrcIp, pHeader->src, 16);
-              memcpy(pDstIp, pHeader->dst, 16);
+              memcpy(Ip6Src, pHeader->src, 16);
+              memcpy(Ip6Dst, pHeader->dst, 16);
     pData      = (char*)pHeader + HEADER_LENGTH;
 }
 static void writeHeader(struct header * pHeader)
@@ -67,8 +106,8 @@
            pHeader->versionTrafficFlow  = version << 4;
            pHeader->protocol            = protocol;
            pHeader->hoplimit            = 255;
-    memcpy(pHeader->dst, pDstIp, 16);
-    memcpy(pHeader->src, pSrcIp, 16);
+    memcpy(pHeader->dst, Ip6Dst, 16);
+    memcpy(pHeader->src, Ip6Src, 16);
            pHeader->dataLength          = NetToHost16(dataLength);
 }
 
@@ -79,11 +118,11 @@
     LogF("  Version           %d\r\n", version);
     LogF("  Payload length    %d\r\n", dataLength);
     LogF("  Hop limit         %d\r\n", hoplimit);
-    NetProtocolToString(protocol, sizeof(text), text);
+    IpProtocolToString(protocol, sizeof(text), text);
     LogF("  Protocol          %s\r\n", text);
-    NetIp6AddressToString(pSrcIp, sizeof(text), text);
+    Ip6AddressToString(Ip6Src, sizeof(text), text);
     LogF("  Source IP         %s\r\n", text);
-    NetIp6AddressToString(pDstIp, sizeof(text), text);
+    Ip6AddressToString(Ip6Dst, sizeof(text), text);
     LogF("  Destination IP    %s\r\n", text);
 }
 
@@ -128,26 +167,32 @@
     struct header * pHeader = (header*)pPacket;
     readHeader(pHeader);
     
-    bool isMe        = getIsSame(pDstIp, SlaacLinkLocalIp);
-    bool isMulticast = pDstIp[0] == 0xFF;
-    bool isSolicited = getIsSolicited(pDstIp);
-    bool isGroup     = isSolicited && getIsSameGroup(pDstIp, SlaacLinkLocalIp);
+    bool isMe        = getIsSame(Ip6Dst, SlaacLinkLocalIp);
+    bool isMulticast = Ip6Dst[0] == 0xFF;
+    bool isSolicited = getIsSolicited(Ip6Dst);
+    bool isGroup     = isSolicited && getIsSameGroup(Ip6Dst, SlaacLinkLocalIp);
     
     bool doIt = isMe || (isMulticast && !isSolicited) || isGroup;
     
-    if (!doIt) return DO_NOTHING;
+    if (!doIt)
+    {
+        char text[100];
+        Ip6AddressToString(Ip6Dst, sizeof(text), text);
+        LogTimeF("IP6 filtered out ip %s \r\n", text);
+        return DO_NOTHING;
+    }
     
-    ArAdd6(pSrcMac, pSrcIp);
+    ArAdd6(pSrcMac, Ip6Src);
     
     if (DEBUG) logHeader("IP6 packet received");
 
     int action = DO_NOTHING;
     switch (protocol)
     {
-        case HOPOPT: action = DO_NOTHING;                                                    break;
-        case ICMP6:  action = Icmp6HandleReceivedPacket(pSrcIp, pDstIp, &dataLength, pData); break;
-        case UDP:    action =  Udp6HandleReceivedPacket(pSrcIp, pDstIp, &dataLength, pData); break;
-        case TCP:    action =  Tcp6HandleReceivedPacket(pSrcIp, pDstIp, &dataLength, pData); break;        
+        case HOPOPT: action = DO_NOTHING;                                                          break;
+        case ICMP6:  action = Icmp6HandleReceivedPacket(Ip6Src, Ip6Dst, &dataLength, pData); break;
+        case UDP:    action =  Udp6HandleReceivedPacket(Ip6Src, Ip6Dst, &dataLength, pData); break;
+        case TCP:    action =  Tcp6HandleReceivedPacket(Ip6Src, Ip6Dst, &dataLength, pData); break;        
         default:
             logHeader("IP6 packet unhandled");
             return DO_NOTHING;
@@ -174,13 +219,13 @@
     int action = DO_NOTHING;
     if (action == DO_NOTHING)
     {
-        action = Icmp6PollForPacketToSend(pData, &dataLength, pSrcIp, pDstIp);
+        action = Icmp6PollForPacketToSend(pData, &dataLength, Ip6Src, Ip6Dst);
         protocol = ICMP6;
     }
     
     if (action == DO_NOTHING)
     {
-        action = Udp6PollForPacketToSend(pData, &dataLength, pSrcIp, pDstIp);
+        action = Udp6PollForPacketToSend(pData, &dataLength, Ip6Src, Ip6Dst);
         protocol = UDP;
     }
     if (!action) return DO_NOTHING;
@@ -189,7 +234,7 @@
         case UNICAST:
         case UNICAST_DNS:
         case UNICAST_DHCP:
-            ArRev6(pDstIp, pDstMac);             //Make the remote MAC from NP
+            ArRev6(Ip6Dst, pDstMac);             //Make the remote MAC from NP
             break;
     }