epoloader

#!/usr/bin/python -uO

import argparse
import io
import os
import struct
import sys
import termios
import time
import tty
import math
import serial

global debug
global enabled_messages

PREAMBLE = 0x2404
EPO_CMD = 0x02d2
EOW = 0x0a0d
UART_CMD = 253
SATS_PER_EPO = 32
SECONDS_PER_HOUR = 3600
GPS_OFFSET_SECONDS = 315964786
SECONDS_PER_WEEK = 604800
HOURS_PER_WEEK = 168

baudrate = {
    termios.B4800: 4800,
    termios.B9600: 9600,
    termios.B19200: 19200,
    termios.B38400: 38400,
    termios.B57600: 57600,
    termios.B115200: 115200,
}

baudrates = [4800, 9600, 19200, 38400, 57600, 115200 ]

def Convert2UTC(GPSHour):
    GPSHour *= SECONDS_PER_HOUR
    GPSHour += GPS_OFFSET_SECONDS
    return time.strftime("%Y-%m-%d %H:%M:%S", time.gmtime(GPSHour))

def Convert2Local(GPSHour):
    GPSHour *= SECONDS_PER_HOUR
    GPSHour += GPS_OFFSET_SECONDS
    return time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(GPSHour))

def crc8(d):
    crc = 0
    if isinstance(d, (bytearray)):
        ba = d
    elif isinstance(d, (bytes)):
        ba = bytearray(d)
    else:
        ba = bytearray(d.encode("ASCII"))

    for b in ba:
        crc ^= (b & 0xff)
    return crc


def read_response(fg, timeout, r="PMTK001"):
    fg.flushInput()
    while True:
        try:
            for line in fg:
                if len(line) < 3:
                    continue
                spl = line.decode("ASCII").strip()[1:-3].split(',')
                if spl[0] == r:
                    return spl
        except:
            pass
    return None

def send_string(fg, string, flush=True):
    if isinstance(string, (bytearray)):
        ba = string
    elif isinstance(string, (bytes)):
        ba = bytearray(string)
    else:
        ba = bytearray(string.encode("ASCII"))

    buf = b"$%s*%02x\r\n" % (ba, crc8(ba))
    if debug: print(">> %s" % buf.decode("ASCII").strip())
    fg.write(buf)
    if flush:
        fg.flush()

def send_and_wait(fg, string, timeout, r="PMTK001,0,3", retries=0):
    spl = None
    count = 0
    temp_timeout = fg.timeout
    fg.timeout = timeout
    while spl == None and count <= retries:
        resp = None
        send_string(fg, string, False);
        fg.reset_input_buffer()
        fg.flush()
        resp = fg.read_until()
        try:
            sresp = resp.decode("ASCII")
            if debug:
                print("<< %s" % sresp.strip())
            ix = sresp.find(r)
            if ix >= 0:
                spl = sresp.strip()[ix:-3].split(',')
                break
        except:
            pass
        count = count + 1
        time.sleep(0.500)


    fg.timeout = temp_timeout
    return spl


def ping_unit(fg):
    resp = send_and_wait(fg, "PMTK000", 0.250, retries=1)
    if resp:
        return True
    return False

def convert_arg_line_to_args(arg_line):
    for arg in arg_line.split():
        if not arg.strip():
            continue
        yield arg

def set_nmea(fg, rate, quiet=False):
    if not quiet: print("Setting NMEA at %d baud." % rate)
    current_rate = fg.baudrate
    fg.baudrate = rate
    if ping_unit(fg):
        print("   Unit is currently in NMEA mode at %d baud." % rate);
        return True
    if not quiet: print("   No response.  Seeing if it's in binary mode.")
    buf = bytearray(b'\0' * 14)
    struct.pack_into("<HHHBIBH", buf, 0, PREAMBLE, 14, UART_CMD, 0, rate, 0, EOW)
    struct.pack_into("B", buf, 11, crc8(buf[2:11]))
    fg.flushInput()
    fg.write(buf)
    fg.flushOutput()
    time.sleep(0.500)
    if ping_unit(fg):
        if not quiet: print("   It might have been.  Anyway, it's now in NMEA mode at %d baud." % rate);
        return True
    if ping_unit(fg):
        if not quiet: print("   It might have been.  Anyway, it's now in NMEA mode at %d baud." % rate);
        return True
    if not quiet: print("   Apparently not. Failed.");
    fg.baudrate = current_rate
    return False

