ardupilot/libraries/AP_HAL_F4Light/Scheduler.cpp

/*
(c) 2017 night_ghost@ykoctpa.ru
*/

#pragma GCC optimize ("O2")


#include "Scheduler.h"

#include <stdio.h>
#include <AP_HAL_F4Light/AP_HAL_F4Light.h>


#include "Semaphores.h"
#include "I2CDevice.h"

#include <timer.h>


#include <AP_Notify/AP_Notify.h>
#include <AP_Math/AP_Math.h>
#include <AP_Param_Helper/AP_Param_Helper.h>

#include "RCInput.h"
#include <systick.h>
#include "GPIO.h"
#include <usb.h>


using namespace F4Light;
extern const AP_HAL::HAL& hal;


AP_HAL::Proc  Scheduler::_failsafe  IN_CCM= NULL;

Revo_IO    Scheduler::_io_proc[F4Light_SCHEDULER_MAX_IO_PROCS] IN_CCM;
uint8_t    Scheduler::_num_io_proc IN_CCM=0;

void *   Scheduler::_delay_cb_handle IN_CCM=0;

uint32_t Scheduler::timer5_ovf_cnt IN_CCM=0;

bool Scheduler::_initialized IN_CCM=false;

Handler Scheduler::on_disarm_handler IN_CCM;
task_t * Scheduler::_idle_task IN_CCM;

void *Scheduler::boost_task;

static void loc_ret(){}

#define STACK_GUARD 0x60a4d51aL

// Reference running task
task_t* s_running IN_CCM;
task_t* next_task IN_CCM;
// Main task and run queue
task_t Scheduler::s_main = { 0 }; // NOT in CCM to can't be corrupted by stack
uint16_t Scheduler::task_n=0;


struct Scheduler::IO_COMPLETION Scheduler::io_completion[MAX_IO_COMPLETION] IN_CCM;

uint8_t Scheduler::num_io_completion  IN_CCM= 0;


// Initial top stack for task allocation
size_t Scheduler::s_top IN_CCM; //  = MAIN_STACK_SIZE; - CCM not initialized!


#ifdef SHED_PROF
uint64_t Scheduler::shed_time = 0;
bool     Scheduler::flag_10s = false;
uint64_t Scheduler::task_time IN_CCM = 0;
uint64_t Scheduler::delay_time IN_CCM = 0;
uint64_t Scheduler::delay_int_time IN_CCM = 0;
uint32_t Scheduler::max_loop_time IN_CCM =0;
uint64_t Scheduler::ioc_time IN_CCM =0;
uint64_t Scheduler::sleep_time IN_CCM =0;
uint32_t Scheduler::max_delay_err=0;

uint32_t Scheduler::tick_micros IN_CCM;    // max exec time
uint32_t Scheduler::tick_count IN_CCM;     // number of calls
uint64_t Scheduler::tick_fulltime IN_CCM;  // full consumed time to calc mean
#endif


#ifdef MTASK_PROF
 uint32_t Scheduler::max_wfe_time IN_CCM =0;
 uint32_t Scheduler::tsched_count IN_CCM;
 uint32_t Scheduler::tsched_sw_count IN_CCM;
 uint32_t Scheduler::tsched_count_y IN_CCM;
 uint32_t Scheduler::tsched_sw_count_y IN_CCM;
 uint32_t Scheduler::tsched_count_t IN_CCM;
 uint32_t Scheduler::tsched_sw_count_t IN_CCM;
 #ifdef SHED_DEBUG
  revo_sched_log Scheduler::logbuf[SHED_DEBUG_SIZE] IN_CCM;
  uint16_t Scheduler::sched_log_ptr;
 #endif

uint32_t Scheduler::lowest_stack = (uint32_t)-1;
#endif


bool Scheduler::_in_io_proc IN_CCM =0;
#ifdef MPU_DEBUG
uint32_t Scheduler::MPU_overflow_cnt IN_CCM;
uint32_t Scheduler::MPU_restart_cnt IN_CCM;
uint32_t Scheduler::MPU_count IN_CCM;
uint32_t Scheduler::MPU_Time IN_CCM;
#endif
volatile bool Scheduler::need_switch_task IN_CCM;
task_t *Scheduler::_forced_task IN_CCM;


Scheduler::Scheduler()
{

    s_running = &s_main;         //  CCM don't initialized! - Reference running task
    next_task = &s_main;         
    s_top = MAIN_STACK_SIZE;     // Initial top stack for task allocation

// init main task
    memset(&s_main, 0, sizeof(s_main));

    Revo_handler h = { .vp=loc_ret }; // fake handler to not 0

    s_main.next = &s_main; // linked list
    s_main.prev = &s_main;
    s_main.priority = MAIN_PRIORITY; // base priority
    s_main.active = true;  // not paused
    s_main.handle = h.h;        // to not 0
    s_main.guard = STACK_GUARD; // to check corruption of TCB by stack overflow

#ifdef MTASK_PROF
    s_main.stack_free = (uint32_t) -1;
#endif

    _forced_task = NULL;
}


// to do when nothing to do 
static void idle_task(){
    while(1){
        __WFE(); 
        // see RM090 12.2.3
        TIMER6->regs->SR &= TIMER_SR_UIF;    // reset pending bit
        NVIC_ClearPendingIRQ(TIM6_DAC_IRQn); // timer6 as event generator - reset IRQ
        
        Scheduler::yield(0);
    }
}

