Diff: eth/eth.cpp

Revision:

13:9cd54f7db57a

Parent:

11:c051adb70c5a

Child:

14:e75a59c1123d

--- a/eth/eth.cpp	Thu Apr 20 13:50:30 2017 +0000
+++ b/eth/eth.cpp	Mon May 01 18:20:55 2017 +0000
@@ -7,8 +7,12 @@
 #include  "ip6.h"
 #include  "phy.h"
 #include  "eth.h"
+#include  "mac.h"
 
 #define HEADER_SIZE 14
+
+#define DEBUG false
+
 __packed struct header
 {
     char     dst[6];
@@ -16,8 +20,6 @@
     uint16_t typ;
 };
 
-char EthLocalMac[6];
-
 static void typeToString(uint16_t type, int size, char* text)
 {
     switch (type)
@@ -28,25 +30,6 @@
         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)
 {
@@ -63,67 +46,60 @@
         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;
+            if (type == IPV6) MacMakeMulticastNode6(pHeader->dst);
+            else              MacMakeMulticastNode4(pHeader->dst);
             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;
+            if (type == IPV6) MacMakeMulticastRouter6(pHeader->dst);
+            else              MacMakeMulticastRouter4(pHeader->dst);
             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;
+            if (type == IPV6) MacMakeMulticastMdns6(pHeader->dst);
+            else              MacMakeMulticastMdns4(pHeader->dst);
             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;
+            if (type == IPV6) MacMakeMulticastLlmnr6(pHeader->dst);
+            else              MacMakeMulticastLlmnr4(pHeader->dst);
             break;
         case BROADCAST:
-            memset(pHeader->dst, 0xFF, 6); //Set to broadcast
+            MacMakeBroadcast(pHeader->dst);
             break;
         default:
             LogTimeF("Unknown ETH action %d\r\n", action);
             return;
     }
-    memcpy(pHeader->src,  EthLocalMac, 6);        //Put our MAC into the source
+    memcpy(pHeader->src, MacLocal, 6);        //Put our MAC into the source
     pHeader->typ = NetToHost16(type);
     
     *pSize = HEADER_SIZE + dataLength;
 }
+void LogHeader(struct header* pHeader, char* title)
+{
+    char text[20];
+    LogTimeF("ETH %s\r\n", title);
+    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(NetToHost16(pHeader->typ), sizeof(text), text);
+    LogTimeF("EtherType:    %s\r\n", text);        
+}
 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;
+    bool doIt = MacCompareUnicastLocal        (pHeader->dst) || MacCompareBroadcast           (pHeader->dst) ||
+                MacCompareMulticastLocal4     (pHeader->dst) || MacCompareMulticastLocal6     (pHeader->dst) ||
+                MacCompareMulticastAllNodes4  (pHeader->dst) || MacCompareMulticastAllNodes6  (pHeader->dst) ||
+                MacCompareMulticastAllRouters4(pHeader->dst) || MacCompareMulticastAllRouters6(pHeader->dst) ||
+                MacCompareMulticastMdns4      (pHeader->dst) || MacCompareMulticastMdns6      (pHeader->dst) ||
+                MacCompareMulticastLlmnr4     (pHeader->dst) || MacCompareMulticastLlmnr6     (pHeader->dst);
     
     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.
+    
+    if (DEBUG) LogHeader(pHeader, "received packet");
     
     uint16_t type = NetToHost16(pHeader->typ);
     if (type < 1500) return DO_NOTHING; //drop 802.3 messages
@@ -135,19 +111,11 @@
         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);        
+            LogHeader(pHeader, "packet not handled");
             break;
     }
     
     finalisePacket(action, type, dataLength, pPacket, pSize);
-    
     return action;
 }
 int EthPollForPacketToSend(void* pPacket, int* pSize)
@@ -173,6 +141,8 @@
     
     finalisePacket(action, type, dataLength, pPacket, pSize);
     
+    if (DEBUG) LogHeader(pHeader, "sent packet");
+    
     return action;
 }