/**
 * @brief   Configures and activates the ADC peripheral.
 *
 * @param[in] adcp      pointer to the @p ADCDriver object
 *
 * @notapi
 */
void adc_lld_start(ADCDriver *adcp) {

  /* If in stopped state then enables the ADC and DMA clocks.*/
  if (adcp->state == ADC_STOP) {
#if STM32_ADC_USE_ADC1
    if (&ADCD1 == adcp) {
      bool_t b;
      b = dmaStreamAllocate(adcp->dmastp,
                            STM32_ADC_ADC1_DMA_IRQ_PRIORITY,
                            (stm32_dmaisr_t)adc_lld_serve_rx_interrupt,
                            (void *)adcp);
      chDbgAssert(!b, "adc_lld_start(), #1", "stream already allocated");
      dmaStreamSetPeripheral(adcp->dmastp, &ADC1->DR);
      rccEnableADC1(FALSE);
    }
#endif /* STM32_ADC_USE_ADC1 */

#if STM32_ADC_USE_ADC2
    if (&ADCD2 == adcp) {
      bool_t b;
      b = dmaStreamAllocate(adcp->dmastp,
                            STM32_ADC_ADC2_DMA_IRQ_PRIORITY,
                            (stm32_dmaisr_t)adc_lld_serve_rx_interrupt,
                            (void *)adcp);
      chDbgAssert(!b, "adc_lld_start(), #2", "stream already allocated");
      dmaStreamSetPeripheral(adcp->dmastp, &ADC2->DR);
      rccEnableADC2(FALSE);
    }
#endif /* STM32_ADC_USE_ADC2 */

#if STM32_ADC_USE_ADC3
    if (&ADCD3 == adcp) {
      bool_t b;
      b = dmaStreamAllocate(adcp->dmastp,
                            STM32_ADC_ADC3_DMA_IRQ_PRIORITY,
                            (stm32_dmaisr_t)adc_lld_serve_rx_interrupt,
                            (void *)adcp);
      chDbgAssert(!b, "adc_lld_start(), #3", "stream already allocated");
      dmaStreamSetPeripheral(adcp->dmastp, &ADC3->DR);
      rccEnableADC3(FALSE);
    }
#endif /* STM32_ADC_USE_ADC3 */

    /* ADC initial setup, starting the analog part here in order to reduce
       the latency when starting a conversion.*/
    adcp->adc->CR1 = 0;
    adcp->adc->CR2 = 0;
    adcp->adc->CR2 = ADC_CR2_ADON;
  }
}

void i2c_t::Init() {
    Standby();
    Resume();
    // ==== DMA ====
    // Here only unchanged parameters of the DMA are configured.
#ifdef STM32F2XX
    if      (ii2c == I2C1) DmaChnl = 1;
    else if (ii2c == I2C2) DmaChnl = 7;
    else                   DmaChnl = 3;   // I2C3
#endif
    dmaStreamAllocate(PDmaTx, IRQ_PRIO_MEDIUM, i2cDmaIrqHandler, this);
    dmaStreamSetPeripheral(PDmaTx, &ii2c->DR);
    dmaStreamAllocate(PDmaRx, IRQ_PRIO_MEDIUM, i2cDmaIrqHandler, this);
    dmaStreamSetPeripheral(PDmaRx, &ii2c->DR);
}

/**
 * @brief   Configures and activates the NAND peripheral.
 *
 * @param[in] nandp         pointer to the @p NANDDriver object
 *
 * @notapi
 */
void nand_lld_start(NANDDriver *nandp) {

  bool b;

  if (FSMCD1.state == FSMC_STOP)
    fsmc_start(&FSMCD1);

  if (nandp->state == NAND_STOP) {
    b = dmaStreamAllocate(nandp->dma,
                          STM32_EMC_FSMC1_IRQ_PRIORITY,
                          (stm32_dmaisr_t)nand_lld_serve_transfer_end_irq,
                          (void *)nandp);
    osalDbgAssert(!b, "stream already allocated");
    nandp->dmamode = STM32_DMA_CR_CHSEL(NAND_DMA_CHANNEL) |
                     STM32_DMA_CR_PL(STM32_NAND_NAND1_DMA_PRIORITY) |
                     STM32_DMA_CR_PSIZE_BYTE |
                     STM32_DMA_CR_MSIZE_BYTE |
                     STM32_DMA_CR_DMEIE |
                     STM32_DMA_CR_TEIE |
                     STM32_DMA_CR_TCIE;
    /* dmaStreamSetFIFO(nandp->dma,
                    STM32_DMA_FCR_DMDIS | NAND_STM32_DMA_FCR_FTH_LVL); */
    nandp->nand->PCR = calc_eccps(nandp) | FSMC_PCR_PTYP | FSMC_PCR_PBKEN;
    nandp->nand->PMEM = nandp->config->pmem;
    nandp->nand->PATT = nandp->config->pmem;
    nandp->isr_handler = nand_isr_handler;
    nand_ready_isr_enable(nandp);
  }
}

