/*
** ===================================================================
**     Method      :  CI2C1_SendBlock (component InternalI2C)
**     Description :
**         When working as a MASTER, this method writes one (7-bit
**         addressing) or two (10-bit addressing) slave address bytes
**         inclusive of R/W bit = 0 to the I2C bus and then writes the
**         block of characters to the bus. The slave address must be
**         specified before, by the "SelectSlave" or "SlaveSelect10"
**         method or in component initialization section, "Target slave
**         address init" property. If interrupt service is enabled and
**         the method returns ERR_OK, it doesn't mean that transmission
**         was successful. The state of transmission is detectable by
**         means of events (OnTransmitData, OnError or OnArbitLost).
**         Data to be send is not copied to an internal buffer and
**         remains in the original location. Therefore the content of
**         the buffer should not be changed until the transmission is
**         complete. Event OnTransmitData can be used to detect the end
**         of the transmission.
**         When working as a SLAVE, this method writes a block of
**         characters to the internal output slave buffer and then,
**         after the master starts the communication, to the I2C bus.
**         If no character is ready for a transmission (internal output
**         slave buffer is empty), the "Empty character" will be sent
**         (see "Empty character" property). In SLAVE mode the data are
**         copied to an internal buffer, if specified by "Output buffer
**         size" property.
**     Parameters  :
**         NAME            - DESCRIPTION
**       * Ptr             - Pointer to the block of data to send.
**         Siz             - Size of the block.
**       * Snt             - Amount of data sent (moved to a buffer).
**                           In master mode, if interrupt support is
**                           enabled, the parameter always returns the
**                           same value as the parameter 'Siz' of this
**                           method.
**     Returns     :
**         ---             - Error code, possible codes:
**                           ERR_OK - OK
**                           ERR_SPEED - This device does not work in
**                           the active speed mode
**                           ERR_DISABLED -  Device is disabled
**                           ERR_BUSY - The slave device is busy, it
**                           does not respond by the acknowledge (only
**                           in master mode and when interrupt service
**                           is disabled)
**                           ERR_BUSOFF - Clock timeout elapsed or
**                           device cannot transmit data
**                           ERR_TXFULL - Transmitter is full. Some data
**                           has not been sent. (slave mode only)
**                           ERR_ARBITR - Arbitration lost (only when
**                           interrupt service is disabled and in master
**                           mode)
** ===================================================================
*/
byte CI2C1_SendBlock(void *Ptr,word Siz,word *Snt)
{
  LDD_TError Error;

  if (Siz == 0U) {                     /* Test variable Size on zero */
    *Snt = 0U;
    return ERR_OK;                     /* If zero then OK */
  }
  EnterCritical();                     /* Enter the critical section */
  Error = IntI2cLdd1_MasterSendBlock(IntI2cLdd1_DeviceDataPtr, (LDD_TData *)Ptr, (LDD_I2C_TSize)Siz, LDD_I2C_SEND_STOP); /* Send one data byte */
  if (Error == ERR_BUSY) {
    ExitCritical();                    /* Exit the critical section */
    return ERR_BUSOFF;
  }
  OutLenM = Siz;                       /* Set length of output bufer's content */
  ExitCritical();                      /* Exit the critical section */
  *Snt = Siz;                          /* Dummy number of really sent chars */
  return ERR_OK;                       /* OK */
}

/*
** ===================================================================
**     Method      :  Serial_2_ClearRxBuf (component AsynchroSerial)
**     Description :
**         Clears the receive buffer.
**         This method is available only if non-zero length of the
**         input buffer is defined and the receiver property is enabled.
**     Parameters  : None
**     Returns     :
**         ---             - Error code, possible codes:
**                           ERR_OK - OK
**                           ERR_SPEED - This device does not work in
**                           the active speed mode
** ===================================================================
*/
byte Serial_2_ClearRxBuf(void)
{
  EnterCritical();                     /* Save the PS register */
  Serial_2_InpLen = 0x00U;             /* Set number of chars in the receive buffer to 0 */
  InpIndxR = 0x00U;                    /* Reset read index to the receive buffer */
  InpIndxW = 0x00U;                    /* Reset write index to the receive buffer */
  SerFlag &= (byte)(~(byte)(CHAR_IN_RX | FULL_RX)); /* Clear the flags indicating a char in buffer */
  ExitCritical();                      /* Restore the PS register */
  return ERR_OK;                       /* OK */
}

