# sine wave synthesis with ADC loopback
#
####
# Asynch hardware:
# DMA 0 moves sine array to PWM slice 1A (cc low 16 bits)
# DMA 1 resets the source pointer for DMA 0
# DMA 2 moves ADC to PWM slice 0A
# DMA 3 restarts DMA 2
# PWM 1A outputs sine wave connected to ADC input
# PWM 0A outputs ADC reading
# NOTE -- you may need to power cycle the rp2040 f
####
# Wiring:
# PWM slice 1A (GPIO18) -> 2K/10nF lowpass (sine output)
#    lowpass  -> ADC0 (GPiO26)
#    lowpass  -> scope probe
# PWM slice 0A (GPIO0) -> 2K/10nF lowpass (ADC loopback)
#    lowpass  -> scope probe
# rp2040 ground -> lowpass, scope
# Lowpass cutoff 50 Kradians/sec ~ 8 KHz
####
# datasheet
# https://datasheets.raspberrypi.org/rp2040/rp2040-datasheet.pdf
# ====================================
# imports
import array
import time
import machine
import math
from ctypes import addressof

# ====================================
# === Register write functions =======
# Each peripheral register block is allocated 4kB of address space,
# with registers accessed using one of 4 methods,selected by
# address decode.
# Addr + 0x0000 : normal read write access
# Addr + 0x1000 : atomic XOR on write
# Addr + 0x2000 : atomic bitmask set on write
# Addr + 0x3000 : atomic bitmask clear on write
def IOreg_write(reg_addr, data):
    machine.mem32[reg_addr] = data
#   
def IOreg_set(reg_addr, data):
    machine.mem32[reg_addr + 0x2000] = data
#
def IOreg_clr(reg_addr, data):
    machine.mem32[reg_addr + 0x3000] = data
#   
def IOreg_xor(reg_addr, data):
    machine.mem32[reg_addr + 0x1000] = data

# === ADC ============================
ADC_BASE = 0x4004c000
ADC_CS = ADC_BASE
ADC_RESULT = 0x04 + ADC_BASE
ADC_FIFO_CS = 0x08 + ADC_BASE
ADC_FIFO = 0x0c + ADC_BASE
ADC_DIV = 0x10 + ADC_BASE
# === bits in ADC_CS
# Round-robin sampling. 1 bit per channel.
# Set all bits to 0 to disable.
def ADC_RROBIN(channels):
    return (channels & 0x0f)<<16
# Select analog mux input. Updated automatically in roundrobin mode.
def ADC_AINSEL(channel):
    return (channel & 0x07)<<12
# ready -- RO -
# 1 if the ADC is ready to start a new conversion.
# Implies any previous conversion has completed.
# 0 whilst conversion in progress.
def ADC_READY(ready):
    return (ADC_CS & (1<<8))
# Continuously perform conversions whilst this bit is 1. A
# new conversion will start immediately after the previous
ADC_START_MANY = (1<<3)
ADC_START_ONCE = (1<<2)
# Power on temperature sensor. 1 - enabled.
ADC_TS_EN = (1<<1)
# Power on ADC and enable its clock.
ADC_EN = 1

# === bits in ADC_FIFO CS
def ADC_THRESH(fifo_level):
    return (fifo_level & 0x0f)<<24
# If 1: assert DMA requests when FIFO contains data
ADC_DREQ_EN = (1<<3)
# If 1: FIFO results are right-shifted to be one byte in size.
# Enables DMA to byte buffers.
ADC_SHIFT = (1<<1)
# enable fifo
ADC_FIFO_EN = 1

# bit in ADC_DIV
# Clock divider. If non-zero, CS_START_MANY will start conversions
# at regular intervals rather than back-to-back.
# The divider is reset when either of these fields are written.
# Total period is 1 + INT + FRAC / 256
# Dividing from a 48 MHz COCK!
def ADC_Div_int(int_part):
    return (int_part & 0xffff)<<8
def ADC_Div_frac(frac_part):
    return (frac_part & 0xff)

# === IO =============================
# define low level i/o
# Function select -- datasheeet section 2.19.2
IO_base = 0x40014000
PADS_BASE = 0x4001c000
# list of registrs sectionn 2.19.6
# page 268 has bit defines of registers
def GPIO_CTRL(chan_num):
    return chan_num*8 + 4 + IO_base

def GPIO_STATUS(chan_num):
    return chan_num*8 + IO_base