def set_speed(fg, current_rate, requested_rate, count, delay):
    speed_string = "PMTK251,%d" % requested_rate
#    send_string = b"$%s*%02x\r\n" % (speed_string, crc8(speed_string))
    i = iter(range(count, -1, -1))
    while next(i):
        fg.baudrate = current_rate
        time.sleep(0.100)
        fg.reset_input_buffer()
        send_string(fg, speed_string, True)
        resp = fg.read_until()
#        print(resp)
        fg.baudrate = requested_rate
        time.sleep(0.250)
        if ping_unit(fg):
            return True
    fg.baudrate = current_rate
    return False

def cleanup(fi, fg, device, port_baudrate, keep):
    global enabled_messages
    if fg:
        if enabled_messages is not None:
            enabled_messages[0] = "PMTK314"
            send_and_wait(fg, ','.join(enabled_messages), 2, r="PMTK001,314,3", retries=2)

        tty.setraw(fg, tty.TCSANOW)

    if fg and not keep:
        set_speed(fg, fg.baudrate, port_baudrate, 3, 0.057)
        fg.baudrate = port_baudrate

    if fg:
        fg.close()
    if fi:
        fi.close()

def get_known_state(device, speed, port_baudrate, keep):
    fg = serial.Serial(port=device, timeout=1)
#    tty.setraw(fg, tty.TCSANOW)

    if set_nmea(fg, speed):
        print("GPS and port are now synchronized at %d" % speed)
        return fg

    print("GPS isn't at the desired %d baud rate.  Trying the current port rate of %d baud." % (speed, port_baudrate))
    if set_nmea(fg, port_baudrate):
        print("Attempting to set it to %d baud" % speed)
        if set_speed(fg, port_baudrate, speed, 3, 0.250):
            print("GPS and port are now synchronized at %d" % speed)
            return fg
        else:
            print("Couldn't set the new speed for some reason.")
            return None

    print("GPS isn't at the current port rate either.")
    foundrate = 0
    for rate in baudrates:
        isnmea = set_nmea(fg, rate)
        if isnmea:
            foundrate = rate
            break

    if foundrate == 0:
        print("Unable to locate unit at port %s" % device)
        return None

    print("Found unit at %d baud.  Attempting to set it to %d baud" % (foundrate, speed))

    if not set_speed(fg, foundrate, speed, 1, 0.100):
        print("Failed to set baudrate to %d" % speed)
        return None

    print("GPS and port are now synchronized at %d" % speed)
    return fg


def getBinaryResponse(fg, command):
    limit = 0
    while True:
        b = fg.read(1)
        if len(b) == 0:
            break

        if b != b'\x04':
            continue

        b = fg.read(1)
        if b != b'\x24':
            continue

        length, cmd = struct.unpack("<HH", fg.read(4))
        if cmd == command:
            return struct.unpack("<HBBH", fg.read(6))[0:2]
        else:
            struct.unpack("<%dBBH" % (length - 9), fg.read(length - 6))

        limit += 1
        if limit > 10:
            break
    return (-1, -1)


EPO_SET_SIZE = 2304
SAT_SET_SIZE = 72
FRAME_LENGTH = 227
file_size = 0
enabled_messages = None

def get_input_file(args):
    global file_size
    global EPO_SET_SIZE
    global SAT_SET_SIZE
    global FRAME_LENGTH

    try:
        file_size = os.stat(args.input_file).st_size
        fi = io.open(args.input_file, mode="rb")
    except:
        print(sys.exc_info()[1])
        return None

    header = fi.read(75)
    fi.close()
    if header[0:3] == header[60:63]:
#        EPO_SET_SIZE = 1920
#        SAT_SET_SIZE = 60
#        FRAME_LENGTH = 191
        print("NOT Opening EPO Type I file with %d sets" % (file_size / EPO_SET_SIZE))
        sys.exit(1)
    elif header[0:3] == header[72:75]:
        EPO_SET_SIZE = 2304
        SAT_SET_SIZE = 72
        FRAME_LENGTH = 227
        print("Opening EPO Type II file with %d sets" % (file_size / EPO_SET_SIZE))
    else:
        print("%s is not a valid EPO file." % args.input_file, file=sys.stderr)
        return None

    if file_size % EPO_SET_SIZE != 0:
        print("The size of %s is not an even multiple of EPO_SET_SIZE(%d).  It may be corrupted." % (args.input_file, EPO_SET_SIZE), file=sys.stderr)
        return None

    fi = io.open(args.input_file, mode="rb", buffering=0)
    return fi

