Add CONSOLE_ENABLE build option to LUFA.

diff --git a/common.mk b/common.mk
index 9bf34f2b8e4b2560fe966c1863c10097bc870b16..580a8cf8ffc8b7d2800cad67b09b7080c16dbcb2 100644 (file)
--- a/common.mk
+++ b/common.mk
@@ -15,20 +15,24 @@ ifdef MOUSEKEY_ENABLE
     OPT_DEFS += -DMOUSEKEY_ENABLE
 endif
 
-ifdef PS2_MOUSE_ENABLE
-    SRC += $(COMMON_DIR)/ps2.c \
-           $(COMMON_DIR)/ps2_mouse.c
-    OPT_DEFS += -DPS2_MOUSE_ENABLE
-endif
-
 ifdef EXTRAKEY_ENABLE
     OPT_DEFS += -DEXTRAKEY_ENABLE
 endif
 
+ifdef CONSOLE_ENABLE
+    OPT_DEFS += -DCONSOLE_ENABLE
+endif
+
 ifdef NKRO_ENABLE
     OPT_DEFS += -DNKRO_ENABLE
 endif
 
+ifdef PS2_MOUSE_ENABLE
+    SRC += $(COMMON_DIR)/ps2.c \
+           $(COMMON_DIR)/ps2_mouse.c
+    OPT_DEFS += -DPS2_MOUSE_ENABLE
+endif
+
 ifdef $(or MOUSEKEY_ENABLE, PS2_MOUSE_ENABLE)
     OPT_DEFS += -DMOUSE_ENABLE
 endif

diff --git a/keyboard/hhkb/Makefile.lufa b/keyboard/hhkb/Makefile.lufa
index 0dc295e621a3c2693e36ef8f33db47bb025a41e4..5acc93a23ccade3456d9c1229e5ac3d0a5e03182 100644 (file)
--- a/keyboard/hhkb/Makefile.lufa
+++ b/keyboard/hhkb/Makefile.lufa
@@ -101,7 +101,8 @@ OPT_DEFS += -DINTERRUPT_CONTROL_ENDPOINT
 #   comment out to disable the options.
 #
 MOUSEKEY_ENABLE = yes  # Mouse keys
-EXTRAKEY_ENABLE = yes  # Audio control and System control
+EXTRAKEY_ENABLE = yes  # Media control and System control
+CONSOLE_ENABLE = yes   # Console for debug
 #NKRO_ENABLE = yes     # USB Nkey Rollover
 #PS2_MOUSE_ENABLE = yes        # PS/2 mouse(TrackPoint) support
 

diff --git a/keyboard/hhkb/config.h b/keyboard/hhkb/config.h
index a8cefd5edc5c391792d5c7dd526486a3840f3ea7..bf946ac01eb0aed21dbe0c2e73e6a44a1cfa98cb 100644 (file)
--- a/keyboard/hhkb/config.h
+++ b/keyboard/hhkb/config.h
@@ -24,7 +24,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #define VENDOR_ID       0xFEED
 #define PRODUCT_ID      0xCAFE
-#define DEVICE_VER      0x0101
+#define DEVICE_VER      0x0103
 #define MANUFACTURER    t.m.k.
 #define PRODUCT         HHKB mod
 

diff --git a/protocol/lufa/descriptor.c b/protocol/lufa/descriptor.c
index 3c98819ff7d996af73f3aef24b1538aaab7c627d..ff98d88769f263f9055be0d9f97f84dedd2895f8 100644 (file)
--- a/protocol/lufa/descriptor.c
+++ b/protocol/lufa/descriptor.c
@@ -80,6 +80,7 @@ const USB_Descriptor_HIDReport_Datatype_t PROGMEM KeyboardReport[] =
     HID_RI_END_COLLECTION(0),
 };
 
+#ifdef MOUSE_ENABLE
 const USB_Descriptor_HIDReport_Datatype_t PROGMEM MouseReport[] =
 {
     HID_RI_USAGE_PAGE(8, 0x01), /* Generic Desktop */
@@ -127,29 +128,10 @@ const USB_Descriptor_HIDReport_Datatype_t PROGMEM MouseReport[] =
         HID_RI_END_COLLECTION(0),
     HID_RI_END_COLLECTION(0),
 };
