/*************************************************************************************************************
 * fir.c
 *************************************************************************************************************
 */
#include <std.h>
#include <sys.h>
#include <tsk.h>
#include <mem.h>
#include <stdlib.h>

#include <math.h>
#include <dsplib.h>
#include <string.h>

#include "mailbox.h"
#include "tokliBIOS.h"

extern void firasm(DATA *in, DATA *coef, DATA *out, int taps, int loops);

static Uns fir_bksnd(struct dsptask *task, Uns bid, Uns cnt);
static Uns fir_tctl(struct dsptask *task, Uns ctlcmd);

static void fir_init(struct dsptask *task);
static void fir_exec(struct dsptask *task);
static void fir_mem_alloc(struct dsptask *task);

static unsigned short filter_stat;
static float *in_buf_ptr = NULL;	// Ptr to input data
static float *out_buf_ptr = NULL;	// Ptr to output data
static DATA *in_q15_ptr = NULL;		// Ptr to Q15 data for input
static DATA *out_q15_ptr = NULL;	// Ptr to Q15 data for output

static float *coef_buf_ptr = NULL;	// Ptr to coefficient
static DATA *coef_q15_ptr = NULL;	// Ptr to Q15 coefficient

static Uns num_of_data = 0;
static Uns num_of_coef = 0;

/*
 *	IOCTL commands
 */
#define FIR_INIT		0x0080	// Initialization
#define FIR_NUM_DATA	0x0081	// Num of data
#define FIR_NUM_TAPS	0x0082	// Num of taps
#define FIR_WRITE_DATA	0x0083	// Write data
#define FIR_WRITE_COEF	0x0084	// Write coefficient
#define FIR_MEM_ALLOC	0x0085	// Memory allocation

/*
* MPEG4 decoder task: normal priority
*/
static struct TSK_Attrs fir_attr = {
	DSPTASK_DEFAULT_PRIORITY,		// priority
	NULL,							// stack
	DSPTASK_DEFAULT_STACKSIZE,		// stacksize
	DSPTASK_DEFAULT_SYSSTACKSIZE,	// sysstacksize
	0,								// stackseg: can't be overridden
	NULL,							// environ
	NULL,							// name: can't be overridden
	TRUE							// exitflag
};

struct dsptask task_fir = {
	TID_MAGIC,	// tid
	(String)"fir",	// name
	MBCMD_TTYP_BKDM | MBCMD_TTYP_BKMD | MBCMD_TTYP_ASND,
			// ttyp: active block snd, passive block rcv
	fir_bksnd,
	NULL,
	fir_tctl,
	&fir_attr
};

struct dsptask *dbg_task=NULL;
void dbg_print(int n)
{
	Char errmsg[80];

	if (dbg_task == NULL) return;
	
	SYS_sprintf(errmsg, (String)"%d\n", n);
	dbg(dbg_task, errmsg);
	return;
}

void dbg_dump(struct dsptask *task, char *title, void *p, int words)
{
	int i;
	Char s[10], msg[80];
	unsigned short *sp;

	SYS_sprintf(msg, (String)"%s\n", title);
	dbg(task, msg);

	sp = (unsigned short *)p;
	
	for (i=0; i<words; i++) {
		if (i!=0 && (i%8)==0) {
			dbg(task, msg);
		}
		SYS_sprintf(s, (String)"%04x ", sp[i]);
		if ((i%8)==0)
			strcpy(msg, s);
		else
			strcat(msg, s);
	}
	
	if (i%8)
		dbg(task, msg);
	
	return;
}

void memcpy32(void *dst, void *src, unsigned int n)
{
	register int i;
	unsigned long lv = (unsigned long)src;
	unsigned long *lp, *lq;

	if (n<2)
		return; // error!
		
	lv++;
	lp = (unsigned long *)dst;
	lq = (unsigned long *)lv;

	for (i=0; i<(n/2); i++)
		*lp++ = *lq++;
		
	return;		
}

