From 21b93da627503b7523362d236ca826a4ef2b83e5 Mon Sep 17 00:00:00 2001
From: Miller Lowe <Miller.Lowe@trailtech.net>
Date: Wed, 27 Nov 2019 12:19:02 -0800
Subject: [PATCH 1/2] When used on a busy can bus on Linux/socketcan we had
 overruns as soon as the 500k bus utilization went above 30%.

This commit enables the use of socketcan filtering when used as a UDS device.

overall commands Sent        : 6000
overall average response time: 0.0048991614580154415
overall standard deviation   : 0.002319849086112244
overall max delay            : 0.05056476593017578
overall min delay            : 0.0018734931945800781
---
 pyvit/dispatch.py     | 19 +++++++++++++++++++
 pyvit/hw/socketcan.py | 18 ++++++++++++++++++
 pyvit/proto/uds.py    |  2 ++
 3 files changed, 39 insertions(+)

diff --git a/pyvit/dispatch.py b/pyvit/dispatch.py
index 3d4bdfb..6f6b3d4 100644
--- a/pyvit/dispatch.py
+++ b/pyvit/dispatch.py
@@ -26,6 +26,22 @@ def __init__(self, device, single_process = False):
         self._running = False
         self._single_process = single_process
 
+        #
+        # Will only operate on filters if the underlying can hardware supports 
+        # filtering.
+        #
+        if hasattr(device, 'apply_filters'):
+            self._filter_list = []
+        else:
+            self._filter_list = None
+
+    def add_filter(self, id, mask):
+        if self._filter_list != None:
+            self._filter_list.append((id,mask))
+
+        if self._device.running:
+            self._device.apply_filters(self._filter_list)
+
     def add_receiver(self, rx_queue):
         if self.is_running:
             raise Exception('dispatcher must be stopped to add receiver')
@@ -53,6 +69,9 @@ def start(self):
         if self.is_running:
             raise Exception('dispatcher already running')
 
+        if self._filter_list:
+            self._device.apply_filters(self._filter_list)
+
         self._device.start()
         self._tx_queue = Queue()
 
diff --git a/pyvit/hw/socketcan.py b/pyvit/hw/socketcan.py
index 50bfb21..0d2f221 100644
--- a/pyvit/hw/socketcan.py
+++ b/pyvit/hw/socketcan.py
@@ -6,6 +6,7 @@
 
 
 class SocketCanDev:
+
     def __init__(self, ndev):
         self.running = False
 
@@ -25,6 +26,23 @@ def start(self):
     def stop(self):
         pass
 
+    def apply_filters(self, filter_list):
+        #
+        # The can_filter struct looks like
+        # struct can_filter{
+        #       uint32_t id;
+        #       uint32_t mask;
+        # };
+        # the *optval is an array of the above structures.  The size of the array 
+        # cannot exceed 512 entries.
+        #
+        encodedArray = b""
+        for (fid, mask) in filter_list:
+            entry = struct.pack('<LL', fid, mask)
+            encodedArray += entry
+
+        self.socket.setsockopt(socket.SOL_CAN_RAW, socket.CAN_RAW_FILTER, encodedArray)
+
     def recv(self):
         assert self.running, 'device not running'
         frame_format = "=IB3xBBBBBBBB"
diff --git a/pyvit/proto/uds.py b/pyvit/proto/uds.py
index 149d275..ebbd1c0 100644
--- a/pyvit/proto/uds.py
+++ b/pyvit/proto/uds.py
@@ -1448,6 +1448,8 @@ def __init__(self, dispatcher=False, tx_arb_id=0x7E0, rx_arb_id=0x7E8, extended_
             self.transport_layer = IsotpNormalAddressing(dispatcher, tx_arb_id)
             self.transport_layer.rx_arb_id = rx_arb_id
 
+        dispatcher.add_filter(id=rx_arb_id, mask=0x7ff)
+
     def request(self, service, timeout=0.5):
         self.transport_layer.send(service.encode())
         if self.transport_layer.N_TAtype == N_TAtype.physical:

From dae55fc9a397a8bb257b576072f10547b6856e87 Mon Sep 17 00:00:00 2001
From: Miller Lowe <Miller.Lowe@trailtech.net>
Date: Mon, 2 Dec 2019 13:33:03 -0800
Subject: [PATCH 2/2] clear queues on stop

---
 pyvit/dispatch.py | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/pyvit/dispatch.py b/pyvit/dispatch.py
index 3d4bdfb..89f0e8e 100644
--- a/pyvit/dispatch.py
+++ b/pyvit/dispatch.py
@@ -78,6 +78,16 @@ def stop(self):
         self._device.stop()
         self._running = False
 
+        #
+        # Clear any data from current queues
+        #
+        for q in self._rx_queues:
+            while not q.empty():
+                q.get()
+        while not self._tx_queue.empty():
+                q.get()
+
+
     @property
     def is_running(self):
         return self._running