-
-const USB_Descriptor_HIDReport_Datatype_t PROGMEM ConsoleReport[] =
-{
-    HID_RI_USAGE_PAGE(16, 0xFF31), /* Vendor Page(PJRC Teensy compatible) */
-    HID_RI_USAGE(8, 0x74), /* Vendor Usage(PJRC Teensy compatible) */
-    HID_RI_COLLECTION(8, 0x01), /* Application */
-        HID_RI_USAGE(8, 0x75), /* Vendor Usage 0x75 */
-        HID_RI_LOGICAL_MINIMUM(8, 0x00),
-        HID_RI_LOGICAL_MAXIMUM(8, 0xFF),
-        HID_RI_REPORT_COUNT(8, CONSOLE_EPSIZE),
-        HID_RI_REPORT_SIZE(8, 0x08),
-        HID_RI_INPUT(8, HID_IOF_DATA | HID_IOF_VARIABLE | HID_IOF_ABSOLUTE),
-        HID_RI_USAGE(8, 0x76), /* Vendor Usage 0x76 */
-        HID_RI_LOGICAL_MINIMUM(8, 0x00),
-        HID_RI_LOGICAL_MAXIMUM(8, 0xFF),
-        HID_RI_REPORT_COUNT(8, CONSOLE_EPSIZE),
-        HID_RI_REPORT_SIZE(8, 0x08),
-        HID_RI_OUTPUT(8, HID_IOF_DATA | HID_IOF_VARIABLE | HID_IOF_ABSOLUTE | HID_IOF_NON_VOLATILE),
-    HID_RI_END_COLLECTION(0),
-};
+#endif
 
 #ifdef EXTRAKEY_ENABLE
-const USB_Descriptor_HIDReport_Datatype_t PROGMEM ExtraReport[] =
+const USB_Descriptor_HIDReport_Datatype_t PROGMEM ExtrakeyReport[] =
 {
     HID_RI_USAGE_PAGE(8, 0x01), /* Generic Desktop */
     HID_RI_USAGE(8, 0x80), /* System Control */
@@ -179,6 +161,28 @@ const USB_Descriptor_HIDReport_Datatype_t PROGMEM ExtraReport[] =
 };
 #endif
 