void Scheduler::init()
{
    if(in_interrupt()){ // some interrupt caused restart at ISR level
        AP_HAL::panic("HAL initialization on ISR level=0x%x", (uint8_t)(SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk));
    }
    memset(_io_proc,      0, sizeof(_io_proc) );
    memset(io_completion, 0, sizeof(io_completion) );


    // The PendSV exception is always enabled so disable interrupts
    // to prevent it from occurring while being configured 
    noInterrupts();

    NVIC_SetPriority(PendSV_IRQn,  PENDSV_INT_PRIORITY);      // lowest priority so all IRQs can't be switced
    NVIC_SetPriority(SVCall_IRQn,  SVC_INT_PRIORITY);         // priority 14 - the same as Timer7 ISR
    NVIC_SetPriority(SysTick_IRQn, SYSTICK_INT_PRIORITY);     // priority 5 - less thah fast device IO ISRs but higher than USB


    // Ensure the effect of the priority change occurs before 
    // clearing PRIMASK to ensure that future PendSV exceptions 
    // are taken at the new priority 
    asm volatile("dsb \n");
    asm volatile("isb \n");

    interrupts();


    CLEAR_BIT(SCB->SCR, ((uint32_t)SCB_SCR_SLEEPDEEP_Msk)); //we don't need deep sleep
    SET_BIT(  SCB->SCR, ((uint32_t)SCB_SCR_SEVONPEND_Msk)); //we need Event on each interrupt

/*[ DEBUG
    SCnSCB->ACTLR |= SCnSCB_ACTLR_DISDEFWBUF_Msk; // disable imprecise exceptions
//]*/

    timer_foreach(timer_reset); // timer_reset(dev) moved out from configTimeBase so reset by hands


    {// timeslice timer, not SYSTICK because we need to restart it by hands
        uint32_t period    = (2000000UL / SHED_FREQ) - 1; 
                // dev    period   freq, kHz
        configTimeBase(TIMER7, period, 2000);       //2MHz 0.5us ticks
        Revo_handler h = { .isr = _timer_isr_event };
        timer_attach_interrupt(TIMER7, TIMER_UPDATE_INTERRUPT, h.h , SVC_INT_PRIORITY); // almost lowest priority, higher than Pend_SW to schedule task switch
        TIMER7->regs->CR1 |=  TIMER_CR1_URS;  // interrupt only by overflow, not by update
        timer_resume(TIMER7);
    }

    {// timer5 - 32-bit general timer, unused for other needs
     // so we can read micros32() directly from its counter and micros64() from counter and overflows
        configTimeBase(TIMER5, 0, 1000);       //1MHz 1us ticks
        timer_set_count(TIMER5,(1000000/SHED_FREQ)/2); // to not interfere with TIMER7
        Revo_handler h = { .isr = _timer5_ovf };
        timer_attach_interrupt(TIMER5, TIMER_UPDATE_INTERRUPT, h.h, MPU_INT_PRIORITY); // high priority
        timer_resume(TIMER5);
    }

    {     // only Timer6 from spare timers has personal NVIC line - TIM6_DAC_IRQn
        uint32_t freq = configTimeBase(TIMER6, 0, 20000);     // 20MHz - we here don't know real freq so can't set period
        timer_set_reload(TIMER6, freq / 1000000);             // period to generate 1uS requests
        timer_enable_irq(TIMER6, TIMER_UPDATE_INTERRUPT);     // enable interrupt requests from timer but not enable them in NVIC - will be events
        timer_resume(TIMER6);
    }

    { // timer to generate more precise delays via quant termination
                // dev    period   freq, kHz
        configTimeBase(TIMER14, 0, 1000);       //1MHz 1us ticks
        Revo_handler h = { .isr = _tail_timer_event };
        timer_attach_interrupt(TIMER14, TIMER_UPDATE_INTERRUPT, h.h , SVC_INT_PRIORITY); // priority 14 - the same as Timer7 and SVC
        TIMER14->regs->CR1 &= ~(TIMER_CR1_ARPE | TIMER_CR1_URS); // not buffered preload, interrupt by overflow or by UG set
    }


    { // timer to generate interrupt for driver's IO_Completion
                // dev    period   freq, kHz
        configTimeBase(TIMER13, 0, 1000);       //1MHz 1us ticks
        Revo_handler h = { .isr = _ioc_timer_event };
        timer_attach_interrupt(TIMER13, TIMER_UPDATE_INTERRUPT, h.h , IOC_INT_PRIORITY); // priority 12
        TIMER13->regs->CR1 &= ~(TIMER_CR1_ARPE | TIMER_CR1_URS); // not buffered preload, interrupt by overflow or by UG set
    }

    void *task = _start_task((uint32_t)idle_task, 256); // only for one context
    set_task_priority(task, 255);                       // lowest possible, to fill delay()
    _idle_task=(task_t *)task;
    set_task_active(task);                              // tasks are created paused so run it
}



// it can't be started on init() because should be stopped in later_init()
void Scheduler::start_stats_task(){
#ifdef DEBUG_BUILD
// show stats output each 10 seconds
    Revo_handler h = { .vp = _set_10s_flag };
    void *task = _register_timer_task(10000000, h.h, NULL);
    set_task_priority(task, IO_PRIORITY+1); // lower than IO_thread
#endif

// task list is filled. so now we can do a trick -
//    dequeue_task(_idle_task); // exclude idle task from task queue, it will be used by direct link.
                              //  its own .next still shows to next task so no problems will. This works but...

}

void Scheduler::_delay(uint16_t ms)
{
    uint32_t start = _micros();
#ifdef SHED_PROF
    uint32_t t=start;
#endif

    uint32_t dt = ms * 1000;
    uint32_t now;

    while((now=_micros()) - start < dt) {
        if (hal.scheduler->_min_delay_cb_ms <= ms) { // MAVlink callback uses 5ms
            hal.scheduler->call_delay_cb();
            yield(1000 - (_micros() - now)); // to not stop MAVlink callback
        } else {
            yield(dt); // for full time
        }
    }


#ifdef SHED_PROF
    uint32_t us=_micros()-t;
    delay_time     +=us;

#endif
}

// also see resume_boost()
// this used from InertialSensor only
void Scheduler::_delay_microseconds_boost(uint16_t us){
    boost_task=get_current_task();

#ifdef SHED_PROF
    uint32_t t = _micros(); 
#endif

    yield(us); // yield raises priority by 6 so task will be high-priority for 1st time

#ifdef SHED_PROF
    uint32_t r_us=_micros()-t; // real time
    delay_time     +=r_us;
#endif
    boost_task=NULL;
}

#define NO_YIELD_TIME 8 // uS

void Scheduler::_delay_microseconds(uint16_t us)
{

#ifdef SHED_PROF
    uint32_t t = _micros(); 
#endif
    
    uint16_t no_yield_t;    // guard time for main process
    no_yield_t=NO_YIELD_TIME;

    if(us > no_yield_t){
        yield(us);

 #ifdef SHED_PROF
        uint32_t r_us=_micros()-t; // real time
        delay_time     +=r_us;
 #endif

    }else{
        _delay_us_ny(us);
    }

}

