Adding encrypted packets to a buffer, so they can be decrypted when t…

…he CPU has time

Firmware/radio/parameters.c

@@ -79,7 +79,7 @@ __code const parameter_info_t parameter_s_info[PARAM_S_MAX] = {
 __code const parameter_info_t parameter_r_info[PARAM_R_MAX] = {
 	{"TARGET_RSSI",     255},
 	{"HYSTERESIS_RSSI", 50},
-	{"ENCRYPTION",		0}
+	{"ENCRYPTION_LEVEL", 0}
 };
 
 /// In-RAM parameter store.
@@ -275,14 +275,20 @@ param_r_check(__pdata enum Param_R_ID id, __data uint32_t val)
 		case PARAM_R_ENCRYPTION:
 			// Make sure first nibble (key length) is valid: 0, 1, 2
 			if ((val & 0xf ) > 3)
+      {
 				return false;
+      }
 			// Make sure second nibble (crypto type) is valid: 0, 1
 			if (((val>>4) & 0xf) > 1)
+      {
 				return false;
+      }
 			// Make sure that if second nibble (crypto type) is > 0,
 			// the first nibble (key length) is not zero.
 			if (((val>>4) & 0xf) > 0 && (val & 0xf ) == 0)
+      {
 				return false;
+      }
 			break;
 #endif
 
@@ -699,6 +705,7 @@ param_set_encryption_key(__xdata unsigned char *key)
 	} else {
 		// We have sufficient characters for the encryption key.
 		// If too many characters, then it will just ignore extra ones
+    printf("key len %d\n",key_length);
 		convert_to_hex(key, encryption_key, key_length);
 	}
 

Firmware/radio/serial.c

@@ -52,17 +52,23 @@
 #ifdef CPU_SI1030
 #define RX_BUFF_MAX 1024 //2048
 #define TX_BUFF_MAX 1024
+#define ENCRYPT_BUFF_MAX 17*20
 #else
 #define RX_BUFF_MAX 1860
 #define TX_BUFF_MAX 645
 #endif // CPU_SI1030
 
 __xdata uint8_t rx_buf[RX_BUFF_MAX] = {0};
 __xdata uint8_t tx_buf[TX_BUFF_MAX] = {0};
-
+#ifdef CPU_SI1030
+__xdata uint8_t encrypt_buf[ENCRYPT_BUFF_MAX] = {0};
+#endif
 // FIFO insert/remove pointers
 static volatile __pdata uint16_t				rx_insert, rx_remove;
 static volatile __pdata uint16_t				tx_insert, tx_remove;
+#ifdef CPU_SI1030
+static volatile __pdata uint16_t				encrypt_insert, encrypt_remove;
+#endif
 
 // count of number of bytes we are allowed to send due to a RTS low reading
 static uint8_t rts_count;
@@ -197,9 +203,13 @@ serial_init(register uint8_t speed)
 
 	// reset buffer state, discard all data
 	rx_insert = 0;
-	tx_remove = 0;
+	rx_remove = 0;
 	tx_insert = 0;
-	tx_remove = 0;
+  tx_remove = 0;
+#ifdef CPU_SI1030
+  encrypt_insert = 0;
+  encrypt_remove = 0;
+#endif
 	tx_idle = true;
 
 	// configure timer 1 for bit clock generation
@@ -253,16 +263,73 @@ _serial_write(register uint8_t c) __reentrant
 }
 
 #ifdef CPU_SI1030
-// Encrypted packets arn't bigger than 32 bytes
-// Limited by packet.c packet_get_next()
-static __xdata uint8_t len_decrypted;
-static __xdata uint8_t decrypt_buf[32];
+// If on appropriate CPU and encryption configured, then attempt to decrypt it
+void
+decryptPackets(void)
+{
+  // Encrypted packets arn't bigger than 32 bytes
+  // Limited by packet.c packet_get_next()
+  static __pdata uint8_t len_decrypted;
+  static __xdata uint8_t decrypt_buf[32];
+
+  if(BUF_NOT_EMPTY(encrypt) && aes_get_encryption_level() > 0)
+  {
+    if (encrypt_buf[encrypt_remove] == 0)
+    {
+      __critical {
+        encrypt_remove = 0;
+      }
+    }
+//    if (aes_decrypt(&encrypt_buf[encrypt_remove+1], encrypt_buf[encrypt_remove], decrypt_buf, &len_decrypted) != 0) {
+//      panic("error while trying to decrypt data");
+//    }
+    // zero the packet as we have read it.
+    len_decrypted = encrypt_buf[encrypt_remove];
+    encrypt_buf[encrypt_remove] = 0;
+
+    printf("eb %u:%u!%u - ea ",encrypt_remove, encrypt_insert, len_decrypted);
+    __critical {
+      encrypt_remove += len_decrypted + 1;
+      if (encrypt_remove >= sizeof(encrypt_buf)) {
+        encrypt_remove = 0;
+      }
+    }
+    printf("%u\n",encrypt_remove);
+  }
+}
+
+void
+serial_decrypt_buf(__xdata uint8_t * buf, __pdata uint8_t count)
+{
+  if (aes_get_encryption_level() > 0) {
+    // write to the end of the ring buffer or front if we dont have space
+    if (count > sizeof(encrypt_buf) - (encrypt_insert + 1)) {
+      encrypt_insert = 0;
+    }
+    // Insert the length of the packet
+    encrypt_buf[encrypt_insert] = count;
+    //printf("ic %u \n",count, encrypt_insert);
+    memcpy(&encrypt_buf[encrypt_insert+1], buf, count);
+
+    __critical {
+      encrypt_insert += count + 1;
+      if (encrypt_insert >= sizeof(encrypt_buf)) {
+        encrypt_insert -= sizeof(encrypt_buf);
+      }
+    }
+    // Zero the next packet for the parser.
+    encrypt_buf[encrypt_insert] = 0;
+  }
+  else {
+    serial_write_buf(buf, count);
+  }
+}
 #endif // CPU_SI1030
 
 // write as many bytes as will fit into the serial transmit buffer
 // if encryption turned on, decrypt the packet.
 void
