8 files changed, 1110 insertions(+), 46 deletions(-)

M include/delay.h
A include/display.h
A include/otm8009a.h
M src/clocks.c
A src/display.c
M src/main.c
A src/otm8009a.c
M src/usb_device.c

@@ 4,7 4,8 @@
 #include <stdint.h>
 #include <stm32h7xx.h>
 
-#define SYSCLK_HZ       ((uint32_t)480000000UL)
+#define SYSCLK_HZ ((uint32_t)64000000UL)
+/* #define SYSCLK_HZ       ((uint32_t)480000000UL) */
 #define D1CPRE_DIVIDER  ((uint32_t)1UL)
 #define HCLK_DIVIDER    ((uint32_t)2UL)
 

@@ 0,0 1,134 @@
+#include <stm32h747xx.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#ifndef DISPLAY_H
+#define DISPLAY_H
+
+typedef enum {
+  DSI_VIDEO_NON_BURST_PULSES = 0,
+  DSI_VIDEO_NON_BURST_EVENTS = 1,
+  DSI_VIDEO_BURST = 2,
+} dsi_video_mode_t;
+
+typedef struct {
+  uint16_t active_width;
+  uint16_t active_height;
+  uint16_t h_back_porch;
+  uint16_t h_front_porch;
+  uint16_t v_back_porch;
+  uint16_t v_front_porch;
+
+  // ?
+  /* uint16_t h_sync_lane_byte_cycles; */
+
+  uint16_t h_sync;
+  uint16_t v_sync;
+  bool h_sync_pol;
+  bool v_sync_pol;
+  bool not_data_enable_pol;
+  bool pixel_clock_pol;
+
+  dsi_video_mode_t video_mode;
+
+  uint8_t channel;
+} display_config_t;
+
+typedef enum {
+  CODING_16_BIT_1 = 0,
+  CODING_16_BIT_2 = 1,
+  CODING_16_BIT_3 = 2,
+  CODING_18_BIT_1 = 3,
+  CODING_18_BIT_2 = 4,
+  CODING_24_BIT = 5,
+} dsi_color_coding_t;
+
+typedef enum {
+  LTDC_ARGB8888 = 0,
+  LTDC_RGB888 = 1,
+  LTDC_RGB565 = 2,
+  LTDC_ARGB1555 = 3,
+  LTDC_ARGB4444 = 4,
+  LTDC_L8 = 5,
+  LTDC_AL44 = 6,
+  LTDC_AL88 = 7,
+} ltdc_layer_pixel_format_t;
+
+typedef struct {
+  uint8_t channel;
+  bool color_loosely_packed;
+  dsi_color_coding_t color_coding;
+  dsi_color_coding_t wrapper_color_coding;
+  uint8_t lp_size;
+  uint8_t vlp_size;
+} dsi_config_t;
+
+typedef enum {
+  DSI_MODE_ADAPTED_COMMAND,
+  DSI_MODE_VIDEO,
+} dsi_mode_t;
+
+typedef struct {
+  uint8_t divn;
+  uint8_t idf;
+  uint8_t odf;
+  uint8_t eckdiv;
+} dsi_pll_config_t;
+
+typedef struct {
+  bool crc_reception_enable;
+  bool ecc_reception_enable;
+  bool bta_enable;
+  bool eotp_reception_enable;
+  bool eotp_transmission_enable;
+} dsi_flow_config_t;
+
+typedef struct {
+  uint8_t data_lanes;
+  bool auto_clock_lane_control;
+  uint8_t clock_control;
+
+  uint8_t clock_hs2lp;
+  uint8_t clock_lp2hs;
+
+  uint8_t data_hs2lp;
+  uint8_t data_lp2hs;
+  uint8_t max_read_time;
+
+  uint8_t stop_wait_time;
+
+  uint8_t unit_interval;
+} dsi_phy_config_t;
+
+
+typedef struct {
+  DSI_TypeDef* dsi;
+  LTDC_TypeDef* ltdc;
+  LTDC_Layer_TypeDef* layer;
+  display_config_t config;
+} display_t;
+
+void display_init(display_t *display, DSI_TypeDef *dsi, LTDC_TypeDef* ltdc, LTDC_Layer_TypeDef* layer, display_config_t display_config);
+void display_start(display_t* display);
+
+void display_setup(display_t *display, uint32_t dsi_khz, uint32_t ltdc_khz,
+                   dsi_mode_t mode,
+                   dsi_pll_config_t pll, dsi_phy_config_t phy,
+                   dsi_flow_config_t flow, dsi_config_t dsi_config);
+
+void display_command_mode(display_t* display);
+void display_video_mode(display_t* display);
+
+void display_dsi_short_write(display_t* display, uint8_t lsb, uint8_t msb, uint8_t discriminant);
+void display_dsi_long_write(display_t* display, uint8_t cmd, uint8_t* data, uint16_t len, uint8_t discriminant);
+void display_dsi_read(display_t* display, uint8_t* buffer);
+
+void display_set_command_mode_transmission_kind(display_t* display, bool low_power);
+
+// Layer calls
+void display_layer_setup(display_t *display, void* buffer, ltdc_layer_pixel_format_t pixel_format);
+void display_set_framebuffer(display_t* display, void* buffer);
+void display_refresh(display_t* display);
+void display_reload(display_t *display);
+
+#endif // DISPLAY_H

