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 __init__(self, host='', port=0, router=None, interface='', logger_name='tcp_client', recv_buffer=5000, queue_length=0, loglevel='info'):
'''
:param str host: destination host.
:param int port: destination port.
:param router: class which provides `route_tcp_msg(fkie_iop_node_manager.message.Message)` method. If `None` receive will be disabled.
'''
self._closed = False
self._connected = False
self._connection_error_printed = False
self.logger = NMLogger('%s[%s:%d]' % (logger_name, host, port), loglevel)
self._router = router
self._recv_buffer = recv_buffer
self._queue_length = queue_length
self._socket_type = socket.AF_INET
self._queue_send = queue.PQueue(queue_length, 'queue_%s_send_%s:%d' % (logger_name, host, port), loglevel=loglevel)
self._raddr = (host, port)
self.interface = interface
self._first_send_msg = True
if self.interface:
addrinfo = getaddrinfo(self.interface)
self._socket_type = addrinfo[0]
self._endpoint_client = AddressBook.Endpoint(AddressBook.Endpoint.TCP, host, port)
self._message_parser = MessageParser(self._endpoint_client, stream=True, loglevel=loglevel)
self._thread_connect = threading.Thread(target=self._connect, args=(self._raddr,))
self._thread_connect.start()
self._thread_send = threading.Thread(target=self._loop_send)
self._thread_send.start()
def __init__(self,
name,
remove_on_close=False,
force_bind=False,
root_path='/tmp',
recv_buffer=5000,
loglevel='info'):
self._closed = False
self.logger = NMLogger('uds[%s]' % name, loglevel)
self._remove_on_close = remove_on_close
self._recv_buffer = recv_buffer
socket.socket.__init__(self, socket.AF_UNIX, socket.SOCK_DGRAM)
self.setblocking(True)
self._socket_path = os.path.join(root_path, name)
self._parser = MessageParser(AddressBook.Endpoint(
AddressBook.Endpoint.UDS, self._socket_path),
loglevel=loglevel)
if os.path.exists(self._socket_path) and not force_bind:
self.logger.debug("Connect to local socket %s" % self._socket_path)
self.connect(self._socket_path)
else:
self.logger.debug("Create local socket connection %s" %
self._socket_path)
if os.path.exists(self._socket_path):
os.unlink(self._socket_path)
self.bind(self._socket_path)
os.chmod(self._socket_path,
stat.S_IRWXU | stat.S_IRWXG | stat.S_IRWXO)
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 __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 __init__(self,
default_port=3794,
addrbook_udp={},
addrbook_tcp={},
loglevel='info'):
'''
:param fkie_iop_node_manager.config.Config cfg: configuration
'''
self.logger = NMLogger('addrbook', loglevel)
self._default_port = default_port
self._map = {}
self._static_tcp_port_map = {}
self._static_udp = self._read_static_addr(addrbook_udp,
AddressBook.Endpoint.UDP)
self._static_tcp = self._read_static_addr(addrbook_tcp,
AddressBook.Endpoint.TCP)
self.logger.debug('AddressBook initialized with %s' % self)
def __init__(self,
connection,
router=None,
logger_name='tcp_input',
recv_buffer=5000,
queue_length=0,
close_callback=None,
loglevel='info'):
'''
:param (str,int) connection: client address.
:param router: class which provides `route_tcp_msg(fkie_iop_node_manager.message.Message)` method. If `None` receive will be disabled.
'''
self._closed = False
self._send_error_printed = False
self._connection = connection
self._raddr = connection.getpeername()
self.logger = NMLogger(
'%s[%s:%d]' % (logger_name, self._raddr[0], self._raddr[1]),
loglevel)
self._router = router
self._recv_buffer = recv_buffer
self._queue_length = queue_length
self._close_callback = close_callback
self._first_send_msg = True
self._queue_send = queue.PQueue(
queue_length,
'queue_%s_send_%s:%d' %
(logger_name, self._raddr[0], self._raddr[1]),
loglevel=loglevel)
self._endpoint_client = AddressBook.Endpoint(AddressBook.Endpoint.TCP,
self._raddr[0],
self._raddr[1])
self._message_parser = MessageParser(self._endpoint_client,
stream=True,
loglevel=loglevel)
self._thread_send = threading.Thread(target=self._loop_send)
self._thread_send.start()
if self._router is not None:
self._thread_recv = threading.Thread(target=self._loop_recv)
self._thread_recv.start()
def __init__(self, filename='', version='', params={}):
self._stop = False
self._mutex = threading.RLock()
self.version = version
self.filename = filename
if not self.filename:
self.filename = os.path.expanduser('~/.config/iop.fkie/iop_node_manager.yaml')
cfg_path = os.path.dirname(self.filename)
if not os.path.isdir(cfg_path):
os.makedirs(cfg_path)
self._reload_callbacks = []
self._param_callbacks = {}
self._cfg = None
self.logger = NMLogger('config')
self.reload()
NMLogger.setall_loglevel(self.param('global/loglevel', 'info'))
self.apply(params, save=False)
self.msg_ids = {} # (int)id: (str)Name
self._read_msg_ids()
self._cfgif = None
self._cfgif_address = None
self.add_param_listener('global/loglevel', self._callback_change_loglevel)
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))
def __init__(self,
port,
mgroup,
router=None,
ttl=16,
interface='',
logger_name='udp_mc',
send_buffer=0,
recv_buffer=0,
queue_length=0,
loglevel='info'):
'''
Creates a socket, bind it to a given port and join to a given multicast
group. IPv4 and IPv6 are supported.
:param int port: the port to bind the socket
:param str mgroup: the multicast group to join
:param router: class which provides `route_udp_msg(fkie_iop_node_manager.message.Message)` method. If `None` receive will be disabled.
:param type: fkie_iop_node_manager.queue
:param int ttl: time to leave (Default: 20)
:param str interface: IP of interface to bind (Default: '').
'''
self.logger = NMLogger(
'%s[%s:%d]' % (logger_name, mgroup.replace('.', '_'), port),
loglevel)
self.port = port
self.mgroup = mgroup
self._lock = threading.RLock()
self._closed = False
self._recv_buffer = recv_buffer
self._locals = [ip for _ifname, ip in localifs()]
self._locals.append('localhost')
self._sender_endpoints = {}
self.sock_5_error_printed = []
self.SOKET_ERRORS_NEEDS_RECONNECT = False
self.interface = interface
# get the AF_INET information for group to ensure that the address family
# of group is the same as for interface
addrinfo = getaddrinfo(self.mgroup)
self.interface_ip = ''
if self.interface:
addrinfo = getaddrinfo(self.interface, addrinfo[0])
if addrinfo is not None:
self.interface_ip = addrinfo[4][0]
self.logger.debug("destination: %s" % self.mgroup)
self.logger.debug("interface : %s (detected ip: %s)" %
(self.interface, self.interface_ip))
self.logger.debug("inet: %s" % str(addrinfo))
socket.socket.__init__(self, addrinfo[0], socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
self.logger.info("Create multicast socket @('%s', %d)" %
(self.mgroup, port))
# initialize multicast socket
# Allow multiple copies of this program on one machine
if hasattr(socket, "SO_REUSEPORT"):
try:
self.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
except Exception:
self.logger.warning(
"SO_REUSEPORT failed: Protocol not available, some functions are not available."
)
# Set Time-to-live (optional) and loop count
ttl_bin = struct.pack('@i', ttl)
if addrinfo[0] == socket.AF_INET: # IPv4
self.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL,
ttl_bin)
self.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1)
else: # IPv6
self.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_MULTICAST_HOPS,
ttl_bin)
self.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_MULTICAST_LOOP, 1)
try:
if addrinfo[0] == socket.AF_INET: # IPv4
# Create group_bin for de-register later
# Set socket options for multicast specific interface or general
if not self.interface_ip:
self.group_bin = socket.inet_pton(
socket.AF_INET, self.mgroup) + struct.pack(
'=I', socket.INADDR_ANY)
self.setsockopt(socket.IPPROTO_IP,
socket.IP_ADD_MEMBERSHIP, self.group_bin)
else:
self.group_bin = socket.inet_aton(
self.mgroup) + socket.inet_aton(self.interface_ip)
self.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF,
socket.inet_aton(self.interface_ip))
self.setsockopt(socket.IPPROTO_IP,
socket.IP_ADD_MEMBERSHIP, self.group_bin)
else: # IPv6
# Create group_bin for de-register later
# Set socket options for multicast
self.group_bin = socket.inet_pton(addrinfo[0],
self.mgroup) + struct.pack(
'@I', socket.INADDR_ANY)
self.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_JOIN_GROUP,
self.group_bin)
except socket.error as errobj:
msg = str(errobj)
if errobj.errno in [errno.ENODEV]:
msg = "socket.error[%d]: %s,\nis multicast route set? e.g. sudo route add -net 224.0.0.0 netmask 224.0.0.0 eth0" % (
errobj.errno, msg)
raise Exception(msg)
# set buffer size if configured
if send_buffer:
old_bufsize = self.getsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF)
if old_bufsize != send_buffer:
self.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF,
send_buffer)
bufsize = self.getsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF)
self.logger.debug("Changed buffer size from %d to %d" %
(old_bufsize, bufsize))
# Bind to the port
try:
# bind to default interfaces if not unicast socket was created
self.bind((self.interface_ip, port))
except socket.error as errobj:
msg = str(errobj)
self.logger.critical(
"Unable to bind multicast to interface: %s, check that it exists: %s"
% (self.mgroup, msg))
raise
self._router = router
self._queue_send = queue.PQueue(queue_length,
'queue_udp_send',
loglevel=loglevel)
self._parser_mcast = MessageParser(None, loglevel=loglevel)
self.addrinfo = addrinfo
# create a thread to handle the received multicast messages
if self._router is not None:
self._thread_recv = threading.Thread(target=self._loop_recv)
self._thread_recv.start()
self._thread_send = threading.Thread(target=self._loop_send)
self._thread_send.start()
class UDPmcSocket(socket.socket):
'''
The UdpSocket class enables the send and receive UDP messages to a
multicast group and unicast address. The unicast socket is only created if
'send_mcast' and 'listen_mcast' parameter are set to False or a specific interface is defined.
'''
def __init__(self,
port,
mgroup,
router=None,
ttl=16,
interface='',
logger_name='udp_mc',
send_buffer=0,
recv_buffer=0,
queue_length=0,
loglevel='info'):
'''
Creates a socket, bind it to a given port and join to a given multicast
group. IPv4 and IPv6 are supported.
:param int port: the port to bind the socket
:param str mgroup: the multicast group to join
:param router: class which provides `route_udp_msg(fkie_iop_node_manager.message.Message)` method. If `None` receive will be disabled.
:param type: fkie_iop_node_manager.queue
:param int ttl: time to leave (Default: 20)
:param str interface: IP of interface to bind (Default: '').
'''
self.logger = NMLogger(
'%s[%s:%d]' % (logger_name, mgroup.replace('.', '_'), port),
loglevel)
self.port = port
self.mgroup = mgroup
self._lock = threading.RLock()
self._closed = False
self._recv_buffer = recv_buffer
self._locals = [ip for _ifname, ip in localifs()]
self._locals.append('localhost')
self._sender_endpoints = {}
self.sock_5_error_printed = []
self.SOKET_ERRORS_NEEDS_RECONNECT = False
self.interface = interface
# get the AF_INET information for group to ensure that the address family
# of group is the same as for interface
addrinfo = getaddrinfo(self.mgroup)
self.interface_ip = ''
if self.interface:
addrinfo = getaddrinfo(self.interface, addrinfo[0])
if addrinfo is not None:
self.interface_ip = addrinfo[4][0]
self.logger.debug("destination: %s" % self.mgroup)
self.logger.debug("interface : %s (detected ip: %s)" %
(self.interface, self.interface_ip))
self.logger.debug("inet: %s" % str(addrinfo))
socket.socket.__init__(self, addrinfo[0], socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
self.logger.info("Create multicast socket @('%s', %d)" %
(self.mgroup, port))
# initialize multicast socket
# Allow multiple copies of this program on one machine
if hasattr(socket, "SO_REUSEPORT"):
try:
self.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
except Exception:
self.logger.warning(
"SO_REUSEPORT failed: Protocol not available, some functions are not available."
)
# Set Time-to-live (optional) and loop count
ttl_bin = struct.pack('@i', ttl)
if addrinfo[0] == socket.AF_INET: # IPv4
self.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL,
ttl_bin)
self.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1)
else: # IPv6
self.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_MULTICAST_HOPS,
ttl_bin)
self.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_MULTICAST_LOOP, 1)
try:
if addrinfo[0] == socket.AF_INET: # IPv4
# Create group_bin for de-register later
# Set socket options for multicast specific interface or general
if not self.interface_ip:
self.group_bin = socket.inet_pton(
socket.AF_INET, self.mgroup) + struct.pack(
'=I', socket.INADDR_ANY)
self.setsockopt(socket.IPPROTO_IP,
socket.IP_ADD_MEMBERSHIP, self.group_bin)
else:
self.group_bin = socket.inet_aton(
self.mgroup) + socket.inet_aton(self.interface_ip)
self.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF,
socket.inet_aton(self.interface_ip))
self.setsockopt(socket.IPPROTO_IP,
socket.IP_ADD_MEMBERSHIP, self.group_bin)
else: # IPv6
# Create group_bin for de-register later
# Set socket options for multicast
self.group_bin = socket.inet_pton(addrinfo[0],
self.mgroup) + struct.pack(
'@I', socket.INADDR_ANY)
self.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_JOIN_GROUP,
self.group_bin)
except socket.error as errobj:
msg = str(errobj)
if errobj.errno in [errno.ENODEV]:
msg = "socket.error[%d]: %s,\nis multicast route set? e.g. sudo route add -net 224.0.0.0 netmask 224.0.0.0 eth0" % (
errobj.errno, msg)
raise Exception(msg)
# set buffer size if configured
if send_buffer:
old_bufsize = self.getsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF)
if old_bufsize != send_buffer:
self.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF,
send_buffer)
bufsize = self.getsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF)
self.logger.debug("Changed buffer size from %d to %d" %
(old_bufsize, bufsize))
# Bind to the port
try:
# bind to default interfaces if not unicast socket was created
self.bind((self.interface_ip, port))
except socket.error as errobj:
msg = str(errobj)
self.logger.critical(
"Unable to bind multicast to interface: %s, check that it exists: %s"
% (self.mgroup, msg))
raise
self._router = router
self._queue_send = queue.PQueue(queue_length,
'queue_udp_send',
loglevel=loglevel)
self._parser_mcast = MessageParser(None, loglevel=loglevel)
self.addrinfo = addrinfo
# create a thread to handle the received multicast messages
if self._router is not None:
self._thread_recv = threading.Thread(target=self._loop_recv)
self._thread_recv.start()
self._thread_send = threading.Thread(target=self._loop_send)
self._thread_send.start()
def close(self):
'''
Unregister from the multicast group and close the socket.
'''
self._closed = True
self.logger.info("Close multicast socket")
try:
# shutdown to cancel recvfrom()
socket.socket.shutdown(self, socket.SHUT_RD)
except socket.error:
pass
# Use the stored group_bin to de-register
if self.addrinfo[0] == socket.AF_INET: # IPv4
self.setsockopt(socket.IPPROTO_IP, socket.IP_DROP_MEMBERSHIP,
self.group_bin)
else: # IPv6
self.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_LEAVE_GROUP,
self.group_bin)
socket.socket.close(self)
self._queue_send.clear()
def send_queued(self, msg):
try:
self._queue_send.put(msg)
except queue.Full as full:
print(traceback.format_exc())
self.logger.warning("Can't send message: %s" % full)
except Exception as e:
self.logger.warning("Error while put message into queue: %s" % e)
def _loop_send(self):
while not self._closed:
# Wait for next available Message. This method cancel waiting on clear() of PQueue and return None.
msg = self._queue_send.get()
if msg is not None:
dst = msg.tinfo_dst
if dst is None: # or msg.dst_id.has_wildcards():
dst = AddressBook.Endpoint(AddressBook.Endpoint.UDP,
self.mgroup,
self.getsockname()[1])
if dst is not None:
self._sendto(msg, dst)
else:
self.logger.warning(
"Can't send message to %s, destination not found!" %
(dst))
# TODO: add retry mechanism
def _sendto(self, msg, endpoint):
# send to given addresses
try:
# self.logger.debug("Send to %s:%d" % (endpoint.address, endpoint.port))
val = self.sendto(msg.bytes(), (endpoint.address, endpoint.port))
if val != msg.raw_size:
raise Exception("not complete send %d of %d" %
(val, msg.raw_size))
if endpoint.address in SEND_ERRORS:
del SEND_ERRORS[endpoint.address]
except socket.error as errobj:
erro_msg = "Error while send to '%s': %s" % (endpoint.address,
errobj)
SEND_ERRORS[endpoint.address] = erro_msg
# -2: Name or service not known
if errobj.errno in [-5, -2]:
if endpoint.address not in self.sock_5_error_printed:
self.logger.warning(erro_msg)
self.sock_5_error_printed.append(endpoint.address)
else:
self.logger.warning(erro_msg)
if errobj.errno in [
errno.ENETDOWN, errno.ENETUNREACH, errno.ENETRESET, errno
]:
self.SOKET_ERRORS_NEEDS_RECONNECT = True
except Exception as e:
erro_msg = "Send to host '%s' failed: %s" % (endpoint.address, e)
self.logger.warning(erro_msg)
SEND_ERRORS[endpoint.address] = erro_msg
def _loop_recv(self):
'''
This method handles the received multicast messages.
'''
while not self._closed:
try:
(data, address) = self.recvfrom(self._recv_buffer)
if data and not self._closed and address[
0] not in self._locals: # skip messages received from self
msgs = self._parser_mcast.unpack(data)
for msg in msgs:
try:
msg.tinfo_src = self._sender_endpoints[address]
except KeyError:
endpoint = AddressBook.Endpoint(
AddressBook.Endpoint.UDP, address[0],
address[1])
msg.tinfo_src = endpoint
self._sender_endpoints[address] = endpoint
# self.logger.debug("Received from %s" % (msg.tinfo_src))
self._router.route_udp_msg(msg)
except socket.timeout:
pass
except queue.Full as full_error:
self.logger.warning(
"Error while process received multicast message: %s" %
full_error)
except socket.error:
if not self._closed:
self.logger.warning("socket error: %s" %
traceback.format_exc())
class TCPClient(socket.socket):
def __init__(self, host='', port=0, router=None, interface='', logger_name='tcp_client', recv_buffer=5000, queue_length=0, loglevel='info'):
'''
:param str host: destination host.
:param int port: destination port.
:param router: class which provides `route_tcp_msg(fkie_iop_node_manager.message.Message)` method. If `None` receive will be disabled.
'''
self._closed = False
self._connected = False
self._connection_error_printed = False
self.logger = NMLogger('%s[%s:%d]' % (logger_name, host, port), loglevel)
self._router = router
self._recv_buffer = recv_buffer
self._queue_length = queue_length
self._socket_type = socket.AF_INET
self._queue_send = queue.PQueue(queue_length, 'queue_%s_send_%s:%d' % (logger_name, host, port), loglevel=loglevel)
self._raddr = (host, port)
self.interface = interface
self._first_send_msg = True
if self.interface:
addrinfo = getaddrinfo(self.interface)
self._socket_type = addrinfo[0]
self._endpoint_client = AddressBook.Endpoint(AddressBook.Endpoint.TCP, host, port)
self._message_parser = MessageParser(self._endpoint_client, stream=True, loglevel=loglevel)
self._thread_connect = threading.Thread(target=self._connect, args=(self._raddr,))
self._thread_connect.start()
self._thread_send = threading.Thread(target=self._loop_send)
self._thread_send.start()
def __eq__(self, other):
return self._raddr == other._raddr
def close(self):
self._closed = True
self._connected = False
self.logger.info("Close connection, own socket: %s" % str(self.getsockname()))
self._queue_send.clear()
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:
self._queue_send.put(msg)
except queue.Full as full:
self.logger.debug(traceback.format_exc())
self.logger.warning("Can't send message: %s" % full)
except Exception as e:
self.logger.warning("Error while put message into queue: %s" % e)
def _loop_send(self):
while not self._closed:
try:
# Waits for next available Message. get() method cancel waiting on clear() of PQueue and return None.
msg = self._queue_send.get()
if msg is not None:
try:
if self._connected:
self.logger.debug("Send message to %s:%d" % (self._raddr[0], self._raddr[1]))
self.send(msg.bytes(prepend_version=self._first_send_msg), socket.MSG_DONTWAIT)
self._first_send_msg = False
elif not self._thread_connect.is_alive():
self._thread_connect = threading.Thread(target=self._connect, args=(self._raddr,))
self._thread_connect.start()
self._queue_send.put(msg)
except Exception as err:
self.logger.debug("Error while send message through TCP: %s" % err)
self._connected = False
try:
# try to close on exception
socket.socket.shutdown(self, socket.SHUT_RDWR)
except Exception:
pass
try:
# reconnect on exception
self._thread_connect = threading.Thread(target=self._connect, args=(self._raddr,))
self._thread_connect.start()
self._queue_send.put(msg)
except Exception:
pass
except Exception as serr:
self.logger.debug("Error on send message to %s:%d: %s" % (self._raddr[0], self._raddr[1], serr))
def _loop_recv(self):
while self._connected:
try:
data = self.recv(self._recv_buffer)
if data:
# parse the message and put it in recv queue
msgs = self._message_parser.unpack(data)
for msg in msgs:
self.logger.debug("Received from %s" % (msg.tinfo_src))
self._router.route_tcp_msg(msg)
else:
self._connected = False
except socket.timeout:
pass
except Exception:
if not self._closed:
self.logger.warning(traceback.format_exc())
else:
self.logger.debug(traceback.format_exc())
def _connect(self, address):
'''
Tries to connect to a destination address. On success a thread with receive loop will be started.
This method is called in a thread.
'''
try:
self.logger.debug("Connecting to %s:%d" % (address[0], address[1]))
socket.socket.__init__(self, self._socket_type, socket.SOCK_STREAM)
self.connect(address)
self._message_parser._version = None
self.logger.debug("Connected to %s:%d" % (address[0], address[1]))
self._connected = True
self._connection_error_printed = False
self._first_send_msg = True
if self._router is not None:
self._thread_recv = threading.Thread(target=self._loop_recv)
self._thread_recv.start()
if self._connection_error_printed:
self.logger.info("Connected to %s:%d" % (address[0], address[1]))
except Exception as err:
if not self._connection_error_printed:
self.logger.warning("Error while TCP connect: %s" % err)
self._connection_error_printed = True
else:
self.logger.debug("Error while TCP connect: %s" % err)
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 TCPInput(object):
def __init__(self,
connection,
router=None,
logger_name='tcp_input',
recv_buffer=5000,
queue_length=0,
close_callback=None,
loglevel='info'):
'''
:param (str,int) connection: client address.
:param router: class which provides `route_tcp_msg(fkie_iop_node_manager.message.Message)` method. If `None` receive will be disabled.
'''
self._closed = False
self._send_error_printed = False
self._connection = connection
self._raddr = connection.getpeername()
self.logger = NMLogger(
'%s[%s:%d]' % (logger_name, self._raddr[0], self._raddr[1]),
loglevel)
self._router = router
self._recv_buffer = recv_buffer
self._queue_length = queue_length
self._close_callback = close_callback
self._first_send_msg = True
self._queue_send = queue.PQueue(
queue_length,
'queue_%s_send_%s:%d' %
(logger_name, self._raddr[0], self._raddr[1]),
loglevel=loglevel)
self._endpoint_client = AddressBook.Endpoint(AddressBook.Endpoint.TCP,
self._raddr[0],
self._raddr[1])
self._message_parser = MessageParser(self._endpoint_client,
stream=True,
loglevel=loglevel)
self._thread_send = threading.Thread(target=self._loop_send)
self._thread_send.start()
if self._router is not None:
self._thread_recv = threading.Thread(target=self._loop_recv)
self._thread_recv.start()
def getpeername(self):
return self._raddr
def __eq__(self, other):
return self._raddr == other._raddr
def close(self):
self._closed = True
self.logger.info("Close input connection, own socket: %s" %
str(self._connection.getsockname()))
self._queue_send.clear()
try:
# Important: Close read and direction
self._connection.shutdown(socket.SHUT_RDWR)
except Exception:
self.logger.debug(traceback.format_exc())
self._connection.close()
def send_queued(self, msg):
try:
self._queue_send.put(msg)
except queue.Full as full:
self.logger.debug(traceback.format_exc())
self.logger.warning("Can't send message: %s" % full)
except Exception as e:
self.logger.warning("Error while put message into queue: %s" % e)
def _loop_send(self):
while not self._closed:
try:
# Waits for next available Message. get() method cancel waiting on clear() of PQueue and return None.
msg = self._queue_send.get()
if msg is not None:
try:
self.logger.debug("Send message to %s:%d" %
(self._raddr[0], self._raddr[1]))
self._connection.sendall(
msg.bytes(prepend_version=self._first_send_msg),
socket.MSG_DONTWAIT)
self._first_send_msg = False
except Exception as err:
self.logger.debug(
"Error while send message through TCP input connection: %s"
% err)
self._connected = False
try:
self._connection.shutdown(socket.SHUT_RD)
except Exception as errshd:
print("ERR shutdown on send:", errshd)
except Exception as serr:
self.logger.debug("Error on send message to %s:%d: %s" %
(self._raddr[0], self._raddr[1], serr))
def _loop_recv(self):
while not self._closed:
try:
data = self._connection.recv(self._recv_buffer)
if data:
# parse the message and put it in recv queue
msgs = self._message_parser.unpack(data)
for msg in msgs:
self.logger.debug("Received from %s" % (msg.tinfo_src))
self._router.route_tcp_msg(msg)
else:
if self._close_callback:
self._close_callback(self)
try:
# try to shutdown the connection
self._connection.shutdown(socket.SHUT_RD)
except Exception as errshd:
print("ERR shutdown on recv:", errshd)
except socket.timeout:
pass
except Exception:
if not self._closed:
self.logger.warning(traceback.format_exc())
def __init__(self,
port=0,
router=None,
interface='',
logger_name='udp',
default_dst=None,
send_buffer=0,
recv_buffer=0,
queue_length=0,
loglevel='info'):
'''
Creates a socket, bind it to a given interface+port for unicast send/receive.
IPv4 and IPv6 are supported.
:param int port: the port to bind the socket. If zero an empty one will be used.
:param router: class which provides `route_udp_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
:param tuple(str,int) default_dst: used for loopback to send messages to predefined destination.
'''
self._closed = False
self.logger = NMLogger('%s[%s:%d]' % (logger_name, interface, port),
loglevel)
self.interface = interface
self.port = port
self._router = router
self._default_dst = default_dst
self._recv_buffer = recv_buffer
self._sender_endpoints = {}
self.sock_5_error_printed = []
# If interface isn't specified, try to find an non localhost interface to
# get some info for binding. Otherwise use localhost
# if not self.interface:
# ifaces = localifs()
# for iface in ifaces:
# if not (iface[1].startswith('127') or iface[1].startswith('::1')):
# self.interface = iface[1]
# break
self.logger.info("+ Bind to unicast socket @(%s:%s)" %
(self.interface, port))
socket_type = socket.AF_INET
bind_ip = self.interface
if self.interface:
addrinfo = getaddrinfo(self.interface)
socket_type = addrinfo[0]
bind_ip = addrinfo[4][0]
# Configure socket type
socket.socket.__init__(self, socket_type, socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
# Bind to the port
try:
self.logger.debug("Ucast bind to: (%s:%s)" % (bind_ip, port))
self.bind((bind_ip, port))
except socket.error as errobj:
msg = str(errobj)
self.logger.critical(
"Unable to bind unicast to interface: %s, check that it exists: %s"
% (bind_ip, msg))
raise
if self.port == 0:
self.port = self.getsockname()[1]
if send_buffer:
# update buffer size
old_bufsize = self.getsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF)
if old_bufsize != send_buffer:
self.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF,
send_buffer)
# self.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, buffersize)
bufsize = self.getsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF)
self.logger.debug("Changed buffer size from %d to %d" %
(old_bufsize, bufsize))
self._parser_ucast = MessageParser(None, loglevel=loglevel)
self._queue_send = queue.PQueue(queue_length,
'queue_%s_send' % logger_name,
loglevel=loglevel)
# create a thread to handle the received unicast messages
if self._router is not None:
self._thread_recv = threading.Thread(target=self._loop_recv)
self._thread_recv.start()
self._thread_send = threading.Thread(target=self._loop_send)
self._thread_send.start()
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()
class AddressBook():
class Endpoint:
UDS = 0
UDP = 1
TCP = 2
def __init__(self, etype, address='', port=None):
self.etype = etype
self.address = address
self.port = port
def __repr__(self):
return "Endpoint<%s; %s%s>" % (self.etype_str(), self.address,
':%d' % self.port
if self.port is not None else '')
def etype_str(self):
result = ''
if self.etype == self.UDS:
result = 'UDS'
elif self.etype == self.UDP:
result = 'UDP'
elif self.etype == self.TCP:
result = 'TCP'
return result
def address_str(self):
result = self.address
if self.port is not None:
result += ':%d' % self.port
return result
def address_tuple(self):
return (self.address, self.port)
def __eq__(self, other):
return self.etype == other.etype and self.address == other.address and self.port == other.port
def __ne__(self, other):
return not self.__eq__(other)
def __init__(self,
default_port=3794,
addrbook_udp={},
addrbook_tcp={},
loglevel='info'):
'''
:param fkie_iop_node_manager.config.Config cfg: configuration
'''
self.logger = NMLogger('addrbook', loglevel)
self._default_port = default_port
self._map = {}
self._static_tcp_port_map = {}
self._static_udp = self._read_static_addr(addrbook_udp,
AddressBook.Endpoint.UDP)
self._static_tcp = self._read_static_addr(addrbook_tcp,
AddressBook.Endpoint.TCP)
self.logger.debug('AddressBook initialized with %s' % self)
def __str__(self):
return "<AddressBook discovered[%d]=%s, configured_udp[%d]=%s, configured_tcp[%d]=%s/>" % (
len(self._map), self._map, len(self._static_udp), self._static_udp,
len(self._static_tcp), self._static_tcp)
def _read_static_addr(self, from_dict, etype):
etype_str = 'None'
if etype == AddressBook.Endpoint.UDP:
etype_str = 'UDP'
elif etype == AddressBook.Endpoint.TCP:
etype_str = 'TCP'
result = []
for addr, items in from_dict.items():
host, port = self._parse_hostport(addr, self._default_port)
endpoint = AddressBook.Endpoint(etype, host, port)
if isinstance(items, list):
for item in items:
jid = JausAddress.from_string(item)
self.logger.info("Add from config: %s:%s [%s]" %
(jid, endpoint, etype_str))
result.append((jid, endpoint))
else:
jid = JausAddress.from_string(items)
self.logger.info("Add from config: %s:%s [%s]" %
(jid, endpoint, etype_str))
result.append((jid, endpoint))
if etype == AddressBook.Endpoint.TCP:
self._static_tcp_port_map[host] = port
return result
def _parse_hostport(self, param, default_port):
if param[-1] == ']':
# ipv6 literal (with no port)
return (param, default_port)
out = param.rsplit(":", 1)
if len(out) == 1:
# No port
port = default_port
else:
try:
port = int(out[1])
except ValueError:
raise ValueError("Invalid host:port '%s'" % param)
return (out[0], port)
def apply_destination(self, msg):
'''
Searches for destination address/socket and set the `tinfo_dst` of the message
to the AddressBook.Endpoint object if destination was found. In this case the result is True.
If detination was not found `tinfo_dst` will not be changed and False is returned.
:rtype: bool
'''
try:
# try first in discovered addresses
msg.tinfo_dst = self._map[msg.dst_id]
return True
except KeyError:
# lookup in configured addresses, take first found. UDP is preferred.
for jaus_id, entry in self._static_udp:
if msg.dst_id.match(jaus_id):
# take first found
msg.tinfo_dst = entry
return True
# lookup in configured TCP addresses, take first found. UDP is preferred.
for jaus_id, entry in self._static_tcp:
if msg.dst_id.match(jaus_id):
# take first found
msg.tinfo_dst = entry
return True
return False
def add(self, msg):
'''
tinfo_src should be valid.
'''
if not msg.src_id.has_wildcards():
self._add(msg.src_id, msg.tinfo_src)
def add_jaus_address(self, jaus_address, address, port, ep_type):
if not jaus_address.has_wildcards():
endpoint = AddressBook.Endpoint(ep_type, address, port)
self._add(jaus_address, endpoint)
def _add(self, jaus_address, endpoint):
if jaus_address in self._map:
current_addr = self._map[jaus_address]
if endpoint != current_addr:
self.logger.warning("Changed address for %s: old=%s, new %s" %
(jaus_address, current_addr, endpoint))
self._map[jaus_address] = endpoint
else:
self.logger.info("Added new address for %s: %s" %
(jaus_address, endpoint))
self._map[jaus_address] = endpoint
def remove(self, jaus_address):
if not jaus_address.has_wildcards():
if jaus_address in self._map:
try:
del self._map[jaus_address]
except KeyError as err:
self.logger.warning("Can not remove %s: %s" %
(jaus_address, err))
class UDSSocket(socket.socket):
'''
Wrapper for Unix Domain Sockets.
'''
def __init__(self,
name,
remove_on_close=False,
force_bind=False,
root_path='/tmp',
recv_buffer=5000,
loglevel='info'):
self._closed = False
self.logger = NMLogger('uds[%s]' % name, loglevel)
self._remove_on_close = remove_on_close
self._recv_buffer = recv_buffer
socket.socket.__init__(self, socket.AF_UNIX, socket.SOCK_DGRAM)
self.setblocking(True)
self._socket_path = os.path.join(root_path, name)
self._parser = MessageParser(AddressBook.Endpoint(
AddressBook.Endpoint.UDS, self._socket_path),
loglevel=loglevel)
if os.path.exists(self._socket_path) and not force_bind:
self.logger.debug("Connect to local socket %s" % self._socket_path)
self.connect(self._socket_path)
else:
self.logger.debug("Create local socket connection %s" %
self._socket_path)
if os.path.exists(self._socket_path):
os.unlink(self._socket_path)
self.bind(self._socket_path)
os.chmod(self._socket_path,
stat.S_IRWXU | stat.S_IRWXG | stat.S_IRWXO)
@property
def socket_path(self):
return self._socket_path
def reconnect(self):
self.logger.debug("Reconnect to local socket %s" % self._socket_path)
self.connect(self._socket_path)
def close(self):
'''
Close the socket.
'''
self._closed = True
self.logger.info("Close socket")
self.shutdown(socket.SHUT_RDWR)
socket.socket.close(self)
if self._remove_on_close:
try:
os.unlink(self._socket_path)
except OSError:
if os.path.exists(self._socket_path):
raise
def recv_msg(self):
'''
This method handles the received messages.
'''
running = True
while not self._closed and running:
try:
data = self.recv(self._recv_buffer)
if data:
return self._parser.unpack(data)
else:
running = False
except socket.error as err:
err = err.args[0]
if err == errno.EAGAIN or err == errno.EWOULDBLOCK:
#time.sleep(0.001)
continue
else:
running = False
if not self._closed:
self.logger.warning("Reported socket error: %s" %
traceback.format_exc())
return []
def send_msg(self, msg):
'''
This method sends the messages in the send queue.
'''
try:
val = self.send(msg.bytes())
return val == msg.raw_size
except Exception as e:
self.logger.warning("Error while send message [len: %d]: %s" %
(len(msg.bytes()), e))
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()]
class Config:
RECV_BUFFER = 5000 # 4094 should be also enough if MTU_Size = "4079"
RECV_TIMEOUT = 1
def __init__(self, filename='', version='', params={}):
self._stop = False
self._mutex = threading.RLock()
self.version = version
self.filename = filename
if not self.filename:
self.filename = os.path.expanduser('~/.config/iop.fkie/iop_node_manager.yaml')
cfg_path = os.path.dirname(self.filename)
if not os.path.isdir(cfg_path):
os.makedirs(cfg_path)
self._reload_callbacks = []
self._param_callbacks = {}
self._cfg = None
self.logger = NMLogger('config')
self.reload()
NMLogger.setall_loglevel(self.param('global/loglevel', 'info'))
self.apply(params, save=False)
self.msg_ids = {} # (int)id: (str)Name
self._read_msg_ids()
self._cfgif = None
self._cfgif_address = None
self.add_param_listener('global/loglevel', self._callback_change_loglevel)
def init_cfgif(self, cfgif=''):
try:
p_cfgif = cfgif
if not p_cfgif:
p_cfgif = self.param('global/cfgif', ':37940')
sliptres = p_cfgif.split(':')
host = ''
port = 37940
if len(sliptres) == 1:
if sliptres[0]:
port = int(sliptres[0])
else:
# empty: disable configuration interface
self.logger.info("Dynamic configuration disabled by empty parameter `global/cfgif`")
return
else:
host = sliptres[0]
port = int(sliptres[1])
self.logger.info("Listen for dynamic parameter @%s:%d" % (host, port))
# self._cfgif = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self._cfgif = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
self._cfgif.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
except Exception:
self.logger.warning("SO_REUSEPORT not available.")
self._cfgif_address = (host, port)
self._cfgif.bind((host, port))
self._cfgif.listen(2)
self._thread_listen_for_config = threading.Thread(target=self._recv_dynamic_parameter)
self._thread_listen_for_config.start()
except Exception as err:
print(traceback.format_exc())
self.logger.warning("Can not initilaize configuration UDP interface with param `%s`: %s" % (p_cfgif, err))
def close(self):
self._stop = True
if self._cfgif is not None:
self.logger.info("Close configuration socket")
try:
# we call socket.SHUT_RDWR to cancel bloking recv method
self._cfgif.shutdown(socket.SHUT_RD)
# connect to cancel accept()
# close_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# close_socket.connect(self._cfgif_address)
# close_socket.close()
except Exception:
self.logger.debug(traceback.format_exc())
self._cfgif.close()
def default(self):
'''
Creates a new default configuration.
Value supports follow tags: {:value, :min, :max, :default, :hint(str), :ro(bool)}
'''
result = {
'global': {
'version': {':value': self.version, ':ro': True},
'file': {':value': self.filename, ':ro': True},
'reset': {':value': False, ':hint': 'if this flag is set to True the configuration will be reseted'},
'loglevel': {':value': 'info', ':default': 'info', ':hint': 'Possible values: debug, info, warning, error, critical. (Default: info)'},
'cfgif': {':value': ':37940', ':default': ':37940', ':hint': 'Configuration interface. Disabled if empty. (Default: :37940)'}
},
'transport':
{
'local':
{
'root': {':value': '/tmp', ':default': '/tmp', ':hint': "The components communicate to the node manager through file sockets. For each component a new socket with it`s id will be created in this path."},
'nm_path': {':value': 'JuniorRTE', ':default': 'JuniorRTE', ':hint': "Contact socket name to the node manager."},
'queue_length': {':value': 100, ':default': 100, ':hint': "Maximal message count for each priority in the send queue."},
'send_buffer': {':value': 0, ':default': 0, ':hint': "Size of the send buffer. Zero do not changes the default buffer."}
},
'udp':
{
'port': {':value': 3794, ':default': 3794, ':hint': "By default, uses the port reserved for JAUS as assigned by the IANA for all UDP traffic. Changing the UDP port assignment is not recommended. (Default: 3794)."},
'use_mcast': {':value': True, ':default': True, ':hint': "If disabled, only unicast communication will be used. You need to specify the address book!"},
'group': {':value': '239.255.0.1', ':default': '239.255.0.1', ':hint': "Broadcasts are restricted to a multicast IP address. (Default: 239.255.0.1)."},
'ttl': {':value': 16, ':default': 16, ':hint': "Time to leave (Default: 16)."},
'queue_length': {':value': 100, ':default': 100, ':hint': "Maximal message count for each priority in the send queue."},
'interface': {':value': '', ':default': '', ':hint': "By default it binds to all network interfaces. You can bind it to specific one by specifying the address like 192.168.101.10"},
'send_buffer': {':value': 0, ':default': 0, ':hint': "Size of the send buffer. Zero do not changes the default buffer."},
},
'loopback_debug':
{
'enable': {':value': False, ':default': False, ':hint': "Enable to mirror messages sent on its local socket to a UDP port. (Default: False)"},
'port': {':value': 55555, ':default': 55555, ':hint': "Destination port (Default: 55555)."},
'address': {':value': '127.0.0.1', ':default': '127.0.0.1', ':hint': "IP that will show in Wireshark, used if `use_mcast` is False."},
'use_mcast': {':value': False, ':hint': "If disabled, unicast address is used in other case the message will be broadcasted to a multicast IP. (Default: False)"},
'group': {':value': '239.255.0.100', ':default': '239.255.0.100', ':hint': "If `use_mcast` is True, the messages are broadcasted to a multicast IP address. (Default: 239.255.0.100)."},
'interface': {':value': '', ':default': '', ':hint': "By default it binds to all network interfaces. You can bind it to specific one by specifying the address like 192.168.101.10. Only if use_mcast is set to `True`."},
'queue_length': {':value': 100, ':default': 100, ':hint': "Maximal message count for each priority in the send queue."}
},
'tcp':
{
'enable': {':value': False, ':default': False, ':hint': "Enable TCP communication. (Default: False)"},
'port': {':value': 3794, ':default': 3794, ':hint': "By default, uses the port reserved for JAUS as assigned by the IANA for all TCP traffic. Changing the TCP port assignment is not recommended. (Default: 3794)."},
'queue_length': {':value': 100, ':default': 100, ':hint': "Maximal message count for each priority in the send queue."},
'interface': {':value': '', ':default': '', ':hint': "By default it binds to all network interfaces. You can bind it to specific one by specifying the address like 192.168.101.10"},
},
},
'addrbook':
{
'udp': {
':value': {}, # {'192.168.0.2:3794': ['101.1.15', '101.255.255']},
':hint': "Dictionary entries: {IP:PORT : single or list Jaus ID}. Jaus ID can be unique or contain wildcards (subsystem: 65535, node or component: 255)."
},
'tcp': {
':value': {}, # {'128.7.92.233': '1.1.1', '128.7.92.114': '1.1.3'}, # {'192.168.0.2:3794': ['101.1.15', '101.255.255']},
':hint': "Dictionary entries: {IP:PORT : single or list Jaus ID}. Jaus ID can be unique or contain wildcards (subsystem: 65535, node or component: 255)."
}
},
'priority':
{
'override': {':value': False, ':hint': "Override priority for messages defined in config list."},
'map': {
':value': {
'0x000D': 3, # RequestControl: critical
'0x000F': 3, # ConfirmControl: critical
'0x4403': 1, # ReportLocalPose: high
'0xD742': 0 # ReportCostMap2D: low
},
':hint': "A dictionary of message id as HEX (0x1234) and priority (0: low, 1 standard, 2: hight, 3: critical)"
},
},
'statistics':
{
'enable': {':value': False, ':default': False, ':hint': "Enable statistics of routed messages. (Default: False)"},
'path': {':value': '~/.iop/statistics', ':default': '~/.iop/statistics', ':hint': "Directory where to save statistics", ':path': 'dir'},
'msg_names': {':value': 'msg.ids', ':default': 'msg.ids', ':hint': "File with mapping of message id to their name. In case of relative path the configuration path is taken.", ':path': 'file'}
}
}
return result
def param(self, param_name, default_value=None, extract_value=True):
'''
Returns parameter value for given param_name.
:param str param_name: name of the parameter. Namespace is separated by '/'.
:param default_value: returns this value if parameter was not found (Default: None)
:param bool extract_value: Since value is a dictionary with additional informations,
try to extract value by default on True or return all options by False (Default: True).
'''
result = default_value
try:
path = param_name.split('/')
value = self._cfg
# go through the path
for item in path:
value = value[item]
# extract value
if isinstance(value, dict):
if extract_value and ':value' in value:
result = value[':value']
else:
result = value
else:
result = value
except Exception as exc:
self.logger.debug("Cant't get parameter '%s', full parameter path: '%s'; return default: %s" % (exc, param_name, result))
return result
def set_param(self, param_name, value, tag=':value', save_on_change=True):
'''
Sets new value to a parameter. The parameter can contain namespaces separated by '/'.
Since a value can contain different tags, you can change the tag value
by specifying the tag parameter.
:param: str param_name: parameter name with namespaces.
:param: value: new value.
:param: str tag: tag name of parameter. It should begin with ':'.
'''
changed = False
try:
path = os.path.dirname(param_name).split('/')
val_tag = tag if tag else ':value'
cfg_item = self._cfg
for item in path:
if item:
if item in cfg_item:
cfg_item = cfg_item[item]
else:
cfg_item[item] = {}
cfg_item = cfg_item[item]
changed = True
pname = os.path.basename(param_name)
if pname in cfg_item:
if isinstance(cfg_item[pname], dict):
if self._is_writable(cfg_item[pname]):
changed = cfg_item[pname][val_tag] != value
cfg_item[pname][val_tag] = value
else:
raise Exception('%s is a read only parameter!' % param_name)
else:
changed = cfg_item[pname] != value
cfg_item[pname] = value
else:
# create new parameter entry
cfg_item[pname] = {val_tag: value}
changed = True
if changed:
if save_on_change:
self.save()
self._notify_param_listener(param_name, value)
return True
except Exception as exc:
self.logger.debug("Cant't set parameter '%s': '%s'" % (param_name, exc))
return False
def reload(self):
'''
Load the configuration from file. If file does not exists default configuration will be used.
After configuration is loaded all subscribers are notified.
'''
with self._mutex:
try:
with open(self.filename, 'r') as stream:
result = ruamel.yaml.load(stream, Loader=ruamel.yaml.Loader)
if result is None:
self.logger.info('reset configuration file %s' % self.filename)
self._cfg = self.default()
self.save()
else:
self.logger.info('loaded configuration from %s' % self.filename)
self._cfg = result
except (ruamel.yaml.YAMLError, IOError) as exc:
self.logger.info('%s: use default configuration!' % exc)
self._cfg = self.default()
self._notify_reload_listener()
def save(self, reset=False, save_msg_ids=False):
'''
Saves current configuration to file.
'''
if reset:
self._cfg = self.default()
with open(self.filename, 'w') as stream:
try:
ruamel.yaml.dump(self._cfg, stream, Dumper=ruamel.yaml.RoundTripDumper)
self.logger.debug("Configuration saved to '%s'" % self.filename)
except ruamel.yaml.YAMLError as exc:
self.logger.warning("Cant't save configuration to '%s': %s" % (self.filename, exc))
if save_msg_ids:
filename = self.param('statistics/msg_names', 'msg.ids')
if not os.path.isabs(filename):
filename = os.path.join(os.path.dirname(self.filename), filename)
with open(filename, 'w+') as fp:
for key, name in self.msg_ids.items():
line = '0x%.4x %s\n' % (key, name)
fp.write(line)
def yaml(self, _nslist=[]):
'''
:param list nslist: Filter option. Currently not used!
:return: Create YAML string representation from configuration dictionary structure.
:rtype: str
'''
return ruamel.yaml.dump(self._cfg)
def apply(self, data, save=True):
'''
Applies data (string representation of YAML).
After new data are set the configuration will be saved to file.
All subscribers are notified.
:param str data: YAML as string representation.
'''
with self._mutex:
data_dict = data
if type(data) != dict:
data_dict = ruamel.yaml.load(data, Loader=ruamel.yaml.Loader)
self._cfg = self._apply_recursive(data_dict, self._cfg)
do_reset = self.param('global/reset', False)
if do_reset:
self.logger.info("Reset configuration requested!")
self._cfg = self.default()
else:
self.logger.debug("new configuration applied, save now.")
if save:
self.save()
self._notify_reload_listener()
def _apply_recursive(self, new_data, curr_data, path=''):
new_cfg = dict(curr_data)
if path:
path = "%s:" % path
if not curr_data:
return new_data
for key, value in new_data.items():
try:
curr_value = None
if key in new_cfg:
curr_value = new_cfg[key]
if self._is_writable(value):
if isinstance(curr_value, dict):
if isinstance(value, dict):
new_cfg[key] = self._apply_recursive(value, curr_value, "%s%s" % (path, key))
elif key not in [':hint', ':default', ':ro', ':min', ':max', ':alt']:
new_cfg[key][':value'] = value
elif isinstance(curr_value, dict):
new_cfg[key] = value
else:
self.logger.warning("added new configuration key: %s%s: %s" % (path, key, value))
new_cfg[key] = value
except Exception:
import traceback
self.logger.warning("_apply_recursive error: %s, use old value: %s" % (traceback.format_exc(), str(value)))
new_cfg[key] = value
return new_cfg
def _is_writable(self, value):
if isinstance(value, dict) and ':ro' in value:
return value[':ro']
return True
def _callback_change_loglevel(self, param, loglevel):
NMLogger.setall_loglevel(loglevel)
def add_reload_listener(self, callback, call=True):
'''
Adds a subscriber to change notifications. All subscribers are notified on any changes.
:param callback: Method of type callback(Settings)
:param call: if True the callback is called after adding. (Default: True)
'''
with self._mutex:
if callback not in self._reload_callbacks:
self._reload_callbacks.append(callback)
if call:
callback(self)
def _notify_reload_listener(self):
with self._mutex:
for callback in self._reload_callbacks:
callback(self)
def add_param_listener(self, paramname, callback):
'''
Adds a subscriber to dynamic changes of a specific parameter.
:param str paramname: Name of the parameter
:param callback: Method of type callback(paramname[str], value)
:param call: if True the callback is called after adding. (Default: True)
'''
if paramname not in self._param_callbacks:
self._param_callbacks[paramname] = []
self.logger.debug("Add parameter listener %s for %s" % (callback, paramname))
self._param_callbacks[paramname].append(callback)
def _notify_param_listener(self, paramname, value):
for pn, callbacks in self._param_callbacks.items():
if paramname.startswith(pn):
for callback in callbacks:
callback(paramname, value)
def _recv_dynamic_parameter(self):
while not self._stop:
try:
connection, addr = self._cfgif.accept()
if not self._stop:
data = connection.recv(65535)
for line in data.splitlines():
pv = line.decode("utf-8") .split(': ')
self.logger.info("Parameter change requested from %s: %s" % (addr, pv))
if len(pv) == 2:
try:
if pv[0] in self._param_callbacks:
for clbk in self._param_callbacks[pv[0]]:
clbk(pv[0], pv[1])
else:
self.logger.warning("No callback for dynamic parameter found: %s" % pv)
except Exception as perr:
self.logger.warning("Error while notify parameter listener on changed parameter '%s': %s" % (pv[0], perr))
connection.close()
except Exception as err:
if hasattr(err, 'errno') and err.errno == 22:
# handle shutdown
return
print(traceback.format_exc())
self.logger.warning("Error while receive configuration through UDP interface on %s: %s" % (self._cfgif_address, err))
# raise Exception(traceback.format_exc())
def msg_name(self, msgid):
'''
Returns to given message id a name if exists.
Returns `Unknown` if id not found.
'''
try:
return self.msg_ids[msgid]
except KeyError:
pass
return 'Unknown'
def _read_msg_ids(self):
# try load from file
filename = self.param('statistics/msg_names', 'msg.ids')
incfg = False
if not os.path.isabs(filename):
filename = os.path.join(os.path.dirname(self.filename), filename)
incfg = True
if os.path.exists(filename):
self.logger.info("read message ids from %s" % filename)
with open(filename, 'r') as fp:
for line in fp.readlines():
try:
kn = line.rstrip('\n').split()
self.msg_ids[int(kn[0], 16)] = kn[1]
except Exception:
print(traceback.format_exc())
elif incfg:
self.logger.info("create default message id list")
# create default list
self._init_msgs_ids()
def _init_msgs_ids(self):
self.msg_ids[int('2B00', 16)] = 'QueryIdentification'
self.msg_ids[int('000D', 16)] = 'RequestControl'
self.msg_ids[int('200D', 16)] = 'QueryControl'
self.msg_ids[int('21F0', 16)] = 'QueryEvents'
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
def _callback_change_loglevel(self, param, loglevel):
NMLogger.setall_loglevel(loglevel)
class UDPucSocket(socket.socket):
def __init__(self,
port=0,
router=None,
interface='',
logger_name='udp',
default_dst=None,
send_buffer=0,
recv_buffer=0,
queue_length=0,
loglevel='info'):
'''
Creates a socket, bind it to a given interface+port for unicast send/receive.
IPv4 and IPv6 are supported.
:param int port: the port to bind the socket. If zero an empty one will be used.
:param router: class which provides `route_udp_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
:param tuple(str,int) default_dst: used for loopback to send messages to predefined destination.
'''
self._closed = False
self.logger = NMLogger('%s[%s:%d]' % (logger_name, interface, port),
loglevel)
self.interface = interface
self.port = port
self._router = router
self._default_dst = default_dst
self._recv_buffer = recv_buffer
self._sender_endpoints = {}
self.sock_5_error_printed = []
# If interface isn't specified, try to find an non localhost interface to
# get some info for binding. Otherwise use localhost
# if not self.interface:
# ifaces = localifs()
# for iface in ifaces:
# if not (iface[1].startswith('127') or iface[1].startswith('::1')):
# self.interface = iface[1]
# break
self.logger.info("+ Bind to unicast socket @(%s:%s)" %
(self.interface, port))
socket_type = socket.AF_INET
bind_ip = self.interface
if self.interface:
addrinfo = getaddrinfo(self.interface)
socket_type = addrinfo[0]
bind_ip = addrinfo[4][0]
# Configure socket type
socket.socket.__init__(self, socket_type, socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
# Bind to the port
try:
self.logger.debug("Ucast bind to: (%s:%s)" % (bind_ip, port))
self.bind((bind_ip, port))
except socket.error as errobj:
msg = str(errobj)
self.logger.critical(
"Unable to bind unicast to interface: %s, check that it exists: %s"
% (bind_ip, msg))
raise
if self.port == 0:
self.port = self.getsockname()[1]
if send_buffer:
# update buffer size
old_bufsize = self.getsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF)
if old_bufsize != send_buffer:
self.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF,
send_buffer)
# self.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, buffersize)
bufsize = self.getsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF)
self.logger.debug("Changed buffer size from %d to %d" %
(old_bufsize, bufsize))
self._parser_ucast = MessageParser(None, loglevel=loglevel)
self._queue_send = queue.PQueue(queue_length,
'queue_%s_send' % logger_name,
loglevel=loglevel)
# create a thread to handle the received unicast messages
if self._router is not None:
self._thread_recv = threading.Thread(target=self._loop_recv)
self._thread_recv.start()
self._thread_send = threading.Thread(target=self._loop_send)
self._thread_send.start()
def close(self):
""" Cleanup and close the socket"""
self._closed = True
self.logger.info("Close unicast socket")
try:
# shutdown to cancel recvfrom()
socket.socket.shutdown(self, socket.SHUT_RD)
except socket.error:
pass
socket.socket.close(self)
self._queue_send.clear()
def send_queued(self, msg):
try:
self._queue_send.put(msg)
except queue.Full as full:
print(traceback.format_exc())
self.logger.warning("Can't send message: %s" % full)
except Exception as e:
self.logger.warning("Error while put message into queue: %s" % e)
def _loop_send(self):
while not self._closed:
# Waits for next available Message. This method cancel waiting on clear() of PQueue and return None.
msg = self._queue_send.get()
if msg is not None:
dst = msg.tinfo_dst
if self._default_dst is not None:
# it is a loopback socket, send to fictive debug destination
dst = AddressBook.Endpoint(AddressBook.Endpoint.UDP,
self._default_dst[0],
self._default_dst[1])
if dst is not None:
# send to given addresses
self._sendto(msg.bytes(), dst.address, dst.port)
# TODO: add retry mechanism?
def _sendto(self, msg, addr, port):
'''
Sends the given message to the joined multicast group. Some errors on send
will be ignored (``ENETRESET``, ``ENETDOWN``, ``ENETUNREACH``)
:param str msg: message to send
:param str addr: IPv4 or IPv6 address
:param int port: destination port
'''
try:
self.logger.debug("Send to %s:%d" % (addr, port))
self.sendto(msg, (addr, port))
except socket.error as errobj:
msg = str(errobj)
if errobj.errno in [-5]:
if addr not in self.sock_5_error_printed:
self.logger.warning("socket.error[%d]: %s, addr: %s" %
(errobj.errno, msg, addr))
self.sock_5_error_printed.append(addr)
elif errobj.errno in [errno.EINVAL, -2]:
raise
elif errobj.errno not in [
errno.ENETDOWN, errno.ENETUNREACH, errno.ENETRESET
]:
raise
def _loop_recv(self):
'''
This method handles the received unicast messages.
'''
while not self._closed:
try:
(data, address) = self.recvfrom(self._recv_buffer)
if data and not self._closed:
msgs = self._parser_ucast.unpack(data)
for msg in msgs:
try:
msg.tinfo_src = self._sender_endpoints[address]
except KeyError:
endpoint = AddressBook.Endpoint(
AddressBook.Endpoint.UDP, address[0],
address[1])
msg.tinfo_src = endpoint
self._sender_endpoints[address] = endpoint
self.logger.debug("Received from %s" % (msg.tinfo_src))
self._router.route_udp_msg(msg)
except queue.Full as full_error:
self.logger.warning(
"Error while process received unicast message: %s" %
full_error)
except socket.error:
if not self._closed:
self.logger.warning("unicast socket error: %s" %
traceback.format_exc())
