2 files changed, 38 insertions, 2 deletions
diff --git a/serial_link/protocol/byte_stuffer.c b/serial_link/protocol/byte_stuffer.c
index 95ce86524f..7ce01a96ab 100644
--- a/serial_link/protocol/byte_stuffer.c
+++ b/serial_link/protocol/byte_stuffer.c
@@ -25,5 +25,39 @@ SOFTWARE.
 #include "protocol/byte_stuffer.h"
 #include "protocol/frame_validator.h"
 
-void recv_byte(uint8_t data) {
+// This implements the "Consistent overhead byte stuffing protocol"
+// https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing
+// http://www.stuartcheshire.org/papers/COBSforToN.pdf
+
+typedef struct byte_stuffer_state {
+    uint16_t next_zero;
+    uint16_t data_pos;
+    uint8_t data[256];
+}byte_stuffer_state_t;
+
+void init_byte_stuffer_state(byte_stuffer_state_t* state) {
+    state->next_zero = 0;
+    state->data_pos = 0;
+}
+
+void recv_byte(byte_stuffer_state_t* state, uint8_t data) {
+    if (state->next_zero == 0) {
+        state->next_zero = data;
+        state->data_pos = 0;
+        return;
+    }
+
+    state->next_zero--;
+    if (data == 0) {
+        recv_frame(state->data, state->data_pos);
+    }
+    else {
+        if (state->next_zero == 0) {
+            state->next_zero = data;
+            state->data[state->data_pos++] = 0;
+        }
+        else {
+            state->data[state->data_pos++] = data;
+        }
+    }
 }
diff --git a/serial_link/protocol/byte_stuffer.h b/serial_link/protocol/byte_stuffer.h
index 3b9f9ea5d4..9a5551fab5 100644
--- a/serial_link/protocol/byte_stuffer.h
+++ b/serial_link/protocol/byte_stuffer.h
@@ -22,4 +22,6 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.
 */
 
-void recv_byte(uint8_t data);
+typedef struct byte_stuffer_state byte_stuffer_state_t;
+void init_byte_stuffer_state(byte_stuffer_state_t* state);
+void recv_byte(byte_stuffer_state_t* state, uint8_t data);