Diff: ip6/icmp/icmp6.cpp

Revision:

43:bc028d5a6424

Child:

44:83ce5ace337b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ip6/icmp/icmp6.cpp	Thu Oct 19 20:56:58 2017 +0000
@@ -0,0 +1,168 @@
+#include   "mbed.h"
+#include    "log.h"
+#include    "net.h"
+#include "action.h"
+#include     "ns.h"
+#include     "ra.h"
+#include     "ip.h"
+#include    "ip6.h"
+#include  "slaac.h"
+#include  "echo6.h"
+
+#define HEADER_LENGTH 4
+__packed struct header
+{
+    uint8_t  type;
+    uint8_t  code;
+    uint16_t checksum;
+};
+static uint8_t  type;
+static uint8_t  code;
+static uint16_t checksum;
+static uint16_t calculatedChecksum;
+static int      dataLength;
+static void*    pData;
+
+static void typeToString(uint8_t type, int size, char* text)
+{
+    switch (type)
+    {
+        case 128: strncpy (text, "Echo Request"           , size); break;
+        case 129: strncpy (text, "Echo Reply"             , size); break;
+        case 133: strncpy (text, "Router solicit"         , size); break;
+        case 134: strncpy (text, "Router advertisement"   , size); break;
+        case 135: strncpy (text, "Neighbour solicit"      , size); break;
+        case 136: strncpy (text, "Neighbour advertisement", size); break;
+        case 137: strncpy (text, "Redirect"               , size); break;
+        default:  snprintf(text,                            size, "unknown type %u", type); break;
+    }
+}
+static uint16_t calculateChecksum(char* pSrcIp, char* pDstIp, int size, void* pPacket)
+{
+    __packed struct pseudo
+    {
+        char     src[16];
+        char     dst[16];
+        uint32_t len;
+        uint8_t  zer[3];
+        uint8_t  pro;
+    } pseudo;
+    
+    Ip6Copy(pseudo.src, pSrcIp);
+    Ip6Copy(pseudo.dst, pDstIp);
+    pseudo.len    = NetToHost32(size);
+    pseudo.zer[0] = 0;
+    pseudo.zer[1] = 0;
+    pseudo.zer[2] = 0;
+    pseudo.pro    = ICMP6;
+
+    return NetCheckSumTwo(sizeof(pseudo), &pseudo, size, pPacket);
+}
+static void logHeader()
+{
+    char text[100];
+    if (NetTraceVerbose)
+    {
+        Log ("ICMP6 header\r\n");
+        typeToString(type, sizeof(text),   text);
+        LogF("  Type           %s\r\n",    text);
+        LogF("  Code           %u\r\n",    code);
+        LogF("  Checksum (hex) %04hX\r\n", checksum);
+        LogF("  Calculated     %04hX\r\n", calculatedChecksum);
+        LogF("  Data length    %d\r\n",    dataLength);
+    }
+    else
+    {
+        Log ("ICMP6 header ");
+        typeToString(type, sizeof(text),   text);
+        Log(text);
+        Log("\r\n");
+    }
+}
+static void readHeader(char* pSrcIp, char* pDstIp, void* pPacket, int size)
+{
+    struct header* pHeader = (header*)pPacket;
+                  type =             pHeader->type;
+                  code =             pHeader->code;
+              checksum = NetToHost16(pHeader->checksum);
+    calculatedChecksum = calculateChecksum(pSrcIp, pDstIp, size, pPacket);
+                 pData = (char*)pPacket + HEADER_LENGTH;
+            dataLength =           size - HEADER_LENGTH;
+}
+static void writeHeader(void* pPacket, int size, char* pSrcIp, char* pDstIp)
+{    
+    struct header* pHeader = (header*)pPacket;
+    pHeader->type     = type;
+    pHeader->code     = code;
+    pHeader->checksum = 0;
+    pHeader->checksum = calculateChecksum(pSrcIp, pDstIp, size, pPacket);
+    calculatedChecksum = 0;
+}
+static void (*pTraceBack)(void);
+static void trace()
+{
+    pTraceBack();
+    logHeader();
+}
+int Icmp6HandleReceivedPacket(void (*traceback)(void), char* pSrcIp, char* pDstIp, int* pSize, void* pPacket)
+{
+    pTraceBack = traceback;
+    
+    readHeader(pSrcIp, pDstIp, pPacket, *pSize);
+
+    int action = DO_NOTHING;
+    switch (type)
+    {
+        case 128: //Echo request - Ping
+            action = Echo6HandleRequest(trace, &type, &code);
+            break;
+        case 133: //Router solicit
+            return DO_NOTHING; //We are not a router so quietly drop this
+        case 134: //Router advertisement
+            action = RaHandleReceivedAdvertisement(trace, pData, &dataLength);
+            break;
+        case 135: //Neighbour solicit
+            action = NsHandleReceivedSolicitation(trace, pData, &dataLength, &type, &code);
+            break;
+        case 136: //Neighbour advertisement
+            action = NsHandleReceivedAdvertisement(trace, pData, &dataLength);
+            break;
+        default:
+            LogTimeF("ICMP6 unknown packet type %d", type);
+            return DO_NOTHING;
+    }
+    if (!action) return DO_NOTHING;
+    
+    Ip6Copy(pDstIp, pSrcIp);
+    Ip6Copy(pSrcIp, SlaacLinkLocalIp);
+    Ip6DestIpFromDest(ActionGetDestPart(action), pDstIp);
+
+    *pSize = HEADER_LENGTH + dataLength;
+    
+    writeHeader(pPacket, *pSize, pSrcIp, pDstIp);
+    
+    if (ActionGetTracePart(action)) logHeader();
+    
+    return action;
+}
+int Icmp6PollForPacketToSend(void* pPacket, int* pSize, char* pSrcIp, char* pDstIp)
+{    
+    pData = (char*)pPacket + HEADER_LENGTH;
+    int action  = DO_NOTHING;
+    if (!action) action = RaGetWaitingSolicitation(pData, &dataLength, &type, &code);
+    if (!action) action = NsGetWaitingSolicitation(pData, &dataLength, &type, &code);
+    if (!action) return DO_NOTHING;
+
+    Ip6Copy(pSrcIp, SlaacLinkLocalIp);
+    
+    Ip6DestIpFromDest(ActionGetDestPart(action), pDstIp);
+
+    *pSize = HEADER_LENGTH + dataLength;
+    
+    writeHeader(pPacket, *pSize, pSrcIp, pDstIp);
+    
+    if (ActionGetTracePart(action)) logHeader();
+    
+    return action;
+
+}