# This file is part of Scapy # See http://www.secdev.org/projects/scapy for more information # Copyright (C) Nils Weiss # Copyright (C) Enrico Pozzobon # Copyright (C) Alexander Schroeder # This program is published under a GPLv2 license # scapy.contrib.description = ISO-TP (ISO 15765-2) # scapy.contrib.status = loads """ ISOTPSocket. """ import ctypes from ctypes.util import find_library import struct import socket import time import traceback import heapq from threading import Thread, Event, Lock from scapy.packet import Packet from scapy.fields import BitField, FlagsField, StrLenField, \ ThreeBytesField, XBitField, ConditionalField, \ BitEnumField, ByteField, XByteField, BitFieldLenField, StrField from scapy.compat import chb, orb from scapy.layers.can import CAN import scapy.modules.six as six import scapy.automaton as automaton import six.moves.queue as queue from scapy.error import Scapy_Exception, warning, log_loading, log_runtime from scapy.supersocket import SuperSocket, SO_TIMESTAMPNS from scapy.config import conf from scapy.consts import LINUX from scapy.contrib.cansocket import PYTHON_CAN from scapy.sendrecv import sniff from scapy.sessions import DefaultSession __all__ = ["ISOTP", "ISOTPHeader", "ISOTPHeaderEA", "ISOTP_SF", "ISOTP_FF", "ISOTP_CF", "ISOTP_FC", "ISOTPSoftSocket", "ISOTPSession", "ISOTPSocket", "ISOTPSocketImplementation", "ISOTPMessageBuilder", "ISOTPScan"] USE_CAN_ISOTP_KERNEL_MODULE = False if six.PY3 and LINUX: LIBC = ctypes.cdll.LoadLibrary(find_library("c")) try: if conf.contribs['ISOTP']['use-can-isotp-kernel-module']: USE_CAN_ISOTP_KERNEL_MODULE = True except KeyError: log_loading.info("Specify 'conf.contribs['ISOTP'] = " "{'use-can-isotp-kernel-module': True}' to enable " "usage of can-isotp kernel module.") CAN_MAX_IDENTIFIER = (1 << 29) - 1 # Maximum 29-bit identifier CAN_MTU = 16 CAN_MAX_DLEN = 8 ISOTP_MAX_DLEN_2015 = (1 << 32) - 1 # Maximum for 32-bit FF_DL ISOTP_MAX_DLEN = (1 << 12) - 1 # Maximum for 12-bit FF_DL N_PCI_SF = 0x00 # /* single frame */ N_PCI_FF = 0x10 # /* first frame */ N_PCI_CF = 0x20 # /* consecutive frame */ N_PCI_FC = 0x30 # /* flow control */ class ISOTP(Packet): name = 'ISOTP' fields_desc = [ StrField('data', B"") ] __slots__ = Packet.__slots__ + ["src", "dst", "exsrc", "exdst"] def answers(self, other): if other.__class__ == self.__class__: return self.payload.answers(other.payload) return 0 def __init__(self, *args, **kwargs): self.src = None self.dst = None self.exsrc = None self.exdst = None if "src" in kwargs: self.src = kwargs["src"] del kwargs["src"] if "dst" in kwargs: self.dst = kwargs["dst"] del kwargs["dst"] if "exsrc" in kwargs: self.exsrc = kwargs["exsrc"] del kwargs["exsrc"] if "exdst" in kwargs: self.exdst = kwargs["exdst"] del kwargs["exdst"] Packet.__init__(self, *args, **kwargs) self.validate_fields() def validate_fields(self): if self.src is not None: if not 0 <= self.src <= CAN_MAX_IDENTIFIER: raise Scapy_Exception("src is not a valid CAN identifier") if self.dst is not None: if not 0 <= self.dst <= CAN_MAX_IDENTIFIER: raise Scapy_Exception("dst is not a valid CAN identifier") if self.exsrc is not None: if not 0 <= self.exsrc <= 0xff: raise Scapy_Exception("exsrc is not a byte") if self.exdst is not None: if not 0 <= self.exdst <= 0xff: raise Scapy_Exception("exdst is not a byte") def fragment(self): data_bytes_in_frame = 7 if self.exdst is not None: data_bytes_in_frame = 6 if len(self.data) > ISOTP_MAX_DLEN_2015: raise Scapy_Exception("Too much data in ISOTP message") if len(self.data) <= data_bytes_in_frame: # We can do this in a single frame frame_data = struct.pack('B', len(self.data)) + self.data if self.exdst: frame_data = struct.pack('B', self.exdst) + frame_data if self.dst is None or self.dst <= 0x7ff: pkt = CAN(identifier=self.dst, data=frame_data) else: pkt = CAN(identifier=self.dst, flags="extended", data=frame_data) return [pkt] # Construct the first frame if len(self.data) <= ISOTP_MAX_DLEN: frame_header = struct.pack(">H", len(self.data) + 0x1000) else: frame_header = struct.pack(">HI", 0x1000, len(self.data)) if self.exdst: frame_header = struct.pack('B', self.exdst) + frame_header idx = 8 - len(frame_header) frame_data = self.data[0:idx] if self.dst is None or self.dst <= 0x7ff: frame = CAN(identifier=self.dst, data=frame_header + frame_data) else: frame = CAN(identifier=self.dst, flags="extended", data=frame_header + frame_data) # Construct consecutive frames n = 1 pkts = [frame] while idx < len(self.data): frame_data = self.data[idx:idx + data_bytes_in_frame] frame_header = struct.pack("b", (n % 16) + N_PCI_CF) n += 1 idx += len(frame_data) if self.exdst: frame_header = struct.pack('B', self.exdst) + frame_header if self.dst is None or self.dst <= 0x7ff: pkt = CAN(identifier=self.dst, data=frame_header + frame_data) else: pkt = CAN(identifier=self.dst, flags="extended", data=frame_header + frame_data) pkts.append(pkt) return pkts @staticmethod def defragment(can_frames, use_extended_addressing=None): if len(can_frames) == 0: raise Scapy_Exception("ISOTP.defragment called with 0 frames") dst = can_frames[0].identifier for frame in can_frames: if frame.identifier != dst: warning("Not all CAN frames have the same identifier") parser = ISOTPMessageBuilder(use_extended_addressing) for c in can_frames: parser.feed(c) results = [] while parser.count > 0: p = parser.pop() if (use_extended_addressing is True and p.exdst is not None) \ or (use_extended_addressing is False and p.exdst is None) \ or (use_extended_addressing is None): results.append(p) if len(results) == 0: return None if len(results) > 0: warning("More than one ISOTP frame could be defragmented from the " "provided CAN frames, returning the first one.") return results[0] class ISOTPHeader(CAN): name = 'ISOTPHeader' fields_desc = [ FlagsField('flags', 0, 3, ['error', 'remote_transmission_request', 'extended']), XBitField('identifier', 0, 29), ByteField('length', None), ThreeBytesField('reserved', 0), ] def extract_padding(self, p): return p, None def post_build(self, pkt, pay): """ This will set the ByteField 'length' to the correct value. """ if self.length is None: pkt = pkt[:4] + chb(len(pay)) + pkt[5:] return pkt + pay def guess_payload_class(self, payload): """ ISOTP encodes the frame type in the first nibble of a frame. """ t = (orb(payload[0]) & 0xf0) >> 4 if t == 0: return ISOTP_SF elif t == 1: return ISOTP_FF elif t == 2: return ISOTP_CF else: return ISOTP_FC class ISOTPHeaderEA(ISOTPHeader): name = 'ISOTPHeaderExtendedAddress' fields_desc = ISOTPHeader.fields_desc + [ XByteField('extended_address', 0), ] def post_build(self, p, pay): """ This will set the ByteField 'length' to the correct value. 'chb(len(pay) + 1)' is required, because the field 'extended_address' is counted as payload on the CAN layer """ if self.length is None: p = p[:4] + chb(len(pay) + 1) + p[5:] return p + pay ISOTP_TYPE = {0: 'single', 1: 'first', 2: 'consecutive', 3: 'flow_control'} class ISOTP_SF(Packet): name = 'ISOTPSingleFrame' fields_desc = [ BitEnumField('type', 0, 4, ISOTP_TYPE), BitFieldLenField('message_size', None, 4, length_of='data'), StrLenField('data', '', length_from=lambda pkt: pkt.message_size) ] class ISOTP_FF(Packet): name = 'ISOTPFirstFrame' fields_desc = [ BitEnumField('type', 1, 4, ISOTP_TYPE), BitField('message_size', 0, 12), ConditionalField(BitField('extended_message_size', 0, 32), lambda pkt: pkt.message_size == 0), StrField('data', '', fmt="B") ] class ISOTP_CF(Packet): name = 'ISOTPConsecutiveFrame' fields_desc = [ BitEnumField('type', 2, 4, ISOTP_TYPE), BitField('index', 0, 4), StrField('data', '', fmt="B") ] class ISOTP_FC(Packet): name = 'ISOTPFlowControlFrame' fields_desc = [ BitEnumField('type', 3, 4, ISOTP_TYPE), BitEnumField('fc_flag', 0, 4, {0: 'continue', 1: 'wait', 2: 'abort'}), ByteField('block_size', 0), ByteField('separation_time', 0), ] class ISOTPMessageBuilderIter(object): slots = ["builder"] def __init__(self, builder): self.builder = builder def __iter__(self): return self def __next__(self): while self.builder.count: return self.builder.pop() raise StopIteration next = __next__ class ISOTPMessageBuilder(object): """ Utility class to build ISOTP messages out of CAN frames, used by both ISOTP.defragment() and ISOTPSession. This class attempts to interpret some CAN frames as ISOTP frames, both with and without extended addressing at the same time. For example, if an extended address of 07 is being used, all frames will also be interpreted as ISOTP single-frame messages. CAN frames are fed to an ISOTPMessageBuilder object with the feed() method and the resulting ISOTP frames can be extracted using the pop() method. """ class Bucket(object): def __init__(self, total_len, first_piece, ts=None): self.pieces = list() self.total_len = total_len self.current_len = 0 self.ready = None self.src = None self.exsrc = None self.time = ts self.push(first_piece) def push(self, piece): self.pieces.append(piece) self.current_len += len(piece) if self.current_len >= self.total_len: if six.PY3: isotp_data = b"".join(self.pieces) else: isotp_data = "".join(map(str, self.pieces)) self.ready = isotp_data[:self.total_len] def __init__(self, use_ext_addr=None, did=None, basecls=None): """ Initialize a ISOTPMessageBuilder object :param use_ext_addr: True for only attempting to defragment with extended addressing, False for only attempting to defragment without extended addressing, or None for both :param basecls: the class of packets that will be returned, defaults to ISOTP """ self.ready = [] self.buckets = {} self.use_ext_addr = use_ext_addr self.basecls = basecls or ISOTP self.dst_ids = None self.last_ff = None self.last_ff_ex = None if did is not None: if hasattr(did, "__iter__"): self.dst_ids = did else: self.dst_ids = [did] def feed(self, can): """Attempt to feed an incoming CAN frame into the state machine""" if not isinstance(can, Packet) and hasattr(can, "__iter__"): for p in can: self.feed(p) return identifier = can.identifier if self.dst_ids is not None and identifier not in self.dst_ids: return data = bytes(can.data) if len(data) > 1 and self.use_ext_addr is not True: self._try_feed(identifier, None, data, can.time) if len(data) > 2 and self.use_ext_addr is not False: ea = six.indexbytes(data, 0) self._try_feed(identifier, ea, data[1:], can.time) @property def count(self): """Returns the number of ready ISOTP messages built from the provided can frames""" return len(self.ready) def __len__(self): return self.count def pop(self, identifier=None, ext_addr=None): """ Returns a built ISOTP message :param identifier: if not None, only return isotp messages with this destination :param ext_addr: if identifier is not None, only return isotp messages with this extended address for destination :returns: an ISOTP packet, or None if no message is ready """ if identifier is not None: for i in range(len(self.ready)): b = self.ready[i] iden = b[0] ea = b[1] if iden == identifier and ext_addr == ea: return ISOTPMessageBuilder._build(self.ready.pop(i), self.basecls) return None if len(self.ready) > 0: return ISOTPMessageBuilder._build(self.ready.pop(0), self.basecls) return None def __iter__(self): return ISOTPMessageBuilderIter(self) @staticmethod def _build(t, basecls=ISOTP): bucket = t[2] p = basecls(bucket.ready) if hasattr(p, "dst"): p.dst = t[0] if hasattr(p, "exdst"): p.exdst = t[1] if hasattr(p, "src"): p.src = bucket.src if hasattr(p, "exsrc"): p.exsrc = bucket.exsrc if hasattr(p, "time"): p.time = bucket.time return p def _feed_first_frame(self, identifier, ea, data, ts): if len(data) < 3: # At least 3 bytes are necessary: 2 for length and 1 for data return False header = struct.unpack('>H', bytes(data[:2]))[0] expected_length = header & 0x0fff isotp_data = data[2:] if expected_length == 0 and len(data) >= 6: expected_length = struct.unpack('>I', bytes(data[2:6]))[0] isotp_data = data[6:] key = (ea, identifier, 1) if ea is None: self.last_ff = key else: self.last_ff_ex = key self.buckets[key] = self.Bucket(expected_length, isotp_data, ts) return True def _feed_single_frame(self, identifier, ea, data, ts): if len(data) < 2: # At least 2 bytes are necessary: 1 for length and 1 for data return False length = six.indexbytes(data, 0) & 0x0f isotp_data = data[1:length + 1] if length > len(isotp_data): # CAN frame has less data than expected return False self.ready.append((identifier, ea, self.Bucket(length, isotp_data, ts))) return True def _feed_consecutive_frame(self, identifier, ea, data): if len(data) < 2: # At least 2 bytes are necessary: 1 for sequence number and # 1 for data return False first_byte = six.indexbytes(data, 0) seq_no = first_byte & 0x0f isotp_data = data[1:] key = (ea, identifier, seq_no) bucket = self.buckets.pop(key, None) if bucket is None: # There is no message constructor waiting for this frame return False bucket.push(isotp_data) if bucket.ready is None: # full ISOTP message is not ready yet, put it back in # buckets list next_seq = (seq_no + 1) % 16 key = (ea, identifier, next_seq) self.buckets[key] = bucket else: self.ready.append((identifier, ea, bucket)) return True def _feed_flow_control_frame(self, identifier, ea, data): if len(data) < 3: # At least 2 bytes are necessary: 1 for sequence number and # 1 for data return False keys = [self.last_ff, self.last_ff_ex] if not any(keys): return False buckets = [self.buckets.pop(k, None) for k in keys] self.last_ff = None self.last_ff_ex = None if not any(buckets): # There is no message constructor waiting for this frame return False for key, bucket in zip(keys, buckets): if bucket is None: continue bucket.src = identifier bucket.exsrc = ea self.buckets[key] = bucket return True def _try_feed(self, identifier, ea, data, ts): first_byte = six.indexbytes(data, 0) if len(data) > 1 and first_byte & 0xf0 == N_PCI_SF: self._feed_single_frame(identifier, ea, data, ts) if len(data) > 2 and first_byte & 0xf0 == N_PCI_FF: self._feed_first_frame(identifier, ea, data, ts) if len(data) > 1 and first_byte & 0xf0 == N_PCI_CF: self._feed_consecutive_frame(identifier, ea, data) if len(data) > 1 and first_byte & 0xf0 == N_PCI_FC: self._feed_flow_control_frame(identifier, ea, data) class ISOTPSession(DefaultSession): """Defragment ISOTP packets 'on-the-flow'. Usage: >>> sniff(session=ISOTPSession) """ def __init__(self, *args, **kwargs): DefaultSession.__init__(self, *args, **kwargs) self.m = ISOTPMessageBuilder( use_ext_addr=kwargs.pop("use_ext_addr", None), did=kwargs.pop("did", None), basecls=kwargs.pop("basecls", None)) def on_packet_received(self, pkt): if not pkt: return if isinstance(pkt, list): for p in pkt: ISOTPSession.on_packet_received(self, p) return self.m.feed(pkt) while len(self.m) > 0: rcvd = self.m.pop() if self._supersession: self._supersession.on_packet_received(rcvd) else: DefaultSession.on_packet_received(self, rcvd) class ISOTPSoftSocket(SuperSocket): """ This class is a wrapper around the ISOTPSocketImplementation, for the reasons described below. The ISOTPSoftSocket aims to be fully compatible with the Linux ISOTP sockets provided by the can-isotp kernel module, while being usable on any operating system. Therefore, this socket needs to be able to respond to an incoming FF frame with a FC frame even before the recv() method is called. A thread is needed for receiving CAN frames in the background, and since the lower layer CAN implementation is not guaranteed to have a functioning POSIX select(), each ISOTP socket needs its own CAN receiver thread. SuperSocket automatically calls the close() method when the GC destroys an ISOTPSoftSocket. However, note that if any thread holds a reference to an ISOTPSoftSocket object, it will not be collected by the GC. The implementation of the ISOTP protocol, along with the necessary thread, are stored in the ISOTPSocketImplementation class, and therefore: * There no reference from ISOTPSocketImplementation to ISOTPSoftSocket * ISOTPSoftSocket can be normally garbage collected * Upon destruction, ISOTPSoftSocket.close() will be called * ISOTPSoftSocket.close() will call ISOTPSocketImplementation.close() * RX background thread can be stopped by the garbage collector """ nonblocking_socket = True def __init__(self, can_socket=None, sid=0, did=0, extended_addr=None, extended_rx_addr=None, rx_block_size=0, rx_separation_time_min=0, padding=False, listen_only=False, basecls=ISOTP): """ Initialize an ISOTPSoftSocket using the provided underlying can socket :param can_socket: a CANSocket instance, preferably filtering only can frames with identifier equal to did :param sid: the CAN identifier of the sent CAN frames :param did: the CAN identifier of the received CAN frames :param extended_addr: the extended address of the sent ISOTP frames (can be None) :param extended_rx_addr: the extended address of the received ISOTP frames (can be None) :param rx_block_size: block size sent in Flow Control ISOTP frames :param rx_separation_time_min: minimum desired separation time sent in Flow Control ISOTP frames :param padding: If True, pads sending packets with 0x00 which not count to the payload. Does not affect receiving packets. :param basecls: base class of the packets emitted by this socket """ if six.PY3 and LINUX and isinstance(can_socket, six.string_types): from scapy.contrib.cansocket import CANSocket can_socket = CANSocket(can_socket) elif isinstance(can_socket, six.string_types): raise Scapy_Exception("Provide a CANSocket object instead") self.exsrc = extended_addr self.exdst = extended_rx_addr self.src = sid self.dst = did impl = ISOTPSocketImplementation( can_socket, src_id=sid, dst_id=did, padding=padding, extended_addr=extended_addr, extended_rx_addr=extended_rx_addr, rx_block_size=rx_block_size, rx_separation_time_min=rx_separation_time_min, listen_only=listen_only ) self.ins = impl self.outs = impl self.impl = impl if basecls is None: warning('Provide a basecls ') self.basecls = basecls def close(self): if not self.closed: self.impl.close() self.outs = None self.ins = None SuperSocket.close(self) def begin_send(self, p): """Begin the transmission of message p. This method returns after sending the first frame. If multiple frames are necessary to send the message, this socket will unable to send other messages until either the transmission of this frame succeeds or it fails.""" if hasattr(p, "sent_time"): p.sent_time = time.time() return self.outs.begin_send(bytes(p)) def recv_raw(self, x=0xffff): """Receive a complete ISOTP message, blocking until a message is received or the specified timeout is reached. If self.timeout is 0, then this function doesn't block and returns the first frame in the receive buffer or None if there isn't any.""" msg = self.ins.recv() t = time.time() return self.basecls, msg, t def recv(self, x=0xffff): msg = SuperSocket.recv(self, x) if hasattr(msg, "src"): msg.src = self.src if hasattr(msg, "dst"): msg.dst = self.dst if hasattr(msg, "exsrc"): msg.exsrc = self.exsrc if hasattr(msg, "exdst"): msg.exdst = self.exdst return msg @staticmethod def select(sockets, remain=None): """This function is called during sendrecv() routine to wait for sockets to be ready to receive """ blocking = remain is None or remain > 0 def find_ready_sockets(): return list(filter(lambda x: not x.ins.rx_queue.empty(), sockets)) ready_sockets = find_ready_sockets() if len(ready_sockets) > 0 or not blocking: return ready_sockets, None exit_select = Event() def my_cb(msg): exit_select.set() try: for s in sockets: s.ins.rx_callbacks.append(my_cb) exit_select.wait(remain) finally: for s in sockets: try: s.ins.rx_callbacks.remove(my_cb) except ValueError: pass except AttributeError: pass ready_sockets = find_ready_sockets() return ready_sockets, None ISOTPSocket = ISOTPSoftSocket class CANReceiverThread(Thread): """ Helper class that receives CAN frames and feeds them to the provided callback. It relies on CAN frames being enqueued in the CANSocket object and not being lost if they come before the sniff method is called. This is true in general since sniff is usually implemented as repeated recv(), but might be false in some implementation of CANSocket """ def __init__(self, can_socket, callback): """ Initialize the thread. In order for this thread to be able to be stopped by the destructor of another object, it is important to not keep a reference to the object in the callback function. :param socket: the CANSocket upon which this class will call the sniff() method :param callback: function to call whenever a CAN frame is received """ self.socket = can_socket self.callback = callback self.exiting = False self._thread_started = Event() self.exception = None Thread.__init__(self) self.name = "CANReceiver" + self.name def start(self): Thread.start(self) if not self._thread_started.wait(5): raise Scapy_Exception("CAN RX thread not started in 5s.") def run(self): self._thread_started.set() try: def prn(msg): if not self.exiting: self.callback(msg) while 1: try: sniff(store=False, timeout=1, count=1, stop_filter=lambda x: self.exiting, prn=prn, opened_socket=self.socket) except ValueError as ex: if not self.exiting: raise ex if self.exiting: return except Exception as ex: self.exception = ex def stop(self): self.exiting = True class TimeoutScheduler: """A timeout scheduler which uses a single thread for all timeouts, unlike python's own Timer objects which use a thread each.""" VERBOSE = False GRACE = .1 _mutex = Lock() _event = Event() _thread = None _handles = [] # must use heapq functions! @staticmethod def schedule(timeout, callback): """Schedules the execution of a timeout. The function `callback` will be called in `timeout` seconds. Returns a handle that can be used to remove the timeout.""" when = TimeoutScheduler._time() + timeout handle = TimeoutScheduler.Handle(when, callback) handles = TimeoutScheduler._handles with TimeoutScheduler._mutex: # Add the handler to the heap, keeping the invariant # Time complexity is O(log n) heapq.heappush(handles, handle) must_interrupt = (handles[0] == handle) # Start the scheduling thread if it is not started already if TimeoutScheduler._thread is None: t = Thread(target=TimeoutScheduler._task) must_interrupt = False TimeoutScheduler._thread = t TimeoutScheduler._event.clear() t.start() if must_interrupt: # if the new timeout got in front of the one we are currently # waiting on, the current wait operation must be aborted and # updated with the new timeout TimeoutScheduler._event.set() # Return the handle to the timeout so that the user can cancel it return handle @staticmethod def cancel(handle): """Provided its handle, cancels the execution of a timeout.""" handles = TimeoutScheduler._handles with TimeoutScheduler._mutex: if handle in handles: # Time complexity is O(n) handle._cb = None handles.remove(handle) heapq.heapify(handles) if len(handles) == 0: # set the event to stop the wait - this kills the thread TimeoutScheduler._event.set() else: Exception("Handle not found") @staticmethod def clear(): """Cancels the execution of all timeouts.""" with TimeoutScheduler._mutex: TimeoutScheduler._handles.clear() # set the event to stop the wait - this kills the thread TimeoutScheduler._event.set() @staticmethod def _peek_next(): """Returns the next timeout to execute, or `None` if list is empty, without modifying the list""" with TimeoutScheduler._mutex: handles = TimeoutScheduler._handles if len(handles) == 0: return None else: return handles[0] @staticmethod def _wait(handle): """Waits until it is time to execute the provided handle, or until another thread calls _event.set()""" if handle is None: when = TimeoutScheduler.GRACE else: when = handle._when # Check how much time until the next timeout now = TimeoutScheduler._time() to_wait = when - now # Wait until the next timeout, # or until event.set() gets called in another thread. if to_wait > 0: log_runtime.debug("TimeoutScheduler Thread going to sleep @ %f " + "for %fs", now, to_wait) interrupted = TimeoutScheduler._event.wait(to_wait) new = TimeoutScheduler._time() log_runtime.debug("TimeoutScheduler Thread awake @ %f, slept for" + " %f, interrupted=%d", new, new - now, interrupted) # Clear the event so that we can wait on it again, # Must be done before doing the callbacks to avoid losing a set(). TimeoutScheduler._event.clear() @staticmethod def _task(): """Executed in a background thread, this thread will automatically start when the first timeout is added and stop when the last timeout is removed or executed.""" log_runtime.debug("TimeoutScheduler Thread spawning @ %f", TimeoutScheduler._time()) time_empty = None try: while 1: handle = TimeoutScheduler._peek_next() if handle is None: now = TimeoutScheduler._time() if time_empty is None: time_empty = now # 100 ms of grace time before killing the thread if TimeoutScheduler.GRACE < now - time_empty: return TimeoutScheduler._wait(handle) TimeoutScheduler._poll() finally: # Worst case scenario: if this thread dies, the next scheduled # timeout will start a new one log_runtime.debug("TimeoutScheduler Thread dying @ %f", TimeoutScheduler._time()) TimeoutScheduler._thread = None @staticmethod def _poll(): """Execute all the callbacks that were due until now""" handles = TimeoutScheduler._handles handle = None while 1: with TimeoutScheduler._mutex: now = TimeoutScheduler._time() if len(handles) == 0 or handles[0]._when > now: # There is nothing to execute yet return # Time complexity is O(log n) handle = heapq.heappop(handles) # Call the callback here, outside of the mutex callback = handle._cb if handle is not None else None if callback is not None: try: callback() except Exception: traceback.print_exc() @staticmethod def _time(): if six.PY2: return time.time() return time.monotonic() class Handle: """Handle for a timeout, consisting of a callback and a time when it should be executed.""" __slots__ = '_when', '_cb' def __init__(self, when, cb): self._when = when self._cb = cb def cancel(self): """Cancels this timeout, preventing it from executing its callback""" self._cb = None return TimeoutScheduler.cancel(self) def __cmp__(self, other): diff = self._when - other._when return 0 if diff == 0 else (1 if diff > 0 else -1) def __lt__(self, other): return self._when < other._when """ISOTPSoftSocket definitions.""" # Enum states ISOTP_IDLE = 0 ISOTP_WAIT_FIRST_FC = 1 ISOTP_WAIT_FC = 2 ISOTP_WAIT_DATA = 3 ISOTP_SENDING = 4 # /* Flow Status given in FC frame */ ISOTP_FC_CTS = 0 # /* clear to send */ ISOTP_FC_WT = 1 # /* wait */ ISOTP_FC_OVFLW = 2 # /* overflow */ class ISOTPSocketImplementation(automaton.SelectableObject): """ Implementation of an ISOTP "state machine". Most of the ISOTP logic was taken from https://github.com/hartkopp/can-isotp/blob/master/net/can/isotp.c This class is separated from ISOTPSoftSocket to make sure the background thread can't hold a reference to ISOTPSoftSocket, allowing it to be collected by the GC. """ def __init__(self, can_socket, src_id, dst_id, padding=False, extended_addr=None, extended_rx_addr=None, rx_block_size=0, rx_separation_time_min=0, listen_only=False): """ :param can_socket: a CANSocket instance, preferably filtering only can frames with identifier equal to did :param src_id: the CAN identifier of the sent CAN frames :param dst_id: the CAN identifier of the received CAN frames :param padding: If True, pads sending packets with 0x00 which not count to the payload. Does not affect receiving packets. :param extended_addr: Extended Address byte to be added at the beginning of every CAN frame _sent_ by this object. Can be None in order to disable extended addressing on sent frames. :param extended_rx_addr: Extended Address byte expected to be found at the beginning of every CAN frame _received_ by this object. Can be None in order to disable extended addressing on received frames. :param rx_block_size: Block Size byte to be included in every Control Flow Frame sent by this object. The default value of 0 means that all the data will be received in a single block. :param rx_separation_time_min: Time Minimum Separation byte to be included in every Control Flow Frame sent by this object. The default value of 0 indicates that the peer will not wait any time between sending frames. :param listen_only: Disables send of flow control frames """ automaton.SelectableObject.__init__(self) self.can_socket = can_socket self.dst_id = dst_id self.src_id = src_id self.padding = padding self.fc_timeout = 1 self.cf_timeout = 1 self.filter_warning_emitted = False self.extended_rx_addr = extended_rx_addr self.ea_hdr = b"" if extended_addr is not None: self.ea_hdr = struct.pack("B", extended_addr) self.listen_only = listen_only self.rxfc_bs = rx_block_size self.rxfc_stmin = rx_separation_time_min self.rx_queue = queue.Queue() self.rx_len = -1 self.rx_buf = None self.rx_sn = 0 self.rx_bs = 0 self.rx_idx = 0 self.rx_state = ISOTP_IDLE self.txfc_bs = 0 self.txfc_stmin = 0 self.tx_gap = 0 self.tx_buf = None self.tx_sn = 0 self.tx_bs = 0 self.tx_idx = 0 self.rx_ll_dl = 0 self.tx_state = ISOTP_IDLE self.tx_timeout_handle = None self.rx_timeout_handle = None self.rx_thread = CANReceiverThread(can_socket, self.on_can_recv) self.tx_mutex = Lock() self.rx_mutex = Lock() self.send_mutex = Lock() self.tx_done = Event() self.tx_exception = None self.tx_callbacks = [] self.rx_callbacks = [] self.rx_thread.start() def __del__(self): self.close() def can_send(self, load): if self.padding: load += bytearray(CAN_MAX_DLEN - len(load)) if self.src_id is None or self.src_id <= 0x7ff: self.can_socket.send(CAN(identifier=self.src_id, data=load)) else: self.can_socket.send(CAN(identifier=self.src_id, flags="extended", data=load)) def on_can_recv(self, p): if not isinstance(p, CAN): raise Scapy_Exception("argument is not a CAN frame") if p.identifier != self.dst_id: if not self.filter_warning_emitted: warning("You should put a filter for identifier=%x on your" "CAN socket" % self.dst_id) self.filter_warning_emitted = True else: self.on_recv(p) def close(self): self.rx_thread.stop() def _rx_timer_handler(self): """Method called every time the rx_timer times out, due to the peer not sending a consecutive frame within the expected time window""" with self.rx_mutex: if self.rx_state == ISOTP_WAIT_DATA: # we did not get new data frames in time. # reset rx state self.rx_state = ISOTP_IDLE warning("RX state was reset due to timeout") def _tx_timer_handler(self): """Method called every time the tx_timer times out, which can happen in two situations: either a Flow Control frame was not received in time, or the Separation Time Min is expired and a new frame must be sent.""" with self.tx_mutex: if (self.tx_state == ISOTP_WAIT_FC or self.tx_state == ISOTP_WAIT_FIRST_FC): # we did not get any flow control frame in time # reset tx state self.tx_state = ISOTP_IDLE self.tx_exception = "TX state was reset due to timeout" self.tx_done.set() raise Scapy_Exception(self.tx_exception) elif self.tx_state == ISOTP_SENDING: # push out the next segmented pdu src_off = len(self.ea_hdr) max_bytes = 7 - src_off while 1: load = self.ea_hdr load += struct.pack("B", N_PCI_CF + self.tx_sn) load += self.tx_buf[self.tx_idx:self.tx_idx + max_bytes] self.can_send(load) self.tx_sn = (self.tx_sn + 1) % 16 self.tx_bs += 1 self.tx_idx += max_bytes if len(self.tx_buf) <= self.tx_idx: # we are done self.tx_state = ISOTP_IDLE self.tx_done.set() for cb in self.tx_callbacks: cb() return if self.txfc_bs != 0 and self.tx_bs >= self.txfc_bs: # stop and wait for FC self.tx_state = ISOTP_WAIT_FC self.tx_timeout_handle = TimeoutScheduler.schedule( self.fc_timeout, self._tx_timer_handler) return if self.tx_gap == 0: continue else: self.tx_timeout_handle = TimeoutScheduler.schedule( self.tx_gap, self._tx_timer_handler) def on_recv(self, cf): """Function that must be called every time a CAN frame is received, to advance the state machine.""" data = bytes(cf.data) if len(data) < 2: return ae = 0 if self.extended_rx_addr is not None: ae = 1 if len(data) < 3: return if six.indexbytes(data, 0) != self.extended_rx_addr: return n_pci = six.indexbytes(data, ae) & 0xf0 if n_pci == N_PCI_FC: with self.tx_mutex: self._recv_fc(data[ae:]) elif n_pci == N_PCI_SF: with self.rx_mutex: self._recv_sf(data[ae:]) elif n_pci == N_PCI_FF: with self.rx_mutex: self._recv_ff(data[ae:]) elif n_pci == N_PCI_CF: with self.rx_mutex: self._recv_cf(data[ae:]) def _recv_fc(self, data): """Process a received 'Flow Control' frame""" if (self.tx_state != ISOTP_WAIT_FC and self.tx_state != ISOTP_WAIT_FIRST_FC): return 0 if self.tx_timeout_handle is not None: self.tx_timeout_handle.cancel() self.tx_timeout_handle = None if len(data) < 3: self.tx_state = ISOTP_IDLE self.tx_exception = "CF frame discarded because it was too short" self.tx_done.set() raise Scapy_Exception(self.tx_exception) # get communication parameters only from the first FC frame if self.tx_state == ISOTP_WAIT_FIRST_FC: self.txfc_bs = six.indexbytes(data, 1) self.txfc_stmin = six.indexbytes(data, 2) if ((self.txfc_stmin > 0x7F) and ((self.txfc_stmin < 0xF1) or (self.txfc_stmin > 0xF9))): self.txfc_stmin = 0x7F if six.indexbytes(data, 2) <= 127: tx_gap = six.indexbytes(data, 2) / 1000.0 elif 0xf1 <= six.indexbytes(data, 2) <= 0xf9: tx_gap = (six.indexbytes(data, 2) & 0x0f) / 10000.0 else: tx_gap = 0 self.tx_gap = tx_gap self.tx_state = ISOTP_WAIT_FC isotp_fc = six.indexbytes(data, 0) & 0x0f if isotp_fc == ISOTP_FC_CTS: self.tx_bs = 0 self.tx_state = ISOTP_SENDING # start cyclic timer for sending CF frame self.tx_timeout_handle = TimeoutScheduler.schedule( self.tx_gap, self._tx_timer_handler) elif isotp_fc == ISOTP_FC_WT: # start timer to wait for next FC frame self.tx_state = ISOTP_WAIT_FC self.tx_timeout_handle = TimeoutScheduler.schedule( self.fc_timeout, self._tx_timer_handler) elif isotp_fc == ISOTP_FC_OVFLW: # overflow in receiver side self.tx_state = ISOTP_IDLE self.tx_exception = "Overflow happened at the receiver side" self.tx_done.set() raise Scapy_Exception(self.tx_exception) else: self.tx_state = ISOTP_IDLE self.tx_exception = "Unknown FC frame type" self.tx_done.set() raise Scapy_Exception(self.tx_exception) return 0 def _recv_sf(self, data): """Process a received 'Single Frame' frame""" if self.rx_timeout_handle is not None: self.rx_timeout_handle.cancel() self.rx_timeout_handle = None if self.rx_state != ISOTP_IDLE: warning("RX state was reset because single frame was received") self.rx_state = ISOTP_IDLE length = six.indexbytes(data, 0) & 0xf if len(data) - 1 < length: return 1 msg = data[1:1 + length] self.rx_queue.put(msg) for cb in self.rx_callbacks: cb(msg) self.call_release() return 0 def _recv_ff(self, data): """Process a received 'First Frame' frame""" if self.rx_timeout_handle is not None: self.rx_timeout_handle.cancel() self.rx_timeout_handle = None if self.rx_state != ISOTP_IDLE: warning("RX state was reset because first frame was received") self.rx_state = ISOTP_IDLE if len(data) < 7: return 1 self.rx_ll_dl = len(data) # get the FF_DL self.rx_len = (six.indexbytes(data, 0) & 0x0f) * 256 + six.indexbytes( data, 1) ff_pci_sz = 2 # Check for FF_DL escape sequence supporting 32 bit PDU length if self.rx_len == 0: # FF_DL = 0 => get real length from next 4 bytes self.rx_len = six.indexbytes(data, 2) << 24 self.rx_len += six.indexbytes(data, 3) << 16 self.rx_len += six.indexbytes(data, 4) << 8 self.rx_len += six.indexbytes(data, 5) ff_pci_sz = 6 # copy the first received data bytes data_bytes = data[ff_pci_sz:] self.rx_idx = len(data_bytes) self.rx_buf = data_bytes # initial setup for this pdu reception self.rx_sn = 1 self.rx_state = ISOTP_WAIT_DATA # no creation of flow control frames if not self.listen_only: # send our first FC frame load = self.ea_hdr load += struct.pack("BBB", N_PCI_FC, self.rxfc_bs, self.rxfc_stmin) self.can_send(load) # wait for a CF self.rx_bs = 0 self.rx_timeout_handle = TimeoutScheduler.schedule( self.cf_timeout, self._rx_timer_handler) return 0 def _recv_cf(self, data): """Process a received 'Consecutive Frame' frame""" if self.rx_state != ISOTP_WAIT_DATA: return 0 if self.rx_timeout_handle is not None: self.rx_timeout_handle.cancel() self.rx_timeout_handle = None # CFs are never longer than the FF if len(data) > self.rx_ll_dl: return 1 # CFs have usually the LL_DL length if len(data) < self.rx_ll_dl: # this is only allowed for the last CF if self.rx_len - self.rx_idx > self.rx_ll_dl: warning("Received a CF with insuffifient length") return 1 if six.indexbytes(data, 0) & 0x0f != self.rx_sn: # Wrong sequence number warning("RX state was reset because wrong sequence number was " "received") self.rx_state = ISOTP_IDLE return 1 self.rx_sn = (self.rx_sn + 1) % 16 self.rx_buf += data[1:] self.rx_idx = len(self.rx_buf) if self.rx_idx >= self.rx_len: # we are done self.rx_buf = self.rx_buf[0:self.rx_len] self.rx_state = ISOTP_IDLE self.rx_queue.put(self.rx_buf) for cb in self.rx_callbacks: cb(self.rx_buf) self.call_release() self.rx_buf = None return 0 # perform blocksize handling, if enabled if self.rxfc_bs != 0: self.rx_bs += 1 # check if we reached the end of the block if self.rx_bs >= self.rxfc_bs and not self.listen_only: # send our FC frame load = self.ea_hdr load += struct.pack("BBB", N_PCI_FC, self.rxfc_bs, self.rxfc_stmin) self.can_send(load) # wait for another CF self.rx_timeout_handle = TimeoutScheduler.schedule( self.cf_timeout, self._rx_timer_handler) return 0 def begin_send(self, x): """Begins sending an ISOTP message. This method does not block.""" with self.tx_mutex: if self.tx_state != ISOTP_IDLE: raise Scapy_Exception("Socket is already sending, retry later") self.tx_done.clear() self.tx_exception = None self.tx_state = ISOTP_SENDING length = len(x) if length > ISOTP_MAX_DLEN_2015: raise Scapy_Exception("Too much data for ISOTP message") if len(self.ea_hdr) + length <= 7: # send a single frame data = self.ea_hdr data += struct.pack("B", length) data += x self.tx_state = ISOTP_IDLE self.can_send(data) self.tx_done.set() for cb in self.tx_callbacks: cb() return # send the first frame data = self.ea_hdr if length > ISOTP_MAX_DLEN: data += struct.pack(">HI", 0x1000, length) else: data += struct.pack(">H", 0x1000 | length) load = x[0:8 - len(data)] data += load self.can_send(data) self.tx_buf = x self.tx_sn = 1 self.tx_bs = 0 self.tx_idx = len(load) self.tx_state = ISOTP_WAIT_FIRST_FC self.tx_timeout_handle = TimeoutScheduler.schedule( self.fc_timeout, self._tx_timer_handler) def send(self, p): """Send an ISOTP frame and block until the message is sent or an error happens.""" with self.send_mutex: self.begin_send(p) # Wait until the tx callback is called send_done = self.tx_done.wait(30) if self.tx_exception is not None: raise Scapy_Exception(self.tx_exception) if not send_done: raise Scapy_Exception("ISOTP send not completed in 30s") return def recv(self, timeout=None): """Receive an ISOTP frame, blocking if none is available in the buffer for at most 'timeout' seconds.""" try: return self.rx_queue.get(timeout is None or timeout > 0, timeout) except queue.Empty: return None def check_recv(self): """Implementation for SelectableObject""" return not self.rx_queue.empty() if six.PY3 and LINUX: from scapy.arch.linux import get_last_packet_timestamp, SIOCGIFINDEX """ISOTPNativeSocket definitions:""" CAN_ISOTP = 6 # ISO 15765-2 Transport Protocol SOL_CAN_BASE = 100 # from can.h SOL_CAN_ISOTP = SOL_CAN_BASE + CAN_ISOTP # /* for socket options affecting the socket (not the global system) */ CAN_ISOTP_OPTS = 1 # /* pass struct can_isotp_options */ CAN_ISOTP_RECV_FC = 2 # /* pass struct can_isotp_fc_options */ # /* sockopts to force stmin timer values for protocol regression tests */ CAN_ISOTP_TX_STMIN = 3 # /* pass __u32 value in nano secs */ CAN_ISOTP_RX_STMIN = 4 # /* pass __u32 value in nano secs */ CAN_ISOTP_LL_OPTS = 5 # /* pass struct can_isotp_ll_options */ CAN_ISOTP_LISTEN_MODE = 0x001 # /* listen only (do not send FC) */ CAN_ISOTP_EXTEND_ADDR = 0x002 # /* enable extended addressing */ CAN_ISOTP_TX_PADDING = 0x004 # /* enable CAN frame padding tx path */ CAN_ISOTP_RX_PADDING = 0x008 # /* enable CAN frame padding rx path */ CAN_ISOTP_CHK_PAD_LEN = 0x010 # /* check received CAN frame padding */ CAN_ISOTP_CHK_PAD_DATA = 0x020 # /* check received CAN frame padding */ CAN_ISOTP_HALF_DUPLEX = 0x040 # /* half duplex error state handling */ CAN_ISOTP_FORCE_TXSTMIN = 0x080 # /* ignore stmin from received FC */ CAN_ISOTP_FORCE_RXSTMIN = 0x100 # /* ignore CFs depending on rx stmin */ CAN_ISOTP_RX_EXT_ADDR = 0x200 # /* different rx extended addressing */ # /* default values */ CAN_ISOTP_DEFAULT_FLAGS = 0 CAN_ISOTP_DEFAULT_EXT_ADDRESS = 0x00 CAN_ISOTP_DEFAULT_PAD_CONTENT = 0xCC # /* prevent bit-stuffing */ CAN_ISOTP_DEFAULT_FRAME_TXTIME = 0 CAN_ISOTP_DEFAULT_RECV_BS = 0 CAN_ISOTP_DEFAULT_RECV_STMIN = 0x00 CAN_ISOTP_DEFAULT_RECV_WFTMAX = 0 CAN_ISOTP_DEFAULT_LL_MTU = CAN_MTU CAN_ISOTP_DEFAULT_LL_TX_DL = CAN_MAX_DLEN CAN_ISOTP_DEFAULT_LL_TX_FLAGS = 0 class SOCKADDR(ctypes.Structure): # See /usr/include/i386-linux-gnu/bits/socket.h for original struct _fields_ = [("sa_family", ctypes.c_uint16), ("sa_data", ctypes.c_char * 14)] class TP(ctypes.Structure): # This struct is only used within the SOCKADDR_CAN struct _fields_ = [("rx_id", ctypes.c_uint32), ("tx_id", ctypes.c_uint32)] class ADDR_INFO(ctypes.Union): # This struct is only used within the SOCKADDR_CAN struct # This union is to future proof for future can address information _fields_ = [("tp", TP)] class SOCKADDR_CAN(ctypes.Structure): # See /usr/include/linux/can.h for original struct _fields_ = [("can_family", ctypes.c_uint16), ("can_ifindex", ctypes.c_int), ("can_addr", ADDR_INFO)] class IFREQ(ctypes.Structure): # The two fields in this struct were originally unions. # See /usr/include/net/if.h for original struct _fields_ = [("ifr_name", ctypes.c_char * 16), ("ifr_ifindex", ctypes.c_int)] class ISOTPNativeSocket(SuperSocket): desc = "read/write packets at a given CAN interface using CAN_ISOTP " \ "socket " can_isotp_options_fmt = "@2I4B" can_isotp_fc_options_fmt = "@3B" can_isotp_ll_options_fmt = "@3B" sockaddr_can_fmt = "@H3I" auxdata_available = True def __build_can_isotp_options( self, flags=CAN_ISOTP_DEFAULT_FLAGS, frame_txtime=0, ext_address=CAN_ISOTP_DEFAULT_EXT_ADDRESS, txpad_content=0, rxpad_content=0, rx_ext_address=CAN_ISOTP_DEFAULT_EXT_ADDRESS): return struct.pack(self.can_isotp_options_fmt, flags, frame_txtime, ext_address, txpad_content, rxpad_content, rx_ext_address) # == Must use native not standard types for packing == # struct can_isotp_options { # __u32 flags; /* set flags for isotp behaviour. */ # /* __u32 value : flags see below */ # # __u32 frame_txtime; /* frame transmission time (N_As/N_Ar) */ # /* __u32 value : time in nano secs */ # # __u8 ext_address; /* set address for extended addressing */ # /* __u8 value : extended address */ # # __u8 txpad_content; /* set content of padding byte (tx) */ # /* __u8 value : content on tx path */ # # __u8 rxpad_content; /* set content of padding byte (rx) */ # /* __u8 value : content on rx path */ # # __u8 rx_ext_address; /* set address for extended addressing */ # /* __u8 value : extended address (rx) */ # }; def __build_can_isotp_fc_options(self, bs=CAN_ISOTP_DEFAULT_RECV_BS, stmin=CAN_ISOTP_DEFAULT_RECV_STMIN, wftmax=CAN_ISOTP_DEFAULT_RECV_WFTMAX): return struct.pack(self.can_isotp_fc_options_fmt, bs, stmin, wftmax) # == Must use native not standard types for packing == # struct can_isotp_fc_options { # # __u8 bs; /* blocksize provided in FC frame */ # /* __u8 value : blocksize. 0 = off */ # # __u8 stmin; /* separation time provided in FC frame */ # /* __u8 value : */ # /* 0x00 - 0x7F : 0 - 127 ms */ # /* 0x80 - 0xF0 : reserved */ # /* 0xF1 - 0xF9 : 100 us - 900 us */ # /* 0xFA - 0xFF : reserved */ # # __u8 wftmax; /* max. number of wait frame transmiss. */ # /* __u8 value : 0 = omit FC N_PDU WT */ # }; def __build_can_isotp_ll_options(self, mtu=CAN_ISOTP_DEFAULT_LL_MTU, tx_dl=CAN_ISOTP_DEFAULT_LL_TX_DL, tx_flags=CAN_ISOTP_DEFAULT_LL_TX_FLAGS ): return struct.pack(self.can_isotp_ll_options_fmt, mtu, tx_dl, tx_flags) # == Must use native not standard types for packing == # struct can_isotp_ll_options { # # __u8 mtu; /* generated & accepted CAN frame type */ # /* __u8 value : */ # /* CAN_MTU (16) -> standard CAN 2.0 */ # /* CANFD_MTU (72) -> CAN FD frame */ # # __u8 tx_dl; /* tx link layer data length in bytes */ # /* (configured maximum payload length) */ # /* __u8 value : 8,12,16,20,24,32,48,64 */ # /* => rx path supports all LL_DL values */ # # __u8 tx_flags; /* set into struct canfd_frame.flags */ # /* at frame creation: e.g. CANFD_BRS */ # /* Obsolete when the BRS flag is fixed */ # /* by the CAN netdriver configuration */ # }; def __get_sock_ifreq(self, sock, iface): socket_id = ctypes.c_int(sock.fileno()) ifr = IFREQ() ifr.ifr_name = iface.encode('ascii') ret = LIBC.ioctl(socket_id, SIOCGIFINDEX, ctypes.byref(ifr)) if ret < 0: m = u'Failure while getting "{}" interface index.'.format( iface) raise Scapy_Exception(m) return ifr def __bind_socket(self, sock, iface, sid, did): socket_id = ctypes.c_int(sock.fileno()) ifr = self.__get_sock_ifreq(sock, iface) if sid > 0x7ff: sid = sid | socket.CAN_EFF_FLAG if did > 0x7ff: did = did | socket.CAN_EFF_FLAG # select the CAN interface and bind the socket to it addr = SOCKADDR_CAN(ctypes.c_uint16(socket.PF_CAN), ifr.ifr_ifindex, ADDR_INFO(TP(ctypes.c_uint32(did), ctypes.c_uint32(sid)))) error = LIBC.bind(socket_id, ctypes.byref(addr), ctypes.sizeof(addr)) if error < 0: warning("Couldn't bind socket") def __set_option_flags(self, sock, extended_addr=None, extended_rx_addr=None, listen_only=False, padding=False, transmit_time=100): option_flags = CAN_ISOTP_DEFAULT_FLAGS if extended_addr is not None: option_flags = option_flags | CAN_ISOTP_EXTEND_ADDR else: extended_addr = CAN_ISOTP_DEFAULT_EXT_ADDRESS if extended_rx_addr is not None: option_flags = option_flags | CAN_ISOTP_RX_EXT_ADDR else: extended_rx_addr = CAN_ISOTP_DEFAULT_EXT_ADDRESS if listen_only: option_flags = option_flags | CAN_ISOTP_LISTEN_MODE if padding: option_flags = option_flags | CAN_ISOTP_TX_PADDING \ | CAN_ISOTP_RX_PADDING sock.setsockopt(SOL_CAN_ISOTP, CAN_ISOTP_OPTS, self.__build_can_isotp_options( frame_txtime=transmit_time, flags=option_flags, ext_address=extended_addr, rx_ext_address=extended_rx_addr)) def __init__(self, iface=None, sid=0, did=0, extended_addr=None, extended_rx_addr=None, listen_only=False, padding=False, transmit_time=100, basecls=ISOTP): if not isinstance(iface, six.string_types): if hasattr(iface, "ins") and hasattr(iface.ins, "getsockname"): iface = iface.ins.getsockname() if isinstance(iface, tuple): iface = iface[0] else: raise Scapy_Exception("Provide a string or a CANSocket " "object as iface parameter") self.iface = iface or conf.contribs['NativeCANSocket']['iface'] self.can_socket = socket.socket(socket.PF_CAN, socket.SOCK_DGRAM, CAN_ISOTP) self.__set_option_flags(self.can_socket, extended_addr, extended_rx_addr, listen_only, padding, transmit_time) self.src = sid self.dst = did self.exsrc = extended_addr self.exdst = extended_rx_addr self.can_socket.setsockopt(SOL_CAN_ISOTP, CAN_ISOTP_RECV_FC, self.__build_can_isotp_fc_options()) self.can_socket.setsockopt(SOL_CAN_ISOTP, CAN_ISOTP_LL_OPTS, self.__build_can_isotp_ll_options()) self.can_socket.setsockopt( socket.SOL_SOCKET, SO_TIMESTAMPNS, 1 ) self.__bind_socket(self.can_socket, self.iface, sid, did) self.ins = self.can_socket self.outs = self.can_socket if basecls is None: warning('Provide a basecls ') self.basecls = basecls def recv_raw(self, x=0xffff): """ Receives a packet, then returns a tuple containing (cls, pkt_data, time) """ # noqa: E501 try: pkt, _, ts = self._recv_raw(self.ins, x) except BlockingIOError: # noqa: F821 warning('Captured no data, socket in non-blocking mode.') return None except socket.timeout: warning('Captured no data, socket read timed out.') return None except OSError: # something bad happened (e.g. the interface went down) warning("Captured no data.") return None if ts is None: ts = get_last_packet_timestamp(self.ins) return self.basecls, pkt, ts def recv(self, x=0xffff): msg = SuperSocket.recv(self, x) if hasattr(msg, "src"): msg.src = self.src if hasattr(msg, "dst"): msg.dst = self.dst if hasattr(msg, "exsrc"): msg.exsrc = self.exsrc if hasattr(msg, "exdst"): msg.exdst = self.exdst return msg __all__.append("ISOTPNativeSocket") if USE_CAN_ISOTP_KERNEL_MODULE: ISOTPSocket = ISOTPNativeSocket # ################################################################### # #################### ISOTPSCAN #################################### # ################################################################### def send_multiple_ext(sock, ext_id, packet, number_of_packets): """ Send multiple packets with extended addresses at once Args: sock: socket for can interface ext_id: extended id. First id to send. packet: packet to send number_of_packets: number of packets send This function is used for scanning with extended addresses. It sends multiple packets at once. The number of packets is defined in the number_of_packets variable. It only iterates the extended ID, NOT the actual ID of the packet. This method is used in extended scan function. """ end_id = min(ext_id + number_of_packets, 255) for i in range(ext_id, end_id + 1): packet.extended_address = i sock.send(packet) def get_isotp_packet(identifier=0x0, extended=False, extended_can_id=False): """ Craft ISO TP packet Args: identifier: identifier of crafted packet extended: boolean if packet uses extended address extended_can_id: boolean if CAN should use extended Ids """ if extended: pkt = ISOTPHeaderEA() / ISOTP_FF() pkt.extended_address = 0 pkt.data = b'\x00\x00\x00\x00\x00' else: pkt = ISOTPHeader() / ISOTP_FF() pkt.data = b'\x00\x00\x00\x00\x00\x00' if extended_can_id: pkt.flags = "extended" pkt.identifier = identifier pkt.message_size = 100 return pkt def filter_periodic_packets(packet_dict, verbose=False): """ Filter for periodic packets Args: packet_dict: Dictionary with Send-to-ID as key and a tuple (received packet, Recv_ID) verbose: Displays further information ISOTP-Filter for periodic packets (same ID, always same timegap) Deletes periodic packets in packet_dict """ filter_dict = {} for key, value in packet_dict.items(): pkt = value[0] idn = value[1] if idn not in filter_dict: filter_dict[idn] = ([key], [pkt]) else: key_lst, pkt_lst = filter_dict[idn] filter_dict[idn] = (key_lst + [key], pkt_lst + [pkt]) for idn in filter_dict: key_lst = filter_dict[idn][0] pkt_lst = filter_dict[idn][1] if len(pkt_lst) < 3: continue tg = [p1.time - p2.time for p1, p2 in zip(pkt_lst[1:], pkt_lst[:-1])] if all(abs(t1 - t2) < 0.001 for t1, t2 in zip(tg[1:], tg[:-1])): if verbose: print("[i] Identifier 0x%03x seems to be periodic. " "Filtered.") for k in key_lst: del packet_dict[k] def get_isotp_fc(id_value, id_list, noise_ids, extended, packet, verbose=False): """Callback for sniff function when packet received Args: id_value: packet id of send packet id_list: list of received IDs noise_ids: list of packet IDs which will not be considered when received during scan extended: boolean if extended scan packet: received packet verbose: displays information during scan If received packet is a FlowControl and not in noise_ids append it to id_list """ if packet.flags and packet.flags != "extended": return if noise_ids is not None and packet.identifier in noise_ids: return try: index = 1 if extended else 0 isotp_pci = orb(packet.data[index]) >> 4 isotp_fc = orb(packet.data[index]) & 0x0f if isotp_pci == 3 and 0 <= isotp_fc <= 2: if verbose: print("[+] Found flow-control frame from identifier 0x%03x" " when testing identifier 0x%03x" % (packet.identifier, id_value)) if isinstance(id_list, dict): id_list[id_value] = (packet, packet.identifier) elif isinstance(id_list, list): id_list.append(id_value) else: raise TypeError("Unknown type of id_list") else: noise_ids.append(packet.identifier) except Exception as e: print("[!] Unknown message Exception: %s on packet: %s" % (e, repr(packet))) def scan(sock, scan_range=range(0x800), noise_ids=None, sniff_time=0.1, extended_can_id=False, verbose=False): """Scan and return dictionary of detections Args: sock: socket for can interface scan_range: hexadecimal range of IDs to scan. Default is 0x0 - 0x7ff noise_ids: list of packet IDs which will not be considered when received during scan sniff_time: time the scan waits for isotp flow control responses after sending a first frame extended_can_id: Send extended can frames verbose: displays information during scan ISOTP-Scan - NO extended IDs found_packets = Dictionary with Send-to-ID as key and a tuple (received packet, Recv_ID) """ return_values = dict() for value in scan_range: sock.sniff(prn=lambda pkt: get_isotp_fc(value, return_values, noise_ids, False, pkt, verbose), timeout=sniff_time, started_callback=lambda: sock.send( get_isotp_packet(value, False, extended_can_id))) cleaned_ret_val = dict() for tested_id in return_values.keys(): for value in range(max(0, tested_id - 2), tested_id + 2, 1): sock.sniff(prn=lambda pkt: get_isotp_fc(value, cleaned_ret_val, noise_ids, False, pkt, verbose), timeout=sniff_time * 10, started_callback=lambda: sock.send( get_isotp_packet(value, False, extended_can_id))) return cleaned_ret_val def scan_extended(sock, scan_range=range(0x800), scan_block_size=32, extended_scan_range=range(0x100), noise_ids=None, sniff_time=0.1, extended_can_id=False, verbose=False): """Scan with ISOTP extended addresses and return dictionary of detections Args: sock: socket for can interface scan_range: hexadecimal range of IDs to scan. Default is 0x0 - 0x7ff scan_block_size: count of packets send at once extended_scan_range: range to search for extended ISOTP addresses noise_ids: list of packet IDs which will not be considered when received during scan sniff_time: time the scan waits for isotp flow control responses after sending a first frame extended_can_id: Send extended can frames verbose: displays information during scan If an answer-packet found -> slow scan with single packages with extended ID 0 - 255 found_packets = Dictionary with Send-to-ID as key and a tuple (received packet, Recv_ID) """ return_values = dict() scan_block_size = scan_block_size or 1 for value in scan_range: pkt = get_isotp_packet(value, extended=True, extended_can_id=extended_can_id) id_list = [] r = list(extended_scan_range) for ext_isotp_id in range(r[0], r[-1], scan_block_size): sock.sniff(prn=lambda p: get_isotp_fc(ext_isotp_id, id_list, noise_ids, True, p, verbose), timeout=sniff_time * 3, started_callback=lambda: send_multiple_ext( sock, ext_isotp_id, pkt, scan_block_size)) # sleep to prevent flooding time.sleep(sniff_time) # remove duplicate IDs id_list = list(set(id_list)) for ext_isotp_id in id_list: for ext_id in range(max(ext_isotp_id - 2, 0), min(ext_isotp_id + scan_block_size + 2, 256)): pkt.extended_address = ext_id full_id = (value << 8) + ext_id sock.sniff(prn=lambda pkt: get_isotp_fc(full_id, return_values, noise_ids, True, pkt, verbose), timeout=sniff_time * 2, started_callback=lambda: sock.send(pkt)) return return_values def ISOTPScan(sock, scan_range=range(0x7ff + 1), extended_addressing=False, extended_scan_range=range(0x100), noise_listen_time=2, sniff_time=0.1, output_format=None, can_interface=None, extended_can_id=False, verbose=False): """Scan for ISOTP Sockets on a bus and return findings Args: sock: CANSocket object to communicate with the bus under scan scan_range: hexadecimal range of CAN-Identifiers to scan. Default is 0x0 - 0x7ff extended_addressing: scan with ISOTP extended addressing extended_scan_range: range for ISOTP extended addressing values noise_listen_time: seconds to listen for default communication on the bus sniff_time: time the scan waits for isotp flow control responses after sending a first frame output_format: defines the format of the returned results (text, code or sockets). Provide a string e.g. "text". Default is "socket". can_interface: interface used to create the returned code/sockets extended_can_id: Use Extended CAN-Frames verbose: displays information during scan Scan for ISOTP Sockets in the defined range and returns found sockets in a specified format. The format can be: - text: human readable output - code: python code for copy&paste - sockets: if output format is not specified, ISOTPSockets will be created and returned in a list """ if verbose: print("Filtering background noise...") # Send dummy packet. In most cases, this triggers activity on the bus. dummy_pkt = CAN(identifier=0x123, data=b'\xaa\xbb\xcc\xdd\xee\xff\xaa\xbb') background_pkts = sock.sniff(timeout=noise_listen_time, started_callback=lambda: sock.send(dummy_pkt)) noise_ids = list(set(pkt.identifier for pkt in background_pkts)) if extended_addressing: found_packets = scan_extended(sock, scan_range, extended_scan_range=extended_scan_range, noise_ids=noise_ids, sniff_time=sniff_time, extended_can_id=extended_can_id, verbose=verbose) else: found_packets = scan(sock, scan_range, noise_ids=noise_ids, sniff_time=sniff_time, extended_can_id=extended_can_id, verbose=verbose) filter_periodic_packets(found_packets, verbose) if output_format == "text": return generate_text_output(found_packets, extended_addressing) if output_format == "code": return generate_code_output(found_packets, can_interface, extended_addressing) if can_interface is None: can_interface = sock return generate_isotp_list(found_packets, can_interface, extended_addressing) def generate_text_output(found_packets, extended_addressing=False): """Generate a human readable output from the result of the `scan` or the `scan_extended` function. Args: found_packets: result of the `scan` or `scan_extended` function extended_addressing: print results from a scan with ISOTP extended addressing """ if not found_packets: return "No packets found." text = "\nFound %s ISOTP-FlowControl Packet(s):" % len(found_packets) for pack in found_packets: if extended_addressing: send_id = pack // 256 send_ext = pack - (send_id * 256) ext_id = hex(orb(found_packets[pack][0].data[0])) text += "\nSend to ID: %s" \ "\nSend to extended ID: %s" \ "\nReceived ID: %s" \ "\nReceived extended ID: %s" \ "\nMessage: %s" % \ (hex(send_id), hex(send_ext), hex(found_packets[pack][0].identifier), ext_id, repr(found_packets[pack][0])) else: text += "\nSend to ID: %s" \ "\nReceived ID: %s" \ "\nMessage: %s" % \ (hex(pack), hex(found_packets[pack][0].identifier), repr(found_packets[pack][0])) padding = found_packets[pack][0].length == 8 if padding: text += "\nPadding enabled" else: text += "\nNo Padding" text += "\n" return text def generate_code_output(found_packets, can_interface, extended_addressing=False): """Generate a copy&past-able output from the result of the `scan` or the `scan_extended` function. Args: found_packets: result of the `scan` or `scan_extended` function can_interface: description string for a CAN interface to be used for the creation of the output. extended_addressing: print results from a scan with ISOTP extended addressing """ result = "" if not found_packets: return result header = "\n\nimport can\n" \ "conf.contribs['CANSocket'] = {'use-python-can': %s}\n" \ "load_contrib('cansocket')\n" \ "load_contrib('isotp')\n\n" % PYTHON_CAN for pack in found_packets: if extended_addressing: send_id = pack // 256 send_ext = pack - (send_id * 256) ext_id = orb(found_packets[pack][0].data[0]) result += "ISOTPSocket(%s, sid=0x%x, did=0x%x, padding=%s, " \ "extended_addr=0x%x, extended_rx_addr=0x%x, " \ "basecls=ISOTP)\n" % \ (can_interface, send_id, int(found_packets[pack][0].identifier), found_packets[pack][0].length == 8, send_ext, ext_id) else: result += "ISOTPSocket(%s, sid=0x%x, did=0x%x, padding=%s, " \ "basecls=ISOTP)\n" % \ (can_interface, pack, int(found_packets[pack][0].identifier), found_packets[pack][0].length == 8) return header + result def generate_isotp_list(found_packets, can_interface, extended_addressing=False): """Generate a list of ISOTPSocket objects from the result of the `scan` or the `scan_extended` function. Args: found_packets: result of the `scan` or `scan_extended` function can_interface: description string for a CAN interface to be used for the creation of the output. extended_addressing: print results from a scan with ISOTP extended addressing """ socket_list = [] for pack in found_packets: pkt = found_packets[pack][0] dest_id = pkt.identifier pad = True if pkt.length == 8 else False if extended_addressing: source_id = pack >> 8 source_ext = int(pack - (source_id * 256)) dest_ext = orb(pkt.data[0]) socket_list.append(ISOTPSocket(can_interface, sid=source_id, extended_addr=source_ext, did=dest_id, extended_rx_addr=dest_ext, padding=pad, basecls=ISOTP)) else: source_id = pack socket_list.append(ISOTPSocket(can_interface, sid=source_id, did=dest_id, padding=pad, basecls=ISOTP)) return socket_list