void vMBMasterPortTimersDisable()
{
	palClearPad(GPIOC, GPIOC_PIN9);
	//palClearPad(GPIOC, GPIOC_PIN10);
	chSysLockFromISR();
    chVTResetI(&vt35);
	chVTResetI(&vtdelay);
	chVTResetI(&vtout);
	chSysUnlockFromISR();
}

/**
 * @brief   Master transmission.
 *
 * @param[in] i2cp      pointer to the @p I2CDriver object
 * @param[in] addr      slave device address (7 bits) without R/W bit
 * @param[in] txbuf     transmit data buffer pointer
 * @param[in] txbytes   number of bytes to be transmitted
 * @param[out] rxbuf     receive data buffer pointer
 * @param[in] rxbytes   number of bytes to be received
 * @param[in] timeout   the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 *
 * @notapi
 */
msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr, 
                                       const uint8_t *txbuf, size_t txbytes, 
                                       uint8_t *rxbuf, const uint8_t rxbytes, 
                                       systime_t timeout) {
  VirtualTimer vt;

  /* Global timeout for the whole operation.*/
  if (timeout != TIME_INFINITE)
    chVTSetI(&vt, timeout, i2c_lld_safety_timeout, (void *)i2cp);

  i2cp->addr = addr;
  i2cp->txbuf = txbuf;
  i2cp->txbytes = txbytes;
  i2cp->txidx = 0;
  i2cp->rxbuf = rxbuf;
  i2cp->rxbytes = rxbytes;
  i2cp->rxidx = 0;

  bscdevice_t *device = i2cp->device;
  device->slaveAddress = addr;
  device->dataLength = txbytes;
  device->status = CLEAR_STATUS;

  /* Enable Interrupts and start transfer.*/
  device->control |= (BSC_INTT | BSC_INTD | START_WRITE);

  /* Is this really needed? there is an outer lock already */
  chSysLock();

  i2cp->thread = chThdSelf();
  chSchGoSleepS(THD_STATE_SUSPENDED);
  if ((timeout != TIME_INFINITE) && chVTIsArmedI(&vt))
    chVTResetI(&vt);

  chSysUnlock();

  msg_t status = chThdSelf()->p_u.rdymsg;

  if (status == RDY_OK && rxbytes > 0) {
    /* The TIMEOUT_INFINITE prevents receive from setting up it's own timer.*/
    status = i2c_lld_master_receive_timeout(i2cp, addr, rxbuf, 
					    rxbytes, TIME_INFINITE);
    if ((timeout != TIME_INFINITE) && chVTIsArmedI(&vt))
      chVTResetI(&vt);
  }

  return status;
}

/**
 * @brief               Stops the interrupts and timers so sensors will stop 
 *                      beeing serviced. The queued sensor reads are lost as
 *                      well.
 * 
 * @param[in] srdp      Pointer to the SensorReadDriver object.
 * 
 * @return              The operation status.
 * @retval MSG_OK       The disabling of interrupts and starting of the
 *                      virtual timers was successful.
 * @retval MSG_RESET    The driver was not in the correct state.
 * 
 * @api
 */
msg_t SensorReadStop(SensorReadDriver *srdp)
{
    size_t i;

    chDbgCheck(srdp != NULL);

    if (srdp->state == SRD_STARTED)
    {
        osalSysLock();

        /* Disable interrupts for interrupt driven sensors */
        for (i = 0; i < srdp->interrupt_sensor_cnt; i++)
            extChannelDisableI(&SRD_EXT_DRIVER,
                              srdp->interrupt_sensor_ptr[i].interrupt_channel);

        /* Reset timers for polled driven sensors */
        for (i = 0; i < srdp->polled_sensor_cnt; i++)
            chVTResetI(srdp->polled_sensor_ptr[i].polling_vt);

        osalSysUnlock();

        /* Reset the mailbox */
        chMBReset(&srdp->srd_mailbox);

        /* Everything OK, transverse the state */
        srdp->state = SRD_STOPPED;

        return MSG_OK;    
    }
    else
        return MSG_RESET;
}

