#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]));}