icmp6.c

00001 #include <stdint.h>
00002 
00003 #include     "log.h"
00004 #include     "net.h"
00005 #include  "action.h"
00006 #include      "ns.h"
00007 #include      "ra.h"
00008 #include      "rs.h"
00009 #include      "ip.h"
00010 #include "ip6addr.h"
00011 #include   "slaac.h"
00012 #include   "echo6.h"
00013 #include   "dest6.h"
00014 #include "checksum.h"
00015 
00016 static char* hdrPtrType    (char* pPacket) { return pPacket + 0; } //1
00017 static char* hdrPtrCode    (char* pPacket) { return pPacket + 1; } //1
00018 static char* hdrPtrChecksum(char* pPacket) { return pPacket + 2; } //2
00019 static const int HEADER_LENGTH = 4;
00020 
00021 static uint8_t  type;
00022 static uint8_t  code;
00023 static uint16_t checksum;
00024 static uint16_t calculatedChecksum;
00025 
00026 static void logType(uint8_t type)
00027 {
00028     switch (type)
00029     {
00030         case   1: Log ("Destination unreacheable" ); break;
00031         case 128: Log ("Echo Request"             ); break;
00032         case 129: Log ("Echo Reply"               ); break;
00033         case 133: Log ("Router solicit"           ); break;
00034         case 134: Log ("Router advertisement"     ); break;
00035         case 135: Log ("Neighbour solicit"        ); break;
00036         case 136: Log ("Neighbour advertisement"  ); break;
00037         case 137: Log ("Redirect"                 ); break;
00038         default:  LogF("Unknown type %u", type    ); break;
00039     }
00040 }
00041 static uint16_t calculateChecksum(char* pSrcIp, char* pDstIp, int size, char* pPacket)
00042 {   
00043     uint32_t sum = 0;
00044     uint32_t pro32 = ICMP6;
00045      sum = CheckSumAddDirect(sum,   16, pSrcIp );
00046      sum = CheckSumAddDirect(sum,   16, pDstIp );
00047      sum = CheckSumAddInvert(sum,    4, &size  );
00048      sum = CheckSumAddInvert(sum,    4, &pro32 );
00049     return CheckSumFinDirect(sum, size, pPacket);
00050 }
00051 static void logHeader()
00052 {
00053     if (NetTraceVerbose)
00054     {
00055         Log ("ICMP6 header\r\n");
00056         Log ("  Type           "); logType(type); Log("\r\n");
00057         LogF("  Code           %u\r\n",    code);
00058         LogF("  Checksum (hex) %04hX\r\n", checksum);
00059         LogF("  Calculated     %04hX\r\n", calculatedChecksum);
00060     }
00061     else
00062     {
00063         Log ("ICMP6 header ");
00064         logType(type);
00065         Log("\r\n");
00066     }
00067 }
00068 static void readHeader(char* pSrcIp, char* pDstIp, char* pPacket, int size)
00069 {
00070     type = *hdrPtrType(pPacket);
00071     code = *hdrPtrCode(pPacket);
00072     NetDirect16(&checksum, hdrPtrChecksum(pPacket));
00073     calculatedChecksum = calculateChecksum(pSrcIp, pDstIp, size, pPacket);
00074 }
00075 static void writeHeader(char* pPacket, int size, char* pSrcIp, char* pDstIp)
00076 {
00077     *hdrPtrType(pPacket) = type;
00078     *hdrPtrCode(pPacket) = code;
00079     checksum = 0;
00080     NetDirect16(hdrPtrChecksum(pPacket), &checksum);
00081     checksum = calculateChecksum(pSrcIp, pDstIp, size, pPacket);
00082     NetDirect16(hdrPtrChecksum(pPacket), &checksum);
00083     calculatedChecksum = 0;
00084 }
00085 static void (*pTraceBack)(void);
00086 static void trace()
00087 {
00088     pTraceBack();
00089     logHeader();
00090 }
00091 int Icmp6HandleReceivedPacket(void (*traceback)(void), int scope, char* pPacketRx, int sizeRx, char* pPacketTx, int* pSizeTx, char* pSrcIp, char* pDstIp)
00092 {
00093     pTraceBack = traceback;
00094         
00095     readHeader(pSrcIp, pDstIp, pPacketRx, sizeRx);
00096     
00097     int dataLengthRx =    sizeRx - HEADER_LENGTH;
00098     int dataLengthTx =  *pSizeTx - HEADER_LENGTH;
00099     char* pPayloadRx = pPacketRx + HEADER_LENGTH;
00100     char* pPayloadTx = pPacketTx + HEADER_LENGTH;
00101 
00102     int action = DO_NOTHING;
00103     switch (type)
00104     {
00105         case   1: //Destination unreacheable
00106             action = Dest6HandleRequest(trace, &type, &code);
00107             break;
00108         case 128: //Echo request - Ping
00109             action = Echo6HandleRequest(trace, &type, &code, pPayloadRx, dataLengthRx, pPayloadTx, &dataLengthTx);
00110             break;
00111         case 133: //Router solicit
00112             return DO_NOTHING; //We are not a router so quietly drop this
00113         case 134: //Router advertisement
00114             action = RaHandleReceivedAdvertisement(trace, pPayloadRx, &dataLengthRx);
00115             break;
00116         case 135: //Neighbour solicit
00117             action = NsHandleReceivedSolicitation(trace, &type, &code, pPayloadRx, dataLengthRx, pPayloadTx, &dataLengthTx);
00118             break;
00119         case 136: //Neighbour advertisement
00120             action = NsHandleReceivedAdvertisement(trace, pPayloadRx, &dataLengthRx);
00121             break;
00122         case 137: //Redirect
00123             return DO_NOTHING; //Not sure yet how to handle these so ignore
00124         default:
00125             LogTimeF("ICMP6 unknown packet type %d\r\n", type);
00126             return DO_NOTHING;
00127     }
00128     if (!action) return DO_NOTHING;
00129     
00130     Ip6AddrCopy(pDstIp, pSrcIp);
00131     SlaacAddressFromScope(scope,                     pSrcIp);
00132       Ip6AddrFromDest (ActionGetDestPart(action), pDstIp);
00133 
00134     *pSizeTx = HEADER_LENGTH + dataLengthTx;
00135     
00136     writeHeader(pPacketTx, *pSizeTx, pSrcIp, pDstIp);
00137     
00138     if (ActionGetTracePart(action)) logHeader();
00139     
00140     return action;
00141 }
00142 int Icmp6PollForPacketToSend(char* pPacket, int* pSize, char* pSrcIp, char* pDstIp)
00143 {    
00144     char* pData    = pPacket + HEADER_LENGTH;
00145     int dataLength =  *pSize - HEADER_LENGTH;
00146     int action  = DO_NOTHING;
00147     if (!action) action = RsGetWaitingSolicitation(pData, &dataLength, &type, &code);
00148     if (!action) action = NsGetWaitingSolicitation(pData, &dataLength, &type, &code);
00149     if (!action) return DO_NOTHING;
00150 
00151     int scope = SCOPE_LINK_LOCAL;
00152     SlaacAddressFromScope(scope,                     pSrcIp);
00153       Ip6AddrFromDest (ActionGetDestPart(action), pDstIp);
00154 
00155     *pSize = HEADER_LENGTH + dataLength;
00156     
00157     writeHeader(pPacket, *pSize, pSrcIp, pDstIp);
00158     
00159     if (ActionGetTracePart(action)) logHeader();
00160     
00161     return action;
00162 
00163 }