/**
 * @brief   Configures and activates the ADC peripheral.
 *
 * @param[in] adcp      pointer to the @p ADCDriver object
 *
 * @notapi
 */
void adc_lld_start(ADCDriver *adcp) {

  /* If in stopped state then enables the ADC and DMA clocks.*/
  if (adcp->state == ADC_STOP) {
#if STM32_ADC_USE_ADC1
    if (&ADCD1 == adcp) {
      bool_t b;
      b = dmaStreamAllocate(adcp->dmastp,
                            STM32_ADC_ADC1_DMA_IRQ_PRIORITY,
                            (stm32_dmaisr_t)adc_lld_serve_rx_interrupt,
                            (void *)adcp);
      chDbgAssert(!b, "adc_lld_start(), #1", "stream already allocated");
      dmaStreamSetPeripheral(adcp->dmastp, &ADC1->DR);
      rccEnableADC1(FALSE);
#if STM32_ADCSW == STM32_ADCSW_HSI14
      /* Clock from HSI14, no need for jitter removal.*/
      ADC1->CFGR2 = 0;
#else
#if STM32_ADCPRE == STM32_ADCPRE_DIV2
      ADC1->CFGR2 = ADC_CFGR2_JITOFFDIV2;
#else
      ADC1->CFGR2 = ADC_CFGR2_JITOFFDIV4;
#endif
#endif
    }
#endif /* STM32_ADC_USE_ADC1 */

    /* ADC initial setup, starting the analog part here in order to reduce
       the latency when starting a conversion.*/
    adcp->adc->CR = ADC_CR_ADEN;
    while (!(adcp->adc->ISR & ADC_ISR_ADRDY))
      ;
  }
}

void DbgUart_t::Init(uint32_t ABaudrate) {
    PWrite = TXBuf;
    PRead = TXBuf;
    ICountToSendNext = 0;
    IDmaIsIdle = true;
    PinSetupAlterFuncOutput(GPIOA, 9, omPushPull);      // TX1

    // ==== USART configuration ====
    rccEnableUSART1(FALSE);     // UART clock
    USART1->BRR = Clk.APB2FreqHz / ABaudrate;
    USART1->CR2 = 0;
    USART1->CR3 = USART_CR3_DMAT;   // Enable DMA at transmitter
    USART1->CR1 = USART_CR1_TE;     // Transmitter enabled

    // ==== DMA ====
    // Here only the unchanged parameters of the DMA are configured.
    dmaStreamAllocate     (STM32_DMA1_STREAM4, 1, DbgUartIrq, NULL);
    dmaStreamSetPeripheral(STM32_DMA1_STREAM4, &USART1->DR);
    dmaStreamSetMode      (STM32_DMA1_STREAM4,
            STM32_DMA_CR_PL(0b10) |     // Priority is high
            STM32_DMA_CR_MSIZE_BYTE |
            STM32_DMA_CR_PSIZE_BYTE |
            STM32_DMA_CR_MINC |         // Memory pointer increase
            STM32_DMA_CR_DIR_M2P |      // Direction is memory to peripheral
            STM32_DMA_CR_TCIE           // Enable Transmission Complete IRQ
             );
    USART1->CR1 |= USART_CR1_UE;        // Enable USART
}

