Backport socketcan fix: Read the BCM status before creating a Task

karlding · hardbyte · commit 0e3812d218f0 · 2019-12-29T09:32:50.000+13:00
Currently bus.send_periodic blindly wraps the BCM socket API, and sends a BCM operation with the TX_SETUP opcode. However, the semantics that the kernel driver provides are an "upsert". As such, when one attempts to create two Tasks with the same CAN Arbitration ID, it ends up silently modifying the periodic messages, which is contrary to what our API implies. This fixes the problem by first sending a TX_READ opcode with the CAN Arbitration ID that we wish to create. The kernel driver will return -EINVAL if a periodic transmission with the given Arbitration ID does not exist. If this is the case, then we can continue creating the Task, otherwise we error and notify the user. hardbyte#605
diff --git a/can/interfaces/socketcan/constants.py b/can/interfaces/socketcan/constants.py
@@ -9,8 +9,9 @@
 CAN_EFF_FLAG        = 0x80000000
 
 # BCM opcodes
-CAN_BCM_TX_SETUP    = 1
-CAN_BCM_TX_DELETE   = 2
+CAN_BCM_TX_SETUP   = 1
+CAN_BCM_TX_DELETE  = 2
+CAN_BCM_TX_READ    = 3
 
 # BCM flags
 SETTIMER            = 0x0001
diff --git a/can/interfaces/socketcan/socketcan.py b/can/interfaces/socketcan/socketcan.py
@@ -346,8 +346,39 @@ def _tx_setup(self, message):
             count = 0
             ival1 = 0
             ival2 = self.period
-        header = build_bcm_transmit_header(self.can_id_with_flags, count, ival1,
-                                           ival2, self.flags)
+
+        # First do a TX_READ before creating a new task, and check if we get
+        # EINVAL. If so, then we are referring to a CAN message with the same
+        # ID
+        check_header = build_bcm_header(
+            opcode=CAN_BCM_TX_READ,
+            flags=0,
+            count=0,
+            ival1_seconds=0,
+            ival1_usec=0,
+            ival2_seconds=0,
+            ival2_usec=0,
+            can_id=self.can_id_with_flags,
+            nframes=0,
+        )
+        try:
+            self.bcm_socket.send(check_header)
+        except OSError as e:
+            assert e.errno == errno.EINVAL
+        else:
+            raise ValueError(
+                "A periodic Task for Arbitration ID {} has already been created".format(
+                    message.arbitration_id
+                )
+            )
+
+        header = build_bcm_transmit_header(
+            self.can_id_with_flags,
+            count,
+            ival1,
+            ival2,
+            self.flags
+        )
         frame = build_can_frame(message)
         log.debug("Sending BCM command")
         send_bcm(self.bcm_socket, header + frame)
