import gdb
import time

# get the current inferior, i.e. the connection to target
inferior = gdb.selected_inferior()

def target_break():
    if inferior.threads()[0].is_running():
        gdb.execute(interrupt)

def target_continue():
    t = inferior.threads()
    if not(inferior.threads()[0].is_running()):
        gdb.execute('continue')

def target_u32read(address, endian='little'):
    target_break()
    value = inferior.read_memory(address, 4)
    return int.from_bytes(value, byteorder=endian)

def target_u32write(address, value, endian='little'):
    target_break()
    data = value.to_bytes(4, byteorder=endian)  # Adjust byteorder as needed
    inferior.write_memory(address, data)

def target_regset(name, value):
    target_break()
    gdb.execute(f'set ${name} = {value}')



def target_prepare():
    """
    Prepares the target for IAP operations.
    This implementation is an example for the LPC546xx series of chips.
    """
    
    LPC546XX_WDT_MODE = 0x4000C000
    LPC546XX_WDT_PERIOD_MAX = 0xFFFFFF
    LPC546XX_WDT_PROTECT = (1 << 4)
    LPC546XX_MAINCLKSELA = 0x40000280
    LPC546XX_MAINCLKSELB = 0x40000284
    LPC546XX_AHBCLKDIV =   0x40000380
    LPC546XX_FLASHCFG  =   0x40000400

    gdb.execute('set mem inaccessible-by-default off')
    target_break()
    gdb.Breakpoint("*0x00020000")
    gdb.Breakpoint("*0x00020001")
    # check if watchdog timer's on
    wdt_mode = target_u32read(LPC546XX_WDT_MODE)
    # if WDT on, we can't disable it, but we may be able to set a long period
    if (wdt_mode and not(wdt_mode & LPC546XX_WDT_PROTECT)):
        target_u32write(LPC546XX_WDT_CNT, LPC546XX_WDT_PERIOD_MAX)

    # SYSCLK: 12MHz FRO
    target_u32write(LPC546XX_MAINCLKSELA, 0)
    # use MAINCLKSELA
    target_u32write(LPC546XX_MAINCLKSELB, 0)
	# div by 1
    target_u32write(LPC546XX_AHBCLKDIV, 0)
	# recommended default configuration
    target_u32write(LPC546XX_FLASHCFG, 0x1A)
    

def iap_command(a0, a1=0, a2=0, a3=0, a4=0):
    print(f"IAPy: Command {a0} {a1} {a2} {a3} {a4}")
    target_break()
    # setup request
    target_u32write(0x2000ffa0, a0)
    target_u32write(0x2000ffa4, a1)
    target_u32write(0x2000ffa8, a2)
    target_u32write(0x2000ffac, a3)
    target_u32write(0x2000ffb0, a4)

    # clear result
    target_u32write(0x2000ffc0, 0xAAAAAAAA)
    target_u32write(0x2000ffc4, 0xAAAAAAAA)
    target_u32write(0x2000ffc8, 0xAAAAAAAA)
    target_u32write(0x2000ffcc, 0xAAAAAAAA)
    target_u32write(0x2000ffd0, 0xAAAAAAAA)

    # call setup
    target_break()    
    target_regset('lr', 0x00020001)
    target_regset('r0', 0x2000ffa0)
    target_regset('r1', 0x2000ffc0)
    # set PC
    gdb.execute('jump *0x03000205')
    # use PSP instead because MSP is most-likely not accessible
    target_regset('psp', 0x2000ffa0) 
    target_regset('control', 0x2)

    # execute and wait
    target_continue()
    time.sleep(0.5)

    # restore execution to MSP
    target_break()
    target_regset('msp', 0x2000ffa0)
    target_regset('control', 0x0)

    # gather results
    res0 = target_u32read(0x2000ffc0)
    res1 = target_u32read(0x2000ffc4)
    res2 = target_u32read(0x2000ffc8)
    res3 = target_u32read(0x2000ffcc)
    res4 = target_u32read(0x2000ffd0)

    return res0,res1,res2,res3,res4

def iap_flash_sector_erase(start_sector, end_sector, sysclk=12000):
    print(f"IAPy: Erasing sectors {start_sector} to {end_sector}")
    target_prepare()
    # prepare sectors
    iap_command(50, start_sector, end_sector)
    # erase sectors
    iap_command(52, start_sector, end_sector, sysclk)
    target_break()