Root/plasma/kernel/rtos.c

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

Archive Download this file

Branches:
master



interactive