+static void lcd_delay(uint16_t us)
+{
+#ifdef LCD_ASYNC
+ lcd_busy = 1;
+ timer_start_us(0, us);
+#else
+ delay_us(us);
+#endif
+}
+
+#ifdef LCD_ASYNC
+static void lcd_wait(void)
+{
+ while(lcd_busy) ;
+}
+
+static void lcd_buffer_push(uint8_t byte)
+{
+ while((lcd_buf_head+1)%sizeof(lcd_buffer)==lcd_buf_tail) ;
+ lcd_buffer[lcd_buf_head] = byte;
+ lcd_buf_head = (lcd_buf_head+1)%sizeof(lcd_buffer);
+}
+
+static uint8_t lcd_buffer_pop(void)
+{
+ uint8_t byte = lcd_buffer[lcd_buf_tail];
+ lcd_buf_tail = (lcd_buf_tail+1)%sizeof(lcd_buffer);
+ return byte;
+}
+
+static void lcd_timer(void)
+{
+ if(lcd_busy)
+ {
+ if(lcd_buf_head!=lcd_buf_tail)
+ {
+ uint8_t byte = lcd_buffer_pop();
+ if(byte==0xFF)
+ {
+ REGSEL_PORT |= REGSEL_BIT;
+ byte = lcd_buffer_pop();
+ }
+ else if(byte>=0x80)
+ REGSEL_PORT &= ~REGSEL_BIT;
+ else
+ REGSEL_PORT |= REGSEL_BIT;
+ lcd_set_data(byte);
+ lcd_clock();
+ lcd_delay(50);
+ }
+ else
+ {
+ lcd_busy = 0;
+ timer_stop(0);
+ }
+ }
+}
+
+TIMER_SET_CALLBACK(0, lcd_timer)
+#endif
+