def setup_device(args, fi):
    global enabled_messages
    try:
        tfg = os.open(args.output_device, os.O_RDWR)
        params = termios.tcgetattr(tfg);
        args.port_baudrate = baudrate[params[5]];
        tty.setraw(tfg, tty.TCSANOW)
    except:
        print(sys.exc_info()[1])
        return None
    finally:
        os.close(tfg)

    if args.speed < 0:
        args.speed = args.port_baudrate

    if args.no_init:
        fg = serial.Serial(args.output_device, timeout=5, baudrate=args.speed)
    else:
        fg = get_known_state(args.output_device, args.speed, args.port_baudrate, args.keep_new_speed)

    if fg is None:
        return None

    enabled_messages = send_and_wait(fg, "PMTK414", 2, r="PMTK514", retries=2)
    if enabled_messages is None:
        print("Unable to save enabled messages", file=sys.stderr)
    else:
        resp = send_and_wait(fg, "PMTK314,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0", 2, r="PMTK001,314,3", retries=2)
        if resp is None:
            print("Unable to temporarily suppress NMEA messages", file=sys.stderr)

    time.sleep(0.500)
    resp = send_and_wait(fg, "PMTK605", 2, r="PMTK705", retries=2)
    if resp is not None:
        print("GPS Version: ", resp)

    if not args.location_string is None or not args.time_string is None:
        if args.time_string == "-":
            t = time.time();
            tm = time.gmtime(t)
            time_string = time.strftime("%Y,%m,%d,%H,%M,%S", tm)
        else:
            time_string = args.time_string

        print("Setting known values: %s  %s" % (args.location_string, time_string))
        resp = send_and_wait(fg, "PMTK740,%s" % time_string, 2, r="PMTK001,740,3", retries=2)
        if resp is None or resp[2] != '3':
            print("ERROR: Unable to set time.", resp, file=sys.stderr)
            cleanup(fi, fg, args.output_device, args.port_baudrate, args.keep_new_speed)
            return None
        print("Time set")
        resp = send_and_wait(fg, "PMTK741,%s,%s" % (args.location_string, time_string), 2, r="PMTK001,741,3", retries=2)
        if resp is None or resp[2] != '3':
            print("ERROR: Unable to set location.", file=sys.stderr)
            cleanup(fi, fg, args.output_device, args.port_baudrate, args.keep_new_speed)
            return None
        print("Location set")

    if args.clear_epo:
        print("Clearing existing EPO data")
        resp = send_and_wait(fg, "PMTK127", 2, r="$CLR,EPO", retries=2)
        if resp is None or resp[1] != 'EPO':
            print("ERROR: Unable to clear existing EPO data.", file=sys.stderr)
            cleanup(fi, fg, args.output_device, args.port_baudrate, args.keep_new_speed)
            return None

    if fi is None:
        cleanup(fi, fg, args.output_device, args.port_baudrate, args.keep_new_speed)
        return None

    print("Setting binary mode, speed: %d" % args.speed)
    send_string(fg, "PMTK253,1,%d" % args.speed)

    time.sleep(0.500)
    tty.setraw(fg, tty.TCSANOW)

    return fg

