git.gag.com Git - debian/gnuradio/blob - gnuradio-examples/python/networking/measurement_slave.py

   1 #!/usr/bin/env python
   2 #
   3 # Copyright 2006 Free Software Foundation, Inc.
   4 #
   5 # This file is part of GNU Radio
   6 #
   7 # GNU Radio is free software; you can redistribute it and/or modify
   8 # it under the terms of the GNU General Public License as published by
   9 # the Free Software Foundation; either version 2, or (at your option)
  10 # any later version.
  11 #
  12 # GNU Radio is distributed in the hope that it will be useful,
  13 # but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 # GNU General Public License for more details.
  16 #
  17 # You should have received a copy of the GNU General Public License
  18 # along with GNU Radio; see the file COPYING.  If not, write to
  19 # the Free Software Foundation, Inc., 51 Franklin Street,
  20 # Boston, MA 02110-1301, USA.
  21 #
  22
  23 import struct
  24 import socket
  25 import asyncore
  26 import sys
  27 import optparse
  28 import random
  29 from gnuradio.eng_option import eng_option
  30 import gnuradio.gr.gr_threading as _threading
  31
  32 LOOP_TIMEOUT = 0.001
  33 BROADCAST_ADDR = '255.255.255.255'
  34
  35 BROADCAST_PORT = 27010   # UDP
  36 CONTROL_PORT   = 27011   # TCP
  37
  38 PKT_HEADER_SIZE = 4      # 32-bit int
  39
  40 logfile = None
  41
  42 def unpack_header(s):
  43     (len,) = struct.unpack('!i', s)
  44     return (len,)
  45
  46 def pack_header(len):
  47     return struct.pack('!i', len)
  48
  49
  50 class control_port_listener(asyncore.dispatcher):
  51     def __init__(self, port=CONTROL_PORT, udp_socket=None, verbose=False):
  52         """
  53         @param port: TCP port to listen on.
  54         @type port: int
  55         """
  56         asyncore.dispatcher.__init__(self)
  57
  58         self._verbose = verbose
  59         self._udp_socket = udp_socket
  60
  61         host = ''                       # symbolic name for localhost
  62         self.create_socket(socket.AF_INET, socket.SOCK_STREAM)
  63         self.set_reuse_addr()
  64
  65         try:
  66             self.bind((host, port))
  67         except socket.error, err:
  68             sys.stderr.write('Failed to bind to %s: %s\n' %
  69                              ((host, port), os.strerror (err.args[0]),))
  70             sys.exit(1)
  71
  72         self.listen(3)
  73
  74     def handle_accept(self):
  75         socket, addr = self.accept()
  76         sys.stderr.write("handle_accept: %r\n" % (addr,))
  77         if not(socket is None):
  78             # instantiate a server
  79             s = control_server(socket, addr, self._udp_socket, self._verbose)
  80
  81
  82 class gr_dispatcher(asyncore.dispatcher):
  83     def __init__(self, sock=None, map=None):
  84         asyncore.dispatcher.__init__(self, sock=sock, map=map)
  85
  86     def read_packet(self):
  87         """
  88         Returns None or packet
  89         """
  90         s = self.recvall(PKT_HEADER_SIZE)
  91         if s == '':
  92             return None
  93
  94         (payload_len,) = unpack_header(s)
  95
  96         payload_len = int(payload_len)
  97
  98         if payload_len == 0:
  99             payload = ''
 100         else:
 101             payload = self.recvall(payload_len)
 102             if len(payload) != payload_len:
 103                 sys.stderr.write ('short recv, expected %d bytes, got %d\n' % (
 104                     payload_len, len(payload)))
 105                 raise RuntimeError, "short recv"
 106
 107         return payload
 108
 109     def recvall(self, buffer_size):
 110         result = ''
 111         while len(result) < buffer_size:
 112             data = self.recv(buffer_size - len(result))
 113             if not data:
 114                 return ''
 115             result += data
 116         return result
 117
 118
 119 class pkt_receiver_thread(_threading.Thread):
 120     def __init__(self, socket):
 121         _threading.Thread.__init__(self)
 122         self.setDaemon(1)
 123         self.socket = socket
 124         self.keep_running = True
 125         self.start()
 126
 127     def run(self):
 128         while self.keep_running:
 129             pkt, sender = self.socket.recvfrom(10000)
 130             if pkt:
 131                 if len(pkt) > 2:
 132                     t = struct.unpack('!H', pkt[0:2])
 133                     seqno = t[0]
 134                 else:
 135                     seqno = -1
 136
 137                 logfile.write('RCVD seqno %4d len %4d from %s\n' % (seqno, len(pkt), sender))
 138                 logfile.flush()
 139
 140
 141 class control_server(gr_dispatcher):
 142     def __init__(self, socket, addr, udp_socket, verbose=False):
 143         gr_dispatcher.__init__(self, sock=socket)
 144
 145         self._udp_socket = udp_socket
 146         self.verbose = verbose
 147         self.setblocking(1)
 148
 149     def writable(self):
 150         return False
 151
 152     def handle_read(self):
 153         pkt = self.read_packet()
 154         if pkt:
 155             annotate = 'ANNOTATE'
 156             if pkt.startswith(annotate):
 157                 logfile.write(pkt[len(annotate)+1:])
 158                 logfile.write('\n')
 159                 logfile.flush()
 160             elif pkt.startswith('SEND'):
 161                 tokens = pkt.split()
 162                 if len(tokens) < 4:
 163                     invalid_packet(pkt)
 164                 else:
 165                     npkts = int(tokens[1])
 166                     size = int(tokens[2])
 167                     power = float(tokens[3])
 168                     send_test_packets(self._udp_socket, npkts, size, power)
 169             else:
 170                 invalid_packet(pkt)
 171
 172     def handle_close(self):
 173         self.close()
 174
 175
 176 def invalid_packet(pkt):
 177     sys.stderr.write('received unrecognized packet: %s\n' % (pkt,))
 178
 179
 180 def make_random_payload(size):
 181     p = [0] * size
 182     if 1:
 183         for i in range(size):
 184             p[i] = chr(random.randint(0, 255))
 185     else:
 186         for i in range(size):
 187             p[i] = chr(i % 256)
 188     return ''.join(p)
 189
 190
 191 def send_test_packets(udp_socket, npkts, size, power):
 192     # we ignore power for now...
 193     size = max(2, size)
 194     payload = make_random_payload(size - 2)
 195     for n in range(npkts):
 196         pkt = struct.pack('!H', n) + payload
 197         udp_socket.sendto(pkt, (BROADCAST_ADDR, BROADCAST_PORT))
 198         #udp_socket.sendall(pkt)
 199
 200
 201 def open_udp_broadcast_socket(gr0_host_ip, port):
 202     s  = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
 203 #    s.bind((gr0_host_ip, port))
 204     s.bind(('', port))
 205     s.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
 206 #    s.connect((BROADCAST_ADDR, port))
 207     return s
 208
 209
 210 def main():
 211     global logfile
 212
 213     usage = 'usage: %prog [options] gr0-ip-addr'
 214     parser = optparse.OptionParser (option_class=eng_option, usage=usage)
 215     parser.add_option('-l', '--logfile', type='string', default=None,
 216                       help="specify log file name [default=<stdout>]")
 217     parser.add_option('-v', '--verbose', action="store_true", default=False,
 218                       help="enable verbose diagnostics")
 219
 220     (options, args) = parser.parse_args ()
 221     if len(args) != 1:
 222         parser.print_help()
 223         sys.exit(1)
 224
 225     gr0_ip_addr = args[0]
 226     if options.logfile is None:
 227         logfile = sys.stdout
 228     else:
 229         logfile = file(options.logfile, 'w')
 230
 231     udp_socket = open_udp_broadcast_socket(gr0_ip_addr, BROADCAST_PORT)
 232     R = pkt_receiver_thread(udp_socket)
 233     L = control_port_listener(CONTROL_PORT, udp_socket=udp_socket, verbose=options.verbose)
 234     asyncore.loop(LOOP_TIMEOUT)
 235
 236
 237 if __name__ == '__main__':
 238     try:
 239         main()
 240     except KeyboardInterrupt:
 241         pass