static msg_t lsm303_acc_update_thread(void *p) {
  I2CDriver *i2cp = (I2CDriver *)p;

  while (TRUE) {
    msg_t msg;

    /* Waiting for the IRQ to happen.*/

    chSysLock();
    acctp = chThdSelf();
    chSchGoSleepS(THD_STATE_SUSPENDED);
    msg = chThdSelf()->p_u.rdymsg;
    chSysUnlock();

    /* If data ready, update all axis.*/
    if (msg == LSM303_ACC_DATA_READY) {
      lsm303_acc_update(i2cp);
      while (status_a != 0x00) {
        lsm303_acc_update(i2cp);
      }
//      chprintf((BaseChannel *)&SERIAL_DRIVER, "ACC T: %d X: %d Y: %d Z: %d\r\n",
//                 acc_update_time, acc_data.x, acc_data.y, acc_data.z);
    }
  }

  return RDY_OK;
}

static msg_t lsm303_mag_update_thread(void *p) {
  I2CDriver *i2cp = (I2CDriver *)p;

  while (TRUE) {
    msg_t msg;

    /* Waiting for the IRQ to happen.*/

    chSysLock();
    magtp = chThdSelf();
    chSchGoSleepS(THD_STATE_SUSPENDED);
    msg = chThdSelf()->p_u.rdymsg;
    chSysUnlock();

    /* If data ready, update all axis.*/
    if (msg == LSM303_MAG_DATA_READY) {
      lsm303_mag_update(i2cp);
//      if (status_m != 0x00) {
//        lsm303_mag_update(i2cp);
//      }
    }
  }

  return RDY_OK;
}

static msg_t thread1(BaseSequentialStream *chp) {
  systime_t time = chTimeNow();    
  /* 
   * Thread-specific parameters
  */
  chRegSetThreadName("t1");
  static int d = 300;
  static int w = 75000;
  volatile uint32_t i, n;
  while(1) {
  if(chThdShouldTerminate())
    return 0;
  palSetPad(GPIOD, GPIOD_LED3);  
  /* 
   * Deadline for current execution of this task
  */ 
  time += d;            
  chprintf(chp, "%s  N  %d   %d   %d  %d\r\n",  chRegGetThreadName(chThdSelf()), chThdGetPriority(),
						chTimeNow(), time, chThdGetTicks(chThdSelf()));
  /* 
   * Do some "work"
  */
  for(i = 0; i < w; i ++) { 
    n = i / 3;
  }
  chprintf(chp, "%s  X  %d   %d   %d  %d\r\n",  chRegGetThreadName(chThdSelf()), chThdGetPriority(),
						chTimeNow(), time, chThdGetTicks(chThdSelf()));
  palClearPad(GPIOD, GPIOD_LED3);   
  /* 
   * Yield control of CPU until the deadline (which is also beginning of next period)
  */
  chThdSleepUntil(time);
  } 
  return 0;
}

int ThreadWait::sleep() {
	chSysLock();
	thread_to_wake = chThdSelf();
	chSchGoSleepS(THD_STATE_SUSPENDED);
	const auto result = chThdSelf()->p_u.rdymsg;
	chSysUnlock();
	return result;
}

uint8_t i2c_t::CmdWriteRead(uint8_t Addr,
        uint8_t *WPtr, uint8_t WLength,
        uint8_t *RPtr, uint8_t RLength) {
    if(IBusyWait() != OK) return FAILURE;
    // Clear flags
    ii2c->SR1 = 0;
    while(RxIsNotEmpty()) (void)ii2c->DR;   // Read DR until it empty
    ClearAddrFlag();
    // Start transmission
    SendStart();
    if(WaitEv5() != OK) return FAILURE;
    SendAddrWithWrite(Addr);
    if(WaitEv6() != OK) { SendStop(); return FAILURE; }
    // Start TX DMA if needed
    if(WLength != 0) {
        if(WaitEv8() != OK) return FAILURE;
        dmaStreamSetMemory0(PDmaTx, WPtr);
        dmaStreamSetTransactionSize(PDmaTx, WLength);
        PRequestingThread = chThdSelf();
        dmaStreamEnable(PDmaTx);
        chSysLock();
        chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
        chSysUnlock();
        dmaStreamDisable(PDmaTx);
    }
    // Read if needed
    if(RLength != 0) {
        if(WaitEv8() != OK) return FAILURE;
        // Send repeated start
        SendStart();
        if(WaitEv5() != OK) return FAILURE;
        SendAddrWithRead(Addr);
        if(WaitEv6() != OK) { SendStop(); return FAILURE; }
        // If number of data to be read is 1, then DMA cannot be used
        if(RLength == 1) {
            AckDisable();
            SendStop();
            if(WaitRx() != OK) return FAILURE;
            *RPtr = ReceiveData();
            if(WaitStop() != OK) return FAILURE;
            return OK;
        }
        else {  // more than 1 byte, use DMA
            AckEnable();
            dmaStreamSetMemory0(PDmaRx, RPtr);
            dmaStreamSetTransactionSize(PDmaRx, RLength);
            DmaLastTransferSet(); // Inform DMA that this is last transfer => do not ACK last byte
            PRequestingThread = chThdSelf();
            dmaStreamEnable(PDmaRx);
            chSysLock();
            chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
            chSysUnlock();
            dmaStreamDisable(PDmaRx);
        } // if lng==1
    } // if != 0
    SendStop();
    return OK;
}

