scheduler.cpp tinykernel/kernel.hpp cstring pico/platform.h pico/multicore.h hardware/sync.h port/m33/port_m33.h tinykernel/kernel.hpp port/m33/port_m33.h C:/Users/yoneda/Documents/Pico_Projects/tinykernel/tinykernel/src/scheduler.cpp cstring hardware/structs/scb.h hardware/sync.h pico/multicore.h pico/platform.h pico/runtime.h stddef.h stdint.h FpuCtx tk tk::detail FpuCtx FpuCtx tk_fpu_scratch[TK_NUM_CORES] [TK_NUM_CORES] tk_fpu_scratch = {} void void isr_systick (void) isr_systick void uintptr_t uintptr_t tk_switch_context (uintptr_t cur_sp) tk_switch_context uintptr_t cur_sp void static void core_start_routine (uint32_t core) core_start_routine uint32_t core void static void core1_entry (void) core1_entry void void void tk_task_return_trampoline (void) tk_task_return_trampoline void #include"tinykernel/kernel.hpp" #include<cstring> #include"pico/platform.h" #include"pico/multicore.h" #include"hardware/sync.h" #include"port/m33/port_m33.h" structFpuCtx{ floats[32]; }; static_assert(sizeof(FpuCtx)==128,"FPUcontextlayoutmustmatchcontext.S"); namespace{ constexpruint32_tTK_NUM_CORES=2; enumclassState:uint8_t{Free=0,Ready,Running,Sleeping,Blocked,Terminated}; structTaskSlot{ Statestate; tk::TaskFnfn; void*arg; constchar*name; uint32_tprio;//0..15,lowerishigherpriority uintptr_tsp;//savedSP(pointsatthecore8frame);0=notstartedyet int64_twake_tick; int32_tslice_left; boolis_idle; FpuCtxfpu; }; static_assert(TK_MAX_TASKS<=255,"slotindexmustfitinabyte"); alignas(16)staticTaskSlotg_slots[TK_MAX_TASKS];//slot0=idle0,slot1=idle1 alignas(8)staticcharg_stacks[TK_MAX_TASKS][TK_DEFAULT_STACK_SIZE]; staticuint8_tg_cur[TK_NUM_CORES]={0,1}; staticvolatileuint32_tg_preempt_disabled[TK_NUM_CORES]={0,0}; staticint64_tg_tick=0; staticboolg_started=false; staticuint8_tg_rr_last[16]={0}; staticspin_lock_t*g_sched_lock=nullptr; staticinlinevoidensure_sched_lock(void){ if(!g_sched_lock){ intlock_num=spin_lock_claim_unused(true); g_sched_lock=spin_lock_instance(lock_num); } } constexpruint32_tkIdlePrio=15u; constexprint32_tkSliceTicks=(int32_t)(((uint64_t)TK_TIME_SLICE_MS*TK_TICKS_PER_SEC+999u)/1000u); static_assert(kSliceTicks>=1,"timeslicemustbeatleastonetick"); staticinlineuint32_tsched_lock(void){ ensure_sched_lock(); returnspin_lock_blocking(g_sched_lock); } staticinlinevoidsched_unlock(uint32_tsave){ spin_unlock(g_sched_lock,save); } staticvoididle_fn(void*arg){ (void)arg; for(;;){ __asm__volatile("wfi"); } } staticinlineboolis_runnable(uint32_tslot_idx,uint32_tcore){ constTaskSlot&s=g_slots[slot_idx]; if(s.is_idle){ returnslot_idx==core;//Core0onlyrunsidle0,Core1onlyrunsidle1 } returns.state==State::Ready; } //Highestprioritylevelwithatleastonerunnabletaskforthiscore. staticint32_tfind_prio_level(uint32_tcore){ for(uint32_tp=0;p<16u;++p){ for(uint32_ti=0;i<TK_MAX_TASKS;++i){ if(g_slots[i].prio==p&&is_runnable(i,core))return(int32_t)p; } } return(int32_t)kIdlePrio; } staticuint32_tpick_round_robin(uint32_tlevel,uint32_tcore){ if(level==kIdlePrio)returncore; for(uint32_tk=1;k<=TK_MAX_TASKS;++k){ uint32_ti=(g_rr_last[level]+k)%TK_MAX_TASKS; if(i>=TK_NUM_CORES&&g_slots[i].prio==level&&is_runnable(i,core))returni; } returncore; } //Choosethenexttaskforthegivencore. staticuint32_tselect_next(uint32_tcore){ int32_tlevel=find_prio_level(core); if(level<0)returncore; returnpick_round_robin((uint32_t)level,core); } }//namespace //Referenced(unmangled)fromcontext.S:scratchareaper-coreforFPUstatetransfer. extern"C"{ alignas(8)FpuCtxtk_fpu_scratch[TK_NUM_CORES]={}; } //SysTickinterrupthandler:advancesticksonCore0,wakessleepingtasks,andcheckstimeslices. extern"C"voidisr_systick(void){ uint32_tcore=get_core_num(); uint32_tsave=sched_lock(); boolneed_switch=false; if(core==0){ ++g_tick; for(uint32_ti=TK_NUM_CORES;i<TK_MAX_TASKS;++i){ TaskSlot&s=g_slots[i]; if(s.state==State::Sleeping&&g_tick>=s.wake_tick){ s.state=State::Ready; need_switch=true; } } } TaskSlot&cur=g_slots[g_cur[core]]; if(cur.is_idle){ if(find_prio_level(core)<(int32_t)kIdlePrio){ need_switch=true; } }else{ --cur.slice_left; if(cur.slice_left<=0){ cur.slice_left=kSliceTicks; need_switch=true; } } sched_unlock(save); if(need_switch){ tk_port_trigger_pendsv(); } } //PendSVcontextswitchfunction:savescur_sp,selectsnexttask,andreturnsnextSP. extern"C"uintptr_ttk_switch_context(uintptr_tcur_sp){ uint32_tcore=get_core_num(); uint32_tsave=sched_lock(); TaskSlot&cur=g_slots[g_cur[core]]; if(g_preempt_disabled[core]>0&&cur.state==State::Running){ sched_unlock(save); returncur_sp; } cur.sp=cur_sp; memcpy(&cur.fpu,&tk_fpu_scratch[core],sizeof(cur.fpu)); if(cur.state==State::Running){ cur.state=State::Ready; } uint32_tnext=select_next(core); TaskSlot&nxt=g_slots[next]; if(nxt.sp==0){ nxt.sp=tk_port_make_frame(reinterpret_cast<uint8_t*>(&g_stacks[next][0]), TK_DEFAULT_STACK_SIZE,reinterpret_cast<void*>(nxt.fn),nxt.arg); } nxt.state=State::Running; memcpy(&tk_fpu_scratch[core],&nxt.fpu,sizeof(nxt.fpu)); nxt.slice_left=kSliceTicks; g_rr_last[nxt.prio]=(uint8_t)next; g_cur[core]=(uint8_t)next; sched_unlock(save); returnnxt.sp; } namespacetk::detail{ voidstatic_register(constchar*name,TaskFnfn,uint32_tprio){ if(prio>=kIdlePrio)prio=14u; for(uint32_ti=TK_NUM_CORES;i<TK_MAX_TASKS;++i){ if(g_slots[i].state==State::Free){ g_slots[i]=TaskSlot{}; g_slots[i].fn=fn; g_slots[i].name=name; g_slots[i].prio=prio; g_slots[i].state=State::Ready; return; } } } }//namespacetk::detail tk::TaskHandletk::create(TaskFnfn,void*arg,uint32_tprio){ TaskHandleh{0xFF}; if(prio>=kIdlePrio)prio=14u; for(uint32_ti=TK_NUM_CORES;i<TK_MAX_TASKS;++i){ if(g_slots[i].state==State::Free){ g_slots[i]=TaskSlot{}; g_slots[i].fn=fn; g_slots[i].arg=arg; g_slots[i].name="dyn"; g_slots[i].prio=prio; g_slots[i].state=State::Ready; h.slot=(uint8_t)i; returnh; } } returnh; } staticvoidcore_start_routine(uint32_tcore){ tk_port_init(TK_TICKS_PER_SEC); uint32_tsave=sched_lock(); uint32_tnext=select_next(core); TaskSlot&nxt=g_slots[next]; if(nxt.sp==0){ nxt.sp=tk_port_make_frame(reinterpret_cast<uint8_t*>(&g_stacks[next][0]), TK_DEFAULT_STACK_SIZE,reinterpret_cast<void*>(nxt.fn),nxt.arg); } nxt.state=State::Running; memcpy(&tk_fpu_scratch[core],&nxt.fpu,sizeof(nxt.fpu)); nxt.slice_left=kSliceTicks; g_rr_last[nxt.prio]=(uint8_t)next; g_cur[core]=(uint8_t)next; sched_unlock(save); tk_port_start(nxt.sp);//neverreturns for(;;)__asm__volatile("wfi"); } staticvoidcore1_entry(void){ core_start_routine(1); } voidtk::start(){ if(g_started)for(;;)__asm__volatile("wfi"); g_started=true; ensure_sched_lock(); //Core0IdleTask TaskSlot&idle0=g_slots[0]; idle0.fn=idle_fn; idle0.name="idle0"; idle0.prio=kIdlePrio; idle0.is_idle=true; idle0.state=State::Ready; //Core1IdleTask TaskSlot&idle1=g_slots[1]; idle1.fn=idle_fn; idle1.name="idle1"; idle1.prio=kIdlePrio; idle1.is_idle=true; idle1.state=State::Ready; //LaunchCore1 multicore_launch_core1(core1_entry); //LaunchCore0 core_start_routine(0); } voidtk::yield(){ uint32_tcore=get_core_num(); uint32_tsave=sched_lock(); g_slots[g_cur[core]].slice_left=0; tk_port_trigger_pendsv(); sched_unlock(save); } voidtk::sleep_ms(uint32_tms){ uint64_tticks=((uint64_t)ms*TK_TICKS_PER_SEC+999u)/1000u; if(ticks<1u)ticks=1u; int32_tsleep_ticks=(int32_t)(ticks>0x7FFFFFFFu?0x7FFFFFFFu:ticks); uint32_tcore=get_core_num(); uint32_tsave=sched_lock(); TaskSlot&cur=g_slots[g_cur[core]]; cur.state=State::Sleeping; cur.wake_tick=g_tick+sleep_ticks; tk_port_trigger_pendsv(); sched_unlock(save); } voidtk::exit(intcode){ (void)code; uint32_tcore=get_core_num(); uint32_tsave=sched_lock(); g_slots[g_cur[core]].state=State::Terminated; tk_port_trigger_pendsv(); sched_unlock(save); for(;;)__asm__volatile("wfi"); } extern"C"voidtk_task_return_trampoline(void){ tk::exit(0); } uint64_ttk::now_ms(){ return(uint64_t)g_tick*1000u/TK_TICKS_PER_SEC; } booltk::lock(){ uint32_tsave=sched_lock(); returnsave==0;//PRIMASKwas0(enabled) } voidtk::unlock(boolwas_enabled){ sched_unlock(was_enabled?0:1); } uint32_ttk::detail::sched_lock(){ return::sched_lock(); } voidtk::detail::sched_unlock(uint32_tsave){ ::sched_unlock(save); } voidtk::detail::block_current_task_locked(){ uint32_tcore=get_core_num(); TaskSlot&cur=g_slots[g_cur[core]]; cur.state=State::Blocked; tk_port_trigger_pendsv(); } voidtk::detail::wake_task_locked(TaskHandlehandle){ if(handle.slot>=TK_MAX_TASKS)return; TaskSlot&s=g_slots[handle.slot]; if(s.state==State::Blocked){ s.state=State::Ready; tk_port_trigger_pendsv(); } } uint8_ttk::detail::current_task_slot(){ returng_cur[get_core_num()]; } static_assert(TK_MAX_TASKS<=sizeof(tk::task_mask_t)*8,"TK_MAX_TASKSmustfitintask_mask_t"); uint8_ttk::detail::find_highest_prio_waiter(task_mask_twait_mask){ uint8_tbest_slot=0xFF; uint32_tbest_prio=0xFFFFFFFFu; for(uint32_ti=0;i<TK_MAX_TASKS;++i){ if(wait_mask&(task_mask_t{1}<<i)){ if(g_slots[i].prio<best_prio){ best_prio=g_slots[i].prio; best_slot=(uint8_t)i; } } } returnbest_slot; } voidtk::preempt_disable(){ uint32_tcore=get_core_num(); uint32_tsave=sched_lock(); ++g_preempt_disabled[core]; sched_unlock(save); } voidtk::preempt_enable(){ uint32_tcore=get_core_num(); uint32_tsave=sched_lock(); if(g_preempt_disabled[core]>0){ --g_preempt_disabled[core]; if(g_preempt_disabled[core]==0){ tk_port_trigger_pendsv(); } } sched_unlock(save); }