/***************************************************************
**
** Broadcom Corp. Confidential
** Copyright 1998-2000 Broadcom Corp. All Rights Reserved.
**
** THIS SOFTWARE MAY ONLY BE USED SUBJECT TO AN EXECUTED
** SOFTWARE LICENSE AGREEMENT BETWEEN THE USER AND BROADCOM.
** YOU HAVE NO RIGHT TO USE OR EXPLOIT THIS MATERIAL EXCEPT
** SUBJECT TO THE TERMS OF SUCH AN AGREEMENT.
**
** File:		bos.c
** Description: encapsulate OS functions.
**
** Created: Mon Sep 25 18:12:35 PDT 2006 by Jeff Fisher
**
**
**
****************************************************************/

#define BOS_C
#include "bos.h"
#include "ucos_ii.h"
#include "bchp_timer.h"
#include "bstd.h"
#include "bchp_sun_top_ctrl.h"
#include "bchp_xpt_fe.h"
#include "bcm_mips.h"
#include "bcm_mips_defs.h"
#include "ministd.h"
#include "bcmmemmgr.h"
#ifdef UCOS_USB
	#include "usbh_cfg.h"
#endif

BDBG_MODULE(bos);

struct b_task_stats g_os_task_stats[OS_LOWEST_PRIO+2];

extern uint32_t expt_stack; 
#define INIT_STACK_SIZE		0x2000
#define B_OS_STK_FILL_BYTE	0xBB
#define B_OS_STK_FILL_INT32 0xBBBBBBBB
extern const char *bsymtable_get_name(uint32_t addr, char *buf, size_t buf_len);

void b_os_check_stack(b_task_stats *task, uint32_t pattern) __attribute__((always_inline));
//unsigned int calc_mips_freq(void) __attribute__((always_inline));
extern unsigned long MIPS_SetInterrupts(unsigned long new_sr);
#define OS_DISABLE 0

extern void bint_handler(void);
b_system_globals_t g_dsp;
b_system_globals_t *g_p_dsp = &g_dsp;


struct b_last_ctxsw
{
	unsigned clock;	/* value of MIPS clock when task was switched in */
	unsigned instr;
} g_last_ctxsw;

static inline void dump(const char *name, const void *addr, size_t nelem)
{
	unsigned off;

	printf("%s: %u bytes from %p\n", name, nelem, addr);
	for (off=0;off<nelem;)
	{
		unsigned i;

		printf("0x%08X: ", off+(unsigned)addr);
		for (i=0;i<16 && off<nelem;i++,off++)
		{
			printf("%02x ", ((uint8_t *)addr)[off]);
		}
		printf("\n");
	}
	printf("-----------------------------------------\n");
	return;
}

/***************************************************************************
 * Determine the MIPS frequency in HZ using a known timer
 */
#if (BCHP_CHIP==7550)
#define TIMER_DIVISOR	1
#define TIMER_SHIFT	9
/* bits 32-9 of this register increment at 90000Hz by default */
#define CLOCK_REGISTER (BCHP_XPT_FE_PARSER0_TIMESTAMP | 0xB0000000)
#else
#define TIMER_DIVISOR	1
#define TIMER_SHIFT	9
#define CLOCK_REGISTER (BCHP_XPT_FE_ATS_TS_MOD300 | 0xB0000000)
#include "bchp_timer.h"
#ifndef BCHP_K0_OFFSET
#define BCHP_K0_OFFSET 	(0xA0000000 | 0x10000000)
#endif
#ifndef WriteReg32
#define WriteReg32(x,v)	{ *((volatile unsigned int*)(BCHP_K0_OFFSET + (unsigned int)(x))) = (v); }
#endif
#ifndef ReadReg32
#define ReadReg32(x)	*((volatile unsigned int*)(BCHP_K0_OFFSET + (unsigned int)(x)))
#endif
#endif

inline unsigned int calc_mips_freq(void)
{
#if (BCHP_CHIP==7550)
	/* we can't use register macros since BKNI is not initialized yet */
	volatile uint32_t * clock_ptr = (uint32_t *)CLOCK_REGISTER;
	uint32_t clock, end_clock;
	uint32_t count;
	
	clock = *clock_ptr >> TIMER_SHIFT;
	count = bcm_read_cp0($9,0);
	end_clock = clock + 90000/TIMER_DIVISOR;
	do
	{
		clock = *clock_ptr >> TIMER_SHIFT;
	}while (end_clock - clock);
	count = bcm_read_cp0($9,0) - count;
	return(count);
#else
	uint32_t tenth_of_a_second = 0x2932E0;	/* 27000000 */
	uint32_t start_count,end_count,freq;
	WriteReg32(BCHP_TIMER_TIMER0_CTRL,
			   tenth_of_a_second | BCHP_TIMER_TIMER0_CTRL_MODE_MASK | BCHP_TIMER_TIMER0_CTRL_ENA_MASK);
	
	start_count = bcm_read_cp0($9,0);
	
	WriteReg32(BCHP_TIMER_TIMER_IS,BCHP_TIMER_TIMER_IS_TMR0TO_MASK);

	do
	{
	}while (!(ReadReg32(BCHP_TIMER_TIMER_IS) & BCHP_TIMER_TIMER_IS_TMR0TO_MASK));

	end_count = bcm_read_cp0($9,0);
	freq = (end_count - start_count) * 10;
	return freq;
#endif
}


/******************************************************************************
* INPUTS:    none
* OUTPUTS:    none.
* RETURNS:    none
* FUNCTION: initialize ucos
******************************************************************************/
void bos_unhandled_handler(register_t * registers)
{
	g_p_dsp->b_in_interrupt = 1;
	OSIntEnter();

	gdb_handler((unsigned int*)registers, registers[ROFF_CAUSE]);

	OSIntExit();
	g_p_dsp->b_in_interrupt = 0;
}

