Server sdo block upload (based on v2.3.0)

canopen/sdo/client.py

@@ -530,6 +530,7 @@
         if seqno == self._ackseq + 1:
             self._ackseq = seqno
         else:
+            logger.debug('Wrong seqno')
             # Wrong sequence number
             response = self._retransmit()
             res_command, = struct.unpack_from("B", response)

canopen/sdo/constants.py

@@ -2,7 +2,12 @@
 
 # Command, index, subindex
 SDO_STRUCT = struct.Struct("<BHB")
+SDO_BLOCKINIT_STRUCT = "<BHBI"  # Command + seqno, index, subindex, size
+SDO_BLOCKACK_STRUCT = struct.Struct("<BBB") # c + ackseq + new blocksize
+SDO_BLOCKEND_STRUCT = struct.Struct("<BH") # c + CRC
+SDO_ABORT_STRUCT = struct.Struct("<BHBI") # c +i + si + Abort code
 
+# Command[5-7]
 REQUEST_SEGMENT_DOWNLOAD = 0 << 5
 REQUEST_DOWNLOAD = 1 << 5
 REQUEST_UPLOAD = 2 << 5
@@ -19,15 +24,29 @@
 RESPONSE_BLOCK_DOWNLOAD = 5 << 5
 RESPONSE_BLOCK_UPLOAD = 6 << 5
 
+# Block transfer sub-commands, Command[0-1]
+SUB_COMMAND_MASK = 0x3
 INITIATE_BLOCK_TRANSFER = 0
 END_BLOCK_TRANSFER = 1
 BLOCK_TRANSFER_RESPONSE = 2
 START_BLOCK_UPLOAD = 3
 
-EXPEDITED = 0x2
-SIZE_SPECIFIED = 0x1
-BLOCK_SIZE_SPECIFIED = 0x2
-CRC_SUPPORTED = 0x4
-NO_MORE_DATA = 0x1
-NO_MORE_BLOCKS = 0x80
-TOGGLE_BIT = 0x10
+EXPEDITED = 0x2             # Expedited and segmented
+SIZE_SPECIFIED = 0x1        # All transfers
+BLOCK_SIZE_SPECIFIED = 0x2  # Block transfer: size specified in message
+CRC_SUPPORTED = 0x4         # client/server CRC capable
+NO_MORE_DATA = 0x1          # Segmented: last segment
+NO_MORE_BLOCKS = 0x80       # Block transfer: last segment
+TOGGLE_BIT = 0x10           # segmented toggle mask
+
+# Block states
+BLOCK_STATE_NONE = -1
+BLOCK_STATE_INIT = 0        # state when entering
+BLOCK_STATE_UPLOAD = 0x10   # delimiter, used for block type check
+BLOCK_STATE_UP_INIT_RESP = 0x11 # state when entering, response during upload
+BLOCK_STATE_UP_DATA = 0x12     # Upload Data transfer state
+BLOCK_STATE_UP_END = 0x13      # End of Upload block transfers
+BLOCK_STATE_DOWNLOAD = 0x20 # delimiter, used for block type check
+BLOCK_STATE_DL_DATA = 0x24     # Download Data transfer state
+BLOCK_STATE_DL_END = 0x25      # End of Download block transfers
+

canopen/sdo/exceptions.py

@@ -1,4 +1,4 @@
-
+from typing import Union
 
 class SdoError(Exception):
     pass
@@ -44,9 +44,15 @@
         0x08000024: "No data available",
     }
 
-    def __init__(self, code: int):
+    def __init__(self, code: Union[int, str]):
         #: Abort code
-        self.code = code
+        if isinstance(code, str):
+            try:
+                self.code = self.from_string(code)
+            except ValueError as e:
+                raise ValueError(f"Unknown SDO abort description: {code}") from e
+        else:
+            self.code = code
 
     def __str__(self):
         text = f"Code 0x{self.code:08X}"
@@ -58,6 +64,12 @@
         """Compare two exception objects based on SDO abort code."""
         return self.code == other.code
 
+    def from_string(self, text):
+        code = list(SdoAbortedError.CODES.keys())[
+            list(SdoAbortedError.CODES.values()).index(text)
+        ]
+        return code
+
 
 class SdoCommunicationError(SdoError):
     """No or unexpected response from slave."""

canopen/sdo/server.py

@@ -8,6 +8,9 @@
 logger = logging.getLogger(__name__)
 
 
+class SdoBlockException(SdoAbortedError):
+    """ Dedicated SDO Block exception. """
+
 class SdoServer(SdoBase):
     """Creates an SDO server."""
 
@@ -27,8 +30,14 @@
         self._index = None
         self._subindex = None
         self.last_received_error = 0x00000000
+        self.sdo_block = None
 
     def on_request(self, can_id, data, timestamp):
+        logger.debug('on_request')
+        if self.sdo_block and self.sdo_block.state != BLOCK_STATE_NONE:
+            self.process_block(data)
+            return
+
         command, = struct.unpack_from("B", data, 0)
         ccs = command & 0xE0
 
@@ -57,6 +66,82 @@
             self.abort()
             logger.exception(exc)
 
