cobs.c (2712B)
1 #include <stdio.h> 2 #include <stdlib.h> 3 #include <assert.h> 4 #include <string.h> 5 6 size_t cobs_encoded_max_size(size_t size) 7 { 8 return 2 + size + size/254; 9 } 10 11 size_t cobs_encode(void* buf, void const* data, size_t len) 12 { 13 unsigned char const* in = data, *end = in + len; 14 unsigned char* out = buf; 15 unsigned char* code_out = out++; 16 17 while (in != end) 18 { 19 if (out - code_out == 0xFF) 20 { 21 *code_out = 0xFF; 22 code_out = out++; 23 continue; 24 } 25 26 if (*in) 27 { 28 *out++ = *in; 29 } 30 else 31 { 32 *code_out = out - code_out; 33 code_out = out++; 34 } 35 ++in; 36 } 37 38 *code_out = out - code_out; 39 *out++ = 0; 40 41 assert(out - (unsigned char*)buf <= (ssize_t)cobs_encoded_max_size(len)); 42 return out - (unsigned char*)buf; 43 } 44 45 size_t cobs_decode(void* buf, void const* data, size_t len) 46 { 47 unsigned char const* in = data, *end = in + len; 48 unsigned char* out = buf; 49 unsigned char n = 0xFF; 50 51 if (!in[len-1]) // not zero terminated 52 return 0; 53 54 while (*in) 55 { 56 if (n != 0xFF) 57 *out++ = 0; 58 59 n = *in; 60 61 if (in + n >= end) // overrun 62 return 0; 63 64 memcpy(out, in + 1, n - 1); 65 out += n - 1; 66 in += n; 67 } 68 69 if (in + 1 != end) // underrun 70 return 0; 71 72 return out - (unsigned char*)buf; 73 } 74 75 struct 76 { 77 size_t de_siz; 78 char* decoded, *encoded; 79 } const tests[] = { 80 {1, "\0", "\x01\x01"}, 81 {1, "A", "\x02" "A"}, 82 {2, "\0\0", "\x01\x01\x01"}, 83 {4, "AB\0C", "\x03""AB\x02""C"}, 84 {4, "ABCD", "\x05""ABCD"}, 85 {4, "A\0\0\0", "\x02""A\x01\x01\x01"}, 86 }; 87 88 void encode_and_print(void* in, size_t size) 89 { 90 unsigned char buf[1024]; 91 size_t n = cobs_encode(buf, in, size); 92 printf("(%lu)", n); 93 for (unsigned j = 0; j < 4; ++j) 94 printf(" %2x", buf[j]); 95 printf(" ..."); 96 for (unsigned j = n - 5; j < n; ++j) 97 printf(" %2x", buf[j]); 98 putchar('\n'); 99 } 100 int main(void) 101 { 102 unsigned char buf[1024]; 103 unsigned char buf2[1024]; 104 for (unsigned i = 0; i < sizeof(tests) / sizeof(*tests); ++i) 105 { 106 memset(buf, '?', sizeof(buf)); 107 size_t n = cobs_encode(buf, tests[i].decoded, tests[i].de_siz); 108 assert(strlen(tests[i].encoded)+1 == n); 109 assert(!memcmp(buf, tests[i].encoded, n)); 110 n = cobs_decode(buf2, buf, n); 111 assert(tests[i].de_siz == n); 112 assert(!memcmp(buf2, tests[i].decoded, n)); 113 114 printf("%u OK\n", i); 115 } 116 117 unsigned char in[512]; 118 119 for (unsigned i = 0; i < 0xFE; ++i) // 01 -> FE 120 in[i] = i+1; 121 encode_and_print(in, 0xFE); 122 123 for (unsigned i = 0; i < 0xFF; ++i) // 00 -> FE 124 in[i] = i; 125 encode_and_print(in, 0xFF); 126 127 for (unsigned i = 0; i < 0xFF; ++i) // 01 -> FF 128 in[i] = i+1; 129 encode_and_print(in, 0xFF); 130 131 for (unsigned i = 0; i < 0xFF; ++i) // 02 -> FF 00 132 in[i] = i+2; 133 encode_and_print(in, 0xFF); 134 135 for (unsigned i = 0; i < 0xFF; ++i) // 03 -> FF 00 01 136 in[i] = i+3; 137 encode_and_print(in, 0xFF); 138 139 140 return 0; 141 }