//----------------------------------------------------------------
fifocell * fifoget (fifo * ff)
{
fifocell * volatile head;
fifocell * next;
short done = 0;
do {
LWARX (&ff->head);
head = ff->head; /* read the head cell */
next = head->link; /* read the next cell */
/*
WARNING: the next pointer still needs to be checked before reading its value
*/
if (head == ff->tail) { /* is queue empty or tail falling behind? */
if (head->link == fifo_end(ff) && STWCX (&ff->head, (void *)head, (void *)head)) /* is queue really empty? */
return 0; /* queue is empty; return NULL */
/* tail is pointing to head in a non empty queue, */
/* try to set tail to the next cell */
CASLNE (&ff->tail, (void *)head, fifo_end(ff));
}
else if (next != fifo_end(ff)) { /* if we are not competing on the dummy cell */
/* and we try to set head to the next cell */
done = STWCX (&ff->head, (void *)head, next);
}
} while (!done);
msAtomicDec (&ff->count);
if (head == &ff->dummy) {
fifoput(ff,head);
head = fifoget(ff);
}
return (fifocell *)head;
}
//----------------------------------------------------------------
fifocell * fifoget(fifo * ff)
{
fifocell * volatile head;
fifocell * next;
unsigned long ic, oc;
short done = 0;
do {
oc = ff->oc; /* read the head modification count */
ic = ff->ic; /* read the tail modification count */
head = ff->head; /* read the head cell */
next = head->link; /* read the next cell */
if (oc == ff->oc) { /* ensures that next is a valid pointer */
/* to avoid failure when reading next value */
if (head == ff->tail) { /* is queue empty or tail falling behind? */
if (next == fifo_end(ff)) /* is queue empty? */
return 0; /* queue is empty; return NULL */
/* tail is pointing to head in a non empty queue, */
/* try to set tail to the next cell */
CAS2 (&ff->tail, head, ic, next, ic+1);
}
else if (next != fifo_end(ff)) { /* if we are not competing on the dummy cell */
/* and we try to set head to the next cell */
done = CAS2 (&ff->head, head, oc, next, oc+1);
}
}
} while (!done);
msAtomicDec (&ff->count);
if (head == &ff->dummy) {
fifoput(ff,head);
head = fifoget(ff);
}
return (fifocell *)head;
}
static void nrf24l01_worker(FAR void *arg)
{
FAR struct nrf24l01_dev_s *dev = (FAR struct nrf24l01_dev_s *) arg;
uint8_t status;
uint8_t fifo_status;
nrf24l01_lock(dev->spi);
status = nrf24l01_readregbyte(dev, NRF24L01_STATUS);
if (status & NRF24L01_RX_DR)
{
/* put CE low */
bool ce = nrf24l01_chipenable(dev, false);
wdbg("RX_DR is set!\n");
/* Read and store all received payloads */
do
{
uint8_t pipeno;
uint8_t pktlen;
uint8_t buf[NRF24L01_MAX_PAYLOAD_LEN];
/* For each packet:
* - Get pipe #
* - Get payload length (either static or dynamic)
* - Read payload content
*/
pipeno = (status & NRF24L01_RX_P_NO_MASK) >> NRF24L01_RX_P_NO_SHIFT;
pktlen = dev->pipedatalen[pipeno];
if (NRF24L01_DYN_LENGTH == pktlen)
{
/* If dynamic length payload need to use R_RX_PL_WID command to get actual length */
nrf24l01_access(dev, MODE_READ, NRF24L01_R_RX_PL_WID, &pktlen, 1);
}
/* Get payload content */
nrf24l01_access(dev, MODE_READ, NRF24L01_R_RX_PAYLOAD, buf, pktlen);
fifoput(dev, pipeno, buf, pktlen);
sem_post(&dev->sem_rx); /* Wake-up any thread waiting in recv */
status = nrf24l01_readreg(dev, NRF24L01_FIFO_STATUS, &fifo_status, 1);
wdbg("FIFO_STATUS=%02x\n", fifo_status);
wdbg("STATUS=%02x\n", status);
}
while (!(fifo_status | NRF24L01_RX_EMPTY));
/* Clear interrupt sources */
nrf24l01_writeregbyte(dev, NRF24L01_STATUS, NRF24L01_RX_DR);
/* Restore CE */
nrf24l01_chipenable(dev, ce);
#ifndef CONFIG_DISABLE_POLL
if (dev->pfd)
{
dev->pfd->revents |= POLLIN; /* Data available for input */
wvdbg("Wake up polled fd");
sem_post(dev->pfd->sem);
}
#endif
}
