static bool outint(SerialDriver *sdp) {

  if (sdp->com_data != INVALID_SOCKET) {
    int n;
    uint8_t data[1];

    /*
     * Input.
     */
    chSysLockFromISR();
    n = sdRequestDataI(sdp);
    chSysUnlockFromISR();
    if (n < 0)
      return false;
    data[0] = (uint8_t)n;
    n = send(sdp->com_data, (char *)data, sizeof(data), 0);
    switch (n) {
    case 0:
      closesocket(sdp->com_data);
      sdp->com_data = INVALID_SOCKET;
      chSysLockFromISR();
      chnAddFlagsI(sdp, CHN_DISCONNECTED);
      chSysUnlockFromISR();
      return false;
    case SOCKET_ERROR:
      if (WSAGetLastError() == WSAEWOULDBLOCK)
        return false;
      closesocket(sdp->com_data);
      sdp->com_data = INVALID_SOCKET;
      return false;
    }
    return true;
  }
  return false;
}

//-----------------------------------------------------------------------------
static void 
vectornav_spi_end_cb(SPIDriver * spip)
{
	(void)spip;

	switch( async_vn_msg.state )
	{
	case VN_ASYNC_1ST_SPI_CB:
		chSysLockFromISR();
		spiUnselectI(VND1.spip);
		async_vn_msg.state = VN_ASYNC_1ST_SLEEP;
		gptStartOneShotI(VND1.gpdp, VN_SLEEPTIME);
		chSysUnlockFromISR();
		break;

	case VN_ASYNC_2ND_SPI_CB:
		chSysLockFromISR();
		spiUnselectI(VND1.spip);
		// here we have the register
		vectornav_dispatch_register(async_vn_msg.reg, async_vn_msg.buf_size, async_vn_msg.buf);
		async_vn_msg.state = VN_ASYNC_2ND_SLEEP;
		gptStartOneShotI(VND1.gpdp, VN_SLEEPTIME);
		chSysUnlockFromISR();
		break;

	case VN_ASYNC_INACTIVE:
	case VN_ASYNC_1ST_SLEEP:
	case VN_ASYNC_2ND_SLEEP:
	default:
		// @TODO: assert?
		break;
	}
}

static bool inint(SerialDriver *sdp) {

  if (sdp->com_data != INVALID_SOCKET) {
    int i;
    uint8_t data[32];

    /*
     * Input.
     */
    int n = recv(sdp->com_data, (char *)data, sizeof(data), 0);
    switch (n) {
    case 0:
      closesocket(sdp->com_data);
      sdp->com_data = INVALID_SOCKET;
      chSysLockFromISR();
      chnAddFlagsI(sdp, CHN_DISCONNECTED);
      chSysUnlockFromISR();
      return false;
    case SOCKET_ERROR:
      if (WSAGetLastError() == WSAEWOULDBLOCK)
        return false;
      closesocket(sdp->com_data);
      sdp->com_data = INVALID_SOCKET;
      return false;
    }
    for (i = 0; i < n; i++) {
      chSysLockFromISR();
      sdIncomingDataI(sdp, data[i]);
      chSysUnlockFromISR();
    }
    return true;
  }
  return false;
}

/*
 * Handles the USB driver global events.
 */
