#include <stdint.h>
#include <stm32h747xx.h>
#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"
#include "exti.h"
#include "registers.h"
#include "pin.h"
#include "fmc.h"
#include "otm8009a.h"

#define LED1_GPIO GPIOI
#define LED1_PIN 12
/* #define LED2_PIN 13 */

pin_t wkup;
pin_t led1;
exti_t exti_wkup;

uint8_t* global_framebuffer;


void init_fmc(fmc_t *fmc)
{
  // select pll 2 r as fmc clock
  reg_write_bits(&RCC->D1CCIPR, 2 << (RCC_D1CCIPR_FMCSEL_Pos), RCC_D1CCIPR_FMCSEL_Msk);

  RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
  fmc_init(fmc);

  #define BURST_LENGTH_1 0x0000
  #define BURST_TYPE_SEQUENTIAL 0x0000
  #define CAS_LATENCY_3 0x0030
  #define OPERATING_MODE_STANDARD 0x0000
  #define WRITEBURST_MODE_SINGLE 0x0200
  uint16_t sdram_mode_register = BURST_LENGTH_1 | BURST_TYPE_SEQUENTIAL |
                                 CAS_LATENCY_3 | OPERATING_MODE_STANDARD |
                                 WRITEBURST_MODE_SINGLE;

  fmc_sdram_timing_t sdram_timing = {
    .startup_delay_us =  100,        // 100 µs
    .max_sd_clock_hz =  100000000,   // 100 MHz
    .refresh_period_ns =  15625,     // 64ms / (4096 rows) = 15625ns
    .mode_register_to_active =  2,   // tMRD = 2 cycles
    .exit_self_refresh =  7,         // tXSR = 70ns
    .active_to_precharge =  4,       // tRAS = 42ns
    .row_cycle =  7,                 // tRC = 70ns
    .row_precharge =  2,             // tRP = 18ns
    .row_to_column =  2,             // tRCD = 18ns
  };
  fmc_sdram_configuration_t sdram_config = {
    .column_bits =  9,
    .row_bits =  12,
    .memory_data_width =  32, // 32-bit
    .internal_banks =  4,     // 4 internal banks
    .cas_latency =  3,        // CAS latency = 3
    .write_protection =  false,
    .read_burst =  true,
    .read_pipe_delay_cycles =  0,
  };

  {
    pin_t pin_addr[12];
    pin_t pin_data[32];
    pin_t pin_ba[2];
    pin_t pin_nbl[4];
    pin_t pin_sdcke1;
    pin_t pin_sdclk;
    pin_t pin_sdncas;
    pin_t pin_sdne1;
    pin_t pin_sdras;
    pin_t pin_sdnwe;

    // A0-A11
    pin_init(&pin_addr[0], GPIOF, 0);
    pin_init(&pin_addr[1], GPIOF, 1);
    pin_init(&pin_addr[2], GPIOF, 2);
    pin_init(&pin_addr[3], GPIOF, 3);
    pin_init(&pin_addr[4], GPIOF, 4);
    pin_init(&pin_addr[5], GPIOF, 5);
    pin_init(&pin_addr[6], GPIOF, 12);
    pin_init(&pin_addr[7], GPIOF, 13);
    pin_init(&pin_addr[8], GPIOF, 14);
    pin_init(&pin_addr[9], GPIOF, 15);
    pin_init(&pin_addr[10], GPIOG, 0);
    pin_init(&pin_addr[11], GPIOG, 1);
    // BA0-BA1
    pin_init(&pin_ba[0], GPIOG, 4);
    pin_init(&pin_ba[1], GPIOG, 5);
    // D0-D31
    pin_init(&pin_data[0], GPIOD, 14);
    pin_init(&pin_data[1], GPIOD, 15);
    pin_init(&pin_data[2], GPIOD, 0);
    pin_init(&pin_data[3], GPIOD, 1);
    pin_init(&pin_data[4], GPIOE, 7);
    pin_init(&pin_data[5], GPIOE, 8);
    pin_init(&pin_data[6], GPIOE, 9);
    pin_init(&pin_data[7], GPIOE, 10);
    pin_init(&pin_data[8], GPIOE, 11);
    pin_init(&pin_data[9], GPIOE, 12);
    pin_init(&pin_data[10], GPIOE, 13);
    pin_init(&pin_data[11], GPIOE, 14);
    pin_init(&pin_data[12], GPIOE, 15);
    pin_init(&pin_data[13], GPIOD, 8);
    pin_init(&pin_data[14], GPIOD, 9);
    pin_init(&pin_data[15], GPIOD, 10);
    pin_init(&pin_data[16], GPIOH, 8);
    pin_init(&pin_data[17], GPIOH, 9);
    pin_init(&pin_data[18], GPIOH, 10);
    pin_init(&pin_data[19], GPIOH, 11);
    pin_init(&pin_data[20], GPIOH, 12);
    pin_init(&pin_data[21], GPIOH, 13);
    pin_init(&pin_data[22], GPIOH, 14);
    pin_init(&pin_data[23], GPIOH, 15);
    pin_init(&pin_data[24], GPIOI, 0);
    pin_init(&pin_data[25], GPIOI, 1);
    pin_init(&pin_data[26], GPIOI, 2);
    pin_init(&pin_data[27], GPIOI, 3);
    pin_init(&pin_data[28], GPIOI, 6);
    pin_init(&pin_data[29], GPIOI, 7);
    pin_init(&pin_data[30], GPIOI, 9);
    pin_init(&pin_data[31], GPIOI, 10);

    pin_init(&pin_nbl[0], GPIOE, 0);
    pin_init(&pin_nbl[1], GPIOE, 1);
    pin_init(&pin_nbl[2], GPIOI, 4);
    pin_init(&pin_nbl[3], GPIOI, 5);

    pin_init(&pin_sdcke1, GPIOH, 7);
    pin_init(&pin_sdclk, GPIOG, 8);
    pin_init(&pin_sdncas, GPIOG, 15);
    pin_init(&pin_sdne1, GPIOH, 6);
    pin_init(&pin_sdras, GPIOF, 11);
    pin_init(&pin_sdnwe, GPIOH, 5);

    // A0-A11
    pin_into_alternate_highspeed(&pin_addr[0], 12);
    pin_into_alternate_highspeed(&pin_addr[1], 12);
    pin_into_alternate_highspeed(&pin_addr[2], 12);
    pin_into_alternate_highspeed(&pin_addr[3], 12);
    pin_into_alternate_highspeed(&pin_addr[4], 12);
    pin_into_alternate_highspeed(&pin_addr[5], 12);
    pin_into_alternate_highspeed(&pin_addr[6], 12);
    pin_into_alternate_highspeed(&pin_addr[7], 12);
    pin_into_alternate_highspeed(&pin_addr[8], 12);
    pin_into_alternate_highspeed(&pin_addr[9], 12);
    pin_into_alternate_highspeed(&pin_addr[10], 12);
    pin_into_alternate_highspeed(&pin_addr[11], 12);
    // BA0-BA1
    pin_into_alternate_highspeed(&pin_ba[0], 12);
    pin_into_alternate_highspeed(&pin_ba[1], 12);
    // D0-D31
    pin_into_alternate_highspeed(&pin_data[0], 12);
    pin_into_alternate_highspeed(&pin_data[1], 12);
    pin_into_alternate_highspeed(&pin_data[2], 12);
    pin_into_alternate_highspeed(&pin_data[3], 12);
    pin_into_alternate_highspeed(&pin_data[4], 12);
    pin_into_alternate_highspeed(&pin_data[5], 12);
    pin_into_alternate_highspeed(&pin_data[6], 12);
    pin_into_alternate_highspeed(&pin_data[7], 12);
    pin_into_alternate_highspeed(&pin_data[8], 12);
    pin_into_alternate_highspeed(&pin_data[9], 12);
    pin_into_alternate_highspeed(&pin_data[10], 12);
    pin_into_alternate_highspeed(&pin_data[11], 12);
    pin_into_alternate_highspeed(&pin_data[12], 12);
    pin_into_alternate_highspeed(&pin_data[13], 12);
    pin_into_alternate_highspeed(&pin_data[14], 12);
    pin_into_alternate_highspeed(&pin_data[15], 12);
    pin_into_alternate_highspeed(&pin_data[16], 12);
    pin_into_alternate_highspeed(&pin_data[17], 12);
    pin_into_alternate_highspeed(&pin_data[18], 12);
    pin_into_alternate_highspeed(&pin_data[19], 12);
    pin_into_alternate_highspeed(&pin_data[20], 12);
    pin_into_alternate_highspeed(&pin_data[21], 12);
    pin_into_alternate_highspeed(&pin_data[22], 12);
    pin_into_alternate_highspeed(&pin_data[23], 12);
    pin_into_alternate_highspeed(&pin_data[24], 12);
    pin_into_alternate_highspeed(&pin_data[25], 12);
    pin_into_alternate_highspeed(&pin_data[26], 12);
    pin_into_alternate_highspeed(&pin_data[27], 12);
    pin_into_alternate_highspeed(&pin_data[28], 12);
    pin_into_alternate_highspeed(&pin_data[29], 12);
    pin_into_alternate_highspeed(&pin_data[30], 12);
    pin_into_alternate_highspeed(&pin_data[31], 12);

    pin_into_alternate_highspeed(&pin_nbl[0], 12);
    pin_into_alternate_highspeed(&pin_nbl[1], 12);
    pin_into_alternate_highspeed(&pin_nbl[2], 12);
    pin_into_alternate_highspeed(&pin_nbl[3], 12);

    pin_into_alternate_highspeed(&pin_sdcke1, 12);
    pin_into_alternate_highspeed(&pin_sdclk, 12);
    pin_into_alternate_highspeed(&pin_sdncas, 12);
    pin_into_alternate_highspeed(&pin_sdne1, 12);
    pin_into_alternate_highspeed(&pin_sdras, 12);
    pin_into_alternate_highspeed(&pin_sdnwe, 12);

    fmc_sdram_configure(fmc, SDRAM_BANK2, sdram_config, sdram_timing, sdram_mode_register);
  }
}