@@ 0,0 1,90 @@
+#include "display.h"
+
+#ifndef OTM80009A_H
+#define OTM80009A_H
+
+#define OTM8009A_CMD_NOP 0x00 // NOP command
+#define OTM8009A_CMD_SWRESET 0x01 // Sw reset command
+#define OTM8009A_CMD_RDDMADCTL 0x0B // Read Display MADCTR command : read memory display access ctrl
+#define OTM8009A_CMD_RDDCOLMOD 0x0C // Read Display pixel format
+#define OTM8009A_CMD_SLPIN 0x10 // Sleep In command
+#define OTM8009A_CMD_SLPOUT 0x11 // Sleep Out command
+#define OTM8009A_CMD_PTLON 0x12 // Partial mode On command
+
+#define OTM8009A_CMD_DISPOFF 0x28 // Display Off command
+#define OTM8009A_CMD_DISPON 0x29 // Display On command
+
+#define OTM8009A_CMD_CASET 0x2A // Column address set command
+#define OTM8009A_CMD_PASET 0x2B // Page address set command
+
+#define OTM8009A_CMD_RAMWR 0x2C // Memory (GRAM) write command
+#define OTM8009A_CMD_RAMRD 0x2E // Memory (GRAM) read command
+
+#define OTM8009A_CMD_PLTAR 0x30 // Partial area command (4 parameters)
+
+#define OTM8009A_CMD_TEOFF 0x34 // Tearing Effect Line Off command : command with no parameter
+
+#define OTM8009A_CMD_TEEON 0x35 // Tearing Effect Line On command : command with 1 parameter 'TELOM'
+
+// Parameter TELOM : Tearing Effect Line Output Mode : possible values
+#define OTM8009A_TEEON_TELOM_VBLANKING_INFO_ONLY 0x00
+#define OTM8009A_TEEON_TELOM_VBLANKING_AND_HBLANKING_INFO 0x01
+
+#define OTM8009A_CMD_MADCTR 0x36 // Memory Access write control command
+
+// Possible used values of MADCTR
+#define OTM8009A_MADCTR_MODE_PORTRAIT 0x00
+#define OTM8009A_MADCTR_MODE_LANDSCAPE 0x60 // MY = 0, MX = 1, MV = 1, ML = 0, RGB = 0
+
+#define OTM8009A_CMD_IDMOFF 0x38 // Idle mode Off command
+#define OTM8009A_CMD_IDMON 0x39 // Idle mode On command
+
+#define OTM8009A_CMD_COLMOD 0x3A // Interface Pixel format command
+
+// Possible values of COLMOD parameter corresponding to used pixel formats
+#define OTM8009A_COLMOD_RGB565 0x55
+#define OTM8009A_COLMOD_RGB888 0x77
+#define OTM8009A_COLMOD_RGB888_3T 0b11100111
+
+#define OTM8009A_CMD_RAMWRC 0x3C // Memory write continue command
+#define OTM8009A_CMD_RAMRDC 0x3E // Memory read continue command
+
+#define OTM8009A_CMD_WRTESCN 0x44 // Write Tearing Effect Scan line command
+#define OTM8009A_CMD_RDSCNL 0x45 // Read  Tearing Effect Scan line command
+
+// CABC Management : ie : Content Adaptive Back light Control in IC OTM8009a
+#define OTM8009A_CMD_WRDISBV 0x51 // Write Display Brightness command
+#define OTM8009A_CMD_WRCTRLD 0x53 // Write CTRL Display command
+#define OTM8009A_CMD_WRCABC 0x55 // Write Content Adaptive Brightness command
+#define OTM8009A_CMD_WRCABCMB 0x5E // Write CABC Minimum Brightness command
+
+#define OTM8009A_CMD_ID1 0xDA // Read ID1 command
+#define OTM8009A_CMD_ID2 0xDB // Read ID2 command
+#define OTM8009A_CMD_ID3 0xDC // Read ID3 command
+
+typedef enum {
+  OTM8009A_35HZ = 0,
+  OTM8009A_40HZ = 1,
+  OTM8009A_45HZ = 2,
+  OTM8009A_50HZ = 3,
+  OTM8009A_55HZ = 4,
+  OTM8009A_60HZ = 5,
+  OTM8009A_70HZ = 6,
+} otm8009a_frame_rates_t;
+
+typedef enum {
+  OTM8009A_COLORMAP_RGB,
+  OTM8009A_COLORMAP_BGR,
+} otm8009a_colormap_t;
+
+typedef enum {
+  OTM8009A_MODE_PORTRAIT,
+  OTM8009A_MODE_LANDSCAPE,
+} otm8009a_mode_t;
+
+void otm8009_init(display_t *display, otm8009a_mode_t mode,
+                  otm8009a_frame_rates_t frame_rate,
+                  otm8009a_colormap_t colormap,
+                  uint16_t width, uint16_t height);
+
+#endif // OTM80009A_H

@@ 53,10 53,11 @@ void clocks_pll_configure(clock_pll_t pll, uint8_t vcosel,
 
   reg_write_bits_pos(&RCC->PLLCFGR,
                      (enable_p << 0) | (enable_q << 1) | (enable_r << 2),
-                     3*pll + RCC_PLLCFGR_DIVR1EN_Pos, 0x7);
+                     3*pll + RCC_PLLCFGR_DIVP1EN_Pos, 0x7);
 
   reg_write_bits_pos(&RCC->PLLCFGR, vcosel, (4*pll + RCC_PLLCFGR_PLL1VCOSEL_Pos), 1);
 
+
   reg_write_bits_pos(&RCC->PLLCKSELR, divm, RCC_PLLCKSELR_DIVM1_Pos + ((pll << 1) << 3), 0x3F);
   reg_write_bits_pos(&RCC->PLLCKSELR, source, RCC_PLLCKSELR_PLLSRC_Pos, 0x3);
 

