/**
 * @brief   Creates a new thread allocating the memory from the specified
 *          memory pool.
 * @pre     The configuration options @p CH_CFG_USE_DYNAMIC and
 *          @p CH_CFG_USE_MEMPOOLS must be enabled in order to use this
 *          function.
 * @pre     The pool must be initialized to contain only objects with
 *          alignment @p PORT_WORKING_AREA_ALIGN.
 * @note    A thread can terminate by calling @p chThdExit() or by simply
 *          returning from its main function.
 * @note    The memory allocated for the thread is not released automatically,
 *          it is responsibility of the creator thread to call @p chThdWait()
 *          and then release the allocated memory.
 *
 * @param[in] mp        pointer to the memory pool object
 * @param[in] prio      the priority level for the new thread
 * @param[in] pf        the thread function
 * @param[in] arg       an argument passed to the thread function. It can be
 *                      @p NULL.
 * @return              The pointer to the @p thread_t structure allocated for
 *                      the thread into the working space area.
 * @retval  NULL        if the memory pool is empty.
 *
 * @api
 */
thread_t *chThdCreateFromMemoryPool(memory_pool_t *mp, tprio_t prio,
                                    tfunc_t pf, void *arg) {
  void *wsp;

  chDbgCheck(mp != NULL);

  wsp = chPoolAlloc(mp);
  if (wsp == NULL) {
    return NULL;
  }

#if CH_DBG_FILL_THREADS == TRUE
  _thread_memfill((uint8_t *)wsp,
                  (uint8_t *)wsp + mp->object_size,
                  CH_DBG_STACK_FILL_VALUE);
#endif

  return chThdCreateStatic(wsp, mp->object_size, prio, pf, arg);
}

caddr_t _sbrk_r(struct _reent *r, int incr)
{
#if CH_USE_MEMCORE
  void *p;

  chDbgCheck(incr > 0, "_sbrk_r");

  (void)r;
  p = chCoreAlloc((size_t)incr);
  if (p == NULL) {
    __errno_r(r) = ENOMEM;
    return (caddr_t)-1;
  }
  return (caddr_t)p;
#else
  __errno_r(r) = ENOMEM;
  return (caddr_t)-1;
#endif
}

/**
 * @brief   Performs a wait operation on a semaphore with timeout specification.
 *
 * @param[in] sp        pointer to a @p semaphore_t structure
 * @param[in] timeout   the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_IMMEDIATE immediate timeout.
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              A message specifying how the invoking thread has been
 *                      released from the semaphore.
 * @retval NIL_MSG_OK   if the thread has not stopped on the semaphore or the
 *                      semaphore has been signaled.
 * @retval NIL_MSG_RST  if the semaphore has been reset using @p chSemReset().
 * @retval NIL_MSG_TMO  if the semaphore has not been signaled or reset within
 *                      the specified timeout.
 *
 * @sclass
 */
msg_t chSemWaitTimeoutS(semaphore_t *sp, systime_t timeout) {

  chDbgCheckClassS();
  chDbgCheck(sp != NULL);

  /* Note, the semaphore counter is a volatile variable so accesses are
     manually optimized.*/
  cnt_t cnt = sp->cnt;
  if (cnt <= (cnt_t)0) {
    if (TIME_IMMEDIATE == timeout) {
      return MSG_TIMEOUT;
    }
    sp->cnt = cnt - (cnt_t)1;
    nil.current->u1.semp = sp;
    return chSchGoSleepTimeoutS(NIL_STATE_WTSEM, timeout);
  }
  sp->cnt = cnt - (cnt_t)1;
  return MSG_OK;
}

/**
 * @brief   Changes the running thread priority level then reschedules if
 *          necessary.
 * @note    The function returns the real thread priority regardless of the
 *          current priority that could be higher than the real priority
 *          because the priority inheritance mechanism.
 *
 * @param[in] newprio   the new priority level of the running thread
 * @return              The old priority level.
 *
 * @api
 */
