Как сделать context switch на STM32

в 11:24, , рубрики: C, cortex-m, embox, freertos, Nuttx, open source, stm32, Блог компании Embox, микроконтроллеры, операционные системы, осрв, программирование микроконтроллеров, системное программирование

Как сделать context switch на STM32 - 1
Добрый день!
Потоки… Переключение контекстов… Базовая сущность ОС. И конечно, при разработке библиотек и приложений мы всегда полагаемся на то, что реализация потоков безошибочна. Поэтому было неожиданно найти грубую ошибку в переключении потоков для STM32 на ОСРВ Embox, когда уже продолжительное время работали и сеть, и файловая система и многие сторонние библиотеки. И мы даже успели похвастаться о своих достижениях на хабре.

Я бы хотел рассказать про то, как мы делали переключение потоков для Cortex-M, и тестировали на STM32. Кроме того, постараюсь рассказать о том как это сделано в других ОС — NuttX и FreeRTOS.

Ну что ж, сначала пара слов о том, как обнаружилась проблема. В тот момент я собирал очередную поделку — робота с разными датчиками. В какой-то момент мне захотелось поуправлять двумя шаговыми двигателями, при этом каждый управлялся из отдельного потока (потоки абсолютно идентичны). Итог — пока один мотор не закончит вращение, второй даже не начинал.

Я сел за отладку. Оказалось, что в потоках просто отключены все прерывания! Вы скажете, а как вообще могло тогда что-то работать? Все просто — много где есть sleep(), mutex_lock() и прочие “wait”, и за счет них потоки естественным образом и переключались. Проблема была, очевидно, связана с переключением контекстов для STM32F4, на которой я ее и обнаружил.

Давайте детальнее разберем проблему. Переключение контекстов потоков происходит в том числе по таймеру, то есть по прерываниям. Схематично обработку прерывания в Embox можно представить так:

void irq_handler(pt_regs_t *regs) {
        ...
    int irq = get_irq_number(regs);
    {
        ipl_enable();
        irq_dispatch(irq);
        ipl_disable();
    }
    irqctrl_eoi(irq);
        ...
    critical_dispatch_pending();
}

Вся суть в том, что сначала вызывается обработчик прерывания irq_dispatch, после этого “заканчивается” обработка прерывания, и контекст переключается на другой поток, если планировщик этого требует внутри critical_dispatch_pending. И тут очень важно, что состояние процессора в данном потоке должно быть такое же как и до того как его прервали, включая разрешение или запрещение прерываний. За разрешение прерываний отвечает бит в xPSR, который укладывается на стек самим процессором во время входа в прерывание, при выходе из прерываний он достается со стека. Проблема заключается в том, что так как мы имеем вытесняющую многозадачность, мы можем, войдя в прерывании на одном потоке, захотеть выйти на стеке другого потока, в котором конечно нет сохраненного xPSR. Более того, как и большинство ОС, мы имеем примитивы синхронизации, например, pthread_mutex_lock(), которые могут привести к переключению контекста не из прерывания. Мы вообще стали сомневаться, можно ли на cortex-m организовать вытесняющую многозадачность, ведь эта архитектура хорошо оптимизирована под небольшие задачи. Но стоп! А как же тогда работают другие операционки?

Обработка прерываний на Cortex-M

Давайте для начала разберемся как устроена обработка прерываний на Cortex-M.
Как сделать context switch на STM32 - 2
На картинке показаны стеки в двух режимах — с плавающей точкой и без нее. Когда происходит прерывание, процессор сохраняет на стек соответствующие регистры, а в регистр LR помещает одно из следующих значений приведенных в таблице ниже. То есть, если прерывание вложенное, то там будет 0xFFFFFFF1.
Как сделать context switch на STM32 - 3
Далее вызывается обработчик прерывания ОС, в конце которого обычно выполняется “bx lr” (напомним, что в LR находится 0xFFFFFFXX). После этого восстанавливаются автоматически сохраненные регистры, и исполнение программы продолжается.

Теперь рассмотрим, как же происходит переключение контекстов в разных ОС.

FreeRTOS

Давайте начнем с FreeRTOS. Для этого заглянем в portable/GCC/ARM_CM4F/port.c. Ниже представлен код функции xPortSysTickHandler:

