Diff: ip6/icmp/ndp/ns.cpp

Revision:

46:40d33e9037e4

Parent:

45:3dd57903ec99

Child:

47:73af5c0b0dc2

diff -r 3dd57903ec99 -r 40d33e9037e4 ip6/icmp/ndp/ns.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ip6/icmp/ndp/ns.cpp	Tue Oct 24 07:01:35 2017 +0000
@@ -0,0 +1,196 @@
+#include   "mbed.h"
+#include    "log.h"
+#include    "net.h"
+#include "action.h"
+#include    "ip6.h"
+#include    "mac.h"
+#include     "nr.h"
+#include     "ar.h"
+#include    "ip6.h"
+#include  "slaac.h"
+
+bool NsTraceRecvSol = false;
+bool NsTraceRecvAdv = false;
+bool NsTraceSendSol = false;
+
+static char srcMAC[6];
+static char tgtMAC[6];
+
+char NsAddressToResolve[16];
+bool NsResolveRequestFlag = false;
+
+static int logOptions(char* p)
+{
+    int type = *p++;
+    int size = *p++;
+    char text[100];
+    switch (type)
+    {
+        case 1:
+            MacToString(p, sizeof(text), text);
+            LogF("  Src MAC     %s\r\n", text);
+            break;
+        case 2:
+            MacToString(p, sizeof(text), text);
+            LogF("  Tgt MAC     %s\r\n", text);
+            break;
+        default:
+            LogF("  Unknown option %d\r\n", type);
+            break;
+    }
+    return size * 8;
+}
+
+void logHeader(void* pPacket, int* pSize)
+{
+    __packed struct header
+    {
+        uint32_t  reserved;
+        char      target[16];
+    };
+    struct header* pHeader = (header*)pPacket;
+    char* pData = (char*)pHeader + sizeof(struct header);
+    int dataLength = *pSize - sizeof(struct header);
+    
+    Log("NDP header\r\n");
+    LogF("  Size        %d\r\n", *pSize);
+    char text[100];
+    Ip6AddressToString(pHeader->target, sizeof(text), text);
+    LogF("  Target      %s\r\n", text);
+    
+    char* p = pData;
+    char* pE = pData + dataLength;
+    
+    while(p < pE) p += logOptions(p);
+}
+
+static int decodeOptions(char* p)
+{
+    int type = *p++;
+    int size = *p++;
+    switch (type)
+    {
+        case 1:
+            memcpy(srcMAC, p, 6);
+            break;
+        case 2:
+            memcpy(tgtMAC, p, 6);
+            break;
+        default:
+            LogF("  Unknown option %d\r\n", type);
+            break;
+    }
+    return size * 8;
+}
+
+int NsHandleReceivedSolicitation(void (*traceback)(void), void* pPacket, int* pSize, uint8_t* pType, uint8_t* pCode)
+{
+    __packed struct header
+    {
+        uint32_t  reserved;
+        char      target[16];
+    };
+    struct header* pHeader = (header*)pPacket;
+    char* pData = (char*)pHeader + sizeof(struct header);
+    int dataLength = *pSize - sizeof(struct header);
+    
+    //Check it is us
+    if (!SlaacScope(pHeader->target)) return DO_NOTHING;
+
+    if (NsTraceRecvSol)
+    {
+        if (NetTraceNewLine) Log("\r\n");
+        LogTimeF("NDP received neighbour solicit\r\n");
+        if (NetTraceStack) traceback();
+        logHeader(pPacket, pSize);
+    }
+    char* p = pData;
+    char* pE = pData + dataLength;
+    
+    while(p < pE) p += decodeOptions(p);
+    
+    //Send advertisement
+    *pType = 136;
+    *pCode = 0;
+    pHeader->reserved = 0x00000060; //R=0 (not a router); S=1 (solicited); O=1 (override)
+                                    //pHeader->target is unchanged
+    
+    p = pData;
+    *p++ = 2; //Target MAC option
+    *p++ = 1; //8 bytes
+    memcpy(p, MacLocal, 6);
+    p += 6;
+    
+    *pSize = sizeof(struct header) + p - pData;
+    
+    if (NsTraceRecvSol) logHeader(pPacket, pSize);
+    
+    return ActionMakeFromDestAndTrace(UNICAST, NsTraceRecvSol && NetTraceStack);
+
+}
+int NsHandleReceivedAdvertisement(void (*traceback)(void), void* pPacket, int* pSize)
+{
+    __packed struct header
+    {
+        uint32_t  reserved;
+        char      target[16];
+    };
+    struct header* pHeader = (header*)pPacket;
+    char* pData = (char*)pHeader + sizeof(struct header);
+    int dataLength = *pSize - sizeof(struct header);
+    
+    if (NsTraceRecvAdv)
+    {
+        if (NetTraceNewLine) Log("\r\n");
+        LogTimeF("NDP received neighbour advertise\r\n");
+        if (NetTraceStack) traceback();
+        logHeader(pPacket, pSize);
+    }
+    
+    char* p = pData;
+    char* pE = pData + dataLength;
+    
+    while(p < pE) p += decodeOptions(p);
+    
+    ArAddIp6Record(tgtMAC, pHeader->target);
+    NrMakeRequestForNameFromIp6(pHeader->target);
+    
+    return DO_NOTHING;
+}
+
+int NsGetWaitingSolicitation(void* pPacket, int* pSize, uint8_t* pType, uint8_t* pCode)
+{    
+    if (!NsResolveRequestFlag) return DO_NOTHING;
+    NsResolveRequestFlag = false;
+        
+    __packed struct header
+    {
+        uint32_t  reserved;
+        char      target[16];
+    };
+    struct header* pHeader = (header*)pPacket;
+    pHeader->reserved = 0;
+    memcpy(pHeader->target, NsAddressToResolve, 16);
+
+    *pType = 135; //Neighbour solicitation
+    *pCode = 0;
+    
+    char* pData = (char*)pHeader + sizeof(struct header);
+    char* p = pData;
+    *p++ = 1; //Source MAC option
+    *p++ = 1; //8 bytes
+    memcpy(p, MacLocal, 6);
+    p += 6;
+    
+    *pSize = sizeof(struct header) + p - pData;
+
+    if (NsTraceSendSol)
+    {
+        if (NetTraceNewLine) Log("\r\n");
+        LogTimeF("NDP sent neighbour solicit\r\n");
+        logHeader(pPacket, pSize);
+    }
+    
+    return ActionMakeFromDestAndTrace(MULTICAST_NODE, NsTraceSendSol && NetTraceStack);
+    
+}