OpenCores
URL https://opencores.org/ocsvn/plasma/plasma/trunk

Subversion Repositories plasma

[/] [plasma/] [trunk/] [kernel/] [rtos.c] - Blame information for rev 417

Go to most recent revision | Details | Compare with Previous | View Log

Line No. Rev Author Line
1 400 rhoads
/*--------------------------------------------------------------------
2
 * TITLE: Plasma Real Time Operating System
3
 * AUTHOR: Steve Rhoads (rhoadss@yahoo.com)
4
 * DATE CREATED: 12/17/05
5
 * FILENAME: rtos.c
6
 * PROJECT: Plasma CPU core
7
 * COPYRIGHT: Software placed into the public domain by the author.
8
 *    Software 'as is' without warranty.  Author liable for nothing.
9
 * DESCRIPTION:
10
 *    Plasma Real Time Operating System
11
 *    Fully pre-emptive RTOS with support for:
12
 *       Heaps, Threads, Semaphores, Mutexes, Message Queues, and Timers.
13
 *    This file tries to be hardware independent except for calls to:
14
 *       MemoryRead() and MemoryWrite() for interrupts.
15 402 rhoads
 *    Support for multiple CPUs using symmetric multiprocessing.
16 400 rhoads
 *--------------------------------------------------------------------*/
17
#include "plasma.h"
18
#include "rtos.h"
19
 
20
#define HEAP_MAGIC 0x1234abcd
21
#define THREAD_MAGIC 0x4321abcd
22
#define SEM_RESERVED_COUNT 2
23
#define INFO_COUNT 4
24
#define HEAP_COUNT 8
25
 
26 407 rhoads
#define PRINTF_DEBUG(STRING, A, B)
27 416 rhoads
//#define PRINTF_DEBUG(STRING, A, B) UartPrintfCritical(STRING, A, B)
28 400 rhoads
 
29
/*************** Structures ***************/
30
#ifdef WIN32
31
   #define setjmp _setjmp
32
   //x86 registers
33
   typedef struct jmp_buf2 {
34
      uint32 Ebp, Ebx, Edi, Esi, sp, pc, extra[10];
35
   } jmp_buf2;
36
#elif defined(ARM_CPU)
37
   //ARM registers
38
   typedef struct jmp_buf2 {
39
      uint32 r[13], sp, lr, pc, cpsr, extra[5];
40
   } jmp_buf2;
41
#else  
42
   //Plasma registers
43
   typedef struct jmp_buf2 {
44
      uint32 s[9], gp, sp, pc;
45
   } jmp_buf2;
46
#endif
47
 
48
typedef struct HeapNode_s {
49
   struct HeapNode_s *next;
50
   int size;
51
} HeapNode_t;
52
 
53
struct OS_Heap_s {
54
   uint32 magic;
55
   const char *name;
56
   OS_Semaphore_t *semaphore;
57
   HeapNode_t *available;
58
   HeapNode_t base;
59
   int count;
60
   struct OS_Heap_s *alternate;
61
};
62
//typedef struct OS_Heap_s OS_Heap_t;
63
 
64
typedef enum {
65
   THREAD_PEND    = 0,       //Thread in semaphore's linked list
66
   THREAD_READY   = 1,       //Thread in ThreadHead linked list
67
   THREAD_RUNNING = 2        //Thread == ThreadCurrent[cpu]
68
} OS_ThreadState_e;
69
 
70
struct OS_Thread_s {
71
   const char *name;         //Name of thread
72
   OS_ThreadState_e state;   //Pending, ready, or running
73
   int cpuIndex;             //Which CPU is running the thread
74
   int cpuLock;              //Lock the thread to a specific CPU
75
   jmp_buf env;              //Registers saved during context swap
76
   OS_FuncPtr_t funcPtr;     //First function called
77
   void *arg;                //Argument to first function called
78
   uint32 priority;          //Priority of thread (0=low, 255=high)
79
   uint32 ticksTimeout;      //Tick value when semaphore pend times out
80
   void *info[INFO_COUNT];   //User storage
81
   OS_Semaphore_t *semaphorePending;  //Semaphore thread is blocked on
82
   int returnCode;           //Return value from semaphore pend
83
   uint32 processId;         //Process ID if using MMU
84
   OS_Heap_t *heap;          //Heap used if no heap specified
85
   struct OS_Thread_s *next; //Linked list of threads by priority
86
   struct OS_Thread_s *prev;
87
   struct OS_Thread_s *nextTimeout; //Linked list of threads by timeout
88
   struct OS_Thread_s *prevTimeout;
89
   uint32 magic[1];          //Bottom of stack to detect stack overflow
90
};
91
//typedef struct OS_Thread_s OS_Thread_t;
92
 
93
struct OS_Semaphore_s {
94
   const char *name;
95
   struct OS_Thread_s *threadHead; //threads pending on semaphore
96
   int count;
97
};
98
//typedef struct OS_Semaphore_s OS_Semaphore_t;
99
 
100
struct OS_Mutex_s {
101
   OS_Semaphore_t *semaphore;
102
   OS_Thread_t *thread;
103 407 rhoads
   uint32 priorityRestore;
104 400 rhoads
   int count;
105
};
106
//typedef struct OS_Mutex_s OS_Mutex_t;
107
 
108
struct OS_MQueue_s {
109
   const char *name;
110
   OS_Semaphore_t *semaphore;
111
   int count, size, used, read, write;
112
};
113
//typedef struct OS_MQueue_s OS_MQueue_t;
114
 
115
struct OS_Timer_s {
116
   const char *name;
117
   struct OS_Timer_s *next, *prev;
118
   uint32 ticksTimeout;
119
   uint32 ticksRestart;
120
   int active;
121
   OS_TimerFuncPtr_t callback;
122
   OS_MQueue_t *mqueue;
123
   uint32 info;
124
};
125
//typedef struct OS_Timer_s OS_Timer_t;
126
 
127
 
128
/*************** Globals ******************/
129
static OS_Heap_t *HeapArray[HEAP_COUNT];
130
static int InterruptInside[OS_CPU_COUNT];
131
static int ThreadNeedReschedule[OS_CPU_COUNT];
132
static OS_Thread_t *ThreadCurrent[OS_CPU_COUNT];  //Currently running thread(s)
133
static OS_Thread_t *ThreadHead;   //Linked list of threads sorted by priority
134
static OS_Thread_t *TimeoutHead;  //Linked list of threads sorted by timeout
135
static int ThreadSwapEnabled;
136
static uint32 ThreadTime;         //Number of ~10ms ticks since reboot
137
static void *NeedToFree;          //Closed but not yet freed thread
138
static OS_Semaphore_t SemaphoreReserved[SEM_RESERVED_COUNT];
139
static OS_Semaphore_t *SemaphoreSleep;
140 402 rhoads
static OS_Semaphore_t *SemaphoreRelease; //Protects NeedToFree
141 400 rhoads
static OS_Semaphore_t *SemaphoreLock;
142
static OS_Semaphore_t *SemaphoreTimer;
143
static OS_Timer_t *TimerHead;     //Linked list of timers sorted by timeout
144 402 rhoads
static OS_FuncPtr_t Isr[32];      //Interrupt service routines
145
#if defined(WIN32) && OS_CPU_COUNT > 1
146
static unsigned int Registration[OS_CPU_COUNT];
147
#endif
148 400 rhoads
 
