def on_recv(self, cf):
# type: (Packet) -> None
"""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.rx_ext_address is not None:
ae = 1
if len(data) < 3:
return
if six.indexbytes(data, 0) != self.rx_ext_address:
return
n_pci = six.indexbytes(data, ae) & 0xf0
if n_pci == N_PCI_FC:
self._recv_fc(data[ae:])
elif n_pci == N_PCI_SF:
self._recv_sf(data[ae:], cf.time)
elif n_pci == N_PCI_FF:
self._recv_ff(data[ae:], cf.time)
elif n_pci == N_PCI_CF:
self._recv_cf(data[ae:])
def on_recv(self, cf):
"""Function that must be called every time a CAN frame is received, to
advance the state machine."""
self.mutex.acquire()
try:
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:
self._recv_fc(data[ae:])
elif n_pci == N_PCI_SF:
if len(cf.data) > 8:
raise Scapy_Exception("CANFD not implemented")
self._recv_sf(data[ae:])
elif n_pci == N_PCI_FF:
self._recv_ff(data[ae:])
elif n_pci == N_PCI_CF:
self._recv_cf(data[ae:])
finally:
self.mutex.release()
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.ready))
return True
def _try_feed(self, identifier, ea, data):
first_byte = six.indexbytes(data, 0)
if len(data) > 1 and first_byte & 0xf0 == N_PCI_SF:
self._feed_single_frame(identifier, ea, data)
if len(data) > 2 and first_byte & 0xf0 == N_PCI_FF:
self._feed_first_frame(identifier, ea, data)
if len(data) > 1 and first_byte & 0xf0 == N_PCI_CF:
self._feed_consecutive_frame(identifier, ea, data)
def _recv_fc(self, data):
# type: (bytes) -> None
"""Process a received 'Flow Control' frame"""
if (self.tx_state != ISOTP_WAIT_FC and
self.tx_state != ISOTP_WAIT_FIRST_FC):
return
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)
def _recv_cf(self, data):
"""Process a received 'Consecutive Frame' frame"""
if self.rx_state != ISOTP_WAIT_DATA:
return 0
self.rx_timer.cancel()
# 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
if self.rx_callback:
self.rx_callback(None)
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_state = ISOTP_IDLE
self.rx_messages.append(self.rx_buf)
if self.rx_callback:
self.rx_callback(self.rx_buf)
self.rx_buf = None
return 0
# no creation of flow control frames
if self.listen_mode:
return 0
# perform blocksize handling, if enabled
if self.rxfc_bs != 0:
self.rx_bs += 1
if self.rxfc_bs == 0 or self.rx_bs < self.rxfc_bs:
self.rx_timer.start(1)
return 0
# we reached the specified blocksize self.rxfc_bs
# send our FC frame
load = self.ea_hdr
load += struct.pack("BBB", N_PCI_FC, self.rxfc_bs, self.rxfc_stmin)
self.sendfunc(load)
return 0
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
self.tx_timer.cancel()
if len(data) < 3:
warning("CF frame discarded because it was too short")
self.tx_state = ISOTP_IDLE
if self.tx_callback:
self.tx_callback(None)
return 1
# 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_timer.start(self.tx_gap)
elif isotp_fc == ISOTP_FC_WT:
# start timer to wait for next FC frame
self.tx_state = ISOTP_WAIT_FC
self.tx_timer.start(1)
elif isotp_fc == ISOTP_FC_OVFLW:
# overflow in receiver side
warning("Overflow happened at the receiver side")
self.tx_state = ISOTP_IDLE
if self.tx_callback:
self.tx_callback(None)
else:
warning("Unknown CF frame type")
self.tx_state = ISOTP_IDLE
if self.tx_callback:
self.tx_callback(None)
return 0
def feed(self, can):
"""Attempt to feed an incoming CAN frame into the state machine"""
assert (isinstance(can, CAN))
identifier = can.identifier
data = bytes(can.data)
if len(data) > 1 and self.use_ext_addr is not True:
self._try_feed(identifier, None, data)
if len(data) > 2 and self.use_ext_addr is not False:
ea = six.indexbytes(data, 0)
self._try_feed(identifier, ea, data[1:])
def _try_feed(self, identifier, ea, data, ts):
# type: (int, Optional[int], bytes, Union[EDecimal, float]) -> None
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)
def _feed_single_frame(self, identifier, ea, data):
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, isotp_data))
return True
def _feed_single_frame(self, identifier, ea, data, ts):
# type: (int, Optional[int], bytes, Union[EDecimal, float]) -> bool
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 _recv_ff(self, data, ts):
# type: (bytes, Union[float, EDecimal]) -> None
"""Process a received 'First Frame' frame"""
log_runtime.debug("Processing FF")
if self.rx_timeout_handle is not None:
self.rx_timeout_handle.cancel()
self.rx_timeout_handle = None
if self.rx_state != ISOTP_IDLE:
if conf.verb > 2:
warning("RX state was reset because first frame was received")
self.rx_state = ISOTP_IDLE
if len(data) < 7:
return
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
self.rx_ts = ts
# 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)
def _recv_sf(self, data):
"""Process a received 'Single Frame' frame"""
self.rx_timer.cancel()
if self.rx_state != ISOTP_IDLE:
warning("RX state was reset because single frame was received")
self.rx_state = ISOTP_IDLE
if self.rx_callback:
self.rx_callback(None)
length = six.indexbytes(data, 0) & 0xf
if len(data) - 1 < length:
return 1
msg = data[1:1 + length]
assert (len(msg) == length)
self.rx_messages.append(msg)
if self.rx_callback:
self.rx_callback(msg)
return 0
def _recv_sf(self, data, ts):
# type: (bytes, Union[float, EDecimal]) -> None
"""Process a received 'Single Frame' frame"""
log_runtime.debug("Processing SF")
if self.rx_timeout_handle is not None:
self.rx_timeout_handle.cancel()
self.rx_timeout_handle = None
if self.rx_state != ISOTP_IDLE:
if conf.verb > 2:
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
msg = data[1:1 + length]
self.rx_queue.send((msg, ts))
def _recv_ff(self, data):
"""Process a received 'First Frame' frame"""
self.rx_timer.cancel()
if self.rx_state != ISOTP_IDLE:
warning("RX state was reset because first frame was received")
self.rx_state = ISOTP_IDLE
if self.rx_callback:
self.rx_callback(None)
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 self.listen_mode:
return 0
# send our first FC frame
load = self.ea_hdr
load += struct.pack("BBB", N_PCI_FC, self.rxfc_bs, self.rxfc_stmin)
self.sendfunc(load)
self.rx_bs = 0
self.rx_timer.start(1)
return 0
def feed(self, can):
# type: (Union[Iterable[Packet], Packet]) -> None
"""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
if not isinstance(can, Packet):
return
if self.dst_ids is not None and can.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(can.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(can.identifier, ea, data[1:], can.time)
def _recv_cf(self, data):
# type: (bytes) -> None
"""Process a received 'Consecutive Frame' frame"""
log_runtime.debug("Processing CF")
if self.rx_state != ISOTP_WAIT_DATA:
return
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
# 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:
if conf.verb > 2:
warning("Received a CF with insufficient length")
return
if six.indexbytes(data, 0) & 0x0f != self.rx_sn:
# Wrong sequence number
if conf.verb > 2:
warning("RX state was reset because wrong sequence number was "
"received")
self.rx_state = ISOTP_IDLE
return
if self.rx_buf is None:
if conf.verb > 2:
warning("rx_buf not filled with data!")
self.rx_state = ISOTP_IDLE
return
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.send((self.rx_buf, self.rx_ts))
self.rx_buf = None
return
# 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.rx_bs = 0
self.can_send(load)
# wait for another CF
log_runtime.debug("Wait for another CF")
self.rx_timeout_handle = TimeoutScheduler.schedule(
self.cf_timeout, self._rx_timer_handler)
