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()]