void Scheduler::_delay_us_ny(uint16_t us){ // precise no yield delay
    uint32_t rtime = stopwatch_getticks(); // get start ticks first

    uint32_t dt = us_ticks * us;  // delay time in ticks
    
    while ((stopwatch_getticks() - rtime) < dt) {
        // __WFE(); -- not helps very much
    }    

#ifdef SHED_PROF
    delay_time     +=us;
    
#endif

}

void Scheduler::register_delay_callback(AP_HAL::Proc proc, uint16_t min_time_ms)
{
    static bool init_done=false;
    if(!init_done){     // small hack to load HAL parameters in needed time

        ((HAL_F4Light&) hal).lateInit();
        
        init_done=true;
    }

    AP_HAL::Scheduler::register_delay_callback(proc, min_time_ms);


/* 
1 - it should run in delay() only 
2 - it should be removed after init done
    if(proc) {
        _delay_cb_handle = start_task(proc);
    } else {
        stop_task(_delay_cb_handle);
    }
*/
}


void Scheduler::_run_io(void)
{
    if (_in_io_proc) {
        return;
    }
    _in_io_proc = true;

    // now call the IO based drivers. TODO: per-task stats
    for (int i = 0; i < _num_io_proc; i++) {
        if (_io_proc[i].h) {
            revo_call_handler(_io_proc[i].h,0);
            if(_io_proc[i].flags == IO_ONCE){
                _io_proc[i].h = 0;
            }
        }
    }

    _in_io_proc = false;
}


void Scheduler::_register_io_process(Handler h, Revo_IO_Flags flags)
{
    if(_num_io_proc>=F4Light_SCHEDULER_MAX_IO_PROCS) return;

    if(_num_io_proc==0){
        void *task = start_task(_run_io, IO_STACK_SIZE);
        set_task_period(task, 1000); 
        set_task_priority(task, IO_PRIORITY); 
    }

    uint8_t i;
    for(i=0; i<_num_io_proc; i++){ // find free slots
        if(_io_proc[i].h == 0) {  // found
            _io_proc[i].h = h; 
            _io_proc[i].flags = flags;
            return;
        }
    }

    i=_num_io_proc++;
    _io_proc[i].h = h;
    _io_proc[i].flags = flags;
}


#ifdef MTASK_PROF
void Scheduler::check_stack(uint32_t sp) { // check for stack usage
    
//    uint32_t * stack = (uint32_t *)sp;

    // Stack frame contains:
    // r0, r1, r2, r3, r12, r14, the return address and xPSR
    // - Stacked R0  = stack[0]
    // - Stacked R1  = stack[1]
    // - Stacked R2  = stack[2]
    // - Stacked R3  = stack[3]
    // - Stacked R12 = stack[4]
    // - Stacked LR  = stack[5]
    // - Stacked PC  = stack[6]
    // - Stacked xPSR= stack[7]
        
    if(ADDRESS_IN_CCM(sp)){
        if(sp<lowest_stack){ lowest_stack=sp; }
    }
}
#endif


void Scheduler::_run_timer_procs(bool called_from_isr) {

    // and the failsafe, if one is setted 
    if (_failsafe) {
        static uint32_t last_failsafe=0;
        uint32_t t=_millis();
        if(t-last_failsafe>10){
            last_failsafe = t+50; // 50ms = 20Hz
            _failsafe();
        }
    }

}

void Scheduler::_timer_isr_event(uint32_t v  /* TIM_TypeDef *tim */) {
#ifdef MTASK_PROF
    uint32_t sp; 
    // Get stack pointer
    asm volatile ("MRS     %0, PSP\n\t"  : "=rm" (sp));

    check_stack(sp);
#endif
    static uint32_t last_timer_procs=0;

    uint32_t now = _micros();

    if(now - last_timer_procs >= 1000) {
        last_timer_procs = now;
        _run_timer_procs(true);
    }


#ifndef MTASK_PROF
    _switch_task();
#else
    
    if(task_n==0 || need_switch_task) return;        // if there no tasks or already planned
    
    next_task = get_next_task();
    tsched_count++;

    if(next_task != s_running) { // if we should switch task
        s_running->sw_type=0;
        tsched_sw_count++;
        plan_context_switch();     // plan context switch after return from ISR
    }
#endif
}

void Scheduler::_timer5_ovf(uint32_t v /* TIM_TypeDef *tim */) {
    timer5_ovf_cnt++;
}

uint64_t Scheduler::_micros64() {
#pragma pack(push, 1)
    union {
        uint64_t t;
        uint32_t w[2];
    } now;
#pragma pack(pop)

    noInterrupts();
    now.w[0] = _micros();
    now.w[1] = timer5_ovf_cnt;
    interrupts();
    return now.t;
}


void Scheduler::system_initialized()
{
#ifndef I_KNOW_WHAT_I_DO
    if (_initialized) {
        AP_HAL::panic("PANIC: scheduler::system_initialized called more than once");
    }
#endif
    _initialized = true;
    
    board_set_rtc_register(0,RTC_SIGNATURE_REG); // clear bootloader flag after init done
}


void Scheduler::_reboot(bool hold_in_bootloader) {

    if(hold_in_bootloader) {
#if 1
        if(is_bare_metal() || hal_param_helper->_boot_dfu) { // bare metal build without bootloader of parameter set

            board_set_rtc_register(DFU_RTC_SIGNATURE, RTC_SIGNATURE_REG);

        } else
#endif
            board_set_rtc_register(BOOT_RTC_SIGNATURE, RTC_SIGNATURE_REG);
    }

    _delay(100);

    NVIC_SystemReset();

    _delay(1000);
}


void Scheduler::reboot(bool hold_in_bootloader) {
    
    hal.console->println("GOING DOWN FOR A REBOOT\r\n");
    _reboot(hold_in_bootloader);
}



#ifdef DEBUG_BUILD

extern "C" {
    extern  void *__brkval;
    extern  void *__brkval_ccm;
}


