--- a/lcd.c	Sun Jan 22 17:10:51 2012 +1030
+++ b/lcd.c	Sat Feb 04 13:29:31 2012 +1030
@@ -56,28 +56,28 @@
 */
 
 #include <stdio.h>
+#include <stdint.h>
 #include "stm32f10x.h"
 #include "lcd.h"
-
-static void	LCD_Doinit(void);
+#include "delay.h"
 
 #define Bank1_LCD_C	((uint32_t)0x60000000)    /* Register Address */
 #define Bank1_LCD_D	((uint32_t)0x60020000)    /* Data Address */
 
 void
-LCD_WR_Reg(uint16_t index, uint16_t val) {
+lcd_wr_reg(uint16_t index, uint16_t val) {
     *(__IO uint16_t *)(Bank1_LCD_C) = index;
     *(__IO uint16_t *)(Bank1_LCD_D) = val;
 }
 
 uint16_t
-LCD_RD_Reg(uint16_t index) {
+lcd_rd_reg(uint16_t index) {
     *(__IO uint16_t *)(Bank1_LCD_C) = index;
     return(*(__IO uint16_t *)(Bank1_LCD_D));
 }
 
 uint16_t
-LCD_RD_Data(int first) {
+lcd_rd_data(int first) {
     uint16_t a = 0;
     if (first)
 	a = *(__IO uint16_t *) (Bank1_LCD_D); 	/* Dummy */
@@ -87,313 +87,17 @@
 }
 
 void
-LCD_WR_StartData(uint16_t x, uint16_t y) {
-    LCD_WR_Reg(0x20, x);
-    LCD_WR_Reg(0x21, y);
+lcd_wr_startdata(uint16_t x, uint16_t y) {
+    lcd_wr_reg(0x20, x);
+    lcd_wr_reg(0x21, y);
     *(__IO uint16_t *)(Bank1_LCD_C) = 0x22;	/* Start GRAM write */
 }
 
 void
-LCD_WR_Data(uint16_t val) {
+lcd_wr_data(uint16_t val) {
     *(__IO uint16_t *)(Bank1_LCD_D) = val;
 }
 
-/* This is a bit rough and ready */
-void
-Delay(__IO uint32_t nCount) {
-    __IO uint32_t i;
-    
-    for(; nCount != 0; nCount--)
-	for (i = 0; i < 3900; i++)
-	    ;
-}
-
-uint8_t
-SPI_WriteByte(uint8_t out) {
-    uint8_t in = 0; 
-
-    /* Wait until the transmit buffer is empty */
-    while (SPI_I2S_GetFlagStatus(SPI1,SPI_I2S_FLAG_TXE) == RESET)
-	;
-    
-    /* Send the byte  */
-    SPI_I2S_SendData(SPI1, out); 
-
-    /* Wait until a data is received */
-    while (SPI_I2S_GetFlagStatus(SPI1,SPI_I2S_FLAG_RXNE) == RESET)
-	;
-    
-    /* Get the received data */
-    in = SPI_I2S_ReceiveData(SPI1); 
-
-    /* Return the shifted data */
-    return (in);
-}
-
-#define TP_SELECT()	GPIO_ResetBits(GPIOB, GPIO_Pin_7)
-#define TP_DESELECT()	GPIO_SetBits(GPIOB, GPIO_Pin_7)
-uint16_t
-TPRead(uint8_t type) { 
-    uint16_t x = 0;
-
-    /* Select device */
-    TP_SELECT();
-
-    /* Do conversion */
-    Delay(10);
-    SPI_WriteByte(type);
-
-    /* Read result */
-    Delay(10);
-    x = SPI_WriteByte(0x00);
-    x <<= 8;
-    x |= SPI_WriteByte(0x00);
-    Delay(10);
-
-    /* De-select device */
-    TP_DESELECT();
-
-    /* Right justify 12 bit result */
-    x = x >> 3;
-    return (x);
-}
-
-#define FL_SELECT()	GPIO_ResetBits(GPIOA, GPIO_Pin_4)
-#define FL_DESELECT()	GPIO_SetBits(GPIOA, GPIO_Pin_4)
-
-#define FL_READ		0x03
-#define FL_HSREAD	0x0b
-#define FL_32KERASE	0x52
-#define FL_64KERASE	0xd8
-#define FL_CHIPERASE	0x60
-#define FL_BYTEPROG	0x02
-#define FL_AAIWP	0xad
-#define FL_RDSR		0x05
-#define FL_EWSR		0x50
-#define FL_WRSR		0x01
-#define FL_WREN		0x06
-#define FL_WRDI		0x04
-#define FL_RDID		0x90
-#define FL_JEDECID	0x9f
-#define FL_EBSY		0x70
-#define FL_DBSY		0x80
-
-uint16_t
-FlashReadID(void) {
-    uint8_t	fac, dev;
-    
-    FL_SELECT();			/* Select device */
-
-    SPI_WriteByte(FL_RDID);		/* Send command */
-    SPI_WriteByte(0x00);		/* Send address cycles (ID data starts at 0) */
-    SPI_WriteByte(0x00);
-    SPI_WriteByte(0x00);
-    fac = SPI_WriteByte(0x00);		/* Read ID */
-    dev = SPI_WriteByte(0x00);
-  
-    FL_DESELECT();			/* De-select device */
-
-    return fac << 8 | dev;
-}
-
-uint8_t
-FlashReadStatus(void) {
-    uint8_t	status;
-    
-    FL_SELECT();			/* Select device */
-
-    SPI_WriteByte(FL_RDSR);		/* Send command */
-    SPI_WriteByte(0x00);		/* Send dummy byte for address cycle */
-    status = SPI_WriteByte(0x00);	/* Read status */
-  
-    FL_DESELECT();			/* De-select device */
-
-    return status;
-}
-
-void
-FlashWriteStatus(uint8_t status) {
-    /* Enable status write */
-    FL_SELECT();			/* Select device */
-    SPI_WriteByte(FL_EWSR);		/* Send command */
-    SPI_WriteByte(0x00);		/* Send data byte */
-    FL_DESELECT();
-
-    /* Actually write status */
-    FL_SELECT();			/* Re-select device for new command */			
-    SPI_WriteByte(FL_WRSR);		/* Send command */
-    SPI_WriteByte(status);		/* Send data byte */
-    FL_DESELECT();			/* De-select device */
-}
-
-void
-LCD_init(void) {
-    GPIO_InitTypeDef			GPIO_InitStructure;
-    SPI_InitTypeDef			SPI_InitStructure;
-    FSMC_NORSRAMInitTypeDef		FSMC_NORSRAMInitStructure;
-    FSMC_NORSRAMTimingInitTypeDef	p;
-    TIM_TimeBaseInitTypeDef		TIM_TimeBaseStructure;
-    
-    /* Enable FSMC clock */
-    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_FSMC, ENABLE);
-
-    /* Enable alternate function IO clock */
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
-
-    /* Enable GPIOD clock */
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE);
-
-    /* Enable GPIOD clock */
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOE, ENABLE);
-
-    /* Configures LCD Control lines (FSMC Pins) in alternate function Push-Pull mode.
-     *
-     * PD0(D2), PD1(D3), PD4(NOE), PD5(NWE), PD7(NE1/CS), PD8(D13), PD9(D14),
-     * PD10(D15), PD11(A16/RS) PD14(D0), PD15(D1)
-     */
-    GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_7 |
-				   GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11 | 
-				   GPIO_Pin_14 | GPIO_Pin_15);
-    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
-    GPIO_Init(GPIOD, &GPIO_InitStructure);
-
-    /* PE7(D4), PE8(D5), PE9(D6), PE10(D7), PE11(D8), PE12(D9), PE13(D10),
-     * PE14(D11), PE15(D12)
-     */
-    GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 | 
-				   GPIO_Pin_11 | GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 |
-				   GPIO_Pin_15);
-    GPIO_Init(GPIOE, &GPIO_InitStructure);
-
-    /* Configure backlight control (PD13/FSMC_A18 remapped to TIM4_CH2) */
-    /* Enable TIM4 clock */
-    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
-
-    /* Enable timer function
-     * Note source clock is SYSCLK / 2 = 36MHz
-     */
-    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
-    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
-    GPIO_Init(GPIOD, &GPIO_InitStructure);
-
-    /* Remap TIM4_CH2 to PD13 */
-    GPIO_PinRemapConfig(GPIO_Remap_TIM4, ENABLE);
-    
-    /* Reset TIM4 */
-    TIM_DeInit(TIM4);
-
-    /* Time Base configuration */
-    TIM_TimeBaseStructure.TIM_Period = 999;
-    TIM_TimeBaseStructure.TIM_Prescaler = 0;
-    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
-    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Down;
-
-    TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
-
-    /* Init to full brightness */
-    lcd_setpwm(1000);
-
-    /* Enable timer */
-    TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
-    TIM_ARRPreloadConfig(TIM4, ENABLE);
-    TIM_Cmd(TIM4, ENABLE);
-
-    /* Configure reset pin (PE1) as GPIO out PP */
-    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
-    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
-    GPIO_Init(GPIOE, &GPIO_InitStructure);
-
-    /* Configures the Parallel interface (FSMC) for LCD (Parallel mode) */
-    /* Timing configuration */
-    p.FSMC_AddressSetupTime = 5;
-    p.FSMC_AddressHoldTime = 5;
-    p.FSMC_DataSetupTime = 5;
-    p.FSMC_BusTurnAroundDuration = 0;
-    p.FSMC_CLKDivision = 0;
-    p.FSMC_DataLatency = 0;
-    p.FSMC_AccessMode = FSMC_AccessMode_A;
-
-    /* FSMC_Bank1_NORSRAM1 configured as follows:
-       - Data/Address MUX = Disable
-       - Memory Type = SRAM
-       - Data Width = 16bit
-       - Write Operation = Enable
-       - Extended Mode = Disable
-       - Asynchronous Wait = Disable */
-    FSMC_NORSRAMInitStructure.FSMC_Bank = FSMC_Bank1_NORSRAM1;
-    FSMC_NORSRAMInitStructure.FSMC_DataAddressMux = FSMC_DataAddressMux_Disable;
-    FSMC_NORSRAMInitStructure.FSMC_MemoryType = FSMC_MemoryType_SRAM;
-    FSMC_NORSRAMInitStructure.FSMC_MemoryDataWidth = FSMC_MemoryDataWidth_16b;
-    FSMC_NORSRAMInitStructure.FSMC_BurstAccessMode = FSMC_BurstAccessMode_Disable;
-    FSMC_NORSRAMInitStructure.FSMC_AsynchronousWait = FSMC_AsynchronousWait_Disable;
-    FSMC_NORSRAMInitStructure.FSMC_WaitSignalPolarity = FSMC_WaitSignalPolarity_Low;
-    FSMC_NORSRAMInitStructure.FSMC_WrapMode = FSMC_WrapMode_Disable;
-    FSMC_NORSRAMInitStructure.FSMC_WaitSignalActive = FSMC_WaitSignalActive_BeforeWaitState;
-    FSMC_NORSRAMInitStructure.FSMC_WriteOperation = FSMC_WriteOperation_Enable;
-    FSMC_NORSRAMInitStructure.FSMC_WaitSignal = FSMC_WaitSignal_Disable;
-    FSMC_NORSRAMInitStructure.FSMC_ExtendedMode = FSMC_ExtendedMode_Disable;
-    FSMC_NORSRAMInitStructure.FSMC_WriteBurst = FSMC_WriteBurst_Disable;
-    FSMC_NORSRAMInitStructure.FSMC_ReadWriteTimingStruct = &p;
-    FSMC_NORSRAMInitStructure.FSMC_WriteTimingStruct = &p;
-    FSMC_NORSRAMInit(&FSMC_NORSRAMInitStructure);  
-
-    /* Enable FSMC_Bank1_NORSRAM1 */
-    FSMC_NORSRAMCmd(FSMC_Bank1_NORSRAM1, ENABLE);
-
-    /* Configure touch screen controller
-     *
-     * Connected to SPI1 which is shared with the AT45DB161D.
-     *
-     * The touch screen is selected with PB7.
-     * The flash chip is selected with PA4.
-     */
-
-    /* Enable SPI1 clock */
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE);
-
-    /* Configure MOSI, MISO and SCLK as alternate function PP */
-    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7;
-    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
-    GPIO_Init(GPIOA, &GPIO_InitStructure);
-
-    /* Configure flash chip select pin (PA4) as GPIO out PP */
-    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
-    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
-    GPIO_Init(GPIOA, &GPIO_InitStructure);
-
-    /* Configure touch chip select pin (PB7) as GPIO out PP */
-    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
-    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
-    GPIO_Init(GPIOB, &GPIO_InitStructure);
-
-    /* De-select touch & flash */
-    GPIO_SetBits(GPIOA, GPIO_Pin_4);
-    GPIO_SetBits(GPIOB, GPIO_Pin_7);
-    
-    /* SPI1 Config */
-    SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
-    SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
-    SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
-    SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
-    SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
-    SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
-    SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_64;
-    SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
-    SPI_InitStructure.SPI_CRCPolynomial = 7;
-    SPI_Init(SPI1, &SPI_InitStructure);
-
-    /* SPI1 enable */
-    SPI_Cmd(SPI1, ENABLE);  
-
-    LCD_Doinit();
-}
-
 void
 lcd_setpwm(uint16_t val) {
     TIM_OCInitTypeDef			TIM_OCInitStructure;
@@ -408,127 +112,206 @@
 
 }
 