xPortSysTickHandler

void xPortSysTickHandler( void )
{
    /* The SysTick runs at the lowest interrupt priority, so when this interrupt
    executes all interrupts must be unmasked.  There is therefore no need to
    save and then restore the interrupt mask value as its value is already
    known. */
    portDISABLE_INTERRUPTS();
    {
        /* Increment the RTOS tick. */
        if( xTaskIncrementTick() != pdFALSE )
        {
            /* A context switch is required.  Context switching is performed in
            the PendSV interrupt.  Pend the PendSV interrupt. */
            portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT;
        }
    }
    portENABLE_INTERRUPTS();
}

Это обработчик аппаратного таймера. Здесь мы видим, что если нужно сделать переключение контекстов, то инициируется некое прерывание PendSV. Как говорит документация — “PendSV is an interrupt-driven request for system-level service. In an OS environment, use PendSV for context switching when no other exception is active.” Внутри обработчика прерывания xPortPendSVHandler непосредственно и происходит переключение контекстов:

xPortPendSVHandler

void xPortPendSVHandler( void )
{
    /* This is a naked function. */
 
    __asm volatile
    (
    "   mrs r0, psp                         n"
    "   isb                                 n"
    "                                       n"
    "   ldr r3, pxCurrentTCBConst           n" /* Get the location of the current TCB. */
    "   ldr r2, [r3]                        n"
    "                                       n"
    "   tst r14, #0x10                      n" /* Is the task using the FPU context?  If so, push high vfp registers. */
    "   it eq                               n"
    "   vstmdbeq r0!, {s16-s31}             n"
    "                                       n"
    "   stmdb r0!, {r4-r11, r14}            n" /* Save the core registers. */
    "                                       n"
    "   str r0, [r2]                        n" /* Save the new top of stack into the first member of the TCB. */
    "                                       n"
    "   stmdb sp!, {r3}                     n"
    "   mov r0, %0                          n"
    "   msr basepri, r0                     n"
    "   dsb                                 n"
    "   isb                                 n"
    "   bl vTaskSwitchContext               n"
    "   mov r0, #0                          n"
    "   msr basepri, r0                     n"
    "   ldmia sp!, {r3}                     n"
    "                                       n"
    "   ldr r1, [r3]                        n" /* The first item in pxCurrentTCB is the task top of stack. */
    "   ldr r0, [r1]                        n"
    "                                       n"
    "   ldmia r0!, {r4-r11, r14}            n" /* Pop the core registers. */
    "                                       n"
    "   tst r14, #0x10                      n" /* Is the task using the FPU context?  If so, pop the high vfp registers too. */
 
    "   it eq                               n"
    "   vstmdbeq r0!, {s16-s31}             n"
    "                                       n"
    "   stmdb r0!, {r4-r11, r14}            n" /* Save the core registers. */
    "                                       n"
    "   str r0, [r2]                        n" /* Save the new top of stack into the first member of the TCB. */
    "                                       n"
    "   stmdb sp!, {r3}                     n"
    "   mov r0, %0                          n"
    "   msr basepri, r0                     n"
    "   dsb                                 n"
    "   isb                                 n"
    "   bl vTaskSwitchContext               n"
    "   mov r0, #0                          n"
    "   msr basepri, r0                     n"
    "   ldmia sp!, {r3}                     n"
    "                                       n"
    "   ldr r1, [r3]                        n" /* The first item in pxCurrentTCB is the task top of stack. */
    "   ldr r0, [r1]                        n"
    "                                       n"
    "   ldmia r0!, {r4-r11, r14}            n" /* Pop the core registers. */
    "                                       n"
    "   tst r14, #0x10                      n" /* Is the task using the FPU context?  If so, pop the high vfp registers too. */
    "   it eq                               n"
    "   vldmiaeq r0!, {s16-s31}             n"
    "                                       n"
    "   msr psp, r0                         n"
    "   isb                                 n"
    "                                       n"
    #ifdef WORKAROUND_PMU_CM001 /* XMC4000 specific errata workaround. */
        #if WORKAROUND_PMU_CM001 == 1
    "           push { r14 }                n"
    "           pop { pc }                  n"
        #endif
    #endif
    "                                       n"
    "   bx r14                              n"
    "                                       n"
    "   .align 4                            n"
    "pxCurrentTCBConst: .word pxCurrentTCB  n"
    ::"i"(configMAX_SYSCALL_INTERRUPT_PRIORITY)
    );
}

