Michael Spencer / Smoothie

Fork of Smoothie to port to mbed non-LPC targets.

Dependencies:   mbed

Fork of Smoothie by Stéphane Cachat

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?

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