From ede7b5c44353b516e5ee5272972408d0ec67f1c0 Mon Sep 17 00:00:00 2001
From: Mark Qvist <mark@adepta.io>
Date: Fri, 5 Apr 2019 14:04:18 +0200
Subject: [PATCH] Added modem mode KISS commands

---
 protocol/KISS.c  | 18 ++++++++++++++++++
 protocol/KISS.h  |  2 ++
 util/constants.h |  6 +++++-
 3 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/protocol/KISS.c b/protocol/KISS.c
index 5cd1a1a..ec2dcc6 100755
--- a/protocol/KISS.c
+++ b/protocol/KISS.c
@@ -400,6 +400,7 @@ void kiss_serialCallback(uint8_t sbyte) {
         // TODO: Remove this
         } else if (command == CMD_PRINT_CONFIG) {
             config_print();
+            kiss_output_modem_mode();
         } else if (command == CMD_AUDIO_PEAK) {
             if (sbyte == 0x01) {
                 kiss_output_afsk_peak();
@@ -410,11 +411,28 @@ void kiss_serialCallback(uint8_t sbyte) {
             } else {
                 config_enable_diagnostics();
             }
+        }  else if (command == CMD_MODE) {
+            if (sbyte == 0x00) {
+                kiss_output_modem_mode();
+            }
         }
         
     }
 }
 
+void kiss_output_modem_mode(void) {
+    fputc(FEND, &serial->uart0);
+    fputc(CMD_MODE, &serial->uart0);
+    if (BITRATE == 300) {
+        fputc(MODE_AFSK_300, &serial->uart0);
+    } else if (BITRATE == 1200) {
+        fputc(MODE_AFSK_1200, &serial->uart0);
+    } else if (BITRATE == 2400) {
+        fputc(MODE_AFSK_2400, &serial->uart0);
+    }
+    fputc(FEND, &serial->uart0);
+}
+
 void kiss_output_afsk_peak(void) {
     fputc(FEND, &serial->uart0);
     fputc(CMD_AUDIO_PEAK, &serial->uart0);
diff --git a/protocol/KISS.h b/protocol/KISS.h
index dd008b2..9f2dcc2 100755
--- a/protocol/KISS.h
+++ b/protocol/KISS.h
@@ -33,6 +33,7 @@
 #define CMD_REBOOT_CONFIRM 0x9A
 #define CMD_AUDIO_PEAK 0x12
 #define CMD_ENABLE_DIAGNOSTICS 0x13
+#define CMD_MODE 0x14
 #define CMD_PRINT_CONFIG 0xF0
 #define CMD_RETURN 0xFF
 
@@ -43,6 +44,7 @@ void kiss_flushQueue(void);
 void kiss_csma(void);
 void kiss_poll(void);
 
+void kiss_output_modem_mode(void);
 void kiss_output_config(uint8_t* data, size_t length);
 void kiss_output_afsk_peak(void);
 
diff --git a/util/constants.h b/util/constants.h
index b201d35..e71de12 100755
--- a/util/constants.h
+++ b/util/constants.h
@@ -6,4 +6,8 @@
 #define START_FROM_BOOTLOADER 0x01
 #define START_FROM_POWERON 0x02
 #define START_FROM_BROWNOUT 0x03
-#define START_FROM_JTAG 0x04
\ No newline at end of file
+#define START_FROM_JTAG 0x04
+
+#define MODE_AFSK_300 0x01
+#define MODE_AFSK_1200 0x02
+#define MODE_AFSK_2400 0x03
\ No newline at end of file