usb_device_t* init_usb()
{
  pin_t ulpi_clk, ulpi_stp, ulpi_dir, ulpi_nxt,
    ulpi_d0, ulpi_d1, ulpi_d2, ulpi_d3, ulpi_d4,
    ulpi_d5, ulpi_d6, ulpi_d7, otg_hs_overcurrent;
  pin_init(&ulpi_clk, GPIOA, 5);
  pin_init(&ulpi_stp, GPIOC, 0);
  pin_init(&ulpi_dir, GPIOI, 11);
  pin_init(&ulpi_nxt, GPIOH, 4);

  pin_init(&ulpi_d0, GPIOA, 3);
  pin_init(&ulpi_d1, GPIOB, 0);
  pin_init(&ulpi_d2, GPIOB, 1);
  pin_init(&ulpi_d3, GPIOB, 10);
  pin_init(&ulpi_d4, GPIOB, 11);
  pin_init(&ulpi_d5, GPIOB, 12);
  pin_init(&ulpi_d6, GPIOB, 13);
  pin_init(&ulpi_d7, GPIOB, 5);
  pin_init(&otg_hs_overcurrent, GPIOJ, 1);

  pin_into_alternate_highspeed(&ulpi_clk, 10);
  pin_into_alternate_highspeed(&ulpi_stp, 10);
  pin_into_alternate_highspeed(&ulpi_dir, 10);
  pin_into_alternate_highspeed(&ulpi_nxt, 10);
  pin_into_alternate_highspeed(&ulpi_d0, 10);
  pin_into_alternate_highspeed(&ulpi_d1, 10);
  pin_into_alternate_highspeed(&ulpi_d2, 10);
  pin_into_alternate_highspeed(&ulpi_d3, 10);
  pin_into_alternate_highspeed(&ulpi_d4, 10);
  pin_into_alternate_highspeed(&ulpi_d5, 10);
  pin_into_alternate_highspeed(&ulpi_d6, 10);
  pin_into_alternate_highspeed(&ulpi_d7, 10);

  pin_into_input_highspeed(&otg_hs_overcurrent);

  void *usb_dev = usb_device_init(USB_OTG_HS1, &USB_CLASS_CDC_ACM,
                                  0x1234, 0x1111, u"Frantisek Bohacek",
                                  u"Display", 1, NULL);
  cdc_acm_configure(usb_dev, 512 * 8); // 8 full packets maximum size
  usb_device_setup(usb_dev);

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

  // pc13 input - wakeup button
  pin_init(&wkup, GPIOC, 13);
  pin_into_input(&wkup);

  exti_init(&exti_wkup, 13, EXTI, SYSCFG);
  exti_external_interrupt(&exti_wkup, EXTI_GPIOC);
  exti_rising_interrupt(&exti_wkup);
  exti_enable_interrupt(&exti_wkup);
}

