diff --git a/demos/device/keyboard/main.c b/demos/device/keyboard/main.c
index f8af02c6..f7d24cbe 100644
--- a/demos/device/keyboard/main.c
+++ b/demos/device/keyboard/main.c
@@ -93,6 +93,7 @@ void led_blinking_task(void * p_para)
 #if TUSB_CFG_DEVICE_HID_KEYBOARD
 void keyboard_device_app_task(void * p_para)
 {
+#if 1
   if (tusb_device_is_configured())
   {
     static uint32_t count =0;
@@ -106,12 +107,14 @@ void keyboard_device_app_task(void * p_para)
       );
     }
   }
+#endif
 }
 #endif
 
 #if TUSB_CFG_DEVICE_HID_MOUSE
 void mouse_device_app_task(void * p_para)
 {
+#if 1
   if (tusb_device_is_configured())
   {
     static uint32_t count =0;
@@ -124,6 +127,7 @@ void mouse_device_app_task(void * p_para)
               .y = 20 } );
     }
   }
+#endif
 }
 #endif
 
diff --git a/demos/device/keyboard/tusb_config.h b/demos/device/keyboard/tusb_config.h
index 8e4b635a..7dc912ee 100644
--- a/demos/device/keyboard/tusb_config.h
+++ b/demos/device/keyboard/tusb_config.h
@@ -93,7 +93,7 @@
 #define TUSB_CFG_DEVICE_CONTROL_PACKET_SIZE   64
 
 //------------- CLASS -------------//
-#define TUSB_CFG_DEVICE_HID_KEYBOARD  0
+#define TUSB_CFG_DEVICE_HID_KEYBOARD  1
 #define TUSB_CFG_DEVICE_HID_MOUSE     0
 #define TUSB_CFG_DEVICE_HID_GENERIC   0
 #define TUSB_CFG_DEVICE_MSC           0
diff --git a/demos/device/keyboard/tusb_descriptors.h b/demos/device/keyboard/tusb_descriptors.h
index 7e092a7d..2cdee40c 100644
--- a/demos/device/keyboard/tusb_descriptors.h
+++ b/demos/device/keyboard/tusb_descriptors.h
@@ -109,6 +109,13 @@ typedef ATTR_PACKED_STRUCT(struct)
   tusb_descriptor_endpoint_t                     mouse_endpoint;
 #endif
 
+//------------- Mass Storage -------------//
+#if TUSB_CFG_DEVICE_MSC
+  tusb_descriptor_interface_t                    msc_interface;
+  tusb_descriptor_endpoint_t                     msc_endpoint_in;
+  tusb_descriptor_endpoint_t                     msc_endpoint_out;
+#endif
+
   uint8_t                                        null_termination; // NXP rom driver requires this to work
 } app_descriptor_configuration_t;
 
diff --git a/tests/lpc175x_6x/test/test_dcd_lpc175x_6x.c b/tests/lpc175x_6x/test/test_dcd_lpc175x_6x.c
index d1f8b0e9..abbd408d 100644
--- a/tests/lpc175x_6x/test/test_dcd_lpc175x_6x.c
+++ b/tests/lpc175x_6x/test/test_dcd_lpc175x_6x.c
@@ -41,8 +41,10 @@
 #include "errors.h"
 #include "type_helper.h"
 
+#include "mock_usbd_dcd.h"
 #include "dcd_lpc175x_6x.h"
 
+usbd_device_info_t usbd_devices[CONTROLLER_DEVICE_NUMBER];
 extern dcd_dma_descriptor_t* dcd_udca[32];
 
 void setUp(void)