void Scheduler::_print_stats(){
    static int cnt=0;

    if(flag_10s) {
        flag_10s=false;

#if defined(USE_MPU)
        mpu_disable();      // we need access to all tasks
#endif

        
        uint32_t t=_millis();
        const int Kf=100;
        
        switch(cnt++) {
        
        case 0:{
#ifdef SHED_PROF
            float eff= (task_time)/(float)(task_time+shed_time);

            static float shed_eff=0;

            if(is_zero(shed_eff)) shed_eff = eff;
            else              shed_eff = shed_eff*(1 - 1/Kf) + eff*(1/Kf);

            printf("\nSched stats: uptime %lds\n  %% of full time: %5.2f  Efficiency %5.3f max loop time %ld\n", t/1000, (task_time/10.0)/t /* in percent*/ , shed_eff, max_loop_time);
            printf("delay times: in main %5.2f including in timer %5.2f",         (delay_time/10.0)/t, (delay_int_time/10.0)/t);
            max_loop_time=0;

#ifdef ISR_PROF
            printf("\nISR time %5.2f max %5.2f", (isr_time/10.0/(float)us_ticks)/t, max_isr_time/(float)us_ticks );
            max_isr_time=0;
#endif
#ifdef MPU_DEBUG
            printf("MPU overflows: %ld restarts %ld max samples %ld time %ld\n", MPU_overflow_cnt, MPU_restart_cnt, MPU_count, MPU_Time); MPU_overflow_cnt=0; MPU_restart_cnt=0; MPU_count=0; MPU_Time=0;
#endif

            printf("\nPPM max buffer size: %d\n", RCInput::max_num_pulses); RCInput::max_num_pulses=0;
#endif
                        
        } break;

        case 1:{
#ifdef SHED_PROF 
#endif
    
            }break;
            
        case 2:{
#ifdef MTASK_PROF    
            task_t* ptr = &s_main;

            uint32_t  fc=tsched_count+tsched_count_y+tsched_count_t;
            printf("\nsched time: by timer %5.2f%% sw %5.2f%% in yield %5.2f%% sw %5.2f%% in tails %5.2f%% sw %5.2f%%\n", 100.0*tsched_count/fc, 100.0 * tsched_sw_count/tsched_count, 100.0*tsched_count_y/fc,100.0 * tsched_sw_count_y/tsched_count_y, 100.0*tsched_count_t/fc, 100.0 * tsched_sw_count_t/tsched_count_t);
        
            do {
                printf("task %d (0x%015llx) time: %7.2f%% mean %8.1fuS max %5lduS full %7lduS wait sem. %6lduS free stack 0x%lx\n",  
                          ptr->id, ptr->handle, 100.0 * ptr->time/1000.0 / t, 
                                                              (float)ptr->time / ptr->count, 
                                                                       ptr->max_time, 
                                                                                  ptr->work_time,       ptr->sem_max_wait, ptr->stack_free);
        
                ptr->max_time=0; // reset times
                ptr->work_time=0;
                ptr->sem_max_wait=0;
                ptr->quants=0;
                ptr->quants_time=0;
                ptr->max_paused=0;
                
                ptr = ptr->next;
            } while(ptr != &s_main);
#endif
            }break;

        case 3: {
            uint8_t n = I2CDevice::get_dev_count();
            printf("\nI2C stats\n");
    
            for(uint8_t i=0; i<n; i++){
                I2CDevice * d = I2CDevice::get_device(i);
                if(d){
                    printf("bus %d addr %x errors %ld last error=%d state=%d\n",d->get_bus(), d->get_addr(), d->get_error_count(), d->get_last_error(), d->get_last_error_state());   
                }
            }
            }break;

        case 4: {
            uint32_t heap_ptr = (uint32_t)__brkval; // upper bound of sbrk()
            uint32_t bottom   = (uint32_t)&_sdata;
            
            // 48K after boot 72K while logging on
            printf("\nMemory used: static %ldk full %ldk\n",((uint32_t)&_edata-bottom+1023)/1024, (heap_ptr-bottom+1023)/1024);
            printf("Free stack: %ldk\n",(lowest_stack - (uint32_t)&_eccm)/1024);
            printf("CCM use: %ldk\n",((uint32_t)__brkval_ccm - (uint32_t)&_sccm)/1024);

            } break;
        
        case 5: {
            printf("\nIO completion %7.3f%%\n",  ioc_time/1000.0/t*100);
            uint64_t iot=0;
            for(uint8_t i=0; i<num_io_completion; i++){
                struct IO_COMPLETION &io = io_completion[i];
                
                if(io.handler) {
                    if(io.count){
                        printf("task %llx time %7.3f%% mean %7.3fuS max %lduS\n", io.handler,  100.0 * io.time / t / 1000, (float)io.time/io.count, io.max_time);
                        io.max_time=0; 
                        iot+=io.time;
                    }
                }    
            }
            if(ioc_time)
                printf("IO completion effectiveness=%7.3f%%\n",  100.0 * iot/ioc_time);
        
            }break;
            
        case 6:
        default:
            cnt=0;
            break;
        }
    }
}

void Scheduler::_set_10s_flag(){
    flag_10s=true;
    _print_stats();
}
#endif

/*
[    common implementation of all Device.PeriodicCallback;
*/

AP_HAL::Device::PeriodicHandle Scheduler::_register_timer_task(uint32_t period_us, Handler proc, F4Light::Semaphore *sem){

    // all drivers will runs at individual IO tasks
    void *task = _start_task(proc, SMALL_TASK_STACK);
    if(task){
        set_task_priority(task, DRIVER_PRIORITY);
        set_task_semaphore(task, sem);
        set_task_period(task, period_us); // setting of period allows task to run
    }
    return (AP_HAL::Device::PeriodicHandle)task;
}




bool Scheduler::adjust_timer_task(AP_HAL::Device::PeriodicHandle h, uint32_t period_us)
{
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wcast-align" // yes I know

    task_t *p = (task_t *)h;
 #pragma GCC diagnostic pop
    p->period = period_us;
    return true;
}

bool Scheduler::unregister_timer_task(AP_HAL::Device::PeriodicHandle h)
{
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wcast-align"
    task_t *p = (task_t *)h;
 #pragma GCC diagnostic pop

    noInterrupts(); // 64-bits should be 
    p->handle=0L;
    interrupts();
    return true;
}
// ]