def GPIO_PAD_CTRL(pad_num):
    return (pad_num*4 + 4) + PADS_BASE

#==== DMA ============================
# register adddresses for DMA
# datasheet page 102
DMA_base = 0x50000000
# ===================================
# === DMA channel control registers
# valid channels: 0 to 11
def DMA_RD_ADDR(ch_num):
    return (0x40*ch_num + DMA_base)
def DMA_WR_ADDR(ch_num):
    return (0x40*ch_num + 0x04 + DMA_base)
def DMA_TR_COUNT(ch_num):
    return (0x40*ch_num + 0x08 + DMA_base)
def DMA_CTRL(ch_num):
    return (0x40*ch_num + 0x0c + DMA_base)

# === Kill channels
# kill the channels by writing one to the bit number of thh channel
DMA_Ch_ABORT = 0x444 + DMA_base
# pacing timer The pacing timer produces TREQ assertions
# at a rate set by ((X/Y) * sys_clk)<1
# Where X is top 16 bits, Y the bottom 16 bits
DMA_TIMER0 = 0x420 + DMA_base
DMA_TIMER1 = 0x424 + DMA_base
#
# === bits in ctrl register
# busy during transfer
DMA_BUSY = (1<<24) # bit 24 high when busy
# turn off channel done IRQ
DMA_IRQ_QUIET = (1<<21) # bit 21 turn off interrupt
# Triggger Source table page 96 section 2.5.3.1
# More tirgger info page 114
# 0x0 to 0x3a → select DREQ n as TREQ from table above
# 0x3b → Select Timer 0 as TREQ
# 0x3c → Select Timer 1 as TREQ
# 0x3d → Select Timer 2 as TREQ (Optional)
# 0x3e → Select Timer 3 as TREQ (Optional)
# 0x3f → Permanent request, for unpaced transfers.
# bits 15:20 trigger request source
def DMA_TREQ(trigger_source):
    return (trigger_source & 0x3f)<<15
# When this channel completes, it will trigger the channel
# indicated by CHAIN_TO. Disable by setting CHAIN_TO =
# (this channel).
def DMA_CHAIN_TO (next_ch):
    return (next_ch & 0x0f)<<11 # bits 11:14 next chnnel #
# Select whether RING_SIZE applies to read or writeaddresses.
# If 0, read addresses are wrapped on a (1 << RING_SIZE)
# boundary. If 1, write addresses are wrapped.
DMA_RING_SEL = (1<<10) # bits 10 -- loop on write addr
# If 1, write addresses are wrapped.
# Ring sizes between 2 and 32768 BYTES are possible. This
# can apply to either read or write addresses, based on
# value of RING_SEL. NOTE BYTES, not words
# 0x0 → RING_NONE
def DMA_RING_SIZE(ring_size):
    return  (ring_size & 0x0f)<<6 # bits 6:9
# increment or keep constant wrrite nd read addr
# useful for peripheril write/read
DMA_WR_INC = (1<<5) # bits 5
DMA_RD_INC = (1<<4) # bits 4
# Set the size of each bus transfer (byte/halfword/word).
# READ_ADDR and WRITE_ADDR advance by this amount
# (1/2/4 bytes) with each transfer.
# 0x0 → SIZE_BYTE
# 0x1 → SIZE_HALFWORD
# 0x2 → SIZE_WORD
def DMA_DATA_WIDTH(data_width):
    return (data_width & 0x03)<<2 # bits 2:3
# give this channel more access if several channels aare on
DMA_HIGH_PRI = (1<<1) # bit 1
# turn on the channel
DMA_EN = 1 # bits 0

# === PWM ============================
# ssection 4.5
PWM_base =  0x40050000

def PWM_CSR(slice_num):
    return (0x14 * slice_num + PWM_base)
# 4.5.3. List of Registers
#PWM_ch0_csr = 0x00 + PWM_base
#PWM_ch1_csr = 0x14 + PWM_base
# bit fields for CSR
# 0x0 → Free-running counting at rate by fractional divider
# 0x1 → Fractional divider operation is gated by PWM B pin.
# 0x2 → Counter advances with each rising edge of PWM B pin.
# 0x3 → Counter advances with each falling edge of PWM B pin.
def PWM_divmode(mode):
    return (mode & 0x03)<<4
