# SIO Interpolator test -- interp_test.py
# NOTE: the SIO bus address block is ONLY VALID from the
# CPU bus masters, NOT from DMA bus masters
# 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
#
def IOreg_read(reg_addr):
    return machine.mem32[reg_addr]
#
def IOreg_read16(reg_addr):
    return machine.mem16[reg_addr]

# === SIO interpolator  ==============
# 2.3.1.6.2 Blend mode page 37
# 2.3.1.7 registers  page 53
"""
0x080 INTERP0_ACCUM0 Read/write access to accumulator 0
0x084 INTERP0_ACCUM1 Read/write access to accumulator 1
0x088 INTERP0_BASE0 Read/write access to BASE0 register.
0x08c INTERP0_BASE1 Read/write access to BASE1 register.
0x090 INTERP0_BASE2 Read/write access to BASE2 register.
0x094 INTERP0_POP_LANE0 Read LANE0 result,
  and simultaneously write lane results to both accumulators (POP).
0x098 INTERP0_POP_LANE1 Read LANE1 result,
  and simultaneously write lane results to both accumulators (POP).
0x09c INTERP0_POP_FULL Read FULL result,
  and simultaneously write lane results to both accumulators (POP).
0x0a0 INTERP0_PEEK_LANE0 Read LANE0 result,
  without altering any internal state (PEEK).
0x0a4 INTERP0_PEEK_LANE1 Read LANE1 result,
  without altering any internal state (PEEK).
0x0a8 INTERP0_PEEK_FULL Read FULL result,
  without altering any internal state (PEEK).
0x0ac INTERP0_CTRL_LANE0 Control register for lane 0
0x0b0 INTERP0_CTRL_LANE1 Control register for lane 1
0x0b4 INTERP0_ACCUM0_ADD Values written here are
  atomically added to ACCUM0
0x0b8 INTERP0_ACCUM1_ADD Values written here are
  atomically added to ACCUM1
0x0bc INTERP0_BASE_1AND0 On write,
  the lower 16 bits go to BASE0, upper bits to BASE1 simultaneously.
"""
# NOTE: /the SIO_BASE address block is ONLY VALID from the
# CPU bus masters, NOT from DMA bus masters!
SIO_BASE = 0xd0000000
INTERP0_ACCUM0 = 0x080 + SIO_BASE
INTERP0_ACCUM1 = 0x084 + SIO_BASE
INTERP0_BASE0 = 0x088 + SIO_BASE
INTERP0_BASE1 = 0x08c + SIO_BASE
INTERP0_BASE2 = 0x090 + SIO_BASE
INTERP0_POP_LANE0 = 0x094 + SIO_BASE
INTERP0_POP_LANE1 = 0x098 + SIO_BASE
INTERP0_POP_FULL = 0x09c + SIO_BASE
INTERP0_PEEK_LANE0 = 0x0a0 + SIO_BASE
INTERP0_PEEK_LANE1 = 0x0a4 + SIO_BASE
INTERP0_PEEK_FULL = 0x0a8 + SIO_BASE
INTERP0_ACCUM0_ADD = 0x0b4 + SIO_BASE
INTERP0_ACCUM1_ADD = 0x0b8 + SIO_BASE
INTERP0_BASE_1AND0 = 0xbc + SIO_BASE
INTERP0_CTRL_LANE0 = 0x0ac + SIO_BASE
INTERP0_CTRL_LANE1 = 0x0b0 + SIO_BASE
# bits in control registers
# note that only interp0, lane0 has the 'blend' bit
INTERP_CTRL_BLEND = (1<<21)
# ORed into bits 29:28 of the lane result presented to the
# processor on the bus.
def INTERP_CTRL_FORCE_MSB(bits):
    return (bits & 0x03) << 19
# If 1, mask + shift is bypassed for LANE0 result.
INTERP_CTRL_ADD_RAW = (1<<18)
#
INTERP_CTRL_CROSS_RESULT = (1<<17)
#
INTERP_CTRL_CROSS_INPUT = (1<<16)
#
INTERP_CTRL_SIGNED = (1<<15)
# The most-significant bit allowed to pass by the mask
# Setting MSB < LSB may cause chip to turn inside-out
#   (actually in manual)
def INTERP_CTRL_MASK_MSB(bits):
    return (bits & 0x1f) << 10
# The least-significant bit allowed to pass by the mask
def INTERP_CTRL_MASK_LSB(bits):
    return (bits & 0x1f) << 5
# Logical right-shift applied to accumulator before masking R
def INTERP_CTRL_SHIFT(bits):
    return (bits & 0x1f)

"""
If BLEND mode is enabled:
- LANE1 result is a linear interpolation between BASE0 and
  BASE1, controlled by the 8 LSBs of lane 1 shift
  and mask value (a fractional number between 0 and 255/256ths)
- LANE0 result does not have BASE0 added (yields only
  the 8 LSBs of lane 1 shift+mask value)
- FULL result does not have lane 1 shift+mask value added
  (BASE2 + lane 0 shift+mask)
- LANE1 SIGNED flag controls whether the interpolation is
  signed or unsigned.
 
  Output = x0 + alpha * (x1 - x0)
  x0 is stored in Base0
  x1 is stored in Base1
  alpha is stored in Accum1
  Read interpolated value in peek Lane1
"""

# test interp blenad mode
x0 = 100
x1 = 200
alpha = 128 # used by hardware as 128/255

# turn on blend, use all the bits
IOreg_write(INTERP0_CTRL_LANE0, INTERP_CTRL_BLEND |
                              INTERP_CTRL_MASK_MSB(31))

# use all the bits for alpha (from accum1)
IOreg_write(INTERP0_CTRL_LANE1, INTERP_CTRL_MASK_MSB(31))
# set the inputs
IOreg_write(INTERP0_BASE0, x0)
IOreg_write(INTERP0_BASE1, x1)

for i in range(8):
    alpha = i*32
    IOreg_write(INTERP0_ACCUM1, alpha)
    print(i, 'alpha=',IOreg_read16(INTERP0_ACCUM1)/255,
         ' interp value=',IOreg_read16(INTERP0_PEEK_LANE1))