tprio_t chThdSetPriority(tprio_t newprio) {
  tprio_t oldprio;

  chDbgCheck(newprio <= HIGHPRIO, "chThdSetPriority");

  chSysLock();
#if CH_USE_MUTEXES
  oldprio = currp->p_realprio;
  if ((currp->p_prio == currp->p_realprio) || (newprio > currp->p_prio))
    currp->p_prio = newprio;
  currp->p_realprio = newprio;
#else
  oldprio = currp->p_prio;
  currp->p_prio = newprio;
#endif
  chSchRescheduleS();
  chSysUnlock();
  return oldprio;
}

/**
 * @brief   Performs a wait operation on a semaphore with timeout specification.
 *
 * @param[in] sp        pointer to a @p semaphore_t structure
 * @param[in] time      the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_IMMEDIATE immediate timeout.
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              A message specifying how the invoking thread has been
 *                      released from the semaphore.
 * @retval MSG_OK       if the thread has not stopped on the semaphore or the
 *                      semaphore has been signaled.
 * @retval MSG_RESET    if the semaphore has been reset using @p chSemReset().
 * @retval MSG_TIMEOUT  if the semaphore has not been signaled or reset within
 *                      the specified timeout.
 *
 * @sclass
 */
msg_t chSemWaitTimeoutS(semaphore_t *sp, systime_t time) {

  chDbgCheckClassS();
  chDbgCheck(sp != NULL);
  chDbgAssert(((sp->s_cnt >= 0) && queue_isempty(&sp->s_queue)) ||
              ((sp->s_cnt < 0) && queue_notempty(&sp->s_queue)),
              "inconsistent semaphore");

  if (--sp->s_cnt < 0) {
    if (TIME_IMMEDIATE == time) {
      sp->s_cnt++;
      return MSG_TIMEOUT;
    }
    currp->p_u.wtobjp = sp;
    sem_insert(currp, &sp->s_queue);
    return chSchGoSleepTimeoutS(CH_STATE_WTSEM, time);
  }
  return MSG_OK;
}

/**
 * @brief   Configures and activates the CAN peripheral.
 *
 * @param[in] canp      pointer to the @p CANDriver object
 * @param[in] config    pointer to the @p CANConfig object
 */
void canStart(CANDriver *canp, const CANConfig *config) {

  chDbgCheck((canp != NULL) && (config != NULL), "canStart");

  chSysLock();
  chDbgAssert((canp->cd_state == CAN_STOP) ||
              (canp->cd_state == CAN_STARTING) ||
              (canp->cd_state == CAN_READY),
              "canStart(), #1",
              "invalid state");
  while (canp->cd_state == CAN_STARTING)
    chThdSleepS(1);
  if (canp->cd_state == CAN_STOP) {
    canp->cd_config = config;
    can_lld_start(canp);
    canp->cd_state = CAN_READY;
  }
  chSysUnlock();
}

/**
 * @brief   Can frame receive.
 * @details The function waits until a frame is received.
 * @note    Trying to receive while in sleep mode simply enqueues the thread.
 *
 * @param[in] canp      pointer to the @p CANDriver object
 * @param[out] crfp     pointer to the buffer where the CAN frame is copied
 * @param[in] timeout   the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_IMMEDIATE immediate timeout (useful in an
 *                        event driven scenario where a thread never blocks
 *                        for I/O).
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              The operation result.
 * @retval RDY_OK       a frame has been received and placed in the buffer.
 * @retval RDY_TIMEOUT  operation not finished within the specified time or
 *                      frame not immediately available if invoked using
 *                      @p TIME_IMMEDIATE.
 * @retval RDY_RESET    driver stopped while waiting.
 */
msg_t canReceive(CANDriver *canp, CANRxFrame *crfp, systime_t timeout) {

  chDbgCheck((canp != NULL) && (crfp != NULL), "canReceive");

  chSysLock();
  chDbgAssert((canp->cd_state == CAN_READY) || (canp->cd_state == CAN_SLEEP),
              "canReceive(), #1",
              "invalid state");
  while ((canp->cd_state == CAN_SLEEP) || !can_lld_can_receive(canp)) {
    msg_t msg = chSemWaitTimeoutS(&canp->cd_rxsem, timeout);
    if (msg != RDY_OK) {
      chSysUnlock();
      return msg;
    }
  }
  can_lld_receive(canp, crfp);
  chSysUnlock();
  return RDY_OK;
}