+    def process_block(self, request):
+        """
+        Process a block request, using a state mechanisme from SdoBlock class 
+        to handle the different states of the block transfer.
+
+        :param request:
+            CAN message containing EMCY or SDO request.
+        """
+
+        logger.debug('process_block')
+        command, _, _, code = SDO_ABORT_STRUCT.unpack_from(request)
+        if command == 0x80:
+            # Abort received
+            logger.error('Abort: 0x%08X' % code)
+            self.sdo_block = None
+            return
+
+        if BLOCK_STATE_UPLOAD < self.sdo_block.state < BLOCK_STATE_DOWNLOAD:
+            logger.debug('BLOCK_STATE_UPLOAD')
+            command, _, _= SDO_STRUCT.unpack_from(request)
+            # in upload state
+            if self.sdo_block.state == BLOCK_STATE_UP_INIT_RESP:
+                logger.debug('BLOCK_STATE_UP_INIT_RESP')
+                #init response was sent, client required to send new request
+                if (command & REQUEST_BLOCK_UPLOAD) != REQUEST_BLOCK_UPLOAD:
+                    raise SdoBlockException("Unknown SDO command specified")
+                if (command & START_BLOCK_UPLOAD) != START_BLOCK_UPLOAD:
+                    raise SdoBlockException("Unknown SDO command specified")
+
+                # now start blasting data to client from server
+                self.sdo_block.update_state(BLOCK_STATE_UP_DATA)
+
+                blocks = self.sdo_block.get_upload_blocks()
+                for block in blocks:
+                    self.send_response(block)
+
+            elif self.sdo_block.state == BLOCK_STATE_UP_DATA:
+                logger.debug('BLOCK_STATE_UP_DATA')
+                command, ackseq, newblk = SDO_BLOCKACK_STRUCT.unpack_from(request)
+                if (command & REQUEST_BLOCK_UPLOAD) != REQUEST_BLOCK_UPLOAD:
+                    raise SdoBlockException("Unknown SDO command specified")
+                elif (command & BLOCK_TRANSFER_RESPONSE) != BLOCK_TRANSFER_RESPONSE:
+                    raise SdoBlockException("Unknown SDO command specified")
+                elif (ackseq != self.sdo_block.last_seqno):
+                    self.sdo_block.data_uploaded = self.sdo_block.data_succesfull_upload
+
+                if self.sdo_block.size == self.sdo_block.data_uploaded:
+                    logger.debug('BLOCK_STATE_UP_DATA last data')
+                    self.sdo_block.update_state(BLOCK_STATE_UP_END)
+                    response = bytearray(8)
+                    command = RESPONSE_BLOCK_UPLOAD
+                    command |= END_BLOCK_TRANSFER
+                    n = self.sdo_block.last_bytes << 2
+                    command |= n
+                    logger.debug('Last no byte: %d, CRC: x%04X',
+                                 self.sdo_block.last_bytes,
+                                 self.sdo_block.crc_value)
+                    SDO_BLOCKEND_STRUCT.pack_into(response, 0, command,
+                                                  self.sdo_block.crc_value)
+                    self.send_response(response)
+                else:
+                    blocks = self.sdo_block.get_upload_blocks()
+                    for block in blocks:
+                        self.send_response(block)
+
+            elif self.sdo_block.state == BLOCK_STATE_UP_END:
+                self.sdo_block = None
+
+        elif BLOCK_STATE_DOWNLOAD < self.sdo_block.state:
+            # in download state
+            logger.debug('BLOCK_STATE_DOWNLOAD')
+        else:
+            # in neither
+            raise SdoBlockException("Data can not be transferred or stored to the application "
+                     "because of the present device state")
+
     def init_upload(self, request):
         _, index, subindex = SDO_STRUCT.unpack_from(request)
         self._index = index
@@ -76,10 +161,10 @@
             struct.pack_into("<L", response, 4, size)
             self._buffer = bytearray(data)
             self._toggle = 0
-
         SDO_STRUCT.pack_into(response, 0, res_command, index, subindex)
         self.send_response(response)
 
+
     def segmented_upload(self, command):
         if command & TOGGLE_BIT != self._toggle:
             # Toggle bit mismatch
@@ -106,12 +191,33 @@
         response[1:1 + size] = data
         self.send_response(response)
 
-    def block_upload(self, data):
-        # We currently don't support BLOCK UPLOAD
-        # according to CIA301 the server is allowed
-        # to switch to regular upload
-        logger.info("Received block upload, switch to regular SDO upload")
-        self.init_upload(data)
+    def block_upload(self, request):
+        """
+        Process an initial block upload request.
+        Create a CAN response message and update the state of the SDO block.
+
+        :param request:
+            CAN message containing SDO request.
+        """
+        logging.debug('Enter server block upload')
+        self.sdo_block = SdoBlock(self._node, request)
+
+        res_command = RESPONSE_BLOCK_UPLOAD
+        res_command |= BLOCK_SIZE_SPECIFIED
+        res_command |= self.sdo_block.crc
+        res_command |= INITIATE_BLOCK_TRANSFER
+        logging.debug('CMD: %02X', res_command)
+        response = bytearray(8)
+
+        struct.pack_into(SDO_STRUCT.format+'I',  # add size
+                         response, 0,
+                         res_command,
+                         self.sdo_block.index,
+                         self.sdo_block.subindex,
+                         self.sdo_block.size)
+        logging.debug('response %s', response)
+        self.sdo_block.update_state(BLOCK_STATE_UP_INIT_RESP)
+        self.send_response(response)
 
     def request_aborted(self, data):
         _, index, subindex, code = struct.unpack_from("<BHBL", data)
