Merge pull request #142 from aabadie/hdlc_bug

drv/hdlc: when encoding a payload, escape FCS bytes if needed
DotBots · Feb 9, 2023 · db1a295 · db1a295
2 parents 79ac616 + 7bb35c8
commit db1a295
Showing 1 changed file with 18 additions and 2 deletions.
diff --git a/drv/hdlc/hdlc.c b/drv/hdlc/hdlc.c
@@ -160,8 +160,24 @@ size_t db_hdlc_encode(const uint8_t *input, size_t input_len, uint8_t *frame) {
     fcs = 0xFFFF - fcs;
 
     // Write the FCS in the frame
-    frame[frame_len++] = (fcs & 0xFF);
-    frame[frame_len++] = ((fcs & 0xFF00) >> 8);
+    if ((fcs & 0xFF) == DB_HDLC_ESCAPE) {
+        frame[frame_len++] = DB_HDLC_ESCAPE;
+        frame[frame_len++] = DB_HDLC_ESCAPE_ESCAPED;
+    } else if ((fcs & 0xFF) == DB_HDLC_FLAG) {
+        frame[frame_len++] = DB_HDLC_ESCAPE;
+        frame[frame_len++] = DB_HDLC_FLAG_ESCAPED;
+    } else {
+        frame[frame_len++] = (fcs & 0xFF);
+    }
+    if (((fcs & 0xFF00) >> 8) == DB_HDLC_ESCAPE) {
+        frame[frame_len++] = DB_HDLC_ESCAPE;
+        frame[frame_len++] = DB_HDLC_ESCAPE_ESCAPED;
+    } else if (((fcs & 0xFF00) >> 8) == DB_HDLC_FLAG) {
+        frame[frame_len++] = DB_HDLC_ESCAPE;
+        frame[frame_len++] = DB_HDLC_FLAG_ESCAPED;
+    } else {
+        frame[frame_len++] = ((fcs & 0xFF00) >> 8);
+    }
 
     // End flag
     frame[frame_len++] = DB_HDLC_FLAG;