198 lines
6.8 KiB
C
198 lines
6.8 KiB
C
#ifndef __FIFO__
|
|
#define __FIFO__
|
|
#include <string.h> //memcpy
|
|
#include "fifo_types.h"
|
|
|
|
//Asynch FIFO implementaion
|
|
//Note these are in the include file to allow the compiler to inline for performance
|
|
|
|
///////////////////////////////////////
|
|
//Shared memory FIFO (sample by sample)
|
|
//Can be any size
|
|
///////////////////////////////////////
|
|
|
|
|
|
static inline unsigned fifo_get_fill(volatile mem_fifo_t * unsafe fifo) {
|
|
unsafe{
|
|
unsigned fifo_fill = 0;
|
|
if (fifo->write_idx >= fifo->read_idx){
|
|
fifo_fill = fifo->write_idx - fifo->read_idx;
|
|
}
|
|
else{
|
|
fifo_fill = (fifo->size + fifo->write_idx) - fifo->read_idx;
|
|
}
|
|
return fifo_fill;
|
|
}
|
|
}
|
|
|
|
static inline unsigned fifo_get_fill_short(volatile mem_fifo_short_t * unsafe fifo) {
|
|
unsafe{
|
|
unsigned fifo_fill = 0;
|
|
if (fifo->write_idx >= fifo->read_idx){
|
|
fifo_fill = fifo->write_idx - fifo->read_idx;
|
|
}
|
|
else{
|
|
fifo_fill = (fifo->size + fifo->write_idx) - fifo->read_idx;
|
|
}
|
|
return fifo_fill;
|
|
}
|
|
}
|
|
|
|
#pragma unsafe arrays
|
|
static inline fifo_ret_t fifo_block_push(volatile mem_fifo_t * unsafe fifo, int data[], unsigned n) {
|
|
unsafe{
|
|
//check there is a block of space large enough
|
|
unsigned space_remaining = fifo->size - fifo_get_fill(fifo) - 1;
|
|
if (n > space_remaining) {
|
|
return FIFO_FULL;
|
|
}
|
|
for (int i = 0; i < n; i++){
|
|
unsigned next_idx = fifo->write_idx + 1;
|
|
if (next_idx == fifo->size) next_idx = 0; //Check for wrap
|
|
fifo->data_base_ptr[fifo->write_idx] = data[i];
|
|
fifo->write_idx = next_idx;
|
|
}
|
|
return FIFO_SUCCESS;
|
|
}
|
|
}
|
|
|
|
#pragma unsafe arrays
|
|
static inline fifo_ret_t fifo_block_push_short(volatile mem_fifo_short_t * unsafe fifo, short data[], unsigned n) {
|
|
unsafe{
|
|
//check there is a block of space large enough
|
|
unsigned space_remaining = fifo->size - fifo_get_fill_short(fifo) - 1;
|
|
if (n > space_remaining) {
|
|
return FIFO_FULL;
|
|
}
|
|
for (int i = 0; i < n; i++){
|
|
unsigned next_idx = fifo->write_idx + 1;
|
|
if (next_idx == fifo->size) next_idx = 0; //Check for wrap
|
|
fifo->data_base_ptr[fifo->write_idx] = data[i];
|
|
fifo->write_idx = next_idx;
|
|
}
|
|
return FIFO_SUCCESS;
|
|
}
|
|
}
|
|
|
|
#pragma unsafe arrays
|
|
static inline fifo_ret_t fifo_block_push_short_fast(volatile mem_fifo_short_t * unsafe fifo, short data[], unsigned n) {
|
|
unsafe{
|
|
//check there is a block of space large enough
|
|
unsigned space_remaining = fifo->size - fifo_get_fill_short(fifo) - 1;
|
|
if (n > space_remaining) {
|
|
return FIFO_FULL;
|
|
}
|
|
//We will write either one or two blocks depending on wrap
|
|
unsigned first_block_size = 0;
|
|
unsigned second_block_size = 0;
|
|
|
|
//See if we need to wrap during block writes
|
|
unsigned space_left_at_top = fifo->size - fifo->write_idx;
|
|
//printf("space_left_at_top %d\n", space_left_at_top);
|
|
//Yes, we do need to wrap
|
|
if (n > space_left_at_top){
|
|
first_block_size = space_left_at_top;
|
|
second_block_size = n - space_left_at_top;
|
|
memcpy(&fifo->data_base_ptr[fifo->write_idx], &data[0], first_block_size * sizeof(short));
|
|
memcpy(&fifo->data_base_ptr[0], &data[first_block_size], second_block_size * sizeof(short));
|
|
fifo->write_idx = second_block_size;
|
|
}
|
|
//No wrap, do all in one go
|
|
else{
|
|
first_block_size = n;
|
|
second_block_size = 0;
|
|
memcpy(&fifo->data_base_ptr[fifo->write_idx], &data[0], first_block_size * sizeof(short));
|
|
fifo->write_idx += first_block_size;
|
|
}
|
|
|
|
return FIFO_SUCCESS;
|
|
}
|
|
}
|
|
|
|
#pragma unsafe arrays
|
|
static inline fifo_ret_t fifo_block_pop(volatile mem_fifo_t * unsafe fifo, int data[], unsigned n) {
|
|
unsafe{
|
|
//Check we have a block big enough to send
|
|
if (n > fifo_get_fill(fifo)){
|
|
return FIFO_EMPTY;
|
|
}
|
|
for (int i = 0; i < n; i++){
|
|
data[i] = fifo->data_base_ptr[fifo->read_idx];
|
|
fifo->read_idx++;
|
|
if (fifo->read_idx == fifo->size) fifo->read_idx = 0; //Check for wrap
|
|
}
|
|
return FIFO_SUCCESS;
|
|
}
|
|
}
|
|
|
|
#pragma unsafe arrays
|
|
static inline fifo_ret_t fifo_block_pop_short(volatile mem_fifo_short_t * unsafe fifo, short data[], unsigned n) {
|
|
unsafe{
|
|
//Check we have a block big enough to send
|
|
if (n > fifo_get_fill_short(fifo)){
|
|
return FIFO_EMPTY;
|
|
}
|
|
for (int i = 0; i < n; i++){
|
|
data[i] = fifo->data_base_ptr[fifo->read_idx];
|
|
fifo->read_idx++;
|
|
if (fifo->read_idx == fifo->size) fifo->read_idx = 0; //Check for wrap
|
|
}
|
|
return FIFO_SUCCESS;
|
|
}
|
|
}
|
|
|
|
#pragma unsafe arrays
|
|
static inline fifo_ret_t fifo_block_pop_short_fast(volatile mem_fifo_short_t * unsafe fifo, short data[], unsigned n) {
|
|
unsafe{
|
|
//Check we have a block big enough to send
|
|
if (n > fifo_get_fill_short(fifo)){
|
|
return FIFO_EMPTY;
|
|
}
|
|
//We will read either one or two blocks depending on wrap
|
|
unsigned first_block_size = 0;
|
|
unsigned second_block_size = 0;
|
|
|
|
//See if we need to wrap during block read
|
|
unsigned num_read_at_top = fifo->size - fifo->read_idx;
|
|
// printf("num_read_at_top %d\n", num_read_at_top);
|
|
//Yes, we do need to wrap
|
|
if (n > num_read_at_top){
|
|
first_block_size = num_read_at_top;
|
|
second_block_size = n - num_read_at_top;
|
|
memcpy(&data[0], &fifo->data_base_ptr[fifo->read_idx], first_block_size * sizeof(short));
|
|
memcpy( &data[first_block_size], &fifo->data_base_ptr[0], second_block_size * sizeof(short));
|
|
fifo->read_idx = second_block_size;
|
|
// printf("wrap\n");
|
|
}
|
|
//No wrap, do all in one go
|
|
else{
|
|
first_block_size = n;
|
|
second_block_size = 0;
|
|
memcpy(&data[0], &fifo->data_base_ptr[fifo->read_idx], first_block_size * sizeof(short));
|
|
fifo->read_idx += first_block_size;
|
|
// printf("no wrap\n");
|
|
|
|
}
|
|
|
|
return FIFO_SUCCESS;
|
|
}
|
|
}
|
|
|
|
//Version of above that returns fill level relative to half full
|
|
static inline int fifo_get_fill_relative_half(volatile mem_fifo_t * unsafe fifo){
|
|
unsafe{
|
|
int fifo_fill = (int)fifo_get_fill(fifo);
|
|
fifo_fill -= (fifo->size / 2);
|
|
return fifo_fill;
|
|
}
|
|
}
|
|
|
|
//Version of above that returns fill level relative to half full
|
|
static inline int fifo_get_fill_relative_half_short(volatile mem_fifo_short_t * unsafe fifo){
|
|
unsafe{
|
|
int fifo_fill = (int)fifo_get_fill_short(fifo);
|
|
fifo_fill -= (fifo->size / 2);
|
|
return fifo_fill;
|
|
}
|
|
}
|
|
#endif |