Unverified Commit ce7d337c authored by Earle F. Philhower, III's avatar Earle F. Philhower, III Committed by GitHub

Add SoftwareSerial-like PIO based UARTs (#391)

Adds support to the core for PIO-based, software-created UARTs, up to 8 (the number of PIO state machines) possible.

By using a custom program on the PIO state machines, it allows for very high bit rates and does not require CPU or interrupts.

Bit widths from 5- to 8-bits, 1 or 2 stop bits, and even/odd/none parity are supported.
parent dc1198bd
......@@ -97,6 +97,7 @@ void analogWriteResolution(int res);
#include "SerialUART.h"
#include "RP2040Support.h"
#include "SerialPIO.h"
#include "Bootsel.h"
// Template which will evaluate at *compile time* to a single 32b number
......
/*
Serial-over-PIO for the Raspberry Pi Pico RP2040
Copyright (c) 2021 Earle F. Philhower, III <earlephilhower@yahoo.com>
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "SerialPIO.h"
#include "CoreMutex.h"
#include <hardware/gpio.h>
#include <map>
#include "pio_uart.pio.h"
// ------------------------------------------------------------------------
// -- Generates a unique program for differing bit lengths
static std::map<int, PIOProgram*> _txMap;
static std::map<int, PIOProgram*> _rxMap;
// Duplicate a program and replace the first insn with a "set x, repl"
static pio_program_t *pio_make_uart_prog(int repl, const pio_program_t *pg) {
pio_program_t *p = new pio_program_t;
p->length = pg->length;
p->origin = pg->origin;
uint16_t *insn = (uint16_t *)malloc(p->length * 2);
memcpy(insn, pg->instructions, p->length * 2);
insn[0] = pio_encode_set(pio_x, repl);
p->instructions = insn;
return p;
}
static PIOProgram *_getTxProgram(int bits) {
auto f = _txMap.find(bits);
if (f == _txMap.end()) {
pio_program_t * p = pio_make_uart_prog(bits, &pio_tx_program);
_txMap.insert({bits, new PIOProgram(p)});
f = _txMap.find(bits);
}
return f->second;
}
static PIOProgram *_getRxProgram(int bits) {
auto f = _rxMap.find(bits);
if (f == _rxMap.end()) {
pio_program_t * p = pio_make_uart_prog(bits, &pio_rx_program);
_rxMap.insert({bits, new PIOProgram(p)});
f = _rxMap.find(bits);
}
return f->second;
}
// ------------------------------------------------------------------------
// TODO - this works, but there must be a faster/better way...
static int _parity(int bits, int data) {
int p = 0;
for (int b = 0; b < bits; b++) {
p ^= (data & (1 << b)) ? 1 : 0;
}
return p;
}
// We need to cache generated SerialPIOs so we can add data to them from
// the shared handler
static SerialPIO *_pioSP[2][4];
static void __not_in_flash_func(_fifoIRQ)() {
for (int p = 0; p < 2; p++) {
for (int sm = 0; sm < 4; sm++) {
SerialPIO *s = _pioSP[p][sm];
if (s) {
s->_handleIRQ();
pio_interrupt_clear((p == 0) ? pio0 : pio1, sm);
}
}
}
}
void __not_in_flash_func(SerialPIO::_handleIRQ)() {
if (_rx == NOPIN) {
return;
}
while ((!pio_sm_is_rx_fifo_empty(_rxPIO, _rxSM)) && ((_writer + 1) % sizeof(_queue) != _reader)) {
uint32_t decode = _rxPIO->rxf[_rxSM];
decode >>= 32 - _rxBits;
uint32_t val = 0;
for (int b = 0; b < _bits + 1; b++) {
val |= (decode & (1 << (b * 2))) ? 1 << b : 0;
}
if (_parity == UART_PARITY_EVEN) {
int p = ::_parity(_bits, val);
int r = (val & (1 << _bits)) ? 1 : 0;
if (p != r) {
// TODO - parity error
continue;
}
} else if (_parity == UART_PARITY_ODD) {
int p = ::_parity(_bits, val);
int r = (val & (1 << _bits)) ? 1 : 0;
if (p == r) {
// TODO - parity error
continue;
}
}
_queue[_writer] = val & ((1 << _bits) - 1);
asm volatile("" ::: "memory"); // Ensure the queue is written before the written count advances
_writer = (_writer + 1) % sizeof(_queue);
}
}
SerialPIO::SerialPIO(pin_size_t tx, pin_size_t rx) {
_tx = tx;
_rx = rx;
mutex_init(&_mutex);
}
void SerialPIO::begin(unsigned long baud, uint16_t config) {
_baud = baud;
switch (config & SERIAL_PARITY_MASK) {
case SERIAL_PARITY_EVEN:
_parity = UART_PARITY_EVEN;
break;
case SERIAL_PARITY_ODD:
_parity = UART_PARITY_ODD;
break;
default:
_parity = UART_PARITY_NONE;
break;
}
switch (config & SERIAL_STOP_BIT_MASK) {
case SERIAL_STOP_BIT_1:
_stop = 1;
break;
default:
_stop = 2;
break;
}
switch (config & SERIAL_DATA_MASK) {
case SERIAL_DATA_5:
_bits = 5;
break;
case SERIAL_DATA_6:
_bits = 6;
break;
case SERIAL_DATA_7:
_bits = 7;
break;
default:
_bits = 8;
break;
}
if ((_tx == NOPIN) && (_rx == NOPIN)) {
DEBUGCORE("ERROR: No pins specified for SerialPIO\n");
return;
}
if (_tx != NOPIN) {
_txBits = _bits + _stop + (_parity != UART_PARITY_NONE ? 1 : 0) + 1/*start bit*/;
_txPgm = _getTxProgram(_txBits);
int off;
if (!_txPgm->prepare(&_txPIO, &_txSM, &off)) {
DEBUGCORE("ERROR: Unable to allocate PIO TX UART, out of PIO resources\n");
// ERROR, no free slots
return;
}
digitalWrite(_tx, HIGH);
pinMode(_tx, OUTPUT);
pio_tx_program_init(_txPIO, _txSM, off, _tx);
pio_sm_clear_fifos(_txPIO, _txSM); // Remove any existing data
// Put the divider into ISR w/o using up program space
pio_sm_put_blocking(_txPIO, _txSM, clock_get_hz(clk_sys) / _baud - 2);
pio_sm_exec(_txPIO, _txSM, pio_encode_pull(false, false));
pio_sm_exec(_txPIO, _txSM, pio_encode_mov(pio_isr, pio_osr));
// Start running!
pio_sm_set_enabled(_txPIO, _txSM, true);
}
if (_rx != NOPIN) {
_writer = 0;
_reader = 0;
_rxBits = 2 * (_bits + _stop + (_parity != UART_PARITY_NONE ? 1 : 0) + 1);
_rxPgm = _getRxProgram(_rxBits);
int off;
if (!_rxPgm->prepare(&_rxPIO, &_rxSM, &off)) {
DEBUGCORE("ERROR: Unable to allocate PIO RX UART, out of PIO resources\n");
return;
}
// Stash away the created RX port for the IRQ handler
_pioSP[pio_get_index(_rxPIO)][_rxSM] = this;
pinMode(_rx, INPUT);
pio_rx_program_init(_rxPIO, _rxSM, off, _rx);
pio_sm_clear_fifos(_rxPIO, _rxSM); // Remove any existing data
// Put phase divider into OSR w/o using add'l program memory
pio_sm_put_blocking(_rxPIO, _rxSM, clock_get_hz(clk_sys) / (_baud * 2) - 2);
pio_sm_exec(_rxPIO, _rxSM, pio_encode_pull(false, false));
// Enable interrupts on rxfifo
switch (_rxSM) {
case 0: pio_set_irq0_source_enabled(_rxPIO, pis_sm0_rx_fifo_not_empty, true); break;
case 1: pio_set_irq0_source_enabled(_rxPIO, pis_sm1_rx_fifo_not_empty, true); break;
case 2: pio_set_irq0_source_enabled(_rxPIO, pis_sm2_rx_fifo_not_empty, true); break;
case 3: pio_set_irq0_source_enabled(_rxPIO, pis_sm3_rx_fifo_not_empty, true); break;
}
irq_set_exclusive_handler(PIO0_IRQ_0, _fifoIRQ);
irq_set_enabled(PIO0_IRQ_0, true);
pio_sm_set_enabled(_rxPIO, _rxSM, true);
}
_running = true;
}
void SerialPIO::end() {
if (!_running) {
return;
}
// TODO: Deallocate PIO resources, stop them
_pioSP[pio_get_index(_rxPIO)][_rxSM] = nullptr;
_running = false;
}
int SerialPIO::peek() {
CoreMutex m(&_mutex);
if (!_running || !m || (_rx == NOPIN)) {
return -1;
}
// If there's something in the FIFO now, just peek at it
uint32_t start = millis();
uint32_t now = millis();
while ((now - start) < _timeout) {
if (_writer != _reader) {
return _queue[_reader];
}
delay(1);
now = millis();
}
return -1; // Nothing available before timeout
}
int SerialPIO::read() {
CoreMutex m(&_mutex);
if (!_running || !m || (_rx == NOPIN)) {
return -1;
}
uint32_t start = millis();
uint32_t now = millis();
while ((now - start) < _timeout) {
if (_writer != _reader) {
auto ret = _queue[_reader];
_reader = (_reader + 1) % sizeof(_queue);
return ret;
}
delay(1);
now = millis();
}
return -1; // Timeout
}
int SerialPIO::available() {
CoreMutex m(&_mutex);
if (!_running || !m || (_rx == NOPIN)) {
return 0;
}
return (_writer - _reader) % sizeof(_queue);
}
int SerialPIO::availableForWrite() {
CoreMutex m(&_mutex);
if (!_running || !m || (_tx == NOPIN)) {
return 0;
}
return 8 - pio_sm_get_tx_fifo_level(_txPIO, _txSM);
}
void SerialPIO::flush() {
CoreMutex m(&_mutex);
if (!_running || !m || (_tx == NOPIN)) {
return;
}
while (!pio_sm_is_tx_fifo_empty(_txPIO, _txSM)) {
delay(1); // Wait for all FIFO to be read
}
// Could have 1 byte being transmitted, so wait for bit times
delay((1000 * (_txBits + 1)) / _baud);
}
size_t SerialPIO::write(uint8_t c) {
CoreMutex m(&_mutex);
if (!_running || !m || (_tx == NOPIN)) {
return 0;
}
uint32_t val = c;
if (_parity == UART_PARITY_NONE) {
val |= 7 << _bits; // Set 2 stop bits, the HW will only transmit the required number
} else if (_parity == UART_PARITY_EVEN) {
val |= ::_parity(_bits, c) << _bits;
val |= 7 << (_bits + 1);
} else {
val |= (1 ^ ::_parity(_bits, c)) << _bits;
val |= 7 << (_bits + 1);
}
val <<= 1; // Start bit = low
pio_sm_put_blocking(_txPIO, _txSM, val);
return 1;
}
SerialPIO::operator bool() {
return _running;
}
/*
Serial-over-PIO for the Raspberry Pi Pico RP2040
Copyright (c) 2021 Earle F. Philhower, III <earlephilhower@yahoo.com>
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#pragma once
#include <Arduino.h>
#include "api/HardwareSerial.h"
#include <stdarg.h>
#include <queue>
#include <hardware/uart.h>
#include "CoreMutex.h"
extern "C" typedef struct uart_inst uart_inst_t;
class SerialPIO : public HardwareSerial {
public:
static const pin_size_t NOPIN = 0xff; // Use in constructor to disable RX or TX unit
SerialPIO(pin_size_t tx, pin_size_t rx);
void begin(unsigned long baud = 115200) override {
begin(baud, SERIAL_8N1);
};
void begin(unsigned long baud, uint16_t config) override;
void end() override;
virtual int peek() override;
virtual int read() override;
virtual int available() override;
virtual int availableForWrite() override;
virtual void flush() override;
virtual size_t write(uint8_t c) override;
using Print::write;
operator bool() override;
// Not to be called by users, only from the IRQ handler. In public so that the C-language IQR callback can access it
void _handleIRQ();
private:
bool _running = false;
pin_size_t _tx, _rx;
int _baud;
int _bits;
uart_parity_t _parity;
int _stop;
mutex_t _mutex;
PIOProgram *_txPgm;
PIO _txPIO;
int _txSM;
int _txBits;
PIOProgram *_rxPgm;
PIO _rxPIO;
int _rxSM;
int _rxBits;
// Lockless, IRQ-handled circular queue
uint32_t _writer;
uint32_t _reader;
uint8_t _queue[32];
};
; pio_uart for the Raspberry Pi Pico RP2040
;
; Based loosely off of the uart_rx/_tx examples in the pico-examples repo,
; but heavily modified and optimized to not require changing the PIO
; clocks so that Tone and Servo won't be affected, and multiple different
; PIO ports can be run at different baud at the same time.
;
; Copyright (c) 2021 Earle F. Philhower, III <earlephilhower@yahoo.com>
;
; This library is free software; you can redistribute it and/or
; modify it under the terms of the GNU Lesser General Public
; License as published by the Free Software Foundation; either
; version 2.1 of the License, or (at your option) any later version.
;
; This library is distributed in the hope that it will be useful,
; but WITHOUT ANY WARRANTY; without even the implied warranty of
; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
; Lesser General Public License for more details.
;
; You should have received a copy of the GNU Lesser General Public
; License along with this library; if not, write to the Free Software
; Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
.program pio_tx
.side_set 1 opt
; We shift out the start and stop bit as part of the FIFO
set x, 9
pull side 1 ; Force stop bit
; Send the bits
bitloop:
out pins, 1
mov y, isr ; ISR is loaded by the setup routine with the period-1 count
wait_bit:
jmp y-- wait_bit
jmp x-- bitloop
% c-sdk {
static inline void pio_tx_program_init(PIO pio, uint sm, uint offset, uint pin_tx) {
// Tell PIO to initially drive output-high on the selected pin, then map PIO
// onto that pin with the IO muxes.
pio_sm_set_pins_with_mask(pio, sm, 1u << pin_tx, 1u << pin_tx);
pio_sm_set_pindirs_with_mask(pio, sm, 1u << pin_tx, 1u << pin_tx);
pio_gpio_init(pio, pin_tx);
pio_sm_config c = pio_tx_program_get_default_config(offset);
// OUT shifts to right, no autopull
sm_config_set_out_shift(&c, true, false, 32);
// We are mapping both OUT and side-set to the same pin, because sometimes
// we need to assert user data onto the pin (with OUT) and sometimes
// assert constant values (start/stop bit)
sm_config_set_out_pins(&c, pin_tx, 1);
sm_config_set_sideset_pins(&c, pin_tx);
// We only need TX, so get an 8-deep FIFO!
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX);
pio_sm_init(pio, sm, offset, &c);
}
%}
.program pio_rx
; IN pin 0 and JMP pin are both mapped to the GPIO used as UART RX.
start:
set x, 18 ; Preload bit counter...we'll shift in the start bit and stop bit, and each bit will be double-recorded (to be fixed by RP2040 code)
wait 0 pin 0 ; Stall until start bit is asserted
bitloop:
; Delay until 1/2 way into the bit time
mov y, osr
wait_half:
jmp y-- wait_half
; Read in the bit
in pins, 1 ; Shift data bit into ISR
jmp x-- bitloop ; Loop all bits
push ; Stuff it and wait for next start
% c-sdk {
static inline void pio_rx_program_init(PIO pio, uint sm, uint offset, uint pin) {
pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false);
pio_gpio_init(pio, pin);
gpio_pull_up(pin);
pio_sm_config c = pio_rx_program_get_default_config(offset);
sm_config_set_in_pins(&c, pin); // for WAIT, IN
sm_config_set_jmp_pin(&c, pin); // for JMP
// Shift to right, autopull disabled
sm_config_set_in_shift(&c, true, false, 32);
pio_sm_init(pio, sm, offset, &c);
}
%}
// -------------------------------------------------- //
// This file is autogenerated by pioasm; do not edit! //
// -------------------------------------------------- //
#if !PICO_NO_HARDWARE
#include "hardware/pio.h"
#endif
// ------ //
// pio_tx //
// ------ //
#define pio_tx_wrap_target 0
#define pio_tx_wrap 5
static const uint16_t pio_tx_program_instructions[] = {
// .wrap_target
0xe029, // 0: set x, 9
0x98a0, // 1: pull block side 1
0x6001, // 2: out pins, 1
0xa046, // 3: mov y, isr
0x0084, // 4: jmp y--, 4
0x0042, // 5: jmp x--, 2
// .wrap
};
#if !PICO_NO_HARDWARE
static const struct pio_program pio_tx_program = {
.instructions = pio_tx_program_instructions,
.length = 6,
.origin = -1,
};
static inline pio_sm_config pio_tx_program_get_default_config(uint offset) {
pio_sm_config c = pio_get_default_sm_config();
sm_config_set_wrap(&c, offset + pio_tx_wrap_target, offset + pio_tx_wrap);
sm_config_set_sideset(&c, 2, true, false);
return c;
}
static inline void pio_tx_program_init(PIO pio, uint sm, uint offset, uint pin_tx) {
// Tell PIO to initially drive output-high on the selected pin, then map PIO
// onto that pin with the IO muxes.
pio_sm_set_pins_with_mask(pio, sm, 1u << pin_tx, 1u << pin_tx);
pio_sm_set_pindirs_with_mask(pio, sm, 1u << pin_tx, 1u << pin_tx);
pio_gpio_init(pio, pin_tx);
pio_sm_config c = pio_tx_program_get_default_config(offset);
// OUT shifts to right, no autopull
sm_config_set_out_shift(&c, true, false, 32);
// We are mapping both OUT and side-set to the same pin, because sometimes
// we need to assert user data onto the pin (with OUT) and sometimes
// assert constant values (start/stop bit)
sm_config_set_out_pins(&c, pin_tx, 1);
sm_config_set_sideset_pins(&c, pin_tx);
// We only need TX, so get an 8-deep FIFO!
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX);
pio_sm_init(pio, sm, offset, &c);
}
#endif
// ------ //
// pio_rx //
// ------ //
#define pio_rx_wrap_target 0
#define pio_rx_wrap 6
static const uint16_t pio_rx_program_instructions[] = {
// .wrap_target
0xe032, // 0: set x, 18
0x2020, // 1: wait 0 pin, 0
0xa047, // 2: mov y, osr
0x0083, // 3: jmp y--, 3
0x4001, // 4: in pins, 1
0x0042, // 5: jmp x--, 2
0x8020, // 6: push block
// .wrap
};
#if !PICO_NO_HARDWARE
static const struct pio_program pio_rx_program = {
.instructions = pio_rx_program_instructions,
.length = 7,
.origin = -1,
};
static inline pio_sm_config pio_rx_program_get_default_config(uint offset) {
pio_sm_config c = pio_get_default_sm_config();
sm_config_set_wrap(&c, offset + pio_rx_wrap_target, offset + pio_rx_wrap);
return c;
}
static inline void pio_rx_program_init(PIO pio, uint sm, uint offset, uint pin) {
pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false);
pio_gpio_init(pio, pin);
gpio_pull_up(pin);
pio_sm_config c = pio_rx_program_get_default_config(offset);
sm_config_set_in_pins(&c, pin); // for WAIT, IN
sm_config_set_jmp_pin(&c, pin); // for JMP
// Shift to right, autopull disabled
sm_config_set_in_shift(&c, true, false, 32);
pio_sm_init(pio, sm, offset, &c);
}
#endif
......@@ -29,6 +29,7 @@ For the latest version, always check https://github.com/earlephilhower/arduino-p
EEPROM <eeprom>
I2S Audio <i2s>
Serial USB and UARTs <serial>
"Software Serial" PIO UART <piouart>
Servo <servo>
SPI <spi>
Wire(I2C) <wire>
......
"SoftwareSerial" PIO-based UART
================================
Equivalent to the Arduino SoftwareSerial library, an emulated UART using
one or two PIO state machines is included in the Arduino-Pico core. This
allows for up to 8 additional serial ports to be run from the RP2040 without
requiring additional CPU resources.
Instantiate a ``SerialPIO(txpin, rxpin)`` object in your sketch and then
use it the same as any other serial port. Even, odd, and no parity modes
are supported, as well as data sizes from 5- to 8-bits.
To instantiate only a serial transmit or receive unit, pass in
``SerialPIO::NOPIN`` as the ``txpin`` or ``rxpin``.
For example, to make a transmit-only port on GP16
.. code:: cpp
SerialPIO transmitter( 16, SerialPIO::NOPIN );
For detailed information about the Serial ports, see the
Arduino `Serial Reference <https://www.arduino.cc/reference/en/language/functions/communication/serial/>`_ .
......@@ -33,3 +33,4 @@ resumeOtherCore KEYWORD2
PIOProgram KEYWORD2
prepare KEYWORD2
SerialPIO KEYWORD2
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment