/**
 * @brief   Stops an ongoing conversion.
 * @details This function stops the currently ongoing conversion and returns
 *          the driver in the @p DAC_READY state. If there was no conversion
 *          being processed then the function does nothing.
 *
 * @param[in] dacp      pointer to the @p DACDriver object
 *
 * @iclass
 */
void dacStopConversionI(DACDriver *dacp) {

  osalDbgCheckClassI();
  osalDbgCheck(dacp != NULL);
  osalDbgAssert((dacp->state == DAC_READY) ||
                (dacp->state == DAC_ACTIVE) ||
                (dacp->state == DAC_COMPLETE),
                "invalid state");

  if (dacp->state != DAC_READY) {
    dac_lld_stop_conversion(dacp);
    dacp->grpp  = NULL;
    dacp->state = DAC_READY;
    _dac_reset_i(dacp);
  }
}

/**
 * @brief   Disables a channel de-activation edge notification.
 * @pre     The PWM unit must have been activated using @p pwmStart().
 * @pre     The channel must have been activated using @p pwmEnableChannel().
 * @note    If the notification is already disabled then the call has no effect.
 *
 * @param[in] pwmp      pointer to a @p PWMDriver object
 * @param[in] channel   PWM channel identifier (0...channels-1)
 *
 * @api
 */
void pwmDisableChannelNotification(PWMDriver *pwmp, pwmchannel_t channel) {

  osalDbgCheck((pwmp != NULL) && (channel < pwmp->channels));

  osalSysLock();

  osalDbgAssert(pwmp->state == PWM_READY, "not ready");
  osalDbgAssert((pwmp->enabled & (1 << channel)) != 0,
                "channel not enabled");
  osalDbgAssert(pwmp->config->channels[channel].callback != NULL,
                "undefined channel callback");

  pwmDisableChannelNotificationI(pwmp, channel);

  osalSysUnlock();
}

void ExtiPnc::pps(bool flag){
  osalDbgCheck(ready);
#if defined(BOARD_BEZVODIATEL)
  if (flag)
    extChannelEnable(&EXTD1, GPIOA_GPS_PPS);
  else
    extChannelDisable(&EXTD1, GPIOA_GPS_PPS);
#elif defined(BOARD_MNU)
  if (flag)
    extChannelEnable(&EXTD1, GPIOB_PPS4);
  else
    extChannelDisable(&EXTD1, GPIOB_PPS4);
#else
#error "Unknown board"
#endif
}

/**
 * @brief   Configures and activates the USB peripheral.
 *
 * @param[in] usbp      pointer to the @p USBDriver object
 * @param[in] config    pointer to the @p USBConfig object
 *
 * @api
 */
void usbStart(USBDriver *usbp, const USBConfig *config) {
  unsigned i;

  osalDbgCheck((usbp != NULL) && (config != NULL));

  osalSysLock();
  osalDbgAssert((usbp->state == USB_STOP) || (usbp->state == USB_READY),
                "invalid state");
  usbp->config = config;
  for (i = 0; i <= (unsigned)USB_MAX_ENDPOINTS; i++) {
    usbp->epc[i] = NULL;
  }
  usb_lld_start(usbp);
  usbp->state = USB_READY;
  osalSysUnlock();
}

/**
 * @brief   Stops a sequential write gracefully.
 *
 * @param[in] mmcp      pointer to the @p MMCDriver object
 *
 * @return              The operation status.
 * @retval HAL_SUCCESS   the operation succeeded.
 * @retval HAL_FAILED    the operation failed.
 *
 * @api
 */
bool mmcStopSequentialWrite(MMCDriver *mmcp) {
  static const uint8_t stop[] = {0xFD, 0xFF};

  osalDbgCheck(mmcp != NULL);

  if (mmcp->state != BLK_WRITING) {
    return HAL_FAILED;
  }

  spiSend(mmcp->config->spip, sizeof(stop), stop);
  spiUnselect(mmcp->config->spip);

  /* Write operation finished.*/
  mmcp->state = BLK_READY;
  return HAL_SUCCESS;
}

/**
 * @brief   Stops an ongoing conversion.
 * @details This function stops the currently ongoing conversion and returns
 *          the driver in the @p ADC_READY state. If there was no conversion
 *          being processed then the function does nothing.
 *
 * @param[in] adcp      pointer to the @p ADCDriver object
 *
 * @iclass
 */
