# -- coding: utf-8 --
import serial
import struct
import time

LCPU_DHCSR = 0x3000EDF0
LCPU_DCRSR = 0x3000EDF4
LCPU_DCRDR = 0x3000EDF8
LCPU_DEMCR = 0x3000EDFC

HCPU_DHCSR = 0xF000EDF0
HCPU_DCRSR = 0xF000EDF4
HCPU_DCRDR = 0xF000EDF8
HCPU_DEMCR = 0xF000EDFC

EXIT_DBG_RSP_TYPE = 0xD0
ENTER_DBG_RSP_TYPE = 0xD1
READ_RSP_TYPE = 0xD2
WRITE_RSP_TYPE = 0xD3

HDR_SIZE = 6
START_WORD = b'\x7E\x79'
response_len = 0    

def print_byte_stream(data):
     print(" ".join(["{:02X}".format(i) for i in (data)]))


class DbgUart:
    def __init__(self, port, baudrate=1000000):
        self.frame = b''
        self.response_len = 0
        self.frame_total_len = 0
        self.ser = serial.Serial()
        self.ser.port = port
        self.ser.baudrate = baudrate
        self.ser.timeout = 0
        try:
            self.ser.open()
        except serial.serialutil.SerialException as e:
            print(f'串口{self.ser.port}打开失败')

    def __del__(self):
        self.ser.close()      

    def _send(self, data):
        hdr = self._create_header(len(data))
        if not self.ser.is_open:
            return
        
        #print_byte_stream(hdr + data)
        self.ser.reset_input_buffer()
        self.ser.write(hdr + data)

    def _create_header(self, len):
        '''
        7E 79 00 01(长度) 10（通道号） 00（校验和）
        '''        
        return START_WORD + struct.pack('<HBB', len, 0x10, 0)
 

    def _read_frame_head(self, ser):
        if (len(self.frame) == 0):
            start_char = ser.read(1)
            if (len(start_char) < 1):
                # print("wrong len")
                return None

            
            if (START_WORD[0] != start_char[0]):
                # print("mismatch")
                return None

            self.frame = start_char
            # print('first')
            # print_byte_stream(self.frame)
        elif (len(self.frame) == 1):
            start_char = ser.read(1)
            if (len(start_char) < 1):
                return None

            if (START_WORD[1] != start_char[0]):
                if (START_WORD[0] == start_char[0]):
                    # print('first1')
                    self.frame = start_char
                    # print_byte_stream(self.frame)
                else:
                    #print('reset_first')
                    #print_byte_stream(start_char)
                    self.frame = ''
                return None

            self.frame += start_char 
            #print_byte_stream(self.frame)       

        if (len(self.frame) >= 2) and (len(self.frame) < HDR_SIZE):
            remaining_head_len = HDR_SIZE - len(self.frame)
            temp = ser.read(remaining_head_len)
            self.frame += temp
            if len(temp) < remaining_head_len:
                return None
            body_len, = struct.unpack("<H", self.frame[2:4])   
            if self.response_len == 0:  
                self.frame_total_len = HDR_SIZE + body_len
            else:
                self.frame_total_len = HDR_SIZE + self.response_len    

    def _read_frame(self, timeout=1.0):
        result = None

        stop_time = time.time() + timeout
        while (True):
            # receive frame head
            if (len(self.frame) < HDR_SIZE):
                self._read_frame_head(self.ser)

            # receive frame body and frame tail
            if (len(self.frame) >= HDR_SIZE) and (len(self.frame) < self.frame_total_len):
                remaining_payload_len = self.frame_total_len - len(self.frame)
                frame_body_tail = self.ser.read(remaining_payload_len)
                self.frame += frame_body_tail

            # check frame
            if (len(self.frame) >= HDR_SIZE) and (len(self.frame) >= self.frame_total_len):
                result = self.frame[HDR_SIZE:]
                self.frame = b''
                break

            if time.time() > stop_time:
                self.frame = b''
                break
        return result

    ''' rsp_data_len is len in bytes excluding rsp_type and tail byte '''
    def _wait_response(self, rsp_type, rsp_data_len=0):
        #workaround for read response
        if (rsp_type == READ_RSP_TYPE):
            self.response_len = rsp_data_len + 2  #plus rsp_type and tail
        data = self._read_frame()
        if (rsp_type == READ_RSP_TYPE):
            self.response_len = 0
        if data is None:
            print("No frame received")
            return None
        if ((data[0] != rsp_type) or (data[-1] != 0x6)):
            print("Invalid frame")
            #print_byte_stream(data)            
            return None
        if len(data) != (rsp_data_len + 2):
            print("Invalid frame len")
            #print_byte_stream(data)                
            return None

        return data


    def enter_dbg_mode(self):
        if not self.ser.is_open:
            return False

        self._send(b"\x41\x54\x53\x46\x33\x32\x05\x21")
        if self._wait_response(ENTER_DBG_RSP_TYPE):
            # print("Succ to enter dbg mode")
            succ = True
        else:
            print("Fail to enter dbg mode")    
            succ = False
        return succ

    def exit_dbg_mode(self):
        self._send(b"\x41\x54\x53\x46\x33\x32\x18\x21")
        if self._wait_response(EXIT_DBG_RSP_TYPE):
            print("Succ to exit dbg mode")
            succ = True
        else:
            print("Fail to exit dbg mode")      
            succ = False
        return succ    

    # write memory
    def write(self, addr, data_size, data):
        packet = b'\x40\x77' + struct.pack('<LH', addr, data_size)
        if data_size == 1:
            data = [data]
        else:
            assert len(data) == data_size, "{} DWORD expected".format(data_size)
        for d in data:
            packet = packet + struct.pack('<L', d)   
        self._send(packet)   

        if self._wait_response(WRITE_RSP_TYPE):
            #print("Succ to write")
            succ = True
        else:
            print("Fail to write")   
            succ = False
        return succ

    ''' read memory '''
    def read(self, addr, data_size):
        if not self.ser.is_open:
            return None

        packet = b'\x40\x72' + struct.pack('<LH', addr, data_size)
        self._send(packet)
        data = self._wait_response(READ_RSP_TYPE, data_size * 4)
        if data:
            data = data[1:(1 + data_size*4)]
            # print_byte_stream(data)
        else:
            print("Fail to read")   
        return data

    def read_and_print(self, addr, data_size):
        data = self.read(addr, data_size)
        if data is None:
            return
        s = ''
        for i in range(data_size):
            if 0 == (i % 4):
                s = "{:08X} =".format(addr)
                addr += 4
            d, = struct.unpack('<L', data[i*4:(i+1)*4])
            s += ' {:08X}'.format(d)
            if 3 == (i % 4):
                print(s)
                s = ''
        if s != '':
            print(s)

    def halt(self, hcpu = True):
        if hcpu:
            DHCSR = HCPU_DHCSR
        else:
            DHCSR = LCPU_DHCSR    
        if not self.write(DHCSR, 1, 0xa05f0003):
            print("CPU cannot be halted")
            return
        data = self.read(DHCSR, 1)
        if data is None:
            print("CPU cannot be halted")
            return
        data, = struct.unpack('<L', data)
        print("DHCSR = {:08X}".format(data))
        if data & 0x20000:
            succ = True
            print('CPU Halted')
        else:
            succ = False
            print('CPU cannot be halted')
        return succ

    def step(self, hcpu = True):
        if hcpu:
            DHCSR = HCPU_DHCSR
        else:
            DHCSR = LCPU_DHCSR    
        if not self.write(DHCSR, 1, 0xa05f0005):
            print("Step fails")
            return
        data = self.read(DHCSR, 1)
        if data is None:
            print("Step fails")
            return False    
        data, = struct.unpack('<L', data)
        print("DHCSR = {:08X}".format(data)) 
        return True

    def go(self, hcpu = True):
        if hcpu:
            DHCSR = HCPU_DHCSR
        else:
            DHCSR = LCPU_DHCSR    
        if not self.write(DHCSR, 1, 0xa05f0000):
            print("CPU go fails")
            return False
        return True

    def regs(self, hcpu = True):
        if hcpu:
            DCRSR = HCPU_DCRSR
            DCRDR = HCPU_DCRDR
            DHCSR = HCPU_DHCSR            
        else:
            DCRSR = LCPU_DCRSR  
            DCRDR = LCPU_DCRDR
            DHCSR = LCPU_DHCSR    

        data = self.read(DHCSR, 1)
        if data is None:
            print('CPU cannot be halted')
            return False
        data, = struct.unpack('<L', data)
        if 0 == (data & 0x20000):
            print('CPU cannot be halted')
            return False
        
        r = []    
        for i in range(21):
            if not self.write(DCRSR, 1, i):
                print('Write DCRSR fail')
                return False
            data = self.read(DCRDR, 1)
            if data is None:
                print('Read DCRDR fail')
                return False
            data, = struct.unpack('<L', data)
            r.append(data)
        for i in range(21):
            if i < 13:
                print('R{} = {:08X}'.format(i, r[i]))
            elif i == 13:
                print('SP(R13) = {:08X}'.format(r[i]))    
            elif i == 14:
                print('LR(R14) = {:08X}'.format(r[i]))    
            elif i == 15:
                print('PC      = {:08X}'.format(r[i]))    
            elif i == 16:
                print('XPSR    = {:08X}'.format(r[i]))          
            elif i == 17:
                print('MSP     = {:08X}'.format(r[i]))      
            elif i == 18:
                print('PSP     = {:08X}'.format(r[i]))                   
            elif i == 20:
                print('CONTROL = {:08X}'.format(r[i]))    

        return True

    def get_pc(self, hcpu = True):
        if hcpu:
            DCRSR = HCPU_DCRSR
            DCRDR = HCPU_DCRDR
            DHCSR = HCPU_DHCSR            
        else:
            DCRSR = LCPU_DCRSR  
            DCRDR = LCPU_DCRDR
            DHCSR = LCPU_DHCSR    

        data = self.read(DHCSR, 1)
        if data is None:
            print('CPU cannot be halted')
            return None
        data, = struct.unpack('<L', data)
        if 0 == (data & 0x20000):
            print('CPU cannot be halted')
            return None
        
        r = []    
        if not self.write(DCRSR, 1, 15):
            print('Write DCRSR fail')
            return None
        data = self.read(DCRDR, 1)
        if data is None:
            print('Read DCRDR fail')
            return None
        data, = struct.unpack('<L', data)
        
        return data


    def wreg(self, i, value, hcpu=True):
        if hcpu:
            DCRSR = HCPU_DCRSR
            DCRDR = HCPU_DCRDR
        else:
            DCRSR = LCPU_DCRSR  
            DCRDR = LCPU_DCRDR        
        if not self.write(DCRSR, 1, i | (1 << 16)):
            print("write DCRSR fails")
            return False
        if not self.write(DCRDR, 1, value):
            print("write DCRDR fails")
            return False
        return True

    def test_ex(self, addr, count=100, data_size=1024*8):    
        for i in range(count):
            print("test[{}]".format(count))
            data = self.read(addr, data_size)
            if data is None:
                break
            for i in range(data_size):
                d, = struct.unpack('<L', data[i*4:(i+1)*4])
                if (d != 0):
                    print("invalid data stream")
                    print_byte_stream(data[i*4:])
                    return False

            time.sleep(0.150)
          
        return True