static void usb_event(USBDriver *usbp, usbevent_t event) {
  extern SerialUSBDriver SDU1;
  extern SerialUSBDriver SDU2;

  switch (event) {
  case USB_EVENT_RESET:
    return;
  case USB_EVENT_ADDRESS:
    return;
  case USB_EVENT_CONFIGURED:
    chSysLockFromISR();

    if (usbp->state == USB_ACTIVE) {
      /* Enables the endpoints specified into the configuration.
         Note, this callback is invoked from an ISR so I-Class functions
         must be used.*/
      usbInitEndpointI(usbp, USB_INTERRUPT_REQUEST_EP_A, &ep1config);
      usbInitEndpointI(usbp, USB_DATA_REQUEST_EP_A, &ep2config);
      usbInitEndpointI(usbp, USB_INTERRUPT_REQUEST_EP_B, &ep3config);
      usbInitEndpointI(usbp, USB_DATA_REQUEST_EP_B, &ep4config);

      /* Resetting the state of the CDC subsystem.*/
      sduConfigureHookI(&SDU1);
      sduConfigureHookI(&SDU2);
    }
    else if (usbp->state == USB_SELECTED) {
      usbDisableEndpointsI(usbp);
    }

    chSysUnlockFromISR();
    return;
  case USB_EVENT_UNCONFIGURED:
    return;
  case USB_EVENT_SUSPEND:
    chSysLockFromISR();

    /* Disconnection event on suspend.*/
    sduDisconnectI(&SDU1);
    sduDisconnectI(&SDU2);

    chSysUnlockFromISR();
    return;
  case USB_EVENT_WAKEUP:
    return;
  case USB_EVENT_STALLED:
    return;
  }
  return;
}

static void spi_end_cb(SPIDriver *spip){
  its++;

  if (stop){
    chSysLockFromISR();
    chBSemSignalI(&sem);
    chSysUnlockFromISR();
    return;
  }
  else{
    chSysLockFromISR();
    spiStartExchangeI(spip, SPI_BUF_SIZE, testbuf_flash, testbuf_ram);
    chSysUnlockFromISR();
  }
}

/*
 * Insertion monitor timer callback function.
 *
 * pointer to the p BaseBlockDevice object
 *
 */
static void tmrfunc( void *p ) 
{
    BaseBlockDevice *bbdp = p;
    
    chSysLockFromISR();
    if( cnt > 0 ) 
    {
        if( blkIsInserted( bbdp ) ) 
        {
            if( --cnt == 0 ) 
            {
                chEvtBroadcastI( &inserted_event );
            }
        }
        else
        {
            cnt = POLLING_INTERVAL;
        }
    }
    else if( ! blkIsInserted( bbdp ) ) 
    {
        cnt = POLLING_INTERVAL;
        chEvtBroadcastI( &removed_event );
    }
    
    chVTSetI( &tmr, MS2ST( POLLING_DELAY ), tmrfunc, bbdp );
    chSysUnlockFromISR();
}

/*
 * Timeout wakeup callback.
 */
static void wakeup(void *p) {
  thread_t *tp = (thread_t *)p;

  chSysLockFromISR();
  switch (tp->p_state) {
  case CH_STATE_READY:
    /* Handling the special case where the thread has been made ready by
       another thread with higher priority.*/
    chSysUnlockFromISR();
    return;
  case CH_STATE_SUSPENDED:
    *(thread_reference_t *)tp->p_u.wtobjp = NULL;
    break;
#if CH_CFG_USE_SEMAPHORES
  case CH_STATE_WTSEM:
    chSemFastSignalI((semaphore_t *)tp->p_u.wtobjp);
    /* Falls into, intentional. */
#endif
#if CH_CFG_USE_CONDVARS && CH_CFG_USE_CONDVARS_TIMEOUT
  case CH_STATE_WTCOND:
#endif
  case CH_STATE_QUEUED:
    /* States requiring dequeuing.*/
    queue_dequeue(tp);
  }
  tp->p_u.rdymsg = MSG_TIMEOUT;
  chSchReadyI(tp);
  chSysUnlockFromISR();
}

/*
 * Handles the USB driver global events.
 */
static void usb_event(USBDriver *usbp, usbevent_t event) {

  switch (event) {
  case USB_EVENT_RESET:
    return;
  case USB_EVENT_ADDRESS:
    return;
  case USB_EVENT_CONFIGURED:
    chSysLockFromISR();

    /* Enables the endpoints specified into the configuration.
       Note, this callback is invoked from an ISR so I-Class functions
       must be used.*/
    usbInitEndpointI(usbp, USBD2_DATA_REQUEST_EP, &ep1config);
    usbInitEndpointI(usbp, USBD2_INTERRUPT_REQUEST_EP, &ep2config);

    chSysUnlockFromISR();
    return;
  case USB_EVENT_UNCONFIGURED:
    return;
  case USB_EVENT_SUSPEND:
    return;
  case USB_EVENT_WAKEUP:
    return;
  case USB_EVENT_STALLED:
    return;
  }
  return;
}