static Uns fir_bksnd(struct dsptask *task, Uns bid, Uns cnt)
{
	Char errmsg[80];
	Uns num;
	
	switch (filter_stat) {

	case FIR_NUM_DATA:
		num_of_data = *(Uns *)ipbuf_d[bid];
		break;
		
	case FIR_NUM_TAPS:
		num_of_coef = *(Uns *)ipbuf_d[bid];
		break;

	case FIR_WRITE_DATA:
		if (cnt/2 > num_of_data) {
			SYS_sprintf(errmsg, (String)"fir_bksnd: memory allocation failure (cnt > num_of_data)!\n");
			dbg(task, errmsg);
			unuse_ipbuf(task, bid);
			return (Uns)-1;
		}

		memcpy32(in_buf_ptr, ipbuf_d[bid], cnt);
		fltoq15(in_buf_ptr, in_q15_ptr, num_of_data); // Covert float format to Q15 format
		
		fir_exec(task);

		num = num_of_data - num_of_coef + 1;
		q15tofl(out_q15_ptr, out_buf_ptr, num);	// Convert Q15 format to float format
		memcpy32(ipbuf_d[bid], out_buf_ptr, num * 2);

		bksnd(task, bid, num * 2);

		return 0;
	
	case FIR_WRITE_COEF:
		if (cnt/2 > num_of_coef) {
			SYS_sprintf(errmsg, (String)"fir_bksnd: memory allocation failure (cnt > num_of_coef)!\n");
			dbg(task, errmsg);
			unuse_ipbuf(task, bid);
			return (Uns)-1;
		}

		memcpy32(coef_buf_ptr, ipbuf_d[bid], cnt);
		fltoq15(coef_buf_ptr, coef_q15_ptr, num_of_coef);	// Covert float format to Q15 format

		break;
	
	default:
		SYS_sprintf(errmsg, (String)"fir_bksnd: invalid status\n");
		dbg(task, errmsg);
		break;
		
	}
	
	unuse_ipbuf(task, bid);

	return 0;
}

static Uns fir_tctl(struct dsptask *task, Uns ctlcmd)
{
	Char errmsg[80];

	filter_stat = ctlcmd;

	switch(ctlcmd) {

	case MBCMD_TCTL_TINIT:
		break;

	case FIR_INIT:
		fir_init(task);
		break;

	case FIR_NUM_DATA:
	case FIR_NUM_TAPS:
	case FIR_WRITE_DATA:
	case FIR_WRITE_COEF:
		break;
		
	case FIR_MEM_ALLOC:
		fir_mem_alloc(task);
		break;

	default:
		// Undefined IOCTL command
		SYS_sprintf(errmsg, (String)"fir_tctl: Undefined IOCTL command: %d\n", ctlcmd);
		dbg(task, errmsg);
		break;
	}
	return 0;
}

void fir_init(struct dsptask *task)
{
	dbg_task = task;

	if (in_buf_ptr != NULL)
		free(in_buf_ptr);
		
	if (out_buf_ptr != NULL)
		free(out_buf_ptr);
		
	if (in_q15_ptr != NULL)
		free(in_q15_ptr);
		
	if (out_q15_ptr != NULL)
		free(out_q15_ptr);
		
	if (coef_buf_ptr != NULL)
		free(coef_buf_ptr);
		
	if (coef_q15_ptr != NULL)
		free(coef_q15_ptr);

	return;
}

void fir_exec(struct dsptask *task)
{
	firasm(in_q15_ptr, coef_q15_ptr, out_q15_ptr, num_of_coef, (num_of_data - num_of_coef) / 2);
	return;
}

