import pyocd
from pyocd.core.helpers import ConnectHelper
from time import sleep


AHB_AP_ADDR = 0x0
CGF_AP_ADDR = 0x1
SEC_AP_ADDR = 0x2
ET_AP_ADDR = 0x3
PWR_AP_ADDR = 0x4

DSSM_BC_FACTORY_RESET = 0x020A
DSSM_BC_MASS_ERASE = 0x020C
DSSM_BC_PW_AUTH = 0x030E
DSSM_DATA_EXCHANGE = 0x00EE
DSSM_WAIT_FOR_DEBUG = 0x0206

DEBUGSS_SECAP_TCR_TRANSMIT_FULL_MASK = 0x00000001
DEBUGSS_SECAP_TCR_TRANSMIT_EMPTY_MASK = 0x00000000
DEBUGSS_SECAP_RCR_RECEIVE_FULL_MASK = 0x00000001
DEBUGSS_SECAP_RCR_RECEIVE_EMPTY_MASK = 0x00000000
SECAP_CTL_MASK = 0xFFFF
SECAP_CMD_MASK = 0x00FE

DSSM_CMD_RECEIVED = 0x0100
DSSM_CMD_NOT_RECEIVED = 0x0101
DSSM_ERROR_UNEXPECTED_COMMAND = 0x0102

TXDATA_ADDR = 0x00
TXCTL_ADDR = 0x04
RXDATA_ADDR = 0x08
RXCTL_ADDR = 0x0C
RXCTL_DATA_AVAIL_MASK = 0x00000001 
SEC_AP_ID_ADDR = 0xFC
SEC_AP_ID_EXPECTED = 0x002E0000


SYSCTL_RESETLVL_ADDRESS =   0x400B0300
SYSCTL_RESETLVL_SYSRESET =  0x0
SYSCTL_RESETLVL_BOOTRST =   0x1
SYSCTL_RESETLVL_BSL_ENTRY = 0x2
SYSCTL_RESETLVL_POR =       0x3
SYSCTL_RESETLVL_BSL_EXIT =  0x4

SYSCTL_RESETCMD_ADDRESS =   0x400B0304
SYSCTL_RESETCMD_RESET =     0xE4000001

def swdReset(debugPort,resetlevel:int):
    accessPort = debugPort.aps[AHB_AP_ADDR]
    accessPort.write_reg(SYSCTL_RESETLVL_ADDRESS, resetlevel)
    accessPort.write_reg(SYSCTL_RESETCMD_ADDRESS, SYSCTL_RESETCMD_RESET)
    
def nReset(debugPort, msDelay:int):
    debugPort.assert_reset(True)
    sleep(msDelay/1000.0)
    debugPort.assert_reset(False)
    
    
def issue_factory_reset():
    # Connect to the first available CMSIS-DAP probe
    session = ConnectHelper.session_with_chosen_probe(
        target_override="mspm0g3519",
        options={
            'auto_unlock': False,
            'frequency': 1000000  # 1 MHz clock
        }
    )
    
    if session is None:
        print("No CMSIS-DAP probe detected")
        return
    
    try:
        # Open the session
        #TODO: In flm_region_builder.py, remove workaround in _add_flash_subregions() once
        session.open()
        
        # Get the target
        target = session.target
        
        # Get the DP (Debug Port)
        dp = target.dp
        
        ap = dp.aps[SEC_AP_ADDR]
        
        if SEC_AP_ADDR < len(dp.aps):
            ap = dp.aps[SEC_AP_ADDR]
        
            # Read a register from this AP
            # This is just an example - replace with the actual register address
            address = SEC_AP_ID_ADDR  # Base address for the AP
            value = ap.read_reg(address)
            if value == SEC_AP_ID_EXPECTED:
                print(f"AP[{SEC_AP_ADDR}] Register at 0x{address:02x}: 0x{value:08x}")
                ap.write_reg(TXCTL_ADDR, DSSM_BC_FACTORY_RESET)
                ap.write_reg(TXDATA_ADDR, 0)
                
                # Issue a device boot reset (nrst pulse  < 1s) to execute factory reset command
                nReset(dp, 200)
                
                while (ap.read_reg(RXCTL_ADDR) & RXCTL_DATA_AVAIL_MASK) == 0:
                    pass
                
                #TODO: Add Password support
                
            else:
                print(f"Incorrect SEC_AP ID detected, no factory reset performed: AP[{address}] Register at 0x{address:02x}: 0x{value:08x}")
        else:
            print(f"AP[{SEC_AP_ADDR}] not available. Only {len(dp.aps)} APs detected.")
        
    finally:
        # Close the session

        session.close()
if __name__ == "__main__":
    issue_factory_reset()