1 files changed, 73 insertions, 17 deletions

diff --git a/src/ao_radio_cmac.c b/src/ao_radio_cmac.c
index 9b406a21..7648a2f5 100644
--- a/src/ao_radio_cmac.c
+++ b/src/ao_radio_cmac.c
@@ -40,62 +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);
 }
 
 static void
 ao_radio_cmac_recv(void) __reentrant
 {
-	ao_cmd_hex();
+	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");
 }
 
-__code struct ao_cmds ao_radio_cmac_cmds[] = {
-	{ ao_radio_cmac_key,	"k <byte> ...\0Set AES-CMAC key." },
+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 },
@@ -105,5 +162,4 @@ void
 ao_radio_cmac_init(void)
 {
 	ao_cmd_register(&ao_radio_cmac_cmds[0]);
-	
 }