class MessageParser:
MIN_PACKET_SIZE_V1 = 16
MIN_PACKET_SIZE_V2 = 14
def __init__(self, sender, stream=False, loglevel='info'):
'''
:param AddressBook.Endpoint sender: the socket where the packed was received or the sender.
:param bool stream: if stream is `True` received data will be concatenated. In this case the message version byte will be checked only in first message.
'''
self._sender = sender
self._version_only_first = stream
self._stream = stream
name = sender
if name is not None:
name = sender.address.replace('.', '_')
self.logger = NMLogger('msg[%s]' % name, loglevel)
self._data = b''
self._version = None
def unpack(self, data, version=None):
if self._stream:
self._data += data
else:
self._data = data
msg_list = []
# self.logger.debug("decode received %dBytes" % len(data))
with_version_byte = False
# read version
while self._data:
# TODO: check for version for each packet in UDP and UDS, for TCP only on connect.
msg = Message()
msg.tinfo_src = self._sender
offset = 0
msg_endidx = 0
data_len = len(self._data)
pkg_vers = version
if self._version_only_first:
# in TCP connection only first message contains version byte. Use known version if it is already set in this parser.
pkg_vers = self._version
if pkg_vers is None:
# determine the message version
if IS_PYTHON_2:
pkg_vers = ord(self._data[0])
else:
pkg_vers = self._data[0]
offset = 1
with_version_byte = True
if self._version_only_first:
self._version = pkg_vers
# parse message depending on version
if pkg_vers == 1:
# check for valid message length
if data_len < self.MIN_PACKET_SIZE_V1 + offset:
self.logger.error(
"return, received data length is to small for message version 1"
)
return msg_list
msg.version = Message.AS5669
offset += 4
(flags, _msg_vers, msg.cmd_code, dst_id, src_id, data_flags,
msg.seqnr) = struct.unpack('<BBHIIHH',
self._data[offset:offset + 16])
msg.dst_id = JausAddress(dst_id)
msg.src_id = JausAddress(src_id)
prio = flags & 0b00001111
msg._priority = 3 if prio == 15 else int((prio - 3) / 3)
msg._acknak = (flags & 0b00110000) >> 4
msg._data_size = data_flags & 0b00001111111111111111
offset += 16
msg_endidx = offset + msg._data_size
msg.set_raw(self._data[:msg_endidx], offset, with_version_byte)
# read message id
if msg._data_size >= 2: # check message size before extract message id
(msg_id, ) = struct.unpack('<H',
self._data[offset:offset + 2])
msg._msg_id = int(msg_id)
msg_list.append(msg)
elif pkg_vers == 2:
# check for valid message length
if data_len < self.MIN_PACKET_SIZE_V2 + offset:
self.logger.error(
"return, received data length %d is to small for message version 2 (%d)"
% (data_len, self.MIN_PACKET_SIZE_V2 + offset))
return msg_list
msg.version = Message.AS5669A
(flags,
msg._data_size) = struct.unpack('<BH',
self._data[offset:offset + 3])
msg.message_type = flags & 0b00111111
hc_flags = flags & 0b11000000 >> 6
offset += 3
if hc_flags:
msg.hc_flags = hc_flags
offset += 2
# TODO: add support for header compression
(data_flags, dst_id,
src_id) = struct.unpack('<BII', self._data[offset:offset + 9])
msg.dst_id = JausAddress(dst_id)
msg.src_id = JausAddress(src_id)
# decode data flags
msg.priority = data_flags & 0b00000011
msg.bcast = (data_flags & 0b00001100) >> 2
msg.acknak = (data_flags & 0b00110000) >> 4
msg.data_flags = (data_flags & 0b11000000) >> 6
msg_endidx = msg._data_size + (
1 if with_version_byte else 0
) # 1 is a byte for message version (no version byte in TCP connections)
if data_len < msg_endidx:
# handling of short message
self.logger.error(
"return, received data %d is smaller than data length in header %d"
% (data_len, msg_endidx))
return msg_list
(msg.seqnr, ) = struct.unpack(
'<H', self._data[msg_endidx - 2:msg_endidx])
offset += 9
msg.set_raw(self._data[:msg_endidx], offset, with_version_byte)
# read message id
if offset + 2 <= msg_endidx: # check message size before extract message id
if msg.data_flags in [0, 1
]: # only on single or first packets
(msg_id, ) = struct.unpack(
'<H', self._data[offset:offset + 2])
msg._msg_id = int(msg_id)
msg_list.append(msg)
else:
return msg_list
# self.logger.debug("decoded %s" % msg)
# remove message bytes from data
if self._stream:
self._data = self._data[msg_endidx:]
else:
self._data = b''
return msg_list
class UDSServer(object):
def __init__(self, router, cfg, addrbook, statistics):
self._stop = False
self._cfg = cfg
self._addrbook = addrbook
self._statistics = statistics
loglevel = self._cfg.param('global/loglevel', 'info')
self.logger = NMLogger('uds_server', loglevel)
override_priority = cfg.param('priority/override', True)
ormap = cfg.param('priority/map', {})
self._priority_map = {}
if override_priority:
# create overide map
try:
for msg_id, prio in ormap.items():
try:
msgid = int(msg_id, 16)
self.logger.info("Override priority for 0x%.4X to %d" %
(msgid, prio))
if prio >= 0 and prio <= 3:
self._priority_map[msgid] = prio
else:
self.logger.warning(
"Ignored invalid priority for %s: %d" %
(msg_id, prio))
except ValueError as ve:
self.logger.warning(
"Ignored invalid message id %s: %s" % (msg_id, ve))
except Exception as err:
import traceback
print(traceback.format_exc())
self.logger.warning("Can not read priority override map: %s" %
err)
self._local_sockets = {}
self._recv_buffer = cfg.RECV_BUFFER
self._root_path = cfg.param('transport/local/root', '/tmp')
self._socket_path_server = os.path.join(
self._root_path, cfg.param('transport/local/nm_path'))
self.logger.info("Listen for local connections @%s" %
(self._socket_path_server))
if os.path.exists(self._socket_path_server):
os.unlink(self._socket_path_server)
self._local_socket = UDSSocket(cfg.param('transport/local/nm_path'),
remove_on_close=True,
force_bind=True,
root_path=self._root_path,
loglevel=self._cfg.param(
'global/loglevel', 'info'))
self._udp_looback = None
self._udp_looback_dest = None
if cfg.param('transport/loopback_debug/enable', False):
self._init_loopback()
# Listen for incoming connections
self._router = router
self._queue_send = queue.PQueue(cfg.param(
'transport/local/queue_length', 0),
'queue_uds_send',
loglevel=loglevel)
self._thread_send = threading.Thread(
target=self._loop_handle_send_queue)
self._thread_send.start()
self._thread_recv = threading.Thread(
target=self._loop_recv_local_socket)
self._thread_recv.start()
def _init_loopback(self):
# initialize loopback socket for debug mode
port = self._cfg.param('transport/loopback_debug/port', 55555)
use_mcast = self._cfg.param('transport/loopback_debug/use_mcast',
False)
address = self._cfg.param('transport/loopback_debug/address',
'169.255.0.100')
buffer_size = self._cfg.param('transport/loopback_debug/buffer_size',
0)
queue_length = self._cfg.param('transport/loopback_debug/queue_length',
0)
if not use_mcast:
if is_local_iface(address):
# create a receive socket to avoid ICMP messages with 'port unreachable'
self.logger.info(
"Loopback destination is local address, create receive socket "
)
self._udp_looback_dest = UDPucSocket(
interface=address,
port=port,
logger_name='loopback_recv',
send_buffer=buffer_size,
recv_buffer=self._recv_buffer,
queue_length=queue_length,
loglevel=self.logger.level())
self._udp_looback = UDPucSocket(interface=address,
logger_name='loopback',
default_dst=(address, port))
else:
self._udp_looback = UDPucSocket(port=port,
logger_name='loopback',
default_dst=(address, port),
send_buffer=buffer_size,
recv_buffer=self._recv_buffer,
queue_length=queue_length,
loglevel=self.logger.level())
else:
interface = self._cfg.param('transport/loopback_debug/interface',
'')
mgroup = self._cfg.param('transport/loopback_debug/group',
'239.255.0.1')
self._udp_looback = UDPmcSocket(port,
mgroup,
ttl=1,
interface=interface,
logger_name='loopback_mc',
send_buffer=buffer_size,
recv_buffer=self._recv_buffer,
queue_length=queue_length,
loglevel=self.logger.level())
def _close_loopback(self):
if self._udp_looback is not None:
self._udp_looback.close()
self._udp_looback = None
if self._udp_looback_dest is not None:
self._udp_looback_dest.close()
self._udp_looback_dest = None
def stop(self):
self._stop = True
self._local_socket.close()
for _key, sock in self._local_sockets.items():
sock.close()
self._local_sockets.clear()
# self._queue_recv_locals.clear()
self._queue_send.clear()
self._close_loopback()
def send_msg(self, msg):
failed = []
not_found = []
found = False
if msg.tinfo_dst is not None:
# found valid destination entry
found = True
self.logger.debug("Send to local socket %s" %
msg.tinfo_dst.address)
if msg.dst_id in self._local_sockets:
sock = self._local_sockets[msg.dst_id]
ok = sock.send_msg(msg)
if not ok:
failed.append(msg.dst_id)
else:
self.logger.debug("No socket for %s found!" %
msg.tinfo_dst.address)
else:
# the destination is None -> send as broadcast
for key, sock in self._local_sockets.items():
# do not send message to the socket received from
if key != msg.src_id:
if key.match(msg.dst_id):
found = True
self.logger.debug("forward message to %s" % (key))
ok = sock.send_msg(msg)
if not ok:
failed.append(msg.dst_id)
if not found and self._local_sockets:
self.logger.debug("No UDS destination found for: %s, seqnr: %d" %
(msg.dst_id, msg.seqnr))
not_found.append(msg.dst_id)
return failed, not_found
def send_queued(self, msg):
try:
self._queue_send.put(msg)
except queue.Full as full_error:
self.logger.warning("Error while put message into send queue: %s" %
full_error)
def _loop_recv_local_socket(self):
'''
Receive messages from all local connections in one thread to reduce thread count.
'''
while not self._stop:
try:
# we listen only to 'JuniorRTE' socket, other local sockets are used for send direction
msgs = self._local_socket.recv_msg()
for msg in msgs:
self._handle_msg(msg)
if self._udp_looback is not None:
self._udp_looback.send_queued(msg)
except Exception:
import traceback
self.logger.warning(traceback.format_exc())
def _handle_msg(self, msg):
try:
if msg is None:
return
if msg.dst_id.zero or msg.cmd_code > 0:
# handle connection requests/closing
try:
self._statistics.add(msg)
if msg.cmd_code == Message.CODE_CONNECT:
# Connection request from client.
self.logger.debug("Connection request from %s" %
msg.src_id)
resp = Message()
resp.version = Message.AS5669
resp.dst_id = msg.src_id
resp.cmd_code = Message.CODE_ACCEPT
dest_sock = self.create_local_socket(msg.src_id)
if dest_sock is not None:
dest_sock.send_msg(resp)
resp.ts_receive = time.time()
resp.tinfo_src = AddressBook.Endpoint(
AddressBook.Endpoint.UDS,
self._local_socket.socket_path)
if dest_sock is not None:
resp.tinfo_dst = AddressBook.Endpoint(
AddressBook.Endpoint.UDS,
dest_sock.socket_path)
self._statistics.add(resp)
elif msg.cmd_code == Message.CODE_CANCEL:
# Disconnect client.
self.logger.debug("Disconnect request from %s" %
msg.src_id)
self.remove_local_socket(msg.src_id)
except Exception as e:
import traceback
print(traceback.format_exc())
self.logger.warning(
"Error while handle connection management message: %s"
% e)
else:
# all other message put in priority queue
try:
# override priority
if self._priority_map:
try:
msg_id = int(msg.msg_id)
if msg_id in self._priority_map:
prio = self._priority_map[msg_id]
# self.logger.debug("Override priority for msg ID: 0x%x, current: %d, new: %d" % (msg_id, msg.priority, prio))
msg.priority = prio
except Exception as err:
import traceback
print(traceback.format_exc())
self.logger.warning(
"can not changed priority: %s" % (err))
self._router.route_local_msg(msg)
if msg.src_id not in self._local_sockets:
self.create_local_socket(msg.src_id)
except Exception as e:
import traceback
print(traceback.format_exc())
self.logger.warning(
"Error while put local message to global queue: %s" %
e)
except Exception as e:
import traceback
print(traceback.format_exc())
self.logger.warning("Error while get send item from queue: %s" % e)
# def send_loopback(self, msg):
# if self._udp_looback is not None:
# self._udp_looback.send_queued(msg)
# def handle_msg(self, msg):
# try:
# if msg is None:
# return
# if msg.dst_id.zero or msg.cmd_code > 0:
# # handle connection requests/closing
# try:
# self._statistics.add(msg)
# if msg.cmd_code == Message.CODE_CONNECT:
# # Connection request from client.
# self.logger.debug("Connection request from %s" % msg.src_id)
# resp = Message()
# resp.version = Message.AS5669
# resp.dst_id = msg.src_id
# resp.cmd_code = Message.CODE_ACCEPT
# dest_sock = self.create_local_socket(msg.src_id)
# if dest_sock is not None:
# dest_sock.send_msg(resp)
# resp.ts_receive = time.time()
# resp.tinfo_src = AddressBook.Endpoint(AddressBook.Endpoint.UDS, self._local_socket.socket_path)
# resp.tinfo_dst = AddressBook.Endpoint(AddressBook.Endpoint.UDS, dest_sock.socket_path)
# self._statistics.add(resp)
# elif msg.cmd_code == Message.CODE_CANCEL:
# # Disconnect client.
# self.logger.debug("Disconnect request from %s" % msg.src_id)
# self.remove_local_socket(msg.src_id)
# except Exception as e:
# print(traceback.format_exc())
# self.logger.warning("Error while handle connection management message: %s" % e)
# else:
# # all other message put in priority queue
# try:
# # override priority
# if self._priority_map:
# msg_id = int(msg.msg_id)
# try:
# if msg_id in self._priority_map:
# prio = self._priority_map[msg_id]
# # self.logger.debug("Override priority for msg ID: 0x%x, current: %d, new: %d" % (msg_id, msg.priority, prio))
# msg.priority = prio
# except KeyError:
# pass
# except Exception as err:
# import traceback
# print(traceback.format_exc())
# self.logger.warning("can not changed priority: %s" % (err))
# if msg.dst_id not in self._local_sockets:
# self.create_local_socket(msg.src_id)
# except Exception as e:
# print(traceback.format_exc())
# self.logger.warning("Error while put local message to global queue: %s" % e)
# except Exception as e:
# print(traceback.format_exc())
# self.logger.warning("Error while get send item from queue: %s" % e)
def _loop_handle_send_queue(self):
while not self._stop:
# send message from outside
try:
msg = self._queue_send.get()
if msg is None:
continue
try:
failed, _not_found = self.send_msg(msg)
if failed and msg.tinfo_dst is not None:
if msg.priority == 3:
# try again for critical messages
failed, _not_found = self.send_msg(msg)
# TODO: put it into send queue back?
# or retry
# this part is still for tests
# print("failed send seqnr: %d, %s" % (msg.seqnr, failed))
# failed, _not_found = self.send_msg(msg)
# if failed:
# failed, _not_found = self.send_msg(msg)
# if failed:
# print(" still failed, skip seqnr: %d, %s" % (msg.seqnr, failed))
pass
except Exception as e:
import traceback
print(traceback.format_exc())
self.logger.warning(
"Error while forward external message: %s" % e)
except Exception as e:
import traceback
print(traceback.format_exc())
self.logger.warning(
"Error while get send item from queue: %s" % e)
def create_local_socket(self, dst_id):
if self._stop:
return None
sock = None
if dst_id not in self._local_sockets:
try:
self.logger.debug("Create local socket connection to %s" %
dst_id)
sock = UDSSocket('%d' % dst_id.value,
root_path=self._root_path,
recv_buffer=self._recv_buffer,
loglevel=self._cfg.param(
'global/loglevel', 'info'))
self._local_sockets[dst_id] = sock
self._addrbook.add_jaus_address(
dst_id,
sock.socket_path,
port=None,
ep_type=AddressBook.Endpoint.UDS)
except Exception as connerr:
self.logger.error("Can't create local socket to %s: %s" %
(dst_id, connerr))
else:
sock = self._local_sockets[dst_id]
# reconnect to socket if new request was received
sock.reconnect()
return sock
def remove_local_socket(self, dst_id):
if self._stop:
return
if dst_id in self._local_sockets:
try:
self.logger.debug("Remove local socket connection to %s" %
dst_id)
sock = self._local_sockets[dst_id]
sock.close()
del self._local_sockets[dst_id]
# remove from address book
self._addrbook.remove(dst_id)
except Exception as connerr:
self.logger.error("Can't close local socket to %s: %s" %
(dst_id, connerr))
class TCPServer(socket.socket):
def __init__(self,
port=0,
router=None,
interface='',
logger_name='tcp',
recv_buffer=5000,
queue_length=0,
loglevel='info'):
'''
:param int port: the port to bind the socket. If zero an empty one will be used.
:param router: class which provides `route_tcp_msg(fkie_iop_node_manager.message.Message)` method. If `None` receive will be disabled.
:param str interface: The interface to bind to. If empty, it binds to all interfaces
'''
self._closed = False
self._lock = threading.RLock()
self.logger = NMLogger('%s[%s:%d]' % (logger_name, interface, port),
loglevel)
self.interface = interface
self.port = port
self._router = router
self._recv_buffer = recv_buffer
self._queue_length = queue_length
self._socket_type = socket.AF_INET
bind_ip = self.interface
if self.interface:
addrinfo = getaddrinfo(self.interface)
self._socket_type = addrinfo[0]
bind_ip = addrinfo[4][0]
socket.socket.__init__(self, self._socket_type, socket.SOCK_STREAM)
self._address = (bind_ip, self.port)
self._message_parser = {}
self._clients = {}
self._thread_bind = threading.Thread(target=self._bind_with_retry)
self._thread_bind.start()
def _bind_with_retry(self):
'''
Try to bind to the socket until it is available or node manager is stopped.
'''
ok = False
self.logger.info("+ Bind to @(%s:%s)" %
(self._address[0], self._address[1]))
while not ok and not self._closed:
try:
self.bind((self._address[0], self._address[1]))
self.listen(5)
# create a thread to handle the received unicast messages
self._thread_loop = threading.Thread(target=self._loop)
self._thread_loop.start()
ok = True
self.logger.info("server ready")
except Exception as err:
self.logger.error(
"TCP bind failed: %s, next try in 5 seconds..." % err)
time.sleep(5)
def close(self):
self._closed = True
self.logger.info("Close socket")
self.logger.debug("Close %s clients: %s" %
(len(self._clients), str(self._clients)))
for _dst, conn in self._clients.items():
conn.close()
try:
# Important: Close read direction
self.shutdown(socket.SHUT_RDWR)
except Exception:
self.logger.debug(traceback.format_exc())
socket.socket.close(self)
def send_queued(self, msg):
try:
if msg.tinfo_dst is not None:
dst = msg.tinfo_dst.address_tuple()
try:
self._clients[dst].send_queued(msg)
except KeyError:
if not is_local_iface(dst[0]):
tcp_client = TCPClient(dst[0],
port=dst[1],
router=self._router,
interface=self.interface,
recv_buffer=self._recv_buffer,
queue_length=self._queue_length,
loglevel=self.logger.level())
tcp_client.send_queued(msg)
self._clients[dst] = tcp_client
else:
# send to all destinations if no specified
for _dst, conn in self._clients.items():
conn.send_queued(msg)
except Exception as err:
self.logger.debug(traceback.format_exc())
self.logger.warning("Error while send message through TCP: %s" %
err)
def _loop(self):
while not self._closed:
try:
connection, client_address = self.accept()
self.logger.debug("Add new input connection from %s" %
str(client_address))
tcp_input = TCPInput(connection,
router=self._router,
recv_buffer=self._recv_buffer,
queue_length=self._queue_length,
close_callback=self._close_callback,
loglevel=self.logger.level())
with self._lock:
if client_address in self._clients:
self._clients[client_address].close()
self._clients[client_address] = tcp_input
except OSError:
pass
except socket.error as rerr:
if rerr.errno != 22:
self.logger.debug("Error in receive loop: %s" %
traceback.format_exc())
def _close_callback(self, connection):
with self._lock:
if connection.getpeername() in self._clients:
self.logger.debug("Remove connection %s" % str(connection))
self._clients[connection.getpeername()].close()
del self._clients[connection.getpeername()]