From 8e60c1e52c6693b1f43d6007b741242fe936926c Mon Sep 17 00:00:00 2001
From: Mark Qvist <mark@adepta.io>
Date: Sun, 3 Nov 2019 20:50:55 +0100
Subject: [PATCH] Implemented CSMA

---
 hardware/AFSK.c |  1 +
 protocol/KISS.c | 27 +++++++++++++++++++++++----
 2 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/hardware/AFSK.c b/hardware/AFSK.c
index 749882e..46e50e3 100755
--- a/hardware/AFSK.c
+++ b/hardware/AFSK.c
@@ -123,6 +123,7 @@ static void AFSK_txStart(Afsk *afsk) {
         afsk->bitstuffCount = 0;
         afsk->sending = true;
         afsk->sending_data = true;
+        LED_RX_OFF();
         LED_TX_ON();
         afsk->preambleLength = DIV_ROUND(config_preamble * BITRATE, 8000);
         AFSK_DAC_IRQ_START();
diff --git a/protocol/KISS.c b/protocol/KISS.c
index e3864a5..7a448fe 100755
--- a/protocol/KISS.c
+++ b/protocol/KISS.c
@@ -30,6 +30,8 @@ Serial *serial;
 volatile ticks_t last_serial_read = 0;
 extern volatile int8_t afsk_peak;
 
+ticks_t last_csma_check = 0;
+
 size_t frame_len;
 bool IN_FRAME;
 bool ESCAPE;
@@ -270,8 +272,15 @@ void kiss_csma(void) {
                     if (config_p == 255) {
                         kiss_flushQueue();
                     } else {
-                        // TODO: Implement real CSMA
-                        kiss_flushQueue();
+                        ticks_t csma_timeout = last_csma_check + ms_to_ticks(config_slottime);
+                        if (timer_clock() > csma_timeout) {
+                            uint8_t r = (uint8_t)random();
+                            if (r < config_p) {
+                                kiss_flushQueue();
+                            } else {
+                                last_csma_check = timer_clock();
+                            }
+                        }
                     }
                 }
             }
@@ -280,8 +289,15 @@ void kiss_csma(void) {
                 if (config_p == 255) {
                     kiss_flushQueue();
                 } else {
-                    // TODO: Implement real CSMA
-                    kiss_flushQueue();
+                    ticks_t csma_timeout = last_csma_check + ms_to_ticks(config_slottime);
+                    if (timer_clock() > csma_timeout) {
+                        uint8_t r = (uint8_t)random();
+                        if (r < config_p) {
+                            kiss_flushQueue();
+                        } else {
+                            last_csma_check = timer_clock();
+                        }
+                    }
                 }
             }
         #endif
@@ -371,6 +387,9 @@ void kiss_flushQueue(void) {
             }
         }
 
+        // TODO: Maybe remove this error state and fail
+        // silently, while reverting to operational state
+        // by resetting all buffers
         if (processed < queue_height) {
             while (true) {
                 LED_TX_ON();