Enable packet-based communcation to command processor

This splits the packet code into master/slave halves and hooks the
slave side up to the getchar/putchar/flush logic in ao_stdio.c

Signed-off-by: Keith Packard <keithp@keithp.com>

11 files changed, 356 insertions, 158 deletions

diff --git a/src/Makefile b/src/Makefile
index 4575f443..d984e9dc 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -45,6 +45,7 @@ ALTOS_DRIVER_SRC = \
 TELE_COMMON_SRC = \
 	ao_gps_print.c \
 	ao_packet.c \
+	ao_packet_slave.c \
 	ao_state.c
 
 #
@@ -52,6 +53,7 @@ TELE_COMMON_SRC = \
 #
 TELE_RECEIVER_SRC =\
 	ao_monitor.c \
+	ao_packet_master.c \
 	ao_rssi.c
 
 #
diff --git a/src/ao.h b/src/ao.h
index aeceb870..65a594c0 100644
--- a/src/ao.h
+++ b/src/ao.h
@@ -106,6 +106,7 @@ ao_start_scheduler(void);
 #define AO_PANIC_EE		4	/* Mis-using eeprom API */
 #define AO_PANIC_LOG		5	/* Failing to read/write log data */
 #define AO_PANIC_CMD		6	/* Too many command sets registered */
+#define AO_PANIC_STDIO		7	/* Too many stdio handlers registered */
 
 /* Stop the operating system, beeping and blinking the reason */
 void
@@ -873,9 +874,24 @@ ao_monitor_init(uint8_t led, uint8_t monitoring) __reentrant;
  * ao_stdio.c
  */
 
+#define AO_READ_AGAIN	((char) -1)
+
+struct ao_stdio {
+	char	(*pollchar)(void);
+	void	(*putchar)(char c) __reentrant;
+	void	(*flush)(void);
+};
+
 void
 flush(void);
 
+extern __xdata uint8_t ao_stdin_ready;
+
+void
+ao_add_stdio(char (*pollchar)(void),
+	     void (*putchar)(char) __reentrant,
+	     void (*flush)(void));
+
 /*
  * ao_ignite.c
  */
@@ -997,7 +1013,42 @@ struct ao_packet_recv {
 	uint8_t			status;
 };
 
+extern __xdata struct ao_packet_recv ao_rx_packet;
+extern __xdata struct ao_packet ao_tx_packet;
+extern __xdata struct ao_task	ao_packet_task;
+extern __xdata uint8_t ao_packet_enable;
+extern __xdata uint8_t ao_packet_master_sleeping;
+extern __pdata uint8_t ao_packet_rx_len, ao_packet_rx_used, ao_packet_tx_used;
+
+void
+ao_packet_send(void);
+
+uint8_t
+ao_packet_recv(void);
+
+void
+ao_packet_flush(void);
+
+void
+ao_packet_putchar(char c) __reentrant;
+
+char
+ao_packet_pollchar(void) __critical;
+
+/* ao_packet_master.c */
+
+void
+ao_packet_master_init(void);
+
+/* ao_packet_slave.c */
+
+void
+ao_packet_slave_start(void);
+
+void
+ao_packet_slave_stop(void);
+
 void
-ao_packet_init(void);
+ao_packet_slave_init(void);
 
 #endif /* _AO_H_ */
diff --git a/src/ao_flight.c b/src/ao_flight.c
index c43d0711..e91a5daa 100644
--- a/src/ao_flight.c
+++ b/src/ao_flight.c
@@ -235,9 +235,9 @@ ao_flight(void)
 			} else {
 				ao_flight_state = ao_flight_idle;
 
-				/* Turn on the Green LED in idle mode
+				/* Turn on packet system in idle mode
 				 */
-				ao_led_on(AO_LED_GREEN);
+				ao_packet_slave_start();
 				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
 			}
 			/* signal successful initialization by turning off the LED */
diff --git a/src/ao_flight_test.c b/src/ao_flight_test.c
index 9fcb00c2..83c63016 100644
--- a/src/ao_flight_test.c
+++ b/src/ao_flight_test.c
@@ -69,6 +69,7 @@ uint8_t ao_adc_head;
 #define ao_usb_disable()
 #define ao_telemetry_set_interval(x)
 #define ao_rdf_set(rdf)
