int bluetooth_input(FAR struct radio_driver_s *radio,
FAR struct iob_s *framelist,
FAR struct bluetooth_frame_meta_s *meta)
{
FAR struct bluetooth_conn_s *conn;
FAR struct iob_s *frame;
FAR struct iob_s *next;
int ret = OK;
/* Check if there is a connection that will accept this packet */
conn = bluetooth_conn_active(meta);
if (conn != NULL)
{
/* Setup for the application callback (NOTE: These should not be
* used by PF_BLUETOOTH sockets).
*/
radio->r_dev.d_appdata = radio->r_dev.d_buf;
radio->r_dev.d_len = 0;
radio->r_dev.d_sndlen = 0;
/* The framelist probably contains only a single frame, but we will
* process it as a list of frames.
*/
for (frame = framelist; frame != NULL; frame = next)
{
/* Remove the frame from the list */
next = frame->io_flink;
frame->io_flink = NULL;
/* Add the frame to the RX queue */
ret = bluetooth_queue_frame(conn, frame, meta);
if (ret < 0)
{
nerr("ERROR: Failed to queue frame: %d\n", ret);
iob_free(frame);
}
}
/* Perform the application callback. The frame may be processed now
* if there is a user wait for an incoming frame. Or it may pend in
* the RX queue until some user process reads the frame. NOTE: The
* return value from bluetooth_callback would distinguish these
* cases: BLUETOOTH_NEWDATA will still be processed if the frame
* was not consumed.
*/
(void)bluetooth_callback(radio, conn, BLUETOOTH_NEWDATA);
}
else
{
nwarn("WARNING: No listener\n");
}
return ret;
}
void iob_free_chain(FAR struct iob_s *iob)
{
FAR struct iob_s *next;
/* Free each IOB in the chain -- one at a time to keep the count straight */
for (; iob; iob = next)
{
next = iob_free(iob);
}
}
FAR struct iob_s *iob_trimtail(FAR struct iob_s *iob, unsigned int trimlen)
{
FAR struct iob_s *entry;
FAR struct iob_s *penultimate;
FAR struct iob_s *last;
unsigned int iosize;
int len;
nlldbg("iob=%p pktlen=%d trimlen=%d\n", iob, iob->io_pktlen, trimlen);
if (iob && trimlen > 0)
{
len = trimlen;
/* Loop until complete the trim */
while (len > 0)
{
/* Calculate the total length of the data in the I/O buffer
* chain and find the last entry in the chain.
*/
penultimate = NULL;
last = NULL;
iosize = 0;
for (entry = iob; entry; entry = entry->io_flink)
{
/* Accumulate the total size of all buffers in the list */
iosize += entry->io_len;
/* Remember the last and the next to the last in the chain */
penultimate = last;
last = entry;
}
/* Trim from the last entry in the chain. Do we trim this entire
* I/O buffer away?
*/
nllvdbg("iob=%p len=%d vs %d\n", last, last->io_len, len);
if (last->io_len <= len)
{
/* Yes.. Consume the entire buffer */
iob->io_pktlen -= last->io_len;
len -= last->io_len;
last->io_len = 0;
/* Free the last, empty buffer in the list */
iob_free(last);
/* There should be a buffer before this one */
if (!penultimate)
{
/* No.. we just freed the head of the chain */
return NULL;
}
/* Unlink the penultimate from the freed buffer */
penultimate->io_flink = NULL;
}
else
{
/* No, then just take what we need from this I/O buffer and
* stop the trim.
*/
iob->io_pktlen -= len;
last->io_len -= len;
len = 0;
}
}
}
return iob;
}
static int bluetooth_queue_frame(FAR struct bluetooth_conn_s *conn,
FAR struct iob_s *frame,
FAR struct bluetooth_frame_meta_s *meta)
{
FAR struct bluetooth_container_s *container;
/* Allocate a container for the frame */
container = bluetooth_container_allocate();
if (container == NULL)
{
nerr("ERROR: Failed to allocate a container\n");
return -ENOMEM;
}
/* Initialize the container */
memset(&container->bn_raddr, 0, sizeof(bt_addr_t));
container->bn_channel = meta->bm_channel;
BLUETOOTH_ADDRCOPY(&container->bn_raddr, &meta->bm_raddr);
DEBUGASSERT(frame != NULL);
container->bn_iob = frame;
/* Add the container to the tail of the list of incoming frames */
container->bn_flink = NULL;
if (conn->bc_rxtail == NULL)
{
conn->bc_rxhead = container;
}
else
{
conn->bc_rxtail->bn_flink = container;
}
#if CONFIG_NET_BLUETOOTH_BACKLOG > 0
/* If incrementing the count would exceed the maximum bc_backlog value, then
* delete the oldest frame from the head of the RX queue.
*/
if (conn->bc_backlog >= CONFIG_NET_BLUETOOTH_BACKLOG)
{
DEBUGASSERT(conn->bc_backlog == CONFIG_NET_BLUETOOTH_BACKLOG);
/* Remove the container from the tail RX input queue. */
container = conn->bc_rxhead;
DEBUGASSERT(container != NULL);
conn->bc_rxhead = container->bn_flink;
container->bn_flink = NULL;
/* Did the RX queue become empty? */
if (conn->bc_rxhead == NULL)
{
conn->bc_rxtail = NULL;
}
DEBUGASSERT(container != NULL && container->bn_iob != NULL);
/* Free both the IOB and the container */
iob_free(container->bn_iob);
bluetooth_container_free(container);
}
else
{
/* Increment the count of frames in the queue. */
conn->bc_backlog++;
}
DEBUGASSERT((int)conn->bc_backlog == bluetooth_count_frames(conn));
#endif
return OK;
}
static ssize_t ieee802154_recvfrom_rxqueue(FAR struct radio_driver_s *radio,
FAR struct ieee802154_recvfrom_s *pstate)
{
FAR struct ieee802154_container_s *container;
FAR struct sockaddr_ieee802154_s *iaddr;
FAR struct ieee802154_conn_s *conn;
FAR struct iob_s *iob;
size_t copylen;
int ret = -EAGAIN;
/* Check if there is anyting in in the RX input queue */
DEBUGASSERT(pstate != NULL && pstate->ir_sock != NULL);
conn = (FAR struct ieee802154_conn_s *)pstate->ir_sock->s_conn;
DEBUGASSERT(conn != NULL);
if (conn->rxhead != NULL)
{
/* Remove the container from the RX input queue. */
container = conn->rxhead;
DEBUGASSERT(container != NULL);
conn->rxhead = container->ic_flink;
container->ic_flink = NULL;
/* Did the RX queue become empty? */
if (conn->rxhead == NULL)
{
conn->rxtail = NULL;
}
#if CONFIG_NET_IEEE802154_BACKLOG > 0
/* Decrement the count of frames in the queue. */
DEBUGASSERT(conn->backlog > 0);
conn->backlog--;
DEBUGASSERT((int)conn->backlog == ieee802154_count_frames(conn));
#endif
/* Extract the IOB containing the frame from the container */
iob = container->ic_iob;
container->ic_iob = NULL;
DEBUGASSERT(iob != NULL);
/* Copy the new packet data into the user buffer */
copylen = iob->io_len - iob->io_offset;
memcpy(pstate->ir_buffer, &iob->io_data[iob->io_offset], copylen);
ninfo("Received %d bytes\n", (int)copylen);
ret = copylen;
/* If a 'from' address poiner was supplied, copy the source address
* in the container there.
*/
if (pstate->ir_from != NULL)
{
iaddr = (FAR struct sockaddr_ieee802154_s *)pstate->ir_from;
iaddr->sa_family = AF_IEEE802154;
memcpy(&iaddr->sa_addr, &container->ic_src, sizeof(struct ieee802154_saddr_s));
}
/* Free both the IOB and the container */
iob_free(iob);
ieee802154_container_free(container);
}
return ret;
}
FAR struct iob_s *iob_trimhead(FAR struct iob_s *iob, unsigned int trimlen)
{
uint16_t pktlen;
ninfo("iob=%p trimlen=%d\n", iob, trimlen);
if (iob && trimlen > 0)
{
/* Trim from the head of the I/IO buffer chain */
pktlen = iob->io_pktlen;
while (trimlen > 0 && iob != NULL)
{
/* Do we trim this entire I/O buffer away? */
ninfo("iob=%p io_len=%d pktlen=%d trimlen=%d\n",
iob, iob->io_len, pktlen, trimlen);
if (iob->io_len <= trimlen)
{
FAR struct iob_s *next;
/* Decrement the trim length and packet length by the full
* data size.
*/
pktlen -= iob->io_len;
trimlen -= iob->io_len;
/* Check if this was the last entry in the chain */
next = iob->io_flink;
if (next == NULL)
{
/* Yes.. break out of the loop returning the empty
* I/O buffer chain containing only one empty entry.
*/
DEBUGASSERT(pktlen == 0);
iob->io_len = 0;
iob->io_offset = 0;
break;
}
/* Free this entry and set the next I/O buffer as the head */
ninfo("iob=%p: Freeing\n", iob);
(void)iob_free(iob);
iob = next;
}
else
{
/* No, then just take what we need from this I/O buffer and
* stop the trim.
*/
pktlen -= trimlen;
iob->io_len -= trimlen;
iob->io_offset += trimlen;
trimlen = 0;
}
}
/* Adjust the pktlen by the number of bytes removed from the head
* of the I/O buffer chain.
*/
iob->io_pktlen = pktlen;
}
return iob;
}