void App_t::LedBlink(uint32_t Duration_ms) {
    PinSet(LED_GPIO, LED_PIN);
    chSysLock()
    if(chVTIsArmedI(&TmrLed)) chVTResetI(&TmrLed);
    chVTSetI(&TmrLed, MS2ST(Duration_ms), LedTmrCallback, nullptr);
    chSysUnlock();
}

static msg_t spi_thread(void *p) {
  unsigned i;
  SPIDriver *spip = (SPIDriver *)p;
  VirtualTimer vt;
  uint8_t txbuf[256];
  uint8_t rxbuf[256];

  /* Prepare transmit pattern.*/
  for (i = 0; i < sizeof(txbuf); i++)
    txbuf[i] = (uint8_t)i;

  /* Continuous transmission.*/
  while (TRUE) {
    /* Starts a VT working as watchdog to catch a malfunction in the SPI
       driver.*/
    chSysLock();
    chVTSetI(&vt, MS2ST(10), tmo, NULL);
    chSysUnlock();

    spiExchange(spip, sizeof(txbuf), txbuf, rxbuf);

    /* Stops the watchdog.*/
    chSysLock();
    if (chVTIsArmedI(&vt))
      chVTResetI(&vt);
    chSysUnlock();
  }
}

static void doScheduleForLater(scheduling_s *scheduling, int delayUs, schfunc_t callback, void *param) {
	int delaySt = MY_US2ST(delayUs);
	if (delaySt <= 0) {
		/**
		 * in case of zero delay, we should invoke the callback
		 */
		callback(param);
		return;
	}

	bool alreadyLocked = lockAnyContext();
	scheduling->callback = callback;
	scheduling->param = param;
	int isArmed = chVTIsArmedI(&scheduling->timer);
	if (isArmed) {
		/**
		 * timer reuse is normal for example in case of sudden RPM increase
		 */
		chVTResetI(&scheduling->timer);
	}

#if EFI_SIMULATOR
	if (callback == (schfunc_t)&seTurnPinLow) {
		printf("setTime cb=seTurnPinLow p=%d\r\n", (int)param);
	} else {
//		printf("setTime cb=%d p=%d\r\n", (int)callback, (int)param);
	}
#endif /* EFI_SIMULATOR */

	chVTSetI(&scheduling->timer, delaySt, (vtfunc_t)timerCallback, scheduling);
	if (!alreadyLocked) {
		unlockAnyContext();
	}
}

/**
 * @brief Stops the timer.
 * @details If the timer was already stopped then the function has no effect.
 *
 * @param etp pointer to an initialized @p EvTimer structure.
 */
void evtStop(EvTimer *etp) {

  chSysLock();

  if (chVTIsArmedI(&etp->et_vt))
    chVTResetI(&etp->et_vt);

  chSysUnlock();
}

/*
 * This callback is invoked when a transmission has physically completed.
 */
static void txend2(UARTDriver *uartp) {

  (void)uartp;
  palSetPad(GPIOD, GPIOD_LED5);
  chSysLockFromISR();
  chVTResetI(&vt5);
  chVTSetI(&vt5, MS2ST(200), led5off, NULL);
  chSysUnlockFromISR();
}

/*
 * This callback is invoked when a transmission has physically completed.
 */
static void txend2(UARTDriver *uartp) {

  (void)uartp;
  palClearPad(GPIOB, GPIOB_LED4);
  chSysLockFromISR();
  chVTResetI(&vt1);
  chVTDoSetI(&vt1, MS2ST(5000), restart, NULL);
  chSysUnlockFromISR();
}

void vMBMasterPortTimersConvertDelayEnable()
{
    /* Set current timer mode, don't change it.*/
    vMBMasterSetCurTimerMode(MB_TMODE_CONVERT_DELAY);

    chSysLockFromISR();
	chVTResetI(&vtdelay);
	chVTSetI(&vtdelay, MS2ST((uint32_t)MB_MASTER_DELAY_MS_CONVERT), timer_timeout_ind, NULL);
	chSysUnlockFromISR();
}