void init_clock(void) {
  // Enable hsi48 for usb
  clocks_enable(CLOCK_HSI48);
  clocks_wait_ready(CLOCK_HSI48);

  // Enable HSE for PLLs
  clocks_enable(CLOCK_HSE);
  clocks_wait_ready(CLOCK_HSE);

  // HSE 25 MHz -> divide by 25 -> 1 MHz -> need VCOSEL 1
  // HSE / 25 * 400 = FVCO = 400 MHz
  // PLL2 R = 200 MHz
  clocks_pll_configure(CLOCK_PLL2, 1, 25, PLL_SOURCE_HSE, 400, 2, 2, 2);
  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); */

  // 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)); */

  // 3. Change VOS
  // 3.1. Check current configuration (VOS 3)
  /* 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)); */

  // 3.3. VOS 0 transition
  /* 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
  clocks_pll_configure(CLOCK_PLL1, 0, 10, PLL_SOURCE_HSE, 384, 2, 0, 0);
  clocks_pll_enable(CLOCK_PLL1);
  clocks_pll_wait_ready(CLOCK_PLL1, 300);

  // Flash setup
  /* uint32_t sysck_freq_hz = 480000000; // 480 MHz */
  /* uint32_t d1cpre_freq_hz = sysck_freq_hz / 1; // 480 Mhz */
  /* 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); */

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

