rp2: Add new port to Raspberry Pi RP2 microcontroller.
This commit adds a new port "rp2" which targets the new Raspberry Pi RP2040 microcontroller. The build system uses pure cmake (with a small Makefile wrapper for convenience). The USB driver is TinyUSB, and there is a machine module with most of the standard classes implemented. Some examples are provided in the examples/rp2/ directory. Work done in collaboration with Graham Sanderson. Signed-off-by: Damien George <damien@micropython.org>
This commit is contained in:
parent
ef3ee7aa10
commit
469345e728
35
examples/rp2/pio_1hz.py
Normal file
35
examples/rp2/pio_1hz.py
Normal file
@ -0,0 +1,35 @@
|
||||
# Example using PIO to blink an LED and raise an IRQ at 1Hz.
|
||||
|
||||
import time
|
||||
from machine import Pin
|
||||
import rp2
|
||||
|
||||
|
||||
@rp2.asm_pio(set_init=rp2.PIO.OUT_LOW)
|
||||
def blink_1hz():
|
||||
# fmt: off
|
||||
# Cycles: 1 + 1 + 6 + 32 * (30 + 1) = 1000
|
||||
irq(rel(0))
|
||||
set(pins, 1)
|
||||
set(x, 31) [5]
|
||||
label("delay_high")
|
||||
nop() [29]
|
||||
jmp(x_dec, "delay_high")
|
||||
|
||||
# Cycles: 1 + 7 + 32 * (30 + 1) = 1000
|
||||
set(pins, 0)
|
||||
set(x, 31) [6]
|
||||
label("delay_low")
|
||||
nop() [29]
|
||||
jmp(x_dec, "delay_low")
|
||||
# fmt: on
|
||||
|
||||
|
||||
# Create the StateMachine with the blink_1hz program, outputting on Pin(25).
|
||||
sm = rp2.StateMachine(0, blink_1hz, freq=2000, set_base=Pin(25))
|
||||
|
||||
# Set the IRQ handler to print the millisecond timestamp.
|
||||
sm.irq(lambda p: print(time.ticks_ms()))
|
||||
|
||||
# Start the StateMachine.
|
||||
sm.active(1)
|
27
examples/rp2/pio_exec.py
Normal file
27
examples/rp2/pio_exec.py
Normal file
@ -0,0 +1,27 @@
|
||||
# Example using PIO to turn on an LED via an explicit exec.
|
||||
#
|
||||
# Demonstrates:
|
||||
# - using set_init and set_base
|
||||
# - using StateMachine.exec
|
||||
|
||||
import time
|
||||
from machine import Pin
|
||||
import rp2
|
||||
|
||||
# Define an empty program that uses a single set pin.
|
||||
@rp2.asm_pio(set_init=rp2.PIO.OUT_LOW)
|
||||
def prog():
|
||||
pass
|
||||
|
||||
|
||||
# Construct the StateMachine, binding Pin(25) to the set pin.
|
||||
sm = rp2.StateMachine(0, prog, set_base=Pin(25))
|
||||
|
||||
# Turn on the set pin via an exec instruction.
|
||||
sm.exec("set(pins, 1)")
|
||||
|
||||
# Sleep for 500ms.
|
||||
time.sleep(0.5)
|
||||
|
||||
# Turn off the set pin via an exec instruction.
|
||||
sm.exec("set(pins, 0)")
|
46
examples/rp2/pio_pinchange.py
Normal file
46
examples/rp2/pio_pinchange.py
Normal file
@ -0,0 +1,46 @@
|
||||
# Example using PIO to wait for a pin change and raise an IRQ.
|
||||
#
|
||||
# Demonstrates:
|
||||
# - PIO wrapping
|
||||
# - PIO wait instruction, waiting on an input pin
|
||||
# - PIO irq instruction, in blocking mode with relative IRQ number
|
||||
# - setting the in_base pin for a StateMachine
|
||||
# - setting an irq handler for a StateMachine
|
||||
# - instantiating 2x StateMachine's with the same program and different pins
|
||||
|
||||
import time
|
||||
from machine import Pin
|
||||
import rp2
|
||||
|
||||
|
||||
@rp2.asm_pio()
|
||||
def wait_pin_low():
|
||||
wrap_target()
|
||||
|
||||
wait(0, pin, 0)
|
||||
irq(block, rel(0))
|
||||
wait(1, pin, 0)
|
||||
|
||||
wrap()
|
||||
|
||||
|
||||
def handler(sm):
|
||||
# Print a (wrapping) timestamp, and the state machine object.
|
||||
print(time.ticks_ms(), sm)
|
||||
|
||||
|
||||
# Instantiate StateMachine(0) with wait_pin_low program on Pin(16).
|
||||
pin16 = Pin(16, Pin.IN, Pin.PULL_UP)
|
||||
sm0 = rp2.StateMachine(0, wait_pin_low, in_base=pin16)
|
||||
sm0.irq(handler)
|
||||
|
||||
# Instantiate StateMachine(1) with wait_pin_low program on Pin(17).
|
||||
pin17 = Pin(17, Pin.IN, Pin.PULL_UP)
|
||||
sm1 = rp2.StateMachine(1, wait_pin_low, in_base=pin17)
|
||||
sm1.irq(handler)
|
||||
|
||||
# Start the StateMachine's running.
|
||||
sm0.active(1)
|
||||
sm1.active(1)
|
||||
|
||||
# Now, when Pin(16) or Pin(17) is pulled low a message will be printed to the REPL.
|
45
examples/rp2/pio_pwm.py
Normal file
45
examples/rp2/pio_pwm.py
Normal file
@ -0,0 +1,45 @@
|
||||
# Example of using PIO for PWM, and fading the brightness of an LED
|
||||
|
||||
from machine import Pin
|
||||
from rp2 import PIO, StateMachine, asm_pio
|
||||
from time import sleep
|
||||
|
||||
|
||||
@asm_pio(sideset_init=PIO.OUT_LOW)
|
||||
def pwm_prog():
|
||||
# fmt: off
|
||||
pull(noblock) .side(0)
|
||||
mov(x, osr) # Keep most recent pull data stashed in X, for recycling by noblock
|
||||
mov(y, isr) # ISR must be preloaded with PWM count max
|
||||
label("pwmloop")
|
||||
jmp(x_not_y, "skip")
|
||||
nop() .side(1)
|
||||
label("skip")
|
||||
jmp(y_dec, "pwmloop")
|
||||
# fmt: on
|
||||
|
||||
|
||||
class PIOPWM:
|
||||
def __init__(self, sm_id, pin, max_count, count_freq):
|
||||
self._sm = StateMachine(sm_id, pwm_prog, freq=2 * count_freq, sideset_base=Pin(pin))
|
||||
# Use exec() to load max count into ISR
|
||||
self._sm.put(max_count)
|
||||
self._sm.exec("pull()")
|
||||
self._sm.exec("mov(isr, osr)")
|
||||
self._sm.active(1)
|
||||
self._max_count = max_count
|
||||
|
||||
def set(self, value):
|
||||
# Minimum value is -1 (completely turn off), 0 actually still produces narrow pulse
|
||||
value = max(value, -1)
|
||||
value = min(value, self._max_count)
|
||||
self._sm.put(value)
|
||||
|
||||
|
||||
# Pin 25 is LED on Pico boards
|
||||
pwm = PIOPWM(0, 25, max_count=(1 << 16) - 1, count_freq=10_000_000)
|
||||
|
||||
while True:
|
||||
for i in range(256):
|
||||
pwm.set(i ** 2)
|
||||
sleep(0.01)
|
44
examples/rp2/pio_uart_tx.py
Normal file
44
examples/rp2/pio_uart_tx.py
Normal file
@ -0,0 +1,44 @@
|
||||
# Example using PIO to create a UART TX interface
|
||||
|
||||
from machine import Pin
|
||||
from rp2 import PIO, StateMachine, asm_pio
|
||||
|
||||
UART_BAUD = 115200
|
||||
PIN_BASE = 10
|
||||
NUM_UARTS = 8
|
||||
|
||||
|
||||
@asm_pio(sideset_init=PIO.OUT_HIGH, out_init=PIO.OUT_HIGH, out_shiftdir=PIO.SHIFT_RIGHT)
|
||||
def uart_tx():
|
||||
# fmt: off
|
||||
# Block with TX deasserted until data available
|
||||
pull()
|
||||
# Initialise bit counter, assert start bit for 8 cycles
|
||||
set(x, 7) .side(0) [7]
|
||||
# Shift out 8 data bits, 8 execution cycles per bit
|
||||
label("bitloop")
|
||||
out(pins, 1) [6]
|
||||
jmp(x_dec, "bitloop")
|
||||
# Assert stop bit for 8 cycles total (incl 1 for pull())
|
||||
nop() .side(1) [6]
|
||||
# fmt: on
|
||||
|
||||
|
||||
# Now we add 8 UART TXs, on pins 10 to 17. Use the same baud rate for all of them.
|
||||
uarts = []
|
||||
for i in range(NUM_UARTS):
|
||||
sm = StateMachine(
|
||||
i, uart_tx, freq=8 * UART_BAUD, sideset_base=Pin(PIN_BASE + i), out_base=Pin(PIN_BASE + i)
|
||||
)
|
||||
sm.active(1)
|
||||
uarts.append(sm)
|
||||
|
||||
# We can print characters from each UART by pushing them to the TX FIFO
|
||||
def pio_uart_print(sm, s):
|
||||
for c in s:
|
||||
sm.put(ord(c))
|
||||
|
||||
|
||||
# Print a different message from each UART
|
||||
for i, u in enumerate(uarts):
|
||||
pio_uart_print(u, "Hello from UART {}!\n".format(i))
|
59
examples/rp2/pio_ws2812.py
Normal file
59
examples/rp2/pio_ws2812.py
Normal file
@ -0,0 +1,59 @@
|
||||
# Example using PIO to drive a set of WS2812 LEDs.
|
||||
|
||||
import array, time
|
||||
from machine import Pin
|
||||
import rp2
|
||||
|
||||
# Configure the number of WS2812 LEDs.
|
||||
NUM_LEDS = 8
|
||||
|
||||
|
||||
@rp2.asm_pio(
|
||||
sideset_init=rp2.PIO.OUT_LOW,
|
||||
out_shiftdir=rp2.PIO.SHIFT_LEFT,
|
||||
autopull=True,
|
||||
pull_thresh=24,
|
||||
)
|
||||
def ws2812():
|
||||
# fmt: off
|
||||
T1 = 2
|
||||
T2 = 5
|
||||
T3 = 3
|
||||
wrap_target()
|
||||
label("bitloop")
|
||||
out(x, 1) .side(0) [T3 - 1]
|
||||
jmp(not_x, "do_zero") .side(1) [T1 - 1]
|
||||
jmp("bitloop") .side(1) [T2 - 1]
|
||||
label("do_zero")
|
||||
nop() .side(0) [T2 - 1]
|
||||
wrap()
|
||||
# fmt: on
|
||||
|
||||
|
||||
# Create the StateMachine with the ws2812 program, outputting on Pin(22).
|
||||
sm = rp2.StateMachine(0, ws2812, freq=8_000_000, sideset_base=Pin(22))
|
||||
|
||||
# Start the StateMachine, it will wait for data on its FIFO.
|
||||
sm.active(1)
|
||||
|
||||
# Display a pattern on the LEDs via an array of LED RGB values.
|
||||
ar = array.array("I", [0 for _ in range(NUM_LEDS)])
|
||||
|
||||
# Cycle colours.
|
||||
for i in range(4 * NUM_LEDS):
|
||||
for j in range(NUM_LEDS):
|
||||
r = j * 100 // (NUM_LEDS - 1)
|
||||
b = 100 - j * 100 // (NUM_LEDS - 1)
|
||||
if j != i % NUM_LEDS:
|
||||
r >>= 3
|
||||
b >>= 3
|
||||
ar[j] = r << 16 | b
|
||||
sm.put(ar, 8)
|
||||
time.sleep_ms(50)
|
||||
|
||||
# Fade out.
|
||||
for i in range(24):
|
||||
for j in range(NUM_LEDS):
|
||||
ar[j] = ar[j] >> 1 & 0x7F7F7F
|
||||
sm.put(ar, 8)
|
||||
time.sleep_ms(50)
|
25
examples/rp2/pwm_fade.py
Normal file
25
examples/rp2/pwm_fade.py
Normal file
@ -0,0 +1,25 @@
|
||||
# Example using PWM to fade an LED.
|
||||
|
||||
import time
|
||||
from machine import Pin, PWM
|
||||
|
||||
|
||||
# Construct PWM object, with LED on Pin(25).
|
||||
pwm = PWM(Pin(25))
|
||||
|
||||
# Set the PWM frequency.
|
||||
pwm.freq(1000)
|
||||
|
||||
# Fade the LED in and out a few times.
|
||||
duty = 0
|
||||
direction = 1
|
||||
for _ in range(8 * 256):
|
||||
duty += direction
|
||||
if duty > 255:
|
||||
duty = 255
|
||||
direction = -1
|
||||
elif duty < 0:
|
||||
duty = 0
|
||||
direction = 1
|
||||
pwm.duty_u16(duty * duty)
|
||||
time.sleep(0.001)
|
162
ports/rp2/CMakeLists.txt
Normal file
162
ports/rp2/CMakeLists.txt
Normal file
@ -0,0 +1,162 @@
|
||||
cmake_minimum_required(VERSION 3.12)
|
||||
|
||||
# Set build type to reduce firmware size
|
||||
if(NOT CMAKE_BUILD_TYPE)
|
||||
set(CMAKE_BUILD_TYPE MinSizeRel)
|
||||
endif()
|
||||
|
||||
# Set main target and component locations
|
||||
set(MICROPYTHON_TARGET firmware)
|
||||
get_filename_component(MPY_DIR "../.." ABSOLUTE)
|
||||
if (PICO_SDK_PATH_OVERRIDE)
|
||||
set(PICO_SDK_PATH ${PICO_SDK_PATH_OVERRIDE})
|
||||
else()
|
||||
set(PICO_SDK_PATH ../../lib/pico-sdk)
|
||||
endif()
|
||||
|
||||
# Include component cmake fragments
|
||||
include(micropy_py.cmake)
|
||||
include(micropy_extmod.cmake)
|
||||
include(${PICO_SDK_PATH}/pico_sdk_init.cmake)
|
||||
|
||||
# Define the top-level project
|
||||
project(${MICROPYTHON_TARGET})
|
||||
|
||||
pico_sdk_init()
|
||||
|
||||
add_executable(${MICROPYTHON_TARGET})
|
||||
|
||||
set(SOURCE_LIB
|
||||
${MPY_DIR}/lib/littlefs/lfs1.c
|
||||
${MPY_DIR}/lib/littlefs/lfs1_util.c
|
||||
${MPY_DIR}/lib/littlefs/lfs2.c
|
||||
${MPY_DIR}/lib/littlefs/lfs2_util.c
|
||||
${MPY_DIR}/lib/mp-readline/readline.c
|
||||
${MPY_DIR}/lib/oofatfs/ff.c
|
||||
${MPY_DIR}/lib/oofatfs/ffunicode.c
|
||||
${MPY_DIR}/lib/timeutils/timeutils.c
|
||||
${MPY_DIR}/lib/utils/gchelper_m0.s
|
||||
${MPY_DIR}/lib/utils/gchelper_native.c
|
||||
${MPY_DIR}/lib/utils/mpirq.c
|
||||
${MPY_DIR}/lib/utils/stdout_helpers.c
|
||||
${MPY_DIR}/lib/utils/sys_stdio_mphal.c
|
||||
${MPY_DIR}/lib/utils/pyexec.c
|
||||
)
|
||||
|
||||
set(SOURCE_DRIVERS
|
||||
${MPY_DIR}/drivers/bus/softspi.c
|
||||
)
|
||||
|
||||
set(SOURCE_PORT
|
||||
machine_adc.c
|
||||
machine_i2c.c
|
||||
machine_pin.c
|
||||
machine_pwm.c
|
||||
machine_spi.c
|
||||
machine_timer.c
|
||||
machine_uart.c
|
||||
machine_wdt.c
|
||||
main.c
|
||||
modmachine.c
|
||||
modrp2.c
|
||||
moduos.c
|
||||
modutime.c
|
||||
mphalport.c
|
||||
mpthreadport.c
|
||||
rp2_flash.c
|
||||
rp2_pio.c
|
||||
tusb_port.c
|
||||
uart.c
|
||||
)
|
||||
|
||||
set(SOURCE_QSTR
|
||||
${SOURCE_PY}
|
||||
${SOURCE_EXTMOD}
|
||||
${MPY_DIR}/lib/utils/mpirq.c
|
||||
${MPY_DIR}/lib/utils/sys_stdio_mphal.c
|
||||
${PROJECT_SOURCE_DIR}/machine_adc.c
|
||||
${PROJECT_SOURCE_DIR}/machine_i2c.c
|
||||
${PROJECT_SOURCE_DIR}/machine_pin.c
|
||||
${PROJECT_SOURCE_DIR}/machine_pwm.c
|
||||
${PROJECT_SOURCE_DIR}/machine_spi.c
|
||||
${PROJECT_SOURCE_DIR}/machine_timer.c
|
||||
${PROJECT_SOURCE_DIR}/machine_uart.c
|
||||
${PROJECT_SOURCE_DIR}/machine_wdt.c
|
||||
${PROJECT_SOURCE_DIR}/modmachine.c
|
||||
${PROJECT_SOURCE_DIR}/modrp2.c
|
||||
${PROJECT_SOURCE_DIR}/moduos.c
|
||||
${PROJECT_SOURCE_DIR}/modutime.c
|
||||
${PROJECT_SOURCE_DIR}/rp2_flash.c
|
||||
${PROJECT_SOURCE_DIR}/rp2_pio.c
|
||||
)
|
||||
|
||||
set(MPY_QSTR_DEFS ${PROJECT_SOURCE_DIR}/qstrdefsport.h)
|
||||
|
||||
# Define mpy-cross flags and frozen manifest
|
||||
set(MPY_CROSS_FLAGS -march=armv7m)
|
||||
set(FROZEN_MANIFEST ${PROJECT_SOURCE_DIR}/manifest.py)
|
||||
|
||||
include(micropy_rules.cmake)
|
||||
|
||||
target_sources(${MICROPYTHON_TARGET} PRIVATE
|
||||
${SOURCE_PY}
|
||||
${SOURCE_EXTMOD}
|
||||
${SOURCE_LIB}
|
||||
${SOURCE_DRIVERS}
|
||||
${SOURCE_PORT}
|
||||
)
|
||||
|
||||
target_include_directories(${MICROPYTHON_TARGET} PRIVATE
|
||||
"${PROJECT_SOURCE_DIR}"
|
||||
"${MPY_DIR}"
|
||||
"${CMAKE_BINARY_DIR}"
|
||||
)
|
||||
|
||||
target_compile_options(${MICROPYTHON_TARGET} PRIVATE
|
||||
-Wall
|
||||
#-Werror
|
||||
-DFFCONF_H=\"${MPY_DIR}/lib/oofatfs/ffconf.h\"
|
||||
-DLFS1_NO_MALLOC -DLFS1_NO_DEBUG -DLFS1_NO_WARN -DLFS1_NO_ERROR -DLFS1_NO_ASSERT
|
||||
-DLFS2_NO_MALLOC -DLFS2_NO_DEBUG -DLFS2_NO_WARN -DLFS2_NO_ERROR -DLFS2_NO_ASSERT
|
||||
)
|
||||
|
||||
target_compile_definitions(${MICROPYTHON_TARGET} PRIVATE
|
||||
PICO_FLOAT_PROPAGATE_NANS=1
|
||||
PICO_STACK_SIZE=0x2000
|
||||
PICO_CORE1_STACK_SIZE=0
|
||||
PICO_PROGRAM_NAME="MicroPython"
|
||||
PICO_NO_PROGRAM_VERSION_STRING=1 # do it ourselves in main.c
|
||||
MICROPY_BUILD_TYPE="${CMAKE_C_COMPILER_ID} ${CMAKE_C_COMPILER_VERSION} ${CMAKE_BUILD_TYPE}"
|
||||
PICO_NO_BI_STDIO_UART=1 # we call it UART REPL
|
||||
)
|
||||
|
||||
target_link_libraries(${MICROPYTHON_TARGET}
|
||||
hardware_adc
|
||||
hardware_dma
|
||||
hardware_flash
|
||||
hardware_i2c
|
||||
hardware_pio
|
||||
hardware_pwm
|
||||
hardware_rtc
|
||||
hardware_spi
|
||||
hardware_sync
|
||||
pico_multicore
|
||||
pico_stdlib_headers
|
||||
pico_stdlib
|
||||
tinyusb_device
|
||||
)
|
||||
|
||||
# todo this is a bit brittle, but we want to move a few source files into RAM (which requires
|
||||
# a linker script modification) until we explicitly add macro calls around the function
|
||||
# defs to move them into RAM.
|
||||
if (PICO_ON_DEVICE AND NOT PICO_NO_FLASH AND NOT PICO_COPY_TO_RAM)
|
||||
pico_set_linker_script(${MICROPYTHON_TARGET} ${CMAKE_CURRENT_LIST_DIR}/memmap_mp.ld)
|
||||
endif()
|
||||
|
||||
pico_add_extra_outputs(${MICROPYTHON_TARGET})
|
||||
|
||||
add_custom_command(TARGET ${MICROPYTHON_TARGET}
|
||||
POST_BUILD
|
||||
COMMAND arm-none-eabi-size --format=berkeley ${PROJECT_BINARY_DIR}/${MICROPYTHON_TARGET}.elf
|
||||
VERBATIM
|
||||
)
|
14
ports/rp2/Makefile
Normal file
14
ports/rp2/Makefile
Normal file
@ -0,0 +1,14 @@
|
||||
# Makefile for micropython on Raspberry Pi RP2
|
||||
#
|
||||
# This is a simple wrapper around cmake
|
||||
|
||||
BUILD = build
|
||||
|
||||
$(VERBOSE)MAKESILENT = -s
|
||||
|
||||
all:
|
||||
[ -d $(BUILD) ] || cmake -S . -B $(BUILD) -DPICO_BUILD_DOCS=0
|
||||
$(MAKE) $(MAKESILENT) -C $(BUILD)
|
||||
|
||||
clean:
|
||||
$(RM) -rf $(BUILD)
|
100
ports/rp2/README.md
Normal file
100
ports/rp2/README.md
Normal file
@ -0,0 +1,100 @@
|
||||
# The RP2 port
|
||||
|
||||
This is a port of MicroPython to the Raspberry Pi RP2 series of microcontrollers.
|
||||
Currently supported features are:
|
||||
|
||||
- REPL over USB VCP, and optionally over UART (on GP0/GP1).
|
||||
- Filesystem on the internal flash, using littlefs2.
|
||||
- Support for native code generation and inline assembler.
|
||||
- `utime` module with sleep, time and ticks functions.
|
||||
- `uos` module with VFS support.
|
||||
- `machine` module with the following classes: `Pin`, `ADC`, `PWM`, `I2C`, `SPI`,
|
||||
`SoftI2C`, `SoftSPI`, `Timer`, `UART`, `WDT`.
|
||||
- `rp2` module with programmable IO (PIO) support.
|
||||
|
||||
See the `examples/rp2/` directory for some example code.
|
||||
|
||||
## Building
|
||||
|
||||
The MicroPython cross-compiler must be built first, which will be used to
|
||||
pre-compile (freeze) built-in Python code. This cross-compiler is built and
|
||||
run on the host machine using:
|
||||
|
||||
$ make -C mpy-cross
|
||||
|
||||
This command should be executed from the root directory of this repository.
|
||||
All other commands below should be executed from the ports/rp2/ directory.
|
||||
|
||||
Building of the RP2 firmware is done entirely using CMake, although a simple
|
||||
Makefile is also provided as a convenience. To build the firmware run (from
|
||||
this directory):
|
||||
|
||||
$ make clean
|
||||
$ make
|
||||
|
||||
You can also build the standard CMake way. The final firmware is found in
|
||||
the top-level of the CMake build directory (`build` by default) and is
|
||||
called `firmware.uf2`.
|
||||
|
||||
## Deploying firmware to the device
|
||||
|
||||
Firmware can be deployed to the device by putting it into bootloader mode
|
||||
(hold down BOOTSEL while powering on or resetting) and then copying
|
||||
`firmware.uf2` to the USB mass storage device that appears.
|
||||
|
||||
If MicroPython is already installed then the bootloader can be entered by
|
||||
executing `import machine; machine.bootloader()` at the REPL.
|
||||
|
||||
## Sample code
|
||||
|
||||
The following samples can be easily run on the board by entering paste mode
|
||||
with Ctrl-E at the REPL, then cut-and-pasting the sample code to the REPL, then
|
||||
executing the code with Ctrl-D.
|
||||
|
||||
### Blinky
|
||||
|
||||
This blinks the on-board LED on the Pico board at 1.25Hz, using a Timer object
|
||||
with a callback.
|
||||
|
||||
```python
|
||||
from machine import Pin, Timer
|
||||
led = Pin(25, Pin.OUT)
|
||||
tim = Timer()
|
||||
def tick(timer):
|
||||
global led
|
||||
led.toggle()
|
||||
|
||||
tim.init(freq=2.5, mode=Timer.PERIODIC, callback=tick)
|
||||
```
|
||||
|
||||
### PIO blinky
|
||||
|
||||
This blinks the on-board LED on the Pico board at 1Hz, using a PIO peripheral and
|
||||
PIO assembler to directly toggle the LED at the required rate.
|
||||
|
||||
```python
|
||||
from machine import Pin
|
||||
import rp2
|
||||
|
||||
@rp2.asm_pio(set_init=rp2.PIO.OUT_LOW)
|
||||
def blink_1hz():
|
||||
# Turn on the LED and delay, taking 1000 cycles.
|
||||
set(pins, 1)
|
||||
set(x, 31) [6]
|
||||
label("delay_high")
|
||||
nop() [29]
|
||||
jmp(x_dec, "delay_high")
|
||||
|
||||
# Turn off the LED and delay, taking 1000 cycles.
|
||||
set(pins, 0)
|
||||
set(x, 31) [6]
|
||||
label("delay_low")
|
||||
nop() [29]
|
||||
jmp(x_dec, "delay_low")
|
||||
|
||||
# Create StateMachine(0) with the blink_1hz program, outputting on Pin(25).
|
||||
sm = rp2.StateMachine(0, blink_1hz, freq=2000, set_base=Pin(25))
|
||||
sm.active(1)
|
||||
```
|
||||
|
||||
See the `examples/rp2/` directory for further example code.
|
120
ports/rp2/machine_adc.c
Normal file
120
ports/rp2/machine_adc.c
Normal file
@ -0,0 +1,120 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "py/mphal.h"
|
||||
#include "hardware/adc.h"
|
||||
|
||||
#define ADC_IS_VALID_GPIO(gpio) ((gpio) >= 26 && (gpio) <= 29)
|
||||
#define ADC_CHANNEL_FROM_GPIO(gpio) ((gpio) - 26)
|
||||
#define ADC_CHANNEL_TEMPSENSOR (4)
|
||||
|
||||
STATIC uint16_t adc_config_and_read_u16(uint32_t channel) {
|
||||
adc_select_input(channel);
|
||||
uint32_t raw = adc_read();
|
||||
const uint32_t bits = 12;
|
||||
// Scale raw reading to 16 bit value using a Taylor expansion (for 8 <= bits <= 16)
|
||||
return raw << (16 - bits) | raw >> (2 * bits - 16);
|
||||
}
|
||||
|
||||
/******************************************************************************/
|
||||
// MicroPython bindings for machine.ADC
|
||||
|
||||
const mp_obj_type_t machine_adc_type;
|
||||
|
||||
typedef struct _machine_adc_obj_t {
|
||||
mp_obj_base_t base;
|
||||
uint32_t channel;
|
||||
} machine_adc_obj_t;
|
||||
|
||||
STATIC void machine_adc_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
|
||||
machine_adc_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
mp_printf(print, "<ADC channel=%u>", self->channel);
|
||||
}
|
||||
|
||||
// ADC(id)
|
||||
STATIC mp_obj_t machine_adc_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
|
||||
// Check number of arguments
|
||||
mp_arg_check_num(n_args, n_kw, 1, 1, false);
|
||||
|
||||
mp_obj_t source = all_args[0];
|
||||
|
||||
uint32_t channel;
|
||||
if (mp_obj_is_int(source)) {
|
||||
// Get and validate channel number.
|
||||
channel = mp_obj_get_int(source);
|
||||
if (!((channel >= 0 && channel <= ADC_CHANNEL_TEMPSENSOR) || ADC_IS_VALID_GPIO(channel))) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("invalid channel"));
|
||||
}
|
||||
|
||||
} else {
|
||||
// Get GPIO and check it has ADC capabilities.
|
||||
channel = mp_hal_get_pin_obj(source);
|
||||
if (!ADC_IS_VALID_GPIO(channel)) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("Pin doesn't have ADC capabilities"));
|
||||
}
|
||||
}
|
||||
|
||||
adc_init();
|
||||
|
||||
if (ADC_IS_VALID_GPIO(channel)) {
|
||||
// Configure the GPIO pin in ADC mode.
|
||||
adc_gpio_init(channel);
|
||||
channel = ADC_CHANNEL_FROM_GPIO(channel);
|
||||
} else if (channel == ADC_CHANNEL_TEMPSENSOR) {
|
||||
// Enable temperature sensor.
|
||||
adc_set_temp_sensor_enabled(1);
|
||||
}
|
||||
|
||||
// Create ADC object.
|
||||
machine_adc_obj_t *o = m_new_obj(machine_adc_obj_t);
|
||||
o->base.type = &machine_adc_type;
|
||||
o->channel = channel;
|
||||
|
||||
return MP_OBJ_FROM_PTR(o);
|
||||
}
|
||||
|
||||
// read_u16()
|
||||
STATIC mp_obj_t machine_adc_read_u16(mp_obj_t self_in) {
|
||||
machine_adc_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
return MP_OBJ_NEW_SMALL_INT(adc_config_and_read_u16(self->channel));
|
||||
}
|
||||
MP_DEFINE_CONST_FUN_OBJ_1(machine_adc_read_u16_obj, machine_adc_read_u16);
|
||||
|
||||
STATIC const mp_rom_map_elem_t machine_adc_locals_dict_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR_read_u16), MP_ROM_PTR(&machine_adc_read_u16_obj) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_CORE_TEMP), MP_ROM_INT(ADC_CHANNEL_TEMPSENSOR) },
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(machine_adc_locals_dict, machine_adc_locals_dict_table);
|
||||
|
||||
const mp_obj_type_t machine_adc_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_ADC,
|
||||
.print = machine_adc_print,
|
||||
.make_new = machine_adc_make_new,
|
||||
.locals_dict = (mp_obj_dict_t *)&machine_adc_locals_dict,
|
||||
};
|
161
ports/rp2/machine_i2c.c
Normal file
161
ports/rp2/machine_i2c.c
Normal file
@ -0,0 +1,161 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "py/mphal.h"
|
||||
#include "py/mperrno.h"
|
||||
#include "extmod/machine_i2c.h"
|
||||
#include "modmachine.h"
|
||||
|
||||
#include "hardware/i2c.h"
|
||||
|
||||
#define DEFAULT_I2C_FREQ (400000)
|
||||
#define DEFAULT_I2C0_SCL (9)
|
||||
#define DEFAULT_I2C0_SDA (8)
|
||||
#define DEFAULT_I2C1_SCL (7)
|
||||
#define DEFAULT_I2C1_SDA (6)
|
||||
|
||||
// SDA/SCL on even/odd pins, I2C0/I2C1 on even/odd pairs of pins.
|
||||
#define IS_VALID_SCL(i2c, pin) (((pin) & 1) == 1 && (((pin) & 2) >> 1) == (i2c))
|
||||
#define IS_VALID_SDA(i2c, pin) (((pin) & 1) == 0 && (((pin) & 2) >> 1) == (i2c))
|
||||
|
||||
typedef struct _machine_i2c_obj_t {
|
||||
mp_obj_base_t base;
|
||||
i2c_inst_t *const i2c_inst;
|
||||
uint8_t i2c_id;
|
||||
uint8_t scl;
|
||||
uint8_t sda;
|
||||
uint32_t freq;
|
||||
} machine_i2c_obj_t;
|
||||
|
||||
STATIC machine_i2c_obj_t machine_i2c_obj[] = {
|
||||
{{&machine_hw_i2c_type}, i2c0, 0, DEFAULT_I2C0_SCL, DEFAULT_I2C0_SDA, 0},
|
||||
{{&machine_hw_i2c_type}, i2c1, 1, DEFAULT_I2C1_SCL, DEFAULT_I2C1_SDA, 0},
|
||||
};
|
||||
|
||||
STATIC void machine_i2c_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
|
||||
machine_i2c_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
mp_printf(print, "I2C(%u, freq=%u, scl=%u, sda=%u)",
|
||||
self->i2c_id, self->freq, self->scl, self->sda);
|
||||
}
|
||||
|
||||
mp_obj_t machine_i2c_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
|
||||
enum { ARG_id, ARG_freq, ARG_scl, ARG_sda };
|
||||
static const mp_arg_t allowed_args[] = {
|
||||
{ MP_QSTR_id, MP_ARG_REQUIRED | MP_ARG_OBJ },
|
||||
{ MP_QSTR_freq, MP_ARG_INT, {.u_int = DEFAULT_I2C_FREQ} },
|
||||
{ MP_QSTR_scl, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_sda, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
};
|
||||
|
||||
// Parse args.
|
||||
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
|
||||
mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
|
||||
|
||||
// Get I2C bus.
|
||||
int i2c_id = mp_obj_get_int(args[ARG_id].u_obj);
|
||||
if (i2c_id < 0 || i2c_id >= MP_ARRAY_SIZE(machine_i2c_obj)) {
|
||||
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("I2C(%d) doesn't exist"), i2c_id);
|
||||
}
|
||||
|
||||
// Get static peripheral object.
|
||||
machine_i2c_obj_t *self = (machine_i2c_obj_t *)&machine_i2c_obj[i2c_id];
|
||||
|
||||
// Set SCL/SDA pins if configured.
|
||||
if (args[ARG_scl].u_obj != mp_const_none) {
|
||||
int scl = mp_hal_get_pin_obj(args[ARG_scl].u_obj);
|
||||
if (!IS_VALID_SCL(self->i2c_id, scl)) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("bad SCL pin"));
|
||||
}
|
||||
self->scl = scl;
|
||||
}
|
||||
if (args[ARG_sda].u_obj != mp_const_none) {
|
||||
int sda = mp_hal_get_pin_obj(args[ARG_sda].u_obj);
|
||||
if (!IS_VALID_SDA(self->i2c_id, sda)) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("bad SDA pin"));
|
||||
}
|
||||
self->sda = sda;
|
||||
}
|
||||
|
||||
// Initialise the I2C peripheral if any arguments given, or it was not initialised previously.
|
||||
if (n_args > 1 || n_kw > 0 || self->freq == 0) {
|
||||
self->freq = args[ARG_freq].u_int;
|
||||
i2c_init(self->i2c_inst, self->freq);
|
||||
self->freq = i2c_set_baudrate(self->i2c_inst, self->freq);
|
||||
gpio_set_function(self->scl, GPIO_FUNC_I2C);
|
||||
gpio_set_function(self->sda, GPIO_FUNC_I2C);
|
||||
gpio_set_pulls(self->scl, true, 0);
|
||||
gpio_set_pulls(self->sda, true, 0);
|
||||
}
|
||||
|
||||
return MP_OBJ_FROM_PTR(self);
|
||||
}
|
||||
|
||||
STATIC int machine_i2c_transfer_single(mp_obj_base_t *self_in, uint16_t addr, size_t len, uint8_t *buf, unsigned int flags) {
|
||||
machine_i2c_obj_t *self = (machine_i2c_obj_t *)self_in;
|
||||
int ret;
|
||||
bool nostop = !(flags & MP_MACHINE_I2C_FLAG_STOP);
|
||||
if (flags & MP_MACHINE_I2C_FLAG_READ) {
|
||||
ret = i2c_read_blocking(self->i2c_inst, addr, buf, len, nostop);
|
||||
} else {
|
||||
if (len <= 2) {
|
||||
// Workaround issue with hardware I2C not accepting short writes.
|
||||
mp_machine_soft_i2c_obj_t soft_i2c = {
|
||||
.base = { &mp_machine_soft_i2c_type },
|
||||
.us_delay = 500000 / self->freq + 1,
|
||||
.us_timeout = 255,
|
||||
.scl = self->scl,
|
||||
.sda = self->sda,
|
||||
};
|
||||
mp_machine_i2c_buf_t bufs = {
|
||||
.len = len,
|
||||
.buf = buf,
|
||||
};
|
||||
mp_hal_pin_open_drain(self->scl);
|
||||
mp_hal_pin_open_drain(self->sda);
|
||||
ret = mp_machine_soft_i2c_transfer(&soft_i2c.base, addr, 1, &bufs, flags);
|
||||
gpio_set_function(self->scl, GPIO_FUNC_I2C);
|
||||
gpio_set_function(self->sda, GPIO_FUNC_I2C);
|
||||
} else {
|
||||
ret = i2c_write_blocking(self->i2c_inst, addr, buf, len, nostop);
|
||||
}
|
||||
}
|
||||
return (ret < 0) ? -MP_EIO : ret;
|
||||
}
|
||||
|
||||
STATIC const mp_machine_i2c_p_t machine_i2c_p = {
|
||||
.transfer = mp_machine_i2c_transfer_adaptor,
|
||||
.transfer_single = machine_i2c_transfer_single,
|
||||
};
|
||||
|
||||
const mp_obj_type_t machine_hw_i2c_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_I2C,
|
||||
.print = machine_i2c_print,
|
||||
.make_new = machine_i2c_make_new,
|
||||
.protocol = &machine_i2c_p,
|
||||
.locals_dict = (mp_obj_dict_t *)&mp_machine_i2c_locals_dict,
|
||||
};
|
449
ports/rp2/machine_pin.c
Normal file
449
ports/rp2/machine_pin.c
Normal file
@ -0,0 +1,449 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2016-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "py/mphal.h"
|
||||
#include "lib/utils/mpirq.h"
|
||||
#include "modmachine.h"
|
||||
#include "extmod/virtpin.h"
|
||||
|
||||
#include "hardware/irq.h"
|
||||
#include "hardware/regs/intctrl.h"
|
||||
#include "hardware/structs/iobank0.h"
|
||||
#include "hardware/structs/padsbank0.h"
|
||||
|
||||
#define GPIO_MODE_IN (0)
|
||||
#define GPIO_MODE_OUT (1)
|
||||
#define GPIO_MODE_OPEN_DRAIN (2)
|
||||
#define GPIO_MODE_ALT (3)
|
||||
|
||||
// These can be or'd together.
|
||||
#define GPIO_PULL_UP (1)
|
||||
#define GPIO_PULL_DOWN (2)
|
||||
|
||||
#define GPIO_IRQ_ALL (0xf)
|
||||
|
||||
// Macros to access the state of the hardware.
|
||||
#define GPIO_GET_FUNCSEL(id) ((iobank0_hw->io[(id)].ctrl & IO_BANK0_GPIO0_CTRL_FUNCSEL_BITS) >> IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB)
|
||||
#define GPIO_IS_OUT(id) (sio_hw->gpio_oe & (1 << (id)))
|
||||
#define GPIO_IS_PULL_UP(id) (padsbank0_hw->io[(id)] & PADS_BANK0_GPIO0_PUE_BITS)
|
||||
#define GPIO_IS_PULL_DOWN(id) (padsbank0_hw->io[(id)] & PADS_BANK0_GPIO0_PDE_BITS)
|
||||
|
||||
// Open drain behaviour is simulated.
|
||||
#define GPIO_IS_OPEN_DRAIN(id) (machine_pin_open_drain_mask & (1 << (id)))
|
||||
|
||||
typedef struct _machine_pin_obj_t {
|
||||
mp_obj_base_t base;
|
||||
uint32_t id;
|
||||
} machine_pin_obj_t;
|
||||
|
||||
typedef struct _machine_pin_irq_obj_t {
|
||||
mp_irq_obj_t base;
|
||||
uint32_t flags;
|
||||
uint32_t trigger;
|
||||
} machine_pin_irq_obj_t;
|
||||
|
||||
STATIC const mp_irq_methods_t machine_pin_irq_methods;
|
||||
|
||||
STATIC const machine_pin_obj_t machine_pin_obj[N_GPIOS] = {
|
||||
{{&machine_pin_type}, 0},
|
||||
{{&machine_pin_type}, 1},
|
||||
{{&machine_pin_type}, 2},
|
||||
{{&machine_pin_type}, 3},
|
||||
{{&machine_pin_type}, 4},
|
||||
{{&machine_pin_type}, 5},
|
||||
{{&machine_pin_type}, 6},
|
||||
{{&machine_pin_type}, 7},
|
||||
{{&machine_pin_type}, 8},
|
||||
{{&machine_pin_type}, 9},
|
||||
{{&machine_pin_type}, 10},
|
||||
{{&machine_pin_type}, 11},
|
||||
{{&machine_pin_type}, 12},
|
||||
{{&machine_pin_type}, 13},
|
||||
{{&machine_pin_type}, 14},
|
||||
{{&machine_pin_type}, 15},
|
||||
{{&machine_pin_type}, 16},
|
||||
{{&machine_pin_type}, 17},
|
||||
{{&machine_pin_type}, 18},
|
||||
{{&machine_pin_type}, 19},
|
||||
{{&machine_pin_type}, 20},
|
||||
{{&machine_pin_type}, 21},
|
||||
{{&machine_pin_type}, 22},
|
||||
{{&machine_pin_type}, 23},
|
||||
{{&machine_pin_type}, 24},
|
||||
{{&machine_pin_type}, 25},
|
||||
{{&machine_pin_type}, 26},
|
||||
{{&machine_pin_type}, 27},
|
||||
{{&machine_pin_type}, 28},
|
||||
{{&machine_pin_type}, 29},
|
||||
};
|
||||
|
||||
// Mask with "1" indicating that the corresponding pin is in simulated open-drain mode.
|
||||
uint32_t machine_pin_open_drain_mask;
|
||||
|
||||
STATIC void gpio_irq(void) {
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
uint32_t intr = iobank0_hw->intr[i];
|
||||
if (intr) {
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
if (intr & 0xf) {
|
||||
uint32_t gpio = 8 * i + j;
|
||||
gpio_acknowledge_irq(gpio, intr & 0xf);
|
||||
machine_pin_irq_obj_t *irq = MP_STATE_PORT(machine_pin_irq_obj[gpio]);
|
||||
if (irq != NULL && (intr & irq->trigger)) {
|
||||
irq->flags = intr & irq->trigger;
|
||||
mp_irq_handler(&irq->base);
|
||||
}
|
||||
}
|
||||
intr >>= 4;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void machine_pin_init(void) {
|
||||
memset(MP_STATE_PORT(machine_pin_irq_obj), 0, sizeof(MP_STATE_PORT(machine_pin_irq_obj)));
|
||||
irq_set_exclusive_handler(IO_IRQ_BANK0, gpio_irq);
|
||||
irq_set_enabled(IO_IRQ_BANK0, true);
|
||||
}
|
||||
|
||||
void machine_pin_deinit(void) {
|
||||
for (int i = 0; i < N_GPIOS; ++i) {
|
||||
gpio_set_irq_enabled(i, GPIO_IRQ_ALL, false);
|
||||
}
|
||||
irq_set_enabled(IO_IRQ_BANK0, false);
|
||||
irq_remove_handler(IO_IRQ_BANK0, gpio_irq);
|
||||
}
|
||||
|
||||
STATIC void machine_pin_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
|
||||
machine_pin_obj_t *self = self_in;
|
||||
uint funcsel = GPIO_GET_FUNCSEL(self->id);
|
||||
qstr mode_qst;
|
||||
if (funcsel == GPIO_FUNC_SIO) {
|
||||
if (GPIO_IS_OPEN_DRAIN(self->id)) {
|
||||
mode_qst = MP_QSTR_OPEN_DRAIN;
|
||||
} else if (GPIO_IS_OUT(self->id)) {
|
||||
mode_qst = MP_QSTR_OUT;
|
||||
} else {
|
||||
mode_qst = MP_QSTR_IN;
|
||||
}
|
||||
} else {
|
||||
mode_qst = MP_QSTR_ALT;
|
||||
}
|
||||
mp_printf(print, "Pin(%u, mode=%q", self->id, mode_qst);
|
||||
bool pull_up = false;
|
||||
if (GPIO_IS_PULL_UP(self->id)) {
|
||||
mp_printf(print, ", pull=%q", MP_QSTR_PULL_UP);
|
||||
pull_up = true;
|
||||
}
|
||||
if (GPIO_IS_PULL_DOWN(self->id)) {
|
||||
if (pull_up) {
|
||||
mp_printf(print, "|%q", MP_QSTR_PULL_DOWN);
|
||||
} else {
|
||||
mp_printf(print, ", pull=%q", MP_QSTR_PULL_DOWN);
|
||||
}
|
||||
}
|
||||
if (funcsel != GPIO_FUNC_SIO) {
|
||||
mp_printf(print, ", alt=%u", funcsel);
|
||||
}
|
||||
mp_printf(print, ")");
|
||||
}
|
||||
|
||||
// pin.init(mode, pull=None, *, value=None, alt=FUNC_SIO)
|
||||
STATIC mp_obj_t machine_pin_obj_init_helper(const machine_pin_obj_t *self, size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
|
||||
enum { ARG_mode, ARG_pull, ARG_value, ARG_alt };
|
||||
static const mp_arg_t allowed_args[] = {
|
||||
{ MP_QSTR_mode, MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE}},
|
||||
{ MP_QSTR_pull, MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE}},
|
||||
{ MP_QSTR_value, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE}},
|
||||
{ MP_QSTR_alt, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = GPIO_FUNC_SIO}},
|
||||
};
|
||||
|
||||
// parse args
|
||||
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
|
||||
mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
|
||||
|
||||
// set initial value (do this before configuring mode/pull)
|
||||
if (args[ARG_value].u_obj != mp_const_none) {
|
||||
gpio_put(self->id, mp_obj_is_true(args[ARG_value].u_obj));
|
||||
}
|
||||
|
||||
// configure mode
|
||||
if (args[ARG_mode].u_obj != mp_const_none) {
|
||||
mp_int_t mode = mp_obj_get_int(args[ARG_mode].u_obj);
|
||||
if (mode == GPIO_MODE_IN) {
|
||||
mp_hal_pin_input(self->id);
|
||||
} else if (mode == GPIO_MODE_OUT) {
|
||||
mp_hal_pin_output(self->id);
|
||||
} else if (mode == GPIO_MODE_OPEN_DRAIN) {
|
||||
mp_hal_pin_open_drain(self->id);
|
||||
} else {
|
||||
// Alternate function.
|
||||
gpio_set_function(self->id, args[ARG_alt].u_int);
|
||||
machine_pin_open_drain_mask &= ~(1 << self->id);
|
||||
}
|
||||
}
|
||||
|
||||
// configure pull (unconditionally because None means no-pull)
|
||||
uint32_t pull = 0;
|
||||
if (args[ARG_pull].u_obj != mp_const_none) {
|
||||
pull = mp_obj_get_int(args[ARG_pull].u_obj);
|
||||
}
|
||||
gpio_set_pulls(self->id, pull & GPIO_PULL_UP, pull & GPIO_PULL_DOWN);
|
||||
|
||||
return mp_const_none;
|
||||
}
|
||||
|
||||
// constructor(id, ...)
|
||||
mp_obj_t mp_pin_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) {
|
||||
mp_arg_check_num(n_args, n_kw, 1, MP_OBJ_FUN_ARGS_MAX, true);
|
||||
|
||||
// get the wanted pin object
|
||||
int wanted_pin = mp_obj_get_int(args[0]);
|
||||
if (!(0 <= wanted_pin && wanted_pin < MP_ARRAY_SIZE(machine_pin_obj))) {
|
||||
mp_raise_ValueError("invalid pin");
|
||||
}
|
||||
const machine_pin_obj_t *self = &machine_pin_obj[wanted_pin];
|
||||
|
||||
if (n_args > 1 || n_kw > 0) {
|
||||
// pin mode given, so configure this GPIO
|
||||
mp_map_t kw_args;
|
||||
mp_map_init_fixed_table(&kw_args, n_kw, args + n_args);
|
||||
machine_pin_obj_init_helper(self, n_args - 1, args + 1, &kw_args);
|
||||
}
|
||||
|
||||
return MP_OBJ_FROM_PTR(self);
|
||||
}
|
||||
|
||||
// fast method for getting/setting pin value
|
||||
STATIC mp_obj_t machine_pin_call(mp_obj_t self_in, size_t n_args, size_t n_kw, const mp_obj_t *args) {
|
||||
mp_arg_check_num(n_args, n_kw, 0, 1, false);
|
||||
machine_pin_obj_t *self = self_in;
|
||||
if (n_args == 0) {
|
||||
// get pin
|
||||
return MP_OBJ_NEW_SMALL_INT(gpio_get(self->id));
|
||||
} else {
|
||||
// set pin
|
||||
bool value = mp_obj_is_true(args[0]);
|
||||
if (GPIO_IS_OPEN_DRAIN(self->id)) {
|
||||
MP_STATIC_ASSERT(GPIO_IN == 0 && GPIO_OUT == 1);
|
||||
gpio_set_dir(self->id, 1 - value);
|
||||
} else {
|
||||
gpio_put(self->id, value);
|
||||
}
|
||||
return mp_const_none;
|
||||
}
|
||||
}
|
||||
|
||||
// pin.init(mode, pull)
|
||||
STATIC mp_obj_t machine_pin_obj_init(size_t n_args, const mp_obj_t *args, mp_map_t *kw_args) {
|
||||
return machine_pin_obj_init_helper(args[0], n_args - 1, args + 1, kw_args);
|
||||
}
|
||||
MP_DEFINE_CONST_FUN_OBJ_KW(machine_pin_init_obj, 1, machine_pin_obj_init);
|
||||
|
||||
// pin.value([value])
|
||||
STATIC mp_obj_t machine_pin_value(size_t n_args, const mp_obj_t *args) {
|
||||
return machine_pin_call(args[0], n_args - 1, 0, args + 1);
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(machine_pin_value_obj, 1, 2, machine_pin_value);
|
||||
|
||||
// pin.low()
|
||||
STATIC mp_obj_t machine_pin_low(mp_obj_t self_in) {
|
||||
machine_pin_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
if (GPIO_IS_OPEN_DRAIN(self->id)) {
|
||||
gpio_set_dir(self->id, GPIO_OUT);
|
||||
} else {
|
||||
gpio_clr_mask(1u << self->id);
|
||||
}
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(machine_pin_low_obj, machine_pin_low);
|
||||
|
||||
// pin.high()
|
||||
STATIC mp_obj_t machine_pin_high(mp_obj_t self_in) {
|
||||
machine_pin_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
if (GPIO_IS_OPEN_DRAIN(self->id)) {
|
||||
gpio_set_dir(self->id, GPIO_IN);
|
||||
} else {
|
||||
gpio_set_mask(1u << self->id);
|
||||
}
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(machine_pin_high_obj, machine_pin_high);
|
||||
|
||||
// pin.toggle()
|
||||
STATIC mp_obj_t machine_pin_toggle(mp_obj_t self_in) {
|
||||
machine_pin_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
if (GPIO_IS_OPEN_DRAIN(self->id)) {
|
||||
if (GPIO_IS_OUT(self->id)) {
|
||||
gpio_set_dir(self->id, GPIO_IN);
|
||||
} else {
|
||||
gpio_set_dir(self->id, GPIO_OUT);
|
||||
}
|
||||
} else {
|
||||
gpio_xor_mask(1u << self->id);
|
||||
}
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(machine_pin_toggle_obj, machine_pin_toggle);
|
||||
|
||||
// pin.irq(handler=None, trigger=IRQ_FALLING|IRQ_RISING, hard=False)
|
||||
STATIC mp_obj_t machine_pin_irq(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
|
||||
enum { ARG_handler, ARG_trigger, ARG_hard };
|
||||
static const mp_arg_t allowed_args[] = {
|
||||
{ MP_QSTR_handler, MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_trigger, MP_ARG_INT, {.u_int = GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE} },
|
||||
{ MP_QSTR_hard, MP_ARG_BOOL, {.u_bool = false} },
|
||||
};
|
||||
machine_pin_obj_t *self = MP_OBJ_TO_PTR(pos_args[0]);
|
||||
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
|
||||
mp_arg_parse_all(n_args - 1, pos_args + 1, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
|
||||
|
||||
// Get the IRQ object.
|
||||
machine_pin_irq_obj_t *irq = MP_STATE_PORT(machine_pin_irq_obj[self->id]);
|
||||
|
||||
// Allocate the IRQ object if it doesn't already exist.
|
||||
if (irq == NULL) {
|
||||
irq = m_new_obj(machine_pin_irq_obj_t);
|
||||
irq->base.base.type = &mp_irq_type;
|
||||
irq->base.methods = (mp_irq_methods_t *)&machine_pin_irq_methods;
|
||||
irq->base.parent = MP_OBJ_FROM_PTR(self);
|
||||
irq->base.handler = mp_const_none;
|
||||
irq->base.ishard = false;
|
||||
MP_STATE_PORT(machine_pin_irq_obj[self->id]) = irq;
|
||||
}
|
||||
|
||||
if (n_args > 1 || kw_args->used != 0) {
|
||||
// Configure IRQ.
|
||||
|
||||
// Disable all IRQs while data is updated.
|
||||
gpio_set_irq_enabled(self->id, GPIO_IRQ_ALL, false);
|
||||
|
||||
// Update IRQ data.
|
||||
irq->base.handler = args[ARG_handler].u_obj;
|
||||
irq->base.ishard = args[ARG_hard].u_bool;
|
||||
irq->flags = 0;
|
||||
irq->trigger = args[ARG_trigger].u_int;
|
||||
|
||||
// Enable IRQ if a handler is given.
|
||||
if (args[ARG_handler].u_obj != mp_const_none) {
|
||||
gpio_set_irq_enabled(self->id, args[ARG_trigger].u_int, true);
|
||||
}
|
||||
}
|
||||
|
||||
return MP_OBJ_FROM_PTR(irq);
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(machine_pin_irq_obj, 1, machine_pin_irq);
|
||||
|
||||
STATIC const mp_rom_map_elem_t machine_pin_locals_dict_table[] = {
|
||||
// instance methods
|
||||
{ MP_ROM_QSTR(MP_QSTR_init), MP_ROM_PTR(&machine_pin_init_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_value), MP_ROM_PTR(&machine_pin_value_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_low), MP_ROM_PTR(&machine_pin_low_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_high), MP_ROM_PTR(&machine_pin_high_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_off), MP_ROM_PTR(&machine_pin_low_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_on), MP_ROM_PTR(&machine_pin_high_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_toggle), MP_ROM_PTR(&machine_pin_toggle_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_irq), MP_ROM_PTR(&machine_pin_irq_obj) },
|
||||
|
||||
// class constants
|
||||
{ MP_ROM_QSTR(MP_QSTR_IN), MP_ROM_INT(GPIO_MODE_IN) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_OUT), MP_ROM_INT(GPIO_MODE_OUT) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_OPEN_DRAIN), MP_ROM_INT(GPIO_MODE_OPEN_DRAIN) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_ALT), MP_ROM_INT(GPIO_MODE_ALT) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_PULL_UP), MP_ROM_INT(GPIO_PULL_UP) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_PULL_DOWN), MP_ROM_INT(GPIO_PULL_DOWN) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_IRQ_RISING), MP_ROM_INT(GPIO_IRQ_EDGE_RISE) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_IRQ_FALLING), MP_ROM_INT(GPIO_IRQ_EDGE_FALL) },
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(machine_pin_locals_dict, machine_pin_locals_dict_table);
|
||||
|
||||
STATIC mp_uint_t pin_ioctl(mp_obj_t self_in, mp_uint_t request, uintptr_t arg, int *errcode) {
|
||||
(void)errcode;
|
||||
machine_pin_obj_t *self = self_in;
|
||||
|
||||
switch (request) {
|
||||
case MP_PIN_READ: {
|
||||
return gpio_get(self->id);
|
||||
}
|
||||
case MP_PIN_WRITE: {
|
||||
gpio_put(self->id, arg);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
STATIC const mp_pin_p_t pin_pin_p = {
|
||||
.ioctl = pin_ioctl,
|
||||
};
|
||||
|
||||
const mp_obj_type_t machine_pin_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_Pin,
|
||||
.print = machine_pin_print,
|
||||
.make_new = mp_pin_make_new,
|
||||
.call = machine_pin_call,
|
||||
.protocol = &pin_pin_p,
|
||||
.locals_dict = (mp_obj_t)&machine_pin_locals_dict,
|
||||
};
|
||||
|
||||
STATIC mp_uint_t machine_pin_irq_trigger(mp_obj_t self_in, mp_uint_t new_trigger) {
|
||||
machine_pin_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
machine_pin_irq_obj_t *irq = MP_STATE_PORT(machine_pin_irq_obj[self->id]);
|
||||
gpio_set_irq_enabled(self->id, GPIO_IRQ_ALL, false);
|
||||
irq->flags = 0;
|
||||
irq->trigger = new_trigger;
|
||||
gpio_set_irq_enabled(self->id, new_trigger, true);
|
||||
return 0;
|
||||
}
|
||||
|
||||
STATIC mp_uint_t machine_pin_irq_info(mp_obj_t self_in, mp_uint_t info_type) {
|
||||
machine_pin_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
machine_pin_irq_obj_t *irq = MP_STATE_PORT(machine_pin_irq_obj[self->id]);
|
||||
if (info_type == MP_IRQ_INFO_FLAGS) {
|
||||
return irq->flags;
|
||||
} else if (info_type == MP_IRQ_INFO_TRIGGERS) {
|
||||
return irq->trigger;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
STATIC const mp_irq_methods_t machine_pin_irq_methods = {
|
||||
.trigger = machine_pin_irq_trigger,
|
||||
.info = machine_pin_irq_info,
|
||||
};
|
||||
|
||||
mp_hal_pin_obj_t mp_hal_get_pin_obj(mp_obj_t obj) {
|
||||
if (!mp_obj_is_type(obj, &machine_pin_type)) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("expecting a Pin"));
|
||||
}
|
||||
machine_pin_obj_t *pin = MP_OBJ_TO_PTR(obj);
|
||||
return pin->id;
|
||||
}
|
197
ports/rp2/machine_pwm.c
Normal file
197
ports/rp2/machine_pwm.c
Normal file
@ -0,0 +1,197 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "py/mphal.h"
|
||||
#include "modmachine.h"
|
||||
|
||||
#include "hardware/clocks.h"
|
||||
#include "hardware/pwm.h"
|
||||
|
||||
/******************************************************************************/
|
||||
// MicroPython bindings for machine.PWM
|
||||
|
||||
const mp_obj_type_t machine_pwm_type;
|
||||
|
||||
typedef struct _machine_pwm_obj_t {
|
||||
mp_obj_base_t base;
|
||||
uint8_t slice;
|
||||
uint8_t channel;
|
||||
} machine_pwm_obj_t;
|
||||
|
||||
STATIC machine_pwm_obj_t machine_pwm_obj[] = {
|
||||
{{&machine_pwm_type}, 0, PWM_CHAN_A},
|
||||
{{&machine_pwm_type}, 0, PWM_CHAN_B},
|
||||
{{&machine_pwm_type}, 1, PWM_CHAN_A},
|
||||
{{&machine_pwm_type}, 1, PWM_CHAN_B},
|
||||
{{&machine_pwm_type}, 2, PWM_CHAN_A},
|
||||
{{&machine_pwm_type}, 2, PWM_CHAN_B},
|
||||
{{&machine_pwm_type}, 3, PWM_CHAN_A},
|
||||
{{&machine_pwm_type}, 3, PWM_CHAN_B},
|
||||
{{&machine_pwm_type}, 4, PWM_CHAN_A},
|
||||
{{&machine_pwm_type}, 4, PWM_CHAN_B},
|
||||
{{&machine_pwm_type}, 5, PWM_CHAN_A},
|
||||
{{&machine_pwm_type}, 5, PWM_CHAN_B},
|
||||
{{&machine_pwm_type}, 6, PWM_CHAN_A},
|
||||
{{&machine_pwm_type}, 6, PWM_CHAN_B},
|
||||
{{&machine_pwm_type}, 7, PWM_CHAN_A},
|
||||
{{&machine_pwm_type}, 7, PWM_CHAN_B},
|
||||
};
|
||||
|
||||
STATIC void machine_pwm_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
|
||||
machine_pwm_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
mp_printf(print, "<PWM slice=%u channel=%u>", self->slice, self->channel);
|
||||
}
|
||||
|
||||
// PWM(pin)
|
||||
STATIC mp_obj_t machine_pwm_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
|
||||
// Check number of arguments
|
||||
mp_arg_check_num(n_args, n_kw, 1, 1, false);
|
||||
|
||||
// Get GPIO to connect to PWM.
|
||||
uint32_t gpio = mp_hal_get_pin_obj(all_args[0]);
|
||||
|
||||
// Get static peripheral object.
|
||||
uint slice = pwm_gpio_to_slice_num(gpio);
|
||||
uint8_t channel = pwm_gpio_to_channel(gpio);
|
||||
const machine_pwm_obj_t *self = &machine_pwm_obj[slice * 2 + channel];
|
||||
|
||||
// Select PWM function for given GPIO.
|
||||
gpio_set_function(gpio, GPIO_FUNC_PWM);
|
||||
|
||||
return MP_OBJ_FROM_PTR(self);
|
||||
}
|
||||
|
||||
STATIC mp_obj_t machine_pwm_deinit(mp_obj_t self_in) {
|
||||
machine_pwm_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
pwm_set_enabled(self->slice, false);
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(machine_pwm_deinit_obj, machine_pwm_deinit);
|
||||
|
||||
// PWM.freq([value])
|
||||
STATIC mp_obj_t machine_pwm_freq(size_t n_args, const mp_obj_t *args) {
|
||||
machine_pwm_obj_t *self = MP_OBJ_TO_PTR(args[0]);
|
||||
uint32_t source_hz = clock_get_hz(clk_sys);
|
||||
if (n_args == 1) {
|
||||
// Get frequency.
|
||||
uint32_t div16 = pwm_hw->slice[self->slice].div;
|
||||
uint32_t top = pwm_hw->slice[self->slice].top;
|
||||
uint32_t pwm_freq = 16 * source_hz / div16 / top;
|
||||
return MP_OBJ_NEW_SMALL_INT(pwm_freq);
|
||||
} else {
|
||||
// Set the frequency, making "top" as large as possible for maximum resolution.
|
||||
// Maximum "top" is set at 65534 to be able to achieve 100% duty with 65535.
|
||||
#define TOP_MAX 65534
|
||||
mp_int_t freq = mp_obj_get_int(args[1]);
|
||||
uint32_t div16_top = 16 * source_hz / freq;
|
||||
uint32_t top = 1;
|
||||
for (;;) {
|
||||
// Try a few small prime factors to get close to the desired frequency.
|
||||
if (div16_top >= 16 * 5 && div16_top % 5 == 0 && top * 5 <= TOP_MAX) {
|
||||
div16_top /= 5;
|
||||
top *= 5;
|
||||
} else if (div16_top >= 16 * 3 && div16_top % 3 == 0 && top * 3 <= TOP_MAX) {
|
||||
div16_top /= 3;
|
||||
top *= 3;
|
||||
} else if (div16_top >= 16 * 2 && top * 2 <= TOP_MAX) {
|
||||
div16_top /= 2;
|
||||
top *= 2;
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (div16_top < 16) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("freq too large"));
|
||||
} else if (div16_top >= 256 * 16) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("freq too small"));
|
||||
}
|
||||
pwm_hw->slice[self->slice].div = div16_top;
|
||||
pwm_hw->slice[self->slice].top = top;
|
||||
return mp_const_none;
|
||||
}
|
||||
}
|
||||
MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(machine_pwm_freq_obj, 1, 2, machine_pwm_freq);
|
||||
|
||||
// PWM.duty_u16([value])
|
||||
STATIC mp_obj_t machine_pwm_duty_u16(size_t n_args, const mp_obj_t *args) {
|
||||
machine_pwm_obj_t *self = MP_OBJ_TO_PTR(args[0]);
|
||||
uint32_t top = pwm_hw->slice[self->slice].top;
|
||||
if (n_args == 1) {
|
||||
// Get duty cycle.
|
||||
uint32_t cc = pwm_hw->slice[self->slice].cc;
|
||||
cc = (cc >> (self->channel ? PWM_CH0_CC_B_LSB : PWM_CH0_CC_A_LSB)) & 0xffff;
|
||||
return MP_OBJ_NEW_SMALL_INT(cc * 65535 / (top + 1));
|
||||
} else {
|
||||
// Set duty cycle.
|
||||
mp_int_t duty_u16 = mp_obj_get_int(args[1]);
|
||||
uint32_t cc = duty_u16 * (top + 1) / 65535;
|
||||
pwm_set_chan_level(self->slice, self->channel, cc);
|
||||
pwm_set_enabled(self->slice, true);
|
||||
return mp_const_none;
|
||||
}
|
||||
}
|
||||
MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(machine_pwm_duty_u16_obj, 1, 2, machine_pwm_duty_u16);
|
||||
|
||||
// PWM.duty_ns([value])
|
||||
STATIC mp_obj_t machine_pwm_duty_ns(size_t n_args, const mp_obj_t *args) {
|
||||
machine_pwm_obj_t *self = MP_OBJ_TO_PTR(args[0]);
|
||||
uint32_t source_hz = clock_get_hz(clk_sys);
|
||||
uint32_t slice_hz = 16 * source_hz / pwm_hw->slice[self->slice].div;
|
||||
if (n_args == 1) {
|
||||
// Get duty cycle.
|
||||
uint32_t cc = pwm_hw->slice[self->slice].cc;
|
||||
cc = (cc >> (self->channel ? PWM_CH0_CC_B_LSB : PWM_CH0_CC_A_LSB)) & 0xffff;
|
||||
return MP_OBJ_NEW_SMALL_INT((uint64_t)cc * 1000000000ULL / slice_hz);
|
||||
} else {
|
||||
// Set duty cycle.
|
||||
mp_int_t duty_ns = mp_obj_get_int(args[1]);
|
||||
uint32_t cc = (uint64_t)duty_ns * slice_hz / 1000000000ULL;
|
||||
if (cc > 65535) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("duty larger than period"));
|
||||
}
|
||||
pwm_set_chan_level(self->slice, self->channel, cc);
|
||||
pwm_set_enabled(self->slice, true);
|
||||
return mp_const_none;
|
||||
}
|
||||
}
|
||||
MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(machine_pwm_duty_ns_obj, 1, 2, machine_pwm_duty_ns);
|
||||
|
||||
STATIC const mp_rom_map_elem_t machine_pwm_locals_dict_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR_deinit), MP_ROM_PTR(&machine_pwm_deinit_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_freq), MP_ROM_PTR(&machine_pwm_freq_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_duty_u16), MP_ROM_PTR(&machine_pwm_duty_u16_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_duty_ns), MP_ROM_PTR(&machine_pwm_duty_ns_obj) },
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(machine_pwm_locals_dict, machine_pwm_locals_dict_table);
|
||||
|
||||
const mp_obj_type_t machine_pwm_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_PWM,
|
||||
.print = machine_pwm_print,
|
||||
.make_new = machine_pwm_make_new,
|
||||
.locals_dict = (mp_obj_dict_t *)&machine_pwm_locals_dict,
|
||||
};
|
278
ports/rp2/machine_spi.c
Normal file
278
ports/rp2/machine_spi.c
Normal file
@ -0,0 +1,278 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "py/mphal.h"
|
||||
#include "py/mperrno.h"
|
||||
#include "extmod/machine_spi.h"
|
||||
#include "modmachine.h"
|
||||
|
||||
#include "hardware/spi.h"
|
||||
#include "hardware/dma.h"
|
||||
|
||||
#define DEFAULT_SPI_BAUDRATE (1000000)
|
||||
#define DEFAULT_SPI_POLARITY (0)
|
||||
#define DEFAULT_SPI_PHASE (0)
|
||||
#define DEFAULT_SPI_BITS (8)
|
||||
#define DEFAULT_SPI_FIRSTBIT (SPI_MSB_FIRST)
|
||||
#define DEFAULT_SPI0_SCK (6)
|
||||
#define DEFAULT_SPI0_MOSI (7)
|
||||
#define DEFAULT_SPI0_MISO (4)
|
||||
#define DEFAULT_SPI1_SCK (10)
|
||||
#define DEFAULT_SPI1_MOSI (11)
|
||||
#define DEFAULT_SPI1_MISO (8)
|
||||
|
||||
#define IS_VALID_PERIPH(spi, pin) ((((pin) & 8) >> 3) == (spi))
|
||||
#define IS_VALID_SCK(spi, pin) (((pin) & 3) == 2 && IS_VALID_PERIPH(spi, pin))
|
||||
#define IS_VALID_MOSI(spi, pin) (((pin) & 3) == 3 && IS_VALID_PERIPH(spi, pin))
|
||||
#define IS_VALID_MISO(spi, pin) (((pin) & 3) == 0 && IS_VALID_PERIPH(spi, pin))
|
||||
|
||||
typedef struct _machine_spi_obj_t {
|
||||
mp_obj_base_t base;
|
||||
spi_inst_t *const spi_inst;
|
||||
uint8_t spi_id;
|
||||
uint8_t polarity;
|
||||
uint8_t phase;
|
||||
uint8_t bits;
|
||||
uint8_t firstbit;
|
||||
uint8_t sck;
|
||||
uint8_t mosi;
|
||||
uint8_t miso;
|
||||
uint32_t baudrate;
|
||||
} machine_spi_obj_t;
|
||||
|
||||
STATIC machine_spi_obj_t machine_spi_obj[] = {
|
||||
{
|
||||
{&machine_spi_type}, spi0, 0,
|
||||
DEFAULT_SPI_POLARITY, DEFAULT_SPI_PHASE, DEFAULT_SPI_BITS, DEFAULT_SPI_FIRSTBIT,
|
||||
DEFAULT_SPI0_SCK, DEFAULT_SPI0_MOSI, DEFAULT_SPI0_MISO,
|
||||
0,
|
||||
},
|
||||
{
|
||||
{&machine_spi_type}, spi1, 1,
|
||||
DEFAULT_SPI_POLARITY, DEFAULT_SPI_PHASE, DEFAULT_SPI_BITS, DEFAULT_SPI_FIRSTBIT,
|
||||
DEFAULT_SPI1_SCK, DEFAULT_SPI1_MOSI, DEFAULT_SPI1_MISO,
|
||||
0,
|
||||
},
|
||||
};
|
||||
|
||||
STATIC void machine_spi_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
|
||||
machine_spi_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
mp_printf(print, "SPI(%u, baudrate=%u, polarity=%u, phase=%u, bits=%u, sck=%u, mosi=%u, miso=%u)",
|
||||
self->spi_id, self->baudrate, self->polarity, self->phase, self->bits,
|
||||
self->sck, self->mosi, self->miso);
|
||||
}
|
||||
|
||||
mp_obj_t machine_spi_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
|
||||
enum { ARG_id, ARG_baudrate, ARG_polarity, ARG_phase, ARG_bits, ARG_firstbit, ARG_sck, ARG_mosi, ARG_miso };
|
||||
static const mp_arg_t allowed_args[] = {
|
||||
{ MP_QSTR_id, MP_ARG_REQUIRED | MP_ARG_OBJ },
|
||||
{ MP_QSTR_baudrate, MP_ARG_INT, {.u_int = DEFAULT_SPI_BAUDRATE} },
|
||||
{ MP_QSTR_polarity, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = DEFAULT_SPI_POLARITY} },
|
||||
{ MP_QSTR_phase, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = DEFAULT_SPI_PHASE} },
|
||||
{ MP_QSTR_bits, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = DEFAULT_SPI_BITS} },
|
||||
{ MP_QSTR_firstbit, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = DEFAULT_SPI_FIRSTBIT} },
|
||||
{ MP_QSTR_sck, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_mosi, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_miso, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
};
|
||||
|
||||
// Parse the arguments.
|
||||
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
|
||||
mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
|
||||
|
||||
// Get the SPI bus id.
|
||||
int spi_id = mp_obj_get_int(args[ARG_id].u_obj);
|
||||
if (spi_id < 0 || spi_id >= MP_ARRAY_SIZE(machine_spi_obj)) {
|
||||
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("SPI(%d) doesn't exist"), spi_id);
|
||||
}
|
||||
|
||||
// Get static peripheral object.
|
||||
machine_spi_obj_t *self = (machine_spi_obj_t *)&machine_spi_obj[spi_id];
|
||||
|
||||
// Set SCK/MOSI/MISO pins if configured.
|
||||
if (args[ARG_sck].u_obj != mp_const_none) {
|
||||
int sck = mp_hal_get_pin_obj(args[ARG_sck].u_obj);
|
||||
if (!IS_VALID_SCK(self->spi_id, sck)) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("bad SCK pin"));
|
||||
}
|
||||
self->sck = sck;
|
||||
}
|
||||
if (args[ARG_mosi].u_obj != mp_const_none) {
|
||||
int mosi = mp_hal_get_pin_obj(args[ARG_mosi].u_obj);
|
||||
if (!IS_VALID_MOSI(self->spi_id, mosi)) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("bad MOSI pin"));
|
||||
}
|
||||
self->mosi = mosi;
|
||||
}
|
||||
if (args[ARG_miso].u_obj != mp_const_none) {
|
||||
int miso = mp_hal_get_pin_obj(args[ARG_miso].u_obj);
|
||||
if (!IS_VALID_MISO(self->spi_id, miso)) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("bad MISO pin"));
|
||||
}
|
||||
self->miso = miso;
|
||||
}
|
||||
|
||||
// Initialise the SPI peripheral if any arguments given, or it was not initialised previously.
|
||||
if (n_args > 1 || n_kw > 0 || self->baudrate == 0) {
|
||||
self->baudrate = args[ARG_baudrate].u_int;
|
||||
self->polarity = args[ARG_polarity].u_int;
|
||||
self->phase = args[ARG_phase].u_int;
|
||||
self->bits = args[ARG_bits].u_int;
|
||||
self->firstbit = args[ARG_firstbit].u_int;
|
||||
if (self->firstbit == SPI_LSB_FIRST) {
|
||||
mp_raise_NotImplementedError(MP_ERROR_TEXT("LSB"));
|
||||
}
|
||||
|
||||
spi_init(self->spi_inst, self->baudrate);
|
||||
self->baudrate = spi_set_baudrate(self->spi_inst, self->baudrate);
|
||||
spi_set_format(self->spi_inst, self->bits, self->polarity, self->phase, self->firstbit);
|
||||
gpio_set_function(self->sck, GPIO_FUNC_SPI);
|
||||
gpio_set_function(self->miso, GPIO_FUNC_SPI);
|
||||
gpio_set_function(self->mosi, GPIO_FUNC_SPI);
|
||||
}
|
||||
|
||||
return MP_OBJ_FROM_PTR(self);
|
||||
}
|
||||
|
||||
STATIC void machine_spi_init(mp_obj_base_t *self_in, size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
|
||||
enum { ARG_baudrate, ARG_polarity, ARG_phase, ARG_bits, ARG_firstbit };
|
||||
static const mp_arg_t allowed_args[] = {
|
||||
{ MP_QSTR_baudrate, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = -1} },
|
||||
{ MP_QSTR_polarity, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = -1} },
|
||||
{ MP_QSTR_phase, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = -1} },
|
||||
{ MP_QSTR_bits, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = -1} },
|
||||
{ MP_QSTR_firstbit, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = -1} },
|
||||
};
|
||||
|
||||
// Parse the arguments.
|
||||
machine_spi_obj_t *self = (machine_spi_obj_t *)self_in;
|
||||
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
|
||||
mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
|
||||
|
||||
// Reconfigure the baudrate if requested.
|
||||
if (args[ARG_baudrate].u_int != -1) {
|
||||
self->baudrate = spi_set_baudrate(self->spi_inst, args[ARG_baudrate].u_int);
|
||||
}
|
||||
|
||||
// Reconfigure the format if requested.
|
||||
bool set_format = false;
|
||||
if (args[ARG_polarity].u_int != -1) {
|
||||
self->polarity = args[ARG_polarity].u_int;
|
||||
set_format = true;
|
||||
}
|
||||
if (args[ARG_phase].u_int != -1) {
|
||||
self->phase = args[ARG_phase].u_int;
|
||||
set_format = true;
|
||||
}
|
||||
if (args[ARG_bits].u_int != -1) {
|
||||
self->bits = args[ARG_bits].u_int;
|
||||
set_format = true;
|
||||
}
|
||||
if (args[ARG_firstbit].u_int != -1) {
|
||||
self->firstbit = args[ARG_firstbit].u_int;
|
||||
if (self->firstbit == SPI_LSB_FIRST) {
|
||||
mp_raise_NotImplementedError(MP_ERROR_TEXT("LSB"));
|
||||
}
|
||||
}
|
||||
if (set_format) {
|
||||
spi_set_format(self->spi_inst, self->bits, self->polarity, self->phase, self->firstbit);
|
||||
}
|
||||
}
|
||||
|
||||
STATIC void machine_spi_transfer(mp_obj_base_t *self_in, size_t len, const uint8_t *src, uint8_t *dest) {
|
||||
machine_spi_obj_t *self = (machine_spi_obj_t *)self_in;
|
||||
// Use DMA for large transfers if channels are available
|
||||
const size_t dma_min_size_threshold = 32;
|
||||
int chan_tx = -1;
|
||||
int chan_rx = -1;
|
||||
if (len >= dma_min_size_threshold) {
|
||||
// Use two DMA channels to service the two FIFOs
|
||||
chan_tx = dma_claim_unused_channel(false);
|
||||
chan_rx = dma_claim_unused_channel(false);
|
||||
}
|
||||
bool use_dma = chan_rx >= 0 && chan_tx >= 0;
|
||||
// note src is guaranteed to be non-NULL
|
||||
bool write_only = dest == NULL;
|
||||
|
||||
if (use_dma) {
|
||||
uint8_t dev_null;
|
||||
dma_channel_config c = dma_channel_get_default_config(chan_tx);
|
||||
channel_config_set_transfer_data_size(&c, DMA_SIZE_8);
|
||||
channel_config_set_dreq(&c, spi_get_index(self->spi_inst) ? DREQ_SPI1_TX : DREQ_SPI0_TX);
|
||||
dma_channel_configure(chan_tx, &c,
|
||||
&spi_get_hw(self->spi_inst)->dr,
|
||||
src,
|
||||
len,
|
||||
false);
|
||||
|
||||
c = dma_channel_get_default_config(chan_rx);
|
||||
channel_config_set_transfer_data_size(&c, DMA_SIZE_8);
|
||||
channel_config_set_dreq(&c, spi_get_index(self->spi_inst) ? DREQ_SPI1_RX : DREQ_SPI0_RX);
|
||||
channel_config_set_read_increment(&c, false);
|
||||
channel_config_set_write_increment(&c, !write_only);
|
||||
dma_channel_configure(chan_rx, &c,
|
||||
write_only ? &dev_null : dest,
|
||||
&spi_get_hw(self->spi_inst)->dr,
|
||||
len,
|
||||
false);
|
||||
|
||||
dma_start_channel_mask((1u << chan_rx) | (1u << chan_tx));
|
||||
dma_channel_wait_for_finish_blocking(chan_rx);
|
||||
dma_channel_wait_for_finish_blocking(chan_tx);
|
||||
}
|
||||
|
||||
// If we have claimed only one channel successfully, we should release immediately
|
||||
if (chan_rx >= 0) {
|
||||
dma_channel_unclaim(chan_rx);
|
||||
}
|
||||
if (chan_tx >= 0) {
|
||||
dma_channel_unclaim(chan_tx);
|
||||
}
|
||||
|
||||
if (!use_dma) {
|
||||
// Use software for small transfers, or if couldn't claim two DMA channels
|
||||
if (write_only) {
|
||||
spi_write_blocking(self->spi_inst, src, len);
|
||||
} else {
|
||||
spi_write_read_blocking(self->spi_inst, src, dest, len);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
STATIC const mp_machine_spi_p_t machine_spi_p = {
|
||||
.init = machine_spi_init,
|
||||
.transfer = machine_spi_transfer,
|
||||
};
|
||||
|
||||
const mp_obj_type_t machine_spi_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_SPI,
|
||||
.print = machine_spi_print,
|
||||
.make_new = machine_spi_make_new,
|
||||
.protocol = &machine_spi_p,
|
||||
.locals_dict = (mp_obj_dict_t *)&mp_machine_spi_locals_dict,
|
||||
};
|
165
ports/rp2/machine_timer.c
Normal file
165
ports/rp2/machine_timer.c
Normal file
@ -0,0 +1,165 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "py/mperrno.h"
|
||||
#include "py/mphal.h"
|
||||
#include "pico/time.h"
|
||||
|
||||
#define ALARM_ID_INVALID (-1)
|
||||
#define TIMER_MODE_ONE_SHOT (0)
|
||||
#define TIMER_MODE_PERIODIC (1)
|
||||
|
||||
typedef struct _machine_timer_obj_t {
|
||||
mp_obj_base_t base;
|
||||
struct alarm_pool *pool;
|
||||
alarm_id_t alarm_id;
|
||||
uint32_t mode;
|
||||
uint64_t delta_us; // for periodic mode
|
||||
mp_obj_t callback;
|
||||
} machine_timer_obj_t;
|
||||
|
||||
const mp_obj_type_t machine_timer_type;
|
||||
|
||||
STATIC int64_t alarm_callback(alarm_id_t id, void *user_data) {
|
||||
machine_timer_obj_t *self = user_data;
|
||||
mp_sched_schedule(self->callback, MP_OBJ_FROM_PTR(self));
|
||||
if (self->mode == TIMER_MODE_ONE_SHOT) {
|
||||
return 0;
|
||||
} else {
|
||||
return -self->delta_us;
|
||||
}
|
||||
}
|
||||
|
||||
STATIC void machine_timer_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
|
||||
machine_timer_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
qstr mode = self->mode == TIMER_MODE_ONE_SHOT ? MP_QSTR_ONE_SHOT : MP_QSTR_PERIODIC;
|
||||
mp_printf(print, "Timer(mode=%q, period=%u, tick_hz=1000000)", mode, self->delta_us);
|
||||
}
|
||||
|
||||
STATIC mp_obj_t machine_timer_init_helper(machine_timer_obj_t *self, size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
|
||||
enum { ARG_mode, ARG_callback, ARG_period, ARG_tick_hz, ARG_freq, };
|
||||
static const mp_arg_t allowed_args[] = {
|
||||
{ MP_QSTR_mode, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = TIMER_MODE_PERIODIC} },
|
||||
{ MP_QSTR_callback, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_period, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 0xffffffff} },
|
||||
{ MP_QSTR_tick_hz, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 1000} },
|
||||
{ MP_QSTR_freq, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
};
|
||||
|
||||
// Parse args
|
||||
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
|
||||
mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
|
||||
|
||||
self->mode = args[ARG_mode].u_int;
|
||||
if (args[ARG_freq].u_obj != mp_const_none) {
|
||||
// Frequency specified in Hz
|
||||
#if MICROPY_PY_BUILTINS_FLOAT
|
||||
self->delta_us = (uint64_t)(MICROPY_FLOAT_CONST(1000000.0) / mp_obj_get_float(args[ARG_freq].u_obj));
|
||||
#else
|
||||
self->delta_us = 1000000 / mp_obj_get_int(args[ARG_freq].u_obj);
|
||||
#endif
|
||||
} else {
|
||||
// Period specified
|
||||
self->delta_us = (uint64_t)args[ARG_period].u_int * 1000000 / args[ARG_tick_hz].u_int;
|
||||
}
|
||||
if (self->delta_us < 1) {
|
||||
self->delta_us = 1;
|
||||
}
|
||||
|
||||
self->callback = args[ARG_callback].u_obj;
|
||||
self->alarm_id = alarm_pool_add_alarm_in_us(self->pool, self->delta_us, alarm_callback, self, true);
|
||||
if (self->alarm_id == -1) {
|
||||
mp_raise_OSError(MP_ENOMEM);
|
||||
}
|
||||
|
||||
return mp_const_none;
|
||||
}
|
||||
|
||||
STATIC mp_obj_t machine_timer_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) {
|
||||
machine_timer_obj_t *self = m_new_obj_with_finaliser(machine_timer_obj_t);
|
||||
self->base.type = &machine_timer_type;
|
||||
self->pool = alarm_pool_get_default();
|
||||
self->alarm_id = ALARM_ID_INVALID;
|
||||
|
||||
// Get timer id (only soft timer (-1) supported at the moment)
|
||||
mp_int_t id = -1;
|
||||
if (n_args > 0) {
|
||||
id = mp_obj_get_int(args[0]);
|
||||
--n_args;
|
||||
++args;
|
||||
}
|
||||
if (id != -1) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("Timer doesn't exist"));
|
||||
}
|
||||
|
||||
if (n_args > 0 || n_kw > 0) {
|
||||
// Start the timer
|
||||
mp_map_t kw_args;
|
||||
mp_map_init_fixed_table(&kw_args, n_kw, args + n_args);
|
||||
machine_timer_init_helper(self, n_args, args, &kw_args);
|
||||
}
|
||||
|
||||
return MP_OBJ_FROM_PTR(self);
|
||||
}
|
||||
|
||||
STATIC mp_obj_t machine_timer_init(size_t n_args, const mp_obj_t *args, mp_map_t *kw_args) {
|
||||
machine_timer_obj_t *self = MP_OBJ_TO_PTR(args[0]);
|
||||
if (self->alarm_id != ALARM_ID_INVALID) {
|
||||
alarm_pool_cancel_alarm(self->pool, self->alarm_id);
|
||||
self->alarm_id = ALARM_ID_INVALID;
|
||||
}
|
||||
return machine_timer_init_helper(self, n_args - 1, args + 1, kw_args);
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(machine_timer_init_obj, 1, machine_timer_init);
|
||||
|
||||
STATIC mp_obj_t machine_timer_deinit(mp_obj_t self_in) {
|
||||
machine_timer_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
if (self->alarm_id != ALARM_ID_INVALID) {
|
||||
alarm_pool_cancel_alarm(self->pool, self->alarm_id);
|
||||
self->alarm_id = ALARM_ID_INVALID;
|
||||
}
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(machine_timer_deinit_obj, machine_timer_deinit);
|
||||
|
||||
STATIC const mp_rom_map_elem_t machine_timer_locals_dict_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&machine_timer_deinit_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_init), MP_ROM_PTR(&machine_timer_init_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_deinit), MP_ROM_PTR(&machine_timer_deinit_obj) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_ONE_SHOT), MP_ROM_INT(TIMER_MODE_ONE_SHOT) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_PERIODIC), MP_ROM_INT(TIMER_MODE_PERIODIC) },
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(machine_timer_locals_dict, machine_timer_locals_dict_table);
|
||||
|
||||
const mp_obj_type_t machine_timer_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_Timer,
|
||||
.print = machine_timer_print,
|
||||
.make_new = machine_timer_make_new,
|
||||
.locals_dict = (mp_obj_dict_t *)&machine_timer_locals_dict,
|
||||
};
|
246
ports/rp2/machine_uart.c
Normal file
246
ports/rp2/machine_uart.c
Normal file
@ -0,0 +1,246 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "py/stream.h"
|
||||
#include "py/mphal.h"
|
||||
#include "py/mperrno.h"
|
||||
#include "modmachine.h"
|
||||
|
||||
#include "hardware/uart.h"
|
||||
|
||||
#define DEFAULT_UART_BAUDRATE (115200)
|
||||
#define DEFAULT_UART_BITS (8)
|
||||
#define DEFAULT_UART_STOP (1)
|
||||
#define DEFAULT_UART0_TX (0)
|
||||
#define DEFAULT_UART0_RX (1)
|
||||
#define DEFAULT_UART1_TX (4)
|
||||
#define DEFAULT_UART1_RX (5)
|
||||
|
||||
#define IS_VALID_PERIPH(uart, pin) (((((pin) + 4) & 8) >> 3) == (uart))
|
||||
#define IS_VALID_TX(uart, pin) (((pin) & 3) == 0 && IS_VALID_PERIPH(uart, pin))
|
||||
#define IS_VALID_RX(uart, pin) (((pin) & 3) == 1 && IS_VALID_PERIPH(uart, pin))
|
||||
|
||||
typedef struct _machine_uart_obj_t {
|
||||
mp_obj_base_t base;
|
||||
uart_inst_t *const uart;
|
||||
uint8_t uart_id;
|
||||
uint32_t baudrate;
|
||||
uint8_t bits;
|
||||
uart_parity_t parity;
|
||||
uint8_t stop;
|
||||
uint8_t tx;
|
||||
uint8_t rx;
|
||||
} machine_uart_obj_t;
|
||||
|
||||
STATIC machine_uart_obj_t machine_uart_obj[] = {
|
||||
{{&machine_uart_type}, uart0, 0, 0, DEFAULT_UART_BITS, UART_PARITY_NONE, DEFAULT_UART_STOP, DEFAULT_UART0_TX, DEFAULT_UART0_RX},
|
||||
{{&machine_uart_type}, uart1, 1, 0, DEFAULT_UART_BITS, UART_PARITY_NONE, DEFAULT_UART_STOP, DEFAULT_UART1_TX, DEFAULT_UART1_RX},
|
||||
};
|
||||
|
||||
STATIC const char *_parity_name[] = {"None", "0", "1"};
|
||||
|
||||
/******************************************************************************/
|
||||
// MicroPython bindings for UART
|
||||
|
||||
STATIC void machine_uart_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
|
||||
machine_uart_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
mp_printf(print, "UART(%u, baudrate=%u, bits=%u, parity=%s, stop=%u, tx=%d, rx=%d)",
|
||||
self->uart_id, self->baudrate, self->bits, _parity_name[self->parity],
|
||||
self->stop, self->tx, self->rx);
|
||||
}
|
||||
|
||||
STATIC mp_obj_t machine_uart_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
|
||||
enum { ARG_id, ARG_baudrate, ARG_bits, ARG_parity, ARG_stop, ARG_tx, ARG_rx };
|
||||
static const mp_arg_t allowed_args[] = {
|
||||
{ MP_QSTR_id, MP_ARG_REQUIRED | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_baudrate, MP_ARG_INT, {.u_int = -1} },
|
||||
{ MP_QSTR_bits, MP_ARG_INT, {.u_int = -1} },
|
||||
{ MP_QSTR_parity, MP_ARG_OBJ, {.u_rom_obj = MP_ROM_INT(-1)} },
|
||||
{ MP_QSTR_stop, MP_ARG_INT, {.u_int = -1} },
|
||||
{ MP_QSTR_tx, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_rx, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
};
|
||||
|
||||
// Parse args.
|
||||
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
|
||||
mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
|
||||
|
||||
// Get UART bus.
|
||||
int uart_id = mp_obj_get_int(args[ARG_id].u_obj);
|
||||
if (uart_id < 0 || uart_id >= MP_ARRAY_SIZE(machine_uart_obj)) {
|
||||
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("UART(%d) doesn't exist"), uart_id);
|
||||
}
|
||||
|
||||
// Get static peripheral object.
|
||||
machine_uart_obj_t *self = (machine_uart_obj_t *)&machine_uart_obj[uart_id];
|
||||
|
||||
// Set baudrate if configured.
|
||||
if (args[ARG_baudrate].u_int > 0) {
|
||||
self->baudrate = args[ARG_baudrate].u_int;
|
||||
}
|
||||
|
||||
// Set bits if configured.
|
||||
if (args[ARG_bits].u_int > 0) {
|
||||
self->bits = args[ARG_bits].u_int;
|
||||
}
|
||||
|
||||
// Set parity if configured.
|
||||
if (args[ARG_parity].u_obj != MP_OBJ_NEW_SMALL_INT(-1)) {
|
||||
if (args[ARG_parity].u_obj == mp_const_none) {
|
||||
self->parity = UART_PARITY_NONE;
|
||||
} else if (mp_obj_get_int(args[ARG_parity].u_obj) & 1) {
|
||||
self->parity = UART_PARITY_ODD;
|
||||
} else {
|
||||
self->parity = UART_PARITY_EVEN;
|
||||
}
|
||||
}
|
||||
|
||||
// Set stop bits if configured.
|
||||
if (args[ARG_stop].u_int > 0) {
|
||||
self->stop = args[ARG_stop].u_int;
|
||||
}
|
||||
|
||||
// Set TX/RX pins if configured.
|
||||
if (args[ARG_tx].u_obj != mp_const_none) {
|
||||
int tx = mp_hal_get_pin_obj(args[ARG_tx].u_obj);
|
||||
if (!IS_VALID_TX(self->uart_id, tx)) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("bad TX pin"));
|
||||
}
|
||||
self->tx = tx;
|
||||
}
|
||||
if (args[ARG_rx].u_obj != mp_const_none) {
|
||||
int rx = mp_hal_get_pin_obj(args[ARG_rx].u_obj);
|
||||
if (!IS_VALID_RX(self->uart_id, rx)) {
|
||||
mp_raise_ValueError(MP_ERROR_TEXT("bad RX pin"));
|
||||
}
|
||||
self->rx = rx;
|
||||
}
|
||||
|
||||
// Initialise the UART peripheral if any arguments given, or it was not initialised previously.
|
||||
if (n_args > 1 || n_kw > 0 || self->baudrate == 0) {
|
||||
if (self->baudrate == 0) {
|
||||
self->baudrate = DEFAULT_UART_BAUDRATE;
|
||||
}
|
||||
uart_init(self->uart, self->baudrate);
|
||||
uart_set_format(self->uart, self->bits, self->stop, self->parity);
|
||||
uart_set_fifo_enabled(self->uart, true);
|
||||
gpio_set_function(self->tx, GPIO_FUNC_UART);
|
||||
gpio_set_function(self->rx, GPIO_FUNC_UART);
|
||||
}
|
||||
|
||||
return MP_OBJ_FROM_PTR(self);
|
||||
}
|
||||
|
||||
STATIC mp_obj_t machine_uart_any(mp_obj_t self_in) {
|
||||
machine_uart_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
return MP_OBJ_NEW_SMALL_INT(uart_is_readable(self->uart));
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(machine_uart_any_obj, machine_uart_any);
|
||||
|
||||
STATIC mp_obj_t machine_uart_sendbreak(mp_obj_t self_in) {
|
||||
machine_uart_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
uart_set_break(self->uart, true);
|
||||
mp_hal_delay_us(13000000 / self->baudrate + 1);
|
||||
uart_set_break(self->uart, false);
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(machine_uart_sendbreak_obj, machine_uart_sendbreak);
|
||||
|
||||
STATIC const mp_rom_map_elem_t machine_uart_locals_dict_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR_any), MP_ROM_PTR(&machine_uart_any_obj) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_read), MP_ROM_PTR(&mp_stream_read_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_readline), MP_ROM_PTR(&mp_stream_unbuffered_readline_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_readinto), MP_ROM_PTR(&mp_stream_readinto_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_write), MP_ROM_PTR(&mp_stream_write_obj) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_sendbreak), MP_ROM_PTR(&machine_uart_sendbreak_obj) },
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(machine_uart_locals_dict, machine_uart_locals_dict_table);
|
||||
|
||||
STATIC mp_uint_t machine_uart_read(mp_obj_t self_in, void *buf_in, mp_uint_t size, int *errcode) {
|
||||
machine_uart_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
// TODO support timeout
|
||||
uint8_t *dest = buf_in;
|
||||
for (size_t i = 0; i < size; ++i) {
|
||||
while (!uart_is_readable(self->uart)) {
|
||||
MICROPY_EVENT_POLL_HOOK
|
||||
}
|
||||
*dest++ = uart_get_hw(self->uart)->dr;
|
||||
}
|
||||
return size;
|
||||
}
|
||||
|
||||
STATIC mp_uint_t machine_uart_write(mp_obj_t self_in, const void *buf_in, mp_uint_t size, int *errcode) {
|
||||
machine_uart_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
// TODO support timeout
|
||||
const uint8_t *src = buf_in;
|
||||
for (size_t i = 0; i < size; ++i) {
|
||||
while (!uart_is_writable(self->uart)) {
|
||||
MICROPY_EVENT_POLL_HOOK
|
||||
}
|
||||
uart_get_hw(self->uart)->dr = *src++;
|
||||
}
|
||||
return size;
|
||||
}
|
||||
|
||||
STATIC mp_uint_t machine_uart_ioctl(mp_obj_t self_in, mp_uint_t request, mp_uint_t arg, int *errcode) {
|
||||
machine_uart_obj_t *self = self_in;
|
||||
mp_uint_t ret;
|
||||
if (request == MP_STREAM_POLL) {
|
||||
uintptr_t flags = arg;
|
||||
ret = 0;
|
||||
if ((flags & MP_STREAM_POLL_RD) && uart_is_readable(self->uart)) {
|
||||
ret |= MP_STREAM_POLL_RD;
|
||||
}
|
||||
if ((flags & MP_STREAM_POLL_WR) && uart_is_writable(self->uart)) {
|
||||
ret |= MP_STREAM_POLL_WR;
|
||||
}
|
||||
} else {
|
||||
*errcode = MP_EINVAL;
|
||||
ret = MP_STREAM_ERROR;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
STATIC const mp_stream_p_t uart_stream_p = {
|
||||
.read = machine_uart_read,
|
||||
.write = machine_uart_write,
|
||||
.ioctl = machine_uart_ioctl,
|
||||
.is_text = false,
|
||||
};
|
||||
|
||||
const mp_obj_type_t machine_uart_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_UART,
|
||||
.print = machine_uart_print,
|
||||
.make_new = machine_uart_make_new,
|
||||
.getiter = mp_identity_getiter,
|
||||
.iternext = mp_stream_unbuffered_iter,
|
||||
.protocol = &uart_stream_p,
|
||||
.locals_dict = (mp_obj_dict_t *)&machine_uart_locals_dict,
|
||||
};
|
78
ports/rp2/machine_wdt.c
Normal file
78
ports/rp2/machine_wdt.c
Normal file
@ -0,0 +1,78 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "modmachine.h"
|
||||
|
||||
#include "hardware/watchdog.h"
|
||||
|
||||
typedef struct _machine_wdt_obj_t {
|
||||
mp_obj_base_t base;
|
||||
} machine_wdt_obj_t;
|
||||
|
||||
STATIC const machine_wdt_obj_t machine_wdt = {{&machine_wdt_type}};
|
||||
|
||||
STATIC mp_obj_t machine_wdt_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
|
||||
enum { ARG_id, ARG_timeout };
|
||||
static const mp_arg_t allowed_args[] = {
|
||||
{ MP_QSTR_id, MP_ARG_INT, {.u_int = 0} },
|
||||
{ MP_QSTR_timeout, MP_ARG_INT, {.u_int = 5000} },
|
||||
};
|
||||
|
||||
// Parse the arguments.
|
||||
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
|
||||
mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
|
||||
|
||||
// Verify the WDT id.
|
||||
mp_int_t id = args[ARG_id].u_int;
|
||||
if (id != 0) {
|
||||
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("WDT(%d) doesn't exist"), id);
|
||||
}
|
||||
|
||||
// Start the watchdog (timeout is in milliseconds).
|
||||
watchdog_enable(args[ARG_timeout].u_int, false);
|
||||
|
||||
return MP_OBJ_FROM_PTR(&machine_wdt);
|
||||
}
|
||||
|
||||
STATIC mp_obj_t machine_wdt_feed(mp_obj_t self_in) {
|
||||
(void)self_in;
|
||||
watchdog_update();
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(machine_wdt_feed_obj, machine_wdt_feed);
|
||||
|
||||
STATIC const mp_rom_map_elem_t machine_wdt_locals_dict_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR_feed), MP_ROM_PTR(&machine_wdt_feed_obj) },
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(machine_wdt_locals_dict, machine_wdt_locals_dict_table);
|
||||
|
||||
const mp_obj_type_t machine_wdt_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_WDT,
|
||||
.make_new = machine_wdt_make_new,
|
||||
.locals_dict = (mp_obj_dict_t *)&machine_wdt_locals_dict,
|
||||
};
|
208
ports/rp2/main.c
Normal file
208
ports/rp2/main.c
Normal file
@ -0,0 +1,208 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#include "py/compile.h"
|
||||
#include "py/runtime.h"
|
||||
#include "py/gc.h"
|
||||
#include "py/mperrno.h"
|
||||
#include "py/stackctrl.h"
|
||||
#include "lib/mp-readline/readline.h"
|
||||
#include "lib/utils/gchelper.h"
|
||||
#include "lib/utils/pyexec.h"
|
||||
#include "tusb.h"
|
||||
#include "uart.h"
|
||||
#include "modmachine.h"
|
||||
#include "modrp2.h"
|
||||
#include "genhdr/mpversion.h"
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
#include "pico/binary_info.h"
|
||||
#include "hardware/rtc.h"
|
||||
#include "hardware/structs/rosc.h"
|
||||
|
||||
extern uint8_t __StackTop, __StackBottom;
|
||||
static char gc_heap[192 * 1024];
|
||||
|
||||
// Embed version info in the binary in machine readable form
|
||||
bi_decl(bi_program_version_string(MICROPY_GIT_TAG));
|
||||
|
||||
// Add a section to the picotool output similar to program features, but for frozen modules
|
||||
// (it will aggregate BINARY_INFO_ID_MP_FROZEN binary info)
|
||||
bi_decl(bi_program_feature_group_with_flags(BINARY_INFO_TAG_MICROPYTHON,
|
||||
BINARY_INFO_ID_MP_FROZEN, "frozen modules",
|
||||
BI_NAMED_GROUP_SEPARATE_COMMAS | BI_NAMED_GROUP_SORT_ALPHA));
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
#if MICROPY_HW_ENABLE_UART_REPL
|
||||
bi_decl(bi_program_feature("UART REPL"))
|
||||
setup_default_uart();
|
||||
mp_uart_init();
|
||||
#endif
|
||||
|
||||
#if MICROPY_HW_ENABLE_USBDEV
|
||||
bi_decl(bi_program_feature("USB REPL"))
|
||||
tusb_init();
|
||||
#endif
|
||||
|
||||
#if MICROPY_PY_THREAD
|
||||
bi_decl(bi_program_feature("thread support"))
|
||||
mp_thread_init();
|
||||
#endif
|
||||
|
||||
// Start and initialise the RTC
|
||||
datetime_t t = {
|
||||
.year = 2021,
|
||||
.month = 1,
|
||||
.day = 1,
|
||||
.dotw = 5, // 0 is Sunday, so 5 is Friday
|
||||
.hour = 0,
|
||||
.min = 0,
|
||||
.sec = 0,
|
||||
};
|
||||
rtc_init();
|
||||
rtc_set_datetime(&t);
|
||||
|
||||
// Initialise stack extents and GC heap.
|
||||
mp_stack_set_top(&__StackTop);
|
||||
mp_stack_set_limit(&__StackTop - &__StackBottom - 256);
|
||||
gc_init(&gc_heap[0], &gc_heap[MP_ARRAY_SIZE(gc_heap)]);
|
||||
|
||||
for (;;) {
|
||||
|
||||
// Initialise MicroPython runtime.
|
||||
mp_init();
|
||||
mp_obj_list_init(MP_OBJ_TO_PTR(mp_sys_path), 0);
|
||||
mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR_));
|
||||
mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR__slash_lib));
|
||||
mp_obj_list_init(MP_OBJ_TO_PTR(mp_sys_argv), 0);
|
||||
|
||||
// Initialise sub-systems.
|
||||
readline_init0();
|
||||
machine_pin_init();
|
||||
rp2_pio_init();
|
||||
|
||||
// Execute _boot.py to set up the filesystem.
|
||||
pyexec_frozen_module("_boot.py");
|
||||
|
||||
// Execute user scripts.
|
||||
pyexec_file_if_exists("boot.py");
|
||||
if (pyexec_mode_kind == PYEXEC_MODE_FRIENDLY_REPL) {
|
||||
pyexec_file_if_exists("main.py");
|
||||
}
|
||||
|
||||
for (;;) {
|
||||
if (pyexec_mode_kind == PYEXEC_MODE_RAW_REPL) {
|
||||
if (pyexec_raw_repl() != 0) {
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
if (pyexec_friendly_repl() != 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
mp_printf(MP_PYTHON_PRINTER, "MPY: soft reboot\n");
|
||||
rp2_pio_deinit();
|
||||
machine_pin_deinit();
|
||||
gc_sweep_all();
|
||||
mp_deinit();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void gc_collect(void) {
|
||||
gc_collect_start();
|
||||
gc_helper_collect_regs_and_stack();
|
||||
#if MICROPY_PY_THREAD
|
||||
mp_thread_gc_others();
|
||||
#endif
|
||||
gc_collect_end();
|
||||
}
|
||||
|
||||
void nlr_jump_fail(void *val) {
|
||||
printf("FATAL: uncaught exception %p\n", val);
|
||||
mp_obj_print_exception(&mp_plat_print, MP_OBJ_FROM_PTR(val));
|
||||
for (;;) {
|
||||
__breakpoint();
|
||||
}
|
||||
}
|
||||
|
||||
#ifndef NDEBUG
|
||||
void MP_WEAK __assert_func(const char *file, int line, const char *func, const char *expr) {
|
||||
printf("Assertion '%s' failed, at file %s:%d\n", expr, file, line);
|
||||
panic("Assertion failed");
|
||||
}
|
||||
#endif
|
||||
|
||||
uint32_t rosc_random_u32(void) {
|
||||
uint32_t value = 0;
|
||||
for (size_t i = 0; i < 32; ++i) {
|
||||
value = value << 1 | rosc_hw->randombit;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
const char rp2_help_text[] =
|
||||
"Welcome to MicroPython!\n"
|
||||
"\n"
|
||||
"For online help please visit https://micropython.org/help/.\n"
|
||||
"\n"
|
||||
"For access to the hardware use the 'machine' module. RP2 specific commands\n"
|
||||
"are in the 'rp2' module.\n"
|
||||
"\n"
|
||||
"Quick overview of some objects:\n"
|
||||
" machine.Pin(pin) -- get a pin, eg machine.Pin(0)\n"
|
||||
" machine.Pin(pin, m, [p]) -- get a pin and configure it for IO mode m, pull mode p\n"
|
||||
" methods: init(..), value([v]), high(), low(), irq(handler)\n"
|
||||
" machine.ADC(pin) -- make an analog object from a pin\n"
|
||||
" methods: read_u16()\n"
|
||||
" machine.PWM(pin) -- make a PWM object from a pin\n"
|
||||
" methods: deinit(), freq([f]), duty_u16([d]), duty_ns([d])\n"
|
||||
" machine.I2C(id) -- create an I2C object (id=0,1)\n"
|
||||
" methods: readfrom(addr, buf, stop=True), writeto(addr, buf, stop=True)\n"
|
||||
" readfrom_mem(addr, memaddr, arg), writeto_mem(addr, memaddr, arg)\n"
|
||||
" machine.SPI(id, baudrate=1000000) -- create an SPI object (id=0,1)\n"
|
||||
" methods: read(nbytes, write=0x00), write(buf), write_readinto(wr_buf, rd_buf)\n"
|
||||
" machine.Timer(freq, callback) -- create a software timer object\n"
|
||||
" eg: machine.Timer(freq=1, callback=lambda t:print(t))\n"
|
||||
"\n"
|
||||
"Pins are numbered 0-29, and 26-29 have ADC capabilities\n"
|
||||
"Pin IO modes are: Pin.IN, Pin.OUT, Pin.ALT\n"
|
||||
"Pin pull modes are: Pin.PULL_UP, Pin.PULL_DOWN\n"
|
||||
"\n"
|
||||
"Useful control commands:\n"
|
||||
" CTRL-C -- interrupt a running program\n"
|
||||
" CTRL-D -- on a blank line, do a soft reset of the board\n"
|
||||
" CTRL-E -- on a blank line, enter paste mode\n"
|
||||
"\n"
|
||||
"For further help on a specific object, type help(obj)\n"
|
||||
"For a list of available modules, type help('modules')\n"
|
||||
;
|
||||
|
3
ports/rp2/manifest.py
Normal file
3
ports/rp2/manifest.py
Normal file
@ -0,0 +1,3 @@
|
||||
freeze("modules")
|
||||
freeze("$(MPY_DIR)/drivers/onewire")
|
||||
include("$(MPY_DIR)/extmod/uasyncio/manifest.py")
|
252
ports/rp2/memmap_mp.ld
Normal file
252
ports/rp2/memmap_mp.ld
Normal file
@ -0,0 +1,252 @@
|
||||
/* Based on GCC ARM embedded samples.
|
||||
Defines the following symbols for use by code:
|
||||
__exidx_start
|
||||
__exidx_end
|
||||
__etext
|
||||
__data_start__
|
||||
__preinit_array_start
|
||||
__preinit_array_end
|
||||
__init_array_start
|
||||
__init_array_end
|
||||
__fini_array_start
|
||||
__fini_array_end
|
||||
__data_end__
|
||||
__bss_start__
|
||||
__bss_end__
|
||||
__end__
|
||||
end
|
||||
__HeapLimit
|
||||
__StackLimit
|
||||
__StackTop
|
||||
__stack (== StackTop)
|
||||
*/
|
||||
|
||||
MEMORY
|
||||
{
|
||||
FLASH(rx) : ORIGIN = 0x10000000, LENGTH = 2048k
|
||||
RAM(rwx) : ORIGIN = 0x20000000, LENGTH = 256k
|
||||
SCRATCH_X(rwx) : ORIGIN = 0x20040000, LENGTH = 4k
|
||||
SCRATCH_Y(rwx) : ORIGIN = 0x20041000, LENGTH = 4k
|
||||
}
|
||||
|
||||
ENTRY(_entry_point)
|
||||
|
||||
SECTIONS
|
||||
{
|
||||
/* Second stage bootloader is prepended to the image. It must be 256 bytes big
|
||||
and checksummed. It is usually built by the boot_stage2 target
|
||||
in the Pico SDK
|
||||
*/
|
||||
|
||||
.flash_begin : {
|
||||
__flash_binary_start = .;
|
||||
} > FLASH
|
||||
|
||||
.boot2 : {
|
||||
__boot2_start__ = .;
|
||||
KEEP (*(.boot2))
|
||||
__boot2_end__ = .;
|
||||
} > FLASH
|
||||
|
||||
ASSERT(__boot2_end__ - __boot2_start__ == 256,
|
||||
"ERROR: Pico second stage bootloader must be 256 bytes in size")
|
||||
|
||||
/* The second stage will always enter the image at the start of .text.
|
||||
The debugger will use the ELF entry point, which is the _entry_point
|
||||
symbol if present, otherwise defaults to start of .text.
|
||||
This can be used to transfer control back to the bootrom on debugger
|
||||
launches only, to perform proper flash setup.
|
||||
*/
|
||||
|
||||
.text : {
|
||||
__reset_start = .;
|
||||
KEEP (*(.reset))
|
||||
. = ALIGN(256);
|
||||
__reset_end = .;
|
||||
ASSERT(__reset_end - __reset_start == 256, "ERROR: reset section should only be 256 bytes");
|
||||
KEEP (*(.vectors))
|
||||
/* TODO revisit this now memset/memcpy/float in ROM */
|
||||
/* bit of a hack right now to exclude all floating point and time critical (e.g. memset, memcpy) code from
|
||||
* FLASH ... we will include any thing excluded here in .data below by default */
|
||||
*(.init)
|
||||
/* Change for MicroPython... excluse gc.c, parse.c, vm.c from flash */
|
||||
*(EXCLUDE_FILE(*libgcc.a: *libc.a: *lib_a-mem*.o *libm.a: *gc.c.obj *vm.c.obj *parse.c.obj) .text*)
|
||||
*(.fini)
|
||||
/* Pull all c'tors into .text */
|
||||
*crtbegin.o(.ctors)
|
||||
*crtbegin?.o(.ctors)
|
||||
*(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
|
||||
*(SORT(.ctors.*))
|
||||
*(.ctors)
|
||||
/* Followed by destructors */
|
||||
*crtbegin.o(.dtors)
|
||||
*crtbegin?.o(.dtors)
|
||||
*(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
|
||||
*(SORT(.dtors.*))
|
||||
*(.dtors)
|
||||
|
||||
*(.eh_frame*)
|
||||
. = ALIGN(4);
|
||||
} > FLASH
|
||||
|
||||
.rodata : {
|
||||
*(EXCLUDE_FILE(*libgcc.a: *libc.a:*lib_a-mem*.o *libm.a:) .rodata*)
|
||||
. = ALIGN(4);
|
||||
*(SORT_BY_ALIGNMENT(SORT_BY_NAME(.flashdata*)))
|
||||
. = ALIGN(4);
|
||||
} > FLASH
|
||||
|
||||
.ARM.extab :
|
||||
{
|
||||
*(.ARM.extab* .gnu.linkonce.armextab.*)
|
||||
} > FLASH
|
||||
|
||||
__exidx_start = .;
|
||||
.ARM.exidx :
|
||||
{
|
||||
*(.ARM.exidx* .gnu.linkonce.armexidx.*)
|
||||
} > FLASH
|
||||
__exidx_end = .;
|
||||
|
||||
/* Machine inspectable binary information */
|
||||
. = ALIGN(4);
|
||||
__binary_info_start = .;
|
||||
.binary_info :
|
||||
{
|
||||
KEEP(*(.binary_info.keep.*))
|
||||
*(.binary_info.*)
|
||||
} > FLASH
|
||||
__binary_info_end = .;
|
||||
. = ALIGN(4);
|
||||
|
||||
/* End of .text-like segments */
|
||||
__etext = .;
|
||||
|
||||
.ram_vector_table (COPY): {
|
||||
*(.ram_vector_table)
|
||||
} > RAM
|
||||
|
||||
.data : {
|
||||
__data_start__ = .;
|
||||
*(vtable)
|
||||
|
||||
*(.time_critical*)
|
||||
|
||||
/* remaining .text and .rodata; i.e. stuff we exclude above because we want it in RAM */
|
||||
*(.text*)
|
||||
. = ALIGN(4);
|
||||
*(.rodata*)
|
||||
. = ALIGN(4);
|
||||
|
||||
*(.data*)
|
||||
|
||||
. = ALIGN(4);
|
||||
*(.after_data.*)
|
||||
. = ALIGN(4);
|
||||
/* preinit data */
|
||||
PROVIDE_HIDDEN (__mutex_array_start = .);
|
||||
KEEP(*(SORT(.mutex_array.*)))
|
||||
KEEP(*(.mutex_array))
|
||||
PROVIDE_HIDDEN (__mutex_array_end = .);
|
||||
|
||||
. = ALIGN(4);
|
||||
/* preinit data */
|
||||
PROVIDE_HIDDEN (__preinit_array_start = .);
|
||||
KEEP(*(SORT(.preinit_array.*)))
|
||||
KEEP(*(.preinit_array))
|
||||
PROVIDE_HIDDEN (__preinit_array_end = .);
|
||||
|
||||
. = ALIGN(4);
|
||||
/* init data */
|
||||
PROVIDE_HIDDEN (__init_array_start = .);
|
||||
KEEP(*(SORT(.init_array.*)))
|
||||
KEEP(*(.init_array))
|
||||
PROVIDE_HIDDEN (__init_array_end = .);
|
||||
|
||||
. = ALIGN(4);
|
||||
/* finit data */
|
||||
PROVIDE_HIDDEN (__fini_array_start = .);
|
||||
*(SORT(.fini_array.*))
|
||||
*(.fini_array)
|
||||
PROVIDE_HIDDEN (__fini_array_end = .);
|
||||
|
||||
*(.jcr)
|
||||
. = ALIGN(4);
|
||||
/* All data end */
|
||||
__data_end__ = .;
|
||||
} > RAM AT> FLASH
|
||||
|
||||
.uninitialized_data (COPY): {
|
||||
. = ALIGN(4);
|
||||
*(.uninitialized_data*)
|
||||
} > RAM
|
||||
|
||||
/* Start and end symbols must be word-aligned */
|
||||
.scratch_x : {
|
||||
__scratch_x_start__ = .;
|
||||
*(.scratch_x.*)
|
||||
. = ALIGN(4);
|
||||
__scratch_x_end__ = .;
|
||||
} > SCRATCH_X AT > FLASH
|
||||
__scratch_x_source__ = LOADADDR(.scratch_x);
|
||||
|
||||
.scratch_y : {
|
||||
__scratch_y_start__ = .;
|
||||
*(.scratch_y.*)
|
||||
. = ALIGN(4);
|
||||
__scratch_y_end__ = .;
|
||||
} > SCRATCH_Y AT > FLASH
|
||||
__scratch_y_source__ = LOADADDR(.scratch_y);
|
||||
|
||||
.bss : {
|
||||
. = ALIGN(4);
|
||||
__bss_start__ = .;
|
||||
*(SORT_BY_ALIGNMENT(SORT_BY_NAME(.bss*)))
|
||||
*(COMMON)
|
||||
. = ALIGN(4);
|
||||
__bss_end__ = .;
|
||||
} > RAM
|
||||
|
||||
.heap (COPY):
|
||||
{
|
||||
__end__ = .;
|
||||
end = __end__;
|
||||
*(.heap*)
|
||||
__HeapLimit = .;
|
||||
} > RAM
|
||||
|
||||
/* .stack*_dummy section doesn't contains any symbols. It is only
|
||||
* used for linker to calculate size of stack sections, and assign
|
||||
* values to stack symbols later
|
||||
*
|
||||
* stack1 section may be empty/missing if platform_launch_core1 is not used */
|
||||
|
||||
/* by default we put core 0 stack at the end of scratch Y, so that if core 1
|
||||
* stack is not used then all of SCRATCH_X is free.
|
||||
*/
|
||||
.stack1_dummy (COPY):
|
||||
{
|
||||
*(.stack1*)
|
||||
} > SCRATCH_X
|
||||
.stack_dummy (COPY):
|
||||
{
|
||||
*(.stack*)
|
||||
} > SCRATCH_Y
|
||||
|
||||
.flash_end : {
|
||||
__flash_binary_end = .;
|
||||
} > FLASH
|
||||
|
||||
/* stack limit is poorly named, but historically is maximum heap ptr */
|
||||
__StackLimit = ORIGIN(RAM) + LENGTH(RAM);
|
||||
__StackOneTop = ORIGIN(SCRATCH_X) + LENGTH(SCRATCH_X);
|
||||
__StackTop = ORIGIN(SCRATCH_Y) + LENGTH(SCRATCH_Y);
|
||||
__StackOneBottom = __StackOneTop - SIZEOF(.stack1_dummy);
|
||||
__StackBottom = __StackTop - SIZEOF(.stack_dummy);
|
||||
PROVIDE(__stack = __StackTop);
|
||||
|
||||
/* Check if data + heap + stack exceeds RAM limit */
|
||||
ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed")
|
||||
/* todo assert on extra code */
|
||||
}
|
||||
|
40
ports/rp2/micropy_extmod.cmake
Normal file
40
ports/rp2/micropy_extmod.cmake
Normal file
@ -0,0 +1,40 @@
|
||||
# CMake fragment for MicroPython extmod component
|
||||
|
||||
set(SOURCE_EXTMOD
|
||||
${MPY_DIR}/extmod/machine_i2c.c
|
||||
${MPY_DIR}/extmod/machine_mem.c
|
||||
${MPY_DIR}/extmod/machine_pulse.c
|
||||
${MPY_DIR}/extmod/machine_signal.c
|
||||
${MPY_DIR}/extmod/machine_spi.c
|
||||
${MPY_DIR}/extmod/modbtree.c
|
||||
${MPY_DIR}/extmod/modframebuf.c
|
||||
${MPY_DIR}/extmod/modonewire.c
|
||||
${MPY_DIR}/extmod/moduasyncio.c
|
||||
${MPY_DIR}/extmod/modubinascii.c
|
||||
${MPY_DIR}/extmod/moducryptolib.c
|
||||
${MPY_DIR}/extmod/moductypes.c
|
||||
${MPY_DIR}/extmod/moduhashlib.c
|
||||
${MPY_DIR}/extmod/moduheapq.c
|
||||
${MPY_DIR}/extmod/modujson.c
|
||||
${MPY_DIR}/extmod/modurandom.c
|
||||
${MPY_DIR}/extmod/modure.c
|
||||
${MPY_DIR}/extmod/moduselect.c
|
||||
${MPY_DIR}/extmod/modussl_axtls.c
|
||||
${MPY_DIR}/extmod/modussl_mbedtls.c
|
||||
${MPY_DIR}/extmod/modutimeq.c
|
||||
${MPY_DIR}/extmod/moduwebsocket.c
|
||||
${MPY_DIR}/extmod/moduzlib.c
|
||||
${MPY_DIR}/extmod/modwebrepl.c
|
||||
${MPY_DIR}/extmod/uos_dupterm.c
|
||||
${MPY_DIR}/extmod/utime_mphal.c
|
||||
${MPY_DIR}/extmod/vfs.c
|
||||
${MPY_DIR}/extmod/vfs_blockdev.c
|
||||
${MPY_DIR}/extmod/vfs_fat.c
|
||||
${MPY_DIR}/extmod/vfs_fat_diskio.c
|
||||
${MPY_DIR}/extmod/vfs_fat_file.c
|
||||
${MPY_DIR}/extmod/vfs_lfs.c
|
||||
${MPY_DIR}/extmod/vfs_posix.c
|
||||
${MPY_DIR}/extmod/vfs_posix_file.c
|
||||
${MPY_DIR}/extmod/vfs_reader.c
|
||||
${MPY_DIR}/extmod/virtpin.c
|
||||
)
|
134
ports/rp2/micropy_py.cmake
Normal file
134
ports/rp2/micropy_py.cmake
Normal file
@ -0,0 +1,134 @@
|
||||
# CMake fragment for MicroPython core py component
|
||||
|
||||
set(MPY_PY_DIR "${MPY_DIR}/py")
|
||||
set(MPY_PY_QSTRDEFS "${MPY_PY_DIR}/qstrdefs.h")
|
||||
set(MPY_GENHDR_DIR "${CMAKE_BINARY_DIR}/genhdr")
|
||||
set(MPY_MPVERSION "${MPY_GENHDR_DIR}/mpversion.h")
|
||||
set(MPY_MODULEDEFS "${MPY_GENHDR_DIR}/moduledefs.h")
|
||||
set(MPY_QSTR_DEFS_LAST "${MPY_GENHDR_DIR}/qstr.i.last")
|
||||
set(MPY_QSTR_DEFS_SPLIT "${MPY_GENHDR_DIR}/qstr.split")
|
||||
set(MPY_QSTR_DEFS_COLLECTED "${MPY_GENHDR_DIR}/qstrdefs.collected.h")
|
||||
set(MPY_QSTR_DEFS_PREPROCESSED "${MPY_GENHDR_DIR}/qstrdefs.preprocessed.h")
|
||||
set(MPY_QSTR_DEFS_GENERATED "${MPY_GENHDR_DIR}/qstrdefs.generated.h")
|
||||
set(MPY_FROZEN_CONTENT "${CMAKE_BINARY_DIR}/frozen_content.c")
|
||||
|
||||
# All py/ source files
|
||||
set(SOURCE_PY
|
||||
${MPY_PY_DIR}/argcheck.c
|
||||
${MPY_PY_DIR}/asmarm.c
|
||||
${MPY_PY_DIR}/asmbase.c
|
||||
${MPY_PY_DIR}/asmthumb.c
|
||||
${MPY_PY_DIR}/asmx64.c
|
||||
${MPY_PY_DIR}/asmx86.c
|
||||
${MPY_PY_DIR}/asmxtensa.c
|
||||
${MPY_PY_DIR}/bc.c
|
||||
${MPY_PY_DIR}/binary.c
|
||||
${MPY_PY_DIR}/builtinevex.c
|
||||
${MPY_PY_DIR}/builtinhelp.c
|
||||
${MPY_PY_DIR}/builtinimport.c
|
||||
${MPY_PY_DIR}/compile.c
|
||||
${MPY_PY_DIR}/emitbc.c
|
||||
${MPY_PY_DIR}/emitcommon.c
|
||||
${MPY_PY_DIR}/emitglue.c
|
||||
${MPY_PY_DIR}/emitinlinethumb.c
|
||||
${MPY_PY_DIR}/emitinlinextensa.c
|
||||
${MPY_PY_DIR}/emitnarm.c
|
||||
${MPY_PY_DIR}/emitnthumb.c
|
||||
${MPY_PY_DIR}/emitnx64.c
|
||||
${MPY_PY_DIR}/emitnx86.c
|
||||
${MPY_PY_DIR}/emitnxtensa.c
|
||||
${MPY_PY_DIR}/emitnxtensawin.c
|
||||
${MPY_PY_DIR}/formatfloat.c
|
||||
${MPY_PY_DIR}/frozenmod.c
|
||||
${MPY_PY_DIR}/gc.c
|
||||
${MPY_PY_DIR}/lexer.c
|
||||
${MPY_PY_DIR}/malloc.c
|
||||
${MPY_PY_DIR}/map.c
|
||||
${MPY_PY_DIR}/modarray.c
|
||||
${MPY_PY_DIR}/modbuiltins.c
|
||||
${MPY_PY_DIR}/modcmath.c
|
||||
${MPY_PY_DIR}/modcollections.c
|
||||
${MPY_PY_DIR}/modgc.c
|
||||
${MPY_PY_DIR}/modio.c
|
||||
${MPY_PY_DIR}/modmath.c
|
||||
${MPY_PY_DIR}/modmicropython.c
|
||||
${MPY_PY_DIR}/modstruct.c
|
||||
${MPY_PY_DIR}/modsys.c
|
||||
${MPY_PY_DIR}/modthread.c
|
||||
${MPY_PY_DIR}/moduerrno.c
|
||||
${MPY_PY_DIR}/mpprint.c
|
||||
${MPY_PY_DIR}/mpstate.c
|
||||
${MPY_PY_DIR}/mpz.c
|
||||
${MPY_PY_DIR}/nativeglue.c
|
||||
${MPY_PY_DIR}/nlr.c
|
||||
${MPY_PY_DIR}/nlrpowerpc.c
|
||||
${MPY_PY_DIR}/nlrsetjmp.c
|
||||
${MPY_PY_DIR}/nlrthumb.c
|
||||
${MPY_PY_DIR}/nlrx64.c
|
||||
${MPY_PY_DIR}/nlrx86.c
|
||||
${MPY_PY_DIR}/nlrxtensa.c
|
||||
${MPY_PY_DIR}/obj.c
|
||||
${MPY_PY_DIR}/objarray.c
|
||||
${MPY_PY_DIR}/objattrtuple.c
|
||||
${MPY_PY_DIR}/objbool.c
|
||||
${MPY_PY_DIR}/objboundmeth.c
|
||||
${MPY_PY_DIR}/objcell.c
|
||||
${MPY_PY_DIR}/objclosure.c
|
||||
${MPY_PY_DIR}/objcomplex.c
|
||||
${MPY_PY_DIR}/objdeque.c
|
||||
${MPY_PY_DIR}/objdict.c
|
||||
${MPY_PY_DIR}/objenumerate.c
|
||||
${MPY_PY_DIR}/objexcept.c
|
||||
${MPY_PY_DIR}/objfilter.c
|
||||
${MPY_PY_DIR}/objfloat.c
|
||||
${MPY_PY_DIR}/objfun.c
|
||||
${MPY_PY_DIR}/objgenerator.c
|
||||
${MPY_PY_DIR}/objgetitemiter.c
|
||||
${MPY_PY_DIR}/objint.c
|
||||
${MPY_PY_DIR}/objint_longlong.c
|
||||
${MPY_PY_DIR}/objint_mpz.c
|
||||
${MPY_PY_DIR}/objlist.c
|
||||
${MPY_PY_DIR}/objmap.c
|
||||
${MPY_PY_DIR}/objmodule.c
|
||||
${MPY_PY_DIR}/objnamedtuple.c
|
||||
${MPY_PY_DIR}/objnone.c
|
||||
${MPY_PY_DIR}/objobject.c
|
||||
${MPY_PY_DIR}/objpolyiter.c
|
||||
${MPY_PY_DIR}/objproperty.c
|
||||
${MPY_PY_DIR}/objrange.c
|
||||
${MPY_PY_DIR}/objreversed.c
|
||||
${MPY_PY_DIR}/objset.c
|
||||
${MPY_PY_DIR}/objsingleton.c
|
||||
${MPY_PY_DIR}/objslice.c
|
||||
${MPY_PY_DIR}/objstr.c
|
||||
${MPY_PY_DIR}/objstringio.c
|
||||
${MPY_PY_DIR}/objstrunicode.c
|
||||
${MPY_PY_DIR}/objtuple.c
|
||||
${MPY_PY_DIR}/objtype.c
|
||||
${MPY_PY_DIR}/objzip.c
|
||||
${MPY_PY_DIR}/opmethods.c
|
||||
${MPY_PY_DIR}/pairheap.c
|
||||
${MPY_PY_DIR}/parse.c
|
||||
${MPY_PY_DIR}/parsenum.c
|
||||
${MPY_PY_DIR}/parsenumbase.c
|
||||
${MPY_PY_DIR}/persistentcode.c
|
||||
${MPY_PY_DIR}/profile.c
|
||||
${MPY_PY_DIR}/pystack.c
|
||||
${MPY_PY_DIR}/qstr.c
|
||||
${MPY_PY_DIR}/reader.c
|
||||
${MPY_PY_DIR}/repl.c
|
||||
${MPY_PY_DIR}/ringbuf.c
|
||||
${MPY_PY_DIR}/runtime.c
|
||||
${MPY_PY_DIR}/runtime_utils.c
|
||||
${MPY_PY_DIR}/scheduler.c
|
||||
${MPY_PY_DIR}/scope.c
|
||||
${MPY_PY_DIR}/sequence.c
|
||||
${MPY_PY_DIR}/showbc.c
|
||||
${MPY_PY_DIR}/smallint.c
|
||||
${MPY_PY_DIR}/stackctrl.c
|
||||
${MPY_PY_DIR}/stream.c
|
||||
${MPY_PY_DIR}/unicode.c
|
||||
${MPY_PY_DIR}/vm.c
|
||||
${MPY_PY_DIR}/vstr.c
|
||||
${MPY_PY_DIR}/warning.c
|
||||
)
|
91
ports/rp2/micropy_rules.cmake
Normal file
91
ports/rp2/micropy_rules.cmake
Normal file
@ -0,0 +1,91 @@
|
||||
# CMake fragment for MicroPython rules
|
||||
|
||||
target_sources(${MICROPYTHON_TARGET} PRIVATE
|
||||
${MPY_MPVERSION}
|
||||
${MPY_QSTR_DEFS_GENERATED}
|
||||
${MPY_FROZEN_CONTENT}
|
||||
)
|
||||
|
||||
# Command to force the build of another command
|
||||
|
||||
add_custom_command(
|
||||
OUTPUT FORCE_BUILD
|
||||
COMMENT ""
|
||||
COMMAND echo -n
|
||||
)
|
||||
|
||||
# Generate mpversion.h
|
||||
|
||||
add_custom_command(
|
||||
OUTPUT ${MPY_MPVERSION}
|
||||
COMMAND ${CMAKE_COMMAND} -E make_directory ${MPY_GENHDR_DIR}
|
||||
COMMAND python3 ${MPY_DIR}/py/makeversionhdr.py ${MPY_MPVERSION}
|
||||
DEPENDS FORCE_BUILD
|
||||
)
|
||||
|
||||
# Generate moduledefs.h
|
||||
# This is currently hard-coded to support modarray.c only, because makemoduledefs.py doesn't support absolute paths
|
||||
|
||||
add_custom_command(
|
||||
OUTPUT ${MPY_MODULEDEFS}
|
||||
COMMAND python3 ${MPY_PY_DIR}/makemoduledefs.py --vpath="." ../../../py/modarray.c > ${MPY_MODULEDEFS}
|
||||
DEPENDS ${MPY_MPVERSION}
|
||||
${SOURCE_QSTR}
|
||||
)
|
||||
|
||||
# Generate qstrs
|
||||
|
||||
# If any of the dependencies in this rule change then the C-preprocessor step must be run.
|
||||
# It only needs to be passed the list of SOURCE_QSTR files that have changed since it was
|
||||
# last run, but it looks like it's not possible to specify that with cmake.
|
||||
add_custom_command(
|
||||
OUTPUT ${MPY_QSTR_DEFS_LAST}
|
||||
COMMAND ${CMAKE_C_COMPILER} -E \$\(C_INCLUDES\) \$\(C_FLAGS\) -DNO_QSTR ${SOURCE_QSTR} > ${MPY_GENHDR_DIR}/qstr.i.last
|
||||
DEPENDS ${MPY_MODULEDEFS}
|
||||
${SOURCE_QSTR}
|
||||
VERBATIM
|
||||
)
|
||||
|
||||
add_custom_command(
|
||||
OUTPUT ${MPY_QSTR_DEFS_SPLIT}
|
||||
COMMAND python3 ${MPY_DIR}/py/makeqstrdefs.py split qstr ${MPY_GENHDR_DIR}/qstr.i.last ${MPY_GENHDR_DIR}/qstr _
|
||||
COMMAND touch ${MPY_QSTR_DEFS_SPLIT}
|
||||
DEPENDS ${MPY_QSTR_DEFS_LAST}
|
||||
VERBATIM
|
||||
)
|
||||
|
||||
add_custom_command(
|
||||
OUTPUT ${MPY_QSTR_DEFS_COLLECTED}
|
||||
COMMAND python3 ${MPY_DIR}/py/makeqstrdefs.py cat qstr _ ${MPY_GENHDR_DIR}/qstr ${MPY_QSTR_DEFS_COLLECTED}
|
||||
DEPENDS ${MPY_QSTR_DEFS_SPLIT}
|
||||
VERBATIM
|
||||
)
|
||||
|
||||
add_custom_command(
|
||||
OUTPUT ${MPY_QSTR_DEFS_PREPROCESSED}
|
||||
COMMAND cat ${MPY_PY_QSTRDEFS} ${MPY_QSTR_DEFS} ${MPY_QSTR_DEFS_COLLECTED} | sed "s/^Q(.*)/\"&\"/" | ${CMAKE_C_COMPILER} -E \$\(C_INCLUDES\) \$\(C_FLAGS\) - | sed "s/^\\\"\\(Q(.*)\\)\\\"/\\1/" > ${MPY_QSTR_DEFS_PREPROCESSED}
|
||||
DEPENDS ${MPY_PY_QSTRDEFS} ${MPY_QSTR_DEFS} ${MPY_QSTR_DEFS_COLLECTED}
|
||||
VERBATIM
|
||||
)
|
||||
|
||||
add_custom_command(
|
||||
OUTPUT ${MPY_QSTR_DEFS_GENERATED}
|
||||
COMMAND python3 ${MPY_PY_DIR}/makeqstrdata.py ${MPY_QSTR_DEFS_PREPROCESSED} > ${MPY_QSTR_DEFS_GENERATED}
|
||||
DEPENDS ${MPY_QSTR_DEFS_PREPROCESSED}
|
||||
VERBATIM
|
||||
)
|
||||
|
||||
# Build frozen code
|
||||
|
||||
target_compile_options(${MICROPYTHON_TARGET} PUBLIC
|
||||
-DMICROPY_QSTR_EXTRA_POOL=mp_qstr_frozen_const_pool
|
||||
-DMICROPY_MODULE_FROZEN_MPY=\(1\)
|
||||
)
|
||||
|
||||
add_custom_command(
|
||||
OUTPUT ${MPY_FROZEN_CONTENT}
|
||||
COMMAND python3 ${MPY_DIR}/tools/makemanifest.py -o ${MPY_FROZEN_CONTENT} -v "MPY_DIR=${MPY_DIR}" -v "PORT_DIR=${PROJECT_SOURCE_DIR}" -b "${CMAKE_BINARY_DIR}" -f${MPY_CROSS_FLAGS} ${FROZEN_MANIFEST}
|
||||
DEPENDS FORCE_BUILD
|
||||
${MPY_QSTR_DEFS_GENERATED}
|
||||
VERBATIM
|
||||
)
|
101
ports/rp2/modmachine.c
Normal file
101
ports/rp2/modmachine.c
Normal file
@ -0,0 +1,101 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "py/mphal.h"
|
||||
#include "extmod/machine_i2c.h"
|
||||
#include "extmod/machine_mem.h"
|
||||
#include "extmod/machine_spi.h"
|
||||
|
||||
#include "modmachine.h"
|
||||
#include "hardware/clocks.h"
|
||||
#include "hardware/watchdog.h"
|
||||
#include "pico/bootrom.h"
|
||||
|
||||
#define RP2_RESET_PWRON (1)
|
||||
#define RP2_RESET_WDT (3)
|
||||
|
||||
STATIC mp_obj_t machine_reset(void) {
|
||||
watchdog_reboot(0, SRAM_END, 0);
|
||||
for (;;) {
|
||||
__wfi();
|
||||
}
|
||||
return mp_const_none;
|
||||
}
|
||||
MP_DEFINE_CONST_FUN_OBJ_0(machine_reset_obj, machine_reset);
|
||||
|
||||
STATIC mp_obj_t machine_reset_cause(void) {
|
||||
int reset_cause;
|
||||
if (watchdog_caused_reboot()) {
|
||||
reset_cause = RP2_RESET_WDT;
|
||||
} else {
|
||||
reset_cause = RP2_RESET_PWRON;
|
||||
}
|
||||
return MP_OBJ_NEW_SMALL_INT(reset_cause);
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_0(machine_reset_cause_obj, machine_reset_cause);
|
||||
|
||||
STATIC mp_obj_t machine_bootloader(void) {
|
||||
reset_usb_boot(0, 0);
|
||||
return mp_const_none;
|
||||
}
|
||||
MP_DEFINE_CONST_FUN_OBJ_0(machine_bootloader_obj, machine_bootloader);
|
||||
|
||||
STATIC mp_obj_t machine_freq(void) {
|
||||
return MP_OBJ_NEW_SMALL_INT(clock_get_hz(clk_sys));
|
||||
}
|
||||
MP_DEFINE_CONST_FUN_OBJ_0(machine_freq_obj, machine_freq);
|
||||
|
||||
STATIC const mp_rom_map_elem_t machine_module_globals_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_umachine) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_reset), MP_ROM_PTR(&machine_reset_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_reset_cause), MP_ROM_PTR(&machine_reset_cause_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_bootloader), MP_ROM_PTR(&machine_bootloader_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_freq), MP_ROM_PTR(&machine_freq_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_mem8), MP_ROM_PTR(&machine_mem8_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_mem16), MP_ROM_PTR(&machine_mem16_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_mem32), MP_ROM_PTR(&machine_mem32_obj) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_ADC), MP_ROM_PTR(&machine_adc_type) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_I2C), MP_ROM_PTR(&machine_hw_i2c_type) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_SoftI2C), MP_ROM_PTR(&mp_machine_soft_i2c_type) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_Pin), MP_ROM_PTR(&machine_pin_type) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_PWM), MP_ROM_PTR(&machine_pwm_type) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_SPI), MP_ROM_PTR(&machine_spi_type) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_SoftSPI), MP_ROM_PTR(&mp_machine_soft_spi_type) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_Timer), MP_ROM_PTR(&machine_timer_type) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_UART), MP_ROM_PTR(&machine_uart_type) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_WDT), MP_ROM_PTR(&machine_wdt_type) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_PWRON_RESET), MP_ROM_INT(RP2_RESET_PWRON) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_WDT_RESET), MP_ROM_INT(RP2_RESET_WDT) },
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(machine_module_globals, machine_module_globals_table);
|
||||
|
||||
const mp_obj_module_t mp_module_machine = {
|
||||
.base = { &mp_type_module },
|
||||
.globals = (mp_obj_dict_t *)&machine_module_globals,
|
||||
};
|
18
ports/rp2/modmachine.h
Normal file
18
ports/rp2/modmachine.h
Normal file
@ -0,0 +1,18 @@
|
||||
#ifndef MICROPY_INCLUDED_RP2_MODMACHINE_H
|
||||
#define MICROPY_INCLUDED_RP2_MODMACHINE_H
|
||||
|
||||
#include "py/obj.h"
|
||||
|
||||
extern const mp_obj_type_t machine_adc_type;
|
||||
extern const mp_obj_type_t machine_hw_i2c_type;
|
||||
extern const mp_obj_type_t machine_pin_type;
|
||||
extern const mp_obj_type_t machine_pwm_type;
|
||||
extern const mp_obj_type_t machine_spi_type;
|
||||
extern const mp_obj_type_t machine_timer_type;
|
||||
extern const mp_obj_type_t machine_uart_type;
|
||||
extern const mp_obj_type_t machine_wdt_type;
|
||||
|
||||
void machine_pin_init(void);
|
||||
void machine_pin_deinit(void);
|
||||
|
||||
#endif // MICROPY_INCLUDED_RP2_MODMACHINE_H
|
41
ports/rp2/modrp2.c
Normal file
41
ports/rp2/modrp2.c
Normal file
@ -0,0 +1,41 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "modrp2.h"
|
||||
|
||||
STATIC const mp_rom_map_elem_t rp2_module_globals_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_rp2) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_Flash), MP_ROM_PTR(&rp2_flash_type) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_PIO), MP_ROM_PTR(&rp2_pio_type) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_StateMachine), MP_ROM_PTR(&rp2_state_machine_type) },
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(rp2_module_globals, rp2_module_globals_table);
|
||||
|
||||
const mp_obj_module_t mp_module_rp2 = {
|
||||
.base = { &mp_type_module },
|
||||
.globals = (mp_obj_dict_t *)&rp2_module_globals,
|
||||
};
|
38
ports/rp2/modrp2.h
Normal file
38
ports/rp2/modrp2.h
Normal file
@ -0,0 +1,38 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
#ifndef MICROPY_INCLUDED_RP2_MODRP2_H
|
||||
#define MICROPY_INCLUDED_RP2_MODRP2_H
|
||||
|
||||
#include "py/obj.h"
|
||||
|
||||
extern const mp_obj_type_t rp2_flash_type;
|
||||
extern const mp_obj_type_t rp2_pio_type;
|
||||
extern const mp_obj_type_t rp2_state_machine_type;
|
||||
|
||||
void rp2_pio_init(void);
|
||||
void rp2_pio_deinit(void);
|
||||
|
||||
#endif // MICROPY_INCLUDED_RP2_MODRP2_H
|
15
ports/rp2/modules/_boot.py
Normal file
15
ports/rp2/modules/_boot.py
Normal file
@ -0,0 +1,15 @@
|
||||
import os
|
||||
import machine, rp2
|
||||
|
||||
|
||||
# Try to mount the filesystem, and format the flash if it doesn't exist.
|
||||
# Note: the flash requires the programming size to be aligned to 256 bytes.
|
||||
bdev = rp2.Flash()
|
||||
try:
|
||||
vfs = os.VfsLfs2(bdev, progsize=256)
|
||||
except:
|
||||
os.VfsLfs2.mkfs(bdev, progsize=256)
|
||||
vfs = os.VfsLfs2(bdev, progsize=256)
|
||||
os.mount(vfs, "/")
|
||||
|
||||
del os, bdev, vfs
|
294
ports/rp2/modules/rp2.py
Normal file
294
ports/rp2/modules/rp2.py
Normal file
@ -0,0 +1,294 @@
|
||||
# rp2 module: uses C code from _rp2, plus asm_pio decorator implemented in Python.
|
||||
# MIT license; Copyright (c) 2020-2021 Damien P. George
|
||||
|
||||
from _rp2 import *
|
||||
from micropython import const
|
||||
|
||||
_PROG_DATA = const(0)
|
||||
_PROG_OFFSET_PIO0 = const(1)
|
||||
_PROG_OFFSET_PIO1 = const(2)
|
||||
_PROG_EXECCTRL = const(3)
|
||||
_PROG_SHIFTCTRL = const(4)
|
||||
_PROG_OUT_PINS = const(5)
|
||||
_PROG_SET_PINS = const(6)
|
||||
_PROG_SIDESET_PINS = const(7)
|
||||
_PROG_MAX_FIELDS = const(8)
|
||||
|
||||
|
||||
class PIOASMError(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class PIOASMEmit:
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
out_init=None,
|
||||
set_init=None,
|
||||
sideset_init=None,
|
||||
in_shiftdir=0,
|
||||
out_shiftdir=0,
|
||||
autopush=False,
|
||||
autopull=False,
|
||||
push_thresh=32,
|
||||
pull_thresh=32
|
||||
):
|
||||
from array import array
|
||||
|
||||
self.labels = {}
|
||||
execctrl = 0
|
||||
shiftctrl = (
|
||||
(pull_thresh & 0x1F) << 25
|
||||
| (push_thresh & 0x1F) << 20
|
||||
| out_shiftdir << 19
|
||||
| in_shiftdir << 18
|
||||
| autopull << 17
|
||||
| autopush << 16
|
||||
)
|
||||
self.prog = [array("H"), -1, -1, execctrl, shiftctrl, out_init, set_init, sideset_init]
|
||||
|
||||
self.wrap_used = False
|
||||
|
||||
if sideset_init is None:
|
||||
self.sideset_count = 0
|
||||
elif isinstance(sideset_init, int):
|
||||
self.sideset_count = 1
|
||||
else:
|
||||
self.sideset_count = len(sideset_init)
|
||||
|
||||
def start_pass(self, pass_):
|
||||
if pass_ == 1:
|
||||
if not self.wrap_used and self.num_instr:
|
||||
self.wrap()
|
||||
self.delay_max = 31
|
||||
if self.sideset_count:
|
||||
self.sideset_opt = self.num_sideset != self.num_instr
|
||||
if self.sideset_opt:
|
||||
self.prog[_PROG_EXECCTRL] |= 1 << 30
|
||||
self.sideset_count += 1
|
||||
self.delay_max >>= self.sideset_count
|
||||
self.pass_ = pass_
|
||||
self.num_instr = 0
|
||||
self.num_sideset = 0
|
||||
|
||||
def __getitem__(self, key):
|
||||
return self.delay(key)
|
||||
|
||||
def delay(self, delay):
|
||||
if self.pass_ > 0:
|
||||
if delay > self.delay_max:
|
||||
raise PIOASMError("delay too large")
|
||||
self.prog[_PROG_DATA][-1] |= delay << 8
|
||||
return self
|
||||
|
||||
def side(self, value):
|
||||
self.num_sideset += 1
|
||||
if self.pass_ > 0:
|
||||
set_bit = 13 - self.sideset_count
|
||||
self.prog[_PROG_DATA][-1] |= self.sideset_opt << 12 | value << set_bit
|
||||
return self
|
||||
|
||||
def wrap_target(self):
|
||||
self.prog[_PROG_EXECCTRL] |= self.num_instr << 7
|
||||
|
||||
def wrap(self):
|
||||
assert self.num_instr
|
||||
self.prog[_PROG_EXECCTRL] |= (self.num_instr - 1) << 12
|
||||
self.wrap_used = True
|
||||
|
||||
def label(self, label):
|
||||
if self.pass_ == 0:
|
||||
if label in self.labels:
|
||||
raise PIOASMError("duplicate label {}".format(label))
|
||||
self.labels[label] = self.num_instr
|
||||
|
||||
def word(self, instr, label=None):
|
||||
self.num_instr += 1
|
||||
if self.pass_ > 0:
|
||||
if label is None:
|
||||
label = 0
|
||||
else:
|
||||
if not label in self.labels:
|
||||
raise PIOASMError("unknown label {}".format(label))
|
||||
label = self.labels[label]
|
||||
self.prog[_PROG_DATA].append(instr | label)
|
||||
return self
|
||||
|
||||
def nop(self):
|
||||
return self.word(0xA042)
|
||||
|
||||
def jmp(self, cond, label=None):
|
||||
if label is None:
|
||||
label = cond
|
||||
cond = 0 # always
|
||||
return self.word(0x0000 | cond << 5, label)
|
||||
|
||||
def wait(self, polarity, src, index):
|
||||
if src == 6:
|
||||
src = 1 # "pin"
|
||||
elif src != 0:
|
||||
src = 2 # "irq"
|
||||
return self.word(0x2000 | polarity << 7 | src << 5 | index)
|
||||
|
||||
def in_(self, src, data):
|
||||
if not 0 < data <= 32:
|
||||
raise PIOASMError("invalid bit count {}".format(data))
|
||||
return self.word(0x4000 | src << 5 | data & 0x1F)
|
||||
|
||||
def out(self, dest, data):
|
||||
if dest == 8:
|
||||
dest = 7 # exec
|
||||
if not 0 < data <= 32:
|
||||
raise PIOASMError("invalid bit count {}".format(data))
|
||||
return self.word(0x6000 | dest << 5 | data & 0x1F)
|
||||
|
||||
def push(self, value=0, value2=0):
|
||||
value |= value2
|
||||
if not value & 1:
|
||||
value |= 0x20 # block by default
|
||||
return self.word(0x8000 | (value & 0x60))
|
||||
|
||||
def pull(self, value=0, value2=0):
|
||||
value |= value2
|
||||
if not value & 1:
|
||||
value |= 0x20 # block by default
|
||||
return self.word(0x8080 | (value & 0x60))
|
||||
|
||||
def mov(self, dest, src):
|
||||
if dest == 8:
|
||||
dest = 4 # exec
|
||||
return self.word(0xA000 | dest << 5 | src)
|
||||
|
||||
def irq(self, mod, index=None):
|
||||
if index is None:
|
||||
index = mod
|
||||
mod = 0 # no modifiers
|
||||
return self.word(0xC000 | (mod & 0x60) | index)
|
||||
|
||||
def set(self, dest, data):
|
||||
return self.word(0xE000 | dest << 5 | data)
|
||||
|
||||
|
||||
_pio_funcs = {
|
||||
# source constants for wait
|
||||
"gpio": 0,
|
||||
# "pin": see below, translated to 1
|
||||
# "irq": see below function, translated to 2
|
||||
# source/dest constants for in_, out, mov, set
|
||||
"pins": 0,
|
||||
"x": 1,
|
||||
"y": 2,
|
||||
"null": 3,
|
||||
"pindirs": 4,
|
||||
"pc": 5,
|
||||
"status": 5,
|
||||
"isr": 6,
|
||||
"osr": 7,
|
||||
"exec": 8, # translated to 4 for mov, 7 for out
|
||||
# operation functions for mov's src
|
||||
"invert": lambda x: x | 0x08,
|
||||
"reverse": lambda x: x | 0x10,
|
||||
# jmp condition constants
|
||||
"not_x": 1,
|
||||
"x_dec": 2,
|
||||
"not_y": 3,
|
||||
"y_dec": 4,
|
||||
"x_not_y": 5,
|
||||
"pin": 6,
|
||||
"not_osre": 7,
|
||||
# constants for push, pull
|
||||
"noblock": 0x01,
|
||||
"block": 0x21,
|
||||
"iffull": 0x40,
|
||||
"ifempty": 0x40,
|
||||
# constants and modifiers for irq
|
||||
# "noblock": see above
|
||||
# "block": see above
|
||||
"clear": 0x40,
|
||||
"rel": lambda x: x | 0x10,
|
||||
# functions
|
||||
"wrap_target": None,
|
||||
"wrap": None,
|
||||
"label": None,
|
||||
"word": None,
|
||||
"nop": None,
|
||||
"jmp": None,
|
||||
"wait": None,
|
||||
"in_": None,
|
||||
"out": None,
|
||||
"push": None,
|
||||
"pull": None,
|
||||
"mov": None,
|
||||
"irq": None,
|
||||
"set": None,
|
||||
}
|
||||
|
||||
|
||||
def asm_pio(**kw):
|
||||
emit = PIOASMEmit(**kw)
|
||||
|
||||
def dec(f):
|
||||
nonlocal emit
|
||||
|
||||
gl = _pio_funcs
|
||||
gl["wrap_target"] = emit.wrap_target
|
||||
gl["wrap"] = emit.wrap
|
||||
gl["label"] = emit.label
|
||||
gl["word"] = emit.word
|
||||
gl["nop"] = emit.nop
|
||||
gl["jmp"] = emit.jmp
|
||||
gl["wait"] = emit.wait
|
||||
gl["in_"] = emit.in_
|
||||
gl["out"] = emit.out
|
||||
gl["push"] = emit.push
|
||||
gl["pull"] = emit.pull
|
||||
gl["mov"] = emit.mov
|
||||
gl["irq"] = emit.irq
|
||||
gl["set"] = emit.set
|
||||
|
||||
old_gl = f.__globals__.copy()
|
||||
f.__globals__.clear()
|
||||
f.__globals__.update(gl)
|
||||
|
||||
emit.start_pass(0)
|
||||
f()
|
||||
|
||||
emit.start_pass(1)
|
||||
f()
|
||||
|
||||
f.__globals__.clear()
|
||||
f.__globals__.update(old_gl)
|
||||
|
||||
return emit.prog
|
||||
|
||||
return dec
|
||||
|
||||
|
||||
# sideset_count is inclusive of enable bit
|
||||
def asm_pio_encode(instr, sideset_count):
|
||||
emit = PIOASMEmit()
|
||||
emit.delay_max = 31
|
||||
emit.sideset_count = sideset_count
|
||||
if emit.sideset_count:
|
||||
emit.delay_max >>= emit.sideset_count
|
||||
emit.pass_ = 1
|
||||
emit.num_instr = 0
|
||||
emit.num_sideset = 0
|
||||
|
||||
gl = _pio_funcs
|
||||
gl["nop"] = emit.nop
|
||||
# gl["jmp"] = emit.jmp currently not supported
|
||||
gl["wait"] = emit.wait
|
||||
gl["in_"] = emit.in_
|
||||
gl["out"] = emit.out
|
||||
gl["push"] = emit.push
|
||||
gl["pull"] = emit.pull
|
||||
gl["mov"] = emit.mov
|
||||
gl["irq"] = emit.irq
|
||||
gl["set"] = emit.set
|
||||
|
||||
exec(instr, gl)
|
||||
|
||||
if len(emit.prog[_PROG_DATA]) != 1:
|
||||
raise PIOASMError("expecting exactly 1 instruction")
|
||||
return emit.prog[_PROG_DATA][0]
|
103
ports/rp2/moduos.c
Normal file
103
ports/rp2/moduos.c
Normal file
@ -0,0 +1,103 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2016 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/objstr.h"
|
||||
#include "py/runtime.h"
|
||||
#include "extmod/vfs.h"
|
||||
#include "extmod/vfs_fat.h"
|
||||
#include "extmod/vfs_lfs.h"
|
||||
#include "genhdr/mpversion.h"
|
||||
|
||||
STATIC const qstr os_uname_info_fields[] = {
|
||||
MP_QSTR_sysname, MP_QSTR_nodename,
|
||||
MP_QSTR_release, MP_QSTR_version, MP_QSTR_machine
|
||||
};
|
||||
STATIC const MP_DEFINE_STR_OBJ(os_uname_info_sysname_obj, MICROPY_PY_SYS_PLATFORM);
|
||||
STATIC const MP_DEFINE_STR_OBJ(os_uname_info_nodename_obj, MICROPY_PY_SYS_PLATFORM);
|
||||
STATIC const MP_DEFINE_STR_OBJ(os_uname_info_release_obj, MICROPY_VERSION_STRING);
|
||||
STATIC const MP_DEFINE_STR_OBJ(os_uname_info_version_obj, MICROPY_GIT_TAG " on " MICROPY_BUILD_DATE " (" MICROPY_BUILD_TYPE ")");
|
||||
STATIC const MP_DEFINE_STR_OBJ(os_uname_info_machine_obj, MICROPY_HW_BOARD_NAME " with " MICROPY_HW_MCU_NAME);
|
||||
|
||||
STATIC MP_DEFINE_ATTRTUPLE(
|
||||
os_uname_info_obj,
|
||||
os_uname_info_fields,
|
||||
5,
|
||||
(mp_obj_t)&os_uname_info_sysname_obj,
|
||||
(mp_obj_t)&os_uname_info_nodename_obj,
|
||||
(mp_obj_t)&os_uname_info_release_obj,
|
||||
(mp_obj_t)&os_uname_info_version_obj,
|
||||
(mp_obj_t)&os_uname_info_machine_obj
|
||||
);
|
||||
|
||||
STATIC mp_obj_t os_uname(void) {
|
||||
return (mp_obj_t)&os_uname_info_obj;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_0(os_uname_obj, os_uname);
|
||||
|
||||
STATIC const mp_rom_map_elem_t os_module_globals_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_uos) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_uname), MP_ROM_PTR(&os_uname_obj) },
|
||||
|
||||
#if MICROPY_VFS
|
||||
{ MP_ROM_QSTR(MP_QSTR_chdir), MP_ROM_PTR(&mp_vfs_chdir_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_getcwd), MP_ROM_PTR(&mp_vfs_getcwd_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_listdir), MP_ROM_PTR(&mp_vfs_listdir_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_mkdir), MP_ROM_PTR(&mp_vfs_mkdir_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_remove), MP_ROM_PTR(&mp_vfs_remove_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_rename), MP_ROM_PTR(&mp_vfs_rename_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_rmdir), MP_ROM_PTR(&mp_vfs_rmdir_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_stat), MP_ROM_PTR(&mp_vfs_stat_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_statvfs), MP_ROM_PTR(&mp_vfs_statvfs_obj) },
|
||||
#endif
|
||||
|
||||
// The following are MicroPython extensions.
|
||||
|
||||
#if MICROPY_PY_OS_DUPTERM
|
||||
{ MP_ROM_QSTR(MP_QSTR_dupterm), MP_ROM_PTR(&mp_uos_dupterm_obj) },
|
||||
#endif
|
||||
|
||||
#if MICROPY_VFS
|
||||
{ MP_ROM_QSTR(MP_QSTR_ilistdir), MP_ROM_PTR(&mp_vfs_ilistdir_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_mount), MP_ROM_PTR(&mp_vfs_mount_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_umount), MP_ROM_PTR(&mp_vfs_umount_obj) },
|
||||
#if MICROPY_VFS_FAT
|
||||
{ MP_ROM_QSTR(MP_QSTR_VfsFat), MP_ROM_PTR(&mp_fat_vfs_type) },
|
||||
#endif
|
||||
#if MICROPY_VFS_LFS1
|
||||
{ MP_ROM_QSTR(MP_QSTR_VfsLfs1), MP_ROM_PTR(&mp_type_vfs_lfs1) },
|
||||
#endif
|
||||
#if MICROPY_VFS_LFS2
|
||||
{ MP_ROM_QSTR(MP_QSTR_VfsLfs2), MP_ROM_PTR(&mp_type_vfs_lfs2) },
|
||||
#endif
|
||||
#endif
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(os_module_globals, os_module_globals_table);
|
||||
|
||||
const mp_obj_module_t mp_module_uos = {
|
||||
.base = { &mp_type_module },
|
||||
.globals = (mp_obj_dict_t *)&os_module_globals,
|
||||
};
|
127
ports/rp2/modutime.c
Normal file
127
ports/rp2/modutime.c
Normal file
@ -0,0 +1,127 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2013-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "lib/timeutils/timeutils.h"
|
||||
#include "extmod/utime_mphal.h"
|
||||
#include "hardware/rtc.h"
|
||||
|
||||
// localtime([secs])
|
||||
// Convert a time expressed in seconds since the Epoch into an 8-tuple which
|
||||
// contains: (year, month, mday, hour, minute, second, weekday, yearday)
|
||||
// If secs is not provided or None, then the current time from is used.
|
||||
STATIC mp_obj_t time_localtime(size_t n_args, const mp_obj_t *args) {
|
||||
if (n_args == 0 || args[0] == mp_const_none) {
|
||||
// Get current date and time.
|
||||
datetime_t t;
|
||||
rtc_get_datetime(&t);
|
||||
mp_obj_t tuple[8] = {
|
||||
mp_obj_new_int(t.year),
|
||||
mp_obj_new_int(t.month),
|
||||
mp_obj_new_int(t.day),
|
||||
mp_obj_new_int(t.hour),
|
||||
mp_obj_new_int(t.min),
|
||||
mp_obj_new_int(t.sec),
|
||||
mp_obj_new_int((t.dotw + 6) % 7), // convert 0=Sunday to 6=Sunday
|
||||
mp_obj_new_int(timeutils_year_day(t.year, t.month, t.day)),
|
||||
};
|
||||
return mp_obj_new_tuple(8, tuple);
|
||||
} else {
|
||||
// Convert given seconds to tuple.
|
||||
mp_int_t seconds = mp_obj_get_int(args[0]);
|
||||
timeutils_struct_time_t tm;
|
||||
timeutils_seconds_since_epoch_to_struct_time(seconds, &tm);
|
||||
mp_obj_t tuple[8] = {
|
||||
tuple[0] = mp_obj_new_int(tm.tm_year),
|
||||
tuple[1] = mp_obj_new_int(tm.tm_mon),
|
||||
tuple[2] = mp_obj_new_int(tm.tm_mday),
|
||||
tuple[3] = mp_obj_new_int(tm.tm_hour),
|
||||
tuple[4] = mp_obj_new_int(tm.tm_min),
|
||||
tuple[5] = mp_obj_new_int(tm.tm_sec),
|
||||
tuple[6] = mp_obj_new_int(tm.tm_wday),
|
||||
tuple[7] = mp_obj_new_int(tm.tm_yday),
|
||||
};
|
||||
return mp_obj_new_tuple(8, tuple);
|
||||
}
|
||||
}
|
||||
MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(time_localtime_obj, 0, 1, time_localtime);
|
||||
|
||||
// mktime()
|
||||
// This is inverse function of localtime. It's argument is a full 8-tuple
|
||||
// which expresses a time as per localtime. It returns an integer which is
|
||||
// the number of seconds since the Epoch.
|
||||
STATIC mp_obj_t time_mktime(mp_obj_t tuple) {
|
||||
size_t len;
|
||||
mp_obj_t *elem;
|
||||
mp_obj_get_array(tuple, &len, &elem);
|
||||
|
||||
// localtime generates a tuple of len 8. CPython uses 9, so we accept both.
|
||||
if (len < 8 || len > 9) {
|
||||
mp_raise_TypeError(MP_ERROR_TEXT("mktime needs a tuple of length 8 or 9"));
|
||||
}
|
||||
|
||||
return mp_obj_new_int_from_uint(timeutils_mktime(mp_obj_get_int(elem[0]),
|
||||
mp_obj_get_int(elem[1]), mp_obj_get_int(elem[2]), mp_obj_get_int(elem[3]),
|
||||
mp_obj_get_int(elem[4]), mp_obj_get_int(elem[5])));
|
||||
}
|
||||
MP_DEFINE_CONST_FUN_OBJ_1(time_mktime_obj, time_mktime);
|
||||
|
||||
// time()
|
||||
// Return the number of seconds since the Epoch.
|
||||
STATIC mp_obj_t time_time(void) {
|
||||
datetime_t t;
|
||||
rtc_get_datetime(&t);
|
||||
return mp_obj_new_int_from_ull(timeutils_seconds_since_epoch(t.year, t.month, t.day, t.hour, t.min, t.sec));
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_0(time_time_obj, time_time);
|
||||
|
||||
STATIC const mp_rom_map_elem_t mp_module_time_globals_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_utime) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_gmtime), MP_ROM_PTR(&time_localtime_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_localtime), MP_ROM_PTR(&time_localtime_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_mktime), MP_ROM_PTR(&time_mktime_obj) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_time), MP_ROM_PTR(&time_time_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_time_ns), MP_ROM_PTR(&mp_utime_time_ns_obj) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_sleep), MP_ROM_PTR(&mp_utime_sleep_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_sleep_ms), MP_ROM_PTR(&mp_utime_sleep_ms_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_sleep_us), MP_ROM_PTR(&mp_utime_sleep_us_obj) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_ticks_ms), MP_ROM_PTR(&mp_utime_ticks_ms_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_ticks_us), MP_ROM_PTR(&mp_utime_ticks_us_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_ticks_cpu), MP_ROM_PTR(&mp_utime_ticks_cpu_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_ticks_add), MP_ROM_PTR(&mp_utime_ticks_add_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_ticks_diff), MP_ROM_PTR(&mp_utime_ticks_diff_obj) },
|
||||
};
|
||||
|
||||
STATIC MP_DEFINE_CONST_DICT(mp_module_time_globals, mp_module_time_globals_table);
|
||||
|
||||
const mp_obj_module_t mp_module_utime = {
|
||||
.base = { &mp_type_module },
|
||||
.globals = (mp_obj_dict_t *)&mp_module_time_globals,
|
||||
};
|
196
ports/rp2/mpconfigport.h
Normal file
196
ports/rp2/mpconfigport.h
Normal file
@ -0,0 +1,196 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
// Options controlling how MicroPython is built, overriding defaults in py/mpconfig.h
|
||||
|
||||
#include <stdint.h>
|
||||
#include "hardware/spi.h"
|
||||
#include "hardware/sync.h"
|
||||
#include "pico/binary_info.h"
|
||||
|
||||
// Board and hardware specific configuration
|
||||
#define MICROPY_HW_BOARD_NAME "Raspberry Pi Pico"
|
||||
#define MICROPY_HW_MCU_NAME "RP2040"
|
||||
#define MICROPY_HW_ENABLE_UART_REPL (0) // useful if there is no USB
|
||||
#define MICROPY_HW_ENABLE_USBDEV (1)
|
||||
|
||||
// Memory allocation policies
|
||||
#define MICROPY_GC_STACK_ENTRY_TYPE uint16_t
|
||||
#define MICROPY_ALLOC_PATH_MAX (128)
|
||||
#define MICROPY_QSTR_BYTES_IN_HASH (1)
|
||||
|
||||
// MicroPython emitters
|
||||
#define MICROPY_PERSISTENT_CODE_LOAD (1)
|
||||
#define MICROPY_EMIT_THUMB (1)
|
||||
#define MICROPY_EMIT_THUMB_ARMV7M (0)
|
||||
#define MICROPY_EMIT_INLINE_THUMB (1)
|
||||
#define MICROPY_EMIT_INLINE_THUMB_FLOAT (0)
|
||||
#define MICROPY_EMIT_INLINE_THUMB_ARMV7M (0)
|
||||
|
||||
// Python internal features
|
||||
#define MICROPY_READER_VFS (1)
|
||||
#define MICROPY_ENABLE_GC (1)
|
||||
#define MICROPY_ENABLE_FINALISER (1)
|
||||
#define MICROPY_STACK_CHECK (1)
|
||||
#define MICROPY_ENABLE_EMERGENCY_EXCEPTION_BUF (1)
|
||||
#define MICROPY_KBD_EXCEPTION (1)
|
||||
#define MICROPY_HELPER_REPL (1)
|
||||
#define MICROPY_REPL_AUTO_INDENT (1)
|
||||
#define MICROPY_LONGINT_IMPL (MICROPY_LONGINT_IMPL_MPZ)
|
||||
#define MICROPY_ENABLE_SOURCE_LINE (1)
|
||||
#define MICROPY_FLOAT_IMPL (MICROPY_FLOAT_IMPL_FLOAT)
|
||||
#define MICROPY_MODULE_BUILTIN_INIT (1)
|
||||
#define MICROPY_MODULE_WEAK_LINKS (1)
|
||||
#define MICROPY_CAN_OVERRIDE_BUILTINS (1)
|
||||
#define MICROPY_ENABLE_SCHEDULER (1)
|
||||
#define MICROPY_SCHEDULER_DEPTH (8)
|
||||
|
||||
// Fine control over Python builtins, classes, modules, etc
|
||||
#define MICROPY_PY_FUNCTION_ATTRS (1)
|
||||
#define MICROPY_PY_BUILTINS_STR_UNICODE (1)
|
||||
#define MICROPY_PY_BUILTINS_MEMORYVIEW (1)
|
||||
#define MICROPY_PY_BUILTINS_ROUND_INT (1)
|
||||
#define MICROPY_PY_ALL_SPECIAL_METHODS (1)
|
||||
#define MICROPY_PY_BUILTINS_INPUT (1)
|
||||
#define MICROPY_PY_BUILTINS_HELP (1)
|
||||
#define MICROPY_PY_BUILTINS_HELP_TEXT rp2_help_text
|
||||
#define MICROPY_PY_BUILTINS_HELP_MODULES (1)
|
||||
#define MICROPY_PY_ARRAY_SLICE_ASSIGN (1)
|
||||
#define MICROPY_PY___FILE__ (0)
|
||||
#define MICROPY_PY_MICROPYTHON_MEM_INFO (1)
|
||||
#define MICROPY_PY_IO_IOBASE (1)
|
||||
#define MICROPY_PY_IO_FILEIO (1)
|
||||
#define MICROPY_PY_SYS_MAXSIZE (1)
|
||||
#define MICROPY_PY_SYS_STDFILES (1)
|
||||
#define MICROPY_PY_SYS_STDIO_BUFFER (1)
|
||||
#define MICROPY_PY_SYS_PLATFORM "rp2"
|
||||
#define MICROPY_PY_THREAD (1)
|
||||
#define MICROPY_PY_THREAD_GIL (0)
|
||||
|
||||
// Extended modules
|
||||
#define MICROPY_EPOCH_IS_1970 (1)
|
||||
#define MICROPY_PY_UASYNCIO (1)
|
||||
#define MICROPY_PY_UCTYPES (1)
|
||||
#define MICROPY_PY_UZLIB (1)
|
||||
#define MICROPY_PY_UJSON (1)
|
||||
#define MICROPY_PY_URE (1)
|
||||
#define MICROPY_PY_URE_MATCH_GROUPS (1)
|
||||
#define MICROPY_PY_URE_MATCH_SPAN_START_END (1)
|
||||
#define MICROPY_PY_URE_SUB (1)
|
||||
#define MICROPY_PY_UHASHLIB (1)
|
||||
#define MICROPY_PY_UBINASCII (1)
|
||||
#define MICROPY_PY_UTIME_MP_HAL (1)
|
||||
#define MICROPY_PY_URANDOM (1)
|
||||
#define MICROPY_PY_URANDOM_EXTRA_FUNCS (1)
|
||||
#define MICROPY_PY_URANDOM_SEED_INIT_FUNC (rosc_random_u32())
|
||||
#define MICROPY_PY_USELECT (1)
|
||||
#define MICROPY_PY_MACHINE (1)
|
||||
#define MICROPY_PY_MACHINE_PIN_MAKE_NEW mp_pin_make_new
|
||||
#define MICROPY_PY_MACHINE_I2C (1)
|
||||
#define MICROPY_PY_MACHINE_SPI (1)
|
||||
#define MICROPY_PY_MACHINE_SPI_MSB (SPI_MSB_FIRST)
|
||||
#define MICROPY_PY_MACHINE_SPI_LSB (SPI_LSB_FIRST)
|
||||
#define MICROPY_PY_FRAMEBUF (1)
|
||||
#define MICROPY_VFS (1)
|
||||
#define MICROPY_VFS_LFS2 (1)
|
||||
|
||||
// Use VfsLfs2's types for fileio/textio
|
||||
#define mp_type_fileio mp_type_vfs_lfs2_fileio
|
||||
#define mp_type_textio mp_type_vfs_lfs2_textio
|
||||
|
||||
// Use VFS's functions for import stat and builtin open
|
||||
#define mp_import_stat mp_vfs_import_stat
|
||||
#define mp_builtin_open_obj mp_vfs_open_obj
|
||||
|
||||
// Hooks to add builtins
|
||||
|
||||
#define MICROPY_PORT_BUILTINS \
|
||||
{ MP_ROM_QSTR(MP_QSTR_open), MP_ROM_PTR(&mp_builtin_open_obj) },
|
||||
|
||||
extern const struct _mp_obj_module_t mp_module_machine;
|
||||
extern const struct _mp_obj_module_t mp_module_onewire;
|
||||
extern const struct _mp_obj_module_t mp_module_rp2;
|
||||
extern const struct _mp_obj_module_t mp_module_uos;
|
||||
extern const struct _mp_obj_module_t mp_module_utime;
|
||||
|
||||
#define MICROPY_PORT_BUILTIN_MODULES \
|
||||
{ MP_OBJ_NEW_QSTR(MP_QSTR_machine), (mp_obj_t)&mp_module_machine }, \
|
||||
{ MP_OBJ_NEW_QSTR(MP_QSTR__onewire), (mp_obj_t)&mp_module_onewire }, \
|
||||
{ MP_OBJ_NEW_QSTR(MP_QSTR__rp2), (mp_obj_t)&mp_module_rp2 }, \
|
||||
{ MP_ROM_QSTR(MP_QSTR_uos), MP_ROM_PTR(&mp_module_uos) }, \
|
||||
{ MP_ROM_QSTR(MP_QSTR_utime), MP_ROM_PTR(&mp_module_utime) }, \
|
||||
|
||||
#define MICROPY_PORT_ROOT_POINTERS \
|
||||
const char *readline_hist[8]; \
|
||||
void *machine_pin_irq_obj[30]; \
|
||||
void *rp2_pio_irq_obj[2]; \
|
||||
void *rp2_state_machine_irq_obj[8]; \
|
||||
|
||||
#define MP_STATE_PORT MP_STATE_VM
|
||||
|
||||
// Miscellaneous settings
|
||||
|
||||
// TODO need to look and see if these could/should be spinlock/mutex
|
||||
#define MICROPY_BEGIN_ATOMIC_SECTION() save_and_disable_interrupts()
|
||||
#define MICROPY_END_ATOMIC_SECTION(state) restore_interrupts(state)
|
||||
|
||||
#if MICROPY_HW_ENABLE_USBDEV
|
||||
#define MICROPY_HW_USBDEV_TASK_HOOK extern void tud_task(void); tud_task();
|
||||
#define MICROPY_VM_HOOK_COUNT (10)
|
||||
#define MICROPY_VM_HOOK_INIT static uint vm_hook_divisor = MICROPY_VM_HOOK_COUNT;
|
||||
#define MICROPY_VM_HOOK_POLL if (--vm_hook_divisor == 0) { \
|
||||
vm_hook_divisor = MICROPY_VM_HOOK_COUNT; \
|
||||
MICROPY_HW_USBDEV_TASK_HOOK \
|
||||
}
|
||||
#define MICROPY_VM_HOOK_LOOP MICROPY_VM_HOOK_POLL
|
||||
#define MICROPY_VM_HOOK_RETURN MICROPY_VM_HOOK_POLL
|
||||
#else
|
||||
#define MICROPY_HW_USBDEV_TASK_HOOK
|
||||
#endif
|
||||
|
||||
#define MICROPY_EVENT_POLL_HOOK \
|
||||
do { \
|
||||
extern void mp_handle_pending(bool); \
|
||||
mp_handle_pending(true); \
|
||||
best_effort_wfe_or_timeout(make_timeout_time_ms(1)); \
|
||||
MICROPY_HW_USBDEV_TASK_HOOK \
|
||||
} while (0);
|
||||
|
||||
#define MICROPY_MAKE_POINTER_CALLABLE(p) ((void *)((mp_uint_t)(p) | 1))
|
||||
|
||||
#define MP_SSIZE_MAX (0x7fffffff)
|
||||
typedef intptr_t mp_int_t; // must be pointer size
|
||||
typedef uintptr_t mp_uint_t; // must be pointer size
|
||||
typedef intptr_t mp_off_t;
|
||||
|
||||
// We need to provide a declaration/definition of alloca()
|
||||
#include <alloca.h>
|
||||
|
||||
#define BINARY_INFO_TAG_MICROPYTHON BINARY_INFO_MAKE_TAG('M', 'P')
|
||||
#define BINARY_INFO_ID_MP_FROZEN 0x4a99d719
|
||||
#define MICROPY_FROZEN_LIST_ITEM(name, file) bi_decl(bi_string(BINARY_INFO_TAG_MICROPYTHON, BINARY_INFO_ID_MP_FROZEN, name))
|
||||
|
||||
extern uint32_t rosc_random_u32(void);
|
142
ports/rp2/mphalport.c
Normal file
142
ports/rp2/mphalport.c
Normal file
@ -0,0 +1,142 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "py/stream.h"
|
||||
#include "py/mphal.h"
|
||||
#include "lib/timeutils/timeutils.h"
|
||||
#include "tusb.h"
|
||||
#include "uart.h"
|
||||
#include "hardware/rtc.h"
|
||||
|
||||
#if MICROPY_HW_ENABLE_UART_REPL
|
||||
|
||||
#ifndef UART_BUFFER_LEN
|
||||
// reasonably big so we can paste
|
||||
#define UART_BUFFER_LEN 256
|
||||
#endif
|
||||
|
||||
STATIC uint8_t stdin_ringbuf_array[UART_BUFFER_LEN];
|
||||
ringbuf_t stdin_ringbuf = { stdin_ringbuf_array, sizeof(stdin_ringbuf_array) };
|
||||
|
||||
#endif
|
||||
|
||||
#if MICROPY_KBD_EXCEPTION
|
||||
|
||||
int mp_interrupt_char = -1;
|
||||
|
||||
void tud_cdc_rx_wanted_cb(uint8_t itf, char wanted_char) {
|
||||
(void)itf;
|
||||
(void)wanted_char;
|
||||
tud_cdc_read_char(); // discard interrupt char
|
||||
mp_keyboard_interrupt();
|
||||
}
|
||||
|
||||
void mp_hal_set_interrupt_char(int c) {
|
||||
mp_interrupt_char = c;
|
||||
tud_cdc_set_wanted_char(c);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
uintptr_t mp_hal_stdio_poll(uintptr_t poll_flags) {
|
||||
uintptr_t ret = 0;
|
||||
#if MICROPY_HW_ENABLE_UART_REPL
|
||||
if ((poll_flags & MP_STREAM_POLL_RD) && ringbuf_peek(&stdin_ringbuf) != -1) {
|
||||
ret |= MP_STREAM_POLL_RD;
|
||||
}
|
||||
#endif
|
||||
#if MICROPY_HW_ENABLE_USBDEV
|
||||
if (tud_cdc_connected() && tud_cdc_available()) {
|
||||
ret |= MP_STREAM_POLL_RD;
|
||||
}
|
||||
#endif
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Receive single character
|
||||
int mp_hal_stdin_rx_chr(void) {
|
||||
for (;;) {
|
||||
#if MICROPY_HW_ENABLE_UART_REPL
|
||||
int c = ringbuf_get(&stdin_ringbuf);
|
||||
if (c != -1) {
|
||||
return c;
|
||||
}
|
||||
#endif
|
||||
#if MICROPY_HW_ENABLE_USBDEV
|
||||
if (tud_cdc_connected() && tud_cdc_available()) {
|
||||
uint8_t buf[1];
|
||||
uint32_t count = tud_cdc_read(buf, sizeof(buf));
|
||||
if (count) {
|
||||
return buf[0];
|
||||
}
|
||||
}
|
||||
#endif
|
||||
MICROPY_EVENT_POLL_HOOK
|
||||
}
|
||||
}
|
||||
|
||||
// Send string of given length
|
||||
void mp_hal_stdout_tx_strn(const char *str, mp_uint_t len) {
|
||||
#if MICROPY_HW_ENABLE_UART_REPL
|
||||
mp_uart_write_strn(str, len);
|
||||
#endif
|
||||
|
||||
#if MICROPY_HW_ENABLE_USBDEV
|
||||
if (tud_cdc_connected()) {
|
||||
for (size_t i = 0; i < len;) {
|
||||
uint32_t n = len - i;
|
||||
if (n > CFG_TUD_CDC_EP_BUFSIZE) {
|
||||
n = CFG_TUD_CDC_EP_BUFSIZE;
|
||||
}
|
||||
while (n > tud_cdc_write_available()) {
|
||||
tud_task();
|
||||
tud_cdc_write_flush();
|
||||
}
|
||||
uint32_t n2 = tud_cdc_write(str + i, n);
|
||||
tud_task();
|
||||
tud_cdc_write_flush();
|
||||
i += n2;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void mp_hal_delay_ms(mp_uint_t ms) {
|
||||
absolute_time_t t = make_timeout_time_ms(ms);
|
||||
while (!time_reached(t)) {
|
||||
mp_handle_pending(true);
|
||||
best_effort_wfe_or_timeout(t);
|
||||
MICROPY_HW_USBDEV_TASK_HOOK
|
||||
}
|
||||
}
|
||||
|
||||
uint64_t mp_hal_time_ns(void) {
|
||||
datetime_t t;
|
||||
rtc_get_datetime(&t);
|
||||
uint64_t s = timeutils_seconds_since_epoch(t.year, t.month, t.day, t.hour, t.min, t.sec);
|
||||
return s * 1000000000ULL;
|
||||
}
|
113
ports/rp2/mphalport.h
Normal file
113
ports/rp2/mphalport.h
Normal file
@ -0,0 +1,113 @@
|
||||
/*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*
|
||||
*/
|
||||
#ifndef MICROPY_INCLUDED_RP2_MPHALPORT_H
|
||||
#define MICROPY_INCLUDED_RP2_MPHALPORT_H
|
||||
|
||||
#include "py/mpconfig.h"
|
||||
#include "py/ringbuf.h"
|
||||
#include "pico/time.h"
|
||||
|
||||
extern int mp_interrupt_char;
|
||||
extern ringbuf_t stdin_ringbuf;
|
||||
|
||||
void mp_hal_set_interrupt_char(int c);
|
||||
|
||||
static inline void mp_hal_delay_us(mp_uint_t us) {
|
||||
sleep_us(us);
|
||||
}
|
||||
|
||||
static inline void mp_hal_delay_us_fast(mp_uint_t us) {
|
||||
busy_wait_us(us);
|
||||
}
|
||||
|
||||
#define mp_hal_quiet_timing_enter() MICROPY_BEGIN_ATOMIC_SECTION()
|
||||
#define mp_hal_quiet_timing_exit(irq_state) MICROPY_END_ATOMIC_SECTION(irq_state)
|
||||
|
||||
static inline mp_uint_t mp_hal_ticks_us(void) {
|
||||
return time_us_32();
|
||||
}
|
||||
|
||||
static inline mp_uint_t mp_hal_ticks_ms(void) {
|
||||
return to_ms_since_boot(get_absolute_time());
|
||||
}
|
||||
|
||||
static inline mp_uint_t mp_hal_ticks_cpu(void) {
|
||||
// ticks_cpu() is defined as using the highest-resolution timing source
|
||||
// in the system. This is usually a CPU clock, but doesn't have to be.
|
||||
return time_us_32();
|
||||
}
|
||||
|
||||
// C-level pin HAL
|
||||
|
||||
#include "py/obj.h"
|
||||
#include "hardware/gpio.h"
|
||||
|
||||
#define MP_HAL_PIN_FMT "%u"
|
||||
#define mp_hal_pin_obj_t uint
|
||||
|
||||
extern uint32_t machine_pin_open_drain_mask;
|
||||
|
||||
mp_hal_pin_obj_t mp_hal_get_pin_obj(mp_obj_t pin_in);
|
||||
|
||||
static inline unsigned int mp_hal_pin_name(mp_hal_pin_obj_t pin) {
|
||||
return pin;
|
||||
}
|
||||
|
||||
static inline void mp_hal_pin_input(mp_hal_pin_obj_t pin) {
|
||||
gpio_set_function(pin, GPIO_FUNC_SIO);
|
||||
gpio_set_dir(pin, GPIO_IN);
|
||||
machine_pin_open_drain_mask &= ~(1 << pin);
|
||||
}
|
||||
|
||||
static inline void mp_hal_pin_output(mp_hal_pin_obj_t pin) {
|
||||
gpio_set_function(pin, GPIO_FUNC_SIO);
|
||||
gpio_set_dir(pin, GPIO_OUT);
|
||||
machine_pin_open_drain_mask &= ~(1 << pin);
|
||||
}
|
||||
|
||||
static inline void mp_hal_pin_open_drain(mp_hal_pin_obj_t pin) {
|
||||
gpio_set_function(pin, GPIO_FUNC_SIO);
|
||||
gpio_set_dir(pin, GPIO_IN);
|
||||
gpio_put(pin, 0);
|
||||
machine_pin_open_drain_mask |= 1 << pin;
|
||||
}
|
||||
|
||||
static inline int mp_hal_pin_read(mp_hal_pin_obj_t pin) {
|
||||
return gpio_get(pin);
|
||||
}
|
||||
|
||||
static inline void mp_hal_pin_write(mp_hal_pin_obj_t pin, int v) {
|
||||
gpio_put(pin, v);
|
||||
}
|
||||
|
||||
static inline void mp_hal_pin_od_low(mp_hal_pin_obj_t pin) {
|
||||
gpio_set_dir(pin, GPIO_OUT);
|
||||
}
|
||||
|
||||
static inline void mp_hal_pin_od_high(mp_hal_pin_obj_t pin) {
|
||||
gpio_set_dir(pin, GPIO_IN);
|
||||
}
|
||||
|
||||
#endif // MICROPY_INCLUDED_RP2_MPHALPORT_H
|
105
ports/rp2/mpthreadport.c
Normal file
105
ports/rp2/mpthreadport.c
Normal file
@ -0,0 +1,105 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "py/gc.h"
|
||||
#include "py/mpthread.h"
|
||||
#include "pico/stdlib.h"
|
||||
#include "pico/multicore.h"
|
||||
|
||||
#if MICROPY_PY_THREAD
|
||||
|
||||
extern uint8_t __StackTop, __StackBottom;
|
||||
|
||||
void *core_state[2];
|
||||
|
||||
STATIC void *(*core1_entry)(void *) = NULL;
|
||||
STATIC void *core1_arg = NULL;
|
||||
STATIC uint32_t *core1_stack = NULL;
|
||||
STATIC size_t core1_stack_num_words = 0;
|
||||
|
||||
void mp_thread_init(void) {
|
||||
mp_thread_set_state(&mp_state_ctx.thread);
|
||||
core1_entry = NULL;
|
||||
}
|
||||
|
||||
void mp_thread_gc_others(void) {
|
||||
if (get_core_num() == 0) {
|
||||
// GC running on core0, trace core1's stack, if it's running.
|
||||
if (core1_entry != NULL) {
|
||||
gc_collect_root((void **)core1_stack, core1_stack_num_words);
|
||||
}
|
||||
} else {
|
||||
// GC running on core1, trace core0's stack.
|
||||
gc_collect_root((void **)&__StackBottom, (&__StackTop - &__StackBottom) / sizeof(uintptr_t));
|
||||
}
|
||||
}
|
||||
|
||||
STATIC void core1_entry_wrapper(void) {
|
||||
if (core1_entry) {
|
||||
core1_entry(core1_arg);
|
||||
}
|
||||
core1_entry = NULL;
|
||||
// returning from here will loop the core forever (WFI)
|
||||
}
|
||||
|
||||
void mp_thread_create(void *(*entry)(void *), void *arg, size_t *stack_size) {
|
||||
// Check if core1 is already in use.
|
||||
if (core1_entry != NULL) {
|
||||
mp_raise_msg(&mp_type_OSError, MP_ERROR_TEXT("core1 in use"));
|
||||
}
|
||||
|
||||
core1_entry = entry;
|
||||
core1_arg = arg;
|
||||
|
||||
if (*stack_size == 0) {
|
||||
*stack_size = 4096; // default stack size
|
||||
} else if (*stack_size < 2048) {
|
||||
*stack_size = 2048; // minimum stack size
|
||||
}
|
||||
|
||||
// Round stack size to a multiple of the word size.
|
||||
core1_stack_num_words = *stack_size / sizeof(uint32_t);
|
||||
*stack_size = core1_stack_num_words * sizeof(uint32_t);
|
||||
|
||||
// Allocate stack.
|
||||
core1_stack = m_new(uint32_t, core1_stack_num_words);
|
||||
|
||||
// Create thread on core1.
|
||||
multicore_reset_core1();
|
||||
multicore_launch_core1_with_stack(core1_entry_wrapper, core1_stack, *stack_size);
|
||||
|
||||
// Adjust stack_size to provide room to recover from hitting the limit.
|
||||
*stack_size -= 512;
|
||||
}
|
||||
|
||||
void mp_thread_start(void) {
|
||||
}
|
||||
|
||||
void mp_thread_finish(void) {
|
||||
}
|
||||
|
||||
#endif // MICROPY_PY_THREAD
|
64
ports/rp2/mpthreadport.h
Normal file
64
ports/rp2/mpthreadport.h
Normal file
@ -0,0 +1,64 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
#ifndef MICROPY_INCLUDED_RP2_MPTHREADPORT_H
|
||||
#define MICROPY_INCLUDED_RP2_MPTHREADPORT_H
|
||||
|
||||
#include "py/mpthread.h"
|
||||
#include "pico/mutex.h"
|
||||
|
||||
typedef struct mutex mp_thread_mutex_t;
|
||||
|
||||
extern void *core_state[2];
|
||||
|
||||
void mp_thread_init(void);
|
||||
void mp_thread_gc_others(void);
|
||||
|
||||
static inline void mp_thread_set_state(struct _mp_state_thread_t *state) {
|
||||
core_state[get_core_num()] = state;
|
||||
}
|
||||
|
||||
static inline struct _mp_state_thread_t *mp_thread_get_state(void) {
|
||||
return core_state[get_core_num()];
|
||||
}
|
||||
|
||||
static inline void mp_thread_mutex_init(mp_thread_mutex_t *m) {
|
||||
mutex_init(m);
|
||||
}
|
||||
|
||||
static inline int mp_thread_mutex_lock(mp_thread_mutex_t *m, int wait) {
|
||||
if (wait) {
|
||||
mutex_enter_blocking(m);
|
||||
return 1;
|
||||
} else {
|
||||
return mutex_try_enter(m, NULL);
|
||||
}
|
||||
}
|
||||
|
||||
static inline void mp_thread_mutex_unlock(mp_thread_mutex_t *m) {
|
||||
mutex_exit(m);
|
||||
}
|
||||
|
||||
#endif // MICROPY_INCLUDED_RP2_MPTHREADPORT_H
|
3
ports/rp2/qstrdefsport.h
Normal file
3
ports/rp2/qstrdefsport.h
Normal file
@ -0,0 +1,3 @@
|
||||
// qstrs specific to this port
|
||||
// *FORMAT-OFF*
|
||||
Q(/lib)
|
146
ports/rp2/rp2_flash.c
Normal file
146
ports/rp2/rp2_flash.c
Normal file
@ -0,0 +1,146 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "extmod/vfs.h"
|
||||
#include "modrp2.h"
|
||||
#include "hardware/flash.h"
|
||||
#include "pico/binary_info.h"
|
||||
|
||||
#define BLOCK_SIZE_BYTES (FLASH_SECTOR_SIZE)
|
||||
|
||||
#ifndef MICROPY_HW_FLASH_STORAGE_BYTES
|
||||
#define MICROPY_HW_FLASH_STORAGE_BYTES (1408 * 1024)
|
||||
#endif
|
||||
|
||||
#ifndef MICROPY_HW_FLASH_STORAGE_BASE
|
||||
#define MICROPY_HW_FLASH_STORAGE_BASE (PICO_FLASH_SIZE_BYTES - MICROPY_HW_FLASH_STORAGE_BYTES)
|
||||
#endif
|
||||
|
||||
static_assert(MICROPY_HW_FLASH_STORAGE_BASE + MICROPY_HW_FLASH_STORAGE_BYTES <= PICO_FLASH_SIZE_BYTES, "MICROPY_HW_FLASH_STORAGE_BYTES too big");
|
||||
|
||||
typedef struct _rp2_flash_obj_t {
|
||||
mp_obj_base_t base;
|
||||
uint32_t flash_base;
|
||||
uint32_t flash_size;
|
||||
} rp2_flash_obj_t;
|
||||
|
||||
STATIC rp2_flash_obj_t rp2_flash_obj = {
|
||||
.base = { &rp2_flash_type },
|
||||
.flash_base = MICROPY_HW_FLASH_STORAGE_BASE,
|
||||
.flash_size = MICROPY_HW_FLASH_STORAGE_BYTES,
|
||||
};
|
||||
|
||||
// Tag the flash drive in the binary as readable/writable (but not reformatable)
|
||||
bi_decl(bi_block_device(
|
||||
BINARY_INFO_TAG_MICROPYTHON,
|
||||
"MicroPython",
|
||||
XIP_BASE + MICROPY_HW_FLASH_STORAGE_BASE,
|
||||
MICROPY_HW_FLASH_STORAGE_BYTES,
|
||||
NULL,
|
||||
BINARY_INFO_BLOCK_DEV_FLAG_READ |
|
||||
BINARY_INFO_BLOCK_DEV_FLAG_WRITE |
|
||||
BINARY_INFO_BLOCK_DEV_FLAG_PT_UNKNOWN));
|
||||
|
||||
STATIC mp_obj_t rp2_flash_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
|
||||
// Check args.
|
||||
mp_arg_check_num(n_args, n_kw, 0, 0, false);
|
||||
|
||||
// Return singleton object.
|
||||
return MP_OBJ_FROM_PTR(&rp2_flash_obj);
|
||||
}
|
||||
|
||||
STATIC mp_obj_t rp2_flash_readblocks(size_t n_args, const mp_obj_t *args) {
|
||||
rp2_flash_obj_t *self = MP_OBJ_TO_PTR(args[0]);
|
||||
uint32_t offset = mp_obj_get_int(args[1]) * BLOCK_SIZE_BYTES;
|
||||
mp_buffer_info_t bufinfo;
|
||||
mp_get_buffer_raise(args[2], &bufinfo, MP_BUFFER_WRITE);
|
||||
if (n_args == 4) {
|
||||
offset += mp_obj_get_int(args[3]);
|
||||
}
|
||||
memcpy(bufinfo.buf, (void *)(XIP_BASE + self->flash_base + offset), bufinfo.len);
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(rp2_flash_readblocks_obj, 3, 4, rp2_flash_readblocks);
|
||||
|
||||
STATIC mp_obj_t rp2_flash_writeblocks(size_t n_args, const mp_obj_t *args) {
|
||||
rp2_flash_obj_t *self = MP_OBJ_TO_PTR(args[0]);
|
||||
uint32_t offset = mp_obj_get_int(args[1]) * BLOCK_SIZE_BYTES;
|
||||
mp_buffer_info_t bufinfo;
|
||||
mp_get_buffer_raise(args[2], &bufinfo, MP_BUFFER_READ);
|
||||
if (n_args == 3) {
|
||||
flash_range_erase(self->flash_base + offset, bufinfo.len);
|
||||
// TODO check return value
|
||||
} else {
|
||||
offset += mp_obj_get_int(args[3]);
|
||||
}
|
||||
flash_range_program(self->flash_base + offset, bufinfo.buf, bufinfo.len);
|
||||
// TODO check return value
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(rp2_flash_writeblocks_obj, 3, 4, rp2_flash_writeblocks);
|
||||
|
||||
STATIC mp_obj_t rp2_flash_ioctl(mp_obj_t self_in, mp_obj_t cmd_in, mp_obj_t arg_in) {
|
||||
rp2_flash_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
mp_int_t cmd = mp_obj_get_int(cmd_in);
|
||||
switch (cmd) {
|
||||
case MP_BLOCKDEV_IOCTL_INIT:
|
||||
return MP_OBJ_NEW_SMALL_INT(0);
|
||||
case MP_BLOCKDEV_IOCTL_DEINIT:
|
||||
return MP_OBJ_NEW_SMALL_INT(0);
|
||||
case MP_BLOCKDEV_IOCTL_SYNC:
|
||||
return MP_OBJ_NEW_SMALL_INT(0);
|
||||
case MP_BLOCKDEV_IOCTL_BLOCK_COUNT:
|
||||
return MP_OBJ_NEW_SMALL_INT(self->flash_size / BLOCK_SIZE_BYTES);
|
||||
case MP_BLOCKDEV_IOCTL_BLOCK_SIZE:
|
||||
return MP_OBJ_NEW_SMALL_INT(BLOCK_SIZE_BYTES);
|
||||
case MP_BLOCKDEV_IOCTL_BLOCK_ERASE: {
|
||||
uint32_t offset = mp_obj_get_int(arg_in) * BLOCK_SIZE_BYTES;
|
||||
flash_range_erase(self->flash_base + offset, BLOCK_SIZE_BYTES);
|
||||
// TODO check return value
|
||||
return MP_OBJ_NEW_SMALL_INT(0);
|
||||
}
|
||||
default:
|
||||
return mp_const_none;
|
||||
}
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_3(rp2_flash_ioctl_obj, rp2_flash_ioctl);
|
||||
|
||||
STATIC const mp_rom_map_elem_t rp2_flash_locals_dict_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR_readblocks), MP_ROM_PTR(&rp2_flash_readblocks_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_writeblocks), MP_ROM_PTR(&rp2_flash_writeblocks_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_ioctl), MP_ROM_PTR(&rp2_flash_ioctl_obj) },
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(rp2_flash_locals_dict, rp2_flash_locals_dict_table);
|
||||
|
||||
const mp_obj_type_t rp2_flash_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_Flash,
|
||||
.make_new = rp2_flash_make_new,
|
||||
.locals_dict = (mp_obj_dict_t *)&rp2_flash_locals_dict,
|
||||
};
|
780
ports/rp2/rp2_pio.c
Normal file
780
ports/rp2/rp2_pio.c
Normal file
@ -0,0 +1,780 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include "py/binary.h"
|
||||
#include "py/runtime.h"
|
||||
#include "py/mperrno.h"
|
||||
#include "py/mphal.h"
|
||||
#include "lib/utils/mpirq.h"
|
||||
#include "modrp2.h"
|
||||
|
||||
#include "hardware/clocks.h"
|
||||
#include "hardware/irq.h"
|
||||
#include "hardware/pio.h"
|
||||
|
||||
#define PIO_NUM(pio) ((pio) == pio0 ? 0 : 1)
|
||||
|
||||
typedef struct _rp2_pio_obj_t {
|
||||
mp_obj_base_t base;
|
||||
PIO pio;
|
||||
uint8_t irq;
|
||||
} rp2_pio_obj_t;
|
||||
|
||||
typedef struct _rp2_pio_irq_obj_t {
|
||||
mp_irq_obj_t base;
|
||||
uint32_t flags;
|
||||
uint32_t trigger;
|
||||
} rp2_pio_irq_obj_t;
|
||||
|
||||
typedef struct _rp2_state_machine_obj_t {
|
||||
mp_obj_base_t base;
|
||||
PIO pio;
|
||||
uint8_t irq;
|
||||
uint8_t sm; // 0-3
|
||||
uint8_t id; // 0-7
|
||||
} rp2_state_machine_obj_t;
|
||||
|
||||
typedef struct _rp2_state_machine_irq_obj_t {
|
||||
mp_irq_obj_t base;
|
||||
uint8_t flags;
|
||||
uint8_t trigger;
|
||||
} rp2_state_machine_irq_obj_t;
|
||||
|
||||
STATIC const rp2_state_machine_obj_t rp2_state_machine_obj[8];
|
||||
|
||||
STATIC mp_obj_t rp2_state_machine_init_helper(const rp2_state_machine_obj_t *self, size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
|
||||
|
||||
STATIC void pio_irq0(PIO pio) {
|
||||
uint32_t ints = pio->ints0;
|
||||
|
||||
// Acknowledge SM0-3 IRQs if they are enabled on this IRQ0.
|
||||
pio->irq = ints >> 8;
|
||||
|
||||
// Call handler if it is registered, for PIO irqs.
|
||||
rp2_pio_irq_obj_t *irq = MP_STATE_PORT(rp2_pio_irq_obj[PIO_NUM(pio)]);
|
||||
if (irq != NULL && (ints & irq->trigger)) {
|
||||
irq->flags = ints & irq->trigger;
|
||||
mp_irq_handler(&irq->base);
|
||||
}
|
||||
|
||||
// Call handler if it is registered, for StateMachine irqs.
|
||||
for (size_t i = 0; i < 4; ++i) {
|
||||
rp2_state_machine_irq_obj_t *irq = MP_STATE_PORT(rp2_state_machine_irq_obj[PIO_NUM(pio) * 4 + i]);
|
||||
if (irq != NULL && ((ints >> (8 + i)) & irq->trigger)) {
|
||||
irq->flags = 1;
|
||||
mp_irq_handler(&irq->base);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
STATIC void pio0_irq0(void) {
|
||||
pio_irq0(pio0);
|
||||
}
|
||||
|
||||
STATIC void pio1_irq0(void) {
|
||||
pio_irq0(pio1);
|
||||
}
|
||||
|
||||
void rp2_pio_init(void) {
|
||||
// Reset all PIO instruction memory.
|
||||
pio_clear_instruction_memory(pio0);
|
||||
pio_clear_instruction_memory(pio1);
|
||||
|
||||
// Set up interrupts.
|
||||
memset(MP_STATE_PORT(rp2_pio_irq_obj), 0, sizeof(MP_STATE_PORT(rp2_pio_irq_obj)));
|
||||
memset(MP_STATE_PORT(rp2_state_machine_irq_obj), 0, sizeof(MP_STATE_PORT(rp2_state_machine_irq_obj)));
|
||||
irq_set_exclusive_handler(PIO0_IRQ_0, pio0_irq0);
|
||||
irq_set_exclusive_handler(PIO1_IRQ_0, pio1_irq0);
|
||||
}
|
||||
|
||||
void rp2_pio_deinit(void) {
|
||||
// Disable and clear interrupts.
|
||||
irq_set_mask_enabled((1u << PIO0_IRQ_0) | (1u << PIO0_IRQ_1), false);
|
||||
irq_remove_handler(PIO0_IRQ_0, pio0_irq0);
|
||||
irq_remove_handler(PIO1_IRQ_0, pio1_irq0);
|
||||
}
|
||||
|
||||
/******************************************************************************/
|
||||
// Helper functions to manage asm_pio data structure.
|
||||
|
||||
#define ASM_PIO_CONFIG_DEFAULT { -1, 0, 0, 0 };
|
||||
|
||||
enum {
|
||||
PROG_DATA,
|
||||
PROG_OFFSET_PIO0,
|
||||
PROG_OFFSET_PIO1,
|
||||
PROG_EXECCTRL,
|
||||
PROG_SHIFTCTRL,
|
||||
PROG_OUT_PINS,
|
||||
PROG_SET_PINS,
|
||||
PROG_SIDESET_PINS,
|
||||
PROG_MAX_FIELDS,
|
||||
};
|
||||
|
||||
typedef struct _asm_pio_config_t {
|
||||
int8_t base;
|
||||
uint8_t count;
|
||||
uint8_t pindirs;
|
||||
uint8_t pinvals;
|
||||
} asm_pio_config_t;
|
||||
|
||||
STATIC void asm_pio_override_shiftctrl(mp_obj_t arg, uint32_t bits, uint32_t lsb, pio_sm_config *config) {
|
||||
if (arg != mp_const_none) {
|
||||
config->shiftctrl = (config->shiftctrl & ~bits) | (mp_obj_get_int(arg) << lsb);
|
||||
}
|
||||
}
|
||||
|
||||
STATIC void asm_pio_get_pins(const char *type, mp_obj_t prog_pins, mp_obj_t arg_base, asm_pio_config_t *config) {
|
||||
if (prog_pins != mp_const_none) {
|
||||
// The PIO program specified pins for initialisation on out/set/sideset.
|
||||
if (mp_obj_is_integer(prog_pins)) {
|
||||
// A single pin specified, set its dir and value.
|
||||
config->count = 1;
|
||||
mp_int_t value = mp_obj_get_int(prog_pins);
|
||||
config->pindirs = value >> 1;
|
||||
config->pinvals = value & 1;
|
||||
} else {
|
||||
// An array of pins specified, set their dirs and values.
|
||||
size_t count;
|
||||
mp_obj_t *items;
|
||||
mp_obj_get_array(prog_pins, &count, &items);
|
||||
config->count = count;
|
||||
for (size_t i = 0; i < config->count; ++i) {
|
||||
mp_int_t value = mp_obj_get_int(items[i]);
|
||||
config->pindirs |= (value >> 1) << i;
|
||||
config->pinvals |= (value & 1) << i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (arg_base != mp_const_none) {
|
||||
// The instantiation of the PIO program specified a base pin.
|
||||
config->base = mp_hal_get_pin_obj(arg_base);
|
||||
}
|
||||
}
|
||||
|
||||
STATIC void asm_pio_init_gpio(PIO pio, uint32_t sm, asm_pio_config_t *config) {
|
||||
uint32_t pinmask = ((1 << config->count) - 1) << config->base;
|
||||
pio_sm_set_pins_with_mask(pio, sm, config->pinvals << config->base, pinmask);
|
||||
pio_sm_set_pindirs_with_mask(pio, sm, config->pindirs << config->base, pinmask);
|
||||
for (size_t i = 0; i < config->count; ++i) {
|
||||
gpio_set_function(config->base + i, pio == pio0 ? GPIO_FUNC_PIO0 : GPIO_FUNC_PIO1);
|
||||
}
|
||||
}
|
||||
|
||||
/******************************************************************************/
|
||||
// PIO object
|
||||
|
||||
STATIC const mp_irq_methods_t rp2_pio_irq_methods;
|
||||
|
||||
STATIC rp2_pio_obj_t rp2_pio_obj[] = {
|
||||
{ { &rp2_pio_type }, pio0, PIO0_IRQ_0 },
|
||||
{ { &rp2_pio_type }, pio1, PIO1_IRQ_0 },
|
||||
};
|
||||
|
||||
STATIC void rp2_pio_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
|
||||
rp2_pio_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
mp_printf(print, "PIO(%u)", self->pio == pio0 ? 0 : 1);
|
||||
}
|
||||
|
||||
// constructor(id)
|
||||
STATIC mp_obj_t rp2_pio_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) {
|
||||
mp_arg_check_num(n_args, n_kw, 1, 1, false);
|
||||
|
||||
// Get the PIO object.
|
||||
int pio_id = mp_obj_get_int(args[0]);
|
||||
if (!(0 <= pio_id && pio_id < MP_ARRAY_SIZE(rp2_pio_obj))) {
|
||||
mp_raise_ValueError("invalid PIO");
|
||||
}
|
||||
const rp2_pio_obj_t *self = &rp2_pio_obj[pio_id];
|
||||
|
||||
// Return the PIO object.
|
||||
return MP_OBJ_FROM_PTR(self);
|
||||
}
|
||||
|
||||
// PIO.add_program(prog)
|
||||
STATIC mp_obj_t rp2_pio_add_program(mp_obj_t self_in, mp_obj_t prog_in) {
|
||||
rp2_pio_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
|
||||
// Get the program data.
|
||||
mp_obj_t *prog;
|
||||
mp_obj_get_array_fixed_n(prog_in, PROG_MAX_FIELDS, &prog);
|
||||
mp_buffer_info_t bufinfo;
|
||||
mp_get_buffer_raise(prog[PROG_DATA], &bufinfo, MP_BUFFER_READ);
|
||||
|
||||
// Add the program data to the PIO instruction memory.
|
||||
struct pio_program pio_program = { bufinfo.buf, bufinfo.len / 2, -1 };
|
||||
if (!pio_can_add_program(self->pio, &pio_program)) {
|
||||
mp_raise_OSError(MP_ENOMEM);
|
||||
}
|
||||
uint offset = pio_add_program(self->pio, &pio_program);
|
||||
|
||||
// Store the program offset in the program object.
|
||||
prog[PROG_OFFSET_PIO0 + PIO_NUM(self->pio)] = MP_OBJ_NEW_SMALL_INT(offset);
|
||||
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_2(rp2_pio_add_program_obj, rp2_pio_add_program);
|
||||
|
||||
// PIO.remove_program([prog])
|
||||
STATIC mp_obj_t rp2_pio_remove_program(size_t n_args, const mp_obj_t *args) {
|
||||
rp2_pio_obj_t *self = MP_OBJ_TO_PTR(args[0]);
|
||||
|
||||
// Default to remove all programs.
|
||||
uint8_t length = 32;
|
||||
uint offset = 0;
|
||||
|
||||
if (n_args > 1) {
|
||||
// Get specific program to remove.
|
||||
mp_obj_t *prog;
|
||||
mp_obj_get_array_fixed_n(args[1], PROG_MAX_FIELDS, &prog);
|
||||
mp_buffer_info_t bufinfo;
|
||||
mp_get_buffer_raise(prog[PROG_DATA], &bufinfo, MP_BUFFER_READ);
|
||||
length = bufinfo.len / 2;
|
||||
offset = mp_obj_get_int(prog[PROG_OFFSET_PIO0 + PIO_NUM(self->pio)]);
|
||||
if (offset < 0) {
|
||||
mp_raise_ValueError("prog not in instruction memory");
|
||||
}
|
||||
// Invalidate the program offset in the program object.
|
||||
prog[PROG_OFFSET_PIO0 + PIO_NUM(self->pio)] = MP_OBJ_NEW_SMALL_INT(-1);
|
||||
}
|
||||
|
||||
// Remove the program from the instruction memory.
|
||||
struct pio_program pio_program = { NULL, length, -1 };
|
||||
pio_remove_program(self->pio, &pio_program, offset);
|
||||
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(rp2_pio_remove_program_obj, 1, 2, rp2_pio_remove_program);
|
||||
|
||||
// PIO.state_machine(id, prog, freq=-1, *, set=None)
|
||||
STATIC mp_obj_t rp2_pio_state_machine(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
|
||||
rp2_pio_obj_t *self = MP_OBJ_TO_PTR(pos_args[0]);
|
||||
|
||||
// Get and verify the state machine id.
|
||||
mp_int_t sm_id = mp_obj_get_int(pos_args[1]);
|
||||
if (!(0 <= sm_id && sm_id < 4)) {
|
||||
mp_raise_ValueError("invalide state machine");
|
||||
}
|
||||
|
||||
// Return the correct StateMachine object.
|
||||
const rp2_state_machine_obj_t *sm = &rp2_state_machine_obj[(self->pio == pio0 ? 0 : 4) + sm_id];
|
||||
|
||||
if (n_args > 2 || kw_args->used > 0) {
|
||||
// Configuration arguments given so init this StateMachine.
|
||||
rp2_state_machine_init_helper(sm, n_args - 2, pos_args + 2, kw_args);
|
||||
}
|
||||
|
||||
return MP_OBJ_FROM_PTR(sm);
|
||||
}
|
||||
MP_DEFINE_CONST_FUN_OBJ_KW(rp2_pio_state_machine_obj, 2, rp2_pio_state_machine);
|
||||
|
||||
// PIO.irq(handler=None, trigger=IRQ_SM0|IRQ_SM1|IRQ_SM2|IRQ_SM3, hard=False)
|
||||
STATIC mp_obj_t rp2_pio_irq(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
|
||||
enum { ARG_handler, ARG_trigger, ARG_hard };
|
||||
static const mp_arg_t allowed_args[] = {
|
||||
{ MP_QSTR_handler, MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_trigger, MP_ARG_INT, {.u_int = 0xf00} },
|
||||
{ MP_QSTR_hard, MP_ARG_BOOL, {.u_bool = false} },
|
||||
};
|
||||
|
||||
// Parse the arguments.
|
||||
rp2_pio_obj_t *self = MP_OBJ_TO_PTR(pos_args[0]);
|
||||
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
|
||||
mp_arg_parse_all(n_args - 1, pos_args + 1, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
|
||||
|
||||
// Get the IRQ object.
|
||||
rp2_pio_irq_obj_t *irq = MP_STATE_PORT(rp2_pio_irq_obj[PIO_NUM(self->pio)]);
|
||||
|
||||
// Allocate the IRQ object if it doesn't already exist.
|
||||
if (irq == NULL) {
|
||||
irq = m_new_obj(rp2_pio_irq_obj_t);
|
||||
irq->base.base.type = &mp_irq_type;
|
||||
irq->base.methods = (mp_irq_methods_t *)&rp2_pio_irq_methods;
|
||||
irq->base.parent = MP_OBJ_FROM_PTR(self);
|
||||
irq->base.handler = mp_const_none;
|
||||
irq->base.ishard = false;
|
||||
MP_STATE_PORT(rp2_pio_irq_obj[PIO_NUM(self->pio)]) = irq;
|
||||
}
|
||||
|
||||
if (n_args > 1 || kw_args->used != 0) {
|
||||
// Configure IRQ.
|
||||
|
||||
// Disable all IRQs while data is updated.
|
||||
irq_set_enabled(self->irq, false);
|
||||
|
||||
// Update IRQ data.
|
||||
irq->base.handler = args[ARG_handler].u_obj;
|
||||
irq->base.ishard = args[ARG_hard].u_bool;
|
||||
irq->flags = 0;
|
||||
irq->trigger = args[ARG_trigger].u_int;
|
||||
|
||||
// Enable IRQ if a handler is given.
|
||||
if (args[ARG_handler].u_obj != mp_const_none) {
|
||||
self->pio->inte0 = irq->trigger;
|
||||
irq_set_enabled(self->irq, true);
|
||||
}
|
||||
}
|
||||
|
||||
return MP_OBJ_FROM_PTR(irq);
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(rp2_pio_irq_obj, 1, rp2_pio_irq);
|
||||
|
||||
STATIC const mp_rom_map_elem_t rp2_pio_locals_dict_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR_add_program), MP_ROM_PTR(&rp2_pio_add_program_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_remove_program), MP_ROM_PTR(&rp2_pio_remove_program_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_state_machine), MP_ROM_PTR(&rp2_pio_state_machine_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_irq), MP_ROM_PTR(&rp2_pio_irq_obj) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_IN_LOW), MP_ROM_INT(0) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_IN_HIGH), MP_ROM_INT(1) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_OUT_LOW), MP_ROM_INT(2) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_OUT_HIGH), MP_ROM_INT(3) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_SHIFT_LEFT), MP_ROM_INT(0) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_SHIFT_RIGHT), MP_ROM_INT(1) },
|
||||
|
||||
{ MP_ROM_QSTR(MP_QSTR_IRQ_SM0), MP_ROM_INT(0x100) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_IRQ_SM1), MP_ROM_INT(0x200) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_IRQ_SM2), MP_ROM_INT(0x400) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_IRQ_SM3), MP_ROM_INT(0x800) },
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(rp2_pio_locals_dict, rp2_pio_locals_dict_table);
|
||||
|
||||
const mp_obj_type_t rp2_pio_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_PIO,
|
||||
.print = rp2_pio_print,
|
||||
.make_new = rp2_pio_make_new,
|
||||
.locals_dict = (mp_obj_dict_t *)&rp2_pio_locals_dict,
|
||||
};
|
||||
|
||||
STATIC mp_uint_t rp2_pio_irq_trigger(mp_obj_t self_in, mp_uint_t new_trigger) {
|
||||
rp2_pio_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
rp2_pio_irq_obj_t *irq = MP_STATE_PORT(rp2_pio_irq_obj[PIO_NUM(self->pio)]);
|
||||
irq_set_enabled(self->irq, false);
|
||||
irq->flags = 0;
|
||||
irq->trigger = new_trigger;
|
||||
irq_set_enabled(self->irq, true);
|
||||
return 0;
|
||||
}
|
||||
|
||||
STATIC mp_uint_t rp2_pio_irq_info(mp_obj_t self_in, mp_uint_t info_type) {
|
||||
rp2_pio_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
rp2_pio_irq_obj_t *irq = MP_STATE_PORT(rp2_pio_irq_obj[PIO_NUM(self->pio)]);
|
||||
if (info_type == MP_IRQ_INFO_FLAGS) {
|
||||
return irq->flags;
|
||||
} else if (info_type == MP_IRQ_INFO_TRIGGERS) {
|
||||
return irq->trigger;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
STATIC const mp_irq_methods_t rp2_pio_irq_methods = {
|
||||
.trigger = rp2_pio_irq_trigger,
|
||||
.info = rp2_pio_irq_info,
|
||||
};
|
||||
|
||||
/******************************************************************************/
|
||||
// StateMachine object
|
||||
|
||||
STATIC const mp_irq_methods_t rp2_state_machine_irq_methods;
|
||||
|
||||
STATIC const rp2_state_machine_obj_t rp2_state_machine_obj[] = {
|
||||
{ { &rp2_state_machine_type }, pio0, PIO0_IRQ_0, 0, 0 },
|
||||
{ { &rp2_state_machine_type }, pio0, PIO0_IRQ_0, 1, 1 },
|
||||
{ { &rp2_state_machine_type }, pio0, PIO0_IRQ_0, 2, 2 },
|
||||
{ { &rp2_state_machine_type }, pio0, PIO0_IRQ_0, 3, 3 },
|
||||
{ { &rp2_state_machine_type }, pio1, PIO1_IRQ_0, 0, 4 },
|
||||
{ { &rp2_state_machine_type }, pio1, PIO1_IRQ_0, 1, 5 },
|
||||
{ { &rp2_state_machine_type }, pio1, PIO1_IRQ_0, 2, 6 },
|
||||
{ { &rp2_state_machine_type }, pio1, PIO1_IRQ_0, 3, 7 },
|
||||
};
|
||||
|
||||
STATIC void rp2_state_machine_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
|
||||
rp2_state_machine_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
mp_printf(print, "StateMachine(%u)", self->id);
|
||||
}
|
||||
|
||||
// StateMachine.init(prog, freq=-1, *,
|
||||
// in_base=None, out_base=None, set_base=None, sideset_base=None,
|
||||
// in_shiftdir=None, out_shiftdir=None, push_thresh=None, pull_thresh=None,
|
||||
// )
|
||||
STATIC mp_obj_t rp2_state_machine_init_helper(const rp2_state_machine_obj_t *self, size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
|
||||
enum {
|
||||
ARG_prog, ARG_freq,
|
||||
ARG_in_base, ARG_out_base, ARG_set_base, ARG_sideset_base,
|
||||
ARG_in_shiftdir, ARG_out_shiftdir, ARG_push_thresh, ARG_pull_thresh
|
||||
};
|
||||
static const mp_arg_t allowed_args[] = {
|
||||
{ MP_QSTR_prog, MP_ARG_REQUIRED | MP_ARG_OBJ },
|
||||
{ MP_QSTR_freq, MP_ARG_INT, {.u_int = -1} },
|
||||
{ MP_QSTR_in_base, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_out_base, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_set_base, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_sideset_base, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_in_shiftdir, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_out_shiftdir, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_push_thresh, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_pull_thresh, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
};
|
||||
|
||||
// Parse the arguments.
|
||||
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
|
||||
mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
|
||||
|
||||
// Get the program.
|
||||
mp_obj_t *prog;
|
||||
mp_obj_get_array_fixed_n(args[ARG_prog].u_obj, PROG_MAX_FIELDS, &prog);
|
||||
|
||||
// Get and the program offset, and load it into memory if it's not already there.
|
||||
mp_int_t offset = mp_obj_get_int(prog[PROG_OFFSET_PIO0 + PIO_NUM(self->pio)]);
|
||||
if (offset < 0) {
|
||||
rp2_pio_add_program(&rp2_pio_obj[PIO_NUM(self->pio)], args[ARG_prog].u_obj);
|
||||
offset = mp_obj_get_int(prog[PROG_OFFSET_PIO0 + PIO_NUM(self->pio)]);
|
||||
}
|
||||
|
||||
// Compute the clock divider.
|
||||
float div;
|
||||
if (args[ARG_freq].u_int < 0) {
|
||||
div = 1;
|
||||
} else if (args[ARG_freq].u_int == 0) {
|
||||
div = 0;
|
||||
} else {
|
||||
div = (float)clock_get_hz(clk_sys) / (float)args[ARG_freq].u_int;
|
||||
}
|
||||
|
||||
// Disable and reset the state machine.
|
||||
pio_sm_init(self->pio, self->sm, offset, NULL);
|
||||
|
||||
// Build the state machine config.
|
||||
pio_sm_config config = pio_get_default_sm_config();
|
||||
sm_config_set_clkdiv(&config, div);
|
||||
config.execctrl = mp_obj_get_int_truncated(prog[PROG_EXECCTRL]);
|
||||
config.shiftctrl = mp_obj_get_int_truncated(prog[PROG_SHIFTCTRL]);
|
||||
|
||||
// Adjust wrap top/bottom to account for location of program in instruction memory.
|
||||
config.execctrl += (offset << PIO_SM0_EXECCTRL_WRAP_TOP_LSB)
|
||||
+ (offset << PIO_SM0_EXECCTRL_WRAP_BOTTOM_LSB);
|
||||
|
||||
// Configure in pin base, if needed.
|
||||
if (args[ARG_in_base].u_obj != mp_const_none) {
|
||||
sm_config_set_in_pins(&config, mp_hal_get_pin_obj(args[ARG_in_base].u_obj));
|
||||
}
|
||||
|
||||
// Configure out pins, if needed.
|
||||
asm_pio_config_t out_config = ASM_PIO_CONFIG_DEFAULT;
|
||||
asm_pio_get_pins("out", prog[PROG_OUT_PINS], args[ARG_out_base].u_obj, &out_config);
|
||||
if (out_config.base >= 0) {
|
||||
sm_config_set_out_pins(&config, out_config.base, out_config.count);
|
||||
}
|
||||
|
||||
// Configure set pin, if needed.
|
||||
asm_pio_config_t set_config = ASM_PIO_CONFIG_DEFAULT;
|
||||
asm_pio_get_pins("set", prog[PROG_SET_PINS], args[ARG_set_base].u_obj, &set_config);
|
||||
if (set_config.base >= 0) {
|
||||
sm_config_set_set_pins(&config, set_config.base, set_config.count);
|
||||
}
|
||||
|
||||
// Configure sideset pin, if needed.
|
||||
asm_pio_config_t sideset_config = ASM_PIO_CONFIG_DEFAULT;
|
||||
asm_pio_get_pins("sideset", prog[PROG_SIDESET_PINS], args[ARG_sideset_base].u_obj, &sideset_config);
|
||||
if (sideset_config.base >= 0) {
|
||||
uint32_t count = sideset_config.count;
|
||||
if (config.execctrl & (1 << PIO_SM0_EXECCTRL_SIDE_EN_LSB)) {
|
||||
// When sideset is optional, count includes the option bit.
|
||||
++count;
|
||||
}
|
||||
config.pinctrl |= count << PIO_SM0_PINCTRL_SIDESET_COUNT_LSB;
|
||||
sm_config_set_sideset_pins(&config, sideset_config.base);
|
||||
}
|
||||
|
||||
// Override shift state if needed.
|
||||
asm_pio_override_shiftctrl(args[ARG_in_shiftdir].u_obj, PIO_SM0_SHIFTCTRL_IN_SHIFTDIR_BITS, PIO_SM0_SHIFTCTRL_IN_SHIFTDIR_LSB, &config);
|
||||
asm_pio_override_shiftctrl(args[ARG_out_shiftdir].u_obj, PIO_SM0_SHIFTCTRL_OUT_SHIFTDIR_BITS, PIO_SM0_SHIFTCTRL_OUT_SHIFTDIR_LSB, &config);
|
||||
asm_pio_override_shiftctrl(args[ARG_push_thresh].u_obj, PIO_SM0_SHIFTCTRL_PUSH_THRESH_BITS, PIO_SM0_SHIFTCTRL_PUSH_THRESH_LSB, &config);
|
||||
asm_pio_override_shiftctrl(args[ARG_pull_thresh].u_obj, PIO_SM0_SHIFTCTRL_PULL_THRESH_BITS, PIO_SM0_SHIFTCTRL_PULL_THRESH_LSB, &config);
|
||||
|
||||
// Configure the state machine.
|
||||
pio_sm_set_config(self->pio, self->sm, &config);
|
||||
|
||||
// Configure the GPIO.
|
||||
if (out_config.base >= 0) {
|
||||
asm_pio_init_gpio(self->pio, self->sm, &out_config);
|
||||
}
|
||||
if (set_config.base >= 0) {
|
||||
asm_pio_init_gpio(self->pio, self->sm, &set_config);
|
||||
}
|
||||
if (sideset_config.base >= 0) {
|
||||
asm_pio_init_gpio(self->pio, self->sm, &sideset_config);
|
||||
}
|
||||
|
||||
return mp_const_none;
|
||||
}
|
||||
|
||||
// StateMachine(id, ...)
|
||||
STATIC mp_obj_t rp2_state_machine_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) {
|
||||
mp_arg_check_num(n_args, n_kw, 1, MP_OBJ_FUN_ARGS_MAX, true);
|
||||
|
||||
// Get the StateMachine object.
|
||||
mp_int_t sm_id = mp_obj_get_int(args[0]);
|
||||
if (!(0 <= sm_id && sm_id < MP_ARRAY_SIZE(rp2_state_machine_obj))) {
|
||||
mp_raise_ValueError("invalid StateMachine");
|
||||
}
|
||||
const rp2_state_machine_obj_t *self = &rp2_state_machine_obj[sm_id];
|
||||
|
||||
if (n_args > 1 || n_kw > 0) {
|
||||
// Configuration arguments given so init this StateMachine.
|
||||
mp_map_t kw_args;
|
||||
mp_map_init_fixed_table(&kw_args, n_kw, args + n_args);
|
||||
rp2_state_machine_init_helper(self, n_args - 1, args + 1, &kw_args);
|
||||
}
|
||||
|
||||
// Return the StateMachine object.
|
||||
return MP_OBJ_FROM_PTR(self);
|
||||
}
|
||||
|
||||
STATIC mp_obj_t rp2_state_machine_init(size_t n_args, const mp_obj_t *args, mp_map_t *kw_args) {
|
||||
return rp2_state_machine_init_helper(MP_OBJ_TO_PTR(args[0]), n_args - 1, args + 1, kw_args);
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(rp2_state_machine_init_obj, 1, rp2_state_machine_init);
|
||||
|
||||
// StateMachine.active([value])
|
||||
STATIC mp_obj_t rp2_state_machine_active(size_t n_args, const mp_obj_t *args) {
|
||||
rp2_state_machine_obj_t *self = MP_OBJ_TO_PTR(args[0]);
|
||||
if (n_args > 1) {
|
||||
pio_sm_set_enabled(self->pio, self->sm, mp_obj_is_true(args[1]));
|
||||
}
|
||||
return mp_obj_new_bool((self->pio->ctrl >> self->sm) & 1);
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(rp2_state_machine_active_obj, 1, 2, rp2_state_machine_active);
|
||||
|
||||
// StateMachine.exec(instr)
|
||||
STATIC mp_obj_t rp2_state_machine_exec(mp_obj_t self_in, mp_obj_t instr_in) {
|
||||
rp2_state_machine_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
mp_obj_t rp2_module = mp_import_name(MP_QSTR_rp2, mp_const_none, MP_OBJ_NEW_SMALL_INT(0));
|
||||
mp_obj_t asm_pio_encode = mp_load_attr(rp2_module, MP_QSTR_asm_pio_encode);
|
||||
uint32_t sideset_count = self->pio->sm[self->sm].pinctrl >> PIO_SM0_PINCTRL_SIDESET_COUNT_LSB;
|
||||
mp_obj_t encoded_obj = mp_call_function_2(asm_pio_encode, instr_in, MP_OBJ_NEW_SMALL_INT(sideset_count));
|
||||
mp_int_t encoded = mp_obj_get_int(encoded_obj);
|
||||
pio_sm_exec(self->pio, self->sm, encoded);
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_2(rp2_state_machine_exec_obj, rp2_state_machine_exec);
|
||||
|
||||
// StateMachine.get(buf=None, shift=0)
|
||||
STATIC mp_obj_t rp2_state_machine_get(size_t n_args, const mp_obj_t *args) {
|
||||
rp2_state_machine_obj_t *self = MP_OBJ_TO_PTR(args[0]);
|
||||
mp_buffer_info_t bufinfo;
|
||||
bufinfo.buf = NULL;
|
||||
uint32_t shift = 0;
|
||||
if (n_args > 1) {
|
||||
if (args[1] != mp_const_none) {
|
||||
mp_get_buffer_raise(args[1], &bufinfo, MP_BUFFER_WRITE);
|
||||
if (bufinfo.typecode == BYTEARRAY_TYPECODE) {
|
||||
bufinfo.typecode = 'b';
|
||||
} else {
|
||||
bufinfo.typecode |= 0x20; // make lowercase to support upper and lower
|
||||
}
|
||||
}
|
||||
if (n_args > 2) {
|
||||
shift = mp_obj_get_int(args[2]);
|
||||
}
|
||||
}
|
||||
uint8_t *dest = bufinfo.buf;
|
||||
const uint8_t *dest_top = dest + bufinfo.len;
|
||||
for (;;) {
|
||||
while (pio_sm_is_rx_fifo_empty(self->pio, self->sm)) {
|
||||
// This delay must be fast.
|
||||
mp_handle_pending(true);
|
||||
MICROPY_HW_USBDEV_TASK_HOOK
|
||||
}
|
||||
uint32_t value = pio_sm_get(self->pio, self->sm) >> shift;
|
||||
if (dest == NULL) {
|
||||
return mp_obj_new_int_from_uint(value);
|
||||
}
|
||||
if (dest >= dest_top) {
|
||||
return args[1];
|
||||
}
|
||||
if (bufinfo.typecode == 'b') {
|
||||
*(uint8_t *)dest = value;
|
||||
dest += sizeof(uint8_t);
|
||||
} else if (bufinfo.typecode == 'h') {
|
||||
*(uint16_t *)dest = value;
|
||||
dest += sizeof(uint16_t);
|
||||
} else if (bufinfo.typecode == 'i') {
|
||||
*(uint32_t *)dest = value;
|
||||
dest += sizeof(uint32_t);
|
||||
} else {
|
||||
mp_raise_ValueError("unsupported buffer type");
|
||||
}
|
||||
}
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(rp2_state_machine_get_obj, 1, 3, rp2_state_machine_get);
|
||||
|
||||
// StateMachine.put(value, shift=0)
|
||||
STATIC mp_obj_t rp2_state_machine_put(size_t n_args, const mp_obj_t *args) {
|
||||
rp2_state_machine_obj_t *self = MP_OBJ_TO_PTR(args[0]);
|
||||
uint32_t shift = 0;
|
||||
if (n_args > 2) {
|
||||
shift = mp_obj_get_int(args[2]);
|
||||
}
|
||||
uint32_t data;
|
||||
mp_buffer_info_t bufinfo;
|
||||
if (!mp_get_buffer(args[1], &bufinfo, MP_BUFFER_READ)) {
|
||||
data = mp_obj_get_int_truncated(args[1]);
|
||||
bufinfo.buf = &data;
|
||||
bufinfo.len = sizeof(uint32_t);
|
||||
bufinfo.typecode = 'I';
|
||||
}
|
||||
const uint8_t *src = bufinfo.buf;
|
||||
const uint8_t *src_top = src + bufinfo.len;
|
||||
while (src < src_top) {
|
||||
uint32_t value;
|
||||
if (bufinfo.typecode == 'B' || bufinfo.typecode == BYTEARRAY_TYPECODE) {
|
||||
value = *(uint8_t *)src;
|
||||
src += sizeof(uint8_t);
|
||||
} else if (bufinfo.typecode == 'H') {
|
||||
value = *(uint16_t *)src;
|
||||
src += sizeof(uint16_t);
|
||||
} else if (bufinfo.typecode == 'I') {
|
||||
value = *(uint32_t *)src;
|
||||
src += sizeof(uint32_t);
|
||||
} else {
|
||||
mp_raise_ValueError("unsupported buffer type");
|
||||
}
|
||||
while (pio_sm_is_tx_fifo_full(self->pio, self->sm)) {
|
||||
// This delay must be fast.
|
||||
mp_handle_pending(true);
|
||||
MICROPY_HW_USBDEV_TASK_HOOK
|
||||
}
|
||||
pio_sm_put(self->pio, self->sm, value << shift);
|
||||
}
|
||||
return mp_const_none;
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(rp2_state_machine_put_obj, 2, 3, rp2_state_machine_put);
|
||||
|
||||
// StateMachine.irq(handler=None, trigger=0|1, hard=False)
|
||||
STATIC mp_obj_t rp2_state_machine_irq(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
|
||||
enum { ARG_handler, ARG_trigger, ARG_hard };
|
||||
static const mp_arg_t allowed_args[] = {
|
||||
{ MP_QSTR_handler, MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE} },
|
||||
{ MP_QSTR_trigger, MP_ARG_INT, {.u_int = 1} },
|
||||
{ MP_QSTR_hard, MP_ARG_BOOL, {.u_bool = false} },
|
||||
};
|
||||
|
||||
// Parse the arguments.
|
||||
rp2_state_machine_obj_t *self = MP_OBJ_TO_PTR(pos_args[0]);
|
||||
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
|
||||
mp_arg_parse_all(n_args - 1, pos_args + 1, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
|
||||
|
||||
// Get the IRQ object.
|
||||
rp2_state_machine_irq_obj_t *irq = MP_STATE_PORT(rp2_state_machine_irq_obj[self->id]);
|
||||
|
||||
// Allocate the IRQ object if it doesn't already exist.
|
||||
if (irq == NULL) {
|
||||
irq = m_new_obj(rp2_state_machine_irq_obj_t);
|
||||
irq->base.base.type = &mp_irq_type;
|
||||
irq->base.methods = (mp_irq_methods_t *)&rp2_state_machine_irq_methods;
|
||||
irq->base.parent = MP_OBJ_FROM_PTR(self);
|
||||
irq->base.handler = mp_const_none;
|
||||
irq->base.ishard = false;
|
||||
MP_STATE_PORT(rp2_state_machine_irq_obj[self->id]) = irq;
|
||||
}
|
||||
|
||||
if (n_args > 1 || kw_args->used != 0) {
|
||||
// Configure IRQ.
|
||||
|
||||
// Disable all IRQs while data is updated.
|
||||
irq_set_enabled(self->irq, false);
|
||||
|
||||
// Update IRQ data.
|
||||
irq->base.handler = args[ARG_handler].u_obj;
|
||||
irq->base.ishard = args[ARG_hard].u_bool;
|
||||
irq->flags = 0;
|
||||
irq->trigger = args[ARG_trigger].u_int;
|
||||
|
||||
// Enable IRQ if a handler is given.
|
||||
if (args[ARG_handler].u_obj == mp_const_none) {
|
||||
self->pio->inte0 &= ~(1 << (8 + self->sm));
|
||||
} else {
|
||||
self->pio->inte0 |= 1 << (8 + self->sm);
|
||||
}
|
||||
|
||||
if (self->pio->inte0) {
|
||||
irq_set_enabled(self->irq, true);
|
||||
}
|
||||
}
|
||||
|
||||
return MP_OBJ_FROM_PTR(irq);
|
||||
}
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(rp2_state_machine_irq_obj, 1, rp2_state_machine_irq);
|
||||
|
||||
STATIC const mp_rom_map_elem_t rp2_state_machine_locals_dict_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR_init), MP_ROM_PTR(&rp2_state_machine_init_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_active), MP_ROM_PTR(&rp2_state_machine_active_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_exec), MP_ROM_PTR(&rp2_state_machine_exec_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_get), MP_ROM_PTR(&rp2_state_machine_get_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_put), MP_ROM_PTR(&rp2_state_machine_put_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_irq), MP_ROM_PTR(&rp2_state_machine_irq_obj) },
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(rp2_state_machine_locals_dict, rp2_state_machine_locals_dict_table);
|
||||
|
||||
const mp_obj_type_t rp2_state_machine_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_StateMachine,
|
||||
.print = rp2_state_machine_print,
|
||||
.make_new = rp2_state_machine_make_new,
|
||||
.locals_dict = (mp_obj_dict_t *)&rp2_state_machine_locals_dict,
|
||||
};
|
||||
|
||||
STATIC mp_uint_t rp2_state_machine_irq_trigger(mp_obj_t self_in, mp_uint_t new_trigger) {
|
||||
rp2_state_machine_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
rp2_state_machine_irq_obj_t *irq = MP_STATE_PORT(rp2_state_machine_irq_obj[PIO_NUM(self->pio)]);
|
||||
irq_set_enabled(self->irq, false);
|
||||
irq->flags = 0;
|
||||
irq->trigger = new_trigger;
|
||||
irq_set_enabled(self->irq, true);
|
||||
return 0;
|
||||
}
|
||||
|
||||
STATIC mp_uint_t rp2_state_machine_irq_info(mp_obj_t self_in, mp_uint_t info_type) {
|
||||
rp2_state_machine_obj_t *self = MP_OBJ_TO_PTR(self_in);
|
||||
rp2_state_machine_irq_obj_t *irq = MP_STATE_PORT(rp2_state_machine_irq_obj[PIO_NUM(self->pio)]);
|
||||
if (info_type == MP_IRQ_INFO_FLAGS) {
|
||||
return irq->flags;
|
||||
} else if (info_type == MP_IRQ_INFO_TRIGGERS) {
|
||||
return irq->trigger;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
STATIC const mp_irq_methods_t rp2_state_machine_irq_methods = {
|
||||
.trigger = rp2_state_machine_irq_trigger,
|
||||
.info = rp2_state_machine_irq_info,
|
||||
};
|
34
ports/rp2/tusb_config.h
Normal file
34
ports/rp2/tusb_config.h
Normal file
@ -0,0 +1,34 @@
|
||||
/*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*
|
||||
*/
|
||||
#ifndef MICROPY_INCLUDED_RP2_TUSB_CONFIG_H
|
||||
#define MICROPY_INCLUDED_RP2_TUSB_CONFIG_H
|
||||
|
||||
#define CFG_TUSB_RHPORT0_MODE (OPT_MODE_DEVICE)
|
||||
|
||||
#define CFG_TUD_CDC (1)
|
||||
#define CFG_TUD_CDC_RX_BUFSIZE (256)
|
||||
#define CFG_TUD_CDC_TX_BUFSIZE (256)
|
||||
|
||||
#endif // MICROPY_INCLUDED_RP2_TUSB_CONFIG_H
|
115
ports/rp2/tusb_port.c
Normal file
115
ports/rp2/tusb_port.c
Normal file
@ -0,0 +1,115 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2019 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "tusb.h"
|
||||
|
||||
#define USBD_VID (0x2E8A) // Raspberry Pi
|
||||
#define USBD_PID (0x0005) // RP2 MicroPython
|
||||
|
||||
#define USBD_DESC_LEN (TUD_CONFIG_DESC_LEN + TUD_CDC_DESC_LEN)
|
||||
#define USBD_MAX_POWER_MA (250)
|
||||
|
||||
#define USBD_ITF_CDC (0) // needs 2 interfaces
|
||||
#define USBD_ITF_MAX (2)
|
||||
|
||||
#define USBD_CDC_EP_CMD (0x81)
|
||||
#define USBD_CDC_EP_OUT (0x02)
|
||||
#define USBD_CDC_EP_IN (0x82)
|
||||
#define USBD_CDC_CMD_MAX_SIZE (8)
|
||||
#define USBD_CDC_IN_OUT_MAX_SIZE (64)
|
||||
|
||||
#define USBD_STR_0 (0x00)
|
||||
#define USBD_STR_MANUF (0x01)
|
||||
#define USBD_STR_PRODUCT (0x02)
|
||||
#define USBD_STR_SERIAL (0x03)
|
||||
#define USBD_STR_CDC (0x04)
|
||||
|
||||
// Note: descriptors returned from callbacks must exist long enough for transfer to complete
|
||||
|
||||
static const tusb_desc_device_t usbd_desc_device = {
|
||||
.bLength = sizeof(tusb_desc_device_t),
|
||||
.bDescriptorType = TUSB_DESC_DEVICE,
|
||||
.bcdUSB = 0x0200,
|
||||
.bDeviceClass = TUSB_CLASS_MISC,
|
||||
.bDeviceSubClass = MISC_SUBCLASS_COMMON,
|
||||
.bDeviceProtocol = MISC_PROTOCOL_IAD,
|
||||
.bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE,
|
||||
.idVendor = USBD_VID,
|
||||
.idProduct = USBD_PID,
|
||||
.bcdDevice = 0x0100,
|
||||
.iManufacturer = USBD_STR_MANUF,
|
||||
.iProduct = USBD_STR_PRODUCT,
|
||||
.iSerialNumber = USBD_STR_SERIAL,
|
||||
.bNumConfigurations = 1,
|
||||
};
|
||||
|
||||
static const uint8_t usbd_desc_cfg[USBD_DESC_LEN] = {
|
||||
TUD_CONFIG_DESCRIPTOR(1, USBD_ITF_MAX, USBD_STR_0, USBD_DESC_LEN,
|
||||
TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP, USBD_MAX_POWER_MA),
|
||||
|
||||
TUD_CDC_DESCRIPTOR(USBD_ITF_CDC, USBD_STR_CDC, USBD_CDC_EP_CMD,
|
||||
USBD_CDC_CMD_MAX_SIZE, USBD_CDC_EP_OUT, USBD_CDC_EP_IN, USBD_CDC_IN_OUT_MAX_SIZE),
|
||||
};
|
||||
|
||||
static const char *const usbd_desc_str[] = {
|
||||
[USBD_STR_MANUF] = "MicroPython",
|
||||
[USBD_STR_PRODUCT] = "Board in FS mode",
|
||||
[USBD_STR_SERIAL] = "000000000000", // TODO
|
||||
[USBD_STR_CDC] = "Board CDC",
|
||||
};
|
||||
|
||||
const uint8_t *tud_descriptor_device_cb(void) {
|
||||
return (const uint8_t *)&usbd_desc_device;
|
||||
}
|
||||
|
||||
const uint8_t *tud_descriptor_configuration_cb(uint8_t index) {
|
||||
(void)index;
|
||||
return usbd_desc_cfg;
|
||||
}
|
||||
|
||||
const uint16_t *tud_descriptor_string_cb(uint8_t index, uint16_t langid) {
|
||||
#define DESC_STR_MAX (20)
|
||||
static uint16_t desc_str[DESC_STR_MAX];
|
||||
|
||||
uint8_t len;
|
||||
if (index == 0) {
|
||||
desc_str[1] = 0x0409; // supported language is English
|
||||
len = 1;
|
||||
} else {
|
||||
if (index >= sizeof(usbd_desc_str) / sizeof(usbd_desc_str[0])) {
|
||||
return NULL;
|
||||
}
|
||||
const char *str = usbd_desc_str[index];
|
||||
for (len = 0; len < DESC_STR_MAX - 1 && str[len]; ++len) {
|
||||
desc_str[1 + len] = str[len];
|
||||
}
|
||||
}
|
||||
|
||||
// first byte is length (including header), second byte is string type
|
||||
desc_str[0] = (TUSB_DESC_STRING << 8) | (2 * len + 2);
|
||||
|
||||
return desc_str;
|
||||
}
|
63
ports/rp2/uart.c
Normal file
63
ports/rp2/uart.c
Normal file
@ -0,0 +1,63 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "py/runtime.h"
|
||||
#include "py/ringbuf.h"
|
||||
#include "py/mphal.h"
|
||||
#include "uart.h"
|
||||
|
||||
#include "hardware/uart.h"
|
||||
#include "hardware/irq.h"
|
||||
#include "hardware/regs/uart.h"
|
||||
|
||||
#if MICROPY_HW_ENABLE_UART_REPL
|
||||
|
||||
void uart_irq(void) {
|
||||
uart_get_hw(uart_default)->icr = UART_UARTICR_BITS; // clear interrupt flags
|
||||
if (uart_is_readable(uart_default)) {
|
||||
int c = uart_getc(uart_default);
|
||||
#if MICROPY_KBD_EXCEPTION
|
||||
if (c == mp_interrupt_char) {
|
||||
mp_keyboard_interrupt();
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
ringbuf_put(&stdin_ringbuf, c);
|
||||
}
|
||||
}
|
||||
|
||||
void mp_uart_init(void) {
|
||||
uart_get_hw(uart_default)->imsc = UART_UARTIMSC_BITS; // enable mask
|
||||
uint irq_num = uart_get_index(uart_default) ? UART1_IRQ : UART0_IRQ;
|
||||
irq_set_exclusive_handler(irq_num, uart_irq);
|
||||
irq_set_enabled(irq_num, true); // enable irq
|
||||
}
|
||||
|
||||
void mp_uart_write_strn(const char *str, size_t len) {
|
||||
uart_write_blocking(uart_default, (const uint8_t *)str, len);
|
||||
}
|
||||
|
||||
#endif
|
32
ports/rp2/uart.h
Normal file
32
ports/rp2/uart.h
Normal file
@ -0,0 +1,32 @@
|
||||
/*
|
||||
* This file is part of the MicroPython project, http://micropython.org/
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020-2021 Damien P. George
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
#ifndef MICROPY_INCLUDED_RP2_UART_H
|
||||
#define MICROPY_INCLUDED_RP2_UART_H
|
||||
|
||||
void mp_uart_init(void);
|
||||
void mp_uart_write_strn(const char *str, size_t len);
|
||||
|
||||
#endif // MICROPY_INCLUDED_RP2_UART_H
|
Loading…
Reference in New Issue
Block a user