static bool connint(SerialDriver *sdp) {

  if (sdp->com_data == INVALID_SOCKET) {
    struct sockaddr addr;
    int addrlen = sizeof(addr);

    if ((sdp->com_data = accept(sdp->com_listen, &addr, &addrlen)) == INVALID_SOCKET)
      return false;

    if (ioctlsocket(sdp->com_data, FIONBIO, &nb) != 0) {
      printf("%s: Unable to setup non blocking mode on data socket\n", sdp->com_name);
      goto abort;
    }
    chSysLockFromISR();
    chnAddFlagsI(sdp, CHN_CONNECTED);
    chSysUnlockFromISR();
    return true;
  }
  return false;
abort:
  if (sdp->com_listen != INVALID_SOCKET)
    closesocket(sdp->com_listen);
  if (sdp->com_data != INVALID_SOCKET)
    closesocket(sdp->com_data);
  WSACleanup();
  exit(1);
}

/*
 * This callback is invoked when a transmission buffer has been completely
 * read by the driver.
 */
static void txend1(UARTDriver *uartp) {

  its++;
  chSysLockFromISR();
  uartStartSendI(uartp, STORM_BUF_LEN, txbuf);
  chSysUnlockFromISR();
}

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

  chSysLockFromISR();
  uartStartReceiveI(&UARTD6, STORM_BUF_LEN, rxbuf);
  chSysUnlockFromISR();
}

/**
 * Adc error callback
 *
 * Fired if DMA error happens or ADC overflows
 */
static void adcerrorcallback(ADCDriver *adcp, adcerror_t err)
{
  chSysLockFromISR();
  adcp_err = adcp;
  adc_error_flag = err;
  chSysUnlockFromISR();
}

/**
 * @brief   Generic endpoint OUT handler.
 *
 * @param[in] usbp      pointer to the @p USBDriver object
 * @param[in] ep        endpoint number
 *
 * @notapi
 */
static void otg_epout_handler(USBDriver *usbp, usbep_t ep) {
  stm32_otg_t *otgp = usbp->otg;
  uint32_t epint = otgp->oe[ep].DOEPINT;

  /* Resets all EP IRQ sources.*/
  otgp->oe[ep].DOEPINT = epint;

  if ((epint & DOEPINT_STUP) && (otgp->DOEPMSK & DOEPMSK_STUPM)) {
    /* Setup packets handling, setup packets are handled using a
       specific callback.*/
    _usb_isr_invoke_setup_cb(usbp, ep);

  }
  if ((epint & DOEPINT_XFRC) && (otgp->DOEPMSK & DOEPMSK_XFRCM)) {
    /* Receive transfer complete.*/
    USBOutEndpointState *osp = usbp->epc[ep]->out_state;

    if (osp->rxsize < osp->totsize) {
      /* In case the transaction covered only part of the total transfer
         then another transaction is immediately started in order to
         cover the remaining.*/
      osp->rxsize = osp->totsize - osp->rxsize;
      osp->rxcnt  = 0;
      usb_lld_prepare_receive(usbp, ep);
      chSysLockFromISR();
      usb_lld_start_out(usbp, ep);
      chSysUnlockFromISR();
    }
    else {
      /* End on OUT transfer.*/
      _usb_isr_invoke_out_cb(usbp, ep);
    }
  }
}

static void usb_event(USBDriver *usbp, usbevent_t event) {
    switch (event) {
        case USB_EVENT_RESET:
            return;
        case USB_EVENT_ADDRESS:
            return;
        case USB_EVENT_CONFIGURED:
            chSysLockFromISR();
            /* Enable the endpoints specified in the configuration.
            Note, this callback is invoked from an ISR so I-Class functions must be used.*/
            usbInitEndpointI(usbp, USBD2_DATA_IN_EP, &ep1config);
            usbInitEndpointI(usbp, USBD2_INTERRUPT_REQUEST_EP, &ep2config);

            sduConfigureHookI(&UsbCDC.SDU2);   // Resetting the state of the CDC subsystem
            App.SignalEvtI(EVTMSK_USB_READY);
            chSysUnlockFromISR();
            return;
        case USB_EVENT_SUSPEND:
            return;
        case USB_EVENT_WAKEUP:
            return;
        case USB_EVENT_STALLED:
            return;
    } // switch
}