#define DISPLAY_WIDTH 800UL
#define DISPLAY_HEIGHT 480UL

typedef struct {
  uint8_t b;
  uint8_t g;
  uint8_t r;
} 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 = DISPLAY_WIDTH, height = DISPLAY_HEIGHT;

  // 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, display_t* display, pixel_rgb888_t* framebuffer);

uint16_t cdc_receive_callback(usb_device_t* device, uint8_t* data, uint16_t count);
uint8_t* cdc_receive_location_callback(usb_device_t* device, uint16_t count);

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

  for (uint32_t i = 0; i < DISPLAY_HEIGHT * DISPLAY_WIDTH; i++) {
    framebuffer[i].r = 0;
    framebuffer[i].g = 0;
    framebuffer[i].b = 0;
  }

  // App starts, enable interrupts
  __enable_irq();

  usb_device_t* usb_dev = init_usb();
  usb_device_wait_for_handshake(usb_dev);

  app_loop(usb_dev, &display, framebuffer);
}

bool image_command_handling = false;
bool image_command_handled = false;
uint8_t* framebuffer_ptr;
uint32_t read_count;
#define TOTAL_PIXEL_BYTES DISPLAY_WIDTH * DISPLAY_HEIGHT * 3

uint16_t cdc_receive_callback(usb_device_t *device, uint8_t *data,
                              uint16_t count) {

  if (!image_command_handling && data[0] == 'i') {
    framebuffer_ptr = global_framebuffer;
    read_count = 0;
    image_command_handling = true;

    for (uint16_t i = 0; i < count - 1; i++) {
      framebuffer_ptr[i] = data[i + 1];
    }
    framebuffer_ptr += count - 1;
    read_count += count - 1;
    return count;
  }

  return 0;
}

