From 0315e8b09a592500759a54f5f983b016f91afdf3 Mon Sep 17 00:00:00 2001
From: Sam Willcocks <sam@wlcx.cc>
Date: Thu, 9 Nov 2023 15:09:20 +0000
Subject: [PATCH] Add icpdas interface

---
 can/interfaces/__init__.py    |  2 +
 can/interfaces/icpdas_ecan.py | 79 +++++++++++++++++++++++++++++++++++
 test/test_icpdas_ecan.py      | 24 +++++++++++
 3 files changed, 105 insertions(+)
 create mode 100644 can/interfaces/icpdas_ecan.py
 create mode 100644 test/test_icpdas_ecan.py

diff --git a/can/interfaces/__init__.py b/can/interfaces/__init__.py
index f220d28e5..e60390e9a 100644
--- a/can/interfaces/__init__.py
+++ b/can/interfaces/__init__.py
@@ -13,6 +13,7 @@
     "cantact",
     "etas",
     "gs_usb",
+    "icpdas_ecan",
     "ics_neovi",
     "iscan",
     "ixxat",
@@ -59,6 +60,7 @@
     "neousys": ("can.interfaces.neousys", "NeousysBus"),
     "etas": ("can.interfaces.etas", "EtasBus"),
     "socketcand": ("can.interfaces.socketcand", "SocketCanDaemonBus"),
+    "icpdas_ecan": ("can.interfaces.icpdas_ecan", "ICPDASEcanBus"),
 }
 
 
diff --git a/can/interfaces/icpdas_ecan.py b/can/interfaces/icpdas_ecan.py
new file mode 100644
index 000000000..e79a6abfa
--- /dev/null
+++ b/can/interfaces/icpdas_ecan.py
@@ -0,0 +1,79 @@
+"""
+Interface for ECAN CAN Bus gateways manufactured by ICP DAS.
+"""
+
+from typing import Any, Optional, Tuple
+from can import BusABC
+from can.message import Message
+import can.typechecking
+import socket
+
+DEFAULT_PORT = 10003
+
+def encode_message(msg: Message) -> bytes:
+    buf = bytearray()
+    # 1-byte prefix depending on whether this is a standard/extended and data/remote frame
+    if msg.is_remote_frame:
+        if msg.is_extended_id:
+            buf.extend(b"E")
+        else:
+            buf.extend(b"T")
+    else:
+        if msg.is_extended_id:
+            buf.extend(b"e")
+        else:
+            buf.extend(b"t")
+    # encode the id
+    if msg.is_extended_id:
+        buf.extend(f"{msg.arbitration_id:08x}".encode("ascii"))
+    else:
+        buf.extend(f"{msg.arbitration_id:03x}".encode("ascii"))
+    buf.extend(f"{len(msg.data):01x}".encode("ascii"))
+    if not msg.is_remote_frame:
+        buf.extend("".join([f"{b:02x}" for b in msg.data]).encode("ascii"))
+
+    assert len(buf) == 1 + (8 if msg.is_extended_id else 3) + 1 + (len(msg.data) * 2)
+
+    buf.extend(b"\r")
+    return buf
+
+def decode_message(raw: bytes) -> Message:
+    flag = raw[0:1]
+    if flag not in [b"e", b"E", b"t", b"T"]:
+        raise ValueError(f"Unexpected flag: {raw[0]}")
+    is_extended = flag in (b"e", b"E")
+    is_remote = flag in (b"T", b"E")
+    id_ = int((raw[1:9] if is_extended else raw[1:4]).decode("ascii"), 16)
+    length = int(raw[9:10] if is_extended else raw[4:5], 16)
+    return Message(
+        arbitration_id=id_,
+        is_extended_id=is_extended,
+        is_remote_frame=is_remote,
+        dlc=length,
+        data=bytes.fromhex(raw[(10 if is_extended else 5):].decode("ascii")),
+        check=True,
+    )
+
+
+class ICPDASEcanBus(BusABC):
+    def __init__(self, channel: str, port: int = DEFAULT_PORT, can_filters: can.typechecking.CanFilters | None = None, **kwargs: object):
+        super().__init__(channel, can_filters, **kwargs)
+        self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self._sock.connect((channel, port))
+        self.raw_buf = bytearray()
+
+    def _recv_internal(self, timeout: float | None) -> Tuple[Message | None, bool]:
+        # If we have stuff in the buffer, decode it
+        while b"\r" not in self.raw_buf:
+        # TODO: nonblocking?
+            self.raw_buf.extend(self._sock.recv(4096))
+        raw_msg, rest = self.raw_buf.split(b"\r", maxsplit=1)
+        self.raw_buf = rest
+        try:
+            return decode_message(raw_msg), False
+        except ValueError as e:
+            print("error decoding message: ", e)
+            return None, False
+
+    def send(self, msg: Message, timeout: float | None = None) -> None:
+       self._sock.send(encode_message(msg))
\ No newline at end of file
diff --git a/test/test_icpdas_ecan.py b/test/test_icpdas_ecan.py
new file mode 100644
index 000000000..82c3b7a77
--- /dev/null
+++ b/test/test_icpdas_ecan.py
@@ -0,0 +1,24 @@
+from can.message import Message
+from can.interfaces.icpdas_ecan import encode_message, decode_message
+import pytest
+
+@pytest.mark.parametrize(("id_", "remote", "extended", "data"), [
+    (0, False, False, []),
+    (13, True, False, []),
+    (2047, False, False, [1,2,3,4]),
+    (2047, False, False, [1,2,3,4,5,6,7,8]),
+    (12345, False, True, [1,2,3,4,5,6,7,8]),
+    (12345, True, True, []),
+])
+def test_encode_decode(id_, remote, extended, data):
+    """Test that a message is encoded and decoded the same."""
+    d = [1, 2, 3, 4, 5, 6, 7, 8]
+    m = Message(
+        arbitration_id=id_,
+        is_extended_id=extended,
+        is_remote_frame=remote,
+        data = bytes(data),
+        dlc=len(data),
+    )
+
+    assert decode_message(encode_message(m).strip(b"\r")).equals(m)
\ No newline at end of file