1 files changed, 7 insertions, 88 deletions

diff --git a/gm_platform/fw/cobs.c b/gm_platform/fw/cobs.c
index 22dcac0..041dd8f 100644
--- a/gm_platform/fw/cobs.c
+++ b/gm_platform/fw/cobs.c
@@ -2,87 +2,11 @@
 #include "serial.h"
 #include "cobs.h"
 
-/*@ requires \valid(dst + (0..dstlen-1));
-  @ requires \valid_read(src + (0..srclen-1));
-  @ requires \separated(dst + (0..dstlen-1), src + (0..srclen-1));
-  @
-  @ behavior valid:
-  @     assumes 0 <= srclen <= 254;
-  @     assumes 0 <= dstlen <= 65535;
-  @     assumes dstlen >= srclen+2;
-  @     assigns dst[0..srclen+1];
-  @     ensures \forall integer i; (0 <= i < srclen && \old(src[i]) != 0) ==> dst[i+1] == src[i];
-  @     ensures \result == srclen+2;
-  @     ensures \forall integer i; 0 <= i <= srclen ==> dst[i] != 0;
-  @     ensures dst[srclen+1] == 0;
-  @
-  @ behavior invalid:
-  @     assumes    srclen < 0 || srclen > 254
-  @             || dstlen < 0 || dstlen > 65535
-  @             || dstlen < srclen+2;
-  @     assigns \nothing;
-  @     ensures \result == -1;
-  @
-  @ complete behaviors;
-  @ disjoint behaviors;
- @*/
-ssize_t cobs_encode(char *dst, size_t dstlen, char *src, size_t srclen) {
-    if (dstlen > 65535 || srclen > 254)
-        return -1;
-    //@ assert 0 <= dstlen <= 65535 && 0 <= srclen <= 254;
-
-    if (dstlen < srclen+2)
-        return -1;
-    //@ assert 0 <= srclen < srclen+2 <= dstlen;
-
-    size_t p = 0;
-    /*@ loop invariant 0 <= p <= srclen+1;
-      @ loop invariant \forall integer i; 0 <= i < p ==> dst[i] != 0;
-      @ loop invariant \forall integer i; 0 < i < p ==> (src[i-1] != 0 ==> dst[i] == src[i-1]);
-      @ loop assigns p, dst[0..srclen+1];
-      @ loop variant srclen-p+1;
-      @*/
-    while (p <= srclen) {
-
-        char val;
-        if (p != 0 && src[p-1] != 0) {
-            val = src[p-1];
-
-        } else {
-            size_t q = p;
-            /*@ loop invariant 0 <= p <= q <= srclen;
-              @ loop invariant \forall integer i; p <= i < q ==> src[i] != 0;
-              @ loop assigns q;
-              @ loop variant srclen-q;
-              @*/
-            while (q < srclen && src[q] != 0)
-                q++;
-            //@ assert q == srclen || src[q] == 0;
-            //@ assert q <= srclen <= 254;
-            val = (char)q-p+1;
-            //@ assert val != 0;
-        }
-
-        dst[p] = val;
-        p++;
-    }
-
-    dst[p] = 0;
-    //@ assert p == srclen+1;
-
-    return srclen+2;
-}
-
-int cobs_encode_usart(char *src, size_t srclen) {
+int cobs_encode_usart(int (*output)(char), char *src, size_t srclen) {
     if (srclen > 254)
         return -1;
-    //@ assert 0 <= srclen <= 254;
 
     size_t p = 0;
-    /*@ loop invariant 0 <= p <= srclen+1;
-      @ loop assigns p;
-      @ loop variant srclen-p+1;
-      @*/
     while (p <= srclen) {
 
         char val;
@@ -91,25 +15,20 @@ int cobs_encode_usart(char *src, size_t srclen) {
 
         } else {
             size_t q = p;
-            /*@ loop invariant 0 <= p <= q <= srclen;
-              @ loop invariant \forall integer i; p <= i < q ==> src[i] != 0;
-              @ loop assigns q;
-              @ loop variant srclen-q;
-              @*/
             while (q < srclen && src[q] != 0)
                 q++;
-            //@ assert q == srclen || src[q] == 0;
-            //@ assert q <= srclen <= 254;
             val = (char)q-p+1;
-            //@ assert val != 0;
         }
 
-        usart_putc(val);
+        int rv = output(val);
+        if (rv)
+            return rv;
         p++;
     }
 
-    usart_putc(0);
-    //@ assert p == srclen+1;
+    int rv = output(0);
+    if (rv)
+        return rv;
 
     return 0;
 }