void adcStopConversionI(ADCDriver *adcp) {

  osalDbgCheckClassI();
  osalDbgCheck(adcp != NULL);
  osalDbgAssert((adcp->state == ADC_READY) ||
                (adcp->state == ADC_ACTIVE) ||
                (adcp->state == ADC_COMPLETE),
                "invalid state");

  if (adcp->state != ADC_READY) {
    adc_lld_stop_conversion(adcp);
    adcp->grpp  = NULL;
    adcp->state = ADC_READY;
    _adc_reset_i(adcp);
  }
}

/**
 * @brief   Disables all the active endpoints.
 * @details This function disables all the active endpoints except the
 *          endpoint zero.
 * @note    This function must be invoked in response of a SET_CONFIGURATION
 *          message with configuration number zero.
 *
 * @param[in] usbp      pointer to the @p USBDriver object
 *
 * @iclass
 */
void usbDisableEndpointsI(USBDriver *usbp) {
  unsigned i;

  osalDbgCheckClassI();
  osalDbgCheck(usbp != NULL);
  osalDbgAssert(usbp->state == USB_SELECTED, "invalid state");

  usbp->transmitting &= ~1U;
  usbp->receiving    &= ~1U;
  for (i = 1; i <= (unsigned)USB_MAX_ENDPOINTS; i++) {
    usbp->epc[i] = NULL;
  }

  /* Low level endpoints deactivation.*/
  usb_lld_disable_endpoints(usbp);
}

/**
 * @brief   Waits for a completed capture.
 * @note    The operation could be performed in polled mode depending on.
 * @note    In order to use this function notifications must be disabled.
 * @pre     The driver must be in @p ICU_WAITING or  @p ICU_ACTIVE states.
 * @post    After the capture is available the driver is in @p ICU_ACTIVE
 *          state. If a capture fails then the driver is in @p ICU_WAITING
 *          state.
 *
 * @param[in] icup      pointer to the @p ICUDriver object
 * @return              The capture status.
 * @retval false        if the capture is successful.
 * @retval true         if a timer overflow occurred.
 *
 * @api
 */
bool icuWaitCapture(ICUDriver *icup) {
  bool result;

  osalDbgCheck(icup != NULL);

  osalSysLock();
  osalDbgAssert((icup->state == ICU_WAITING) || (icup->state == ICU_ACTIVE),
                "invalid state");
  osalDbgAssert(icuAreNotificationsEnabledX(icup) == false,
                "notifications enabled");
  result = icu_lld_wait_capture(icup);
  icup->state = result ? ICU_WAITING : ICU_ACTIVE;
  osalSysUnlock();

  return result;
}

/**
 * @brief   Deactivates the PWM peripheral.
 *
 * @param[in] pwmp      pointer to a @p PWMDriver object
 *
 * @api
 */
void pwmStop(PWMDriver *pwmp) {

  osalDbgCheck(pwmp != NULL);

  osalSysLock();

  osalDbgAssert((pwmp->state == PWM_STOP) || (pwmp->state == PWM_READY),
                "invalid state");

  pwm_lld_stop(pwmp);
  pwmp->enabled = 0;
  pwmp->config  = NULL;
  pwmp->state   = PWM_STOP;

  osalSysUnlock();
}

/**
 * @brief   Writes one or more blocks.
 *
 * @param[in] sdcp      pointer to the @p SDCDriver object
 * @param[in] startblk  first block to write
 * @param[out] buf      pointer to the write buffer
 * @param[in] n         number of blocks to write
 *
 * @return              The operation status.
 * @retval HAL_SUCCESS  operation succeeded.
 * @retval HAL_FAILED   operation failed.
 *
 * @notapi
 */