Но теперь давайте представим, что мы переключаемся на новый поток, который будет исполнять, скажем, некую функцию fn. То есть если мы просто поместим в PC адрес функции fn, то сразу же попадем в правильное место, но с неправильным контекстом — из прерывания же мы не вышли! FreeRTOS предлагает следующее решение. Давайте изначально проинициализируем создаваемый поток так, как если бы мы выходили из прерывания — /* Simulate the stack frame as it would be created by a context switch interrupt. */. В таком случае мы сначала “по-честному” выйдем из обработчика xPortPendSVHandler, то есть окажемся в правильном контексте, после чего, следуя по подготовленному стеку, попадем в fn. Ниже приведен код такой подготовки потока:

pxPortInitialiseStack

StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters )
{
    /* Simulate the stack frame as it would be created by a context switch
    interrupt. */
 
    /* Offset added to account for the way the MCU uses the stack on entry/exit
    of interrupts, and to ensure alignment. */
    pxTopOfStack--;
 
    *pxTopOfStack = portINITIAL_XPSR;   /* xPSR */
    pxTopOfStack--;
    *pxTopOfStack = ( ( StackType_t ) pxCode ) & portSTART_ADDRESS_MASK;    /* PC */
    pxTopOfStack--;
    *pxTopOfStack = ( StackType_t ) portTASK_RETURN_ADDRESS;    /* LR */
 
    /* Save code space by skipping register initialisation. */
    pxTopOfStack -= 5;  /* R12, R3, R2 and R1. */
    *pxTopOfStack = ( StackType_t ) pvParameters;   /* R0 */
 
    /* A save method is being used that requires each task to maintain its
    own exec return value. */
    pxTopOfStack--;
    *pxTopOfStack = portINITIAL_EXEC_RETURN;
 
    pxTopOfStack -= 8;  /* R11, R10, R9, R8, R7, R6, R5 and R4. */
 
    return pxTopOfStack;
}

Итак, это был один из способов, предложенный во FreeRTOS.

NuttX

Давайте теперь посмотрим на другой метод, предложенный в NuttX. Это еще одна относительная известная ОС для разных мелких железок.

Главная часть обработки прерывания происходит внутри функции up_doirq, это по сути обработчик прерывания второго уровня, вызывается из ассемблерного кода. В нем происходит решение того надо ли переключаться на другой поток. Данная функция вернет необходимый контекст нового потока.

up_doirq

uint32_t *up_doirq(int irq, uint32_t *regs)
{
  board_autoled_on(LED_INIRQ);
#ifdef CONFIG_SUPPRESS_INTERRUPTS
  PANIC();
#else
  uint32_t *savestate;
 
  /* Nested interrupts are not supported in this implementation.  If you want
   * to implement nested interrupts, you would have to (1) change the way that
   * CURRENT_REGS is handled and (2) the design associated with
   * CONFIG_ARCH_INTERRUPTSTACK.  The savestate variable will not work for
   * that purpose as implemented here because only the outermost nested
   * interrupt can result in a context switch.
   */
 
  /* Current regs non-zero indicates that we are processing an interrupt;
   * CURRENT_REGS is also used to manage interrupt level context switches.
   */
 
  savestate    = (uint32_t *)CURRENT_REGS;
  CURRENT_REGS = regs;
 
  /* Acknowledge the interrupt */
 
  up_ack_irq(irq);
 
  /* Deliver the IRQ */
 
  irq_dispatch(irq, regs);
 
  /* If a context switch occurred while processing the interrupt then
   * CURRENT_REGS may have change value.  If we return any value different
   * from the input regs, then the lower level will know that a context
   * switch occurred during interrupt processing.
   */
 
  regs = (uint32_t *)CURRENT_REGS;
 
  /* Restore the previous value of CURRENT_REGS.  NULL would indicate that
   * we are no longer in an interrupt handler.  It will be non-NULL if we
   * are returning from a nested interrupt.
   */
 
  CURRENT_REGS = savestate;
#endif
  board_autoled_off(LED_INIRQ);
  return regs;
}