@@ 0,0 1,350 @@
+#include <stm32h747xx.h>
+#include "display.h"
+#include "registers.h"
+#include "delay.h"
+
+void display_init(display_t *display, DSI_TypeDef *dsi, LTDC_TypeDef* ltdc, LTDC_Layer_TypeDef* layer, display_config_t display_config) {
+  display->dsi = dsi;
+  display->ltdc = ltdc;
+  display->config = display_config;
+  display->layer = layer;
+}
+
+void display_setup(display_t *display, uint32_t dsi_khz, uint32_t ltdc_khz,
+                   dsi_mode_t mode,
+                   dsi_pll_config_t pll, dsi_phy_config_t phy,
+                   dsi_flow_config_t flow, dsi_config_t dsi_config) {
+
+  // TODO: interrupts?
+  RCC->APB3ENR |= RCC_APB3ENR_DSIEN | RCC_APB3ENR_LTDCEN;
+
+  // Configure the RCC
+  RCC->APB3RSTR |= RCC_APB3RSTR_LTDCRST | RCC_APB3RSTR_DSIRST;
+  delay_us(3);
+  RCC->APB3RSTR &= ~(RCC_APB3RSTR_LTDCRST | RCC_APB3RSTR_DSIRST);
+
+  RCC->APB3ENR |= RCC_APB3ENR_DSIEN | RCC_APB3ENR_LTDCEN;
+
+  // Turn on DSI regulator, wait for ready
+  reg_set_bits(&display->dsi->WRPCR, DSI_WRPCR_REGEN);
+  while (!(display->dsi->WISR & DSI_WISR_RRIF));
+
+  // Confiugre DSI PLL, turn it ON and wait for lock
+  reg_write_bits(&display->dsi->WRPCR, (pll.divn << DSI_WRPCR_PLL_NDIV_Pos) |
+                 (pll.idf << DSI_WRPCR_PLL_IDF_Pos) |
+                 (pll.odf << DSI_WRPCR_PLL_ODF_Pos),
+                 DSI_WRPCR_PLL_NDIV_Msk | DSI_WRPCR_PLL_IDF_Msk | DSI_WRPCR_PLL_ODF_Msk);
+
+  reg_set_bits(&display->dsi->WRPCR, DSI_WRPCR_PLLEN);
+  delay_us(400);
+  while (!(display->dsi->WISR & DSI_WISR_PLLLIF));
+
+  // Enable D-PHY
+  reg_set_bits(&display->dsi->PCTLR, DSI_PCTLR_DEN);
+
+  // Enable D-PHY clock lane
+  reg_set_bits(&display->dsi->PCTLR, DSI_PCTLR_CKE);
+
+  // Configure LTDC
+  reg_write_bits(&display->ltdc->GCR,
+                 (display->config.h_sync_pol << LTDC_GCR_HSPOL_Pos) |
+                     (display->config.v_sync_pol << LTDC_GCR_VSPOL_Pos) |
+                 (display->config.not_data_enable_pol << LTDC_GCR_DEPOL_Pos) | (display->config.pixel_clock_pol << LTDC_GCR_PCPOL_Pos),
+                 LTDC_GCR_HSPOL_Msk | LTDC_GCR_VSPOL_Msk | LTDC_GCR_DEPOL_Msk | LTDC_GCR_PCPOL_Msk);
+
+  reg_write_bits(&display->ltdc->SSCR,
+                 ((display->config.v_sync - 1) << LTDC_SSCR_VSH_Pos) |
+                 ((display->config.h_sync - 1) << LTDC_SSCR_HSW_Pos),
+                 LTDC_SSCR_VSH_Msk | LTDC_SSCR_HSW_Msk);
+
+  reg_write_bits(&display->ltdc->BPCR,
+                 ((display->config.v_sync + display->config.v_back_porch - 1) << LTDC_BPCR_AVBP_Pos) |
+                 ((display->config.h_sync + display->config.h_back_porch - 1) << LTDC_BPCR_AHBP_Pos),
+                 LTDC_BPCR_AVBP_Msk | LTDC_BPCR_AHBP_Msk);
+
+  reg_write_bits(&display->ltdc->AWCR,
+                 ((display->config.v_sync + display->config.v_back_porch +
+                   display->config.active_height - 1) << LTDC_AWCR_AAH_Pos) |
+                 ((display->config.active_width + display->config.h_sync +
+                   display->config.h_back_porch - 1) << LTDC_AWCR_AAW_Pos),
+                 LTDC_AWCR_AAH_Msk | LTDC_AWCR_AAW_Msk);
+
+  reg_write_bits(&display->ltdc->TWCR,
+                 ((display->config.active_height +
+                   display->config.v_front_porch +
+                   display->config.v_back_porch - 1)
+                  << LTDC_TWCR_TOTALH_Pos) |
+                 ((display->config.active_width +
+                   display->config.h_front_porch +
+                   display->config.h_back_porch - 1)
+                  << LTDC_TWCR_TOTALW_Pos),
+                 LTDC_TWCR_TOTALH_Msk | LTDC_TWCR_TOTALW_Msk);
+
+  reg_write_bits(&display->ltdc->BCCR, 0, LTDC_BCCR_BCBLUE_Msk | LTDC_BCCR_BCRED_Msk | LTDC_BCCR_BCGREEN_Msk);
+
+  // Enable LTDC in the LTDC
+  reg_set_bits(&display->ltdc->GCR, LTDC_GCR_LTDCEN);
+
+  // Configure D-PHY parameters in dsi host and wrapper
+  reg_write_bits(&display->dsi->WPCR[0], phy.unit_interval << DSI_WPCR0_UIX4_Pos, DSI_WPCR0_UIX4_Msk);
+  reg_write_bits(&display->dsi->PCONFR, phy.data_lanes << DSI_PCONFR_NL_Pos, DSI_PCONFR_NL_Msk);
+  reg_write_bits(&display->dsi->CLCR,
+                 (phy.auto_clock_lane_control << DSI_CLCR_ACR_Pos) |
+                     (phy.clock_control << DSI_CLCR_DPCC_Pos),
+                 DSI_CLCR_ACR_Msk | DSI_CLCR_DPCC_Msk);
+
+  uint32_t max_delay = (phy.clock_hs2lp > phy.clock_lp2hs) ? phy.clock_hs2lp : phy.clock_lp2hs;
+  reg_write_bits(&display->dsi->CLTCR,
+                 (max_delay << DSI_CLTCR_HS2LP_TIME_Pos) |
+                 (max_delay << DSI_CLTCR_LP2HS_TIME_Pos),
+                 DSI_CLTCR_HS2LP_TIME_Msk | DSI_CLTCR_LP2HS_TIME_Msk);
+  reg_write_bits(&display->dsi->DLTCR,
+                 (phy.data_hs2lp << DSI_DLTCR_HS2LP_TIME_Pos) |
+                     (phy.data_lp2hs << DSI_DLTCR_LP2HS_TIME_Pos) |
+                 (phy.max_read_time << DSI_DLTCR_MRD_TIME_Pos),
+                 DSI_DLTCR_HS2LP_TIME_Msk | DSI_DLTCR_LP2HS_TIME_Msk | DSI_DLTCR_MRD_TIME_Msk);
+
+  reg_write_bits(&display->dsi->PCONFR,
+                 phy.stop_wait_time << DSI_PCONFR_SW_TIME_Pos,
+                 DSI_PCONFR_SW_TIME_Msk);
+
+  // Configure DSI host timing
+  reg_write_bits(&display->dsi->CCR,
+                 pll.eckdiv << DSI_CCR_TXECKDIV_Pos,
+                 DSI_CCR_TXECKDIV_Msk);
+  // TODO: are timeouts required to be configured?
+  /* reg_write_bits(&display->dsi->TCCR[0], );*/
+
+  // Configure DSI host flow control and dbi interface
+  reg_write_bits(&display->dsi->PCR,
+                 (flow.bta_enable << DSI_PCR_BTAE_Pos) |
+                 (flow.ecc_reception_enable << DSI_PCR_ECCRXE_Pos) |
+                 (flow.crc_reception_enable << DSI_PCR_CRCRXE_Pos) |
+                 (flow.eotp_reception_enable << DSI_PCR_ETRXE_Pos) |
+                 (flow.eotp_transmission_enable << DSI_PCR_ETTXE_Pos),
+                 DSI_PCR_BTAE_Msk | DSI_PCR_ECCRXE_Msk | DSI_PCR_ECCRXE_Msk | DSI_PCR_ETRXE_Msk | DSI_PCR_ETTXE_Msk);
+
+  // Configure DSI host for video mode
+  if (mode == DSI_MODE_VIDEO) {
+    reg_clear_bits(&display->dsi->MCR, DSI_MCR_CMDM);
+    reg_write_bits(&display->dsi->WCFGR, (0 << DSI_WCFGR_DSIM_Pos) |
+                   (0 << DSI_WCFGR_TESRC_Pos) |
+                   (0 << DSI_WCFGR_TEPOL_Pos) | (0 << DSI_WCFGR_AR_Pos),
+                   DSI_WCFGR_DSIM_Msk | DSI_WCFGR_TESRC_Msk | DSI_WCFGR_TEPOL_Msk | DSI_WCFGR_AR_Msk);
+
+    reg_write_bits(&display->dsi->VMCR,
+                   (display->config.video_mode << DSI_VMCR_VMT_Pos) |
+                       (1 << DSI_VMCR_LPVSAE_Pos) | (1 << DSI_VMCR_LPVBPE_Pos) |
+                       (1 << DSI_VMCR_LPVFPE_Pos) | (1 << DSI_VMCR_LPVAE_Pos) |
+                       (1 << DSI_VMCR_LPHBPE_Pos) | (1 << DSI_VMCR_LPHFPE_Pos) |
+                       (1 << DSI_VMCR_LPCE_Pos) | (1 << DSI_VMCR_FBTAAE_Pos),
+                   DSI_VMCR_VMT_Msk | DSI_VMCR_LPVSAE_Msk |
+                   DSI_VMCR_LPVBPE_Msk | DSI_VMCR_LPVFPE_Msk |
+                   DSI_VMCR_LPVAE_Msk | DSI_VMCR_LPHBPE_Msk |
+                   DSI_VMCR_LPHFPE_Msk |
+                   DSI_VMCR_LPCE_Msk | DSI_VMCR_FBTAAE_Msk);
+
+    reg_write_bits(&display->dsi->VPCR,
+                   display->config.active_width << DSI_VPCR_VPSIZE_Pos,
+                   DSI_VPCR_VPSIZE_Msk);
+
+    reg_write_bits(&display->dsi->VCCR, 1 << DSI_VCCR_NUMC_Pos, DSI_VCCR_NUMC_Msk);
+    reg_write_bits(&display->dsi->VNPCR, 0 << DSI_VNPCR_NPSIZE_Pos, DSI_VNPCR_NPSIZE_Msk);
+
+    // Horizontal sync active
+    uint32_t hsa = display->config.h_sync * dsi_khz / ltdc_khz;
+    reg_write_bits(&display->dsi->VHSACR, (hsa << DSI_VHSACR_HSA_Pos), DSI_VHSACR_HSA_Msk);
+    // Horizontal back porch
+    uint32_t hbp = display->config.h_back_porch * dsi_khz / ltdc_khz;
+    reg_write_bits(&display->dsi->VHBPCR, (hbp << DSI_VHBPCR_HBP_Pos), DSI_VHBPCR_HBP_Msk);
+
+    uint32_t hline = display->config.h_sync + display->config.h_back_porch + display->config.active_width + display->config.h_front_porch;
+    hline *= dsi_khz;
+    hline /= ltdc_khz;
+    reg_write_bits(&display->dsi->VLCR, (hline << DSI_VLCR_HLINE_Pos), DSI_VLCR_HLINE_Msk);
+
+    reg_write_bits(&display->dsi->VVSACR, display->config.v_sync << DSI_VVSACR_VSA_Pos, DSI_VVSACR_VSA_Msk);
+    reg_write_bits(&display->dsi->VVBPCR, display->config.v_back_porch << DSI_VVBPCR_VBP_Pos, DSI_VVBPCR_VBP_Msk);
+    reg_write_bits(&display->dsi->VVFPCR, display->config.v_front_porch << DSI_VVFPCR_VFP_Pos, DSI_VVFPCR_VFP_Msk);
+    reg_write_bits(&display->dsi->VVACR, display->config.active_height << DSI_VVACR_VA_Pos, DSI_VVACR_VA_Msk);
+  } else {
+    reg_set_bits(&display->dsi->MCR, DSI_MCR_CMDM);
+
+    reg_write_bits(&display->dsi->WCFGR, (1 << DSI_WCFGR_DSIM_Pos) |
+                   (0 << DSI_WCFGR_TESRC_Pos) |
+                   (0 << DSI_WCFGR_TEPOL_Pos) | (0 << DSI_WCFGR_AR_Pos) | (0 << DSI_WCFGR_VSPOL_Pos),
+                   DSI_WCFGR_DSIM_Msk | DSI_WCFGR_TESRC_Msk | DSI_WCFGR_TEPOL_Msk | DSI_WCFGR_AR_Msk | DSI_WCFGR_VSPOL_Msk);
+
+    reg_write_bits(&display->dsi->LCCR, display->config.active_width << DSI_LCCR_CMDSIZE_Pos, DSI_LCCR_CMDSIZE_Msk);
+    reg_set_bits(&display->dsi->CMCR, DSI_CMCR_TEARE);
+  }
+
+  // Configure DSI host LTDC
+  reg_write_bits(&display->dsi->LVCIDR,
+                 dsi_config.channel << DSI_LVCIDR_VCID_Pos,
+                 DSI_LVCIDR_VCID_Msk);
+
+  reg_write_bits(&display->dsi->LPCR,
+                 (0 << DSI_LPCR_DEP_Pos) |
+                 (0 << DSI_LPCR_VSP_Pos) |
+                 (0 << DSI_LPCR_HSP_Pos),
+                 DSI_LPCR_DEP_Msk | DSI_LPCR_VSP_Msk | DSI_LPCR_HSP_Msk);
+
+  reg_write_bits(&display->dsi->LCOLCR,
+                 (dsi_config.color_coding << DSI_LCOLCR_COLC_Pos) |
+                 (dsi_config.color_loosely_packed << DSI_LCOLCR_LPE_Pos) ,
+                 DSI_LCOLCR_LPE_Msk | DSI_LCOLCR_COLC_Msk);
+
+
+  reg_write_bits(&display->dsi->WCFGR,
+                 (dsi_config.wrapper_color_coding << DSI_WCFGR_COLMUX_Pos),
+                 DSI_WCFGR_COLMUX_Msk);
+
+  reg_write_bits(&display->dsi->LPMCR,
+                 (dsi_config.lp_size << DSI_LPMCR_LPSIZE_Pos) | (dsi_config.vlp_size << DSI_LPMCR_VLPSIZE_Pos),
+                 DSI_LPMCR_LPSIZE_Msk | DSI_LPMCR_VLPSIZE_Msk);
+
+  // TODO is this required?
+  // Send DCS commands to configure the display?
+
+  // Start the LTDC flow through DSI wrapper
+    // display_refresh does that
+}
+
+void display_start(display_t* display)
+{
+  // Enable DSI host
+  reg_set_bits(&display->dsi->CR, DSI_CR_EN);
+
+  // Enable DSI wrapper
+  reg_set_bits(&display->dsi->WCR, DSI_WCR_DSIEN);
+}
+
+void display_layer_setup(display_t *display, void* buffer, ltdc_layer_pixel_format_t pixel_format) {
+  uint16_t h_win_start = 0 +
+    reg_read_bits_pos(&display->ltdc->BPCR, LTDC_BPCR_AHBP_Pos, LTDC_BPCR_AHBP_Msk >> LTDC_BPCR_AHBP_Pos) + 1;
+  uint16_t h_win_stop = display->config.active_width +
+    reg_read_bits_pos(&display->ltdc->BPCR, LTDC_BPCR_AHBP_Pos, LTDC_BPCR_AHBP_Msk >> LTDC_BPCR_AHBP_Pos);
+
+  reg_write_bits(&display->layer->WHPCR, (h_win_start << LTDC_LxWHPCR_WHSTPOS_Pos) |
+                 (h_win_stop << LTDC_LxWHPCR_WHSPPOS_Pos)
+                 , LTDC_LxWHPCR_WHSPPOS_Msk | LTDC_LxWHPCR_WHSTPOS_Msk);
+
+  uint16_t v_win_start = 0 +
+    reg_read_bits_pos(&display->ltdc->BPCR, LTDC_BPCR_AVBP_Pos, LTDC_BPCR_AVBP_Msk >> LTDC_BPCR_AVBP_Pos) + 1;
+  uint16_t v_win_stop = display->config.active_height +
+    reg_read_bits_pos(&display->ltdc->BPCR, LTDC_BPCR_AVBP_Pos, LTDC_BPCR_AVBP_Msk >> LTDC_BPCR_AVBP_Pos);
+
+  reg_write_bits(&display->layer->WVPCR,
+                 (v_win_start << LTDC_LxWVPCR_WVSTPOS_Pos) |
+                 (v_win_stop << LTDC_LxWVPCR_WVSPPOS_Pos),
+                 LTDC_LxWVPCR_WVSPPOS_Msk | LTDC_LxWVPCR_WVSTPOS_Msk);
+
+  reg_write_bits(&display->layer->PFCR, pixel_format << LTDC_LxPFCR_PF_Pos, LTDC_LxPFCR_PF_Msk);
+
+  reg_clear_bits(&display->layer->DCCR,
+                 LTDC_LxDCCR_DCALPHA_Msk | LTDC_LxDCCR_DCBLUE_Msk | LTDC_LxDCCR_DCGREEN_Msk | LTDC_LxDCCR_DCRED_Msk);
+  reg_write_bits(&display->layer->CACR,
+                 0xFF << LTDC_LxCACR_CONSTA_Pos, LTDC_LxCACR_CONSTA_Msk);
+  reg_write_bits(
+                 &display->layer->BFCR,
+                 (0b110 << LTDC_LxBFCR_BF1_Pos) | (0b111 << LTDC_LxBFCR_BF2_Pos),
+                 LTDC_LxBFCR_BF2_Msk | LTDC_LxBFCR_BF1_Msk);
+
+  uint32_t width = display->config.active_width;
+  uint32_t height = display->config.active_height;
+
+  uint8_t bytes_per_pixel;
+  switch (pixel_format) {
+  case LTDC_ARGB8888:
+    bytes_per_pixel = 4;
+    break;
+  case LTDC_RGB888:
+    bytes_per_pixel = 3;
+    break;
+  case LTDC_ARGB1555:
+  case LTDC_ARGB4444:
+  case LTDC_RGB565:
+  case LTDC_AL88:
+    bytes_per_pixel = 2;
+    break;
+  default:
+    bytes_per_pixel = 1;
+    break;
+  }
+
+  reg_write_bits(&display->layer->CFBLR,
+                 (width * bytes_per_pixel) << LTDC_LxCFBLR_CFBP_Pos |
+                 (width * bytes_per_pixel + 7) << LTDC_LxCFBLR_CFBLL_Pos,
+                 LTDC_LxCFBLR_CFBP_Msk | LTDC_LxCFBLR_CFBLL_Msk);
+
+  reg_write_bits(&display->layer->CFBLNR, height << LTDC_LxCFBLNR_CFBLNBR_Pos, LTDC_LxCFBLNR_CFBLNBR_Msk);
+
+  display_set_framebuffer(display, buffer);
+
+  reg_set_bits(&display->layer->CR, LTDC_LxCR_LEN);
+
+  display_reload(display);
+}
+
+void display_refresh(display_t *display) {
+  reg_set_bits(&display->dsi->WCR, DSI_WCR_LTDCEN);
+}
+
+void display_reload(display_t *display) {
+  reg_set_bits(&display->ltdc->SRCR, LTDC_SRCR_IMR);
+}
+
+void display_set_framebuffer(display_t *display, void* buffer) {
+  reg_write_bits(&display->layer->CFBAR, ((uint32_t)buffer) << LTDC_LxCFBAR_CFBADD_Pos, LTDC_LxCFBAR_CFBADD_Msk);
+}
+
+void display_set_command_mode_transmission_kind(display_t *display,
+                                                bool low_power) {
+  reg_write_bits(&display->dsi->CMCR, low_power ? 0xFFFFFFFF : 0x00000000,
+                 DSI_CMCR_GSW0TX | DSI_CMCR_GSW1TX | DSI_CMCR_GSW2TX |
+                 DSI_CMCR_GSR0TX | DSI_CMCR_GSR1TX | DSI_CMCR_GSR2TX |
+                 DSI_CMCR_GLWTX | DSI_CMCR_DSW0TX | DSI_CMCR_DSW1TX |
+                 DSI_CMCR_DSR0TX | DSI_CMCR_DLWTX | DSI_CMCR_MRDPS);
+}
+
+void display_dsi_short_write(display_t *display, uint8_t lsb, uint8_t msb,
+                             uint8_t discriminant) {
+  while (!(display->dsi->GPSR & DSI_GPSR_CMDFE));
+
+  display->dsi->GHCR = (msb << DSI_GHCR_WCMSB_Pos) | (lsb << DSI_GHCR_WCLSB_Pos) |
+    (display->config.channel << DSI_GHCR_VCID_Pos) | (discriminant << DSI_GHCR_DT_Pos);
+}
+
+typedef union {
+  uint32_t word;
+  uint16_t shortword[2];
+  uint8_t chars[4];
+} dsi_long_data_t;
+
+void display_dsi_long_write(display_t *display, uint8_t cmd, uint8_t *data,
+                            uint16_t len, uint8_t discriminant) {
+  while (!(display->dsi->GPSR & DSI_GPSR_CMDFE));
+
+  dsi_long_data_t gpdr;
+  gpdr.word = 0;
+  gpdr.chars[0] = cmd;
+
+  for (uint8_t i = 0; i < 3 && i < len; i++) {
+    gpdr.chars[i + 1] = data[i];
+  }
+
+  display->dsi->GPDR = gpdr.word;
+
+  for (uint8_t i = 3; i < len; i += 4) {
+    for (uint8_t j = 0; j < 3 && i + j < len; j++) {
+      gpdr.chars[j] = data[i + j];
+    }
+    display->dsi->GPDR = gpdr.word;
+  }
+
+  len++;
+  display_dsi_short_write(display, (len & 0xFF), (len >> 8) & 0xFF, discriminant);
+}
+
+/* void display_dsi_read(display_t* display, uint8_t* buffer); */

