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