Initial commit

Lts/Tools/strefu_sim.py

@@ -0,0 +1,430 @@
+#! /usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+#
+# @RDE_HEADER@
+# $Id$
+# $HeadURL$
+# $Author$
+# Created by Christian Fröbel, NLE, 2019-01-29
+#
+
+import socket
+import sys
+import textwrap
+import threading
+import weakref
+from argparse import ArgumentParser, RawDescriptionHelpFormatter
+from collections import namedtuple
+from contextlib import ExitStack, contextmanager
+from queue import Empty, Queue
+from socketserver import StreamRequestHandler, TCPServer, ThreadingMixIn
+from threading import Lock, Thread
+
+
+@contextmanager
+def autoShutdown(target):
+    try:
+        yield target
+    finally:
+        target.initShutdown()
+
+@contextmanager
+def autoJoin(thread):
+    try:
+        yield thread
+    finally:
+        thread.join()
+
+@contextmanager
+def startThreadContext(target, thread):
+    with ExitStack() as stack:
+        thread.start()
+        stack.enter_context(autoJoin(thread))
+        stack.enter_context(autoShutdown(target))
+        yield
+
+
+#
+# Hier registrieren sich die Sender für die Verbindungen zu den AVS_Interface-Instanzen.
+#
+class _Registry(object):
+    def __init__(self):
+        self._senders = []
+        self._mutex = Lock()
+
+    @property
+    def senders(self):
+        with self._mutex:
+            return tuple(self._senders)
+
+    def register(self, sender):
+        with self._mutex:
+            assert sender not in self._senders, 'sender is registered already'
+            self._senders.append(sender)
+            print('client count: {}'.format(len(self._senders)))
+
+    def unregister(self, sender):
+        with self._mutex:
+            try:
+                self._senders.remove(sender)
+            except ValueError:
+                assert False, 'sender is not registered'
+            print('client count: {}'.format(len(self._senders)))
+
+
+class _Server(ThreadingMixIn, TCPServer):
+    allow_reuse_address = True
+    timeout = 1
+
+
+class _Acceptor(object):
+    def __init__(self, port, registry, handlerFactory):
+        self._port = port
+        self._registry = registry
+        self._handlerFactory = handlerFactory
+        self._doRun = True
+
+    def run(self):
+        server = _Server(('0.0.0.0', self._port), self._handlerFactory)
+        while self._doRun:
+            server.handle_request() # mit Timeout
+            self._joinFinishedHandlers()
+
+        self._joinAllHandlers()
+
+    def initShutdown(self):
+        self._doRun = False
+
+    def _joinFinishedHandlers(self):
+        for handler in self._registry.senders:
+            if not handler.isAlive():
+                handler.join(0) # ohne Timeout
+
+    def _joinAllHandlers(self):
+        for handler in self._registry.senders:
+            handler.initShutdown()
+        for handler in self._registry.senders:
+            handler.join()
+
+
+class _RequestHandlerFactory(object):
+    def __init__(self, cmdQueue, registry, channelManager):
+        self._cmdQueue = cmdQueue
+        self._registry = registry
+        self._channelManager = channelManager
+
+    def __call__(self, *args):
+        return _RequestHandler(self._cmdQueue, self._registry, self._channelManager, *args)
+
+
+#
+# Läuft in einem separaten Thread. Den Thread setzen wir aber nicht selbst auf.
+# Daher müssen wir den erst ermitteln.
+#
+class _RequestHandler(StreamRequestHandler):
+    def __init__(self, cmdQueue, registry, channelManager, *args):
+        # Reihenfolge wichtig; super() muss am Ende stehen
+        self._cmdQueue = cmdQueue
+        self._registry = registry
+        self._channelManager = channelManager
+        self._thread = None
+        super().__init__(*args)
+
+    def handle(self):
+        print('new connection from: {}'.format(self.client_address))
+        self._thread = threading.current_thread()
+        self._registry.register(self)
+        self._channelManager.activateAllChannels(self)
+        while True:
+            line = self.rfile.readline().decode('utf-8')
+            if len(line) == 0: # EOF
+                break
+            self._process(line)
+
+    def send(self, line):
+        terminatedLine = line + '\r\n'
+        self.wfile.write(terminatedLine.encode('utf-8'))
+
+    def isAlive(self):
+        return self._thread.is_alive()
+
+    def join(self, timeout=None):
+        self._thread.join(timeout)
+        self._registry.unregister(self)
+
+    # Schließen des Sockets (der Leseseite), damit der Service aus dem blockierenden read() rauskommt.
+    def initShutdown(self):
+        _shutdownAndCloseSocket(self.request)
+
+    def _process(self, line):
+        cmd = self._parseLine(line)
+        self.send('200 ok')
+        self._cmdQueue.put(cmd)
+
+    # Zeile parsen und AvsCommand liefern
+    def _parseLine(self, line):
+        parts = line.split()
+        token = parts.pop(0).lower()
+        args = parts
+        if token == 'speechreq':
+            assert len(args) == 1, 'invalid command format'
+        elif token == 'speechreqwd':
+            assert len(args) == 1, 'invalid command format'
+        elif token == 'channelusage':
+            assert len(args) == 3, 'invalid command format'
+        elif token == 'configchanged':
+            assert len(args) == 0, 'invalid command format'
+        elif token == 'keepalive':
+            assert len(args) == 0, 'invalid command format'
+        else:
+            assert False, 'unknown command: {}'.format(line)
+
+        return AvsCommand(token, args)
+
+
+AvsCommand = namedtuple('AvsCommand', 'token, args')
+
+
+#
+# Liest die Control-Commands aus einem file-like Stream (z.B. stdin).
+# Sollte im Main-Thread laufen, weil hier auch die Behandlung von KeyboardInterrupt (Ctrl-C) gemacht wird.
+#
+class _ControlRequestHandler(object):
+
+    def __init__(self, inputStream, cmdQueue):
+        self._inputStream = inputStream
+        self._cmdQueue = cmdQueue
+
+    def handle(self):
+        try:
+            self._handle()
+        except KeyboardInterrupt:
+            pass
+
+    def _handle(self):
+        while True:
+            line = self._inputStream.readline()
+            if len(line) == 0: # EOF
+                break
+            self._process(line)
+
+    def _process(self, line):
+        cmd = self._parseLine(line.rstrip())
+        if cmd is not None:
+            self._cmdQueue.put(cmd)
+
+    def _parseLine(self, line):
+        parts = line.split()
+        if len(parts) == 0:
+            return None
+        token = parts.pop(0).lower()
+        args = parts
+        if token == 'speechreq':
+            assert len(args) == 1, 'invalid command format'
+        elif token == 'speechreqwd':
+            assert len(args) == 1, 'invalid command format'
+        else:
+            print('unknown command: {}'.format(line))
+            return None
+
+        return CtrlCommand(token, args)
+
+CtrlCommand = namedtuple('CtrlCommand', 'token, args')
+
+
+class _CommandExecutor(object):
+    def __init__(self, cmdQueue, channelManager):
+        self._cmdQueue = cmdQueue
+        self._channelManager = channelManager
+        self._doRun = True
+
+    def run(self):
+        while True:
+            try:
+                cmd = self._cmdQueue.get(timeout=1)
+            except Empty:
+                if self._doRun:
+                    continue
+                else:
+                    break
+            self._process(cmd)
+
+    def initShutdown(self):
+        self._doRun = False
+
+    def _process(self, cmd):
+        if cmd.token in ('speechreq', 'speechreqwd'):
+            self._channelManager.handle(cmd)
+            return
+        # den Rest ignorieren
+
+
+class _ChannelState(object):
+    def __init__(self, channel, registry):
+        self._channel  = channel
+        self._registry = registry
+
+    def handle(self, cmd):
+        if isinstance(cmd, AvsCommand):
+            return self._handleAvsCommand(cmd)
+        else:
+            return self._handleCtrlCommand(cmd)
+
+    def _handleAvsCommand(self, cmd):
+        assert False, 'must be implemented'
+
+    def _handleCtrlCommand(self, cmd):
+        assert False, 'must be implemented'
+
+    def _send(self, line):
+        for sender in self._registry.senders:
+            sender.send(line)
+
+
+class _Idle(_ChannelState):
+    def _handleAvsCommand(self, cmd):
+        if cmd.token == 'speechreq':
+            self._send('205 {} speechgrantop'.format(self._channel.ssrc))
+            return self._channel.avsTalking
+        return self
+
+    def _handleCtrlCommand(self, cmd):
+        if cmd.token == 'speechreq':
+            self._send('206 {} speechgrant'.format(self._channel.ssrc))
+            return self._channel.fuGeTalking
+        return self
+
+    def __str__(self):
+        return '{}: Idle'.format(self._channel.ssrc)
+
+
+class _AvsTalking(_ChannelState):
+    def _handleAvsCommand(self, cmd):
+        if cmd.token == 'speechreqwd':
+            self._send('207 {} speechreqwd'.format(self._channel.ssrc))
+            return self._channel.idle
+        return self
+
+    def _handleCtrlCommand(self, cmd):
+        return self
+
+    def __str__(self):
+        return '{}: AvsTalking'.format(self._channel.ssrc)
+
+
+class _FuGeTalking(_ChannelState):
+    def _handleAvsCommand(self, cmd):
+        return self
+
+    def _handleCtrlCommand(self, cmd):
+        if cmd.token == 'speechreqwd':
+            self._send('207 {} speechreqwd'.format(self._channel.ssrc))
+            return self._channel.idle
+        return self
+
+    def __str__(self):
+        return '{}: FuGeTalking'.format(self._channel.ssrc)
+
+
+class _Channel(object):
+    def __init__(self, ssrc, registry):
+        self._ssrc         = ssrc
+        self._idle         = _Idle(weakref.proxy(self), registry)
+        self._avsTalking   = _AvsTalking(weakref.proxy(self), registry)
+        self._fuGeTalking  = _FuGeTalking(weakref.proxy(self), registry)
+        self._currentState = self._idle
+
+    @property
+    def ssrc(self): return self._ssrc
+    @property
+    def idle(self): return self._idle
+    @property
+    def avsTalking(self): return self._avsTalking
+    @property
+    def fuGeTalking(self): return self._fuGeTalking
+
+    def handle(self, cmd):
+        self._currentState = self._currentState.handle(cmd)
+        print(self._currentState)
+
+
+class _ChannelManager(object):
+    def __init__(self, ssrcStart, channelCount, registry):
+        self._ssrcStart = ssrcStart
+        self._channels = []
+        for i in range(channelCount):
+            ssrc = i + self._ssrcStart
+            self._channels.append(_Channel(ssrc, registry))
+
+    def handle(self, cmd):
+        assert cmd.token in ('speechreq', 'speechreqwd'), 'cannot handle command: {} {}'.format(cmd.token, cmd.args)
+        try:
+            ssrc = int(cmd.args[0])
+        except (ValueError, IndexError):
+            assert False, 'ssrc expected as first arg: {} {}'.format(cmd.token, cmd.args)
+
+        index = ssrc - self._ssrcStart
+        assert index >= 0 and index < len(self._channels), 'ssrc out of range: {}'.format(ssrc)
+        channel = self._channels[index]
+        channel.handle(cmd)
+
+    def activateAllChannels(self, sender):
+        for c in self._channels:
+            sender.send('208 {} trcallactiv'.format(c.ssrc))
+
+
+def _shutdownAndCloseSocket(sock):
+    try:
+        sock.shutdown(socket.SHUT_RD) # wichtig, close() alleine reicht nicht
+    except socket.error:
+        print('socket error')
+        # kann passieren, wenn der Socket schon geschlossen wurde
+        pass
+    sock.close()
+
+
+def _parseCmdLine():
+    parser = ArgumentParser(description=textwrap.dedent('''
+        Simulator for the StReFu (Steuerrechner Funk).
+        The program simulates the interface described in GUEZSB60_ICD-0059_Anb-TrSpFu_AVS.
+        It provides a port where AVS_Interface instances can connect to.
+        At the console (stdin) it waits for control messages. With the following messages
+        a user can indicate that a radio unit was triggered or that the trigger was released:
+        - speechreq <ssrc>
+        - speechreqwd <ssrc>
+        The implemented channel management is very simple. The first request for a channel --
+        either from AVS or from the user -- wins. The channel stays allocated until it is
+        explicitly released.
+        The data streams for each channel must be provided separately.
+    '''),
+    formatter_class=RawDescriptionHelpFormatter)
+    parser.add_argument('--listen', '-l',        type=int, metavar='N', default=8300, help='Listening port for connections from AVS_Interface; default: 8300')
+    parser.add_argument('--ssrc-start',          type=int, metavar='N', default=1001, help='SSRC of the first channel; default: 1001')
+    parser.add_argument('--channel-count', '-c', type=int, metavar='N', default=56,   help='Number of CNR channels; default: 56')
+    return parser.parse_args()
+
+
+def _main():
+    options = _parseCmdLine()
+
+    cmdQueue       = Queue()
+    registry       = _Registry()
+    channelManager = _ChannelManager(options.ssrc_start, options.channel_count, registry)
+    handlerFactory = _RequestHandlerFactory(cmdQueue, registry, channelManager)
+    acceptor       = _Acceptor(options.listen, registry, handlerFactory)
+    acceptorThread = Thread(target=acceptor.run)
+
+    executor       = _CommandExecutor(cmdQueue, channelManager)
+    executorThread = Thread(target=executor.run)
+
+    controlRequestHandler = _ControlRequestHandler(sys.stdin, cmdQueue)
+
+    with ExitStack() as stack:
+        stack.enter_context(startThreadContext(executor, executorThread))
+        stack.enter_context(startThreadContext(acceptor, acceptorThread))
+        controlRequestHandler.handle() # beendet sich bei EOF oder Ctrl-C
+
+
+if __name__ == '__main__':
+    _main()