/**
 * @brief   Transmits data via the I2C bus as master.
 * @details When performing reading through write you can not write more than
 *          3 bytes of data to I2C slave. This is SAM7 platform limitation.
 *
 * @param[in] i2cp      pointer to the @p I2CDriver object
 * @param[in] addr      slave device address
 * @param[in] txbuf     pointer to the transmit buffer
 * @param[in] txbytes   number of bytes to be transmitted
 * @param[out] rxbuf    pointer to the receive buffer
 * @param[in] rxbytes   number of bytes to be received
 * @param[in] timeout   this value is ignored on SAM7 platform.
 *                      .
 * @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().
 *
 * @notapi
 */
msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
                                      const uint8_t *txbuf, size_t txbytes,
                                      uint8_t *rxbuf, size_t rxbytes,
                                      systime_t timeout) {
  (void)timeout;

  /* SAM7 specific check */
  chDbgCheck(((rxbytes == 0) || ((txbytes > 0) && (txbytes < 4) && (rxbuf != NULL))),
             "i2c_lld_master_transmit_timeout");

  /* prepare to read through write operation */
  if (rxbytes > 0){
    AT91C_BASE_TWI->TWI_MMR |= txbytes << 8;

    /* store internal slave address in TWI_IADR registers */
    AT91C_BASE_TWI->TWI_IADR = 0;
    while (txbytes > 0){
      AT91C_BASE_TWI->TWI_IADR = (AT91C_BASE_TWI->TWI_IADR << 8);
      AT91C_BASE_TWI->TWI_IADR |= *(txbuf++);
      txbytes--;
    }
    /* Internal address of I2C slave was set in special Atmel registers.
     * Now we must call read function. The I2C cell automatically sends
     * bytes from IADR register to bus and issues repeated start. */
    return i2c_lld_master_receive_timeout(i2cp, addr, rxbuf, rxbytes, timeout);
  }
  else{
    if (txbytes == 1){
      /* In single data byte master read or write, the START and STOP
       * must both be set. */
      AT91C_BASE_TWI->TWI_CR |= AT91C_TWI_STOP;
    }
    AT91C_BASE_TWI->TWI_MMR  = 0;
    AT91C_BASE_TWI->TWI_MMR |= addr << 16;

    /* enable just needed interrupts */
    AT91C_BASE_TWI->TWI_IER = AT91C_TWI_TXRDY | AT91C_TWI_NACK;

    /* correct size and pointer because first byte will be written
     * for issue start condition */
    i2cp->txbuf   = txbuf + 1;
    i2cp->txbytes = txbytes - 1;

    /* According to datasheet there is no need to set START manually
     * we just need to write first byte in THR */
    AT91C_BASE_TWI->TWI_THR = txbuf[0];

    /* Waits for the operation completion.*/
    i2cp->thread = chThdSelf();
    chSchGoSleepS(THD_STATE_SUSPENDED);

    return chThdSelf()->p_u.rdymsg;
  }
}

