Diff: eth/eth.cpp

Revision:

10:f0854784e960

Child:

11:c051adb70c5a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/eth/eth.cpp	Sun Apr 16 14:21:55 2017 +0000
@@ -0,0 +1,176 @@
+#include "mbed.h"
+#include   "io.h"
+#include  "log.h"
+#include  "net.h"
+#include  "arp.h"
+#include  "ip4.h"
+#include  "ip6.h"
+#include  "phy.h"
+#include  "eth.h"
+
+#define HEADER_SIZE 14
+__packed struct header
+{
+    char     dst[6];
+    char     src[6];
+    uint16_t typ;
+};
+
+char EthLocalMac[6];
+
+static void typeToString(uint16_t type, int size, char* text)
+{
+    switch (type)
+    {
+        case ARP:  strncpy (text, "ARP" , size);        break;
+        case IPV4: strncpy (text, "IPV4", size);        break;
+        case IPV6: strncpy (text, "IPV6", size);        break;
+        default:   snprintf(text, size, "%04hX", type); break;
+    }
+}
+static bool getIsSolicited(char* p)
+{
+    if (*p++ != 0x33) return false;
+    if (*p++ != 0x33) return false;
+    if (*p++ != 0xff) return false;
+    return true;
+}
+static bool getIsSame(char* pA, char* pB)
+{
+    return memcmp(pA, pB, 6) == 0;
+}
+static bool getIsSameGroup(char* pA, char* pB)
+{
+    pA += 3;
+    pB += 3;
+    if (*pA++ != *pB++) return false;
+    if (*pA++ != *pB++) return false;
+    return *pA == *pB;
+}
+
+static void finalisePacket(int action, int type, int dataLength, void* pPacket, int* pSize)
+{
+    struct header * pHeader = (header*)pPacket;
+
+    switch (action)
+    {
+        case DO_NOTHING:
+            return;
+        case UNICAST:
+        case UNICAST_DNS:
+        case UNICAST_DHCP:
+            break;
+        case MULTICAST_NODE:
+            pHeader->dst[0] = type == IPV6 ? 0x33 : 0x01;
+            pHeader->dst[1] = type == IPV6 ? 0x33 : 0x00;
+            pHeader->dst[2] = type == IPV6 ? 0x00 : 0x5e;
+            pHeader->dst[3] = 0x00;
+            pHeader->dst[4] = 0x00;
+            pHeader->dst[5] = 0x01;
+            break;
+        case MULTICAST_ROUTER:
+            pHeader->dst[0] = type == IPV6 ? 0x33 : 0x01;
+            pHeader->dst[1] = type == IPV6 ? 0x33 : 0x00;
+            pHeader->dst[2] = type == IPV6 ? 0x00 : 0x5e;
+            pHeader->dst[3] = 0x00;
+            pHeader->dst[4] = 0x00;
+            pHeader->dst[5] = 0x02;
+            break;
+        case MULTICAST_MDNS:
+            pHeader->dst[0] = type == IPV6 ? 0x33 : 0x01;
+            pHeader->dst[1] = type == IPV6 ? 0x33 : 0x00;
+            pHeader->dst[2] = type == IPV6 ? 0x00 : 0x5e;
+            pHeader->dst[3] = 0x00;
+            pHeader->dst[4] = 0x00;
+            pHeader->dst[5] = 0xfb;
+            break;
+        case MULTICAST_LLMNR:
+            pHeader->dst[0] = type == IPV6 ? 0x33 : 0x01;
+            pHeader->dst[1] = type == IPV6 ? 0x33 : 0x00;
+            pHeader->dst[2] = type == IPV6 ? 0x00 : 0x5e;
+            pHeader->dst[3] = type == IPV6 ? 0x01 : 0x00;
+            pHeader->dst[4] = type == IPV6 ? 0x00 : 0x00;
+            pHeader->dst[5] = type == IPV6 ? 0x03 : 0xfc;
+            break;
+        case BROADCAST:
+            memset(pHeader->dst, 0xFF, 6); //Set to broadcast
+            break;
+        default:
+            LogTimeF("Unknown ETH action %d\r\n", action);
+            return;
+    }
+    memcpy(pHeader->src,  EthLocalMac, 6);        //Put our MAC into the source
+    pHeader->typ = NetToHost16(type);
+    
+    *pSize = HEADER_SIZE + dataLength;
+}
+int EthHandlePacket(void* pPacket, int* pSize)
+{
+    struct header * pHeader = (header*)pPacket;
+    int dataLength = *pSize - HEADER_SIZE;
+    void* pData = (char*)pPacket + HEADER_SIZE;
+    
+    bool isSpanningTree = pHeader->dst[0] == 0x01 && pHeader->dst[1] == 0x80 && pHeader->dst[2] == 0xC2;
+    
+    bool isMe           = getIsSame(pHeader->dst, EthLocalMac);
+    bool isMulticast    = pHeader->dst[0] & 0x01;
+    bool isSolicited    = getIsSolicited(pHeader->dst);
+    bool isGroup        = isSolicited && getIsSameGroup(pHeader->dst, EthLocalMac);
+    
+    bool doIt = isMe || (isMulticast && !isSolicited) || isGroup;
+    
+    if (!doIt) return DO_NOTHING;
+
+    if (isSpanningTree) return DO_NOTHING; //Drop - these multicast messages come from Sonos devices to prevent issues between wireless and wired.
+    
+    uint16_t type = NetToHost16(pHeader->typ);
+    if (type < 1500) return DO_NOTHING; //drop 802.3 messages
+
+    int   action = DO_NOTHING;
+    switch (type)
+    {
+        case ARP:  action = ArpHandleReceivedPacket(pHeader->src, pData, &dataLength, pHeader->dst); break;
+        case IPV4: action = Ip4HandleReceivedPacket(pHeader->src, pData, &dataLength, pHeader->dst); break;
+        case IPV6: action = Ip6HandleReceivedPacket(pHeader->src, pData, &dataLength, pHeader->dst); break;
+        default:
+            char text[20];
+            LogTimeF("\r\nEthernet packet not handled\r\n");
+            NetMacToString(pHeader->dst, sizeof(text), text);
+            LogTimeF("Destination:  %s\r\n", text);
+            NetMacToString(pHeader->src, sizeof(text), text);
+            LogTimeF("Source:       %s\r\n", text);
+            typeToString(type, sizeof(text), text);
+            LogTimeF("EtherType:    %s\r\n", text);        
+            break;
+    }
+    
+    finalisePacket(action, type, dataLength, pPacket, pSize);
+    
+    return action;
+}
+int EthPollForPacketToSend(void* pPacket, int* pSize)
+{
+    struct header * pHeader = (header*)pPacket;
+    void* pData = (char*)pPacket + HEADER_SIZE;
+    
+    int dataLength = 0;
+    int type = 0;
+    int action = DO_NOTHING;
+
+    if (action == DO_NOTHING)
+    {
+        action = Ip6PollForPacketToSend(pData, &dataLength, pHeader->dst);
+        type = IPV6;
+    }
+    
+    if (action == DO_NOTHING)
+    {
+        action = Ip4PollForPacketToSend(pData, &dataLength, pHeader->dst);
+        type = IPV4;
+    }
+    
+    finalisePacket(action, type, dataLength, pPacket, pSize);
+    
+    return action;
+}
+