#include "ucos_dev.h"
#include "bos.h"
#include "ucos_ii.h"
#include "serial.h"
#include "bkni.h"
#include "bcmmemmgr.h"
#include "ministd.h"
#include <sys/errno.h>
#include <sys/time.h>
#include <sys/times.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>


int ucos_fork_r(struct _reent *r);
int ucos_kill_r(struct _reent *r, int pid, int sig);
int ucos_execve_r(struct _reent *r, const char *, char *const *, char *const *);
int ucos_wait_r(struct _reent *r, int *);
void ucos_exit(int ret);
int ucos_gettimeofday_r(struct _reent *r, struct timeval *tp, void *tzp);
_CLOCK_T_ ucos_times_r(struct _reent *, struct tms *);
int ucos_getpid_r(struct _reent *r);
int ucos_creat_r(struct _reent *r, const char *path, int mode);
void* ucos_sbrk_r(struct _reent *r, ptrdiff_t ptrdiff);

int  ucos_com_open_r( struct _reent *r, const char *path, int flags, int mode );
int  ucos_com_close_r( struct _reent *r, int fd );
long ucos_com_write_r( struct _reent *r, int fd, const char *ptr, int len );
long ucos_com_read_r( struct _reent *r, int fd, char *ptr, int len );
off_t ucos_com_lseek_r(struct _reent *r, int fd,  off_t offset, int whence);
int ucos_com_fstat_r(struct _reent *r, int fd, struct stat *buf);

static sys_table_t g_sys_table =
{
	ucos_fork_r,
	ucos_kill_r,
	ucos_execve_r,
	ucos_wait_r,
	ucos_exit,
	ucos_gettimeofday_r,
	ucos_times_r,
	ucos_getpid_r,
	ucos_creat_r,
	ucos_sbrk_r,
};

sys_table_t *g_p_sys_table = &g_sys_table;

static io_device_table_t g_com0_sys_table =
{
	"stdin",
	NULL,
	ucos_com_open_r,
	ucos_com_close_r,
	ucos_com_write_r,
	ucos_com_read_r,
	ucos_com_lseek_r,
	ucos_com_fstat_r,
};
static io_device_table_t g_com1_sys_table =
{
	"stdout",
	NULL,
	ucos_com_open_r,
	ucos_com_close_r,
	ucos_com_write_r,
	ucos_com_read_r,
	ucos_com_lseek_r,
	ucos_com_fstat_r,
};
static io_device_table_t g_com2_sys_table =
{
	"stderr",
	NULL,
	ucos_com_open_r,
	ucos_com_close_r,
	ucos_com_write_r,
	ucos_com_read_r,
	ucos_com_lseek_r,
	ucos_com_fstat_r,
};

io_device_table_t *g_device_tables[] = 
{
	&g_com0_sys_table,
	&g_com1_sys_table,
	&g_com2_sys_table,
	NULL
};



extern io_device_table_t *g_device_tables[];

int ucos_fork_r(struct _reent *r)
{
	__errno_r(r) = ENOTSUP;
	return -1;
}
int ucos_kill_r(struct _reent *r, int pid, int sig)
{
	__errno_r(r) = ENOTSUP;
	return -1;
}
int ucos_execve_r(struct _reent *r, const char *path, char *const *argv, char *const *envp)
{
	__errno_r(r) = ENOTSUP;
	return -1;
}
int ucos_wait_r(struct _reent *r, int *status)
{
	__errno_r(r) = ENOTSUP;
	return -1;
}

void ucos_exit(int ret)
{
	__asm__("sdbbp");
}
#define do_div(n,base) ({ \
	int __res; \
	__res = ((unsigned long) (n)) % (unsigned) (base); \
	(n) = ((unsigned long) (n)) / (unsigned) (base); \
	__res; })
	
#define do_div64_32(res, high, low, base) ({ \
	unsigned long __quot, __mod; \
	unsigned long __cf, __tmp, __tmp2, __i; \
	\
	__asm__(".set	push\n\t" \
		".set	noat\n\t" \
		".set	noreorder\n\t" \
		"move	%2, $0\n\t" \
		"move	%3, $0\n\t" \
		"b	1f\n\t" \
		" li	%4, 0x21\n" \
		"0:\n\t" \
		"sll	$1, %0, 0x1\n\t" \
		"srl	%3, %0, 0x1f\n\t" \
		"or	%0, $1, %5\n\t" \
		"sll	%1, %1, 0x1\n\t" \
		"sll	%2, %2, 0x1\n" \
		"1:\n\t" \
		"bnez	%3, 2f\n\t" \
		"sltu	%5, %0, %z6\n\t" \
		"bnez	%5, 3f\n\t" \
		"2:\n\t" \
		" addiu	%4,%4,-1\n\t" \
		"subu	%0, %0, %z6\n\t" \
		"addiu	%2, %2, 1\n" \
		"3:\n\t" \
		"bnez	%4, 0b\n\t" \
		" srl	%5, %1, 0x1f\n\t" \
		".set	pop" \
		: "=&r" (__mod), "=&r" (__tmp), "=&r" (__quot), "=&r" (__cf), \
		  "=&r" (__i), "=&r" (__tmp2) \
		: "Jr" (base), "0" (high), "1" (low)); \
	\
	(res) = __quot; \
	__mod; })
	
