Fix: clear old reservations (related to #35). Fix: Set WaitingForSendReserveID state.

Author: Poikilos

openlcb/canbus/canphysicallayergridconnect.py

@@ -11,12 +11,20 @@
 '''
 
 
-from typing import Union
+import struct
+import sys
+
+from logging import getLogger
+from typing import Tuple, Union
+
 from openlcb.canbus.canphysicallayer import CanPhysicalLayer
 from openlcb.canbus.canframe import CanFrame
 from openlcb.frameencoder import FrameEncoder
 from openlcb.portinterface import PortInterface
 
+logger = getLogger(__name__)
+
+
 GC_START_BYTE = 0x3a  # :
 GC_END_BYTE = 0x3b  # ;
 
@@ -154,11 +162,70 @@ def handleDataString(self, string: str) -> int:
         # formerly pushString formerly receiveString
         return self.handleData(string.encode("utf-8"))
 
-    def handleData(self, data: Union[bytes, bytearray], verbose=False) -> int:
+    @classmethod
+    def nextPacketRange(cls, data: Union[bytes, bytearray],
+                        start: int=0) -> Tuple[int, int]:
+        """Get the packet slice if any.
+        Returns:
+            tuple(int, int): Position of ':' and ';', otherwise None.
+        """
+        firstI = data.find(GC_START_BYTE, start)
+        if firstI < 0:
+            return None
+        lastI = data.find(GC_END_BYTE, firstI+1)
+        if lastI < 0:
+            return None
+        return (firstI, lastI+1)
+
+    @classmethod
+    def nextPacket(cls, data: Union[bytes, bytearray]) -> bytearray:
+        """Get the packet including ':' and ';'."""
+        indices = cls.nextPacketRange(data)
+        if indices is None:
+            return None
+        return data[indices[0]:indices[1]]
+
+    @staticmethod
+    def readInt32(data, start):
+        # chunk = data[start:start+8]
+        # return int.from_bytes(chunk, 'big') - 0x30303030 + ((chunk & 0x40404040) >> 6) * 9
+        """Fast hex bytearray → 32-bit int (8 hex chars, ASCII)"""
+        # branchless conversion for uppercase/lowercase or digits:
+        # - (data[i] & 15): Gives the low 4 bits (0-9 for digits
+        #   '0'-'9', or 1-6 for 'A'-'F'/'a'-'f').
+        # - ((data[i] >> 6) & 1): Detects letters—0 for digits (ASCII
+        #   48-57), 1 for uppercase/lowercase letters (ASCII 65-70 or
+        #   97-102).
+        # - * 9: Adds an extra 9 only for letters (e.g., 'A' low bits=1
+        #     → 1+9=10; 'F'=6 → 6+9=15). This avoids if/else branches
+        #     for better performance in tight loops.
+        v = 0
+        for i in range(start, start+8):
+            v = (v << 4) + (data[i] & 15) + ((data[i] >> 6) & 1) * 9
+        return v
+
+    @staticmethod
+    def fromHexBytes(b: bytearray, start: int, stop: int) -> bytearray:
+        """ASCII hex bytearray → binary bytearray (any even length)"""
+        r = bytearray((stop-start) // 2)
+        i = start
+        while i < stop:
+            x, y = b[i], b[i+1]
+            r[i//2] = ((x & 15) + ((x >> 6) & 1) * 9) << 4 | \
+                ((y & 15) + ((y >> 6) & 1) * 9)
+            i += 2
+        return r
+
+    def handleDataOptimized(self, data: Union[bytes, bytearray],
+                            test_output=None, verbose=False) -> int:
         """Provide characters from the outside link to be parsed
 
         Args:
             data (Union[bytes,bytearray]): new data from outside link
+            test_output (list, optional): List-like object to hold
+                resulting frames--for testing only (In normal operation,
+                this method only uses self.fireFrameReceived(cf) to
+                queue frames).
             verbose (bool, optional): If True, print each frame
                 detected.
 
@@ -168,6 +235,7 @@ def handleData(self, data: Union[bytes, bytearray], verbose=False) -> int:
         frameCount = 0
         self.inboundBuffer += data
         lastByte = 0  # last index is at ';'
+
         if GC_END_BYTE in self.inboundBuffer:
             #  ^ ';' ends message so we have at least one (CR/LF not required)
             # found end, now find start of that same message, earlier in buffer
@@ -178,7 +246,7 @@ def handleData(self, data: Union[bytes, bytearray], verbose=False) -> int:
                 if self.inboundBuffer[index] == 0x3A:  # ':' starts message
                     # now start to accumulate data from entire message
                     header = 0
-                    for offset in range(2, 9+1):
+                    for offset in range(2, 9+1):  # skip first 2 bytes (":X")
                         nextChar = (self.inboundBuffer[index+offset])
                         nextByte = (nextChar & 0xF)+9 if nextChar > 0x39 else nextChar & 0xF  # noqa: E501
                         header = (header << 4)+nextByte
@@ -190,6 +258,12 @@ def handleData(self, data: Union[bytes, bytearray], verbose=False) -> int:
                             break
                         # two characters are data
                         byte1 = self.inboundBuffer[index+11+2*dataItem]
+                        # Convert from UTF-8 to ordinal (0x39 is
+                        #   "9", so assume UTF-8 code higher than 39 is
+                        #   "A"-"F" [NOTE: "a"="f" would also work due
+                        #   to `& 0xF` but are N/A in GridConnect]):
+                        #   - 0x30-0x39 & 0xF yields 0-9
+                        #   - 0x41-0x46 or 0x61-0x66 & 0xF + 9 yields 10-15
                         part1 = (byte1 & 0xF)+9 if byte1 > 0x39 else byte1 & 0xF  # noqa: E501
                         byte2 = self.inboundBuffer[index+11+2*dataItem+1]
                         part2 = (byte2 & 0xF)+9 if byte2 > 0x39 else byte2 & 0xF  # noqa: E501
@@ -206,6 +280,8 @@ def handleData(self, data: Union[bytes, bytearray], verbose=False) -> int:
                     # lastByte is index of ; in this message
 
                     cf = CanFrame(header, outData)
+                    if test_output is not None:
+                        test_output.add(cf)
                     frameCount += 1
                     self.fireFrameReceived(cf)
                     if verbose:
@@ -215,3 +291,80 @@ def handleData(self, data: Union[bytes, bytearray], verbose=False) -> int:
             # shorten buffer by removing the processed message
             self.inboundBuffer = self.inboundBuffer[lastByte:]
         return frameCount
+
+    def handleData(self, data: Union[bytes, bytearray],
+                   test_output=None, verbose=False) -> int:
+        """Provide characters from the outside link to be parsed
+
+        Args:
+            data (Union[bytes,bytearray]): new data from outside link
+            test_output (list, optional): List-like object to hold
+                resulting frames--for testing only (In normal operation,
+                this method only uses self.fireFrameReceived(cf) to
+                queue frames).
+            verbose (bool, optional): If True, print each frame
+                detected.
+
+        Returns:
+            int: The number of frames completed by inboundBuffer+data.
+        """
+        cls = type(self)
+        frameCount = 0
+        self.inboundBuffer += data
+        lastByte = 0  # last index is at ';'
+        start = 0
+        while True:
+            positions = cls.nextPacketRange(self.inboundBuffer, start=start)
+            if positions is None:
+                break
+            first, last = positions
+            del positions
+            if last - first < 8:
+                logger.warning(
+                    "[handleData] Skipped malformed"
+                    " (< 8 pairs) packet {}"
+                    .format(repr(self.inboundBuffer[first:last])))
+                start = last
+                continue
+            if self.inboundBuffer[first+1] != ord(b'X'):  # 0x58 (88)
+                logger.warning(
+                    "[handleData] Skipped malformed packet"
+                    " (No 'X' in {})"
+                    .format(repr(self.inboundBuffer[first:last])))
+                start = last + 1  # +1 to skip ";"
+                continue
+            headerI = first + 2  # skip 2 chars: ":X"
+            dataI = headerI + 8
+            header_bytes = bytearray.fromhex(
+                self.inboundBuffer[headerI:dataI])
+            if last - dataI > 0:
+                if (last - dataI) % 2 > 0:
+                    logger.warning(
+                        "[handleData] Skipped malformed packet"
+                        " (Incomplete pair in {})"
+                        .format(repr(self.inboundBuffer[first:last])))
+                    start = last + 1  # +1 to skip ";"
+                    continue
+                outData = bytearray.fromhex(
+                    self.inboundBuffer[dataI:last])
+            else:
+                outData = bytearray()
+
+            # Convert 4-byte big-endian header to 29-bit integer
+            header = struct.unpack('>I', header_bytes)[0]
+
+            cf = CanFrame(header, outData)
+            if test_output is not None:
+                test_output.add(cf)
+            frameCount += 1
+            self.fireFrameReceived(cf)
+            if verbose:
+                print("- RECV {}".format(
+                    self.inboundBuffer[first:last].strip()))
+
+            start = last + 1  # +1 to skip ";"
+
+        if lastByte > 0:
+            del self.inboundBuffer[0:lastByte+1]
+
+        return frameCount