Diff: ip4/icmp/icmp4.c

Revision:

136:8a65abb0dc63

Parent:

61:aad055f1b0d1

diff -r a7f026a09543 -r 8a65abb0dc63 ip4/icmp/icmp4.c
--- a/ip4/icmp/icmp4.c	Sat Mar 23 12:25:48 2019 +0000
+++ b/ip4/icmp/icmp4.c	Sat Apr 06 11:20:20 2019 +0000
@@ -6,22 +6,25 @@
 #include    "ip4.h"
 #include   "dhcp.h"
 #include  "echo4.h"
+#include "checksum.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 char* hdrTypePtr    (char* pPacket) { return pPacket +  0; } //1
+static char* hdrCodePtr    (char* pPacket) { return pPacket +  1; } //1
+static char* hdrChecksumPtr(char* pPacket) { return pPacket +  2; } //2
+#define HEADER_LENGTH 4
+
+static uint8_t  hdrTypeGet    (char* pPacket) { return                     *hdrTypePtr    (pPacket);            }
+static uint8_t  hdrCodeGet    (char* pPacket) { return                     *hdrCodePtr    (pPacket);            }
+static uint16_t hdrChecksumGet(char* pPacket) { uint16_t r; NetDirect16(&r, hdrChecksumPtr(pPacket)); return r; } //Don't invert the checksum
+
+static void     hdrTypeSet    (char* pPacket, uint8_t  value) {            *hdrTypePtr    (pPacket) = value;  }
+static void     hdrCodeSet    (char* pPacket, uint8_t  value) {            *hdrCodePtr    (pPacket) = value;  }
+static void     hdrChecksumSet(char* pPacket, uint16_t value) { NetDirect16(hdrChecksumPtr(pPacket), &value); } //Don't invert the checksum
 
 static void logType(uint16_t type)
 {
@@ -32,56 +35,44 @@
         default:           LogF("Unknown type %d", type); break;
     }
 }
-static void logHeader()
+static void logHeader(int size, char* pPacket)
 {            
     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);
+        LogF("  Type           "); logType(hdrTypeGet    (pPacket)); Log("\r\n");
+        LogF("  Code           %u\r\n",    hdrCodeGet    (pPacket));
+        LogF("  Checksum (hex) %04hX\r\n", hdrChecksumGet(pPacket));
+        LogF("  Calculated     %04hX\r\n", CheckSum(size, pPacket));
     }
     else
     {
         Log("ICMP4 header ");
-        logType(type);        
+        logType(hdrTypeGet(pPacket));        
         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 int traceSize = 0;
+static char* tracePacket;
 static void (*pTraceBack)(void);
 static void trace()
 {
     pTraceBack();
-    logHeader();
+    logHeader(traceSize, tracePacket);
 }
-int Icmp4HandleReceivedPacket(void (*traceback)(void), void* pPacketRx, int sizeRx, void* pPacketTx, int* pSizeTx, uint32_t* pSrcIp, uint32_t* pDstIp)
+int Icmp4HandleReceivedPacket(void (*traceback)(void), char* pPacketRx, int sizeRx, char* pPacketTx, int* pSizeTx, uint32_t* pSrcIp, uint32_t* pDstIp)
 {
     pTraceBack = traceback;
+    tracePacket = pPacketRx;
+    traceSize = sizeRx;
 
-    readHeader(pPacketRx, sizeRx);
+    uint8_t type = hdrTypeGet(pPacketRx);
+    uint8_t code = hdrCodeGet(pPacketRx);
     
-    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 dataLengthRx =           sizeRx - HEADER_LENGTH;
+    int dataLengthTx =         *pSizeTx - HEADER_LENGTH;
+    char* pPayloadRx = (char*)pPacketRx + HEADER_LENGTH;
+    char* pPayloadTx = (char*)pPacketTx + HEADER_LENGTH;
     
     int action = DO_NOTHING;
     switch (type)
@@ -102,11 +93,15 @@
     *pDstIp = *pSrcIp;
     *pSrcIp = DhcpLocalIp;
 
-    *pSizeTx = sizeof(struct header) + dataLengthTx;
+    *pSizeTx = HEADER_LENGTH + dataLengthTx;
     
-    writeHeader(pPacketTx, *pSizeTx);
+    hdrTypeSet    (pPacketTx, type    );
+    hdrCodeSet    (pPacketTx, code    );
+    hdrChecksumSet(pPacketTx, 0       );
+    uint16_t checksum = CheckSum(*pSizeTx, pPacketTx);
+    hdrChecksumSet(pPacketTx, checksum);
     
-    if (ActionGetTracePart(action)) logHeader();
+    if (ActionGetTracePart(action)) logHeader(*pSizeTx, pPacketTx);
     
     return action;
 }