diff --git a/stm32/aioc-fw/Src/led.c b/stm32/aioc-fw/Src/led.c
new file mode 100644
index 0000000..1cbee73
--- /dev/null
+++ b/stm32/aioc-fw/Src/led.c
@@ -0,0 +1,31 @@
+#include "led.h"
+#include "stm32f3xx_hal.h"
+
+void LED_Init(void)
+{
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+
+    /* Configure LED pins as TIM4 CH3/CH4 */
+    GPIO_InitTypeDef GpioLedInit = {
+        .Pin = LED_GPIO_PIN1 | LED_GPIO_PIN2,
+        .Mode = GPIO_MODE_AF_PP,
+        .Pull = GPIO_NOPULL,
+        .Speed = GPIO_SPEED_FREQ_LOW,
+        .Alternate = GPIO_AF2_TIM4
+    };
+
+    HAL_GPIO_Init(LED_GPIO, &GpioLedInit);
+
+    /* Initialize Timer */
+    __HAL_RCC_TIM4_CLK_ENABLE();
+
+    LED_TIMER->CR1 = TIM_CLOCKDIVISION_DIV1 | TIM_COUNTERMODE_UP | TIM_AUTORELOAD_PRELOAD_ENABLE;
+    LED_TIMER->ARR = 511;
+    LED_TIMER->PSC = (HAL_RCCEx_GetPeriphCLKFreq(RCC_PERIPHCLK_TIM1) / 500000) - 1;
+    LED_TIMER->CCMR2 =  TIM_OCMODE_PWM1 << 8 | TIM_OCMODE_PWM1 | TIM_CCMR2_OC3PE | TIM_CCMR2_OC4PE;
+    LED_TIMER->CCER = (0 << TIM_CCER_CC3P_Pos) | (1 << TIM_CCER_CC4P_Pos) | TIM_CCER_CC3E | TIM_CCER_CC4E;
+    LED_TIMER->CCR3 = 0;
+    LED_TIMER->CCR4 = 512;
+    LED_TIMER->EGR = TIM_EGR_UG;
+    LED_TIMER->CR1 |= TIM_CR1_CEN;
+}
diff --git a/stm32/aioc-fw/Src/led.h b/stm32/aioc-fw/Src/led.h
new file mode 100644
index 0000000..58672c5
--- /dev/null
+++ b/stm32/aioc-fw/Src/led.h
@@ -0,0 +1,16 @@
+#ifndef LED_H_
+#define LED_H_
+
+#include "stm32f3xx_hal.h"
+
+#define LED_TIMER       TIM4
+#define LED_GPIO        GPIOB
+#define LED_GPIO_PIN1   GPIO_PIN_8
+#define LED_GPIO_PIN2   GPIO_PIN_9
+
+#define LED_SET1(x) LED_TIMER->CCR3 = ( ((uint32_t) ((x) * 255)) & 0xFF )
+#define LED_SET2(x) LED_TIMER->CCR4 = (512 - ( ((uint32_t) ((x) * 255)) & 0xFF ))
+
+void LED_Init(void);
+
+#endif /* LED_H_ */
diff --git a/stm32/aioc-fw/Src/main.c b/stm32/aioc-fw/Src/main.c
index 3c05bd2..7adf64e 100644
--- a/stm32/aioc-fw/Src/main.c
+++ b/stm32/aioc-fw/Src/main.c
@@ -1,10 +1,7 @@
 #include "stm32f3xx.h"
 #include "stm32f3xx_hal.h"
-#include "stm32f3xx_hal_rcc.h"
-#include "stm32f3xx_hal_rcc_ex.h"
-#include "stm32f3xx_hal_gpio.h"
-#include "stm32f3xx_hal_tim.h"
-#include "tusb.h"
+#include "led.h"
+#include "usb.h"
 
 static void SystemClock_Config(void)
 {
@@ -60,134 +57,7 @@ static void SystemClock_Config(void)
     HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_PLLCLK_DIV2, RCC_MCODIV_1);
 }
 
