1 files changed, 47 insertions, 0 deletions
diff --git a/src/dle.c b/src/dle.c
new file mode 100644
index 0000000..51b3c75
--- /dev/null
+++ b/src/dle.c
@@ -0,0 +1,47 @@
+#include "dle.h"
+
+/* Those nifty DLE-sequences */
+#define DLE 0x10
+#define ETX 0x03
+#define DC4 0x14
+
+size_t dle_decode(uint8_t* s, size_t ls, uint8_t* d, size_t *ld, void (*dle_func) (uint8_t c, void *user), void *user, int *dle_flag) {
+    size_t ns, nd;
+    assert(s && ls && d && ld && *ld && dle_flag);
+
+    for (ns = nd = 0; ns < ls && nd < *ld; ns++) {
+        
+        if (*dle_flag) {
+            if (s[ns] == DLE)
+                d[nd++] = DLE;
+            else {
+                if (dle_func)
+                    dle_func(s[ns], user);
+            }
+            
+            *dle_flag = 0;
+        } else {
+            if (s[ns] == DLE)
+                *dle_flag = 1;
+            else
+                d[nd++] = s[ns];
+        }
+    }
+
+    *ld = nd;
+    return ns;
+}
+
+size_t dle_encode(uint8_t* s, size_t ls, uint8_t* d, size_t *ld) {
+    size_t ns, nd;
+
+    for (ns = nd = 0; ns < ls && nd < *ld; ns++) {
+        if (s[ns] == DLE)
+            d[nd++] = DLE;
+
+        d[nd++] = s[ns];
+    }
+
+    *ld = nd;
+    return ns;
+}