void Lcd_t::Init(void) {
    BckLt.Init(LCD_BCKLT_GPIO, LCD_BCKLT_PIN, LCD_BCKLT_TMR, LCD_BCKLT_CHNL, LCD_TOP_BRIGHTNESS);
    // Remap Timer15 to PB14 & PB15
    AFIO->MAPR2 |= 0x00000001;
    // ==== GPIOs ====
    // Configure LCD_XRES, LCD_XCS, LCD_SCLK & LCD_SDA as Push-Pull output
    PinSetupOut(LCD_GPIO, LCD_XRES, omPushPull);
    PinSetupOut(LCD_GPIO, LCD_XRES, omPushPull);
    PinSetupOut(LCD_GPIO, LCD_XCS,  omPushPull);
    PinSetupOut(LCD_GPIO, LCD_SCLK, omPushPull);
    PinSetupOut(LCD_GPIO, LCD_SDA,  omPushPull);
    // ========================= Init LCD ======================================
    SCLK_Lo();
    XCS_Hi();
    // Reset display
    XRES_Lo();
    chThdSleepMilliseconds(7);
    XRES_Hi();
    WriteCmd(0xAF);    // display ON
    // Reset display again
    XRES_Lo();
    chThdSleepMilliseconds(7);
    XRES_Hi();
    chThdSleepMilliseconds(7);
    // Initial commands
    WriteCmd(0xAF);    // display ON
    WriteCmd(0xA4);    // Set normal display mode
    WriteCmd(0x2F);    // Charge pump on
    WriteCmd(0x40);    // Set start row address = 0

    WriteCmd(0xC8);    // Mirror Y axis
    //WriteCmd(0xA1);    // Mirror X axis
    // Set x=0, y=0
    WriteCmd(0xB0);    // Y axis initialization
    WriteCmd(0x10);    // X axis initialisation1
    WriteCmd(0x00);    // X axis initialisation2
    Cls();             // clear LCD buffer

    // ====================== Switch to USART + DMA ============================
    PinSetupAlterFuncOutput(LCD_GPIO, LCD_SCLK, omPushPull);
    PinSetupAlterFuncOutput(LCD_GPIO, LCD_SDA, omPushPull);
    // Workaround hardware bug with disabled CK3 when SPI2 is enabled
    SPI2->CR2 |= SPI_CR2_SSOE;
    // ==== USART init ==== clock enabled, idle low, first edge, enable last bit pulse
    rccEnableUSART3(FALSE);
    USART3->CR1 = USART_CR1_UE;     // Enable
    USART3->BRR = Clk.APB1FreqHz / LCD_UART_SPEED;
    USART3->CR2 = USART_CR2_CLKEN | USART_CR2_LBCL; // Enable clock, enable last bit clock
    USART3->CR1 = USART_CR1_UE | USART_CR1_M | USART_CR1_TE;
    USART3->CR3 = USART_CR3_DMAT;   // Enable DMA at transmitter
    // DMA
    dmaStreamAllocate     (LCD_DMA, IRQ_PRIO_LOW, nullptr, NULL);
    dmaStreamSetPeripheral(LCD_DMA, &USART3->DR);
    dmaStreamSetMemory0   (LCD_DMA, IBuf);
    dmaStreamSetTransactionSize(LCD_DMA, LCD_VIDEOBUF_SIZE);
    dmaStreamSetMode      (LCD_DMA, LCD_DMA_TX_MODE);
    // Start transmission
    XCS_Lo();
    dmaStreamEnable(LCD_DMA);
}

void DbgUart_t::Init(uint32_t ABaudrate) {
    PWrite = TXBuf;
    PRead = TXBuf;
    ICountToSendNext = 0;
    IDmaIsIdle = true;
    //PinSetupAlterFunc(GPIOA, 9, omPushPull, pudNone, AF7);      // TX1
    PinSetupAlterFunc(GPIOA, 2, omPushPull, pudNone, AF7);      // TX2

    // ==== USART configuration ====
    UART_RCC_ENABLE();
    UART->BRR = Clk.APB2FreqHz / ABaudrate;
    UART->CR2 = 0;
    UART->CR3 = USART_CR3_DMAT;   // Enable DMA at transmitter
    UART->CR1 = USART_CR1_TE;     // Transmitter enabled

    // ==== DMA ====
    // Here only the unchanged parameters of the DMA are configured.
    dmaStreamAllocate     (UART_DMA, 1, DbgUartIrq, NULL);
    dmaStreamSetPeripheral(UART_DMA, &UART->DR);
    dmaStreamSetMode      (UART_DMA,
            STM32_DMA_CR_CHSEL(UART_DMA_CHNL) |
            DMA_PRIORITY_LOW |
            STM32_DMA_CR_MSIZE_BYTE |
            STM32_DMA_CR_PSIZE_BYTE |
            STM32_DMA_CR_MINC |         // Memory pointer increase
            STM32_DMA_CR_DIR_M2P |      // Direction is memory to peripheral
            STM32_DMA_CR_TCIE           // Enable Transmission Complete IRQ
             );
    UART->CR1 |= USART_CR1_UE;        // Enable USART
}