149
/***************** Heap *******************/
150
/******************************************/
151
OS_Heap_t *OS_HeapCreate(const char *name, void *memory, uint32 size)
152
{
153
   OS_Heap_t *heap;
154
 
155
   assert(((uint32)memory & 3) == 0);
156
   heap = (OS_Heap_t*)memory;
157
   heap->magic = HEAP_MAGIC;
158
   heap->name = name;
159
   heap->semaphore = OS_SemaphoreCreate(name, 1);
160
   heap->available = (HeapNode_t*)(heap + 1);
161
   heap->available->next = &heap->base;
162
   heap->available->size = (size - sizeof(OS_Heap_t)) / sizeof(HeapNode_t);
163
   heap->base.next = heap->available;
164
   heap->base.size = 0;
165
   heap->count = 0;
166
   heap->alternate = NULL;
167
   return heap;
168
}
169
 
170
 
171
/******************************************/
172
void OS_HeapDestroy(OS_Heap_t *heap)
173
{
174
   OS_SemaphoreDelete(heap->semaphore);
175
}
176
 
177
 
178
/******************************************/
179
//Modified from Kernighan & Ritchie "The C Programming Language"
180
void *OS_HeapMalloc(OS_Heap_t *heap, int bytes)
181
{
182
   HeapNode_t *node, *prevp;
183
   int nunits;
184
 
185
   if(heap == NULL && OS_ThreadSelf())
186
      heap = OS_ThreadSelf()->heap;
187
   if((uint32)heap < HEAP_COUNT)
188
      heap = HeapArray[(int)heap];
189
   nunits = (bytes + sizeof(HeapNode_t) - 1) / sizeof(HeapNode_t) + 1;
190
   OS_SemaphorePend(heap->semaphore, OS_WAIT_FOREVER);
191
   prevp = heap->available;
192
   for(node = prevp->next; ; prevp = node, node = node->next)
193
   {
194
      if(node->size >= nunits)       //Big enough?
195
      {
196
         if(node->size == nunits)    //Exactly
197
            prevp->next = node->next;
198
         else
199
         {                           //Allocate tail end
200
            node->size -= nunits;
201
            node += node->size;
202
            node->size = nunits;
203
         }
204
         heap->available = prevp;
205
         node->next = (HeapNode_t*)heap;
206 416 rhoads
         PRINTF_DEBUG("malloc(%d, %d)\n", node->size * sizeof(HeapNode_t), heap->count);
207
         ++heap->count;
208 400 rhoads
         OS_SemaphorePost(heap->semaphore);
209
         //UartPrintfCritical("OS_HeapMalloc(%d)=0x%x\n", bytes, (int)(node+1));
210
         return (void*)(node + 1);
211
      }
212
      if(node == heap->available)   //Wrapped around free list
213
      {
214
         OS_SemaphorePost(heap->semaphore);
215
         if(heap->alternate)
216
            return OS_HeapMalloc(heap->alternate, bytes);
217
         printf("M%d ", heap->count);
218
         return NULL;
219
      }
220 395 rhoads
   }
221 400 rhoads
}
222
 
223
 
224
/******************************************/
225
//Modified from K&R
226
void OS_HeapFree(void *block)
227
{
228
   OS_Heap_t *heap;
229
   HeapNode_t *bp, *node;
230
 
231
   //UartPrintfCritical("OS_HeapFree(0x%x)\n", block);
232
   assert(block);
233
   bp = (HeapNode_t*)block - 1;   //point to block header
234
   heap = (OS_Heap_t*)bp->next;
235
   assert(heap->magic == HEAP_MAGIC);
236
   if(heap->magic != HEAP_MAGIC)
237
      return;
238 416 rhoads
   OS_SemaphorePend(heap->semaphore, OS_WAIT_FOREVER);
239 400 rhoads
   --heap->count;
240 416 rhoads
   PRINTF_DEBUG("free(%d, %d)\n", bp->size * sizeof(HeapNode_t), heap->count);
241 400 rhoads
   for(node = heap->available; !(node < bp && bp < node->next); node = node->next)
242
   {
243
      if(node >= node->next && (bp > node || bp < node->next))
244
         break;               //freed block at start or end of area
245
   }
246
 
247
   if(bp + bp->size == node->next)   //join to upper
248
   {
249
      bp->size += node->next->size;
250
      bp->next = node->next->next;
251
   }
252
   else
253
   {
254
      bp->next = node->next;
255
   }
256
 
257
   if(node + node->size == bp)       //join to lower
258
   {
259
      node->size += bp->size;
260
      node->next = bp->next;
261
   }
262
   else
263
      node->next = bp;
264
   heap->available = node;
265
   OS_SemaphorePost(heap->semaphore);
266
}
267
 
268
 
269
/******************************************/
270
void OS_HeapAlternate(OS_Heap_t *heap, OS_Heap_t *alternate)
271
{
272
   heap->alternate = alternate;
273
}
274
 
275
 
276
/******************************************/
277
void OS_HeapRegister(void *index, OS_Heap_t *heap)
278
{
279
   if((uint32)index < HEAP_COUNT)
280
      HeapArray[(int)index] = heap;
281
}
282
 
283
 
284
 
285
/***************** Thread *****************/
286
/******************************************/
287
//Linked list of threads sorted by priority
288
//The listed list is either ThreadHead (ready to run threads not including
289
//the currently running thread) or a list of threads waiting on a semaphore.
290
//Must be called with interrupts disabled
291
static void OS_ThreadPriorityInsert(OS_Thread_t **head, OS_Thread_t *thread)
292
{
293
   OS_Thread_t *node, *prev;
294
 
295
   prev = NULL;
296
   for(node = *head; node; node = node->next)
297
   {
298
      if(node->priority < thread->priority)
299
         break;
300
      prev = node;
301
   }
302
 
303
   if(prev == NULL)
304
   {
305
      thread->next = *head;
306
      thread->prev = NULL;
307
      if(*head)
308
         (*head)->prev = thread;
309
      *head = thread;
310
   }
311
   else
312
   {
313
      if(prev->next)
314
         prev->next->prev = thread;
315
      thread->next = prev->next;
316
      thread->prev = prev;
317
      prev->next = thread;
318
   }
319
   assert(ThreadHead);
320
   thread->state = THREAD_READY;
321
}
322
 
323
 
