diff --git a/boards/OLIMEX_STM32_H103/board.h b/boards/OLIMEX_STM32_H103/board.h
index 8c050e5..82785e6 100644
--- a/boards/OLIMEX_STM32_H103/board.h
+++ b/boards/OLIMEX_STM32_H103/board.h
@@ -114,11 +114,11 @@
  * Everything input with pull-up except:
  * PC6  - Normal input because there is an external resistor.
  * PC7  - Normal input because there is an external resistor.
- * PC11 - Push Pull output (CAN CNTRL).
+ * PC11 - Open Drain output (USB disconnect).
  * PC12 - Push Pull output (LED).
  */
 #define VAL_GPIOCCRL            0x44888888      /*  PC7...PC0 */
-#define VAL_GPIOCCRH            0x88833888      /* PC15...PC8 */
+#define VAL_GPIOCCRH            0x88837888      /* PC15...PC8 */
 #define VAL_GPIOCODR            0xFFFFFFFF
 
 /*
diff --git a/boards/OLIMEX_STM32_H103/board.mk b/boards/OLIMEX_STM32_H103/board.mk
index cb03841..09b9505 100644
--- a/boards/OLIMEX_STM32_H103/board.mk
+++ b/boards/OLIMEX_STM32_H103/board.mk
@@ -1,5 +1,5 @@
 # List of all the board related files.
-BOARDSRC = ${CHIBIOS}/boards/OLIMEX_STM32_P103/board.c
+BOARDSRC = ../boards/OLIMEX_STM32_H103/board.c
 
 # Required include directories
-BOARDINC = ${CHIBIOS}/boards/OLIMEX_STM32_P103
+BOARDINC = ../boards/OLIMEX_STM32_H103
diff --git a/src/Makefile b/src/Makefile
index 7572ecc..34bd64b 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -57,7 +57,6 @@ LDSCRIPT= gpgtoken.ld
 
 # Imported source files
 CHIBIOS = ../ChibiOS_2.0.2
-include $(CHIBIOS)/boards/OLIMEX_STM32_P103/board.mk
 include $(CHIBIOS)/os/hal/platforms/STM32/platform.mk
 include $(CHIBIOS)/os/hal/hal.mk
 include $(CHIBIOS)/os/ports/GCC/ARMCMx/STM32F10x/port.mk
@@ -65,6 +64,7 @@ include $(CHIBIOS)/os/kernel/kernel.mk
 include $(CHIBIOS)/test/test.mk
 include stmusb.mk
 include usbcdc.mk
+include ../boards/OLIMEX_STM32_H103/board.mk
 
 # C sources that can be compiled in ARM or THUMB mode depending on the global
 # setting.
@@ -78,7 +78,7 @@ CSRC = $(PORTSRC) \
        $(CHIBIOS)/os/various/syscalls.c \
        $(STMUSBSRC) \
        $(USBCDCSRC) \
-       main.c
+       main.c hw_config.c usb_lld.c
 
 # List ASM source files here
 ASMSRC = $(PORTASM) \
diff --git a/src/main.c b/src/main.c
index 2473b8a..bf3f864 100644
--- a/src/main.c
+++ b/src/main.c
@@ -27,6 +27,13 @@
 #include "ch.h"
 #include "hal.h"
 #include "test.h"
+#include "usb_lld.h"
+
+#include "usb_lib.h"
+#include "usb_istr.h"
+#include "usb_desc.h"
+#include "hw_config.h"
+#include "usb_pwr.h"
 
 /*
  * Red LEDs blinker thread, times are in milliseconds.
@@ -36,14 +43,20 @@ static msg_t Thread1(void *arg) {
 
   (void)arg;
   while (TRUE) {
-    palClearPad(IOPORT3, GPIOC_LED);
-    chThdSleepMilliseconds(500);
-    palSetPad(IOPORT3, GPIOC_LED);
-    chThdSleepMilliseconds(500);
+    palClearPad (IOPORT3, GPIOC_LED);
+    chThdSleepMilliseconds (500);
+    palSetPad (IOPORT3, GPIOC_LED);
+    chThdSleepMilliseconds (500);
   }
   return 0;
 }
 
+extern uint32_t count_in;
+extern __IO uint32_t count_out;
+extern uint8_t buffer_in[VIRTUAL_COM_PORT_DATA_SIZE];
+extern uint8_t buffer_out[VIRTUAL_COM_PORT_DATA_SIZE];
+extern void USB_Init (void);
+
 /*
  * Entry point, note, the main() function is already a thread in the system
  * on entry.
@@ -53,24 +66,34 @@ int main(int argc, char **argv) {
   (void)argc;
   (void)argv;
 
-  /*
-   * Activates the serial driver 2 using the driver default configuration.
-   */
-  sdStart(&SD2, NULL);
+  usb_lld_init ();
+  USB_Init();
 
   /*
    * Creates the blinker thread.
    */
-  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
+  chThdCreateStatic (waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
 
-  /*
-   * Normal main() thread activity, in this demo it does nothing except
-   * sleeping in a loop and check the button state.
-   */
   while (TRUE) {
     if (palReadPad(IOPORT1, GPIOA_BUTTON))
-      TestThread(&SD2);
-    chThdSleepMilliseconds(500);
+      palSetPad (IOPORT3, GPIOC_LED);
+  
+    if ((count_out != 0) && (bDeviceState == CONFIGURED)) {
+      uint8_t i;
+
+      for (i = 0; i<count_out; i++) {
+	buffer_in[(count_in+i)%64] = buffer_out[i];
+      }
+      count_in += count_out;
+      count_out = 0;
+    }
+
+    if (count_in > 0) {
+      USB_SIL_Write (EP1_IN, buffer_in, count_in);
+      SetEPTxValid (ENDP1);
+    }
+
+    chThdSleepMilliseconds (50);
   }
   return 0;
 }
diff --git a/src/usbcdc.mk b/src/usbcdc.mk
index ba89c37..a8dcf00 100644
--- a/src/usbcdc.mk
+++ b/src/usbcdc.mk
@@ -1,5 +1,4 @@
 USBCDCDIR = ../USBCDC
 USBCDCSRC= $(USBCDCDIR)/usb_desc.c \
 	$(USBCDCDIR)/usb_endp.c $(USBCDCDIR)/usb_istr.c \
-	$(USBCDCDIR)/usb_prop.c $(USBCDCDIR)/usb_pwr.c \
-	$(USBCDCDIR)/hw_config.c
+	$(USBCDCDIR)/usb_prop.c $(USBCDCDIR)/usb_pwr.c