/**
 * @brief   Can frame transmission.
 * @details The specified frame is queued for transmission, if the hardware
 *          queue is full then the invoking thread is queued.
 * @note    Trying to transmit while in sleep mode simply enqueues the thread.
 *
 * @param[in] canp      pointer to the @p CANDriver object
 * @param[in] ctfp       pointer to the CAN frame to be transmitted
 * @param[in] timeout   the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_IMMEDIATE immediate timeout.
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              The operation result.
 * @retval RDY_OK       the frame has been queued for transmission.
 * @retval RDY_TIMEOUT  operation not finished within the specified time.
 * @retval RDY_RESET    driver stopped while waiting.
 */
msg_t canTransmit(CANDriver *canp, const CANTxFrame *ctfp, systime_t timeout) {

  chDbgCheck((canp != NULL) && (ctfp != NULL), "canTransmit");

  chSysLock();
  chDbgAssert((canp->cd_state == CAN_READY) || (canp->cd_state == CAN_SLEEP),
              "canTransmit(), #1",
              "invalid state");
  while ((canp->cd_state == CAN_SLEEP) || !can_lld_can_transmit(canp)) {
    msg_t msg = chSemWaitTimeoutS(&canp->cd_txsem, timeout);
    if (msg != RDY_OK) {
      chSysUnlock();
      return msg;
    }
  }
  can_lld_transmit(canp, ctfp);
  chSysUnlock();
  return RDY_OK;
}

/**
 * @brief   EDMA channel allocation.
 *
 * @param[in] channel   the channel number
 *
 * @special
 */
void edmaChannelRelease(edma_channel_t channel) {

  chDbgCheck((channel < 0) && (channel >= SPC5_EDMA_NCHANNELS),
             "edmaChannelAllocate");
  chDbgAssert(channels[channel] != NULL,
              "edmaChannelRelease(), #1",
              "not allocated");

  /* Enforcing a stop.*/
  edmaChannelStop(channel);

  /* Clearing ISR sources for the channel.*/
  SPC5_EDMA.CIRQR.R = channel;
  SPC5_EDMA.CEEIR.R = channel;
  SPC5_EDMA.CER.R   = channel;

  /* The channels is flagged as available.*/
  channels[channel] = NULL;
}

/**
 * @brief   Stops a sequential read gracefully.
 *
 * @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_t mmcStopSequentialRead(MMCDriver *mmcp) {
  static const uint8_t stopcmd[] = {0x40 | MMCSD_CMD_STOP_TRANSMISSION,
                                    0, 0, 0, 0, 1, 0xFF};

  chDbgCheck(mmcp != NULL, "mmcStopSequentialRead");

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

  spiSend(mmcp->config->spip, sizeof(stopcmd), stopcmd);
/*  result = recvr1(mmcp) != 0x00;*/
  /* Note, ignored r1 response, it can be not zero, unknown issue.*/
  (void) recvr1(mmcp);

  /* Read operation finished.*/
  spiUnselect(mmcp->config->spip);
  mmcp->state = BLK_READY;
  return CH_SUCCESS;
}

/**
 * @brief   Starts a DAC conversion.
 * @details Starts an asynchronous conversion operation.
 * @post    The callbacks associated to the conversion group will be invoked
 *          on buffer fill and error events.
 * @note    The buffer is organized as a matrix of M*N elements where M is the
 *          channels number configured into the conversion group and N is the
 *          buffer depth. The samples are sequentially written into the buffer
 *          with no gaps.
 *
 * @param[in] dacp      pointer to the @p DACDriver object
 * @param[in] grpp      pointer to a @p DACConversionGroup object
 * @param[in] samples   pointer to the samples buffer
 * @param[in] depth     buffer depth (matrix rows number). The buffer depth
 *                      must be one or an even number.
 *
 * @iclass
 */