-static void
-LCD_Doinit(void) {
+void
+lcd_init(void) {
     uint16_t				id;
 
     /* Initialise LCD panel */
 
     /* Pull reset pin low */
-    Delay(1);
+    delay(1);
     GPIO_ResetBits(GPIOE, GPIO_Pin_1);
-    Delay(10);
+    delay(10);
     GPIO_SetBits(GPIOE, GPIO_Pin_1);
-    Delay(50);
+    delay(50);
 
-    id = LCD_RD_Reg(0x00);
+    id = lcd_rd_reg(0x00);
     if (id != 0x9325) {
 	printf("LCD ID doesn't match, expected 0x9325 got 0x%x\r\n", id);
 	return;
     }
     printf("LCD ID matches\r\n");
     
-    LCD_WR_Reg(0x00e3, 0x3008); /* Set internal timing (not documented) */
-    LCD_WR_Reg(0x00e7, 0x0012); /* Set internal timing (not documented) */
-    LCD_WR_Reg(0x00ef, 0x1231); /* Set internal timing (not documented) */
-    LCD_WR_Reg(0x0000, 0x0001); /* Start Oscillation */
-    Delay(50);
-    LCD_WR_Reg(0x0001, 0x0100); /* set SS (S720 -> S1) */
-    LCD_WR_Reg(0x0002, 0x0700); /* set line inversion (B/C + EOR) */
-    LCD_WR_Reg(0x0004, 0x0000); /* no resizing */
-    LCD_WR_Reg(0x0008, 0x0202); /* set the back porch and front porch (2 lines each) */
-    LCD_WR_Reg(0x0009, 0x0000); /* set non-display area refresh cycle ISC[3:0] */
-    LCD_WR_Reg(0x000a, 0x0000); /* FMARK function */
+    lcd_wr_reg(0x00e3, 0x3008); /* Set internal timing (not documented) */
+    lcd_wr_reg(0x00e7, 0x0012); /* Set internal timing (not documented) */
+    lcd_wr_reg(0x00ef, 0x1231); /* Set internal timing (not documented) */
+    lcd_wr_reg(0x0000, 0x0001); /* Start Oscillation */
+    delay(50);
+    lcd_wr_reg(0x0001, 0x0100); /* set SS (S720 -> S1) */
+    lcd_wr_reg(0x0002, 0x0700); /* set line inversion (B/C + EOR) */
+    lcd_wr_reg(0x0004, 0x0000); /* no resizing */
+    lcd_wr_reg(0x0008, 0x0202); /* set the back porch and front porch (2 lines each) */
+    lcd_wr_reg(0x0009, 0x0000); /* set non-display area refresh cycle ISC[3:0] */
+    lcd_wr_reg(0x000a, 0x0000); /* FMARK function */
 
-    LCD_WR_Reg(0x000c, 0x0000); /* RGB ctl - Internal clock, 18bit interface */
-    LCD_WR_Reg(0x000d, 0x0000); /* Frame marker Position */
-    LCD_WR_Reg(0x000f, 0x0000); /* RGB interface polarity */
+    lcd_wr_reg(0x000c, 0x0000); /* RGB ctl - Internal clock, 18bit interface */
+    lcd_wr_reg(0x000d, 0x0000); /* Frame marker Position */
+    lcd_wr_reg(0x000f, 0x0000); /* RGB interface polarity */
 
     /* Power On sequence  */
-    LCD_WR_Reg(0x0010, 0x0000); /* SAP, BT[3:0], AP, DSTB, SLP, STB */
-    LCD_WR_Reg(0x0011, 0x0007); /* DC1[2:0], DC0[2:0], VC[2:0] */
-    LCD_WR_Reg(0x0012, 0x0000); /* VREG1OUT voltage */
-    LCD_WR_Reg(0x0013, 0x0000); /* VDV[4:0] for VCOM amplitude */
-    Delay(200);                 /* Dis-charge capacitor power voltage */
-    LCD_WR_Reg(0x0010, 0x1690); /* SAP, BT[3:0], AP, DSTB, SLP, STB */
-    LCD_WR_Reg(0x0011, 0x0227); /* R11h = 0x0221 at VCI = 3.3V, DC1[2:0], DC0[2:0], VC[2:0] */
-    Delay(50);                  /* Delay 50ms */
-    LCD_WR_Reg(0x0012, 0x001c); /* External reference voltage= Vci; */
-    Delay(50);                  /* Delay 50ms */
-    LCD_WR_Reg(0x0013, 0x1800); /* R13 = 1200 when R12 = 009D; VDV[4:0] for VCOM amplitude */
-    LCD_WR_Reg(0x0029, 0x001c); /* R29 = 000C when R12 = 009D; VCM[5:0] for VCOMH */
-    LCD_WR_Reg(0x002b, 0x000d); /* Frame Rate = 91Hz */
-    Delay(50);                  /* Delay 50ms */
+    lcd_wr_reg(0x0010, 0x0000); /* SAP, BT[3:0], AP, DSTB, SLP, STB */
+    lcd_wr_reg(0x0011, 0x0007); /* DC1[2:0], DC0[2:0], VC[2:0] */
+    lcd_wr_reg(0x0012, 0x0000); /* VREG1OUT voltage */
+    lcd_wr_reg(0x0013, 0x0000); /* VDV[4:0] for VCOM amplitude */
+    delay(200);                 /* Dis-charge capacitor power voltage */
+    lcd_wr_reg(0x0010, 0x1690); /* SAP, BT[3:0], AP, DSTB, SLP, STB */
+    lcd_wr_reg(0x0011, 0x0227); /* R11h = 0x0221 at VCI = 3.3V, DC1[2:0], DC0[2:0], VC[2:0] */
+    delay(50);                  /* delay 50ms */
+    lcd_wr_reg(0x0012, 0x001c); /* External reference voltage= Vci; */
+    delay(50);                  /* delay 50ms */
+    lcd_wr_reg(0x0013, 0x1800); /* R13 = 1200 when R12 = 009D; VDV[4:0] for VCOM amplitude */
+    lcd_wr_reg(0x0029, 0x001c); /* R29 = 000C when R12 = 009D; VCM[5:0] for VCOMH */
+    lcd_wr_reg(0x002b, 0x000d); /* Frame Rate = 91Hz */
+    delay(50);                  /* delay 50ms */
 
     /* Set GRAM area */
-    LCD_WR_Reg(0x0050, 0x0000); /* Horizontal GRAM Start Address */
-    LCD_WR_Reg(0x0051, 0x00ef); /* Horizontal GRAM End Address */
-    LCD_WR_Reg(0x0052, 0x0000); /* Vertical GRAM Start Address */
-    LCD_WR_Reg(0x0053, 0x013f); /* Vertical GRAM Start Address */
+    lcd_wr_reg(0x0050, 0x0000); /* Horizontal GRAM Start Address */
+    lcd_wr_reg(0x0051, 0x00ef); /* Horizontal GRAM End Address */
+    lcd_wr_reg(0x0052, 0x0000); /* Vertical GRAM Start Address */
+    lcd_wr_reg(0x0053, 0x013f); /* Vertical GRAM Start Address */
 
-    LCD_WR_Reg(0x0060, 0xa700); /* Gate Scan Line, drive G320 -> G1 */
-    LCD_WR_Reg(0x0061, 0x0003); /* VLE & REV */
-    LCD_WR_Reg(0x006a, 0x0000); /* set scrolling line */
+    lcd_wr_reg(0x0060, 0xa700); /* Gate Scan Line, drive G320 -> G1 */
+    lcd_wr_reg(0x0061, 0x0003); /* VLE & REV */
+    lcd_wr_reg(0x006a, 0x0000); /* set scrolling line */
 
     /* Partial Display Control */
-    LCD_WR_Reg(0x0080, 0x0000); /* Image 1 */
-    LCD_WR_Reg(0x0081, 0x0000);
-    LCD_WR_Reg(0x0082, 0x0000);
-    LCD_WR_Reg(0x0083, 0x0000); /* Image 2 */
-    LCD_WR_Reg(0x0084, 0x0000);
-    LCD_WR_Reg(0x0085, 0x0000);
+    lcd_wr_reg(0x0080, 0x0000); /* Image 1 */
+    lcd_wr_reg(0x0081, 0x0000);
+    lcd_wr_reg(0x0082, 0x0000);
+    lcd_wr_reg(0x0083, 0x0000); /* Image 2 */
+    lcd_wr_reg(0x0084, 0x0000);
+    lcd_wr_reg(0x0085, 0x0000);
 
     /* Panel Control */
-    LCD_WR_Reg(0x0090, 0x0010);
-    LCD_WR_Reg(0x0092, 0x0000);
-    LCD_WR_Reg(0x0093, 0x0003);
-    LCD_WR_Reg(0x0095, 0x0110);
-    LCD_WR_Reg(0x0097, 0x0000); /* Undocumented */
-    LCD_WR_Reg(0x0098, 0x0000); /* Undocumented */
+    lcd_wr_reg(0x0090, 0x0010);
+    lcd_wr_reg(0x0092, 0x0000);
+    lcd_wr_reg(0x0093, 0x0003);
+    lcd_wr_reg(0x0095, 0x0110);
+    lcd_wr_reg(0x0097, 0x0000); /* Undocumented */
+    lcd_wr_reg(0x0098, 0x0000); /* Undocumented */
 
-    LCD_WR_Reg(0x0007, 0x0133); /* Display on, 262k colour mode (vs 8) */
+    lcd_wr_reg(0x0007, 0x0133); /* Display on, 262k colour mode (vs 8) */
 
-    LCD_WR_Reg(0x0003, 0x1030); /* set GRAM write direction and enable BGR, 64K colours, 1 transfers/pixel. */
+    lcd_wr_reg(0x0003, 0x1030); /* set GRAM write direction and enable BGR, 64K colours, 1 transfers/pixel. */
 
     /* Adjust the Gamma Curve */
-    LCD_WR_Reg(0x0030, 0x0006);
-    LCD_WR_Reg(0x0031, 0x0101);
-    LCD_WR_Reg(0x0032, 0x0003);
-    LCD_WR_Reg(0x0035, 0x0106);
-    LCD_WR_Reg(0x0036, 0x0b02);
-    LCD_WR_Reg(0x0037, 0x0302);
-    LCD_WR_Reg(0x0038, 0x0707);
-    LCD_WR_Reg(0x0039, 0x0007);
-    LCD_WR_Reg(0x003c, 0x0600);
-    LCD_WR_Reg(0x003d, 0x020b);
+    lcd_wr_reg(0x0030, 0x0006);
+    lcd_wr_reg(0x0031, 0x0101);
+    lcd_wr_reg(0x0032, 0x0003);
+    lcd_wr_reg(0x0035, 0x0106);
+    lcd_wr_reg(0x0036, 0x0b02);
+    lcd_wr_reg(0x0037, 0x0302);
+    lcd_wr_reg(0x0038, 0x0707);
+    lcd_wr_reg(0x0039, 0x0007);
+    lcd_wr_reg(0x003c, 0x0600);
+    lcd_wr_reg(0x003d, 0x020b);
 
     fputs("Testing\r\n", stdout);
-    LCD_WR_StartData(0, 0);
-    LCD_WR_Data(0xa5a5);
-    LCD_WR_Data(0x5a5a);
-    LCD_WR_StartData(0, 0);
-    if ((id = LCD_RD_Data(1)) != 0xa5a5)
+    lcd_wr_startdata(0, 0);
+    lcd_wr_data(0xa5a5);
+    lcd_wr_data(0x5a5a);
+    lcd_wr_startdata(0, 0);
+    if ((id = lcd_rd_data(1)) != 0xa5a5)
 	printf("Expected 0xa5a5 got 0x%04x\r\n", id);
     
-    if ((id = LCD_RD_Data(0)) != 0x5a5a)
+    if ((id = lcd_rd_data(0)) != 0x5a5a)
 	printf("Expected 0x5a5a got 0x%04x\r\n", id);
 
     fputs("Filling\r\n", stdout);
     /* Fill panel */
-    LCD_WR_StartData(0, 0);
-
-#define RGB24_565(R, G, B) (((((R) >> 3) & 0x1f) << 11) | ((((G) >> 2) & 0x3f) << 5) | (((B) >> 3) & 0x1f))
+    lcd_wr_startdata(0, 0);
 
     for (int x = 0; x < 320; x++) {
 	for (int y = 0; y < 240; y++) {
 	    if (((x / 5) % 3) == 0)
-		LCD_WR_Data(RGB24_565(255, 0, 0));
+		lcd_wr_data(LCD_RED);
 	    else if (((x / 5) % 3) == 1)
-		LCD_WR_Data(RGB24_565(0, 255, 0));
+		lcd_wr_data(LCD_GREEN);
 	    else
-		LCD_WR_Data(RGB24_565(0, 0, 255));
+		lcd_wr_data(LCD_BLUE);
 	}
     }
 
+    lcd_circle(50, 50, 20, LCD_BLACK);
+
+}
+
+void
+lcd_pixel(uint16_t x, uint16_t y, uint16_t colour) {
+    if (x > LCD_HEIGHT || y > LCD_WIDTH)
+	return;
+
+    lcd_wr_startdata(x, y);
+    lcd_wr_data(colour);
 }
 
+/* 
+ * Draw a circle
+ * From http://en.wikipedia.org/wiki/Midpoint_circle_algorithm 
+ */
+void
+lcd_circle(uint16_t x0, uint16_t y0, uint16_t radius, uint16_t colour) {
+    int16_t	f;
+    uint16_t	ddF_x, ddF_y, x, y;
+    
+    f = 1 - radius;
+    ddF_x = 1;
+    ddF_y = -2 * radius;
+    x = 0;
+    y = radius;
+ 
+    lcd_pixel(x0, y0 + radius, colour);
+    lcd_pixel(x0, y0 - radius, colour);
+    lcd_pixel(x0 + radius, y0, colour);
+    lcd_pixel(x0 - radius, y0, colour);
+ 
+    while(x < y) {
+	// ddF_x == 2 * x + 1;
+	// ddF_y == -2 * y;
+	// f == x*x + y*y - radius*radius + 2*x - y + 1;
+	if(f >= 0) {
+	    y--;
+	    ddF_y += 2;
+	    f += ddF_y;
+	}
+	x++;
+	ddF_x += 2;
+	f += ddF_x;    
+	lcd_pixel(x0 + x, y0 + y, colour);
+	lcd_pixel(x0 - x, y0 + y, colour);
+	lcd_pixel(x0 + x, y0 - y, colour);
+	lcd_pixel(x0 - x, y0 - y, colour);
+	lcd_pixel(x0 + y, y0 + x, colour);
+	lcd_pixel(x0 - y, y0 + x, colour);
+	lcd_pixel(x0 + y, y0 - x, colour);
+	lcd_pixel(x0 - y, y0 - x, colour);
+    }
+}
+
+
+/*
+ * Draw a line
+ * From http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
+ */
+void
+lcd_line(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t colour) {
+    uint16_t dx, dy, d, x, y;
+
+    dx = x1 - x0;
+    dy = y1 - y0;
+    
+    d = 2 * dy - dx;
+    lcd_pixel(x0, y0, colour);
+    y = y0;
+    
+    for (x = x0 + 1; x <= x1; x++) {
+	if (d > 0) {
+	    y++;
+	    lcd_pixel(x, y, colour);
+	    d += 2 * dy - 2 * dx;
+	} else {
+	    lcd_pixel(x, y, colour);
+	    d += 2 * dy;
+	}
+    }
+}