/******************************************************************************
* INPUTS:    none
* OUTPUTS:    none.
* RETURNS:    none
* FUNCTION: initialize ucos
******************************************************************************/
void bos_handler(register_t * registers)
{
	register_t cause;
	register_t compare;
	register_t status;
	uint32_t clock;
	uint32_t exit_clock;
	uint32_t instr_enter;
	uint32_t instr_exit;
	struct b_task_stats *cur_stats;

	clock = bcm_read_cp0($9,0);
	instr_enter = bcm_read_cp0($25, 2);

	cur_stats = &g_os_task_stats[OSTCBCur->OSTCBPrio];

	status = registers[ROFF_SR];
	if (0 == (status & ST0_IE))
	{
		/* We are trying to enter critical section. Just return. */
		return;
	}

	cause = registers[ROFF_CAUSE];

	g_p_dsp->b_in_interrupt = 1;
	OSIntEnter();
	g_stat_isr_cnt++;

	cause = (cause >> 8) & 0xFF;
	if (cause & 0x80)
	{		  /* timer interrupt */
		bos_trace(eTRACE_TICK,OSPrioHighRdy,OSTCBCur->OSTCBPrio,clock);
		/* update 64 bit clock counter */
		if (clock <  g_running_clock.clock_low)
		{ /* overflow */
			g_running_clock.clock_hi++;
		}
		g_running_clock.clock_low = clock;
#if 0
		{
			compare = clock + OS_HW_TICKS;	/* 939391 with 375 MH frequency */
			
			__asm__("mtc0 %0, $11"::"r"(compare));
			//__asm__("nop");
		}
#else
		__asm__("mfc0 %0, $11":"=r"(compare));
		do
		{
			compare += OS_HW_TICKS;
		} while ((compare - clock) > OS_HW_TICKS);
		__asm__("mtc0 %0, $11"::"r"(compare));
		__asm__("nop");
#endif
		OSTimeTick();
	}
	if (0x4 & cause)
	{	 /* chip interrupt */
		unsigned long status;
		status = bcm_read_cp0($12,0);
		bcm_write_cp0($12, 0, (status & 0xFFFFFFFCUL));

		bint_handler();

		bcm_write_cp0($12, 0, status);
	}
	if (~0x84 & cause)
	{
		bos_unhandled_handler(registers);            
	}
	OSIntExit();
	exit_clock = bcm_read_cp0($9,0);
	instr_exit = bcm_read_cp0($25, 2);
	if (!(cause & 0x20))
	{
		g_stat_isr_time += exit_clock - clock; /* update isr time */
		g_p_dsp->stat_isr_cnt = instr_enter - instr_exit;
		cur_stats->clock_cnt -= exit_clock - clock;	/* adjust current task */
		cur_stats->instr_cnt -= instr_enter - instr_exit;
	}
	g_p_dsp->b_in_interrupt = 0;
}


/******************************************************************************
* INPUTS:    none
* OUTPUTS:    none.
* RETURNS:    none
* FUNCTION: initialize ucos
******************************************************************************/


static const char idle[]="idle";
static const char isr[]="ISR";
static const char stat[]="stat";
static const char usb_async[]="usb";

void bos_init(void)
{
	int i;
	unsigned int mips_freq;

	mips_freq = calc_mips_freq();

	memset(g_p_dsp,0,sizeof(b_system_globals_t));

	g_p_dsp->b_ticks_per_second = g_ticks_per_second;
	g_running_clock.clock_freq = mips_freq;
	g_cycles_per_tick = (mips_freq/g_ticks_per_second);
	OSRegisterException(0, bos_handler, NULL);
	for (i = 1; i < 32; ++i)
	{
		/* Skip Syscall */
		if (i != 8)
			OSRegisterException(i, bos_unhandled_handler, NULL);
	}

	/* wipe out task statistics */
	memset(g_os_task_stats, 0, sizeof(g_os_task_stats));

	/* initialize ISR pseudo task */
	g_os_task_stats[OS_LOWEST_PRIO+1].last_time =   g_os_task_stats[OS_LOWEST_PRIO].last_time;
	memcpy(g_os_task_stats[OS_LOWEST_PRIO+1].name, isr,  sizeof(isr));
	g_os_task_stats[OS_LOWEST_PRIO+1].stack = &expt_stack;
	g_os_task_stats[OS_LOWEST_PRIO+1].stack_size = INIT_STACK_SIZE/sizeof(uint32_t);
	g_os_task_stats[OS_LOWEST_PRIO+1].stack_left = INIT_STACK_SIZE/sizeof(uint32_t);
	g_os_task_stats[OS_LOWEST_PRIO+1].stack_fast_scan = 0;

	bcm_write_cp0($25, 6, 0x80000211); /* DCACHE and Instruction/Cycle counter */
	bcm_write_cp0($25, 4, 0x80288024);
	bcm_write_cp0($25, 5, 0x80488044);

	bcm_write_cp0($25, 0, 0)  /* PerfCount0 reset to 0 (it is a decrementing count) */
	bcm_write_cp0($25, 1, 0)  /* PerfCount1 reset to 0 (it is a decrementing count) */
	bcm_write_cp0($25, 2, 0)  /* PerfCount2 reset to 0 (it is a decrementing count) */
	bcm_write_cp0($25, 3, 0)  /* PerfCount3 reset to 0 (it is a decrementing count) */

	OSInit();

	return;
}


