Commit | Line | Data |
---|---|---|
618e6b09 MM |
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 | } |