Merge can-echo-server:use-custom-implementation into can-echo-server:master

[Jonathan Cave]

candump does not seem to support sending FD packets when in bridge mode. Hence switching to own echo implementation.

diff --git a/snap/hooks/install b/snap/hooks/install
index 23f32f4..6f5b4f5 100755
--- a/snap/hooks/install
+++ b/snap/hooks/install
@@ -1,4 +1,4 @@
 #!/bin/bash
 
 snapctl set interface=can0
-snapctl set delay=2000000
+snapctl set delay=2
diff --git a/snap/snapcraft.yaml b/snap/snapcraft.yaml
index 8540afb..67a85ea 100644
--- a/snap/snapcraft.yaml
+++ b/snap/snapcraft.yaml
@@ -1,21 +1,22 @@
 name: can-echo-server
-version: '0.1'
+version: '0.2'
 summary: Creates a deamon that echoes any CAN Packets received
 description: |
   The CAN packet is echoed unmodified to the same interface on which it was
   received.
 
 grade: stable
-confinement: devmode
+confinement: strict
 
 apps:
   can-echo-server:
     command: bin/can_echo_server.sh
     daemon: simple
+    plugs:
+      - can-bus
+      - network-control
 
 parts:
-  can-utils:
-    source-tag: v2018.02.0
   can-echo-server:
     plugin: dump
     source: src/