-#define LED1_SET(x) TIM4->CCR3 = ( ((uint32_t) ((x) * 255)) & 0xFF )
-#define LED2_SET(x) TIM4->CCR4 = (512 - ( ((uint32_t) ((x) * 255)) & 0xFF ))
 
-static void LED_Init(void)
-{
-    __HAL_RCC_GPIOB_CLK_ENABLE();
-
-    /* Configure LED pins as TIM4 CH3/CH4 */
-    GPIO_InitTypeDef GpioLedInit = {
-        .Pin = GPIO_PIN_8 | GPIO_PIN_9,
-        .Mode = GPIO_MODE_AF_PP,
-        .Pull = GPIO_NOPULL,
-        .Speed = GPIO_SPEED_FREQ_LOW,
-        .Alternate = GPIO_AF2_TIM4
-    };
-
-    HAL_GPIO_Init(GPIOB, &GpioLedInit);
-
-    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_RESET);
-    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_SET);
-
-    /* Initialize Timer */
-	__HAL_RCC_TIM4_CLK_ENABLE();
-
-	TIM4->CR1 = TIM_CLOCKDIVISION_DIV1 | TIM_COUNTERMODE_UP | TIM_AUTORELOAD_PRELOAD_ENABLE;
-	TIM4->ARR = 511;
-	TIM4->PSC = (HAL_RCCEx_GetPeriphCLKFreq(RCC_PERIPHCLK_TIM1) / 500000) - 1;
-	TIM4->CCMR2 =  TIM_OCMODE_PWM1 << 8 | TIM_OCMODE_PWM1 | TIM_CCMR2_OC3PE | TIM_CCMR2_OC4PE;
-	TIM4->CCER = (0 << TIM_CCER_CC3P_Pos) | (1 << TIM_CCER_CC4P_Pos) | TIM_CCER_CC3E | TIM_CCER_CC4E;
-	TIM4->CCR3 = 0;
-	TIM4->CCR4 = 512;
-	TIM4->EGR = TIM_EGR_UG;
-	TIM4->CR1 |= TIM_CR1_CEN;
-}
-
-static void USB_Init(void)
-{
-	  __HAL_REMAPINTERRUPT_USB_ENABLE();
-
-	  /* Configure USB DM and DP pins */
-	  __HAL_RCC_GPIOA_CLK_ENABLE();
-	  GPIO_InitTypeDef GPIO_InitStruct;
-	  GPIO_InitStruct.Pin = (GPIO_PIN_11 | GPIO_PIN_12);
-	  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-	  GPIO_InitStruct.Pull = GPIO_NOPULL;
-	  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
-	  GPIO_InitStruct.Alternate = GPIO_AF14_USB;
-	  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-
-	  // Enable USB clock
-	  __HAL_RCC_USB_CLK_ENABLE();
-}
-
-
-// echo to either Serial0 or Serial1
-// with Serial0 as all lower case, Serial1 as all upper case
-static void echo_serial_port(uint8_t itf, uint8_t buf[], uint32_t count)
-{
-  uint8_t const case_diff = 'a' - 'A';
-
-  for(uint32_t i=0; i<count; i++)
-  {
-    if (itf == 0)
-    {
-      // echo back 1st port as lower case
-      if (isupper(buf[i])) buf[i] += case_diff;
-    }
-    else
-    {
-      // echo back 2nd port as upper case
-      if (islower(buf[i])) buf[i] -= case_diff;
-    }
-
-    tud_cdc_n_write_char(itf, buf[i]);
-  }
-  tud_cdc_n_write_flush(itf);
-}
-
-//--------------------------------------------------------------------+
-// USB CDC
-//--------------------------------------------------------------------+
-static void cdc_task(void)
-{
-  uint8_t itf;
-
-  for (itf = 0; itf < CFG_TUD_CDC; itf++)
-  {
-    // connected() check for DTR bit
-    // Most but not all terminal client set this when making connection
-    // if ( tud_cdc_n_connected(itf) )
-    {
-      if ( tud_cdc_n_available(itf) )
-      {
-        uint8_t buf[64];
-
-        uint32_t count = tud_cdc_n_read(itf, buf, sizeof(buf));
-
-        // echo back to both serial ports
-        echo_serial_port(0, buf, count);
-        echo_serial_port(1, buf, count);
-      }
-    }
-  }
-}
-
-
-// Invoked when cdc when line state changed e.g connected/disconnected
-void tud_cdc_line_state_cb(uint8_t itf, bool dtr, bool rts)
-{
-  (void) itf;
-  (void) rts;
-
-  // TODO set some indicator
-  if ( dtr )
-  {
-	  LED2_SET(1.0);
-  }else
-  {
-	  LED2_SET(0.0);
-  }
-}
-
-// Invoked when CDC interface received data from host
-void tud_cdc_rx_cb(uint8_t itf)
-{
-  (void) itf;
-  LED1_SET(1.0);
-}
 
 
 int main(void)
