Can anyone here help my with this ?
Moderator edit: add url tags
If you want help post your code here and explain all about it here.
Have a look at the demo Several Things at a Time - much simpler.
...R
addr_l = (uint8_t)(uint16_t)(stack + stack_size );
addr_h = (uint8_t)(uint16_t)(stack + stack_size + 1);
your "addr_h" either needs some shifting, or you have to use r23 or r25 in your go_to_func() function.
I don't know why you're not just passing the full-length "int" ... (it's not the only thing wrong, but it's the big thing.)
You know that there's a lot more to switching tasks than changing the SP, right? You haven't done anything with the registers that are supposed to be saved by the callee...
PS: this is a good time to learn to use the simulator in AS7...
I'd suggest laying out and manipulating the new stack the same way that the processor does, with post-decrement of a pointer for 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"
}