static struct
{
	b_timeval last_time;
	unsigned calib_idle_ticks;
	unsigned calib_ms;
	unsigned last_idle_ctr;
	unsigned last_ctxsw_ctr;
	unsigned last_isr_cnt;
	unsigned last_isr_time;
} b_os_load_data;

/* returns difference between two timevals in ms */
static unsigned
b_os_tv_diff(const b_timeval *end, const b_timeval *begin) 
{
	unsigned ms;

	ms = ((int)(end->tv_usec - begin->tv_usec))/1000;
	ms += (end->tv_sec - begin->tv_sec)*1000;
	if (ms==0)
	{
		ms=1;
	}
	return ms;
}

#ifdef CONFIG_ENABLE_EMU
	#define B_OS_IDLE_SCALE 10
#else
	#define B_OS_IDLE_SCALE 1
#endif

/* this function calibrates how fast IDLE task runs */
void
bos_calibrate(void)
{
	unsigned int flags;
#ifndef CONFIG_ENABLE_SIM
	b_timeval start;
	unsigned int idle_ctr;

	GETTIMEOFDAY(&start);
	idle_ctr = OSIdleCtr;
	bos_sleep(100/B_OS_IDLE_SCALE);
#endif
	GETTIMEOFDAY(&b_os_load_data.last_time);

	flags=bos_enter_critical();
	b_os_load_data.last_idle_ctr = OSIdleCtr;
	b_os_load_data.last_ctxsw_ctr = OSCtxSwCtr;
	b_os_load_data.last_isr_time = g_stat_isr_time;
	b_os_load_data.last_isr_cnt = g_stat_isr_cnt;
#ifndef CONFIG_ENABLE_SIM
	b_os_load_data.calib_idle_ticks = (b_os_load_data.last_idle_ctr - idle_ctr)*B_OS_IDLE_SCALE;	/* prescale idle_ticks, 20 */
	b_os_load_data.calib_ms = b_os_tv_diff(&b_os_load_data.last_time, &start)*B_OS_IDLE_SCALE;		/* 96 ms */
#else

	/* These values assume the mips is running at 250MHz (i.e. STRAP_CPU_SPEED_1) */
	b_os_load_data.calib_idle_ticks = 364;
	b_os_load_data.calib_ms = 95;
#endif
	bos_exit_critical(flags);
	BDBG_MSG(("calib_idle_ticks %u calib_ms %u\n", b_os_load_data.calib_idle_ticks, b_os_load_data.calib_ms));
	return;
}

/******************************************************************************
* INPUTS:    none
* OUTPUTS:    none.
* RETURNS:    none
* FUNCTION: start ucos
******************************************************************************/
void bos_start(void)
{
#if 0
	unsigned int count;

	/* Ensure timer interrupt does not fire before OSStart has completed.  If it does
	   then we will see a race condition where the realExcHandler will try and dereference
	   OSTCBCur before it has been initialised. This will result in a TLB exception being
	   generated because of a NULL pointer exception. */
	count = bcm_read_cp0($9,0);
	count += (g_cycles_per_tick * g_ticks_per_second) / 1000;	/* 1ms wait */
	bcm_write_cp0($11,0,count);
#endif
	OSStart();
};

void OSTaskCreateHook(OS_TCB *ptcb)
{
	struct b_task_stats *stat = &g_os_task_stats[ptcb->OSTCBPrio];

	BDBG_ASSERT(ptcb->OSTCBPrio <= OS_LOWEST_PRIO);

	if (ptcb->OSTCBPrio == 2)
	{
		printf("Stack[%03d] = 0x%08x-0x%08x %d words\n",ptcb->OSTCBPrio,stat->stack,ptcb->OSTCBStkPtr,stat->stack_size);
	}


	if (!stat->stack && (ptcb->OSTCBStkSize > 0))
	{
		stat->stack = (const uint32_t*)ptcb->OSTCBStkBottom;
		stat->clock_cnt = 0;
		stat->instr_cnt = 0;
		stat->active_cnt = 0;
		stat->stack_size = ptcb->OSTCBStkSize;
		stat->stack_left = ptcb->OSTCBStkSize;
		stat->stack_fast_scan = 0;
#if (OS_TASK_STAT_STK_CHK_EN == 0)
		memset((void*)stat->stack, B_OS_STK_FILL_BYTE,((ptcb->OSTCBStkSize - OS_FRAME_SIZE )* 4) - 4);
#endif
#if OS_TASK_NAME_SIZE > 1
		strncpy(stat->name,ptcb->OSTCBTaskName,16);
#endif
	}
	printf("Stack[%03d] = 0x%08x-0x%08x %d words\n",ptcb->OSTCBPrio,stat->stack,ptcb->OSTCBStkPtr,stat->stack_size);
}


/******************************************************************************
 * Create a task
 ******************************************************************************/
bresult bos_start_task(
		b_task_t *handle,	  /* [out] new thread handle, passed to b_stop_task when finished */
		const b_task_params *params, /* optional param structure containing Priority, task name, and stack size */
		b_task_func func,	  /* function to be called by new thread */
		void *data		  /* data passed func */
		)
{
	struct b_task_stats *stat = &g_os_task_stats[params->priority];
	stat->name[sizeof(stat->name)-1]='\0';
	memcpy(stat->name, params->name, sizeof(stat->name)-2);
#if (OS_TASK_STAT_STK_CHK_EN == 0)
	memset(params->stack, B_OS_STK_FILL_BYTE, params->stack_size*sizeof(uint32_t));
#endif

	/* check to make sure no duplicate use of same task */
#if 0//BKTEMP
	if(NULL == stat->stack)
		BDBG_WRN(("\n\nERR: task stack [%s] %d\n\n",stat->name, params->priority));
#else
	BDBG_ASSERT(NULL == stat->stack);
#endif
	stat->stack = (const uint32_t*)params->stack;
	stat->stack_size = params->stack_size;
	stat->stack_left = params->stack_size;
	stat->clock_cnt = 0;
	stat->instr_cnt = 0;
	stat->active_cnt = 0;
	stat->stack_fast_scan = 0;
	BDBG_WRN(("TASK [%s] %d\n", stat->name, params->priority ));

	*handle = params->priority;

	if (OSTaskCreateExt(func,data,(OS_STK*)&params->stack[params->stack_size - 1],
				params->priority,params->priority,(OS_STK*)params->stack,params->stack_size,
				NULL,OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR) != OS_NO_ERR)
	{
		return berr_invalid_parameter;
	}
	return b_ok;
}

