__asm void PendSV_Handler ()
{
PRESERVE8
IMPORT g_thread_current
IMPORT g_thread_next
switch_start
; Backup next thread address to R12
LDR R0, =g_thread_current ; Get pointer (current)
LDR R1, =g_thread_next ; Get pointer (next)
LDR R1, [R1] ; Load next thread address
MOV R12, R1 ; Save to R12
LDR R2, [R0] ; Load current thread address
STR R1, [R0] ; g_thread_current = g_thread_next
TST R2, R2
BEQ switch_next
; Save registers to stack, the registers in stack would be
; (H->L)
; xPSR, ReturnAddress(), LR(R14), R12, R3, R2, R1, R0
; R7, R6, R5, R4,
; R11, R10, R9, R8,
; Current LR(R14)
MRS R0, PSP
SUBS R0, #4 * 9
STR R0, [R2] ; Save PSP to thread handler
MOV R3, R9
MOV R2, R8
MOV R1, LR
STM R0!, {R1 - R3}
void GoToReturn( void )
{
address ra;
ra = ReturnAddress();
if( !IS_NIL_ADDR( ra ) ) {
GoToAddr( ra );
} else {
Error( ERR_NONE, LIT( ERR_NO_RETURN_ADDRESS ) );
}
}
void * __restrict FastAlloc::_Allocate(size_t dwSize)
{
OSEnterMutex(hAllocationMutex);
//assert(dwSize);
if(!dwSize) dwSize = 1;
LPVOID lpMemory;
Pool *pool;
if(dwSize < 0x8001)
{
MemInfo *meminfo = GetMemInfo(dwSize);
if(!meminfo->nextFree) //no pools have been created for this section
{
lpMemory = OSVirtualAlloc(0x10000);
if (!lpMemory) DumpError(TEXT("Out of memory while trying to allocate %d bytes at %p"), dwSize, ReturnAddress());
Pool *&poollist = PoolList[PtrTo32(lpMemory)>>24];
if(!poollist)
{
poollist = (Pool*)OSVirtualAlloc(sizeof(Pool)*256);
if (!poollist) DumpError(TEXT("Out of memory while trying to allocate %d bytes at %p"), dwSize, ReturnAddress());
zero(poollist, sizeof(Pool)*256);
}
pool = &poollist[(PtrTo32(lpMemory)>>16)&0xFF];
pool->lpMem = lpMemory;
pool->bytesTotal = 0x10000;
pool->meminfo = meminfo;
pool->firstFreeMem = (FreeMemInfo*)lpMemory;
pool->lastFreeMem = (FreeMemInfo*)lpMemory;
meminfo->nextFree = (FreeMemInfo*)lpMemory;
meminfo->nextFree->num = meminfo->maxBlocks;
meminfo->nextFree->lpPool = pool;
meminfo->nextFree->lpPrev = meminfo->nextFree->lpNext = NULL;
}