void dacStartConversionI(DACDriver *dacp,
                         const DACConversionGroup *grpp,
                         const dacsample_t *samples,
                         size_t depth) {

  chDbgCheckClassI();
  chDbgCheck((dacp != NULL) && (grpp != NULL) && (samples != NULL) &&
             ((depth == 1) || ((depth & 1) == 0)),
             "dacStartConversionI");
  chDbgAssert((dacp->state == DAC_READY) ||
              (dacp->state == DAC_COMPLETE) ||
              (dacp->state == DAC_ERROR),
              "dacStartConversionI(), #1", "not ready");

  dacp->samples  = samples;
  dacp->depth    = depth;
  dacp->grpp     = grpp;
  dacp->state    = DAC_ACTIVE;
  dac_lld_start_conversion(dacp);
}

/**
 * @brief   Releases a DMA stream.
 * @details The stream is freed and, if required, the DMA clock disabled.
 *          Trying to release a unallocated stream is an illegal operation
 *          and is trapped if assertions are enabled.
 * @pre     The stream must have been allocated using @p dmaStreamAllocate().
 * @post    The stream is again available.
 * @note    This function can be invoked in both ISR or thread context.
 *
 * @param[in] dmastp    pointer to a stm32_dma_stream_t structure
 *
 * @special
 */
void dmaStreamRelease(const stm32_dma_stream_t *dmastp) {

  chDbgCheck(dmastp != NULL, "dmaRelease");

  /* Check if the streams is not taken.*/
  chDbgAssert((dma_streams_mask & (1 << dmastp->selfindex)) != 0,
              "dmaRelease(), #1", "not allocated");

  /* Disables the associated IRQ vector.*/
  nvicDisableVector(dmastp->vector);

  /* Marks the stream as not allocated.*/
  dma_streams_mask &= ~(1 << dmastp->selfindex);

  /* Shutting down clocks that are no more required, if any.*/
  if ((dma_streams_mask & STM32_DMA1_STREAMS_MASK) == 0)
    rccDisableDMA1(FALSE);
  if ((dma_streams_mask & STM32_DMA2_STREAMS_MASK) == 0)
    rccDisableDMA2(FALSE);
}

/**
 * @brief   Changes the operation mode of a channel.
 * @note    This function attempts to write over the current configuration
 *          structure that must have been not declared constant. This
 *          violates the @p const qualifier in @p extStart() but it is
 *          intentional.
 * @note    This function cannot be used if the configuration structure is
 *          declared @p const.
 * @note    The effect of this function on constant configuration structures
 *          is not defined.
 *
 * @param[in] extp      pointer to the @p EXTDriver object
 * @param[in] channel   channel to be changed
 * @param[in] extcp     new configuration for the channel
 *
 * @iclass
 */
void extSetChannelModeI(EXTDriver *extp,
                        expchannel_t channel,
                        const EXTChannelConfig *extcp) {
  EXTChannelConfig *oldcp;

  chDbgCheck((extp != NULL) && (channel < EXT_MAX_CHANNELS) &&
             (extcp != NULL), "extSetChannelModeI");

  chDbgAssert(extp->state == EXT_ACTIVE,
              "extSetChannelModeI(), #1", "invalid state");

  /* Note that here the access is enforced as non-const, known access
     violation.*/
  oldcp = (EXTChannelConfig *)&extp->config->channels[channel];

  /* Overwiting the old channels configuration then the channel is reconfigured
     by the low level driver.*/
  *oldcp = *extcp;
  ext_lld_channel_enable(extp, channel);
}

/**
 * @brief   Signals all the Event Listeners registered on the specified Event
 *          Source.
 * @details This function variants ORs the specified event flags to all the
 *          threads registered on the @p event_source_t in addition to the
 *          event flags specified by the threads themselves in the
 *          @p event_listener_t objects.
 * @post    This function does not reschedule so a call to a rescheduling
 *          function must be performed before unlocking the kernel. Note that
 *          interrupt handlers always reschedule on exit so an explicit
 *          reschedule must not be performed in ISRs.
 *
 * @param[in] esp       pointer to the @p event_source_t structure
 * @param[in] flags     the flags set to be added to the listener flags mask
 *
 * @iclass
 */