void
route_show(void) {
  int i;
  print_route(NULL);
  EnterCritical();
  for (i = 0 ; i < GETNUMROUTEENTRIES ; i++) {
    print_route(GETROUTEP(i));
  }
  ExitCritical();
}

/*
** ===================================================================
**     Method      :  SM1_GetError (component SynchroMaster)
**     Description :
**         Returns a set of errors on the channel (errors that cannot
**         be returned in given methods). The component accumulates
**         errors in a set; after calling [GetError] this set is
**         returned and cleared. This method is available only if the
**         "Interrupt service/event" property is enabled.
**     Parameters  :
**         NAME            - DESCRIPTION
**       * Err             - A pointer to the returned set of errors
**     Returns     :
**         ---             - Error code (if GetError did not succeed),
**                           possible codes:
**                           ERR_OK - OK
**                           ERR_SPEED - This device does not work in
**                           the active speed mode
** ===================================================================
*/
byte SM1_GetError(SM1_TError *Err)
{
  EnterCritical();                     /* Disable global interrupts */
  Err->err = 0U;
  Err->errName.OverRun = (((ErrFlag & OVERRUN_ERR) != 0U)? 1U : 0U); /* Overrun error */
  Err->errName.RxBufOvf = (((ErrFlag & FULL_RX) != 0U)? 1U : 0U); /* Buffer overflow */
  ErrFlag = 0x00U;                     /* Reset error flags */
  ExitCritical();                      /* Enable global interrupts */
  return ERR_OK;                       /* OK */
}

/*
** ===================================================================
**     Method      :  SM1_ClearRxBuf (component SynchroMaster)
**     Description :
**         Clears the receive buffer. This method is available only if
**         a non-zero length of input buffer is defined.
**     Parameters  : None
**     Returns     :
**         ---             - Error code, possible codes:
**                           ERR_OK - OK
**                           ERR_SPEED - This device does not work in
**                           the active speed mode
** ===================================================================
*/
byte SM1_ClearRxBuf(void)
{
  EnterCritical();                     /* Disable global interrupts */
  SM1_InpLen = 0U;                     /* Set number of chars in the transmit buffer to 0 */
  InpIndexW = 0x00U;                   /* Set index on the first item in the transmit buffer */
  InpIndexR = 0x00U;
  SerFlag &= (byte)~(OVERRUN_ERR | FULL_RX); /* Clear flags */
  ExitCritical();                      /* Enable global interrupts */
  return ERR_OK;                       /* OK */
}

/*
** ===================================================================
**     Method      :  TMOUT1_LeaveCounter (component Timeout)
**
**     Description :
**         To be called to return the counter. Note that a counter
**         always should be returned so it can be reused.
**     Parameters  :
**         NAME            - DESCRIPTION
**         handle          - Counter handle
**     Returns     : Nothing
** ===================================================================
*/
void TMOUT1_LeaveCounter(TMOUT1_CounterHandle handle)
{
  if (handle==TMOUT1_OUT_OF_HANDLE) {
    return;
  }
  EnterCritical();
  TMOUT1_Counters[handle] = 0;
  TMOUT1_FreeCounters[handle]=TRUE;
  ExitCritical();
}

/*
** ===================================================================
**     Method      :  hwExpIn_GetRxIdle (bean AsynchroSerial)
**
**     Description :
**         Returns the state of the receiver idle flag. This method
**         is available only if event <OnIdle> is disabled.
**     Parameters  : None
**     Returns     :
**         ---             - The state of the receiver idle flag.
** ===================================================================
*/
bool hwExpIn_GetRxIdle(void)
{
  bool Result;

  EnterCritical();                     /* Save the PS register */
  Result = (bool)((SerFlag & IDLE_ERR)?1:0); /* If idle signal has been received? */
  SerFlag &= ~IDLE_ERR;                /* Reset idle signal flag */
  ExitCritical();                      /* Restore the PS register */
  return Result;                       /* OK */
}