/**
 * @brief   Configures and activates the ADC peripheral.
 *
 * @param[in] adcp      pointer to the @p ADCDriver object
 *
 * @notapi
 */
void adc_lld_start(ADCDriver *adcp) {

  /* If in stopped state then enables the ADC and DMA clocks.*/
  if (adcp->state == ADC_STOP) {
#if STM32_ADC_USE_ADC1
    if (&ADCD1 == adcp) {
      bool b;
      b = dmaStreamAllocate(adcp->dmastp,
                            STM32_ADC_ADC1_DMA_IRQ_PRIORITY,
                            (stm32_dmaisr_t)adc_lld_serve_rx_interrupt,
                            (void *)adcp);
      osalDbgAssert(!b, "stream already allocated");
      dmaStreamSetPeripheral(adcp->dmastp, &ADC1->DR);
      rccEnableADC1(FALSE);

      /* Clock settings.*/
      adcp->adc->CFGR2 = STM32_ADC_CKMODE;
    }
#endif /* STM32_ADC_USE_ADC1 */

    /* ADC initial setup, starting the analog part here in order to reduce
       the latency when starting a conversion.*/
    adcp->adc->CR = ADC_CR_ADEN;
    while (!(adcp->adc->ISR & ADC_ISR_ADRDY))
      ;
  }
}

 */