def main():
    global debug
    global enabled_messages
    global file_size
    global EPO_SET_SIZE
    global SAT_SET_SIZE
    global FRAME_LENGTH

    parser = argparse.ArgumentParser(fromfile_prefix_chars='@',
    description="Loads EPO data sets to MT3339 GPS",
    epilog="You can use '@filename' to read arguments from a file.")
    parser.convert_arg_line_to_args = convert_arg_line_to_args
    group = parser.add_argument_group(title="optional known time and location parameters")
    group.add_argument("-t", "--time", dest="time_string", help="Current UTC or UTC from host", metavar="yyyy,mm,dd,hh,mm,ss | - ")
    group.add_argument("-l", "--location", dest="location_string", help="Current location specified in decimal degrees and meters", metavar="lat.dddddd,lon.dddddd,alt")

    parser.add_argument("-s", "--speed", type=int, default=-1, dest="speed", help="Interface speed", choices=[4800, 9600, 19200, 38400, 57600, 115200])
    parser.add_argument("-k", "--keep-new-speed", dest="keep_new_speed", default=False, action="store_true", help="Don't restore the old speed on exit")
    parser.add_argument("-c", "--clear", dest="clear_epo", default=False, action="store_true", help="Clears the existing EPO data from the unit")
    parser.add_argument("-n", "--no-init", dest="no_init", default=False, action="store_true", help="Skips the initialization")
    parser.add_argument("-m", "--max-sets", type=int, default=26, dest="max_sets", help="Send only n sets")
    parser.add_argument("-d", "--debug", default=False, dest="debug", action="store_true", help="Print debugging info")

    parser.add_argument("input_file", metavar="<EPO_File>", help="EPO File or '-' to just set the known parameters")
    parser.add_argument("output_device", metavar="<gps_device>", help="GPS serial device such as '/dev/ttyUSB0'")

    args = parser.parse_args()
    debug=args.debug

    fi = None
    if args.input_file != "-":
        fi = get_input_file(args)

    if fi is None:
        sys.exit(1)

    fg = setup_device(args, fi)
    if fg is None:
        sys.exit(1)

    seq = 0;
    total_read = 0;
    failed = False
    max_reached = False
    epo_sets = 0

    print("Sending %d EPO sets of %d" % (args.max_sets, file_size / EPO_SET_SIZE))
    while total_read < file_size:
        for lseq in range(16):
            buf = bytearray(b'\0' * FRAME_LENGTH)
            struct.pack_into("<HHHH", buf, 0, PREAMBLE, FRAME_LENGTH, EPO_CMD, seq)
            data_start = 8
            data_end = data_start + SAT_SET_SIZE * 2
            red = fi.read(data_end - data_start)
            total_read += len(red)
            buf[data_start:data_end] = red

            struct.pack_into("<BH", buf, FRAME_LENGTH - 3, crc8(buf[2: FRAME_LENGTH - 3]), EOW)

            if lseq == 0:
                epo_sets += 1
                set_start_time = int(struct.unpack("<I", red[0:3] + b'\0')[0])
                print("Sending set %4d.  Valid from %s UTC to %s UTC"
                      % (epo_sets, Convert2UTC(set_start_time),
                         Convert2UTC(set_start_time + 6)))
                if seq == 0:
                    start = set_start_time

            fg.flushInput()
            fg.write(buf)
            fg.flush()
            rseq, result = getBinaryResponse(fg, 2)
            if result != 1:
                failed = True
                print("Transfer failed", file=sys.stderr)
                break
            if seq != rseq:
                failed = True
                print("Sequence error: %d != %d" % (seq, result), file=sys.stderr)
                break
            seq += 1
            if args.max_sets > 0 and lseq == 10 and epo_sets == args.max_sets:
                max_reached = True
                break

        if max_reached or failed:
            break

    fi.close()
    if not failed:
        end = set_start_time
        buf = bytearray(b'\0' * FRAME_LENGTH)
        struct.pack_into("<HHHH", buf, 0, PREAMBLE, FRAME_LENGTH, EPO_CMD, 0xffff)
        struct.pack_into("<BH", buf, FRAME_LENGTH - 3, crc8(buf[2: FRAME_LENGTH - 3]), EOW)
        fg.write(buf)
        fg.flush()
        seq, result = getBinaryResponse(fg, 2)
        if result != 1:
            print("Final Transfer failed", file=sys.stderr)
        else:
            print("================================================================================")

    set_nmea(fg, args.speed, quiet=True)
    time.sleep(1.00)

    resp = send_and_wait(fg, "PMTK607", 2, r="PMTK707", retries=1)
    cleanup(fi, fg, args.output_device, args.port_baudrate, args.keep_new_speed)

    if resp is None:
        print("ERROR: EPO in NVRAM couldn't be verified", file=sys.stderr)
        return 1

    vstart = int(int(resp[2]) * 168 + (int(resp[3]) / 3600))
    vend = int(int(resp[4]) * 168 + (int(resp[5]) / 3600))

    nsets = math.floor(((int(resp[1])) * 1920) / 2304 / 2)

    fileout=sys.stdout
    rc = 0
    if vstart != start or vend != end:
        fileout=sys.stderr
        rc = 1
    print("%3d sets sent.     Valid from %s UTC to %s UTC"
          % (epo_sets, Convert2UTC(start), Convert2UTC(end + 6)),
          file=fileout)
    print("    sets in NVRAM: Valid from %s UTC to %s UTC" %
          (Convert2UTC(vstart), Convert2UTC(vend + 6)),
          file=fileout)
    if rc == 0:
        print("Verified EPO in NVRAM matches file")
    else:
        print("ERROR: EPO in NVRAM doesnt match file", file=sys.stderr)
    return rc

if __name__ == "__main__":

    sys.exit(main() or 0)