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;
}
data_type fifoBuf_putData(t_fifo_buffer *buf, const void *data, data_type len)
{ // add data to the buffer
data_type wr = buf->wr;
data_type buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
data_type num_bytes = fifoBuf_getFree(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 - wr;
if (j > num_bytes)
j = num_bytes;
memcpy(buff + wr, p + i, j);
i += j;
num_bytes -= j;
wr += j;
if (wr >= buf_size)
wr = 0;
}
buf->wr = wr;
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;
}
/**
* 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);
}
/**
* Initialises COM layer
* \param[out] handle
* \param[in] driver
* \param[in] id
* \return < 0 if initialisation failed
*/
int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len)
{
PIOS_Assert(com_id);
PIOS_Assert(driver);
bool has_rx = (rx_buffer && rx_buffer_len > 0);
bool has_tx = (tx_buffer && tx_buffer_len > 0);
PIOS_Assert(has_rx || has_tx);
PIOS_Assert(driver->bind_tx_cb || !has_tx);
PIOS_Assert(driver->bind_rx_cb || !has_rx);
struct pios_com_dev *com_dev;
com_dev = (struct pios_com_dev *)PIOS_COM_alloc();
if (!com_dev) {
goto out_fail;
}
com_dev->driver = driver;
com_dev->lower_id = lower_id;
com_dev->has_rx = has_rx;
com_dev->has_tx = has_tx;
if (has_rx) {
fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len);
#if defined(PIOS_INCLUDE_FREERTOS)
vSemaphoreCreateBinary(com_dev->rx_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
(com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, (uint32_t)com_dev);
if (com_dev->driver->rx_start) {
/* Start the receiver */
(com_dev->driver->rx_start)(com_dev->lower_id,
fifoBuf_getFree(&com_dev->rx));
}
}
if (has_tx) {
fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len);
#if defined(PIOS_INCLUDE_FREERTOS)
vSemaphoreCreateBinary(com_dev->tx_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
(com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, (uint32_t)com_dev);
}
#if defined(PIOS_INCLUDE_FREERTOS)
com_dev->sendbuffer_sem = xSemaphoreCreateMutex();
#endif /* PIOS_INCLUDE_FREERTOS */
*com_id = (uint32_t)com_dev;
return 0;
out_fail:
return -1;
}
/**
* Send the buffer over UART
* return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* return number of bytes transmitted on success
*/
int8_t Serial_SendBufferNonBlocking(const uint8_t *buffer,
uint16_t len)
{
if (len > fifoBuf_getFree(&tx_buffer))
{
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
cli();
uint8_t bytes_into_fifo = fifoBuf_putData(&tx_buffer, buffer, len);
sei();
if (bytes_into_fifo > 0)
{
/* More data has been put in the tx buffer, make sure the tx is started */
UCSR0B |= _BV(UDRIE0);
}
return (bytes_into_fifo);
}
data_type fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b)
{ // add a data byte to the buffer
data_type wr = buf->wr;
data_type buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
data_type num_bytes = fifoBuf_getFree(buf);
if (num_bytes < 1)
return 0;
buff[wr] = b;
if (++wr >= buf_size)
wr = 0;
buf->wr = wr;
return 1; // return number of bytes copied
}
/**
* puts a byte onto the receive buffer
* \param[in] USART USART name
* \param[in] b byte which should be put into Rx buffer
* \return 0 if no error
* \return -1 if buffer full (retry)
*/
static int32_t PIOS_USART_RxBufferPut(uint32_t usart_id, uint8_t b)
{
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;
bool valid = PIOS_USART_validate(usart_dev);
PIOS_Assert(valid)
if (fifoBuf_getFree(&usart_dev->rx) < 1) {
/* Buffer full (retry) */
return -1;
}
/* Copy received byte into receive buffer */
/* This operation should be atomic! */
fifoBuf_putByte(&usart_dev->rx, b);
/* No error */
return 0;
}
/**
* Transfer bytes from port buffers into another buffer
* \param[in] port COM port
* \returns Byte from buffer
*/
uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms)
{
PIOS_Assert(buf);
PIOS_Assert(buf_len);
uint16_t bytes_from_fifo;
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) */
PIOS_Assert(0);
}
PIOS_Assert(com_dev->has_rx);
check_again:
bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len);
if (bytes_from_fifo == 0) {
/* No more bytes in receive buffer */
/* Make sure the receiver is running while we wait */
if (com_dev->driver->rx_start) {
/* Notify the lower layer that there is now room in the rx buffer */
(com_dev->driver->rx_start)(com_dev->lower_id,
fifoBuf_getFree(&com_dev->rx));
}
if (timeout_ms > 0) {
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) {
/* Make sure we don't come back here again */
timeout_ms = 0;
goto check_again;
}
#else
PIOS_DELAY_WaitmS(1);
timeout_ms--;
goto check_again;
#endif
}
}
/* Return received byte */
return bytes_from_fifo;
}
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);
}
bool PIOS_BMA180_IRQHandler(void)
{
bma180_irqs++;
const static uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
uint8_t pios_bma180_dmabuf[8];
// If we can't get the bus then just move on for efficiency
bool woken = false;
if(PIOS_BMA180_ClaimBusISR(&woken) != 0) {
return woken; // Something else is using bus, miss this data
}
PIOS_SPI_TransferBlock(dev->spi_id,pios_bma180_req_buf,(uint8_t *) pios_bma180_dmabuf,
sizeof(pios_bma180_dmabuf), NULL);
// TODO: Make this conversion depend on configuration scale
struct pios_bma180_data data;
// Don't release bus till data has copied
PIOS_BMA180_ReleaseBus(&woken);
// Must not return before releasing bus
if(fifoBuf_getFree(&dev->fifo) < sizeof(data))
return woken;
// Bottom two bits indicate new data and are constant zeros. Don't right
// shift because it drops sign bit
data.x = ((pios_bma180_dmabuf[2] << 8) | pios_bma180_dmabuf[1]);
data.y = ((pios_bma180_dmabuf[4] << 8) | pios_bma180_dmabuf[3]);
data.z = ((pios_bma180_dmabuf[6] << 8) | pios_bma180_dmabuf[5]);
data.x /= 4;
data.y /= 4;
data.z /= 4;
data.temperature = pios_bma180_dmabuf[7];
fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data));
return woken;
}