/******************************************************************************
 * Stop/delete a task
 ******************************************************************************/
void bos_stop_task(b_task_t handle)
{
#if OS_TASK_DEL_EN > 0
	INT8U err;
	INT8U prio = (handle == OS_PRIO_SELF)?OSTCBCur->OSTCBPrio:handle;
	memset(&g_os_task_stats[prio],0, sizeof(b_task_stats));
	err = OSTaskDel(handle);
	if(OS_ERR_NONE != err){
		BDBG_ERR(("%d:%02x",__LINE__,err));
	}
#endif
}

/******************************************************************************
 * create an event queue using the even pool provided.
 ******************************************************************************/
bresult bos_create_queue(
		b_queue_t *handle,	/* new event queue */
		b_event_t *events,	/* event buffer of size num_events */
		int num_events	/* number of events in event buffer */
		)
{
	/* TODO check queue size does not exceed UINT16 */
	OS_EVENT* p_os_event = OSQCreate((void**)events,(INT16U)(num_events));
	if (p_os_event)
	{
		*handle = (b_queue_t)p_os_event;
		return b_ok;
	}
	*handle = (b_queue_t)NULL;
	return berr_out_of_memory;
}

/******************************************************************************
 * delete an event queue..
 ******************************************************************************/
void bos_delete_queue(
		b_queue_t *handle	 /* event queue */
		)
{
#if OS_Q_DEL_EN > 0
	INT8U           err;
	*handle = (b_queue_t)OSQDel((OS_EVENT*)(*handle),OS_DEL_ALWAYS,&err);
	if(OS_ERR_NONE != err){
		BDBG_ERR(("%d:%02x",__LINE__,err));
	}
#endif
}

/******************************************************************************
 * Post and event to the queue..
 ******************************************************************************/
bresult bos_post_event(
		b_queue_t handle,	  /* event queue */
		b_event_t *event		  /* event to post */
		)
{
	INT8U err;
	if ((err = OSQPost((OS_EVENT*)handle,event)) != OS_NO_ERR)
	{
		//BKNI_Printf("OSQPost err = %d\n",err);

		return berr_external_error;
	}
	return b_ok;
}

/******************************************************************************
 * Wait for an event..
 ******************************************************************************/
b_event_t *bos_pend_event(
		b_queue_t handle,	 /* event queue */
		int timeout_ms	 /* timeout in milliseconds */
		)
{
	INT8U err;
	if (!handle)
	{
		volatile int *null=0;*null=*null;
	}

	if (timeout_ms == 0)
	{
		return(b_event_t*)OSQAccept((OS_EVENT*)handle,&err);
	}
	else if (timeout_ms < 0)
	{
		b_event_t *p_evt;
		while ((p_evt = (b_event_t*)OSQPend((OS_EVENT*)handle,0xFFFF,&err)) == NULL)
		{
			if ((err != OS_ERR_NONE) && (err != OS_ERR_TIMEOUT))
			{
				//BKNI_Printf("OSQPend err = %d\n",err);
				//__asm__("sdbbp");
				p_evt = NULL;

				break;
			}
		}
		return p_evt;
	}
	else if (timeout_ms < TICKS_TO_MS(1))
	{ /* set timeout as 1 tick if timeout_ms is lower than 1 tick time */
		timeout_ms = TICKS_TO_MS(1);    
	}
	return(b_event_t*)OSQPend((OS_EVENT*)handle,MS_TO_TICKS(timeout_ms),&err);
}

/******************************************************************************
 * create an event queue using the even pool provided.
 ******************************************************************************/
bresult bos_create_mutex(
		b_mutex_t *handle		/* mutex reference */
		)
{
	OS_EVENT* p_os_event = OSQCreate((void**)&handle->event_queue,1);
	if (p_os_event)
	{
		handle->queue = (b_queue_t)p_os_event;
		return bos_post_event(handle->queue,&(handle->event));
	}
	return berr_out_of_memory;
}

/******************************************************************************
 * delete an event queue..
 ******************************************************************************/
void bos_delete_mutex(
		b_mutex_t *handle	 /* mutex reference */
		)
{
#if OS_Q_DEL_EN > 0
	INT8U           err;
	OSQDel((OS_EVENT*)handle->queue,OS_DEL_ALWAYS,&err);
	if(OS_ERR_NONE != err){
		BDBG_ERR(("%d:%02x",__LINE__,err));
	}
#endif
}

/******************************************************************************
 * Post and event to the queue..
 ******************************************************************************/
bresult bos_acquire_mutex(
		b_mutex_t *handle,	 /* mutex reference */
		int timeout_ms	 /* timeout in milliseconds */
		)
{
	return(bos_pend_event(handle->queue,timeout_ms) != NULL) ? b_ok : berr_timeout;
}

/******************************************************************************
 * Wait for an event..
 ******************************************************************************/
bresult bos_release_mutex(
		b_mutex_t *handle	 /* event queue */
		)
{
	return bos_post_event(handle->queue,&handle->event);
}

