+/rtos
git clone https://git.julia.blue/+/rtos
413cd76
master
1#include "OS.h" 2#include "ADC.h" 3#include "ST7735.h" 4#include "eDisk.h" 5#include "esp8266.h" 6#include "heap.h" 7#include "interpreter.h" 8#include "interrupts.h" 9#include "io.h" 10#include "launchpad.h" 11#include "littlefs.h" 12#include "loader.h" 13#include "printf.h" 14#include "std.h" 15#include "timer.h" 16#include "tivaware/gpio.h" 17#include "tivaware/hw_gpio.h" 18#include "tivaware/hw_ints.h" 19#include "tivaware/hw_memmap.h" 20#include "tivaware/hw_nvic.h" 21#include "tivaware/hw_timer.h" 22#include "tivaware/hw_types.h" 23#include "tivaware/mpu.h" 24#include "tivaware/rom.h" 25#include "tivaware/timer.h" 26#include <stdint.h> 27#include <stdnoreturn.h> 28 29typedef struct { 30uint32_t * text ; 31uint32_t * data ; 32uint16_t pid ; 33uint8_t threads ; 34bool alive ; 35}PCB ; 36 37typedef struct TCB { 38uint32_t * sp ; 39struct TCB * next_tcb ; 40struct TCB * prev_tcb ; 41PCB * parent_process ; 42 43uint32_t id ; 44const char * name ; 45OutputDevice out_device ; 46 47struct TCB * next_blocked ; 48 49uint32_t sleep_time ; 50 51bool asleep ; 52bool blocked ; 53bool alive ; 54uint8_t priority ; 55 56uint32_t * stack ; 57}TCB ; 58 59#define MAX_THREADS 8 60#define MAX_PROCESSES 4 61#define MIN_STACK_SIZE 512 62 63static TCB threads [MAX_THREADS ]; 64static uint8_t thread_count = 0 ; 65 66static PCB processes [MAX_PROCESSES ]; 67static uint8_t process_count = 0 ; 68 69extern uint32_t _eheap ; 70static TCB idle = { 71 .next_tcb = & idle , 72 .prev_tcb = & idle , 73 .id = 0 , 74 .alive = true, 75 .priority = 255 , 76 .name = "OS Idle" , 77 .parent_process = 0 , 78 .stack = (uint32_t * )& _eheap , 79}; 80 81volatile TCB * current_thread = & idle ; 82bool os_running ; 83 84static noreturnvoid idle_task (void ) { 85os_running = true; 86enable_interrupts (); 87while (true) {wait_for_interrupts (); } 88} 89 90static void insert_behind (TCB * a ,TCB * b ) { 91a -> next_tcb = b ; 92a -> prev_tcb = b -> prev_tcb ; 93a -> prev_tcb -> next_tcb = a ; 94b -> prev_tcb = a ; 95} 96 97static void insert_thread (TCB * adding ) { 98uint32_t crit = start_critical (); 99if (adding -> priority < current_thread -> priority ) { 100TCB * next = current_thread -> next_tcb ; 101if (adding -> priority < next -> priority ) { 102adding -> next_tcb = adding -> prev_tcb = adding ; 103current_thread -> next_tcb = adding ; 104OS_Suspend (); 105 }else if (next -> priority == adding -> priority ) { 106insert_behind (adding ,next ); 107 } 108 }else if (adding -> priority == current_thread -> priority ) { 109insert_behind (adding , 110 (current_thread -> blocked || current_thread -> asleep ) 111 ?current_thread -> next_tcb 112 : (TCB * )current_thread ); 113 } 114end_critical (crit ); 115} 116 117// only called from sleep/kill/suspend so no additional critical section needed 118static void remove_current_thread () { 119if (current_thread -> next_tcb == current_thread ) { 120uint8_t my_count = thread_count ; 121TCB * new_current = & idle ; 122idle .next_tcb = & idle ; 123for (int i = 0 ;my_count > 0 ;++ i ) { 124if (!threads [i ].alive ) { 125continue ; 126 } 127-- my_count ; 128if (!(threads [i ].asleep || threads [i ].blocked )) { 129if (threads [i ].priority < new_current -> priority ) { 130new_current = & threads [i ]; 131new_current -> next_tcb = new_current -> prev_tcb = new_current ; 132 }else if (threads [i ].priority == new_current -> priority ) { 133insert_behind (& threads [i ],new_current ); 134 } 135 } 136 } 137current_thread -> next_tcb = new_current ; 138 }else { 139current_thread -> prev_tcb -> next_tcb = current_thread -> next_tcb ; 140current_thread -> next_tcb -> prev_tcb = current_thread -> prev_tcb ; 141 } 142OS_Suspend (); 143} 144 145static void portd_init (void ) { 146ROM_SysCtlPeripheralEnable (SYSCTL_PERIPH_GPIOD ); 147ROM_GPIOPinTypeGPIOOutput (GPIO_PORTD_BASE ,0x0F ); 148} 149 150void OS_Init (void ) { 151disable_interrupts (); 152os_running = false; 153heap_init (); 154launchpad_init (); 155enable_button_interupts (3 ); 156portd_init (); 157uart_init (); 158temperature_init (); 159SSI0_Init (10 ); 160} 161 162void OS_InitSemaphore (Sema4 * sem ,int32_t value ) { 163sem -> value = value ; 164sem -> blocked_head = 0 ; 165} 166 167void OS_Wait (Sema4 * sem ) { 168uint32_t crit = start_critical (); 169current_thread -> next_blocked = 0 ; 170if (sem -> value -- >=0 ) { 171end_critical (crit ); 172return ; 173 } 174current_thread -> blocked = true; 175if (!sem -> blocked_head ) { 176sem -> blocked_head = (TCB * )current_thread ; 177 }else { 178TCB * tail = sem -> blocked_head ; 179while (tail -> next_blocked && 180tail -> next_blocked -> priority <=current_thread -> priority ) { 181tail = tail -> next_blocked ; 182 } 183current_thread -> next_blocked = tail -> next_blocked ; 184tail -> next_blocked = (TCB * )current_thread ; 185 } 186remove_current_thread (); 187end_critical (crit ); 188} 189 190void OS_Signal (Sema4 * sem ) { 191uint32_t crit = start_critical (); 192if (++ sem -> value >=0 ) { 193end_critical (crit ); 194return ; 195 } 196sem -> blocked_head -> blocked = false; 197insert_thread (sem -> blocked_head ); 198sem -> blocked_head = sem -> blocked_head -> next_blocked ; 199end_critical (crit ); 200} 201 202static uint32_t thread_uuid = 1 ; 203bool OS_AddThread (void (* task )(void ),const char * name ,uint32_t stack_size , 204uint32_t priority ) { 205uint32_t crit = start_critical (); 206if (thread_count >=MAX_THREADS - 1 ) { 207end_critical (crit ); 208return false; 209 } 210++ thread_count ; 211uint8_t thread_index = 0 ; 212while (threads [thread_index ].alive ) {thread_index ++ ; } 213TCB * adding = & threads [thread_index ]; 214 215if ((adding -> parent_process = current_thread -> parent_process )) { 216++ adding -> parent_process -> threads ; 217 } 218adding -> alive = true; 219adding -> asleep = false; 220adding -> sleep_time = 0 ; 221adding -> priority = priority ; 222adding -> id = thread_uuid ++ ; 223adding -> name = name ; 224adding -> next_tcb = adding -> prev_tcb = & idle ; 225adding -> out_device = UART ; 226 227// initialize stack 228stack_size = max (stack_size ,MIN_STACK_SIZE )+ 32 ;// extra is for MPU 229adding -> stack = malloc (stack_size ); 230if (!adding -> stack ) { 231return false; 232 } 233adding -> sp = & adding -> stack [stack_size /4 - 1 ]; 234// TODO: check if 8 byte stack alignment matters 235adding -> sp = (uint32_t * )((uint8_t * )adding -> sp - (uint32_t )adding -> sp %8 ); 236adding -> sp -= 18 ;// Space for floating point registers 237* (-- adding -> sp )= 0x21000000 ;// PSR 238* (-- adding -> sp )= (uint32_t )task ;// PC 239* (-- adding -> sp )= (uint32_t )OS_Kill ;// LR 240if (adding -> parent_process ) { 241// R9 is the static base 242* (adding -> sp - 8 )= (uint32_t )adding -> parent_process -> data ; 243 } 244adding -> sp -= 13 ;// Space for R0-R12 245 246insert_thread (adding ); 247end_critical (crit ); 248return true; 249} 250 251bool OS_AddProcess (void (* entry )(void ),void * text ,void * data , 252uint32_t stack_size ,uint32_t priority ) { 253uint32_t crit = start_critical (); 254if (process_count == MAX_PROCESSES ) { 255return false; 256 } 257++ process_count ; 258uint8_t add_idx = 0 ; 259for (int i = 0 ;i < MAX_PROCESSES ;++ i ) { 260if (!processes [i ].alive ) { 261add_idx = i ; 262break ; 263 } 264 } 265processes [add_idx ].text = text ; 266processes [add_idx ].data = data ; 267processes [add_idx ].alive = true; 268PCB * saved_parent = current_thread -> parent_process ; 269current_thread -> parent_process = & processes [add_idx ]; 270OS_AddThread (entry ,"Process entry" ,stack_size ,priority ); 271current_thread -> parent_process = saved_parent ; 272end_critical (crit ); 273return true; 274} 275 276uint32_t OS_Id (void ) { 277return current_thread -> id ; 278} 279 280// performance measurments for periodic tasks 281#ifdef TRACK_JITTER 282static int32_t max_jitter ; 283static uint32_t jitter_histogram [128 ]= {0 }; 284#endif 285 286typedef struct ptask { 287void (* task )(void ); 288struct ptask * next ; 289uint32_t reload ; 290uint32_t current ; 291uint32_t last ; 292uint8_t priority ; 293}PTask ; 294 295#define MAX_PTASKS 4 296static PTask ptasks [MAX_PTASKS ]; 297static uint8_t num_ptasks ; 298static PTask * current_ptask ; 299 300static void setup_next_ptask (uint32_t lag ); 301 302static void ptask_insert (PTask * task ) { 303if (!current_ptask ) { 304current_ptask = task ; 305task -> next = 0 ; 306return ; 307 } 308if (task -> priority < current_ptask -> priority ) { 309task -> next = current_ptask ; 310current_ptask = task ; 311 } 312PTask * temp = current_ptask ; 313while (temp -> next && task -> priority > temp -> next -> priority ) { 314temp = temp -> next ; 315 } 316task -> next = temp -> next ; 317temp -> next = task ; 318} 319 320static void periodic_task (void ) { 321uint32_t time = OS_Time (); 322do { 323uint32_t current = OS_Time (); 324#ifdef TRACK_JITTER 325uint32_t jitter = to_us ( 326difference (current - current_ptask -> last ,current_ptask -> reload )); 327max_jitter = max (max_jitter ,jitter ); 328uint8_t idx = min ( 329sizeof (jitter_histogram ) /sizeof (jitter_histogram [0 ])- 1 ,jitter ); 330++ jitter_histogram [idx ]; 331#endif 332current_ptask -> last = current ; 333current_ptask -> task (); 334 }while ((current_ptask = current_ptask -> next )); 335setup_next_ptask (OS_Time ()- time ); 336} 337 338static void setup_next_ptask (uint32_t lag ) { 339int32_t min_time = INT32_MAX ; 340for (int i = 0 ;i < num_ptasks ;++ i ) { 341min_time = min (min_time ,ptasks [i ].current ); 342 } 343uint32_t threshold = max (min_time ,lag ); 344for (int i = 0 ;i < num_ptasks ;++ i ) { 345if (ptasks [i ].current <=threshold ) { 346ptask_insert (& ptasks [i ]); 347ptasks [i ].current = 348max (0 ,ptasks [i ].reload - (threshold - ptasks [i ].current )); 349 }else { 350ptasks [i ].current -= threshold ; 351 } 352 } 353timer_enable (2 ,max (min_time - lag ,1 ),periodic_task ,1 , false); 354} 355 356bool OS_AddPeriodicThread (void (* task )(void ),uint32_t period , 357uint32_t priority ) { 358if (num_ptasks >=MAX_PTASKS - 1 ) { 359return false; 360 } 361PTask * current = & ptasks [num_ptasks ++ ]; 362current -> priority = priority ; 363current -> task = task ; 364current -> last = os_running ?OS_Time () :0 ; 365current -> current = current -> reload = period ; 366// if the first periodic task is added after the OS starts, the oneshot 367// timer isn't running yet 368if (num_ptasks == 1 && os_running ) { 369setup_next_ptask (0 ); 370 } 371return true; 372} 373 374static void (* sw1task )(void ); 375static void (* sw2task )(void ); 376 377const uint32_t debounce_ms = 20 ; 378static uint32_t last_sw1 ; 379static uint32_t last_sw2 ; 380 381void gpio_portf_handler (void ) { 382uint32_t now = to_ms (OS_Time ()); 383if (HWREG (GPIO_PORTF_BASE + GPIO_O_RIS )& 0x01 ) { 384if (sw1task && now - last_sw1 > debounce_ms ) { 385sw1task (); 386 } 387last_sw1 = now ; 388 } 389if (HWREG (GPIO_PORTF_BASE + GPIO_O_RIS )& 0x10 ) { 390if (sw2task && now - last_sw2 > debounce_ms ) { 391sw2task (); 392 } 393last_sw2 = now ; 394 } 395HWREG (GPIO_PORTF_BASE + GPIO_O_ICR )= 0x11 ; 396} 397 398void OS_AddSW1Task (void (* task )(void )) { 399sw1task = task ; 400} 401 402void OS_AddSW2Task (void (* task )(void )) { 403sw2task = task ; 404} 405 406// TODO: structure this the same as periodic tasks to incur minimum overhead 407static void sleep_task (void ) { 408uint32_t reload = get_timer_reload (1 ); 409for (int i = 0 ;i < MAX_THREADS ;i ++ ) { 410if (threads [i ].asleep ) { 411if (threads [i ].sleep_time <=reload ) { 412threads [i ].sleep_time = 0 ; 413threads [i ].asleep = false; 414insert_thread (& threads [i ]); 415 }else { 416threads [i ].sleep_time -= reload ; 417 } 418 } 419 } 420} 421 422void OS_Sleep (uint32_t time ) { 423uint32_t crit = start_critical (); 424current_thread -> asleep = true; 425current_thread -> sleep_time = 426time + get_timer_reload (1 )- get_timer_value (1 ); 427remove_current_thread (); 428end_critical (crit ); 429} 430 431void OS_Kill (void ) { 432uint32_t crit = start_critical (); 433-- thread_count ; 434current_thread -> alive = false; 435free (current_thread -> stack ); 436remove_current_thread (); 437// Handle process cleanup if needed 438if (current_thread -> parent_process ) { 439if (!-- current_thread -> parent_process -> threads ) { 440-- process_count ; 441current_thread -> parent_process -> alive = false; 442free (current_thread -> parent_process -> data ); 443free (current_thread -> parent_process -> text ); 444 } 445 } 446end_critical (crit ); 447} 448 449// Called from within the context switch to change which stack the MPU protects 450void mpu_swap_region (void ) { 451uint32_t temp = (uint32_t )current_thread -> stack ; 452// 32 byte alignment required for MPU regions 453if (temp %32 ) { 454temp += 32 - temp %32 ; 455 } 456ROM_MPURegionSet (0 ,temp , 457MPU_RGN_SIZE_32B |MPU_RGN_PERM_NOEXEC | 458MPU_RGN_PERM_PRV_NO_USR_NO |MPU_RGN_ENABLE ); 459} 460 461void OS_Suspend (void ) { 462ROM_IntPendSet (FAULT_PENDSV ); 463} 464 465void OS_ClearTime (void ) { 466ROM_SysCtlPeripheralEnable (SYSCTL_PERIPH_WTIMER5 ); 467ROM_TimerConfigure (WTIMER5_BASE ,TIMER_CFG_PERIODIC_UP ); 468ROM_TimerControlStall (WTIMER5_BASE ,TIMER_A , true); 469ROM_TimerEnable (WTIMER5_BASE ,TIMER_A ); 470HWREG (WTIMER5_BASE + TIMER_O_TAV )= 0 ; 471} 472 473uint32_t OS_Time (void ) { 474return ROM_TimerValueGet (WTIMER5_BASE ,TIMER_A ); 475} 476 477noreturnvoid OS_Launch (uint32_t time_slice ) { 478ROM_MPUEnable (MPU_CONFIG_PRIV_DEFAULT ); 479ROM_IntEnable (FAULT_MPU ); 480ROM_IntPrioritySet (FAULT_PENDSV ,0xff );// priority is high 3 bits 481ROM_IntPrioritySet (FAULT_SYSTICK ,6 <<5 ); 482ROM_SysTickPeriodSet (time_slice ); 483ROM_SysTickIntEnable (); 484ROM_SysTickEnable (); 485timer_enable (1 ,ms (1 ),& sleep_task ,3 , true); 486if (num_ptasks ) { 487setup_next_ptask (0 ); 488 } 489OS_ClearTime (); 490idle_task (); 491} 492 493void systick_handler () { 494ROM_IntPendSet (FAULT_PENDSV ); 495} 496 497void OS_ReportJitter (void ) { 498#ifdef TRACK_JITTER 499printf ("Max Jitter: %d microseconds\n\r" ,max_jitter ); 500uint32_t most = 0 ,most_idx ; 501uint32_t sum = 0 ; 502uint32_t total_num = 0 ; 503for (int i = 0 ;i < sizeof (jitter_histogram ) /sizeof (jitter_histogram [0 ]); 504++ i ) { 505uint32_t num = jitter_histogram [i ]; 506sum += i * num ; 507total_num += num ; 508if (num > most ) { 509most = num ; 510most_idx = i ; 511 } 512 } 513printf ("Modal Jitter: %d microseconds\n\r" ,most_idx ); 514printf ("Average Jitter: %d microseconds\n\r" ,sum /total_num ); 515#else 516puts ("Jitter tracking not enabled..." ); 517#endif 518} 519 520// TODO: change the stack pointer so that using the stack in this handler 521// doesn't overwrite the HeapNode for the stack that just overflowed 522void memory_management_fault_handler (void ) { 523HeapNode * stack_allocation = heap_node_from_alloc (current_thread -> stack ); 524uint16_t stack_size = stack_allocation -> size - 32 ; 525OS_RedirectOutput (UART ); 526printf ("\n\n\rSTACK OVERFLOW\n\rThread '%s' overflowed its %d byte stack, " 527"Consider increasing it.\n\r" , 528current_thread -> name ,stack_size ); 529printf ("FAULTSTAT: 0x%08x\n\r" ,HWREG (NVIC_FAULT_STAT )); 530printf ("Address accessed: 0x%08x\n\r" ,HWREG (NVIC_MM_ADDR )); 531while (true) { 532busy_wait (7 ,ms (500 )); 533led_toggle (RED_LED |BLUE_LED ); 534 } 535} 536 537void hardfault_handler (void ) { 538OS_RedirectOutput (UART ); 539printf ("\n\n\rHARD FAULT in thread '%s'\n\r" ,current_thread -> name ); 540printf ("FAULTSTAT: 0x%08x\n\r" ,HWREG (NVIC_FAULT_STAT )); 541printf ("HFAULTSTAT: 0x%08x\n\r" ,HWREG (NVIC_HFAULT_STAT )); 542while (true) { 543busy_wait (7 ,ms (500 )); 544led_toggle (RED_LED ); 545 } 546} 547 548// Exported symbol for dynamic function loading 549typedef struct { 550const char * name ; 551void * ptr ; 552}Symbol ; 553 554static const Symbol symtab []= {{"OS_Id" , (void * )OS_Id }, 555 {"printf" , (void * )printf }}; 556 557void * OS_function_lookup (const char * name ) { 558for (int i = 0 ;i < sizeof (symtab ) /sizeof (Symbol );++ i ) { 559if (streq (symtab [i ].name ,name )) { 560return symtab [i ].ptr ; 561 } 562 } 563return 0 ; 564} 565 566void OS_LoadProgram (char * name ) { 567printf ("Loading: '%s'...\n\r" ,name ); 568if (!exec_elf (name )) { 569puts ("load program error" ); 570 } 571} 572 573void OS_RedirectOutput (OutputDevice device ) { 574current_thread -> out_device = device ; 575} 576 577void OS_RedirectString (const char * str ) { 578if (current_thread -> out_device & UART ) { 579uart_puts (str ); 580 } 581if (current_thread -> out_device & ESP ) { 582ESP8266_Send (str ); 583ESP8266_Send ("\n\r" ); 584 } 585if (current_thread -> out_device & FS ) { 586// TODO: write to open file 587 } 588if (current_thread -> out_device & SCREEN ) { 589lcd_puts (str ); 590 } 591} 592 593void OS_RedirectChar (char c ) { 594char buf [2 ]= {c ,'\0' }; 595if (current_thread -> out_device & UART ) { 596uart_putchar (c ); 597 } 598if (current_thread -> out_device & ESP ) { 599ESP8266_Send (buf ); 600 } 601if (current_thread -> out_device & FS ) { 602// TODO: write to open file 603 } 604if (current_thread -> out_device & SCREEN ) { 605lcd_putchar (c ); 606 } 607}