make remote_endpoint_operation thread-safe

tools/odrive/discovery.py

@@ -7,6 +7,7 @@ import json
 import time
 import threading
 import odrive.protocol
+import odrive.utils
 import odrive.remote_object
 import odrive.usbbulk_transport
 import odrive.serial_transport
@@ -35,29 +36,35 @@ def find_all(path, serial_number,
         This queries the endpoint 0 on that channel to gain information
         about the interface, which is then used to init the corresponding object.
         """
-        printer("Connecting to device on " + channel._name)
         try:
-            json_bytes = channel.remote_endpoint_read_buffer(0)
-        except (odrive.protocol.TimeoutException, odrive.protocol.ChannelBrokenException):
-            raise odrive.protocol.DeviceInitException("no response - probably incompatible")
-        json_crc16 = odrive.protocol.calc_crc16(odrive.protocol.PROTOCOL_VERSION, json_bytes)
-        channel._interface_definition_crc = json_crc16
-        try:
-            json_string = json_bytes.decode("ascii")
-        except UnicodeDecodeError:
-            raise odrive.protocol.DeviceInitException("device responded on endpoint 0 with something that is not ASCII")
-        printer("JSON: " + json_string)
-        try:
-            json_data = json.loads(json_string)
-        except json.decoder.JSONDecodeError as error:
-            raise odrive.protocol.DeviceInitException("device responded on endpoint 0 with something that is not JSON: " + str(error))
-        json_data = {"name": "odrive", "members": json_data}
-        obj = odrive.remote_object.RemoteObject(json_data, None, channel, None, printer)
-        device_serial_number = serial_number if hasattr(obj, 'serial_number') else "[unknown serial number]"
-        if serial_number != None and device_serial_number != serial_number:
-            printer("Ignoring device with serial number {}".format(device_serial_number))
-            return
-        did_discover_object_callback(obj)
+            printer("Connecting to device on " + channel._name)
+            try:
+                json_bytes = channel.remote_endpoint_read_buffer(0)
+            except (odrive.utils.TimeoutException, odrive.protocol.ChannelBrokenException):
+                printer("no response - probably incompatible")
+                return
+            json_crc16 = odrive.protocol.calc_crc16(odrive.protocol.PROTOCOL_VERSION, json_bytes)
+            channel._interface_definition_crc = json_crc16
+            try:
+                json_string = json_bytes.decode("ascii")
+            except UnicodeDecodeError:
+                printer("device responded on endpoint 0 with something that is not ASCII")
+                return
+            printer("JSON: " + json_string)
+            try:
+                json_data = json.loads(json_string)
+            except json.decoder.JSONDecodeError as error:
+                printer("device responded on endpoint 0 with something that is not JSON: " + str(error))
+                return
+            json_data = {"name": "odrive", "members": json_data}
+            obj = odrive.remote_object.RemoteObject(json_data, None, channel, None, printer)
+            device_serial_number = serial_number if hasattr(obj, 'serial_number') else "[unknown serial number]"
+            if serial_number != None and device_serial_number != serial_number:
+                printer("Ignoring device with serial number {}".format(device_serial_number))
+                return
+            did_discover_object_callback(obj)
+        except Exception as ex:
+            printer("Unexpected exception after discovering channel: " + str(ex))
 
     # For each connection type, kick off an appropriate discovery loop
     for search_spec in path.split(','):

tools/odrive/protocol.py

@@ -3,6 +3,10 @@
 import time
 import struct
 import sys
+import threading
+import odrive.utils
+from odrive.utils import wait_any
+from odrive.utils import Event
 
 import abc
 
@@ -63,9 +67,6 @@ def calc_crc16(remainder, value):
 #print(hex(calc_crc16(0xfeef, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37])))
 
 
-class TimeoutException(Exception):
-    pass
-
 class DeviceInitException(Exception):
     pass
 
@@ -204,11 +205,14 @@ class Channel(PacketSink):
     _outbound_seq_no = 0
     _interface_definition_crc = 0
     _expected_acks = {}
+    _responses = {}
 
     # Choose these parameters to be sensible for a specific transport layer
     _resend_timeout = 0.1     # [s]
     _send_attempts = 5
 
+    _channel_broken = Event()
+
     def __init__(self, name, input, output):
         """
         Params:
@@ -220,6 +224,33 @@ class Channel(PacketSink):
         self._name = name
         self._input = input
         self._output = output
+        self._my_lock = threading.Lock()
+        self.start_receiver_thread(Event()) # TODO: use app_shutdown_token
+
+    def start_receiver_thread(self, cancellation_token):
+        """
+        Starts the receiver thread that processes incoming messages.
+        The thread quits as soon as the channel enters a broken state.
+        """
+        def receiver_thread():
+            try:
+                while (not cancellation_token.is_set()) and (not self._channel_broken.is_set()):
+                    # Set an arbitrary deadline because the get_packet function
+                    # currently doesn't support a cancellation_token
+                    deadline = time.monotonic() + 1.0
+                    try:
+                        response = self._input.get_packet(deadline)
+                    except odrive.utils.TimeoutException:
+                        continue # try again
+                    except ChannelDamagedException:
+                        continue # try again
+                    # Process response
+                    # This should not throw an exception, otherwise the channel breaks
+                    self.process_packet(response)
+                print("receiver thread is exiting")
+            finally:
+                self._channel_broken.set()
+        threading.Thread(target=receiver_thread, daemon=True).start()
 
     def remote_endpoint_operation(self, endpoint_id, input, expect_ack, output_length):
         if input is None:
@@ -230,9 +261,13 @@ class Channel(PacketSink):
         if (expect_ack):
             endpoint_id |= 0x8000
 
-        self._outbound_seq_no = ((self._outbound_seq_no + 1) & 0x7fff)
-        self._outbound_seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the legacy protocol
-        seq_no = self._outbound_seq_no
+        self._my_lock.acquire()
+        try:
+            self._outbound_seq_no = ((self._outbound_seq_no + 1) & 0x7fff)
+            seq_no = self._outbound_seq_no
+        finally:
+            self._my_lock.release()
+        seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the legacy protocol
         packet = struct.pack('<HHH', seq_no, endpoint_id, output_length)
         packet = packet + input
 
@@ -245,32 +280,32 @@ class Channel(PacketSink):
         packet = packet + struct.pack('<H', trailer)
 
         if (expect_ack):
-            self._expected_acks[seq_no] = None
-            attempt = 0
-            while (attempt < self._send_attempts):
-                try:
-                    self._output.process_packet(packet)
-                except ChannelDamagedException:
-                    attempt += 1
-                    continue # resend
-                deadline = time.monotonic() + self._resend_timeout
-                # Read and process packets until we get an ack or need to resend
-                # TODO: support I/O driven reception (wait on semaphore)
-                while True:
+            ack_event = Event()
+            self._expected_acks[seq_no] = ack_event
+            try:
+                attempt = 0
+                while (attempt < self._send_attempts):
+                    self._my_lock.acquire()
                     try:
-                        response = self._input.get_packet(deadline)
-                    except TimeoutException:
-                        break # resend
+                        self._output.process_packet(packet)
                     except ChannelDamagedException:
-                        break # resend
-                    # process response, which is hopefully our ACK
-                    self.process_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()
+                        attempt += 1
+                        continue # resend
+                    finally:
+                        self._my_lock.release()
+                    # Wait for ACK until the resend timeout is exceeded
+                    try:
+                        if wait_any(ack_event, self._channel_broken, timeout=self._resend_timeout) != 0:
+                            raise ChannelBrokenException()
+                    except odrive.utils.TimeoutException:
+                        attempt += 1
+                        continue # resend
+                    return self._responses.pop(seq_no)
+                    # TODO: record channel statistics
+                raise ChannelBrokenException() # Too many resend attempts
+            finally:
+                self._expected_acks.pop(seq_no)
+                self._responses.pop(seq_no, None)
         else:
             # fire and forget
             self._output.process_packet(packet)
@@ -300,7 +335,12 @@ class Channel(PacketSink):
 
         if (seq_no & 0x8000):
             seq_no &= 0x7fff
-            self._expected_acks[seq_no] = packet[2:]
+            ack_signal = self._expected_acks.get(seq_no, None)
+            if (ack_signal):
+                self._responses[seq_no] = packet[2:]
+                ack_signal.set()
+            else:
+                print("received unexpected ACK: " + str(seq_no))
 
         else:
             #if (calc_crc16(CRC16_INIT, struct.pack('<HBB', PROTOCOL_VERSION, packet[-2], packet[-1]))):

tools/odrive/serial_transport.py

@@ -9,6 +9,7 @@ import time
 import serial
 import serial.tools.list_ports
 import odrive.protocol
+import odrive.utils
 
 ODRIVE_BAUDRATE = 115200
 
@@ -35,7 +36,7 @@ class SerialStreamTransport(odrive.protocol.StreamSource, odrive.protocol.Stream
     def get_bytes_or_fail(self, n_bytes, deadline):
         result = self.get_bytes(n_bytes, deadline)
         if len(result) < n_bytes:
-            raise odrive.protocol.TimeoutException("expected {} bytes but got only {}", n_bytes, len(result))
+            raise odrive.utils.TimeoutException("expected {} bytes but got only {}", n_bytes, len(result))
         return result