/******************************************************************************
 * Sleep for sleep_ms milliseconds..
 ******************************************************************************/

void bos_sleep(
		unsigned int sleep_ms	  /* number of milliseconds to delay */
		)
{
	unsigned int ticks = MS_TO_TICKS(sleep_ms);
	if (ticks <= 0)
	{
		ticks = 1;
	}
	OSTimeDly(ticks);
}
/******************************************************************************
 * Enter critical section..
 ******************************************************************************/
unsigned int bos_enter_critical(void)
{
	unsigned int flags;
	//OS_ENTER_CRITICAL(flags);
	flags = MIPS_SetInterrupts(OS_DISABLE);
	return flags;
}
/******************************************************************************
 * Exit critical section..
 ******************************************************************************/
void bos_exit_critical(unsigned int flags)
{
	MIPS_SetInterrupts(flags);
	//OS_EXIT_CRITICAL(flags);
}

/******************************************************************************
 * Return the current OS tick count.
 ******************************************************************************/

unsigned int bos_getticks(void)
{
	return OSTimeGet();
}

/******************************************************************************
 * Return the current clock.
 ******************************************************************************/

void bos_getclock(b_clock *clock)
{
	unsigned int flags;
	uint32_t mips_clock;

	flags = bos_enter_critical();
	mips_clock = bcm_read_cp0($9,0);
	/* update 64 bit clock counter */
	if (mips_clock <  g_running_clock.clock_low)
	{ /* overflow */
		g_running_clock.clock_hi++;
	}
	g_running_clock.clock_low = mips_clock;
	clock->clock_hi = g_running_clock.clock_hi;
	bos_exit_critical(flags);
	clock->clock_low = mips_clock;
	clock->clock_freq = g_running_clock.clock_freq;
}


void bos_getload(b_cpu_load *load)
{
	b_timeval now;
	unsigned long flags;
	unsigned idle_ctr;
	unsigned mips_freq;

	if ((b_os_load_data.calib_ms==0) || (0 == b_os_load_data.calib_idle_ticks))
	{
		BDBG_WRN(("calib_ms = %d, calib_idle_ticks = %d\n",b_os_load_data.calib_ms,b_os_load_data.calib_idle_ticks));
		goto error;
	}
	GETTIMEOFDAY(&now);
	flags = bos_enter_critical();
	idle_ctr = OSIdleCtr - b_os_load_data.last_idle_ctr;
	load->ctxsw_rate = OSCtxSwCtr - b_os_load_data.last_ctxsw_ctr;
	load->isr = g_stat_isr_time - b_os_load_data.last_isr_time;
	load->isr_rate = g_stat_isr_cnt - b_os_load_data.last_isr_cnt;
	b_os_load_data.last_idle_ctr = OSIdleCtr;
	b_os_load_data.last_ctxsw_ctr = OSCtxSwCtr;
	b_os_load_data.last_isr_time = g_stat_isr_time;
	b_os_load_data.last_isr_cnt = g_stat_isr_cnt;
	load->duration = b_os_tv_diff(&now, &b_os_load_data.last_time);
	if (load->duration==0)
	{
		bos_exit_critical(flags);
		BDBG_WRN(("load->duration = %d\n",load->duration));
		goto error;
	}
	b_os_load_data.last_time = now;
	g_os_task_stats[OS_LOWEST_PRIO+1].last_time=now;
	bos_exit_critical(flags);
	/*
	   BDBG_MSG(("calib_idle_ticks %u calib_ms %u\n", b_os_load_data.calib_idle_ticks, b_os_load_data.calib_ms));
	   BDBG_MSG(("idle_ctr %u duration %u\n", idle_ctr, load->duration));
	 */
	/* 
	 * below we calculate this 
	 *   100 * (idle_ctr/load->duration) / (b_os_load_data.calib_idle_ticks/b_os_load_data.calib_ms)
	 *  and carefully rearrange it to both prevent rounding and overflows  
	 */
	load->ctxsw_rate = (load->ctxsw_rate*1000)/load->duration;
	load->isr_rate = (load->isr_rate*1000)/load->duration;
	/* ISR load is 
	 *  (isr_time * 100)/(clock * (load->duration/1000))
	 * rearranged to keep it from overflowing and maintain accuracy */
	mips_freq = g_running_clock.clock_freq/(1000*(100/10));
	if (mips_freq==0)
	{
		BDBG_WRN(("g_running_clock.clock_freq = %d\n",g_running_clock.clock_freq));
		goto error;
	}
	load->isr = 10*load->isr;
	load->isr_us = load->isr/(mips_freq/10);
	load->isr = load->isr/(mips_freq*load->duration);
	load->idle =  ((idle_ctr/32)*((b_os_load_data.calib_ms*100*32)/load->duration))/b_os_load_data.calib_idle_ticks;
	return;
error:
	load->isr = 0;
	load->duration = 0;
	load->idle = 0;
	load->isr_rate = 0;
	load->ctxsw_rate = 0;
	return;
}

extern inline void b_os_check_stack(b_task_stats *task, uint32_t pattern)
{
	unsigned i;
	unsigned left;
	unsigned virgin;

	for (virgin=0, left=i=task->stack_left;i>0;i--)
	{
		if (task->stack[i] != pattern)
		{
			left = i;
			virgin = 0;
		}
		else
		{
			virgin++;
			if (task->stack_fast_scan>0 && left == task->stack_left && virgin>=4)
			{ /* if dirty marker didn't move and have seen 4 markers in a row, quit */
				break;
			}
		}
	}
	if (left!=task->stack_left)
	{
		task->stack_fast_scan = 0; /* don't use fast scan for the next round */
		task->stack_left = left;
	}
	else
	{
		if (task->stack_fast_scan==0)
		{
			task->stack_fast_scan = 16;
		}
		else
		{
			task->stack_fast_scan --;
		}
	}
	return;
}
struct b_task_stats *bos_taskinfo(int priority)
{
	return &g_os_task_stats[priority];
}