bool sdc_lld_write_aligned(SDCDriver *sdcp, uint32_t startblk,
                           const uint8_t *buf, uint32_t blocks) {
    uint32_t resp[1];

    osalDbgCheck(blocks < 0x1000000 / MMCSD_BLOCK_SIZE);

    sdcp->sdio->DTIMER = STM32_SDC_WRITE_TIMEOUT;

    /* Checks for errors and waits for the card to be ready for writing.*/
    if (_sdc_wait_for_transfer_state(sdcp))
        return HAL_FAILED;

    /* Prepares the DMA channel for writing.*/
    dmaStreamSetMemory0(sdcp->dma, buf);
    dmaStreamSetTransactionSize(sdcp->dma,
                                (blocks * MMCSD_BLOCK_SIZE) / sizeof (uint32_t));
    dmaStreamSetMode(sdcp->dma, sdcp->dmamode | STM32_DMA_CR_DIR_M2P);
    dmaStreamEnable(sdcp->dma);

    /* Setting up data transfer.*/
    sdcp->sdio->ICR   = STM32_SDIO_ICR_ALL_FLAGS;
    sdcp->sdio->MASK  = SDIO_MASK_DCRCFAILIE |
                        SDIO_MASK_DTIMEOUTIE |
                        SDIO_MASK_STBITERRIE |
                        SDIO_MASK_TXUNDERRIE |
                        SDIO_MASK_DATAENDIE;
    sdcp->sdio->DLEN  = blocks * MMCSD_BLOCK_SIZE;

    /* Talk to card what we want from it.*/
    if (sdc_lld_prepare_write(sdcp, startblk, blocks, resp) == TRUE)
        goto error;

    /* Transaction starts just after DTEN bit setting.*/
    sdcp->sdio->DCTRL = SDIO_DCTRL_DBLOCKSIZE_3 |
                        SDIO_DCTRL_DBLOCKSIZE_0 |
                        SDIO_DCTRL_DMAEN |
                        SDIO_DCTRL_DTEN;

    if (sdc_lld_wait_transaction_end(sdcp, blocks, resp) == TRUE)
        goto error;

    return HAL_SUCCESS;

error:
    sdc_lld_error_cleanup(sdcp, blocks, resp);
    return HAL_FAILED;
}

/**
 * @brief   Enters the sleep mode.
 * @details This function puts the CAN driver in sleep mode and broadcasts
 *          the @p sleep_event event source.
 * @pre     In order to use this function the option @p CAN_USE_SLEEP_MODE must
 *          be enabled and the @p CAN_SUPPORTS_SLEEP mode must be supported
 *          by the low level driver.
 *
 * @param[in] canp      pointer to the @p CANDriver object
 *
 * @api
 */
void canSleep(CANDriver *canp) {

  osalDbgCheck(canp != NULL);

  osalSysLock();
  osalDbgAssert((canp->state == CAN_READY) || (canp->state == CAN_SLEEP),
                "invalid state");
  if (canp->state == CAN_READY) {
    can_lld_sleep(canp);
    canp->state = CAN_SLEEP;
#if !defined(CAN_ENFORCE_USE_CALLBACKS)
    osalEventBroadcastFlagsI(&canp->sleep_event, (eventflags_t)0);
    osalOsRescheduleS();
#endif
  }
  osalSysUnlock();
}

static msg_t gyro_read_cooked(void *ip, float axes[]) {
  uint32_t i;
  int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
  msg_t msg;

  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
              (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));

  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
              "gyro_read_cooked(), invalid state");

  msg = gyro_read_raw(ip, raw);
  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES ; i++){
    axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->gyrosensitivity[i];
  }
  return msg;
}

/**
 * @brief   Waits for card idle condition.
 *
 * @param[in] mmcp      pointer to the @p MMCDriver object
 *
 * @return              The operation status.
 * @retval CH_SUCCESS   the operation succeeded.
 * @retval CH_FAILED    the operation failed.
 *
 * @api
 */
bool mmcSync(MMCDriver *mmcp) {

  osalDbgCheck(mmcp != NULL);

  if (mmcp->state != BLK_READY)
    return CH_FAILED;

  /* Synchronization operation in progress.*/
  mmcp->state = BLK_SYNCING;

  spiStart(mmcp->config->spip, mmcp->config->hscfg);
  sync(mmcp);

  /* Synchronization operation finished.*/
  mmcp->state = BLK_READY;
  return CH_SUCCESS;
}

/**
 * @brief   Configures and starts the driver.
 *
 * @param[in] sdup      pointer to a @p SerialUSBDriver object
 * @param[in] config    the serial over USB driver configuration
 *
 * @api
 */
void sduStart(SerialUSBDriver *sdup, const SerialUSBConfig *config) {
  USBDriver *usbp = config->usbp;

  osalDbgCheck(sdup != NULL);

  osalSysLock();
  osalDbgAssert((sdup->state == SDU_STOP) || (sdup->state == SDU_READY),
                "invalid state");
  usbp->in_params[config->bulk_in - 1U]   = sdup;
  usbp->out_params[config->bulk_out - 1U] = sdup;
  if (config->int_in > 0U) {
    usbp->in_params[config->int_in - 1U]  = sdup;
  }
  sdup->config = config;
  sdup->state = SDU_READY;
  osalSysUnlock();
}

/**
 * @brief   Reset bias values for the BaseAccelerometer.
 * @note    Default biases value are obtained from device datasheet when
 *          available otherwise they are considered zero.
 *
 * @param[in] ip        pointer to @p BaseAccelerometer interface.
 *
 * @return              The operation status.
 * @retval MSG_OK       if the function succeeded.
 */
