3 files changed, 107 insertions, 35 deletions

diff --git a/src/cc1111/ao_arch.h b/src/cc1111/ao_arch.h
index 23589e66..f0f0daae 100644
--- a/src/cc1111/ao_arch.h
+++ b/src/cc1111/ao_arch.h
@@ -205,17 +205,23 @@ struct ao_adc {
 #define AO_ADC_RING	32
 
 /* ao_button.c */
+#ifdef HAS_BUTTON
 void
-ao_p2_isr(void);
+ao_p0_isr(void) ao_arch_interrupt(13);
 
 void
-ao_button_init(void);
+ao_p1_isr(void) ao_arch_interrupt(15);
 
-#if HAS_BUTTON_P0
 void
-ao_p0_isr(void) ao_arch_interrupt(13);
+ao_p2_isr(void);
+
+#define HAS_P2_ISR	1
+
 #endif
 
+void
+ao_button_init(void);
+
 char
 ao_button_get(void) __critical;
 
diff --git a/src/cc1111/ao_button.c b/src/cc1111/ao_button.c
index 3516d559..77a8dde8 100644
--- a/src/cc1111/ao_button.c
+++ b/src/cc1111/ao_button.c
@@ -19,14 +19,22 @@
 
 volatile __xdata struct ao_fifo ao_button_fifo;
 
-#define BUTTON_1_PIN	(P0_4)
-#define BUTTON_1_MASK	(1 << 4)	/* P0_4 */
+static __code struct {
+	uint8_t	mask;
+	uint8_t	reg;
+} ao_buttons[] = {
+#ifdef BUTTON_1_MASK
+	{ BUTTON_1_MASK, BUTTON_1_REG },
+#endif
+#ifdef BUTTON_2_MASK
+	{ BUTTON_2_MASK, BUTTON_2_REG },
+#endif
+#ifdef BUTTON_3_MASK
+	{ BUTTON_3_MASK, BUTTON_3_REG },
+#endif
+};
 
-#define BUTTON_2_PIN	(P2_3)
-#define BUTTON_2_MASK	(1 << 3)	/* P2_3 */
-
-#define BUTTON_3_PIN	(P2_4)
-#define BUTTON_3_MASK	(1 << 4)	/* P2_4 */
+#define NUM_BUTTONS	((sizeof ao_buttons) / sizeof (ao_buttons[0]))
 
 static void
 ao_button_insert(char n)
@@ -35,52 +43,101 @@ ao_button_insert(char n)
 	ao_wakeup(&ao_button_fifo);
 }
 
+static void
+ao_button_isr(uint8_t flag, uint8_t reg)
+{
+	uint8_t	b;
+
+	for (b = 0; b < NUM_BUTTONS; b++)
+		if (ao_buttons[b].reg == reg && (ao_buttons[b].mask & flag))
+			ao_button_insert(b + 1);
+}
+
+static uint8_t
+ao_button_mask(uint8_t reg)
+{
+	uint8_t	b;
+	uint8_t mask = 0;
+
+	for (b = 0; b < NUM_BUTTONS; b++)
+		if (ao_buttons[b].reg == reg)
+			mask |= ao_buttons[b].mask;
+	return mask;
+}
+
 char
 ao_button_get(void) __critical
 {
 	char	b;
 
 	while (ao_fifo_empty(ao_button_fifo))
-		ao_sleep(&ao_button_fifo);
+		if (ao_sleep(&ao_button_fifo))
+			return 0;
 	ao_fifo_remove(ao_button_fifo, b);
 	return b;
 }
 
 void
-ao_p2_isr(void)
+ao_p0_isr(void) ao_arch_interrupt(13)
 {
-	if (P2IFG & BUTTON_2_MASK)
-		ao_button_insert(2);
-	if (P2IFG & BUTTON_3_MASK)
-		ao_button_insert(3);
-	P2IFG = 0;
+	P0IF = 0;
+	ao_button_isr(P0IFG, 0);
+	P0IFG = 0;
 }
 
 void
-ao_p0_isr(void) ao_arch_interrupt(13)
+ao_p1_isr(void) ao_arch_interrupt(15)
 {
-	P0IF = 0;
-	if (P0IFG & BUTTON_1_MASK)
-		ao_button_insert(1);
-	P0IFG = 0;
+	P1IF = 0;
+	ao_button_isr(P1IFG, 1);
+	P1IFG = 0;
+}
+
+/* Shared with USB */
+void
+ao_p2_isr(void)
+{
+	ao_button_isr(P2IFG, 2);
+	P2IFG = 0;
 }
 
 void
 ao_button_init(void)
 {
+	uint8_t	mask;
+
 	/* Pins are configured as inputs with pull-up by default */
 
-	/* Enable interrupts for P2_0 - P2_4
-	 * Enable interrupts for P0_4 - P0_7
-	 * Set P2 interrupts to falling edge
-	 * Set P0 interrupts to falling edge
-	 */
-	
 	/* Enable interrupts for P0 inputs */
-	IEN1 |= IEN1_P0IE;
+	mask = ao_button_mask(0);
+	if (mask) {
+		if (mask & 0x0f)
+			PICTL |= PICTL_P0IENL;
+		if (mask & 0xf0)
+			PICTL |= PICTL_P0IENH;
+		P0IFG = 0;
+		P0IF = 0;
+		IEN1 |= IEN1_P0IE;
+		PICTL |= PICTL_P0ICON;
+	}
 
-	/* Enable interrupts for P2 inputs */
-	IEN2 |= IEN2_P2IE;
+	/* Enable interrupts for P1 inputs */
+	mask = ao_button_mask(1);
+	if (mask) {
+		P1IEN |= mask;
+		P1IFG = 0;
+		P1IF = 0;
+		IEN2 |= IEN2_P1IE;
+		PICTL |= PICTL_P1ICON;
+	}
 
-	PICTL |= PICTL_P2IEN | PICTL_P0IENH | PICTL_P2ICON | PICTL_P0ICON;
+	/* Enable interrupts for P2 inputs */
+	mask = ao_button_mask(2);
+	if (mask) {
+		PICTL |= PICTL_P2IEN;
+		P2IFG = 0;
+		P2IF = 0;
+		IEN2 |= IEN2_P2IE;
+		PICTL |= PICTL_P2ICON;
+	}
 }
diff --git a/src/cc1111/ao_lcd_port.c b/src/cc1111/ao_lcd_port.c
index 324cc3d3..e61b1a60 100644
--- a/src/cc1111/ao_lcd_port.c
+++ b/src/cc1111/ao_lcd_port.c
@@ -17,15 +17,24 @@
 
 #include "ao.h"
 
+static void
+ao_lcd_port_delay(void)
+{
+	uint8_t	i;
+
+	for (i = 0; i < 100; i++)
+		ao_arch_nop();
+}
+
 void
 ao_lcd_port_put_nibble(uint8_t rs, uint8_t nibble)
 {
 	P0 = (P0 & 0xf0) | (nibble & 0x0f);
 	P1_1 = rs;
 	P1_0 = 1;
-	ao_delay(1);
+	ao_lcd_port_delay();
 	P1_0 = 0;
-	ao_delay(1);
+	ao_lcd_port_delay();
 }
 
 void