+#ifdef CONSOLE_ENABLE
+const USB_Descriptor_HIDReport_Datatype_t PROGMEM ConsoleReport[] =
+{
+    HID_RI_USAGE_PAGE(16, 0xFF31), /* Vendor Page(PJRC Teensy compatible) */
+    HID_RI_USAGE(8, 0x74), /* Vendor Usage(PJRC Teensy compatible) */
+    HID_RI_COLLECTION(8, 0x01), /* Application */
+        HID_RI_USAGE(8, 0x75), /* Vendor Usage 0x75 */
+        HID_RI_LOGICAL_MINIMUM(8, 0x00),
+        HID_RI_LOGICAL_MAXIMUM(8, 0xFF),
+        HID_RI_REPORT_COUNT(8, CONSOLE_EPSIZE),
+        HID_RI_REPORT_SIZE(8, 0x08),
+        HID_RI_INPUT(8, HID_IOF_DATA | HID_IOF_VARIABLE | HID_IOF_ABSOLUTE),
+        HID_RI_USAGE(8, 0x76), /* Vendor Usage 0x76 */
+        HID_RI_LOGICAL_MINIMUM(8, 0x00),
+        HID_RI_LOGICAL_MAXIMUM(8, 0xFF),
+        HID_RI_REPORT_COUNT(8, CONSOLE_EPSIZE),
+        HID_RI_REPORT_SIZE(8, 0x08),
+        HID_RI_OUTPUT(8, HID_IOF_DATA | HID_IOF_VARIABLE | HID_IOF_ABSOLUTE | HID_IOF_NON_VOLATILE),
+    HID_RI_END_COLLECTION(0),
+};
+#endif
+
 #ifdef NKRO_ENABLE
 const USB_Descriptor_HIDReport_Datatype_t PROGMEM NKROReport[] =
 {
@@ -342,16 +346,17 @@ const USB_Descriptor_Configuration_t PROGMEM ConfigurationDescriptor =
 #endif
 
     /*
-     * Console
+     * Extra
      */
-    .Console_Interface =
+#ifdef EXTRAKEY_ENABLE
+    .Extrakey_Interface =
         {
             .Header                 = {.Size = sizeof(USB_Descriptor_Interface_t), .Type = DTYPE_Interface},
 
-            .InterfaceNumber        = CONSOLE_INTERFACE,
+            .InterfaceNumber        = EXTRAKEY_INTERFACE,
             .AlternateSetting       = 0x00,
 
-            .TotalEndpoints         = 2,
+            .TotalEndpoints         = 1,
 
             .Class                  = HID_CSCP_HIDClass,
             .SubClass               = HID_CSCP_NonBootSubclass,
@@ -360,7 +365,7 @@ const USB_Descriptor_Configuration_t PROGMEM ConfigurationDescriptor =
             .InterfaceStrIndex      = NO_DESCRIPTOR
         },
 
-    .Console_HID =
+    .Extrakey_HID =
         {
             .Header                 = {.Size = sizeof(USB_HID_Descriptor_HID_t), .Type = HID_DTYPE_HID},
 
@@ -368,41 +373,32 @@ const USB_Descriptor_Configuration_t PROGMEM ConfigurationDescriptor =
             .CountryCode            = 0x00,
             .TotalReportDescriptors = 1,
             .HIDReportType          = HID_DTYPE_Report,
-            .HIDReportLength        = sizeof(ConsoleReport)
+            .HIDReportLength        = sizeof(ExtrakeyReport)
         },
 
-    .Console_INEndpoint =
+    .Extrakey_INEndpoint =
         {
             .Header                 = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint},
 
-            .EndpointAddress        = (ENDPOINT_DIR_IN | CONSOLE_IN_EPNUM),
+            .EndpointAddress        = (ENDPOINT_DIR_IN | EXTRAKEY_IN_EPNUM),
             .Attributes             = (EP_TYPE_INTERRUPT | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA),
-            .EndpointSize           = CONSOLE_EPSIZE,
-            .PollingIntervalMS      = 0x01
-        },
-
-    .Console_OUTEndpoint =
-        {
-            .Header                 = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint},
-
-            .EndpointAddress        = (ENDPOINT_DIR_OUT | CONSOLE_OUT_EPNUM),
-            .Attributes             = (EP_TYPE_INTERRUPT | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA),
-            .EndpointSize           = CONSOLE_EPSIZE,
+            .EndpointSize           = EXTRAKEY_EPSIZE,
             .PollingIntervalMS      = 0x01
         },
+#endif
 
     /*
-     * Extra
+     * Console
      */
-#ifdef EXTRAKEY_ENABLE
-    .Extra_Interface =
+#ifdef CONSOLE_ENABLE
+    .Console_Interface =
         {
             .Header                 = {.Size = sizeof(USB_Descriptor_Interface_t), .Type = DTYPE_Interface},
 
-            .InterfaceNumber        = EXTRA_INTERFACE,
+            .InterfaceNumber        = CONSOLE_INTERFACE,
             .AlternateSetting       = 0x00,
 
-            .TotalEndpoints         = 1,
+            .TotalEndpoints         = 2,
 
             .Class                  = HID_CSCP_HIDClass,
             .SubClass               = HID_CSCP_NonBootSubclass,
@@ -411,7 +407,7 @@ const USB_Descriptor_Configuration_t PROGMEM ConfigurationDescriptor =
             .InterfaceStrIndex      = NO_DESCRIPTOR
         },
 
-    .Extra_HID =
+    .Console_HID =
         {
             .Header                 = {.Size = sizeof(USB_HID_Descriptor_HID_t), .Type = HID_DTYPE_HID},
 
@@ -419,16 +415,26 @@ const USB_Descriptor_Configuration_t PROGMEM ConfigurationDescriptor =
             .CountryCode            = 0x00,
             .TotalReportDescriptors = 1,
             .HIDReportType          = HID_DTYPE_Report,
-            .HIDReportLength        = sizeof(ExtraReport)
+            .HIDReportLength        = sizeof(ConsoleReport)
         },
 
-    .Extra_INEndpoint =
+    .Console_INEndpoint =
         {
             .Header                 = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint},
 