void sdc_lld_start(SDCDriver *sdcp) {

  sdcp->dmamode = STM32_DMA_CR_CHSEL(DMA_CHANNEL) |
                  STM32_DMA_CR_PL(STM32_SDC_SDIO_DMA_PRIORITY) |
                  STM32_DMA_CR_PSIZE_WORD |
                  STM32_DMA_CR_MSIZE_WORD |
                  STM32_DMA_CR_MINC;

#if (defined(STM32F4XX) || defined(STM32F2XX))
  sdcp->dmamode |= STM32_DMA_CR_PFCTRL |
                   STM32_DMA_CR_PBURST_INCR4 |
                   STM32_DMA_CR_MBURST_INCR4;
#endif

  if (sdcp->state == BLK_STOP) {
    /* Note, the DMA must be enabled before the IRQs.*/
    bool_t b;
    b = dmaStreamAllocate(sdcp->dma, STM32_SDC_SDIO_IRQ_PRIORITY, NULL, NULL);
    chDbgAssert(!b, "i2c_lld_start(), #3", "stream already allocated");
    dmaStreamSetPeripheral(sdcp->dma, &SDIO->FIFO);
#if (defined(STM32F4XX) || defined(STM32F2XX))
    dmaStreamSetFIFO(sdcp->dma, STM32_DMA_FCR_DMDIS | STM32_DMA_FCR_FTH_FULL);
#endif
    nvicEnableVector(STM32_SDIO_NUMBER,
                     CORTEX_PRIORITY_MASK(STM32_SDC_SDIO_IRQ_PRIORITY));
    rccEnableSDIO(FALSE);
  }

  /* Configuration, card clock is initially stopped.*/
  SDIO->POWER  = 0;
  SDIO->CLKCR  = 0;
  SDIO->DCTRL  = 0;
  SDIO->DTIMER = 0;

void Adc_t::Init() {
    rccResetADC1();
    rccEnableADC1(FALSE);           // Enable digital clock
    // Configure
    ADC1->CFGR1 = (ADC_CFGR1_CONT | ADC_CFGR1_DMAEN); // Enable Continuous mode and DMA request
    ADC1->CFGR2 = (0b01 << 30);     // Clock: PCLK/2
    // Setup channels
    ADC1->CHSELR = 0;
    for(uint8_t i=0; i < ADC_CHANNEL_CNT; i++) {
        ADC1->CHSELR |= (1 << AdcChannels[i]);
    }
    ADC1->SMPR = (uint32_t)ast55d5Cycles;       // Setup sampling time
    // Calibrate
    uint32_t cnt=0;
    ADC1->CR |= ADC_CR_ADCAL;   // Start calibration
    while(BitIsSet(ADC1->CR, ADC_CR_ADCAL)) {
        if(cnt++ >= 63000) {
            Uart.Printf("ADC calib fail\r");
            return;
        }
    }
    // Enable ADC
    ADC1->CR |= ADC_CR_ADEN;   // Enable ADC
    while(!BitIsSet(ADC1->ISR, ADC_ISR_ADRDY)); // Wait until ADC is ready
    // ==== DMA ====
    dmaStreamAllocate     (ADC_DMA, IRQ_PRIO_LOW, AdcTxIrq, NULL);
    dmaStreamSetPeripheral(ADC_DMA, &ADC1->DR);
    dmaStreamSetMode      (ADC_DMA, ADC_DMA_MODE);
//    Uart.Printf("ADC is set\r");
}

void DbgUart_t::Init(uint32_t ABaudrate) {
    PWrite = TXBuf;
    PRead = TXBuf;
    ICountToSendNext = 0;
    IDmaIsIdle = true;
    PinSetupAlterFunc(GPIOA, 9, omPushPull, pudNone, AF7);      // TX1

    // ==== USART configuration ====
    rccEnableUSART1(FALSE);     // UART clock, no clock in low-power
    USART1->BRR = Clk.APB2FreqHz / 115200;
    USART1->CR2 = 0;
    USART1->CR3 = USART_CR3_DMAT;   // Enable DMA at transmitter
    USART1->CR1 = USART_CR1_TE;     // Transmitter enabled

    // ==== DMA ====
    // Here only the unchanged parameters of the DMA are configured.
    dmaStreamAllocate     (STM32_DMA2_STREAM7, 1, DbgUartIrq, NULL);
    dmaStreamSetPeripheral(STM32_DMA2_STREAM7, &USART1->DR);
    dmaStreamSetMode      (STM32_DMA2_STREAM7,
            STM32_DMA_CR_CHSEL(4) |     // DMA2 Stream7 Channel4 is USART1_TX request
            DMA_PRIORITY_LOW |
            STM32_DMA_CR_MSIZE_BYTE |
            STM32_DMA_CR_PSIZE_BYTE |
            STM32_DMA_CR_MINC |         // Memory pointer increase
            STM32_DMA_CR_DIR_M2P |      // Direction is memory to peripheral
            STM32_DMA_CR_TCIE           // Enable Transmission Complete IRQ
             );
    USART1->CR1 |= USART_CR1_UE;        // Enable USART
}

/**
 * @brief   Configures and activates the QSPI peripheral.
 *
 * @param[in] qspip     pointer to the @p QSPIDriver object
 *
 * @notapi
 */
void qspi_lld_start(QSPIDriver *qspip) {

  /* If in stopped state then full initialization.*/
  if (qspip->state == QSPI_STOP) {
#if STM32_QSPI_USE_QUADSPI1
    if (&QSPID1 == qspip) {
      bool b = dmaStreamAllocate(qspip->dma,
                                 STM32_QSPI_QUADSPI1_DMA_IRQ_PRIORITY,
                                 (stm32_dmaisr_t)qspi_lld_serve_dma_interrupt,
                                 (void *)qspip);
      osalDbgAssert(!b, "stream already allocated");
      rccEnableQUADSPI1(false);
    }
#endif

    /* Common initializations.*/
    dmaStreamSetPeripheral(qspip->dma, &qspip->qspi->DR);
  }

  /* QSPI setup and enable.*/
  qspip->qspi->DCR = qspip->config->dcr;
  qspip->qspi->CR  = ((STM32_QSPI_QUADSPI1_PRESCALER_VALUE - 1U) << 24U) |
                      QUADSPI_CR_TCIE | QUADSPI_CR_DMAEN | QUADSPI_CR_EN;
  qspip->qspi->FCR = QUADSPI_FCR_CTEF | QUADSPI_FCR_CTCF |
                     QUADSPI_FCR_CSMF | QUADSPI_FCR_CTOF;
}

/**
 * @brief   Configures and activates the ADC peripheral.
 *
 * @param[in] adcp      pointer to the @p ADCDriver object
 *
 * @notapi
 */
void adc_lld_start(ADCDriver *adcp) {

  /* If in stopped state then enables the ADC and DMA clocks.*/
  if (adcp->state == ADC_STOP) {
#if STM32_ADC_USE_ADC1
    if (&ADCD1 == adcp) {
      bool b;
      b = dmaStreamAllocate(adcp->dmastp,
                            STM32_ADC_ADC12_DMA_IRQ_PRIORITY,
                            (stm32_dmaisr_t)adc_lld_serve_dma_interrupt,
                            (void *)adcp);
      osalDbgAssert(!b, "stream already allocated");
      rccEnableADC12(FALSE);
    }
#endif /* STM32_ADC_USE_ADC1 */

#if STM32_ADC_USE_ADC3
    if (&ADCD3 == adcp) {
      bool b;
      b = dmaStreamAllocate(adcp->dmastp,
                            STM32_ADC_ADC34_DMA_IRQ_PRIORITY,
                            (stm32_dmaisr_t)adc_lld_serve_dma_interrupt,
                            (void *)adcp);
      osalDbgAssert(!b, "stream already allocated");
      rccEnableADC34(FALSE);
    }
#endif /* STM32_ADC_USE_ADC2 */

    /* Setting DMA peripheral-side pointer.*/
#if STM32_ADC_DUAL_MODE
      dmaStreamSetPeripheral(adcp->dmastp, &adcp->adcc->CDR);
#else
      dmaStreamSetPeripheral(adcp->dmastp, &adcp->adcm->DR);
#endif

    /* Clock source setting.*/
    adcp->adcc->CCR = STM32_ADC_ADC12_CLOCK_MODE | ADC_DMA_MDMA;

    /* Master ADC calibration.*/
    adc_lld_vreg_on(adcp);
    adc_lld_calibrate(adcp);

    /* Master ADC enabled here in order to reduce conversions latencies.*/
    adc_lld_analog_on(adcp);
  }
}

void Uart_t::Init(uint32_t ABaudrate, GPIO_TypeDef *PGpioTx, const uint16_t APinTx, GPIO_TypeDef *PGpioRx, const uint16_t APinRx) {
#else
void Uart_t::Init(uint32_t ABaudrate, GPIO_TypeDef *PGpioTx, const uint16_t APinTx) {
#endif
    PinSetupAlterFunc(PGpioTx, APinTx, omPushPull, pudNone, UART_AF);
    IBaudrate = ABaudrate;
    // ==== USART configuration ====
    if(UART == USART1) {rccEnableUSART1(FALSE); }
    else if(UART == USART2) {rccEnableUSART2(FALSE); }

    OnAHBFreqChange();  // Setup baudrate

    UART->CR2 = 0;
#if UART_USE_DMA    // ==== DMA ====
    dmaStreamAllocate     (UART_DMA_TX, IRQ_PRIO_MEDIUM, CmdUartTxIrq, NULL);
    dmaStreamSetPeripheral(UART_DMA_TX, &UART_TX_REG);
    dmaStreamSetMode      (UART_DMA_TX, UART_DMA_TX_MODE);
    IDmaIsIdle = true;
#endif

#if UART_RX_ENABLED
    UART->CR1 = USART_CR1_TE | USART_CR1_RE;        // TX & RX enable
    UART->CR3 = USART_CR3_DMAT | USART_CR3_DMAR;    // Enable DMA at TX & RX

    PinSetupAlterFunc(PGpioRx, APinRx,  omOpenDrain, pudPullUp, UART_AF);

    dmaStreamAllocate     (UART_DMA_RX, IRQ_PRIO_LOW, nullptr, NULL);
    dmaStreamSetPeripheral(UART_DMA_RX, &UART_RX_REG);
    dmaStreamSetMemory0   (UART_DMA_RX, IRxBuf);
    dmaStreamSetTransactionSize(UART_DMA_RX, UART_RXBUF_SZ);
    dmaStreamSetMode      (UART_DMA_RX, UART_DMA_RX_MODE);
    dmaStreamEnable       (UART_DMA_RX);
    // Thread
    IPThd = chThdCreateStatic(waUartRxThread, sizeof(waUartRxThread), LOWPRIO, UartRxThread, NULL);
#else
    UART->CR1 = USART_CR1_TE;     // Transmitter enabled
#if UART_USE_DMA
    UART->CR3 = USART_CR3_DMAT;   // Enable DMA at transmitter
#endif
#endif
    UART->CR1 |= USART_CR1_UE;    // Enable USART
}

void Adc_t::Init() {
    rccEnableADC1(FALSE);   // Enable digital clock
    SetupClk(adcDiv4);      // Setup ADCCLK
    SetChannelCount(ADC_CHANNEL_CNT);
    for(uint8_t i = 0; i < ADC_CHANNEL_CNT; i++) ChannelConfig(AdcChannels[i]);
    // ==== DMA ====
    // Here only unchanged parameters of the DMA are configured.
    dmaStreamAllocate     (ADC_DMA, IRQ_PRIO_LOW, AdcTxIrq, NULL);
    dmaStreamSetPeripheral(ADC_DMA, &ADC1->DR);
    dmaStreamSetMode      (ADC_DMA, ADC_DMA_MODE);
}

// CNV IRQ
void eAdc_t::IIrqHandler() {
    CskTmr.Disable();
    SPI1->DR = 0;
    Adc.Rslt = 0;
    dmaStreamAllocate     (EADC_DMA, IRQ_PRIO_MEDIUM, SIrqDmaHandler, NULL);
    dmaStreamSetPeripheral(EADC_DMA, &ADC_SPI->DR);
    dmaStreamSetMode      (EADC_DMA, EADC_DMA_MODE);
    dmaStreamSetMemory0(EADC_DMA, &Adc.Rslt);
    dmaStreamSetTransactionSize(EADC_DMA, 1);
    dmaStreamEnable(EADC_DMA);
    ADC_CNV_LOW();
}

int main(void)
{
  static const evhandler_t evhndl[] = {InsertHandler, RemoveHandler};
  struct EventListener el0, el1;

  // os init
  halInit();
  chSysInit();

  // setup LED pads
  palSetPadMode(GPIOD, 12, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
  palClearPad(GPIOD, 12); // green LED
  palSetPadMode(GPIOD, 15, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
  palClearPad(GPIOD, 15); // blue LED

  // setup pads to USART2 function (connect these pads through RS232 transceiver with PC, terminal emu needs 38400 baud)
  sdStart(&SD2, NULL);
  palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7) | PAL_STM32_OSPEED_HIGHEST); // TX
  palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7)); // RX

  // setup pads to SPI1 function (connect these pads to your SD card accordingly)
  palSetPadMode(GPIOC, 4, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); // NSS
  palSetPadMode(GPIOA, 5, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); // SCK
  palSetPadMode(GPIOA, 6, PAL_MODE_ALTERNATE(5)); // MISO
  palSetPadMode(GPIOA, 7, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); // MOSI
  palSetPad(GPIOC, 4); // set NSS high

  // initialize MMC driver
  mmcObjectInit(&MMCD1, &SPID1, &ls_spicfg, &hs_spicfg, mmc_is_protected, mmc_is_inserted);
  mmcStart(&MMCD1, NULL);

  // ChibiOS has no I2S support yet;
  // codec.c initializes everything necessary
  // except the I2S TX DMA interrupt vector (because it would
  // conflict with the ChibiOS kernel)
  // we can make ChibiOS call our own handler by letting
  // it create the DMA stream for SPI3
  dmaStreamAllocate(SPID3.dmatx,
                    STM32_SPI_SPI3_IRQ_PRIORITY,
                    (stm32_dmaisr_t)I2SDmaTxInterrupt,
                    &SPID3);

  // blink thread; also checks the user button
  chThdCreateStatic(waBlinkThread, sizeof(waBlinkThread), NORMALPRIO, BlinkThread, NULL);

  chEvtRegister(&MMCD1.inserted_event, &el0, 0);
  chEvtRegister(&MMCD1.removed_event, &el1, 1);
  while(TRUE)
  {
    chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
  }
}