uint8_t* cdc_receive_location_callback(usb_device_t *device, uint16_t count) {
  if (!image_command_handling) {
    return NULL;
  }

  uint8_t* ptr = framebuffer_ptr;
  framebuffer_ptr += count;
  read_count += count;

  if (read_count >= TOTAL_PIXEL_BYTES) {
    image_command_handling = false;
    image_command_handled = true;
  }

  return framebuffer_ptr;
}

void app_loop(usb_device_t* usb_dev, display_t* display, pixel_rgb888_t* framebuffer)
{
  global_framebuffer = (uint8_t*)framebuffer;

  cdc_data_set_receive_callback(usb_dev, cdc_receive_callback);
  cdc_data_set_receive_location_callback(usb_dev, cdc_receive_location_callback);

  uint8_t data[16];

  bool handling_cmd = false;
  uint8_t cmd;
  uint32_t pos = 0;

  display_refresh(display);

  while (1) {
    uint16_t received = cdc_data_receive(usb_dev, data, 1);

    if (image_command_handled) {
      display_refresh(display);
      image_command_handled = false;
    }

    if (received == 0) {
      continue;
    }

    if (!handling_cmd) {
      cmd = data[0];
      pos = 0;
    }

    switch (cmd) {
    case 'p':
      cdc_data_send_blocking(usb_dev, (uint8_t*)"Pong!\r\n", 0);
      break;
    case 'l':
      pin_toggle(&led1);
      break;
    case 'e':
      handling_cmd = true;

      if (data[0] == 'E') {
        handling_cmd = false;
      }

      cdc_data_send_blocking(usb_dev, data, received);
      break;
    case 'r':
      for (uint32_t i = 0; i < DISPLAY_HEIGHT * DISPLAY_WIDTH; i++) {
        framebuffer[i].r = 0xFF;
        framebuffer[i].g = 0;
        framebuffer[i].b = 0;
      }
      display_refresh(display);
      break;
    case 'g':
      for (uint32_t i = 0; i < DISPLAY_HEIGHT * DISPLAY_WIDTH; i++) {
        framebuffer[i].r = 0;
        framebuffer[i].g = 0xFF;
        framebuffer[i].b = 0;
      }
      display_refresh(display);
      break;
    case 'b':
      for (uint32_t i = 0; i < DISPLAY_HEIGHT * DISPLAY_WIDTH; i++) {
        framebuffer[i].r = 0;
        framebuffer[i].g = 0;
        framebuffer[i].b = 0xFF;
      }
      display_refresh(display);
      break;
    case 'B':
      for (uint32_t i = 0; i < DISPLAY_HEIGHT * DISPLAY_WIDTH; i++) {
        framebuffer[i].r = 0;
        framebuffer[i].g = 0;
        framebuffer[i].b = 0;
      }
      display_refresh(display);
      break;
    case 'i': {
      uint8_t* framebuffer_ptr = (uint8_t*)framebuffer;
      uint32_t bytes = DISPLAY_WIDTH * DISPLAY_HEIGHT * 3;

      pos = 0;

      while (pos < bytes) {
        uint32_t max = bytes - pos;
        if (max > 65535) {
          max = 65535;
        }

        uint16_t received = cdc_data_receive(usb_dev, framebuffer_ptr, max);
        pos += received;
        framebuffer_ptr += received;
      }

      display_refresh(display);
      break;
    }
    }
  }
}


// Interrupt handlers

void hard_fault_handler() {
  volatile uint32_t cfsr = SCB->CFSR;
  volatile uint32_t hfsr = SCB->HFSR;
  volatile uint32_t bfar = SCB->BFAR;

  while(1) {}
}

void exti15_10_handler(void)
{
  if (exti_is_interrupt(&exti_wkup)) {
    exti_clear_interrupt(&exti_wkup);
    pin_toggle(&led1);
  }
}