void vMBMasterPortTimersRespondTimeoutEnable()
{
	//chprintf((BaseSequentialStream *)&itm_port, "%s\n", "TimeOut Enable");
	//palSetPad(GPIOC, GPIOC_PIN10);
    /* Set current timer mode, don't change it.*/
    chSysLockFromISR();
	chVTResetI(&vtout);
	chVTSetI(&vtout, MS2ST((uint32_t)MB_MASTER_TIMEOUT_MS_RESPOND), timer_timeout_ind, NULL);
	chSysUnlockFromISR();
}

/*
 * This callback is invoked when a transmission has physically completed.
 */
static void txend2(UARTDriver *uartp) {

  (void)uartp;
  palClearPad(GPIOE, GPIOE_LED3_RED);
  chSysLockFromIsr();
  if (chVTIsArmedI(&vt1))
    chVTResetI(&vt1);
  chVTSetI(&vt1, MS2ST(5000), restart, NULL);
  chSysUnlockFromIsr();
}

/*
 * This callback is invoked when a transmission has physically completed.
 */
static void txend2(UARTDriver *uartp) {

  (void)uartp;
  palSetPad(IOPORT3, GPIOC_LED);
  chSysLockFromIsr();
  if (chVTIsArmedI(&vt1))
    chVTResetI(&vt1);
  chVTSetI(&vt1, MS2ST(5000), restart, NULL);
  chSysUnlockFromIsr();
}

/*
 * This callback is invoked when a receive buffer has been completely written.
 */
static void rxend(UARTDriver *uartp) {

  (void)uartp;

  /* Flashing the LED each time a character is received.*/
  palSetPad(GPIOD, GPIOD_LED3);
  chSysLockFromISR();
  chVTResetI(&vt3);
  chVTSetI(&vt3, MS2ST(200), led3off, NULL);
  chSysUnlockFromISR();
}

/*
 * This callback is invoked when a character is received but the application
 * was not ready to receive it, the character is passed as parameter.
 */
static void rxchar(UARTDriver *uartp, uint16_t c) {

  (void)uartp;
  (void)c;
  /* Flashing the LED each time a character is received.*/
  palSetPad(GPIOB, GPIOB_LED4);
  chSysLockFromISR();
  chVTResetI(&vt2);
  chVTDoSetI(&vt2, MS2ST(200), ledoff, NULL);
  chSysUnlockFromISR();
}

void vMBMasterPortTimersT35Enable()
{
	//chprintf((BaseSequentialStream *)&itm_port, "%s\n", "T35 Enable");
    /* Set current timer mode, don't change it.*/
    vMBMasterSetCurTimerMode(MB_TMODE_T35);
	palSetPad(GPIOC, GPIOC_PIN9);

	chSysLockFromISR();
	chVTResetI(&vt35);
	chVTSetI(&vt35, US2ST((uint32_t)2000), timer_timeout_ind, NULL);
	chSysUnlockFromISR();
}

/**
 * @brief   Receives data via the I2C bus as master.
 * @details Number of receiving bytes must be more than 1 because of stm32
 *          hardware restrictions.
 *
 * @param[in] i2cp      pointer to the @p I2CDriver object
 * @param[in] addr      slave device address
 * @param[out] rxbuf    pointer to the receive buffer
 * @param[in] rxbytes   number of bytes to be received
 * @param[in] timeout   the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              The operation status.
 * @retval RDY_OK       if the function succeeded.
 * @retval RDY_RESET    if one or more I2C errors occurred, the errors can
 *                      be retrieved using @p i2cGetErrors().
 * @retval RDY_TIMEOUT  if a timeout occurred before operation end. <b>After a
 *                      timeout the driver must be stopped and restarted
 *                      because the bus is in an uncertain state</b>.
 *
 * @notapi
 */
