altos: Get AES CBC-MAC packet transfers running

[fw/altos] / src / ao_radio_cmac.c

diff --git a/src/ao_radio_cmac.c b/src/ao_radio_cmac.c
index 4b118d02ec0b73ce8206c5f3ad63c44e46a1f360..7648a2f599b46cc08f67022b95a3bafb214ea2bf 100644 (file)
--- a/src/ao_radio_cmac.c
+++ b/src/ao_radio_cmac.c
@@ -22,6 +22,7 @@
 
 static __xdata uint8_t cmac_key[AO_CMAC_KEY_LEN];
 static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN + AO_CMAC_KEY_LEN + 2 + AO_CMAC_KEY_LEN];
+static __pdata uint8_t ao_radio_cmac_len;
 
 static uint8_t
 getnibble(void)
@@ -39,54 +40,119 @@ getnibble(void)
        return 0;
 }
 
+static uint8_t
+getbyte(void)
+{
+       uint8_t b;
+       b = getnibble() << 4;
+       b |= getnibble();
+       return b;
+}
+       
 static void
 ao_radio_cmac_key(void) __reentrant
 {
        uint8_t i;
 
        for (i = 0; i < AO_CMAC_KEY_LEN; i++) {
-               ao_cmd_hex();
+               cmac_key[i] = getbyte();
                if (ao_cmd_status != ao_cmd_success)
                        return;
-               cmac_key[i] = ao_cmd_lex_i;
        }
 }
 
 static void
 ao_radio_cmac_send(void) __reentrant
 {
-       uint8_t i, b;
+       uint8_t i;
+       uint8_t len;
 
-       ao_cmd_hex();
+       ao_cmd_decimal();
        if (ao_cmd_status != ao_cmd_success)
                return;
-       if (ao_cmd_lex_i > AO_CMAC_MAX_LEN || ao_cmd_lex_i % AO_CMAC_KEY_LEN != 0) {
+       if (ao_cmd_lex_i < AO_CMAC_KEY_LEN ||
+           ao_cmd_lex_i > AO_CMAC_MAX_LEN ||
+           ao_cmd_lex_i % AO_CMAC_KEY_LEN != 0)
+       {
                ao_cmd_status = ao_cmd_syntax_error;
                return;
        }
-       for (i = 0; i < ao_cmd_lex_i; i++) {
-               b = getnibble() << 4;
-               b |= getnibble();
+       flush();
+       len = ao_cmd_lex_i;
+       for (i = 0; i < len; i++) {
+               cmac_data[i] = getbyte();
                if (ao_cmd_status != ao_cmd_success)
                        return;
-               cmac_data[i] = b;
        }
        ao_mutex_get(&ao_aes_mutex);
        ao_aes_set_mode(ao_aes_mode_cbc_mac);
        ao_aes_set_key(cmac_key);
-       for (i = 0; i < ao_cmd_lex_i; i += AO_CMAC_KEY_LEN) {
-               if (i + AO_CMAC_KEY_LEN < ao_cmd_lex_i)
+       ao_aes_zero_iv();
+       for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
+               if (i + AO_CMAC_KEY_LEN < len)
                        ao_aes_run(&cmac_data[i], NULL);
                else
-                       ao_aes_run(&cmac_data[i], &cmac_data[ao_cmd_lex_i]);
+                       ao_aes_run(&cmac_data[i], &cmac_data[len]);
        }
        ao_mutex_put(&ao_aes_mutex);
+#if HAS_MONITOR
        ao_set_monitor(0);
-       ao_radio_send(cmac_data, ao_cmd_lex_i + AO_CMAC_KEY_LEN);
+#endif
+       printf("send:");
+       for (i = 0; i < len + AO_CMAC_KEY_LEN; i++)
+               printf(" %02x", cmac_data[i]);
+       printf("\n"); flush();
+       ao_radio_send(cmac_data, len + AO_CMAC_KEY_LEN);
 }
 
-__code struct ao_cmds ao_radio_cmac_cmds[] = {
-       { ao_radio_cmac_key,    "k <byte> ...\0Set AES-CMAC key." },
+static void
+ao_radio_cmac_recv(void) __reentrant
+{
+       uint8_t         len, i;
+       uint16_t        timeout;
+
+       ao_cmd_decimal();
+       if (ao_cmd_status != ao_cmd_success)
+               return;
+       if (ao_cmd_lex_i < AO_CMAC_KEY_LEN ||
+           ao_cmd_lex_i > AO_CMAC_MAX_LEN ||
+           ao_cmd_lex_i % AO_CMAC_KEY_LEN != 0)
+       {
+               ao_cmd_status = ao_cmd_syntax_error;
+               return;
+       }
+       len = ao_cmd_lex_i;
+       ao_cmd_decimal();
+       if (ao_cmd_status != ao_cmd_success)
+               return;
+       timeout = ao_cmd_lex_i;
+#if HAS_MONITOR
+       ao_set_monitor(0);
+#endif
+       if (timeout)
+               ao_alarm(timeout);
+       if (!ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2)) {
+               printf("timeout\n");
+               return;
+       }
+       ao_mutex_get(&ao_aes_mutex);
+       ao_aes_set_mode(ao_aes_mode_cbc_mac);
+       ao_aes_set_key(cmac_key);
+       ao_aes_zero_iv();
+       for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
+               if (i + AO_CMAC_KEY_LEN < len)
+                       ao_aes_run(&cmac_data[i], NULL);
+               else
+                       ao_aes_run(&cmac_data[i], &cmac_data[len + AO_CMAC_KEY_LEN + 2]);
+       }
+       printf ("PACKET ");
+       for (i = 0; i < len + AO_CMAC_KEY_LEN + 2 + AO_CMAC_KEY_LEN; i++)
+               printf("%02x", cmac_data[i]);
+       printf ("\n");
+}
+
+static __code struct ao_cmds ao_radio_cmac_cmds[] = {
+       { ao_radio_cmac_key,    "k\0Set AES-CMAC key. 16 key bytes follow on next line" },
        { ao_radio_cmac_send,   "s <length>\0Send AES-CMAC packet. Bytes to send follow on next line" },
        { ao_radio_cmac_recv,   "S <length> <timeout>\0Receive AES-CMAC packet. Timeout in ms" },
        { 0, NULL },
@@ -96,5 +162,4 @@ void
 ao_radio_cmac_init(void)
 {
        ao_cmd_register(&ao_radio_cmac_cmds[0]);
-       
 }