import rp2
from machine import Pin
from rp2 import PIO
import time

# number of consecutive GPIO; initial GPIO direction (in or out pin); initial GPIO value (high or low)
@rp2.asm_pio(out_init=[PIO.OUT_LOW])
def echo():
    wrap_target()
    mov(pins, isr)     
    mov(isr, invert(isr))
    pull(block)      
    mov(x, osr)
    mov(y, x)
    label("loop")
    jmp(y_dec, "loop")  
    wrap()

sm = rp2.StateMachine(0, echo, freq=1_000_000, out_base=Pin(7))
sm.active(1)

def play(freq):
  if freq:
    sm.put(1_000_000//freq)
  else:
    sm.put(0)

print("Start")
FDEBUG_ADDR = 0x50200000 + 0x008
TXSTALL_BIT = 1 << 24

# Read FDEBUG
print("TXSTALL_1 =", bool(machine.mem32[FDEBUG_ADDR] & TXSTALL_BIT))
time.sleep(1)
sm.put(1_000_000)
# Write FDEBUG
#machine.mem32[FDEBUG_ADDR] = TXSTALL_BIT
time.sleep(0.1)
# Read FDEBUG
print("TXSTALL_2 =", bool(machine.mem32[FDEBUG_ADDR] & TXSTALL_BIT))
time.sleep(1)
# Read FDEBUG
print("TXSTALL_3 =", bool(machine.mem32[FDEBUG_ADDR] & TXSTALL_BIT))

while True:
  time.sleep(1)

play(500)
time.sleep(1)
play(1000)
time.sleep(1)
play(0)