git.sesse.net Git - pistorm/blob - a314/files_pi/ethernet.py

   1 #!/usr/bin/python3
   2 # -*- coding: utf-8 -*-
   3
   4 # Copyright (c) 2020 Niklas Ekström
   5
   6 import logging
   7 import os
   8 import pytun
   9 import select
  10 import socket
  11 import struct
  12 import sys
  13 import time
  14
  15 logging.basicConfig(format = '%(levelname)s, %(asctime)s, %(name)s, line %(lineno)d: %(message)s')
  16 logger = logging.getLogger(__name__)
  17 logger.setLevel(logging.INFO)
  18
  19 MSG_REGISTER_REQ        = 1
  20 MSG_REGISTER_RES        = 2
  21 MSG_DEREGISTER_REQ      = 3
  22 MSG_DEREGISTER_RES      = 4
  23 MSG_READ_MEM_REQ        = 5
  24 MSG_READ_MEM_RES        = 6
  25 MSG_WRITE_MEM_REQ       = 7
  26 MSG_WRITE_MEM_RES       = 8
  27 MSG_CONNECT             = 9
  28 MSG_CONNECT_RESPONSE    = 10
  29 MSG_DATA                = 11
  30 MSG_EOS                 = 12
  31 MSG_RESET               = 13
  32
  33 def wait_for_msg():
  34     header = b''
  35     while len(header) < 9:
  36         data = drv.recv(9 - len(header))
  37         if not data:
  38             logger.error('Connection to a314d was closed, terminating.')
  39             exit(-1)
  40         header += data
  41     (plen, stream_id, ptype) = struct.unpack('=IIB', header)
  42     payload = b''
  43     while len(payload) < plen:
  44         data = drv.recv(plen - len(payload))
  45         if not data:
  46             logger.error('Connection to a314d was closed, terminating.')
  47             exit(-1)
  48         payload += data
  49     return (stream_id, ptype, payload)
  50
  51 def send_register_req(name):
  52     m = struct.pack('=IIB', len(name), 0, MSG_REGISTER_REQ) + name
  53     drv.sendall(m)
  54
  55 def send_read_mem_req(address, length):
  56     m = struct.pack('=IIBII', 8, 0, MSG_READ_MEM_REQ, address, length)
  57     drv.sendall(m)
  58
  59 def read_mem(address, length):
  60     send_read_mem_req(address, length)
  61     _, ptype, payload = wait_for_msg()
  62     if ptype != MSG_READ_MEM_RES:
  63         logger.error('Expected MSG_READ_MEM_RES but got %s. Shutting down.', ptype)
  64         exit(-1)
  65     return payload
  66
  67 def send_write_mem_req(address, data):
  68     m = struct.pack('=IIBI', 4 + len(data), 0, MSG_WRITE_MEM_REQ, address) + data
  69     drv.sendall(m)
  70
  71 def write_mem(address, data):
  72     send_write_mem_req(address, data)
  73     _, ptype, _ = wait_for_msg()
  74     if ptype != MSG_WRITE_MEM_RES:
  75         logger.error('Expected MSG_WRITE_MEM_RES but got %s. Shutting down.', ptype)
  76         exit(-1)
  77
  78 def send_connect_response(stream_id, result):
  79     m = struct.pack('=IIBB', 1, stream_id, MSG_CONNECT_RESPONSE, result)
  80     drv.sendall(m)
  81
  82 def send_data(stream_id, data):
  83     m = struct.pack('=IIB', len(data), stream_id, MSG_DATA) + data
  84     drv.sendall(m)
  85
  86 def send_eos(stream_id):
  87     m = struct.pack('=IIB', 0, stream_id, MSG_EOS)
  88     drv.sendall(m)
  89
  90 def send_reset(stream_id):
  91     m = struct.pack('=IIB', 0, stream_id, MSG_RESET)
  92     drv.sendall(m)
  93
  94 ### A314 communication routines above. Actual driver below.
  95
  96 current_stream_id = None
  97 done = False
  98 rbuf = b''
  99
 100 DEV_NAME = 'tap0'
 101 SERVICE_NAME = b'ethernet'
 102
 103 READ_FRAME_REQ = 1
 104 WRITE_FRAME_REQ = 2
 105 READ_FRAME_RES = 3
 106 WRITE_FRAME_RES = 4
 107
 108 mem_read_queue = []
 109 mem_write_queue = []
 110
 111 # Can buffer as many frames as fit in memory.
 112 # Maybe should have a limit on the number of buffers?
 113 waiting_read_reqs = []
 114 buffered_frames = []
 115
 116 DROP_START_SECS = 15.0
 117
 118 def process_tap_frame(frame):
 119     if current_stream_id is None:
 120         return
 121
 122     global drop_start
 123     if drop_start:
 124         if time.time() < start_time + DROP_START_SECS:
 125             return
 126         drop_start = False
 127
 128     if waiting_read_reqs:
 129         stream_id, address, length = waiting_read_reqs.pop(0)
 130
 131         if length < len(frame):
 132             logger.error('Fatal error, read frame from TAP larger than buffer')
 133
 134         mem_write_queue.append((stream_id, address, len(frame)))
 135         send_write_mem_req(address, frame)
 136     else:
 137         buffered_frames.append(frame)
 138
 139 def process_stream_data(stream_id, data):
 140     address, length, kind = struct.unpack('>IHH', data)
 141     if kind == WRITE_FRAME_REQ:
 142         mem_read_queue.append((stream_id, address, length))
 143         send_read_mem_req(address, length)
 144     elif kind == READ_FRAME_REQ:
 145         if buffered_frames:
 146             frame = buffered_frames.pop(0)
 147
 148             if length < len(frame):
 149                 logger.error('Fatal error, read frame from TAP larger than buffer')
 150
 151             mem_write_queue.append((stream_id, address, len(frame)))
 152             send_write_mem_req(address, frame)
 153         else:
 154             waiting_read_reqs.append((stream_id, address, length))
 155
 156 def process_read_mem_res(frame):
 157     tap.write(frame)
 158
 159     stream_id, address, length = mem_read_queue.pop(0)
 160     if stream_id == current_stream_id:
 161         send_data(stream_id, struct.pack('>IHH', address, length, WRITE_FRAME_RES))
 162
 163 def process_write_mem_res():
 164     stream_id, address, length = mem_write_queue.pop(0)
 165     if stream_id == current_stream_id:
 166         send_data(stream_id, struct.pack('>IHH', address, length, READ_FRAME_RES))
 167
 168 def process_drv_msg(stream_id, ptype, payload):
 169     global current_stream_id
 170
 171     if ptype == MSG_CONNECT:
 172         if payload == SERVICE_NAME and current_stream_id is None:
 173             logger.info('Amiga connected')
 174             current_stream_id = stream_id
 175             send_connect_response(stream_id, 0)
 176         else:
 177             send_connect_response(stream_id, 3)
 178     elif ptype == MSG_READ_MEM_RES:
 179         process_read_mem_res(payload)
 180     elif ptype == MSG_WRITE_MEM_RES:
 181         process_write_mem_res()
 182     elif current_stream_id == stream_id:
 183         if ptype == MSG_DATA:
 184             process_stream_data(stream_id, payload)
 185         elif ptype == MSG_EOS:
 186             # EOS is not used.
 187             pass
 188         elif ptype == MSG_RESET:
 189             current_stream_id = None
 190             waiting_read_reqs.clear()
 191             buffered_frames.clear()
 192             logger.info('Amiga disconnected')
 193
 194 try:
 195     idx = sys.argv.index('-ondemand')
 196 except ValueError:
 197     idx = -1
 198
 199 if idx != -1:
 200     fd = int(sys.argv[idx + 1])
 201     drv = socket.socket(fileno=fd)
 202 else:
 203     drv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
 204     drv.connect(('localhost', 7110))
 205     drv.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
 206
 207     send_register_req(SERVICE_NAME)
 208     _, _, payload = wait_for_msg()
 209     if payload[0] != 1:
 210         logger.error('Unable to register ethernet with driver, shutting down')
 211         drv.close()
 212         done = True
 213
 214 if not done:
 215     try:
 216         tap = pytun.TunTapDevice(name=DEV_NAME, flags=pytun.IFF_TAP | pytun.IFF_NO_PI)
 217     except:
 218         logger.error('Unable to open tap device at ' + DEV_NAME)
 219         done = True
 220
 221     start_time = time.time()
 222     drop_start = True
 223
 224 if not done:
 225     logger.info('Ethernet service is running')
 226
 227 while not done:
 228     try:
 229         rl, _, _ = select.select([drv, tap], [], [], 10.0)
 230     except KeyboardInterrupt:
 231         rl = []
 232         if current_stream_id is not None:
 233             send_reset(current_stream_id)
 234         drv.close()
 235         done = True
 236
 237     if drv in rl:
 238         buf = drv.recv(1600)
 239         if not buf:
 240             if current_stream_id is not None:
 241                 send_reset(current_stream_id)
 242             drv.close()
 243             done = True
 244         else:
 245             rbuf += buf
 246             while True:
 247                 if len(rbuf) < 9:
 248                     break
 249
 250                 (plen, stream_id, ptype) = struct.unpack('=IIB', rbuf[:9])
 251                 if len(rbuf) < 9 + plen:
 252                     break
 253
 254                 payload = rbuf[9:9+plen]
 255                 rbuf = rbuf[9+plen:]
 256
 257                 process_drv_msg(stream_id, ptype, payload)
 258
 259     if tap in rl:
 260         frame = tap.read(1600)
 261         process_tap_frame(frame)
 262
 263 tap.close()
 264 logger.info('Ethernet service stopped')