#!/usr/bin/env python3
# tools/lwl/ocdconsole.py
#
#   Copyright (C) 2019 Dave Marples. All rights reserved.
#   Author: Dave Marples <dave@marples.net>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in
#    the documentation and/or other materials provided with the
#    distribution.
# 3. Neither the name NuttX nor the names of its contributors may be
#    used to endorse or promote products derived from this software
#    without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
#
# Usage
# =====
#
# No special python modules are needed, it should be possible to run the
# application simply as shown below;
#
# ------------------------------------------
# $ ./ocdconsole.py
# ==Link Activated
#
# nsh>
# nsh> help
# help usage:  help [-v] [<cmd>]
#
#  ?        echo     exit     hexdump  ls       mh       sleep    xd
#  cat      exec     help     kill     mb       mw       usleep
# nsh>
# ------------------------------------------
#
# This code is designed to be 'hardy' and will survive a shutdown and
# restart of the openocd process. When your target application
# changes then the location of the upword and downword may change,
# so they are re-searched for again. To speed up the start process
# consider putting those words at fixed locations (e.g. via the
# linker file) and referencing them directly.
#

LWL_ACTIVESHIFT = 31
LWL_DNSENSESHIFT = 30
LWL_UPSENSESHIFT = 29
LWL_OCTVALSHIFT = 27
LWL_PORTSHIFT = 24

LWL_PORTMASK = 7 << LWL_PORTSHIFT
LWL_SENSEMASK = 3 << LWL_UPSENSESHIFT
LWL_OCTVALMASK = 3 << LWL_OCTVALSHIFT

LWL_ACTIVE = 1 << LWL_ACTIVESHIFT
LWL_DNSENSEBIT = 1 << LWL_DNSENSESHIFT
LWL_UPSENSEBIT = 1 << LWL_UPSENSESHIFT

LWL_SIG = 0x7216A318

LWL_PORT_CONSOLE = 1

# Memory to scan through looking for signature
baseaddr = 0x20000000
length = 0x8000

import time
import socket
import os

if os.name == "nt":
    import msvcrt
else:
    import sys, select, termios, tty


def kbhit():
    """Returns True if a keypress is waiting to be read in stdin, False otherwise."""
    if os.name == "nt":
        return msvcrt.kbhit()
    else:
        dr, dw, de = select.select([sys.stdin], [], [], 0)
        return dr != []


def dooutput(x):
    if x & 255 == 10:
        print("\r", flush=True)
    else:
        print(chr(x), end="", flush=True)


###############################################################################
# Code from here to *** below was taken from GPL'ed ocd_rpc_example.py and is
# available in its original form  at contrib/rpc_examples in the openocd tree.
#
# This code was approved for re-release under BSD (licence at the head of this
# file) by the original author (Andreas Ortmann, ortmann@finf.uni-hannover.de)
# via email to Dave Marples on 3rd June 2019.
# email ID: 15e1f0a0-9592-bd07-c996-697f44860877@finf.uni-hannover.de
###############################################################################


def strToHex(data):
    return map(strToHex, data) if isinstance(data, list) else int(data, 16)


class oocd:
    NL = "\x1A"

    def __init__(self, verbose=False):
        self.verbose = verbose
        self.tclRpcIp = "127.0.0.1"
        self.tclRpcPort = 6666
        self.bufferSize = 4096

        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

    def __enter__(self):
        self.sock.connect((self.tclRpcIp, self.tclRpcPort))
        return self

    def __exit__(self, type, value, traceback):
        try:
            self.send("exit")
        finally:
            self.sock.close()

    def send(self, cmd):
        """Send a command string to TCL RPC. Return the result that was read."""
        data = (cmd + oocd.NL).encode("utf-8")
        if self.verbose:
            print("<- ", data)

        self.sock.send(data)
        return self._recv()

    def _recv(self):
        """Read from the stream until the NL was received."""
        data = bytes()
        while True:
            chunk = self.sock.recv(self.bufferSize)
            data += chunk
            if bytes(oocd.NL, encoding="utf-8") in chunk:
                break

        data = data.decode("utf-8").strip()
        data = data[:-1]  # strip trailing NL

        return data

    def readVariable(self, address):
        raw = self.send("%s 0x%x" % (self.mdwText, address)).split(": ")
        return None if (len(raw) < 2) else strToHex(raw[1])

    def writeVariable(self, address, value):
        assert value is not None
        self.send("mww 0x%x 0x%x" % (address, value))

    def testInterface(self):
        self.mdwText = "ocd_mdw"
        if self.readVariable(baseaddr) != None:
            return
        self.mdwText = "mdw"
        if self.readVariable(baseaddr) != None:
            return
        raise ConnectionRefusedError