После возврата из функции мы снова оказываемся в обработчике первого уровня. И если нужно переключаться на новый поток, то модифицируем на стеке автоматически сохраненные при входе в прерывание регистры так, чтобы по завершении обработки прерывания попасть в нужный поток. Ниже приведен фрагмент кода.

    bl      up_doirq                /* R0=IRQ, R1=register save (msp) */
    mov     r1, r4                  /* Recover R1=main stack pointer */
 
    /* On return from up_doirq, R0 will hold a pointer to register context
     * array to use for the interrupt return.  If that return value is the same
     * as current stack pointer, then things are relatively easy.
     */
 
    cmp     r0, r1                  /* Context switch? */
    beq     l2                      /* Branch if no context switch */
	//Далее копируем регистры
…
    /* We are returning with a pending context switch.  This case is different
     * because in this case, the register save structure does not lie in the
     * stack but, rather, within a TCB structure.  We'll have to copy some
     * values to the stack.
     */
 
    add     r1, r0, #SW_XCPT_SIZE   /* R1=Address of HW save area in reg array */
    ldmia   r1, {r4-r11}            /* Fetch eight registers in HW save area */
    ldr     r1, [r0, #(4*REG_SP)]   /* R1=Value of SP before interrupt */
    stmdb   r1!, {r4-r11}           /* Store eight registers in HW save area */
#ifdef CONFIG_BUILD_PROTECTED
    ldmia   r0, {r2-r11,r14}        /* Recover R4-R11, r14 + 2 temp values */
#else
    ldmia   r0, {r2-r11}            /* Recover R4-R11 + 2 temp values */
#endif
	…

То есть в Nuttx (в отличие от FreeRTOS) уже модифицируются автоматически сохраненные на стек значения регистров. Это, пожалуй, основное отличие. Кроме того, можно заметить, что они прекрасно обходятся без PendSV (хотя ARM рекомендует :) ). Ну и последнее — само переключение контекстов у них отложенное, происходит через стек прерывания, а не по принципу — “сохранили старые значения и тут же загрузили в регистры новые”.

Embox

Наконец, про то как это сделано в Embox. Основная идея заключается в том, чтобы добавить некоторую дополнительную функцию (назовем ее __irq_trampoline), в которой сделать переключение контекстов уже “в обычном режиме”, а не в режиме обработки прерывания, и после этого уже по-настоящему выйти из обработчика прерывания. То есть, иными словами, мы постарались полностью сохранить логику, описанную в начале статьи:

void irq_handler(pt_regs_t *regs) {
          ...
    int irq = get_irq_number(regs);
    {
          ipl_enable();
          irq_dispatch(irq);
          ipl_disable();
    }
    irqctrl_eoi(irq); // Только тут теперь будет небольшая хитрость, а не прямой вызов
          ...
}

Для начала приведу рисунок, на котором представлена картина в целом. А далее объясню по частям что есть что.
Как сделать context switch на STM32 - 4
Как это делается? Идея в следующем. Обработчик прерывания сначала выполняется обычным образом, как и на других платформах. Но при выходе из обработчика мы на самом деле модифицируем стек и выходим совсем в другое место — в __pending_handle! При этом происходит это так, как если бы прерывание действительно случилось на входе функции __pending_handle. Ниже приведен код, который модифицирует стек, чтобы выйти в __pending_handle. Я постарался написать к особо важным местам комменты на русском.

// Регистры сохраняемые процессором при входе в прерывание
struct cpu_saved_ctx {
    uint32_t r[5];
    uint32_t lr;
    uint32_t pc;
    uint32_t psr;
};
 