void fir_mem_alloc(struct dsptask *task)
{
	Char errmsg[80];

	in_buf_ptr = (float *)malloc(sizeof(float) * num_of_data);
	if (in_buf_ptr == NULL) {
		SYS_sprintf(errmsg, (String)"fir_mem_alloc: memory allocation failure (in_buf_ptr)!\n");
		goto err_fir_mem_alloc;
	}

	out_buf_ptr = (float *)malloc(sizeof(float) * num_of_data);
	if (out_buf_ptr == NULL) {
		SYS_sprintf(errmsg, (String)"fir_mem_alloc: memory allocation failure (out_buf_ptr)!\n");
		goto err_fir_mem_alloc;
	}

	in_q15_ptr = (DATA *)malloc(sizeof(DATA) * num_of_data);
	if (in_q15_ptr == NULL) {
		SYS_sprintf(errmsg, (String)"fir_mem_alloc: memory allocation failure (in_q15_ptr)!\n");
		goto err_fir_mem_alloc;
	}

	out_q15_ptr = (DATA *)malloc(sizeof(DATA) * num_of_data);
	if (out_q15_ptr == NULL) {
		SYS_sprintf(errmsg, (String)"fir_mem_alloc: memory allocation failure (out_q15_ptr)!\n");
		goto err_fir_mem_alloc;
	}

	coef_buf_ptr = (float *)malloc(sizeof(float) * num_of_coef);
	if (coef_buf_ptr == NULL) {
		SYS_sprintf(errmsg, (String)"fir_mem_alloc: memory allocation failure (coef_buf_ptr)!\n");
		goto err_fir_mem_alloc;
	}

	coef_q15_ptr = (DATA *)malloc(sizeof(DATA) * num_of_coef);
	if (coef_q15_ptr == NULL) {
		SYS_sprintf(errmsg, (String)"fir_mem_alloc: memory allocation failure (coef_q15_ptr)!\n");
		goto err_fir_mem_alloc;
	}

	return;
	
err_fir_mem_alloc:
	dbg(task, errmsg);
	return;
}






;*************************************************************************************************************
;* firasm.asm
;*************************************************************************************************************
;*
    .def _firasm
	.cpl_on
	.arms_on
	.c54cm_off


	.text

; FIR filter
_firasm:

	PSH mmap(@ST1_55)	; Save status registers
	PSH mmap(@ST2_55)
	
	BCLR C54CM		; Clear C54 compatible mode
	BSET SXMD		; Set sign extension mode
	BSET ST1_M40		; Set 40 bit mode
	BSET FRCT		; Set fractional mode
	

	MOV XAR0,XAR3		; XAR0 points to first input data
	ADD #1,AR3		; XAR3 points to second input data

	MOV XAR1, XCDP		; Set AR1 as circular buffer for Coefficients
	MOV AR1, mmap(@BSAC)	; Set start address of circular buffer
	MOV T0, mmap(@BKC)	; Set size of circular buffer
	MOV #0, CDP
	BSET CDPLC

	SUB #3, T0

	MOV T0, CSR		; Set inner loop count
	MOV T1, BRC0		; Set outer loop count

	RPTBLOCAL end 
	
	  MPY *AR0+, *CDP+, AC0
::	  MPY *AR3+, *CDP+, AC1
||	  RPT CSR
	    MAC *AR0+, *CDP+, AC0
::	    MAC *AR3+, *CDP+, AC1
	
	  MAC *(AR0-T0), *CDP+, AC0
::	  MAC *(AR3-T0), *CDP+, AC1

	  
end: MOV pair(HI(AC0)), dbl(*AR2+)

	POP mmap(@ST2_55)
 	POP mmap(@ST1_55)
	
	RET





/*************************************************************************************************************
 * sysinfo.c
 *************************************************************************************************************
 */
#include <std.h>
#include "tokliBIOS.h"

/*
 *  task information
 */

#define	N_TASK	1
Int	_n_task = N_TASK;

extern struct dsptask task_fir;

struct dsptask	*task_user[N_TASK] = {
	&task_fir
};

/*
 * ipbuf configuration
 */

#define	IPBUF_SZ	0x200
#define	IPBUF_LINES	4
#define	IPBUF_KEEP	2

Uns	_ipbuf_sz    = IPBUF_SZ;
Uns	_ipbuf_lines = IPBUF_LINES;
Uns	_ipbuf_keep  = IPBUF_KEEP;

#pragma DATA_SECTION(_ipbuf_body, "ipbuf")
Int	_ipbuf_body[(IPBUF_HEADER_SZ+(LgUns)IPBUF_SZ)*IPBUF_LINES];