# *** Incorporated code ends ######################################################

if __name__ == "__main__":

    def show(*args):
        print(*args, end="\n\n")

    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    while True:
        try:
            tty.setraw(fd)
            with oocd() as ocd:
                while True:
                    # Find the location for the communication variables
                    # =================================================
                    try:
                        ocd.testInterface()
                        downwordaddr = 0
                        while downwordaddr < length:
                            if ocd.readVariable(baseaddr + downwordaddr) == LWL_SIG:
                                break
                            downwordaddr = downwordaddr + 4

                        if downwordaddr >= length:
                            print("ERROR: Cannot find signature\r")
                            exit(1)

                        # We have the base address, so get the variables themselves
                        # =========================================================
                        downwordaddr = baseaddr + downwordaddr + 4
                        upwordaddr = downwordaddr + 4
                        downword = LWL_ACTIVE

                        # Now wake up the link...keep on trying if it goes down
                        # =====================================================
                        while True:
                            ocd.writeVariable(downwordaddr, downword)
                            upword = ocd.readVariable(upwordaddr)
                            if upword & LWL_ACTIVE != 0:
                                print("==Link Activated\r")
                                break
                    except (
                        BrokenPipeError,
                        ConnectionRefusedError,
                        ConnectionResetError,
                    ) as e:
                        raise e

                    # Now run the comms loop until something fails
                    # ============================================
                    try:
                        while True:
                            ocd.writeVariable(downwordaddr, downword)
                            upword = ocd.readVariable(upwordaddr)
                            if upword & LWL_ACTIVE == 0:
                                print("\r==Link Deactivated\r")
                                break
                            if kbhit():
                                charin = sys.stdin.read(1)
                                if ord(charin) == 3:
                                    sys.exit(0)
                                if downword & LWL_DNSENSEBIT:
                                    downword = downword & LWL_UPSENSEBIT
                                else:
                                    downword = (
                                        downword & LWL_UPSENSEBIT
                                    ) | LWL_DNSENSEBIT
                                downword |= (
                                    (LWL_PORT_CONSOLE << LWL_PORTSHIFT)
                                    | (1 << LWL_OCTVALSHIFT)
                                    | LWL_ACTIVE
                                    | ord(charin)
                                )

                            if (upword & LWL_UPSENSEBIT) != (downword & LWL_UPSENSEBIT):
                                incomingPort = (upword & LWL_PORTMASK) >> LWL_PORTSHIFT
                                if incomingPort == LWL_PORT_CONSOLE:
                                    incomingBytes = (
                                        upword & LWL_OCTVALMASK
                                    ) >> LWL_OCTVALSHIFT
                                    if incomingBytes >= 1:
                                        dooutput(upword & 255)
                                    if incomingBytes >= 2:
                                        dooutput((upword >> 8) & 255)
                                    if incomingBytes == 3:
                                        dooutput((upword >> 16) & 255)

                                if downword & LWL_UPSENSEBIT:
                                    downword = downword & ~LWL_UPSENSEBIT
                                else:
                                    downword = downword | LWL_UPSENSEBIT
                    except (
                        ConnectionResetError,
                        ConnectionResetError,
                        BrokenPipeError,
                    ) as e:
                        print("\r==Link Lost\r")
                        raise e

        except (BrokenPipeError, ConnectionRefusedError, ConnectionResetError) as e:
            time.sleep(1)
            continue
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)