-serial_write_buf(__xdata uint8_t * __data buf, __pdata uint8_t count)
+serial_write_buf(__xdata uint8_t * buf, __pdata uint8_t count)
 //, bool encrypted)
 {
 	__pdata uint16_t space;
@@ -271,22 +338,6 @@ serial_write_buf(__xdata uint8_t * __data buf, __pdata uint8_t count)
 	if (count == 0) {
 		return;
 	}
-
-// If on appropriate CPU and encryption configured, then attempt to decrypt it
-#ifdef CPU_SI1030
-  TP10 = true;
-    if (aes_get_encryption_level() > 0) {
-      memcpy(decrypt_buf, buf, count);
-      if (aes_decrypt(buf, count, decrypt_buf, &len_decrypted) != 0) {
-        panic("error while trying to decrypt data");
-      }
-      TP10 = false;
-//      //memcpy(buf, decrypt_buf, len_decrypted);
-//      buf = decrypt_buf;
-//      count = len_decrypted;
-      return;
-    }
-#endif // CPU_SI1030
 
 	// discard any bytes that don't fit. We can't afford to
 	// wait for the buffer to drain as we could miss a frequency
@@ -416,7 +467,7 @@ serial_peekx(uint16_t offset)
 // tries to be as efficient as possible, while disabling interrupts
 // for as short a time as possible
 bool
-serial_read_buf(__xdata uint8_t * __data buf, __pdata uint8_t count)
+serial_read_buf(__xdata uint8_t * buf, __pdata uint8_t count)
 {
 	__pdata uint16_t n1;
 	// the caller should have already checked this, 

Firmware/radio/serial.h

@@ -66,7 +66,12 @@ extern bool	serial_write(register uint8_t c);
 /// @param	buf		Pointer to the data to write.
 /// @param	count		The number of bytes to write.
 ///
-extern void	serial_write_buf(__xdata uint8_t * __data buf, __pdata uint8_t count);
+extern void	serial_write_buf(__xdata uint8_t * buf, __pdata uint8_t count);
+extern void serial_decrypt_buf(__xdata uint8_t * buf, __pdata uint8_t count);
+
+/// Decrypt any packets in the buffer and push to the serial layer
+///
+extern void decryptPackets(void);
 
 /// Check for space in the write FIFO
 ///
@@ -118,7 +123,7 @@ extern uint8_t	serial_peekx(uint16_t offset);
 ///				to satisfy the request (no bytes are read
 ///				in this case).
 ///
-extern bool	serial_read_buf(__xdata uint8_t * __data buf, __pdata uint8_t count);
+extern bool	serial_read_buf(__xdata uint8_t * buf, __pdata uint8_t count);
 
 /// Check for bytes in the read FIFO
 ///

Firmware/radio/tdm.c

@@ -382,7 +382,7 @@ static void temperature_update(void)
 static void
 link_update(void)
 {
-  static uint8_t unlock_count, temperature_count;
+  static uint8_t unlock_count = 10, temperature_count;
   if (received_packet) {
     unlock_count = 0;
     received_packet = false;
@@ -393,7 +393,7 @@ link_update(void)
     unlock_count++;
   }
 
-  if (unlock_count < 6) {
+  if (unlock_count < 2) {
     LED_RADIO = LED_ON;
   } else {
 #ifdef TDM_SYNC_LOGIC
@@ -656,11 +656,9 @@ tdm_serial_loop(void)
               !at_mode_active) {
           // its user data - send it out
           // the serial port
-          //printf("rcv(%d,[", len);
           LED_ACTIVITY = LED_ON;
-          serial_write_buf(pbuf, len);
+          serial_decrypt_buf(pbuf, len);
           LED_ACTIVITY = LED_OFF;
-          //printf("]\n");
         }
       }
       continue;
@@ -674,6 +672,12 @@ tdm_serial_loop(void)
     tdm_state_update(tdelta);
     last_t = tnow;
 
+#ifdef CPU_SI1030
+    // If we have any packets that need decrypting lets do it now.
+    decryptPackets();
+    tnow = timer2_tick();
+#endif
+
     // update link status every 0.5s
     if (tnow - last_link_update > 32768) {
       link_update();