; ; thread.s ; ; Preemptive multitasking of idle threads (MDW) ; ; © 1994-1998 Straylight ; ;----- Licensing note ------------------------------------------------------- ; ; This file is part of Straylight's Sapphire library. ; ; Sapphire is free software; you can redistribute it and/or modify ; it under the terms of the GNU General Public License as published by ; the Free Software Foundation; either version 2, or (at your option) ; any later version. ; ; Sapphire is distributed in the hope that it will be useful, ; but WITHOUT ANY WARRANTY; without even the implied warranty of ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ; GNU General Public License for more details. ; ; You should have received a copy of the GNU General Public License ; along with Sapphire. If not, write to the Free Software Foundation, ; 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. ;----- Standard header ------------------------------------------------------ GET libs:header GET libs:swis ;----- External dependencies ------------------------------------------------ GET sapphire:alloc GET sapphire:suballoc GET sapphire:idle GET sapphire:msgs GET sapphire:sapphire ;----- Main code ------------------------------------------------------------ AREA |Sapphire$$Code|,CODE,READONLY thr__stkSize EQU 1024 thr__idleFreq EQU 1 ; --- thread_create --- ; ; On entry: R0 == size of stack to allocate, or 0 for a default ; R1 == pointer to thread routine ; R2 == workspace pointer to pass in R10 ; R3 == workspace pointer to pass in R12 ; ; On exit: R0 == thread handle for the thread ; May return an error ; ; Use: Creates a new thread running `in the background' (i.e. over ; idle events). ; ; The thread is passed control with the registers R10 and R12 ; set up from R1 and R2 passed to this routine and R13 pointing ; to the top of a stack chunk. R0 on entry contains the ; thread's handle. The thread is passed the scratchpad ; address (in R11). The values of other registers are ; indeterminate and must not be relied upon. ; ; The default stack size for a new thread is 1K, although this ; may change in future. ; ; The thread may exit by calling thread_destroy or by ; returning in the normal way. EXPORT thread_create thread_create ROUT STMFD R13!,{R1-R8,R12,R14} ;Save some registers WSPACE thr__wSpace ;Load my workspace pointer ; --- Try to allocate the stack --- CMP R0,#0 ;Does he want the default? MOVEQ R0,#thr__stkSize ;Yes -- give it to him CMP R0,#256 ;Make sure it's big enough MOVLT R0,#256 ;If not, make it bigger MOV R5,R0 ;Keep a copy of the size BL alloc ;Try to allocate it nicely BLCS alloc_error ;It failed, so get an error BVS %99thread_create ;And skip to the end MOV R6,R0 ;Keep the stack block ptr ; --- Now allocate a thread block --- MOV R0,#thr__size ;The size of the block BL alloc ;Allocate the block for me BLCS alloc_error ;It failed, so get an error BVS %98thread_create ;If it failed, skip onward ; --- Set up the initial context --- ADD R5,R6,R5 ;R5 is the inital R13 to give MOV R14,R1 ;Get start address in R14 MOV R1,R2 ;Move `R10' down a register MOV R2,R11 ;Give scratchpad in R11 ADR R4,thread_destroy ;Returning destroys thread STMFD R5!,{R1-R4,R14} ;Save these on the stack STMFD R5!,{R0-R9} ;Fill the rest with rubbish ; --- Fill in the block --- ADD R14,R0,#thr__suspend ;Don't fill in the links STMIA R14,{R1-R8} ;Store information in block MOV R1,#0 ;Thread is not suspended yet MOV R2,#0 ;Thread not blocked on sem MOV R3,#0 ;Start at standard priority MOV R4,#thr__idleFreq ;Set standard timeslice STMIA R14!,{R1-R4} ;Save these in the block MOV R4,#0 ;Not in critical section MOV R7,#0 ;No flags to speak of yet MOV R8,#0 ;No error handler defined STMIA R14!,{R4-R8} ;Fill the rest in too BL thr__insert ;Insert it into the list ; --- Bump the active counter --- BL thr__incCount ;Bump the active counter LDMFD R13!,{R1-R8,R12,R14} ;Restore all the registers BICS PC,R14,#V_flag ;Return without V set ; --- We stumbled across a mishap --- 98thread_create MOV R4,R0 ;Keep the error pointer MOV R0,R6 ;Point to the stack block BL free ;Deallocate it -- don't want MOV R0,R4 ;Point to the error again 99thread_create ADD R2,R0,#4 ;Point to the error text ADR R0,thr__noCreate ;Point to the error block BL msgs_error ;Set up the error nicely LDMFD R13!,{R1-R8,R12,R14} ;Restore all the registers ORRS PC,R14,#V_flag ;Return with V set thr__noCreate DCD 1 DCB "thrNOCRT",0 LTORG ; --- thread_setPriority --- ; ; On entry: R0 == thread handle ; R1 == new priority to set ; ; On exit: -- ; ; Use: Changes the priority of a thread. The priority if a thread ; is a signed integer. The highest priority thread is the one ; which runs. If more than one thread has maximum priority, ; they are run in a cyclical order. EXPORT thread_setPriority thread_setPriority ROUT STMFD R13!,{R2,R3,R12,R14} ;Save some registers here WSPACE thr__wSpace ;Load my workspace pointer LDR R14,[R0,#thr__priority] ;Get the current priority CMP R1,R14 ;Are we changing anything? LDMEQFD R13!,{R2,R3,R12,PC}^ ;No -- return right now ; --- Unlink the thread --- LDMIA R0,{R2,R3} ;Get the next and previous CMP R2,#0 ;Is there a next pointer? STRNE R3,[R2,#thr__prev] ;Yes -- fill in its prev CMP R3,#0 ;Is there a prev pointer? STRNE R2,[R3,#thr__next] ;Yes -- fill in its next STREQ R2,wsp__threads ;No -- make next first thread ; --- Change priority and insert the thread again --- STR R1,[R0,#thr__priority] ;Save the new priority BL thr__insert ;Insert in the right place LDMFD R13!,{R2,R3,R12,PC}^ ;Return to caller LTORG ; --- thread_setTimeSlice --- ; ; On entry: R0 == thread handle ; R1 == new timeslice size, in centiseconds ; ; On exit: -- ; ; Use: Changes a thread's timeslice size. Specify 0 to indicate ; that thread shouldn't be pre-empted. EXPORT thread_setTimeSlice thread_setTimeSlice ROUT STR R1,[R0,#thr__timeSlice] ;Save the new timeslice MOVS PC,R14 ;Return to caller LTORG ; --- thread_destroy --- ; ; On entry: R0 == thread handle to destroy, if not executing a thread ; ; On exit: -- ; ; Use: Destroys either the current thread or a thread with the ; the given handle if no thread is executing currently. You ; can't destroy an arbitrary thread while running in one. ; ; If a thread is waiting for a semaphore, it is removed from ; the waiting list. EXPORT thread_destroy thread_destroy ROUT STMFD R13!,{R0-R5,R12,R14} ;Save some registers WSPACE thr__wSpace ;Find my workspace MOV R5,R0 ;Keep the thread pointer ; --- Find out which thread to destroy --- LDR R4,wsp__current ;Get the current thread CMP R4,#0 ;Is there one running? MOVNE R5,R4 ;Yes -- destroy it then ; --- If the thread was active, decrement active count --- LDR R14,[R5,#thr__suspend] ;Get the suspension counter CMP R14,#0 ;Is it currently active? BLEQ thr__decCount ;Yes -- decrement actives ; --- Remove thread from semaphore waiting lists --- LDR R3,[R5,#thr__semaphore] ;Get the blocking semaphore CMP R3,#0 ;Is there one? BEQ %10thread_destroy ;No -- skip this part MOV R2,#0 ;Previous item in the list LDR R0,[R3,#sem__blocked] ;Get the blocked list 00 CMP R0,#0 ;Have we reached the end? BEQ %10thread_destroy ;Yes -- skip onwards LDR R14,[R0,#sml__thread] ;Get the waiting thread hnd CMP R14,R5 ;Is it this thread? MOVNE R2,R0 ;No -- update previous ptr LDRNE R0,[R0,#sml__next] ;Find the next link block BNE %00thread_destroy ;And move to next block ; --- Delete this thread waiting block --- LDR R14,[R0,#sml__next] ;Find the next link block CMP R2,#0 ;Is there a previous block? STRNE R14,[R2,#sml__next] ;Yes -- store it as next STREQ R14,[R3,#sem__blocked] ;No -- next is new first item CMP R14,#0 ;Is there a next block? STREQ R2,[R3,#sem__blockEnd] ;No -- previous one is last BL free ;Destroy the link block ; --- Delink the thread then --- 10 LDMIA R5,{R1,R2} ;R1 == next, R2 == prev CMP R1,#0 ;Is there a next? STRNE R2,[R1,#thr__prev] ;Yes -- store prev away CMP R2,#0 ;Is there a prev? STRNE R1,[R2,#thr__next] ;Yes -- store next pointer STREQ R1,wsp__threads ;No -- store as list head ; --- Destroy the thread's memory --- LDR R0,[R5,#thr__stack] ;Find the thread's stack BL free ;Get rid of it -- it's no use MOV R0,R5 ;Get the pointer back again MOV R1,#thr__size ;The size of the block BL sub_free ;Destroy the thread block ; --- Now we must return, but where to? --- ; ; If the caller is not the thread itself, this is easy. ; If it *was* the thread, we've just killed its stack, so ; we can't return to it. Instead, we do the same job as ; thread_yield. CMP R4,#0 ;Was there a current thread? LDMEQFD R13!,{R0-R5,R12,PC}^ ;No -- return normally MOV R0,#0 ;We're stopping current thrd STR R0,wsp__current ;So clear current thread BL thr__end ;Kill off any handlers LDR R13,wsp__stackPtr ;Get the system stack pointer LDMFD R13!,{R0-R12,PC}^ ;Return to thread dispatcher LTORG ; --- thr__decCount --- ; ; On entry: -- ; ; On exit: -- ; ; Use: Decrements the active threads counter, and disables the idle ; claimer if there aren't any left. thr__decCount ROUT STMFD R13!,{R0-R3,R14} ;Save registers LDR R14,wsp__active ;Get the active counter SUBS R14,R14,#1 ;Decrement the counter STR R14,wsp__active ;Store it back again LDMGTFD R13!,{R0-R3,PC}^ ;If some still active, return ; --- Remove the idle claiming routine --- MOV R0,#thr__idleFreq ;How often I should be called ADR R1,thr__idles ;Point to the idle handler MOV R2,#0 ;Don't care about R10 MOV R3,R12 ;Pass workspace in R12 BL idle_removeHandler ;Stop it from being called LDMFD R13!,{R0-R3,PC}^ ;Return to caller LTORG ; --- thr__incCount --- ; ; On entry: -- ; ; On exit: -- ; ; Use: Increments the active threads counter, and adds in the idle ; claimer if it was previously disabled. thr__incCount ROUT STMFD R13!,{R0-R3,R14} ;Save registers LDR R14,wsp__active ;Get the active counter ADD R14,R14,#1 ;Increment the counter STR R14,wsp__active ;Store it back again CMP R14,#1 ;Was it previously off? LDMGTFD R13!,{R0-R3,PC}^ ;No -- return right now ; --- Add in the idle claiming routine --- MOV R0,#thr__idleFreq ;How often I should be called ADR R1,thr__idles ;Point to the idle handler MOV R2,#0 ;Don't care about R10 MOV R3,R12 ;Pass workspace in R12 BL idle_handler ;Get it called nicely LDMFD R13!,{R0-R3,PC}^ ;Return to caller LTORG ; --- thr__idles --- ; ; On entry: -- ; ; On exit: -- ; ; Use: Handles idle events by passing control to each thread in ; turn. This is the main scheduler for the threads. ; ; We use a simple but well-respected algorithm. We find the ; first active thread in the list, move it to the end of its ; priority group and run it. thr__idles ROUT ; --- Save current context away --- ; ; We don't actually return from this routine -- actually ; we restore from thread_destroy, thread_suspend or ; thread_yield, or on the CallBack from a timer interrupt. STMFD R13!,{R0-R12,R14} ;Save context on the stack STR R13,wsp__stackPtr ;Save the stack pointer ; --- Now we need to find an unsuspended thread --- ; ; There must be an active thread, because we don't get called ; unless the count is non-0 LDR R10,wsp__threads ;Find the first thread 10thr__idles CMP R10,#0 ;Is this the end? LDMEQFD R13!,{R0-R12,PC}^ ;Yes -- nothing to do then LDR R0,[R10,#thr__suspend] ;Get the thread's suspend CMP R0,#0 ;Is this one active? LDRNE R10,[R10,#thr__next] ;No -- get the next one out BNE %10thr__idles ;And go round again ; --- We have an active thread --- ; ; We now unlink the thread and move it to the end of its ; priority group in the list, so next time the next one in ; the group gets a chance. LDR R9,[R10,#thr__priority] ;Get the thread's priority LDMIA R10,{R0,R1} ;Get next and prev pointers CMP R0,#0 ;Is there a next pointer? STRNE R1,[R0,#thr__prev] ;Yes -- fill in its prev CMP R1,#0 ;Is there a prev pointer? STRNE R0,[R1,#thr__next] ;Yes -- fill in its next STREQ R0,wsp__threads ;No -- make it the new first ; --- Now search forwards for the end of the group --- 20thr__idles CMP R0,#0 ;Is this the list end? BEQ %25thr__idles ;Yes -- skip out of loop LDR R8,[R0,#thr__priority] ;Get the next one's priority CMP R8,R9 ;Do they match up nicely? MOVGE R1,R0 ;This is the previous one LDRGE R0,[R0,#thr__next] ;Get the next one out BGE %20thr__idles ;And loop round again ; --- Insert our thread between R1 and R0 --- 25thr__idles CMP R1,#0 ;Is the previous one OK? STRNE R10,[R1,#thr__next] ;Yes -- fill in its next STREQ R10,wsp__threads ;Otherwise make it first one CMP R0,#0 ;Is there a next one? STRNE R10,[R0,#thr__prev] ;Yes -- fill in its previous STMIA R10,{R0,R1} ;Save next and prev back ; --- Now we can run the thread at last --- LDR R0,[R10,#thr__timeSlice] ;Load the timeslice we want BL thr__start ;Set up timer interrupt STR R10,wsp__current ;Save the current thread LDR R13,[R10,#thr__stackPtr] ;Find its stack pointer LDMFD R13!,{R0-R12,R14,PC}^ ;Start it up again LTORG ; --- thr__insert --- ; ; On entry: R0 == pointer to thread block ; ; On exit: -- ; ; Use: Inserts a thread into the thread list in the right place for ; its priority. thr__insert ROUT STMFD R13!,{R1-R4,R14} ;Save some registers LDR R4,[R0,#thr__priority] ;Get the thread;s priority LDR R1,wsp__threads ;Find the first item MOV R2,#0 ;No previous list item yet 10thr__insert CMP R1,#0 ;Is this the end of the list? BEQ %20thr__insert ;Yes -- skip forwards LDR R3,[R1,#thr__priority] ;Get its priority ready CMP R3,R4 ;How do they compare? MOVGT R2,R1 ;Too high -- now previous LDRGT R1,[R1,#thr__next] ;Find the next thread handle BGT %10thr__insert ;And loop round for another ; --- Insert the thread between R2 and R1 --- 20thr__insert CMP R2,#0 ;Is there a previous one? STRNE R0,[R2,#thr__next] ;Yes -- fill in its next STREQ R0,wsp__threads ;No -- this is the first one CMP R1,#0 ;Is there a next one? STRNE R0,[R1,#thr__prev] ;Yes -- fill in its prev STMIA R0,{R1,R2} ;Store next and prev in block LDMFD R13!,{R1-R4,PC}^ ;Return to caller then LTORG ; --- thread_suspend --- ; ; On entry: R0 == thread handle, or 0 for the current thread ; ; On exit: -- ; ; Use: Suspends a thread's execution. If a thread is currently ; running, that thread is suspended. Otherwise, any thread ; may be suspended. ; ; If the thread is currently claiming semaphores, the ; semaphores are not released, because we don't whether the ; system is in a fit state for this. ; ; Thread suspensions are counted. i.e. if you suspend a thread ; 5 times, you have to resume it 5 times for it to become ; active again. EXPORT thread_suspend thread_suspend ROUT STMFD R13!,{R12,R14} ;Save some registers away WSPACE thr__wSpace ;Locate my workspace LDR R14,wsp__current ;Get the current thread CMP R0,#0 ;Is current one wanted? CMPNE R0,R14 ;Or is it just coincidence? BNE %10thread_suspend ;No -- deal with that case ; --- We want to suspend the current thread --- ; ; To suspend, we need to save the current thread context, ; and resume from the system stack. We know the thread must ; be active currently, otherwise we wouldn't be executing ; it! STMFD R13!,{R0-R12} ;Save entire context on stack STR R13,[R14,#thr__stackPtr] ;Save the thread's stack ptr MOV R0,#1 ;Thread must be active STR R0,[R14,#thr__suspend] ;So store 1 as suspend count BL thr__decCount ;There's one less active now ; --- Now restore context to dispatcher --- BL thr__end ;Stop all the handlers MOV R0,#0 ;There is no current thread STR R0,wsp__current ;So clear current pointer LDR R13,wsp__stackPtr ;Get the system stack pointer LDMFD R13!,{R0-R12,PC}^ ;Restore context again ; --- We're just meant to suspend any old thread --- ; ; We just need to bump its counter -- this is easy --- 10 LDR R14,[R0,#thr__suspend] ;Get current suspend count ADD R14,R14,#1 ;Bump it up one STR R14,[R0,#thr__suspend] ;Store it back again CMP R14,#1 ;Was it previously active? BLEQ thr__decCount ;Yes -- decrement actives LDMFD R13!,{R12,PC}^ ;Return to caller happy LTORG ; --- thread_resume --- ; ; On entry: R0 == thread handle ; ; On exit: -- ; ; Use: Allows a suspended thread to continue operations. If you ; resume a thread more times than it has been suspended, ; any excess resumes are ignored. You can't resume a thread ; to stop it being blocked by a semaphore. EXPORT thread_resume thread_resume ROUT STMFD R13!,{R10,R12,R14} ;Save some registers WSPACE thr__wSpace ;Find my workspace address ; --- Make sure the thread isn't running now --- LDR R14,[R0,#thr__suspend] ;Get the suspension counter CMP R14,#0 ;Is it zero already? LDMEQFD R13!,{R10,R12,PC}^ ;Yes -- return right now ; --- Decrement the counter --- LDR R10,[R0,#thr__semaphore] ;Get the blocking semaphore CMP R10,#0 ;Is there a blocking sem? MOVEQ R10,#0 ;No -- minimum count is 0 MOVNE R10,#1 ;Yes -- minimum count is 1 SUB R14,R14,#1 ;Decrement the counter CMP R14,R10 ;Is it too low? MOVLT R14,R10 ;Yes -- bring it up again STR R14,[R0,#thr__suspend] ;Store the counter back again ; --- Bump the active count if thread now active --- CMP R14,#0 ;Is the thread active now? BLEQ thr__incCount ;Yes -- increment the count CMP R14,#1 ;Is the thread almost active? LDMNEFD R13!,{R10,R12,PC}^ ;No -- return right now ; --- Check if the thread can start from a semaphore --- STMFD R13!,{R0-R2} ;Save some registers MOV R10,R0 ;Look after the thread handle LDR R14,[R10,#thr__semaphore] ;Is it waiting for a sem? CMP R14,#0 ;Is the pointer null? BEQ %20thread_resume ;Yes -- skip to end LDR R0,[R14,#sem__counter] ;Is it signalled enough? CMP R0,#0 ;If so, it isn't 0 BEQ %20thread_resume ;If not, skip to the end ; --- Go through the waiting list --- ; ; We want to find the entry in the list, but we also want ; to avoid `jumping the queue' -- i.e. getting the semaphore ; before an unsuspended thread. With the algorithms used ; in thread_signal, I don't think this can happen, but it's ; as well to make sure. We have to go through the list ; anyway. LDR R0,[R14,#sem__blocked] ;Find the blocked list MOV R2,#0 ;No previous block found 10 CMP R0,#0 ;Is it the end of the list? BEQ %20thread_resume ;Yes -- it's all over then LDR R1,[R0,#sml__thread] ;Get the thread's handle CMP R1,R10 ;Do they match up? BEQ %11thread_resume ;Yes -- skip onwards LDR R1,[R1,#thr__suspend] ;Get the suspended count CMP R1,#0 ;Is this thread active? BEQ %20thread_resume ;Yes -- skip to end MOV R2,R0 ;Set up the previous pointer LDR R0,[R0,#sml__next] ;Get the next block B %10thread_resume ;And go back round the loop ; --- Remove it from the waiting list --- 11thread_resume LDR R1,[R0,#sml__next] ;Get the next list item CMP R2,#0 ;Is there a previous one? STRNE R1,[R2,#sml__next] ;Yes -- fill in its next ptr STREQ R1,[R14,#sem__blocked] ;No -- it's the new list head CMP R1,#0 ;Is there a next block? STREQ R2,[R14,#sem__blockEnd] ;No -- fill in the last ptr MOV R2,R14 ;Keep the semaphore pointer MOV R1,#sml__size ;Size of the link blocks BL sub_free ;Destroy the link block ; --- Now decrement the semaphore counter --- LDR R0,[R2,#sem__counter] ;Get the current counter SUB R0,R0,#1 ;Decrement it nicely STR R0,[R2,#sem__counter] ;Store it back again MOV R0,#0 ;Thread not suspended at all STR R0,[R10,#thr__suspend] ;Store 0 as suspend count BL thr__incCount ;Increment active threads 20thread_resume LDMFD R13!,{R0-R2,R10,R12,PC}^ ;Return to caller LTORG ; --- thread_yield --- ; ; On entry: -- ; ; On exit: -- ; ; Use: Pauses the thread for a while. You only need to use this ; call if you have stopped the current thread from being ; timesliced. EXPORT thread_yield thread_yield ROUT STMFD R13!,{R0-R14} ;Save the old context WSPACE thr__wSpace ;Find my workspace pointer LDR R0,wsp__current ;Get the current thread ptr BL thr__end ;Kill off the handlers STR R13,[R0,#thr__stackPtr] ;Save the thread's stack ptr MOV R0,#0 ;Current thread is stopped STR R0,wsp__current ;So clear the pointer LDR R13,wsp__stackPtr ;Get the system stack pointer LDMFD R13!,{R0-R12,PC}^ ;Restore main system context LTORG ; --- thread_createSem --- ; ; On entry: R0 == initial value for semaphore (0 for counter, 1 for ; mutex) ; ; On exit: R0 == semaphore handle and V clear if all went well ; R0 == pointer to error and V set if something went wrong ; ; Use: Creates a semaphore with the given initial counter value. ; ; The semaphore can be used to provide serialised access to ; a resource by initialising its value to 1 and performing the ; following: ; ; thread_wait(mySemaphore) ; // ; // Do things with the resource ; // ; thread_signal(mySemaphore) ; ; Or you can inform a thread that it has items in its input ; queue by having the following in the thread code: ; ; while true ; thread_wait(theSemaphore) ; getFromQueue(myQueue,item) ; process(item) ; endWhile ; ; and when inserting queue items: ; ; addToQueue(item) ; thread_signal(theSemaphore) ; ; It is distinctly possible that input queue management will ; be introduced in a separate Sapphire module. EXPORT thread_createSem thread_createSem ROUT STMFD R13!,{R0-R3,R14} ;Save some registers MOV R0,#sem__size ;The size of a semaphore BL sub_alloc ;Try to create the semaphore BVS %99thread_createSem ;If it failed, tidy up LDR R1,[R13],#4 ;Get the initial sem value MOV R2,#0 ;No threads waiting either MOV R3,#0 ;So no last link block STMIA R0,{R1-R3} ;Save in semaphore block LDMFD R13!,{R1-R3,PC} ;Return to caller ; --- Error occurred --- 99 ADD R2,R0,#4 ;Point to the error text ADR R0,thr__noSemCrt ;Point to error block BL msgs_error ;Translate the error message ADD R13,R13,#4 ;Don't restore R0 from stack LDMFD R13!,{R1-R3,PC} ;Return with V set on exit thr__noSemCrt DCD 1 DCB "thrNOSEMCRT",0 LTORG ; --- thread_destroySem --- ; ; On entry: R0 == semaphore handle ; ; On exit: -- ; ; Use: Destroys a semaphore when it's no use any more. If threads ; are waiting for it, an error is generated. EXPORT thread_destroySem thread_destroySem ROUT STMFD R13!,{R1,R14} ;Save some registers away LDR R14,[R0,#sem__blocked] ;Get the waiting list head CMPEQ R14,#0 ;Is there a waiting list? BNE %90thread_destroySem ;If so, complain MOV R1,#sem__size ;The size of a semaphore BL sub_free ;Free up the block LDMFD R13!,{R1,PC}^ ;Return to caller ; --- The semaphore is still in use --- 90 ADR R0,thr__semInUse ;Point to error message BL msgs_error ;Translate the message SWI OS_GenerateError ;It's a serious problem thr__semInUse DCD 1 DCB "thrSEMINUSE",0 LTORG ; --- thread_threaded --- ; ; On entry: -- ; ; On exit: CS if currently running a thread, CC otherwise ; ; Use: Informs the caller whether a thread is currently executing. EXPORT thread_threaded thread_threaded ROUT STMFD R13!,{R12} ;Save a register WSPACE thr__wSpace ;Load my workspace address LDR R12,wsp__current ;Load current thread handle CMP R12,#0 ;Is there a thread running? LDMFD R13!,{R12} ;Restore the register ORRNES PC,R14,#C_flag ;Return CS if thread running BICEQS PC,R14,#C_flag ;Otherwise return CC LTORG ; --- thread_wait --- ; ; On entry: R0 == semaphore handle ; ; On exit: If successful, R0 preserved and V clear. ; If failed, R0 == pointer to error block and V set ; ; Use: Waits on a sempahore. The algorithm actually is as follows: ; ; if semaphore.counter == 0 then ; addToWaitingList(semaphore,currentThread) ; suspend(currentThread) ; else ; semaphore.counter -= 1 ; endIf ; ; See thread_createSem for suggestions on how to make use of ; semaphores. EXPORT thread_wait thread_wait ROUT BIC R14,R14,#V_flag ;Assume that all is well STMFD R13!,{R10,R12-R14} ;Save some registers WSPACE thr__wSpace ;Locate my workspace LDR R10,wsp__current ;Find the current thread CMP R10,#0 ;Is it nonexistant? BEQ %99thread_wait ;Yes -- this is an error ; --- Do the messing about with the counter --- LDR R14,[R0,#sem__counter] ;Get the semaphore counter SUBS R14,R14,#1 ;Decrement the counter STRGE R14,[R0,#sem__counter] ;If it was nonzero, store LDMEQFD R13!,{R10,R12,R14,PC}^ ;And return to caller ; --- Add the thread to the waiting list --- STMFD R13!,{R0-R9} ;Save rest of the context MOV R9,R0 ;Keep the semaphore handle MOV R0,#sml__size ;Size of a link block BL sub_alloc ;Allocate a link block BVS %90thread_wait ;If it failed, return error MOV R1,#0 ;No next waiting thread STMIA R0,{R1,R10} ;Save information in block LDR R1,[R9,#sem__blockEnd] ;Get the last waiting block CMP R1,#0 ;Is there one at all? STRNE R0,[R1,#sml__next] ;Yes -- store in its next STREQ R0,[R9,#sem__blocked] ;No -- it's the first one STR R0,[R9,#sem__blockEnd] ;It's certainly the last one ; --- Now suspend the thread for a while --- MOV R1,#1 ;Thread is currently active STR R1,[R10,#thr__suspend] ;Not any more it isn't STR R9,[R10,#thr__semaphore] ;Remember the blocking sem ; --- Now switch back to main context --- BL thr__end ;Remove all the handlers STR R13,[R10,#thr__stackPtr] ;Save the current stackptr LDR R13,wsp__stackPtr ;Find the system stack ptr LDMFD R13!,{R0-R12,PC}^ ;And return to normality ; --- An error occurred while suspending --- 90thread_wait ADD R2,R0,#4 ;Point to the error text ADR R0,thr__noWaitSem ;Point to the error message BL msgs_error ;Translate the error ADD R13,R13,#4 ;Don't restore R0 on exit LDMFD R13!,{R1-R12,R14} ;Restore all the registers ORRS PC,R14,#V_flag ;Return the error back thr__noWaitSem DCD 1 DCB "thrNOWAITSEM",0 ; --- The caller isn't a thread --- 99thread_wait ADR R0,thr__notAThrd ;Point to the error BL msgs_error ;Translate the message SWI OS_GenerateError ;And generate the error thr__notAThrd DCD 1 DCB "thrNOTATHRD",0 LTORG ; --- thread_signal --- ; ; On entry: R0 == semaphore handle ; ; On exit: -- ; ; Use: Increments a semaphore's counter if no threads are waiting ; for it, or releases a thread waiting for the semaphore. ; ; The actual algorithm is shown below: ; ; if semaphore.waitingList != 0 then ; thread = removeFromWaitingList(semaphore) ; unSuspend(thread) ; else ; semaphore.counter += 1; ; endif ; ; See thread_createSem for suggestions on how to make use of ; semaphores. EXPORT thread_signal thread_signal ROUT STMFD R13!,{R0-R3,R10,R12,R14} ;Save a load of registers WSPACE thr__wSpace ;Locate my workspace ; --- Find a thread to restore control to --- MOV R3,R0 ;Look after the sem handle MOV R2,#0 ;No previous block yet LDR R0,[R3,#sem__blocked] ;Find blocked list head 10thread_signal CMP R0,#0 ;Is this the end? BEQ %30thread_signal ;Yes -- increment counter LDR R10,[R0,#sml__thread] ;Get the thread handle out LDR R14,[R10,#thr__suspend] ;Find its suspended count CMP R14,#1 ;Is it only blocked by sem? MOVGT R2,R0 ;No -- this is now prev LDRGT R0,[R0,#sml__next] ;Find the next one BGT %10thread_signal ;And check that one out ; --- Found a suitable thread --- LDR R14,[R0,#sml__next] ;Get the next pointer out CMP R2,#0 ;Is there a previous block? STRNE R14,[R2,#sml__next] ;Yes -- fix its next ptr STREQ R14,[R3,#sem__blocked] ;No -- this is now first one CMP R14,#0 ;Is there a next block? STREQ R2,[R3,#sem__blockEnd] ;Yes -- previous is now last MOV R1,#sml__size ;The size of a link block BL sub_free ;Don't need it any more ; --- Let the new thread go --- MOV R0,#0 ;Thread is no longer blocked STR R0,[R10,#thr__suspend] ;Unblock the thread nicely STR R0,[R10,#thr__semaphore] ;No blocking semaphore now LDMFD R13!,{R0-R3,R10,R12,PC}^ ;Return to caller happily ; --- No threads found -- increment the counter --- 30thread_signal LDR R14,[R3,#sem__counter] ;Get the current counter ADD R14,R14,#1 ;Increment it a little bit STR R14,[R3,#sem__counter] ;Store new counter back LDMFD R13!,{R0-R3,R10,R12,PC}^ ;Return to caller happily LTORG ; --- thread_init --- ; ; On entry: -- ; ; On exit: -- ; ; Use: Initialises the thread system for use. EXPORT thread_init thread_init ROUT STMFD R13!,{R12,R14} ;Save some registers ; --- Make sure we're not initialised --- WSPACE thr__wSpace ;Find my workspace pointer LDR R14,wsp__flags ;Load my flags word TST R14,#thrFlag__inited ;Am I initialised yet? LDMNEFD R13!,{R12,PC}^ ;Yes -- return right now ORR R14,R14,#thrFlag__inited ;Set the flag STR R14,wsp__flags ;Save the flags word back ; --- Initialise the rest of the workspace --- STMFD R13!,{R0-R4} ;Save some more registers MOV R0,#0 ;No threads registered yet MOV R1,#0 ;No active threads, then MOV R2,#0 ;No thread running now MOV R3,#0 ;No system stack pointer STMIB R12,{R0-R3,R11} ;Save them in the workspace ; --- Start up other things we need --- BL alloc_init ;We need redirectble alloc BL sub_init ;And allocating small blocks LDMFD R13!,{R0-R4,R12,PC}^ ;Return to caller LTORG thr__wSpace DCD 0 ;----- Handling pre-emption of threads -------------------------------------- ; --- thr__start --- ; ; On entry: R0 == timeslice size in centiseconds ; ; On exit: -- ; ; Use: Sets up handlers and a timer interrupt for starting a ; thread. thr__start ROUT STMFD R13!,{R0-R4,R14} ;Save some registers away ADR R4,wsp__handlers ;Point to handlers save area ; --- Set up the handlers properly --- MOV R0,#7 ;Install CallBack handler ADR R1,thr__callBack ;Point to handler routine MOV R2,R12 ;Point to my workspace ADR R3,wsp__regBuff ;Point to the register block SWI OS_ChangeEnvironment ;Install the handler STMIA R4!,{R1-R3} ;Save the old handler MOV R0,#10 ;Install event handler ADR R1,thr__events ;Point to handler routine MOV R2,R12 ;Point to my workspace SWI OS_ChangeEnvironment ;Install the handler STMIA R4!,{R1-R3} ;Save the old handler MOV R0,#6 ;Install error handler ADR R1,thr__errors ;Point to handler routine MOV R2,R12 ;Point to my workspace MOV R3,R11 ;Use Scratchpad for error SWI OS_ChangeEnvironment ;Install the handler STMIA R4!,{R1-R3} ;Save the old handler MOV R0,#11 ;Install exit handler ADR R1,thr__exit ;Point to handler routine MOV R2,R12 ;Point to my workspace SWI OS_ChangeEnvironment ;Install the handler STMIA R4!,{R1-R3} ;Save the old handler ; --- Enable my event --- MOV R0,#14 ;Enable an event MOV R1,#9 ;I'll use the user event SWI OS_Byte ;Enable the event for me ; --- Set up the timer for me --- LDR R0,[R13,#0] ;Load the timeslice value CMP R0,#0 ;Do we set the timer? ADRNE R1,thr__timer ;Point to timer routine MOVNE R2,R12 ;Point to my workspace SWINE OS_CallAfter ;Install the handler then LDMFD R13!,{R0-R4,PC}^ ;Return to caller LTORG ; --- thr__end --- ; ; On entry: -- ; ; On exit: -- ; ; Use: Removes all the handlers set up when a thread starts thr__end ROUT STMFD R13!,{R0-R4,R14} ;Save some registers ; --- Restore handlers to their old values --- ADR R4,wsp__handlers ;Point to saved handlers MOV R0,#7 ;Restore CallBack handler LDMIA R4!,{R1-R3} ;Restore handler registers SWI OS_ChangeEnvironment ;Put them all back then MOV R0,#10 ;Restore Event handler LDMIA R4!,{R1-R3} ;Restore handler registers SWI OS_ChangeEnvironment ;Put them all back then MOV R0,#6 ;Restore Error handler LDMIA R4!,{R1-R3} ;Restore handler registers SWI OS_ChangeEnvironment ;Put them all back then MOV R0,#11 ;Restore Exit handler LDMIA R4!,{R1-R3} ;Restore handler registers SWI OS_ChangeEnvironment ;Put them all back then MOV R0,#13 ;Disable an event MOV R1,#9 ;I'm using the user event SWI OS_Byte ;Disable it nicely ; --- Get rid of my ticker event --- ; ; We try and remove it -- if it was never there anyway, ; or it's happened, we don't care. ADR R0,thr__timer ;Point to my timer handler MOV R1,R12 ;Get my workspace pointer SWI XOS_RemoveTickerEvent ;Remove the ticker event ; --- Kill off the `In error' flag --- LDR R14,wsp__flags ;Load my flags word BIC R14,R14,#thrFlag__inErr ;Clear the flag bit STR R14,wsp__flags ;Store it back again then LDMFD R13!,{R0-R4,PC}^ ;Return to caller LTORG ; --- thr__errors --- ; ; On entry: R0 == pointer to my workspace ; ; On exit: Doesn't ; ; Use: Handles an error in a thread. thr__errors ROUT SWI OS_IntOff ;Disable interrupts a while MOV R12,R0 ;Point to my workspace LDR R11,wsp__R11 ;Load scratchpad address LDR R10,wsp__current ;Find the current thread MOV R1,#1 ;Enter a critical section STR R1,[R10,#thr__critCount] ;Save critical counter SWI OS_IntOn ;Safe to interrupt again ADD R9,R10,#thr__errorHnd ;Locate the error handler LDR R5,[R9],#4 ;Get the handler address CMP R5,#0 ;Is there one set up? BEQ %50thr__errors ;No -- skip round next bit ; --- Am I already doing this? --- LDR R14,wsp__flags ;Load my flags word TST R14,#thrFlag__inErr ;Am I already going? BNE %50thr__errors ;Yes -- skip round next bit ; --- Handle the error and find resume point --- ORR R14,R14,#thrFlag__inErr ;Remember I'm doing this STR R14,wsp__flags ;Save it back again ADD R0,R11,#4 ;Point to error block LDMIA R9,{R1,R13} ;Get registers to pass out STMFD R13!,{R12} ;Save the old R12 away MOV R12,R1 ;Pass its workspace pointer MOV R14,PC ;Set up return address MOV PC,R5 ;Call the handler ; --- Call the resume point --- LDMFD R13!,{R12} ;Restore my workspace LDR R14,wsp__flags ;Find my flags again BIC R14,R14,#thrFlag__inErr ;We're coming out again STR R14,wsp__flags ;Store the flags back nicely BL thread_leaveCrit ;Leave the critical section MOV R12,R1 ;Set up resumer's workspace MOV PC,R0 ;Call the resumer ; --- It all went wrong --- 50thr__errors LDR R13,wsp__stackPtr ;Get the system stack pointer MOV R0,#0 ;No current thread STR R0,wsp__current ;So clear the pointer BL thr__end ;Remove all its handlers MOV R0,R10 ;Point to the thread BL thread_destroy ;Kill off the thread ADD R0,R11,#4 ;Point to error block SWI OS_GenerateError ;Pass to main handler LTORG ; --- thr__exit --- ; ; On entry: R12 == my workspace address ; ; On exit: -- ; ; Use: Handles OS_Exit calls in a thread. Closes down the thread ; and propagates the exit thr__exit ROUT LDR R11,wsp__R11 ;Load scratchpad pointer SWI OS_IntOff ;Disable interrupts off LDR R13,wsp__stackPtr ;Get the system stack pointer BL thr__end ;Remove all its handlers SWI OS_IntOn ;Save to interrupt again LDR R0,wsp__current ;Get the current thread MOV R1,#0 ;No current thread STR R1,wsp__current ;So clear the pointer BL thread_destroy ;Destroy the thread SWI OS_Exit LTORG ; --- thr__callBack --- ; ; On entry: R12 == pointer to my workspace ; ; On exit: -- ; ; Use: Performs appropriate operations on receipt of a timer ; interrupt. If the thread is in a critical section, it is ; marked `should be stopped'. Otherwise, the context is ; saved on the thread's stack and control returned to the ; main program thr__callBack ROUT LDR R10,wsp__current ;Get the current thread LDR R14,[R10,#thr__critCount] ;Is it in a critical bit? CMP R14,#0 ;If so, this is <>0 BEQ %10thr__callBack ;Otherwise skip forwards ; --- Set the thread's flag and continue --- LDR R14,[R10,#thr__flags] ;Get the thread's flags word ORR R14,R14,#tFlag__timeUp ;Kill it on exit from crit STR R14,[R10,#thr__flags] ;Store flags word back again ADR R14,wsp__regBuff ;Point to save buffer LDMIA R14,{R0-R14}^ ;Get the user registers out MOV R0,R0 ;Otherwise ARM complains LDR R14,[R14,#15*4] ;Load the saved PC MOVS PC,R14 ;Return to the thread ; --- Save the thread's context --- 10thr__callBack ADR R14,wsp__regBuff ;Point to save buffer LDR R11,[R14,#13*4] ;Find the thread's stack ptr SUB R11,R11,#15*4 ;Make way for the context STR R11,[R10,#thr__stackPtr] ;Save the thread's stack ptr LDMIA R14!,{R0-R7} STMIA R11!,{R0-R7} LDMIA R14!,{R0-R7} STMIA R11!,{R0-R4,R6,R7} ;Don't transfer R13 ; --- Go back to the main program --- LDR R11,wsp__stackPtr ;Find the system stack ptr LDR R14,[R11,#13*4] ;Get the saved link register TEQP R14,#0 ;Set this as the current mode MOV R0,R0 ;Stop ARM from being odd MOV R13,R11 ;Point to the stack properly BL thr__end ;Stop all the handlers LDMFD R13!,{R0-R12,PC}^ ;Rejoin the main thread LTORG ; --- thr__events --- ; ; On entry: R0 == event code (ideally this is 9) ; R1 == my magic identifier number (just this thing, y'know) ; ; On exit: R12 may contain 1 ; ; Use: Handles events. If it's the magic timer event, then I get ; myself a CallBack. thr__events ROUT CMP R0,#9 ;Is it a user event? MOVNES PC,R14 ;No -- return right away STMFD R13!,{R14} ;Save the link away LDR R14,thr__myMagic ;Get my magic number CMP R1,R14 ;Does it match up? MOVEQ R12,#1 ;Yes -- request a CallBack LDMFD R13!,{PC}^ ;Return to caller LTORG thr__myMagic DCB "MDW!" ; --- thr__timer --- ; ; On entry: -- ; ; On exit: -- ; ; Use: Generates an event that can eventually wend its way to my ; CallBack handler thr__timer ROUT STMFD R13!,{R0,R1,R14} ;Save some registers MOV R0,#9 ;Make a user event LDR R1,thr__myMagic ;My special identifier SWI OS_GenerateEvent ;Make the event happen LDMFD R13!,{R0,R1,PC}^ ;Return to caller LTORG ; --- thread_enterCrit --- ; ; On entry: -- ; ; On exit: -- ; ; Use: Declares that the current thread is about to enter a ; critical section and must not be interrupted. EXPORT thread_enterCrit thread_enterCrit ROUT STMFD R13!,{R10,R12,R14} ;Save some registers away WSPACE thr__wSpace ;Find my workspace LDR R10,wsp__current ;Get the current thread CMP R10,#0 ;Is there one? LDMEQFD R13!,{R10,R12,PC}^ ;No -- this is a no-op then LDR R14,[R10,#thr__critCount] ;Get the current counter ADD R14,R14,#1 ;Bump it along one STR R14,[R10,#thr__critCount] ;Save the counter back LDMFD R13!,{R10,R12,PC}^ ;Return to caller LTORG ; --- thread_leaveCrit --- ; ; On entry: -- ; ; On exit: -- ; ; Use: Declares that the current thread has left the critical ; section and can be interrupted again. EXPORT thread_leaveCrit thread_leaveCrit ROUT STMFD R13!,{R10,R12-R14} ;Save some registers away WSPACE thr__wSpace ;Find my workspace LDR R10,wsp__current ;Get the current thread CMP R10,#0 ;Is there one? LDMEQFD R13!,{R10,R12,R14,PC}^ ;No -- this is a no-op then LDR R14,[R10,#thr__critCount] ;Get the current counter SUBS R14,R14,#1 ;Chop one off it MOVLT R14,#0 ;Stop it going negative STR R14,[R10,#thr__critCount] ;Save the counter back LDMLEFD R13!,{R10,R12,R14,PC}^ ;If still critical, leave it ; --- Check to see if it should stop now --- LDR R14,[R10,#thr__flags] ;Get the thread's flags TST R14,#tFlag__timeUp ;Has the timer happened? LDMEQFD R13!,{R10,R12,R14,PC}^ ;No -- leave it to go then BL thr__end ;Stop the handlers and things STMFD R13!,{R0-R9} ;Save rest of the context STR R13,[R10,#thr__stackPtr] ;Save the stack pointer LDR R13,wsp__stackPtr ;Load the system stack ptr STMFD R13!,{R0-R12,PC}^ ;Return to main thread LTORG ; --- thread_errorHandler --- ; ; On entry: R0 == pointer to routine to call ; R1 == R12 value to call with ; R2 == R13 value to call with ; ; On exit: -- ; ; Use: Sets up the error handler for a thread. EXPORT thread_errorHandler thread_errorHandler STMFD R13!,{R12,R14} ;Save some registers WSPACE thr__wSpace ;Locate my workspace LDR R14,wsp__current ;Get the current thread ADD R14,R14,#thr__errorHnd ;Point to handler block STMIA R14,{R0-R2} ;Save the information away LDMFD R13!,{R12,PC}^ ;Return to caller LTORG ;----- Data structures ------------------------------------------------------ ; --- The thread structure --- ^ 0 thr__next # 4 ;Pointer to next thread thr__prev # 4 ;Pointer to previous thread thr__suspend # 4 ;Thread's suspension counter thr__semaphore # 4 ;Pointer to blocking sem thr__priority # 4 ;Priority of this thread thr__timeSlice # 4 ;Size of timeslice for thread thr__critCount # 4 ;Critical section counter thr__stackPtr # 4 ;Thread's stack pointer thr__stack # 4 ;Pointer to thread's stack thr__flags # 4 ;Various flags for the thread thr__errorHnd # 4*3 ;The error handler for it thr__size # 0 ;Size of this block tFlag__timeUp EQU (1<<0) ;Pre-empt after critical sect ; --- The semaphore link structure --- ^ 0 sml__next # 4 ;Pointer to next link block sml__thread # 4 ;Pointer to suspended thread sml__size # 0 ;Size of this block ; --- The main semaphore block --- ^ 0 sem__counter # 4 ;Semaphore counter sem__blocked # 4 ;List of blocked threads sem__blockEnd # 4 ;Pointer to last link in list sem__size # 0 ;Size of this block ;----- Workspace ------------------------------------------------------------ ^ 0,R12 thr__wStart # 0 wsp__flags # 4 ;Various useful flags wsp__threads # 4 ;Pointer to list of threads wsp__active # 4 ;Counter of active threads wsp__current # 4 ;Pointer to current thread wsp__stackPtr # 4 ;The system stack pointer wsp__R11 # 4 ;Sapphire's R11 pointer wsp__handlers # 12*4 ;Old handlers to restore wsp__regBuff # 16*4 ;Register save buffer thr__wSize EQU {VAR}-thr__wStart thrFlag__inited EQU (1<<0) ;Are we initialised yet? thrFlag__inErr EQU (1<<1) ;We are handling an error AREA |Sapphire$$LibData|,CODE,READONLY DCD thr__wSize DCD thr__wSpace DCD 256 DCD thread_init ;----- That's all, folks ---------------------------------------------------- END