/*---------------------------------------------------------------------------*/
void
watchdog_periodic(void)
{
#if PLATFORM_CONF_ENABLE_WATCHDOG
  EnterCritical();
  SIM_SRVCOP = SIM_SRVCOP_SRVCOP(COP_KEY_1);
  SIM_SRVCOP = SIM_SRVCOP_SRVCOP(COP_KEY_2);
  /* {Default RTOS Adapter} Critical section end, general PE function is used */
  ExitCritical();
#endif /* PLATFORM_CONF_ENABLE_WATCHDOG */
}

void TACHO_CalcSpeed(void) {
#if 1
  /* we calculate the speed as follow:
                              1000         
  steps/sec =  delta * ----------------- 
                       samplePeriod (ms) 
  As this function may be called very frequently, it is important to make it as efficient as possible!
   */
  int16_t deltaLeft, deltaRight, newLeft, newRight, oldLeft, oldRight;
  int32_t speedLeft, speedRight;
  bool negLeft, negRight;

  EnterCritical();
  oldLeft = (int16_t)TACHO_LeftPosHistory[TACHO_PosHistory_Index]; /* oldest left entry */
  oldRight = (int16_t)TACHO_RightPosHistory[TACHO_PosHistory_Index]; /* oldest right entry */
  if (TACHO_PosHistory_Index==0) { /* get newest entry */
    newLeft = (int16_t)TACHO_LeftPosHistory[NOF_HISTORY-1];
    newRight = (int16_t)TACHO_RightPosHistory[NOF_HISTORY-1];
  } else {
    newLeft = (int16_t)TACHO_LeftPosHistory[TACHO_PosHistory_Index-1];
    newRight = (int16_t)TACHO_RightPosHistory[TACHO_PosHistory_Index-1];
  }
  ExitCritical();
  deltaLeft = oldLeft-newLeft; /* delta of oldest position and most recent one */
  /* use unsigned arithmetic */
  if (deltaLeft < 0) {
    deltaLeft = -deltaLeft;
    negLeft = TRUE;
  } else {
    negLeft = FALSE;
  }
  deltaRight = oldRight-newRight; /* delta of oldest position and most recent one */
  /* use unsigned arithmetic */
  if (deltaRight < 0) {
    deltaRight = -deltaRight;
    negRight = TRUE;
  } else {
    negRight = FALSE;
  }
  /* calculate speed. this is based on the delta and the time (number of samples or entries in the history table) */
  speedLeft = (int32_t)(deltaLeft * 1000/(TACHO_SAMPLE_PERIOD_MS*(NOF_HISTORY-1)));
  if (negLeft) {
    speedLeft = -speedLeft;
  }
  speedRight = (int32_t)(deltaRight * 1000/(TACHO_SAMPLE_PERIOD_MS*(NOF_HISTORY-1)));
  if (negRight) {
    speedRight = -speedRight;
  }
  TACHO_currLeftSpeed = -speedLeft; /* store current speed in global variable */
  TACHO_currRightSpeed = -speedRight; /* store current speed in global variable */
#else
  /*! \todo Implement function */ 
#endif
}

int
get_ifnum_cardno(int ifnum) {
  if_entry_p ife;
  int cardno;
  ASSERTIFNUM(ifnum);
  EnterCritical();
  ife = GETIFP(ifnum);
  cardno = ife->cardno;
  ExitCritical();
  return cardno;
}

/*
** ===================================================================
**     Method      :  RC_SendChar (component AsynchroSerial)
**     Description :
**         Sends one character to the channel. If the component is
**         temporarily disabled (Disable method) SendChar method only
**         stores data into an output buffer. In case of a zero output
**         buffer size, only one character can be stored. Enabling the
**         component (Enable method) starts the transmission of the
**         stored data. This method is available only if the
**         transmitter property is enabled.
**     Parameters  :
**         NAME            - DESCRIPTION
**         Chr             - Character to send
**     Returns     :
**         ---             - Error code, possible codes:
**                           ERR_OK - OK
**                           ERR_SPEED - This device does not work in
**                           the active speed mode
**                           ERR_TXFULL - Transmitter is full
** ===================================================================
*/
byte RC_SendChar(RC_TComData Chr)
{
  if ((SerFlag & FULL_TX) != 0U) {     /* Is any char is in TX buffer */
    return ERR_TXFULL;                 /* If yes then error */
  }
  EnterCritical();                     /* Disable global interrupts */
  OutBuffer = Chr;                     /* Store char to temporary variable */
  (void)ASerialLdd1_SendBlock(ASerialLdd1_DeviceDataPtr, (LDD_TData *)&OutBuffer, 1U); /* Send one data byte */
  SerFlag |= (FULL_TX);                /* Set the flag "full TX buffer" */
  ExitCritical();                      /* Enable global interrupts */
  return ERR_OK;                       /* OK */
}