void i2c_t::Init(I2C_TypeDef *pi2c, uint32_t BitrateHz) {
    ii2c = pi2c;
    IBitrateHz = BitrateHz;
    if(ii2c == I2C1) {
        IPGpio = GPIOB;
        ISclPin = 6;
        ISdaPin = 7;
        PDmaTx = STM32_DMA1_STREAM6;
        PDmaRx = STM32_DMA1_STREAM7;
    }
    Standby();
    Resume();

    // ==== DMA ====
    // Here only unchanged parameters of the DMA are configured.
    // Setup Dma TX
    dmaStreamAllocate(PDmaTx, IRQ_PRIO_MEDIUM, i2cDmaIrqHandler, this);
    dmaStreamSetPeripheral(PDmaTx, &ii2c->DR);
    dmaStreamSetMode      (PDmaTx,
            DMA_PRIORITY_LOW |
            STM32_DMA_CR_MSIZE_BYTE |
            STM32_DMA_CR_PSIZE_BYTE |
            STM32_DMA_CR_MINC |         // Memory pointer increase
            STM32_DMA_CR_DIR_M2P |      // Direction is memory to peripheral
            STM32_DMA_CR_TCIE           // Enable Transmission Complete IRQ
             );

    // Setup Dma RX
    dmaStreamAllocate(PDmaRx, IRQ_PRIO_MEDIUM, i2cDmaIrqHandler, this);
    dmaStreamSetPeripheral(PDmaRx, &ii2c->DR);
    dmaStreamSetMode      (PDmaRx,
            DMA_PRIORITY_LOW |
            STM32_DMA_CR_MSIZE_BYTE |
            STM32_DMA_CR_PSIZE_BYTE |
            STM32_DMA_CR_MINC |         // Memory pointer increase
            STM32_DMA_CR_DIR_P2M        // Direction is peripheral to memory
            | STM32_DMA_CR_TCIE         // Enable Transmission Complete IRQ
             );
}