324
/******************************************/
325
//Must be called with interrupts disabled
326
static void OS_ThreadPriorityRemove(OS_Thread_t **head, OS_Thread_t *thread)
327
{
328
   assert(thread->magic[0] == THREAD_MAGIC);  //check stack overflow
329
   if(thread->prev == NULL)
330
      *head = thread->next;
331
   else
332
      thread->prev->next = thread->next;
333
   if(thread->next)
334
      thread->next->prev = thread->prev;
335
   thread->next = NULL;
336
   thread->prev = NULL;
337
}
338
 
339
 
340
/******************************************/
341
//Linked list of threads sorted by timeout value
342
//Must be called with interrupts disabled
343
static void OS_ThreadTimeoutInsert(OS_Thread_t *thread)
344
{
345
   OS_Thread_t *node, *prev;
346
   int diff;
347
 
348
   prev = NULL;
349
   for(node = TimeoutHead; node; node = node->nextTimeout)
350
   {
351
      diff = thread->ticksTimeout - node->ticksTimeout;
352
      if(diff <= 0)
353
         break;
354
      prev = node;
355
   }
356
 
357
   if(prev == NULL)
358
   {
359
      thread->nextTimeout = TimeoutHead;
360
      thread->prevTimeout = NULL;
361
      if(TimeoutHead)
362
         TimeoutHead->prevTimeout = thread;
363
      TimeoutHead = thread;
364
   }
365
   else
366
   {
367
      if(prev->nextTimeout)
368
         prev->nextTimeout->prevTimeout = thread;
369
      thread->nextTimeout = prev->nextTimeout;
370
      thread->prevTimeout = prev;
371
      prev->nextTimeout = thread;
372
   }
373
}
374
 
375
 
376
/******************************************/
377
//Must be called with interrupts disabled
378
static void OS_ThreadTimeoutRemove(OS_Thread_t *thread)
379
{
380
   if(thread->prevTimeout == NULL && TimeoutHead != thread)
381
      return;         //not in list
382
   if(thread->prevTimeout == NULL)
383
      TimeoutHead = thread->nextTimeout;
384
   else
385
      thread->prevTimeout->nextTimeout = thread->nextTimeout;
386
   if(thread->nextTimeout)
387
      thread->nextTimeout->prevTimeout = thread->prevTimeout;
388
   thread->nextTimeout = NULL;
389
   thread->prevTimeout = NULL;
390
}
391
 
392
 
393
/******************************************/
394
//Loads highest priority thread from the ThreadHead linked list
395
//The currently running thread isn't in the ThreadHead list
396
//Must be called with interrupts disabled
397
static void OS_ThreadReschedule(int roundRobin)
398
{
399
   OS_Thread_t *threadNext, *threadCurrent;
400
   int rc, cpuIndex = OS_CpuIndex();
401
 
402
   if(ThreadSwapEnabled == 0 || InterruptInside[cpuIndex])
403
   {
404
      ThreadNeedReschedule[cpuIndex] |= 2 + roundRobin;  //Reschedule later
405
      return;
406
   }
407
   ThreadNeedReschedule[cpuIndex] = 0;
408
 
409
   //Determine which thread should run
410
   threadNext = ThreadHead;
411
   while(threadNext && threadNext->cpuLock != -1 &&
412
         threadNext->cpuLock != cpuIndex)
413
      threadNext = threadNext->next;         //Skip CPU locked threads
414
   if(threadNext == NULL)
415
      return;
416
   threadCurrent = ThreadCurrent[cpuIndex];
417
 
418
   if(threadCurrent == NULL ||
419
      threadCurrent->state == THREAD_PEND ||
420
      threadCurrent->priority < threadNext->priority ||
421
      (roundRobin && threadCurrent->priority == threadNext->priority))
422
   {
423
      //Swap threads
424
      ThreadCurrent[cpuIndex] = threadNext;
425
      if(threadCurrent)
426
      {
427
         assert(threadCurrent->magic[0] == THREAD_MAGIC); //check stack overflow
428
         if(threadCurrent->state == THREAD_RUNNING)
429
            OS_ThreadPriorityInsert(&ThreadHead, threadCurrent);
430 416 rhoads
         //PRINTF_DEBUG("Pause(%d,%s) ", OS_CpuIndex(), threadCurrent->name);
431 400 rhoads
         rc = setjmp(threadCurrent->env);  //ANSI C call to save registers
432
         if(rc)
433 402 rhoads
         {
434 416 rhoads
            //PRINTF_DEBUG("Resume(%d,%s) ", OS_CpuIndex(), 
435
            //   ThreadCurrent[OS_CpuIndex()]->name);
436 400 rhoads
            return;  //Returned from longjmp()
437 402 rhoads
         }
438 400 rhoads
      }
439
 
440
      //Remove the new running thread from the ThreadHead linked list
441
      threadNext = ThreadCurrent[OS_CpuIndex()]; //removed warning
442
      assert(threadNext->state == THREAD_READY);
443
      OS_ThreadPriorityRemove(&ThreadHead, threadNext);
444
      threadNext->state = THREAD_RUNNING;
445
      threadNext->cpuIndex = OS_CpuIndex();
446 402 rhoads
#if defined(WIN32) && OS_CPU_COUNT > 1
447
      //Set the Windows thread that the Plasma thread is running on
448
      ((jmp_buf2*)threadNext->env)->extra[0] = Registration[threadNext->cpuIndex];
449
#endif
450 400 rhoads
      longjmp(threadNext->env, 1);         //ANSI C call to restore registers
451
   }
452
}
453
 
454
 
455
/******************************************/
456
void OS_ThreadCpuLock(OS_Thread_t *thread, int cpuIndex)
457
{
458
   thread->cpuLock = cpuIndex;
459
   if(thread == OS_ThreadSelf() && cpuIndex != (int)OS_CpuIndex())
460
      OS_ThreadSleep(1);
461
}
462
 
463
 
464
/******************************************/
465
static void OS_ThreadInit(void *arg)
466
{
467
   uint32 cpuIndex = OS_CpuIndex();
468
   (void)arg;
469
 
470 407 rhoads
   PRINTF_DEBUG("Starting(%d,%s) ", cpuIndex, OS_ThreadSelf()->name);
471 400 rhoads
   OS_CriticalEnd(1);
472
   ThreadCurrent[cpuIndex]->funcPtr(ThreadCurrent[cpuIndex]->arg);
473
   OS_ThreadExit();
474
}
475
 
476
 
477
/******************************************/
478
//Stops warning "argument X might be clobbered by `longjmp'"
479
static void OS_ThreadRegsInit(jmp_buf env)
480
{
481
   setjmp(env); //ANSI C call to save registers
482
}
483
 
484
 
485
/******************************************/
486
OS_Thread_t *OS_ThreadCreate(const char *name,
487
                             OS_FuncPtr_t funcPtr,
488
                             void *arg,
489
                             uint32 priority,
490
                             uint32 stackSize)