void i2cDmaIrqHandler(void *p, uint32_t flags) {
    chSysLockFromISR();
    i2c_t *pi2c = (i2c_t*)p;
//    Uart.PrintfNow("\r===T===");
    chThdResumeI(&pi2c->ThdRef, (msg_t)0);
    chSysUnlockFromISR();
}

void doSlowAdc(void) {

	efiAssertVoid(CUSTOM_ERR_6658, getRemainingStack(chThdGetSelfX())> 32, "lwStAdcSlow");

#if EFI_INTERNAL_ADC

	/* 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(OBD_PCM_Processor_Fault, "ADC slow not ready?");
		slowAdc.errorsCount++;
		chSysUnlockFromISR()
		;
		return;
	}
	adcStartConversionI(&ADC_SLOW_DEVICE, &adcgrpcfgSlow, slowAdc.samples, ADC_BUF_DEPTH_SLOW);
	chSysUnlockFromISR()
	;
#endif /* EFI_INTERNAL_ADC */
}

static void pwmpcb_fast(PWMDriver *pwmp) {
	efiAssertVoid(CUSTOM_ERR_6659, getRemainingStack(chThdGetSelfX())> 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(OBD_PCM_Processor_Fault, "ADC fast not ready?");
		chSysUnlockFromISR()
		;
		return;
	}
	adcStartConversionI(&ADC_FAST_DEVICE, &adcgrpcfg_fast, fastAdc.samples, ADC_BUF_DEPTH_FAST);
	chSysUnlockFromISR()
	;
	fastAdc.conversionCount++;
#endif /* EFI_INTERNAL_ADC */
}

static void wakeup_tx(void *p) {
	(void)p;

	chSysLockFromISR();
	chEvtSignalI(tx_tp, (eventmask_t) 1);
	chSysUnlockFromISR();
}

/*
 * Handles the USB driver global events.
 */
static void usb_event(USBDriver *usbp, usbevent_t event) {
  extern SerialUSBDriver SDU1;

  switch (event) {
  case USB_EVENT_RESET:
    return;
  case USB_EVENT_ADDRESS:
    return;
  case USB_EVENT_CONFIGURED:
    chSysLockFromISR();

    /* Enables the endpoints specified into the configuration.
       Note, this callback is invoked from an ISR so I-Class functions
       must be used.*/
    usbInitEndpointI(usbp, USBD1_DATA_REQUEST_EP, &ep1config);
    usbInitEndpointI(usbp, USBD1_INTERRUPT_REQUEST_EP, &ep2config);

    /* Resetting the state of the CDC subsystem.*/
    sduConfigureHookI(&SDU1);

    chSysUnlockFromISR();
    return;
  case USB_EVENT_SUSPEND:
    return;
  case USB_EVENT_WAKEUP:
    return;
  case USB_EVENT_STALLED:
    return;
  }
  return;
}

/**
 * @brief   Get a message from the queue.
 */
osEvent osMessageGet(osMessageQId queue_id,
                     uint32_t millisec) {

  msg_t msg;
  osEvent event;

  event.def.message_id = queue_id;

  if (port_is_isr_context()) {

    /* Waiting makes no sense in ISRs so any value except "immediate"
       makes no sense.*/
    if (millisec != 0) {
      event.status = osErrorValue;
      return event;
    }

    chSysLockFromISR();
    msg = chMBFetchI((mailbox_t *)queue_id, (msg_t*)&event.value.v);
    chSysUnlockFromISR();
  }
  else {
    msg = chMBFetch((mailbox_t *)queue_id,
                    (msg_t*)&event.value.v,
                    (systime_t)millisec);
  }

  /* Returned event type.*/
  event.status = msg == MSG_OK ? osEventMessage : osEventTimeout;
  return event;
}