void CmdUart_t::Init(uint32_t ABaudrate) {
    PWrite = TXBuf;
    PRead = TXBuf;
    IDmaIsIdle = true;
    IFullSlotsCount = 0;
    PinSetupAlterFunc(UART_GPIO, UART_TX_PIN, omPushPull, pudNone, UART_AF);

    // ==== USART configuration ====
    UART_RCC_ENABLE();
//    UART->CR1 = USART_CR1_UE;     // Enable USART
    UART->BRR = Clk.APBFreqHz / ABaudrate;
    UART->CR2 = 0;
    // ==== DMA ====
    dmaStreamAllocate     (UART_DMA_TX, IRQ_PRIO_HIGH, CmdUartTxIrq, NULL);
    dmaStreamSetPeripheral(UART_DMA_TX, &UART->TDR);
    dmaStreamSetMode      (UART_DMA_TX, UART_DMA_TX_MODE);

#if UART_RX_ENABLED
    UART->CR1 = USART_CR1_TE | USART_CR1_RE;        // TX & RX enable
    UART->CR3 = USART_CR3_DMAT | USART_CR3_DMAR;    // Enable DMA at TX & RX

    IResetCmd();
    PinSetupAlterFunc(UART_GPIO, UART_RX_PIN,  omOpenDrain, pudPullUp, UART_AF);

    dmaStreamAllocate     (UART_DMA_RX, IRQ_PRIO_LOW, nullptr, NULL);
    dmaStreamSetPeripheral(UART_DMA_RX, &UART->DR);
    dmaStreamSetMemory0   (UART_DMA_RX, IRxBuf);
    dmaStreamSetTransactionSize(UART_DMA_RX, UART_RXBUF_SZ);
    dmaStreamSetMode      (UART_DMA_RX, UART_DMA_RX_MODE);
    dmaStreamEnable       (UART_DMA_RX);
    // Create and start thread
    chThdCreateStatic(waUartRxThread, sizeof(waUartRxThread), NORMALPRIO, (tfunc_t)UartRxThread, NULL);
#else
    UART->CR1 = USART_CR1_TE;     // Transmitter enabled
    UART->CR3 = USART_CR3_DMAT;   // Enable DMA at transmitter
#endif
    UART->CR1 |= USART_CR1_UE;    // Enable USART
}

void Adc_t::Init() {
    rccEnableADC1(FALSE);   	// Enable digital clock
    SetupClk(ADC_CLK_DIVIDER);  // Setup ADCCLK
    // Setup channels
    SetSequenceLength(ADC_SEQ_LEN);
    uint8_t SeqIndx = 1;    // First sequence item is 1, not 0
    for(uint8_t i=0; i < ADC_CHANNEL_CNT; i++) {
		SetChannelSampleTime(AdcChannels[i], ADC_SAMPLE_TIME);
		for(uint8_t j=0; j<ADC_SAMPLE_CNT; j++) SetSequenceItem(SeqIndx++, AdcChannels[i]);
	}
    // ==== DMA ====
    dmaStreamAllocate     (ADC_DMA, IRQ_PRIO_LOW, AdcTxIrq, NULL);
    dmaStreamSetPeripheral(ADC_DMA, &ADC1->DR);
    dmaStreamSetMode      (ADC_DMA, ADC_DMA_MODE);
}