/*************************************************************************************************************
 * fir.cmd
 *************************************************************************************************************
 */
SECTIONS {
        ipbuf: align=0x4    {} > SARAM
        dbgbuf: {} > SARAM
}





/*************************************************************************************************************
 * fir_app.c
 *************************************************************************************************************
 */
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <math.h>

/* ioctl for fir dsptask device */
#define FIR_INIT	0x0080	// Initialization
#define FIR_NUM_DATA	0x0081	// Num of data
#define FIR_NUM_TAPS	0x0082	// Num of taps
#define FIR_WRITE_DATA	0x0083	// Write data
#define FIR_WRITE_COEF	0x0084	// Write coefficient
#define FIR_MEM_ALLOC	0x0085	// Memory allocation

float coef[15] = {
2.138261220088319e-03, 
6.334857723412623e-03, 
-3.947855266034449e-18, 
-3.312179298358531e-02, 
-4.006136998985733e-02, 
7.734675062134248e-02, 
2.889400004322881e-01, 
4.000000000000000e-01, 
2.889400004322881e-01, 
7.734675062134248e-02, 
-4.006136998985733e-02, 
-3.312179298358531e-02, 
-3.947855266034449e-18, 
6.334857723412623e-03, 
2.138261220088319e-03
};

#define IN_FILE	"in"
#define OUT_FILE "out"

void generate_data(FILE *fp, float *x, int n)
{
	int i;
	float dx;
	float dn;
						
	dx = 2.0 * 2.0 * 3.1415 / (float)n;
	dn = 100.0 * 2.0 * 3.1415 / (float)n;

	for (i=0; i<n; i++) {
		x[i] = 0.6 * sin(dx * i) + 0.4 * cos(dn * i); 
		fprintf(fp, "%f\n", x[i]); 
	}

	return;			
}

void save_data(FILE *fp, float *y, int n)
{
	int i;

	for (i=0; i<n; i++) {
		fprintf(fp, "%f\n", y[i]);
	}
	return;
}

int main(int argc, char *argv[])
{
	float *x, *y;
	short taps=15;
	short n;
	int fd;
	FILE *ifp, *ofp;

	if (argc > 2) {
		printf("Usage: %s <count>\n", argv[0]);
		return -1;
	}

	ifp = fopen(IN_FILE, "w");
	if (ifp == NULL) {
		perror("generating input data");
		return -1;
	}

	if (argc == 2) {
		n = atoi(argv[1]);
	}
	else { // argc == 1
		n = 200;
	}

	x = (float *)malloc(sizeof(float) * n);
	if (x == NULL) {
		perror("Memory allocation error");
		return -1;
	}

	y = (float *)malloc(sizeof(float) * (n - taps + 1));
	if (x == NULL) {
		perror("Memory allocation error");
		return -1;
	}

	generate_data(ifp, x, n);

	fclose(ifp);

	ifp = fopen(IN_FILE, "r");
	if (ifp == NULL) {
		perror("input data file");
		return -1;
	}
	ofp = fopen(OUT_FILE, "w");
	if (ifp == NULL) {
		perror("output data file");
		return -1;
	}

	fd = open("/dev/dsptask/fir", O_RDWR);
	if (fd == -1) {
		perror("open dsptask device file");
		return -1;
	}
	ioctl(fd, FIR_INIT);

	ioctl(fd, FIR_NUM_DATA);
	write(fd, &n, sizeof(short));

	ioctl(fd, FIR_NUM_TAPS);
	write(fd, &taps, sizeof(short));

	ioctl(fd, FIR_MEM_ALLOC);

	ioctl(fd, FIR_WRITE_COEF);
	write(fd, &coef[0], sizeof(float) * taps);
	ioctl(fd, FIR_WRITE_DATA);

	write(fd, &x[0], sizeof(float) * n);

	read(fd, &y[0], sizeof(float) * (n - taps + 1));

	save_data(ofp, y, n - taps + 1);

	fclose(ifp);
	fclose(ofp);

	close(fd);
}