uint8_t i2c_t::CmdWriteRead(uint8_t Addr,
        uint8_t *WPtr, uint8_t WLength,
        uint8_t *RPtr, uint8_t RLength) {
    if(IBusyWait() != OK) return FAILURE;
    // Clear flags
    ii2c->SR1 = 0;
    while(RxIsNotEmpty()) (void)ii2c->DR;   // Read DR until it empty
    ClearAddrFlag();
    // Start transmission
    SendStart();
    if(WaitEv5() != OK) return FAILURE;
    SendAddrWithWrite(Addr);
    if(WaitEv6() != OK) { SendStop(); return FAILURE; }
    ClearAddrFlag();
    // Start TX DMA if needed
    if(WLength != 0) {
        if(WaitEv8() != OK) return FAILURE;
        dmaStreamSetMemory0(PDmaTx, WPtr);
        dmaStreamSetMode   (PDmaTx, I2C_DMATX_MODE);
        dmaStreamSetTransactionSize(PDmaTx, WLength);
        chSysLock();
        PRequestingThread = chThdSelf();
        dmaStreamEnable(PDmaTx);
        chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
        chSysUnlock();
        dmaStreamDisable(PDmaTx);
    }
    // Read if needed
    if(RLength != 0) {
        if(WaitEv8() != OK) return FAILURE;
        // Send repeated start
        SendStart();
        if(WaitEv5() != OK) return FAILURE;
        SendAddrWithRead(Addr);
        if(WaitEv6() != OK) { SendStop(); return FAILURE; }
        // If single byte is to be received, disable ACK before clearing ADDR flag
        if(RLength == 1) AckDisable();
        else AckEnable();
        ClearAddrFlag();
        dmaStreamSetMemory0(PDmaRx, RPtr);
        dmaStreamSetMode   (PDmaRx, I2C_DMARX_MODE);
        dmaStreamSetTransactionSize(PDmaRx, RLength);
        DmaLastTransferSet(); // Inform DMA that this is last transfer => do not ACK last byte
        chSysLock();
        PRequestingThread = chThdSelf();
        dmaStreamEnable(PDmaRx);
        chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
        chSysUnlock();
        dmaStreamDisable(PDmaRx);
    } // if != 0
    else WaitBTF(); // if nothing to read, just stop
    SendStop();
    return OK;
}

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

/**
 * @brief   Receives data via the I2C bus as master.
 * @details Number of receiving bytes must be more than 1 on STM32F1x. This is
 *          hardware restriction.
 *
 * @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) {

    (void)i2cp;
    (void)addr;
    (void)rxbuf;
    (void)rxbytes;
    (void)timeout;
    uint32_t status;
    uint32_t stop_sent;
    i2cdef_t i2c = i2cp->i2c;

    /* Set read mode, slave address and 3 internal address byte lengths */
    i2c->TWI_MMR = 0;
    i2c->TWI_MMR = TWI_MMR_MREAD | TWI_MMR_DADR(addr->chip) | ((addr->len << TWI_MMR_IADRSZ_Pos) & TWI_MMR_IADRSZ_Msk);

    /* Set internal address for remote chip */
    i2c->TWI_IADR = 0;
    i2c->TWI_IADR = twi_mk_addr(addr->addr, addr->len);




    /* Send a START condition */
    if (rxbytes = 1) {
        i2c->TWI_CR = TWI_CR_START | TWI_CR_STOP;
        while (!(i2c->TWI_SR & TWI_SR_RXRDY));
        *rxbuf = i2c->TWI_RHR;

    } else {
        i2c->TWI_PTCR = TWI_PTCR_RXTDIS | TWI_PTCR_TXTDIS;


        i2c->TWI_IER = TWI_IER_ENDRX | TWI_IER_NACK | TWI_IER_OVRE;

        chSysLock();
        i2cp->thread = chThdSelf();
        i2c->TWI_RNPR = 0;
        i2c->TWI_RNCR = 0;
        i2c->TWI_RPR = (uint32_t) rxbuf;
        i2c->TWI_RCR = rxbytes - 2;
        i2c->TWI_PTCR = TWI_PTCR_RXTEN;
        i2cp->curbuf = rxbuf + (rxbytes - 2);

        i2c->TWI_CR = TWI_CR_START;
        chSchGoSleepS(THD_STATE_SUSPENDED);

        return chThdSelf()->p_u.rdymsg;
    }

    return RDY_OK;
}

/**
 * @brief   CPU pulse.
 * @note    The current implementation is not totally reliable.
 *
 * @param[in] duration      CPU pulse duration in milliseconds
 */
void test_cpu_pulse(unsigned duration) {
  systime_t start, end, now;

  start = chThdSelf()->p_time;
  end = start + MS2ST(duration);
  do {
    now = chThdSelf()->p_time;
#if defined(SIMULATOR)
    ChkIntSources();
#endif
  }
  while (end > start ? (now >= start) && (now < end) :
                       (now >= start) || (now < end));
}

/**
 * @brief   Performs an ADC conversion.
 * @details Performs a synchronous conversion operation.
 * @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.
 * @return              The operation result.
 * @retval RDY_OK       Conversion finished.
 * @retval RDY_RESET    The conversion has been stopped using
 *                      @p acdStopConversion() or @p acdStopConversionI(),
 *                      the result buffer may contain incorrect data.
 * @retval RDY_TIMEOUT  The conversion has been stopped because an hardware
 *                      error.
 *
 * @api
 */
