pio_rp2350.cpp

// ---------------------------------------------
//           This file is part of
//      _  _   __    _   _    __    __
//     ( \/ ) /__\  ( )_( )  /__\  (  )
//      \  / /(__)\  ) _ (  /(__)\  )(__
//      (__)(__)(__)(_) (_)(__)(__)(____)
//
//     Yet Another HW Abstraction Library
//      Copyright (C) Andreas Terstegge
//      BSD Licensed (see file LICENSE)
//
// ---------------------------------------------
//
// PIO driver for RP2350.
//
#include <cassert>
#include "pio_rp2350.h"
#include "system_rp2350.h"
 
using namespace _PIO0_;
using namespace _PIO1_;
using namespace _PIO2_;
using namespace _RESETS_;
 
// The three PIO instances
pio_rp2350 pio_rp2350::pio0(PIO0, 0);
pio_rp2350 pio_rp2350::pio1(PIO1, 1);
pio_rp2350 pio_rp2350::pio2(PIO2, 2);
 
function<void()> pio_rp2350::_handler_pio0[16] = { nullptr };
function<void()> pio_rp2350::_handler_pio1[16] = { nullptr };
function<void()> pio_rp2350::_handler_pio2[16] = { nullptr };
 
void SM_regs::init() {
    // Initialize SM registers with reset values;
    SM_CLKDIV                 = 0;
    SM_CLKDIV.INT             = 1;
    SM_EXECCTRL               = 0;
    SM_EXECCTRL.WRAP_TOP      = 0x1f;
    SM_SHIFTCTRL              = 0;
    SM_SHIFTCTRL.OUT_SHIFTDIR = 1;
    SM_SHIFTCTRL.IN_SHIFTDIR  = 1;
    SM_PINCTRL                = 0;
    SM_PINCTRL.SET_COUNT      = 5;
}
 
void SM::setRegister(out_dest_t reg, uint32_t val,
                     uint8_t offset, uint8_t size) {
    // Store current configuration
    bool en = isEnabled();
    disable();
    uint8_t out_base  = regs.SM_PINCTRL.OUT_BASE;
    uint8_t out_count = regs.SM_PINCTRL.OUT_COUNT;
    // Configure bits to write
    regs.SM_PINCTRL.OUT_BASE  = offset;
    regs.SM_PINCTRL.OUT_COUNT = size;
    // Empty the TX Fifo
    while(pio.FLEVEL & (0xf << (sm_index << 3))) {
        execute( op_PULL(0, 0) );
    }
    // Write value to TX Fifo first.
    // Then pull it and write it to destination.
    pio.TXF[sm_index] = val;
    execute( op_PULL(0, 0)      );
    execute( op_OUT (0, reg, 0) );
    // Restore original state
    regs.SM_PINCTRL.OUT_BASE  = out_base;
    regs.SM_PINCTRL.OUT_COUNT = out_count;
    if (en) enable();
}
 
void SM::setClock(uint32_t hz) {
    regs.SM_CLKDIV.INT  =   CLK_SYS / hz;
    uint64_t frac       = ((CLK_SYS % hz) << 8);
    //frac               += hz/2;
    frac               /= hz;
    regs.SM_CLKDIV.FRAC = frac;
}
 
void SM::attachIrq(const function<void()>& handler) const {
    switch(pio_index) {
        case 0:
            pio_rp2350::_handler_pio0[sm_index + 8] = handler;
            break;
        case 1:
            pio_rp2350::_handler_pio1[sm_index + 8] = handler;
            break;
        case 2:
            pio_rp2350::_handler_pio2[sm_index + 8] = handler;
            break;
    }
}
 
void SM::enableIrq() {
    pio_set.IRQ0_INTE = 1 << (sm_index + 8);
}
 
void SM::disableIrq() {
    pio_clr.IRQ0_INTE = 1 << (sm_index + 8);
}
 
void SM::attachTXNFULLIrq(const function<void()>& handler) const {
    if (pio_index)
        pio_rp2350::_handler_pio1[sm_index + 4] = handler;
    else
        pio_rp2350::_handler_pio0[sm_index + 4] = handler;
}
 
void SM::enableTXNFULLIrq() {
    pio_set.IRQ0_INTE = 1 << (sm_index + 4);
}
 
void SM::disableTXNFULLIrq() {
    pio_clr.IRQ0_INTE = 1 << (sm_index + 4);
}
 
void SM::attachRXNEMPTYIrq(const function<void()>& handler) const {
    if (pio_index)
        pio_rp2350::_handler_pio1[sm_index + 0] = handler;
    else
        pio_rp2350::_handler_pio0[sm_index + 0] = handler;
}
 
void SM::enableRXNEMPTYIrq() {
    pio_set.IRQ0_INTE = 1 << (sm_index);
}
 
