##################################################################
# This code is for the RPi Pico 2W!!
##################################################################

from machine import mem32, Pin
import time
import rp2
from micropython import const

# ================================================================
@micropython.asm_thumb
def toggleAsmAsym():   # 33.3 MHz
    movwt (r1, 0xD0000010)  # GPIO_OUT
    #movwt (r1, 0xD0000028)  # GPIO_XOR
    mov(r2, 0x0001)
    mov(r3, 0x0000)

    label(loop)
    
    strb(r2, [r1,0])
    strb(r3, [r1,0])
    b(loop)

# ================================================================
@micropython.asm_thumb
def toggleAsmSym():   # 25 MHz
    movwt (r1, 0xD0000010)  # GPIO_OUT
    #movwt (r1, 0xD0000028)  # GPIO_XOR
    mov(r2, 0x0001)
    mov(r3, 0x0000)

    label(loop)

    strb(r2, [r1,0])
    nop()
    strb(r3, [r1,0])
    b(loop)

# ================================================================
@micropython.viper
def toggleViperSym():   # 16.6 mHz
    one = const(1)
    ptr = ptr32(0xD0000028)  # GPIO toggle register

    while True:
        ptr[0] = one

# ================================================================
@micropython.viper
def toggleViperAsym():   # 33MHz
    constOne   = 1
    
    ptrSet  = ptr32(0xD0000018)  # GPIO set register
    ptrClr  = ptr32(0xD0000020)  # GPIO clr register
    while True:
        ptrSet[0] = constOne
        ptrClr[0] = constOne


def main():

    print("Micropython pin toggle speed tests") 

    # set CPU clock
    machine.freq(int(100*1e6))

    # configure test port/pin by using the pin class
    led=Pin(0,mode=Pin.OUT)  
    addrSIO = const(0xD0000000)

    # show GPIO 'out' register content OFF and ON state
    value=mem32[addrSIO + 0x10]
    print("GPIO out: " + hex(value)) 
    led.on()
    value=mem32[addrSIO + 0x10]
    print("GPIO out: " + hex(value)) 

    # read CPU id just for fun (gives the current core number we are running on)
    value=mem32[addrSIO + 0x00]
    print("CPU-ID  : " + hex(value)) 

    time.sleep_ms(3)   # time for sending the prints above

    # set GPIO output properties (not neccessary)
    # addr = const(0x40038004)  # Pad Ctrl GPIO0
    # mem32[addr] = mem32[addr] or 1

    # 4 different tests
    # toggleAsmAsym()        # 33 MHz
    # toggleAsmSym()         # 25 MHz
    # toggleViperSym()       # 16.6 MHz
    toggleViperAsym()      # 33 MHz

if __name__ == '__main__':
    main()