Diff: ip6/ip6.cpp

Revision:

10:f0854784e960

Child:

11:c051adb70c5a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ip6/ip6.cpp	Sun Apr 16 14:21:55 2017 +0000
@@ -0,0 +1,196 @@
+#include  "mbed.h"
+#include   "log.h"
+#include   "net.h"
+#include "icmp6.h"
+#include  "udp6.h"
+#include  "tcp6.h"
+#include    "ar.h"
+#include  "dhcp.h"
+#include "slaac.h"
+#include   "eth.h"
+#include   "ip6.h"
+
+#define HEADER_LENGTH 40
+
+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};
+
+char Ip6Llmnr[] = {     0xff, 0x02, 0x00, 0x00,
+                        0x00, 0x00, 0x00, 0x00,
+                        0x00, 0x00, 0x00, 0x00,
+                        0x00, 0x01, 0x00, 0x03};
+
+__packed struct header
+{
+    uint32_t versionTrafficFlow;
+    uint16_t dataLength;
+    uint8_t  protocol;
+    uint8_t  hoplimit;
+    char     src[16];
+    char     dst[16];
+};
+static bool getIsSolicited(char* p)
+{
+    if (*p++ != 0xff) return false;
+    if (*p++ != 0x02) return false;
+    
+    if (*p++ != 0x00) return false;
+    if (*p++ != 0x00) return false;
+    
+    if (*p++ != 0x00) return false;
+    if (*p++ != 0x00) return false;
+    
+    if (*p++ != 0x00) return false;
+    if (*p++ != 0x00) return false;
+    
+    if (*p++ != 0x00) return false;
+    if (*p++ != 0x00) return false;
+    
+    if (*p++ != 0x00) return false;
+    if (*p++ != 0x01) return false;
+    
+    if (*p++ != 0xff) return false;
+    
+    return true;
+}
+static bool getIsSame(char* pA, char* pB)
+{
+    return memcmp(pA, pB, 16) == 0;
+}
+static bool getIsSameGroup(char* pA, char* pB)
+{
+    pA += 13;
+    pB += 13;
+    if (*pA++ != *pB++) return false;
+    if (*pA++ != *pB++) return false;
+    return *pA == *pB;
+}
+int Ip6HandleReceivedPacket(char* pSrcMac, void* pPacket, int* pSize, char* pDstMac)
+{
+    struct header * pHeader = (header*)pPacket;
+    
+    uint8_t     version  =            (pHeader->versionTrafficFlow >> 4) & 0xF;
+    int      dataLength  = NetToHost16(pHeader->dataLength);
+    uint8_t    protocol  =             pHeader->protocol;
+    uint8_t    hoplimit  =             pHeader->hoplimit;
+    char        src[16];   memcpy(src, pHeader->src, 16);
+    char        dst[16];   memcpy(dst, pHeader->dst, 16);
+    void*          pData = (char*)pPacket + HEADER_LENGTH;
+    
+    bool isMe        = getIsSame(dst, SlaacLinkLocalIp);
+    bool isMulticast = dst[0] == 0xFF;
+    bool isSolicited = getIsSolicited(dst);
+    bool isGroup     = isSolicited && getIsSameGroup(dst, SlaacLinkLocalIp);
+    
+    bool doIt = isMe || (isMulticast && !isSolicited) || isGroup;
+    
+    if (!doIt) return DO_NOTHING;
+    
+    ArAdd6(pSrcMac, src);
+    int action = DO_NOTHING;
+    switch (protocol)
+    {
+        case ICMP6: action = Icmp6HandleReceivedPacket(src, dst, &dataLength, pData); break;
+        case UDP:   action =  Udp6HandleReceivedPacket(src, dst, &dataLength, pData); break;
+        case TCP:   action =  Tcp6HandleReceivedPacket(src, dst, &dataLength, pData); break;        
+        default:
+            char text[100];
+            LogTimeF("IP6 packet unhandled\r\n");
+            LogF("  Size              %d\r\n", *pSize);
+            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);
+            LogF("  Protocol          %s\r\n", text);
+            NetIp6AddressToString(src, sizeof(text), text);
+            LogF("  Source IP         %s\r\n", text);
+            NetIp6AddressToString(dst, sizeof(text), text);
+            LogF("  Destination IP    %s\r\n", text);
+            return DO_NOTHING;
+    }
+    switch (action)
+    {
+        case DO_NOTHING:
+            return DO_NOTHING;
+        case UNICAST:
+            memcpy(pDstMac, pSrcMac, 6);
+            break;
+        case MULTICAST_ROUTER:
+            break;
+        default:
+            LogTimeF("Ip6 unknown action %d\r\n", action);
+            return DO_NOTHING;
+    }
+
+    memcpy(pHeader->dst, dst, 16);
+    memcpy(pHeader->src, src, 16);
+    pHeader->dataLength   = NetToHost16(dataLength);
+        
+    *pSize = HEADER_LENGTH + dataLength;
+    
+    return action;
+}
+int Ip6PollForPacketToSend(void* pPacket, int* pSize, char* pDstMac)
+{    
+    void* pData = (char*)pPacket + HEADER_LENGTH;
+    int dataLength = 0;
+    
+    char pSrcIp[16];
+    char pDstIp[16];
+    uint8_t protocol = 0;
+    int action = DO_NOTHING;
+    if (action == DO_NOTHING)
+    {
+        action = Icmp6PollForPacketToSend(pData, &dataLength, pSrcIp, pDstIp);
+        protocol = ICMP6;
+    }
+    
+    if (action == DO_NOTHING)
+    {
+        action = Udp6PollForPacketToSend(pData, &dataLength, pSrcIp, pDstIp);
+        protocol = UDP;
+    }
+    if (action == DO_NOTHING) return DO_NOTHING;
+    
+    ArRev6(pDstIp, pDstMac);             //Make the remote MAC from ARP
+
+    uint8_t version  = 6;
+    uint8_t hoplimit = 255;
+    
+    struct header * pHeader = (header*)pPacket;
+    pHeader->versionTrafficFlow  = version << 4;
+    pHeader->protocol            = protocol;
+    pHeader->hoplimit            = 255;
+    memcpy(pHeader->dst, pDstIp, 16);
+    memcpy(pHeader->src, pSrcIp, 16);
+    pHeader->dataLength   = NetToHost16(dataLength);
+    
+    *pSize = HEADER_LENGTH + dataLength;
+    
+    LogTimeF("Ip6 sending waiting packet\r\n");
+    char text[100];
+    LogF("  Size              %d\r\n", *pSize);
+    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);
+    LogF("  Protocol          %s\r\n", text);
+    NetIp6AddressToString(pSrcIp, sizeof(text), text);
+    LogF("  Source IP         %s\r\n", text);
+    NetIp6AddressToString(pDstIp, sizeof(text), text);
+    LogF("  Destination IP    %s\r\n", text);
+    
+    return action;
+}