@@ -201,17 +71,11 @@ int main(void)
 
     USB_Init();
 
-    tud_init(BOARD_TUD_RHPORT);
+    LED_SET1(0.0);
+    LED_SET1(0.0);
 
-    LED1_SET(0.0);
-    LED2_SET(0.0);
-
-    volatile uint32_t ii = 0;
-
-    /* TODO - Add your application code here */
     while (1) {
-        tud_task();
-        cdc_task();
+        USB_Task();
     }
 
   return 0;
@@ -258,27 +122,3 @@ void SysTick_Handler(void) {
     HAL_IncTick();
 }
 
-
-// FIXME: Do all three need to be handled, or just the LP one?
-// USB high-priority interrupt (Channel 74): Triggered only by a correct
-// transfer event for isochronous and double-buffer bulk transfer to reach
-// the highest possible transfer rate.
-void USB_HP_IRQHandler(void)
-{
-  tud_int_handler(0);
-}
-
-// USB low-priority interrupt (Channel 75): Triggered by all USB events
-// (Correct transfer, USB reset, etc.). The firmware has to check the
-// interrupt source before serving the interrupt.
-void USB_LP_IRQHandler(void)
-{
-  tud_int_handler(0);
-}
-
-// USB wakeup interrupt (Channel 76): Triggered by the wakeup event from the USB
-// Suspend mode.
-void USBWakeUp_RMP_IRQHandler(void)
-{
-  tud_int_handler(0);
-}
diff --git a/stm32/aioc-fw/Src/usb.c b/stm32/aioc-fw/Src/usb.c
new file mode 100644
index 0000000..5fb60aa
--- /dev/null
+++ b/stm32/aioc-fw/Src/usb.c
@@ -0,0 +1,60 @@
+#include <usb.h>
+#include "stm32f3xx_hal.h"
+#include "tusb.h"
+#include "usb_serial.h"
+
+// FIXME: Do all three need to be handled, or just the LP one?
+// USB high-priority interrupt (Channel 74): Triggered only by a correct
+// transfer event for isochronous and double-buffer bulk transfer to reach
+// the highest possible transfer rate.
+void USB_HP_IRQHandler(void)
+{
+  tud_int_handler(0);
+}
+
+// USB low-priority interrupt (Channel 75): Triggered by all USB events
+// (Correct transfer, USB reset, etc.). The firmware has to check the
+// interrupt source before serving the interrupt.
+void USB_LP_IRQHandler(void)
+{
+  tud_int_handler(0);
+}
+
+// USB wakeup interrupt (Channel 76): Triggered by the wakeup event from the USB
+// Suspend mode.
+void USBWakeUp_RMP_IRQHandler(void)
+{
+  tud_int_handler(0);
+}
+
+
+void USB_Init(void)
+{
+    __HAL_REMAPINTERRUPT_USB_ENABLE();
+
+    /* Configure USB DM and DP pins */
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    GPIO_InitTypeDef GPIO_InitStruct;
+    GPIO_InitStruct.Pin = (GPIO_PIN_11 | GPIO_PIN_12);
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+    GPIO_InitStruct.Alternate = GPIO_AF14_USB;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+    // Enable USB clock
+    __HAL_RCC_USB_CLK_ENABLE();
+
+    // Init classes
+    USB_SerialInit();
+
+    // Start USB Stack
+    tud_init(BOARD_TUD_RHPORT);
+}
+
+void USB_Task(void)
+{
+    USB_SerialTask();
+    tud_task();
+}
+
diff --git a/stm32/aioc-fw/Src/usb.h b/stm32/aioc-fw/Src/usb.h
new file mode 100644
index 0000000..927afaf
--- /dev/null
+++ b/stm32/aioc-fw/Src/usb.h
@@ -0,0 +1,7 @@
+#ifndef USB_H_
+#define USB_H_
+
+void USB_Init(void);
+void USB_Task(void);
+
+#endif /* USB_H_ */
diff --git a/stm32/aioc-fw/Src/usb_serial.c b/stm32/aioc-fw/Src/usb_serial.c
new file mode 100644
index 0000000..7a7b5ee
--- /dev/null
+++ b/stm32/aioc-fw/Src/usb_serial.c
@@ -0,0 +1,90 @@
+#include "usb_serial.h"
+#include "tusb.h"
+#include "led.h"
+
+// Invoked when CDC interface received data from host
+
+void tud_cdc_rx_cb(uint8_t itf)
+{
+  (void) itf;
+  LED_SET1(1.0);
+}
+
+// Invoked when cdc when line state changed e.g connected/disconnected
+void tud_cdc_line_state_cb(uint8_t itf, bool dtr, bool rts)
+{
+  (void) itf;
+  (void) rts;
+
+  // TODO set some indicator
+  if ( dtr )
+  {
+	  LED_SET2(1.0);
+  }else
+  {
+	  LED_SET2(0.0);
+  }
+}
+
+
+// echo to either Serial0 or Serial1
+// with Serial0 as all lower case, Serial1 as all upper case
+static void echo_serial_port(uint8_t itf, uint8_t buf[], uint32_t count)
+{
+  uint8_t const case_diff = 'a' - 'A';
+
+  for(uint32_t i=0; i<count; i++)
+  {
+    if (itf == 0)
+    {
+      // echo back 1st port as lower case
+      if (isupper(buf[i])) buf[i] += case_diff;
+    }
+    else
+    {
+      // echo back 2nd port as upper case
+      if (islower(buf[i])) buf[i] -= case_diff;
+    }
+
+    tud_cdc_n_write_char(itf, buf[i]);
+  }
+  tud_cdc_n_write_flush(itf);
+}
+
+//--------------------------------------------------------------------+
+// USB CDC
+//--------------------------------------------------------------------+
+static void cdc_task(void)
+{
+  uint8_t itf;
+
+  for (itf = 0; itf < CFG_TUD_CDC; itf++)
+  {
+    // connected() check for DTR bit
+    // Most but not all terminal client set this when making connection
+    // if ( tud_cdc_n_connected(itf) )
+    {
+      if ( tud_cdc_n_available(itf) )
+      {
+        uint8_t buf[64];
+
+        uint32_t count = tud_cdc_n_read(itf, buf, sizeof(buf));
+
+        // echo back to both serial ports
+        echo_serial_port(0, buf, count);
+        echo_serial_port(1, buf, count);
+      }
+    }
+  }
+}
+
+void USB_SerialInit(void)
+{
+
+
+}
+
+void  USB_SerialTask(void)
+{
+    cdc_task();
+}
diff --git a/stm32/aioc-fw/Src/usb_serial.h b/stm32/aioc-fw/Src/usb_serial.h
new file mode 100644
index 0000000..d1aed36
--- /dev/null
+++ b/stm32/aioc-fw/Src/usb_serial.h
@@ -0,0 +1,7 @@
+#ifndef USB_SERIAL_H_
+#define USB_SERIAL_H_
+
+void USB_SerialInit(void);
+void USB_SerialTask(void);
+
+#endif /* USB_SERIAL_H_ */