-            .EndpointAddress        = (ENDPOINT_DIR_IN | EXTRA_IN_EPNUM),
+            .EndpointAddress        = (ENDPOINT_DIR_IN | CONSOLE_IN_EPNUM),
             .Attributes             = (EP_TYPE_INTERRUPT | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA),
-            .EndpointSize           = EXTRA_EPSIZE,
+            .EndpointSize           = CONSOLE_EPSIZE,
+            .PollingIntervalMS      = 0x01
+        },
+
+    .Console_OUTEndpoint =
+        {
+            .Header                 = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint},
+
+            .EndpointAddress        = (ENDPOINT_DIR_OUT | CONSOLE_OUT_EPNUM),
+            .Attributes             = (EP_TYPE_INTERRUPT | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA),
+            .EndpointSize           = CONSOLE_EPSIZE,
             .PollingIntervalMS      = 0x01
         },
 #endif
@@ -515,13 +521,15 @@ uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue,
                 Size    = sizeof(USB_HID_Descriptor_HID_t);
                 break;
 #endif
-            case CONSOLE_INTERFACE:
-                Address = &ConfigurationDescriptor.Console_HID;
+#ifdef EXTRAKEY_ENABLE
+            case EXTRAKEY_INTERFACE:
+                Address = &ConfigurationDescriptor.Extrakey_HID;
                 Size    = sizeof(USB_HID_Descriptor_HID_t);
                 break;
-#ifdef EXTRAKEY_ENABLE
-            case EXTRA_INTERFACE:
-                Address = &ConfigurationDescriptor.Extra_HID;
+#endif
+#ifdef CONSOLE_ENABLE
+            case CONSOLE_INTERFACE:
+                Address = &ConfigurationDescriptor.Console_HID;
                 Size    = sizeof(USB_HID_Descriptor_HID_t);
                 break;
 #endif
@@ -539,15 +547,17 @@ uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue,
                 Size    = sizeof(MouseReport);
                 break;
 #endif
+#ifdef EXTRAKEY_ENABLE
+            case EXTRAKEY_INTERFACE:
+                Address = &ExtrakeyReport;
+                Size    = sizeof(ExtrakeyReport);
+                break;
+#endif
+#ifdef CONSOLE_ENABLE
             case CONSOLE_INTERFACE:
                 Address = &ConsoleReport;
                 Size    = sizeof(ConsoleReport);
                 break;
-#ifdef EXTRAKEY_ENABLE
-            case EXTRA_INTERFACE:
-                Address = &ExtraReport;
-                Size    = sizeof(ExtraReport);
-                break;
 #endif
             }
             break;

diff --git a/protocol/lufa/descriptor.h b/protocol/lufa/descriptor.h
index 09fb24acc1ef5aa47ae721cc9136cfdd903cbbe4..44f20d5a26b36855d24f7cbd59cacb007cc69428 100644 (file)
--- a/protocol/lufa/descriptor.h
+++ b/protocol/lufa/descriptor.h
@@ -57,24 +57,26 @@ typedef struct
     USB_HID_Descriptor_HID_t              Keyboard_HID;
     USB_Descriptor_Endpoint_t             Keyboard_INEndpoint;
 
-    // Mouse HID Interface
 #ifdef MOUSE_ENABLE
+    // Mouse HID Interface
     USB_Descriptor_Interface_t            Mouse_Interface;
     USB_HID_Descriptor_HID_t              Mouse_HID;
     USB_Descriptor_Endpoint_t             Mouse_INEndpoint;
 #endif
 
+#ifdef EXTRAKEY_ENABLE
+    // Extrakey HID Interface
+    USB_Descriptor_Interface_t            Extrakey_Interface;
+    USB_HID_Descriptor_HID_t              Extrakey_HID;
+    USB_Descriptor_Endpoint_t             Extrakey_INEndpoint;
+#endif
+
+#ifdef CONSOLE_ENABLE
     // Console HID Interface
     USB_Descriptor_Interface_t            Console_Interface;
     USB_HID_Descriptor_HID_t              Console_HID;
     USB_Descriptor_Endpoint_t             Console_INEndpoint;
     USB_Descriptor_Endpoint_t             Console_OUTEndpoint;
