From a2c3fd391c6813adc305a9596d3bbd8d53915e28 Mon Sep 17 00:00:00 2001
From: Mark Qvist <mark@adepta.io>
Date: Wed, 28 Nov 2018 23:56:48 +0100
Subject: [PATCH] Added KISS flow control

---
 protocol/KISS.c | 20 +++++++++++++++++---
 protocol/KISS.h |  1 +
 protocol/LLP.c  |  6 ++++++
 protocol/LLP.h  |  1 +
 4 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/protocol/KISS.c b/protocol/KISS.c
index 76c6eb1..85ea64e 100755
--- a/protocol/KISS.c
+++ b/protocol/KISS.c
@@ -11,6 +11,7 @@ Serial *serial;
 size_t frame_len;
 bool IN_FRAME;
 bool ESCAPE;
+bool FLOWCONTROL;
 
 uint8_t command = CMD_UNKNOWN;
 unsigned long custom_preamble = CONFIG_AFSK_PREAMBLE_LEN;
@@ -24,6 +25,7 @@ void kiss_init(LLPCtx *ctx, Afsk *afsk, Serial *ser) {
     llpCtx = ctx;
     serial = ser;
     channel = afsk;
+    FLOWCONTROL = false;
 }
 
 void kiss_messageCallback(LLPCtx *ctx) {
@@ -84,9 +86,15 @@ void kiss_csma(LLPCtx *ctx, uint8_t *buf, size_t len) {
                 }
             }
         }
-
     }
-    
+
+    if (FLOWCONTROL) {
+        while (!ctx->ready_for_data) { /* Wait */ }
+        fputc(FEND, &serial->uart0);
+        fputc(CMD_READY, &serial->uart0);
+        fputc(0x01, &serial->uart0);
+        fputc(FEND, &serial->uart0);
+    }
 }
 
 void kiss_checkTimeout(bool force) {
@@ -138,7 +146,13 @@ void kiss_serialCallback(uint8_t sbyte) {
                 slotTime = sbyte * 10;
             } else if (command == CMD_P) {
                 p = sbyte;
-            } 
+            } else if (command == CMD_READY) {
+                if (sbyte == 0x00) {
+                    FLOWCONTROL = false;
+                } else {
+                    FLOWCONTROL = true;
+                }
+            }
             
         }
     #endif
diff --git a/protocol/KISS.h b/protocol/KISS.h
index 45c0c6f..d48d679 100755
--- a/protocol/KISS.h
+++ b/protocol/KISS.h
@@ -20,6 +20,7 @@
 #define CMD_TXTAIL 0x04
 #define CMD_FULLDUPLEX 0x05
 #define CMD_SETHARDWARE 0x06
+#define CMD_READY 0x0F
 #define CMD_RETURN 0xFF
 
 void kiss_init(LLPCtx *ctx, Afsk *afsk, Serial *ser);
diff --git a/protocol/LLP.c b/protocol/LLP.c
index e970824..436f867 100755
--- a/protocol/LLP.c
+++ b/protocol/LLP.c
@@ -301,6 +301,7 @@ void llp_broadcast(LLPCtx *ctx, const void *_buf, size_t len) {
 }
 
 void llp_send(LLPCtx *ctx, LLPAddress *dst, const void *_buf, size_t len) {
+    ctx->ready_for_data = false;
     ctx->interleaveCounter = 0;
     ctx->crc_out = CRC_CCIT_INIT_VAL;
     uint8_t *buffer = (uint8_t*)_buf;
@@ -349,9 +350,11 @@ void llp_send(LLPCtx *ctx, LLPAddress *dst, const void *_buf, size_t len) {
     // And transmit a HDLC_FLAG to signify
     // end of the transmission.
     fputc(HDLC_FLAG, ctx->ch);
+    ctx->ready_for_data = true;
 }
 
 void llp_sendRaw(LLPCtx *ctx, const void *_buf, size_t len) {
+    ctx->ready_for_data = false;
     ctx->crc_out = CRC_CCIT_INIT_VAL;
     fputc(HDLC_FLAG, ctx->ch);
     const uint8_t *buf = (const uint8_t *)_buf;
@@ -363,6 +366,8 @@ void llp_sendRaw(LLPCtx *ctx, const void *_buf, size_t len) {
     llp_putchar(ctx, crch);
 
     fputc(HDLC_FLAG, ctx->ch);
+
+    ctx->ready_for_data = true;
 }
 
 void llp_init(LLPCtx *ctx, LLPAddress *address, FILE *channel, llp_callback_t hook) {
@@ -371,6 +376,7 @@ void llp_init(LLPCtx *ctx, LLPAddress *address, FILE *channel, llp_callback_t ho
     ctx->hook = hook;
     ctx->address = address;
     ctx->crc_in = ctx->crc_out = CRC_CCIT_INIT_VAL;
+    ctx->ready_for_data = true;
 
     memset(&broadcast_address, 0, sizeof(broadcast_address));
     broadcast_address.network = LLP_ADDR_BROADCAST;
diff --git a/protocol/LLP.h b/protocol/LLP.h
index 8631cf1..556fa2a 100755
--- a/protocol/LLP.h
+++ b/protocol/LLP.h
@@ -53,6 +53,7 @@ typedef struct LLPCtx {
     llp_callback_t hook;
     bool sync;
     bool escape;
+    bool ready_for_data;
     uint8_t interleaveCounter;                      // Keeps track of when we have received an entire interleaved block
     uint8_t interleaveOut[LLP_INTERLEAVE_SIZE];     // A buffer for interleaving bytes before they are sent
     uint8_t interleaveIn[LLP_INTERLEAVE_SIZE];      // A buffer for storing interleaved bytes before they are deinterleaved