fifo.c

嵌入式LIUNX的流媒体控制器程序哦 号可以

#include <sched.h>
#include <stdlib.h>
#include <sys/ipc.h>
#include <sys/shm.h>

#include "fifo.h"

#define	MAX_BUFLEN	256
#define	FIFO_MAGIC	0x45671234;

/* Spin-lock parameter */
#define	MAX_SPIN_COUNT		50
#define	SPIN_SLEEP_DURATION	2000001

/* Tag parameter */
#define	MAX_TAGS			16

/* Local variable */
const char *name_prefix = "/tmp/";

/* Local data structure */
typedef	volatile int	fifo_spinlock_t;
typedef	volatile int	fifo_tag_t;

#pragma pack(1)
struct fifo_header_t {
	int	magic;

	int	shmid;

	fifo_spinlock_t		rlock;
	fifo_spinlock_t		wlock;

	int	head;
	int	tail;

	int	nslot;
	int	slotsz;

	fifo_tag_t			tags[MAX_TAGS];

	int	dummy[32];
};

struct slot_header_t {
	short	size;
};

#pragma pack()

/* Local functions */
inline void tag_trigger(fifo_tag_t *tag)
{
	__asm__ __volatile__ (
		"movl	$1, %0"
		: "=m" (*tag));
}

inline int tag_testandset(fifo_tag_t *tag)
{
	int ret;
	__asm__ __volatile__ (
		"xchgl	%0, %1"
		: "=r" (ret), "=m" (*tag)
		: "0" (0), "m"(*tag)
		: "memory");
	return ret;
}

inline int testandset(fifo_spinlock_t *lock)
{
	int ret;
	__asm__ __volatile__ (
		"xchgl	%0, %1"
		: "=r" (ret), "=m" (*lock)
		: "0" (1), "m"(*lock)
		: "memory");
	return ret;
}

static int fifo_spinlock_acquire(fifo_spinlock_t *lock, int noblock)
{
	int cnt = 0;
	struct timespec tm;

	while (testandset(lock)) {
		if(noblock)
			return -1;

		if (cnt < MAX_SPIN_COUNT) {
			sched_yield();
			cnt++;
		} else {
			tm.tv_sec = 0;
			tm.tv_nsec = SPIN_SLEEP_DURATION;
			nanosleep(&tm, NULL);
			cnt = 0;
		}
	}

	return 0;
}

static int fifo_spinlock_release(fifo_spinlock_t *lock)
{
	__asm__ __volatile__ (
		"movl	$0, %0"
		: "=m" (*lock));
	
	return 0;
}

static key_t fifo_calc_key(const char *name)
{
	key_t fkey;
	char bufname[MAX_BUFLEN];

	/* Generate IPC key */
	memset(bufname , 0, MAX_BUFLEN);
	strcpy(bufname, name_prefix);
	strncat(bufname, name, MAX_BUFLEN - strlen(name_prefix) - 4);
	fkey = ftok(bufname, 'T');

	return fkey;
}

/* Create and destroy FIFO */
HFIFO fifo_create(const char *fifoname, int nslot, size_t slotsz)
{
	int shmid;
	HFIFO handle;
	struct fifo_header_t *header;

	/* Allocate shared memory */
	shmid = shmget(fifo_calc_key(fifoname),
			sizeof(struct fifo_header_t)
			+ nslot*(slotsz + sizeof(struct slot_header_t))
			+ 16,
			IPC_CREAT | SHM_R | SHM_W);

	if(shmid == -1)
	{
		perror("Allocating FIFO");
		/* exit(-1); */

		return NULL;
	}

	handle = shmat(shmid, 0, 0);
	if((int) handle == -1)
	{
		perror("Mapping memory");
		/* exit(-1); */

		return NULL;
	}

	/* Initialize FIFO header */
	header = (struct fifo_header_t*) handle;

	header->magic	= FIFO_MAGIC;
	header->shmid	= shmid;
	header->rlock	= 0;
	header->wlock	= 0;

	header->head	= 0;
	header->tail	= 0;

	header->nslot	= nslot;
	header->slotsz	= slotsz;

	return handle;
}

void fifo_destroy(HFIFO handle)
{
	struct fifo_header_t *header;

	/* Assign pointer */
	header 	    = (struct fifo_header_t*) handle;

	shmctl(header->shmid, IPC_RMID, NULL);
}