void chEvtBroadcastFlagsI(event_source_t *esp, eventflags_t flags) {
  event_listener_t *elp;

  chDbgCheckClassI();
  chDbgCheck(esp != NULL);

  elp = esp->next;
  /*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/
  while (elp != (event_listener_t *)esp) {
  /*lint -restore*/
    elp->flags |= flags;
    /* When flags == 0 the thread will always be signaled because the
       source does not emit any flag.*/
    if ((flags == (eventflags_t)0) ||
        ((elp->flags & elp->wflags) != (eventflags_t)0)) {
      chEvtSignalI(elp->listener, elp->events);
    }
    elp = elp->next;
  }
}

/**
 * @brief   Unregisters an Event Listener from its Event Source.
 * @note    If the event listener is not registered on the specified event
 *          source then the function does nothing.
 * @note    For optimal performance it is better to perform the unregister
 *          operations in inverse order of the register operations (elements
 *          are found on top of the list).
 *
 * @param[in] esp       pointer to the  @p event_source_t structure
 * @param[in] elp       pointer to the @p event_listener_t structure
 *
 * @api
 */
void chEvtUnregister(event_source_t *esp, event_listener_t *elp) {
  event_listener_t *p;

  chDbgCheck((esp != NULL) && (elp != NULL));

  /*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/
  p = (event_listener_t *)esp;
  /*lint -restore*/
  chSysLock();
  /*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/
  while (p->next != (event_listener_t *)esp) {
  /*lint -restore*/
    if (p->next == elp) {
      p->next = elp->next;
      break;
    }
    p = p->next;
  }
  chSysUnlock();
}

/**
 * @brief   Performs a wait operation on a semaphore with timeout specification.
 *
 * @param[in] sp        pointer to a @p Semaphore structure
 * @param[in] time      the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_IMMEDIATE immediate timeout.
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              A message specifying how the invoking thread has been
 *                      released from the semaphore.
 * @retval RDY_OK       if the thread has not stopped on the semaphore or the
 *                      semaphore has been signaled.
 * @retval RDY_RESET    if the semaphore has been reset using @p chSemReset().
 * @retval RDY_TIMEOUT  if the semaphore has not been signaled or reset within
 *                      the specified timeout.
 *
 * @sclass
 */
msg_t chSemWaitTimeoutS(Semaphore *sp, systime_t time) {

  chDbgCheck(sp != NULL, "chSemWaitTimeoutS");

  chDbgAssert(((sp->s_cnt >= 0) && isempty(&sp->s_queue)) ||
              ((sp->s_cnt < 0) && notempty(&sp->s_queue)),
              "chSemWaitTimeoutS(), #1",
              "inconsistent semaphore");

  if (--sp->s_cnt < 0) {
    if (TIME_IMMEDIATE == time) {
      sp->s_cnt++;
      return RDY_TIMEOUT;
    }
    currp->p_u.wtobjp = sp;
    sem_insert(currp, &sp->s_queue);
    return chSchGoSleepTimeoutS(THD_STATE_WTSEM, time);
  }
  return RDY_OK;
}

/**
 * @brief   Starts an ADC conversion.
 * @details Starts an asynchronous conversion operation.
 * @post    The callbacks associated to the conversion group will be invoked
 *          on buffer fill and error events.
 * @note    The buffer is organized as a matrix of M*N elements where M is the
 *          channels number configured into the conversion group and N is the
 *          buffer depth. The samples are sequentially written into the buffer
 *          with no gaps.
 *
 * @param[in] adcp      pointer to the @p ADCDriver object
 * @param[in] grpp      pointer to a @p ADCConversionGroup object
 * @param[out] samples  pointer to the samples buffer
 * @param[in] depth     buffer depth (matrix rows number). The buffer depth
 *                      must be one or an even number.
 *
 * @iclass
 */