void SM::disableRXNEMPTYIrq() {
    pio_clr.IRQ0_INTE = 1 << (sm_index);
}
 
pio_rp2350::pio_rp2350(PIO0_t & pio, uint8_t pio_index)
: _pio(pio),
  _pio_xor(*(PIO0_t *)((uint8_t *)&pio + 0x1000)),
  _pio_set(*(PIO0_t *)((uint8_t *)&pio + 0x2000)),
  _pio_clr(*(PIO0_t *)((uint8_t *)&pio + 0x3000)),
  _pio_index(pio_index),
  _next_free_addr(0) {
 
    // Get PIO out of reset state
    if (&pio == &PIO0) {
        RESETS_CLR.RESET.PIO0 <<= 1;
    } else if (&pio == &PIO1) {
        RESETS_CLR.RESET.PIO1 <<= 1;
    } else if (&pio == &PIO2) {
        RESETS_CLR.RESET.PIO2 <<= 1;
    } else {
        assert(false);
    }
 
    // Set base address of SM register banks
    _sm_regbanks = (SM_regs *)&pio.SM0_CLKDIV;
    // Disable and restart all state machines
    _pio.CTRL.SM_ENABLE  = 0x0;
    _pio.CTRL.SM_RESTART = 0xf;
    // Erase instruction memory
    for(auto & mem : _pio.INSTR_MEM) {
        mem = 0;
    }
    // Enable PIO interrupts in NVIC
    NVIC_EnableIRQ(PIO0_IRQ_0_IRQn);
    NVIC_EnableIRQ(PIO1_IRQ_0_IRQn);
    NVIC_EnableIRQ(PIO2_IRQ_0_IRQn);
}
 
pio_rp2350::~pio_rp2350() {
    // Disable all state machines
    _pio.CTRL.SM_ENABLE  = 0x0;
}
 
SM* pio_rp2350::loadProgram(const pio_program & prgm) {
    // Find a free state machine
    uint8_t sm_index;
    for(sm_index=0; sm_index < 4; ++sm_index) {
        if (!_in_use[sm_index]) break;
    }
    assert(sm_index < 4);
    // Set 'used' marker
    _in_use[sm_index] = true;
    // Check program origin
    uint8_t load_at_addr;
    if (prgm.origin == -1) {
        // Reloctable program
        load_at_addr = _next_free_addr;
    } else {
        // Absolute origin
        assert(prgm.origin >= _next_free_addr);
        load_at_addr = prgm.origin;
    }
    // Check program size
    assert((load_at_addr + prgm.length) <= 32);
    // Load the PIO program
    for(int i=0; i < prgm.length; i++) {
        uint16_t inst = prgm.instructions[i];
        // Relocate JMP instruction if needed
        if (is_JMP(inst) && (prgm.origin == -1)) {
            inst += load_at_addr;
        }
        _pio.INSTR_MEM[i + load_at_addr] = inst;
    }
    // Generate a state machine and initialize it
    SM* sm = new SM(_pio, _pio_set, _pio_clr, _pio_index, sm_index,
                    load_at_addr, _sm_regbanks[sm_index]);
    // Set the initial PC
    sm->setRegister(out_dest_t::PC, load_at_addr);
    // Update the next free addr
    _next_free_addr += load_at_addr + prgm.length;
    // Return the state machine in charge
    return sm;
}
 
extern "C" {
    void PIO0_IRQ_0_Handler(void) {
        uint32_t status = PIO0.IRQ0_INTS;
        while (uint8_t pos = __builtin_ffs((int)status)) {
            status ^= 1 << --pos;
            if (pos > 7) {
                PIO0.IRQ = 1 << (pos - 8);
            }
            if (pio_rp2350::_handler_pio0[pos])
                pio_rp2350::_handler_pio0[pos]();
        }
    }
 
    void PIO1_IRQ_0_Handler(void) {
        uint32_t status = PIO1.IRQ0_INTS;
        while (uint8_t pos = __builtin_ffs((int)status)) {
            status ^= 1 << --pos;
            if (pos > 7) {
                PIO1.IRQ = 1 << (pos - 8);
            }
            if (pio_rp2350::_handler_pio1[pos])
                pio_rp2350::_handler_pio1[pos]();
        }
    }
 
    void PIO2_IRQ_0_Handler(void) {
        uint32_t status = PIO2.IRQ0_INTS;
        while (uint8_t pos = __builtin_ffs((int)status)) {
            status ^= 1 << --pos;
            if (pos > 7) {
                PIO2.IRQ = 1 << (pos - 8);
            }
            if (pio_rp2350::_handler_pio2[pos])
                pio_rp2350::_handler_pio2[pos]();
        }
    }
}