/* Get an existing FIFO handle */
HFIFO fifo_find_handle(const char *fifoname, size_t *pslotsz)
{
	int shmid;
	HFIFO handle;

	shmid = shmget(fifo_calc_key(fifoname),
			0, SHM_R | SHM_W);

	if(shmid == -1)
	{
		perror("Allocating FIFO");
		/* exit(-1); */
		return NULL;
	}
	
	handle = shmat(shmid, 0, 0);
	if((int) handle == -1)
	{
		perror("Mapping memory");
		/* exit(-1); */
		return NULL;
	}

	return handle;
}

/* Put and get data from FIFO */
int fifo_put(HFIFO handle, void *data, size_t size, int flag)
{
	int rv;
	struct fifo_header_t *header;
	unsigned char *buf_base;

	/* Assign pointer */
	header 	    = (struct fifo_header_t*) handle;
	buf_base    = ((unsigned char*) handle) + sizeof(struct fifo_header_t);

	/* Try to acquire lock for writing buffer */
	if(flag & FIFO_FLG_NOBLOCK)
	{
		if(fifo_spinlock_acquire(&(header->wlock), 1) != 0)
		{
			return -1;
		}
	}
	else
	{
		fifo_spinlock_acquire(&(header->wlock), 0);
	}

	/* Put data into fifo */
	{
		int new_head;
		unsigned char *fbuf;
		struct slot_header_t *hdr;
		
		new_head = (header->head + 1) % header->nslot;

		/* Check whether FIFO is full */
		if(new_head == header->tail)
		{
			rv = E_FIFO_FULL;
			goto ret;
		}

		if(size > header->slotsz)
			return E_FIFO_OVERSIZE;

		hdr  = (struct slot_header_t*) (buf_base
			+ (header->slotsz + sizeof(struct slot_header_t)) * header->head);
		fbuf = ((unsigned char*)hdr) + sizeof(struct slot_header_t);

		memcpy(fbuf, data, size);
		hdr->size = size;
		rv = size;

		header->head = new_head;
	}

ret:
	/* Release lock */
	fifo_spinlock_release(&(header->wlock));

	return rv;
}

int fifo_get(HFIFO handle, void *buf,  size_t size, int flag)
{
	int rv;

	struct fifo_header_t *header;
	unsigned char *buf_base;

	/* Assign pointer */
	header 	    = (struct fifo_header_t*) handle;
	buf_base    = ((unsigned char*) handle) + sizeof(struct fifo_header_t);

	/* Try to acquire lock for writing buffer */
	if(flag & FIFO_FLG_NOBLOCK)
	{
		if(fifo_spinlock_acquire(&(header->rlock), 1) != 0)
		{
			return -1;
		}
	}
	else
	{
		fifo_spinlock_acquire(&(header->rlock), 0);
	}

	/* Put data into fifo */
	{
		int new_tail;
		unsigned char *fbuf;
		struct slot_header_t *hdr;
		
		new_tail = (header->tail + 1) % header->nslot;

		/* Check whether FIFO is full */
		if(header->head == header->tail)
		{
			rv = E_FIFO_EMPTY;
			goto ret;
		}

		hdr  = (struct slot_header_t*) (buf_base
				+ (header->slotsz + sizeof(struct slot_header_t)) * header->tail);
		fbuf = ((unsigned char*)hdr) + sizeof(struct slot_header_t);

		if(hdr->size > size)
		{
			rv = E_FIFO_OVERSIZE;
			goto ret;
		}

		memcpy(buf, fbuf, hdr->size);
		rv = hdr->size;

		header->tail = new_tail;
	}

ret:
	/* Release lock */
	fifo_spinlock_release(&(header->rlock));

	return rv;
}

/* Tag trigger and reset */
int fifo_tag_trigger(HFIFO handle, int tagid)
{
	struct fifo_header_t *header;

	/* Assign pointer */
	header 	    = (struct fifo_header_t*) handle;

	if(tagid < 0 || tagid >= MAX_TAGS)
		return -1;
	
	tag_trigger(&(header->tags[tagid]));

	return 0;
}

int fifo_tag_check(HFIFO handle, int tagid)
{
	struct fifo_header_t *header;

	/* Assign pointer */
	header 	    = (struct fifo_header_t*) handle;

	if(tagid < 0 || tagid >= MAX_TAGS)
		return -1;
	
	return tag_testandset(&(header->tags[tagid]));
}