//[ -------- preemptive multitasking --------


#if 0 // once started tasks are never ended

task_t* Scheduler::get_empty_task(){
    task_t* ptr = &s_main;

    do {
        if(ptr->handler == NULL)  return ptr;
        
        ptr = ptr->next;
    } while(ptr != &s_main);

    return NULL;
}

#endif

void Scheduler::stop_task(void *h){
    if(h) {
        task_t *tp = (task_t *)h ;
        noInterrupts();    
        tp->handle = 0;
        interrupts();
    }
}


// task's executor, which calls user's function having semaphore
void Scheduler::do_task(task_t *task) {
    while(1){
        uint32_t t=0;
        if(task->handle && task->active) { // Task Switch occures asyncronously so we should wait until task becomes active again
            task->time_start=_micros();
            if(task->sem) {// if task requires a semaphore - block on it
                if(!task->sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
                    yield(0);   // can't be
                    continue;
                }
                revo_call_handler(task->handle, task->id); 
                task->sem->give(); // give semaphore when task finished
            } else {
                revo_call_handler(task->handle, task->id); 
            }
#ifdef MTASK_PROF
            t = _micros()-task->time_start; // execution time
            if(t > task->work_time)  task->work_time=t;
            if(task->t_paused > task->max_paused) {
                task->max_paused = task->t_paused;
            }
            if(task->count_paused>task->max_c_paused) {
                task->max_c_paused = task->count_paused;
            }
            task->count_paused=0;
            task->t_paused=0;
#endif
            task->active=false;                    // turn off active, to know when task is started again. last! or can never give semaphore
            task->curr_prio = task->priority - 6;  // just activated task will have a highest priority for one quant
        }
        yield(0);        // give up quant remainder
    } // endless loop
}

void Scheduler::enqueue_task(task_t &tp) { // add new task to run queue, starting main task
    tp.next = &s_main;  // prepare for insert task into linked list
    tp.prev = s_main.prev;
    tp.id = ++task_n;   // counter - new task is created

    noInterrupts();     // we will break linked list so do it in critical section
    s_main.prev->next = &tp;
    s_main.prev = &tp;
    interrupts();       // now TCB is ready to task scheduler
}

void Scheduler::dequeue_task(task_t &tp) { // remove task from run queue
    noInterrupts();     // we will break linked list so do it in critical section
    tp.prev->next = tp.next;
    tp.next->prev = tp.prev;
    interrupts();       // done
}


// Create task descriptor
uint32_t Scheduler::fill_task(task_t &tp){
    memset(&tp,0,sizeof(tp));

    // fill required fields
    tp.priority  = MAIN_PRIORITY;  // default priority equal to main task
    tp.curr_prio = MAIN_PRIORITY;  // current priority the same
#ifdef MTASK_PROF
    tp.start=_micros(); 
    tp.stack_free = (uint32_t) -1;
#endif
    tp.guard = STACK_GUARD;

    return (uint32_t)&tp;
}

// create task descriptor and context
void * Scheduler::init_task(Handler handler, const uint8_t* stack){
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-align" // yes I know

//  task_t *task = (task_t *)((uint32_t)(stack-sizeof(task_t)) & 0xFFFFFFFCUL); // control block below memory top, 4-byte alignment
    task_t *task = (task_t *)((uint32_t)(stack-sizeof(task_t)) & 0xFFFFFFE0UL); // control block below memory top, 32-byte alignment for MMU page
#pragma GCC diagnostic pop

    fill_task(*task);  // fill task descriptor
    task->stack  = stack;

    /*
     * ARM Architecture Procedure Call Standard [AAPCS] requires 8-byte stack alignment.
     * This means that we must get top of stack aligned _after_ context "pushing", at
     * interrupt entry.
     */
    uint32_t *sp =(uint32_t *) (((uint32_t)task - 4) & 0xFFFFFFF8UL); // below TCB

// HW frame
    *(--sp)  = 0x01000000UL;          // xPSR
    //           61000000
    *(--sp)  = ((uint32_t)do_task);   // PC Entry Point - task executor
    *(--sp)  = ((uint32_t)do_task)|1; // LR the same, with thumb bit set
    sp -= 4;                          // emulate "push R12,R3,R2,R1"
    *(--sp)  = (uint32_t)task;        // emulate "push r0"
// SW frame, context saved as  "STMDB     R0!, {R4-R11, LR}"
    *(--sp)  = 0xFFFFFFFDUL;          // emulate "push lr" =exc_return: Return to Thread mode, floating-point context inactive, execution uses PSP after return.
#if 0    
    asm volatile (
        "MOV       R0, %0       \n\t"
        "STMDB     R0!, {R4-R11}\n\t"  : "+rm" (sp) ); // push real registers - they can be global register variables
        "MOV       %0,R0        \n\t"
#else
    sp -= 8;                        // emulate "push R4-R11"
#endif
    task->sp=(uint8_t *)sp;           // set stack pointer of task

    // task is not active so we need not to disable interrupts
    task->handle = handler; // save handler to TCB

    return (void *)task;
}



// create a paused task
void * NOINLINE Scheduler::_start_task(Handler handle, size_t stackSize)
{
    // Check called from main task
    if (!_in_main_thread() ) return NULL; 
    
    if(in_interrupt()) {
        AP_HAL::panic("start_task called from ISR 0x%x", (uint8_t)(SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk));
    }

#if defined(USE_MPU)
    mpu_disable();      // we need access to new tasks TCB which can be overlapped by guard page
#endif

    // Adjust stack size with size of task context
    stackSize += sizeof(task_t)+8; // for alignment

    if (s_main.stack == NULL) {       // first call, initialize all task subsystem
        s_main.stack = (const uint8_t*)RAMEND - s_top; // remember bottom of stack of main task on first call
    }

    const uint8_t *sp=(const uint8_t*)s_main.prev->stack; // top of stack for new task    

    task_t *task=(task_t *) init_task(handle, sp);    // give stack top as parameter, will correct later
    sp-= stackSize;               // calc stack bottom
    task->stack = sp;             // correct to bottom of stack
    stack_bottom = (caddr_t)sp;   // and remember for memory allocator
    s_top += stackSize;           // adjust used size at stack top

    enqueue_task(*task);          // task is ready, now we can add new task to run queue
                                  //  task will not be executed because .active==0

    return (void *)task;    // return address of task descriptor as task handle
}