491
{
492
   OS_Thread_t *thread;
493
   uint8 *stack;
494
   jmp_buf2 *env;
495
   uint32 state;
496 402 rhoads
#ifdef WIN32
497
   int stackFrameSize;
498
#endif
499 400 rhoads
 
500
   OS_SemaphorePend(SemaphoreRelease, OS_WAIT_FOREVER);
501
   if(NeedToFree)
502
      OS_HeapFree(NeedToFree);
503
   NeedToFree = NULL;
504
   OS_SemaphorePost(SemaphoreRelease);
505
 
506
   if(stackSize == 0)
507
      stackSize = STACK_SIZE_DEFAULT;
508
   if(stackSize < STACK_SIZE_MINIMUM)
509
      stackSize = STACK_SIZE_MINIMUM;
510
   thread = (OS_Thread_t*)OS_HeapMalloc(NULL, sizeof(OS_Thread_t) + stackSize);
511
   assert(thread);
512
   if(thread == NULL)
513
      return NULL;
514
   memset(thread, 0, sizeof(OS_Thread_t));
515
   stack = (uint8*)(thread + 1);
516
   memset(stack, 0xcd, stackSize);
517
 
518
   thread->name = name;
519
   thread->state = THREAD_READY;
520
   thread->cpuLock = -1;
521
   thread->funcPtr = funcPtr;
522
   thread->arg = arg;
523
   thread->priority = priority;
524
   thread->semaphorePending = NULL;
525
   thread->returnCode = 0;
526
   if(OS_ThreadSelf())
527
   {
528
      thread->processId = OS_ThreadSelf()->processId;
529
      thread->heap = OS_ThreadSelf()->heap;
530
   }
531
   else
532
   {
533
      thread->processId = 0;
534
      thread->heap = NULL;
535
   }
536
   thread->next = NULL;
537
   thread->prev = NULL;
538
   thread->nextTimeout = NULL;
539
   thread->prevTimeout = NULL;
540
   thread->magic[0] = THREAD_MAGIC;
541
 
542
   OS_ThreadRegsInit(thread->env);
543
   env = (jmp_buf2*)thread->env;
544 402 rhoads
   env->pc = (uint32)OS_ThreadInit;
545
#ifndef WIN32
546 400 rhoads
   env->sp = (uint32)stack + stackSize - 24; //minimum stack frame size
547 402 rhoads
#else
548
   stackFrameSize = env->Ebp - env->sp;
549
   env->Ebp = (uint32)stack + stackSize - 24;//stack frame base pointer
550
   env->sp =  env->Ebp - stackFrameSize;
551
#endif
552 400 rhoads
 
553
   //Add thread to linked list of ready to run threads
554
   state = OS_CriticalBegin();
555
   OS_ThreadPriorityInsert(&ThreadHead, thread);
556
   OS_ThreadReschedule(0);                   //run highest priority thread
557
   OS_CriticalEnd(state);
558
   return thread;
559
}
560
 
561
 
562
/******************************************/
563
void OS_ThreadExit(void)
564
{
565
   uint32 state, cpuIndex = OS_CpuIndex();
566
 
567
   for(;;)
568
   {
569
      //Free the memory for closed but not yet freed threads
570
      OS_SemaphorePend(SemaphoreRelease, OS_WAIT_FOREVER);
571
      if(NeedToFree)
572
         OS_HeapFree(NeedToFree);
573
      NeedToFree = NULL;
574
      OS_SemaphorePost(SemaphoreRelease);
575
 
576
      state = OS_CriticalBegin();
577
      if(NeedToFree)
578
      {
579
         OS_CriticalEnd(state);
580
         continue;
581
      }
582
      ThreadCurrent[cpuIndex]->state = THREAD_PEND;
583
      NeedToFree = ThreadCurrent[cpuIndex];
584
      OS_ThreadReschedule(0);
585
      OS_CriticalEnd(state);
586
   }
587
}
588
 
589
 
590
/******************************************/
591
//Return currently running thread
592
OS_Thread_t *OS_ThreadSelf(void)
593
{
594
   return ThreadCurrent[OS_CpuIndex()];
595
}
596
 
597
 
598
/******************************************/
599
//Sleep for ~10 msecs ticks
600
void OS_ThreadSleep(int ticks)
601
{
602
   OS_SemaphorePend(SemaphoreSleep, ticks);
603
}
604
 
605
 
606
/******************************************/
607
//Return the number of ~10 msecs ticks since reboot
608
uint32 OS_ThreadTime(void)
609
{
610
   return ThreadTime;
611
}
612
 
613
 
614
/******************************************/
615
//Save thread unique values
616
void OS_ThreadInfoSet(OS_Thread_t *thread, uint32 index, void *Info)
617
{
618
   if(index < INFO_COUNT)
619
      thread->info[index] = Info;
620
}
621
 
622
 
623
/******************************************/
624
void *OS_ThreadInfoGet(OS_Thread_t *thread, uint32 index)
625
{
626
   if(index < INFO_COUNT)
627
      return thread->info[index];
628
   return NULL;
629
}
630
 
631
 
632
/******************************************/
633
uint32 OS_ThreadPriorityGet(OS_Thread_t *thread)
634
{
635
   return thread->priority;
636
}
637
 
638
 
639
/******************************************/
640
void OS_ThreadPrioritySet(OS_Thread_t *thread, uint32 priority)
641
{
642
   uint32 state;
643
   state = OS_CriticalBegin();
644
   thread->priority = priority;
645
   if(thread->state == THREAD_READY)
646
   {
647
      OS_ThreadPriorityRemove(&ThreadHead, thread);
648
      OS_ThreadPriorityInsert(&ThreadHead, thread);
649
      OS_ThreadReschedule(0);
650
   }
651
   OS_CriticalEnd(state);
652
}
653
 
654
 
655
/******************************************/
656
void OS_ThreadProcessId(OS_Thread_t *thread, uint32 processId, OS_Heap_t *heap)
657
{
658
   thread->processId = processId;
659
   thread->heap = heap;
660
}
661
 
662
 
663
/******************************************/
664
//Must be called with interrupts disabled every ~10 msecs
665
//Will wake up threads that have timed out waiting on a semaphore
666 402 rhoads
static void OS_ThreadTick(void *Arg)
667 400 rhoads
{
668
   OS_Thread_t *thread;
669
   OS_Semaphore_t *semaphore;
670
   int diff;
671
   (void)Arg;
672
 
673
   ++ThreadTime;         //Number of ~10 msec ticks since reboot
674
   while(TimeoutHead)
675
   {
676
      thread = TimeoutHead;
677
      diff = ThreadTime - thread->ticksTimeout;
678
      if(diff < 0)
679
         break;
680
 
681
      //The thread has timed out waiting for a semaphore
682
      OS_ThreadTimeoutRemove(thread);
683
      semaphore = thread->semaphorePending;
684
      ++semaphore->count;
685
      thread->semaphorePending = NULL;
686
      thread->returnCode = -1;
687
      OS_ThreadPriorityRemove(&semaphore->threadHead, thread);
688
      OS_ThreadPriorityInsert(&ThreadHead, thread);
689
   }
690
   OS_ThreadReschedule(1);    //Run highest priority thread
691
}
692
 
