gps303.py

   1 #!/usr/bin/env python3
   2
   3 from enum import Enum
   4 from select import poll, POLLIN, POLLERR, POLLHUP, POLLPRI
   5 from socket import socket, AF_INET, SOCK_STREAM, SOL_SOCKET, SO_REUSEADDR
   6 from struct import pack, unpack
   7 import sys
   8 from time import time
   9
  10 PORT = 4303
  11
  12 class P(Enum):
  13     UNKNOWN = 0x00
  14     LOGIN = 0x01
  15     STATUS = 0x13
  16     HIBERNATION = 0x14
  17     time = 0x30
  18     SETUP = 0x57
  19
  20
  21
  22 def answer_setup(data):
  23     return bytes.fromhex("0300310000000000000000000000000000000000000000000000003b3b3b")
  24
  25 def handle_packet(packet, addr, when):
  26     xx, length, proto = unpack("!2sBB", packet[:4])
  27     crlf = packet[-2:]
  28     data = packet[4:-2]
  29     if xx != b"xx" or crlf != b"\r\n" or (length > 1 and len(packet) != length + 2):
  30         print("bad packet:", packet.hex())
  31     print("length", length, "proto", hex(proto))
  32     try:
  33         p = P(proto)
  34     except ValueError:
  35         p = P.UNKNOWN
  36     payload = b""
  37     if p == P.LOGIN:
  38         print("imei", data[:-1].hex(), "ver", data[-1:].hex())
  39     elif p == P.SETUP:
  40         payload = answer_setup(data)
  41     length = len(payload)+1
  42     if length > 6:
  43         length -= 6
  44     return b"xx" + pack("B", length) + pack("B", proto) + payload + b"\r\n"
  45
  46 if __name__ == "__main__":
  47     ctlsock = socket(AF_INET, SOCK_STREAM)
  48     ctlsock.setsockopt(SOL_SOCKET, SO_REUSEADDR, 1)
  49     ctlsock.bind(("", PORT))
  50     ctlsock.listen(5)
  51     ctlfd = ctlsock.fileno()
  52     pollset = poll()
  53     pollset.register(ctlfd, POLLIN | POLLERR | POLLHUP | POLLPRI)
  54     clnt_dict = {}
  55     while True:
  56         try:
  57             events = pollset.poll(1000)
  58         except KeyboardInterrupt:
  59             print("Exiting")
  60             sys.exit(0)
  61         for fd, ev in events:
  62             if fd == ctlfd:
  63                 if ev & POLLIN:
  64                     clntsock, clntaddr = ctlsock.accept()
  65                     clntfd = clntsock.fileno()
  66                     clnt_dict[clntfd] = (clntsock, clntaddr)
  67                     pollset.register(clntfd, POLLIN | POLLERR | POLLHUP | POLLPRI)
  68                     print("accepted connection from", clntaddr, "fd", clntfd)
  69                 if ev & ~POLLIN:
  70                     print("unexpected event on ctlfd:", ev)
  71             else:
  72                 try:
  73                     clntsock, clntaddr = clnt_dict[fd]
  74                 except KeyError:  # this socket closed already
  75                     continue
  76                 if ev & POLLIN:
  77                     packet = clntsock.recv(4096)
  78                     when = time()
  79                     print("packet", packet, "from", clntaddr, "from fd", fd)
  80                     if packet:
  81                         response = handle_packet(packet, clntaddr, when)
  82                         if response:
  83                             try:
  84                                 # Ignore possibility of blocking
  85                                 clntsock.send(response)
  86                             except OSError as e:
  87                                 print("sending to socket", fd, "error", e)
  88                     else:
  89                         print("disconnect")
  90                         pollset.unregister(fd)
  91                         clntsock.close()
  92                         del clnt_dict[fd]
  93                 if ev & ~POLLIN:
  94                     print("unexpected event", ev, "on fd", fd)