-
-    // Extra HID Interface
-#ifdef EXTRAKEY_ENABLE
-    USB_Descriptor_Interface_t            Extra_Interface;
-    USB_HID_Descriptor_HID_t              Extra_HID;
-    USB_Descriptor_Endpoint_t             Extra_INEndpoint;
 #endif
 } USB_Descriptor_Configuration_t;
 
@@ -89,12 +91,16 @@ typedef struct
 #endif 
 
 #ifdef EXTRAKEY_ENABLE
-#   define EXTRA_INTERFACE          (MOUSE_INTERFACE + 1)
+#   define EXTRAKEY_INTERFACE       (MOUSE_INTERFACE + 1)
 #else
-#   define EXTRA_INTERFACE          MOUSE_INTERFACE
+#   define EXTRAKEY_INTERFACE       MOUSE_INTERFACE
 #endif 
 
-#define CONSOLE_INTERFACE           (EXTRA_INTERFACE + 1)
+#ifdef CONSOLE_ENABLE
+#   define CONSOLE_INTERFACE        (EXTRAKEY_INTERFACE + 1)
+#else
+#   define CONSOLE_INTERFACE        EXTRAKEY_INTERFACE
+#endif
 
 
 /* nubmer of interfaces */
@@ -103,15 +109,29 @@ typedef struct
 
 // Endopoint number and size
 #define KEYBOARD_IN_EPNUM           1
-#define MOUSE_IN_EPNUM              2
-#define CONSOLE_IN_EPNUM            3
-#define CONSOLE_OUT_EPNUM           4
-#define EXTRA_IN_EPNUM              5
+
+#ifdef MOUSE_ENABLE
+#   define MOUSE_IN_EPNUM           (KEYBOARD_IN_EPNUM + 1) 
+#else
+#   define MOUSE_IN_EPNUM           KEYBOARD_IN_EPNUM
+#endif
+
+#ifdef EXTRAKEY_ENABLE
+#   define EXTRAKEY_IN_EPNUM        (MOUSE_IN_EPNUM + 1)
+#else
+#   define EXTRAKEY_IN_EPNUM        MOUSE_IN_EPNUM 
+#endif
+
+#ifdef CONSOLE_ENABLE
+#   define CONSOLE_IN_EPNUM         (EXTRAKEY_IN_EPNUM + 1)
+#   define CONSOLE_OUT_EPNUM        (EXTRAKEY_IN_EPNUM + 2)
+#endif
+
 
 #define KEYBOARD_EPSIZE             8
 #define MOUSE_EPSIZE                8
+#define EXTRAKEY_EPSIZE             8
 #define CONSOLE_EPSIZE              32
-#define EXTRA_EPSIZE                8
 
 
 uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue,

diff --git a/protocol/lufa/lufa.c b/protocol/lufa/lufa.c
index 10511ba67b27339177740065f280a403a38cceba..99d241ded04b408df7c705e33bd121fe5804adba 100644 (file)
--- a/protocol/lufa/lufa.c
+++ b/protocol/lufa/lufa.c
@@ -111,6 +111,7 @@ void SetupHardware(void)
     USB_Device_EnableSOFEvents();
 }
 
