diff --git a/Firmware/Inc/freertos_vars.h b/Firmware/Inc/freertos_vars.h
index fd0bf85d..e0853071 100644
--- a/Firmware/Inc/freertos_vars.h
+++ b/Firmware/Inc/freertos_vars.h
@@ -2,12 +2,12 @@
 #ifndef __FREERTOS_H
 #define __FREERTOS_H
 
-// List of semaphore
-osSemaphoreId sem_usb_irq;
+// List of semaphores
+extern osSemaphoreId sem_usb_irq;
 
 // List of threads
-osThreadId thread_motor_0;
-osThreadId thread_motor_1;
-osThreadId thread_cmd_parse;
+extern osThreadId thread_motor_0;
+extern osThreadId thread_motor_1;
+extern osThreadId thread_cmd_parse;
 
 #endif /* __FREERTOS_H */
\ No newline at end of file
diff --git a/Firmware/Inc/usbd_cdc_if.h b/Firmware/Inc/usbd_cdc_if.h
index 84d30b6b..8bf41d3c 100644
--- a/Firmware/Inc/usbd_cdc_if.h
+++ b/Firmware/Inc/usbd_cdc_if.h
@@ -103,6 +103,8 @@
 extern USBD_CDC_ItfTypeDef  USBD_Interface_fops_FS;
 
 /* USER CODE BEGIN EXPORTED_VARIABLES */
+extern uint8_t *USBRxBuffer;
+extern uint32_t USBRxBufferLen;
 /* USER CODE END EXPORTED_VARIABLES */
 
 /**
diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index daed1296..c8fbc251 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -74,6 +74,9 @@ void Axis::StateMachineLoop() {
                 enable_control_ = false;
             }
         }
+
+        // give some time to lower priority threads
+        osDelay(100);
     }
     legacy_motor_ref_->thread_ready = false;
 }
\ No newline at end of file
diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp
index 06d318cd..3bf034fa 100644
--- a/Firmware/MotorControl/commands.cpp
+++ b/Firmware/MotorControl/commands.cpp
@@ -1,20 +1,22 @@
 
 /* Includes ------------------------------------------------------------------*/
 
-//#define INCLUDE_LEGACY_PROTOCOL
+// TODO: remove this option
+#define ENABLE_LEGACY_PROTOCOL
 
 #include "low_level.h"
 #include "protocol.hpp"
 #include "freertos_vars.h"
 #include "commands.h"
 
-#ifdef INCLUDE_LEGACY_PROTOCOL
+#ifdef ENABLE_LEGACY_PROTOCOL
 #include "legacy_commands.h"
 #endif
 
 #include <cmsis_os.h>
 #include <memory>
 #include <usbd_cdc_if.h>
+#include <usb_device.h>
 #include <usart.h>
 #include <gpio.h>
 
@@ -52,17 +54,31 @@ Endpoint endpoints[] = {
 constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]);
 
 
