Diff: ip4/ip4.cpp

Revision:

10:f0854784e960

Child:

11:c051adb70c5a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ip4/ip4.cpp	Sun Apr 16 14:21:55 2017 +0000
@@ -0,0 +1,225 @@
+#include "mbed.h"
+#include  "log.h"
+#include  "net.h"
+#include "icmp.h"
+#include "udptcp4.h"
+#include   "ar.h"
+#include "dhcp.h"
+#include  "eth.h"
+#include  "ip4.h"
+
+#define DEBUG false
+
+#define HEADER_LENGTH 20
+
+__packed struct header
+{
+    uint8_t  versionIhl;
+    uint8_t  tos;
+    uint16_t length;
+    uint16_t id;
+    uint16_t flagsOffset;
+    uint8_t  ttl;
+    uint8_t  protocol;
+    uint16_t checksum;
+    uint32_t src;
+    uint32_t dst;
+};
+
+//Header variables
+static uint8_t      version;
+static int     headerLength;
+static uint8_t          tos;
+static uint16_t          id;
+static bool    dontFragment;
+static bool    moreFragments;
+static uint16_t      offset;
+static uint8_t          ttl;
+static uint8_t     protocol;
+static uint16_t    checksum;
+static uint16_t     calcsum;
+static uint32_t       srcIp;
+static uint32_t       dstIp;
+static void*          pData;
+static int       dataLength;
+
+static void protocolToString(uint16_t protocol, int size, char* text)
+{
+    switch (protocol)
+    {
+        case ICMP: strncpy (text, "ICMP", size);         break;
+        case  TCP: strncpy (text, "TCP" , size);         break;
+        case  UDP: strncpy (text, "UDP" , size);         break;
+        default:   snprintf(text, size, "%d", protocol); break;
+    }
+}
+
+void logHeader(char* title)
+{
+    char text[30];
+    LogTimeF("%s\r\n", title);
+    LogF("  Version           %d\r\n", version);
+    LogF("  Header length     %d\r\n", headerLength);
+    LogF("  Type of service   %d\r\n", tos);
+    LogF("  Data length       %d\r\n", dataLength);
+    LogF("  Identification    %d\r\n", id);
+    if (dontFragment)  LogF("  Don't fragment\r\n");
+    else               LogF("  Do fragment\r\n");
+    if (moreFragments) LogF("  More fragments\r\n");
+    else               LogF("  No more fragments\r\n");
+    LogF("  Offset            %d\r\n", offset);
+    LogF("  Time to live      %d\r\n", ttl);
+    protocolToString(protocol, sizeof(text), text);
+    LogF("  Protocol          %s\r\n", text);
+    LogF("  Checksum (hex)    %04hX\r\n", checksum);
+    LogF("  Calculated (hex)  %04hX\r\n",  calcsum);
+    NetIp4AddressToString(srcIp, sizeof(text), text);
+    LogF("  Source IP         %s\r\n", text);
+    NetIp4AddressToString(dstIp, sizeof(text), text);
+    LogF("  Destination IP    %s\r\n", text);
+}
+void readHeader(struct header * pHeader)
+{
+             version       =             pHeader->versionIhl >> 4;
+    uint8_t  ihl           =             pHeader->versionIhl & 0xF;
+             headerLength  =             ihl * 4;
+             tos           =             pHeader->tos;
+    uint16_t totalLength   = NetToHost16(pHeader->length);
+             id            = NetToHost16(pHeader->id);
+    uint16_t flagsOffset   = NetToHost16(pHeader->flagsOffset);
+             dontFragment  =             flagsOffset & 0x4000;
+             moreFragments =             flagsOffset & 0x8000;
+             offset        =             flagsOffset & 0x1FFF;
+             ttl           =             pHeader->ttl;
+             protocol      =             pHeader->protocol;
+             checksum      = NetToHost16(pHeader->checksum);
+             calcsum       = NetCheckSum(headerLength, pHeader);
+             srcIp         =             pHeader->src;
+             dstIp         =             pHeader->dst;
+             pData         =      (char*)pHeader + headerLength;
+             dataLength    =         totalLength - headerLength;
+}
+void writeHeader(struct header * pHeader)
+{
+    uint16_t flagsOffset = offset;
+    if (dontFragment)  flagsOffset |= 0x4000;
+    if (moreFragments) flagsOffset |= 0x8000;
+
+    uint8_t ihl          = headerLength >> 2;
+    pHeader->versionIhl  = (version << 4) + ihl;
+    pHeader->tos         = tos;
+    pHeader->id          = NetToHost16(id);
+    pHeader->flagsOffset = NetToHost16(flagsOffset);
+    pHeader->ttl         = ttl;
+    pHeader->protocol    = protocol;
+    
+    pHeader->dst         = dstIp;
+    pHeader->src         = srcIp;
+    pHeader->length      = NetToHost16(headerLength + dataLength);
+    pHeader->checksum    = 0;
+    pHeader->checksum    = NetCheckSum(headerLength, pHeader);
+    calcsum              = 0;
+}
+int Ip4HandleReceivedPacket(char* pSrcMac, void* pPacket, int* pSize, char* pDstMac)
+{
+    struct header * pHeader = (header*)pPacket;
+    readHeader(pHeader);
+    
+    bool isMe        = dstIp == DhcpLocalIp;
+    bool isBroadcast = dstIp == IP4_BROADCAST_ADDRESS;
+    bool isMulticast = (dstIp & 0xE0) == 0xE0; //224.x.x.x == 1110 0000 == E0.xx.xx.xx == xx.xx.xx.E0 in little endian
+    
+    if (!isMe && !isBroadcast && !isMulticast) return DO_NOTHING;
+    
+    ArAdd4(pSrcMac, srcIp);
+    
+    if (DEBUG) logHeader("IP4 packet received");
+
+    int action = DO_NOTHING;
+    switch (protocol)
+    {
+        case ICMP: action = IcmpHandleReceivedPacket(&srcIp, &dstIp, &dataLength, pData); break;
+        case UDP:  action = Udp4HandleReceivedPacket(&srcIp, &dstIp, &dataLength, pData); break;
+        case TCP:  action = Tcp4HandleReceivedPacket(&srcIp, &dstIp, &dataLength, pData); break;        
+        default:
+            logHeader("IP4 packet unhandled");
+            return DO_NOTHING;
+    }
+    
+    switch (action)
+    {
+        case DO_NOTHING:
+            return DO_NOTHING;
+        case UNICAST:
+        case UNICAST_DNS:
+        case UNICAST_DHCP:
+            memcpy(pDstMac, pSrcMac, 6);
+            break;
+        case MULTICAST_NODE:
+        case MULTICAST_ROUTER:
+        case MULTICAST_MDNS:
+        case MULTICAST_LLMNR:
+        case BROADCAST:
+            break;
+        default:
+            LogTimeF("Ip4HandleReceivedPacket unknown action %d\r\n", action);
+            return DO_NOTHING;
+    }
+
+    if (DEBUG) logHeader("IP4 packet replied to");
+    
+    writeHeader(pHeader);
+    
+    *pSize = headerLength + dataLength;
+    
+    return action;
+}
+int Ip4PollForPacketToSend(void* pPacket, int* pSize, char* pDstMac)
+{
+    struct header * pHeader = (header*)pPacket;
+    
+    headerLength = HEADER_LENGTH;
+    pData = (char*)pPacket + headerLength;
+    
+    srcIp = 0;
+    dstIp = 0;
+    
+    dataLength = 0;
+    int action  = DO_NOTHING;
+    if (!action) action = Udp4PollForPacketToSend(pData, &dataLength, &srcIp, &dstIp);
+    
+    switch (action)
+    {
+        case DO_NOTHING:
+            return DO_NOTHING;
+        case UNICAST:
+        case UNICAST_DNS:
+        case UNICAST_DHCP:
+            ArRev4(dstIp, pDstMac); //Make the remote MAC from ARP
+            break;
+        case MULTICAST_MDNS:
+        case MULTICAST_LLMNR:
+        case BROADCAST:
+            break;
+        default:
+            LogTimeF("Ip4PollForPacketToSend unknown action %d\r\n", action);
+            return DO_NOTHING;
+    }
+
+    version       = 4;
+    tos           = 0;
+    id            = 0;
+    dontFragment  = true;
+    moreFragments = false;
+    offset        = 0;
+    ttl           = 255;
+    protocol      = UDP;
+    
+    if (DEBUG) logHeader("IP4 packet replied to");
+
+    writeHeader(pHeader);
+    
+    *pSize = headerLength + dataLength;
+    
+    return action;
+}