static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield)
{
struct pios_com_dev *com_dev = (struct pios_com_dev *)context;
bool valid = PIOS_COM_validate(com_dev);
PIOS_Assert(valid);
PIOS_Assert(com_dev->has_rx);
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len);
if (bytes_into_fifo > 0) {
/* Data has been added to the buffer */
PIOS_COM_UnblockRx(com_dev, need_yield);
}
if (headroom) {
*headroom = fifoBuf_getFree(&com_dev->rx);
}
return bytes_into_fifo;
}
static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
{
struct pios_com_dev * com_dev = (struct pios_com_dev *)context;
bool valid = PIOS_COM_validate(com_dev);
PIOS_Assert(valid);
PIOS_Assert(buf);
PIOS_Assert(buf_len);
PIOS_Assert(com_dev->has_tx);
uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len);
if (bytes_from_fifo > 0) {
/* More space has been made in the buffer */
PIOS_COM_UnblockTx(com_dev, need_yield);
}
if (headroom) {
*headroom = fifoBuf_getUsed(&com_dev->tx);
}
return (bytes_from_fifo);
}
/**
* Sends a package over given port
* (blocking function)
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return -2 if mutex can't be taken;
* \return -3 if data cannot be sent in the max allotted time of 5000msec
* \return number of bytes transmitted on success
*/
int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len)
{
struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id;
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
PIOS_Assert(com_dev->has_tx);
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->sendbuffer_sem, 5) != pdTRUE) {
return -2;
}
#endif /* PIOS_INCLUDE_FREERTOS */
uint32_t max_frag_len = fifoBuf_getSize(&com_dev->tx);
uint32_t bytes_to_send = len;
while (bytes_to_send) {
uint32_t frag_size;
if (bytes_to_send > max_frag_len) {
frag_size = max_frag_len;
} else {
frag_size = bytes_to_send;
}
int32_t rc = PIOS_COM_SendBufferNonBlockingInternal(com_dev, buffer, frag_size);
if (rc >= 0) {
bytes_to_send -= rc;
buffer += rc;
} else {
switch (rc) {
case -1:
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreGive(com_dev->sendbuffer_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
/* Device is invalid, this will never work */
return -1;
case -2:
/* Device is busy, wait for the underlying device to free some space and retry */
/* Make sure the transmitter is running while we wait */
if (com_dev->driver->tx_start) {
(com_dev->driver->tx_start)(com_dev->lower_id,
fifoBuf_getUsed(&com_dev->tx));
}
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->tx_sem, 5000) != pdTRUE) {
xSemaphoreGive(com_dev->sendbuffer_sem);
return -3;
}
#endif
continue;
default:
/* Unhandled return code */
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreGive(com_dev->sendbuffer_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
return rc;
}
}
}
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreGive(com_dev->sendbuffer_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
return len;
}
