4 Copyright (c) 2016 Fred Sundvik
6 Permission is hereby granted, free of charge, to any person obtaining a copy
7 of this software and associated documentation files (the "Software"), to deal
8 in the Software without restriction, including without limitation the rights
9 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10 copies of the Software, and to permit persons to whom the Software is
11 furnished to do so, subject to the following conditions:
13 The above copyright notice and this permission notice shall be included in all
14 copies or substantial portions of the Software.
16 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
25 #include "serial_link/protocol/byte_stuffer.h"
26 #include "serial_link/protocol/frame_validator.h"
27 #include "serial_link/protocol/physical.h"
30 // This implements the "Consistent overhead byte stuffing protocol"
31 // https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing
32 // http://www.stuartcheshire.org/papers/COBSforToN.pdf
34 #define MAX_FRAME_SIZE 1024
37 typedef struct byte_stuffer_state {
41 uint8_t data[MAX_FRAME_SIZE];
42 }byte_stuffer_state_t;
44 static byte_stuffer_state_t states[NUM_LINKS];
46 void init_byte_stuffer_state(byte_stuffer_state_t* state) {
49 state->long_frame = false;
52 void init_byte_stuffer(void) {
54 for (i=0;i<NUM_LINKS;i++) {
55 init_byte_stuffer_state(&states[i]);
59 void byte_stuffer_recv_byte(uint8_t link, uint8_t data) {
60 byte_stuffer_state_t* state = &states[link];
61 // Start of a new frame
62 if (state->next_zero == 0) {
63 state->next_zero = data;
64 state->long_frame = data == 0xFF;
71 if (state->next_zero == 0) {
72 // The frame is completed
73 if (state->data_pos > 0) {
74 validator_recv_frame(link, state->data, state->data_pos);
78 // The frame is invalid, so reset
79 init_byte_stuffer_state(state);
83 if (state->data_pos == MAX_FRAME_SIZE) {
84 // We exceeded our maximum frame size
85 // therefore there's nothing else to do than reset to a new frame
86 state->next_zero = data;
87 state->long_frame = data == 0xFF;
90 else if (state->next_zero == 0) {
91 if (state->long_frame) {
92 // This is part of a long frame, so continue
93 state->next_zero = data;
94 state->long_frame = data == 0xFF;
97 // Special case for zeroes
98 state->next_zero = data;
99 state->data[state->data_pos++] = 0;
103 state->data[state->data_pos++] = data;
108 static void send_block(uint8_t link, uint8_t* start, uint8_t* end, uint8_t num_non_zero) {
109 send_data(link, &num_non_zero, 1);
111 send_data(link, start, end-start);
115 void byte_stuffer_send_frame(uint8_t link, uint8_t* data, uint16_t size) {
116 const uint8_t zero = 0;
118 uint16_t num_non_zero = 1;
119 uint8_t* end = data + size;
120 uint8_t* start = data;
122 if (num_non_zero == 0xFF) {
123 // There's more data after big non-zero block
124 // So send it, and start a new block
125 send_block(link, start, data, num_non_zero);
131 // A zero encountered, so send the block
132 send_block(link, start, data, num_non_zero);
142 send_block(link, start, data, num_non_zero);
143 send_data(link, &zero, 1);