+// extern void task0(void), task1(void), task2(void);
+//
+// int size_stack = sizeof(task0_stack);
+//
+// p1.stack_ptr = ((unsigned int) task0_stack) + size_stack - 0x1C;
+// p1.stack_ptr[6] = (uint32_t) task0;
+// p1.stack_ptr[7] = 0x01000000;
+// p2.stack_ptr = ((unsigned int) task0_stack) + size_stack - 0x1C;
+// p2.stack_ptr[6] = (uint32_t) task1;
+// p2.stack_ptr[7] = 0x01000000;
+// p3.stack_ptr = ((unsigned int) task0_stack) + size_stack - 0x1C;
+// p3.stack_ptr[6] = (uint32_t) task2;
+// p3.stack_ptr[7] = 0x01000000;
+//
+// set_psp(p1.stack_ptr[-7]);
+//
+// for(;;);
+
+ /* Initialize processes */
+ //PSP_array[0] = ((unsigned int) task0_stack) + sizeof(task0_stack) - 32*4;
+ //HW_ADDR(PSP_array[0] + 0x18) = (unsigned long) task0;
+ //HW_ADDR(PSP_array[0] + 0x1C) = 0x01000000;
+ //PSP_array[1] = ((unsigned int) task1_stack) + sizeof(task1_stack) - 32*4;
+ //HW_ADDR(PSP_array[1] + 0x18) = (unsigned long) task1;
+ //HW_ADDR(PSP_array[1] + 0x1C) = 0x01000000;
+ //PSP_array[2] = ((unsigned int) task2_stack) + sizeof(task2_stack) - 32*4;
+ //HW_ADDR(PSP_array[2] + 0x18) = (unsigned long) task2;
+ //HW_ADDR(PSP_array[2] + 0x1C) = 0x01000000;
+
+ //extern void pendsv_handler;
+
+ ivt_set_gate(14, pendsv_handler_c, 0);
+
+ //set_psp((PSP_array[curr_task] + 32*4 ));
+
+ //int startpsp = PSP_array[curr_task] + 16*4;
+
+ //asm volatile ("msr psp, %0" : : "r" (startpsp));
+
+
+ //asm volatile ("mov r0, 0x3" "\n\t"
+ //"msr control, r0" "\n\t"
+ //"isb" "\n\t");
+
+ //for(;;);
+
+ //set current PSP
+ //printf("0: %x\n", PSP_array[0]);
+ //printf("1: %x", PSP_array[0]);
+
+ //task0();