693
 
694
 
695
/***************** Semaphore **************/
696
/******************************************/
697
//Create a counting semaphore
698
OS_Semaphore_t *OS_SemaphoreCreate(const char *name, uint32 count)
699
{
700
   OS_Semaphore_t *semaphore;
701
   static int semCount = 0;
702
 
703
   if(semCount < SEM_RESERVED_COUNT)
704
      semaphore = &SemaphoreReserved[semCount++];  //Heap not ready yet
705
   else
706
      semaphore = (OS_Semaphore_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_Semaphore_t));
707
   assert(semaphore);
708
   if(semaphore == NULL)
709
      return NULL;
710
 
711
   semaphore->name = name;
712
   semaphore->threadHead = NULL;
713
   semaphore->count = count;
714
   return semaphore;
715
}
716
 
717
 
718
/******************************************/
719
void OS_SemaphoreDelete(OS_Semaphore_t *semaphore)
720
{
721
   while(semaphore->threadHead)
722
      OS_SemaphorePost(semaphore);
723
   OS_HeapFree(semaphore);
724
}
725
 
726
 
727
/******************************************/
728
//Sleep the number of ticks (~10ms) until the semaphore is acquired
729
int OS_SemaphorePend(OS_Semaphore_t *semaphore, int ticks)
730
{
731
   uint32 state, cpuIndex;
732
   OS_Thread_t *thread;
733
   int returnCode=0;
734
 
735 416 rhoads
   //PRINTF_DEBUG("SemPend(%d,%s) ", OS_CpuIndex(), semaphore->name);
736 400 rhoads
   assert(semaphore);
737
   assert(InterruptInside[OS_CpuIndex()] == 0);
738
   state = OS_CriticalBegin();    //Disable interrupts
739
   if(--semaphore->count < 0)
740
   {
741
      //Semaphore not available
742
      if(ticks == 0)
743
      {
744
         ++semaphore->count;
745
         OS_CriticalEnd(state);
746
         return -1;
747
      }
748
 
749
      //Need to sleep until the semaphore is available
750
      cpuIndex = OS_CpuIndex();
751
      thread = ThreadCurrent[cpuIndex];
752
      assert(thread);
753
      thread->semaphorePending = semaphore;
754
      thread->ticksTimeout = ticks + OS_ThreadTime();
755
 
756
      //FYI: The current thread isn't in the ThreadHead linked list
757
      //Place the thread into a sorted linked list of pending threads
758
      OS_ThreadPriorityInsert(&semaphore->threadHead, thread);
759
      thread->state = THREAD_PEND;
760
      if(ticks != OS_WAIT_FOREVER)
761
         OS_ThreadTimeoutInsert(thread); //Check every ~10ms for timeouts
762
      assert(ThreadHead);
763
      OS_ThreadReschedule(0);           //Run highest priority thread
764
      returnCode = thread->returnCode;  //Will be -1 if timed out
765
   }
766
   OS_CriticalEnd(state);               //Re-enable interrupts
767
   return returnCode;
768
}
769
 
770
 
771
/******************************************/
772
//Release a semaphore and possibly wake up a blocked thread
773
void OS_SemaphorePost(OS_Semaphore_t *semaphore)
774
{
775
   uint32 state;
776
   OS_Thread_t *thread;
777
 
778 416 rhoads
   //PRINTF_DEBUG("SemPost(%d,%s) ", OS_CpuIndex(), semaphore->name);
779 400 rhoads
   assert(semaphore);
780
   state = OS_CriticalBegin();
781
   if(++semaphore->count <= 0)
782
   {
783
      //Wake up a thread that was waiting for this semaphore
784
      thread = semaphore->threadHead;
785
      OS_ThreadTimeoutRemove(thread);
786
      OS_ThreadPriorityRemove(&semaphore->threadHead, thread);
787
      OS_ThreadPriorityInsert(&ThreadHead, thread);
788
      thread->semaphorePending = NULL;
789
      thread->returnCode = 0;
790
      OS_ThreadReschedule(0);
791
   }
792
   OS_CriticalEnd(state);
793
}
794
 
795
 
796
 
797
/***************** Mutex ******************/
798
/******************************************/
799
//Create a mutex (a thread aware semaphore)
800
OS_Mutex_t *OS_MutexCreate(const char *name)
801
{
802
   OS_Mutex_t *mutex;
803
 
804
   mutex = (OS_Mutex_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_Mutex_t));
805
   if(mutex == NULL)
806
      return NULL;
807
   mutex->semaphore = OS_SemaphoreCreate(name, 1);
808
   if(mutex->semaphore == NULL)
809
      return NULL;
810
   mutex->thread = NULL;
811
   mutex->count = 0;
812
   return mutex;
813
}
814
 
815
 
816
/******************************************/
817
void OS_MutexDelete(OS_Mutex_t *mutex)
818
{
819
   OS_SemaphoreDelete(mutex->semaphore);
820
   OS_HeapFree(mutex);
821
}
822
 
823
 
824
/******************************************/
825
void OS_MutexPend(OS_Mutex_t *mutex)
826
{
827
   OS_Thread_t *thread;
828 407 rhoads
   uint32 state;
829 400 rhoads
 
830
   assert(mutex);
831
   thread = OS_ThreadSelf();
832
   if(thread == mutex->thread)
833
   {
834
      ++mutex->count;
835
      return;
836
   }
837 407 rhoads
 
838
   state = OS_CriticalBegin();
839
   //Priority inheritance to prevent priority inversion
840
   if(mutex->thread && mutex->thread->priority < thread->priority)
841
      OS_ThreadPrioritySet(mutex->thread, thread->priority);
842
 
843 400 rhoads
   OS_SemaphorePend(mutex->semaphore, OS_WAIT_FOREVER);
844 407 rhoads
   mutex->priorityRestore = thread->priority;
845 400 rhoads
   mutex->thread = thread;
846
   mutex->count = 1;
847 407 rhoads
   OS_CriticalEnd(state);
848 400 rhoads
}
849
 
850
 
851
/******************************************/
852
void OS_MutexPost(OS_Mutex_t *mutex)
853
{
854 407 rhoads
   OS_Thread_t *thread = OS_ThreadSelf();
855
   uint32 state, priorityRestore;
856
 
857 400 rhoads
   assert(mutex);
858 407 rhoads
   assert(mutex->thread == thread);
859 400 rhoads
   assert(mutex->count > 0);
860
   if(--mutex->count <= 0)
861
   {
862 407 rhoads
      state = OS_CriticalBegin();
863 400 rhoads
      mutex->thread = NULL;
864 407 rhoads
      priorityRestore = mutex->priorityRestore;
865 400 rhoads
      OS_SemaphorePost(mutex->semaphore);
866 407 rhoads
      if(priorityRestore < thread->priority)
867
         OS_ThreadPrioritySet(thread, priorityRestore);
868
      OS_CriticalEnd(state);
869 400 rhoads
   }
870
}
871
 