/*
** ===================================================================
**     Method      :  TMOUT1_AddTick (component Timeout)
**
**     Description :
**         Method to be called from a priodic timer or interrupt. It
**         will decrement all current counters by one down to zero.
**     Parameters  : None
**     Returns     : Nothing
** ===================================================================
*/
void TMOUT1_AddTick(void)
{
  byte i;

  EnterCritical();
  for(i=0;i<TMOUT1_NOF_COUNTERS;i++) {
    if (TMOUT1_Counters[i]>0) {
      TMOUT1_Counters[i]--;
    }
  }
  ExitCritical();
}

/*
** ===================================================================
**     Method      :  TMOUT1_CounterExpired (component Timeout)
**
**     Description :
**         Returns true if the timeout counter has been expired
**     Parameters  :
**         NAME            - DESCRIPTION
**         handle          - The timeout handle retrieved using
**                           GetCounter()
**     Returns     :
**         ---             - Returns TRUE if the counter has been
**                           expired, FALSE otherwise
** ===================================================================
*/
bool TMOUT1_CounterExpired(TMOUT1_CounterHandle handle)
{
  bool res;

  if (handle==TMOUT1_OUT_OF_HANDLE) {
    return TRUE;
  }
  EnterCritical();
  res = (bool)(TMOUT1_Counters[handle]==0);
  ExitCritical();
  return res;
}

/* ===================================================================*/
bool SPI_SD_GetBlockReceivedStatus(LDD_TDeviceData *DeviceDataPtr)
{
  uint8_t Status;                      /* Temporary variable for flag saving */

  /* {Default RTOS Adapter} Critical section begin, general PE function is used */
  EnterCritical();
  Status = ((SPI_SD_TDeviceDataPtr)DeviceDataPtr)->SerFlag; /* Save flag for return */
  ((SPI_SD_TDeviceDataPtr)DeviceDataPtr)->SerFlag &= (uint8_t)(~(uint8_t)BLOCK_RECEIVED); /* Clear data block sent flag */
  /* {Default RTOS Adapter} Critical section end, general PE function is used */
  ExitCritical();
  return (bool)(((Status & BLOCK_RECEIVED) != 0U)? TRUE : FALSE); /* Return saved status */
}

void wakeup (void *chan) {
  int i;

  d0printf("%d wakeup chan: %p\n", getpid(),chan);

  global_ftable->counter1++;
  EnterCritical();
  for (i = 0; i < NENV; i++) 
    if (synch_table->wchan[i] == chan) 
      synch_table->wchan[i] = (void *)0;
  ExitCritical();
}

/* ===================================================================*/
LDD_TError ASerialLdd3_GetError(LDD_TDeviceData *DeviceDataPtr, LDD_SERIAL_TError *ErrorPtr)
{
  ASerialLdd3_TDeviceDataPtr DeviceDataPrv = (ASerialLdd3_TDeviceDataPtr)DeviceDataPtr;

  /* {Bareboard RTOS Adapter} Critical section begin (RTOS function call is defined by Bareboard RTOS Adapter property) */
  EnterCritical();
  *ErrorPtr = DeviceDataPrv->ErrFlag;
  DeviceDataPrv->ErrFlag = 0x00U;      /* Reset error flags */
  /* {Bareboard RTOS Adapter} Critical section ends (RTOS function call is defined by Bareboard RTOS Adapter property) */
  ExitCritical();
  return ERR_OK;                       /* OK */
}

