def control_rpm_down(self):
self.frames.append(
CANMessage(
self._status2['id_command'], 2,
bytes.fromhex(self._status2['commands']['rpm_down'] + '20'),
False, CANMessage.DataFrame))
return ""
def do_effect(self, can_msg, args):
if args['action'] == 'read' and can_msg.CANData: # READ
if self._params['id_command'] == can_msg.CANFrame.frame_id:
for cmd, value in self._params['commands'].items():
len_cmd = int(len(value) / 2)
if can_msg.CANFrame.frame_length == len_cmd and can_msg.CANFrame.frame_raw_data[
0:len_cmd] == bytes.fromhex(value):
if cmd == 'lock':
self._params['status'] = 'Locked'
self._last_sent -= self._params.get(
'reports_delay', 1.0)
if cmd == 'unlock':
self._params['status'] = 'Unlocked'
self._last_sent -= self._params.get(
'reports_delay', 1.0)
elif args['action'] == 'write' and not can_msg.CANData and self._params[
'status'] != 'Turned off': # Write
if (time.process_time() - self._last_sent) >= self._params.get(
'reports_delay', 1.0):
can_msg.CANFrame = CANMessage(
self._params.get('id_report', 0xffff),
len(
self._params.get('reports', {}).get(
self._params['status'], '')) / 2,
bytes.fromhex(
self._params.get('reports',
{}).get(self._params['status'],
'0000')), False,
CANMessage.DataFrame)
can_msg.CANData = True
can_msg.bus = self._bus
self._last_sent = time.process_time()
return can_msg
def control_unlock(self):
self.frames.append(
CANMessage(self._status2['id_command'],
int(len(self._status2['commands']['unlock']) / 2),
bytes.fromhex(self._status2['commands']['unlock']),
False, CANMessage.DataFrame))
return ""
def dev_write(self, line):
self.dprint(0, "CMD: " + line)
fid = line.split(":")[0]
length = line.split(":")[1]
data = line.split(":")[2]
self.server.write_can(CANMessage.init_data(int(fid), int(length), bytes.fromhex(data)[:int(length)]))
return "Sent!"
def dlights_on(self):
self.frames.append(
CANMessage(self._status2['id_command'],
int(len(self._status2['commands']['distance']) / 2),
bytes.fromhex(self._status2['commands']['distance']),
False, CANMessage.DataFrame))
return ""
def generate_rpm(self):
if self._status2['status'] == 1:
self.current += self.rpm_up - self.rpm_down
self._status2['rpm'] = self.current + random.randrange(-20, 20)
if self._status2['rpm'] < 150:
self._status2['status'] = 2 # dead
elif self._status2['rpm'] > 4500:
self._status2['status'] = 2 # also dead
elif self._status2['status'] == 3:
self._status2['rpm'] = 0
elif self._status2['status'] == 2:
self._status2['rpm'] = 0
self.rpm_up = 0
self.rpm_down = 0
if self._status2['status'] in [2, 3, 1]:
if (time.process_time() - self._last_sent) >= self._status2.get(
'reports_delay', 0.5):
self.frames.append(
CANMessage(
self._status2.get('id_report', 0xffff), 3,
self._status2['status'].to_bytes(1, byteorder='big') +
self._status2['rpm'].to_bytes(2, byteorder='big'),
False, CANMessage.DataFrame))
self._last_sent = time.process_time()
def handle(self):
# self.request is the TCP socket connected to the client
print("TCP2CAN connected to " + str(self.server.prt))
self.server.selfx.set_error_text("TCP2CAN connected to " + str(self.server.prt))
self.server._access_in.clear()
self.server._access_out.clear()
while True:
# Get header first
data = self.request.recv(4)
if data[0:1] == b'c':
# Header
if data[1] == 1: # Request for frames
while self.server._access_out.is_set():
time.sleep(0.0001)
self.server._access_out.set()
ready = len(self.server.CANList_out)
sz = struct.pack("!H", ready)
send_msg = b'c\x02' + sz
self.request.sendall(send_msg)
send_msg = b''
for can_msg in self.server.CANList_out:
# 16 byte
send_msg += b'ct\x03' + (b'\x00' * (4 - len(can_msg.frame_raw_id))) + can_msg.frame_raw_id + can_msg.frame_raw_length + can_msg.frame_raw_data + (b'\x00' * (8 - can_msg.frame_length))
if ready > 0:
self.request.sendall(send_msg)
self.server.CANList_out = []
self.server._access_out.clear()
elif data[1] == 4: # Incoming frames...
ready = struct.unpack("!H", data[2:4])[0]
inc_size = 16 * ready
if ready > 0:
inc_data = self.request.recv(inc_size)
idx = 0
while ready != 0:
packet = inc_data[idx:idx + 16]
if packet[0:3] != b'ct\x05':
print('SERVER GOT INCORRECT DATA')
self.server.selfx.set_error_text('SERVER GOT INCORRECT DATA')
break
else:
fid = struct.unpack("!I", packet[3:7])[0]
flen = packet[7]
fdata = packet[8:16]
while self.server._access_in.is_set():
time.sleep(0.0001)
self.server._access_in.set()
self.server.CANList_in.append(
CANMessage.init_data(int(fid), flen, fdata)
)
self.server._access_in.clear()
idx += 16
ready -= 1
def handle(self):
while True:
try:
self.socket.sendall(b'c\x01\x00\x00') # Request for frames
inc_header = self.socket.recv(4) # Get header first
if inc_header[0:2] != b'c\x02':
self.selfx.dprint(0, "HEADER ERROR")
self.selfx.set_error_text('HEADER ERROR')
continue
else:
ready = struct.unpack("!H", inc_header[2:4])[0]
inc_size = 16 * ready
if ready > 0:
inc_data = self.socket.recv(inc_size) # Get frames
idx = 0
while ready != 0:
packet = inc_data[idx:idx + 16]
if packet[0:3] != b'ct\x03':
self.selfx.dprint(0, 'CLIENT GOT INCORRECT DATA')
self.selfx.set_error_text('CLIENT GOT INCORRECT DATA')
break
else:
fid = struct.unpack("!I", packet[3:7])[0]
flen = packet[7]
fdata = packet[8:16]
while self._access_in.is_set():
time.sleep(0.0001)
self._access_in.set()
self.CANList_in.append(
CANMessage.init_data(int(fid), flen, fdata)
)
self._access_in.clear()
idx += 16
ready -= 1
while self._access_out.is_set():
time.sleep(0.0001)
self._access_out.set()
ready = len(self.CANList_out)
if ready > 0:
sz = struct.pack("!H", ready)
send_msg = b'c\x04' + sz
self.socket.sendall(send_msg)
send_msg = b''
for can_msg in self.CANList_out:
# 16 byte
send_msg += b'ct\x05' + (b'\x00' * (4 - len(can_msg.frame_raw_id))) + can_msg.frame_raw_id + can_msg.frame_raw_length + can_msg.frame_raw_data + (b'\x00' * (8 - can_msg.frame_length))
if ready > 0:
self.socket.sendall(send_msg)
self.CANList_out = []
self._access_out.clear()
except Exception as e:
self.selfx.set_error_text('TCPClient: recv response error:' + str(e))
traceback.print_exc()
def do_read(self, can, params):
"""Read from the hardware."""
frame = b''
while not frame:
frame = self._C232.read_frame()
if not frame or not self._C232.is_valid_frame(frame):
continue
can.bus = self._bus
self.dprint(
1, 'New frame: {0} (hex: {1})'.format(frame,
self.get_hex(frame)))
if frame.startswith(
(can232.CMD_TRANSMIT_STD,
can232.CMD_TRANSMIT_RTR_STD)): # 11bit CAN frame
id = struct.unpack('!H',
binascii.unhexlify(frame[1:4].zfill(4)))[0]
length = int(frame[4:5], 16)
if frame.startswith(
can232.CMD_TRANSMIT_STD): # RTR frames don't have data
# TODO: Handle timestamp in frame if timestamp enabled..
data = list(binascii.unhexlify(frame[5:]))
type = CANMessage.DataFrame
else:
# TODO: Handle timestamp in frame if timestamp enabled..
type = CANMessage.RemoteFrame
can.CANFrame = CANMessage(id, length, data, False, type)
can.CANData = True
elif frame.startswith(
(can232.CMD_TRANSMIT_EXT,
can232.CMD_TRANSMIT_RTR_EXT)): # 29bit CAN frame
id = struct.unpack('!I', binascii.unhexlify(frame[1:9]))[0]
length = int(frame[9:10], 16)
if frame.startswith(
can232.CMD_TRANSMIT_EXT): # RTR frames don't have data
# TODO: Handle timestamp in frame if timestamp enabled..
data = list(binascii.unhexlify(frame[10:]))
type = CANMessage.DataFrame
else:
# TODO: Handle timestamp in frame if timestamp enabled..
type = CANMessage.RemoteFrame
can.CANFrame = CANMessage(id, length, data, True, type)
can.CANData = True
return can
def do_init(self, params):
self._status2 = params
self._current = 1 # 1 - blocked, 2 - ALERT, 3 - authenticated
self._curr_auth = []
self.stop = False
cmds = self._status2['stop_engine'].split(':')
self.can_stop = CANMessage(
int(cmds[0], 16) if cmds[0][0:2] == '0x' else int(cmds[0]),
int(cmds[1]), bytes.fromhex(cmds[2]), False, CANMessage.DataFrame)
self._try = 0
self.last = time.process_time()
def generate_can(fid, data, padding=None): # generate CAN messages seq
"""Generate a CAN message in ISO TP format.
:param int fid: CAN ID for the message
:param list data: CAN message data
:param int padding: Value of the padding, e.g. 0x00 (if padding is required)
:return: List of cantoolz.can.CANMessage messages in ISO TP format representing the data.
:rtype: list
"""
_length = len(data)
can_msg_list = []
if _length < 8:
padding_data = []
padding_length = 0
if padding is not None:
padding_data = [int(padding)] * (7 - _length)
padding_length = 8 - _length - 1
can_msg_list.append(CANMessage.init_data(fid, _length + 1 + padding_length, [_length] + data[:_length] + padding_data)) # Single
elif _length > 4095:
return []
else:
can_msg_list.append(CANMessage.init_data(fid, 8, [(_length >> 8) + 0x10] + [_length & 0xFF] + data[:6]))
seq = 1
bytes = 6
while bytes != _length: # Rest
sent = min(_length - bytes, 7)
padding_data = []
padding_length = 0
if padding and sent < 7:
padding_data = [int(padding)] * (7 - sent)
padding_length = 8 - sent - 1
can_msg_list.append(CANMessage.init_data(fid, 1 + sent + padding_length, [seq + 0x20] + data[bytes:bytes + sent] + padding_data))
bytes += sent
seq += 1
if seq > 0xF:
seq = 0
return can_msg_list
def generate_vin(self):
curr_t = time.process_time()
if self.vin_gen[0] == 0 or curr_t - self.vin_gen[0] > self.vin_gen[2]:
if self.vin_gen[
1] == 0 or curr_t - self.vin_gen[1] > self.vin_gen[3]:
self.frames.append(
CANMessage.init_data(self._status2.get('vin_id', 1),
len(self.vin[self.count_part]),
self.vin[self.count_part]))
self.count_part += 1
self.vin_gen[1] = curr_t
if self.count_part == len(self.vin):
self.vin_gen[0] = curr_t
self.count_part = 0
def send_command(self, index):
cmd = self._commands[index]
name = list([x for x in list(cmd.keys()) if x not in ["cmd"]])[0]
(fid, length, data_hex) = cmd[name].split(":")
fid = fid.strip()
length = int(length)
data_hex = data_hex.strip()
fid = int(fid, 0)
data_hex = bytes.fromhex(data_hex)[:8]
self._frames.append(CANMessage.init_data(fid, length, data_hex))
return name + " has been added to queue!"
def do_read(self, can_msg):
if self._run and not can_msg.CANData:
try:
can_frame = self.socket.recv(16)
self.dprint(2, "READ: " + self.get_hex(can_frame))
if len(can_frame) == 16:
idf = struct.unpack("I", can_frame[0:4])[0]
if idf & 0x80000000:
idf &= 0x7FFFFFFF
can_msg.CANFrame = CANMessage.init_data(
idf, can_frame[4], can_frame[8:8 + can_frame[4]])
can_msg.bus = self._bus
can_msg.CANData = True
except:
return can_msg
return can_msg
def cmd_write(self, frame):
frame = frame.strip().split(":")
fid = frame[0].strip()
fid = int(fid, 0)
if len(frame) == 2:
length = len(bytes.fromhex(frame[1].strip()))
data = bytes.fromhex(frame[1].strip())
elif len(frame) == 3:
length = int(frame[1].strip())
data = bytes.fromhex(frame[2].strip())
else:
return "Bad format"
self.can_buffer_write.append(CANMessage.init_data(fid, length, data))
return "Added to queue"
def do_start(self, args):
self.queue_messages = []
self.last = time.clock()
data = [0, 0, 0, 0, 0, 0, 0, 0]
if 'body' in args:
data = list(bytes.fromhex(args['body']))
iso_mode = self._get_iso_mode(args)
padding = args.get('padding', None)
shift = int(args.get('shift', 0x8))
if 'range' not in args:
self.dprint(1, "No range specified")
self._active = False
self.set_error_text("ERROR: No range specified")
return
start, end = args.get('range', [0, 0])
for i in range(int(start), int(end)):
if iso_mode == 1:
iso_list = ISOTPMessage.generate_can(i, data, padding)
iso_list.reverse()
self.queue_messages.extend(iso_list)
elif iso_mode == 0:
self.queue_messages.append(
CANMessage.init_data(i, len(data), data[:8]))
elif iso_mode == 2:
for service in args.get('services', []):
uds_m = UDSMessage(shift, padding)
for service_id in self._get_range(service['service']):
# https://github.com/eik00d/CANToolz/issues/93 by @DePierre
subservice_ids = service.get('sub', None)
if subservice_ids is None:
subservice_ids = [None]
else:
subservice_ids = self._get_range(subservice_ids)
for subservice_id in subservice_ids:
iso_list = uds_m.add_request(
i, service_id, subservice_id,
service.get('data', []))
iso_list.reverse()
self.queue_messages.extend(iso_list)
self._full = len(self.queue_messages)
self._last = 0
def fuzz(self, fuzz_list, idf, data, bytes_to_fuzz, level, iso_mode):
messages = []
x_data = [] + data
for byte in fuzz_list:
x_data[bytes_to_fuzz[level]] = byte
if level != 0:
messages.extend(
self.fuzz(fuzz_list, idf, x_data, bytes_to_fuzz, level - 1,
iso_mode))
else:
if iso_mode == 1:
iso_list = ISOTPMessage.generate_can(idf, x_data)
iso_list.reverse()
messages.extend(iso_list)
else:
messages.append(
CANMessage.init_data(idf, len(x_data), x_data[:8]))
return messages
def cmd_write(self, def_p, frame):
frame = frame.strip().split(":")
fid = frame[0].strip()
if fid[0:2] == '0x':
fid = int(fid, 16)
else:
fid = int(fid)
if len(frame) == 2:
length = len(bytes.fromhex(frame[1].strip()))
data = bytes.fromhex(frame[1].strip())
elif len(frame) == 3:
length = int(frame[1].strip())
data = bytes.fromhex(frame[2].strip())
else:
return "Bad format"
self.can_buffer_write.append(CANMessage.init_data(fid, length, data))
return "Added to queue"
def dev_write(self, data):
self.dprint(1, "CMD: " + data)
ret = "Sent!"
if self._run:
try:
idf, dataf = data.strip().split('#')
dataf = bytes.fromhex(dataf)
idf = int(idf, 16)
lenf = min(8, len(dataf))
message = CANSploitMessage()
message.CANData = True
message.CANFrame = CANMessage.init_data(
idf, lenf, dataf[0:lenf])
self.do_write(message)
except Exception as e:
traceback.print_exc()
ret = str(e)
else:
ret = "Module is not active!"
return ret
def parse_file(self, name, _bus):
try:
with open(name.strip(), "r") as ins:
# "[TIME_STAMP]0x111:4:11223344"
for line in ins:
if len(line[:].split(":")) >= 3:
fid = line[:].split(":")[0].strip()
if fid[0] == "[" and fid.find(']') > 0:
time_stamp = float(fid[1:fid.find(']')])
fid = fid.split(']')[1].strip()
elif fid[0] == "<" and fid.find('>') > 0:
time_stamp = float(fid[1:fid.find('>')])
self.add_timestamp(time_stamp)
continue
elif len(line[:-1].split(":")) >= 3:
time_stamp = -1.0
else:
time_stamp = -1.0
num_fid = int(fid, 0)
length = line[:].split(":")[1]
data = line[:].split(":")[2]
if data[-1:] == "\n":
data = data[:-1]
if data[-1:] == "\r":
data = data[:-1]
msg = CANSploitMessage()
msg.CANFrame = CANMessage.init_data(
num_fid, int(length),
bytes.fromhex(data)[:8])
msg.CANData = True
msg.bus = _bus
self._stream.append([time_stamp, msg])
self._size += 1
except Exception as e:
print(str(e))
return "Can't open files with CAN messages: " + str(e)
return "Loaded from file: " + str(len(self)) + " messages"
def do_read(self, can_msg):
data = b""
if self._serialPort.inWaiting() > 0:
while not can_msg.CANData and not can_msg.debugData:
byte = self._serialPort.read(1)
if byte == b"":
break
data += byte
if data[-1:] == b"\r":
can_msg.bus = self._bus # Bus USBtin
if data[0:1] == b"t":
# "t10F81122334455667788"
_length = int(data[4:5])
_id = struct.unpack(
"!H",
bytes.fromhex("0" +
data[1:4].decode('ISO-8859-1')))[0]
_data = list(
bytes.fromhex(data[5:-1].decode('ISO-8859-1')))
can_msg.CANFrame = CANMessage(_id, _length, _data,
False,
CANMessage.DataFrame)
can_msg.CANData = True
elif data[0:1] == b"T": # Extended
# "T0011111181122334455667788"
_length = int(data[9:10])
_id = struct.unpack(
"!I",
bytes.fromhex(data[1:9].decode('ISO-8859-1')))[0]
_data = list(
bytes.fromhex(data[10:-1].decode('ISO-8859-1')))
can_msg.CANFrame = CANMessage(_id, _length, _data,
True,
CANMessage.DataFrame)
can_msg.CANData = True
elif data[0:1] == b"r": # RTR
_length = int(data[4:5])
_id = struct.unpack(
"!H",
bytes.fromhex("0" +
data[1:4].decode('ISO-8859-1')))[0]
can_msg.CANFrame = CANMessage(_id, _length, [], False,
CANMessage.RemoteFrame)
can_msg.CANData = True
elif data[0:1] == b"R": # Extended RTR
_length = int(data[9:10])
_id = struct.unpack(
"!I",
bytes.fromhex(data[1:9].decode('ISO-8859-1')))[0]
can_msg.CANFrame = CANMessage(_id, _length, [], True,
CANMessage.RemoteFrame)
can_msg.CANData = True
elif data[0:1] == b"z" or data[0:1] == b"Z" or data[
0:1] == b"\r" or data[0:1] == b"\x07":
break
else:
can_msg.debugData = True
can_msg.debugText = {'text': data.decode("ISO-8859-1")}
self.dprint(
1, "USBtin DREAD: " + data.decode("ISO-8859-1"))
self.dprint(2, "USBtin READ: " + data.decode("ISO-8859-1"))
return can_msg
def do_effect(self, can_msg, args):
if self._status2['status'] > 0:
self.generate_rpm()
self.generate_vin()
if args['action'] == 'read' and can_msg.CANData: # READ
if self._status2['id_command'] == can_msg.CANFrame.frame_id:
for cmd, value in self._status2['commands'].items():
len_cmd = int(len(str(value)) / 2)
if self._status2['status'] != 0 and cmd == "rpm_up":
len_cmd2 = len_cmd + 1
if can_msg.CANFrame.frame_length == len_cmd2 and can_msg.CANFrame.frame_raw_data[
0:len_cmd] == bytes.fromhex(
value
)[0:len_cmd] and self._status2['status'] == 1:
self.rpm_up += ord(
can_msg.CANFrame.
frame_raw_data[len_cmd:len_cmd2])
elif self._status2['status'] != 0 and cmd == "rpm_down":
len_cmd2 = len_cmd + 1
if can_msg.CANFrame.frame_length == len_cmd2 and can_msg.CANFrame.frame_raw_data[
0:len_cmd] == bytes.fromhex(
value
)[0:len_cmd] and self._status2['status'] == 1:
self.rpm_down += ord(
can_msg.CANFrame.
frame_raw_data[len_cmd:len_cmd2])
elif self._status2['status'] != 0 and cmd == "stop":
if can_msg.CANFrame.frame_length == len_cmd and can_msg.CANFrame.frame_raw_data[
0:len_cmd] == bytes.fromhex(value)[0:len_cmd]:
self._status2['status'] = 3
self.current = 0
elif cmd == "init":
if not self.init_sess:
self.init_sess = ISOTPMessage(
can_msg.CANFrame.frame_id)
ret = self.init_sess.add_can(can_msg.CANFrame)
if ret < 0:
self.init_sess = None
elif ret == 1:
if self.init_sess.message_length != 17:
self.init_sess = None
else:
key = self.init_sess.message_data
i = 0
vin_x = ""
for byte_k in key:
vin_x += chr(byte_k ^ ord(self._auth[i])
) # XOR with KEY (to get VIN)
i += 1
if vin_x != self._vin: # auth failed, send error back
self.frames.append(
CANMessage(
self._status2.get(
'id_report',
0xffff), 2, b'\xff\xff', False,
CANMessage.DataFrame))
else: # Auth complite
self._status2['status'] = 1
self.frames.append(
CANMessage(
self._status2.get(
'id_report',
0xffff), 2, b'\xff\x01', False,
CANMessage.DataFrame))
if self.current < 150:
self.current = self.default
self.init_sess = None
elif self._status2['id_uds'] == can_msg.CANFrame.frame_id:
if not self.init_sess2:
self.init_sess2 = ISOTPMessage(can_msg.CANFrame.frame_id)
ret = self.init_sess2.add_can(can_msg.CANFrame)
if ret < 0:
self.init_sess2 = None
elif ret == 1:
uds_msg = UDSMessage(self._status2.get('uds_shift', 8))
uds_msg.add_raw_request(self.init_sess2)
if can_msg.CANFrame.frame_id in uds_msg.sessions:
if 0x27 in uds_msg.sessions[
can_msg.CANFrame.frame_id]: # Check service
if 1 in uds_msg.sessions[
can_msg.CANFrame.
frame_id][0x27]: # Check sub: SEED request
self._seed = [
random.randrange(1, 100),
random.randrange(1, 100),
random.randrange(1, 100),
random.randrange(1, 100)
]
# Generate UDS reponse
self.frames.extend(
uds_msg.add_request(
self._status2['id_uds'] +
self._status2['uds_shift'], # ID
0x27 + 0x40, # Service
0x01, # Sub function
self._seed)) # data
elif 2 in uds_msg.sessions[
can_msg.CANFrame.
frame_id][0x27]: # Check sub: KEY enter
if self._seed is not None:
key_x = ""
key = uds_msg.sessions[
can_msg.CANFrame.frame_id][0x27][2][
'data'] # Read key
i = 0
for byte_k in key:
key_x += chr(byte_k
^ self._seed[i % 4])
i += 1
if key_x == self._uds_auth:
self._uds_auth_done = True
self.frames.extend(
uds_msg.add_request(
self._status2['id_uds'] + self.
_status2['uds_shift'], # ID
0x27 + 0x40, # Service
0x02, # Sub function
[0xFF])) # data
else:
self._uds_auth_done = False
self.frames.extend(
uds_msg.add_request(
self._status2['id_uds'] + self.
_status2['uds_shift'], # ID
0x27 + 0x40, # Service
0x02, # Sub function
[0x00])) # data
self._seed = None
elif 0x2e in uds_msg.sessions[
can_msg.CANFrame.
frame_id] and 0x55 in uds_msg.sessions[
can_msg.CANFrame.
frame_id][0x2e] and uds_msg.sessions[
can_msg.CANFrame.frame_id][0x2e][0x55][
'data'][0] == 0x55:
if self._uds_auth_done:
new_key = ''.join(
chr(x) for x in uds_msg.sessions[
can_msg.CANFrame.frame_id][0x2e][0x55]
['data'][1:])
if len(new_key) == 17:
self._uds_auth_done = False
self._auth = new_key
self.frames.extend(
uds_msg.add_request(
self._status2['id_uds'] +
self._status2['uds_shift'], # ID
0x2e + 0x40, # Service
0x55, # Sub function
[0x55])) # data
else:
self._uds_auth_done = False
self.frames.extend(
uds_msg.add_request(
self._status2['id_uds'] +
self._status2['uds_shift'], # ID
0x2e + 0x40, # Service
0x00, # Sub function
[0x00]))
else:
self._uds_auth_done = False
self.frames.extend(
uds_msg.add_request(
self._status2['id_uds'] +
self._status2['uds_shift'], # ID
0x2e + 0x40, # Service
0x00, # Sub function
[0x00])) # data
self.init_sess2 = None
elif args['action'] == 'write' and not can_msg.CANData:
if len(self.frames) > 0:
can_msg.CANFrame = self.frames.pop(0)
can_msg.CANData = True
can_msg.bus = self._bus
return can_msg
def do_read(self, can_msg, args):
data = b""
counter = 0
rBus = int(args.get("bus", 0))
if 0 < rBus < 4:
if len(self._queue[rBus - 1]) > 0:
if not can_msg.CANData and not can_msg.debugData:
can_msg.CANFrame = self._queue[rBus - 1].pop(0)
can_msg.bus = rBus
can_msg.CANData = True
print("BUS " + str(rBus))
print("POP: " + str(can_msg.CANFrame))
elif self._serialPort.inWaiting() > 0:
while not can_msg.CANData and not can_msg.debugData:
byte = self._serialPort.read(1)
if byte == b"":
break
data += byte
counter += 1
if data[-2:] == b"\r\n": # End
if data[0:
1] == b'\x03' and counter == 16: # Packet received
tmp_data = data[2:-3]
_id = struct.unpack("!H", tmp_data[0:2])[
0] # TODO: ADD SUPPORT EXTENDED and RTR
_data = tmp_data[2:-1]
_length = tmp_data[-1]
bus = (struct.unpack("B", data[1:2])[0])
if rBus == bus:
can_msg.CANFrame = CANMessage(
_id, _length, _data, False,
CANMessage.DataFrame)
can_msg.bus = str(self._bus) + '_' + str(bus)
can_msg.CANData = True
print("BUS " + str(rBus))
print("MESS:" + str(_id) + " " + str(_length) +
" " + self.get_hex(_data))
print(self.get_hex(tmp_data))
else:
self._queue[bus - 1].append(
CANMessage(_id, _length, _data, False,
CANMessage.DataFrame))
print("BUS " + str(rBus))
print("QUEU:" + str(_id) + " " + str(_length) +
" " + self.get_hex(_data))
print(self.get_hex(tmp_data))
elif data[0:1] == b'{' and data[
-3:-2] == b'}': # Debug info
can_msg.debugData = True
can_msg.debugText = {
'text': data.decode("ISO-8859-1")
}
else:
break
self.dprint(2, "READ: " + self.get_hex(data))
return can_msg
def control_stop_engine(self):
self.frames.append(
CANMessage(self._status2['id_command'], 1,
bytes.fromhex(self._status2['commands']['stop']), False,
CANMessage.DataFrame))
return ""
