static int32_t PIOS_COM_SendBufferNonBlockingInternal(struct pios_com_dev *com_dev, const uint8_t *buffer, uint16_t len)
{
PIOS_Assert(com_dev);
PIOS_Assert(com_dev->has_tx);
if (com_dev->driver->available && !com_dev->driver->available(com_dev->lower_id)) {
/*
* Underlying device is down/unconnected.
* Dump our fifo contents and act like an infinite data sink.
* Failure to do this results in stale data in the fifo as well as
* possibly having the caller block trying to send to a device that's
* no longer accepting data.
*/
fifoBuf_clearData(&com_dev->tx);
return len;
}
if (len > fifoBuf_getFree(&com_dev->tx)) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len);
if (bytes_into_fifo > 0) {
/* More data has been put in the tx buffer, make sure the tx is started */
if (com_dev->driver->tx_start) {
com_dev->driver->tx_start(com_dev->lower_id,
fifoBuf_getUsed(&com_dev->tx));
}
}
return bytes_into_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 0 on success
*/
int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len)
{
struct pios_com_dev * com_dev = PIOS_COM_find_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);
int32_t rc;
do {
rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len);
#if defined(PIOS_INCLUDE_FREERTOS)
if (rc == -2) {
/* 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 (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) {
return -3;
}
}
#endif
} while (rc == -2);
return len;
}
data_type fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, data_type len)
{ // get data from the buffer without removing it
data_type rd = buf->rd;
data_type buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
// get number of bytes available
data_type num_bytes = fifoBuf_getUsed(buf);
if (num_bytes > len)
num_bytes = len;
if (num_bytes < 1)
return 0; // return number of bytes copied
uint8_t *p = (uint8_t *)data;
data_type i = 0;
while (num_bytes > 0)
{
data_type j = buf_size - rd;
if (j > num_bytes)
j = num_bytes;
memcpy(p + i, buff + rd, j);
i += j;
num_bytes -= j;
rd += j;
if (rd >= buf_size)
rd = 0;
}
return i; // return number of bytes copied
}
/**
* puts more than one byte onto the transmit buffer (used for atomic sends)
* \param[in] USART USART name
* \param[in] *buffer pointer to buffer to be sent
* \param[in] len number of bytes to be sent
* \return 0 if no error
* \return -1 if buffer full or cannot get all requested bytes (retry)
*/
static int32_t PIOS_USART_TxBufferPutMoreNonBlocking(uint32_t usart_id, const uint8_t *buffer, uint16_t len)
{
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;
bool valid = PIOS_USART_validate(usart_dev);
PIOS_Assert(valid)
if (len >= fifoBuf_getFree(&usart_dev->tx)) {
/* Buffer cannot accept all requested bytes (retry) */
return -1;
}
/* Copy bytes to be transmitted into transmit buffer */
/* This operation should be atomic! */
PIOS_IRQ_Disable();
uint16_t used = fifoBuf_getUsed(&usart_dev->tx);
fifoBuf_putData(&usart_dev->tx,buffer,len);
if (used == 0) {
/* enable sending when buffer was previously empty */
USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE);
}
PIOS_IRQ_Enable();
/* No error */
return 0;
}
void fifoBuf_removeData(t_fifo_buffer *buf, data_type len)
{
// remove a number of bytes from the buffer
data_type rd = buf->rd;
data_type buf_size = buf->buf_size;
// get number of bytes available
data_type num_bytes = fifoBuf_getUsed(buf);
if(num_bytes > len)
{
num_bytes = len;
}
if(num_bytes < 1)
{
return; // nothing to remove
}
rd += num_bytes;
if(rd >= buf_size)
{
rd -= buf_size;
}
buf->rd = rd;
}
/**
* Sends a package over given port
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len)
{
struct pios_com_dev * com_dev = PIOS_COM_find_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 (len >= fifoBuf_getFree(&com_dev->tx)) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
PIOS_IRQ_Disable();
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len);
PIOS_IRQ_Enable();
if (bytes_into_fifo > 0) {
/* More data has been put in the tx buffer, make sure the tx is started */
if (com_dev->driver->tx_start) {
com_dev->driver->tx_start(com_dev->lower_id,
fifoBuf_getUsed(&com_dev->tx));
}
}
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 = PIOS_COM_find_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);
PIOS_IRQ_Disable();
uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len);
PIOS_IRQ_Enable();
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);
}
/**
* returns number of used bytes in transmit buffer
* \param[in] USART USART name
* \return number of used bytes
* \return 0 if USART not available
*/
static int32_t PIOS_USART_TxBufferUsed(uint32_t usart_id)
{
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;
bool valid = PIOS_USART_validate(usart_dev);
PIOS_Assert(valid)
return (fifoBuf_getUsed(&usart_dev->tx));
}
data_type fifoBuf_getFree(t_fifo_buffer *buf)
{
// return the free space size in the buffer
data_type buf_size = buf->buf_size;
data_type num_bytes = fifoBuf_getUsed(buf);
return ((buf_size - num_bytes) - 1);
}
uint16_t fifoBuf_getFree(t_fifo_buffer *buf)
{ // return the free space size in the buffer
uint16_t buf_size = buf->buf_size;
uint16_t num_bytes = fifoBuf_getUsed(buf);
return ((buf_size - num_bytes) - 1);
}
/**
* @brief Get data from fifo
* @param [out] buffer pointer to a @ref pios_bma180_data structure to receive data
* @return 0 for success, -1 for failure (no data available)
*/
int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data * buffer)
{
if(PIOS_BMA180_Validate(dev) != 0)
return -1;
if(fifoBuf_getUsed(&dev->fifo) < sizeof(*buffer))
return -1;
fifoBuf_getData(&dev->fifo, (uint8_t *) buffer, sizeof(*buffer));
return 0;
}
/**
* Get the number of bytes waiting in the buffer
* \param[in] port COM port
* \return Number of bytes used in buffer
*/
int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
PIOS_Assert(0);
}
PIOS_Assert(com_dev->has_rx);
return (fifoBuf_getUsed(&com_dev->rx));
}
/**
* 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 number of bytes transmitted on success
*/
int32_t PIOS_COM_SendBuffer(uintptr_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);
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_SendBufferNonBlocking(com_id, buffer, frag_size);
if (rc >= 0) {
bytes_to_send -= rc;
buffer += rc;
} else {
switch (rc) {
case -1:
/* 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) {
return -3;
}
#endif
continue;
default:
/* Unhandled return code */
return rc;
}
}
}
return len;
}
int16_t fifoBuf_getBytePeek(t_fifo_buffer *buf)
{ // get a data byte from the buffer without removing it
uint16_t rd = buf->rd;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes < 1)
return -1; // no byte returned
return buf->buf_ptr[rd]; // return the byte
}
int8_t fifoBuf_getBytePeek(t_fifo_buffer *buf)
{
// get a data byte from the buffer without removing it
data_type rd = buf->rd;
// get number of bytes available
data_type num_bytes = fifoBuf_getUsed(buf);
if(num_bytes < 1)
{
return -1;
}
return buf->buf_ptr[rd];
}
/**
* Gets a byte from the receive buffer
* \param[in] USART USART name
* \return -1 if no new byte available
* \return >= 0: actual byte received
*/
static int32_t PIOS_USART_RxBufferGet(uint32_t usart_id)
{
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;
bool valid = PIOS_USART_validate(usart_dev);
PIOS_Assert(valid)
if (fifoBuf_getUsed(&usart_dev->rx) == 0) {
/* Nothing new in the buffer */
return -1;
}
/* get byte - this operation should be atomic! */
uint8_t b = fifoBuf_getByte(&usart_dev->rx);
/* Return received byte */
return b;
}
uint8_t processRX() {
if (ProgPort == Usb) {
while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) {
for (int32_t x = 0; x < 63; ++x) {
mReceive_Buffer[x] = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB);
}
processComand(mReceive_Buffer);
}
} else if (ProgPort == Serial) {
if (fifoBuf_getUsed(&ssp_buffer) >= 63) {
for (int32_t x = 0; x < 63; ++x) {
mReceive_Buffer[x] = fifoBuf_getByte(&ssp_buffer);
}
processComand(mReceive_Buffer);
}
}
return TRUE;
}
void fifoBuf_removeData(t_fifo_buffer *buf, uint16_t len)
{ // remove a number of bytes from the buffer
uint16_t rd = buf->rd;
uint16_t buf_size = buf->buf_size;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes > len)
num_bytes = len;
if (num_bytes < 1)
return; // nothing to remove
rd += num_bytes;
if (rd >= buf_size)
rd -= buf_size;
buf->rd = rd;
}
int16_t fifoBuf_getByte(t_fifo_buffer *buf)
{ // get a data byte from the buffer
uint16_t rd = buf->rd;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes < 1)
return -1; // no byte returned
uint8_t b = buff[rd];
if (++rd >= buf_size)
rd = 0;
buf->rd = rd;
return b; // return the byte
}
uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len)
{ // get data from our rx buffer
uint16_t rd = buf->rd;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes > len)
num_bytes = len;
if (num_bytes < 1)
return 0; // return number of bytes copied
uint8_t *p = (uint8_t *)data;
uint16_t i = 0;
while (num_bytes > 0)
{
uint16_t j = buf_size - rd;
if (j > num_bytes)
j = num_bytes;
memcpy(p + i, buff + rd, j);
i += j;
num_bytes -= j;
rd += j;
if (rd >= buf_size)
rd = 0;
}
buf->rd = rd;
return i; // return number of bytes copied
}
int8_t fifoBuf_getByte(t_fifo_buffer *buf)
{
// get data byte from buffer
data_type rd = buf->rd;
data_type buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
// get number of bytes available
data_type num_bytes = fifoBuf_getUsed(buf);
if(num_bytes < 1)
{
return -1;
}
uint8_t b = buff[rd];
if(++rd >= buf_size)
{
rd = 0;
}
buf->rd = rd;
return b;
}
