\ The Q4 multitasker. \ ---------------------------------------------------------------- \ \ This is the Q4 round-robin multitasker. \ \ This is about a simple as a tasker can get, yet it is easily extensible \ to handle fancier needs. \ \ Multitasker tasks are known by a Task Control Block, or TCB. The TCB \ holds all the necessary context to restore the proper running state of \ a task. \ \ On the threaded architecture of the QS22032 CPU, the return stack is \ the only stack operated solely by the hardware, so we use it as the \ starting point for context saving. The parameter stack also needs to \ be saved and restored. The return stack may be saved by having a \ memory region set aside for its context, and remembering the return \ stack pointer, or RP register. \ \ The parameter stack likewise needs a memory region, but since the \ return stack grows in the direction of numerically increasing \ addresses, and the parameter stack grows in the direction of \ numerically decreasing addresses, the 2 stacks can share a common \ memory region. The parameter stack may be saved by saving the top of \ stack register, or T register, and the parameter stack pointer, or SP \ register. \ \ Interrupts may only be serviced at the end of a code word, so only the \ threaded program counter, or LP register, needs to be saved to keep \ track of the next instruction. \ \ This tasker runs all tasks at the same priority in round robin order. \ The tasks are linked together in a scheduling loop, which is a \ circularly linked list. The dispatcher maintains a current_task \ pointer that points to the TCB of the currently executing task. When \ a task relinquished control of the cpu, the dispatcher advances the \ current_task pointer by using the loop_link pointer in the TCB to \ determine the next task to dispatch. \ \ \ Because task need to wait on serially reusable resources, a resource \ control block, or RCB is defined to control this waiting. The RCB \ maintinas a pointer to the current owner of the resource, and a set of \ head and tail pointers to maintain a queue of tasks waiting for that \ resource. \ \ If the TCB being considered for dispatch is currently waiting for some \ serially reusable resource, its waiting_for_link will be non-zero. If \ the waiting_for_link is non-zero, then the dispatcher passes over that \ task and moves on to the next one. If the waiting_for_link_code is \ zero, then that task is not waiting and is eligible to run, so the \ dispatcher wakes up its context. \ \ ---------------- The RCB looks like this. \ \ \ <- - - - - resource_owner \ - - - resource_queue_head \ | resource_queue_tail - - - \ | | \ | | \ V V \ TCB - - -> TCB - - - >TCB - - ->TCB - -* \ \ ---------------- Structure of an RCB. s{ cell :: resource_owner \ pointer to TCB that currently owns this resource cell :: resource_queue_head \ pointer to head of queue of TCBs waiting for this resource cell :: resource_queue_tail \ pointer to tail of queue of TCBs waiting for this resource }s def RCB_size \ the size of as RCB f: resource ( -- ) \ ( -- ^RCB )\ \ make an RCB and give it a name inits \ uses initialized ram area qhere 00 , ( resource_owner ) 00 , ( resource_queue_head ) 00 , ( resource_queue_tail ) rom constant ;f \ ---------------- The TCB looks like this. \ \ \ - - - - -> loop_link - - - - - - - -> \ <- - - - - waiting_for_link \ - - - - -> wait_queue_link - - - - -> \ - - - - R0_save \ | S0_save - - - - - - - - - \ | RP_save - - - - - - - | \ - - -> bottom-of-Rstack | | \ : | | \ etc. | | \ : | | \ V | | \ LP_save | | \ - - - - SP_save <- - - - - - | \ | / / / / | \ | \ \ \ \ | \ | / / / / | \ - - -> top-of-Pstack | \ T_save | \ A | \ : | \ etc. | \ : | \ bottom-of Pstack <- - - - \ \ \ ---------------- Structure of a TCB. s{ cell :: loop_link \ pointer to next lower priority TCB cell :: waiting_for_link \ pointer to RCB the TCB is waiting on cell :: wait_queue_link \ pointer to next TCB after this in line for the RCB cell :: R0_save \ pointer to bottom of Rstack cell :: S0_save \ pointer to bottom of Pstack cell :: RP_save \ saved value of RP register }s def TCB_hdr \ this is a TCB header \ ---------------- The pool of TCBs. f[ d' 7 constant num_TCBs \ how many TCBs there are d' 32 cells constant TCB_stack_size \ how big a task's stack buffer is TCB_stack_size TCB_hdr + constant TCB_size \ how big each TCB is TCB_size num_TCBs * constant TCB_pool_size \ how big the TCB pool is ]f cr cr .( NOTE: rr-tasker is configured for d' ) f[ base @ decimal num_TCBs . base ! ]f .( tasks. ) cr .( Each task has a stack size of d' ) f[ base @ decimal TCB_stack_size 01 cells / . base ! ]f .( cells. ) cr variable TCB_pool \ list head for pool of free TCBs ram qhere TCB_pool_size allot \ where the pool is at rom constant TCB_pool_area \ pointer to where the TCBs live f[ \ pass tasker parameters back to simulator for tracing TCB_pool_area var ! TCB_size ! num_TCBs ! ]f : INIT_TCB_pool ( -- ) \ initialize the TCB pool ( at power-up ) nil TCB_pool ! \ start with an empty pool TCB_pool_area \ point to first TCB in pool num_TCBs literal 0 \ do this for each TCB in the pool do nil over waiting_for_link ! \ clear links nil over wait_queue_link ! dup TCB_hdr literal + cell- over R0_save ! \ initialize ptr to base of Rstack dup TCB_size literal + over S0_save ! \ initialize ptr to base of Pstack TCB_pool @ over ! dup TCB_pool ! \ put this TCB into the pool TCB_size literal + \ point to next TCB in the pool loop drop ; : allocate_TCB ( -- ^TCB ) \ get a TCB from the pool \ \ Since this routine is frequently called from interrupt handlers, \ it must of necessity be re-entrant. \ disable \ protect link manipulation! TCB_pool @ dup @ TCB_pool ! \ remove an item from the chain nil over ! \ clear links nil over waiting_for_link ! nil over wait_queue_link ! enable ; \ it is safe once again : free_TCB ( ^TCB -- ) \ put a TCB back into the pool \ \ Since this routine is frequently called from interrupt handlers, \ it must of necessity be re-entrant. \ disable \ protect link manipulation! nil over waiting_for_link ! \ clear links nil over wait_queue_link ! TCB_pool @ over ! TCB_pool ! \ add an item to the chain enable ; \ it is safe once again \ ---------------- Task context save and restore. code sleep ( ^TCB -- ^TCB ) \ put a task to sleep \ \ SLEEP is called from PAUSE, which is called from the application. \ Since SLEEP takes a pointer to the TCB, this is in the T register, \ and therefore the application's T register is already pushed on \ the Pstack. Since the application calls PAUSE, the application's \ LP register is already pushed onto the Rstack. All that is left \ for SLEEP to do is to save the SP register on the Rstack, and save \ the RP register in the TCB header. \ [rp] ++ sp sto \ save the Pstack pointer t u mov \ point to tcb f[ 5 cells ]f # u add \ point to the RP_SAVE slot in the TCB header [u] rp sto \ save the Rstack pointer \ \ Old running context is now saved, \ restore exec's stack pointers and \ return from whence we came. \ f[ spbase @ 01 cells - ]f sp ldi \ restore exec's stack pointers last f[ rpbase @ ]f rp ldi \ return from whence we were called \ but with both stacks empty except for ^TCB in T register end-code code wake ( ^TCB -- ) \ wake up a sleeping task \ \ WAKE takes a pointer to a TCB and restores the context of that task \ so that it is running. It is essentially the reversal of the SLEEP \ routine. \ f[ 5 cells ]f # t add \ point to the RP_SAVE slot in the TCB header [t] rp lod \ restore the Rstack pointer [rp] -- sp lod \ restore the Pstack pointer [rp] -- lp lod \ restore the threaded instruction pointer [sp] ++ t lod \ restore the top-of-stack register last nop \ attempt to solve pipeline problems related to interrupt timing? end-code \ ---------------- Application program interface words. variable current_task \ pointer to currently executing task f[ current_task var ! ]f \ pass back to simulator for tracing INIT: INIT_TCB_pool \ initialize the pool of free TCBs nil current_task ! \ there is no current task ;INIT : dispatch ( ^TCB -- ) begin \ Find the next runnable task. @ \ point to next candidate task dup waiting_for_link @ while \ is it waiting? repeat \ yes, find one that is not waiting dup current_task ! wake ; \ Wake up the next task to run. : pause ( -- ) \ the relinquish operation: a cooperative context switch current_task @ sleep \ Put the current task to sleep. dispatch noop ; \ run the next available task : find_pred ( ^TCB -- ^pred ) \ find the predecessor of a TCB in the dispatch loop >r r@ \ remember TCB pointer begin @ \ point to successor dup @ r@ = \ have we gone all the way around the loop? until \ no, keep looking until we do... r> drop ; \ yes, return pointer to predecessor of TCB : kill ( ^tcb -- ) \ remove a task from the system \ FIXME must handle case that task is not in dispatch loop! dup find_pred >r \ point to TCB's predecessor dup @ \ point to TCB's successor r@ ! \ remove the TCB from the dispatch loop free_TCB \ return it to the TCB pool r> dup current_task ! \ make TCB's predecessor the current task dispatch ; \ dispatch another task : hari-kari ( -- ) \ commit suicide -- kill yourself current_task @ kill ; code sp-move ( src dst len -- ) \ like cmove but faster, only valid for words \ \ this word is used to copy the user's stack to a new task's stack \ [rp] ++ lp sto \ save lp [sp] ++ v lod \ destination pointer [rp] ++ v sto \ save task's resulting stack pointer [sp] ++ u lod \ source pointer \ \ WARNING: This is a dirty trick we are getting ready to perform. \ We are going to adjust the SP for the return while we \ still have important values on the stack. This should not \ cause any trouble, since we are not interruptable inside \ a code word. Just a word to the wise... \ t sp add \ pop variable length parameter list off stack tz? f[ 04 cells ]f # cp add \ leading loop exit test to handle zero length move \ -move loop- [u] -- lp lod \ fetch a source word [v] ++ lp sto \ store it at the destination f[ 04 cells ]f # cp sub \ loop if more to go f[ 01 cells ]f # t sub \ decrement count ( delay slot ) \ -end loop- [rp] -- t lod \ return new task's sp to caller f[ 01 cells ]f # t add \ adjust [rp] -- lp lod \ restore lp \ last nop \ exit ( delay slot to allow for diddling with lp ) end-code : spawn ( args... nargs 'word -- ^TCB ) \ spawn a new task but don't enque it yet \ \ NOTE: Must *ALWAYS* have at least 1 argument! \ \ Since this routine is frequently called from interrupt handlers, \ handlers, it must, of necessity, be re-entrant. \ \ Step 1. Get a TCB and put the priority in it. \ allocate_TCB >r \ get a new TCB nil r@ loop_link ! \ link the new TCB to nowhere 0 r@ waiting_for_link ! \ not waiting for anything ( nargs 'word .... ^TCB ) ( .... ) \ \ Step 2. Initialize its Return stack with: \ (a) task exit routine address for when task "returns". \ (b) task starting routine address \ r@ R0_save @ >r \ get initial return stack pointer ['] hari-kari 2- r> cell+ >r r@ ! \ put task exit routine address on Rstack 2- r> cell+ >r r@ ! \ put task starting address on Rstack ( nargs .... ^TCB t.rp ) ( .... 'hari-kari 'word ) \ \ Step 3. Copy caller's arguments to task's parameter stack. \ sp@ over cells+ \ caller's args start address r> r@ swap >r S0_save @ cell- \ point at base of Pstack rot cells dup >r - r@ \ re-order cmove's args & save byte count sp-move \ copy caller's args to task's Pstack ( t.sp .... ^TCB t.rp len ) ( args... .... 'hari-kari 'word ) \ Step 4. Put task's initial stack pointer on top of his return stack \ r> drop r> cell+ >r r@ ! ( .... ^TCB t.rp ) ( args... .... 'hari-kari 'word t.sp ) \ \ Step 5. Put task's initial return stack pointer in RP_save field of TCB. \ r> r@ RP_save ! ( .... ^TCB ) ( args... .... 'hari-kari 'word t.sp ) \ \ Step 6. Return task's TCB pointer to caller. \ r> ( .... ) ( args... .... 'hari-kari 'word t.sp ) ; : insert ( ^TCB -- ) \ insert a task's TCB into the dispatch loop current_task @ >r \ point to somewhere in the dispatch loop r@ @ over ! \ set the new TCB's pointer r> ! ; \ point the old TCB to the new one \ ---------------------------------------------------------------- Suspension & resumption: FREEZE & THAW. : freeze ( ^tcb -- ) \ suspend the execution of a task -1 swap waiting_for_link ! ; : thaw ( ^tcb -- ) \ resume the execution of a task nil swap waiting_for_link ! ; \ ---------------------------------------------------------------- Resource management: GIVE & TAKE. : take ( ^RCB -- ) \ take ownership of a resource \ \ NOTE: This word is *ONLY* valid when called \ at task level! \ >r \ remember RCB pointer r@ @ ( resource_owner ) \ is it available? if \ no, r@ resource_queue_tail @ \ is queue empty? if \ no, current_task @ r@ resource_queue_tail @ ! \ make TCB at tail of queue point to this TCB current_task @ r@ resource_queue_tail ! \ make RCB's tail pointer point to this TCB too else \ yes, queue is empty current_task @ \ point to this task's TCB dup r@ resource_queue_head ! \ it is at both the head r@ resource_queue_tail ! \ and the tail of this RCB's queue then nil current_task @ wait_queue_link ! \ terminate queue r> current_task @ waiting_for_link ! \ show what RCB this TCB is waiting for else \ yes, it is available current_task @ r> ! \ take ownership of it then ; : give ( ^RCB -- ) \ give up ownership of a resource \ \ NOTE: This word is valid when called at \ either task or interrupt level. \ >r \ remember RCB pointer r@ resource_queue_tail @ \ is the RCB's queue empty? if \ no, deque from head of resource r@ resource_queue_head @ \ point to tcb at head of resource queue dup wait_queue_link @ \ point to its successor dup 0= \ is there one? if \ no, nil r@ resource_queue_tail ! \ null out tail pointer too then r@ resource_queue_head ! \ set new head pointer dup r> resource_owner ! \ resource owner = tmp thaw \ allow that task to run else \ yes, this RCB's queue is empty nil r> ( resource_owner ) ! \ say nobody owns this resource then ; : wait_for ( resource -- ) \ wait for a resource to finish doing its hardware thing current_task @ \ point to current task's tcb tuck waiting_for_link ! \ say this task is waiting for that resource pause ; \ go run another task \ ---------------------------------------------------------------- Mailboxes: GET and PUT. \ \ The mailbox facility implements a FIFO queue of messages with task \ synchronization. Each message is a 32-bit word, typically a pointer \ to a message buffer, but it could be anything. The messages \ themselves are kept in a circular buffer region that is of a size \ determined at compilation. A mailbox empty condition is detected so \ that a getting task can wait until message is put to an empty mailbox, \ but no full condition is detected since a full condition indicates \ that either the mailbox is undersized, or that the getter is just \ falling hopelessly behind the putter. f[ 8 constant MB_num_slots ]f \ number of slots in a mailbox's circular buffer ( *MUST* be a power of 2 !!! ) f[ MB_num_slots 01 - cells ]f constant MB_wrap_mask \ mask to wrap FIFO around at end of circular buffer s{ cell :: MB_waiting_task \ pointer to TCB waiting on empty mailbox cell :: MB_message_queue_head \ slot number of head of message queue cell :: MB_message_queue_tail \ slot number of tail of message queue f[ MB_num_slots cells ]f :: MB_circular_buffer \ start of circular message queue buffer area. }s def MB_size f: mailbox ( -- ) \ ( -- ^mb ) \ \ create a mailbox inits qhere \ remember where it is in initialized ram 00 , ( MB_waiting_task ) 00 , ( MB_message_queue_head ) 00 , ( MB_message_queue_tail ) MB_num_slots allot ( MB_circular_buffer ) rom constant ;f \ \ : put ( msg ^mb -- ) \ put a message into a mailbox \ \ NOTE: This word is usable at either the task or interrupt level. \ >r \ remember mailbox pointer disable r@ MB_waiting_task @ dup \ is anybody waiting on this mailbox? if \ yes, thaw \ allow him to run again nil r@ MB_waiting_task ! \ say nobody is waiting on this mailbox anymore else \ no, nobody waiting drop \ discard null waiting task's TCB pointer then r@ MB_message_queue_tail @ \ get offset to tail of message queue cell+ MB_wrap_mask and \ advance to next slot dup r@ MB_message_queue_tail ! \ save updated offset r> MB_circular_buffer + ! \ put message into FIFO enable ; : get ( ^mb -- msg ) \ get a message out of a mailbox \ \ NOTE: This word is usable *ONLY* at the task level! \ >r \ remember mailbox pointer begin disable \ start critical section r@ MB_message_queue_head @ \ get message queu head r@ MB_message_queue_tail @ \ and tail = \ are they the same? while \ no, that means queue is not empty current_task @ r@ MB_waiting_task ! \ yes, empty queue. say what task is waiting for this mailbox r@ current_task @ waiting_for_link ! \ say what that task is waiting for enable \ end critical section pause \ let some other task run repeat \ when we get here we should have something in the mailbox r@ MB_message_queue_head @ \ get offset to head of message queue cell+ MB_wrap_mask and \ advance to next slop dup r@ MB_message_queue_head ! \ save updated offset r> MB_circular_buffer + @ \ get message from FIFO enable ; \ end critical section