Diff: ip4/icmp/icmp4.cpp

Revision:

43:bc028d5a6424

Child:

47:73af5c0b0dc2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ip4/icmp/icmp4.cpp	Thu Oct 19 20:56:58 2017 +0000
@@ -0,0 +1,117 @@
+#include   "mbed.h"
+#include    "log.h"
+#include    "net.h"
+#include "action.h"
+#include    "ip4.h"
+#include   "dhcp.h"
+#include  "echo4.h"
+
+#define HEADER_SIZE 4
+
+#define ECHO_REPLY   0
+#define UNREACHABLE  3
+#define REDIRECT     5
+#define ECHO_REQUEST 8
+
+__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 calculated;
+static int      dataLength;
+static void*    pData;
+
+static void typeToString(uint16_t type, int size, char* text)
+{
+    switch (type)
+    {
+        case ECHO_REPLY:   strncpy (text, "Echo Reply"   , size); break;
+        case ECHO_REQUEST: strncpy (text, "Echo Request" , size); break;
+        default:           snprintf(text,                  size, "Unknown type %d", type); break;
+    }
+}
+static void logHeader()
+{        
+    char text[20];
+    
+    if (NetTraceVerbose)
+    {
+        Log ("ICMP4 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", calculated);
+        LogF("  Data length    %d\r\n",    dataLength);
+    }
+    else
+    {
+        Log("ICMP4 header ");
+        typeToString(type, sizeof(text), text);
+        Log(text);        
+        Log("\r\n");
+    }
+}
+static void readHeader(void* pPacket, int size)
+{
+    struct header* pHeader = (header*)pPacket;
+          type =             pHeader->type;
+          code =             pHeader->code;
+      checksum = NetToHost16(pHeader->checksum);
+    calculated = NetCheckSum(size, pPacket);
+         pData = (char*)pPacket + HEADER_SIZE;
+    dataLength =           size - HEADER_SIZE;
+}
+static void writeHeader(void* pPacket, int size)
+{    
+    struct header* pHeader = (header*)pPacket;
+    pHeader->type     = type;
+    pHeader->code     = code;
+    pHeader->checksum = 0;
+    pHeader->checksum = NetCheckSum(size, pPacket);
+    calculated = 0;
+}
+static void (*pTraceBack)(void);
+static void trace()
+{
+    pTraceBack();
+    logHeader();
+}
+int Icmp4HandleReceivedPacket(void (*traceback)(void), uint32_t* pSrcIp, uint32_t* pDstIp, int* pSize, void * pPacket)
+{
+    pTraceBack = traceback;
+
+    readHeader(pPacket, *pSize);
+    
+    int action = DO_NOTHING;
+    switch (type)
+    {
+        case ECHO_REQUEST:
+            action = Echo4HandleRequest(trace, &type, &code);
+            break;
+        case UNREACHABLE:
+            return DO_NOTHING;
+        case REDIRECT:
+            return DO_NOTHING;
+        default:
+            LogTimeF("ICMP4 packet type %d unknown\r\n", type);
+            return DO_NOTHING;
+    }
+    if (!action) return DO_NOTHING;
+    
+    *pDstIp = *pSrcIp;
+    *pSrcIp = DhcpLocalIp;
+
+    *pSize = HEADER_SIZE + dataLength;
+    
+    writeHeader(pPacket, *pSize);
+    
+    if (ActionGetTracePart(action)) logHeader();
+    
+    return action;
+}