872
 
873
/***************** MQueue *****************/
874
/******************************************/
875
//Create a message queue
876
OS_MQueue_t *OS_MQueueCreate(const char *name,
877
                             int messageCount,
878
                             int messageBytes)
879
{
880
   OS_MQueue_t *queue;
881
   int size;
882
 
883
   assert((messageBytes & 3) == 0);
884
   size = messageBytes / sizeof(uint32);
885
   queue = (OS_MQueue_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_MQueue_t) +
886
      messageCount * size * 4);
887
   if(queue == NULL)
888
      return queue;
889
   queue->name = name;
890
   queue->semaphore = OS_SemaphoreCreate(name, 0);
891
   if(queue->semaphore == NULL)
892
      return NULL;
893
   queue->count = messageCount;
894
   queue->size = size;
895
   queue->used = 0;
896
   queue->read = 0;
897
   queue->write = 0;
898
   return queue;
899
}
900
 
901
 
902
/******************************************/
903
void OS_MQueueDelete(OS_MQueue_t *mQueue)
904
{
905
   OS_SemaphoreDelete(mQueue->semaphore);
906
   OS_HeapFree(mQueue);
907
}
908
 
909
 
910
/******************************************/
911
//Send a message that is messageBytes long (defined during create)
912
int OS_MQueueSend(OS_MQueue_t *mQueue, void *message)
913
{
914
   uint32 state, *dst, *src;
915 393 rhoads
   int i;
916 400 rhoads
 
917
   assert(mQueue);
918
   src = (uint32*)message;
919
   state = OS_CriticalBegin();           //Disable interrupts
920
   if(++mQueue->used > mQueue->count)
921
   {
922
      //The message queue is full so discard the message
923
      --mQueue->used;
924
      OS_CriticalEnd(state);
925
      return -1;
926
   }
927
   dst = (uint32*)(mQueue + 1) + mQueue->write * mQueue->size;
928
   for(i = 0; i < mQueue->size; ++i)     //Copy the message into the queue
929
      dst[i] = src[i];
930
   if(++mQueue->write >= mQueue->count)
931
      mQueue->write = 0;
932
   OS_CriticalEnd(state);                //Re-enable interrupts
933
   OS_SemaphorePost(mQueue->semaphore);  //Wakeup the receiving thread
934
   return 0;
935
}
936
 
937
 
938
/******************************************/
939
//Receive a message that is messageBytes long (defined during create)
940
//Wait at most ~10 msec ticks
941
int OS_MQueueGet(OS_MQueue_t *mQueue, void *message, int ticks)
942
{
943
   uint32 state, *dst, *src;
944
   int i, rc;
945
 
946
   assert(mQueue);
947
   rc = OS_SemaphorePend(mQueue->semaphore, ticks); //Wait for message
948
   if(rc)
949
      return rc;                         //Request timed out so rc = -1
950
   state = OS_CriticalBegin();           //Disable interrupts
951
   --mQueue->used;
952
   dst = (uint32*)message;
953
   src = (uint32*)(mQueue + 1) + mQueue->read * mQueue->size;
954
   for(i = 0; i < mQueue->size; ++i)     //Copy message from the queue
955
      dst[i] = src[i];
956
   if(++mQueue->read >= mQueue->count)
957
      mQueue->read = 0;
958
   OS_CriticalEnd(state);                //Re-enable interrupts
959
   return 0;
960
}
961
 
962
 
963
 
964
/***************** Jobs *******************/
965
/******************************************/
966
static OS_MQueue_t *jobQueue;
967
static OS_Thread_t *jobThread;
968
 
969
//This thread waits for jobs that request a function to be called
970
static void JobThread(void *arg)
971
{
972
   uint32 message[4];
973
   JobFunc_t funcPtr;
974
   (void)arg;
975
   for(;;)
976
   {
977
      OS_MQueueGet(jobQueue, message, OS_WAIT_FOREVER);
978
      funcPtr = (JobFunc_t)message[0];
979
      funcPtr((void*)message[1], (void*)message[2], (void*)message[3]);
980
   }
981
}
982
 
983
 
984
/******************************************/
985
//Call a function using the job thread so the caller won't be blocked
986
void OS_Job(JobFunc_t funcPtr, void *arg0, void *arg1, void *arg2)
987
{
988
   uint32 message[4];
989
   int rc;
990
 
991
   OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
992
   if(jobThread == NULL)
993
   {
994
      jobQueue = OS_MQueueCreate("job", 100, 16);
995
      jobThread = OS_ThreadCreate("job", JobThread, NULL, 150, 4000);
996
   }
997
   OS_SemaphorePost(SemaphoreLock);
998
 
999
   message[0] = (uint32)funcPtr;
1000
   message[1] = (uint32)arg0;
1001
   message[2] = (uint32)arg1;
1002
   message[3] = (uint32)arg2;
1003
   rc = OS_MQueueSend(jobQueue, message);
1004
}
1005
 
1006
 
1007
/***************** Timer ******************/
1008
/******************************************/
1009
//This thread polls the list of timers to see if any have timed out
1010
static void OS_TimerThread(void *arg)
1011
{
1012
   uint32 timeNow;
1013
   int diff, ticks;
1014
   uint32 message[8];
1015
   OS_Timer_t *timer;
1016
   (void)arg;
1017
 
1018
   timeNow = OS_ThreadTime();  //Number of ~10 msec ticks since reboot
1019
   for(;;)
1020
   {
1021
      //Determine how long to sleep
1022
      OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
1023
      if(TimerHead)
1024
         ticks = TimerHead->ticksTimeout - timeNow;
1025
      else
1026
         ticks = OS_WAIT_FOREVER;
1027
      OS_SemaphorePost(SemaphoreLock);
1028
      OS_SemaphorePend(SemaphoreTimer, ticks);
1029
 
1030
      //Send messages for all timed out timers
1031
      timeNow = OS_ThreadTime();
1032
      for(;;)
1033
      {
1034
         timer = NULL;
1035
         OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
1036
         if(TimerHead)
1037
         {
1038
            diff = timeNow - TimerHead->ticksTimeout;
1039
            if(diff >= 0)
1040
               timer = TimerHead;
1041
         }
1042
         OS_SemaphorePost(SemaphoreLock);
1043
         if(timer == NULL)
1044
            break;
1045
         if(timer->ticksRestart)
1046
            OS_TimerStart(timer, timer->ticksRestart, timer->ticksRestart);
1047
         else
1048
            OS_TimerStop(timer);
1049
 
1050
         if(timer->callback)
1051
            timer->callback(timer, timer->info);
1052
         else
1053
         {
1054
            //Send message
1055
            message[0] = MESSAGE_TYPE_TIMER;
1056
            message[1] = (uint32)timer;
1057
            message[2] = timer->info;
1058
            OS_MQueueSend(timer->mqueue, message);
1059
         }
1060
      }
1061
   }
1062
}
1063
 
