Diff: ip6/icmp/icmp6.c

Revision:

61:aad055f1b0d1

Parent:

59:e0e556c8bd46

Child:

136:8a65abb0dc63

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ip6/icmp/icmp6.c	Thu Jan 11 17:38:21 2018 +0000
@@ -0,0 +1,173 @@
+#include <stdint.h>
+
+#include     "log.h"
+#include     "net.h"
+#include  "action.h"
+#include      "ns.h"
+#include      "ra.h"
+#include      "rs.h"
+#include      "ip.h"
+#include "ip6addr.h"
+#include   "slaac.h"
+#include   "echo6.h"
+#include   "dest6.h"
+
+__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 void logType(uint8_t type)
+{
+    switch (type)
+    {
+        case   1: Log ("Destination unreacheable" ); break;
+        case 128: Log ("Echo Request"             ); break;
+        case 129: Log ("Echo Reply"               ); break;
+        case 133: Log ("Router solicit"           ); break;
+        case 134: Log ("Router advertisement"     ); break;
+        case 135: Log ("Neighbour solicit"        ); break;
+        case 136: Log ("Neighbour advertisement"  ); break;
+        case 137: Log ("Redirect"                 ); break;
+        default:  LogF("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;
+    
+    Ip6AddressCopy(pseudo.src, pSrcIp);
+    Ip6AddressCopy(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()
+{
+    if (NetTraceVerbose)
+    {
+        Log ("ICMP6 header\r\n");
+        Log ("  Type           "); logType(type); Log("\r\n");
+        LogF("  Code           %u\r\n",    code);
+        LogF("  Checksum (hex) %04hX\r\n", checksum);
+        LogF("  Calculated     %04hX\r\n", calculatedChecksum);
+    }
+    else
+    {
+        Log ("ICMP6 header ");
+        logType(type);
+        Log("\r\n");
+    }
+}
+static void readHeader(char* pSrcIp, char* pDstIp, void* pPacket, int size)
+{
+    struct header* pHeader = (struct header*)pPacket;
+                  type =             pHeader->type;
+                  code =             pHeader->code;
+              checksum = NetToHost16(pHeader->checksum);
+    calculatedChecksum = calculateChecksum(pSrcIp, pDstIp, size, pPacket);
+}
+static void writeHeader(void* pPacket, int size, char* pSrcIp, char* pDstIp)
+{    
+    struct header* pHeader = (struct header*)pPacket;
+    pHeader->type     = type;
+    pHeader->code     = code;
+    pHeader->checksum = 0;
+    checksum = calculateChecksum(pSrcIp, pDstIp, size, pPacket);
+    pHeader->checksum = checksum;
+    calculatedChecksum = 0;
+}
+static void (*pTraceBack)(void);
+static void trace()
+{
+    pTraceBack();
+    logHeader();
+}
+int Icmp6HandleReceivedPacket(void (*traceback)(void), int scope, void* pPacketRx, int sizeRx, void* pPacketTx, int* pSizeTx, char* pSrcIp, char* pDstIp)
+{
+    pTraceBack = traceback;
+        
+    readHeader(pSrcIp, pDstIp, pPacketRx, sizeRx);
+    
+    int dataLengthRx =           sizeRx - sizeof(struct header);
+    int dataLengthTx =         *pSizeTx - sizeof(struct header);
+    char* pPayloadRx = (char*)pPacketRx + sizeof(struct header);
+    char* pPayloadTx = (char*)pPacketTx + sizeof(struct header);
+
+    int action = DO_NOTHING;
+    switch (type)
+    {
+        case   1: //Destination unreacheable
+            action = Dest6HandleRequest(trace, &type, &code);
+            break;
+        case 128: //Echo request - Ping
+            action = Echo6HandleRequest(trace, &type, &code, pPayloadRx, dataLengthRx, pPayloadTx, &dataLengthTx);
+            break;
+        case 133: //Router solicit
+            return DO_NOTHING; //We are not a router so quietly drop this
+        case 134: //Router advertisement
+            action = RaHandleReceivedAdvertisement(trace, pPayloadRx, &dataLengthRx);
+            break;
+        case 135: //Neighbour solicit
+            action = NsHandleReceivedSolicitation(trace, &type, &code, pPayloadRx, dataLengthRx, pPayloadTx, &dataLengthTx);
+            break;
+        case 136: //Neighbour advertisement
+            action = NsHandleReceivedAdvertisement(trace, pPayloadRx, &dataLengthRx);
+            break;
+        default:
+            LogTimeF("ICMP6 unknown packet type %d\r\n", type);
+            return DO_NOTHING;
+    }
+    if (!action) return DO_NOTHING;
+    
+    Ip6AddressCopy(pDstIp, pSrcIp);
+    SlaacAddressFromScope(scope,                     pSrcIp);
+      Ip6AddressFromDest (ActionGetDestPart(action), pDstIp);
+
+    *pSizeTx = sizeof(struct header) + dataLengthTx;
+    
+    writeHeader(pPacketTx, *pSizeTx, pSrcIp, pDstIp);
+    
+    if (ActionGetTracePart(action)) logHeader();
+    
+    return action;
+}
+int Icmp6PollForPacketToSend(void* pPacket, int* pSize, char* pSrcIp, char* pDstIp)
+{    
+    char* pData = (char*)pPacket + sizeof(struct header);
+    int dataLength = *pSize - sizeof(struct header);
+    int action  = DO_NOTHING;
+    if (!action) action = RsGetWaitingSolicitation(pData, &dataLength, &type, &code);
+    if (!action) action = NsGetWaitingSolicitation(pData, &dataLength, &type, &code);
+    if (!action) return DO_NOTHING;
+
+    int scope = SCOPE_LOCAL;
+    SlaacAddressFromScope(scope,                     pSrcIp);
+      Ip6AddressFromDest (ActionGetDestPart(action), pDstIp);
+
+    *pSize = sizeof(struct header) + dataLength;
+    
+    writeHeader(pPacket, *pSize, pSrcIp, pDstIp);
+    
+    if (ActionGetTracePart(action)) logHeader();
+    
+    return action;
+
+}