#include <stdio.h>#include <stdlib.h>#include <string.h>#include <sys/time.h>#include <time.h>#include <memory.h>#include "fifo.h"#include "gosvr.h"#include "godev.h"static int allow_run = 1;static int PACKAGESIZE = 1024;static struct timeval tv_polling_interval = {0, 100000};static HFIFO slot_fifo;static unsigned char buffer[200000];static unsigned char outputBuffer[200000];static void gosvr_init(){	slot_fifo=fifo_create("slotfifo", 512, PACKAGESIZE+64);	godev_open(0);	godev_open(1);	godev_open(2);	godev_open(3);}static void gosvr_exit(){	godev_close(3);	godev_close(2);	godev_close(1);	godev_close(0);	fifo_destroy(slot_fifo);}static void show_interval(){	static struct timeval last = {0,0};	struct timeval current, delta;	gettimeofday(&current, NULL);	printf("Timestamp: %d.%06d  ", current.tv_sec, current.tv_usec);	if(last.tv_sec != 0)	{		timersub(&current, &last, &delta);		printf("Delta: %d.%06d", delta.tv_sec, delta.tv_usec);	}	last.tv_sec	 = current.tv_sec;	last.tv_usec = current.tv_usec;	printf("\n");}static int RepackageData(unsigned char * pOut, unsigned char * pIn, int size, int streamid){	int i;	int packageNum=0;	int dataSize=0;	for(i=0;i<size;i+=(PACKAGESIZE-8))	{		pOut[packageNum*PACKAGESIZE+0]=0x00;		pOut[packageNum*PACKAGESIZE+1]=streamid;		pOut[packageNum*PACKAGESIZE+2]=0x00;                pOut[packageNum*PACKAGESIZE+3]=0x01;                pOut[packageNum*PACKAGESIZE+4]=0x00;                pOut[packageNum*PACKAGESIZE+5]=0x00;		if(size-i>PACKAGESIZE-8)			dataSize=PACKAGESIZE-8;		else			dataSize=size-i;                pOut[packageNum*PACKAGESIZE+6]=dataSize>>8;		pOut[packageNum*PACKAGESIZE+7]=dataSize%256;		memcpy(pOut+packageNum*PACKAGESIZE+8, pIn+i, dataSize);		packageNum++;	}		pOut[4] = pOut[4] | 0x04;	pOut[(packageNum-1)*PACKAGESIZE+4]= pOut[(packageNum-1)*PACKAGESIZE+4] | 0x01;	//dbg_print("Repackaged.\n");	return packageNum;}static int SendToSharedMemory(unsigned char * pOut, int packageNum){	int i;//	dbg_print("Sent to Memory.\n");	for(i=0;i<packageNum;i++)		fifo_put(slot_fifo, pOut+i*PACKAGESIZE, PACKAGESIZE, i); 	fifo_tag_trigger(slot_fifo, 1);	return 0;}static int gosvr_quantum_operation(){	int i,j;	int packageNum;	static int val = 0;//	show_interval();/*	{		for(i = 0; i < 500; i++)		{			for(j = 0; j < 10000; j++)			{				val++;			}		}	}*/	/* Get stream from device */	for(i=0;i<4;i++)	{		j=godev_get_fs_frame(i, buffer, 200000);	        if(j!=0)		{			packageNum=RepackageData(outputBuffer, buffer, j, 0x10+i);			SendToSharedMemory(outputBuffer, packageNum);        		printf("Big Stream   %d, size %d, \tpackages: %d.\n", i, j, packageNum);		}		j=godev_get_ss_frame(i, buffer, 200000);		if(j!=0)		{			packageNum=RepackageData(outputBuffer, buffer, j, 0x00+i);			SendToSharedMemory(outputBuffer, packageNum);			printf("Small Stream %d, size %d, \tpackages: %d.\n", i, j, packageNum);		}	}		/* Necessory processing */	/* Multicasting out */	return val;}void gosvr_run(){	/* Initialization */	gosvr_init();	/* Polling loop */	{		struct timeval tv_epoch, tv_cur, tv_mark, tv_sleep;		struct timespec tm_sleep;		gettimeofday(&tv_epoch, NULL);		tv_cur.tv_sec  = tv_mark.tv_sec  = tv_epoch.tv_sec;		tv_cur.tv_usec = tv_mark.tv_usec = tv_epoch.tv_usec;		while(allow_run)		{			gosvr_quantum_operation();			/* Calculate polling parameter */			timeradd(&tv_mark, &tv_polling_interval, &tv_mark);			gettimeofday(&tv_cur, NULL);			if(timercmp(&tv_mark, &tv_cur, <)) continue;			timersub(&tv_mark, &tv_cur, &tv_sleep);			tm_sleep.tv_sec	 = tv_sleep.tv_sec;			tm_sleep.tv_nsec = tv_sleep.tv_usec * 1000;			nanosleep(&tm_sleep, NULL);		}	}	/* Exit */	gosvr_exit();}