void ARQ_SetChunkSize(u32 size)
{
u32 level;
_CPU_ISR_Disable(level);
__ARQChunkSize = (size+31)&~31;
_CPU_ISR_Restore(level);
}
void ARQ_Reset()
{
u32 level;
_CPU_ISR_Disable(level);
__ARQInitFlag = 0;
_CPU_ISR_Restore(level);
}
void ARQ_Init()
{
u32 level;
#ifdef _ARQ_DEBUG
printf("ARQ_Init(%02x)\n",__ARQInitFlag);
#endif
if(__ARQInitFlag) return;
_CPU_ISR_Disable(level);
__ARQReqPendingLo = NULL;
__ARQReqPendingHi = NULL;
__ARQCallbackLo = NULL;
__ARQCallbackHi = NULL;
__ARQChunkSize = ARQ_DEF_CHUNK_SIZE;
LWP_InitQueue(&__ARQSyncQueue);
__lwp_queue_init_empty(&__ARQReqQueueLo);
__lwp_queue_init_empty(&__ARQReqQueueHi);
AR_RegisterCallback(__ARInterruptServiceRoutine);
__ARQInitFlag = 1;
_CPU_ISR_Restore(level);
}
u32 AR_Init(u32 *stack_idx_array,u32 num_entries)
{
u32 level;
u32 aram_base = 0x4000;
if(__ARInit_Flag) return aram_base;
_CPU_ISR_Disable(level);
__ARDmaCallback = NULL;
IRQ_Request(IRQ_DSP_ARAM,__ARHandler,NULL);
__UnmaskIrq(IRQMASK(IRQ_DSP_ARAM));
__ARStackPointer = aram_base;
__ARFreeBlocks = num_entries;
__ARBlockLen = stack_idx_array;
_dspReg[13] = (_dspReg[13]&~0xff)|(_dspReg[13]&0xff);
__ARCheckSize();
__ARInit_Flag = 1;
_CPU_ISR_Restore(level);
return __ARStackPointer;
}
void ARQ_PostRequestAsync(ARQRequest *req,u32 owner,u32 dir,u32 prio,u32 aram_addr,u32 mram_addr,u32 len,ARQCallback cb)
{
u32 level;
ARQRequest *p;
req->state = ARQ_TASK_READY;
req->dir = dir;
req->owner = owner;
req->aram_addr = aram_addr;
req->mram_addr = mram_addr;
req->len = len;
req->prio = prio;
req->callback = (cb==NULL) ? __ARQCallbackDummy : cb;
_CPU_ISR_Disable(level);
if(prio==ARQ_PRIO_LO) __lwp_queue_appendI(&__ARQReqQueueLo,&req->node);
else __lwp_queue_appendI(&__ARQReqQueueHi,&req->node);
if(!__ARQReqPendingLo && !__ARQReqPendingHi) {
p = (ARQRequest*)__lwp_queue_getI(&__ARQReqQueueHi);
if(p) {
p->state = ARQ_TASK_RUNNING;
#ifdef _ARQ_DEBUG
printf("ARQ_PostRequest(%02x,%08x,%08x,%d)\n",p->dir,p->aram_addr,p->mram_addr,p->len);
#endif
AR_StartDMA(p->dir,p->mram_addr,p->aram_addr,p->len);
__ARQCallbackHi = p->callback;
__ARQReqPendingHi = p;
}
if(!__ARQReqPendingHi) __ARQServiceQueueLo();
}
_CPU_ISR_Restore(level);
}
/* FIXME: put comments here */
void rtems_exception_init_mngt(void)
{
ISR_Level level;
_CPU_ISR_Disable(level);
_CPU_ISR_install_vector(ARM_EXCEPTION_UNDEF,
_Exception_Handler_Undef_Swi,
NULL);
_CPU_ISR_install_vector(ARM_EXCEPTION_SWI,
_Exception_Handler_Undef_Swi,
NULL);
_CPU_ISR_install_vector(ARM_EXCEPTION_PREF_ABORT,
_Exception_Handler_Abort,
NULL);
_CPU_ISR_install_vector(ARM_EXCEPTION_DATA_ABORT,
_exc_data_abort,
NULL);
_CPU_ISR_install_vector(ARM_EXCEPTION_FIQ,
_Exception_Handler_Abort,
NULL);
_CPU_ISR_install_vector(ARM_EXCEPTION_IRQ,
_Exception_Handler_Abort,
NULL);
_CPU_ISR_Enable(level);
}
u32 __lwp_sema_seize(lwp_sema *sema,u32 id,u32 wait,u64 timeout)
{
u32 level;
lwp_cntrl *exec;
exec = _thr_executing;
exec->wait.ret_code = LWP_SEMA_SUCCESSFUL;
_CPU_ISR_Disable(level);
if(sema->count!=0) {
--sema->count;
_CPU_ISR_Restore(level);
return LWP_SEMA_SUCCESSFUL;
}
if(!wait) {
_CPU_ISR_Restore(level);
exec->wait.ret_code = LWP_SEMA_UNSATISFIED_NOWAIT;
return LWP_SEMA_UNSATISFIED_NOWAIT;
}
__lwp_threadqueue_csenter(&sema->wait_queue);
exec->wait.queue = &sema->wait_queue;
exec->wait.id = id;
_CPU_ISR_Restore(level);
__lwp_threadqueue_enqueue(&sema->wait_queue,timeout);
return LWP_SEMA_SUCCESSFUL;
}
void Reload (void)
{
if (!GetFileToBoot ())
{
// run sm, priiloader on the nand is required
*(vu32*)0x8132FFFB = 0x50756E65;
DCFlushRange((void*)0x8132FFFB,4);
SYS_ResetSystem(SYS_RETURNTOMENU, 0, 0);
}
struct __argv arg;
memset (&arg, 0, sizeof(struct __argv));
memmove(ARGS_ADDR, &arg, sizeof(arg));
DCFlushRange(ARGS_ADDR, sizeof(arg) + arg.length);
memcpy(BOOTER_ADDR, booter_dol, booter_dol_size);
DCFlushRange(BOOTER_ADDR, booter_dol_size);
entrypoint exeEntryPoint;
exeEntryPoint = (entrypoint) BOOTER_ADDR;
/* cleaning up and load dol */
SYS_ResetSystem(SYS_SHUTDOWN, 0, 0);
_CPU_ISR_Disable (cookie);
__exception_closeall ();
exeEntryPoint ();
_CPU_ISR_Restore (cookie);
exit (0);
}
s32 LWP_JoinThread(lwp_t thethread,void **value_ptr)
{
u32 level;
void *return_ptr;
lwp_cntrl *exec,*lwp_thread;
lwp_thread = __lwp_cntrl_open(thethread);
if(!lwp_thread) return 0;
if(__lwp_thread_isexec(lwp_thread)) {
__lwp_thread_dispatchenable();
return EDEADLK; //EDEADLK
}
exec = _thr_executing;
_CPU_ISR_Disable(level);
__lwp_threadqueue_csenter(&lwp_thread->join_list);
exec->wait.ret_code = 0;
exec->wait.ret_arg_1 = NULL;
exec->wait.ret_arg = (void*)&return_ptr;
exec->wait.queue = &lwp_thread->join_list;
exec->wait.id = thethread;
_CPU_ISR_Restore(level);
__lwp_threadqueue_enqueue(&lwp_thread->join_list,LWP_WD_NOTIMEOUT);
__lwp_thread_dispatchenable();
if(value_ptr) *value_ptr = return_ptr;
return 0;
}
void __console_init(void *framebuffer,int xstart,int ystart,int xres,int yres,int stride)
{
unsigned int level;
console_data_s *con = &stdcon;
_CPU_ISR_Disable(level);
con->destbuffer = framebuffer;
con->con_xres = xres;
con->con_yres = yres;
con->con_cols = xres / FONT_XSIZE;
con->con_rows = yres / FONT_YSIZE;
con->con_stride = con->tgt_stride = stride;
con->target_x = xstart;
con->target_y = ystart;
con->font = console_font_8x16;
con->foreground = COLOR_WHITE;
con->background = COLOR_BLACK;
curr_con = con;
__console_clear();
devoptab_list[STD_OUT] = &dotab_stdout;
devoptab_list[STD_ERR] = &dotab_stdout;
_CPU_ISR_Restore(level);
setvbuf(stdout, NULL , _IONBF, 0);
}
void DEBUG_Init(s32 device_type,s32 channel_port)
{
u32 level;
struct uip_ip_addr localip,netmask,gateway;
UIP_LOG("DEBUG_Init()\n");
__lwp_thread_dispatchdisable();
bp_init();
if(device_type==GDBSTUB_DEVICE_USB) {
current_device = usb_init(channel_port);
} else {
localip.addr = uip_ipaddr((const u8_t*)tcp_localip);
netmask.addr = uip_ipaddr((const u8_t*)tcp_netmask);
gateway.addr = uip_ipaddr((const u8_t*)tcp_gateway);
current_device = tcpip_init(&localip,&netmask,&gateway,(u16)channel_port);
}
if(current_device!=NULL) {
_CPU_ISR_Disable(level);
__exception_sethandler(EX_DSI,dbg_exceptionhandler);
__exception_sethandler(EX_PRG,dbg_exceptionhandler);
__exception_sethandler(EX_TRACE,dbg_exceptionhandler);
__exception_sethandler(EX_IABR,dbg_exceptionhandler);
_CPU_ISR_Restore(level);
dbg_initialized = 1;
}
__lwp_thread_dispatchenable();
}
void L2Enhance()
{
u32 level, hid4;
u32 *stub = (u32*)0x80001800;
_CPU_ISR_Disable(level);
hid4 = mfspr(HID4);
// make sure H4A is set before doing anything
if (hid4 & 0x80000000) {
// There's no easy way to flush only L2, so just flush everything
// L2GlobalInvalidate will take care of syncing
DCFlushRangeNoSync((void*)0x80000000, 0x01800000);
DCFlushRangeNoSync((void*)0x90000000, 0x04000000);
// Invalidate L2 (this will disable it first)
L2GlobalInvalidate();
// set bits: L2FM=01, BCO=1, L2MUM=1
hid4 |= 0x24200000;
mtspr(HID4, hid4);
// Re-enable L2
L2Enable();
// look for HBC stub (STUBHAXX)
if (stub[1]==0x53545542 && stub[2]==0x48415858) {
// look for a HID4 write
for (stub += 3; (u32)stub < 0x80003000; stub++) {
if ((stub[0] & 0xFC1FFFFF)==0x7C13FBA6) {
write32((u32)stub, 0x60000000);
break;
}
}
}
}
_CPU_ISR_Restore(level);
}
u32 AR_Init(u32 *stack_idx_array,u32 num_entries)
{
#ifdef _AR_DEBUG
u32 freq;
#endif
u32 level;
u32 aram_base = 0x4000;
if(__ARInit_Flag) return aram_base;
_CPU_ISR_Disable(level);
__ARDmaCallback = NULL;
IRQ_Request(IRQ_DSP_ARAM,__ARHandler,NULL);
__UnmaskIrq(IRQMASK(IRQ_DSP_ARAM));
__ARStackPointer = aram_base;
__ARFreeBlocks = num_entries;
__ARBlockLen = stack_idx_array;
#ifdef _AR_DEBUG
freq = _dspReg[13]&0xff;
if(((f32)freq)!=156.0 && ((f32)freq)!=176.0) {
printf("AR_Init(): Illegal SDRAM refresh value(%f)\n",(f32)(freq));
abort();
}
#endif
_dspReg[13] = (_dspReg[13]&~0xff)|(_dspReg[13]&0xff);
__ARCheckSize();
__ARInit_Flag = 1;
_CPU_ISR_Restore(level);
return __ARStackPointer;
}
void* btmemr_realloc(void *ptr,u32 newsize)
{
u32 level;
u32 size,ptr1,ptr2;
struct mem *rmem,*rmem2;
if(newsize%MEM_ALIGNMENT) newsize += MEM_ALIGNMENT - ((newsize + SIZEOF_STRUCT_MEM)%MEM_ALIGNMENT);
if(newsize>MEM_SIZE) return NULL;
if((u8_t*)ptr<(u8_t*)ram_block || (u8_t*)ptr>=(u8_t*)ram_end) {
ERROR("memr_realloc: illegal memory.\n");
return ptr;
}
_CPU_ISR_Disable(level);
rmem = (struct mem*)((u8_t*)ptr - SIZEOF_STRUCT_MEM);
ptr1 = (u8_t*)rmem - ram_block;
size = rmem->next - ptr1 - SIZEOF_STRUCT_MEM;
if(newsize+SIZEOF_STRUCT_MEM+MIN_SIZE<size) {
ptr2 = ptr1 + SIZEOF_STRUCT_MEM + newsize;
rmem2 = (struct mem*)&ram_block[ptr2];
rmem2->used = 0;
rmem2->next = rmem->next;
rmem2->prev = ptr1;
rmem->next = ptr2;
if(rmem2->next!=MEM_SIZE) ((struct mem*)&ram_block[rmem2->next])->prev = ptr2;
plug_holes(rmem2);
}
_CPU_ISR_Restore(level);
return ptr;
}
u32 ARQ_RemoveOwnerRequest(u32 owner)
{
u32 level,cnt;
ARQRequest *req;
_CPU_ISR_Disable(level);
cnt = 0;
req = (ARQRequest*)__ARQReqQueueHi.first;
while(req!=(ARQRequest*)__lwp_queue_tail(&__ARQReqQueueHi)) {
if(req->owner==owner) {
__lwp_queue_extractI(&req->node);
cnt++;
}
req = (ARQRequest*)req->node.next;
}
req = (ARQRequest*)__ARQReqQueueLo.first;
while(req!=(ARQRequest*)__lwp_queue_tail(&__ARQReqQueueLo)) {
if(req->owner==owner) {
__lwp_queue_extractI(&req->node);
cnt++;
}
req = (ARQRequest*)req->node.next;
}
if(__ARQReqPendingLo && __ARQReqPendingLo==req && __ARQCallbackLo==NULL) __ARQReqPendingLo = NULL;
_CPU_ISR_Restore(level);
return cnt;
}
void __ISR_Handler(uint32_t vector, CPU_Interrupt_frame *ifr)
{
register uint32_t level;
_exception_stack_frame = NULL;
/* Interrupts are disabled upon entry to this Handler */
_Thread_Dispatch_disable_level++;
#if( CPU_HAS_SOFTWARE_INTERRUPT_STACK == TRUE)
if ( _ISR_Nest_level == 0 ) {
/* Install irq stack */
_old_stack_ptr = stack_ptr;
stack_ptr = _CPU_Interrupt_stack_high - 4;
}
#endif
_ISR_Nest_level++;
if ( _ISR_Vector_table[ vector] )
{
(*_ISR_Vector_table[ vector ])(vector, ifr);
};
/* Make sure that interrupts are disabled again */
_CPU_ISR_Disable( level );
_ISR_Nest_level--;
#if( CPU_HAS_SOFTWARE_INTERRUPT_STACK == TRUE)
if( _ISR_Nest_level == 0)
stack_ptr = _old_stack_ptr;
#endif
_Thread_Dispatch_disable_level--;
_CPU_ISR_Enable( level );
if ( _ISR_Nest_level )
return;
if ( _Thread_Dispatch_disable_level ) {
_ISR_Signals_to_thread_executing = FALSE;
return;
}
if ( _Context_Switch_necessary || _ISR_Signals_to_thread_executing ) {
_ISR_Signals_to_thread_executing = FALSE;
/* save off our stack frame so the context switcher can get to it */
_exception_stack_frame = ifr;
_Thread_Dispatch();
/* and make sure its clear in case we didn't dispatch. if we did, its
* already cleared */
_exception_stack_frame = NULL;
}
}
void DSP_Unhalt()
{
u32 level;
_CPU_ISR_Disable(level);
_dspReg[5] = (_dspReg[5]&~(DSPCR_AIINT|DSPCR_ARINT|DSPCR_DSPINT|DSPCR_HALT));
_CPU_ISR_Restore(level);
}
void LCEnable()
{
u32 level;
_CPU_ISR_Disable(level);
__LCEnable();
_CPU_ISR_Restore(level);
}
void DSP_CancelTask(dsptask_t *task)
{
u32 level;
_CPU_ISR_Disable(level);
task->flags |= DSPTASK_CANCEL;
_CPU_ISR_Restore(level);
}
u32 AR_GetDMAStatus()
{
u32 level,ret;
_CPU_ISR_Disable(level);
ret = ((_dspReg[5]&DSPCR_DSPDMA)==DSPCR_DSPDMA);
_CPU_ISR_Restore(level);
return ret;
}
void __ISR_Handler(void)
{
register uint32_t level;
/* Interrupts are disabled upon entry to this Handler */
#if( CPU_HAS_SOFTWARE_INTERRUPT_STACK == TRUE)
if ( _ISR_Nest_level == 0 ) {
/* Install irq stack */
_old_stack_ptr = stack_ptr;
stack_ptr = _CPU_Interrupt_stack_high - 4;
}
#endif
_ISR_Nest_level++;
_Thread_Dispatch_increment_disable_level();
__IIC_Handler();
/* Make sure that interrupts are disabled again */
_CPU_ISR_Disable( level );
_Thread_Dispatch_decrement_disable_level();
_ISR_Nest_level--;
if( _ISR_Nest_level == 0) {
#if( CPU_HAS_SOFTWARE_INTERRUPT_STACK == TRUE)
stack_ptr = _old_stack_ptr;
#endif
if( !_Thread_Dispatch_in_critical_section() )
{
if ( _Thread_Dispatch_necessary ) {
_CPU_ISR_Enable( level );
_Thread_Dispatch();
/* may have switched to another task and not return here immed. */
_CPU_ISR_Disable( level ); /* Keep _pairs_ of Enable/Disable */
}
}
}
_CPU_ISR_Enable( level );
}
void DSP_Halt()
{
u32 level,old;
_CPU_ISR_Disable(level);
old = _dspReg[5];
_dspReg[5] = (old&~(DSPCR_AIINT|DSPCR_ARINT|DSPCR_DSPINT))|DSPCR_HALT;
_CPU_ISR_Restore(level);
}
void ARQ_RemoveRequest(ARQRequest *req)
{
u32 level;
_CPU_ISR_Disable(level);
__lwp_queue_extractI(&req->node);
if(__ARQReqPendingLo && __ARQReqPendingLo==req && __ARQCallbackLo==NULL) __ARQReqPendingLo = NULL;
_CPU_ISR_Restore(level);
}
void RealDSP::Init()
{
_dspReg[5] = (_dspReg[5] & ~(DSPCR_AIINT|DSPCR_ARINT|DSPCR_DSPINT)) | DSPCR_DSPRESET;
_dspReg[5] = (_dspReg[5] & ~(DSPCR_HALT|DSPCR_AIINT|DSPCR_ARINT|DSPCR_DSPINT));
u32 level;
_CPU_ISR_Disable(level);
IRQ_Request(IRQ_DSP_DSP, dsp_irq_handler, nullptr);
_CPU_ISR_Restore(level);
}
void DSP_AssertInt()
{
u32 level;
#ifdef _DSP_DEBUG
printf("DSP_AssertInt()\n");
#endif
_CPU_ISR_Disable(level);
_dspReg[5] = (_dspReg[5]&~(DSPCR_AIINT|DSPCR_ARINT|DSPCR_DSPINT))|DSPCR_PIINT;
_CPU_ISR_Restore(level);
}
void DSP_Reset()
{
u16 old;
u32 level;
_CPU_ISR_Disable(level);
old = _dspReg[5];
_dspReg[5] = (old&~(DSPCR_AIINT|DSPCR_ARINT|DSPCR_DSPINT))|(DSPCR_DSPRESET|DSPCR_RES);
_CPU_ISR_Restore(level);
}
ARCallback AR_RegisterCallback(ARCallback callback)
{
u32 level;
ARCallback old;
_CPU_ISR_Disable(level);
old = __ARDmaCallback;
__ARDmaCallback = callback;
_CPU_ISR_Restore(level);
return old;
}
static __inline__ void __usbgecko_exi_wait(s32 chn)
{
u32 level;
_CPU_ISR_Disable(level);
if(!usbgecko_inited) __usbgecko_init();
while(EXI_Lock(chn,EXI_DEVICE_0,__usbgecko_exi_unlock)==0) {
LWP_ThreadSleep(wait_exi_queue[chn]);
}
_CPU_ISR_Restore(level);
}
u32 InitMem2Manager ()
{
int size = (10*1024*1024);
u32 level;
_CPU_ISR_Disable(level);
size &= ~0x1f; // round down, because otherwise we may exceed the area
void *mem2_heap_ptr = (void *)((u32)SYS_GetArena2Hi()-size);
SYS_SetArena2Hi(mem2_heap_ptr);
_CPU_ISR_Restore(level);
size = __lwp_heap_init(&mem2_heap, mem2_heap_ptr, size, 32);
return size;
}
void ARQ_PostRequest(ARQRequest *req,u32 owner,u32 dir,u32 prio,u32 aram_addr,u32 mram_addr,u32 len)
{
u32 level;
ARQ_PostRequestAsync(req,owner,dir,prio,aram_addr,mram_addr,len,__ARQCallbackSync);
_CPU_ISR_Disable(level);
while(req->state!=ARQ_TASK_FINISHED) {
LWP_ThreadSleep(__ARQSyncQueue);
}
_CPU_ISR_Restore(level);
}