void bos_print_taskinfo(void)
{
	unsigned i;
	unsigned mips_freq = g_running_clock.clock_freq/(1000);	/* prescaled frequency */
	char *pname;

	for (i=0;i<OS_LOWEST_PRIO+2;i++)
	{
		struct b_task_stats *stats = &g_os_task_stats[i];
		b_timeval now;
		unsigned ms;
		unsigned task_ms;
		unsigned task_cnt;
		unsigned task_load;
		unsigned flags;
		unsigned stack_use;
		unsigned issue_rate;
		uint32_t pc=0;

		if (!stats->stack)
		{
			continue;
		}
		GETTIMEOFDAY(&now);
		ms = b_os_tv_diff(&now, &stats->last_time);
		/* capture and reset counters */
		flags = bos_enter_critical();
		if (i==OS_LOWEST_PRIO+1)
		{ /* isr pseudo task */
			task_cnt = g_stat_isr_cnt - b_os_load_data.last_isr_cnt;
			task_ms = g_stat_isr_time - b_os_load_data.last_isr_time; 
		}
		else
		{
			task_ms = stats->clock_cnt;
			task_cnt = stats->active_cnt;
			if (OSTCBPrioTbl[i] && OSTCBPrioTbl[i]->OSTCBStkPtr)
			{
				pc = ((uint32_t *)OSTCBPrioTbl[i]->OSTCBStkPtr)[ROFF_PC/*-ROFF_NUM_REG*/];
			}
		}
		issue_rate = stats->instr_cnt;
		if (task_cnt)
		{
			stats->instr_cnt = 0;
			stats->clock_cnt = 0;
			stats->active_cnt = 0;
			stats->last_time = now;
		}
		bos_exit_critical(flags);
		if (task_ms>1000)
		{
			issue_rate /= (task_ms/1000);
		}
		else
		{
			issue_rate = 0;
		}
#if (OS_TASK_STAT_STK_CHK_EN > 0) && (OS_TASK_CREATE_EXT_EN > 0)
		{   OS_STK_DATA stk_data;
			if (OSTaskStkChk(i, &stk_data) != OS_ERR_NONE)
			{
				printf("error performing stack check for Priority %d\n",i);
			}
			else
			{
				stats->stack_size = stk_data.OSUsed/4 + stk_data.OSFree/4;
				stats->stack_left = stk_data.OSFree/4;
			}
		}
#else
		b_os_check_stack(stats,B_OS_STK_FILL_INT32);
#endif

		/* normalize information  */
		task_load = task_ms / (((mips_freq*10)/10000)*ms);
		task_ms = task_ms/mips_freq;
		if (stats->stack_size>0)
		{
			stack_use = (100*(stats->stack_size - stats->stack_left))/stats->stack_size;
		}
		else
		{
			stack_use = 100;
		}
#if OS_TASK_NAME_SIZE > 1
		if (strlen(stats->name) > 1)
			pname = stats->name;
		else
			pname = OSTCBPrioTbl[i]->OSTCBTaskName;
#else
		pname = stats->name;
#endif
		printf("TASK(%3u):%-16s CPU %2u.%1u%%(%5ums) RATE %2u.%1u%% CTXSW %5u(%5u/s %7uus) STACK %2u%% (%4u:%4u)\n", 
				i, pname, (task_load/10), (task_load%10) , task_ms, issue_rate/10, issue_rate%10, task_cnt, (task_cnt*1000)/ms, 
				(task_ms*1000)/(task_cnt?task_cnt:1), stack_use, (stats->stack_size-stats->stack_left), stats->stack_size); 
	}
	return;
}

	const void *
bos_task_from_stack(const uint32_t *stack)
{
	unsigned i;

	for (i=0;i<OS_LOWEST_PRIO+2;i++)
	{
		if ( (stack >= g_os_task_stats[i].stack && stack <= g_os_task_stats[i].stack + g_os_task_stats[i].stack_size))
		{
			return g_os_task_stats[i].name;
		}
	}
	return NULL;
}

