From 4f4a89881950721d4fd6d3de2319ab48b3353cef Mon Sep 17 00:00:00 2001
From: XANTRONIX Development <dev@xantronix.com>
Date: Sun, 12 Jul 2015 23:48:12 -0500
Subject: [PATCH] This SEEMS valid, but we'll compile later

---
 src/kiss.c | 46 +++++++++++++++++++++++++++++++++++++++-------
 1 file changed, 39 insertions(+), 7 deletions(-)

diff --git a/src/kiss.c b/src/kiss.c
index 1726333..901b858 100644
--- a/src/kiss.c
+++ b/src/kiss.c
@@ -76,42 +76,74 @@ error_io:
     return -1;
 }
 
+static inline ssize_t write_byte(int fd, unsigned char c) {
+    return write(fd, &c, sizeof(c));
+}
+
+static inline ssize_t write_command(int fd, int command, int port) {
+    return write_byte(fd, ((port & 0x0f) << 4) | (command & 0x0f));
+}
+
 ssize_t patty_kiss_write(int fd, const void *buf, size_t len, int port) {
-    size_t i;
+    size_t i, start = 0, end = 0;
 
     unsigned char escape_fend[2] = { PATTY_KISS_FESC, PATTY_KISS_TFEND };
     unsigned char escape_fesc[2] = { PATTY_KISS_FESC, PATTY_KISS_TFESC };
 
-    unsigned char command = (port & 0x0f) << 4;
+    if (write_byte(fd, PATTY_KISS_FEND) < 0) {
+        goto error_io;
+    }
 
-    if (write(fd, &command, 1) < 0) {
+    if (write_command(PATTY_KISS_DATA, port) < 0) {
         goto error_io;
     }
 
     for (i=0; i<len; i++) {
         unsigned char c = ((unsigned char *)buf)[i];
         ssize_t res = 0;
+        unsigned char *escape = NULL;
 
         switch (c) {
             case PATTY_KISS_FEND: {
-                res = write(fd, escape_fend, sizeof(escape_fend)); break;
+                escape = escape_fend; break;
             }
 
             case PATTY_KISS_FESC: {
-                res = write(fd, escape_fesc, sizeof(escape_fesc)); break;
+                escape = escape_fesc; break;
             }
 
             default: {
+                end = i;
                 res = write(fd, &c, 1); break;
             }
         }
 
-        if (res < 0) {
+        if (escape) {
+            if (write(fd, ((unsigned char *)buf) + start, end - start) < 0) {
+                goto error_io;
+            }
+
+            if (write(fd, escape, 2) < 0) {
+                goto error_io;
+            }
+
+            escape = NULL;
+            start  = i + 1;
+            end    = start;
+        }
+    }
+
+    if (end - start) {
+        if (write(fd, ((unsigned char *)buf) + start, end - start) < 0) {
             goto error_io;
         }
     }
 
-    return 0;
+    if (write_byte(fd, PATTY_KISS_FEND) < 0) {
+        goto error_io;
+    }
+
+    return len;
 
 error_io:
     return -1;