STM32duino R61509V LCD TFT Display

// GPIOA: X X X X . X X X X . DB7 DB6 DB5 DB4 . DB3 DB2 DB1 DB0
// GPIOB: X X X X . RST CS X X . X X X X . X X WR RS
#define WIDTH 240
#define HEIGHT 400

void lcdWriteDataWord(uint16_t data) {
    GPIOB->regs->BSRR = 0x04000000; // CS LOW,
    GPIOB->regs->BSRR = 0x00000003; // , WR RS HIGH
    GPIOA->regs->ODR = (GPIOA->regs->ODR & 0xFF00) | (data >> 8);
    GPIOB->regs->BSRR = 0x00020000; // WR LOW,
    GPIOB->regs->BSRR = 0x00000002; // , WR HIGH
    GPIOA->regs->ODR = (GPIOA->regs->ODR & 0xFF00) | (data & 0xFF);
    GPIOB->regs->BSRR = 0x00020000; // WR LOW,
    GPIOB->regs->BSRR = 0x00000003; // , WR RS HIGH
    GPIOB->regs->BSRR = 0x00000400; // , CS HIGH
}

void lcdWriteCommandWord(uint16_t data) {
    GPIOB->regs->BSRR = 0x04000000; // CS LOW,
    GPIOB->regs->BSRR = 0x00010002; // RS LOW, WR HIGH
    GPIOA->regs->ODR = (GPIOA->regs->ODR & 0xFF00) | (data >> 8);
    GPIOB->regs->BSRR = 0x00020000; // WR LOW,
    GPIOB->regs->BSRR = 0x00000002; // , WR HIGH
    GPIOA->regs->ODR = (GPIOA->regs->ODR & 0xFF00) | (data & 0xFF);
    GPIOB->regs->BSRR = 0x00020000; // WR LOW,
    GPIOB->regs->BSRR = 0x00000003; // ,WR RS HIGH
    GPIOB->regs->BSRR = 0x00000400; // , CS HIGH
}
void lcdWriteRegisterWord(uint16_t addr, uint16_t data) {
    lcdWriteCommandWord(addr);
    lcdWriteDataWord(data);
}

void lcdInit() {
    GPIOB->regs->CRH = (GPIOB->regs->CRH & 0xFFFF00FF) | 0x00003300; // RST CS OUT
    GPIOB->regs->CRL = (GPIOB->regs->CRL & 0xFFFFFF00) | 0x00000033; // WR RS OUT
    GPIOB->regs->BSRR = 0x00000C03; // , RST CS WR RS HIGH
    GPIOA->regs->CRL = 0x33333333;  // DB7 DB6 DB5 DB4 DB3 DB2 DB1 DB0 OUT
}

void lcdReset(void) {
    GPIOB->regs->BSRR = 0x08000000; // RST LOW,
    delay(100);
    GPIOB->regs->BSRR = 0x00000800; // , RST HIGH
    delay(100);
}

void lcdSetup(void) {
    lcdWriteRegisterWord(0x0000, 0x0000);
    lcdWriteRegisterWord(0x0000, 0x0000);
    lcdWriteRegisterWord(0x0000, 0x0000);
    lcdWriteRegisterWord(0x0000, 0x0000);
    delay(15);
    lcdWriteRegisterWord(0x0400, 0x6200);     //NL=0x31 (49) i.e. 400 rows
    lcdWriteRegisterWord(0x0008, 0x0808);
    lcdWriteRegisterWord(0x0300, 0x0C00);
    lcdWriteRegisterWord(0x0301, 0x5A0B);
    lcdWriteRegisterWord(0x0302, 0x0906);
    lcdWriteRegisterWord(0x0303, 0x1017);
    lcdWriteRegisterWord(0x0304, 0x2300);
    lcdWriteRegisterWord(0x0305, 0x1700);
    lcdWriteRegisterWord(0x0306, 0x6309);
    lcdWriteRegisterWord(0x0307, 0x0C09);
    lcdWriteRegisterWord(0x0308, 0x100C);
    lcdWriteRegisterWord(0x0309, 0x2232);
    lcdWriteRegisterWord(0x0010, 0x0016);     //69.5Hz         0016
    lcdWriteRegisterWord(0x0011, 0x0101);
    lcdWriteRegisterWord(0x0012, 0x0000);
    lcdWriteRegisterWord(0x0013, 0x0001);
    lcdWriteRegisterWord(0x0100, 0x0330);     //BT,AP
    lcdWriteRegisterWord(0x0101, 0x0237);     //DC0,DC1,VC
    lcdWriteRegisterWord(0x0103, 0x0D00);     //VDV
    lcdWriteRegisterWord(0x0280, 0x6100);     //VCM
    lcdWriteRegisterWord(0x0102, 0xC1B0);     //VRH,VCMR,PSON,PON
    delay(50);
    lcdWriteRegisterWord(0x0001, 0x0100);
    lcdWriteRegisterWord(0x0002, 0x0100);
    lcdWriteRegisterWord(0x0003, 0x1030);     //1030
    lcdWriteRegisterWord(0x0009, 0x0001);
    lcdWriteRegisterWord(0x000C, 0x0000);
    lcdWriteRegisterWord(0x0090, 0x8000);
    lcdWriteRegisterWord(0x000F, 0x0000);
    lcdWriteRegisterWord(0x0210, 0x0000);
    lcdWriteRegisterWord(0x0211, 0x00EF);
    lcdWriteRegisterWord(0x0212, 0x0000);
    lcdWriteRegisterWord(0x0213, 0x018F);     //432=01AF,400=018F
    lcdWriteRegisterWord(0x0500, 0x0000);
    lcdWriteRegisterWord(0x0501, 0x0000);
    lcdWriteRegisterWord(0x0502, 0x005F);     //???
    lcdWriteRegisterWord(0x0401, 0x0001);     //REV=1
    lcdWriteRegisterWord(0x0404, 0x0000);
    delay(50);
    lcdWriteRegisterWord(0x0007, 0x0100);     //BASEE
    delay(50);
    lcdWriteRegisterWord(0x0200, 0x0000);
    lcdWriteRegisterWord(0x0201, 0x0000);
}