static msg_t acc_reset_bias(void *ip) {
  LSM6DS0Driver* devp;
  uint32_t i;
  msg_t msg = MSG_OK;

  osalDbgCheck(ip != NULL);

  /* Getting parent instance pointer.*/
  devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);

  osalDbgAssert((devp->state == LSM6DS0_READY),
                "acc_reset_bias(), invalid state");

  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
    devp->accbias[i] = LSM6DS0_ACC_BIAS;
  return msg;
}

/**
 * @brief   Enforces leaving the sleep mode.
 * @note    The sleep mode is supposed to be usually exited automatically by
 *          an hardware event.
 *
 * @param[in] canp      pointer to the @p CANDriver object
 */
void canWakeup(CANDriver *canp) {

  osalDbgCheck(canp != NULL);

  osalSysLock();
  osalDbgAssert((canp->state == CAN_READY) || (canp->state == CAN_SLEEP),
                "invalid state");
  if (canp->state == CAN_SLEEP) {
    can_lld_wakeup(canp);
    canp->state = CAN_READY;
#if CAN_ENFORCE_USE_CALLBACKS == FALSE
    osalEventBroadcastFlagsI(&canp->wakeup_event, (eventflags_t)0);
    osalOsRescheduleS();
#endif
  }
  osalSysUnlock();
}

static msg_t read_cooked(void *ip, float axes[]) {
  uint32_t i;
  int32_t raw[LIS302DL_NUMBER_OF_AXES];
  msg_t msg;

  osalDbgCheck((ip != NULL) && (axes != NULL));

  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY),
                "read_cooked(), invalid state");

  msg = read_raw(ip, raw);
  for(i = 0; i < LIS302DL_NUMBER_OF_AXES ; i++){
    axes[i] = (raw[i] * ((LIS302DLDriver *)ip)->sensitivity[i]);
    axes[i] -= ((LIS302DLDriver *)ip)->bias[i];
  }
  return msg;
}

/**
 * @brief   Deactivates the UART peripheral.
 *
 * @param[in] uartp     pointer to the @p UARTDriver object
 *
 * @api
 */
void uartStop(UARTDriver *uartp) {

  osalDbgCheck(uartp != NULL);

  osalSysLock();

  osalDbgAssert((uartp->state == UART_STOP) || (uartp->state == UART_READY),
                "invalid state");

  uart_lld_stop(uartp);
  uartp->config  = NULL;
  uartp->state   = UART_STOP;
  uartp->txstate = UART_TX_IDLE;
  uartp->rxstate = UART_RX_IDLE;

  osalSysUnlock();
}

/**
 * @brief   Stops the driver.
 * @details Any thread waiting on the driver's queues will be awakened with
 *          the message @p MSG_RESET.
 *
 * @param[in] sdp       pointer to a @p SerialDriver object
 *
 * @api
 */
void sdStop(SerialDriver *sdp) {

  osalDbgCheck(sdp != NULL);

  osalSysLock();

  osalDbgAssert((sdp->state == SD_STOP) || (sdp->state == SD_READY),
                "invalid state");

  sd_lld_stop(sdp);
  sdp->state = SD_STOP;
  oqResetI(&sdp->oqueue);
  iqResetI(&sdp->iqueue);
  osalOsRescheduleS();

  osalSysUnlock();
}

/**
 * @brief   Gets a bit field from a words array.
 * @note    The bit zero is the LSb of the first word.
 *
 * @param[in] data      pointer to the words array
 * @param[in] end       bit offset of the last bit of the field, inclusive
 * @param[in] start     bit offset of the first bit of the field, inclusive
 *
 * @return              The bits field value, left aligned.
 *
 * @notapi
 */
static uint32_t mmcsd_get_slice(uint32_t *data, uint32_t end, uint32_t start) {
  unsigned startidx, endidx, startoff;
  uint32_t endmask;

  osalDbgCheck((end >= start) && ((end - start) < 32));

  startidx = start / 32;
  startoff = start % 32;
  endidx   = end / 32;
  endmask  = (1 << ((end % 32) + 1)) - 1;

  /* One or two pieces?*/
  if (startidx < endidx)
    return (data[startidx] >> startoff) |               /* Two pieces case. */
           ((data[endidx] & endmask) << (32 - startoff));
  return (data[startidx] & endmask) >> startoff;        /* One piece case.  */
}