Diff: ip4/icmp/icmp4.c

Revision:

61:aad055f1b0d1

Parent:

59:e0e556c8bd46

Child:

136:8a65abb0dc63

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ip4/icmp/icmp4.c	Thu Jan 11 17:38:21 2018 +0000
@@ -0,0 +1,112 @@
+#include <stdint.h>
+
+#include    "log.h"
+#include    "net.h"
+#include "action.h"
+#include    "ip4.h"
+#include   "dhcp.h"
+#include  "echo4.h"
+
+#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 void logType(uint16_t type)
+{
+    switch (type)
+    {
+        case ECHO_REPLY:   Log ("Echo Reply"           ); break;
+        case ECHO_REQUEST: Log ("Echo Request"         ); break;
+        default:           LogF("Unknown type %d", type); break;
+    }
+}
+static void logHeader()
+{            
+    if (NetTraceVerbose)
+    {
+        Log ("ICMP4 header\r\n");
+        LogF("  Type           "); logType(type); Log("\r\n");
+        LogF("  Code           %u\r\n",    code);
+        LogF("  Checksum (hex) %04hX\r\n", checksum);
+        LogF("  Calculated     %04hX\r\n", calculated);
+    }
+    else
+    {
+        Log("ICMP4 header ");
+        logType(type);        
+        Log("\r\n");
+    }
+}
+static void readHeader(void* pPacket, int size)
+{
+    struct header* pHeader = (struct header*)pPacket;
+          type =             pHeader->type;
+          code =             pHeader->code;
+      checksum = NetToHost16(pHeader->checksum);
+    calculated = NetCheckSum(size, pPacket);
+}
+static void writeHeader(void* pPacket, int size)
+{    
+    struct header* pHeader = (struct 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), void* pPacketRx, int sizeRx, void* pPacketTx, int* pSizeTx, uint32_t* pSrcIp, uint32_t* pDstIp)
+{
+    pTraceBack = traceback;
+
+    readHeader(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 ECHO_REQUEST:
+            action = Echo4HandleRequest(trace, &type, &code, pPayloadRx, dataLengthRx, pPayloadTx, &dataLengthTx);
+            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;
+
+    *pSizeTx = sizeof(struct header) + dataLengthTx;
+    
+    writeHeader(pPacketTx, *pSizeTx);
+    
+    if (ActionGetTracePart(action)) logHeader();
+    
+    return action;
+}