Многопоточность Atmega не может манипулировать указателем стека

Question

Я пытаюсь сделать многопоточную библиотеку для контроллера atmega и сначала я пытаюсь манипулировать указателем стека и после возврата функции "go_to_func" программа переходит в функцию "func". Функция "go_to_func" записана в файле asm1.s, и это функция, в которой указатель стека модифицируется так, чтобы программа возвращала по начальному адресу "func", но моя программа не возвращает geo на "func" после возврата из "go_to_func"(когда программа поступает в "func", включается бит 3 PORTD). Может кто-нибудь сказать мне, что я должен написать на "go_to_func", чтобы моя программа работала нормально. Вот информация о дыре моего кода.

main.c

#include <avr/io.h>
#include <stdlib.h>
#include "asm1.h"


#define  CALL_FCT    __attribute__ ((noinline))

#define RETURN_SIZE 2

uint8_t fn_stack[128];

uint8_t addr_l;
uint8_t addr_h;

void  CALL_FCT  func( void )
{
    PORTD = 0x08 ;

    while(1)
    {

    }
}

void  CALL_FCT  init( void (*fn)(void), uint8_t* stack, uint16_t stack_size)
{
    *(stack + stack_size - RETURN_SIZE    ) = (uint16_t)fn >> 8;
    *(stack + stack_size - RETURN_SIZE + 1) = (uint8_t)(uint16_t)fn;

    addr_l = (uint8_t)(uint16_t)(stack + stack_size    );
    addr_h = (uint8_t)(uint16_t)(stack + stack_size + 1);

    go_to_func(addr_l,addr_h);
}

int  CALL_FCT  main(void)
{
    DDRD |= 0x0C ;

    init(func,fn_stack,sizeof(fn_stack));

    PORTD = 0x04 ;

    while (1)
    {
    }
}

asm1.h

#ifndef HEADER_H_
#define HEADER_H_

#include <stdint.h>

extern void go_to_func(uint8_t,uint8_t);

#endif /* HEADER_H_ */

asm1.s

 #include <avr/io.h>

 .global go_to_func
go_to_func:
  out _SFR_IO_ADDR(SPH), r22
  out _SFR_IO_ADDR(SPL), r24
  ret


/* *.lss the file of my program in assembly */
void  CALL_FCT  func( void )
{
    PORTD = 0x08 ;
  96:   88 e0           ldi r24, 0x08   ; 8
  98:   8b b9           out 0x0b, r24   ; 11
  9a:   ff cf           rjmp    .-2         ; 0x9a <func+0x4>

0000009c <init>:
    }
}

void  CALL_FCT  init( void (*fn)(void), uint8_t* stack, uint16_t stack_size)
{
     *(stack + stack_size - RETURN_SIZE    ) = (uint16_t)fn >> 8;
  9c:   9b 01           movw    r18, r22
  9e:   24 0f           add r18, r20
  a0:   35 1f           adc r19, r21
  a2:   f9 01           movw    r30, r18
  a4:   32 97           sbiw    r30, 0x02   ; 2
  a6:   90 83           st  Z, r25
     *(stack + stack_size - RETURN_SIZE + 1) = (uint8_t)(uint16_t)fn;
  a8:   31 96           adiw    r30, 0x01   ; 1
  aa:   80 83           st  Z, r24

     addr_l = (uint8_t)(uint16_t)(stack + stack_size    );
  ac:   20 93 01 01     sts 0x0101, r18 ; 0x800101 <addr_l>
     addr_h = (uint8_t)(uint16_t)(stack + stack_size + 1);
  b0:   4f 5f           subi    r20, 0xFF   ; 255
  b2:   5f 4f           sbci    r21, 0xFF   ; 255
  b4:   64 0f           add r22, r20
  b6:   75 1f           adc r23, r21
  b8:   60 93 00 01     sts 0x0100, r22 ; 0x800100 <_edata>

     go_to_func(addr_l,addr_h);
  bc:   82 2f           mov r24, r18
  be:   0e 94 48 00     call    0x90    ; 0x90 <go_to_func>
  c2:   08 95           ret

000000c4 <main>:
}

int  CALL_FCT  main(void)
{
    DDRD |= 0x0C ;
  c4:   8a b1           in  r24, 0x0a   ; 10
  c6:   8c 60           ori r24, 0x0C   ; 12
  c8:   8a b9           out 0x0a, r24   ; 10

    init(func,fn_stack,sizeof(fn_stack));
  ca:   40 e8           ldi r20, 0x80   ; 128
  cc:   50 e0           ldi r21, 0x00   ; 0
  ce:   62 e0           ldi r22, 0x02   ; 2
  d0:   71 e0           ldi r23, 0x01   ; 1
  d2:   8b e4           ldi r24, 0x4B   ; 75
  d4:   90 e0           ldi r25, 0x00   ; 0
  d6:   0e 94 4e 00     call    0x9c    ; 0x9c <init>

    PORTD = 0x04 ;
  da:   84 e0           ldi r24, 0x04   ; 4
  dc:   8b b9           out 0x0b, r24   ; 11
  de:   ff cf           rjmp    .-2         ; 0xde <main+0x1a>

000000e0 <_exit>:
  e0:   f8 94           cli

000000e2 <__stop_program>:
  e2:   ff cf           rjmp    .-2         ; 0xe2 <__stop_program>

Answer 1

addr_l = (uint8_t)(uint16_t)(stack + stack_size    );
addr_h = (uint8_t)(uint16_t)(stack + stack_size + 1);

ваш "addr_h" либо нуждается в некотором сдвиге, либо вы должны использовать r23 или r25 в вашей функции go_to_func(). (это не единственное, что не так, но это большое дело.) Я не знаю, почему вы не просто передаете полнометражное "int" ...

Вы знаете, что переключение задач намного больше, чем смена SP, верно? Вы ничего не сделали с регистрами, которые должны быть сохранены вызываемым пользователем...

Я бы предложил расположить новый стек и манипулировать им так же, как это делает процессор, с последующим уменьшением указателя для push...

void  CALL_FCT  init( void (*fn)(void), uint8_t* stack, uint16_t stack_size)
{
    uint8_t *newsp = (stack+stack_size) - 1;  // last byte of new stack
    *newsp-- = (uint8_t)(uint16_t)fn; // "push" low fn addr
    *newsp-- = (uint16_t)fn >> 8;  // "push" high fn addr
    go_to_func(newsp);  // newsp is now properly decremented for a "ret"
}

PS: самое время научиться использовать симулятор в AS7...