# invert channel B
PWM_B_inv = 1<<3
# invert channel A
PWM_A_inv = 1<<2
# phase correct
PWM_ph_correct = 1<<1
# enable channel
PWM_EN = 1
#
# divide register
# 12-bit fixed point
# with binary point betweem bit3 and bit4
def PWM_DIV(slice_num):
    return (0x14 * slice_num + PWM_base + 0x04)

# the actual PWM 16-bit couonter
def PWM_CTR(slice_num):
    return (0x14 * slice_num + PWM_base + 0x08)

# counter compare 31:16 B, 15:0 A
def PWM_CC(slice_num):
    return (0x14 * slice_num + PWM_base + 0x0c)

# counter wrap 16 bit value
def PWM_TOP(slice_num):
    return (0x14 * slice_num + PWM_base + 0x10)

PWM_INTR = 0xa4 + PWM_base
# GPIO 18 is PWM slice 1A

# === end of reqister defs ===========

# === define sine array ==============
# DMA ch0 source for PWM signed short
DMA_sine = array.array('h',[0]*256)
i = 0
while i<(256):
    #DMA_sine[i] = int(127 * math.sin(2*math.pi*i/256) + 128)
    # two sine wave in quadrature from one slice
    DMA_sine[i] = int(127 * math.sin(2*math.pi*i/256+1.57) + 128)
                   # (int(127 * math.sin(2*math.pi*i/256+1.57) + 128)<<16))
    i += 1
# DMA ch1 source for the address of the adddress of the array
ptr_to_sine_addr = array.array('i',[addressof(DMA_sine)])

# ====================================
# DMA setup
# === DMA Channel 2 -- ADC to PWM
# Chan 2 and 3 are ping-pong channels
# Channel 3 restarts channel 2
IOreg_write(DMA_RD_ADDR(2), ADC_FIFO)
# channel 0 output A
IOreg_write(DMA_WR_ADDR(2), PWM_CC(0))
# length one 16 bit value
IOreg_write(DMA_TR_COUNT(2), 1)
# conttrol DREQ ADC
data_16 = 0x01 # 16 bit
data_32 = 0x02 #
DREQ_ADC = 36
DREQ_TIMER0 = 0x3b # dat request souce number Timer0
IOreg_write(DMA_CTRL(2), DMA_IRQ_QUIET |
                            DMA_TREQ(DREQ_ADC) | #36 DREQ_ADC
                            #DMA_WR_INC |
                            #DMA_RD_INC |
                            DMA_DATA_WIDTH(data_16) |
                            DMA_CHAIN_TO(3) |
                            DMA_EN )

# set up DMA to retrigger channel 2
# by rewrriting the DMA2 control register
IOreg_write(DMA_RD_ADDR(3), DMA_CTRL(2))
# channel 0 output A
IOreg_write(DMA_WR_ADDR(3), DMA_CTRL(2))
# length one 16 bit value
IOreg_write(DMA_TR_COUNT(3), 1)
IOreg_write(DMA_CTRL(3), DMA_IRQ_QUIET |
                        DMA_DATA_WIDTH(data_32) |
                        DMA_CHAIN_TO(2) |
                        DMA_EN )

# ====================================
# now turn on ADC functions
# for set DIV to 100 cycles (minn is 96)                   
IOreg_write(ADC_DIV, ADC_Div_int(100) | ADC_Div_frac(0))

# ADC channel 0 , repeatting
IOreg_write(ADC_CS, (ADC_RROBIN(0) |
                        ADC_AINSEL(0) |
                        ADC_START_MANY |
                        ADC_EN))

# turn on fifo coupling from ADC to DMA
# shift to 8 bits for PWM
IOreg_write(ADC_FIFO_CS, ADC_THRESH(1) |
                        ADC_SHIFT |
                        ADC_DREQ_EN |
                        ADC_FIFO_EN )

# ADC channel 0 GPIO 26
# output disable, input disable, PUE and PDE disable
IOreg_write(GPIO_PAD_CTRL(26), 0b10000000)
# no special function
#machine.mem32[GPIO_CTRL(26)] = 0

