| 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 | } |