# Copyright (c) 2004-2010 Mellanox Technologies LTD. All rights reserved.
# Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# This software is available to you under a choice of one of two
# licenses.  You may choose to be licensed under the terms of the GNU
# General Public License (GPL) Version 2, available from the file
# COPYING in the main directory of this source tree, or the
# OpenIB.org BSD license below:
#
#     Redistribution and use in source and binary forms, with or
#     without modification, are permitted provided that the following
#     conditions are met:
#
#      - Redistributions of source code must retain the above
#        copyright notice, this list of conditions and the following
#        disclaimer.
#
#      - 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.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
# NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
# BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
# ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
# CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
import ctypes
import struct
import jtag
import re
import math
import time
import os


class GB100_Jtag:
    """Class for interfacing with FTDI 2322x modules via USB/JTAG"""

    state = jtag.states.unknown
    # instr = [('idcode', 'com_32', None, '0b11111101'),
    #          ('jtag_host', 'com_39', None, '0b10100000')
    #          ]
    instr = []

    def __init__(self):
        self.device_handle_1 = ctypes.c_void_p()
        self.device_handle_2 = ctypes.c_void_p()
        self._libraries = None
        if os.name == 'posix':
            os.system("sudo rmmod ftdi_sio")
            os.system("sudo rmmod usbserial")

            try:
                self._libraries = ctypes.CDLL('libftd2xx.so')
            except BaseException:
                self._libraries = ctypes.CDLL(os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', '..', 'ext_libs', 'libftd2xx.so'))

        else:
            self._libraries = ctypes.WinDLL('ftd2xx')

        self._libraries.FT_Open(0, ctypes.byref(self.device_handle_1))
        self._libraries.FT_Open(1, ctypes.byref(self.device_handle_2))

        devA = ctypes.c_void_p()
        idA = ctypes.c_void_p()
        snA = ctypes.create_string_buffer(16)
        descA = ctypes.create_string_buffer(64)

        devB = ctypes.c_void_p()
        idB = ctypes.c_void_p()
        snB = ctypes.create_string_buffer(16)
        descB = ctypes.create_string_buffer(64)

        USBftStatus = self._libraries.FT_GetDeviceInfo(self.device_handle_1, ctypes.byref(devA), ctypes.byref(idA), snA, descA, None)
        USBftStatusB = self._libraries.FT_GetDeviceInfo(self.device_handle_2, ctypes.byref(devB), ctypes.byref(idB), snB, descB, None)

        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)

        if USBftStatusB:
            raise Exception("USBftStatus was non-zero:", USBftStatusB)

        # Device A
        self.deviceA = devA.value
        self.idA = idA.value
        self.serialA = snA.value
        self.descriptionA = descA.value
        print(self.deviceA, self.idA, self.serialA, self.descriptionA)

        # Device B
        self.deviceB = devB.value
        self.idB = idB.value
        self.serialB = snB.value
        self.descriptionB = descB.value
        print(self.deviceB, self.idB, self.serialB, self.descriptionB)

        # Settings for device B
        USBftStatus = self._libraries.FT_ResetDevice(self.device_handle_2)
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)

        mask = 0x00
        mode = 0  # Set MPSSE mode
        USBftStatus = self._libraries.FT_SetBitMode(self.device_handle_2, mask, mode)

        mask = 0x00
        mode = 2  # Set MPSSE mode
        USBftStatus = self._libraries.FT_SetBitMode(self.device_handle_2, mask, mode)
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)

        outstr = struct.pack('BBB', 0x80, 0x40, 0x70)  # NVJTAG_SEL
        self._ft_writeB(outstr)
        time.sleep(1)
        outstr = struct.pack('BBB', 0x80, 0x70, 0x70)  # NVJTAG_SEL
        self._ft_writeB(outstr)

        # Change End

        # Settings for Device A
        USBftStatus = self._libraries.FT_ResetDevice(self.device_handle_1)
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)
        FT_PURGE_RX = 1
        FT_PURGE_TX = 2
        USBftStatus = self._libraries.FT_Purge(self.device_handle_1, FT_PURGE_RX | FT_PURGE_TX)
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)
        USBftStatus = self._libraries.FT_SetUSBParameters(self.device_handle_1, 65536, 65536)
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)

        USBftStatus = self._libraries.FT_SetChars(self.device_handle_1, False, 0, False, 0)
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)
        USBftStatus = self._libraries.FT_SetTimeouts(self.device_handle_1, 5000, 5000)  # was 5000, 5000
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)
        USBftStatus = self._libraries.FT_SetLatencyTimer(self.device_handle_1, 1)  # was 1
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)
        USBftStatus = self._libraries.FT_SetFlowControl(self.device_handle_1, 0x0200, 0, 0)  # 500ms read/write timeout
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)

        mask = 0x00
        mode = 0  # Set MPSSE mode
        USBftStatus = self._libraries.FT_SetBitMode(self.device_handle_1, mask, mode)

        mask = 0x00
        mode = 2  # Set MPSSE mode
        USBftStatus = self._libraries.FT_SetBitMode(self.device_handle_1, mask, mode)
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)

        # Clock Settings(Set to 1MHz)
        outstr = struct.pack('BBB',     # 0x8B,  # 0x8B set 2232H fast/slow mode
                             0x86,      # Setup tck divide
                             0x05,      # Div L 12.5Hz #0xFF  0x1D = 1MHz
                             0x00)

        self._ft_writeA(outstr)

        self._ft_writeA(struct.pack('BBB', 0x80, 0x00, 0xBB))
        self._ft_writeA(struct.pack('BB', 0x8E, 0x04))
        #
        self._ft_writeA(struct.pack('BBB', 0x80, 0x00, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x01, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x00, 0xBB))
        # j._ft_writeA(bytes((0x81)))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x09, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        # j._ft_writeA(bytes((0x81)))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x09, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        # j._ft_writeA(bytes((0x81)))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x09, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))

        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x09, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))

        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x09, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))

        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x09, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))

        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x09, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))

        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x09, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))

        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x09, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))

        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x09, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))

        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x09, 0xBB))
        self._ft_writeA(struct.pack('BBB', 0x80, 0x08, 0xBB))

        self._ft_writeA(struct.pack('BBB', 0x80, 0x20, 0xBB))
        self._ft_writeA(struct.pack('BB', 0x8E, 0x04))

        self._ft_writeA(struct.pack('BBB', 0x80, 0x20, 0xBB))
        self._ft_writeA(struct.pack('BB', 0x8E, 0x04))

        self._ft_writeA(struct.pack('B', 0x85))      # Set to 84 for LoopBack (Verification only)

        # Set the state machine to reset(This is sto track state)
        self.state = jtag.states.reset
        self.last_tdi = 0
        self.last_tms = 1
        self.devA_pin_state = 0x28

        USBftStatus = self._libraries.FT_Purge(self.device_handle_1, FT_PURGE_RX | FT_PURGE_TX)
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)
        time.sleep(1)

        for t in self.instr:
            print("This", t)
            setattr(self, 'w_' + t[0], self.make_write_instr(t))
            setattr(self, 'r_' + t[0], self.make_read_instr(t))

        # End Settings Device A

    # Disable JTAG (Calling destructor)
    def gb100_close(self):

        # disable NVJTAG
        j._disable_nvjtag()

        # Flush Memory
        mask = 0x00
        mode = 0  # Set MPSSE mode - Reset
        USBftStatus = self._libraries.FT_SetBitMode(self.device_handle_1, mask, mode)

        outstr = struct.pack('BBB', 0x80, 0x40, 0x60)  # NVJTAG_SEL = Low
        self._ft_writeB(outstr)
        self.write_tms_0(times=5)

        outstr = struct.pack('BBB', 0x80, 0x00, 0xBB)  # All JTAG Signals Low)
        self._ft_writeA(outstr)
        self.write_tms_0(times=10)

        self._libraries.FT_Close(0, ctypes.byref(self.device_handle_1))
        self._libraries.FT_Close(1, ctypes.byref(self.device_handle_2))

    def make_write_instr(self, instr_tuple):
        def instr():
            return self.write_ir(instr_tuple[3])
        return instr

    def make_read_instr(self, instr_tuple):
        m = re.search(r'com_([0-9]*)', instr_tuple[1])
        if m and m.group(1):
            num_nibbles = int(int(m.group(1)) / 4)

        def instr():
            return self.write_dr('0x' + num_nibbles * '0')

        return instr

    def _write_tdi_bytes(self, byte_list, read=True):
        """Return the command packet needed to write a list of bytes over TDI.
        Bytes are written as-is, so should be LSB first for JTAG.
        """
        if not byte_list:
            return

        outstr = b''
        if len(byte_list) > 0xFFFF:
            raise Exception("Byte input list is too long!")

        for b in byte_list:
            if b > 0xFF:
                raise Exception("Element in list larger than 8-bit:", hex(b))

        length = len(byte_list) - 1
        length_upper = (length & 0xFF00) >> 8
        length_lower = length & 0x00FF

        # For struct.pack()
        struc_len = len(byte_list)
        struc_str = '<' + str(struc_len) + 'B'

        # print("Length of Byte List =", length)
        # print("length_upper, length_lower" ,length_upper,length_lower)

        # byte_str = bytes((b for b in byte_list))
        byte_str = struct.pack(struc_str, *byte_list)

        # bytes_written = ctypes.c_int()

        if read:
            opcode = 0x31
        else:
            opcode = 0x11

        outstr = struct.pack('B', opcode) + struct.pack('BB', length_lower, length_upper) + byte_str  # msb first
        # outstr = bytes((opcode, length_lower, length_upper)) + byte_str  # msb first

        return outstr

    def _write_tdi_bits(self, bit_list, tdi_val=0, read=True):
        """Return the command packet needed to write a list of binary values
        over TDI.
        """
        if not bit_list:
            return
        outstr = b''
        bit_str = ''.join([str(b) for b in bit_list])
        bit_str = bit_str + '0' * (8 - len(bit_str))

        bit_int = int(bit_str, 2)

        length = len(bit_list)
        if length > 8:
            raise ValueError("Input string longer than 8 bits")

        bytes_written = ctypes.c_int()

        if read:
            opcode = 0x33
        else:
            opcode = 0x13
        # print("Length of Bits List =", length)

        outstr = struct.pack('B', opcode) + struct.pack('BB', length - 1, bit_int)  # msb first
        # outstr = bytes((opcode, length - 1, bit_int))  # msb first
        return outstr

    def _write_tms(self, tdi_val, tms, read=False):
        """Return the command packet needed to write a single value to TDI and
        a single or multiple values to TMS.
        tdi_val: string or int: '1', '0', 1, 0
        tms: list, string or jtag.TMSPath: '10010', [1, 0, 0, 1, 0],
        ['1', '0', '0', '1', '0']
        """
        # TODO, make this work for lists longer than 7, maybe recurse?
        # also, formatting could be more consistant

        # Check tdi_val formatting
        tdi_int = int(tdi_val)
        if str(tdi_val) != '0' and str(tdi_val) != '1':
            raise Exception("tdi_val doesn't meet expected format:", tdi_val)

        # Convert tms to list to parse state transitions
        if isinstance(tms, str):
            tms = [int(s) for s in tms]

        tms_len = len(tms)
        if tms_len > 7:
            raise Exception("tms is too long", tms)

        if self.state is not jtag.states.unknown:
            for t in tms:
                # print(t, '{:<12}'.format(self.state), '->', self.state[t])
                self.state = self.state[t]
                # print("****In tms:", self.state)
            # print("Done Shifting")

        if isinstance(tms, list) or isinstance(tms, jtag.TMSPath):
            tms = ''.join([str(x) for x in tms])

        tms = tms[::-1]  # LSb first
        tms_int = int(tms, 2)
        byte1 = (tdi_int << 7) | tms_int

        if read:
            opcode = 0x6B
        else:
            opcode = 0x4B

        # outstr = struct.pack('BBB',[opcode, tms_len - 1, byte1])
        outstr = bytes((opcode, tms_len - 1, byte1))  # tms_len - 1
        return outstr

    def _write_tms_direct(self, tdi_val=0, shift='IR', position='start'):
        """Return the command packet needed to write a single value to TDI and
        a single or multiple values to TMS.
        tdi_val: string or int: '1', '0', 1, 0
        tms: list, string or jtag.TMSPath: '10010', [1, 0, 0, 1, 0],
        ['1', '0', '0', '1', '0']
        """
        if str(tdi_val) != '0' and str(tdi_val) != '1':
            raise Exception("tdi_val doesn't meet expected format:", tdi_val)

        opcode = 0x4B

        if shift == 'IR' and position == 'start':
            # outstr = bytes((opcode, 0x03, 0x03))
            if tdi_val == 0:
                outstr = struct.pack('BBB', opcode, 0x03, 0x03)
            if tdi_val == 1:
                outstr = struct.pack('BBB', opcode, 0x03, 0x83)

        if shift == 'DR' and position == 'start':
            # outstr = bytes((opcode, 0x02, 0x01))
            if tdi_val == 0:
                outstr = struct.pack('BBB', opcode, 0x02, 0x01)
            if tdi_val == 1:
                outstr = struct.pack('BBB', opcode, 0x02, 0x81)

        if shift == 'IR' and position == 'end':
            # outstr = bytes((opcode, 0x01, 0x01))
            if tdi_val == 0:
                outstr = struct.pack('BBB', opcode, 0x01, 0x01)
            if tdi_val == 1:
                outstr = struct.pack('BBB', opcode, 0x01, 0x81)

        if shift == 'DR' and position == 'end':
            # outstr = bytes((opcode, 0x01, 0x01))
            if tdi_val == 0:
                outstr = struct.pack('BBB', opcode, 0x01, 0x01)
            if tdi_val == 1:
                outstr = struct.pack('BBB', opcode, 0x01, 0x81)

        return outstr

    def write_tms_0(self, times, read=False):
        tms_loop = int(times / 7)
        # print("TimesLoop:", tms_loop)
        self._make_tms_low()
        for i in range(tms_loop):
            outstr = bytes((0x4B, 0x08, 0x00))
            # print("write_tms_0:", outstr)
            self._ft_writeA(outstr)
        # self._flush()

    def _make_tms_low(self):
        outstr = bytes((0x80, 0x20, 0xBB))
        self._ft_writeA(outstr)

    def _make_tms_high(self):
        outstr = bytes((0x80, 0x28, 0xBB))
        self._ft_writeA(outstr)

    def _write_wait_cycles_bits(self, wait_cycles, read=False):
        """Return the command packet needed to write a single value to TDI and
        a single or multiple values to TMS.
        tdi_val: string or int: '1', '0', 1, 0
        tms: list, string or jtag.TMSPath: '10010', [1, 0, 0, 1, 0],
        ['1', '0', '0', '1', '0']
        """
        wait_length_bytes = wait_cycles // 8

        tmp = b''
        if wait_length_bytes > 0:
            length_upper = (wait_length_bytes & 0xFF00) >> 8
            length_lower = wait_length_bytes & 0x00FF

            # print("Wait Length Upper, Lower:", length_upper, length_lower)
            opcode = 0x8F
            # outstr = struct.pack('BBB',[opcode, tms_len - 1, byte1])
            outstr = bytes((opcode, length_lower, length_upper))
            tmp += outstr

        wait_length_bits = wait_cycles % 8

        # print("Wait Length Bits:", wait_length_bits)
        opcode = 0x8E
        outstr = bytes((opcode, wait_length_bits))

        if outstr:
            tmp += outstr

        # print("Wait Cycle Outstr:", outstr)
        return outstr

    def wait(self, waitcycles=0):
        if waitcycles > 0:
            # print("***** Have Wait Cycles******")
            self._make_tms_low()
            outstr = self._write_wait_cycles_bits(wait_cycles=waitcycles)
            self._ft_writeA(outstr)

    def _write(self, data, num_bits, total_bits, write_state=None, next_state=None, read=True, waitcycles=None, idle=None):
        """Default next state is shift-ir/dr"""
        byte_list, bit_list, last_bit = self.to_bytes_bits_GB100(data, num_bits, total_bits)
        # byte_list, bit_list, last_bit = self.to_bytes_bits(data, num_bits)
        # print(byte_list, bit_list, last_bit)
        # print(len(byte_list), len(bit_list))

        outstr = b''
        if write_state:
            outstr += self._write_tms(0, self.state[write_state])

        tmp = self._write_tdi_bytes(byte_list, read=read)
        if tmp:
            outstr += tmp

        tmp = self._write_tdi_bits(bit_list, read=read)
        if tmp:
            outstr += tmp

        tmp = b''
        if next_state:
            # print(self.state[next_state])
            tmp = (self._write_tms(last_bit, self.state[next_state], read=read))
        else:
            tmp = (self._write_tms(last_bit, self.state[self.state], read=read))

        for i in range(1):
            outstr += tmp

        tmp = b''
        if waitcycles:
            print('In _write: Adding wait cycles')
            tmp = self._write_wait_cycles_bits(waitcycles)
            outstr += tmp

        # print("*** String before _ft_write", outstr)
        self._ft_writeA(outstr)

        # time.sleep(0.01)
        if read:
            a = self._read(len(byte_list) + (1 if len(bit_list) else 0) + 1)  # + 1
            # print("RETURN BUFFER:", a)
            rebuilt_data = self._rebuild_read_data(a[0], len(byte_list), len(bit_list))

            return self.bin2hex(rebuilt_data)

    def _write_new(self, data, num_bits, total_bits, shift="IR", read=True, waitcycles=None, idle=None):
        """Default next state is shift-ir/dr"""
        byte_list, bit_list, last_bit = self.to_bytes_bits_GB100(data, num_bits, total_bits)
        print('BYTE LIST', byte_list)
        print('BIT LIST', bit_list)
        print('LAST BIT', last_bit)
        # print(len(byte_list), len(bit_list))

        outstr = b''

        # tmp    = bytes((0x80, 0x28, 0xBB))
        tmp = struct.pack('BBB', 0x80, 0x28, 0xBB)
        outstr += tmp

        tmp = self._write_tms_direct(shift=shift, position='start')
        outstr += tmp

        # byte_list
        if byte_list:
            tmp = self._write_tdi_bytes(byte_list, read=read)
            if tmp:
                outstr += tmp

        # bit_list
        if bit_list:
            tmp = self._write_tdi_bits(bit_list, read=read)
            if tmp:
                outstr += tmp

            # tmp = bytes((0x80, 0x28, 0xBB))
            tmp = struct.pack('BBB', 0x80, 0x28, 0xBB)
            outstr += tmp

        # last_bit
        tmp = self._write_tdi_bits([last_bit], read=read)
        if tmp:
            outstr += tmp

        # tmp = bytes((0x80, 0x28, 0xBB))
        tmp = struct.pack('BBB', 0x80, 0x28, 0xBB)
        outstr += tmp

        tmp = self._write_tms_direct(shift=shift, position='end')
        outstr += tmp

        # print("***** String Before _ft_write", outstr)
        self._ft_writeA(outstr)

        # time.sleep(0.01)
        if read:
            a = self._read(len(byte_list) + (1 if len(bit_list) else 0) + 1)  # + 1
            # print("RETURN BUFFER:", a)
            rebuilt_data = self._rebuild_read_data(a[0], len(byte_list), len(bit_list))

            # return self.bin2hex(rebuilt_data)
            return rebuilt_data

    def _write_modified(self, data, num_bits, total_bits, shift="IR", read=True, waitcycles=None, idle=None):
        """Default next state is shift-ir/dr"""
        byte_list, bit_list, last_bit = self.to_bytes_bits_GB100(data, num_bits, total_bits)
        # print('BYTE LIST',byte_list)
        # print('BIT LIST' ,bit_list)
        # print('LAST BIT' ,last_bit)
        # print(len(byte_list), len(bit_list))

        outstr = b''

        self.devA_pin_state = (self.devA_pin_state & 0xFD) | (self.last_tdi << 1)  # set TDI
        self.devA_pin_state = (self.devA_pin_state & 0xF7) | (self.last_tms << 3)  # set TMS

        tmp = struct.pack('BBB', 0x80, self.devA_pin_state, 0xBB)
        outstr += tmp

        tmp = self._write_tms_direct(tdi_val=self.last_tdi, shift=shift, position='start')
        outstr += tmp

        # byte_list
        if byte_list:
            tmp = self._write_tdi_bytes(byte_list, read=read)
            if tmp:
                outstr += tmp
            self.last_tdi = self.getbit(byte_list[-1])
            # print("Last TDI Byte List:",self.last_tdi)

        # bit_list
        if bit_list:
            tmp = self._write_tdi_bits(bit_list, read=read)
            if tmp:
                outstr += tmp

            # tmp = bytes((0x80, 0x28, 0xBB))
            # Only need to check TDI, TMS in shift state
            self.last_tdi = bit_list[-1]
            # print("Last TDI Bit List:", self.last_tdi)

        self.devA_pin_state = (self.devA_pin_state & 0xFD) | (self.last_tdi << 1)  # set TDI
        tmp = struct.pack('BBB', 0x80, self.devA_pin_state, 0xBB)
        outstr += tmp

        # last_bit
        tmp = self._write_tdi_bits([last_bit], read=read)
        if tmp:
            outstr += tmp

        # Only need to check TDI, TMS in shift state
        self.last_tdi = last_bit
        self.devA_pin_state = (self.devA_pin_state & 0xFD) | (self.last_tdi << 1)  # set TDI
        # print("Last TDI Bit:", self.last_tdi)

        # tmp = bytes((0x80, 0x28, 0xBB))
        tmp = struct.pack('BBB', 0x80, self.devA_pin_state, 0xBB)
        outstr += tmp

        tmp = self._write_tms_direct(tdi_val=self.last_tdi, shift=shift, position='end')
        outstr += tmp

        self._ft_writeA(outstr)

        # time.sleep(0.01)
        if read:
            a = self._read(len(byte_list) + (1 if len(bit_list) else 0) + 1)  # + 1
            # print("RETURN BUFFER:", a)
            rebuilt_data = self._rebuild_read_data(a[0], len(byte_list), len(bit_list))
            return rebuilt_data
            # return self.bin2hex(rebuilt_data)

    def _read(self, expected_num):
        """Read expected_num bytes from the FTDI chip, once there are that many
        availible in the buffer. Return the raw bytes as a tuple of binary and
        hex strings."""

        bytes_avail = ctypes.c_int()
        while bytes_avail.value != expected_num:
            # print("BYTES AVAILABLE", bytes_avail.value)
            if bytes_avail.value >= expected_num:  # was >=
                print("*** DEBUG: Bytes Available", bytes_avail.value, expected_num)
                raise Exception("More Bytes in buffer than expected!")
            USBftStatus = self._libraries.FT_GetQueueStatus(self.device_handle_1, ctypes.byref(bytes_avail))
            if USBftStatus:
                raise Exception("USBftStatus Was Non-Zero:", USBftStatus)

        readback = self._ft_readA(bytes_avail.value)
        # print("***** READBACK*****", readback)

        if readback:
            byte_tuple = struct.unpack('B' * len(readback), readback)
            bin_read_str = ''
            hex_read_str = ''
            if byte_tuple:
                bin_read_str = ''.join([bin(byte)[2:].zfill(8) for byte in byte_tuple])
                hex_read_str = ''.join([hex(byte)[2:].zfill(2) for byte in byte_tuple])
        return (bin_read_str, hex_read_str)

    def _flush(self):
        """Flush USB receive buffer"""
        opcode = 0x87
        bytes_written = self._ft_writeA(struct.pack('B', opcode))
        return bytes_written

    def _ft_writeA(self, outstr):
        """Low level call to ftdi dll"""
        outstr += b'\x87'
        # print("_FT_Write", outstr)
        # print("Length of outstr", len(outstr))
        bytes_written = ctypes.c_int()
        USBftStatus = self._libraries.FT_Write(self.device_handle_1, ctypes.c_char_p(outstr), len(outstr),
                                               ctypes.byref(bytes_written))
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)
        # print(outstr)
        # print("Bytes Written in ft_write:", bytes_written.value)
        # time.sleep(0.2)
        return bytes_written.value

    def _ft_writeB(self, outstr):
        """Low level call to ftdi dll"""
        outstr += b'\x87'
        bytes_written = ctypes.c_int()
        USBftStatus = self._libraries.FT_Write(self.device_handle_2, ctypes.c_char_p(outstr), len(outstr),
                                               ctypes.byref(bytes_written))
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)
        # print(outstr)
        # print(bytes_written.value)
        return bytes_written.value

    def _ft_readA(self, numbytes):
        """Low level call to ftdi dll"""

        bytes_read = ctypes.c_int()
        inbuf = ctypes.create_string_buffer(numbytes)
        USBftStatus = self._libraries.FT_Read(self.device_handle_1, inbuf, numbytes, ctypes.byref(bytes_read))
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)
        # print("RETURN BUFFER RAW:", inbuf.raw)
        # print("BYTES AVAILABLE:", bytes_read)
        # time.sleep(0.2)
        return inbuf.raw

    def _ft_readB(self, numbytes):
        """Low level call to ftdi dll"""

        bytes_read = ctypes.c_int()
        inbuf = ctypes.create_string_buffer(numbytes)
        USBftStatus = self._libraries.FT_Read(self.device_handle_2, inbuf, numbytes, ctypes.byref(bytes_read))
        if USBftStatus:
            raise Exception("USBftStatus was non-zero:", USBftStatus)
        # print(inbuf.raw)
        # print(inbuf)
        return inbuf.raw

    def get_id(self):
        """Return the device ID of the part
        State machine transitions current -> reset -> shift_dr -> exit1_dr
        """

        # outstr = self._write_tms('0', j.state.reset)
        outstr = self._write_tms('0', self.state.reset)
        self._ft_writeA(outstr)
        data = self.write_dr(32, 32, '0x00000000')
        return data

    def reset(self):
        """Return to the reset state"""
        # outstr = self._write_tms('0', j.state.reset)
        outstr = self._write_tms('0', self.state.reset)
        self._ft_writeA(outstr)
        return

    def idle(self):
        """Go to idle state and clock once"""
        # outstr = self._write_tms('0', j.state.idle.pad(minpause=2))
        outstr = self._write_tms('0', self.state.idle.pad(minpause=2))
        self._ft_writeA(outstr)
        return

    def write_ir(self, total_bits, num_bits, cmd, next_state=jtag.states.idle, waitcycles=None, read=True):
        """Write cmd while in the shift_ir state, return read back cmd"""
        # print("Shift IR")
        cmd_readback = self._write_modified(cmd, num_bits, total_bits, shift="IR", read=read, waitcycles=waitcycles)
        # print(cmd, cmd_readback)
        # print(cmd_readback)
        return (cmd_readback, cmd)

    def write_dr(self, total_bits, num_bits, data, next_state=jtag.states.idle, waitcycles=None, read=True):
        """Write data while in the shift_dr state, return read back data"""
        # print("Shift DR")
        data_readback = self._write_modified(data, num_bits, total_bits, shift="DR", read=read, waitcycles=waitcycles)  # was next state
        # print(data, data_readback)
        # print(data_readback)
        # return (data_readback, data)
        return data_readback

    def _nvdv_frame_write(self, addr, data):
        """Write data to the fabric using the JTAG2HOST command."""

        cmd = '0xA0'
        addr = '0x' + hex(addr)[2:].zfill(4)
        data = '0x' + hex(data)[2:].zfill(4)
        pack = '0b' + '1' + self.hex2bin(data)[2:] + self.hex2bin(addr)[2:] + '0000' + '0' + '0'
        temp = self.write_ir(cmd, next_state=jtag.states.idle)
        self.write_dr(pack, next_state=jtag.states.idle)
        pack = '0b' + '1' + self.hex2bin(data)[2:] + self.hex2bin(addr)[2:] + '0000' + '0' + '0'
        temp3 = self.write_dr(pack, next_state=jtag.states.idle)
        return (temp, temp3)

    def nvdv_write(self, addr, data):
        '''
        nvdv register write using JTAG2HOST_INTFC
        This is something specific to Nvidia Test Chip
        :param addr: address in int
        :param data: data in int
        :return: returns data written and observed on tdo
        '''
        res = self._nvdv_frame_write(addr, data)
        res_hex = res[1][1]
        if int(res_hex, 16) & 1 == 1:
            res_int = (int(res_hex, 16) >> 22) & 0xFFFF
            return res_int
        else:
            raise Exception("HOST acknowledgement not received")

    def nvdv_read(self, addr):
        '''
        nvdv register write using JTAG2HOST_INTFC
        This is something specific to Nvidia Test Chip
        :param addr: address in int
        :param data: data in int
        :return: returns data written and observed on tdo, this is just for sanity check comparision
        '''
        res = self._nvdv_frame_read(addr)
        res_hex = res[1][1]
        if int(res_hex, 16) & 1 == 1:
            res_int = (int(res_hex, 16) >> 22) & 0xFFFF
            return res_int
        else:
            raise Exception(f"HOST acknowledgement not received.  read_back {res_hex}")

    def _nvdv_frame_read(self, addr):
        """Write command to the fabric using the JTAG2HOST command and read data."""
        cmd = '0xA0'
        addr = '0x' + hex(addr)[2:].zfill(4)
        data = '0x' + hex(0xAAAA)[2:].zfill(4)
        pack = '0b' + '1' + self.hex2bin(data)[2:] + self.hex2bin(addr)[2:] + '0000' + '1' + '0'
        temp = self.write_ir(cmd, next_state=jtag.states.idle)
        self.write_dr(pack, next_state=jtag.states.idle)
        pack = '0b' + '1' + self.hex2bin(data)[2:] + self.hex2bin(addr)[2:] + '0000' + '1' + '0'
        self.write_dr(pack)
        temp3 = self.write_dr(pack)
        return (temp, temp3)

    def hex2bin(self, string):
        return '0b' + bin(int(string, 16))[2:].zfill(len(string[2:] * 4))

    def bin2hex(self, string):
        # return '0x' + hex(int(string, 2))[2:].zfill(math.ceil((len(string[2:]) - 1) / 4) + 1)
        # new implementation
        new_data = string + '0' * ((4 - len(string)) % 4)
        return '0x' + hex(int(new_data, 2))[2:].zfill(math.ceil((len(new_data[2:]) - 1) / 4))

    def getbit(self, x):
        "Get the last bit of the number x"
        return x & (1 << 0) and 1 or 0

    def to_bytes_bits(self, data, num_bits):
        """Return a tuple containing a list of bytes, remainder bits, the value of
        the last bit and a reconstruction of the original string.

        data: string representation of a binary or hex number '0xabc', '0b101010'
        e.g. to_bytes_bits('0xabc')        -> ([0xab], [1, 1, 0], 0)
             to_bytes_bits('0b010100011')  -> ([0x51], [], 1)
        """
        byte_list = []
        bit_list = []

        if data[-1] == 'L':
            data = data[:len(data) - 1]

        if data[:2] == '0b':
            base = 2
            data = data[2:]
        elif data[:2] == '0x':
            temp_data = ''
            base = 16
            data = data[2:]
            for i in range(len(data)):
                temp_data = temp_data + bin(int(data[i:(i + 1)], 16))[2:].zfill(4)
            data = temp_data
        else:
            raise ValueError("Data does not match expected format", data)

        length = len(data)

        data = data[::-1]  # data needs to be LSb first

        for i in range(int(length / 8)):
            byte_list.append(int(data[(8 * i):(8 * (i + 1))], 2))
        if -(length % 8):
            bit_list = [int(b, 2) for b in data[-(length % 8):]]

        print("Byte list", byte_list)
        print("Bit list", bit_list)

        bit_list_last = None
        if bit_list:
            bit_list_last = bit_list[-1]
            bit_list = bit_list[:-1]
        else:
            bit_list = [int(b, 2) for b in bin(int(byte_list[-1]))[2:].zfill(8)]
            byte_list = byte_list[:-1]
            bit_list_last = bit_list[-1]
            bit_list = bit_list[:-1]

        if base == 2:
            original = '0b' + (''.join([bin(b)[2:].zfill(8) for b in byte_list]) +
                               ''.join([bin(b)[2:] for b in bit_list]) +
                               bin(bit_list_last)[2:])[::-1]

        elif base == 16:
            original = hex(int((''.join([bin(b)[2:].zfill(8) for b in byte_list]) +
                                ''.join([bin(b)[2:] for b in bit_list]) +
                                bin(bit_list_last)[2:])[::-1], 2))

        if original[len(original) - 1] == 'L':
            original = original[:len(original) - 1]

        # Need to truncate bit list - Added in program
        if bit_list:
            print("Reached Modifications")
            after_bytes_bytes = num_bits // 8
            after_bytes_bits = num_bits % 8

            print(after_bytes_bytes, after_bytes_bits)

            n = len(bit_list)
            for i in range(0, n - (after_bytes_bits - 1)):
                bit_list.pop()
            print("Done Modifications")
        print(byte_list, bit_list, bit_list_last, original)
        return (byte_list, bit_list, bit_list_last, original)

    def _rebuild_read_data(self, data, num_bytes, num_bits, tms_bit=True):
        """Rebuild a binary string received from the FTDI chip
        Return the reconstructed string MSb first.
        String will be formatted as follows:
            num_bytes of valid bytes

            8 - num_bits of garbage
            num_bits of valid bits

            if tms_bit 1 else 0 bit
            8 - (tms_bit 1 else 0 bit) of garbage
        """

        temp_data = ''

        if num_bytes > 0:
            temp_data = data[0:8 * num_bytes]
            data = data[8 * num_bytes:]

        if num_bits > 0:
            # bits are shifted out msb first, and shifted in from the lsb
            temp_data += data[8 - num_bits: 8]
            if len(data) > 8:
                data = data[8:]

        if tms_bit:
            temp_data += data[8 - tms_bit: 8]
            data = data[8:]

            # temp_data += data[0]  # bit position 0 contains the value of TDO when TDI is clocked in
            # data = data[1:]
        # print('Length of Temp Data:',len(temp_data))
        # print('Remaining Data:', len(data))
        # print("REBUILT DATA",temp_data[::-1])
        return temp_data[::-1]  # '0b' +

    def shift(self, bits):
        for i in range(bits):
            mask = int('0x' + 'f' * math.ceil(bits / 4), 16)
            yield "0x%s" % ('0' * math.ceil(bits / 4) + "%x" % (1 << i & mask))[-math.ceil(bits / 4):]

    def pad_lagging_bits(self, numbits, v, total_bits):
        if v.startswith('0x') or v.startswith('0X'):
            v = v[2:]
        if len(v) % 2 == 1:
            v = '0' + v
        n_pad = (numbits + 3) // 4 * 4 - len(v) * 4
        v = '0' * (n_pad // 4) + v
        return v

    def test_to_bytes_bits(self):
        tests = ['0b0101010101',
                 '0b101010101111111',
                 '0x11223344',
                 '0b00010001001000100011001101000100',
                 '0xf102030f102030f102030f1020304f1020304f1020304f1020304444f1020304f',
                 '0xabc',
                 '0xabcd',
                 '0xabcde',
                 '0xabcdef',
                 '0b010100011']

        for t in tests:
            byte_list, bit_list, bit_list_last, original = self.to_bytes_bits(t)
            print(byte_list, bit_list, bit_list_last, original)
            if t[:2] == '0b':
                if original == t:
                    pass
                else:
                    raise Exception("Test failed")
            elif t[:2] == '0x':
                if original == t:
                    pass
                else:
                    raise Exception("Test failed")
        return

    def read_mpsse_settings(self):
        """Read expected_num bytes from the FTDI chip, once there are that many
        availible in the buffer. Return the raw bytes as a tuple of binary and
        hex strings."""
        for i in range(0xFF):
            self._ft_writeA(struct.pack('BB', 0x85, i))  # set to 84 for LoopBack
            bytes_avail = ctypes.c_int()
            USBftStatus = self._libraries.FT_GetQueueStatus(self.device_handle_1, ctypes.byref(bytes_avail))
            if USBftStatus:
                raise Exception("USBftStatus was non-zero:", USBftStatus)

            readback = self._ft_readA(bytes_avail.value)
            print(i, readback)

    def to_bytes_bits_GB100(self, data, num_bits, total_bits):
        """Return a tuple containing a list of bytes, remainder bits, the value of
        the last bit and a reconstruction of the original string.

        data: string representation of a binary or hex number '0xabc', '0b101010'
        e.g. to_bytes_bits('0xabc')        -> ([0xab], [1, 1, 0], 0)
             to_bytes_bits('0b010100011')  -> ([0x51], [], 1)
        """
        byte_list = []
        bit_list = []

        if data[-1] == 'L':
            data = data[:len(data) - 1]

        if data[:2] == '0b':
            base = 2
            data = data[2:]
        elif data[:2] == '0x':
            temp_data = ''
            base = 16
            data = data[2:]
            for i in range(len(data)):
                temp_data = temp_data + bin(int(data[i:(i + 1)], 16))[2:].zfill(4)
            data = temp_data
        else:
            raise ValueError("Data does not match expected format", data)

        # Adding padding bits
        for pad in range(total_bits - num_bits):
            data = data + '0'
        # print("Data after Padding", data)

        data = data[::-1]  # data needs to be LSB first
        # print("Data after Padding inverse", data)

        length = len(data)
        # print("Length ", length)

        truncate = (length - total_bits)
        # print("truncate", truncate)
        # print("**0",data)
        if truncate > 0:
            data = data[:-(truncate)]
        length = len(data)
        # print("Data after truncate", data)

        # print(int(length / 8))
        # print("**1",data)

        for i in range(int(length / 8)):
            byte_list.append(int(data[(8 * i):(8 * (i + 1))], 2))
            # print(byte_list)
        if -(length % 8):
            bit_list = [int(b, 2) for b in data[-(length % 8):]]

        # print("Byte list", byte_list)
        # print("Bit list",  bit_list)

        bit_list_last = None
        if bit_list:
            bit_list_last = bit_list[-1]
            bit_list = bit_list[:-1]
        else:
            bit_list = [int(b, 2) for b in bin(int(byte_list[-1]))[2:].zfill(8)]
            byte_list = byte_list[:-1]
            bit_list_last = bit_list[-1]
            bit_list = bit_list[:-1]

        if base == 2:
            original = '0b' + (''.join([bin(b)[2:].zfill(8) for b in byte_list]) +
                               ''.join([bin(b)[2:] for b in bit_list]) +
                               bin(bit_list_last)[2:])[::-1]

        elif base == 16:
            original = hex(int((''.join([bin(b)[2:].zfill(8) for b in byte_list]) +
                                ''.join([bin(b)[2:] for b in bit_list]) +
                                bin(bit_list_last)[2:])[::-1], 2))

        if original[len(original) - 1] == 'L':
            original = original[:len(original) - 1]

        # print('FOR DEBUG', byte_list, bit_list, bit_list_last)
        return (byte_list, bit_list, bit_list_last)

    def j2h_read(self, addr=None, check_ack=True):
        # print("JTAG COMPONNET READ : 0x%x" % (addr))

        start = 0x4
        client_cluster_sib = 0x3fd80000
        auto_incr_bits = 0x0
        start_transaction = 0x1
        initshift = 30
        valid_bit = 0
        read_bit = 1
        byte_enables = 0xF
        BAR_addres = 0x00  # 8 bits
        address = addr
        data_bits = 0x00000000   # 112:81
        # -31 - Read , _32 - Valid

        START_READ_DR = start << (initshift + 113) | start_transaction << (initshift + 112) | \
            data_bits << (initshift + 80) | BAR_addres << (initshift + 72) | \
            auto_incr_bits << (initshift + 70) | address << (initshift + 6) | \
            byte_enables << (initshift + 2) | read_bit << (initshift + 1) | \
            valid_bit << (initshift + 0) | client_cluster_sib

        # print("J2H Read Start")

        dr_scan_out = self.write_dr(1008, 146, hex(START_READ_DR))

        self.write_tms_0(times=862)
        self.write_tms_0(times=250)

        read_bit = 1
        start_transaction = 0x0

        END_READ_DR = start << (initshift + 113) | start_transaction << (initshift + 112) | \
            data_bits << (initshift + 80) | BAR_addres << (initshift + 72) | \
            auto_incr_bits << (initshift + 70) | address << (initshift + 6) | \
            byte_enables << (initshift + 2) | read_bit << (initshift + 1) | \
            valid_bit << (initshift + 0) | client_cluster_sib

        dr_scan_out = self.write_dr(1008, 146, hex(END_READ_DR))            # data shifted out
        self.write_tms_0(times=862)
        self.write_tms_0(times=250)

        # temp = int(dr_scan_out[0],16)
        # dr_scan_out =  temp >> (temp.bit_length() - 146)

        actual_tdo_bin = dr_scan_out[0:146]
        actual_data = actual_tdo_bin[4:36]
        actual_valid_bit = int(actual_tdo_bin[-32])
        if actual_valid_bit == 0 and check_ack:
            print(f"Read No Ack after many idle cycles. Scan-Out value = {dr_scan_out}")

        print("Read Data:", '{:0{width}x}'.format(int(actual_data, 2), width=8))
        return '{:0{width}x}'.format(int(actual_data, 2), width=8)

    def j2h_write(self, addr=None, data=None, check_ack=True):

        start = 0x4           # 3bits
        client_cluster_sib = 0x3fd80000
        auto_incr_bits = 0x0
        start_transaction = 0x1
        initshift = 30
        valid_bit = 0
        read_bit = 0
        byte_enables = 0xF
        BAR_addres = 0x00  # 8  bits
        address = addr  # 64 bits
        data_bits = data  # 32 bits

        # print("JTAG2HOST WRITE : 0x%x 0x%x " % (address, data_bits))

        SEND_WRITE_DR = start << (initshift + 113) | start_transaction << (initshift + 112) | \
            data_bits << (initshift + 80) | BAR_addres << (initshift + 72) | \
            auto_incr_bits << (initshift + 70) | address << (initshift + 6) | \
            byte_enables << (initshift + 2) | read_bit << (initshift + 1) | \
            valid_bit << (initshift + 0) | client_cluster_sib

        self.write_dr(1008, 146, hex(SEND_WRITE_DR))
        self.write_tms_0(times=862)
        self.write_tms_0(times=250)

        start_transaction = 0x0
        START_WRITE_DR = start << (initshift + 113) | start_transaction << (initshift + 112) | \
            data_bits << (initshift + 80) | BAR_addres << (initshift + 72) | \
            auto_incr_bits << (initshift + 70) | address << (initshift + 6) | \
            byte_enables << (initshift + 2) | read_bit << (initshift + 1) | \
            valid_bit << (initshift + 0) | client_cluster_sib

        dr_scan_out = self.write_dr(1008, 146, hex(START_WRITE_DR))

        self.write_tms_0(times=862)
        self.write_tms_0(times=20000)

        # look for ack
        data_bits = 0x00000000

        END_WRITE_DR = start << (initshift + 113) | start_transaction << (initshift + 112) | \
            data_bits << (initshift + 80) | BAR_addres << (initshift + 72) | \
            auto_incr_bits << (initshift + 70) | address << (initshift + 6) | \
            byte_enables << (initshift + 2) | read_bit << (initshift + 1) | \
            valid_bit << (initshift + 0) | client_cluster_sib

        dr_scan_out = self.write_dr(1008, 146, hex(END_WRITE_DR))

        actual_tdo_bin = dr_scan_out[0:146]
        actual_valid_bit = int(actual_tdo_bin[-32])
        # if actual_valid_bit == 0 and check_ack == True:
        #      print(f"Read No Ack after many idle cycles. Scan-Out value = {dr_scan_out}")
        print("Wrote Data:", "Address:", hex(addr), "Data:", hex(data))

    def jtag_unlock(self):
        # Sequence from actual silicon
        self.write_ir(10, 10, '0x018')
        self.write_dr(19, 19, '0x00080')

        self.write_tms_0(times=862)    # wait862
        self.write_tms_0(times=100)    # wait100
        self.write_tms_0(times=20)     # wait20

        self.write_ir(872, 10, '0x008')
        self.write_dr(32, 32, '0x00000000')
        self.write_ir(894, 32, '0x8d000000')
        self.write_dr(37, 37, '0x0000000000')
        self.write_tms_0(times=862)    # wait862

        self.write_ir(894, 32, '0x09000000')
        self.write_dr(885, 23, '0x400000')
        self.write_dr(893, 31, '0x02400000')
        self.write_tms_0(times=862)  # wait862
        self.write_tms_0(times=1000000)  # wait862

        self.write_ir(894, 32, '0x09000000')
        self.write_dr(893, 31, '0x02c00000')
        self.write_tms_0(times=862)    # wait862

        self.write_ir(894, 32, '0x08000000')
        self.write_dr(901, 39, '0x7fffc00000')
        self.write_tms_0(times=862)    # wait862

        self.write_ir(894, 32, '0x8d000000')
        self.write_dr(37, 37, '0x0000000000')
        self.write_tms_0(times=862)    # wait862
        self.write_tms_0(times=10)     # wait10

        self.write_ir(894, 32, '0x09000000')
        self.write_dr(893, 31, '0x02400000')
        self.write_tms_0(times=862)    # wait862

        self.write_ir(894, 32, '0x0512a223')
        self.write_tms_0(times=133)
        self.write_ir(1022, 160, '0x0528aa002b20a8a82a505495057a82a4a82a5415')
        self.write_tms_0(times=18)  # wait10
        self.write_ir(1341, 479, '0x029c15e0af02b02b02b02bf82b82bf05415f82bc15e0a82be0af05415f82be0af05782a0ae0ae0ae0af05782a0afc15f05782bc15057c15e0af05415')
        self.write_tms_0(times=7)  # wait7
        self.write_ir(2196, 1334, '0x014364d9364d9364d9364d9364d93646c9b2364d91b26c8d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9364d9')
        self.write_dr(2321, 1459, '0x7ffffffffdef7bfffffffff7efdffffffc01fff76eeffffffffe000ffff01fff803fffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffc7fff1ffff007fffffffffffffffffffffffffffffffffffffffffffffffffedbffffffffffffc01fffe00fffffffffffffffffffffffffc7fff1ffff007fffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff')
        self.write_ir(2196, 1334, '0x3e5f97e5f97e5f97e5f97e5f97e5f95f2fcaf97e57cbf2be5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5')
        self.write_ir(862, 10, '0x3e5')
        self.write_dr(879, 17, '0x00000')
        self.write_tms_0(times=862)  # wait862

        self.write_ir(862, 10, '0x014')
        self.write_tms_0(times=133)  # wait(133)
        self.write_ir(894, 32, '0x0512a223')
        self.write_tms_0(times=133)  # wait(133)
        self.write_ir(1022, 160, '0x0528aa002b20a8a82a505495057a82a4a82a5415')
        self.write_tms_0(times=18)  # wait(17)
        self.write_ir(1341, 479, '0x1b00d906c81b21b21b21b201b21b20364d901b20d906c9b206c8364d901b206c83641b26c86c86c86c83641b26c80d903641b20d93640d906c8364d9')
        self.write_dr(1036, 174, '0x3fff7777ffffffffffffffffffffffffffffffffffff')
        self.write_tms_0(times=7)
        self.write_ir(2196, 1334, '0x3e4f97e5f97e5f97e5f97e5f97e5f95f2fcaf97e57cbf2be5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5f97e5')
        self.write_dr(3259, 2397, '0x00001000040001000040003000040001000040003000040001000040001800020000c0001000060000800030000400010000400010000400010000c00010000400030000400010000400010000400030000c00010000400010000400010000c00010000400010000c00010000400010000c00030000400010000400010000c00010000400010000c00030000400010000400010000400030000400010000400010000c00010000400010000c00010000400010000c00030000400010000c00010000400030000400010000c00010000400010000c00010000400010000c00030000400010000400010000400030000400010000400010000c00010000400010000c00010000400010000c00030000400010000400010000c00010000400010000c00010000400010000c0003')
        self.write_tms_0(times=862)  # wait862

        self.write_ir(2196, 1334, '0x00805415054150541505415054150540a82a0541502a0a815054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415054150541505415')
        self.write_dr(32, 32, '0x00000000')
        self.write_tms_0(times=862)  # wait862

        self.write_ir(2196, 1334, '0x3e405415054140541505415054150540a82a0541502a0a815054150541505415050150541405015054150541505014054150541505415054150541505415054140501505415054150541505414050140541505414054150541505415054150541505415054140541505015054140541505015054150541505415054140501405415054140541505415054150541505415054150541405415054150541505415054150541505414')
        self.write_tms_0(times=133)  # wait17
        self.write_dr(901, 39, '0x0000400000')
        self.write_tms_0(times=862)  # wait862

        self.write_ir(894, 32, '0x05080000')
        self.write_tms_0(times=12)  # wait(12)
        self.write_ir(923, 61, '0x00a00a02a0a80000')
        self.write_ir(914, 52, '0x05017a0a80000')
        self.write_dr(1207, 345, '0x11fff7efd8000038000000000000000000000002000000000000000000000000000000000000007fff80000')
        self.write_tms_0(times=862)  # wait862

        self.write_ir(914, 52, '0x0501780a20000')
        self.write_tms_0(times=22)  # wait
        self.write_ir(953, 91, '0x02800a8280a0280a02a0000')
        self.write_tms_0(times=14)  # wait
        self.write_ir(914, 52, '0x05009502a0000')
        self.write_dr(893, 31, '0x42760000')
        self.write_tms_0(times=862)  # wait862

        self.write_ir(914, 52, '0x05389402a86c7')
        self.write_tms_0(times=133)  # wait(133)
        self.write_ir(1860, 998, '0x01405414054150541405014050140500a028050140280a01405014050140501505505702b05014050140501505014050140501405015805015054150501405014050150501405014050140501502a0a82a0a82a0a805015054150501405014050150501405014050140501505014050140501505014050140501405015')
        self.write_tms_0(times=18)  # wait(18)
        self.write_ir(1254, 392, '0x0509512a0a92a00004a82a254152549502a4a825402a4a812a4a82a09500a89525415254950540952541504a805412a015')
        self.write_tms_0(times=862)  # wait862

        self._ft_writeA(bytes((0x80, 0x28, 0xBB)))
        self._ft_writeA(bytes((0x4b, 0x01, 0x01)))
        self._ft_writeA(bytes((0x80, 0x28, 0xBB)))
        self._ft_writeA(bytes((0x4b, 0x00, 0x01)))
        self._ft_writeA(bytes((0x80, 0x28, 0xBB)))
        self._ft_writeA(bytes((0x4b, 0x00, 0x01)))
        self._ft_writeA(bytes((0x80, 0x20, 0xBB)))
        self._ft_writeA(bytes((0x4b, 0x00, 0x00)))

        self.write_ir(1254, 392, '0x0509512a0a92a00004a82a250142549502a4a825402a4a812a4a82a09500a89525415254950540952541504a805412a015')
        self.write_tms_0(times=10)  # wait(10)
        self.write_ir(1234, 372, '0x0507d0fa0a8fa00003e82a1f47d02a3e81f402a3e80fa3e82a07d00a87d1f4151f47d05407d1f41503e80540fa015')
        self.write_dr(1492, 630, '0x3fffffffeffc0003fbfffe00feffffe008ffffffffffff8ffffffffffffffffffffffffffffffc3fffffc01c7fffffffffffc6fffffe00ffffffc01c3fffffc01c7fffffffffffc63fffffffffffe3')
        self.write_tms_0(times=862)  # wait862

        self.write_ir(1234, 372, '0x0507c0f80a0f800003e0291f07c0283e01f00283e00f83e02807c00a07c1f0141f07c05007c1f01403e00500f8014')
        self.write_tms_0(times=133)  # wait(133)
        self.write_ir(905, 43, '0x02820a80000')
        self.write_ir(914, 52, '0x0501720a80000')
        self.write_dr(971, 109, '0x11fff7efd80000000007fff80000')
        self.write_tms_0(times=862)  # wait862

        self.write_ir(914, 52, '0x05017a0a80000')
        self.write_dr(1207, 345, '0x11fff7efd8000000000000000000000000000000000200000000000000000000000000000358007fff80000')
        self.write_tms_0(times=862)  # wait862
        self.write_tms_0(times=5)  # wait5

        self.write_ir(914, 52, '0x0501720a80000')
        self.write_dr(971, 109, '0x11fff7efd80000000007fff80000')
        self.write_tms_0(times=862)  # wait862

        # To Access Registers
        self.write_ir(914, 52, '0x0509700a80000')
        self.write_ir(914, 52, '0x0502140a80000')

    def _disable_nvjtag(self):
        self.write_ir(872, 10, '0x015')
        self.write_ir(862, 10, '0x014')
        self.write_tms_0(times=17)
        self.write_ir(894, 32, '0x0512a223')
        self.write_tms_0(times=133)
        self.write_ir(1022, 160, '0x052800002b20a8a82a505495057a82a4a82a5415')
        self.write_tms_0(times=18)
        self.write_ir(1301, 439, '0x029c15e0affff82b82bf05415f82bc15e0a82be0af05415f82be0af05782a0ae0ae0ae0af05782a0afc15f05782bc15057c15e0af05415')
        self.write_tms_0(times=7)
        self.write_ir(2196, 1334, '0x01505c1705c1505c1705c1505c1705c0a82e0541702a0b81505c1705c1705c170541705c1505c1705c1705c150541705c1705c170541705c170541705c170541505c1705c170541705c170541505c1705c1705c1505c1705c170541705c170541705c170541505c170541705c1505c170541705c170541705c170541505c1705c1705c1505c1705c170541705c170541705c170541505c1705c170541705c170541705c1705415')
        self.write_ir(862, 10, '0x014')
        self.write_tms_0(times=133)
        self.write_tms_0(times=5)


# Allow for retries for read/write if a transaction fails.
MAX_RETRIES = 20


def open_gb100(device_index=0):
    opened = False
    # The JTAG Device handle cannot be opened simultaneously.
    # Perform retries on device open to allow different tools
    # to run in parallel on one JTAG device.
    while not opened:
        try:
            j = GB100_Jtag(device_index)
            opened = True
        except BaseException:
            time.sleep(1 / 10)
            continue
    return j


def read_gb100(address, device_index):
    try:
        GB100 = open_gb100(int(device_index, 0))
        retries = 0
        success = False
        while (retries < MAX_RETRIES) and not success:
            try:
                res = str(hex(GB100.j2h_read(int(address, 0))))
                success = True
                GB100.gb100_close()
                return res
            except Exception as e:
                if retries == (MAX_RETRIES - 1) and not success:
                    print(e)
                    return str(-1)
                else:
                    retries += 1
                    time.sleep(1 / 10)
                    continue

    except Exception as e:
        print(e)
        return str(-1)


def write_gb100(address, value, device_index):
    try:
        GB100 = open_gb100(int(device_index, 0))
        retries = 0
        success = False
        while (retries < MAX_RETRIES) and not success:
            try:
                GB100.j2h_write(int(address, 0), int(value, 0))
                success = True
                GB100.gb100_close()
                return str(0)
            except Exception as e:
                if retries == (MAX_RETRIES - 1) and not success:
                    print(e)
                    return str(-1)
                else:
                    retries += 1
                    time.sleep(1 / 10)
                    continue
    except Exception as e:
        print(e)
        return str(-1)


if __name__ == "__main__":
    j = None
    j = GB100_Jtag()
    j.jtag_unlock()

    # j.j2h_read(addr=0x00b60000)

    # j.j2h_write(addr=0x00b60000, data=0xA6849FB4)
    # j.j2h_read(addr=0x00b60000)

    # j.j2h_write(addr=0x00b60000, data=0xA684FFFF)
    # j.j2h_read(addr=0x00b60000)
    # j.j2h_read(addr=0x00b60000)

    # j.j2h_write(addr=0x00b60000, data=0xA684F0F0)
    # j.j2h_read(addr=0x00b60000)

    # j.j2h_write(addr=0x00b60000, data=0x00000000)
    # j.j2h_read(addr=0x00b60000)

    # j.j2h_write(addr=0x00b60000, data=0x00000001)
    # j.j2h_write(addr=0x00b60000, data=0x00000002)
    # j.j2h_write(addr=0x00b60000, data=0x00000003)

    # j.j2h_read(addr=0x00b60000)

    # j.j2h_read(addr=0x0)

    # # time.sleep(5)
    # j.j2h_read(addr=0x00E4349C)
    # j.j2h_read(addr=0x00E4349C)
    # j.j2h_write(addr=0x00E4349C, data=0x01000000)
    # j.j2h_read(addr=0x00E4349C)
    # j.j2h_read(addr=0x00E4349C)
    # j.j2h_write(addr=0x00E4349C, data=0x00000000)
    # j.j2h_read(addr=0x00E4349C)
    # j.j2h_read(addr=0x00E4349C)
    # j.j2h_write(addr=0x00b60000, data=0xABCD1234)
    # j.j2h_read(addr=0x00b60000)
    # # time.sleep(5)

    # # XPL Registers:
    # j.j2h_read(addr=0x00e4349c)
    # j.j2h_read(addr=0x00e80004)
    # j.j2h_read(addr=0x00e6e004)
    # j.j2h_read(addr=0x00e6dc04)

    # j.j2h_write(addr=0x00b60000, data=0x00000001)
    # j.j2h_read(addr=0x00b60000)
    # j.j2h_write(addr=0x00b60000, data=0x00000010)
    # j.j2h_read(addr=0x00b60000)
    # j.j2h_write(addr=0x00b60000, data=0x00000100)
    # j.j2h_read(addr=0x00b60000)
    # j.j2h_write(addr=0x00b60000, data=0x00001000)
    # j.j2h_read(addr=0x00b60000)
    # j.j2h_write(addr=0x00b60000, data=0x00010000)
    # j.j2h_read(addr=0x00b60000)
    # j.j2h_write(addr=0x00b60000, data=0x00100000)
    # j.j2h_read(addr=0x00b60000)
    # j.j2h_write(addr=0x00b60000, data=0x01000000)
    # j.j2h_read(addr=0x00b60000)
    # j.j2h_write(addr=0x00b60000, data=0x10000000)
    # j.j2h_read(addr=0x00b60000)
    # j.j2h_write(addr=0x00b60000, data=0x0000FFFF)
    # j.j2h_read(addr=0x00b60000)
    # j.j2h_write(addr=0x00b60000, data=0xFFFF0000)
    # j.j2h_read(addr=0x00b60000)

    # # XPL Registers
    # j.j2h_read(addr=0x00e43058)
    # j.j2h_write(addr=0x00e43058, data=0x180)
    # j.j2h_read(addr=0x00e43058)

    # j.j2h_read(addr=0x00e43064)
    # j.j2h_write(addr=0x00e43064, data=0x60)
    # j.j2h_read(addr=0x00e43064)

    # j.j2h_read(addr=0x00e43070)
    # j.j2h_write(addr=0x00e43070, data=0x220)
    # j.j2h_read(addr=0x00e43070)

    # j.j2h_read(addr=0x00e43004)
    # j.j2h_write(addr=0x00e43004, data=0x80480C11)
    # j.j2h_read(addr=0x00e43004)