diff --git a/src/can_echo_server.py b/src/can_echo_server.py
new file mode 100755
index 0000000..9d224e8
--- /dev/null
+++ b/src/can_echo_server.py
@@ -0,0 +1,163 @@
+#!/usr/bin/env python3
+# This file is part of Checkbox.
+#
+# Copyright 2018 Canonical Ltd.
+# Written by:
+#   Jonathan Cave <[e-mail]>
+#
+# Checkbox is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License version 3,
+# as published by the Free Software Foundation.
+#
+# Checkbox is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with Checkbox.  If not, see <http://www.gnu.org/licenses/>.
+
+import argparse
+import os
+import socket
+import struct
+import sys
+import textwrap
+import threading
+import time
+
+
+class CANSocket():
+
+    # struct module format strings for CAN packets
+    # Normal format:
+    #   <   little-endian
+    #   I   unsigned int (4)    : CAN-ID + EFF/RTR/ERR Flags
+    #   B   unsigned char (1)   : Data length
+    #   3x  padding (3 * 1)     : -
+    #   8s  char array (8 * 1)  : Data
+    FORMAT = "<IB3x8s"
+    # Flexible Data (FD) rate format:
+    #   <    little-endian
+    #   I    unsigned int (4)    : CAN-ID + EFF/RTR/ERR Flags
+    #   B    unsigned char (1)   : Data length
+    #   B    unsigned char (1)   : FD Flags
+    #   2x   padding (2 * 1)     : -
+    #   64s  char array (64 * 1) : Data
+    FD_FORMAT = "<IBB2x64s"
+
+    CAN_MTU = struct.Struct(FORMAT).size
+    CANFD_MTU = struct.Struct(FD_FORMAT).size
+
+    # Socket options from <linux/can/raw.h>
+    CAN_RAW_FILTER = 1         # set 0 .. n can_filter(s)
+    CAN_RAW_ERR_FILTER = 2     # set filter for error frames
+    CAN_RAW_LOOPBACK = 3       # local loopback (default:on)
+    CAN_RAW_RECV_OWN_MSGS = 4  # receive my own msgs (default:off)
+    CAN_RAW_FD_FRAMES = 5      # allow CAN FD frames (default:off)
+    CAN_RAW_JOIN_FILTERS = 6   # all filters must match to trigger
+
+    def __init__(self, interface=None, fdmode=False, loopback=True):
+        self.sock = socket.socket(socket.PF_CAN,  # protocol family
+                                  socket.SOCK_RAW,
+                                  socket.CAN_RAW)
+        self._fdmode = fdmode
+        self._loopback = loopback
+        if interface is not None:
+            self._bind(interface)
+
+    def __enter__(self):
+        return self
+
+    def __exit__(self, *args):
+        self.close()
+
+    def close(self):
+        self.sock.close()
+
+    def _bind(self, interface):
+        self.sock.bind((interface,))
+        if self._fdmode:  # default is off
+            self.sock.setsockopt(socket.SOL_CAN_RAW, self.CAN_RAW_FD_FRAMES, 1)
+        if not self._loopback:  # default is on
+            self.sock.setsockopt(socket.SOL_CAN_RAW, self.CAN_RAW_LOOPBACK, 0)
+
+    def send(self, can_id, data, id_flags=0, fd_flags=0):
+        can_id = can_id | id_flags
+        if self._fdmode:
+            can_pkt = struct.pack(self.FD_FORMAT, can_id, len(data), fd_flags,
+                                  data)
+        else:
+            can_pkt = struct.pack(self.FORMAT, can_id, len(data), data)
+        self.sock.send(can_pkt)
+
+    def recv(self):
+        can_pkt = self.sock.recv(self.CANFD_MTU)
+        nbytes = len(can_pkt)
+        if nbytes == self.CANFD_MTU:
+            can_id, length, fd_flags, data = struct.unpack(self.FD_FORMAT,
+                                                           can_pkt)
+        else:
+            can_id, length, data = struct.unpack(self.FORMAT, can_pkt)
+        can_id &= socket.CAN_EFF_MASK
+        return (can_id, data[:length])
+
+
+def echo_server(args):
+    while True:
+        recv_id_i = None
+        recv_data_b = None
+
+        print('Opening read socket on {}'.format(args.interface))
+        with CANSocket(args.interface, fdmode=True,
+                       loopback=False) as recv_s:
+            recv_id_i, recv_data_b = recv_s.recv()
+
+        print('Received packet')
+        print('  ID  : {:x}'.format(recv_id_i))
+        print('  Data: {}'.format(recv_data_b.hex()))
+
+        time.sleep(args.delay)
+
+        if recv_id_i > 2047:
+            id_flags = socket.CAN_EFF_FLAG
+        else:
+            id_flags = 0
+
+        if len(recv_data_b) == 64:
+            fdmode = True
+        else:
+            fdmode = False
+
+        print('Opening send socket on {}'.format(args.interface))
+        # Open socket, will raise OSError on failure
+        with CANSocket(args.interface, fdmode=fdmode,
+                       loopback=False) as send_s:
+            print('Sending data...', flush=True)
+            try:
+                send_s.send(recv_id_i, recv_data_b, id_flags=id_flags)
+            except OSError as e:
+                print(e, file=sys.stderr)
+                if e.errno == 90:
+                    raise SystemExit(
+                        'ERROR: interface does not support FD Mode')
+                else:
+                    raise SystemExit('ERROR: OSError on attempt to send')
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+        description='CAN Packet Echoer')
+    parser.add_argument('interface', type=str,
+                        help='Interface name e.g. can0')
+    parser.add_argument('delay', type=int,
+                        help='Delay before echoing packets in seconds')
+    parser.set_defaults(func=echo_server)
+
+    args = parser.parse_args()
+    args.func(args)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/src/can_echo_server.sh b/src/can_echo_server.sh
index 48ee03e..c441d15 100755
--- a/src/can_echo_server.sh
+++ b/src/can_echo_server.sh
@@ -3,4 +3,8 @@
 INTERFACE=$(snapctl get interface)
 DELAY=$(snapctl get delay)
 
-candump "$INTERFACE" -B "$INTERFACE" -u "$DELAY"
+ip link set "$INTERFACE" down
+ip link set "$INTERFACE" type can bitrate 1000000 dbitrate 2000000 fd on
+ip link set "$INTERFACE" up
+
+ $SNAP/bin/can_echo_server.py "$INTERFACE" "$DELAY"