import time
import microcontroller
import digitalio

DAP_TRANSFER_OK = 1 << 0
DAP_TRANSFER_RnW = 1 << 1
DAP_TRANSFER_WAIT = 1 << 1
DAP_TRANSFER_FAULT = 1 << 2
DAP_TRANSFER_ERROR = 1 << 3

class SWD:
    def __init__(self, swdio, swclk):
        self.swdio = swdio
        self.swclk = swclk
        self.swclk = digitalio.DigitalInOut(swclk)
        self.swclk.switch_to_output()
        self.swclk.value = 1
        self.swdio = digitalio.DigitalInOut(swdio)
        self.swdio.switch_to_output()
        self.swdio.value = 1
        self.turnaround = 1
        self.idle_cycles = 0
        self.data_phase = 0

    def sw_clk_cycle(self):
        self.swclk.value = 0
        microcontroller.delay_us(1)
        self.swclk.value = 1
        microcontroller.delay_us(1)

    def sw_write_bit(self, bit):
        #self.swdio.switch_to_output()
        self.swdio.value = bit
        self.swclk.value = 0
        microcontroller.delay_us(1)
        self.swclk.value = 1
        microcontroller.delay_us(1)

    def sw_read_bit(self):
        self.swclk.value = 0
        microcontroller.delay_us(1)
        #self.swdio.switch_to_input()
        bit = self.swdio.value
        self.swclk.value = 1
        microcontroller.delay_us(1)
        return bit

    def swd_transfer(self, request, data=0):
        # Packet Request
        parity = 0
        self.sw_write_bit(1)
        bit = request >> 0
        self.sw_write_bit(bit)
        parity += bit
        bit = request >> 1
        self.sw_write_bit(bit)
        parity += bit
        bit = request >> 2
        self.sw_write_bit(bit)
        parity += bit
        bit = request >> 3
        self.sw_write_bit(bit)
        parity += bit
        self.sw_write_bit(parity)
        self.sw_write_bit(0)
        self.sw_write_bit(1)

        # Turnaround
        self.swdio.switch_to_input()
        for n in range(self.turnaround, 0, -1):
            self.sw_clk_cycle()

        # Ack
        bit = self.sw_read_bit()
        ack = bit << 0
        bit = self.sw_read_bit()
        ack |= bit << 1
        bit = self.sw_read_bit()
        ack |= bit << 2

        if (ack == DAP_TRANSFER_OK):
            # Data Transfer
            if (request & DAP_TRANSFER_RnW):
                # Read data
                val = 0
                parity = 0
                for n in range (32, 0, -1):
                    bit = self.sw_read_bit()
                    parity += bit
                    val >>= 1
                    val |= bit << 31
                bit = self.sw_read_bit()
                if ((parity ^ bit) & 1): 
                    ack = DAP_TRANSFER_ERROR
                data = val
                # Turnaround
                for n in range(self.turnaround, 0, -1):
                    self.sw_clk_cycle()
                self.swdio.switch_to_output()
            else:
                # Turnaround
                for n in range(self.turnaround, 0, -1):
                    self.sw_clk_cycle()
                self.swdio.switch_to_output()
                # Write data
                val = data
                parity = 0
                for n in range (32, 0, -1):
                    self.sw_write_bit(val)
                    parity += val
                    val >>= 1
                self.sw_write_bit(parity)
            # Idle cycles
            n = self.idle_cycles
            if (n):
                self.swdio.value = 0
                for _ in range(n, 0, -1):
                    self.sw_clk_cycle()
            self.swdio.value = 1
            return (ack, data)

        if (ack == DAP_TRANSFER_WAIT or ack == DAP_TRANSFER_FAULT):
            # WAIT or FAULT response
            if (self.data_phase and ((request & DAP_TRANSFER_RnW) != 0)):
                for n in range (33, 0, -1):
                    self.sw_clk_cycle()
            # Turnaround
            for n in range(n, self.turnaround, -1):
                self.sw_clk_cycle()
            self.swdio.switch_to_output()
        
            if (self.data_phase and ((request & DAP_TRANSFER_RnW) == 0)):
                self.swdio.value = 0
                for n in range (33, 0, -1):
                    self.sw_clk_cycle()
            self.swdio.value = 1 
            return (ack, data)

        # Protocol error
        for n in range(self.turnaround + 33, 0, -1):
            self.sw_clk_cycle();
        self.swdio.switch_to_output()
        self.swdio.value = 1
        return (ack, data)

if __name__ == "__main__":
    import board
    swd = SWD(board.D12, board.D11)
    while True:
        print("displaying debug port register 0")
        # Read debug port register
        SWD_REG_DP = 0
        SWD_REG_R = 1 << 1
        SWD_REG_ADR = 0 & 0x0c
        tmp_in = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR
        response = swd.swd_transfer(tmp_in)
        ack = response[0]
        data = response[1]
        print("0x%x" %data)
        time.sleep(1)