diff --git a/tests/lpc175x_6x/test/test_usbd.c b/tests/lpc175x_6x/test/test_usbd.c
new file mode 100644
index 00000000..d2e73ee2
--- /dev/null
+++ b/tests/lpc175x_6x/test/test_usbd.c
@@ -0,0 +1,101 @@
+/**************************************************************************/
+/*!
+    @file     test_usbd.c
+    @author   hathach (tinyusb.org)
+
+    @section LICENSE
+
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2013, hathach (tinyusb.org)
+    All rights reserved.
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions are met:
+    1. Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+    2. Redistributions in binary form must reproduce the above copyright
+    notice, this list of conditions and the following disclaimer in the
+    documentation and/or other materials provided with the distribution.
+    3. Neither the name of the copyright holders nor the
+    names of its contributors may be used to endorse or promote products
+    derived from this software without specific prior written permission.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+    EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+    This file is part of the tinyusb stack.
+*/
+/**************************************************************************/
+
+#include <stdlib.h>
+#include "unity.h"
+#include "errors.h"
+#include "type_helper.h"
+
+#include "tusb_descriptors.h"
+
+#include "mock_hid_device.h"
+#include "mock_dcd.h"
+
+#include "usbd.h"
+
+void setUp(void)
+{
+
+}
+
+void tearDown(void)
+{
+}
+
+void test_dcd_init_failed(void)
+{
+  dcd_init_ExpectAndReturn(TUSB_ERROR_FAILED);
+
+  //------------- Code Under Test -------------//
+  TEST_ASSERT_EQUAL(TUSB_ERROR_FAILED, usbd_init() );
+}
+
+tusb_error_t stub_hidd_init(tusb_descriptor_interface_t const* p_interface_desc, uint16_t* p_length, int num_call)
+{
+  switch(num_call)
+  {
+    case 0:
+      TEST_ASSERT_EQUAL_HEX(&app_tusb_desc_configuration.keyboard_interface, p_interface_desc);
+    break;
+
+    default:
+      TEST_FAIL();
+      return TUSB_ERROR_FAILED;
+  }
+
+  return TUSB_ERROR_NONE;
+}
+
+void class_init_epxect(void)
+{
+#if DEVICE_CLASS_HID
+  hidd_init_StubWithCallback(stub_hidd_init);
+#endif
+}
+
+void test_usbd_init(void)
+{
+  dcd_init_ExpectAndReturn(TUSB_ERROR_NONE);
+
+  class_init_epxect();
+  dcd_controller_connect_Expect(0);
+
+
+  //------------- Code Under Test -------------//
+  TEST_ASSERT_STATUS( usbd_init() );
+}
diff --git a/tests/support/tusb_config.h b/tests/support/tusb_config.h
index 6edd5980..83dcf78e 100644
--- a/tests/support/tusb_config.h
+++ b/tests/support/tusb_config.h
@@ -86,14 +86,16 @@
 //--------------------------------------------------------------------+
 // DEVICE CONFIGURATION
 //--------------------------------------------------------------------+
+#define TUSB_CFG_DEVICE_USE_ROM_DRIVER  1
+
+//------------- descriptors -------------//
 #define TUSB_CFG_DEVICE_STRING_MANUFACTURER   "tinyusb.org"
 #define TUSB_CFG_DEVICE_STRING_PRODUCT        "Device Example"
 #define TUSB_CFG_DEVICE_STRING_SERIAL         "1234"
 #define TUSB_CFG_DEVICE_VENDORID              0x1FC9 // NXP
 //#define TUSB_CFG_DEVICE_PRODUCTID           0x4567
 
-
-#define TUSB_CFG_DEVICE_USE_ROM_DRIVER  1
+#define TUSB_CFG_DEVICE_CONTROL_PACKET_SIZE   64
 
 //------------- CLASS -------------//
 #define TUSB_CFG_DEVICE_HID_KEYBOARD  1
diff --git a/tinyusb/class/hid_device.c b/tinyusb/class/hid_device.c
index 94ec6df6..99421863 100644
--- a/tinyusb/class/hid_device.c
+++ b/tinyusb/class/hid_device.c
@@ -50,7 +50,7 @@
 
 #if defined(CAP_DEVICE_ROMDRIVER) && TUSB_CFG_DEVICE_USE_ROM_DRIVER
 #include "device/dcd_nxp_romdriver.h" // TODO remove rom driver dependency
-#endif
+
 
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
@@ -325,5 +325,6 @@ ErrorCode_t HID_EpOut_Hdlr (USBD_HANDLE_T hUsb, void* data, uint32_t event)
   }
   return LPC_OK;
 }
+#endif
 
 #endif
diff --git a/tinyusb/device/usbd.c b/tinyusb/device/usbd.c
index 426ae19a..0845bf68 100644
--- a/tinyusb/device/usbd.c
+++ b/tinyusb/device/usbd.c
@@ -95,10 +95,10 @@ void std_get_descriptor(uint8_t coreid)
     case TUSB_DESC_TYPE_CONFIGURATION:
     {
       uint16_t const requested_length = min16_of(usbd_devices[coreid].setup_packet.wLength, sizeof(app_tusb_desc_configuration)-1);
-      for(uint16_t i=0; i<requested_length; i += 64)
-      {
-        dcd_pipe_control_write(coreid, ((uint8_t*)&app_tusb_desc_configuration) + i, min16_of(64, requested_length - i));
-      }
+      ASSERT(requested_length <= TUSB_CFG_DEVICE_CONTROL_PACKET_SIZE, (void)0 ); // multiple packets requires a task a-like
+
+      dcd_pipe_control_write(coreid, ((uint8_t*)&app_tusb_desc_configuration),
+                             requested_length);
     }
     break;
 
@@ -157,7 +157,7 @@ tusb_error_t usbd_init (void)
   ASSERT_STATUS ( dcd_init() );
 
   uint16_t length = 0;
-  #if TUSB_CFG_DEVICE_HID_KEYBOARD && 0
+  #if TUSB_CFG_DEVICE_HID_KEYBOARD
   ASSERT_STATUS( hidd_init(&app_tusb_desc_configuration.keyboard_interface, &length) );
   #endif