/* ===================================================================*/
LDD_TError ASerialLdd1_GetError(LDD_TDeviceData *DeviceDataPtr, LDD_SERIAL_TError *ErrorPtr)
{
  ASerialLdd1_TDeviceDataPtr DeviceDataPrv = (ASerialLdd1_TDeviceDataPtr)DeviceDataPtr;

  /* {Default RTOS Adapter} Critical section begin, general PE function is used */
  EnterCritical();
  *ErrorPtr = DeviceDataPrv->ErrFlag;
  DeviceDataPrv->ErrFlag = 0x00U;      /* Reset error flags */
  /* {Default RTOS Adapter} Critical section end, general PE function is used */
  ExitCritical();
  return ERR_OK;                       /* OK */
}

/*
** ===================================================================
**     Method      :  Serial_2_SendChar (component AsynchroSerial)
**     Description :
**         Sends one character to the channel. If the component is
**         temporarily disabled (Disable method) SendChar method only
**         stores data into an output buffer. In case of a zero output
**         buffer size, only one character can be stored. Enabling the
**         component (Enable method) starts the transmission of the
**         stored data. This method is available only if the
**         transmitter property is enabled.
**     Parameters  :
**         NAME            - DESCRIPTION
**         Chr             - Character to send
**     Returns     :
**         ---             - Error code, possible codes:
**                           ERR_OK - OK
**                           ERR_SPEED - This device does not work in
**                           the active speed mode
**                           ERR_TXFULL - Transmitter is full
** ===================================================================
*/
byte Serial_2_SendChar(Serial_2_TComData Chr)
{
  if (SerFlag & FULL_TX) {             /* Is any char in TX buffer? */
    return ERR_TXFULL;                 /* If yes then error */
  }
  EnterCritical();                     /* Save the PS register */
  (void)SCI2S1;                        /* Reset interrupt request flag */
  SCI2D = (byte)Chr;                   /* Store char to the transmitter register */
  SCI2C2_TIE = 0x01U;                  /* Enable transmit interrupt */
  SerFlag |= FULL_TX;                  /* Set the flag "full TX buffer" */
  ExitCritical();                      /* Restore the PS register */
  return ERR_OK;                       /* OK */
}

int
iter_ifnum(unsigned int flags) {
  if_entry_p ife;
  EnterCritical();
  for(;;) {
    if (ifnum_iter >= GETNUMIFENTRIES || ifnum_iter < 0) {
      ExitCritical();
      return -1;
    }
    if (flags) {
      ife = GETIFP(ifnum_iter);
      if ((ife->flags & flags) == flags) {
	ExitCritical();
	return ifnum_iter++;
      }
    } else {
      ExitCritical();
      return ifnum_iter++;
    }
    ifnum_iter++;
  }
}

/*
** ===================================================================
**     Method      :  WriteWord (bean IntFLASH)
**
**     Description :
**         This method is internal. It is used by Processor Expert
**         only.
** ===================================================================
*/
static byte WriteWord(dword Addr,word Data16)
{
  EnterCritical();                     /* Enter critical section */
  ClearFlags();                        /* Clear all flags */
  if (FSTAT_CBEIF == 0) {              /* Is command buffer full ? */
    ExitCritical();                    /* Exit critical section */
    return ERR_BUSY;                   /* If yes then error */
  }
  *(word *far) Addr = Data16;          /* Array address and program data */
  FCMD = 32;                           /* Word program command */
  FSTAT = 128;                         /* Clear flag command buffer empty */
  if ((FSTAT_PVIOL == 1)||(FSTAT_ACCERR == 1)) { /* Is protection violation or acces error detected ? */
    ExitCritical();                    /* Exit critical section */
    return ERR_NOTAVAIL;               /* If yes then error */
  }
  while (FSTAT_CBEIF == 0);            /* Wait to buffer empty */
  while (FSTAT_CCIF == 0);             /* Wait to command complete */
  ExitCritical();                      /* Exit critical section */
  if (*(word *far) Addr != Data16)     /* Was attempt to write data to the given address errorneous? */
    return ERR_VALUE;                  /* If yes then error */
 return ERR_OK;                        /* OK */
}