void interrupt_handle(struct context *regs) {
    uint32_t source;
    struct irq_saved_state state;
    struct cpu_saved_ctx *ctx;
 
    ... // Тут обычная обработка прерывания, пропустим
 
    state.sp = regs->sp;
    state.lr = regs->lr;
    assert(!interrupted_from_fpu_mode(state.lr));
    ctx = (struct cpu_saved_ctx*) state.sp;
    memcpy(&state.ctx, ctx, sizeof *ctx);
 
    // Ниже показано то как мы модифицируем стек
    /* It does not matter what value of psr is, just set up sime correct value.
     * This value is only used to go further, after return from interrupt_handle.
     * 0x01000000 is a default value of psr and (ctx->psr & 0xFF) is irq number if any. */
    ctx->psr = 0x01000000 | (ctx->psr & 0xFF);
    ctx->r[0] = (uint32_t) &state; // we want pass the state to __pending_handle()
    ctx->r[1] = (uint32_t) regs; // we want pass the registers to __pending_handle()
    ctx->lr = (uint32_t) __pending_handle;
    ctx->pc = ctx->lr;
 
    /* Now return from interrupt context into __pending_handle */
    __irq_trampoline(state.sp, state.lr);
}

Также приведем код функции __irq_trampoline. В комментариях к функции указано про чит с SP, но чтобы не перегружать статью я это пропускаю. Главное — это “bx r1” в конце функции. Напомню, что в регистре r1 находится второй аргумент функции __irq_trampoline. Если посмотреть код выше, то мы увидим вызов “__irq_trampoline(state.sp, state.lr)”, а это значит, что в регистре r1 находится значение state.lr, которое равно значению 0xFFFFFXX (см. Первый раздел)

__irq_trampoline

.global __irq_trampoline
__irq_trampoline:
 
    cpsid  i
    # r0 contains SP stored on interrupt handler entry. So we keep some data
    # behind SP for a while, but interrupts are disabled by 'cpsid i'
    mov    sp,  r0
    # Return from interrupt handling to usual mode
    bx     r1

Короче говоря, после выхода из функции __irq_trampoline мы раскручиваемся по стеку, выходим из прерывания и попадаем в __pending_handle. В этой функции мы делаем все оставшиеся операции (такие как context switch). При этом при выходе из этой функции нам необходимо вернуть на стек первоначально сохраненные значения регистров, после чего снова войти в прерывание и выйти из него, но уже в первоначальном месте! Для это делается следующая вещь. Мы сначала подготавливаем стек, затем инициируем прерывание PendSV, после чего оказываемся в обработчике __pendsv_handle. А далее обычным способом по-честному выходим из обработчика, но уже по первоначальному старому стеку. Код функций __pending_handle и __pendsv_handle приведен ниже:

__pending_handle и __pendsv_handle

.global __pending_handle
__pending_handle:
    // Тут выгружаем на стек “старый” контекст, чтобы выйти из прерывания
    // уже по-честному, то есть туда где нас изначально прервали.
    # Push initial saved context (state.ctx) on top of the stack
    add    r0, #32
    ldmdb  r0, {r4 - r11}
    push   {r4 - r11}
 
    // Тут восстанавливаем некоторые регистры. Но это не очень значимая деталь,
    // Для понимания эта деталь не важна, пропустим.
    ...
 
    cpsie  i
    // Вот тут переключаем контексты, если требуется
    bl     critical_dispatch_pending
    cpsid  i
    # Generate PendSV interrupt
    // Тут инициируем прерывание PendSV, обработчик приведен ниже
    bl     nvic_set_pendsv
    cpsie  i
    # DO NOT RETURN
1: b       1
 
.global __pendsv_handle
__pendsv_handle:
 
    # 32 == sizeof (struct cpu_saved_ctx)
    add    sp, #32
    # Return to the place we were interrupted at,
    # i.e. before interrupt_handle_enter
    bx     r14

В заключение скажу пару фраз о рассмотренных версиях реализации context_switch. Каждый из рассмотренных методов рабочий, имеет свои достоинства и недостатки. Нам не очень подходит вариант FreeRTOS, так как эта ОС, направлена прежде всего на микроконтроллеры, что влечет за собой некую “захардкоженность” context_switch под конкретный чип. А мы в своей ОС пытаемся предложить даже для микроконтроллеров использовать принципы “большой” ОС, со всеми вытекающими … Приблизительно такой же подход у NuttX, и может быть нам удастся либо реализовать подобный подход, либо улучшить наш с помощью идеи модификации стека. Но на данный момент наша версия вполне справляется со своими задачами, в чем можно убедиться если взять код из репозитория.

Автор: Embox

Источник

Поделиться

* - обязательные к заполнению поля