// task should run periodically, period in uS. this will be high-priority task
void Scheduler::set_task_period(void *h, uint32_t period){
    task_t *task = (task_t *)h;

    task->active = false; // will be first started after 'period'
    task->time_start  = _micros();
    task->period = period;
}


#ifdef SHED_DEBUG
static uint16_t next_log_ptr(uint16_t sched_log_ptr){
    uint16_t lp = sched_log_ptr+ 1;
    if(lp >= SHED_DEBUG_SIZE) lp=0;
    return lp;
}
#endif

// exception occures in armed state - try to kill current task, or reboot if this is main task
void Scheduler::_try_kill_task_or_reboot(uint8_t n){
    task_t  *me = s_running; // current task
    uint8_t tmp = task_n;

    if(tmp==0 || me->id == 0) { // no tasks yet or in main task
        board_set_rtc_register(FORCE_APP_RTC_SIGNATURE, RTC_SIGNATURE_REG); // force bootloader to not wait
        _reboot(false);
    }
    stop_task(me); // exclude task from planning
    task_n = 0;    // printf() can call yield while we now between live and death

    printf("\nTaks %d killed by exception %d!\n",me->id, n);

    task_n = tmp;
    next_task = get_next_task();
}

void Scheduler::_go_next_task() {
    plan_context_switch();    

    while(1);
}

void Scheduler::_stop_multitask(){
    task_n = 0;
}


// this function called only from SVC Level ISRs so there is no need to be reentrant
task_t *Scheduler::get_next_task(){
    task_t *me = s_running; // current task
    task_t *task=_idle_task; // task to switch to, idle_task by default


    uint32_t timeFromLast=0;
    uint32_t remains = 0;

    uint32_t partial_quant=(uint32_t)-1;
    task_t *want_tail = NULL;

    uint32_t now =  _micros();
    me->t_yield = now;

#if defined(USE_MPU)
    mpu_disable();      // we need access to all tasks
#endif

    { // isolate dt
#if defined(MTASK_PROF) 
        uint32_t dt =  now - me->start;       // time in task
        if(dt >= me->in_isr) dt -= me->in_isr;  // minus time in interrupts
        else                 dt=0;

        me->time+=dt;                           // calculate sum
        me->quants_time+=dt;
#endif


#ifdef MTASK_PROF
        if(dt > me->max_time) {
            me->max_time = dt; // maximum to show
        }

 #ifdef SHED_DEBUG
        {
            revo_sched_log &lp = logbuf[sched_log_ptr];
            lp.end = now;
            lp.task_id=me->id;
            lp.ttw = me->ttw;
            lp.in_isr = me->in_isr;
            lp.sw_type=me->sw_type;
            sched_log_ptr = next_log_ptr(sched_log_ptr);
            ZeroIt(logbuf[sched_log_ptr]); // clear next
        }
 #endif
#endif
    }

    if(_forced_task) {
        task = _forced_task;
        _forced_task = NULL;
    } else {

        task_t *ptr = me; // starting from current task
        bool was_yield=false;

        while(true) { // lets try to find task to switch to
            ptr = ptr->next; // Next task in run queue will continue

#if !defined(USE_MPU) || 1 
            if(ptr->guard != STACK_GUARD){ // check for TCB is not damaged
                printf("PANIC: stack guard spoiled in process %d (from %d)\n", task->id, me->id);
                dequeue_task(*ptr); // исключить задачу из планирования
                goto skip_task;    // skip this tasks
            }
#endif

            if(!ptr->handle) goto skip_task; // skip finished tasks

            if(ptr->f_yield) { // task wants to give one quant
                ptr->f_yield = false;
                was_yield = true;
                goto skip_task; // skip this tasks
            }

        
            if(ptr->sem_wait) { // task want a semaphore
                if(ptr->sem_wait->is_taken()) { // task blocked on semaphore
                    task_t *own =(task_t *)ptr->sem_wait->get_owner();
                    if(own != ptr) { // owner is another task?
                        uint32_t dt = now - ptr->sem_start_wait;   // time since start waiting
                        if(ptr->sem_time == HAL_SEMAPHORE_BLOCK_FOREVER || dt < ptr->sem_time) {
                            if(own->curr_prio > ptr->curr_prio) {
                                own->curr_prio=ptr->curr_prio;
                            }
                            goto skip_task; 
                        }
                    }       
                } 
                ptr->sem_wait=NULL; // clear semaphore after release
#ifdef MTASK_PROF
                uint32_t st=now-ptr->sem_start_wait;
                if(st>ptr->sem_max_wait) ptr->sem_max_wait=st; // time of semaphore waiting
#endif            
            }
            
        
            if(!ptr->active){ // non-active, is it periodic?
                if(ptr->period){                 
                    timeFromLast = now - ptr->time_start; // time from last run
                    if( timeFromLast < ptr->period) {     //   is less than task's period?
                        remains = ptr->period - timeFromLast;
                        if(remains>4) {
                            if(remains<partial_quant && ptr->curr_prio <= want_tail->curr_prio) { // exclude low-prio tasks
                                partial_quant=remains; // minimal time remains to next task
                                want_tail = ptr;
                            }
                            goto skip_task; 
                        }// else execute task slightly before
                    }
                } else { // non-active non-periodic tasks with manual activation
                    goto skip_task; // should be skipped
                }
                ptr->active=true; // selected task to run, even if it will lose quant by priority

            } else { // обычный тайм слайс

                if(ptr->ttw){// task wants to wait 
                    timeFromLast = now - ptr->t_yield;     // time since that moment
                    if(timeFromLast < ptr->ttw){           // still less than ttw ?
                        remains = ptr->ttw - timeFromLast; // remaining time to wait
                        if(remains>4) { // context switch time
                            if(remains<partial_quant && ptr->curr_prio <= want_tail->curr_prio) {
                                partial_quant=remains;
                                want_tail = ptr;
                            }
                            goto skip_task; 
                        }// else execute task slightly before
                    }
                } 
            }


            if(ptr->curr_prio <= task->curr_prio){ // select the most priority task, round-robin for equal priorities
                // task loose tick
                if(task->priority != 255) { // not for idle task
                    if(task->curr_prio>1)  task->curr_prio--;      // increase priority if task loose tick
// as a result of the rising priority of the waiting task, we do not completely stop the low priority tasks, but only slow them down
// execution, forcing to skip the number of ticks equal to the priority difference. As a result, the low priority task is performed at a lower speed,
// which can be adjusted by changing the priority difference                    
                }
                task = ptr; // winner
            } else { // ptr loose a chance - increase priority
                if(ptr->priority != 255) { // not for idle task
                    if(ptr->curr_prio>1)  ptr->curr_prio--;
                }    
            }
            
skip_task:
        // we should do this check after EACH task so can't use "continue" which skips ALL loop. 
        // And we can't move this to begin of loop because then interrupted task does not participate in the comparison of priorities
            if(ptr == me) {  // 'me' is the task that works now, so full loop - now we have most-priority task so let it run!
                if(was_yield && task == _idle_task) { // task wants to yield() but there is no other tasks
                    was_yield=false;    // reset flag and loop again
                } else {
                    break;  // task found
                }
            }
        }
    }

    // task to run is selected

 #ifdef SHED_DEBUG
    revo_sched_log &lp = logbuf[sched_log_ptr];
    lp.start = now;
    lp.task_id=task->id;
    lp.prio = task->curr_prio;
    lp.active = task->active;
    lp.time_start = task->time_start;
    lp.quant = partial_quant;
    lp.want_tail = want_tail;
    ZeroIt(logbuf[next_log_ptr(sched_log_ptr)]); // clear next
 #endif

    task->curr_prio=task->priority; // reset current priority to default value
    task->ttw=0;        // time to wait is over
#if defined(MTASK_PROF) 
    task->start = now;  // task startup time
    task->in_isr=0; // reset ISR time
    task->count++;     // full count
    task->quants++;    // one-start count

    uint32_t sz =  s_running->sp - s_running->stack;
    if(sz< s_running->stack_free) s_running->stack_free = sz;

#endif

    if(want_tail && want_tail->curr_prio <= task->curr_prio) { // we have a high-prio task that want to be started next in the middle of tick
        if(partial_quant < TIMER_PERIOD-10) { // if time less than tick
            timer_set_count(TIMER14, 0);
            timer_set_reload(TIMER14, partial_quant+2); // +2 to guarantee
            timer_resume(TIMER14);
        }
    }

    return task;
}



