Diff: eth/mac.cpp

Revision:

60:1d8c7a1e7483

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/eth/mac.cpp	Sat Dec 16 14:39:50 2017 +0000
@@ -0,0 +1,159 @@
+#include   "mbed.h"
+#include    "log.h"
+#include    "net.h"
+#include "action.h"
+#include    "eth.h"
+#include   "http.h"
+
+char MacLocal[6];
+void MacClear(char* mac)
+{
+    memset(mac, 0, 6);
+}
+void MacCopy(char* macTo, char* macFrom)
+{
+    memcpy(macTo, macFrom, 6);
+}
+bool MacIsSame(char* macA, char* macB)
+{
+    return memcmp(macA, macB, 6) == 0;
+}
+bool MacIsEmpty(char* mac)
+{
+           if (*mac) return false;
+    mac++; if (*mac) return false;
+    mac++; if (*mac) return false;
+    mac++; if (*mac) return false;
+    mac++; if (*mac) return false;
+    mac++; if (*mac) return false;
+    return true;
+}
+
+int MacToString(char* mac, int size, char* text)
+{
+    return snprintf(text, size, "%02x:%02x:%02x:%02x:%02x:%02x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+}
+int MacLog(char* mac)
+{
+    return LogF("%02x:%02x:%02x:%02x:%02x:%02x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+}
+int MacHttp(char* mac)
+{
+    return HttpAddF("%02x:%02x:%02x:%02x:%02x:%02x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+}
+
+int MacAccept(char* p)
+{
+    switch (*p)
+    {
+        case 0xff: //Broadcast
+        {
+            p++; if (*p != 0xff) return DO_NOTHING;
+            p++; if (*p != 0xff) return DO_NOTHING;
+            p++; if (*p != 0xff) return DO_NOTHING;
+            p++; if (*p != 0xff) return DO_NOTHING;
+            p++; if (*p != 0xff) return DO_NOTHING;
+            return BROADCAST;
+        }
+        case 0x01: //Multicast IP4
+        {
+            p++; if (*p != 0x00) return DO_NOTHING;
+            p++; if (*p != 0x5e) return DO_NOTHING;
+            p++; if (*p != 0x00) return DO_NOTHING;
+            p++; if (*p != 0x00) return DO_NOTHING;
+            p++;
+            switch (*p) //01 00 5E 00 00 xx
+            {
+                case 0x01: return MULTICAST_NODE;
+                case 0x02: return MULTICAST_ROUTER;
+                case 0xfb: return MULTICAST_MDNS;
+                case 0xfc: return MULTICAST_LLMNR;
+                default:   return DO_NOTHING;
+            }
+        }
+        case 0x33: //Multicast IP6
+        {
+            p++; if (*p != 0x33) return DO_NOTHING;
+            p++;
+            switch (*p) //33 33
+            {
+                case 0x00: //33 33 00 Special address
+                {
+                    p++;
+                    switch (*p)
+                    {
+                        case 0x00: //33 33 00 00
+                        {
+                            p++; if (*p != 0x00) return DO_NOTHING;
+                            p++;
+                            switch (*p) //33 33 00 00 00 xx
+                            {
+                                case 0x01: return MULTICAST_NODE;
+                                case 0x02: return MULTICAST_ROUTER;
+                                case 0xfb: return MULTICAST_MDNS;
+                                default:   return DO_NOTHING;
+                            }
+                        }
+                        case 0x01: //33 33 00 01
+                        {
+                            p++; if (*p != 0x00) return DO_NOTHING;
+                            p++;
+                            switch (*p) //33 33 00 01 00 xx
+                            {
+                                case 0x03: return MULTICAST_LLMNR;
+                                default:   return DO_NOTHING;
+                            }
+                        }
+                        default: return DO_NOTHING;
+                    }
+                }
+                case 0xff: //33 33 FF LL LL LL Solicited address
+                {
+                    char* q = MacLocal + 3;
+                    p++; if (*p != *q++) return DO_NOTHING;
+                    p++; if (*p != *q++) return DO_NOTHING;
+                    p++; if (*p != *q++) return DO_NOTHING;
+                    return SOLICITED_NODE;
+                }
+                default: return DO_NOTHING;
+            }
+        }
+        default: //Unicast to me
+        {
+            char* q = MacLocal;
+                 if (*p != *q++) return DO_NOTHING;
+            p++; if (*p != *q++) return DO_NOTHING;
+            p++; if (*p != *q++) return DO_NOTHING;
+            p++; if (*p != *q++) return DO_NOTHING;
+            p++; if (*p != *q++) return DO_NOTHING;
+            p++; if (*p != *q)   return DO_NOTHING;
+            return UNICAST;
+        }
+    }
+}
+
+void MacMakeFromDest(int dest, int pro, char* p)
+{
+    if (dest == BROADCAST)       { *p++ = 0xff; *p++ = 0xff; *p++ = 0xff; *p++ = 0xff; *p++ = 0xff; *p = 0xff; return; }
+    
+    if (pro == IPV4)
+    {
+        switch (dest)
+        {
+            case MULTICAST_NODE:   *p++ = 0x01; *p++ = 0x00; *p++ = 0x5e; *p++ = 0x00; *p++ = 0x00; *p = 0x01; break;
+            case MULTICAST_ROUTER: *p++ = 0x01; *p++ = 0x00; *p++ = 0x5e; *p++ = 0x00; *p++ = 0x00; *p = 0x02; break;
+            case MULTICAST_MDNS:   *p++ = 0x01; *p++ = 0x00; *p++ = 0x5e; *p++ = 0x00; *p++ = 0x00; *p = 0xfb; break;
+            case MULTICAST_LLMNR:  *p++ = 0x01; *p++ = 0x00; *p++ = 0x5e; *p++ = 0x00; *p++ = 0x00; *p = 0xfc; break;
+        }
+    }
+    else
+    {
+        switch (dest)
+        {
+            case MULTICAST_NODE:   *p++ = 0x33; *p++ = 0x33; *p++ = 0x00; *p++ = 0x00; *p++ = 0x00; *p = 0x01; break;
+            case MULTICAST_ROUTER: *p++ = 0x33; *p++ = 0x33; *p++ = 0x00; *p++ = 0x00; *p++ = 0x00; *p = 0x02; break;
+            case MULTICAST_MDNS:   *p++ = 0x33; *p++ = 0x33; *p++ = 0x00; *p++ = 0x00; *p++ = 0x00; *p = 0xfb; break;
+            case MULTICAST_LLMNR:  *p++ = 0x33; *p++ = 0x33; *p++ = 0x00; *p++ = 0x01; *p++ = 0x00; *p = 0x03; break;
+        }
+    }
+}