static unsigned 
b_div64_32(unsigned *res, unsigned high, unsigned low, unsigned base)
{
	unsigned res_;
	unsigned rem;
	rem = do_div64_32(res_, high, low, base);
	*res = res_;
	return rem;
}

int ucos_gettimeofday_r(struct _reent *r, struct timeval *tp, void *tzp)
{
#if 1
	b_clock clock;
	long long usec_ticks;

	/* get running clock */
	bos_getclock(&clock);
    if(clock.clock_freq>0) {
        usec_ticks = b_div64_32((unsigned *)&tp->tv_sec, clock.clock_hi, clock.clock_low, clock.clock_freq);
        usec_ticks *= (1000000/64); /* scale usec ticks */
        b_div64_32((unsigned *)&tp->tv_usec, usec_ticks>>32, usec_ticks, (clock.clock_freq/64));
    } else {
        tp->tv_usec = 0;
        tp->tv_sec = 0;
    }
#endif
	return 0;
}

extern struct b_task_stats *bos_taskinfo(int priority);

_CLOCK_T_ ucos_times_r(struct _reent *r, struct tms *tms)
{
	b_cpu_load load;
	struct b_task_stats * p_stats = bos_taskinfo(ucos_getpid_r(r));
	bos_getload(&load);
	if (!p_stats || !tms)
	{
		__errno_r(r)	= ENODEV;
		return -1;
	}
	memset(tms,0,sizeof(*tms));
	tms->tms_utime = p_stats->clock_cnt;		/* user time */
	tms->tms_stime = load.duration;
	tms->tms_cutime = load.idle;
	tms->tms_cstime = load.isr;
	return bos_getticks();
}

int ucos_getpid_r(struct _reent *r)
{
	OS_TCB tcb;
	
	int id;
	
	/* use our priority as a task id */
	OSTaskQuery( OS_PRIO_SELF, &tcb );
	id = tcb.OSTCBPrio;
	
	return id;
}

int ucos_creat_r(struct _reent *r, const char *path, int mode)
{
	__errno_r(r) = ENOTSUP;
	return -1;
}


void* ucos_sbrk_r(struct _reent *r, ptrdiff_t ptrdiff)
{
	abort();
	return (void*) NULL;
}

int  ucos_com_open_r( struct _reent *r, const char *path, int flags, int mode )
{
	return 0;
}
int  ucos_com_close_r( struct _reent *r, int fd )
{
	return 0;
}
extern int BKNI_Print_Write(const char *ptr, int len);

long ucos_com_write_r( struct _reent *r, int fd, const char *ptr, int len )
{

    return BKNI_Print_Write(ptr,len);
}

long ucos_com_read_r( struct _reent *r, int fd, char *ptr, int len )
{
	int ch;
	char *orig_ptr = ptr;

	do
	{
		ch = serial_getc(CONSOLE_UART,false);
		if ((ch != -1) && (ch != 0))
		{
			/* Don't print backspace if we have reached beginning of string input */
			if (ch == '\b')
			{
				if (ptr > orig_ptr)
				{
					serial_putc(CONSOLE_UART,'\b');
					serial_putc(CONSOLE_UART,' ');
					serial_putc(CONSOLE_UART,ch);
					ptr--;
				}
				continue;
			}
			serial_putc(CONSOLE_UART,ch);
		   	if ((ch != '\n') && (ch != '\r'))
			{
				*ptr++ = ch;
			}
		}else{
			bos_sleep(20);		/* let othg_p_sys_tableer task run */
		}
		if ((unsigned int)ptr >= (unsigned int)(&orig_ptr[len - 1]))
		{
			break;
		}
	} while ((ch != '\n') && (ch != '\r'));
	*ptr = 0;
	return (long)orig_ptr;
}
off_t ucos_com_lseek_r(struct _reent *r, int fd,  off_t offset, int whence)
{
	return 0;
}
int ucos_com_fstat_r(struct _reent *r, int fd, struct stat *buf)
{
	return 0;
}
_PTR _malloc_r (struct _reent *r, size_t size)
{
	return malloc(size);
}
_VOID _free_r (struct _reent *r, _PTR ptr)
{
	free(ptr);
}
_PTR _realloc_r (struct _reent *r,_PTR ptr, size_t size)
{
	_PTR tmpPtr = NULL;
	if (size == 0)
	{
		if (ptr)
		{
			free(ptr);
			return NULL;
		}
		return ptr;

	}
	
	tmpPtr = malloc(size);
	if (!tmpPtr)
	{
		return NULL;
	}
	
	if (!ptr)
		return tmpPtr;
	
	memcpy(tmpPtr,ptr,size);
	return tmpPtr;
}

_PTR _calloc_r (struct _reent *r,size_t nmbr, size_t size)
{
	return malloc(nmbr * size);
}
_PTR _memalign_r (struct _reent *r, size_t align, size_t size)
{
	return malloc_align(size,align);
}

#ifdef CONFIG_GP
void GP_putchar(char ch)
{
	putchar(ch);
}

int GP_getchar(void)
{
	return getchar();
}
#endif

struct _reent rtbl[OS_MAX_TASKS];