msg_t adcConvert(ADCDriver *adcp,
                 const ADCConversionGroup *grpp,
                 adcsample_t *samples,
                 size_t depth) {
  msg_t msg;

  chSysLock();
  chDbgAssert(adcp->thread == NULL, "adcConvert(), #1", "already waiting");
  adcStartConversionI(adcp, grpp, samples, depth);
  adcp->thread = chThdSelf();
  chSchGoSleepS(THD_STATE_SUSPENDED);
  msg = chThdSelf()->p_u.rdymsg;
  chSysUnlock();
  return msg;
}

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

static void pwmpcb_fast(PWMDriver *pwmp) {
	efiAssertVoid(getRemainingStack(chThdSelf())> 32, "lwStAdcFast");
#if EFI_INTERNAL_ADC
	(void) pwmp;

	/*
	 * Starts an asynchronous ADC conversion operation, the conversion
	 * will be executed in parallel to the current PWM cycle and will
	 * terminate before the next PWM cycle.
	 */
	chSysLockFromIsr()
	;
	if (ADC_FAST_DEVICE.state != ADC_READY &&
	ADC_FAST_DEVICE.state != ADC_COMPLETE &&
	ADC_FAST_DEVICE.state != ADC_ERROR) {
		fastAdc.errorsCount++;
		// todo: when? why? firmwareError("ADC fast not ready?");
		chSysUnlockFromIsr()
		;
		return;
	}
	adcStartConversionI(&ADC_FAST_DEVICE, &adcgrpcfg_fast, fastAdc.samples, ADC_BUF_DEPTH_FAST);
	chSysUnlockFromIsr()
	;
	fastAdc.conversionCount++;
#endif
}

static void pwmpcb_slow(PWMDriver *pwmp) {
	efiAssertVoid(getRemainingStack(chThdSelf())> 32, "lwStAdcSlow");

#if EFI_INTERNAL_ADC
	(void) pwmp;

	/* Starts an asynchronous ADC conversion operation, the conversion
	 will be executed in parallel to the current PWM cycle and will
	 terminate before the next PWM cycle.*/
	slowAdc.conversionCount++;
	chSysLockFromIsr()
	;
	if (ADC_SLOW_DEVICE.state != ADC_READY &&
	ADC_SLOW_DEVICE.state != ADC_COMPLETE &&
	ADC_SLOW_DEVICE.state != ADC_ERROR) {
		// todo: why and when does this happen? firmwareError("ADC slow not ready?");
		slowAdc.errorsCount++;
		chSysUnlockFromIsr()
		;
		return;
	}
	adcStartConversionI(&ADC_SLOW_DEVICE, &adcgrpcfgSlow, slowAdc.samples, ADC_BUF_DEPTH_SLOW);
	chSysUnlockFromIsr()
	;
#endif
}

void appendPrintf(Logging *logging, const char *fmt, ...) {
	efiAssertVoid(getRemainingStack(chThdSelf()) > 128, "lowstck#4");
	va_list ap;
	va_start(ap, fmt);
	vappendPrintf(logging, fmt, ap);
	va_end(ap);
}

void vappendPrintf(Logging *logging, const char *fmt, va_list arg) {
	efiAssertVoid(getRemainingStack(chThdSelf()) > 16, "stack#5b");
	if (!intermediateLoggingBufferInited) {
		firmwareError("intermediateLoggingBufferInited not inited!");
		return;
	}
	int is_locked = isLocked();
	int icsr_vectactive = isIsrContext();
	if (is_locked) {
		vappendPrintfI(logging, fmt, arg);
	} else {
		if (icsr_vectactive == 0) {
			chSysLock()
			;
			vappendPrintfI(logging, fmt, arg);
			chSysUnlock()
			;
		} else {
			chSysLockFromIsr()
			;
			vappendPrintfI(logging, fmt, arg);
			chSysUnlockFromIsr()
			;
		}
	}
}

