diff --git a/CMakeLists.txt b/CMakeLists.txt
index 209ec44..aa6986d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -21,9 +21,9 @@ add_compile_options(
 option(BUILD_TESTS "Build unit tests" OFF)
 option(BUILD_EXAMPLES "Build examples" OFF)
 
-add_subdirectory(kernel)
-add_subdirectory(port/cortex_m4)
-add_subdirectory(hal/stm32f401)
+add_subdirectory(src/kernel)
+add_subdirectory(src/port/cortex_m4)
+add_subdirectory(src/hal/stm32f401)
 
 if(BUILD_TESTS)
     add_subdirectory(tests)
diff --git a/examples/blinky/CMakeLists.txt b/examples/blinky/CMakeLists.txt
index a525ea0..8bd4e4c 100644
--- a/examples/blinky/CMakeLists.txt
+++ b/examples/blinky/CMakeLists.txt
@@ -1,10 +1,10 @@
-add_executable(blinky main.c ${CMAKE_SOURCE_DIR}/startup/startup_stm32f401.c)
+add_executable(blinky main.c ${CMAKE_SOURCE_DIR}/src/startup/startup_stm32f401.c)
 
 target_link_libraries(blinky 
     hal_stm32f401
 )
 
 target_link_options(blinky PRIVATE
-    -T${CMAKE_SOURCE_DIR}/linker/stm32f401.ld
+    -T${CMAKE_SOURCE_DIR}/src/linker/stm32f401.ld
 )
 
diff --git a/examples/rtos_demo/CMakeLists.txt b/examples/rtos_demo/CMakeLists.txt
index 08cf938..4628794 100644
--- a/examples/rtos_demo/CMakeLists.txt
+++ b/examples/rtos_demo/CMakeLists.txt
@@ -1,10 +1,10 @@
-add_executable(rtos_demo main.c ${CMAKE_SOURCE_DIR}/startup/startup_stm32f401.c)
+add_executable(rtos_demo main.c ${CMAKE_SOURCE_DIR}/src/startup/startup_stm32f401.c)
 
 target_link_libraries(rtos_demo
     hal_stm32f401
 )
 
 target_link_options(rtos_demo PRIVATE
-    -T${CMAKE_SOURCE_DIR}/linker/stm32f401.ld
+    -T${CMAKE_SOURCE_DIR}/src/linker/stm32f401.ld
 )
 
diff --git a/port/cortex_m4/src/port.s b/port/cortex_m4/src/port.s
deleted file mode 100644
index 842f8f3..0000000
--- a/port/cortex_m4/src/port.s
+++ /dev/null
@@ -1,3 +0,0 @@
-PendSV_Handler:
-
-port_start_first_task:
\ No newline at end of file
diff --git a/hal/stm32f401/CMakeLists.txt b/src/hal/stm32f401/CMakeLists.txt
similarity index 100%
rename from hal/stm32f401/CMakeLists.txt
rename to src/hal/stm32f401/CMakeLists.txt
diff --git a/hal/stm32f401/include/cmsis_compiler.h b/src/hal/stm32f401/include/cmsis_compiler.h
similarity index 100%
rename from hal/stm32f401/include/cmsis_compiler.h
rename to src/hal/stm32f401/include/cmsis_compiler.h
diff --git a/hal/stm32f401/include/cmsis_gcc.h b/src/hal/stm32f401/include/cmsis_gcc.h
similarity index 100%
rename from hal/stm32f401/include/cmsis_gcc.h
rename to src/hal/stm32f401/include/cmsis_gcc.h
diff --git a/hal/stm32f401/include/core_cm4.h b/src/hal/stm32f401/include/core_cm4.h
similarity index 100%
rename from hal/stm32f401/include/core_cm4.h
rename to src/hal/stm32f401/include/core_cm4.h
diff --git a/hal/stm32f401/include/core_cmFunc.h b/src/hal/stm32f401/include/core_cmFunc.h
similarity index 100%
rename from hal/stm32f401/include/core_cmFunc.h
rename to src/hal/stm32f401/include/core_cmFunc.h
diff --git a/hal/stm32f401/include/core_cmInstr.h b/src/hal/stm32f401/include/core_cmInstr.h
similarity index 100%
rename from hal/stm32f401/include/core_cmInstr.h
rename to src/hal/stm32f401/include/core_cmInstr.h
diff --git a/hal/stm32f401/include/core_cmSimd.h b/src/hal/stm32f401/include/core_cmSimd.h
similarity index 100%
rename from hal/stm32f401/include/core_cmSimd.h
rename to src/hal/stm32f401/include/core_cmSimd.h
diff --git a/hal/stm32f401/include/fault_indicator.h b/src/hal/stm32f401/include/fault_indicator.h
similarity index 100%
rename from hal/stm32f401/include/fault_indicator.h
rename to src/hal/stm32f401/include/fault_indicator.h
diff --git a/hal/stm32f401/include/fpu.h b/src/hal/stm32f401/include/fpu.h
similarity index 100%
rename from hal/stm32f401/include/fpu.h
rename to src/hal/stm32f401/include/fpu.h
diff --git a/hal/stm32f401/include/gpio.h b/src/hal/stm32f401/include/gpio.h
similarity index 100%
rename from hal/stm32f401/include/gpio.h
rename to src/hal/stm32f401/include/gpio.h
diff --git a/hal/stm32f401/include/hal.h b/src/hal/stm32f401/include/hal.h
similarity index 100%
rename from hal/stm32f401/include/hal.h
rename to src/hal/stm32f401/include/hal.h
diff --git a/hal/stm32f401/include/rcc.h b/src/hal/stm32f401/include/rcc.h
similarity index 100%
rename from hal/stm32f401/include/rcc.h
rename to src/hal/stm32f401/include/rcc.h
diff --git a/hal/stm32f401/include/stm32f401xc.h b/src/hal/stm32f401/include/stm32f401xc.h
similarity index 100%
rename from hal/stm32f401/include/stm32f401xc.h
rename to src/hal/stm32f401/include/stm32f401xc.h
diff --git a/hal/stm32f401/src/fault_indicator.c b/src/hal/stm32f401/src/fault_indicator.c
similarity index 100%
rename from hal/stm32f401/src/fault_indicator.c
rename to src/hal/stm32f401/src/fault_indicator.c
diff --git a/hal/stm32f401/src/fpu.c b/src/hal/stm32f401/src/fpu.c
similarity index 100%
rename from hal/stm32f401/src/fpu.c
rename to src/hal/stm32f401/src/fpu.c
diff --git a/hal/stm32f401/src/gpio.c b/src/hal/stm32f401/src/gpio.c
similarity index 100%
rename from hal/stm32f401/src/gpio.c
rename to src/hal/stm32f401/src/gpio.c
diff --git a/hal/stm32f401/src/rcc.c b/src/hal/stm32f401/src/rcc.c
similarity index 100%
rename from hal/stm32f401/src/rcc.c
rename to src/hal/stm32f401/src/rcc.c
diff --git a/kernel/CMakeLists.txt b/src/kernel/CMakeLists.txt
similarity index 100%
rename from kernel/CMakeLists.txt
rename to src/kernel/CMakeLists.txt
diff --git a/kernel/include/mutex.h b/src/kernel/include/mutex.h
similarity index 100%
rename from kernel/include/mutex.h
rename to src/kernel/include/mutex.h
diff --git a/kernel/include/queue.h b/src/kernel/include/queue.h
similarity index 100%
rename from kernel/include/queue.h
rename to src/kernel/include/queue.h
diff --git a/kernel/include/scheduler.h b/src/kernel/include/scheduler.h
similarity index 100%
rename from kernel/include/scheduler.h
rename to src/kernel/include/scheduler.h
diff --git a/kernel/include/semaphore.h b/src/kernel/include/semaphore.h
similarity index 100%
rename from kernel/include/semaphore.h
rename to src/kernel/include/semaphore.h
diff --git a/kernel/include/task.h b/src/kernel/include/task.h
similarity index 100%
rename from kernel/include/task.h
rename to src/kernel/include/task.h
diff --git a/kernel/src/mutex.c b/src/kernel/src/mutex.c
similarity index 100%
rename from kernel/src/mutex.c
rename to src/kernel/src/mutex.c
diff --git a/kernel/src/queue.c b/src/kernel/src/queue.c
similarity index 100%
rename from kernel/src/queue.c
rename to src/kernel/src/queue.c
diff --git a/kernel/src/scheduler.c b/src/kernel/src/scheduler.c
similarity index 100%
rename from kernel/src/scheduler.c
rename to src/kernel/src/scheduler.c
diff --git a/kernel/src/semaphore.c b/src/kernel/src/semaphore.c
similarity index 100%
rename from kernel/src/semaphore.c
rename to src/kernel/src/semaphore.c
diff --git a/kernel/src/task.c b/src/kernel/src/task.c
similarity index 95%
rename from kernel/src/task.c
rename to src/kernel/src/task.c
index cd64680..2cc3916 100644
--- a/kernel/src/task.c
+++ b/src/kernel/src/task.c
@@ -1,7 +1,6 @@
 #include "task.h"
 #include "port.h"
 #include "fault_indicator.h"
-#include "systick.h"
 
 Task task_pool[MAX_TASKS];
 uint32_t stack_pool[MAX_TASKS][TASK_STACK_SIZE];
@@ -24,7 +23,6 @@ Task *task_create(void (*function)(void), uint8_t priority, const char *name) {
     Task *new_task = &task_pool[new_task_slot];
     port_init_task(new_task, priority, new_task_slot, name);
     port_init_task_stack(new_task, function);
-    systick_init();
     return new_task;
 }
 
diff --git a/linker/stm32f401.ld b/src/linker/stm32f401.ld
similarity index 100%
rename from linker/stm32f401.ld
rename to src/linker/stm32f401.ld
diff --git a/port/cortex_m4/CMakeLists.txt b/src/port/cortex_m4/CMakeLists.txt
similarity index 100%
rename from port/cortex_m4/CMakeLists.txt
rename to src/port/cortex_m4/CMakeLists.txt
diff --git a/port/cortex_m4/include/nvic.h b/src/port/cortex_m4/include/nvic.h
similarity index 100%
rename from port/cortex_m4/include/nvic.h
rename to src/port/cortex_m4/include/nvic.h
diff --git a/port/cortex_m4/include/port.h b/src/port/cortex_m4/include/port.h
similarity index 100%
rename from port/cortex_m4/include/port.h
rename to src/port/cortex_m4/include/port.h
diff --git a/port/cortex_m4/include/systick.h b/src/port/cortex_m4/include/systick.h
similarity index 100%
rename from port/cortex_m4/include/systick.h
rename to src/port/cortex_m4/include/systick.h
diff --git a/port/cortex_m4/src/nvic.c b/src/port/cortex_m4/src/nvic.c
similarity index 100%
rename from port/cortex_m4/src/nvic.c
rename to src/port/cortex_m4/src/nvic.c
diff --git a/port/cortex_m4/src/port.c b/src/port/cortex_m4/src/port.c
similarity index 100%
rename from port/cortex_m4/src/port.c
rename to src/port/cortex_m4/src/port.c
diff --git a/src/port/cortex_m4/src/port.s b/src/port/cortex_m4/src/port.s
new file mode 100644
index 0000000..e1f4a18
--- /dev/null
+++ b/src/port/cortex_m4/src/port.s
@@ -0,0 +1,85 @@
+.syntax unified
+.text
+
+.global PendSV_Handler
+.thumb_func
+PendSV_Handler:
+    /*  MRS: move contents of 
+        special register to a 
+        general purpose register */
+    MRS R0, PSP
+    /*  ARM assembly is beautiful,
+        wdym I can do R4-R11 at once,
+        also this is STM-DB, where
+        DB is decrement before each access */
+    STMDB R0!, {R4-R11}
+
+    /* Handle saving PSP */
+
+    /*  Get pointer to current task */
+    LDR R1, =current_task
+    /* Dereference pointer */
+    LDR R1, [R1]
+    /*  current_task->stack_ptr = R0 */
+    STR R0, [R1, #0]
+
+    /* Get Next Task*/
+    BL scheduler_select_next_task
+
+    /* Load new stack_ptr into PSP */
+    LDR R0, =current_task
+    /* Dereference ptr */
+    LDR R0, [R0]
+    /* Load stack ptr */
+    LDR R0, [R0, #0]
+    /* Pop Registers */
+    LDMIA R0!, {R4-R11}
+    /*  Need to return from
+        PSP to thread mode,
+        0xFFFFFFFD returns 
+        to Thread from PSP */
+    MSR PSP, R0
+
+    MOV LR, #0xFFFFFFFD
+    BX LR
+
+/*  Function to manually bootstrap task
+    loop on scheduler start */
+.global port_start_first_task
+.thumb_func
+port_start_first_task:
+    BL scheduler_select_next_task
+    SVC #0
+
+/*  Need SVC exception to 
+    get into Handler mode */
+.global SVC_Handler
+.thumb_func
+SVC_Handler:
+    /* Load new stack_ptr into PSP */
+    LDR R0, =current_task
+    /* Dereference ptr */
+    LDR R0, [R0]
+    /* Load stack ptr */
+    LDR R0, [R0, #0]
+
+    /* Load Fake Context from stack */
+
+    /*  Load all registers of fake frame*/
+    LDMIA R0!, {R4-R11}
+    /*  Store updated PSP back */
+    MSR PSP, R0
+
+    /* Switch from Thread mode to PSP */
+
+    /*  CONTROl register lets me 
+        designate which stack to use */
+    MOV R1, #2
+    MSR CONTROL, R1
+    /* ISB is used to refresh & sync */
+    ISB
+
+    /* Return to Thread Mode */
+
+    MOV LR, #0xFFFFFFFD
+    BX LR
diff --git a/port/cortex_m4/src/systick.c b/src/port/cortex_m4/src/systick.c
similarity index 100%
rename from port/cortex_m4/src/systick.c
rename to src/port/cortex_m4/src/systick.c
diff --git a/startup/startup_stm32f401.c b/src/startup/startup_stm32f401.c
similarity index 100%
rename from startup/startup_stm32f401.c
rename to src/startup/startup_stm32f401.c
index 2692279..3ee7405 100644
--- a/startup/startup_stm32f401.c
+++ b/src/startup/startup_stm32f401.c
@@ -105,11 +105,11 @@ void (* const vector_table[])(void) = {
     BusFault_Handler,
     UsageFault_Handler,
     0,
-    SVC_Handler,
-    DebugMon_Handler,
     0,
     0,
     0,
+    SVC_Handler,
+    DebugMon_Handler,
     0,
     PendSV_Handler,
     SysTick_Handler,