uint32_t fifo_push(fifo_t* p_fifo, const void* p_elem)
{
if (p_elem == NULL)
{
return NRF_ERROR_NULL;
}
uint32_t was_masked;
_DISABLE_IRQS(was_masked);
if (FIFO_IS_FULL(p_fifo))
{
_ENABLE_IRQS(was_masked);
return NRF_ERROR_NO_MEM;
}
void* p_dest = FIFO_ELEM_AT(p_fifo, p_fifo->head & (p_fifo->array_len - 1));
if (p_fifo->memcpy_fptr)
p_fifo->memcpy_fptr(p_dest, p_elem);
else
memcpy(p_dest, p_elem, p_fifo->elem_size);
++p_fifo->head;
_ENABLE_IRQS(was_masked);
return NRF_SUCCESS;
}
static int __sci_adi_read(u32 regPddr)
{
unsigned long val;
int cnt = 2000;
while (FIFO_IS_FULL() && cnt--) {
udelay(1);
}
WARN(cnt == 0, "ADI WAIT timeout!!!");
__raw_writel(regPddr, REG_ADI_RD_CMD);
/*
* wait read operation complete, RD_data[31] will be
* cleared after the read operation complete
*/
do {
val = __raw_readl(REG_ADI_RD_DATA);
} while ((val & BIT_RD_CMD_BUSY) && cnt--);
WARN(cnt == 0, "ADI READ timeout!!!");
/* val high part should be the address of the last read operation */
BUG_ON(TO_ADDR(val) != (regPddr & MASK_RD_ADDR));
return (val & MASK_RD_VALU);
}
static int __sci_adi_write(u32 reg, u16 val, u32 sync)
{
tail_p->reg = reg;
tail_p->val = val;
__p_add(&tail_p, TAIL_ADD);
while (!FIFO_IS_FULL() && (data_in_cache != 0)) {
__raw_writel(head_p->val, head_p->reg);
__p_add(&head_p, HEAD_ADD);
}
if (sync || data_in_cache == CACHE_SIZE) {
sci_adi_fifo_drain();
while (data_in_cache != 0) {
while(FIFO_IS_FULL()){
cpu_relax();
}
__raw_writel(head_p->val, head_p->reg);
__p_add(&head_p, HEAD_ADD);
}
sci_adi_fifo_drain();
}
return 0;
}
inline bool fifo_is_full(fifo_t* p_fifo)
{
return FIFO_IS_FULL(p_fifo);
}
