1#!/usr/bin/env python3

2# This file is part of Checkbox.

3#

4# Copyright 2018 Canonical Ltd.

5# Written by:

6# Jonathan Cave <jonathan.cave@canonical.com>

7#

8# Checkbox is free software: you can redistribute it and/or modify

9# it under the terms of the GNU General Public License version 3,

10# as published by the Free Software Foundation.

11#

12# Checkbox is distributed in the hope that it will be useful,

13# but WITHOUT ANY WARRANTY; without even the implied warranty of

14# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

15# GNU General Public License for more details.

16#

17# You should have received a copy of the GNU General Public License

18# along with Checkbox. If not, see <http://www.gnu.org/licenses/>.

19

20 import argparse

21 import ctypes

22 import os

23 import socket

24 import struct

25 import sys

26 import textwrap

27 import threading

28 import time

29

30

31 class CANSocket:

32

33 # struct module format strings for CAN packets

34 # Normal format:

35 # < little-endian

36 # I unsigned int (4) : CAN-ID + EFF/RTR/ERR Flags

37 # B unsigned char (1) : Data length

38 # 3x padding (3 * 1) : -

39 # 8s char array (8 * 1) : Data

40 FORMAT = "<IB3x8s"

41 # Flexible Data (FD) rate format:

42 # < little-endian

43 # I unsigned int (4) : CAN-ID + EFF/RTR/ERR Flags

44 # B unsigned char (1) : Data length

45 # B unsigned char (1) : FD Flags

46 # 2x padding (2 * 1) : -

47 # 64s char array (64 * 1) : Data

48 FD_FORMAT = "<IBB2x64s"

49

50 CAN_MTU = struct.Struct(FORMAT).size

51 CANFD_MTU = struct.Struct(FD_FORMAT).size

52

53 # Socket options from <linux/can/raw.h>

54 CAN_RAW_FILTER = 1 # set 0 .. n can_filter(s)

55 CAN_RAW_ERR_FILTER = 2 # set filter for error frames

56 CAN_RAW_LOOPBACK = 3 # local loopback (default:on)

57 CAN_RAW_RECV_OWN_MSGS = 4 # receive my own msgs (default:off)

58 CAN_RAW_FD_FRAMES = 5 # allow CAN FD frames (default:off)

59 CAN_RAW_JOIN_FILTERS = 6 # all filters must match to trigger

60

61 def __init__(self, interface=None, fdmode=False, loopback=True):

62 self.sock = socket.socket(

63 socket.PF_CAN, socket.SOCK_RAW, socket.CAN_RAW # protocol family

64 )

65 self._fdmode = fdmode

66 self._loopback = loopback

67 if interface is not None:

68 self._bind(interface)

69

70 def __enter__(self):

71 return self

72

73 def __exit__(self, *args):

74 self.close()

75

76 def close(self):

77 self.sock.close()

78

79 def _bind(self, interface):

80 self.sock.bind((interface,))

81 if self._fdmode: # default is off

82 self.sock.setsockopt(socket.SOL_CAN_RAW, self.CAN_RAW_FD_FRAMES, 1)

83 if not self._loopback: # default is on

84 self.sock.setsockopt(socket.SOL_CAN_RAW, self.CAN_RAW_LOOPBACK, 0)

85

86 def send(self, can_id, data, id_flags=0, fd_flags=0):

87 can_id = can_id | id_flags

88 if self._fdmode:

89 can_pkt = struct.pack(

90 self.FD_FORMAT, can_id, len(data), fd_flags, data

91 )

92 else:

93 can_pkt = struct.pack(self.FORMAT, can_id, len(data), data)

94 self.sock.send(can_pkt)

95

96 def recv(self):

97 can_pkt = self.sock.recv(self.CANFD_MTU)

98 nbytes = len(can_pkt)

99 if nbytes == self.CANFD_MTU:

100 can_id, length, fd_flags, data = struct.unpack(

101 self.FD_FORMAT, can_pkt

102 )

103 else:

104 can_id, length, data = struct.unpack(self.FORMAT, can_pkt)