void lcdDrawPixel(uint16_t x, uint16_t y, uint16_t color) {
    if (x < 0 || x >= WIDTH) return;
    if (y < 0 || y >= HEIGHT) return;
  lcdWriteRegisterWord(0x200, x); // MC
  lcdWriteRegisterWord(0x201, y); // MP
  lcdWriteRegisterWord(0x202, color); // MW
}

void lcdDrawFillRect(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t color){
  int i,j;
  if (x1 >= WIDTH) return;
  if (x1 < 0) x1=0;
  if (x2 < 0) return;
  if (x2 >= WIDTH) x2=WIDTH-1;
  if (y1 >= HEIGHT) return;
  if (y1 < 0) y1=0;
  if (y2 < 0) return;
  if (y2 >= HEIGHT) y2=HEIGHT-1;

  lcdWriteRegisterWord(0x200, x1); // MC
  lcdWriteRegisterWord(0x201, y1); // MP
  lcdWriteRegisterWord(0x210, x1); // SC
  lcdWriteRegisterWord(0x212, y1); // SP
  lcdWriteRegisterWord(0x211, x2); // EC
  lcdWriteRegisterWord(0x213, y2); // EP
  lcdWriteCommandWord(0x202); // MW
  for(j=y1; j<=y2; j++){
     for(i=x1; i<=x2; i++){
       lcdWriteDataWord(color);
     }
  }
}

void lcdDisplayOff(void) {
   lcdWriteRegisterWord(0x0007, 0x0010); // Set GON=0 D1=0, D0=0
}

void lcdDisplayOn(void) {
   lcdWriteRegisterWord(0x0007, 0x0053); // Set GON=1 D1=1, D0=1
}

void lcdFillScreen(uint16_t color) {
  lcdDrawFillRect(0, 0, WIDTH-1, HEIGHT-1, color);
}

uint16_t rgb565_conv(uint8_t r, uint8_t g, uint8_t b) {
  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
}

void setup(){
    lcdInit();
    lcdReset();
    lcdSetup();
}
void loop(){
    lcdFillScreen(rgb565_conv(255,0,0));
    delay(1000);
    lcdFillScreen(rgb565_conv(0,255,0));
    delay(1000);
    lcdFillScreen(rgb565_conv(0,0,255));
    delay(1000);
    for (uint8_t i=0; i<100; i++){
        lcdFillScreen(rgb565_conv(255,0,0));
        lcdFillScreen(rgb565_conv(0,255,0));
        lcdFillScreen(rgb565_conv(0,0,255));
    }
    lcdWriteRegisterWord(0x200, 0); //MC
  lcdWriteRegisterWord(0x201, 0); // MP
  lcdWriteRegisterWord(0x210, 0); // SC
  lcdWriteRegisterWord(0x212, 0); // SP
  lcdWriteRegisterWord(0x211, WIDTH-1); //EC
  lcdWriteRegisterWord(0x213, HEIGHT-1); // EP
  lcdWriteCommandWord(0x202); // MW
  uint16_t color = 0;
  int i,j;
  for(j=0; j<HEIGHT; j++){
     for(i=0; i<WIDTH; i++){
         color = rgb565_conv((i&4)^(j&32)?255:0, (i&8)^(j&32)?255:0, (i&16)^(j&32)?255:0);
       lcdWriteDataWord(color);
     }
  }
  delay(10000);
}