msg_t i2c_lld_master_receive_timeout(I2CDriver *i2cp, i2caddr_t addr,
                                     uint8_t *rxbuf, size_t rxbytes,
                                     systime_t timeout) {
  I2C_TypeDef *dp = i2cp->i2c;
  VirtualTimer vt;
  msg_t rdymsg;

  chDbgCheck((rxbytes > 1), "i2c_lld_master_receive_timeout");

  /* Global timeout for the whole operation.*/
  chVTSetI(&vt, timeout, i2c_lld_safety_timeout, (void *)i2cp);

  /* Releases the lock from high level driver.*/
  chSysUnlock();

  /* Initializes driver fields, LSB = 1 -> receive.*/
  i2cp->addr = (addr << 1) | 0x01;
  i2cp->errors = 0;

  /* RX DMA setup.*/
  dmaStreamSetMemory0(i2cp->dmarx, rxbuf);
  dmaStreamSetTransactionSize(i2cp->dmarx, rxbytes);

  /* Waits until BUSY flag is reset and the STOP from the previous operation
     is completed, alternatively for a timeout condition.*/
  while ((dp->SR2 & I2C_SR2_BUSY) || (dp->CR1 & I2C_CR1_STOP)) {
    if (!chVTIsArmedI(&vt)) {
      chSysLock();
      return RDY_TIMEOUT;
    }
  }

  /* This lock will be released in high level driver.*/
  chSysLock();

  /* Atomic check on the timer in order to make sure that a timeout didn't
     happen outside the critical zone.*/
  if (!chVTIsArmedI(&vt))
    return RDY_TIMEOUT;

  /* Starts the operation.*/
  dp->CR2 |= I2C_CR2_ITEVTEN;
  dp->CR1 |= I2C_CR1_START | I2C_CR1_ACK;

  /* Waits for the operation completion or a timeout.*/
  i2cp->thread = chThdSelf();
  chSchGoSleepS(THD_STATE_SUSPENDED);
  rdymsg = chThdSelf()->p_u.rdymsg;
  if (rdymsg != RDY_TIMEOUT)
    chVTResetI(&vt);

  return rdymsg;
}

/*
 * This callback is invoked when a character is received but the application
 * was not ready to receive it, the character is passed as parameter.
 */
static void rxchar(UARTDriver *uartp, uint16_t c) {

  (void)uartp;
  (void)c;
  /* Flashing the LED each time a character is received.*/
  palSetPad(GPIOE, GPIOE_LED3_RED);
  chSysLockFromIsr();
  if (chVTIsArmedI(&vt2))
    chVTResetI(&vt2);
  chVTSetI(&vt2, MS2ST(200), ledoff, NULL);
  chSysUnlockFromIsr();
}

/* Triggered when the button is pressed or released. The LED4 is set to ON.*/
static void extcb1(EXTDriver *extp, expchannel_t channel) {
    static virtual_timer_t vt4;

    (void)extp;
    (void)channel;
    palSetPad(GPIOA, GPIOA_LED_GREEN);
    chSysLockFromISR();
    chVTResetI(&vt4);
    /* LED4 set to OFF after 200mS.*/
    chVTSetI(&vt4, MS2ST(200), led4off, NULL);
    chSysUnlockFromISR();
}

static void bmk10_execute(void) {
  static VirtualTimer vt1, vt2;
  uint32_t n = 0;

  test_wait_tick();
  test_start_timer(1000);
  do {
    chSysLock();
    chVTSetI(&vt1, 1, tmo, NULL);
    chVTSetI(&vt2, 10000, tmo, NULL);
    chVTResetI(&vt1);
    chVTResetI(&vt2);
    chSysUnlock();
    n++;
#if defined(SIMULATOR)
    ChkIntSources();
#endif
  } while (!test_timer_done);
  test_print("--- Score : ");
  test_printn(n * 2);
  test_println(" timers/S");
}