@@ 3,6 3,7 @@
 #include <core_cm7.h>
 #include <stdlib.h>
 #include "delay.h"
+#include "display.h"
 #include "usb_device.h"
 #include "usb_device_cdc.h"
 #include "clocks.h"


@@ 10,6 11,7 @@
 #include "registers.h"
 #include "pin.h"
 #include "fmc.h"
+#include "otm8009a.h"
 
 #define LED1_GPIO GPIOI
 #define LED1_PIN 12


@@ 243,21 245,24 @@ usb_device_t* init_usb()
   return usb_dev;
 }
 
-void app_loop(usb_device_t* usb_dev);
+void init_button_led(void) {
+  // Pins init
+  pin_init(&led1, LED1_GPIO, LED1_PIN);
+  pin_into_output_pushpull(&led1);
+  pin_toggle(&led1);
+  pin_toggle(&led1);
 
-void main()
-{
-  clocks_wait_ready(CLOCK_HSI);
+  // pc13 input - wakeup button
+  pin_init(&wkup, GPIOC, 13);
+  pin_into_input(&wkup);
 
-  // Clock gating
-  RCC->APB4ENR |= RCC_APB4ENR_SYSCFGEN;
-  RCC->AHB4ENR |= RCC_AHB4ENR_GPIOAEN | RCC_AHB4ENR_GPIOBEN |
-                  RCC_AHB4ENR_GPIOCEN | RCC_AHB4ENR_GPIODEN |
-                  RCC_AHB4ENR_GPIOEEN | RCC_AHB4ENR_GPIOFEN |
-                  RCC_AHB4ENR_GPIOGEN | RCC_AHB4ENR_GPIOHEN |
-                  RCC_AHB4ENR_GPIOIEN | RCC_AHB4ENR_GPIOJEN;
+  exti_init(&exti_wkup, 13, EXTI, SYSCFG);
+  exti_external_interrupt(&exti_wkup, EXTI_GPIOC);
+  exti_rising_interrupt(&exti_wkup);
+  exti_enable_interrupt(&exti_wkup);
+}
 
-  // Clocks section
+void init_clock(void) {
   // Enable hsi48 for usb
   clocks_enable(CLOCK_HSI48);
   clocks_wait_ready(CLOCK_HSI48);


@@ 273,28 278,33 @@ void main()
   clocks_pll_enable(CLOCK_PLL2);
   clocks_pll_wait_ready(CLOCK_PLL2, 300);
 
+  // ltdc
+  clocks_pll_configure(CLOCK_PLL3, 0, 10, PLL_SOURCE_HSE, 96, 4, 4, 4);
+  clocks_pll_enable(CLOCK_PLL3);
+  clocks_pll_wait_ready(CLOCK_PLL3, 300);
+
   // 1. Set pwr configuration
-  reg_write_bits(&PWR->CR3,
-                 (1 << PWR_CR3_SMPSEN_Pos) | (0 << PWR_CR3_LDOEN_Msk) |
-                 (0 << PWR_CR3_BYPASS_Msk),
-                 PWR_CR3_LDOEN_Msk | PWR_CR3_SMPSEN_Msk | PWR_CR3_BYPASS_Msk);
+  /* reg_write_bits(&PWR->CR3, */
+  /*                (1 << PWR_CR3_SMPSEN_Pos) | (0 << PWR_CR3_LDOEN_Msk) | */
+  /*                (0 << PWR_CR3_BYPASS_Msk), */
+  /*                PWR_CR3_LDOEN_Msk | PWR_CR3_SMPSEN_Msk | PWR_CR3_BYPASS_Msk); */
 
   // 2. Verify pwr configuration. If not working, stay here looping!
   // NOTE: this probably means you need to power cycle the board. The
   // configuration can be written only when the chip has just been connected.
-  while (!(PWR->CR3 & PWR_CR3_SMPSEN) || (PWR->CR3 & PWR_CR3_LDOEN) || (PWR->CR3 & PWR_CR3_BYPASS));
+  /* while (!(PWR->CR3 & PWR_CR3_SMPSEN) || (PWR->CR3 & PWR_CR3_LDOEN) || (PWR->CR3 & PWR_CR3_BYPASS)); */
 
   // 3. Change VOS
   // 3.1. Check current configuration (VOS 3)
-  while (!(PWR->CSR1 & PWR_CSR1_ACTVOSRDY));
+  /* while (!(PWR->CSR1 & PWR_CSR1_ACTVOSRDY)); */
 
   // 3.2. VOS 1 transition
-  reg_write_bits(&PWR->D3CR, (11 << PWR_D3CR_VOS_Pos), PWR_D3CR_VOS_Msk);
-  while (!(PWR->CSR1 & PWR_D3CR_VOSRDY));
+  /* reg_write_bits(&PWR->D3CR, (11 << PWR_D3CR_VOS_Pos), PWR_D3CR_VOS_Msk); */
+  /* while (!(PWR->CSR1 & PWR_D3CR_VOSRDY)); */
 
   // 3.3. VOS 0 transition
-  reg_set_bits(&SYSCFG->PWRCR, SYSCFG_PWRCR_ODEN);
-  while (!(PWR->CSR1 & PWR_D3CR_VOSRDY));
+  /* reg_set_bits(&SYSCFG->PWRCR, SYSCFG_PWRCR_ODEN); */
+  /* while (!(PWR->CSR1 & PWR_D3CR_VOSRDY)); */
 
   // 25 MHz / 10 -> 2.5 MHz * 384 = 960 MHz (FVCO)
   // PLL1_P = 960 / 2 => 480 MHz


@@ 308,38 318,181 @@ void main()
   /* uint32_t rcc_hclk = d1cpre_freq_hz / 2; // 240 MHz */
 
   // 240 MHz means wait states = 4, progr delay = 2
-  reg_write_bits(&FLASH->ACR,
-                 (2 << FLASH_ACR_WRHIGHFREQ_Pos) | (4 << FLASH_ACR_LATENCY_Pos),
-                 FLASH_ACR_WRHIGHFREQ_Msk | FLASH_ACR_LATENCY_Msk);
-  while (((FLASH->ACR & FLASH_ACR_LATENCY_Msk) >> FLASH_ACR_LATENCY_Pos) != 4);
+  /* reg_write_bits(&FLASH->ACR, */
+  /*                (2 << FLASH_ACR_WRHIGHFREQ_Pos) | (4 << FLASH_ACR_LATENCY_Pos), */
+  /*                FLASH_ACR_WRHIGHFREQ_Msk | FLASH_ACR_LATENCY_Msk); */
+  /* while (((FLASH->ACR & FLASH_ACR_LATENCY_Msk) >> FLASH_ACR_LATENCY_Pos) != 4); */
 
-  // D1CPRE = sysck / 1 => 480 MHz, HPRE = sysck / 2 => 240 MHz
-  reg_write_bits(&RCC->D1CFGR,
-                 (0 << RCC_D1CFGR_D1CPRE_Pos) | (1 << RCC_D1CFGR_HPRE_Pos) |
-                 (0 << RCC_D1CFGR_D1CPRE_Pos),
-                 RCC_D1CFGR_D1CPRE_Msk | RCC_D1CFGR_HPRE_Msk | RCC_D1CFGR_D1CPRE_Msk);
+  /* // D1CPRE = sysck / 1 => 480 MHz, HPRE = sysck / 2 => 240 MHz */
+  /* reg_write_bits(&RCC->D1CFGR, */
+  /*                (0 << RCC_D1CFGR_D1CPRE_Pos) | (1 << RCC_D1CFGR_HPRE_Pos) | */
+  /*                (0 << RCC_D1CFGR_D1CPRE_Pos), */
+  /*                RCC_D1CFGR_D1CPRE_Msk | RCC_D1CFGR_HPRE_Msk | RCC_D1CFGR_D1CPRE_Msk); */
 
   // Sysck = PLL1_P
-  clocks_system_clock_source(CLOCK_SOURCE_PLL_1_P_CK, 1, 1, 1, 300);
+  // I don't know why, but I can't seem to get usb reliably working with this
+  // system clock, although it shouldn't really affect the usb as usb has other
+  // clock selected... ? Where do I have the speed wrong? Did I forget some kind
+  // of a wait somewhere?
+  /* clocks_system_clock_source(CLOCK_SOURCE_PLL_1_P_CK, 1, 1, 1, 300); */
+}
+
+typedef struct {
+  uint8_t r;
+  uint8_t g;
+  uint8_t b;
+} pixel_rgb888_t;
+
+pixel_rgb888_t* init_display(display_t* display, fmc_t* fmc)
+{
+  pin_t display_reset, display_backlight_en, te;
+
+  pin_init(&display_reset, GPIOG, 3);
+  pin_init(&display_backlight_en, GPIOJ, 12);
+  pin_init(&te, GPIOJ, 2);
+
+  pin_into_output_pushpull(&display_reset);
+  pin_reset(&display_reset);
+  pin_set(&display_reset);
+
+  pin_into_output_pushpull(&display_backlight_en);
+  pin_set(&display_backlight_en);
 
+  pin_into_input(&te);
+
+  uint16_t width = 800, height = 480;
+
+  // phy = 500 MHz
+  uint32_t pix_khz = 62500;
+  uint32_t ltdc_khz = 60000;
+
+  display_config_t display_config = {
+    .channel = 0,
+
+    .active_height = height,
+    .active_width = width,
+    .h_back_porch = 34,
+    .h_front_porch = 34,
+    .v_back_porch = 15,
+    .v_front_porch = 16,
+    .h_sync = 2,
+    .v_sync = 1,
+    .h_sync_pol = false,
+    .v_sync_pol = false,
+    .not_data_enable_pol = false,
+    .pixel_clock_pol = false,
+
+    .video_mode = DSI_VIDEO_BURST,
+  };
+
+  pixel_rgb888_t *framebuffer = (pixel_rgb888_t *)
+    fmc_sdram_allocate(fmc, SDRAM_BANK2, width * height * sizeof(pixel_rgb888_t));
+
+  dsi_pll_config_t pll = {
+    .divn = 100,
+    .idf = 5,
+    .odf = 0,
+    .eckdiv = 4,
+  };
+  dsi_phy_config_t phy = {
+    .auto_clock_lane_control = false,
+    .clock_control = true,
+    .data_lanes = 1, // double
+
+    .data_hs2lp = 35,
+    .data_lp2hs = 35,
+    .clock_hs2lp = 35,
+    .clock_lp2hs = 35,
+
+    .max_read_time = 0,
+    .stop_wait_time = 10,
+    .unit_interval = 8, // 4 000 000 000 / phy hz
+  };
+  dsi_flow_config_t flow = {
+    .crc_reception_enable = false,
+    .ecc_reception_enable = false,
+    .bta_enable = true,
+    .eotp_reception_enable = false,
+    .eotp_transmission_enable = false,
+  };
+  dsi_config_t dsi_config = {
+    .channel = 0,
+    .color_loosely_packed = false,
+    .color_coding = CODING_24_BIT,
+    .wrapper_color_coding = CODING_24_BIT,
+    .lp_size = 4,
+    .vlp_size = 4,
+  };
+
+  display_init(display, DSI, LTDC, LTDC_Layer1, display_config);
+
+  display_setup(display, pix_khz, ltdc_khz, DSI_MODE_ADAPTED_COMMAND, pll, phy, flow, dsi_config);
+  display_layer_setup(display, framebuffer, LTDC_RGB888);
+  display_start(display);
+
+  // Low power
+  display_set_command_mode_transmission_kind(display, true);
+
+  // Initialize display
+  otm8009_init(display,
+               OTM8009A_MODE_LANDSCAPE, OTM8009A_70HZ,
+               OTM8009A_COLORMAP_RGB,
+               width, height);
+
+  // High power
+  display_set_command_mode_transmission_kind(display, false);
+
+  return framebuffer;
+}
+
+void app_loop(usb_device_t* usb_dev);
+
+void main()
+{
   fmc_t fmc;
+  display_t display;
+
+  clocks_wait_ready(CLOCK_HSI);
+
+  // Clock gating
+  RCC->APB4ENR |= RCC_APB4ENR_SYSCFGEN;
+
+  // Clocks section
+  init_clock();
+
+  // Clock gating after clock initialization
+  RCC->APB3ENR |= RCC_APB3ENR_DSIEN | RCC_APB3ENR_LTDCEN;
+  RCC->AHB4ENR |= RCC_AHB4ENR_GPIOAEN | RCC_AHB4ENR_GPIOBEN |
+                  RCC_AHB4ENR_GPIOCEN | RCC_AHB4ENR_GPIODEN |
+                  RCC_AHB4ENR_GPIOEEN | RCC_AHB4ENR_GPIOFEN |
+                  RCC_AHB4ENR_GPIOGEN | RCC_AHB4ENR_GPIOHEN |
+                  RCC_AHB4ENR_GPIOIEN | RCC_AHB4ENR_GPIOJEN;
+
   init_fmc(&fmc);
+  init_button_led();
+  pixel_rgb888_t* framebuffer = init_display(&display, &fmc);
 
-  // Pins init
-  pin_init(&led1, LED1_GPIO, LED1_PIN);
-  pin_into_output_pushpull(&led1);
-  pin_toggle(&led1);
-  pin_toggle(&led1);
+  for (uint32_t i = 0; i < 800 * 480; i++) {
+    framebuffer[i].r = 0xFF;
+    framebuffer[i].g = 0x00;
+    framebuffer[i].b = 0x00;
+  }
 
-  // pc13 input - wakeup button
-  pin_init(&wkup, GPIOC, 13);
-  pin_into_input(&wkup);
+  uint32_t loop = 0;
+  while (true) {
+    /* display_set_framebuffer(&display, framebuffer); */
+    for (uint8_t x = 0; x < 10; x++) {
+      framebuffer[loop*10 + x].g = 0xFF;
+    }
 
-  exti_init(&exti_wkup, 13, EXTI, SYSCFG);
-  exti_external_interrupt(&exti_wkup, EXTI_GPIOC);
-  exti_rising_interrupt(&exti_wkup);
-  exti_enable_interrupt(&exti_wkup);
+    display_refresh(&display);
+    /* display_reload(&display); */
+    delay_ms(500);
+
+    loop++;
+  }
 
+  // App starts, enable interrupts
   __enable_irq();
 
   usb_device_t* usb_dev = init_usb();

@@ 0,0 1,332 @@
+#include "otm8009a.h"
+#include "delay.h"
+
+void otm8009_init(display_t *display, otm8009a_mode_t mode,
+                  otm8009a_frame_rates_t frame_rate,
+                  otm8009a_colormap_t colormap,
+                  uint16_t width, uint16_t height) {
+  display_dsi_short_write(display, OTM8009A_CMD_SWRESET, 0x00, 0x15);
+  delay_ms(100);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x00, 0x15);
+
+  {
+    uint8_t data[] = { 0x80, 0x09, 0x01 };
+    display_dsi_long_write(display, 0xFF, data, 3, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x80, 0x15);
+
+  {
+    uint8_t data[] = { 0x80, 0x09 };
+    display_dsi_long_write(display, 0xFF, data, 2, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x80, 0x15);
+  display_dsi_short_write(display, 0xC4, 0x30, 0x15);
+
+  delay_ms(10);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x8A, 0x15);
+  display_dsi_short_write(display, 0xC4, 0x40, 0x15);
+
+  delay_ms(10);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xB1, 0x15);
+  display_dsi_short_write(display, 0xC5, 0xA9, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x91, 0x15);
+  display_dsi_short_write(display, 0xC5, 0x34, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xB4, 0x15);
+  display_dsi_short_write(display, 0xC0, 0x50, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x00, 0x15);
+  display_dsi_short_write(display, 0xD9, 0x4E, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x81, 0x15);
+  display_dsi_short_write(display, 0xC1, frame_rate | (frame_rate << 4), 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xA1, 0x15);
+  display_dsi_short_write(display, 0xC1, 0x08, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x92, 0x15);
+  display_dsi_short_write(display, 0xC5, 0x01, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x95, 0x15);
+  display_dsi_short_write(display, 0xC5, 0x34, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x00, 0x15);
+
+  {
+    uint8_t data[] = { 0x79, 0x79 };
+    display_dsi_long_write(display, 0xD8, data, 2, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x94, 0x15);
+  display_dsi_short_write(display, 0xC5, 0x33, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xA3, 0x15);
+  display_dsi_short_write(display, 0xC0, 0x1B, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x82, 0x15);
+  display_dsi_short_write(display, 0xC5, 0x83, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x81, 0x15);
+  display_dsi_short_write(display, 0xC4, 0x83, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xA1, 0x15);
+  display_dsi_short_write(display, 0xC1, 0x0E, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xA6, 0x15);
+
+  {
+    uint8_t data[] = { 0x00, 0x01 };
+    display_dsi_long_write(display, 0xB3, data, 2, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x80, 0x15);
+
+  {
+    uint8_t data[] = { 0x85, 0x01, 0x00, 0x84, 0x01, 0x00 };
+    display_dsi_long_write(display, 0xCE, data, 6, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x80, 0x15);
+  {
+    uint8_t data[] = { 0x18, 0x04, 0x03, 0x39, 0x00, 0x00, 0x00, 0x18, 0x03, 0x03, 0x3A, 0x00, 0x00, 0x00 };
+    display_dsi_long_write(display, 0xCE, data, 14, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xB0, 0x15);
+  {
+    uint8_t data[] = { 0x18, 0x04, 0x03, 0x39, 0x00, 0x00, 0x00, 0x18, 0x03, 0x03, 0x3A, 0x00, 0x00, 0x00 };
+    display_dsi_long_write(display, 0xCE, data, 14, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xC0, 0x15);
+  {
+    uint8_t data[] = { 0x01, 0x01, 0x20, 0x20, 0x00, 0x00, 0x01, 0x02, 0x00, 0x00 };
+    display_dsi_long_write(display, 0xCF, data, 10, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x80, 0x15);
+
+  {
+    uint8_t data[] = { 0, 10 };
+    display_dsi_long_write(display, 0xCB, data, 2, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x90, 0x15);
+  {
+    uint8_t data[] = { 0, 15 };
+    display_dsi_long_write(display, 0xCB, data, 2, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xa0, 0x15);
+  {
+    uint8_t data[] = { 0, 15 };
+    display_dsi_long_write(display, 0xCB, data, 2, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xb0, 0x15);
+  {
+    uint8_t data[] = { 0, 10 };
+    display_dsi_long_write(display, 0xCB, data, 2, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xc0, 0x15);
+  {
+    uint8_t data[] = {
+          0x00, 0x04, 0x04, 0x04, 0x04, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+          0x00,
+    };
+    display_dsi_long_write(display, 0xCB, data, 15, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xd0, 0x15);
+  {
+    uint8_t data[] = {
+          0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x04, 0x04, 0x04, 0x04, 0x00, 0x00, 0x00,
+          0x00,
+    };
+    display_dsi_long_write(display, 0xCB, data, 15, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xe0, 0x15);
+  {
+    uint8_t data[] = { 0, 10 };
+    display_dsi_long_write(display, 0xCB, data, 2, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xf0, 0x15);
+  {
+    uint8_t data[] = { 255, 10 };
+    display_dsi_long_write(display, 0xCB, data, 2, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x80, 0x15);
+  {
+    uint8_t data[] = { 0x00, 0x26, 0x09, 0x0B, 0x01, 0x25, 0x00, 0x00, 0x00, 0x00 };
+    display_dsi_long_write(display, 0xCC, data, 10, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x90, 0x15);
+  {
+    uint8_t data[] = {
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x26, 0x0A, 0x0C,
+        0x02,
+    };
+    display_dsi_long_write(display, 0xCC, data, 15, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xa0, 0x15);
+  {
+    uint8_t data[] = {
+        0x25, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00,
+    };
+    display_dsi_long_write(display, 0xCC, data, 15, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xb0, 0x15);
+  {
+    uint8_t data[] = {
+      0x00, 0x25, 0x0C, 0x0A, 0x02, 0x26, 0x00, 0x00, 0x00, 0x00
+    };
+    display_dsi_long_write(display, 0xCC, data, 10, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xc0, 0x15);
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xb0, 0x15);
+  {
+    uint8_t data[] = {
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x25, 0x0B, 0x09,
+        0x01,
+    };
+    display_dsi_long_write(display, 0xCC, data, 15, 0x39);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xd0, 0x15);
+  {
+    uint8_t data[] = {
+      0x26, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+      0x00,
+    };
+    display_dsi_long_write(display, 0xCC, data, 15, 0x39);
+  }
+
+  // PWR_CTRL1 - 0xc580h - 130th parameter - default
+  // Pump 1 min and max DM
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x81, 0x15);
+  display_dsi_short_write(display, 0xc5, 0x66, 0x15);
+
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xb6, 0x15);
+  display_dsi_short_write(display, 0xf5, 0x06, 0x15);
+
+  // CABC LEDPWM frequency adjusted to 19,5kHz
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0xb1, 0x15);
+  display_dsi_short_write(display, 0xc6, 0x06, 0x15);
+
+  // Exit CMD2 mode
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0, 0x15);
+  {
+    uint8_t data[] = {
+      0xFF, 0xFF, 0xFF
+    };
+    display_dsi_long_write(display, 0xFF, data, 3, 0x39);
+  }
+
+  // Standard DCS Initialization TO KEEP CAN BE DONE IN HSDT
+  // NOP - goes back to DCS std command ?
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0, 0x15);
+
+  // Gamma correction 2.2+ table (HSDT possible)
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0, 0x15);
+  {
+    uint8_t data[] = {
+      0x00, 0x09, 0x0F, 0x0E, 0x07, 0x10, 0x0B, 0x0A, 0x04, 0x07, 0x0B, 0x08, 0x0F, 0x10,
+      0x0A, 0x01,
+    };
+    display_dsi_long_write(display, 0xE1, data, 16, 0x39);
+  }
+
+  // Gamma correction 2.2- table (HSDT possible)
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0x00, 0x15);
+  {
+    uint8_t data[] = {
+      0x00, 0x09, 0x0F, 0x0E, 0x07, 0x10, 0x0B, 0x0A, 0x04, 0x07, 0x0B, 0x08, 0x0F, 0x10,
+      0x0A, 0x01,
+    };
+    display_dsi_long_write(display, 0xE2, data, 16, 0x39);
+  }
+
+  // Send Sleep Out command to display : no parameter
+  display_dsi_short_write(display, OTM8009A_CMD_SLPOUT, 0x00, 0x15);
+
+  // Wait for sleep out exit
+  delay_ms(120);
+
+  display_dsi_short_write(display, OTM8009A_CMD_COLMOD, OTM8009A_COLMOD_RGB888, 0x15);
+
+  // Send command to configure display in landscape orientation mode. By default
+  // the orientation mode is portrait
+  // CASET value (Column Address Set) : X direction LCD GRAM boundaries
+  // depending on LCD orientation mode and PASET value (Page Address Set) : Y direction
+  // LCD GRAM boundaries depending on LCD orientation mode
+  // XS[15:0] = 0x000 = 0, XE[15:0] = 0x31F = 799 for landscape mode : apply to CASET
+  // YS[15:0] = 0x000 = 0, YE[15:0] = 0x31F = 799 for portrait mode : apply to PASET
+  //static const uint8_t LcdRegData27[] = {0x00, 0x00, 0x03, 0x1F};
+  //
+  // XS[15:0] = 0x000 = 0, XE[15:0] = 0x1DF = 479 for portrait mode : apply to CASET
+  // YS[15:0] = 0x000 = 0, YE[15:0] = 0x1DF = 479 for landscape mode : apply to PASET
+  //static const uint8_t lcdregdata28[] = {0x00, 0x00, 0x01, 0xdf};
+  uint16_t madctr;
+  if (mode == OTM8009A_MODE_PORTRAIT) {
+    madctr = 0;
+  } else {
+    madctr = OTM8009A_MADCTR_MODE_LANDSCAPE;
+  }
+
+  if (colormap == OTM8009A_COLORMAP_BGR) {
+    madctr |= (1 << 3);
+  }
+
+  display_dsi_short_write(display, OTM8009A_CMD_MADCTR, madctr, 0x15);
+  uint16_t last_col = (width - 1);
+  uint16_t last_row = (height - 1);
+  uint8_t caset[] = {0, 0, last_col >> 8, last_col & 0xFF };
+  uint8_t paset[] = {0, 0, last_row >> 8, last_row & 0xFF};
+
+  display_dsi_long_write(display, OTM8009A_CMD_CASET, caset, 4, 0x39);
+  display_dsi_long_write(display, OTM8009A_CMD_PASET, paset, 4, 0x39);
+
+  //* CABC : Content Adaptive Backlight Control section start
+  // Note : defaut is 0 (lowest Brightness], 0xFF is highest Brightness, try 0x7F : intermediate value
+  display_dsi_short_write(display, OTM8009A_CMD_WRDISBV, 0x7f, 0x15);
+  // defaut is 0, try 0x2C - Brightness Control Block, Display Dimming & BackLight on
+  display_dsi_short_write(display, OTM8009A_CMD_WRCTRLD, 0x2c, 0x15);
+
+  //   /* defaut is 0, try 0x02 - image Content based Adaptive Brightness [Still Picture] */
+  display_dsi_short_write(display, OTM8009A_CMD_WRCABC, 0x02, 0x15);
+
+  //   /* defaut is 0 (lowest Brightness], 0xFF is highest Brightness */
+  display_dsi_short_write(display, OTM8009A_CMD_WRCABCMB, 0xff, 0x15);
+
+  //* CABC : Content Adaptive Backlight Control section end <<
+  // Send Command Display On
+  display_dsi_short_write(display, OTM8009A_CMD_DISPON, 0, 0x15);
+
+  // NOP command
+  display_dsi_short_write(display, OTM8009A_CMD_NOP, 0, 0x15);
+
+  // Send Command GRAM memory write (no parameters) : this initiates frame write via other DSI commands sent by
+  // DSI host from LTDC incoming pixels in video mode
+  display_dsi_short_write(display, OTM8009A_CMD_RAMWR, 0, 0x15);
+
+  {
+    uint8_t data[] = { (533 >> 8) & 0xFF, 533 & 0xFF };
+    display_dsi_long_write(display, OTM8009A_CMD_WRTESCN, data, 2, 0x39);
+  }
+  display_dsi_short_write(display, OTM8009A_CMD_TEEON, OTM8009A_TEEON_TELOM_VBLANKING_INFO_ONLY, 0x15);
+}

@@ 69,6 69,9 @@ void usb_device_wait_for_handshake(void* device_ptr) {
 }
 
 void usb_device_setup(void* device_ptr) {
+  // hsi48 as source
+  reg_write_bits(&RCC->D2CCIP2R, 0b11 << RCC_D2CCIP2R_USBSEL_Pos, RCC_D2CCIP2R_USBSEL_Msk);
+
   RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN | RCC_AHB1ENR_USB1OTGHSULPIEN;
 
   NVIC_SetPriority(OTG_HS_IRQn, 2);