-class USBSender : public PacketWriter {
+// We could theoretically implement the USB channel as a packet based channel,
+// but on some platforms there's no direct USB endpoint access, so the device
+// should better just behave like a serial device.
+class USBSender : public StreamWriter {
 public:
-    int write_packet(const uint8_t* buffer, size_t length) {
-        return (CDC_Transmit_FS(
-            const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
-            well... it's not actually. Stupid STM. */, length) == USBD_OK) ? 0 : -1;
+    int write_bytes(const uint8_t* buffer, size_t length) {
+        // Loop to ensure all bytes get sent
+        // TODO: add timeout
+        while (length) {
+            size_t chunk = length < 64 ? length : 64;
+            while (CDC_Transmit_FS(
+                const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
+                well... it's not actually. Stupid STM. */, chunk) != USBD_OK)
+                osDelay(1);
+            buffer += chunk;
+            length -= chunk;
+        }
+        //printf("USB TX done\r\n"); osDelay(5);
+        return 0;
     }
 } usb_sender;
 
-BidirectionalPacketBasedChannel usb_connection(endpoints, NUM_ENDPOINTS, usb_sender);
-
+PacketToStreamConverter usb_packet_sender(usb_sender);
+BidirectionalPacketBasedChannel usb_connection(endpoints, NUM_ENDPOINTS, usb_packet_sender);
+StreamToPacketConverter usb_stream_writer(usb_connection);
 
 class UART4Sender : public StreamWriter {
 private:
@@ -142,13 +158,12 @@ void communication_task(void const * argument) {
         // Now we check if there is any USB processing to do: we wait for up to 1 ms,
         // before going back to checking UART again.
         int USB_check_timeout = 1;
-        // Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
-        int32_t available = osSemaphoreWait(sem_usb_irq, USB_check_timeout);
-        if (available == osOK) {
-            // We have a new incoming USB transmission: handle it
-            HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
-            // Let the irq (OTG_FS_IRQHandler) fire again.
-            HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
+        int32_t status = osSemaphoreWait(sem_usb_irq, USB_check_timeout);
+        if (status == osOK) {
+            USB_receive_packet(USBRxBuffer, USBRxBufferLen);
+            // Allow receiving more bytes
+            USBD_CDC_SetRxBuffer(&hUsbDeviceFS, USBRxBuffer);
+            USBD_CDC_ReceivePacket(&hUsbDeviceFS);
         }
     }
 
@@ -156,17 +171,18 @@ void communication_task(void const * argument) {
     vTaskDelete(osThreadGetId());
 }
 
-
 void USB_receive_packet(const uint8_t *buffer, size_t length) {
-#ifdef INCLUDE_LEGACY_COMMANDS
+    //printf("[USB] got %d bytes, first is %c\r\n", length, buffer[0]); osDelay(5);
+#ifdef ENABLE_LEGACY_PROTOCOL
     const uint8_t *legacy_commands = (const uint8_t*)"pvcgsmo";
-    while (*(legacy_commands)) {
+    while (*legacy_commands && length) {
         if (buffer[0] == *(legacy_commands++)) {
-            legacy_parse_cmd(buffer, length, SERIAL_PRINTF_IS_USB);
+            //printf("[USB] process legacy command %c\r\n", buffer[0]); osDelay(5);
+            legacy_parse_cmd(buffer, length);
             length = 0;
         }
     }
 #endif
     
-    usb_connection.write_packet(buffer, length);
+    usb_stream_writer.write_bytes(buffer, length);
 }
diff --git a/Firmware/MotorControl/legacy_commands.c b/Firmware/MotorControl/legacy_commands.c
index 733db1e0..d7932c8a 100644
--- a/Firmware/MotorControl/legacy_commands.c
+++ b/Firmware/MotorControl/legacy_commands.c
@@ -8,7 +8,7 @@
 // This automatically updates to the interface that most
 // recently recieved a command. In the future we may want to separate
 // debug printf and the main serial comms.
-SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_NONE;
+SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_UART;
 
 /* Private constant data -----------------------------------------------------*/
 
@@ -112,13 +112,13 @@ static void print_monitoring(int limit);
 
 /* Function implementations --------------------------------------------------*/
 
-void legacy_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface) {
+void legacy_parse_cmd(const uint8_t* buffer, int len) {
     // Set response interface
-    serial_printf_select = response_interface;
+    serial_printf_select = SERIAL_PRINTF_IS_USB;
 
-    // TODO very hacky way of terminating sscanf at end of buffer:
-    // We should do some proper struct packing instead of using sscanf altogether
-    buffer[len-1] = 0;
+    // Cast away const and write beyond the array bounds. Because we can.
+    // (TODO: yeah maybe not, but this should be gone once we disable legacy commands)
+    ((uint8_t *)buffer)[len <= 63 ? len : 63] = 0;
 
     // check incoming packet type
     if (buffer[0] == 'p') {
@@ -214,9 +214,13 @@ void legacy_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interfac
             print_monitoring(limit);
         }
     }
+
+    serial_printf_select = SERIAL_PRINTF_IS_UART;
 }
 
 static void print_monitoring(int limit) {
+    serial_printf_select = SERIAL_PRINTF_IS_USB;
+
     for (int i=0;i<limit;i++) {
         switch (monitoring_slots[i].type) {
         case 0:
@@ -236,4 +240,6 @@ static void print_monitoring(int limit) {
         }
     }
     printf("\n");
+
+    serial_printf_select = SERIAL_PRINTF_IS_UART;
 }
diff --git a/Firmware/MotorControl/legacy_commands.h b/Firmware/MotorControl/legacy_commands.h
index af8e83f5..6ba81239 100644
--- a/Firmware/MotorControl/legacy_commands.h
+++ b/Firmware/MotorControl/legacy_commands.h
@@ -1,6 +1,10 @@
 #ifndef COMMANDS_H
 #define COMMANDS_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 /* Includes ------------------------------------------------------------------*/
 #include "low_level.h"
 /* Exported types ------------------------------------------------------------*/
@@ -23,6 +27,10 @@ extern uint16_t* exposed_uint16[];
 /* Exported functions --------------------------------------------------------*/
 
 /* Exported functions --------------------------------------------------------*/
-void legacy_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface);
+void legacy_parse_cmd(const uint8_t* buffer, int len);
+
+#ifdef __cplusplus
+}
+#endif
 
 #endif /* COMMANDS_H */
diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c
index 459f89ca..a2f8ee90 100644
--- a/Firmware/MotorControl/low_level.c
+++ b/Firmware/MotorControl/low_level.c
@@ -68,7 +68,7 @@ Motor_t motors[] = {
         // .calibration_ok = false,
         .motor_timer = &htim1,
         .next_timings = {TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2},
-        .control_deadline = TIM_1_8_PERIOD_CLOCKS,
+        .control_deadline = (3*TIM_1_8_PERIOD_CLOCKS)/2,
         .last_cpu_time = 0,
         .current_meas = {0.0f, 0.0f},
         .DC_calib = {0.0f, 0.0f},
diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/MotorControl/protocol.cpp
index e0955fbb..874608bf 100644
--- a/Firmware/MotorControl/protocol.cpp
+++ b/Firmware/MotorControl/protocol.cpp
@@ -28,11 +28,22 @@ const char *type_names_[] = {
 
 /* Private function prototypes -----------------------------------------------*/
 
+static void hexdump(const uint8_t* buf, size_t len);
 static void write_buffer(const uint8_t* input, size_t input_length, size_t* skip, uint8_t** output, size_t* output_length);
 static void write_string(const char* str, size_t* skip, uint8_t** output, size_t* output_length);
 
 /* Function implementations --------------------------------------------------*/
 
+// For debugging only
+void hexdump(const uint8_t* buf, size_t len) {
+    for (size_t pos = 0; pos < len; ++pos) {
+        printf(" %02x", buf[pos]);
+        if ((((pos + 1) % 16) == 0) || ((pos + 1) == len))
+            printf("\r\n");
+        osDelay(1);
+    }
+}
+
 // @brief Copies an input buffer to an output buffer, skipping a couple of bytes on the input buffer if required.
 // @param input: input buffer
 // @param input_length: number of bytes in the input buffer
@@ -63,17 +74,17 @@ void Endpoint::write_json(size_t id, size_t* skip, uint8_t** output, size_t* out
         *need_comma = true;
         return;
     } else if (type_id_ < END_TREE) {
-        if (need_comma)
+        if (*need_comma)
             write_string(",", skip, output, output_length);
 
         write_string("{\"name\":\"", skip, output, output_length);
         if (name_)
             write_string(name_, skip, output, output_length);
-        write_string(",\"id\":", skip, output, output_length);
+        write_string("\",\"id\":", skip, output, output_length);
         char id_buf[10];
         snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf
         write_string(id_buf, skip, output, output_length);
-        write_string("\"type\":\"", skip, output, output_length);
+        write_string(",\"type\":\"", skip, output, output_length);
         if (type_names_[type_id_])
             write_string(type_names_[type_id_], skip, output, output_length);
         write_string("\"", skip, output, output_length);
@@ -86,7 +97,7 @@ void Endpoint::write_json(size_t id, size_t* skip, uint8_t** output, size_t* out
                 write_string(",", skip, output, output_length);
                 write_string(json_modifier_, skip, output, output_length);
             }
-            write_string("\"}", skip, output, output_length);
+            write_string("}", skip, output, output_length);
             *need_comma = true;
         }
     }
@@ -139,6 +150,7 @@ int PacketToStreamConverter::write_packet(const uint8_t *buffer, size_t length)
 
     if (output_.write_bytes(header, sizeof(header)))
         return -1;
+    //printf("send payload:\r\n"); osDelay(5); hexdump(buffer, length);
     if (output_.write_bytes(buffer, length))
         return -1;
 
@@ -194,14 +206,15 @@ void BidirectionalPacketBasedChannel::interface_query(const uint8_t* input, size
 }
 
 int BidirectionalPacketBasedChannel::write_packet(const uint8_t* buffer, size_t length) {
+    //printf("got packet of length %d: \r\n", length); osDelay(5); hexdump(buffer, length);
     if (length < 4)
         return -1;
 
     // calculate CRC for later validation
     uint16_t crc16 = calc_crc16(CRC16_INIT, buffer, length - 2);
     uint8_t crc16_termination[] = {
-        (PROTOCOL_VERSION >> 8) & 0xff,
         (PROTOCOL_VERSION >> 0) & 0xff,
+        (PROTOCOL_VERSION >> 8) & 0xff,
         buffer[length - 2],
         buffer[length - 1]
     };
@@ -228,11 +241,13 @@ int BidirectionalPacketBasedChannel::write_packet(const uint8_t* buffer, size_t
         // For endpoint 0 this is just the protocol version, for all other endpoints it's a
         // CRC over the entire JSON descriptor tree (this may change in future versions).
         if (endpoint_id) {
-            crc16_termination[0] = (json_crc_ >> 8) & 0xff;
             crc16_termination[1] = (json_crc_ >> 0) & 0xff;
+            crc16_termination[0] = (json_crc_ >> 8) & 0xff;
         }
-        if (calc_crc16(crc16, crc16_termination, sizeof(crc16_termination)))
+        if (calc_crc16(crc16, crc16_termination, sizeof(crc16_termination))) {
+            //printf("crc16 for endpoint %d failed: expected termination %02x %02x\r\n", endpoint_id, crc16_termination[0], crc16_termination[1]); osDelay(5);
             return -1;
+        }
 
         // TODO: if more bytes than the MTU were requested, should we abort or just return as much as possible?
         uint16_t expected_response_length = read_le<uint16_t>(&buffer, &length);
@@ -244,8 +259,19 @@ int BidirectionalPacketBasedChannel::write_packet(const uint8_t* buffer, size_t
 
         // Send response
         if (expect_response) {
+            size_t tx_size = (requested_size - remaining_size) + 4;
             write_le<uint16_t>(seq_no | 0x8000, tx_buf_);
-            output_.write_packet(tx_buf_, (requested_size - remaining_size) + 4);
+
+            // Add protocol version for CRC calculation (overwritten by actual CRC)
+            tx_buf_[tx_size - 2] = (PROTOCOL_VERSION >> 0) & 0xff;
+            tx_buf_[tx_size - 1] = (PROTOCOL_VERSION >> 8) & 0xff;
+            crc16 = calc_crc16(CRC16_INIT, tx_buf_, tx_size);
+
+            // Append CRC in big endian
+            tx_buf_[tx_size - 2] = (crc16 >> 8) & 0xff;
+            tx_buf_[tx_size - 1] = (crc16 >> 0) & 0xff;
+
+            output_.write_packet(tx_buf_, tx_size);
         }
     }
 
diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp
index 289b7986..4b27756a 100644
--- a/Firmware/MotorControl/protocol.hpp
+++ b/Firmware/MotorControl/protocol.hpp
@@ -58,8 +58,8 @@
 
 
 constexpr uint8_t SYNC_BYTE = '$';
-constexpr uint8_t CRC8_INIT = 0;
-constexpr uint16_t CRC16_INIT = 0;
+constexpr uint8_t CRC8_INIT = 0x42;
+constexpr uint16_t CRC16_INIT = 0x1337;
 constexpr uint16_t PROTOCOL_VERSION = 1;
 constexpr uint16_t TX_BUF_SIZE = 64;
 
@@ -121,8 +121,8 @@ template<typename T>
 static inline T read_le(const uint8_t** buffer, size_t* length) {
     T result;
     size_t cnt = read_le(result, *buffer);
-    buffer += cnt;
-    length -= cnt;
+    *buffer += cnt;
+    *length -= cnt;
     return result;
 }
 
@@ -314,7 +314,7 @@ private:
         if (index < NUM_CHANNEL_SPECIFIC_ENDPOINTS){
             return &channel_specific_endpoints_[index];
         } else if (index < n_endpoints_) {
-            return &global_endpoints_[index];
+            return &global_endpoints_[index - NUM_CHANNEL_SPECIFIC_ENDPOINTS];
         } else {
             return nullptr;
         }
diff --git a/Firmware/Src/freertos.c b/Firmware/Src/freertos.c
index de5824c8..338bb965 100644
--- a/Firmware/Src/freertos.c
+++ b/Firmware/Src/freertos.c
@@ -62,7 +62,13 @@
 osThreadId defaultTaskHandle;
 
 /* USER CODE BEGIN Variables */
+// List of semaphores
+osSemaphoreId sem_usb_irq;
 
+// List of threads
+osThreadId thread_motor_0;
+osThreadId thread_motor_1;
+osThreadId thread_cmd_parse;
 /* USER CODE END Variables */
 
 /* Function prototypes -------------------------------------------------------*/
@@ -89,7 +95,7 @@ void MX_FREERTOS_Init(void) {
   /* USER CODE END RTOS_MUTEX */
 
   /* USER CODE BEGIN RTOS_SEMAPHORES */
-  // Init usb irq binary semaphore, and start with no tolkens by removing the starting one.
+  // Init usb irq binary semaphore, and start with no tokens by removing the starting one.
   osSemaphoreDef(sem_usb_irq);
   sem_usb_irq = osSemaphoreCreate(osSemaphore(sem_usb_irq), 1);
   osSemaphoreWait(sem_usb_irq, 0);
diff --git a/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c b/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c
index 6ea106f5..70b706bb 100644
--- a/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c
+++ b/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c
@@ -180,14 +180,7 @@ void ADC_IRQHandler(void)
 void OTG_FS_IRQHandler(void)
 {
   /* USER CODE BEGIN OTG_FS_IRQn 0 */
-
-  // Mask interrupt, and signal processing of interrupt by usb_cmd_thread
-  // The thread will re-enable the interrupt when all pending irqs are clear.
-  HAL_NVIC_DisableIRQ(OTG_FS_IRQn);
-  osSemaphoreRelease(sem_usb_irq);
-  // Bypass interrupt processing here
-  return;
-
+  
   /* USER CODE END OTG_FS_IRQn 0 */
   HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
   /* USER CODE BEGIN OTG_FS_IRQn 1 */
diff --git a/Firmware/Src/stm32f4xx_it.c b/Firmware/Src/stm32f4xx_it.c
index a740268a..14128ee7 100644
--- a/Firmware/Src/stm32f4xx_it.c
+++ b/Firmware/Src/stm32f4xx_it.c
@@ -266,13 +266,6 @@ void OTG_FS_IRQHandler(void)
 {
   /* USER CODE BEGIN OTG_FS_IRQn 0 */
 
-  // Mask interrupt, and signal processing of interrupt by usb_cmd_thread
-  // The thread will re-enable the interrupt when all pending irqs are clear.
-  HAL_NVIC_DisableIRQ(OTG_FS_IRQn);
-  osSemaphoreRelease(sem_usb_irq);
-  // Bypass interrupt processing here
-  return;
-
   /* USER CODE END OTG_FS_IRQn 0 */
   HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
   /* USER CODE BEGIN OTG_FS_IRQn 1 */
diff --git a/Firmware/Src/usbd_cdc_if.c b/Firmware/Src/usbd_cdc_if.c
index 2ebc251f..65a3b452 100644
--- a/Firmware/Src/usbd_cdc_if.c
+++ b/Firmware/Src/usbd_cdc_if.c
@@ -49,6 +49,8 @@
 /* Includes ------------------------------------------------------------------*/
 #include "usbd_cdc_if.h"
 /* USER CODE BEGIN INCLUDE */
+#include "cmsis_os.h"
+#include "freertos_vars.h"
 #include "utils.h"
 #include "commands.h"
 /* USER CODE END INCLUDE */
@@ -117,6 +119,8 @@ uint8_t UserTxBufferFS[APP_TX_DATA_SIZE];
   */ 
   extern USBD_HandleTypeDef hUsbDeviceFS;
 /* USER CODE BEGIN EXPORTED_VARIABLES */
+uint8_t *USBRxBuffer = UserRxBufferFS;
+uint32_t USBRxBufferLen;
 /* USER CODE END EXPORTED_VARIABLES */
 
 /**
@@ -268,15 +272,12 @@ static int8_t CDC_Control_FS  (uint8_t cmd, uint8_t* pbuf, uint16_t length)
 static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
 {
   /* USER CODE BEGIN 6 */
-  USBD_CDC_SetRxBuffer(&hUsbDeviceFS, &Buf[0]);
-  USBD_CDC_ReceivePacket(&hUsbDeviceFS);
 
-  //Append null termination at end of string
-  int null_idx = MACRO_MIN(*Len, APP_RX_DATA_SIZE-1);
-  Buf[null_idx] = 0;
-
-  USB_receive_packet(Buf, *Len+1);
+  // unblock the processing thread
+  USBRxBufferLen = *Len;
+  osSemaphoreRelease(sem_usb_irq);
 
+  // once the data is handled, the processing thread will start the next transmission
   return (USBD_OK);
   /* USER CODE END 6 */ 
 }
diff --git a/Firmware/tools/odrive/core.py b/Firmware/tools/odrive/core.py
deleted file mode 100644
index 60ca4776..00000000
--- a/Firmware/tools/odrive/core.py
+++ /dev/null
@@ -1,94 +0,0 @@
-"""
-Provides functions for the discovery of ODrive devices
-"""
-
-import sys
-import json
-import odrive
-import odrive.mock_device
-
-class DeviceProperty(property):
-    def __init__(self, device, id, type, can_read, can_write):
-        self._device = device
-        self._id = id
-        self._type = type
-        property.__init__(self,
-            self.fget if can_read else None,
-            self.fset if can_write else None)
-
-    def fget(self, obj):
-        self._device.send("r " + str(self._id) + "\n")
-        # TODO: message based receive
-        response = self._device.receive_until('\n')
-        return self._type(response.strip('\n'))
-
-    def fset(self, obj, value):
-        if not isinstance(value, self._type):
-            raise TypeError("expected value of type {}".format(self._type.__name__))
-        self._device.send("w " + str(self._id) + " " + str(value) + "\n")
-
-
-def create_object(json_data, namespace, device):
-    """
-    Creates an object that implements the specified JSON type description by
-    communicating with the provided device object
-    """
-
-    # Build property list from JSON
-    properties = {}
-    for item in json_data:
-        name = item.get("name", None)
-        if name is None:
-            sys.stderr.write("unnamed property in {}".format(namespace))
-            continue
-
-        type_str = item.get("type", None)
-        if type_str is None:
-            sys.stderr.write("property {} has no specified type".format(name))
-            continue
-
-        if type_str == "tree":
-            properties[name] = create_object(item["content"], namespace + "." + item["name"], device)
-        else:
-            if type_str == "float":
-                property_type = float
-            elif type_str == "int":
-                property_type = int
-            elif type_str == "bool":
-                property_type = bool
-            elif type_str == "uint16":
-                property_type = int
-            else:
-                sys.stderr.write("property {} has unsupported type {}".format(name, type_str))
-                continue
-
-            id_str = item.get("id", None)
-            if id_str is None:
-                sys.stderr.write("property {} has specified ID".format(name))
-                continue
-
-            access_mode = item.get("mode", "rw")
-            properties[name] = DeviceProperty(device, id_str, property_type,
-                                              'r' in access_mode,
-                                              'w' in access_mode)
-
-    # Create a type from the property list and instantiate it
-    jit_type = type(namespace, (object,), properties)
-    new_object = jit_type()
-    return new_object
-
-
-def find_any():
-    """
-    Scans for ODrives on all supported interfaces (currently only USB) and
-    returns the first device that is found. If no device is connected the
-    function blocks.
-    """
-    # TODO: do device discovery and instantiation in a separate thread
-    # TODO: test with real device
-    device = odrive.mock_device.MockDevice()
-    #device = odrive.usbbulk.poll_odrive_bulk_device()
-    device.send('j\n')
-    json_string = device.receive_until('\n')
-    #print("have JSON: " + json_string)
-    return create_object(json.loads(json_string), "odrive.usb_device", device)
diff --git a/Firmware/tools/demo.py b/tools/demo.py
old mode 100644
new mode 100755
similarity index 95%
rename from Firmware/tools/demo.py
rename to tools/demo.py
index 99ecd77e..08f69034
--- a/Firmware/tools/demo.py
+++ b/tools/demo.py
@@ -6,7 +6,7 @@ Example usage of the ODrive python library to monitor and control ODrive devices
 import odrive.core
 
 # Find a connected ODrive (this will block until you connect one)
-my_drive = odrive.core.find_any()
+my_drive = odrive.core.find_any(printer=print)
 
 # The above call returns a python object with a dynamically generated type. The
 # type hierarchy will correspond to the endpoint list in `MotorControl/protocol.cpp`.
diff --git a/tools/odrive/core.py b/tools/odrive/core.py
new file mode 100644
index 00000000..bb2ee653
--- /dev/null
+++ b/tools/odrive/core.py
@@ -0,0 +1,202 @@
+"""
+Provides functions for the discovery of ODrive devices
+"""
+
+import sys
+import time
+import json
+import usb.core
+import usb.util
+import odrive
+import odrive.mock_device
+import odrive.util
+import re
+import serial
+import time
+import os
+import odrive.protocol
+import itertools
+
+def noprint(x):
+  pass
+
+class DeviceProperty(property):
+    def __init__(self, device, id, type, can_read, can_write):
+        self._device = device
+        self._id = id
+        self._type = type
+        property.__init__(self,
+            self.fget if can_read else None,
+            self.fset if can_write else None)
+
+    def fget(self, obj):
+        self._device.send("r " + str(self._id) + "\n")
+        # TODO: message based receive
+        response = self._device.receive_until('\n')
+        return self._type(response.strip('\n'))
+
+    def fset(self, obj, value):
+        if not isinstance(value, self._type):
+            raise TypeError("expected value of type {}".format(self._type.__name__))
+        self._device.send("w " + str(self._id) + " " + str(value) + "\n")
+
+
+def create_object(json_data, namespace, channel):
+    """
+    Creates an object that implements the specified JSON type description by
+    communicating with the provided device object
+    """
+
+    # Build property list from JSON
+    properties = {}
+    for item in json_data:
+        name = item.get("name", None)
+        if name is None:
+            sys.stderr.write("unnamed property in {}".format(namespace))
+            continue
+
+        type_str = item.get("type", None)
+        if type_str is None:
+            sys.stderr.write("property {} has no specified type".format(name))
+            continue
+
+        if type_str == "tree":
+            properties[name] = create_object(item["content"], namespace + "." + item["name"], channel)
+        else:
+            if type_str == "float":
+                property_type = float
+            elif type_str == "int":
+                property_type = int
+            elif type_str == "bool":
+                property_type = bool
+            elif type_str == "uint16":
+                property_type = int
+            else:
+                sys.stderr.write("property {} has unsupported type {}".format(name, type_str))
+                continue
+
+            id_str = item.get("id", None)
+            if id_str is None:
+                sys.stderr.write("property {} has specified ID".format(name))
+                continue
+
+            access_mode = item.get("mode", "rw")
+            properties[name] = DeviceProperty(channel, id_str, property_type,
+                                              'r' in access_mode,
+                                              'w' in access_mode)
+
+    # Create a type from the property list and instantiate it
+    jit_type = type(namespace, (object,), properties)
+    new_object = jit_type()
+    return new_object
+
+class SerialDevice(odrive.protocol.StreamReader, odrive.protocol.StreamWriter):
+    def __init__(self, port, baud):
+        self._dev = serial.Serial(port, baud, timeout=1)
+
+    def write_bytes(self, bytes):
+        self._dev.write(bytes)
+
+    def read_bytes(self, n_bytes, deadline):
+        """
+        Returns n bytes unless the deadline is reached, in which case the bytes
+        that were read up to that point are returned. If deadline is None the
+        function blocks forever. A deadline before the current time corresponds
+        to non-blocking mode.
+        """
+        if deadline is None:
+            self._dev.timeout = None
+        else:
+            self._dev.timeout = max(deadline - time.monotonic(), 0)
+        return self._dev.read(n_bytes)
+
+    def read_bytes_or_fail(self, n_bytes, deadline):
+        result = self.read_bytes(n_bytes, deadline)
+        if len(result) < n_bytes:
+            raise odrive.protocol.TimeoutException()
+        return result
+
+
+def find_usb_channels(vid_pid_pairs=odrive.util.USB_VID_PID_PAIRS, printer=noprint):
+    """
+    Scans for compatible USB devices.
+    Returns a generator of odrive.protocol.Channel objects.
+    """
+    for vid_pid_pair in vid_pid_pairs:
+        usb_device = usb.core.find(idVendor=vid_pid_pair[0], idProduct=vid_pid_pair[1])
+        if usb_device is None:
+            continue
+        printer("Found ODrive via PyUSB")
+        bulk_device = odrive.usbbulk.USBBulkDevice(usb_device, printer)
+        printer(bulk_device.info())
+        bulk_device.init(printer)
+        yield odrive.protocol.Channel(
+                "USB device {}:{}".format(vid_pid_pair[0], vid_pid_pair[1]),
+                bulk_device, bulk_device)
+
+def find_serial_channels(printer=noprint):
+    """
+    Scans for serial devices.
+    Returns a generator of odrive.protocol.Channel objects.
+    Not every returned object necessarily represents a compatible device.
+    """
+    # Look for serial device
+    # TODO: OS specific heuristic to find serial ports
+    for serial_port in filter(re.compile(r'^tty\.usbmodem').search, os.listdir('/dev')):
+        serial_port = '/dev/' + serial_port
+        # If this is actually a USB device, the baudrate setting has no effect
+        try:
+            serial_device = SerialDevice(serial_port, 115200)
+        except serial.serialutil.SerialException:
+            printer("could not open " + serial_port)
+            continue
+        input = odrive.protocol.PacketFromStreamConverter(serial_device)
+        output = odrive.protocol.PacketToStreamConverter(serial_device)
+        yield odrive.protocol.Channel(
+                "serial port {}@{}".format(serial_port, 115200),
+                input, output)
+
+
+def find_all(printer=noprint):
+    """
+    Returns a generator with all the connected devices that speak the ODrive protocol
+    """
+    usb_channels = find_usb_channels(printer=printer)
+    serial_channels = find_serial_channels(printer=printer)
+    for channel in itertools.chain(usb_channels, serial_channels):
+        # TODO: blacklist known bad channels
+        printer("Connecting to device on " + channel._name)
+        try:
+            json_bytes = channel.remote_endpoint_read_buffer(0)
+        except (odrive.protocol.TimeoutException, odrive.protocol.ChannelBrokenException):
+            printer("no response - probably incompatible")
+            continue
+        try:
+            json_string = json_bytes.decode("ascii")
+        except UnicodeDecodeError:
+            printer("device responded on endpoint 0 with something that is not ASCII")
+            continue
+        print("JSON: " + json_string)
+        try:
+            json_data = json.loads(json_string)
+        except json.decoder.JSONDecodeError:
+            printer("device responded on endpoint 0 with something that is not JSON")
+            continue
+        yield create_object(json_data, "odrive", channel)
+
+
+def find_any(printer=noprint):
+    """
+    Scans for ODrives on all supported interfaces and returns the first device
+    that is found. If no device is connected the function blocks.
+    """
+    # TODO: do device discovery and instantiation in a separate thread and just wait on a semaphore here
+
+    # poll for device
+    printer("looking for ODrive...")
+    while True:
+        dev = next(find_all(printer=printer), None)
+        if not dev is None:
+            return dev
+        printer("no device found")
+        time.sleep(1)
diff --git a/Firmware/tools/odrive/mock_device.py b/tools/odrive/mock_device.py
similarity index 100%
rename from Firmware/tools/odrive/mock_device.py
rename to tools/odrive/mock_device.py
diff --git a/Firmware/tools/odrive/protocol.py b/tools/odrive/protocol.py
similarity index 52%
rename from Firmware/tools/odrive/protocol.py
rename to tools/odrive/protocol.py
index d0c34c18..b5aa9099 100644
--- a/Firmware/tools/odrive/protocol.py
+++ b/tools/odrive/protocol.py
@@ -1,10 +1,11 @@
 # See protocol.hpp for an overview of the protocol
 
+import time
 import struct
 
-SYNC_BYTE = '$'
-CRC8_INIT = 0
-CRC16_INIT = 0
+SYNC_BYTE = ord('$')
+CRC8_INIT = 0x42
+CRC16_INIT = 0x1337
 PROTOCOL_VERSION = 1
 
 CRC8_DEFAULT = 0x37 # this must match the polynomial in the C++ implementation
@@ -24,28 +25,41 @@ def calc_crc(remainder, value, polynomial, bitwidth):
     return remainder & ((1 << bitwidth) - 1)
 
 def calc_crc8(remainder, value):
-    if type(value) == bytearray or isinstance(value, list):
-        for b in value:
-            remainder = calc_crc(remainder, b, CRC8_DEFAULT, 8)
+    if isinstance(value, bytearray) or isinstance(value, bytes) or isinstance(value, list):
+        for byte in value:
+            remainder = calc_crc(remainder, byte, CRC8_DEFAULT, 8)
     else:
-        remainder = calc_crc(remainder, b, CRC8_DEFAULT, 8)
+        remainder = calc_crc(remainder, byte, CRC8_DEFAULT, 8)
     return remainder
 
 def calc_crc16(remainder, value):
-    if type(value) == bytearray or isinstance(value, list):
-        for b in value:
-            remainder = calc_crc(remainder, b, CRC16_DEFAULT, 16)
+    if isinstance(value, bytearray) or isinstance(value, bytes) or isinstance(value, list):
+        for byte in value:
+            remainder = calc_crc(remainder, byte, CRC16_DEFAULT, 16)
     else:
-        remainder = calc_crc(remainder, b, CRC16_DEFAULT, 16)
+        remainder = calc_crc(remainder, value, CRC16_DEFAULT, 16)
     return remainder
 
 # Can be verified with http://www.sunshine2k.de/coding/javascript/crc/crc_js.html:
 #print(hex(calc_crc8(0x12, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37])))
 #print(hex(calc_crc16(0xfeef, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37])))
 
+
+class TimeoutException(Exception):
+    pass
+
+class ChannelBrokenException(Exception):
+    pass
+
+class StreamReader(object):
+    pass
+
 class StreamWriter(object):
     pass
-    
+
+class PacketReader(object):
+    pass
+
 class PacketWriter(object):
     pass
 
@@ -59,12 +73,17 @@ class StreamToPacketConverter(StreamWriter):
         self._output = output
 
     def write_bytes(self, bytes):
+        """
+        Processes an arbitrary number of bytes. If one or more full packets are
+        are received, they are sent to this instance's output PacketWriter.
+        Incomplete packets are buffered between subsequent calls to this function.
+        """
         result = None
 
-        for b in bytes:
+        for byte in bytes:
             if (len(self._header) < 3):
                 # Process header byte
-                self._header.append(b)
+                self._header.append(byte)
                 if (len(self._header) == 1) and (self._header[0] != SYNC_BYTE):
                     self._header = []
                 elif (len(self._header) == 2) and (self._header[1] & 0x80):
@@ -75,18 +94,18 @@ class StreamToPacketConverter(StreamWriter):
                     self._packet_length = self._header[1]
             else:
                 # Process payload byte
-                self._packet.append(b)
+                self._packet.append(byte)
 
             # If both header and packet are fully received, hand it on to the packet processor
             if (len(self._header) == 3) and (len(self._packet) == self._packet_length):
                 try:
                     self._output.write_packet(self._packet)
-                except Exception, ex:
+                except Exception as ex:
                     result = ex
                 self._header = []
                 self._packet = []
                 self._packet_length = 0
-        
+
         if isinstance(result, Exception):
             raise Exception("something went wrong")
 
@@ -105,13 +124,48 @@ class PacketToStreamConverter(PacketWriter):
         self._output.write_bytes(header)
         self._output.write_bytes(packet)
 
+class PacketFromStreamConverter(PacketReader, StreamWriter):
+    def __init__(self, input):
+        self._input = input
+    
+    def read_packet(self, deadline):
+        """
+        Requests bytes from the underlying input stream until a full packet is
+        received or the deadline is reached, in which case None is returned. A
+        deadline before the current time corresponds to non-blocking mode.
+        """
+        while True:
+            header = bytes()
+
+            header = header + self._input.read_bytes_or_fail(1, deadline)
+            if (header[0] != SYNC_BYTE):
+                #print("sync byte mismatch")
+                continue
+
+            header = header + self._input.read_bytes_or_fail(1, deadline)
+            if (header[1] & 0x80):
+                #print("packet too large")
+                continue # TODO: support packets larger than 128 bytes
+
+            header = header + self._input.read_bytes_or_fail(1, deadline)
+            if calc_crc8(CRC8_INIT, header) != 0:
+                #print("crc8 mismatch")
+                continue
+
+            packet_length = header[1]
+            return self._input.read_bytes_or_fail(packet_length, deadline)
+
 
 class Channel(PacketWriter):
     _outbound_seq_no = 0
     _interface_definition_crc = bytearray(2)
     _expected_acks = {}
 
-    def __init__(self, input, output):
+    # Chose these parameters to be sensible for a specific transport layer
+    _resend_delay = 5.0     # [s]
+    _send_attempts = 5
+
+    def __init__(self, name, input, output):
         """
         Params:
         input: A PacketReader where this channel will source packets from on
@@ -119,10 +173,13 @@ class Channel(PacketWriter):
                directly by calling write_packet on this instance.
         output: A PacketWriter where this channel will put outgoing packets.
         """
+        self._name = name
         self._input = input
         self._output = output
 
     def remote_endpoint_operation(self, endpoint_id, input, expect_ack, output_length):
+        if input is None:
+            input = bytearray(0)
         if (len(input) >= 128):
             raise Exception("packet larger than 127 currently not supported")
 
@@ -135,30 +192,58 @@ class Channel(PacketWriter):
         packet = packet + input
 
         crc16 = calc_crc16(CRC16_INIT, packet)
-        if (endpoint_id == 0):
+        if (endpoint_id & 0x7fff == 0):
+            print("append crc16 for " + str(struct.pack('<H', PROTOCOL_VERSION)))
             crc16 = calc_crc16(crc16, struct.pack('<H', PROTOCOL_VERSION))
         else:
+            # TODO: set _interface_definition_crc
             crc16 = calc_crc16(crc16, self._interface_definition_crc)
 
-        packet = packet + struct.pack('<H', crc16)
+        # append CRC in big endian
+        packet = packet + struct.pack('>H', crc16)
 
         if (expect_ack):
             self._expected_acks[seq_no] = None
-
-        self._output.write_packet(packet)
-
-        if (expect_ack):
-            # Read and process packets until we get an ack
-            # TODO: add timeout
-            # TODO: support I/O driven reception (wait on semaphore)
-            while (self._expected_acks[seq_no] is None):
-                self.write_packet(self._input.read_packet())
-
-            return self._expected_acks.pop(seq_no, None)
+            attempt = 0
+            while (attempt < self._send_attempts):
+                self._output.write_packet(packet)
+                deadline = time.monotonic() + self._resend_delay
+                # Read and process packets until we get an ack or need to resend
+                # TODO: support I/O driven reception (wait on semaphore)
+                while True:
+                    try:
+                        response = self._input.read_packet(deadline)
+                    except TimeoutException:
+                        break # resend
+                    # process response, which is hopefully our ACK
+                    self.write_packet(response)
+                    if not self._expected_acks[seq_no] is None:
+                        return self._expected_acks.pop(seq_no, None)
+                    break
+                # TODO: record channel statistics
+                attempt += 1
+            raise ChannelBrokenException()
         else:
+            # fire and forget
+            self._output.write_packet(packet)
             return None
     
+    def remote_endpoint_read_buffer(self, endpoint_id):
+        """
+        Handles reads from long endpoints
+        """
+        # TODO: handle device that could (maliciously) send infinite stream
+        buffer = bytes()
+        while True:
+            chunk_length = 64
+            chunk = self.remote_endpoint_operation(0, struct.pack("<I", len(buffer)), True, chunk_length)
+            if (len(chunk) == 0):
+                break
+            buffer += chunk
+        return buffer
+
     def write_packet(self, packet):
+        #print("process packet")
         if (len(packet) < 4):
             raise Exception("packet too short")
 
diff --git a/tools/odrive/usbbulk.py b/tools/odrive/usbbulk.py
index cdda7641..d5f18e9a 100644
--- a/tools/odrive/usbbulk.py
+++ b/tools/odrive/usbbulk.py
@@ -4,43 +4,18 @@
 import usb.core
 import usb.util
 import sys
-import time
-
-from odrive.util import ODriveNotConnectedError
-from odrive.util import USBID_VID_ODRIVE, USBID_PID_ODRIVE
-
-BULK_DEVICE_NOT_FOUND     = 'ODrive BulkDevice Not Found'
-BULK_DEVICE_FOUND         = 'ODrive BulkDevice Found!'
+import odrive.protocol
 
 def noprint(x):
   pass
 
-def poll_odrive_bulk_device(intv=1, printer=noprint):
-  # look for device
-  while 1:
-    time.sleep(intv)
-    try:
-      dev = ODriveBulkDevice()
-      break
-    except:
-      printer(BULK_DEVICE_NOT_FOUND)
-  # found device
-  printer(BULK_DEVICE_FOUND)
-  return dev
-
-class ODriveBulkDevice():
-  def __init__(self, idVendor=None, idProduct=None):
-    # ODrive idVendor
-    if idVendor is None:
-      idVendor = USBID_VID_ODRIVE
-    # ODrive idProduct
-    if idProduct is None:
-      idProduct = USBID_PID_ODRIVE
-    # find our device
-    self.dev = usb.core.find(idVendor=idVendor, idProduct=idProduct)
-    # was it found?
-    if self.dev is None:
-      raise ODriveNotConnectedError()
+# Even though USB is packet based, we do stream based communication because some
+# systems don't allow direct access to the USB endpoints, in which case the
+# device has to behave like a serial device.
+class USBBulkDevice(odrive.protocol.StreamReader, odrive.protocol.StreamWriter):
+  def __init__(self, dev, printer=noprint):
+    self.dev = dev
+    self._name = "USB device {}:{}".format(dev.idVendor, dev.idProduct)
 
   ##
   # information about the connected device
@@ -56,14 +31,13 @@ class ODriveBulkDevice():
           string += "\t\tEndpointAddress {0}\n".format(ep.bEndpointAddress)
     return string
 
-  def init(self):
+  def init(self, printer=noprint):
     try:
-      string = ""
       # detach kernel driver
       try:
         if self.dev.is_kernel_driver_active(1):
           self.dev.detach_kernel_driver(1)
-          string += "Detached Kernel Driver\n"
+          printer("Detached Kernel Driver\n")
       except NotImplementedError:
         pass #is_kernel_driver_active not implemented on Windows
       # set the active configuration. With no arguments, the first
@@ -81,7 +55,7 @@ class ODriveBulkDevice():
               usb.util.ENDPOINT_OUT
       )
       assert self.epw is not None
-      string += "EndpointAddress for writing {}\n".format(self.epw.bEndpointAddress)
+      printer("EndpointAddress for writing {}\n".format(self.epw.bEndpointAddress))
       # read endpoint
       self.epr = usb.util.find_descriptor(self.intf,
           # match the first IN endpoint
@@ -91,8 +65,7 @@ class ODriveBulkDevice():
               usb.util.ENDPOINT_IN
       )
       assert self.epr is not None
-      string += "EndpointAddress for reading {}\n".format(self.epr.bEndpointAddress)
-      return string
+      printer("EndpointAddress for reading {}\n".format(self.epr.bEndpointAddress))
     except usb.core.USBError:
       #return -1
       raise
@@ -100,7 +73,7 @@ class ODriveBulkDevice():
   def shutdown(self):
     return 0
 
-  def send(self, usbBuffer):
+  def write_bytes(self, usbBuffer):
     try:
       ret = self.epw.write(usbBuffer, 0)
       return ret
@@ -108,7 +81,7 @@ class ODriveBulkDevice():
       #return -1
       raise
 
-  def receive(self, bufferLen):
+  def read_bytes(self, bufferLen):
     try:
       ret = self.epr.read(bufferLen, 100)
       return ret
diff --git a/tools/odrive/util.py b/tools/odrive/util.py
index e776f6c3..cd9d866c 100644
--- a/tools/odrive/util.py
+++ b/tools/odrive/util.py
@@ -1,9 +1,6 @@
 # requires pyusb
 #   pip install --pre pyusb
 
-import time
-import usb.core
-import usb.util
 
 # Exceptions
 class ODriveError(Exception):
@@ -12,33 +9,10 @@ class ODriveError(Exception):
 class ODriveNotConnectedError(ODriveError):
   pass
 
-# ODrive generic        1209:0D31
-USBID_VID_ODRIVE        = 0x1209
-USBID_PID_ODRIVE        = 0x0D31
-# ODrive rev 3.1        1209:0D31
-USBID_VID_ODRIVE_3_1    = USBID_VID_ODRIVE
-USBID_PID_ODRIVE_3_1    = USBID_PID_ODRIVE
-USB_DEV_ODRIVE_3_1      = (USBID_VID_ODRIVE_3_1, USBID_PID_ODRIVE_3_1)
+USB_DEV_ODRIVE_3_1      = (0x1209, 0x0D31)
+USB_DEV_ODRIVE_3_3      = (0x1209, 0x0D33)
 # all devices
-all_devices         = [USB_DEV_ODRIVE_3_1]
-
-# device messages
-DEVICE_NOT_FOUND    = 'ODrive Not Found'
-DEVICE_FOUND        = 'ODrive Found!'
+USB_VID_PID_PAIRS         = [USB_DEV_ODRIVE_3_1, USB_DEV_ODRIVE_3_3]
 
 def noprint(x):
   pass
-
-def poll_odrive_device(devices=all_devices, intv=1, printer=noprint):
-  while 1:
-    time.sleep(intv)
-    for device in devices:
-      try:
-        idVendor  = device[0]
-        idProduct = device[1]
-        dev = usb.core.find(idVendor=idVendor, idProduct=idProduct)
-        if dev is not None:
-          printer(DEVICE_FOUND)
-          return device
-      except:
-        printer(DEVICE_NOT_FOUND)