STM32duino S6D1121 LCD TFT Display

#define PINS_D0 PA0
#define PINS_D1 PA1
#define PINS_D2 PA2
#define PINS_D3 PA3
#define PINS_D4 PA4
#define PINS_D5 PA5
#define PINS_D6 PA6
#define PINS_D7 PA7
#define PINS_RST PB11
#define PINS_CS PB10
#define PINS_WR PB1
#define PINS_RS PB0
#define WIDTH 240
#define HEIGHT 320

void lcdWriteDataWord(uint16_t data) {
    GPIOB->regs->BRR = 0x0400; // CS LOW
  GPIOB->regs->BSRR = 0x0003; // WR RS HIGH
  GPIOA->regs->ODR = (GPIOA->regs->ODR & 0xFF00) | (data >> 8);
  GPIOB->regs->BRR = 0x0002; // WR LOW
  GPIOB->regs->BSRR = 0x0002; // WR HIGH
  GPIOA->regs->ODR = (GPIOA->regs->ODR & 0xFF00) | (data & 0xFF);
  GPIOB->regs->BRR = 0x0002; // WR LOW
  GPIOB->regs->BSRR = 0x003; // WR RS HIGH
  GPIOB->regs->BSRR = 0x0400; // CS HIGH
}

void lcdWriteCommandWord(uint16_t data) {
    GPIOB->regs->BRR = 0x0400; // CS LOW
  GPIOB->regs->BSRR = 0x0002; // WR HIGH
  GPIOB->regs->BRR = 0x0001; // RS LOW
  GPIOA->regs->ODR = (GPIOA->regs->ODR & 0xFF00) | (data >> 8);
  GPIOB->regs->BRR = 0x0002; // WR LOW
  GPIOB->regs->BSRR = 0x0002; // WR HIGH
  GPIOA->regs->ODR = (GPIOA->regs->ODR & 0xFF00) | (data & 0xFF);
  GPIOB->regs->BRR = 0x0002; // WR LOW
  GPIOB->regs->BSRR = 0x003; // WR RS HIGH
  GPIOB->regs->BSRR = 0x0400; // CS HIGH
}

void lcdWriteRegisterWord(uint16_t addr, uint16_t data) {
  lcdWriteCommandWord(addr);
  lcdWriteDataWord(data);
}

void lcdInit() {
  pinMode(PINS_RS, OUTPUT);
  digitalWrite(PINS_RS, HIGH);
  pinMode(PINS_WR, OUTPUT);
  digitalWrite(PINS_WR, HIGH);
  pinMode(PINS_RST, OUTPUT);
  digitalWrite(PINS_RST, HIGH);
  pinMode(PINS_CS, OUTPUT);
  digitalWrite(PINS_CS, HIGH);

  pinMode(PINS_D0, OUTPUT);
  pinMode(PINS_D1, OUTPUT);
  pinMode(PINS_D2, OUTPUT);
  pinMode(PINS_D3, OUTPUT);
  pinMode(PINS_D4, OUTPUT);
  pinMode(PINS_D5, OUTPUT);
  pinMode(PINS_D6, OUTPUT);
  pinMode(PINS_D7, OUTPUT);
}

void lcdReset(void) {
  digitalWrite(PINS_RST, LOW);
  delay(100);
  digitalWrite(PINS_RST, HIGH);
  delay(100);
}

void lcdSetup(void) {
   lcdWriteRegisterWord(0x0011,0x2004);
   lcdWriteRegisterWord(0x0013,0xCC00);
   lcdWriteRegisterWord(0x0015,0x2600);
   lcdWriteRegisterWord(0x0014,0x252A);
   lcdWriteRegisterWord(0x0012,0x0033);
   lcdWriteRegisterWord(0x0013,0xCC04);
   lcdWriteRegisterWord(0x0013,0xCC06);
   lcdWriteRegisterWord(0x0013,0xCC4F);
   lcdWriteRegisterWord(0x0013,0x674F);
   lcdWriteRegisterWord(0x0011,0x2003);
   lcdWriteRegisterWord(0x0030,0x2609);
   lcdWriteRegisterWord(0x0031,0x242C);
   lcdWriteRegisterWord(0x0032,0x1F23);
   lcdWriteRegisterWord(0x0033,0x2425);
   lcdWriteRegisterWord(0x0034,0x2226);
   lcdWriteRegisterWord(0x0035,0x2523);
   lcdWriteRegisterWord(0x0036,0x1C1A);
   lcdWriteRegisterWord(0x0037,0x131D);
   lcdWriteRegisterWord(0x0038,0x0B11);
   lcdWriteRegisterWord(0x0039,0x1210);
   lcdWriteRegisterWord(0x003A,0x1315);
   lcdWriteRegisterWord(0x003B,0x3619);
   lcdWriteRegisterWord(0x003C,0x0D00);
   lcdWriteRegisterWord(0x003D,0x000D);
   lcdWriteRegisterWord(0x0016,0x0007);
   lcdWriteRegisterWord(0x0001,0x0027);
   lcdWriteRegisterWord(0x0002,0x0013);
   lcdWriteRegisterWord(0x0003,0x0003);
   lcdWriteRegisterWord(0x0008,0x0303);
   lcdWriteRegisterWord(0x000A,0x000B);
   lcdWriteRegisterWord(0x000B,0x0003);
   lcdWriteRegisterWord(0x000C,0x0000);
   lcdWriteRegisterWord(0x0041,0x0000);
   lcdWriteRegisterWord(0x0050,0x0000);
   lcdWriteRegisterWord(0x0060,0x0005);
   lcdWriteRegisterWord(0x0070,0x000B);
   lcdWriteRegisterWord(0x0071,0x0000);
   lcdWriteRegisterWord(0x0078,0x0000);
   lcdWriteRegisterWord(0x007A,0x0000);
   lcdWriteRegisterWord(0x0079,0x0007);
   lcdWriteRegisterWord(0x0007,0x0053);
   lcdWriteRegisterWord(0x0079,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(0x0020,x); // RAM Address Set 1
   lcdWriteRegisterWord(0x0021,y); // RAM Address Set 2
   lcdWriteRegisterWord(0x0022,color); // Write Data to GRAM
}

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;
  for(j=y1;j<=y2;j++){
     lcdWriteRegisterWord(0x020,x1);
     lcdWriteRegisterWord(0x021,j);
     lcdWriteRegisterWord(0x022,color);
     for(i=x1+1;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(uint16_t r,uint16_t g,uint16_t b){
   unsigned int RR,GG,BB;
   RR = (r * 31 / 255) << 11;
   GG = (g * 63 / 255) << 5;
   BB = (b * 31 / 255);
   return(RR | GG | BB);
}

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));
    }
}