1064
 
1065
/******************************************/
1066
//Create a timer that will send a message upon timeout
1067
OS_Timer_t *OS_TimerCreate(const char *name, OS_MQueue_t *mQueue, uint32 info)
1068
{
1069
   OS_Timer_t *timer;
1070
 
1071
   OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
1072
   if(SemaphoreTimer == NULL)
1073
   {
1074
      SemaphoreTimer = OS_SemaphoreCreate("Timer", 0);
1075
      OS_ThreadCreate("Timer", OS_TimerThread, NULL, 250, 2000);
1076
   }
1077
   OS_SemaphorePost(SemaphoreLock);
1078
 
1079
   timer = (OS_Timer_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_Timer_t));
1080
   if(timer == NULL)
1081
      return NULL;
1082
   timer->name = name;
1083
   timer->callback = NULL;
1084
   timer->mqueue = mQueue;
1085
   timer->next = NULL;
1086
   timer->prev = NULL;
1087
   timer->info = info;
1088
   timer->active = 0;
1089
   return timer;
1090
}
1091
 
1092
 
1093
/******************************************/
1094
void OS_TimerDelete(OS_Timer_t *timer)
1095
{
1096
   OS_TimerStop(timer);
1097
   OS_HeapFree(timer);
1098
}
1099
 
1100
 
1101
/******************************************/
1102
void OS_TimerCallback(OS_Timer_t *timer, OS_TimerFuncPtr_t callback)
1103
{
1104
   timer->callback = callback;
1105
}
1106
 
1107
 
1108
/******************************************/
1109
//Must not be called from an ISR
1110
//In ~10 msec ticks send a message (or callback)
1111
void OS_TimerStart(OS_Timer_t *timer, uint32 ticks, uint32 ticksRestart)
1112
{
1113
   OS_Timer_t *node, *prev;
1114
   int diff, check=0;
1115
 
1116
   assert(timer);
1117
   assert(InterruptInside[OS_CpuIndex()] == 0);
1118
   ticks += OS_ThreadTime();
1119
   if(timer->active)
1120
      OS_TimerStop(timer);
1121
   OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
1122
   if(timer->active)
1123
   {
1124
      //Prevent race condition
1125
      OS_SemaphorePost(SemaphoreLock);
1126
      return;
1127
   }
1128
   timer->ticksTimeout = ticks;
1129
   timer->ticksRestart = ticksRestart;
1130
   timer->active = 1;
1131
   prev = NULL;
1132
   for(node = TimerHead; node; node = node->next)
1133
   {
1134
      diff = ticks - node->ticksTimeout;
1135
      if(diff <= 0)
1136
         break;
1137
      prev = node;
1138
   }
1139
   timer->next = node;
1140
   timer->prev = prev;
1141
   if(node)
1142
      node->prev = timer;
1143
   if(prev == NULL)
1144
   {
1145
      TimerHead = timer;
1146
      check = 1;
1147
   }
1148
   else
1149
      prev->next = timer;
1150
   OS_SemaphorePost(SemaphoreLock);
1151
   if(check)
1152
      OS_SemaphorePost(SemaphoreTimer);  //Wakeup OS_TimerThread
1153
}
1154
 
1155
 
1156
/******************************************/
1157
//Must not be called from an ISR
1158
void OS_TimerStop(OS_Timer_t *timer)
1159
{
1160
   assert(timer);
1161
   assert(InterruptInside[OS_CpuIndex()] == 0);
1162
   OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
1163
   if(timer->active)
1164
   {
1165
      timer->active = 0;
1166
      if(timer->prev == NULL)
1167
         TimerHead = timer->next;
1168
      else
1169
         timer->prev->next = timer->next;
1170
      if(timer->next)
1171
         timer->next->prev = timer->prev;
1172
   }
1173
   OS_SemaphorePost(SemaphoreLock);
1174
}
1175
 
1176
 
1177
/***************** ISR ********************/
1178
/******************************************/
1179
void OS_InterruptServiceRoutine(uint32 status, uint32 *stack)
1180
{
1181
   int i;
1182
   uint32 state, cpuIndex = OS_CpuIndex();
1183
 
1184
   if(status == 0 && Isr[31])
1185
      Isr[31](stack);                   //SYSCALL or BREAK
1186
 
1187
   InterruptInside[cpuIndex] = 1;
1188
   i = 0;
1189
   do
1190
   {
1191
      if(status & 1)
1192
      {
1193
         if(Isr[i])
1194
            Isr[i](stack);
1195
         else
1196
            OS_InterruptMaskClear(1 << i);
1197
      }
1198
      status >>= 1;
1199
      ++i;
1200
   } while(status);
1201
   InterruptInside[cpuIndex] = 0;
1202
 
1203
   state = OS_SpinLock();
1204
   if(ThreadNeedReschedule[cpuIndex])
1205
      OS_ThreadReschedule(ThreadNeedReschedule[cpuIndex] & 1);
1206
   OS_SpinUnlock(state);
1207
}
1208
 
1209
 
1210
/******************************************/
1211
void OS_InterruptRegister(uint32 mask, OS_FuncPtr_t funcPtr)
1212
{
1213
   int i;
1214
 
1215
   for(i = 0; i < 32; ++i)
1216
   {
1217
      if(mask & (1 << i))
1218
         Isr[i] = funcPtr;
1219
   }
1220
}
1221
 
1222
 
1223
/******************************************/
1224
//Plasma hardware dependent
1225
uint32 OS_InterruptStatus(void)
1226
{
1227 402 rhoads
   return MemoryRead(IRQ_STATUS) & MemoryRead(IRQ_MASK);
1228 400 rhoads
}
1229
 
1230
 
1231
/******************************************/
1232
//Plasma hardware dependent
1233
uint32 OS_InterruptMaskSet(uint32 mask)
1234
{
1235
   uint32 state;
1236
   state = OS_CriticalBegin();
1237
   mask |= MemoryRead(IRQ_MASK);
1238
   MemoryWrite(IRQ_MASK, mask);
1239
   OS_CriticalEnd(state);
1240
   return mask;
1241
}
1242
 
1243
 
1244
/******************************************/
1245
//Plasma hardware dependent
1246
uint32 OS_InterruptMaskClear(uint32 mask)
1247
{
1248
   uint32 state;
1249
   state = OS_CriticalBegin();
1250
   mask = MemoryRead(IRQ_MASK) & ~mask;
1251
   MemoryWrite(IRQ_MASK, mask);
1252
   OS_CriticalEnd(state);
1253
   return mask;
1254
}
1255
 
1256
 
