New macro to fill a new process stack: in this way the stack is always aligned.

[bertos.git] / bertos / kern / proc.c

diff --git a/bertos/kern/proc.c b/bertos/kern/proc.c
index 9a1e410b11ffc60cb74df36ff4f07f95a6515a22..15fe5b7f79112bf6b36662611e603148692c6036 100644 (file)
--- a/bertos/kern/proc.c
+++ b/bertos/kern/proc.c
@@ -202,17 +202,23 @@ struct Process *proc_new_with_name(UNUSED_ARG(const char *, name), void (*entry)
        {
                proc = (Process *)stack_base;
                proc->stack = stack_base + PROC_SIZE_WORDS;
+               // On some architecture stack should be aligned, so we do it.
+               proc->stack = (cpu_stack_t *)((uintptr_t)proc->stack + (sizeof(cpu_aligned_stack_t) - ((uintptr_t)proc->stack % sizeof(cpu_aligned_stack_t))));
                if (CPU_SP_ON_EMPTY_SLOT)
                        proc->stack++;
        }
        else
        {
                proc = (Process *)(stack_base + stack_size / sizeof(cpu_stack_t) - PROC_SIZE_WORDS);
-               proc->stack = (cpu_stack_t *)proc;
+               // On some architecture stack should be aligned, so we do it.
+               proc->stack = (cpu_stack_t *)((uintptr_t)proc - ((uintptr_t)proc % sizeof(cpu_aligned_stack_t)));
                if (CPU_SP_ON_EMPTY_SLOT)
                        proc->stack--;
        }
+       /* Ensure stack is aligned */
+       ASSERT((uintptr_t)proc->stack % sizeof(cpu_aligned_stack_t) == 0);
 
+       stack_size -= PROC_SIZE_WORDS * sizeof(cpu_stack_t);
        proc_init_struct(proc);
        proc->user_data = data;
 
@@ -229,22 +235,14 @@ struct Process *proc_new_with_name(UNUSED_ARG(const char *, name), void (*entry)
 
                getcontext(&proc->context);
                proc->context.uc_stack.ss_sp = proc->stack;
-               proc->context.uc_stack.ss_size = stack_size - PROC_SIZE_WORDS - 1;
+               proc->context.uc_stack.ss_size = stack_size - 1;
                proc->context.uc_link = NULL;
                makecontext(&proc->context, (void (*)(void))proc_entry, 1, entry);
 
        #else // !CONFIG_KERN_PREEMPT
-       {
-               size_t i;
-
-               /* Initialize process stack frame */
-               CPU_PUSH_CALL_FRAME(proc->stack, proc_exit);
-               CPU_PUSH_CALL_FRAME(proc->stack, entry);
-
-               /* Push a clean set of CPU registers for asm_switch_context() */
-               for (i = 0; i < CPU_SAVED_REGS_CNT; i++)
-                       CPU_PUSH_WORD(proc->stack, CPU_REG_INIT_VALUE(i));
-       }
+       
+               CPU_CREATE_NEW_STACK(proc->stack, entry, proc_exit);
+               
        #endif // CONFIG_KERN_PREEMPT
 
        #if CONFIG_KERN_MONITOR
@@ -318,9 +316,9 @@ void proc_setPri(struct Process *proc, int pri)
 
                if (proc != CurrentProcess)
                {
-                               //proc_forbid();
-                               //TODO: re-enqueue process
-                               //pric_permit();
+                               proc_forbid();
+                               ATOMIC(sched_reenqueue(proc));
+                               proc_permit();
                }
 }
 #endif // CONFIG_KERN_PRI