+#ifdef CONSOLE_ENABLE
 static void Console_Task(void)
 {
     /* Device must be connected and configured for the task to run */
@@ -146,6 +147,11 @@ static void Console_Task(void)
 
     /* IN packet */
     Endpoint_SelectEndpoint(CONSOLE_IN_EPNUM);
+
+    // fill empty bank
+    while (Endpoint_IsReadWriteAllowed())
+        Endpoint_Write_8(0);
+
     // flash senchar packet
     if (Endpoint_IsINReady()) {
         Endpoint_ClearIN();
@@ -153,6 +159,11 @@ static void Console_Task(void)
 
     Endpoint_SelectEndpoint(ep);
 }
+#else
+static void Console_Task(void)
+{
+}
+#endif
 
 
 /*******************************************************************************
@@ -168,14 +179,9 @@ void EVENT_USB_Device_Disconnect(void)
 {
 }
 
-#define CONSOLE_TASK_INTERVAL 50
 void EVENT_USB_Device_StartOfFrame(void)
 {
-    static uint8_t interval;
-    if (++interval == CONSOLE_TASK_INTERVAL) {
-        Console_Task();
-        interval = 0;
-    }
+    Console_Task();
 }
 
 /** Event handler for the USB_ConfigurationChanged event.
@@ -197,15 +203,17 @@ void EVENT_USB_Device_ConfigurationChanged(void)
 
 #ifdef EXTRAKEY_ENABLE
     /* Setup Extra HID Report Endpoint */
-    ConfigSuccess &= Endpoint_ConfigureEndpoint(EXTRA_IN_EPNUM, EP_TYPE_INTERRUPT, ENDPOINT_DIR_IN,
-                                                EXTRA_EPSIZE, ENDPOINT_BANK_SINGLE);
+    ConfigSuccess &= Endpoint_ConfigureEndpoint(EXTRAKEY_IN_EPNUM, EP_TYPE_INTERRUPT, ENDPOINT_DIR_IN,
+                                                EXTRAKEY_EPSIZE, ENDPOINT_BANK_SINGLE);
 #endif
 
+#ifdef CONSOLE_ENABLE
     /* Setup Console HID Report Endpoints */
     ConfigSuccess &= Endpoint_ConfigureEndpoint(CONSOLE_IN_EPNUM, EP_TYPE_INTERRUPT, ENDPOINT_DIR_IN,
                                                 CONSOLE_EPSIZE, ENDPOINT_BANK_DOUBLE);
     ConfigSuccess &= Endpoint_ConfigureEndpoint(CONSOLE_OUT_EPNUM, EP_TYPE_INTERRUPT, ENDPOINT_DIR_OUT,
                                                 CONSOLE_EPSIZE, ENDPOINT_BANK_SINGLE);
+#endif
 }
 
 /*
@@ -371,7 +379,7 @@ static void send_system(uint16_t data)
         .report_id = REPORT_ID_SYSTEM,
         .usage = data
     };
-    Endpoint_SelectEndpoint(EXTRA_IN_EPNUM);
+    Endpoint_SelectEndpoint(EXTRAKEY_IN_EPNUM);
     if (Endpoint_IsReadWriteAllowed()) {
         Endpoint_Write_Stream_LE(&r, sizeof(report_extra_t), NULL);
         Endpoint_ClearIN();
@@ -384,7 +392,7 @@ static void send_consumer(uint16_t data)
         .report_id = REPORT_ID_CONSUMER,
         .usage = data
     };
-    Endpoint_SelectEndpoint(EXTRA_IN_EPNUM);
+    Endpoint_SelectEndpoint(EXTRAKEY_IN_EPNUM);
     if (Endpoint_IsReadWriteAllowed()) {
         Endpoint_Write_Stream_LE(&r, sizeof(report_extra_t), NULL);
         Endpoint_ClearIN();
@@ -395,12 +403,14 @@ static void send_consumer(uint16_t data)
 /*******************************************************************************
  * sendchar
  ******************************************************************************/
-#define SEND_TIMEOUT 10
+#ifdef CONSOLE_ENABLE
+#define SEND_TIMEOUT 5
 int8_t sendchar(uint8_t c)
 {
     if (USB_DeviceState != DEVICE_STATE_Configured)
       return -1;
 
+    uint8_t ep = Endpoint_GetCurrentEndpoint();
     Endpoint_SelectEndpoint(CONSOLE_IN_EPNUM);
 
     uint8_t timeout = SEND_TIMEOUT;
@@ -411,11 +421,15 @@ int8_t sendchar(uint8_t c)
         case DEVICE_STATE_Suspended:
             return -1;
         }
-        if (Endpoint_IsStalled())
+        if (Endpoint_IsStalled()) {
+            Endpoint_SelectEndpoint(ep);
             return -1;
+        }
         if (prevFN != USB_Device_GetFrameNumber()) {
-            if (!(timeout--))
+            if (!(timeout--)) {
+                Endpoint_SelectEndpoint(ep);
                 return -1;
+            }
             prevFN = USB_Device_GetFrameNumber();
         }
     }
@@ -426,5 +440,12 @@ int8_t sendchar(uint8_t c)
     if (!Endpoint_IsReadWriteAllowed())
         Endpoint_ClearIN();
 
+    Endpoint_SelectEndpoint(ep);
+    return 0;
+}
+#else
+int8_t sendchar(uint8_t c)
+{
     return 0;
 }
+#endif