1 files changed, 84 insertions, 0 deletions

diff --git a/src/drivers/ao_btm.c b/src/drivers/ao_btm.c
index 44155ec1..e70e3048 100644
--- a/src/drivers/ao_btm.c
+++ b/src/drivers/ao_btm.c
@@ -20,6 +20,87 @@
 int8_t			ao_btm_stdio;
 __xdata uint8_t		ao_btm_connected;
 
+#define BT_DEBUG 0
+
+#if BT_DEBUG
+__xdata char		ao_btm_buffer[256];
+int			ao_btm_ptr;
+char			ao_btm_dir;
+
+static void
+ao_btm_add_char(char c)
+{
+	if (ao_btm_ptr < sizeof (ao_btm_buffer))
+		ao_btm_buffer[ao_btm_ptr++] = c;
+}
+
+static void
+ao_btm_log_char(char c, char dir)
+{
+	if (dir != ao_btm_dir) {
+		ao_btm_add_char(dir);
+		ao_btm_dir = dir;
+	}
+	ao_btm_add_char(c);
+}
+
+static void
+ao_btm_log_out_char(char c)
+{
+	ao_btm_log_char(c, '>');
+}
+
+static void
+ao_btm_log_in_char(char c)
+{
+	ao_btm_log_char(c, '<');
+}
+
+/*
+ * Dump everything received from the bluetooth device during startup
+ */
+static void
+ao_btm_dump(void)
+{
+	int i;
+	char c;
+
+	for (i = 0; i < ao_btm_ptr; i++) {
+		c = ao_btm_buffer[i];
+		if (c < ' ' && c != '\n')
+			printf("\\%03o", ((int) c) & 0xff);
+		else
+			putchar(ao_btm_buffer[i]);
+	}
+	putchar('\n');
+}
+
+static void
+ao_btm_speed(void)
+{
+	ao_cmd_decimal();
+	if (ao_cmd_lex_u32 == 57600)
+		ao_serial_set_speed(AO_SERIAL_SPEED_57600);
+	else if (ao_cmd_lex_u32 == 19200)
+		ao_serial_set_speed(AO_SERIAL_SPEED_19200);
+	else
+		ao_cmd_status = ao_cmd_syntax_error;
+}
+
+__code struct ao_cmds ao_btm_cmds[] = {
+	{ ao_btm_dump,		"d\0Dump btm buffer." },
+	{ ao_btm_speed,		"s <19200,57600>\0Set btm serial speed." },
+	{ 0, NULL },
+};
+
+#define ao_btm_log_init()	ao_cmd_register(&ao_btm_cmds[0])
+
+#else
+#define ao_btm_log_in_char(c)
+#define ao_btm_log_out_char(c)
+#define ao_btm_log_init()
+#endif
+
 #define AO_BTM_MAX_REPLY	16
 __xdata char		ao_btm_reply[AO_BTM_MAX_REPLY];
 
@@ -39,6 +120,7 @@ ao_btm_get_line(void)
 	for (;;) {
 
 		while ((c = ao_serial_pollchar()) != AO_READ_AGAIN) {
+			ao_btm_log_in_char(c);
 			if (ao_btm_reply_len < sizeof (ao_btm_reply))
 				ao_btm_reply[ao_btm_reply_len++] = c;
 			if (c == '\r' || c == '\n')
@@ -85,6 +167,7 @@ ao_btm_echo(uint8_t echo)
 void
 ao_btm_putchar(char c)
 {
+	ao_btm_log_out_char(c);
 	ao_serial_putchar(c);
 	ao_delay(1);
 }
@@ -299,4 +382,5 @@ ao_btm_init (void)
 #endif
 
 	ao_add_task(&ao_btm_task, ao_btm, "bt");
+	ao_btm_log_init();
 }