void adcStartConversionI(ADCDriver *adcp,
                         const ADCConversionGroup *grpp,
                         adcsample_t *samples,
                         size_t depth) {

  chDbgCheckClassI();
  chDbgCheck((adcp != NULL) && (grpp != NULL) && (samples != NULL) &&
             ((depth == 1) || ((depth & 1) == 0)),
             "adcStartConversionI");
  chDbgAssert((adcp->state == ADC_READY) ||
              (adcp->state == ADC_COMPLETE) ||
              (adcp->state == ADC_ERROR),
              "adcStartConversionI(), #1", "not ready");

  adcp->samples  = samples;
  adcp->depth    = depth;
  adcp->grpp     = grpp;
  adcp->state    = ADC_ACTIVE;
  adc_lld_start_conversion(adcp);
}

/**
 * @brief   Inserts a thread in the Ready List.
 * @details The thread is positioned behind all threads with higher or equal
 *          priority.
 * @pre     The thread must not be already inserted in any list through its
 *          @p p_next and @p p_prev or list corruption would occur.
 * @post    This function does not reschedule so a call to a rescheduling
 *          function must be performed before unlocking the kernel. Note that
 *          interrupt handlers always reschedule on exit so an explicit
 *          reschedule must not be performed in ISRs.
 *
 * @param[in] tp        the thread to be made ready
 * @return              The thread pointer.
 *
 * @iclass
 */
thread_t *chSchReadyI(thread_t *tp) {
  thread_t *cp;

  chDbgCheckClassI();
  chDbgCheck(tp != NULL);
  chDbgAssert((tp->p_state != CH_STATE_READY) &&
              (tp->p_state != CH_STATE_FINAL),
              "invalid state");

  tp->p_state = CH_STATE_READY;
  cp = (thread_t *)&ch.rlist.r_queue;
  do {
    cp = cp->p_next;
  } while (cp->p_prio >= tp->p_prio);
  /* Insertion on p_prev.*/
  tp->p_next = cp;
  tp->p_prev = cp->p_prev;
  tp->p_prev->p_next = cp->p_prev = tp;

  return tp;
}

/**
 * @brief   Brings the driver in a state safe for card removal.
 *
 * @param[in] mmcp      pointer to the @p MMCDriver object
 * @return              The operation status.
 *
 * @retval CH_SUCCESS   the operation succeeded and the driver is now
 *                      in the @p MMC_INSERTED state.
 * @retval CH_FAILED    the operation failed.
 *
 * @api
 */
bool_t mmcDisconnect(MMCDriver *mmcp) {

  chDbgCheck(mmcp != NULL, "mmcDisconnect");

  chSysLock();
  chDbgAssert((mmcp->state == BLK_ACTIVE) || (mmcp->state == BLK_READY),
              "mmcDisconnect(), #1", "invalid state");
  if (mmcp->state == BLK_ACTIVE) {
    chSysUnlock();
    return CH_SUCCESS;
  }
  mmcp->state = BLK_DISCONNECTING;
  chSysUnlock();

  /* Wait for the pending write operations to complete.*/
  sync(mmcp);

  spiStop(mmcp->config->spip);
  mmcp->state = BLK_ACTIVE;
  return CH_SUCCESS;
}

/**
 * @brief   Allocates an object from a memory pool.
 * @pre     The memory pool must already be initialized.
 *
 * @param[in] mp        pointer to a @p memory_pool_t structure
 * @return              The pointer to the allocated object.
 * @retval NULL         if pool is empty.
 *
 * @iclass
 */
void *chPoolAllocI(memory_pool_t *mp) {
  void *objp;

  chDbgCheckClassI();
  chDbgCheck(mp != NULL);

  objp = mp->next;
  /*lint -save -e9013 [15.7] There is no else because it is not needed.*/
  if (objp != NULL) {
    mp->next = mp->next->next;
  }
  else if (mp->provider != NULL) {
    objp = mp->provider(mp->object_size, mp->align);

    chDbgAssert(MEM_IS_ALIGNED(objp, mp->align),
                "returned object not aligned");
  }
  /*lint -restore*/

  return objp;
}