void readFromFlash(void) {
	efiAssertVoid(getRemainingStack(chThdSelf()) > 256, "read f");
	printMsg(logger, "readFromFlash()");
	flashRead(FLASH_ADDR, (char *) &persistentState, PERSISTENT_SIZE);

	persisted_configuration_state_e result;

	if (!isValidCrc(&persistentState)) {
		result = CRC_FAILED;
		resetConfigurationExt(logger, DEFAULT_ENGINE_TYPE PASS_ENGINE_PARAMETER);
	} else if (persistentState.version != FLASH_DATA_VERSION || persistentState.size != PERSISTENT_SIZE) {
		result = INCOMPATIBLE_VERSION;
		resetConfigurationExt(logger, engineConfiguration->engineType PASS_ENGINE_PARAMETER);
	} else {
		/**
		 * At this point we know that CRC and version number is what we expect. Safe to assume it's a valid configuration.
		 */
		result = OK;
		applyNonPersistentConfiguration(logger PASS_ENGINE_PARAMETER);
	}
	// we can only change the state after the CRC check
	engineConfiguration->firmwareVersion = getRusEfiVersion();

	if (result == CRC_FAILED) {
		printMsg(logger, "Need to reset flash to default due to CRC");
	} else if (result == INCOMPATIBLE_VERSION) {
		printMsg(logger, "Resetting but saving engine type [%d]", engineConfiguration->engineType);
	} else {
		printMsg(logger, "Got valid configuration from flash!");
	}
}

static msg_t detect_thread(void *arg) {
	(void)arg;

	chRegSetThreadName("Detect");

	detect_tp = chThdSelf();

	for(;;) {
		chEvtWaitAny((eventmask_t) 1);

		if (!conf_general_detect_motor_param(detect_current, detect_min_rpm,
				detect_low_duty, &detect_cycle_int_limit, &detect_coupling_k,
				detect_hall_table, &detect_hall_res)) {
			detect_cycle_int_limit = 0.0;
			detect_coupling_k = 0.0;
		}

		int32_t ind = 0;
		send_buffer[ind++] = COMM_DETECT_MOTOR_PARAM;
		buffer_append_int32(send_buffer, (int32_t)(detect_cycle_int_limit * 1000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(detect_coupling_k * 1000.0), &ind);
		memcpy(send_buffer + ind, detect_hall_table, 8);
		ind += 8;
		send_buffer[ind++] = detect_hall_res;
		commands_send_packet(send_buffer, ind);
	}

	return 0;
}

static ALWAYS_INLINE void handleFuel(bool limitedFuel, uint32_t eventIndex, int rpm DECLARE_ENGINE_PARAMETER_S) {
	if (!isInjectionEnabled(engineConfiguration))
		return;
	efiAssertVoid(getRemainingStack(chThdSelf()) > 128, "lowstck#3");
	efiAssertVoid(eventIndex < ENGINE(triggerShape.getLength()), "handleFuel/event index");

	/**
	 * Ignition events are defined by addFuelEvents() according to selected
	 * fueling strategy
	 */
	FuelSchedule *fs = ENGINE(engineConfiguration2)->injectionEvents;

	InjectionEventList *source = &fs->injectionEvents;

	if (!fs->hasEvents[eventIndex])
		return;

	ENGINE(tpsAccelEnrichment.onNewValue(getTPS(PASS_ENGINE_PARAMETER_F) PASS_ENGINE_PARAMETER));
	ENGINE(engineLoadAccelEnrichment.onEngineCycle(PASS_ENGINE_PARAMETER_F));

	ENGINE(fuelMs) = getFuelMs(rpm PASS_ENGINE_PARAMETER) * CONFIG(globalFuelCorrection);

	for (int i = 0; i < source->size; i++) {
		InjectionEvent *event = &source->elements[i];
		if (event->injectionStart.eventIndex != eventIndex)
			continue;
		handleFuelInjectionEvent(i, limitedFuel, event, rpm PASS_ENGINE_PARAMETER);
	}
}

/*
 * Application entry point.
 */
int main(void) {
  halInit();
  chSysInit();

  /*
   * Serial port initialization.
   */
  sdStart(&SD1, NULL); 
  chprintf((BaseSequentialStream *)&SD1, "BCM2835 GPIO Demonstration\r\n");

  ioportid_t ledPort = ONBOARD_LED_PORT;
  uint32_t ledPad = ONBOARD_LED_PAD;

  palSetPadMode(ledPort, ledPad, PAL_MODE_OUTPUT);
  palSetPad(ledPort, ledPad);

  palSetPadMode(GPIO4_PORT, GPIO4_PAD, PAL_MODE_INPUT_PULLUP);

  for (;;) {
    uint32_t button_state = palReadPad(GPIO4_PORT, GPIO4_PAD);
    if (button_state) {
      palSetPad(ledPort, ledPad);
    }
    else {
      palClearPad(ledPort, ledPad);
    }
  }

  /*
   * Events servicing loop.
   */
  chThdWait(chThdSelf());

  return 0;
}