src/libs/Network/net_util.cpp

   1 #include "net_util.h"
   2
   3 #include <cstdio>
   4 #include <cstring>
   5
   6 static uint8_t hexalpha[] = "0123456789ABCDEF";
   7 const uint8_t broadcast[6] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
   8
   9 bool compare_mac(const uint8_t* mac1, const uint8_t* mac2, const uint8_t* mask)
  10 {
  11     uint8_t m;
  12     for (int i = 0; i < 6; i++)
  13     {
  14         m = 0xFF;
  15         if (mask)
  16             m = mask[i];
  17         if ((mac1[i] & m) != (mac2[i] & m))
  18             return false;
  19     }
  20     return true;
  21 }
  22
  23 uint32_t unaligned_u32(uint8_t* buf)
  24 {
  25     return buf[0] | (buf[1] << 8) | (buf[2] << 16) | (buf[3] << 24);
  26 }
  27
  28 uint16_t unaligned_u16(uint8_t* buf)
  29 {
  30     return buf[0] | (buf[1] << 8);
  31 }
  32
  33 int format_ip(uint32_t ip, uint8_t* buf)
  34 {
  35     uint8_t *p = (uint8_t*) &ip;
  36     return snprintf((char*) buf, IP_STR_LEN, "%d.%d.%d.%d", p[3], p[2], p[1], p[0]);
  37 }
  38
  39 int format_mac(uint8_t mac[6], uint8_t buf[MAC_STR_LEN])
  40 {
  41     if (compare_mac(mac, broadcast, broadcast))
  42     {
  43         memcpy(buf, "[Broadcast      ]", MAC_STR_LEN);
  44         return MAC_STR_LEN - 1;
  45     }
  46
  47     int i;
  48     for (i = 0; i < 12; i++)
  49     {
  50         buf[i + (i >> 1)] = hexalpha[(mac[i >> 1] >> ((i & 1)?0:4)) & 0x0F];
  51         if (i < 5)
  52             buf[(i * 3) + 2] = ':';
  53     }
  54     buf[MAC_STR_LEN - 1] = 0;
  55     return MAC_STR_LEN - 1;
  56 }
  57
  58 int checksum16(uint8_t* buf, int count, int start)
  59 {
  60     /* Compute Internet Checksum for "count" bytes
  61      *         beginning at location "addr".
  62      */
  63     register uint32_t sum = start;
  64
  65     while( count > 1 )  {
  66         /*  This is the inner loop */
  67         sum += unaligned_u16(buf);
  68         buf += 2;
  69         count -= 2;
  70     }
  71
  72     /*  Add left-over byte, if any */
  73     if( count > 0 )
  74         sum += * (uint8_t *) buf;
  75
  76     /*  Fold 32-bit sum to 16 bits */
  77     while (sum & 0xFFFF0000)
  78         sum = (sum & 0xFFFF) + (sum >> 16);
  79
  80     return (~sum) & 0xFFFF;
  81 }
  82
  83 uint32_t crc32(uint8_t* buf, int length)
  84 {
  85     static const uint32_t crc32_table[] =
  86     {
  87         0x4DBDF21C, 0x500AE278, 0x76D3D2D4, 0x6B64C2B0,
  88         0x3B61B38C, 0x26D6A3E8, 0x000F9344, 0x1DB88320,
  89         0xA005713C, 0xBDB26158, 0x9B6B51F4, 0x86DC4190,
  90         0xD6D930AC, 0xCB6E20C8, 0xEDB71064, 0xF0000000
  91     };
  92
  93     int n;
  94     uint32_t crc=0;
  95
  96     for (n = 0; n < length; n++)
  97     {
  98         crc = (crc >> 4) ^ crc32_table[(crc ^ (buf[n] >> 0)) & 0x0F];  /* lower nibble */
  99         crc = (crc >> 4) ^ crc32_table[(crc ^ (buf[n] >> 4)) & 0x0F];  /* upper nibble */
 100     }
 101
 102     return crc;
 103 }