Goby3  3.1.5a
2024.05.23
cobs.h
Go to the documentation of this file.
1 /* Copyright 2011, Jacques Fortier. All rights reserved.
2  *
3  * Redistribution and use in source and binary forms are permitted, with or without modification.
4  * Modified by Toby Schneider to be header-only templated functions.
5  */
6 #ifndef COBS_H
7 #define COBS_H
8 
9 #include <cstddef>
10 #include <cstdint>
11 
12 /* Stuffs "length" bytes of data at the location pointed to by
13  * "input", writing the output to the location pointed to by
14  * "output". Returns the number of bytes written to "output".
15  * "Byte" can be any integer representation of an 8-bit byte such as std::uint8_t, std::int8_t or char
16  */
17 template <typename Byte>
18 std::size_t cobs_encode(const Byte* input, std::size_t length, Byte* output)
19 {
20  std::size_t read_index = 0;
21  std::size_t write_index = 1;
22  std::size_t code_index = 0;
23  Byte code = 1;
24 
25  while (read_index < length)
26  {
27  if (input[read_index] == 0)
28  {
29  output[code_index] = code;
30  code = 1;
31  code_index = write_index++;
32  read_index++;
33  }
34  else
35  {
36  output[write_index++] = input[read_index++];
37  code++;
38  if (code == 0xFF)
39  {
40  output[code_index] = code;
41  code = 1;
42  code_index = write_index++;
43  }
44  }
45  }
46 
47  output[code_index] = code;
48 
49  return write_index;
50 }
51 
52 /* Unstuffs "length" bytes of data at the location pointed to by
53  * "input", writing the output * to the location pointed to by
54  * "output". Returns the number of bytes written to "output" if
55  * "input" was successfully unstuffed, and 0 if there was an
56  * error unstuffing "input".
57  * "Byte" can be any integer representation of an 8-bit byte such as std::uint8_t, std::int8_t or char
58  */
59 template <typename Byte>
60 std::size_t cobs_decode(const Byte* input, std::size_t length, Byte* output)
61 {
62  std::size_t read_index = 0;
63  std::size_t write_index = 0;
64  Byte code;
65  Byte i;
66 
67  while (read_index < length)
68  {
69  code = input[read_index];
70 
71  if (read_index + code > length && code != 1)
72  {
73  return 0;
74  }
75 
76  read_index++;
77 
78  for (i = 1; i < code; i++) { output[write_index++] = input[read_index++]; }
79  if (code != 0xFF && read_index != length)
80  {
81  output[write_index++] = '\0';
82  }
83  }
84 
85  return write_index;
86 }
87 
88 #endif
cobs_encode
std::size_t cobs_encode(const Byte *input, std::size_t length, Byte *output)
Definition: cobs.h:18
cobs_decode
std::size_t cobs_decode(const Byte *input, std::size_t length, Byte *output)
Definition: cobs.h:60