# ======================================
# === Channel 1 transfer the ch0 strt address to Ch0 contrl block
# to restart channel 0
IOreg_write(DMA_RD_ADDR(1), addressof(ptr_to_sine_addr))
# load DMA control target address
IOreg_write(DMA_WR_ADDR(1), DMA_RD_ADDR(0))
# just one word transfer
IOreg_write(DMA_TR_COUNT(1), 1)
# the next write starts the transfer
data_32 = 0x02 # 32 bit
# set up control and start the DMA
IOreg_write(DMA_CTRL(1),  DMA_IRQ_QUIET |
                            #DMA_TREQ(perm_trig) |
                            #DMA_WR_INC |
                            #DMA_RD_INC |
                            DMA_DATA_WIDTH(data_32) |
                            DMA_CHAIN_TO(0) |
                            DMA_EN )
#
# === Channel 0 transfer the sine array to the PWM target
IOreg_write(DMA_RD_ADDR(0), addressof(DMA_sine))
IOreg_write(DMA_WR_ADDR(0), PWM_CC(1))
IOreg_write(DMA_TR_COUNT(0), len(DMA_sine))

# === set up the timer for a given frequency ===
# timer X=0x00ff, timer Y=0xfff0
# freq resolution for denom=0xffff:
# 1/(0xffff) * 125000000 / 256 = 7.45 Hz
# useful range 7 Hz to > 15 KHz
# This iterative calculation Sets the numerator
# and denom of timer register by Tylor series  expansion
Fout = 440 # actual desired frequency
Ltable = 256 # sine table length
Fclk = 125e6 # cpu clock ffreq
# get exact X
X0 = (Fout/Fclk) * Ltable * 2**16
# compute actual freq using integger part of X
Fx = Fclk*int(X0)/(Ltable * 2**16)
# Taylor expanson for delta Y
dY = (Fout-Fx) * Ltable * 2**32 /(Fclk * int(X0))
IOreg_write(DMA_TIMER0, (0xffff-int(dY)) | (int(X0)<<16))

# now the conntrol registerr
data_16 = 0x01 # 16 bit
# set up control and start the DMA
# triggers on DMA timer 0 event
IOreg_write(DMA_CTRL(0), DMA_IRQ_QUIET |
                            DMA_TREQ(DREQ_TIMER0) |
                            #DMA_WR_INC |
                            DMA_RD_INC |
                            DMA_DATA_WIDTH(data_16) |                         
                            DMA_CHAIN_TO(1) |
                            DMA_EN )
#
# ====================================
# PWM setup PWM channel 1 GPIO18_CTRL function select
# every overflow triggers the ch0 DMA
PWM_FN = 4
IOreg_write(GPIO_CTRL(18), PWM_FN) # pwm function slice 1A
IOreg_write(GPIO_CTRL(0), PWM_FN )# pwm function slice 0A

# divider of value unity clocks at cpu rate
IOreg_write(PWM_DIV(1), 1<<4) # binary 1 in 8.4 fixed pt
IOreg_write(PWM_TOP(1), 256) # 8-bit pwm
IOreg_write(PWM_CC(1), 128) #50% for testing
#machine.mem32[PWM_ch1_ctr] = 0 # zero the phase
free_run = 0
IOreg_write(PWM_CSR(1), PWM_EN | PWM_divmode(free_run))

# channel 0
# divider of value unity clocks at cpu rate
IOreg_write(PWM_DIV(0), 1<<4) # binary 1 in 8.4 fixed pt
IOreg_write(PWM_TOP(0), 256) # 8-bit pwm
IOreg_write(PWM_CC(0), 128) #50% for testing
IOreg_write(PWM_CSR(0), PWM_EN | PWM_divmode(free_run))

# turn on flag for chanel zero
#IOreg_write(PWM_INTR, 1)
# ===================================
# set the frequency
while True :
   
    Fout = float(input('frequency:' ))
    if Fout == 0 :
        break
    # compute the parmeters for getting the correct
    # output frequeency
    #get exact X
    X0 = (Fout/Fclk) * Ltable * 2**16
    # compute actual freq using integger part of X
    Fx = Fclk*int(X0)/(Ltable * 2**16)
    # Taylor expanson for delta Y
    dY = (Fout-Fx) * Ltable * 2**32 /(Fclk * int(X0))
    #print('x=',int(X0), 'dY=', int(dY))
    IOreg_write(DMA_TIMER0, (0xffff-int(dY)) | (int(X0)<<16))
   
# now Kill the asych peripherials
# kill PWM
IOreg_write(PWM_CSR(0), 0)
IOreg_write(PWM_CSR(1), 0)

#kill all DMA channels
IOreg_write(DMA_Ch_ABORT, 0x0f) # set bits to kill ch0 and ch1
##
## end