chiark - git - mdw - ssr/blob - StraySrc/Libraries/Sapphire/s/thread

   1 ;
   2 ; thread.s
   3 ;
   4 ; Preemptive multitasking of idle threads (MDW)
   5 ;
   6 ; © 1994-1998 Straylight
   7 ;
   8
   9 ;----- Licensing note -------------------------------------------------------
  10 ;
  11 ; This file is part of Straylight's Sapphire library.
  12 ;
  13 ; Sapphire is free software; you can redistribute it and/or modify
  14 ; it under the terms of the GNU General Public License as published by
  15 ; the Free Software Foundation; either version 2, or (at your option)
  16 ; any later version.
  17 ;
  18 ; Sapphire is distributed in the hope that it will be useful,
  19 ; but WITHOUT ANY WARRANTY; without even the implied warranty of
  20 ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  21 ; GNU General Public License for more details.
  22 ;
  23 ; You should have received a copy of the GNU General Public License
  24 ; along with Sapphire.  If not, write to the Free Software Foundation,
  25 ; 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  26
  27 ;----- Standard header ------------------------------------------------------
  28
  29                 GET     libs:header
  30                 GET     libs:swis
  31
  32 ;----- External dependencies ------------------------------------------------
  33
  34                 GET     sapphire:alloc
  35                 GET     sapphire:suballoc
  36                 GET     sapphire:idle
  37                 GET     sapphire:msgs
  38                 GET     sapphire:sapphire
  39
  40 ;----- Main code ------------------------------------------------------------
  41
  42                 AREA    |Sapphire$$Code|,CODE,READONLY
  43
  44 thr__stkSize    EQU     1024
  45 thr__idleFreq   EQU     1
  46
  47 ; --- thread_create ---
  48 ;
  49 ; On entry:     R0 == size of stack to allocate, or 0 for a default
  50 ;               R1 == pointer to thread routine
  51 ;               R2 == workspace pointer to pass in R10
  52 ;               R3 == workspace pointer to pass in R12
  53 ;
  54 ; On exit:      R0 == thread handle for the thread
  55 ;               May return an error
  56 ;
  57 ; Use:          Creates a new thread running `in the background' (i.e. over
  58 ;               idle events).
  59 ;
  60 ;               The thread is passed control with the registers R10 and R12
  61 ;               set up from R1 and R2 passed to this routine and R13 pointing
  62 ;               to the top of a stack chunk.  R0 on entry contains the
  63 ;               thread's handle.  The thread is passed the scratchpad
  64 ;               address (in R11).  The values of other registers are
  65 ;               indeterminate and must not be relied upon.
  66 ;
  67 ;               The default stack size for a new thread is 1K, although this
  68 ;               may change in future.
  69 ;
  70 ;               The thread may exit by calling thread_destroy or by
  71 ;               returning in the normal way.
  72
  73                 EXPORT  thread_create
  74 thread_create   ROUT
  75
  76                 STMFD   R13!,{R1-R8,R12,R14}    ;Save some registers
  77                 WSPACE  thr__wSpace             ;Load my workspace pointer
  78
  79                 ; --- Try to allocate the stack ---
  80
  81                 CMP     R0,#0                   ;Does he want the default?
  82                 MOVEQ   R0,#thr__stkSize        ;Yes -- give it to him
  83                 CMP     R0,#256                 ;Make sure it's big enough
  84                 MOVLT   R0,#256                 ;If not, make it bigger
  85                 MOV     R5,R0                   ;Keep a copy of the size
  86                 BL      alloc                   ;Try to allocate it nicely
  87                 BLCS    alloc_error             ;It failed, so get an error
  88                 BVS     %99thread_create        ;And skip to the end
  89                 MOV     R6,R0                   ;Keep the stack block ptr
  90
  91                 ; --- Now allocate a thread block ---
  92
  93                 MOV     R0,#thr__size           ;The size of the block
  94                 BL      alloc                   ;Allocate the block for me
  95                 BLCS    alloc_error             ;It failed, so get an error
  96                 BVS     %98thread_create        ;If it failed, skip onward
  97
  98                 ; --- Set up the initial context ---
  99
 100                 ADD     R5,R6,R5                ;R5 is the inital R13 to give
 101                 MOV     R14,R1                  ;Get start address in R14
 102                 MOV     R1,R2                   ;Move `R10' down a register
 103                 MOV     R2,R11                  ;Give scratchpad in R11
 104                 ADR     R4,thread_destroy       ;Returning destroys thread
 105                 STMFD   R5!,{R1-R4,R14}         ;Save these on the stack
 106                 STMFD   R5!,{R0-R9}             ;Fill the rest with rubbish
 107
 108                 ; --- Fill in the block ---
 109
 110                 ADD     R14,R0,#thr__suspend    ;Don't fill in the links
 111                 STMIA   R14,{R1-R8}             ;Store information in block
 112                 MOV     R1,#0                   ;Thread is not suspended yet
 113                 MOV     R2,#0                   ;Thread not blocked on sem
 114                 MOV     R3,#0                   ;Start at standard priority
 115                 MOV     R4,#thr__idleFreq       ;Set standard timeslice
 116                 STMIA   R14!,{R1-R4}            ;Save these in the block
 117                 MOV     R4,#0                   ;Not in critical section
 118                 MOV     R7,#0                   ;No flags to speak of yet
 119                 MOV     R8,#0                   ;No error handler defined
 120                 STMIA   R14!,{R4-R8}            ;Fill the rest in too
 121                 BL      thr__insert             ;Insert it into the list
 122
 123                 ; --- Bump the active counter ---
 124
 125                 BL      thr__incCount           ;Bump the active counter
 126                 LDMFD   R13!,{R1-R8,R12,R14}    ;Restore all the registers
 127                 BICS    PC,R14,#V_flag          ;Return without V set
 128
 129                 ; --- We stumbled across a mishap ---
 130
 131 98thread_create MOV     R4,R0                   ;Keep the error pointer
 132                 MOV     R0,R6                   ;Point to the stack block
 133                 BL      free                    ;Deallocate it -- don't want
 134                 MOV     R0,R4                   ;Point to the error again
 135
 136 99thread_create ADD     R2,R0,#4                ;Point to the error text
 137                 ADR     R0,thr__noCreate        ;Point to the error block
 138                 BL      msgs_error              ;Set up the error nicely
 139                 LDMFD   R13!,{R1-R8,R12,R14}    ;Restore all the registers
 140                 ORRS    PC,R14,#V_flag          ;Return with V set
 141
 142 thr__noCreate   DCD     1
 143                 DCB     "thrNOCRT",0
 144
 145                 LTORG
 146
 147 ; --- thread_setPriority ---
 148 ;
 149 ; On entry:     R0 == thread handle
 150 ;               R1 == new priority to set
 151 ;
 152 ; On exit:      --
 153 ;
 154 ; Use:          Changes the priority of a thread.  The priority if a thread
 155 ;               is a signed integer.  The highest priority thread is the one
 156 ;               which runs.  If more than one thread has maximum priority,
 157 ;               they are run in a cyclical order.
 158
 159                 EXPORT  thread_setPriority
 160 thread_setPriority ROUT
 161
 162                 STMFD   R13!,{R2,R3,R12,R14}    ;Save some registers here
 163                 WSPACE  thr__wSpace             ;Load my workspace pointer
 164                 LDR     R14,[R0,#thr__priority] ;Get the current priority
 165                 CMP     R1,R14                  ;Are we changing anything?
 166                 LDMEQFD R13!,{R2,R3,R12,PC}^    ;No -- return right now
 167
 168                 ; --- Unlink the thread ---
 169
 170                 LDMIA   R0,{R2,R3}              ;Get the next and previous
 171                 CMP     R2,#0                   ;Is there a next pointer?
 172                 STRNE   R3,[R2,#thr__prev]      ;Yes -- fill in its prev
 173                 CMP     R3,#0                   ;Is there a prev pointer?
 174                 STRNE   R2,[R3,#thr__next]      ;Yes -- fill in its next
 175                 STREQ   R2,wsp__threads         ;No -- make next first thread
 176
 177                 ; --- Change priority and insert the thread again ---
 178
 179                 STR     R1,[R0,#thr__priority]  ;Save the new priority
 180                 BL      thr__insert             ;Insert in the right place
 181                 LDMFD   R13!,{R2,R3,R12,PC}^    ;Return to caller
 182
 183                 LTORG
 184
 185 ; --- thread_setTimeSlice ---
 186 ;
 187 ; On entry:     R0 == thread handle
 188 ;               R1 == new timeslice size, in centiseconds
 189 ;
 190 ; On exit:      --
 191 ;
 192 ; Use:          Changes a thread's timeslice size.  Specify 0 to indicate
 193 ;               that thread shouldn't be pre-empted.
 194
 195                 EXPORT  thread_setTimeSlice
 196 thread_setTimeSlice ROUT
 197
 198                 STR     R1,[R0,#thr__timeSlice] ;Save the new timeslice
 199                 MOVS    PC,R14                  ;Return to caller
 200
 201                 LTORG
 202
 203
 204 ; --- thread_destroy ---
 205 ;
 206 ; On entry:     R0 == thread handle to destroy, if not executing a thread
 207 ;
 208 ; On exit:      --
 209 ;
 210 ; Use:          Destroys either the current thread or a thread with the
 211 ;               the given handle if no thread is executing currently.  You
 212 ;               can't destroy an arbitrary thread while running in one.
 213 ;
 214 ;               If a thread is waiting for a semaphore, it is removed from
 215 ;               the waiting list.
 216
 217                 EXPORT  thread_destroy
 218 thread_destroy  ROUT
 219
 220                 STMFD   R13!,{R0-R5,R12,R14}    ;Save some registers
 221                 WSPACE  thr__wSpace             ;Find my workspace
 222                 MOV     R5,R0                   ;Keep the thread pointer
 223
 224                 ; --- Find out which thread to destroy ---
 225
 226                 LDR     R4,wsp__current         ;Get the current thread
 227                 CMP     R4,#0                   ;Is there one running?
 228                 MOVNE   R5,R4                   ;Yes -- destroy it then
 229
 230                 ; --- If the thread was active, decrement active count ---
 231
 232                 LDR     R14,[R5,#thr__suspend]  ;Get the suspension counter
 233                 CMP     R14,#0                  ;Is it currently active?
 234                 BLEQ    thr__decCount           ;Yes -- decrement actives
 235
 236                 ; --- Remove thread from semaphore waiting lists ---
 237
 238                 LDR     R3,[R5,#thr__semaphore] ;Get the blocking semaphore
 239                 CMP     R3,#0                   ;Is there one?
 240                 BEQ     %10thread_destroy       ;No -- skip this part
 241
 242                 MOV     R2,#0                   ;Previous item in the list
 243                 LDR     R0,[R3,#sem__blocked]   ;Get the blocked list
 244 00              CMP     R0,#0                   ;Have we reached the end?
 245                 BEQ     %10thread_destroy       ;Yes -- skip onwards
 246                 LDR     R14,[R0,#sml__thread]   ;Get the waiting thread hnd
 247                 CMP     R14,R5                  ;Is it this thread?
 248                 MOVNE   R2,R0                   ;No -- update previous ptr
 249                 LDRNE   R0,[R0,#sml__next]      ;Find the next link block
 250                 BNE     %00thread_destroy       ;And move to next block
 251
 252                 ; --- Delete this thread waiting block ---
 253
 254                 LDR     R14,[R0,#sml__next]     ;Find the next link block
 255                 CMP     R2,#0                   ;Is there a previous block?
 256                 STRNE   R14,[R2,#sml__next]     ;Yes -- store it as next
 257                 STREQ   R14,[R3,#sem__blocked]  ;No -- next is new first item
 258                 CMP     R14,#0                  ;Is there a next block?
 259                 STREQ   R2,[R3,#sem__blockEnd]  ;No -- previous one is last
 260
 261                 BL      free                    ;Destroy the link block
 262
 263                 ; --- Delink the thread then ---
 264
 265 10              LDMIA   R5,{R1,R2}              ;R1 == next, R2 == prev
 266                 CMP     R1,#0                   ;Is there a next?
 267                 STRNE   R2,[R1,#thr__prev]      ;Yes -- store prev away
 268                 CMP     R2,#0                   ;Is there a prev?
 269                 STRNE   R1,[R2,#thr__next]      ;Yes -- store next pointer
 270                 STREQ   R1,wsp__threads         ;No -- store as list head
 271
 272                 ; --- Destroy the thread's memory ---
 273
 274                 LDR     R0,[R5,#thr__stack]     ;Find the thread's stack
 275                 BL      free                    ;Get rid of it -- it's no use
 276                 MOV     R0,R5                   ;Get the pointer back again
 277                 MOV     R1,#thr__size           ;The size of the block
 278                 BL      sub_free                ;Destroy the thread block
 279
 280                 ; --- Now we must return, but where to? ---
 281                 ;
 282                 ; If the caller is not the thread itself, this is easy.
 283                 ; If it *was* the thread, we've just killed its stack, so
 284                 ; we can't return to it.  Instead, we do the same job as
 285                 ; thread_yield.
 286
 287                 CMP     R4,#0                   ;Was there a current thread?
 288                 LDMEQFD R13!,{R0-R5,R12,PC}^    ;No -- return normally
 289
 290                 MOV     R0,#0                   ;We're stopping current thrd
 291                 STR     R0,wsp__current         ;So clear current thread
 292
 293                 BL      thr__end                ;Kill off any handlers
 294                 LDR     R13,wsp__stackPtr       ;Get the system stack pointer
 295                 LDMFD   R13!,{R0-R12,PC}^       ;Return to thread dispatcher
 296
 297                 LTORG
 298
 299 ; --- thr__decCount ---
 300 ;
 301 ; On entry:     --
 302 ;
 303 ; On exit:      --
 304 ;
 305 ; Use:          Decrements the active threads counter, and disables the idle
 306 ;               claimer if there aren't any left.
 307
 308 thr__decCount   ROUT
 309
 310                 STMFD   R13!,{R0-R3,R14}        ;Save registers
 311                 LDR     R14,wsp__active         ;Get the active counter
 312                 SUBS    R14,R14,#1              ;Decrement the counter
 313                 STR     R14,wsp__active         ;Store it back again
 314                 LDMGTFD R13!,{R0-R3,PC}^        ;If some still active, return
 315
 316                 ; --- Remove the idle claiming routine ---
 317
 318                 MOV     R0,#thr__idleFreq       ;How often I should be called
 319                 ADR     R1,thr__idles           ;Point to the idle handler
 320                 MOV     R2,#0                   ;Don't care about R10
 321                 MOV     R3,R12                  ;Pass workspace in R12
 322                 BL      idle_removeHandler      ;Stop it from being called
 323
 324                 LDMFD   R13!,{R0-R3,PC}^        ;Return to caller
 325
 326                 LTORG
 327
 328 ; --- thr__incCount ---
 329 ;
 330 ; On entry:     --
 331 ;
 332 ; On exit:      --
 333 ;
 334 ; Use:          Increments the active threads counter, and adds in the idle
 335 ;               claimer if it was previously disabled.
 336
 337 thr__incCount   ROUT
 338
 339                 STMFD   R13!,{R0-R3,R14}        ;Save registers
 340                 LDR     R14,wsp__active         ;Get the active counter
 341                 ADD     R14,R14,#1              ;Increment the counter
 342                 STR     R14,wsp__active         ;Store it back again
 343                 CMP     R14,#1                  ;Was it previously off?
 344                 LDMGTFD R13!,{R0-R3,PC}^        ;No -- return right now
 345
 346                 ; --- Add in the idle claiming routine ---
 347
 348                 MOV     R0,#thr__idleFreq       ;How often I should be called
 349                 ADR     R1,thr__idles           ;Point to the idle handler
 350                 MOV     R2,#0                   ;Don't care about R10
 351                 MOV     R3,R12                  ;Pass workspace in R12
 352                 BL      idle_handler    ;Get it called nicely
 353                 LDMFD   R13!,{R0-R3,PC}^        ;Return to caller
 354
 355                 LTORG
 356
 357 ; --- thr__idles ---
 358 ;
 359 ; On entry:     --
 360 ;
 361 ; On exit:      --
 362 ;
 363 ; Use:          Handles idle events by passing control to each thread in
 364 ;               turn.  This is the main scheduler for the threads.
 365 ;
 366 ;               We use a simple but well-respected algorithm.  We find the
 367 ;               first active thread in the list, move it to the end of its
 368 ;               priority group and run it.
 369
 370 thr__idles      ROUT
 371
 372                 ; --- Save current context away ---
 373                 ;
 374                 ; We don't actually return from this routine -- actually
 375                 ; we restore from thread_destroy, thread_suspend or
 376                 ; thread_yield, or on the CallBack from a timer interrupt.
 377
 378                 STMFD   R13!,{R0-R12,R14}       ;Save context on the stack
 379                 STR     R13,wsp__stackPtr       ;Save the stack pointer
 380
 381                 ; --- Now we need to find an unsuspended thread ---
 382                 ;
 383                 ; There must be an active thread, because we don't get called
 384                 ; unless the count is non-0
 385
 386                 LDR     R10,wsp__threads        ;Find the first thread
 387 10thr__idles    CMP     R10,#0                  ;Is this the end?
 388                 LDMEQFD R13!,{R0-R12,PC}^       ;Yes -- nothing to do then
 389                 LDR     R0,[R10,#thr__suspend]  ;Get the thread's suspend
 390                 CMP     R0,#0                   ;Is this one active?
 391                 LDRNE   R10,[R10,#thr__next]    ;No -- get the next one out
 392                 BNE     %10thr__idles           ;And go round again
 393
 394                 ; --- We have an active thread ---
 395                 ;
 396                 ; We now unlink the thread and move it to the end of its
 397                 ; priority group in the list, so next time the next one in
 398                 ; the group gets a chance.
 399
 400                 LDR     R9,[R10,#thr__priority] ;Get the thread's priority
 401                 LDMIA   R10,{R0,R1}             ;Get next and prev pointers
 402                 CMP     R0,#0                   ;Is there a next pointer?
 403                 STRNE   R1,[R0,#thr__prev]      ;Yes -- fill in its prev
 404                 CMP     R1,#0                   ;Is there a prev pointer?
 405                 STRNE   R0,[R1,#thr__next]      ;Yes -- fill in its next
 406                 STREQ   R0,wsp__threads         ;No -- make it the new first
 407
 408                 ; --- Now search forwards for the end of the group ---
 409
 410 20thr__idles    CMP     R0,#0                   ;Is this the list end?
 411                 BEQ     %25thr__idles           ;Yes -- skip out of loop
 412                 LDR     R8,[R0,#thr__priority]  ;Get the next one's priority
 413                 CMP     R8,R9                   ;Do they match up nicely?
 414                 MOVGE   R1,R0                   ;This is the previous one
 415                 LDRGE   R0,[R0,#thr__next]      ;Get the next one out
 416                 BGE     %20thr__idles           ;And loop round again
 417
 418                 ; --- Insert our thread between R1 and R0 ---
 419
 420 25thr__idles    CMP     R1,#0                   ;Is the previous one OK?
 421                 STRNE   R10,[R1,#thr__next]     ;Yes -- fill in its next
 422                 STREQ   R10,wsp__threads        ;Otherwise make it first one
 423                 CMP     R0,#0                   ;Is there a next one?
 424                 STRNE   R10,[R0,#thr__prev]     ;Yes -- fill in its previous
 425                 STMIA   R10,{R0,R1}             ;Save next and prev back
 426
 427                 ; --- Now we can run the thread at last ---
 428
 429                 LDR     R0,[R10,#thr__timeSlice] ;Load the timeslice we want
 430                 BL      thr__start              ;Set up timer interrupt
 431                 STR     R10,wsp__current        ;Save the current thread
 432                 LDR     R13,[R10,#thr__stackPtr] ;Find its stack pointer
 433                 LDMFD   R13!,{R0-R12,R14,PC}^   ;Start it up again
 434
 435                 LTORG
 436
 437 ; --- thr__insert ---
 438 ;
 439 ; On entry:     R0 == pointer to thread block
 440 ;
 441 ; On exit:      --
 442 ;
 443 ; Use:          Inserts a thread into the thread list in the right place for
 444 ;               its priority.
 445
 446 thr__insert     ROUT
 447
 448                 STMFD   R13!,{R1-R4,R14}        ;Save some registers
 449
 450                 LDR     R4,[R0,#thr__priority]  ;Get the thread;s priority
 451                 LDR     R1,wsp__threads         ;Find the first item
 452                 MOV     R2,#0                   ;No previous list item yet
 453 10thr__insert   CMP     R1,#0                   ;Is this the end of the list?
 454                 BEQ     %20thr__insert          ;Yes -- skip forwards
 455                 LDR     R3,[R1,#thr__priority]  ;Get its priority ready
 456                 CMP     R3,R4                   ;How do they compare?
 457                 MOVGT   R2,R1                   ;Too high -- now previous
 458                 LDRGT   R1,[R1,#thr__next]      ;Find the next thread handle
 459                 BGT     %10thr__insert          ;And loop round for another
 460
 461                 ; --- Insert the thread between R2 and R1 ---
 462
 463 20thr__insert   CMP     R2,#0                   ;Is there a previous one?
 464                 STRNE   R0,[R2,#thr__next]      ;Yes -- fill in its next
 465                 STREQ   R0,wsp__threads         ;No -- this is the first one
 466                 CMP     R1,#0                   ;Is there a next one?
 467                 STRNE   R0,[R1,#thr__prev]      ;Yes -- fill in its prev
 468                 STMIA   R0,{R1,R2}              ;Store next and prev in block
 469                 LDMFD   R13!,{R1-R4,PC}^        ;Return to caller then
 470
 471                 LTORG
 472
 473 ; --- thread_suspend ---
 474 ;
 475 ; On entry:     R0 == thread handle, or 0 for the current thread
 476 ;
 477 ; On exit:      --
 478 ;
 479 ; Use:          Suspends a thread's execution.  If a thread is currently
 480 ;               running, that thread is suspended.  Otherwise, any thread
 481 ;               may be suspended.
 482 ;
 483 ;               If the thread is currently claiming semaphores, the
 484 ;               semaphores are not released, because we don't whether the
 485 ;               system is in a fit state for this.
 486 ;
 487 ;               Thread suspensions are counted.  i.e. if you suspend a thread
 488 ;               5 times, you have to resume it 5 times for it to become
 489 ;               active again.
 490
 491                 EXPORT  thread_suspend
 492 thread_suspend  ROUT
 493
 494                 STMFD   R13!,{R12,R14}          ;Save some registers away
 495                 WSPACE  thr__wSpace             ;Locate my workspace
 496                 LDR     R14,wsp__current        ;Get the current thread
 497                 CMP     R0,#0                   ;Is current one wanted?
 498                 CMPNE   R0,R14                  ;Or is it just coincidence?
 499                 BNE     %10thread_suspend       ;No -- deal with that case
 500
 501                 ; --- We want to suspend the current thread ---
 502                 ;
 503                 ; To suspend, we need to save the current thread context,
 504                 ; and resume from the system stack.  We know the thread must
 505                 ; be active currently, otherwise we wouldn't be executing
 506                 ; it!
 507
 508                 STMFD   R13!,{R0-R12}           ;Save entire context on stack
 509                 STR     R13,[R14,#thr__stackPtr] ;Save the thread's stack ptr
 510                 MOV     R0,#1                   ;Thread must be active
 511                 STR     R0,[R14,#thr__suspend]  ;So store 1 as suspend count
 512                 BL      thr__decCount           ;There's one less active now
 513
 514                 ; --- Now restore context to dispatcher ---
 515
 516                 BL      thr__end                ;Stop all the handlers
 517                 MOV     R0,#0                   ;There is no current thread
 518                 STR     R0,wsp__current         ;So clear current pointer
 519                 LDR     R13,wsp__stackPtr       ;Get the system stack pointer
 520                 LDMFD   R13!,{R0-R12,PC}^       ;Restore context again
 521
 522                 ; --- We're just meant to suspend any old thread ---
 523                 ;
 524                 ; We just need to bump its counter -- this is easy ---
 525
 526 10              LDR     R14,[R0,#thr__suspend]  ;Get current suspend count
 527                 ADD     R14,R14,#1              ;Bump it up one
 528                 STR     R14,[R0,#thr__suspend]  ;Store it back again
 529                 CMP     R14,#1                  ;Was it previously active?
 530                 BLEQ    thr__decCount           ;Yes -- decrement actives
 531                 LDMFD   R13!,{R12,PC}^          ;Return to caller happy
 532
 533                 LTORG
 534
 535 ; --- thread_resume ---
 536 ;
 537 ; On entry:     R0 == thread handle
 538 ;
 539 ; On exit:      --
 540 ;
 541 ; Use:          Allows a suspended thread to continue operations.  If you
 542 ;               resume a thread more times than it has been suspended,
 543 ;               any excess resumes are ignored.  You can't resume a thread
 544 ;               to stop it being blocked by a semaphore.
 545
 546                 EXPORT  thread_resume
 547 thread_resume   ROUT
 548
 549                 STMFD   R13!,{R10,R12,R14}      ;Save some registers
 550                 WSPACE  thr__wSpace             ;Find my workspace address
 551
 552                 ; --- Make sure the thread isn't running now ---
 553
 554                 LDR     R14,[R0,#thr__suspend]  ;Get the suspension counter
 555                 CMP     R14,#0                  ;Is it zero already?
 556                 LDMEQFD R13!,{R10,R12,PC}^      ;Yes -- return right now
 557
 558                 ; --- Decrement the counter ---
 559
 560                 LDR     R10,[R0,#thr__semaphore] ;Get the blocking semaphore
 561                 CMP     R10,#0                  ;Is there a blocking sem?
 562                 MOVEQ   R10,#0                  ;No -- minimum count is 0
 563                 MOVNE   R10,#1                  ;Yes -- minimum count is 1
 564                 SUB     R14,R14,#1              ;Decrement the counter
 565                 CMP     R14,R10                 ;Is it too low?
 566                 MOVLT   R14,R10                 ;Yes -- bring it up again
 567                 STR     R14,[R0,#thr__suspend]  ;Store the counter back again
 568
 569                 ; --- Bump the active count if thread now active ---
 570
 571                 CMP     R14,#0                  ;Is the thread active now?
 572                 BLEQ    thr__incCount           ;Yes -- increment the count
 573                 CMP     R14,#1                  ;Is the thread almost active?
 574                 LDMNEFD R13!,{R10,R12,PC}^      ;No -- return right now
 575
 576                 ; --- Check if the thread can start from a semaphore ---
 577
 578                 STMFD   R13!,{R0-R2}            ;Save some registers
 579                 MOV     R10,R0                  ;Look after the thread handle
 580                 LDR     R14,[R10,#thr__semaphore] ;Is it waiting for a sem?
 581                 CMP     R14,#0                  ;Is the pointer null?
 582                 BEQ     %20thread_resume        ;Yes -- skip to end
 583
 584                 LDR     R0,[R14,#sem__counter]  ;Is it signalled enough?
 585                 CMP     R0,#0                   ;If so, it isn't 0
 586                 BEQ     %20thread_resume        ;If not, skip to the end
 587
 588                 ; --- Go through the waiting list ---
 589                 ;
 590                 ; We want to find the entry in the list, but we also want
 591                 ; to avoid `jumping the queue' -- i.e. getting the semaphore
 592                 ; before an unsuspended thread.  With the algorithms used
 593                 ; in thread_signal, I don't think this can happen, but it's
 594                 ; as well to make sure.  We have to go through the list
 595                 ; anyway.
 596
 597                 LDR     R0,[R14,#sem__blocked]  ;Find the blocked list
 598                 MOV     R2,#0                   ;No previous block found
 599 10              CMP     R0,#0                   ;Is it the end of the list?
 600                 BEQ     %20thread_resume        ;Yes -- it's all over then
 601                 LDR     R1,[R0,#sml__thread]    ;Get the thread's handle
 602                 CMP     R1,R10                  ;Do they match up?
 603                 BEQ     %11thread_resume        ;Yes -- skip onwards
 604                 LDR     R1,[R1,#thr__suspend]   ;Get the suspended count
 605                 CMP     R1,#0                   ;Is this thread active?
 606                 BEQ     %20thread_resume        ;Yes -- skip to end
 607                 MOV     R2,R0                   ;Set up the previous pointer
 608                 LDR     R0,[R0,#sml__next]      ;Get the next block
 609                 B       %10thread_resume        ;And go back round the loop
 610
 611                 ; --- Remove it from the waiting list ---
 612
 613 11thread_resume LDR     R1,[R0,#sml__next]      ;Get the next list item
 614                 CMP     R2,#0                   ;Is there a previous one?
 615                 STRNE   R1,[R2,#sml__next]      ;Yes -- fill in its next ptr
 616                 STREQ   R1,[R14,#sem__blocked]  ;No -- it's the new list head
 617                 CMP     R1,#0                   ;Is there a next block?
 618                 STREQ   R2,[R14,#sem__blockEnd] ;No -- fill in the last ptr
 619
 620                 MOV     R2,R14                  ;Keep the semaphore pointer
 621                 MOV     R1,#sml__size           ;Size of the link blocks
 622                 BL      sub_free                ;Destroy the link block
 623
 624                 ; --- Now decrement the semaphore counter ---
 625
 626                 LDR     R0,[R2,#sem__counter]   ;Get the current counter
 627                 SUB     R0,R0,#1                ;Decrement it nicely
 628                 STR     R0,[R2,#sem__counter]   ;Store it back again
 629                 MOV     R0,#0                   ;Thread not suspended at all
 630                 STR     R0,[R10,#thr__suspend]  ;Store 0 as suspend count
 631                 BL      thr__incCount           ;Increment active threads
 632
 633 20thread_resume LDMFD   R13!,{R0-R2,R10,R12,PC}^ ;Return to caller
 634
 635                 LTORG
 636
 637 ; --- thread_yield ---
 638 ;
 639 ; On entry:     --
 640 ;
 641 ; On exit:      --
 642 ;
 643 ; Use:          Pauses the thread for a while.  You only need to use this
 644 ;               call if you have stopped the current thread from being
 645 ;               timesliced.
 646
 647                 EXPORT  thread_yield
 648 thread_yield    ROUT
 649
 650                 STMFD   R13!,{R0-R14}           ;Save the old context
 651                 WSPACE  thr__wSpace             ;Find my workspace pointer
 652                 LDR     R0,wsp__current         ;Get the current thread ptr
 653                 BL      thr__end                ;Kill off the handlers
 654                 STR     R13,[R0,#thr__stackPtr] ;Save the thread's stack ptr
 655                 MOV     R0,#0                   ;Current thread is stopped
 656                 STR     R0,wsp__current         ;So clear the pointer
 657                 LDR     R13,wsp__stackPtr       ;Get the system stack pointer
 658                 LDMFD   R13!,{R0-R12,PC}^       ;Restore main system context
 659
 660                 LTORG
 661
 662 ; --- thread_createSem ---
 663 ;
 664 ; On entry:     R0 == initial value for semaphore (0 for counter, 1 for
 665 ;                     mutex)
 666 ;
 667 ; On exit:      R0 == semaphore handle and V clear if all went well
 668 ;               R0 == pointer to error and V set if something went wrong
 669 ;
 670 ; Use:          Creates a semaphore with the given initial counter value.
 671 ;
 672 ;               The semaphore can be used to provide serialised access to
 673 ;               a resource by initialising its value to 1 and performing the
 674 ;               following:
 675 ;
 676 ;               thread_wait(mySemaphore)
 677 ;               //
 678 ;               // Do things with the resource
 679 ;               //
 680 ;               thread_signal(mySemaphore)
 681 ;
 682 ;               Or you can inform a thread that it has items in its input
 683 ;               queue by having the following in the thread code:
 684 ;
 685 ;               while true
 686 ;                 thread_wait(theSemaphore)
 687 ;                 getFromQueue(myQueue,item)
 688 ;                 process(item)
 689 ;               endWhile
 690 ;
 691 ;               and when inserting queue items:
 692 ;
 693 ;               addToQueue(item)
 694 ;               thread_signal(theSemaphore)
 695 ;
 696 ;               It is distinctly possible that input queue management will
 697 ;               be introduced in a separate Sapphire module.
 698
 699                 EXPORT  thread_createSem
 700 thread_createSem ROUT
 701
 702                 STMFD   R13!,{R0-R3,R14}        ;Save some registers
 703                 MOV     R0,#sem__size           ;The size of a semaphore
 704                 BL      sub_alloc               ;Try to create the semaphore
 705                 BVS     %99thread_createSem     ;If it failed, tidy up
 706
 707                 LDR     R1,[R13],#4             ;Get the initial sem value
 708                 MOV     R2,#0                   ;No threads waiting either
 709                 MOV     R3,#0                   ;So no last link block
 710                 STMIA   R0,{R1-R3}              ;Save in semaphore block
 711                 LDMFD   R13!,{R1-R3,PC}         ;Return to caller
 712
 713                 ; --- Error occurred ---
 714
 715 99              ADD     R2,R0,#4                ;Point to the error text
 716                 ADR     R0,thr__noSemCrt        ;Point to error block
 717                 BL      msgs_error              ;Translate the error message
 718                 ADD     R13,R13,#4              ;Don't restore R0 from stack
 719                 LDMFD   R13!,{R1-R3,PC}         ;Return with V set on exit
 720
 721 thr__noSemCrt   DCD     1
 722                 DCB     "thrNOSEMCRT",0
 723
 724                 LTORG
 725
 726 ; --- thread_destroySem ---
 727 ;
 728 ; On entry:     R0 == semaphore handle
 729 ;
 730 ; On exit:      --
 731 ;
 732 ; Use:          Destroys a semaphore when it's no use any more.  If threads
 733 ;               are waiting for it, an error is generated.
 734
 735                 EXPORT  thread_destroySem
 736 thread_destroySem ROUT
 737
 738                 STMFD   R13!,{R1,R14}           ;Save some registers away
 739                 LDR     R14,[R0,#sem__blocked]  ;Get the waiting list head
 740                 CMPEQ   R14,#0                  ;Is there a waiting list?
 741                 BNE     %90thread_destroySem    ;If so, complain
 742
 743                 MOV     R1,#sem__size           ;The size of a semaphore
 744                 BL      sub_free                ;Free up the block
 745                 LDMFD   R13!,{R1,PC}^           ;Return to caller
 746
 747                 ; --- The semaphore is still in use ---
 748
 749 90              ADR     R0,thr__semInUse        ;Point to error message
 750                 BL      msgs_error              ;Translate the message
 751                 SWI     OS_GenerateError        ;It's a serious problem
 752
 753 thr__semInUse   DCD     1
 754                 DCB     "thrSEMINUSE",0
 755
 756                 LTORG
 757
 758 ; --- thread_threaded ---
 759 ;
 760 ; On entry:     --
 761 ;
 762 ; On exit:      CS if currently running a thread, CC otherwise
 763 ;
 764 ; Use:          Informs the caller whether a thread is currently executing.
 765
 766                 EXPORT  thread_threaded
 767 thread_threaded ROUT
 768
 769                 STMFD   R13!,{R12}              ;Save a register
 770                 WSPACE  thr__wSpace             ;Load my workspace address
 771                 LDR     R12,wsp__current        ;Load current thread handle
 772                 CMP     R12,#0                  ;Is there a thread running?
 773                 LDMFD   R13!,{R12}              ;Restore the register
 774                 ORRNES  PC,R14,#C_flag          ;Return CS if thread running
 775                 BICEQS  PC,R14,#C_flag          ;Otherwise return CC
 776
 777                 LTORG
 778
 779 ; --- thread_wait ---
 780 ;
 781 ; On entry:     R0 == semaphore handle
 782 ;
 783 ; On exit:      If successful, R0 preserved and V clear.
 784 ;               If failed, R0 == pointer to error block and V set
 785 ;
 786 ; Use:          Waits on a sempahore.  The algorithm actually is as follows:
 787 ;
 788 ;               if semaphore.counter == 0 then
 789 ;                 addToWaitingList(semaphore,currentThread)
 790 ;                 suspend(currentThread)
 791 ;               else
 792 ;                 semaphore.counter -= 1
 793 ;               endIf
 794 ;
 795 ;               See thread_createSem for suggestions on how to make use of
 796 ;               semaphores.
 797
 798                 EXPORT  thread_wait
 799 thread_wait     ROUT
 800
 801                 BIC     R14,R14,#V_flag         ;Assume that all is well
 802                 STMFD   R13!,{R10,R12-R14}      ;Save some registers
 803                 WSPACE  thr__wSpace             ;Locate my workspace
 804                 LDR     R10,wsp__current        ;Find the current thread
 805                 CMP     R10,#0                  ;Is it nonexistant?
 806                 BEQ     %99thread_wait          ;Yes -- this is an error
 807
 808                 ; --- Do the messing about with the counter ---
 809
 810                 LDR     R14,[R0,#sem__counter]  ;Get the semaphore counter
 811                 SUBS    R14,R14,#1              ;Decrement the counter
 812                 STRGE   R14,[R0,#sem__counter]  ;If it was nonzero, store
 813                 LDMEQFD R13!,{R10,R12,R14,PC}^  ;And return to caller
 814
 815                 ; --- Add the thread to the waiting list ---
 816
 817                 STMFD   R13!,{R0-R9}            ;Save rest of the context
 818                 MOV     R9,R0                   ;Keep the semaphore handle
 819                 MOV     R0,#sml__size           ;Size of a link block
 820                 BL      sub_alloc               ;Allocate a link block
 821                 BVS     %90thread_wait          ;If it failed, return error
 822
 823                 MOV     R1,#0                   ;No next waiting thread
 824                 STMIA   R0,{R1,R10}             ;Save information in block
 825                 LDR     R1,[R9,#sem__blockEnd]  ;Get the last waiting block
 826                 CMP     R1,#0                   ;Is there one at all?
 827                 STRNE   R0,[R1,#sml__next]      ;Yes -- store in its next
 828                 STREQ   R0,[R9,#sem__blocked]   ;No -- it's the first one
 829                 STR     R0,[R9,#sem__blockEnd]  ;It's certainly the last one
 830
 831                 ; --- Now suspend the thread for a while ---
 832
 833                 MOV     R1,#1                   ;Thread is currently active
 834                 STR     R1,[R10,#thr__suspend]  ;Not any more it isn't
 835                 STR     R9,[R10,#thr__semaphore] ;Remember the blocking sem
 836
 837                 ; --- Now switch back to main context ---
 838
 839                 BL      thr__end                ;Remove all the handlers
 840                 STR     R13,[R10,#thr__stackPtr] ;Save the current stackptr
 841                 LDR     R13,wsp__stackPtr       ;Find the system stack ptr
 842                 LDMFD   R13!,{R0-R12,PC}^       ;And return to normality
 843
 844                 ; --- An error occurred while suspending ---
 845
 846 90thread_wait   ADD     R2,R0,#4                ;Point to the error text
 847                 ADR     R0,thr__noWaitSem       ;Point to the error message
 848                 BL      msgs_error              ;Translate the error
 849                 ADD     R13,R13,#4              ;Don't restore R0 on exit
 850                 LDMFD   R13!,{R1-R12,R14}       ;Restore all the registers
 851                 ORRS    PC,R14,#V_flag          ;Return the error back
 852
 853 thr__noWaitSem  DCD     1
 854                 DCB     "thrNOWAITSEM",0
 855
 856                 ; --- The caller isn't a thread ---
 857
 858 99thread_wait   ADR     R0,thr__notAThrd        ;Point to the error
 859                 BL      msgs_error              ;Translate the message
 860                 SWI     OS_GenerateError        ;And generate the error
 861
 862 thr__notAThrd   DCD     1
 863                 DCB     "thrNOTATHRD",0
 864
 865                 LTORG
 866
 867 ; --- thread_signal ---
 868 ;
 869 ; On entry:     R0 == semaphore handle
 870 ;
 871 ; On exit:      --
 872 ;
 873 ; Use:          Increments a semaphore's counter if no threads are waiting
 874 ;               for it, or releases a thread waiting for the semaphore.
 875 ;
 876 ;               The actual algorithm is shown below:
 877 ;
 878 ;               if semaphore.waitingList != 0 then
 879 ;                 thread = removeFromWaitingList(semaphore)
 880 ;                 unSuspend(thread)
 881 ;               else
 882 ;                 semaphore.counter += 1;
 883 ;               endif
 884 ;
 885 ;               See thread_createSem for suggestions on how to make use of
 886 ;               semaphores.
 887
 888                 EXPORT  thread_signal
 889 thread_signal   ROUT
 890
 891                 STMFD   R13!,{R0-R3,R10,R12,R14} ;Save a load of registers
 892                 WSPACE  thr__wSpace             ;Locate my workspace
 893
 894                 ; --- Find a thread to restore control to ---
 895
 896                 MOV     R3,R0                   ;Look after the sem handle
 897                 MOV     R2,#0                   ;No previous block yet
 898                 LDR     R0,[R3,#sem__blocked]   ;Find blocked list head
 899
 900 10thread_signal CMP     R0,#0                   ;Is this the end?
 901                 BEQ     %30thread_signal        ;Yes -- increment counter
 902                 LDR     R10,[R0,#sml__thread]   ;Get the thread handle out
 903                 LDR     R14,[R10,#thr__suspend] ;Find its suspended count
 904                 CMP     R14,#1                  ;Is it only blocked by sem?
 905                 MOVGT   R2,R0                   ;No -- this is now prev
 906                 LDRGT   R0,[R0,#sml__next]      ;Find the next one
 907                 BGT     %10thread_signal        ;And check that one out
 908
 909                 ; --- Found a suitable thread ---
 910
 911                 LDR     R14,[R0,#sml__next]     ;Get the next pointer out
 912                 CMP     R2,#0                   ;Is there a previous block?
 913                 STRNE   R14,[R2,#sml__next]     ;Yes -- fix its next ptr
 914                 STREQ   R14,[R3,#sem__blocked]  ;No -- this is now first one
 915                 CMP     R14,#0                  ;Is there a next block?
 916                 STREQ   R2,[R3,#sem__blockEnd]  ;Yes -- previous is now last
 917
 918                 MOV     R1,#sml__size           ;The size of a link block
 919                 BL      sub_free                ;Don't need it any more
 920
 921                 ; --- Let the new thread go ---
 922
 923                 MOV     R0,#0                   ;Thread is no longer blocked
 924                 STR     R0,[R10,#thr__suspend]  ;Unblock the thread nicely
 925                 STR     R0,[R10,#thr__semaphore] ;No blocking semaphore now
 926                 LDMFD   R13!,{R0-R3,R10,R12,PC}^ ;Return to caller happily
 927
 928                 ; --- No threads found -- increment the counter ---
 929
 930 30thread_signal LDR     R14,[R3,#sem__counter]  ;Get the current counter
 931                 ADD     R14,R14,#1              ;Increment it a little bit
 932                 STR     R14,[R3,#sem__counter]  ;Store new counter back
 933                 LDMFD   R13!,{R0-R3,R10,R12,PC}^ ;Return to caller happily
 934
 935                 LTORG
 936
 937 ; --- thread_init ---
 938 ;
 939 ; On entry:     --
 940 ;
 941 ; On exit:      --
 942 ;
 943 ; Use:          Initialises the thread system for use.
 944
 945                 EXPORT  thread_init
 946 thread_init     ROUT
 947
 948                 STMFD   R13!,{R12,R14}          ;Save some registers
 949
 950                 ; --- Make sure we're not initialised ---
 951
 952                 WSPACE  thr__wSpace             ;Find my workspace pointer
 953                 LDR     R14,wsp__flags          ;Load my flags word
 954                 TST     R14,#thrFlag__inited    ;Am I initialised yet?
 955                 LDMNEFD R13!,{R12,PC}^          ;Yes -- return right now
 956                 ORR     R14,R14,#thrFlag__inited ;Set the flag
 957                 STR     R14,wsp__flags          ;Save the flags word back
 958
 959                 ; --- Initialise the rest of the workspace ---
 960
 961                 STMFD   R13!,{R0-R4}            ;Save some more registers
 962                 MOV     R0,#0                   ;No threads registered yet
 963                 MOV     R1,#0                   ;No active threads, then
 964                 MOV     R2,#0                   ;No thread running now
 965                 MOV     R3,#0                   ;No system stack pointer
 966                 STMIB   R12,{R0-R3,R11}         ;Save them in the workspace
 967
 968                 ; --- Start up other things we need ---
 969
 970                 BL      alloc_init              ;We need redirectble alloc
 971                 BL      sub_init                ;And allocating small blocks
 972
 973                 LDMFD   R13!,{R0-R4,R12,PC}^    ;Return to caller
 974
 975                 LTORG
 976
 977 thr__wSpace     DCD     0
 978
 979 ;----- Handling pre-emption of threads --------------------------------------
 980
 981 ; --- thr__start ---
 982 ;
 983 ; On entry:     R0 == timeslice size in centiseconds
 984 ;
 985 ; On exit:      --
 986 ;
 987 ; Use:          Sets up handlers and a timer interrupt for starting a
 988 ;               thread.
 989
 990 thr__start      ROUT
 991
 992                 STMFD   R13!,{R0-R4,R14}        ;Save some registers away
 993                 ADR     R4,wsp__handlers        ;Point to handlers save area
 994
 995                 ; --- Set up the handlers properly ---
 996
 997                 MOV     R0,#7                   ;Install CallBack handler
 998                 ADR     R1,thr__callBack        ;Point to handler routine
 999                 MOV     R2,R12                  ;Point to my workspace
1000                 ADR     R3,wsp__regBuff         ;Point to the register block
1001                 SWI     OS_ChangeEnvironment    ;Install the handler
1002                 STMIA   R4!,{R1-R3}             ;Save the old handler
1003
1004                 MOV     R0,#10                  ;Install event handler
1005                 ADR     R1,thr__events          ;Point to handler routine
1006                 MOV     R2,R12                  ;Point to my workspace
1007                 SWI     OS_ChangeEnvironment    ;Install the handler
1008                 STMIA   R4!,{R1-R3}             ;Save the old handler
1009
1010                 MOV     R0,#6                   ;Install error handler
1011                 ADR     R1,thr__errors          ;Point to handler routine
1012                 MOV     R2,R12                  ;Point to my workspace
1013                 MOV     R3,R11                  ;Use Scratchpad for error
1014                 SWI     OS_ChangeEnvironment    ;Install the handler
1015                 STMIA   R4!,{R1-R3}             ;Save the old handler
1016
1017                 MOV     R0,#11                  ;Install exit handler
1018                 ADR     R1,thr__exit            ;Point to handler routine
1019                 MOV     R2,R12                  ;Point to my workspace
1020                 SWI     OS_ChangeEnvironment    ;Install the handler
1021                 STMIA   R4!,{R1-R3}             ;Save the old handler
1022
1023                 ; --- Enable my event ---
1024
1025                 MOV     R0,#14                  ;Enable an event
1026                 MOV     R1,#9                   ;I'll use the user event
1027                 SWI     OS_Byte                 ;Enable the event for me
1028
1029                 ; --- Set up the timer for me ---
1030
1031                 LDR     R0,[R13,#0]             ;Load the timeslice value
1032                 CMP     R0,#0                   ;Do we set the timer?
1033                 ADRNE   R1,thr__timer           ;Point to timer routine
1034                 MOVNE   R2,R12                  ;Point to my workspace
1035                 SWINE   OS_CallAfter            ;Install the handler then
1036
1037                 LDMFD   R13!,{R0-R4,PC}^        ;Return to caller
1038
1039                 LTORG
1040
1041 ; --- thr__end ---
1042 ;
1043 ; On entry:     --
1044 ;
1045 ; On exit:      --
1046 ;
1047 ; Use:          Removes all the handlers set up when a thread starts
1048
1049 thr__end        ROUT
1050
1051                 STMFD   R13!,{R0-R4,R14}        ;Save some registers
1052
1053                 ; --- Restore handlers to their old values ---
1054
1055                 ADR     R4,wsp__handlers        ;Point to saved handlers
1056                 MOV     R0,#7                   ;Restore CallBack handler
1057                 LDMIA   R4!,{R1-R3}             ;Restore handler registers
1058                 SWI     OS_ChangeEnvironment    ;Put them all back then
1059                 MOV     R0,#10                  ;Restore Event handler
1060                 LDMIA   R4!,{R1-R3}             ;Restore handler registers
1061                 SWI     OS_ChangeEnvironment    ;Put them all back then
1062                 MOV     R0,#6                   ;Restore Error handler
1063                 LDMIA   R4!,{R1-R3}             ;Restore handler registers
1064                 SWI     OS_ChangeEnvironment    ;Put them all back then
1065                 MOV     R0,#11                  ;Restore Exit handler
1066                 LDMIA   R4!,{R1-R3}             ;Restore handler registers
1067                 SWI     OS_ChangeEnvironment    ;Put them all back then
1068
1069                 MOV     R0,#13                  ;Disable an event
1070                 MOV     R1,#9                   ;I'm using the user event
1071                 SWI     OS_Byte                 ;Disable it nicely
1072
1073                 ; --- Get rid of my ticker event ---
1074                 ;
1075                 ; We try and remove it -- if it was never there anyway,
1076                 ; or it's happened, we don't care.
1077
1078                 ADR     R0,thr__timer           ;Point to my timer handler
1079                 MOV     R1,R12                  ;Get my workspace pointer
1080                 SWI     XOS_RemoveTickerEvent   ;Remove the ticker event
1081
1082                 ; --- Kill off the `In error' flag ---
1083
1084                 LDR     R14,wsp__flags          ;Load my flags word
1085                 BIC     R14,R14,#thrFlag__inErr ;Clear the flag bit
1086                 STR     R14,wsp__flags          ;Store it back again then
1087
1088                 LDMFD   R13!,{R0-R4,PC}^        ;Return to caller
1089
1090                 LTORG
1091
1092 ; --- thr__errors ---
1093 ;
1094 ; On entry:     R0 == pointer to my workspace
1095 ;
1096 ; On exit:      Doesn't
1097 ;
1098 ; Use:          Handles an error in a thread.
1099
1100 thr__errors     ROUT
1101
1102                 SWI     OS_IntOff               ;Disable interrupts a while
1103                 MOV     R12,R0                  ;Point to my workspace
1104                 LDR     R11,wsp__R11            ;Load scratchpad address
1105                 LDR     R10,wsp__current        ;Find the current thread
1106                 MOV     R1,#1                   ;Enter a critical section
1107                 STR     R1,[R10,#thr__critCount] ;Save critical counter
1108                 SWI     OS_IntOn                ;Safe to interrupt again
1109                 ADD     R9,R10,#thr__errorHnd   ;Locate the error handler
1110                 LDR     R5,[R9],#4              ;Get the handler address
1111                 CMP     R5,#0                   ;Is there one set up?
1112                 BEQ     %50thr__errors          ;No -- skip round next bit
1113
1114                 ; --- Am I already doing this? ---
1115
1116                 LDR     R14,wsp__flags          ;Load my flags word
1117                 TST     R14,#thrFlag__inErr     ;Am I already going?
1118                 BNE     %50thr__errors          ;Yes -- skip round next bit
1119
1120                 ; --- Handle the error and find resume point ---
1121
1122                 ORR     R14,R14,#thrFlag__inErr ;Remember I'm doing this
1123                 STR     R14,wsp__flags          ;Save it back again
1124                 ADD     R0,R11,#4               ;Point to error block
1125                 LDMIA   R9,{R1,R13}             ;Get registers to pass out
1126                 STMFD   R13!,{R12}              ;Save the old R12 away
1127                 MOV     R12,R1                  ;Pass its workspace pointer
1128                 MOV     R14,PC                  ;Set up return address
1129                 MOV     PC,R5                   ;Call the handler
1130
1131                 ; --- Call the resume point ---
1132
1133                 LDMFD   R13!,{R12}              ;Restore my workspace
1134                 LDR     R14,wsp__flags          ;Find my flags again
1135                 BIC     R14,R14,#thrFlag__inErr ;We're coming out again
1136                 STR     R14,wsp__flags          ;Store the flags back nicely
1137                 BL      thread_leaveCrit        ;Leave the critical section
1138                 MOV     R12,R1                  ;Set up resumer's workspace
1139                 MOV     PC,R0                   ;Call the resumer
1140
1141                 ; --- It all went wrong ---
1142
1143 50thr__errors   LDR     R13,wsp__stackPtr       ;Get the system stack pointer
1144                 MOV     R0,#0                   ;No current thread
1145                 STR     R0,wsp__current         ;So clear the pointer
1146                 BL      thr__end                ;Remove all its handlers
1147                 MOV     R0,R10                  ;Point to the thread
1148                 BL      thread_destroy          ;Kill off the thread
1149                 ADD     R0,R11,#4               ;Point to error block
1150                 SWI     OS_GenerateError        ;Pass to main handler
1151
1152                 LTORG
1153
1154 ; --- thr__exit ---
1155 ;
1156 ; On entry:     R12 == my workspace address
1157 ;
1158 ; On exit:      --
1159 ;
1160 ; Use:          Handles OS_Exit calls in a thread.  Closes down the thread
1161 ;               and propagates the exit
1162
1163 thr__exit       ROUT
1164
1165                 LDR     R11,wsp__R11            ;Load scratchpad pointer
1166                 SWI     OS_IntOff               ;Disable interrupts off
1167                 LDR     R13,wsp__stackPtr       ;Get the system stack pointer
1168                 BL      thr__end                ;Remove all its handlers
1169                 SWI     OS_IntOn                ;Save to interrupt again
1170                 LDR     R0,wsp__current         ;Get the current thread
1171                 MOV     R1,#0                   ;No current thread
1172                 STR     R1,wsp__current         ;So clear the pointer
1173                 BL      thread_destroy          ;Destroy the thread
1174                 SWI     OS_Exit
1175
1176                 LTORG
1177
1178 ; --- thr__callBack ---
1179 ;
1180 ; On entry:     R12 == pointer to my workspace
1181 ;
1182 ; On exit:      --
1183 ;
1184 ; Use:          Performs appropriate operations on receipt of a timer
1185 ;               interrupt.  If the thread is in a critical section, it is
1186 ;               marked `should be stopped'.  Otherwise, the context is
1187 ;               saved on the thread's stack and control returned to the
1188 ;               main program
1189
1190 thr__callBack   ROUT
1191
1192                 LDR     R10,wsp__current        ;Get the current thread
1193                 LDR     R14,[R10,#thr__critCount] ;Is it in a critical bit?
1194                 CMP     R14,#0                  ;If so, this is <>0
1195                 BEQ     %10thr__callBack        ;Otherwise skip forwards
1196
1197                 ; --- Set the thread's flag and continue ---
1198
1199                 LDR     R14,[R10,#thr__flags]   ;Get the thread's flags word
1200                 ORR     R14,R14,#tFlag__timeUp  ;Kill it on exit from crit
1201                 STR     R14,[R10,#thr__flags]   ;Store flags word back again
1202                 ADR     R14,wsp__regBuff        ;Point to save buffer
1203                 LDMIA   R14,{R0-R14}^           ;Get the user registers out
1204                 MOV     R0,R0                   ;Otherwise ARM complains
1205                 LDR     R14,[R14,#15*4]         ;Load the saved PC
1206                 MOVS    PC,R14                  ;Return to the thread
1207
1208                 ; --- Save the thread's context ---
1209
1210 10thr__callBack ADR     R14,wsp__regBuff        ;Point to save buffer
1211                 LDR     R11,[R14,#13*4]         ;Find the thread's stack ptr
1212                 SUB     R11,R11,#15*4           ;Make way for the context
1213                 STR     R11,[R10,#thr__stackPtr] ;Save the thread's stack ptr
1214                 LDMIA   R14!,{R0-R7}
1215                 STMIA   R11!,{R0-R7}
1216                 LDMIA   R14!,{R0-R7}
1217                 STMIA   R11!,{R0-R4,R6,R7}      ;Don't transfer R13
1218
1219                 ; --- Go back to the main program ---
1220
1221                 LDR     R11,wsp__stackPtr       ;Find the system stack ptr
1222                 LDR     R14,[R11,#13*4]         ;Get the saved link register
1223                 TEQP    R14,#0                  ;Set this as the current mode
1224                 MOV     R0,R0                   ;Stop ARM from being odd
1225                 MOV     R13,R11                 ;Point to the stack properly
1226                 BL      thr__end                ;Stop all the handlers
1227                 LDMFD   R13!,{R0-R12,PC}^       ;Rejoin the main thread
1228
1229                 LTORG
1230
1231 ; --- thr__events ---
1232 ;
1233 ; On entry:     R0 == event code (ideally this is 9)
1234 ;               R1 == my magic identifier number (just this thing, y'know)
1235 ;
1236 ; On exit:      R12 may contain 1
1237 ;
1238 ; Use:          Handles events.  If it's the magic timer event, then I get
1239 ;               myself a CallBack.
1240
1241 thr__events     ROUT
1242
1243                 CMP     R0,#9                   ;Is it a user event?
1244                 MOVNES  PC,R14                  ;No -- return right away
1245                 STMFD   R13!,{R14}              ;Save the link away
1246                 LDR     R14,thr__myMagic        ;Get my magic number
1247                 CMP     R1,R14                  ;Does it match up?
1248                 MOVEQ   R12,#1                  ;Yes -- request a CallBack
1249                 LDMFD   R13!,{PC}^              ;Return to caller
1250
1251                 LTORG
1252
1253 thr__myMagic    DCB     "MDW!"
1254
1255 ; --- thr__timer ---
1256 ;
1257 ; On entry:     --
1258 ;
1259 ; On exit:      --
1260 ;
1261 ; Use:          Generates an event that can eventually wend its way to my
1262 ;               CallBack handler
1263
1264 thr__timer      ROUT
1265
1266                 STMFD   R13!,{R0,R1,R14}        ;Save some registers
1267                 MOV     R0,#9                   ;Make a user event
1268                 LDR     R1,thr__myMagic         ;My special identifier
1269                 SWI     OS_GenerateEvent        ;Make the event happen
1270                 LDMFD   R13!,{R0,R1,PC}^        ;Return to caller
1271
1272                 LTORG
1273
1274 ; --- thread_enterCrit ---
1275 ;
1276 ; On entry:     --
1277 ;
1278 ; On exit:      --
1279 ;
1280 ; Use:          Declares that the current thread is about to enter a
1281 ;               critical section and must not be interrupted.
1282
1283                 EXPORT  thread_enterCrit
1284 thread_enterCrit ROUT
1285
1286                 STMFD   R13!,{R10,R12,R14}      ;Save some registers away
1287                 WSPACE  thr__wSpace             ;Find my workspace
1288                 LDR     R10,wsp__current        ;Get the current thread
1289                 CMP     R10,#0                  ;Is there one?
1290                 LDMEQFD R13!,{R10,R12,PC}^      ;No -- this is a no-op then
1291                 LDR     R14,[R10,#thr__critCount] ;Get the current counter
1292                 ADD     R14,R14,#1              ;Bump it along one
1293                 STR     R14,[R10,#thr__critCount] ;Save the counter back
1294                 LDMFD   R13!,{R10,R12,PC}^      ;Return to caller
1295
1296                 LTORG
1297
1298 ; --- thread_leaveCrit ---
1299 ;
1300 ; On entry:     --
1301 ;
1302 ; On exit:      --
1303 ;
1304 ; Use:          Declares that the current thread has left the critical
1305 ;               section and can be interrupted again.
1306
1307                 EXPORT  thread_leaveCrit
1308 thread_leaveCrit ROUT
1309
1310                 STMFD   R13!,{R10,R12-R14}      ;Save some registers away
1311                 WSPACE  thr__wSpace             ;Find my workspace
1312                 LDR     R10,wsp__current        ;Get the current thread
1313                 CMP     R10,#0                  ;Is there one?
1314                 LDMEQFD R13!,{R10,R12,R14,PC}^  ;No -- this is a no-op then
1315                 LDR     R14,[R10,#thr__critCount] ;Get the current counter
1316                 SUBS    R14,R14,#1              ;Chop one off it
1317                 MOVLT   R14,#0                  ;Stop it going negative
1318                 STR     R14,[R10,#thr__critCount] ;Save the counter back
1319                 LDMLEFD R13!,{R10,R12,R14,PC}^  ;If still critical, leave it
1320
1321                 ; --- Check to see if it should stop now ---
1322
1323                 LDR     R14,[R10,#thr__flags]   ;Get the thread's flags
1324                 TST     R14,#tFlag__timeUp      ;Has the timer happened?
1325                 LDMEQFD R13!,{R10,R12,R14,PC}^  ;No -- leave it to go then
1326                 BL      thr__end                ;Stop the handlers and things
1327                 STMFD   R13!,{R0-R9}            ;Save rest of the context
1328                 STR     R13,[R10,#thr__stackPtr] ;Save the stack pointer
1329                 LDR     R13,wsp__stackPtr       ;Load the system stack ptr
1330                 STMFD   R13!,{R0-R12,PC}^       ;Return to main thread
1331
1332                 LTORG
1333
1334 ; --- thread_errorHandler ---
1335 ;
1336 ; On entry:     R0 == pointer to routine to call
1337 ;               R1 == R12 value to call with
1338 ;               R2 == R13 value to call with
1339 ;
1340 ; On exit:      --
1341 ;
1342 ; Use:          Sets up the error handler for a thread.
1343
1344                 EXPORT  thread_errorHandler
1345 thread_errorHandler
1346                 STMFD   R13!,{R12,R14}          ;Save some registers
1347                 WSPACE  thr__wSpace             ;Locate my workspace
1348                 LDR     R14,wsp__current        ;Get the current thread
1349                 ADD     R14,R14,#thr__errorHnd  ;Point to handler block
1350                 STMIA   R14,{R0-R2}             ;Save the information away
1351                 LDMFD   R13!,{R12,PC}^          ;Return to caller
1352
1353                 LTORG
1354
1355 ;----- Data structures ------------------------------------------------------
1356
1357 ; --- The thread structure ---
1358
1359                 ^       0
1360 thr__next       #       4                       ;Pointer to next thread
1361 thr__prev       #       4                       ;Pointer to previous thread
1362 thr__suspend    #       4                       ;Thread's suspension counter
1363 thr__semaphore  #       4                       ;Pointer to blocking sem
1364 thr__priority   #       4                       ;Priority of this thread
1365 thr__timeSlice  #       4                       ;Size of timeslice for thread
1366 thr__critCount  #       4                       ;Critical section counter
1367 thr__stackPtr   #       4                       ;Thread's stack pointer
1368 thr__stack      #       4                       ;Pointer to thread's stack
1369 thr__flags      #       4                       ;Various flags for the thread
1370 thr__errorHnd   #       4*3                     ;The error handler for it
1371 thr__size       #       0                       ;Size of this block
1372
1373 tFlag__timeUp   EQU     (1<<0)                  ;Pre-empt after critical sect
1374
1375 ; --- The semaphore link structure ---
1376
1377                 ^       0
1378 sml__next       #       4                       ;Pointer to next link block
1379 sml__thread     #       4                       ;Pointer to suspended thread
1380 sml__size       #       0                       ;Size of this block
1381
1382 ; --- The main semaphore block ---
1383
1384                 ^       0
1385 sem__counter    #       4                       ;Semaphore counter
1386 sem__blocked    #       4                       ;List of blocked threads
1387 sem__blockEnd   #       4                       ;Pointer to last link in list
1388 sem__size       #       0                       ;Size of this block
1389
1390 ;----- Workspace ------------------------------------------------------------
1391
1392                 ^       0,R12
1393 thr__wStart     #       0
1394
1395 wsp__flags      #       4                       ;Various useful flags
1396 wsp__threads    #       4                       ;Pointer to list of threads
1397 wsp__active     #       4                       ;Counter of active threads
1398 wsp__current    #       4                       ;Pointer to current thread
1399 wsp__stackPtr   #       4                       ;The system stack pointer
1400 wsp__R11        #       4                       ;Sapphire's R11 pointer
1401 wsp__handlers   #       12*4                    ;Old handlers to restore
1402 wsp__regBuff    #       16*4                    ;Register save buffer
1403
1404 thr__wSize      EQU     {VAR}-thr__wStart
1405
1406 thrFlag__inited EQU     (1<<0)                  ;Are we initialised yet?
1407 thrFlag__inErr  EQU     (1<<1)                  ;We are handling an error
1408
1409                 AREA    |Sapphire$$LibData|,CODE,READONLY
1410
1411                 DCD     thr__wSize
1412                 DCD     thr__wSpace
1413                 DCD     256
1414                 DCD     thread_init
1415
1416 ;----- That's all, folks ----------------------------------------------------
1417
1418                 END