diff --git a/CMakeLists.txt b/CMakeLists.txt
index e043e73..da26485 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -46,6 +46,8 @@ target_include_directories(
         freertos_config INTERFACE ${BSP_PATH}/include)
 # our compiler options, will trickle down through the project
 target_compile_options(freertos_config INTERFACE -c -fmessage-length=0 -g -O0 -mcpu=cortex-a9 -mfpu=vfpv3 -mfloat-abi=hard -ffunction-sections -fdata-sections)
+
+
 add_subdirectory(${FreeRTOS_PATH})
 
 add_subdirectory(bsp_z7)
@@ -57,7 +59,7 @@ add_subdirectory(${MISSION_PATH})
 # ##############################################################################
 
 # Add libraries for all sources.
-target_link_libraries(${TARGET_NAME} PRIVATE freertos_kernel ${CMAKE_SOURCE_DIR}/${BSP_PATH}/lib/libxil.a)
+target_link_libraries(${TARGET_NAME} PUBLIC freertos_kernel)
 
 # Add include paths for all sources.
 target_include_directories(
diff --git a/bsp_z7/CMakeLists.txt b/bsp_z7/CMakeLists.txt
index 15478b6..240bf63 100644
--- a/bsp_z7/CMakeLists.txt
+++ b/bsp_z7/CMakeLists.txt
@@ -1 +1,2 @@
-add_subdirectory(freeRTOS)
\ No newline at end of file
+add_subdirectory(freeRTOS)
+add_subdirectory(ps7_cortexa9_0)
\ No newline at end of file
diff --git a/bsp_z7/lib/libxil.a b/bsp_z7/lib/libxil.a
deleted file mode 120000
index 6304922..0000000
--- a/bsp_z7/lib/libxil.a
+++ /dev/null
@@ -1 +0,0 @@
-../ps7_cortexa9_0/lib/libxil.a
\ No newline at end of file
diff --git a/bsp_z7/ps7_cortexa9_0/CMakeLists.txt b/bsp_z7/ps7_cortexa9_0/CMakeLists.txt
new file mode 100644
index 0000000..8c8d789
--- /dev/null
+++ b/bsp_z7/ps7_cortexa9_0/CMakeLists.txt
@@ -0,0 +1,5 @@
+add_subdirectory(libsrc/scugic)
+add_subdirectory(libsrc/scutimer)
+add_subdirectory(libsrc/scuwdt)
+add_subdirectory(libsrc/standalone)
+add_subdirectory(libsrc/uartps)
\ No newline at end of file
diff --git a/bsp_z7/ps7_cortexa9_0/include/xparameters.h b/bsp_z7/ps7_cortexa9_0/include/xparameters.h
index 5d4b817..165806d 100644
--- a/bsp_z7/ps7_cortexa9_0/include/xparameters.h
+++ b/bsp_z7/ps7_cortexa9_0/include/xparameters.h
@@ -7,6 +7,7 @@
 
 /* Definition for CPU ID */
 #define XPAR_CPU_ID 0U
+#define USE_AMP 0U
 
 /* Definitions for peripheral PS7_CORTEXA9_0 */
 #define XPAR_PS7_CORTEXA9_0_CPU_CLK_FREQ_HZ 666666687
diff --git a/bsp_z7/ps7_cortexa9_0/lib/librsa.a b/bsp_z7/ps7_cortexa9_0/lib/librsa.a
deleted file mode 100644
index acef5d8..0000000
Binary files a/bsp_z7/ps7_cortexa9_0/lib/librsa.a and /dev/null differ
diff --git a/bsp_z7/ps7_cortexa9_0/lib/libxil.a b/bsp_z7/ps7_cortexa9_0/lib/libxil.a
deleted file mode 100644
index 721365e..0000000
Binary files a/bsp_z7/ps7_cortexa9_0/lib/libxil.a and /dev/null differ
diff --git a/bsp_z7/ps7_cortexa9_0/lib/libxilffs.a b/bsp_z7/ps7_cortexa9_0/lib/libxilffs.a
deleted file mode 100644
index 91e939c..0000000
Binary files a/bsp_z7/ps7_cortexa9_0/lib/libxilffs.a and /dev/null differ
diff --git a/bsp_z7/ps7_cortexa9_0/libsrc/scugic/CMakeLists.txt b/bsp_z7/ps7_cortexa9_0/libsrc/scugic/CMakeLists.txt
new file mode 100644
index 0000000..b52f7cb
--- /dev/null
+++ b/bsp_z7/ps7_cortexa9_0/libsrc/scugic/CMakeLists.txt
@@ -0,0 +1,8 @@
+target_sources(${TARGET_NAME} PUBLIC 
+    src/xscugic_g.c
+    src/xscugic.c
+    src/xscugic_intr.c
+    src/xscugic_selftest.c
+    src/xscugic_sinit.c
+    src/xscugic_hw.c
+)
\ No newline at end of file
diff --git a/bsp_z7/ps7_cortexa9_0/libsrc/scutimer/CMakeLists.txt b/bsp_z7/ps7_cortexa9_0/libsrc/scutimer/CMakeLists.txt
new file mode 100644
index 0000000..04f4872
--- /dev/null
+++ b/bsp_z7/ps7_cortexa9_0/libsrc/scutimer/CMakeLists.txt
@@ -0,0 +1,6 @@
+target_sources(${TARGET_NAME} PUBLIC 
+    src/xscutimer.c
+    src/xscutimer_sinit.c
+    src/xscutimer_g.c
+    src/xscutimer_selftest.c
+)
\ No newline at end of file
diff --git a/bsp_z7/ps7_cortexa9_0/libsrc/scuwdt/CMakeLists.txt b/bsp_z7/ps7_cortexa9_0/libsrc/scuwdt/CMakeLists.txt
new file mode 100644
index 0000000..462cc2d
--- /dev/null
+++ b/bsp_z7/ps7_cortexa9_0/libsrc/scuwdt/CMakeLists.txt
@@ -0,0 +1,6 @@
+target_sources(${TARGET_NAME} PUBLIC 
+    src/xscuwdt_sinit.c
+    src/xscuwdt_selftest.c
+    src/xscuwdt_g.c
+    src/xscuwdt.c
+)
\ No newline at end of file
diff --git a/bsp_z7/ps7_cortexa9_0/libsrc/standalone/CMakeLists.txt b/bsp_z7/ps7_cortexa9_0/libsrc/standalone/CMakeLists.txt
new file mode 100644
index 0000000..8c7ebb8
--- /dev/null
+++ b/bsp_z7/ps7_cortexa9_0/libsrc/standalone/CMakeLists.txt
@@ -0,0 +1,52 @@
+target_sources(${TARGET_NAME} PUBLIC 
+    src/translation_table.S
+    src/cpu_init.S
+    src/boot.S
+    src/xil-crt0.S
+    src/asm_vectors.S
+    src/open.c
+    src/xil_exception.c
+    src/sbrk.c
+    src/xl2cc_counter.c
+    src/xil_cache.c
+    src/xil_spinlock.c
+    src/sleep.c
+    src/fstat.c
+    src/fcntl.c
+    src/putnum.c
+    src/_open.c
+    src/xil_testcache.c
+    src/xil_assert.c
+    src/_exit.c
+    src/time.c
+    src/xil_testio.c
+    src/xplatform_info.c
+    src/getpid.c
+    src/write.c
+    src/xil_mem.c
+    src/kill.c
+    src/xil_mmu.c
+    src/lseek.c
+    src/xil_util.c
+    src/getentropy.c
+    src/xil_testmem.c
+    src/isatty.c
+    src/read.c
+    src/inbyte.c
+    src/close.c
+    src/xil_sleeptimer.c
+    src/print.c
+    src/xil_printf.c
+    src/xil_misc_psreset_api.c
+    src/usleep.c
+    src/xil_sleepcommon.c
+    src/vectors.c
+    src/cpputest_time.c
+    src/errno.c
+    src/xpm_counter.c
+    src/xtime_l.c
+    src/outbyte.c
+    src/unlink.c
+    src/abort.c
+    src/_sbrk.c
+)
\ No newline at end of file
diff --git a/bsp_z7/ps7_cortexa9_0/libsrc/uartps/CMakeLists.txt b/bsp_z7/ps7_cortexa9_0/libsrc/uartps/CMakeLists.txt
new file mode 100644
index 0000000..318bca1
--- /dev/null
+++ b/bsp_z7/ps7_cortexa9_0/libsrc/uartps/CMakeLists.txt
@@ -0,0 +1,9 @@
+target_sources(${TARGET_NAME} PUBLIC 
+    src/xuartps.c
+    src/xuartps_hw.c
+    src/xuartps_intr.c
+    src/xuartps_selftest.c
+    src/xuartps_options.c
+    src/xuartps_sinit.c
+    src/xuartps_g.c
+)
\ No newline at end of file
diff --git a/mission/CMakeLists.txt b/mission/CMakeLists.txt
index 43c1760..cf347ca 100644
--- a/mission/CMakeLists.txt
+++ b/mission/CMakeLists.txt
@@ -1 +1 @@
-target_sources(${TARGET_NAME} PRIVATE main.c)
\ No newline at end of file
+target_sources(${TARGET_NAME} PRIVATE main.cpp)
\ No newline at end of file
diff --git a/mission/main.c b/mission/main.cpp
similarity index 91%
rename from mission/main.c
rename to mission/main.cpp
index 5d2a187..dbd3f05 100644
--- a/mission/main.c
+++ b/mission/main.cpp
@@ -62,7 +62,9 @@
 
 /* Standard includes. */
 #include <limits.h>
+extern "C" {
 #include <stdio.h>
+}
 
 /* Scheduler include files. */
 #include "FreeRTOS.h"
@@ -81,7 +83,8 @@
 // #include "partest.h"
 
 /* Xilinx includes. */
-//#include "platform.h"
+// #include "platform.h"
+extern "C" {
 #include "xil_exception.h"
 #include "xparameters.h"
 #include "xscugic.h"
@@ -89,11 +92,15 @@
 #include "xuartps_hw.h"
 
 // TODO why? is in bsp
-void _exit(sint32 status) {
-  (void)status;
-  while (1) {
-    ;
-  }
+
+void _exit(sint32 status);
+
+//   (void)status;
+//   while (1) {
+//     ;
+//   }
+// }
+sint32 _write(sint32 fd, char8 *buf, sint32 nbytes);
 }
 
 /*
@@ -189,7 +196,7 @@ static void prvQueueSendTask(void *pvParameters) {
 
   /* Initialise xNextWakeTime - this only needs to be done once. */
   xNextWakeTime = xTaskGetTickCount();
-  vTaskDelayUntil(&xNextWakeTime, mainQUEUE_SEND_FREQUENCY_MS / 2);
+  // vTaskDelayUntil(&xNextWakeTime, mainQUEUE_SEND_FREQUENCY_MS / 2);
 
   for (;;) {
     /* Place this task in the blocked state until it is time to run again. */
@@ -219,7 +226,7 @@ static void prvQueueReceiveTask(void *pvParameters) {
     FreeRTOSConfig.h. */
     xQueueReceive(xQueue, &ulReceivedValue, portMAX_DELAY);
 
-    uart_send('c');
+    uart_send('0');
 
     /*  To get here something must have been received from the queue, but
     is it the expected value?  If it is, toggle the LED. */
@@ -233,13 +240,14 @@ static void prvQueueReceiveTask(void *pvParameters) {
 
 int main(void) {
 
-  //   *((uint32_t*)0xFFFFFFF0) = 0x030000A8;
-  //   asm("SEV");
-  //   //while(1);
-
   /* Configure the hardware ready to run the demo. */
   prvSetupHardware();
 
+  
+
+  printf("Booting Software\n");
+
+  
   /* Create the queue. */
   xQueue = xQueueCreate(mainQUEUE_LENGTH, sizeof(uint32_t));
 
@@ -274,7 +282,6 @@ int main(void) {
   a privileged mode (not user mode). */
   for (;;)
     ;
-
   /* Don't expect to reach here. */
   return 0;
 }
@@ -308,7 +315,7 @@ static void prvSetupHardware(void) {
   (void)xStatus; /* Remove compiler warning if configASSERT() is not defined. */
 
   /* Initialise the LED port. */
-  //vParTestInitialise();
+  // vParTestInitialise();
 
   /* The Xilinx projects use a BSP that do not allow the start up code to be
   altered easily.  Therefore the vector table used by FreeRTOS is defined in
@@ -436,58 +443,6 @@ is BAD! */
 }
 /*-----------------------------------------------------------*/
 
-void *memcpy(void *pvDest, const void *pvSource, size_t xBytes) {
-  /* The compiler used during development seems to err unless these volatiles
-  are included at -O3 optimisation.  */
-  volatile unsigned char *pcDest = (volatile unsigned char *)pvDest,
-                         *pcSource = (volatile unsigned char *)pvSource;
-  size_t x;
-
-  /* Extremely crude standard library implementations in lieu of having a C
-  library. */
-  if (pvDest != pvSource) {
-    for (x = 0; x < xBytes; x++) {
-      pcDest[x] = pcSource[x];
-    }
-  }
-
-  return pvDest;
-}
-/*-----------------------------------------------------------*/
-
-void *memset(void *pvDest, int iValue, size_t xBytes) {
-  /* The compiler used during development seems to err unless these volatiles
-  are included at -O3 optimisation.  */
-  volatile unsigned char *volatile pcDest =
-      (volatile unsigned char *volatile)pvDest;
-  volatile size_t x;
-
-  /* Extremely crude standard library implementations in lieu of having a C
-  library. */
-  for (x = 0; x < xBytes; x++) {
-    pcDest[x] = (unsigned char)iValue;
-  }
-
-  return pvDest;
-}
-/*-----------------------------------------------------------*/
-
-int memcmp(const void *pvMem1, const void *pvMem2, size_t xBytes) {
-  const volatile unsigned char *pucMem1 = pvMem1, *pucMem2 = pvMem2;
-  volatile size_t x;
-
-  /* Extremely crude standard library implementations in lieu of having a C
-  library. */
-  for (x = 0; x < xBytes; x++) {
-    if (pucMem1[x] != pucMem2[x]) {
-      break;
-    }
-  }
-
-  return xBytes - x;
-}
-/*-----------------------------------------------------------*/
-
 void vInitialiseTimerForRunTimeStats(void) {
   XScuWdt_Config *pxWatchDogInstance;
   uint32_t ulValue;
@@ -536,6 +491,13 @@ void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer,
 /* configUSE_STATIC_ALLOCATION and configUSE_TIMERS are both set to 1, so the
 application must provide an implementation of vApplicationGetTimerTaskMemory()
 to provide the memory that is used by the Timer service task. */
+
+extern "C" {
+void vApplicationGetTimerTaskMemory(StaticTask_t **ppxTimerTaskTCBBuffer,
+                                    StackType_t **ppxTimerTaskStackBuffer,
+                                    uint32_t *pulTimerTaskStackSize);
+}
+
 void vApplicationGetTimerTaskMemory(StaticTask_t **ppxTimerTaskTCBBuffer,
                                     StackType_t **ppxTimerTaskStackBuffer,
                                     uint32_t *pulTimerTaskStackSize) {