1257
/**************** Init ********************/
1258
/******************************************/
1259
//If there aren't any other ready to run threads then spin here
1260 402 rhoads
static int SimulateIsr;
1261 400 rhoads
static void OS_IdleThread(void *arg)
1262
{
1263 407 rhoads
   uint32 IdleCount=0;
1264 400 rhoads
   (void)arg;
1265
 
1266
   //Don't block in the idle thread!
1267
   for(;;)
1268
   {
1269
      ++IdleCount;
1270
#ifndef DISABLE_IRQ_SIM
1271 402 rhoads
      MemoryRead(IRQ_MASK + 4);  //will call Sleep
1272
      if(SimulateIsr && (int)arg == OS_CPU_COUNT-1)
1273
      {
1274
         unsigned int value = IRQ_COUNTER18;   //tick interrupt
1275
         for(;;)
1276
         {
1277
            value |= OS_InterruptStatus();
1278
            if(value == 0)
1279
               break;
1280
            OS_InterruptServiceRoutine(value, 0);
1281
            value = 0;
1282
         }
1283
      }
1284 400 rhoads
#endif
1285 402 rhoads
#if OS_CPU_COUNT > 1
1286
      if((int)arg < OS_CPU_COUNT - 1)
1287
      {
1288
         unsigned int state;
1289
#ifndef WIN32
1290 407 rhoads
         for(IdleCount = 0; IdleCount < 25000; ++IdleCount) ;
1291 402 rhoads
#endif
1292
         state = OS_SpinLock();
1293
         OS_ThreadReschedule(1);
1294
         OS_SpinUnlock(state);
1295
      }
1296
#endif
1297 400 rhoads
   }
1298
}
1299
 
1300
 
1301
/******************************************/
1302
//Plasma hardware dependent
1303
//ISR called every ~10 msecs when bit 18 of the counter register toggles
1304 402 rhoads
static void OS_InterruptTick(void *arg)
1305 400 rhoads
{
1306 402 rhoads
   uint32 state, mask;
1307 400 rhoads
 
1308
   //Toggle looking for IRQ_COUNTER18 or IRQ_COUNTER18_NOT
1309
   state = OS_SpinLock();
1310 402 rhoads
   mask = MemoryRead(IRQ_MASK);
1311
   mask ^= IRQ_COUNTER18 | IRQ_COUNTER18_NOT;
1312 400 rhoads
   MemoryWrite(IRQ_MASK, mask);
1313
   OS_ThreadTick(arg);
1314
   OS_SpinUnlock(state);
1315
}
1316
 
1317
 
1318
/******************************************/
1319
//Initialize the OS by setting up the system heap and the tick interrupt
1320
void OS_Init(uint32 *heapStorage, uint32 bytes)
1321
{
1322
   int i;
1323 402 rhoads
 
1324 400 rhoads
   if((int)OS_Init > 0x10000000)        //Running from DDR?
1325
      OS_AsmInterruptInit();            //Patch interrupt vector
1326
   OS_InterruptMaskClear(0xffffffff);   //Disable interrupts
1327 407 rhoads
   HeapArray[0] = OS_HeapCreate("Heap", heapStorage, bytes);
1328 400 rhoads
   HeapArray[1] = HeapArray[0];
1329 416 rhoads
#ifndef WIN32
1330
   HeapArray[7] = OS_HeapCreate("Alt", (uint8*)RAM_EXTERNAL_BASE +
1331
      RAM_EXTERNAL_SIZE*2, 1024*1024*60);
1332
   OS_HeapAlternate(HeapArray[0], HeapArray[7]);
1333
#endif
1334 400 rhoads
   SemaphoreSleep = OS_SemaphoreCreate("Sleep", 0);
1335
   SemaphoreRelease = OS_SemaphoreCreate("Release", 1);
1336
   SemaphoreLock = OS_SemaphoreCreate("Lock", 1);
1337 402 rhoads
   if((MemoryRead(IRQ_STATUS) & (IRQ_COUNTER18 | IRQ_COUNTER18_NOT)) == 0)
1338 400 rhoads
   {
1339 402 rhoads
      //Detected running in simulator
1340 400 rhoads
      UartPrintfCritical("SimIsr\n");
1341 402 rhoads
      SimulateIsr = 1;
1342 400 rhoads
   }
1343 402 rhoads
   for(i = 0; i < OS_CPU_COUNT; ++i)
1344
      OS_ThreadCreate("Idle", OS_IdleThread, (void*)i, i, 256);
1345 400 rhoads
   //Plasma hardware dependent (register for OS tick interrupt every ~10 msecs)
1346 402 rhoads
   OS_InterruptRegister(IRQ_COUNTER18 | IRQ_COUNTER18_NOT, OS_InterruptTick);
1347
   OS_InterruptMaskSet(IRQ_COUNTER18);
1348 400 rhoads
}
1349
 
1350
 
1351
/******************************************/
1352
//Start thread swapping
1353
void OS_Start(void)
1354
{
1355 402 rhoads
#if defined(WIN32) && OS_CPU_COUNT > 1
1356
   jmp_buf env;
1357
   OS_ThreadRegsInit(env);
1358
   Registration[OS_CpuIndex()] = ((jmp_buf2*)env)->extra[0];
1359
#endif
1360 400 rhoads
   ThreadSwapEnabled = 1;
1361
   (void)OS_SpinLock();
1362
   OS_ThreadReschedule(1);
1363
}
1364
 
1365
 
1366
/******************************************/
1367
//Place breakpoint here
1368
void OS_Assert(void)
1369
{
1370
}
1371
 
1372
 
1373
/**************** Example *****************/
1374
#ifndef NO_MAIN
1375
#ifdef WIN32
1376
static uint8 HeapSpace[1024*512];  //For simulation on a PC
1377
#endif
1378
 
1379
int main(int programEnd, char *argv[])
1380
{
1381
   (void)programEnd;  //Pointer to end of used memory
1382
   (void)argv;
1383
 
1384
   UartPrintfCritical("Starting RTOS\n");
1385 416 rhoads
   MemoryWrite(IRQ_MASK, 0);
1386 400 rhoads
#ifdef WIN32
1387
   OS_Init((uint32*)HeapSpace, sizeof(HeapSpace));  //For PC simulation
1388
#else
1389
   //Create heap in remaining space after program in 1MB external RAM
1390
   OS_Init((uint32*)programEnd,
1391
           RAM_EXTERNAL_BASE + RAM_EXTERNAL_SIZE - programEnd);
1392
#endif
1393
   UartInit();
1394
   OS_ThreadCreate("Main", MainThread, NULL, 100, 4000);
1395 402 rhoads
#if defined(WIN32) && OS_CPU_COUNT > 1
1396
   OS_InitSimulation();
1397
#endif
1398 400 rhoads
   OS_Start();
1399
   return 0;
1400
}
1401
#endif  //NO_MAIN
1402
 

powered by: WebSVN 2.1.0

© copyright 1999-2024 OpenCores.org, equivalent to Oliscience, all rights reserved. OpenCores®, registered trademark.