From 8a35afbebeaa8b4a1512cf8d17abbc7c1e45bbf3 Mon Sep 17 00:00:00 2001
From: Mark Qvist <mark@adepta.io>
Date: Thu, 5 Apr 2018 19:13:07 +0200
Subject: [PATCH] Added escaping to upstream config commands

---
 Framing.h     |  9 +++++++++
 Utilities.cpp | 39 +++++++++++++++++++++++----------------
 2 files changed, 32 insertions(+), 16 deletions(-)

diff --git a/Framing.h b/Framing.h
index 0107764..4c0efe4 100644
--- a/Framing.h
+++ b/Framing.h
@@ -52,4 +52,13 @@ All:					0xc00119d21b80c00200005140c00308c00407c0
 Radio on 				0xc00501c0
 
 Config+on 				0xc00119d21b80c00200005140c00308c00407c00501c0
+
+
+		c1 = self.bandwidth >> 24
+		c2 = self.bandwidth >> 16 & 0xFF
+		c3 = self.bandwidth >> 8 & 0xFF
+		c4 = self.bandwidth & 0xFF
+		data = KISS.escape(chr(c1)+chr(c2)+chr(c3)+chr(c4))
+
+
 */
\ No newline at end of file
diff --git a/Utilities.cpp b/Utilities.cpp
index ff81abd..ec09a84 100644
--- a/Utilities.cpp
+++ b/Utilities.cpp
@@ -51,6 +51,12 @@ void led_indicate_info(int cycles) {
     digitalWrite(pin_led_rx, LOW);
 }
 
+void escapedSerialWrite(uint8_t byte) {
+	if (byte == FEND) { Serial.write(FESC); byte = TFEND; }
+    if (byte == FESC) { Serial.write(FESC); byte = TFESC; }
+    Serial.write(byte);
+}
+
 void kiss_indicate_error(uint8_t error_code) {
 	Serial.write(FEND);
 	Serial.write(CMD_ERROR);
@@ -68,20 +74,20 @@ void kiss_indicate_radiostate() {
 void kiss_indicate_stat_rx() {
 	Serial.write(FEND);
 	Serial.write(CMD_STAT_RX);
-	Serial.write(stat_rx>>24);
-	Serial.write(stat_rx>>16);
-	Serial.write(stat_rx>>8);
-	Serial.write(stat_rx);
+	escapedSerialWrite(stat_rx>>24);
+	escapedSerialWrite(stat_rx>>16);
+	escapedSerialWrite(stat_rx>>8);
+	escapedSerialWrite(stat_rx);
 	Serial.write(FEND);
 }
 
 void kiss_indicate_stat_tx() {
 	Serial.write(FEND);
 	Serial.write(CMD_STAT_TX);
-	Serial.write(stat_tx>>24);
-	Serial.write(stat_tx>>16);
-	Serial.write(stat_tx>>8);
-	Serial.write(stat_tx);
+	escapedSerialWrite(stat_tx>>24);
+	escapedSerialWrite(stat_tx>>16);
+	escapedSerialWrite(stat_tx>>8);
+	escapedSerialWrite(stat_tx);
 	Serial.write(FEND);
 }
 
@@ -116,20 +122,20 @@ void kiss_indicate_txpower() {
 void kiss_indicate_bandwidth() {
 	Serial.write(FEND);
 	Serial.write(CMD_BANDWIDTH);
-	Serial.write(lora_bw>>24);
-	Serial.write(lora_bw>>16);
-	Serial.write(lora_bw>>8);
-	Serial.write(lora_bw);
+	escapedSerialWrite(lora_bw>>24);
+	escapedSerialWrite(lora_bw>>16);
+	escapedSerialWrite(lora_bw>>8);
+	escapedSerialWrite(lora_bw);
 	Serial.write(FEND);
 }
 
 void kiss_indicate_frequency() {
 	Serial.write(FEND);
 	Serial.write(CMD_FREQUENCY);
-	Serial.write(lora_freq>>24);
-	Serial.write(lora_freq>>16);
-	Serial.write(lora_freq>>8);
-	Serial.write(lora_freq);
+	escapedSerialWrite(lora_freq>>24);
+	escapedSerialWrite(lora_freq>>16);
+	escapedSerialWrite(lora_freq>>8);
+	escapedSerialWrite(lora_freq);
 	Serial.write(FEND);
 }
 
@@ -197,3 +203,4 @@ uint8_t getRandom() {
 		return 0x00;
 	}
 }
+