105 can_id &= socket.CAN_EFF_MASK

106 return (can_id, data[:length])

107

108

109 def echo_test(args):

110 # ID conversion and size check

111 print("Using source ID: {}".format(args.can_id))

112 can_id_i = int(args.can_id, 16)

113 if can_id_i > 2047 and not args.effid:

114 raise SystemExit("ERROR: CAN ID to high for SFF")

115 id_flags = 0

116 if args.effid:

117 print("Setting EFF CAN ID flag")

118 id_flags = ctypes.c_ulong(socket.CAN_EFF_FLAG).value

119

120 # Whether to enable local loopback, required for local only test

121 # but only want to parse packets from other end if remote

122 loopback = not args.remote

123

124 # Default data size is 8 bytes but if testing FD Mode use 64

125 data_size = 8

126 if args.fdmode:

127 data_size = 64

128 data_b = os.urandom(data_size)

129 print("Sending data: {}".format(data_b.hex()))

130

131 recv_id_i = None

132 recv_data_b = None

133

134 def receive():

135 nonlocal recv_id_i

136 nonlocal recv_data_b

137 print("Opening read socket on {}".format(args.interface))

138 with CANSocket(

139 args.interface, fdmode=args.fdmode, loopback=loopback

140 ) as recv_s:

141 recv_id_i, recv_data_b = recv_s.recv()

142

143 # Create a receive thread

144 recv_t = threading.Thread(target=receive, daemon=True)

145 recv_t.start()

146 time.sleep(1)

147

148 print("Opening send socket on {}".format(args.interface))

149 # Open socket, will raise OSError on failure

150 with CANSocket(

151 args.interface, fdmode=args.fdmode, loopback=loopback

152 ) as send_s:

153 print("Sending data...", flush=True)

154 try:

155 send_s.send(can_id_i, data_b, id_flags=id_flags)

156 except OSError as e:

157 print(e, file=sys.stderr)

158 if e.errno == 90:

159 raise SystemExit("ERROR: interface does not support FD Mode")

160 else:

161 raise SystemExit("ERROR: OSError on attempt to send")

162

163 recv_t.join(10)

164 if recv_t.is_alive():

165 raise SystemExit("ERROR: Timeout waiting to receive data")

166

167 print("Received packet")

168 print(" ID : {:x}".format(recv_id_i))

169 print(" Data: {}".format(recv_data_b.hex()))

170 if recv_id_i != can_id_i or recv_data_b != data_b:

171 raise SystemExit("ERROR: ID/Data received does not match sent")

172

173 print("\nPASSED")

174

175

176 def main():

177 parser = argparse.ArgumentParser(

178 formatter_class=argparse.RawDescriptionHelpFormatter,

179 description="SocketCAN Tests",

180 epilog=textwrap.dedent(

181 """

182 Examples:

183 socketcan_test.py can0 123

184 socketcan_test.py can0 212 --remote

185 socketcan_test.py can0 FA123 --effid

186 socketcan_test.py can0 E407DB --effid --fdmode"""

187 ).lstrip(),

188 )

189 parser.add_argument("interface", type=str, help="Interface name e.g. can0")

190 parser.add_argument(

191 "can_id",

192 type=str,

193 help=textwrap.dedent(

194 """

195 CAN ID of source in Hex, max of 11 bits using Standard Frame

196 Format (SFF). Specifying use of Extended Frame Format (EFF)

197 allows the use of up to 29 bit IDs."""

198 ).lstrip(),

199 )

200 parser.add_argument(

201 "--remote",

202 action="store_true",

203 help="Expect a remote device to echo the test packet",

204 )

205 parser.add_argument(

206 "--effid", action="store_true", help="Use EFF ID (CAN 2.0 B)"

207 )

208 parser.add_argument(

209 "--fdmode",

210 action="store_true",

211 help="Attempt to send 64 bytes of data i.e. FD mode",

212 )

213 parser.set_defaults(func=echo_test)

214

215 args = parser.parse_args()

216 args.func(args)

217

218

219 if __name__ == "__main__":

220 main()