@@ -217,3 +323,104 @@
             When node responds with an error.
         """
         return self._node.set_data(index, subindex, data)
+
+class SdoBlock():
+    """
+    SdoBlock class to handle block transfer. It keeps track of the
+    current state and the prepares data to be transferred.
+    """
+    state = BLOCK_STATE_NONE
+    crc = False
+    data_uploaded = 0
+    data_succesfull_upload = 0
+    last_bytes = 0
+    crc_value = 0
+    last_seqno = 0
+
+    def __init__(self, node, request, docrc=False):
+        """
+        :param node:
+            Node object owning the server
+        :param request:
+            CAN message containing SDO request.
+        :param docrc:
+            If True, CRC is calculated and checked.
+        """
+        command, index, subindex = SDO_STRUCT.unpack_from(request)
+        # only do crc if crccheck lib is available _and_ if requested
+        _req_crc = (command & CRC_SUPPORTED) == CRC_SUPPORTED
+
+        if (command & SUB_COMMAND_MASK) == INITIATE_BLOCK_TRANSFER:
+            self.state = BLOCK_STATE_INIT
+        else:
+            raise SdoBlockException("Unknown SDO command specified")
+
+        # TODO: CRC of data if requested
+        self.crc = CRC_SUPPORTED if (docrc & _req_crc)  else 0
+        self._node = node
+        self.index = index
+        self.subindex = subindex
+        self.req_blocksize = request[4]
+        self.seqno = 0
+        if not 1 <= self.req_blocksize <= 127:
+            raise SdoBlockException("Invalid block size")
+
+        self.data = self._node.get_data(index,
+                                        subindex,
+                                        check_readable=True)
+        self.size = len(self.data)
+
+    def update_state(self, new_state):
+        """
+        Update the state of the SDO block transfer. The state is
+        updated only if the new state is higher than the current
+        state. Otherwise an exception is raised.
+        """
+        logging.debug('update_state %X -> %X', self.state, new_state)
+        if new_state >= self.state:
+            self.state = new_state
+        else:
+            raise SdoBlockException("Data can not be transferred or stored to the application "
+                     "because of the present device state")
+
+    def get_upload_blocks(self):
+        """
+        Get the blocks of data to be sent to the client. The blocks are
+        created in a messages list of bytearrays. 
+        """
+
+        msgs = []
+
+        # seq no 1 - 127, not 0 -..
+        for seqno in range(1,self.req_blocksize+1):
+            logger.debug('SEQNO %d', seqno)
+            response = bytearray(8)
+            command = 0
+            if self.size <= (self.data_uploaded + 7):
+                # no more segments after this
+                command |= NO_MORE_BLOCKS
+
+            command |= seqno
+            response[0] = command
+            for i in range(7):
+                databyte = self.get_data_byte()
+                if databyte != None:
+                    response[i+1] = databyte
+                else:
+                    self.last_bytes = 7 - i
+                    break
+            msgs.append(response)
+            self.last_seqno = seqno
+
+            if self.size == self.data_uploaded:
+                break
+        logger.debug(msgs)
+        return msgs
+
+    def get_data_byte(self):
+        """Get the next byte of data to be sent to the client."""
+        if self.data_uploaded < self.size:
+            self.data_uploaded += 1
+            return self.data[self.data_uploaded-1]
+        return None
+

test/test_local.py

@@ -37,13 +37,14 @@ def test_expedited_upload(self):
         vendor_id = self.remote_node.sdo[0x1400][1].raw
         self.assertEqual(vendor_id, 0x99)
 
-    def test_block_upload_switch_to_expedite_upload(self):
-        with self.assertRaises(canopen.SdoCommunicationError) as context:
-            with self.remote_node.sdo[0x1008].open('r', block_transfer=True) as fp:
-                pass
-        # We get this since the sdo client don't support the switch
-        # from block upload to expedite upload
-        self.assertEqual("Unexpected response 0x41", str(context.exception))
+    # Remove this test, as Block upload is now supported:
+    # def test_block_upload_switch_to_expedite_upload(self):
+    #     with self.assertRaises(canopen.SdoCommunicationError) as context:
+    #         with self.remote_node.sdo[0x1008].open('r', block_transfer=True) as fp:
+    #             pass
+    #     # We get this since the sdo client don't support the switch
+    #     # from block upload to expedite upload
+    #     self.assertEqual("Unexpected response 0x41", str(context.exception))
 
     def test_block_download_not_supported(self):
         data = b"TEST DEVICE"