class Server():
def __init__(self, cfg_file, version='', params={}):
self.cfg = Config(cfg_file, version, params)
loglevel = self.cfg.param('global/loglevel', 'info')
self.logger = NMLogger('server', loglevel)
self.cfg.init_cfgif()
self._stop = False
default_port = self.cfg.param('transport/udp/port', 3794)
addrbook_udp = self.cfg.param('addrbook/udp', {})
addrbook_tcp = self.cfg.param('addrbook/tcp', {})
self.addrbook = AddressBook(default_port=default_port, addrbook_udp=addrbook_udp, addrbook_tcp=addrbook_tcp, loglevel=loglevel)
self.statistics = Collector(self.cfg)
self._local_mngr = None
self._udp = None
self._tcp_server = None
self._lock = threading.RLock()
def start(self, block=True):
# self._callback_change_loglevel('global/loglevel', self.cfg.param('global/loglevel', 'info'))
self._local_mngr = UDSServer(self, self.cfg, self.addrbook, self.statistics)
self._on_discover = {}
port = self.cfg.param('transport/udp/port', 3794)
mgroup = self.cfg.param('transport/udp/group', '239.255.0.1')
ttl = self.cfg.param('transport/udp/ttl', 16)
use_mcast = self.cfg.param('transport/udp/use_mcast', '')
interface = self.cfg.param('transport/udp/interface', '')
buffer_size = self.cfg.param('transport/udp/buffer_size', 0)
queue_length = self.cfg.param('transport/udp/queue_length', 0)
if use_mcast:
self._udp = UDPmcSocket(port, mgroup, router=self, ttl=ttl, interface=interface, send_buffer=buffer_size, recv_buffer=self.cfg.RECV_BUFFER, queue_length=queue_length, loglevel=self.logger.level())
else:
self._udp = UDPucSocket(port, router=self, interface=interface, send_buffer=buffer_size, recv_buffer=self.cfg.RECV_BUFFER, queue_length=queue_length, loglevel=self.logger.level())
# create TCP server
tcp_enabled = self.cfg.param('transport/tcp/enable', False)
self._tcp_server = None
if tcp_enabled:
tcp_port = self.cfg.param('transport/tcp/port', 3794)
tcp_interface = self.cfg.param('transport/tcp/interface', '')
tcp_queue_length = self.cfg.param('transport/tcp/queue_length', 0)
self._tcp_server = TCPServer(port=tcp_port, router=self, interface=tcp_interface, logger_name='tcp', recv_buffer=self.cfg.RECV_BUFFER, queue_length=tcp_queue_length, loglevel=self.logger.level())
try:
while block:
time.sleep(1)
except KeyboardInterrupt:
print("caught keyboard interrupt, exiting")
def route_local_msg(self, msg):
try:
if msg.dst_id.has_wildcards():
# it is a broadcast message, try send to all matched locals (except sender)
self.logger.debug("send 0x%.4X broadcast from %s" % (msg.msg_id, msg.src_id))
self._local_mngr.send_queued(msg)
# it comes not from UDP socket, send to UDP and TCP
self._udp.send_queued(msg)
if self._tcp_server is not None:
self._tcp_server.send_queued(msg)
msg.forward = True
else:
# it is an unique id, search in address book for receiver
if self.addrbook.apply_destination(msg):
self.logger.debug("send 0x%.4X unicast from %s to %s (%s)" % (msg.msg_id, msg.src_id, msg.dst_id, msg.tinfo_dst))
if msg.tinfo_dst.etype == AddressBook.Endpoint.UDS:
self._local_mngr.send_queued(msg)
elif msg.tinfo_dst.etype == AddressBook.Endpoint.UDP:
# send through UDP socket
self._udp.send_queued(msg)
elif msg.tinfo_dst.etype == AddressBook.Endpoint.TCP:
# send through TCP socket
if self._tcp_server is not None:
self._tcp_server.send_queued(msg)
msg.forward = True
else:
# no receiver found
# do not send every message to not known receiver
ts = 0
if msg.dst_id in self._on_discover:
ts = self._on_discover[msg.dst_id]
ts_cur = time.time()
if ts_cur - ts > 1:
# try to find the receiver -> send as broadcast with ACK requested
self.logger.debug("%s not found, try to discover, send as broadcast with ACK requested" % msg.dst_id)
msg.acknak = 1
msg.tinfo_dst = None
self._udp.send_queued(msg)
if self._tcp_server is not None:
self._tcp_server.send_queued(msg)
self._on_discover[msg.dst_id] = ts_cur
msg.forward = True
self.statistics.add(msg)
except Exception as err:
print("ERROR", err)
def route_udp_msg(self, msg):
try:
self.addrbook.add(msg)
if msg.dst_id.has_wildcards():
# it is a broadcast message, try send to all matched locals (except sender)
self.logger.debug("send 0x%.4X broadcast from %s" % (msg.msg_id, msg.src_id))
self._local_mngr.send_queued(msg)
msg.forward = True
else:
# it is an unique id, search in address book for receiver
if self.addrbook.apply_destination(msg):
self.logger.debug("send 0x%.4X unicast from %s to %s (%s)" % (msg.msg_id, msg.src_id, msg.dst_id, msg.tinfo_dst))
if msg.tinfo_dst.etype == AddressBook.Endpoint.UDS:
self._local_mngr.send_queued(msg)
msg.forward = True
# TODO: forward message to TCP?
self.statistics.add(msg)
except Exception as err:
print("ERROR", err)
def route_tcp_msg(self, msg):
try:
self.addrbook.add(msg)
if msg.dst_id.has_wildcards():
# it is a broadcast message, try send to all matched locals (except sender)
self.logger.debug("send 0x%.4X broadcast from %s" % (msg.msg_id, msg.src_id))
self._local_mngr.send_msg(msg)
msg.forward = True
else:
# it is an unique id, search in address book for receiver
if self.addrbook.apply_destination(msg):
self.logger.debug("send 0x%.4X unicast from %s to %s (%s)" % (msg.msg_id, msg.src_id, msg.dst_id, msg.tinfo_dst))
if msg.tinfo_dst.etype == AddressBook.Endpoint.UDS:
self._local_mngr.send_msg(msg)
msg.forward = True
# TODO: forward message to UDP?
self.statistics.add(msg)
except Exception as err:
print("ERROR", err)
def shutdown(self):
self.cfg.close()
self.statistics.stop()
self._stop = True
if self._udp is not None:
self._udp.close()
if self._local_mngr is not None:
self._local_mngr.stop()
if self._tcp_server is not None:
self._tcp_server.close()
self._tcp_server = None
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()]