void bos_check_stacks(char* str)
{
	unsigned i;
	unsigned mips_freq = g_running_clock.clock_freq/(1000);	/* prescaled frequency */
#if 0
	if (mem_validate(&g_p_dsp->sdram_heap) == 0)
	{
		printf("###### mem_validate failure %s\n",str);
	}
#endif
	for (i=0;i<OS_LOWEST_PRIO+2;i++)
	{
		struct b_task_stats *stats = &g_os_task_stats[i];
		b_timeval now;
		unsigned ms;
		unsigned task_ms;
		unsigned task_cnt;
		unsigned task_load;
		unsigned flags;
		unsigned stack_use;
		unsigned issue_rate;
		uint32_t pc=0;
		const char *name="";
#if defined(BCM_DEBUG)
		char buf[24];
#endif

		if (!stats->stack)
		{
			continue;
		}
		GETTIMEOFDAY(&now);
		ms = b_os_tv_diff(&now, &stats->last_time);
		/* capture and reset counters */
		flags = bos_enter_critical();
		if (i==OS_LOWEST_PRIO+1)
		{ /* isr pseudo task */
			task_cnt = g_stat_isr_cnt - b_os_load_data.last_isr_cnt;
			task_ms = g_stat_isr_time - b_os_load_data.last_isr_time; 
			stats->instr_cnt = g_stat_isr_cnt;
		}
		else
		{
			task_ms = stats->clock_cnt;
			task_cnt = stats->active_cnt;
			if (OSTCBPrioTbl[i] && OSTCBPrioTbl[i]->OSTCBStkPtr)
			{
				pc = ((uint32_t *)OSTCBPrioTbl[i]->OSTCBStkPtr)[ROFF_PC/*-ROFF_NUM_REG*/];
			}
		}
		issue_rate = stats->instr_cnt;
		if (task_cnt)
		{
			stats->instr_cnt = 0;
			stats->clock_cnt = 0;
			stats->active_cnt = 0;
			stats->last_time = now;
		}
		bos_exit_critical(flags);
		if (task_ms>1000)
		{
			issue_rate /= (task_ms/1000);
		}
		else
		{
			issue_rate = 0;
		}
#if defined(BCM_DEBUG)
		if (pc)
		{
			name = bsymtable_get_name(pc, buf, sizeof(buf));
		}
#endif
		/* check stack */
#if (OS_TASK_STAT_STK_CHK_EN > 0) && (OS_TASK_CREATE_EXT_EN > 0)
		{
			OS_STK_DATA stk_data;
			if (OSTaskStkChk(i, &stk_data) != OS_ERR_NONE)
			{
				printf("error performing stack check for Priority %d\n",i);
			}
			else
			{
				stats->stack_size = stk_data.OSUsed/4 + stk_data.OSFree/4;
				stats->stack_left = stk_data.OSFree/4;
			}
		}
#else
		b_os_check_stack(stats,B_OS_STK_FILL_INT32);
#endif

		/* normalize information  */
		task_load = task_ms / ((mips_freq/100)*ms);
		task_ms = task_ms/mips_freq;
		if (stats->stack_size>0)
		{
			stack_use = (100*(stats->stack_size - stats->stack_left))/stats->stack_size;
		}
		else
		{
			stack_use = 100;
		}
		if (stack_use > 90)
		{
			printf("TASK(%2u):%-12s CPU %2u%%(%4ums) RATE %2u.%1u%% CTXSW %5u(%5u/s %7uus) STACK %2u%% (%u:%u) %s@%s\n", i, stats->name, task_load, task_ms, issue_rate/10, issue_rate%10, task_cnt, (task_cnt*1000)/ms, (task_ms*1000)/(task_cnt?task_cnt:1), stack_use, (stats->stack_size-stats->stack_left), stats->stack_size, name,str); 
		}
	}
}

#define RAM_SIZE_MAGIC 0xBABEBAD0
unsigned int bos_ram_size(void)
{
	unsigned int base;
	unsigned int alias;
	unsigned int size;
	unsigned int magic;

	magic = RAM_SIZE_MAGIC;

	base = MEM_SIZE_8M;
	do
	{
		alias = base << 1;
		*(unsigned int*)BCM_PHYS_TO_KSEG1(base - 0x100) = magic;
		if (magic == *(unsigned int*)BCM_PHYS_TO_KSEG1(alias - 0x100))
		{
			size = base;
			break;
		}
		base = alias;
		magic++;
	}while (1);
	return size;
}

/* 
   If you define PROFILE and would like to use this function, change
   OS_TASK_IDLE_HOOK_EN to 2 in os_cfg.h in ucos_ii directory so this
   function is used instead of builtin.
 */
#if (PROFILE && !OS_TASK_IDLE_HOOK_EN)

#define OS_TASK_IDLE_CNT	4096
static unsigned s_addr[OS_TASK_IDLE_CNT/4 + 8];

void OSTaskIdleHook(void)
{
	volatile unsigned *addr = s_addr;
	unsigned i;

	for (i=0;i<n;i+=8)
	{
		(void)addr[i+0];
		(void)addr[i+1];
		(void)addr[i+2];
		(void)addr[i+3];
		(void)addr[i+4];
		(void)addr[i+5];
		(void)addr[i+6];
		(void)addr[i+7];
	}
	return;
}
#endif

extern void bint_handler(void);

/* update task accounting here */
void OSTaskSwHook(void)
{
	uint32_t count;
	struct b_task_stats * old;
	unsigned instr_cnt;

	count = bcm_read_cp0($9,0);
	instr_cnt = bcm_read_cp0($25, 2);
	old = &g_os_task_stats[OSTCBCur->OSTCBPrio];
	old->clock_cnt += count - g_last_ctxsw.clock;
	old->instr_cnt += g_last_ctxsw.instr - instr_cnt;
	old->active_cnt++;
	g_last_ctxsw.clock = count;
	g_last_ctxsw.instr = instr_cnt;

}

#if SUPPORT_DST_PLATFORM

b_task_stats *bos_get_task_info(b_task_t id)
{
	if(id>=OS_MAX_TASKS) return NULL;
	
	return &g_os_task_stats[id];
}