/*
    interrupt to reduce timeslice quant
*/
void Scheduler::_tail_timer_event(uint32_t v /*TIM_TypeDef *tim */){
    timer_pause(TIMER14); // stop tail timer
    timer_generate_update(TIMER7); // tick is over
#ifndef MTASK_PROF
    _switch_task();
#else

    if(need_switch_task || task_n==0) return; // already scheduled context switch

    next_task = get_next_task();

    tsched_count_t++;

    if(next_task != s_running) { // if we should switch task
        s_running->sw_type=1;
        tsched_sw_count_t++;
        plan_context_switch();  
    }
#endif
}


void Scheduler::yield(uint16_t ttw) // time to wait 
{
    if(task_n==0 || in_interrupt()) { // SVC causes HardFault if in interrupt so just nothing to do
 #ifdef USE_WFE
        if(ttw) {__WFE(); }
 #endif
        return;
    }

// if yield() called with a time, then task don't want to run all this time so exclude it from time sliceing
    if(ttw) { // ttw cleared on sleep exit so always 0 if not set specially 
        s_running->ttw=ttw;   // if switching tasks occurs between writing and calling svc, we just add an extra tick
    }
    asm volatile("svc 0");
}


/**
   * Return current task stack size.
   * @return bytes
 */
size_t Scheduler::task_stack(){
  unsigned char marker;
  return (&marker - s_running->stack);
}





// register IO completion routine
uint8_t Scheduler::register_io_completion(Handler handler){

    if(num_io_completion < MAX_IO_COMPLETION){
        io_completion[num_io_completion].handler=handler; // no need to disable interrupts because we increment counter later
        io_completion[num_io_completion].request=false;
        return ++num_io_completion;
    }
    return 0;
}

void Scheduler::_ioc_timer_event(uint32_t v){ // isr at low priority to do all IO completion routines
    bool do_it = false;

#ifdef SHED_PROF
    uint32_t full_t=_micros();
#endif

    do {
        do_it = false;
        for(uint8_t i=0; i<num_io_completion; i++){
            IO_Completion &io = io_completion[i];
            
            if(io.request) {
                io.request=false; // ASAP - it can be set again in interrupt. 
                // we don't need to disable interrupts because all drivers has own queue and can survive a skipping of one request
                if(io.handler){
                    do_it=true;

#ifdef SHED_PROF
                    uint32_t t = _micros();
#endif
                    revo_call_handler(io.handler,i); // call it
#ifdef SHED_PROF
                    t = _micros() - t;
                    io.time += t;
                    io.count++;
                    if(t>io.max_time) io.max_time=t;
#endif
                }
            }
        }
    } while(do_it);

#ifdef SHED_PROF
    full_t=_micros() - full_t;
    ioc_time += full_t;
#endif

#ifdef MTASK_PROF
// To exclude the interruption time from the task's time, which it interrupted
    s_running->in_isr += full_t;
#endif

}

#pragma GCC optimize ("O2") // should ALWAYS be -O2 for tail recursion optimization in PendSV_Handler

/* how to configure and schedule a PendSV exception
from http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0395b/CIHJHFJD.html
*/

void PendSV_Handler(){
    Scheduler::need_switch_task=false;
#if defined(USE_MPU)
    // set the guard page for the next task
    mpu_configure_region(MPU_REGION_0,                              
                        (uint32_t)(next_task->stack) & ~31, // end of stack in the guard page
                         MPU_RASR_ATTR_AP_RO_RO | MPU_RASR_ATTR_NON_CACHEABLE | MPU_RASR_SIZE_32);  // disable write access
    
    mpu_enable(MPU_CTRL_PRIVDEFENA); // enable default memory map
#endif 
    __do_context_switch();
}