+#define ao_packet_slave_start()
 
 enum ao_igniter {
 	ao_igniter_drogue = 0,
diff --git a/src/ao_packet.c b/src/ao_packet.c
index ba559512..3ce7e9ab 100644
--- a/src/ao_packet.c
+++ b/src/ao_packet.c
@@ -17,16 +17,17 @@
 
 #include "ao.h"
 
-static __xdata struct ao_packet_recv rx_packet;
-static __xdata struct ao_packet tx_packet;
+__xdata struct ao_packet_recv ao_rx_packet;
+__xdata struct ao_packet ao_tx_packet;
+__pdata uint8_t ao_packet_rx_len, ao_packet_rx_used, ao_packet_tx_used;
+
 static __xdata char tx_data[AO_PACKET_MAX];
 static __xdata char rx_data[AO_PACKET_MAX];
-static __pdata uint8_t rx_len, rx_used, tx_used;
 static __pdata uint8_t rx_seq;
 
-static __xdata struct ao_task	ao_packet_task;
-static __xdata uint8_t ao_packet_enable;
-static __xdata uint8_t ao_packet_master_sleeping;
+__xdata struct ao_task	ao_packet_task;
+__xdata uint8_t ao_packet_enable;
+__xdata uint8_t ao_packet_master_sleeping;
 
 void
 ao_packet_send(void)
@@ -34,18 +35,18 @@ ao_packet_send(void)
 	ao_led_on(AO_LED_RED);
 	ao_config_get();
 	ao_mutex_get(&ao_radio_mutex);
-	if (tx_used && tx_packet.len == 0) {
-		memcpy(&tx_packet.d, tx_data, tx_used);
-		tx_packet.len = tx_used;
-		tx_packet.seq++;
-		tx_used = 0;
+	if (ao_packet_tx_used && ao_tx_packet.len == 0) {
+		memcpy(&ao_tx_packet.d, tx_data, ao_packet_tx_used);
+		ao_tx_packet.len = ao_packet_tx_used;
+		ao_tx_packet.seq++;
+		ao_packet_tx_used = 0;
 		ao_wakeup(&tx_data);
 	}
 	ao_radio_idle();
 	ao_radio_done = 0;
 	RF_CHANNR = ao_config.radio_channel;
 	ao_dma_set_transfer(ao_radio_dma,
-			    &tx_packet,
+			    &ao_tx_packet,
 			    &RFDXADDR,
 			    sizeof (struct ao_packet),
 			    DMA_CFG0_WORDSIZE_8 |
@@ -74,7 +75,7 @@ ao_packet_recv(void)
 	RF_CHANNR = ao_config.radio_channel;
 	ao_dma_set_transfer(ao_radio_dma,
 			    &RFDXADDR,
-			    &rx_packet,
+			    &ao_rx_packet,
 			    sizeof (struct ao_packet_recv),
 			    DMA_CFG0_WORDSIZE_8 |
 			    DMA_CFG0_TMODE_SINGLE |
@@ -92,160 +93,60 @@ ao_packet_recv(void)
 	ao_led_off(AO_LED_GREEN);
 
 	if (dma_done & AO_DMA_DONE) {
-		if (!(rx_packet.status & PKT_APPEND_STATUS_1_CRC_OK))
+		if (!(ao_rx_packet.status & PKT_APPEND_STATUS_1_CRC_OK))
 			return AO_DMA_ABORTED;
-		if (rx_packet.packet.len == AO_PACKET_SYN) {
-			rx_seq = rx_packet.packet.seq;
-			tx_packet.seq = rx_packet.packet.ack;
-			tx_packet.ack = rx_seq;
-		} else if (rx_packet.packet.len) {
-			if (rx_packet.packet.seq == (uint8_t) (rx_seq + (uint8_t) 1) && rx_used == rx_len) {
-#if 0
-				printf ("rx len %3d seq %3d ack %3d\n",
-					rx_packet.packet.len,
-					rx_packet.packet.seq,
-					rx_packet.packet.ack);
-				flush();
-#endif
-				memcpy(rx_data, rx_packet.packet.d, rx_packet.packet.len);
-				rx_used = 0;
-				rx_len = rx_packet.packet.len;
-				rx_seq = rx_packet.packet.seq;
-				tx_packet.ack = rx_seq;
-				ao_wakeup(&rx_data);
+		if (ao_rx_packet.packet.len == AO_PACKET_SYN) {
+			rx_seq = ao_rx_packet.packet.seq;
+			ao_tx_packet.seq = ao_rx_packet.packet.ack;
+			ao_tx_packet.ack = rx_seq;
+		} else if (ao_rx_packet.packet.len) {
+			if (ao_rx_packet.packet.seq == (uint8_t) (rx_seq + (uint8_t) 1) && ao_packet_rx_used == ao_packet_rx_len) {
+				memcpy(rx_data, ao_rx_packet.packet.d, ao_rx_packet.packet.len);
+				ao_packet_rx_used = 0;
+				ao_packet_rx_len = ao_rx_packet.packet.len;
+				rx_seq = ao_rx_packet.packet.seq;
+				ao_tx_packet.ack = rx_seq;
+				ao_wakeup(&ao_stdin_ready);
 			}
 		}
-		if (rx_packet.packet.ack == tx_packet.seq) {
-			tx_packet.len = 0;
-			ao_wakeup(&tx_packet);
+		if (ao_rx_packet.packet.ack == ao_tx_packet.seq) {
+			ao_tx_packet.len = 0;
+			ao_wakeup(&ao_tx_packet);
 		}
 	}
 	return dma_done;
 }
 
 void
-ao_packet_slave(void)
-{
-	ao_radio_set_packet();
-	tx_packet.addr = ao_serial_number;
-	tx_packet.len = AO_PACKET_SYN;
-	while (ao_packet_enable) {
-		ao_packet_recv();
-		ao_packet_send();
-	}
-	ao_exit();
-}
-
-/* Thread for the master side of the packet link */
-
-void
-ao_packet_master(void)
-{
-	uint8_t	status;
-
-	ao_radio_set_packet();
-	tx_packet.addr = ao_serial_number;
-	tx_packet.len = AO_PACKET_SYN;
-	while (ao_packet_enable) {
-		ao_packet_send();
-		ao_alarm(AO_MS_TO_TICKS(100));
-		status = ao_packet_recv();
-		if (status & AO_DMA_DONE) {
-			/* if we can transmit data, do so */
-			if (tx_used && tx_packet.len == 0)
-				continue;
-			ao_packet_master_sleeping = 1;
-			ao_delay(AO_MS_TO_TICKS(1000));
-			ao_packet_master_sleeping = 0;
-		}
-	}
-	ao_exit();
-}
-
-void
 ao_packet_flush(void)
 {
 	/* If there is data to send, and this is the master,
 	 * then poke the master to send all queued data
 	 */
-	if (tx_used && ao_packet_master_sleeping)
+	if (ao_packet_tx_used && ao_packet_master_sleeping)
 		ao_wake_task(&ao_packet_task);
 }
 
 void
-ao_packet_putchar(char c)
+ao_packet_putchar(char c) __reentrant
 {
-	while (tx_used == AO_PACKET_MAX && ao_packet_enable) {
+	while (ao_packet_tx_used == AO_PACKET_MAX && ao_packet_enable) {
 		ao_packet_flush();
 		ao_sleep(&tx_data);
 	}
 
 	if (ao_packet_enable)
-		tx_data[tx_used++] = c;
+		tx_data[ao_packet_tx_used++] = c;
 }
 
 char
-ao_packet_getchar(void) __critical
+ao_packet_pollchar(void) __critical
 {
-	while (rx_used == rx_len && ao_packet_enable) {
-		/* poke the master to get more data */
-		if (ao_packet_master_sleeping)
-			ao_wake_task(&ao_packet_task);
-		ao_sleep(&rx_data);
-	}
-
 	if (!ao_packet_enable)
-		return 0;
+		return AO_READ_AGAIN;
 
-	return rx_data[rx_used++];
-}
-
-static void
-ao_packet_echo(void) __reentrant
-{
-	uint8_t	c;
-	while (ao_packet_enable) {
-		c = ao_packet_getchar();
-		if (ao_packet_enable) {
-			putchar(c);
-			if (c == (uint8_t) '\n' || c == (uint8_t) '\r')
-				flush();
-		}
-	}
-	ao_exit();
-}
-
-static __xdata struct ao_task	ao_packet_echo_task;
-
-static void
-ao_packet_forward(void) __reentrant
-{
-	char c;
-	ao_packet_enable = 1;
-	ao_cmd_white();
-
-	if (ao_cmd_lex_c == 'm')
-		ao_add_task(&ao_packet_task, ao_packet_master, "master");
-	else
-		ao_add_task(&ao_packet_task, ao_packet_slave, "slave");
-	ao_add_task(&ao_packet_echo_task, ao_packet_echo, "echo");
-	while ((c = getchar()) != '~')
-		ao_packet_putchar(c);
-	ao_packet_enable = 0;
-	ao_radio_abort();
-	while (ao_packet_echo_task.wchan || ao_packet_task.wchan) {
-		ao_wake_task(&ao_packet_echo_task);
-		ao_wake_task(&ao_packet_task);
-	}
-}
-
-__code struct ao_cmds ao_packet_cmds[] = {
-	{ 'p',	ao_packet_forward,	"p {m|s}                            Remote packet link. m=master, s=slave" },
-	{ 0,	ao_packet_forward,	NULL },
-};
+	if (ao_packet_rx_used == ao_packet_rx_len)
+		return AO_READ_AGAIN;
 
-void
-ao_packet_init(void)
-{
-	ao_cmd_register(&ao_packet_cmds[0]);
+	return rx_data[ao_packet_rx_used++];
 }
diff --git a/src/ao_packet_master.c b/src/ao_packet_master.c
new file mode 100644
index 00000000..2751f414
--- /dev/null
+++ b/src/ao_packet_master.c
@@ -0,0 +1,142 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+
+static char
+ao_packet_getchar(void)
+{
+	char c;
+	while ((c = ao_packet_pollchar()) == AO_READ_AGAIN)
+	{
+		if (!ao_packet_enable)
+			break;
+		if (ao_packet_master_sleeping)
+			ao_wake_task(&ao_packet_task);
+		ao_sleep(&ao_stdin_ready);
+	}
+	return c;
+}
+
+static void
+ao_packet_echo(void) __reentrant
+{
+	uint8_t	c;
+	while (ao_packet_enable) {
+		c = ao_packet_getchar();
+		if (ao_packet_enable)
+			putchar(c);
+	}
+	ao_exit();
+}
+
+static __xdata struct ao_task	ao_packet_echo_task;
+static __xdata uint16_t		ao_packet_master_delay;
+static __xdata uint16_t		ao_packet_master_time;
+
+#define AO_PACKET_MASTER_DELAY_SHORT	AO_MS_TO_TICKS(100)
+#define AO_PACKET_MASTER_DELAY_LONG	AO_MS_TO_TICKS(1000)
+#define AO_PACKET_MASTER_DELAY_TIMEOUT	AO_MS_TO_TICKS(2000)
+
+static void
+ao_packet_master_busy(void)
+{
+	ao_packet_master_delay = AO_PACKET_MASTER_DELAY_SHORT;
+	ao_packet_master_time = ao_time();
+}
+
+static void
+ao_packet_master_check_busy(void)
+{
+	int16_t	idle;
+	if (ao_packet_master_delay != AO_PACKET_MASTER_DELAY_SHORT)
+		return;
+	idle = (int16_t) (ao_time() - ao_packet_master_time);
+
+	if (idle > AO_PACKET_MASTER_DELAY_TIMEOUT)
+		ao_packet_master_delay = AO_PACKET_MASTER_DELAY_LONG;
+}
+
+void
+ao_packet_master(void)
+{
+	uint8_t	status;
+
+	ao_radio_set_packet();
+	ao_tx_packet.addr = ao_serial_number;
+	ao_tx_packet.len = AO_PACKET_SYN;
+	ao_packet_master_time = ao_time();
+	ao_packet_master_delay = AO_PACKET_MASTER_DELAY_SHORT;
+	while (ao_packet_enable) {
+		ao_packet_send();
+		if (ao_tx_packet.len)
+			ao_packet_master_busy();
+		ao_packet_master_check_busy();
+		ao_alarm(ao_packet_master_delay);
+		status = ao_packet_recv();
+		if (status & AO_DMA_DONE) {
+			/* if we can transmit data, do so */
+			if (ao_packet_tx_used && ao_tx_packet.len == 0)
+				continue;
+			if (ao_rx_packet.packet.len)
+				ao_packet_master_busy();
+			else
+				flush();
+			ao_packet_master_sleeping = 1;
+			ao_delay(ao_packet_master_delay);
+			ao_packet_master_sleeping = 0;
+		}
+	}
+	ao_exit();
+}
+
+static void
+ao_packet_forward(void) __reentrant
+{
+	char c;
+	ao_packet_enable = 1;
+	ao_cmd_white();
+
+	flush();
+	ao_set_monitor(0);
+	ao_add_task(&ao_packet_task, ao_packet_master, "master");
+	ao_add_task(&ao_packet_echo_task, ao_packet_echo, "echo");
+	while ((c = getchar()) != '~') {
+		if (c == '\r') c = '\n';
+		ao_packet_putchar(c);
+	}
+	ao_packet_enable = 0;
+	ao_radio_abort();
+	while (ao_packet_echo_task.wchan || ao_packet_task.wchan) {
+		ao_wake_task(&ao_packet_echo_task);
+		ao_wake_task(&ao_packet_task);
+		ao_yield();
+	}
+}
+
+
+
+__code struct ao_cmds ao_packet_master_cmds[] = {
+	{ 'p',	ao_packet_forward,	"p                                  Remote packet link." },
+	{ 0,	ao_packet_forward,	NULL },
+};
+
+void
+ao_packet_master_init(void)
+{
+	ao_cmd_register(&ao_packet_master_cmds[0]);
+}
diff --git a/src/ao_packet_slave.c b/src/ao_packet_slave.c
new file mode 100644
index 00000000..ba5ad1c1
--- /dev/null
+++ b/src/ao_packet_slave.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+
+void
+ao_packet_slave(void)
+{
+	ao_radio_set_packet();
+	ao_tx_packet.addr = ao_serial_number;
+	ao_tx_packet.len = AO_PACKET_SYN;
+	while (ao_packet_enable) {
+		ao_packet_recv();
+		ao_packet_send();
+	}
+	ao_exit();
+}
+
+void
+ao_packet_slave_start(void)
+{
+	ao_packet_enable = 1;
+	ao_add_task(&ao_packet_task, ao_packet_slave, "slave");
+}
+
+void
+ao_packet_slave_stop(void)
+{
+	ao_packet_enable = 0;
+	ao_radio_abort();
+	while (ao_packet_task.wchan) {
+		ao_wake_task(&ao_packet_task);
+		ao_yield();
+	}
+	ao_radio_set_telemetry();
+}
+
+void
+ao_packet_slave_init(void)
+{
+	ao_add_stdio(ao_packet_pollchar,
+		     ao_packet_putchar,
+		     ao_packet_flush);
+}
diff --git a/src/ao_stdio.c b/src/ao_stdio.c
index fb8ce093..7bc416e1 100644
--- a/src/ao_stdio.c
+++ b/src/ao_stdio.c
@@ -21,22 +21,56 @@
  * Basic I/O functions to support SDCC stdio package
  */
 
+#define AO_NUM_STDIOS	2
+
+static __xdata struct ao_stdio stdios[AO_NUM_STDIOS];
+static __data int8_t ao_cur_stdio;
+static __data int8_t ao_num_stdios;
+
 void
 putchar(char c)
 {
 	if (c == '\n')
-		ao_usb_putchar('\r');
-	ao_usb_putchar(c);
+		(*stdios[ao_cur_stdio].putchar)('\r');
+	(*stdios[ao_cur_stdio].putchar)(c);
 }
 
 void
 flush(void)
 {
-	ao_usb_flush();
+	stdios[ao_cur_stdio].flush();
 }
 
+__xdata uint8_t ao_stdin_ready;
+
 char
-getchar(void)
+getchar(void) __reentrant
+{
+	char c;
+	int8_t stdio = ao_cur_stdio;
+
+	for (;;) {
+		c = stdios[stdio].pollchar();
+		if (c != AO_READ_AGAIN)
+			break;
+		if (++stdio == ao_num_stdios)
+			stdio = 0;
+		if (stdio == ao_cur_stdio)
+			ao_sleep(&ao_stdin_ready);
+	}
+	ao_cur_stdio = stdio;
+	return c;
+}
+
+void
+ao_add_stdio(char (*pollchar)(void),
+	     void (*putchar)(char),
+	     void (*flush)(void))
 {
-	return ao_usb_getchar();
+	if (ao_num_stdios == AO_NUM_STDIOS)
+		ao_panic(AO_PANIC_STDIO);
+	stdios[ao_num_stdios].pollchar = pollchar;
+	stdios[ao_num_stdios].putchar = putchar;
+	stdios[ao_num_stdios].flush = flush;
+	ao_num_stdios++;
 }
diff --git a/src/ao_teledongle.c b/src/ao_teledongle.c
index 98642180..e4828d80 100644
--- a/src/ao_teledongle.c
+++ b/src/ao_teledongle.c
@@ -33,7 +33,8 @@ main(void)
 	ao_monitor_init(AO_LED_GREEN, TRUE);
 	ao_rssi_init(AO_LED_RED);
 	ao_radio_init();
-	ao_packet_init();
+	ao_packet_slave_init();
+	ao_packet_master_init();
 	ao_config_init();
 	ao_start_scheduler();
 }
diff --git a/src/ao_telemetrum.c b/src/ao_telemetrum.c
index 07737f30..5250078e 100644
--- a/src/ao_telemetrum.c
+++ b/src/ao_telemetrum.c
@@ -40,7 +40,7 @@ main(void)
 	ao_gps_report_init();
 	ao_telemetry_init();
 	ao_radio_init();
-	ao_packet_init();
+	ao_packet_slave_init();
 	ao_igniter_init();
 	ao_config_init();
 	ao_start_scheduler();
diff --git a/src/ao_usb.c b/src/ao_usb.c
index 8926b9ca..daca71a7 100644
--- a/src/ao_usb.c
+++ b/src/ao_usb.c
@@ -53,7 +53,7 @@ ao_usb_isr(void) interrupt 6
 		ao_wakeup(&ao_usb_in_bytes);
 
 	if (USBOIF & (1 << AO_USB_OUT_EP))
-		ao_wakeup(&ao_usb_out_bytes);
+		ao_wakeup(&ao_stdin_ready);
 
 	if (USBCIF & USBCIF_RSTIF)
 		ao_usb_set_interrupts();
@@ -360,7 +360,7 @@ ao_usb_flush(void) __critical
 }
 
 void
-ao_usb_putchar(char c) __critical
+ao_usb_putchar(char c) __critical __reentrant
 {
 	if (!ao_usb_running)
 		return;
@@ -374,16 +374,13 @@ ao_usb_putchar(char c) __critical
 }
 
 char
-ao_usb_getchar(void) __critical
+ao_usb_pollchar(void) __critical
 {
-	__xdata char	c;
+	char c;
 	while (ao_usb_out_bytes == 0) {
-		for (;;) {
-			USBINDEX = AO_USB_OUT_EP;
-			if ((USBCSOL & USBCSOL_OUTPKT_RDY) != 0)
-				break;
-			ao_sleep(&ao_usb_out_bytes);
-		}
+		USBINDEX = AO_USB_OUT_EP;
+		if ((USBCSOL & USBCSOL_OUTPKT_RDY) == 0)
+			return AO_READ_AGAIN;
 		ao_usb_out_bytes = (USBCNTH << 8) | USBCNTL;
 	}
 	--ao_usb_out_bytes;
@@ -395,6 +392,16 @@ ao_usb_getchar(void) __critical
 	return c;
 }
 
+char
+ao_usb_getchar(void)
+{
+	char	c;
+
+	while ((c = ao_usb_pollchar()) == AO_READ_AGAIN)
+		ao_sleep(&ao_stdin_ready);
+	return c;
+}
+
 void
 ao_usb_enable(void)
 {
@@ -438,4 +445,5 @@ ao_usb_init(void)
 	ao_usb_enable();
 
 	ao_add_task(&ao_usb_task, ao_usb_ep0, "usb");
+	ao_add_stdio(ao_usb_pollchar, ao_usb_putchar, ao_usb_flush);
 }