void bos_print_taskinfo_ex(int level, char *(*callback)(void *, char *, int))
{
#if 0//BKTODO
	unsigned i;
	unsigned mips_freq = g_running_clock.clock_freq/(1000); /* prescaled frequency */
	int max_name_len = 0;
	char task_name[17] = {0, };
	// get longest task name
	for(i=0;i<OS_MAX_TASKS+2;i++) {
		int n;
		struct b_task_stats *stats = &g_os_task_stats[i];
		if(!stats->stack) continue;
		n = strlen(stats->name);
		if (max_name_len < n) max_name_len = n; 
	}
	
	for(i=0;i<OS_MAX_TASKS+2;i++) {
		struct b_task_stats *stats = &g_os_task_stats[i];
		b_timeval now;
		unsigned ms;
		unsigned task_ms;
		unsigned task_cnt;
		unsigned task_load;
		unsigned flags;
		unsigned stack_use;
		unsigned issue_rate;
		uint32_t pc=0, r_sp=0, r_ra=0;
		const char *name="";
		char buf[24];
		uint32_t task_runtime;
		char task_runtime_buf[32];

		if(!stats->stack) {
			continue;
		}
		gettimeofday(&now);
		ms = b_os_tv_diff(&now, &stats->last_time);
		/* capture and reset counters */
		flags = bos_enter_critical();
		if(i==OS_MAX_TASKS+1) { /* isr pseudo task */
			task_cnt = g_stat_isr_cnt - b_os_load_data.last_isr_cnt;
			task_ms = g_stat_isr_time - b_os_load_data.last_isr_time; 
		} else {
			task_ms = stats->clock_cnt;
			task_cnt = stats->active_cnt;
			if(OSTCBPrioTbl[i] && OSTCBPrioTbl[i]->OSTCBStkPtr)  {
				pc = ((uint32_t *)OSTCBPrioTbl[i]->OSTCBStkPtr)[ROFF_PC/*-ROFF_NUM_REG*/];
				r_sp = ((uint32_t *)OSTCBPrioTbl[i]->OSTCBStkPtr)[ROFF_SP/*-ROFF_NUM_REG*/];
				r_ra = ((uint32_t *)OSTCBPrioTbl[i]->OSTCBStkPtr)[ROFF_RA/*-ROFF_NUM_REG*/];
			}
		}
		issue_rate = stats->instr_cnt;
		if (task_cnt) {
			stats->instr_cnt = 0;
			stats->clock_cnt = 0;
			stats->active_cnt = 0;
			stats->last_time = now;
		} 
		bos_exit_critical(flags);
		if (task_ms>1000) {
			issue_rate /= (task_ms/1000);
		} else {
			issue_rate = 0;
		}
		if (pc) {
			name = bsymtable_get_name(pc, buf, sizeof(buf));
		} 
		/* check stack */
		b_os_check_stack(stats,i<OS_MAX_TASKS?B_OS_STK_FILL_INT32:0);
		/* normalize information  */
		task_load = task_ms / ((mips_freq/100)*ms);
		task_ms = task_ms/mips_freq;
		if(stats->stack_size>0) {
			stack_use = (100*(stats->stack_size - stats->stack_left))/stats->stack_size;
		} else {
			stack_use = 100;
		}

		// print task name with fixed width. adapt to longest name.
		memset(task_name, ' ', sizeof(task_name));
		strncpy(task_name, stats->name, strlen(stats->name));
		task_name[max_name_len] = 0; // null terminator.

		// average task run time per context switch
		task_runtime = (task_ms*1000)/(task_cnt?task_cnt:1);
		if (task_runtime >= 10000)
			sprintf(task_runtime_buf, "%4ums", task_runtime/1000);
		else
			sprintf(task_runtime_buf, "%4uus", task_runtime);

		if (level == 1 || level == 2) {
			char prefix[8] = {0, };
			memset(prefix, ' ', sizeof(prefix)-1);
			printf("TASK(%2u):%s CPU %2u%%(%4ums) STACK %2u%% (%u:%u) ", 
				i, task_name, task_load, task_ms, 
				stack_use, (stats->stack_size-stats->stack_left), stats->stack_size, 
				0);
			if (level == 1 && callback && i<OS_MAX_TASKS) { // task 63,64 is not normal task.
				OS_EVENT *pevent;
				char outbuf[100];
				flags = bos_enter_critical();
				pevent = OSTCBPrioTbl[i]->OSTCBEventPtr;
				bos_exit_critical(flags);
				/* this 'pevent' is the event that this task is waiting.. 
				   show more detail info about this event.
				*/
				printf("%s\n", callback(pevent, outbuf, sizeof(outbuf)));
			}
			else if (level == 2 && pc && r_sp && r_ra) {
				printf("\n");
				stack_backtrace((unsigned int *)r_sp, (unsigned int *)pc, (unsigned int *)r_ra, prefix);
			}
			else
				printf("\n");
		}
		else { // default
			printf("TASK(%2u):%s CPU %2u%%(%4ums) RATE %2u.%1u%% CTXSW %5u(%4u/s %s) STACK %2u%% (%u:%u)\n", 
				i, task_name, task_load, task_ms, 
				issue_rate/10, issue_rate%10, // rate
				task_cnt, (task_cnt*1000)/ms, task_runtime_buf, // context switch
				stack_use, (stats->stack_size-stats->stack_left), stats->stack_size,
				0);
		}
	}

#if 0
	/* cafrii 080919, separate pending list  */
	if (level == 1 && callback) 
	{
		OS_EVENT *pevent;
		//unsigned flags;
		printf("Pending List:\n");
		
		for(i=0;i<OS_MAX_TASKS;i++) // task 63,64 is not normal task.
		{
			struct b_task_stats *stats = &g_os_task_stats[i];
			if(!stats->stack) // no task.
				continue;
			//flags = bos_enter_critical();
			pevent = OSTCBPrioTbl[i]->OSTCBEventPtr;
			//bos_exit_critical(flags);
			/* this 'pevent' is the event that this task is waiting.. 
			   show more detail info about this event.
			*/
			callback(i, stats->name, pevent);
		}
	}
#endif // 0
#endif
	return;
}

extern unsigned char OSLockNesting;

INT8U OSSchedLockCount(void)
{
	return OSLockNesting;
}


#if 0
/*
	User can use OSUser field for their convenience.
*/
void OSSaveUserData(OS_EVENT *pevent, void *pUser)
{
	pevent->OSUser = pUser;
}

void *OSGetUserData(OS_EVENT *pevent)
{
	return pevent->OSUser;
}
#endif
#endif