void SVC_Handler(){
    uint32_t * svc_args;
    
// SVC can't be used from any interrupt so this is only for reliability
    asm volatile (
        "TST lr, #4     \n"  
        "ite eq         \n"
        "MRSEQ %0, MSP  \n"  
        "MRSNE %0, PSP  \n"  
        : "=rm" (svc_args) );

    Scheduler::SVC_Handler(svc_args);
}


// svc executes on same priority as Timer7 ISR so there is no need to prevent interrupts
void Scheduler::SVC_Handler(uint32_t * svc_args){
    //    * Stack contains:    * r0, r1, r2, r3, r12, r14, the return address and xPSR      
    unsigned int svc_number = ((char *)svc_args[6])[-2];    

    bool ret;
    switch(svc_number)    {        
    case 0:            // Handle SVC 00 - yield()
        timer_generate_update(TIMER7); // tick is over
        timer_pause(TIMER14);

        if(s_running->priority!=255){ // not for idle task or low-priority tasks
            if(s_running->priority<IO_PRIORITY && s_running->ttw){ // the task voluntarily gave up its quant and wants delay, so that at the end of the delay it will have the high priority
                s_running->curr_prio = s_running->priority - 6;
            } else {
                s_running->f_yield = true;      // to guarantee that quant will not return even if there is no high priority tasks
            }
        }
        switch_task();
        break;        

    case 1:{            // Handle SVC 01 - semaphore give(semaphore) returns bool
            Semaphore * sem = (F4Light::Semaphore *)svc_args[0];
            bool v=sem->is_waiting();            
            svc_args[0] = sem->svc_give();
            if(v) switch_task(); // switch context to waiting task if any
        }
        break;

    case 2:{            // Handle SVC 02 - semaphore take(semaphore, time) returns bool
            Semaphore * sem = (F4Light::Semaphore *)svc_args[0];
            uint32_t timeout_ms = svc_args[1];
            svc_args[0] = ret = sem->svc_take(timeout_ms);
            if(!ret)  {                                 // if failed - switch context to pause waiting task
                task_t *own = (task_t *)sem->get_owner();
                task_t *curr_task = s_running;
                curr_task->sem_wait = sem;             // semaphore
                curr_task->sem_start_wait = _micros(); // time when waiting starts
                if(timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER){
                    curr_task->sem_time = timeout_ms;
                } else {
                    curr_task->sem_time = timeout_ms*1000;      // time to wait semaphore
                }
                //Increase the priority of the semaphore's owner up to the priority of the current task
                if(own->priority >= curr_task->priority) own->curr_prio = curr_task->priority-1;
                switch_task(); 
            }
        }
        break;

    case 3:{            // Handle SVC 03 - semaphore take_nonblocking(semaphore) returns bool
            Semaphore * sem = (F4Light::Semaphore *)svc_args[0];
            svc_args[0] = ret = sem->svc_take_nonblocking();
            if(!ret)  { // if failed - switch context to give tick to semaphore's owner
                task_t *own = (task_t *)sem->get_owner();
                task_t *curr_task = s_running;
                //Increase the priority of the semaphore's owner up to the priority of the current task
                if(own->priority >= curr_task->priority) own->curr_prio = curr_task->priority-1;

                switch_task(); 
            }
        }
        break;
        
    case 4: // whats more we can do via SVC?

    default:                // Unknown SVC - just ignore
        break;    
    }
}

// prepare task switch and plan it if needed. This function called only on ISR level 14
void Scheduler::switch_task(){
    timer_pause(TIMER14);          // we will recalculate scheduling
    timer_generate_update(TIMER7); // tick is over
    _switch_task();
}

void Scheduler::_switch_task(){
    if(need_switch_task || task_n==0) return; // already scheduled context switch

    next_task = get_next_task(); // 2.5uS mean full time

#ifdef MTASK_PROF
    tsched_count_y++;
#endif

    if(next_task != s_running) { // if we should switch task
#ifdef MTASK_PROF
        s_running->sw_type=2;
        tsched_sw_count_y++;
#endif
        plan_context_switch();   
    }
#ifdef MTASK_PROF
      else if(next_task == _idle_task){ // the same idle task
        tsched_count_y--; // don't count loops in idle task
    }
#endif
}

////////////////////////////////////
/*
union Revo_handler { // blood, bowels, assembler :) transform functors into a unified view for calling from C
    voidFuncPtr vp;
    AP_HAL::MemberProc mp;          this is C not C ++, so we can not declare the support of functors explicitly, and are forced to pass
    uint64_t h; // treat as handle             <-- as 64-bit integer
    uint32_t w[2]; // words, to check. if this is a functor then the high is the address of the flash and the lower one is the address in RAM.
                                       if this is a function pointer then lower word is an address in flash and high is 0
};
*/

void revo_call_handler(uint64_t hh, uint32_t arg){
    Revo_handler h = { .h = hh };

    if(ADDRESS_IN_FLASH(h.w[0])){
        (h.isr)(arg);
    } else if(ADDRESS_IN_FLASH(h.w[1])) {
        (h.mpa)(arg);
    }
}

void hal_yield(uint16_t ttw){ Scheduler::yield(ttw); }
void hal_delay(uint16_t t){   Scheduler::_delay(t); }
void hal_delay_microseconds(uint16_t t){ Scheduler::_delay_microseconds(t);}

uint32_t hal_micros() { return Scheduler::_micros(); }
void hal_isr_time(uint32_t t) { s_running->in_isr += t; }

// task management for USB and another C code
void hal_set_task_active(void * h) { Scheduler::set_task_active(h); } 
void hal_context_switch_isr() { Scheduler::context_switch_isr(); }
void hal_set_task_priority(void * h, uint8_t prio) {Scheduler::set_task_priority(h, prio); }
void * hal_register_task(voidFuncPtr task, uint32_t stack) {
    Revo_handler r = { .vp=task };
    return Scheduler::_start_task(r.h, stack);
}

bool hal_is_armed() { return hal.util->get_soft_armed();  }
void hal_try_kill_task_or_reboot(uint8_t n) { Scheduler::_try_kill_task_or_reboot(n); }
void hal_go_next_task() { Scheduler::_go_next_task(); }
void hal_stop_multitask() { Scheduler::_stop_multitask(); }