mac.c

00001 #include <stdbool.h>
00002 #include <string.h>
00003 #include <stdio.h>
00004 
00005 #include    "log.h"
00006 #include    "net.h"
00007 #include "action.h"
00008 #include    "eth.h"
00009 #include   "http.h"
00010 
00011 char MacLocal[6];
00012 void MacClear(char* mac)
00013 {
00014     memset(mac, 0, 6);
00015 }
00016 void MacCopy(char* macTo, const char* macFrom)
00017 {
00018     memcpy(macTo, macFrom, 6);
00019 }
00020 bool MacIsSame(const char* macA, const char* macB)
00021 {
00022     return memcmp(macA, macB, 6) == 0;
00023 }
00024 bool MacIsEmpty(const char* mac)
00025 {
00026            if (*mac) return false;
00027     mac++; if (*mac) return false;
00028     mac++; if (*mac) return false;
00029     mac++; if (*mac) return false;
00030     mac++; if (*mac) return false;
00031     mac++; if (*mac) return false;
00032     return true;
00033 }
00034 
00035 void MacParse(const char *text, char *mac)
00036 {
00037     MacClear(mac);
00038     int field = 0;
00039     int byte = 0;
00040     while(true)
00041     {
00042         switch (*text)
00043         {
00044             case ':':
00045                 mac[field] = byte;
00046                 field++;
00047                 if (field > 6) return;
00048                 byte = 0;
00049                 break;
00050             case '0': byte <<= 4; byte |= 0; break;
00051             case '1': byte <<= 4; byte |= 1; break;
00052             case '2': byte <<= 4; byte |= 2; break;
00053             case '3': byte <<= 4; byte |= 3; break;
00054             case '4': byte <<= 4; byte |= 4; break;
00055             case '5': byte <<= 4; byte |= 5; break;
00056             case '6': byte <<= 4; byte |= 6; break;
00057             case '7': byte <<= 4; byte |= 7; break;
00058             case '8': byte <<= 4; byte |= 8; break;
00059             case '9': byte <<= 4; byte |= 9; break;
00060             case 'a':
00061             case 'A': byte <<= 4; byte |= 10; break;
00062             case 'b':
00063             case 'B': byte <<= 4; byte |= 11; break;
00064             case 'c':
00065             case 'C': byte <<= 4; byte |= 12; break;
00066             case 'd':
00067             case 'D': byte <<= 4; byte |= 13; break;
00068             case 'e':
00069             case 'E': byte <<= 4; byte |= 14; break;
00070             case 'f':
00071             case 'F': byte <<= 4; byte |= 15; break;
00072             case 0:
00073                 mac[field] = byte;
00074                 return;
00075         }
00076         text++;
00077     }
00078 }
00079 int MacToString(const char* mac, int size, char* text)
00080 {
00081     return snprintf(text, size, "%02x:%02x:%02x:%02x:%02x:%02x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
00082 }
00083 int MacLog(const char* mac)
00084 {
00085     return LogF("%02x:%02x:%02x:%02x:%02x:%02x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
00086 }
00087 int MacHttp(const char* mac)
00088 {
00089     return HttpAddF("%02x:%02x:%02x:%02x:%02x:%02x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
00090 }
00091 
00092 int MacAccept(const char* p)
00093 {
00094     switch (*p)
00095     {
00096         case 0xff: //Broadcast
00097         {
00098             p++; if (*p != 0xff) return DO_NOTHING;
00099             p++; if (*p != 0xff) return DO_NOTHING;
00100             p++; if (*p != 0xff) return DO_NOTHING;
00101             p++; if (*p != 0xff) return DO_NOTHING;
00102             p++; if (*p != 0xff) return DO_NOTHING;
00103             return BROADCAST;
00104         }
00105         case 0x01: //Multicast IP4
00106         {
00107             p++; if (*p != 0x00) return DO_NOTHING; //01 00
00108             p++; if (*p != 0x5e) return DO_NOTHING; //01 00 5E
00109             p++; if (*p != 0x00) return DO_NOTHING; //01 00 5E 00
00110             p++;
00111             switch (*p)
00112             {
00113                 case 0x00: //01 00 5E 00 00
00114                     p++;
00115                     switch (*p) //01 00 5E 00 00 xx
00116                     {
00117                         case 0x01: return MULTICAST_NODE;
00118                         case 0x02: return MULTICAST_ROUTER;
00119                         case 0xfb: return MULTICAST_MDNS;
00120                         case 0xfc: return MULTICAST_LLMNR;
00121                         default:   return DO_NOTHING;
00122                     }
00123                 case 0x01: //01 00 5E 00 01
00124                     p++;
00125                     switch (*p) //01 00 5E 00 01 xx
00126                     {
00127                         case 0x01: return MULTICAST_NTP;
00128                         default:   return DO_NOTHING;
00129                     }
00130                 default: return DO_NOTHING;
00131             }
00132         }
00133         case 0x33: //Multicast IP6
00134         {
00135             p++; if (*p != 0x33) return DO_NOTHING;
00136             p++;
00137             switch (*p) //33 33
00138             {
00139                 case 0x00: //33 33 00 Special address
00140                 {
00141                     p++;
00142                     switch (*p)
00143                     {
00144                         case 0x00: //33 33 00 00
00145                         {
00146                             p++; 
00147                             switch (*p)
00148                             {
00149                                 case 0x00:
00150                                     p++;
00151                                     switch (*p) //33 33 00 00 00 xx
00152                                     {
00153                                         case 0x01: return MULTICAST_NODE;
00154                                         case 0x02: return MULTICAST_ROUTER;
00155                                         case 0xfb: return MULTICAST_MDNS;
00156                                         default:   return DO_NOTHING;
00157                                     }
00158                                 case 0x01:
00159                                     p++;
00160                                     switch (*p) //33 33 00 00 01 xx
00161                                     {
00162                                         case 0x01: return MULTICAST_NTP;
00163                                         default:   return DO_NOTHING;
00164                                     }
00165                                 default: return DO_NOTHING;
00166                             }
00167                         }
00168                         case 0x01: //33 33 00 01
00169                         {
00170                             p++; if (*p != 0x00) return DO_NOTHING;
00171                             p++;
00172                             switch (*p) //33 33 00 01 00 xx
00173                             {
00174                                 case 0x03: return MULTICAST_LLMNR;
00175                                 default:   return DO_NOTHING;
00176                             }
00177                         }
00178                         default: return DO_NOTHING;
00179                     }
00180                 }
00181                 case 0xff: //33 33 FF LL LL LL Solicited address
00182                 {
00183                     char* q = MacLocal + 3;
00184                     p++; if (*p != *q++) return DO_NOTHING;
00185                     p++; if (*p != *q++) return DO_NOTHING;
00186                     p++; if (*p != *q++) return DO_NOTHING;
00187                     return SOLICITED_NODE;
00188                 }
00189                 default: return DO_NOTHING;
00190             }
00191         }
00192         default: //Unicast to me
00193         {
00194             char* q = MacLocal;
00195                  if (*p != *q++) return DO_NOTHING;
00196             p++; if (*p != *q++) return DO_NOTHING;
00197             p++; if (*p != *q++) return DO_NOTHING;
00198             p++; if (*p != *q++) return DO_NOTHING;
00199             p++; if (*p != *q++) return DO_NOTHING;
00200             p++; if (*p != *q)   return DO_NOTHING;
00201             return UNICAST;
00202         }
00203     }
00204 }
00205 
00206 void MacMakeFromDest(int dest, int pro, char* p)
00207 {
00208     if (dest == BROADCAST)       { *p++ = 0xff; *p++ = 0xff; *p++ = 0xff; *p++ = 0xff; *p++ = 0xff; *p = 0xff; return; }
00209     
00210     if (pro == ETH_IPV4)
00211     {
00212         switch (dest)
00213         {
00214             case MULTICAST_NODE:   *p++ = 0x01; *p++ = 0x00; *p++ = 0x5e; *p++ = 0x00; *p++ = 0x00; *p = 0x01; break;
00215             case MULTICAST_ROUTER: *p++ = 0x01; *p++ = 0x00; *p++ = 0x5e; *p++ = 0x00; *p++ = 0x00; *p = 0x02; break;
00216             case MULTICAST_MDNS:   *p++ = 0x01; *p++ = 0x00; *p++ = 0x5e; *p++ = 0x00; *p++ = 0x00; *p = 0xfb; break;
00217             case MULTICAST_LLMNR:  *p++ = 0x01; *p++ = 0x00; *p++ = 0x5e; *p++ = 0x00; *p++ = 0x00; *p = 0xfc; break;
00218             case MULTICAST_NTP:    *p++ = 0x01; *p++ = 0x00; *p++ = 0x5e; *p++ = 0x00; *p++ = 0x01; *p = 0x01; break;
00219         }
00220     }
00221     else
00222     {
00223         switch (dest)
00224         {
00225             case MULTICAST_NODE:   *p++ = 0x33; *p++ = 0x33; *p++ = 0x00; *p++ = 0x00; *p++ = 0x00; *p = 0x01; break;
00226             case MULTICAST_ROUTER: *p++ = 0x33; *p++ = 0x33; *p++ = 0x00; *p++ = 0x00; *p++ = 0x00; *p = 0x02; break;
00227             case MULTICAST_MDNS:   *p++ = 0x33; *p++ = 0x33; *p++ = 0x00; *p++ = 0x00; *p++ = 0x00; *p = 0xfb; break;
00228             case MULTICAST_LLMNR:  *p++ = 0x33; *p++ = 0x33; *p++ = 0x00; *p++ = 0x01; *p++ = 0x00; *p = 0x03; break;
00229             case MULTICAST_NTP:    *p++ = 0x33; *p++ = 0x33; *p++ = 0x00; *p++ = 0x00; *p++ = 0x01; *p = 0x01; break;
00230         }
00231     }
00232 }