icmp4.c

00001 #include <stdint.h>
00002 
00003 #include    "log.h"
00004 #include    "net.h"
00005 #include "action.h"
00006 #include    "ip4.h"
00007 #include   "dhcp.h"
00008 #include  "echo4.h"
00009 #include "checksum.h"
00010 
00011 #define ECHO_REPLY   0
00012 #define UNREACHABLE  3
00013 #define REDIRECT     5
00014 #define ECHO_REQUEST 8
00015 
00016 static char* hdrTypePtr    (char* pPacket) { return pPacket +  0; } //1
00017 static char* hdrCodePtr    (char* pPacket) { return pPacket +  1; } //1
00018 static char* hdrChecksumPtr(char* pPacket) { return pPacket +  2; } //2
00019 #define HEADER_LENGTH 4
00020 
00021 static uint8_t  hdrTypeGet    (char* pPacket) { return                     *hdrTypePtr    (pPacket);            }
00022 static uint8_t  hdrCodeGet    (char* pPacket) { return                     *hdrCodePtr    (pPacket);            }
00023 static uint16_t hdrChecksumGet(char* pPacket) { uint16_t r; NetDirect16(&r, hdrChecksumPtr(pPacket)); return r; } //Don't invert the checksum
00024 
00025 static void     hdrTypeSet    (char* pPacket, uint8_t  value) {            *hdrTypePtr    (pPacket) = value;  }
00026 static void     hdrCodeSet    (char* pPacket, uint8_t  value) {            *hdrCodePtr    (pPacket) = value;  }
00027 static void     hdrChecksumSet(char* pPacket, uint16_t value) { NetDirect16(hdrChecksumPtr(pPacket), &value); } //Don't invert the checksum
00028 
00029 static void logType(uint16_t type)
00030 {
00031     switch (type)
00032     {
00033         case ECHO_REPLY:   Log ("Echo Reply"           ); break;
00034         case ECHO_REQUEST: Log ("Echo Request"         ); break;
00035         default:           LogF("Unknown type %d", type); break;
00036     }
00037 }
00038 static void logHeader(int size, char* pPacket)
00039 {            
00040     if (NetTraceVerbose)
00041     {
00042         Log ("ICMP4 header\r\n");
00043         LogF("  Type           "); logType(hdrTypeGet    (pPacket)); Log("\r\n");
00044         LogF("  Code           %u\r\n",    hdrCodeGet    (pPacket));
00045         LogF("  Checksum (hex) %04hX\r\n", hdrChecksumGet(pPacket));
00046         LogF("  Calculated     %04hX\r\n", CheckSum(size, pPacket));
00047     }
00048     else
00049     {
00050         Log("ICMP4 header ");
00051         logType(hdrTypeGet(pPacket));        
00052         Log("\r\n");
00053     }
00054 }
00055 static int traceSize = 0;
00056 static char* tracePacket;
00057 static void (*pTraceBack)(void);
00058 static void trace()
00059 {
00060     pTraceBack();
00061     logHeader(traceSize, tracePacket);
00062 }
00063 int Icmp4HandleReceivedPacket(void (*traceback)(void), char* pPacketRx, int sizeRx, char* pPacketTx, int* pSizeTx, uint32_t* pSrcIp, uint32_t* pDstIp)
00064 {
00065     pTraceBack = traceback;
00066     tracePacket = pPacketRx;
00067     traceSize = sizeRx;
00068 
00069     uint8_t type = hdrTypeGet(pPacketRx);
00070     uint8_t code = hdrCodeGet(pPacketRx);
00071     
00072     int dataLengthRx =           sizeRx - HEADER_LENGTH;
00073     int dataLengthTx =         *pSizeTx - HEADER_LENGTH;
00074     char* pPayloadRx = (char*)pPacketRx + HEADER_LENGTH;
00075     char* pPayloadTx = (char*)pPacketTx + HEADER_LENGTH;
00076     
00077     int action = DO_NOTHING;
00078     switch (type)
00079     {
00080         case ECHO_REQUEST:
00081             action = Echo4HandleRequest(trace, &type, &code, pPayloadRx, dataLengthRx, pPayloadTx, &dataLengthTx);
00082             break;
00083         case UNREACHABLE:
00084             return DO_NOTHING;
00085         case REDIRECT:
00086             return DO_NOTHING;
00087         default:
00088             LogTimeF("ICMP4 packet type %d unknown\r\n", type);
00089             return DO_NOTHING;
00090     }
00091     if (!action) return DO_NOTHING;
00092     
00093     *pDstIp = *pSrcIp;
00094     *pSrcIp = DhcpLocalIp;
00095 
00096     *pSizeTx = HEADER_LENGTH + dataLengthTx;
00097     
00098     hdrTypeSet    (pPacketTx, type    );
00099     hdrCodeSet    (pPacketTx, code    );
00100     hdrChecksumSet(pPacketTx, 0       );
00101     uint16_t checksum = CheckSum(*pSizeTx, pPacketTx);
00102     hdrChecksumSet(pPacketTx, checksum);
00103     
00104     if (ActionGetTracePart(action)) logHeader(*pSizeTx, pPacketTx);
00105     
00106     return action;
00107 }