From c796052a90e22d15c6c233c5396843493ed0ea5d Mon Sep 17 00:00:00 2001
From: andryblack <blackicebox@gmail.com>
Date: Wed, 23 Oct 2024 20:25:54 +0300
Subject: [PATCH] [rp] PIO with compile-time assembler

---
 examples/rp_pico/pio_ws2812/main.cpp     | 150 ++++++
 examples/rp_pico/pio_ws2812/project.xml  |  10 +
 src/modm/platform/pio/rp/module.lb       |  61 +++
 src/modm/platform/pio/rp/pio.cpp.in      |  39 ++
 src/modm/platform/pio/rp/pio.hpp.in      |  98 ++++
 src/modm/platform/pio/rp/pio_asm.hpp     | 564 +++++++++++++++++++++++
 src/modm/platform/pio/rp/pio_program.hpp |  33 ++
 src/modm/platform/pio/rp/pio_sm.hpp      | 233 ++++++++++
 8 files changed, 1188 insertions(+)
 create mode 100644 examples/rp_pico/pio_ws2812/main.cpp
 create mode 100644 examples/rp_pico/pio_ws2812/project.xml
 create mode 100644 src/modm/platform/pio/rp/module.lb
 create mode 100644 src/modm/platform/pio/rp/pio.cpp.in
 create mode 100644 src/modm/platform/pio/rp/pio.hpp.in
 create mode 100644 src/modm/platform/pio/rp/pio_asm.hpp
 create mode 100644 src/modm/platform/pio/rp/pio_program.hpp
 create mode 100644 src/modm/platform/pio/rp/pio_sm.hpp

diff --git a/examples/rp_pico/pio_ws2812/main.cpp b/examples/rp_pico/pio_ws2812/main.cpp
new file mode 100644
index 0000000000..a037541f67
--- /dev/null
+++ b/examples/rp_pico/pio_ws2812/main.cpp
@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) 2016, Sascha Schade
+ * Copyright (c) 2017, Niklas Hauser
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+// ----------------------------------------------------------------------------
+
+#include <modm/board.hpp>
+using namespace Board;
+
+/* from https://github.com/raspberrypi/pico-examples/blob/master/pio/ws2812/ws2812.pio */
+
+// constants WS2812
+// static constexpr uint32_t T0H = 350; // ns
+// static constexpr uint32_t T0L = 800;
+// static constexpr uint32_t T1H = 700;
+// static constexpr uint32_t T1L = 600;
+
+// constants WS2812B
+static constexpr uint32_t T0H = 400; // ns
+static constexpr uint32_t T0L = 850;
+static constexpr uint32_t T1H = 850;
+static constexpr uint32_t T1L = 400;
+
+static constexpr uint32_t TimeScale = 50;
+
+static constexpr uint32_t TH_Common = T0H/TimeScale;
+static constexpr uint32_t TH_Add = (T1H-T0H)/TimeScale;
+static constexpr uint32_t TL_Common = T1L/TimeScale;
+static constexpr uint32_t TL_Add = (T0L-T1L)/TimeScale;
+
+static constexpr uint32_t T3 = 12;//
+
+// labels
+struct bitloop {};
+struct do_one {};
+struct do_zero {};
+
+
+/*
+.wrap_target
+bitloop:
+    out x, 1       side 0 [T3 - 1] ; Side-set still takes place when instruction stalls
+    jmp !x do_zero side 1 [T1 - 1] ; Branch on the bit we shifted out. Positive pulse
+do_one:
+    jmp  bitloop   side 1 [T2 - 1] ; Continue driving high, for a long pulse
+do_zero:
+    nop            side 0 [T2 - 1] ; Or drive low, for a short pulse
+.wrap
+*/
+// PIO program
+static constexpr auto pio_prog = modm::platform::PIOProgram::begin()
+	.sideset<1>()
+	.wrapTarget()
+	.label<bitloop>()
+		.instr(pio::Out().x<1>()				.side<0>().delay<TL_Common-1>())
+		.instr(pio::Jmp().not_x().to<do_zero>()	.side<1>().delay<TH_Common-1>())
+	.label<do_one>()
+		.instr(pio::Jmp().to<bitloop>()			.side<1>().delay<TH_Add-1>())
+	.label<do_zero>()
+		.instr(pio::Nop()						.side<0>().delay<TL_Add-1>())
+	.wrap()
+	.end();
+
+
+struct HW
+{
+	using PIO = modm::platform::Pio0;
+	using PIO_SM = PIO::StateMachine<0>;
+	using DataGpio = modm::platform::GpioOutput23;
+};
+
+static inline void write(uint32_t d) {
+	while (HW::PIO_SM::txFifoFull()) {__NOP();}
+	HW::PIO_SM::write(d << 8);
+} 
+
+int
+main()
+{
+
+
+	Board::initialize();
+
+	
+	HW::DataGpio::setOutput(Gpio::OutputType::PushPull, Gpio::SlewRate::Fast);
+	HW::DataGpio::setDriveStrength(Gpio::DriveStrength::mA_12);
+	
+	auto pio_prog_offset = HW::PIO::addProgram(pio_prog);
+
+	HW::PIO::connect<HW::DataGpio::Pad>();
+
+	HW::PIO_SM::addOutput<HW::DataGpio>();
+
+	
+	HW::PIO_SM::config()
+		.setup(pio_prog_offset,pio_prog)
+		.setSidesetPins<HW::DataGpio,1,false,false>()
+		.setFifoJoinTx()
+		.setOutShift<false,true,24>()
+		.setFrequency<Board::SystemClock,1000000000/TimeScale>()
+		.init(pio_prog_offset+pio_prog.getOffset<bitloop>());
+
+	HW::PIO_SM::setEnabled(true);
+
+	constexpr auto delay_val = 5ms;
+
+	while (true)
+	{
+		uint32_t clr = 0;
+		while (clr!=0xff0000) {
+			clr = clr + 0x010000;
+			write(clr);
+			modm::delay(delay_val);
+		}
+		while (clr!=0x000000) {
+			clr = clr - 0x010000;
+			write(clr);
+			modm::delay(delay_val);
+		}
+		while (clr!=0x00ff00) {
+			clr = clr + 0x000100;
+			write(clr);
+			modm::delay(delay_val);
+		}
+		while (clr!=0x000000) {
+			clr = clr - 0x000100;
+			write(clr);
+			modm::delay(delay_val);
+		}
+		while (clr!=0x0000ff) {
+			clr = clr + 0x000001;
+			write(clr);
+			modm::delay(delay_val);
+		}
+		while (clr!=0x000000) {
+			clr = clr - 0x000001;
+			write(clr);
+			modm::delay(delay_val);
+		}
+
+	}
+
+	return 0;
+}
diff --git a/examples/rp_pico/pio_ws2812/project.xml b/examples/rp_pico/pio_ws2812/project.xml
new file mode 100644
index 0000000000..c932d97d93
--- /dev/null
+++ b/examples/rp_pico/pio_ws2812/project.xml
@@ -0,0 +1,10 @@
+<library>
+  <extends>modm:rp-pico</extends>
+  <options>
+    <option name="modm:build:build.path">../../../build/rp_pico/pio_ws2812</option>
+  </options>
+  <modules>
+    <module>modm:build:scons</module>
+    <module>modm:platform:pio:0</module>
+  </modules>
+</library>
diff --git a/src/modm/platform/pio/rp/module.lb b/src/modm/platform/pio/rp/module.lb
new file mode 100644
index 0000000000..22ff18f0c2
--- /dev/null
+++ b/src/modm/platform/pio/rp/module.lb
@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# Copyright (c) 2024, Andrey Kunitsyn
+#
+# This file is part of the modm project.
+#
+# This Source Code Form is subject to the terms of the Mozilla Public
+# License, v. 2.0. If a copy of the MPL was not distributed with this
+# file, You can obtain one at http://mozilla.org/MPL/2.0/.
+# -----------------------------------------------------------------------------
+
+
+class Instance(Module):
+    def __init__(self, instance):
+        self.instance = instance
+
+    def init(self, module):
+        module.name = str(self.instance)
+        module.description = "PIO {} instance".format(self.instance)
+
+    def prepare(self, module, options):
+        return True
+
+    def build(self, env):
+        properties = {
+            "id": self.instance,
+        }
+        env.substitutions = properties
+        env.outbasepath = "modm/src/modm/platform/pio"
+
+        env.template("pio.hpp.in", "pio_{}.hpp".format(self.instance))
+        env.template("pio.cpp.in", "pio_{}.cpp".format(self.instance))
+
+
+
+def init(module):
+    module.name = ":platform:pio"
+    module.description = "Programmable IO block (PIO)"
+
+def prepare(module, options):
+    device = options[":target"]
+    if not device.has_driver("pio:rp*"):
+        return False
+
+    module.depends(
+        ":platform:gpio",
+        ":platform:clockgen",
+        ":architecture:interrupt")
+
+    for instance in listify(device.get_driver("pio")["instance"]):
+        module.add_submodule(Instance(instance))
+
+    return True
+
+def build(env):
+    env.outbasepath = "modm/src/modm/platform/pio"
+    env.copy("pio_asm.hpp")
+    env.copy("pio_program.hpp")
+    env.copy("pio_sm.hpp")
+    pass
diff --git a/src/modm/platform/pio/rp/pio.cpp.in b/src/modm/platform/pio/rp/pio.cpp.in
new file mode 100644
index 0000000000..6113a4ba54
--- /dev/null
+++ b/src/modm/platform/pio/rp/pio.cpp.in
@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2022, Andrey Kunitsyn
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+// ----------------------------------------------------------------------------
+
+#include <modm/architecture/driver/atomic/queue.hpp>
+#include <modm/architecture/interface/atomic_lock.hpp>
+#include <modm/architecture/interface/interrupt.hpp>
+#include <modm/platform/core/resets.hpp>
+
+#include "../device.hpp"
+
+#include "pio_{{ id }}.hpp"
+
+// ----------------------------------------------------------------------------
+
+namespace modm::platform
+{
+    uint32_t Pio{{ id }}::used_instruction_space = 0;
+}
+
+
+
+// ----------------------------------------------------------------------------
+MODM_ISR(PIO{{ id }}_IRQ_0)
+{
+	
+}
+
+MODM_ISR(PIO{{ id }}_IRQ_1)
+{
+	
+}
diff --git a/src/modm/platform/pio/rp/pio.hpp.in b/src/modm/platform/pio/rp/pio.hpp.in
new file mode 100644
index 0000000000..7bffc1924c
--- /dev/null
+++ b/src/modm/platform/pio/rp/pio.hpp.in
@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2022, Andrey Kunitsyn
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+// ----------------------------------------------------------------------------
+
+#pragma once
+
+#include <modm/architecture/interface/peripheral.hpp>
+#include <modm/platform/core/peripherals.hpp>
+#include <modm/platform/gpio/connector.hpp>
+#include <modm/math/algorithm/prescaler.hpp>
+
+#include "pio_program.hpp"
+#include "pio_sm.hpp"
+#include <hardware/structs/pio.h>
+
+namespace modm::platform
+{
+
+
+    /**
+     * Programmable IO block (PIO)
+     *
+     * @ingroup	modm_platform_pio
+     * @author		Andrey Kunitsyn
+     */
+    class Pio{{ id }} : public ::modm::PeripheralDriver
+    {
+    	static inline pio_hw_t& pio() { return *pio{{ id }}_hw; }
+        
+        static uint32_t used_instruction_space;
+        static int findOffset(const PIOProgram& prg) {
+            uint32_t program_mask = (1u << prg.length) - 1;
+            if (prg.origin >= 0) {
+                if (prg.origin > 32 - prg.length) return -1;
+                return used_instruction_space & (program_mask << prg.origin) ? -1 : prg.origin;
+            } else {
+                // work down from the top always
+                for (int i = 32 - prg.length; i >= 0; i--) {
+                    if ((used_instruction_space & (program_mask << static_cast<unsigned int>(i)))==0) {
+                        return i;
+                    }
+                }
+                return -1;
+            }
+        }
+        static void addProgramAtOffset(size_t offset,const PIOProgram& prg) {
+            for (uint8_t i = 0; i < prg.length; ++i) {
+                auto& instr = prg.instructions[i];
+                pio().instr_mem[offset + i] = instr.store(offset);
+            }
+            uint32_t program_mask = (1u << prg.length) - 1;
+            used_instruction_space |= program_mask << offset;
+        }
+
+        template <typename Pio,size_t SM>
+        friend class pio::StateMachine;
+
+    public:
+        static constexpr Peripheral peripherial = Peripheral::Pio{{ id }};
+        template <size_t SM>
+        class StateMachine : public modm::platform::pio::StateMachine<Pio{{ id }},SM> {};
+
+        template< class... Signals >
+        static void
+        connect()
+        {
+            using Connector = GpioConnector<peripherial, Signals...>;
+            Connector::connect();
+        }
+
+        template< class Pin >
+        static void connectPin() {
+            using Connector = GpioConnector<peripherial, typename Pin::Pad>;
+            Connector::connect();
+        }
+
+        template <typename Program>
+        static size_t addProgram(const Program& prg_data) {
+            auto prg = PIOProgram::get(prg_data);
+            auto offset = findOffset(prg);
+            if (offset < 0) {
+                return offset;
+            }
+            auto res = static_cast<size_t>(offset);
+            addProgramAtOffset(res,prg);
+            return res;
+        }
+    };
+
+}	// namespace modm::platform
+
diff --git a/src/modm/platform/pio/rp/pio_asm.hpp b/src/modm/platform/pio/rp/pio_asm.hpp
new file mode 100644
index 0000000000..51410566bd
--- /dev/null
+++ b/src/modm/platform/pio/rp/pio_asm.hpp
@@ -0,0 +1,564 @@
+/*
+ * Copyright (c) 2022, Andrey Kunitsyn
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+// ----------------------------------------------------------------------------
+
+#pragma once
+
+#include <array>
+#include <cstdint>
+#include <cassert>
+
+namespace modm::platform::pio 
+{
+
+	struct Instruction {
+		uint16_t value;
+		constexpr Instruction(uint16_t val) : value(val) {}
+		constexpr uint16_t store(size_t offset) const {
+			return ((value & 0xe000) == 0x0000) ? (value + offset) : value;
+		}
+		constexpr Instruction operator | (uint16_t params) const {
+			return Instruction(value | params);
+		}
+		constexpr bool operator == (uint16_t val) const {
+			return value == val;
+		}
+	};
+
+	enum class Opcode : uint16_t {
+		JMP 	= 0x0000 << 13,
+		WAIT 	= 0x0001 << 13,
+		IN 		= 0x0002 << 13,
+		OUT 	= 0x0003 << 13,
+		PUSH 	= 0x0004 << 13,
+		PULL 	= (0x0004 << 13) | (0x0001 << 7),
+		MOV 	= 0x0005 << 13,
+		IRQ 	= 0x0006 << 13,
+		SET 	= 0x0007 << 13,
+	};
+
+	struct OptionalValue {
+		uint16_t value{0};
+		bool is_set{false};
+	};
+
+	static constexpr uint16_t NO_SIDE_VALUE = 0xffff;
+
+	template <uint16_t side = NO_SIDE_VALUE,uint16_t delay = 0>
+	struct SideDelay {
+		static constexpr const uint16_t side_value = (side == NO_SIDE_VALUE) ? 0 : side;
+		static constexpr const bool side_value_present = side != NO_SIDE_VALUE;
+		static constexpr const uint16_t delay_value = delay;
+	};
+
+	template <typename Res>
+	struct InstructionEncoding {
+		static constexpr const Opcode opcode = Res::opcode;
+		static constexpr const uint16_t DELAY_SIDESET_SHIFT = 8;
+		static constexpr const uint16_t DELAY_SIDESET_MASK = 0x1f << DELAY_SIDESET_SHIFT;
+		static constexpr const uint16_t ARG1_SHIFT = 5;
+
+		using SideDelayConfig = SideDelay<>;
+
+		template <uint16_t side = 0>
+		struct SideValue : Res {
+			using SideDelayConfig = SideDelay<side,0>;
+			constexpr SideValue() {}
+			template <uint16_t delay = 0>
+			struct DelayValue : Res {
+				constexpr DelayValue() {}
+				using SideDelayConfig = SideDelay<side,delay>;
+				template <typename Encoder>
+				constexpr Instruction encode(const Encoder& coder) const {
+					return Res::encode(coder) | (coder.template encodeSideDelay<SideDelayConfig>() << DELAY_SIDESET_SHIFT);
+				}
+			};
+			template <uint16_t val>
+			constexpr auto delay() {
+				return DelayValue<val>();
+			}
+			template <typename Encoder>
+			constexpr Instruction encode(const Encoder& coder) const {
+				return Res::encode(coder) | (coder.template encodeSideDelay<SideDelayConfig>() << DELAY_SIDESET_SHIFT);
+			}
+		};
+		template <uint16_t delay = 0>
+		struct DelayValue : Res {
+			constexpr DelayValue() {}
+			using SideDelayConfig = SideDelay<NO_SIDE_VALUE,delay>;
+			template <typename Encoder>
+			constexpr Instruction encode(const Encoder& coder) const {
+				return Res::encode(coder) | (coder.template encodeSideDelay<SideDelayConfig>() << DELAY_SIDESET_SHIFT);
+			}
+		};
+
+		constexpr InstructionEncoding()  {}
+		template <uint16_t val>
+		constexpr auto side() const {
+			return SideValue<val>();
+		}
+		template <uint16_t val>
+		constexpr auto delay() {
+			return DelayValue<val>();
+		}
+		template <typename Encoder>
+		constexpr Instruction encode(const Encoder& ) const {
+			return Instruction(uint16_t(opcode));
+		}
+	};
+
+	enum class JmpCondition : uint16_t {
+		ALWAYS 	= 0x00,
+		NOT_X 	= 0x01,
+		X_DEC 	= 0x02,
+		NOT_Y	= 0x03,
+		Y_DEC 	= 0x04,
+		X_NE_Y	= 0x05,
+		PIN 	= 0x06,
+		NOT_ORSE = 0x07,
+	};
+	template <typename Label = void,JmpCondition condition = JmpCondition::ALWAYS,uint16_t addr = 0x0000>
+	struct Jmp : InstructionEncoding<Jmp<Label,condition,addr>> {
+		static constexpr const Opcode opcode = Opcode::JMP;
+		static constexpr const JmpCondition condition_value = condition;
+		static constexpr const uint16_t addr_value = addr;
+		static_assert(addr_value < 0x0f, "Invalid addr");
+
+		using LabelName = Label;
+		using Base = InstructionEncoding<Jmp<Label,condition,addr>>;
+		
+		explicit constexpr Jmp() {}
+		static constexpr auto not_x() { return Jmp<LabelName,JmpCondition::NOT_X,addr_value>(); }
+		static constexpr auto x_dec() { return Jmp<LabelName,JmpCondition::X_DEC,addr_value>(); }
+		static constexpr auto not_y() { return Jmp<LabelName,JmpCondition::NOT_Y,addr_value>(); }
+		static constexpr auto y_dec() { return Jmp<LabelName,JmpCondition::Y_DEC,addr_value>(); }
+		static constexpr auto x_ne_y() { return Jmp<LabelName,JmpCondition::X_NE_Y,addr_value>(); }
+		static constexpr auto pin() { return Jmp<LabelName,JmpCondition::PIN,addr_value>(); }
+		static constexpr auto not_osre() { return Jmp<LabelName,JmpCondition::NOT_ORSE,addr_value>(); }
+
+		template <typename ToLabel>
+		static constexpr auto to() { return Jmp<ToLabel,condition_value,addr_value>(); }
+
+		template <typename Encoder>
+		static constexpr uint16_t getAddr(const Encoder& coder) {
+			if constexpr (std::is_same_v<Label,void>) {
+				return addr_value;
+			} else {
+				return coder.template getOffset<Label>();
+			}
+		}
+
+		template <typename Encoder>
+		constexpr Instruction encode(const Encoder& coder) const {
+			return Base::encode(coder) | (uint16_t(condition_value) << Base::ARG1_SHIFT) | getAddr(coder);
+		}
+
+	};
+	struct Wait : InstructionEncoding<Wait> {
+		static constexpr const Opcode opcode = Opcode::WAIT;
+		constexpr Wait() {}
+	};
+	struct In : InstructionEncoding<In> {
+		static constexpr const Opcode opcode = Opcode::IN;
+		constexpr In() : InstructionEncoding() {}
+	};
+	enum class OutDestination : uint16_t {
+		PINS 	= 0x00,
+		X 		= 0x01,
+		Y 		= 0x02,
+		NULL_T 	= 0x03,
+		PINDIRS = 0x04,
+		PC 		= 0x05,
+		ISR 	= 0x06,
+		EXEC 	= 0x07,
+	};
+	template <OutDestination destination = OutDestination::PINS,uint16_t bits = 1>
+	struct Out : InstructionEncoding<Out<destination,bits>> {
+		static constexpr const Opcode opcode = Opcode::OUT;
+		static constexpr OutDestination destination_value = destination;
+		static constexpr uint16_t bits_value = bits;
+		using Base = InstructionEncoding<Out<destination,bits>>;
+		static_assert(bits_value >= 1 && bits_value <= 32,"invalid shift bits");
+		constexpr Out() {}
+		template <uint16_t nbits>
+		constexpr auto pins() { return Out<OutDestination::PINS,nbits>(); }
+		template <uint16_t nbits>
+		constexpr auto x() { return Out<OutDestination::X,nbits>(); }
+		template <uint16_t nbits>
+		constexpr auto y() { return Out<OutDestination::Y,nbits>(); }
+		template <uint16_t nbits>
+		constexpr auto null() { return Out<OutDestination::NULL_T,nbits>(); }
+		template <uint16_t nbits>
+		constexpr auto pindirs() { return Out<OutDestination::PINDIRS,nbits>(); }
+		template <uint16_t nbits>
+		constexpr auto pc() { return Out<OutDestination::PC,nbits>(); }
+		template <uint16_t nbits>
+		constexpr auto isr() { return Out<OutDestination::ISR,nbits>(); }
+		template <uint16_t nbits>
+		constexpr auto exec() { return Out<OutDestination::EXEC,nbits>(); }
+		template <typename Encoder>
+		constexpr Instruction encode(const Encoder& coder) const {
+			return Base::encode(coder) | (uint16_t(destination_value) << Base::ARG1_SHIFT) | 
+				(bits_value == 32 ? 0 : bits_value);
+		}
+	};
+	// @todo
+	struct Push : InstructionEncoding<Push> {
+		static constexpr const Opcode opcode = Opcode::PUSH;
+		constexpr Push() {}
+	};
+	template <bool IfEmpty = false, bool Block = true>
+	struct Pull : InstructionEncoding<Pull<IfEmpty,Block>> {
+		static constexpr const Opcode opcode = Opcode::PULL;
+		using Base = InstructionEncoding<Pull<IfEmpty,Block>>;
+		static constexpr const bool ifempty_value = IfEmpty;
+		static constexpr const bool block_value = Block;
+		constexpr Pull()  {}
+		constexpr auto ifempty() { return Pull<true,block_value>(); }
+		constexpr auto block() { return Pull<ifempty_value,true>(); }
+		constexpr auto noblock() { return Pull<ifempty_value,false>(); }
+		template <typename Encoder>
+		constexpr Instruction encode(const Encoder& coder) const {
+			return Base::encode(coder) | (ifempty_value ? (1<<6) : 0) | (block_value ? (1<<5) : 0);
+		}
+	};
+
+	enum class MovDestination : uint16_t {
+		PINS 	= 0x00,
+		X 		= 0x01,
+		Y 		= 0x02,
+		EXEC 	= 0x04,
+		PC 		= 0x05,
+		ISR 	= 0x06,
+		OSR 	= 0x07,
+	};
+
+	enum class MovSource : uint16_t {
+		PINS 	= 0x00,
+		X 		= 0x01,
+		Y 		= 0x02,
+		NULL_T 	= 0x03,
+		STATUS 	= 0x05,
+		ISR 	= 0x06,
+		OSR 	= 0x07,
+	};
+
+	enum class MovOperation : uint16_t {
+		NONE 	= 0x00,
+		INVERT 	= 0x01,
+		REVERSE = 0x02,
+	};
+
+	struct Mov {
+		
+		template <MovDestination destination,MovOperation operation = MovOperation::NONE>
+		struct MovDst  {
+			template <MovSource source>
+			struct MovImpl : InstructionEncoding<MovImpl<source>> {
+				static constexpr const Opcode opcode = Opcode::MOV;
+				static constexpr MovSource source_value = source;
+				static constexpr MovDestination destination_value = destination;
+				static constexpr MovOperation operation_value = operation;
+
+				static constexpr size_t OP_SHIFT = 3;
+				static constexpr size_t DESTINATION_SHIFT = 5;
+				template <typename Encoder>
+				constexpr Instruction encode(const Encoder& coder) const {
+					return InstructionEncoding<MovImpl<source>>::encode(coder) | uint16_t(source_value) | 
+						(uint16_t(operation_value)<<OP_SHIFT) | 
+						(uint16_t(destination_value) << DESTINATION_SHIFT);
+				}
+			};
+			
+			static constexpr auto invert() { return MovDst<destination,MovOperation::INVERT>(); }
+			static constexpr auto reverse() { return MovDst<destination,MovOperation::REVERSE>(); }
+
+			static  constexpr auto pins() { return MovImpl<MovSource::PINS>(); }
+			static  constexpr auto x() { return MovImpl<MovSource::X>(); }
+			static  constexpr auto y() { return MovImpl<MovSource::Y>(); }
+			static  constexpr auto null() { return MovImpl<MovSource::NULL_T>(); }
+			static  constexpr auto status() { return MovImpl<MovSource::STATUS>(); }
+			static  constexpr auto isr() { return MovImpl<MovSource::ISR>(); }
+			static  constexpr auto osr() { return MovImpl<MovSource::OSR>(); }
+			
+		};
+
+		static constexpr auto pins()  { return MovDst<MovDestination::PINS>(); }
+		static constexpr auto x()  { return MovDst<MovDestination::X>(); }
+		static constexpr auto y()  { return MovDst<MovDestination::Y>(); }
+		static constexpr auto exec()  { return MovDst<MovDestination::EXEC>(); }
+		static constexpr auto pc()  { return MovDst<MovDestination::PC>(); }
+		static constexpr auto isr()  { return MovDst<MovDestination::ISR>(); }
+		static constexpr auto osr()  { return MovDst<MovDestination::OSR>(); }
+
+		
+	};
+	struct Irq : InstructionEncoding<Irq> {
+		static constexpr const Opcode opcode = Opcode::IRQ;
+		constexpr Irq() {}
+	};
+
+	enum class SetDestination : uint16_t{
+		PINS 	= 0x00,
+		X 		= 0x01,
+		Y 		= 0x02,
+		PINDIRS = 0x04,
+	};
+
+	struct Set {
+		template <SetDestination destination,uint16_t value>
+		struct SetImpl : InstructionEncoding<SetImpl<destination,value>> {
+			static_assert(value <= 31,"invalid data value");
+			static constexpr const Opcode opcode = Opcode::SET;
+			static constexpr SetDestination destination_value = destination;
+			static constexpr uint16_t data_value = value;
+			static constexpr size_t DESTINATION_SHIFT = 5;
+			template <typename Encoder>
+			constexpr Instruction encode(const Encoder& coder) const {
+				return InstructionEncoding<SetImpl<destination,value>>::encode(coder) | data_value | 
+					(uint16_t(destination_value) << DESTINATION_SHIFT);
+			}
+		};
+
+		constexpr Set()  {}
+		template <size_t data>
+		static constexpr auto pins() { return SetImpl<SetDestination::PINS,data>(); }
+		template <size_t data>
+		static constexpr auto x() { return SetImpl<SetDestination::X,data>(); }
+		template <size_t data>
+		static constexpr auto y() { return SetImpl<SetDestination::Y,data>(); }
+		template <size_t data>
+		static constexpr auto pindirs() { return SetImpl<SetDestination::PINDIRS,data>(); }
+		
+	};
+
+	static inline constexpr auto Nop() {
+		return Mov().y().y();
+	}
+
+	template <typename ...Labels>
+	struct LabelsList;
+
+	template <typename Settings,typename Labels,typename Instructions>
+	struct ProgramBuilder;
+	
+
+	template <typename Name,size_t Offset>
+	struct Label {
+		using name = Name;
+		static constexpr size_t offset = Offset;
+	};
+	
+	template <>
+	struct LabelsList<> {
+		template <typename Name,size_t offset>
+		struct add_s {
+			using result = LabelsList<Label<Name,offset> >;
+		};
+		template <typename Name,size_t offset>
+		using add = typename add_s<Name,offset>::result;
+
+		template <typename Name>
+		static constexpr bool exist() {
+			return false;
+		}
+	};
+
+	template <typename FirstLabel,typename ...Labels>
+	struct LabelsList<FirstLabel,Labels...> {
+		template <typename Name>
+		static constexpr size_t find(){
+			if constexpr (std::is_same_v<Name,typename FirstLabel::name>) {
+				return FirstLabel::offset;
+			} else {
+				static_assert(sizeof ...(Labels)!=0,"Not found label");
+				return LabelsList<Labels...>::template find<Name>();
+			}
+		} 
+		template <typename Name>
+		static constexpr bool exist() {
+			if constexpr(std::is_same_v<Name,typename FirstLabel::name>) {
+				return true;
+			} else {
+				return LabelsList<Labels...>::template exist<Name>();
+			}
+		}
+		template <typename Name,size_t offset>
+		struct add_s {
+			static_assert(!exist<Name>(),"Already added label");
+			using result = LabelsList<FirstLabel,Labels...,Label<Name,offset> >;
+		};
+		template <typename Name,size_t offset>
+		using add = typename add_s<Name,offset>::result;
+	};
+
+
+	template <typename ...Instructions>
+	struct InstructionsList;
+
+	template <>
+	struct InstructionsList<> {
+		static constexpr size_t length = 0;
+		explicit constexpr InstructionsList() {}
+		template <typename Instruction>
+		constexpr InstructionsList<Instruction> add( Instruction&& instr ) const {
+			return InstructionsList<Instruction>(std::move(instr),*this);
+		}
+	};
+
+	
+	template <typename FirstInstruction,typename ...Instructions>
+	struct InstructionsList<FirstInstruction,Instructions...> {
+		static constexpr size_t length = 1 + InstructionsList<Instructions...>::length;
+		FirstInstruction head;
+		InstructionsList<Instructions...> tail;
+		explicit constexpr InstructionsList(FirstInstruction&& inst,const InstructionsList<Instructions...>& tail) : head(std::move(inst)),tail(tail) {}
+		template <typename Instruction>
+		constexpr InstructionsList<Instruction,FirstInstruction,Instructions...> add( Instruction&& instr) const {
+			return InstructionsList<Instruction,FirstInstruction,Instructions...>(std::move(instr),*this);
+		}
+		template <size_t I,typename ProgramBuilder>
+		constexpr Instruction encode_i(const ProgramBuilder& builder) const {
+			if constexpr (I == length-1) {
+				return head.encode(builder);
+			} else {
+				return tail.template encode_i<I>(builder);
+			}
+		}
+
+		template <typename ProgramBuilder,std::size_t... I>
+		constexpr std::array<Instruction,length> build_i(const ProgramBuilder& builder,std::index_sequence<I...>) const {
+			return {{ encode_i<I>(builder)..., }};
+		};
+		template <typename ProgramBuilder>
+		constexpr std::array<Instruction,length> build(const ProgramBuilder& builder) const {
+			using indexes = std::make_index_sequence<length>;
+			return build_i(builder,indexes{});
+		};
+	};
+
+	
+	template <size_t Length,typename Labels>
+	struct ProgramCode  {
+		static constexpr const size_t length = Length;
+		const uint16_t wrap_target;
+		const uint16_t wrap;
+		constexpr ProgramCode( std::array<Instruction,Length>&& code,uint16_t wrap_target,uint16_t wrap) : wrap_target(wrap_target),wrap(wrap),code(std::move(code)) {
+
+		} 
+		const std::array<Instruction,Length> code;
+		template <typename Label>
+		static constexpr size_t getOffset() {
+			return Labels::template find<Label>();
+		}
+	};
+
+	
+	template <uint32_t sideset_count_t = 0,bool sideset_is_opt_t = true, uint16_t delay_max_t = 31, uint16_t sideset_max_t = 0>
+	struct ProgramSettings {
+		static constexpr uint32_t sideset_count = sideset_count_t;
+		static constexpr bool sideset_is_opt = sideset_is_opt_t;
+		static constexpr uint16_t delay_max = delay_max_t;
+		static constexpr uint16_t sideset_max = sideset_max_t;
+	};
+	template <size_t count>
+	struct ProgramSettingsSideset {
+		static_assert(count <= 5,"maximum number of side set bits is 5");
+		using result = ProgramSettings<count,false,(1u << (5 - count)) - 1,(1u << count) - 1>;
+	};
+	template <size_t count>
+	struct ProgramSettingsOptSideset {
+		static_assert(count <= 4,"maximum number of side set bits is 4");
+		using result = ProgramSettings<count+1,true,(1u << (5 - (count+1))) - 1,(1u << count) - 1>;
+	};
+
+	struct WrapSettings {
+		OptionalValue wrap_target;
+		OptionalValue wrap_instr;
+	};
+
+	template <typename SettingsConfig, typename Labels,typename Instructions>
+	struct ProgramBuilder {
+		using labels = Labels;
+		using Settings = SettingsConfig;
+		Instructions instructions;
+		WrapSettings wrap_settings;
+
+		explicit constexpr ProgramBuilder() {}
+		explicit constexpr ProgramBuilder(Instructions && instructions,const WrapSettings& wrap_settings) : instructions(std::move(instructions)), wrap_settings(wrap_settings) {}
+		explicit constexpr ProgramBuilder(const Instructions & instructions,const WrapSettings& wrap_settings) : instructions(instructions), wrap_settings(wrap_settings) {}
+		
+
+		template <size_t count>
+		constexpr auto sideset() const {
+			return ProgramBuilder<typename ProgramSettingsSideset<count>::result,Labels,Instructions>(instructions,wrap_settings);
+		}
+		template <size_t count>
+		constexpr auto sideset_opt() const {
+			return ProgramBuilder<typename ProgramSettingsOptSideset<count>::result,Labels,Instructions>(instructions,wrap_settings);		
+		}
+		
+		template <typename SideDelayConfig>
+		static constexpr uint16_t encodeSideDelay() {
+			auto res = SideDelayConfig::delay_value;
+			static_assert(SideDelayConfig::delay_value <= Settings::delay_max,"delay too long");
+			
+			if constexpr (SideDelayConfig::side_value_present) {
+				static_assert(SideDelayConfig::side_value<=Settings::sideset_max, "side value overflow");
+				res |= (SideDelayConfig::side_value << (5-Settings::sideset_count));
+				if constexpr (Settings::sideset_is_opt) {
+					res |= 1 << 4;
+				}
+			} else {
+				static_assert(Settings::sideset_is_opt, "sideset is not optional");
+			}
+			//static_assert(side < (1<<sideset_count),"side bits overflow");
+			return res;
+		}
+		template <typename Label>
+		static constexpr size_t getOffset() {
+			return Labels::template find<Label>();
+		}
+		constexpr ProgramBuilder& wrap() {
+			wrap_settings.wrap_instr.is_set = true;
+			//static_assert(Instructions::length>0 && ".wrap cannot be placed before the first program instruction");
+			wrap_settings.wrap_instr.value = Instructions::length-1;
+			return *this;
+		}
+		constexpr ProgramBuilder& wrapTarget() {
+			//static_assert(!settings.wrap_target.is_set && ".wrap_target only once");
+			wrap_settings.wrap_target.value = Instructions::length;
+			wrap_settings.wrap_target.is_set = true;
+			return *this;
+		}
+		template <typename LabelName>
+		constexpr auto label(const LabelName& = LabelName{}) const {
+			return ProgramBuilder<Settings,typename Labels::template add<LabelName,Instructions::length>,Instructions>(instructions,wrap_settings);
+		}
+		template <typename Instr>
+		constexpr auto instr( Instr&& instr ) const {
+			auto added_instructions = instructions.add(std::move(instr));
+			using AddedInstructions = decltype(added_instructions);
+			return ProgramBuilder<Settings,Labels,AddedInstructions>(std::move(added_instructions),wrap_settings);
+		}
+		constexpr ProgramCode<Instructions::length,Labels> end() const {
+			return ProgramCode<Instructions::length,Labels>(instructions.build(*this),
+				wrap_settings.wrap_target.is_set?wrap_settings.wrap_target.value:0,
+				wrap_settings.wrap_instr.is_set?wrap_settings.wrap_instr.value:(Instructions::length-1));
+		}
+	};
+
+	
+
+	using EmptyProgram = ProgramBuilder<ProgramSettings<>,LabelsList<>,InstructionsList<>>;
+	static inline constexpr EmptyProgram  ProgramBegin() {
+		return EmptyProgram();
+	};
+}
\ No newline at end of file
diff --git a/src/modm/platform/pio/rp/pio_program.hpp b/src/modm/platform/pio/rp/pio_program.hpp
new file mode 100644
index 0000000000..e1edc5ad44
--- /dev/null
+++ b/src/modm/platform/pio/rp/pio_program.hpp
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) 2022, Andrey Kunitsyn
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+// ----------------------------------------------------------------------------
+
+#pragma once
+
+#include "pio_asm.hpp"
+
+namespace modm::platform
+{
+	
+	struct PIOProgram {
+		const pio::Instruction* instructions;
+		const uint8_t length;
+		int8_t origin;
+		constexpr PIOProgram(const pio::Instruction* code,uint8_t length,int8_t origin) : instructions(code),length(length),origin(origin) {}
+		static constexpr pio::EmptyProgram begin() {
+			return pio::ProgramBegin();
+		}
+		template <typename Program>
+		static constexpr PIOProgram get(const Program& data) {
+			return PIOProgram(data.code.data(),data.code.size(),-1);
+		}
+	};
+
+}
\ No newline at end of file
diff --git a/src/modm/platform/pio/rp/pio_sm.hpp b/src/modm/platform/pio/rp/pio_sm.hpp
new file mode 100644
index 0000000000..5b5077014a
--- /dev/null
+++ b/src/modm/platform/pio/rp/pio_sm.hpp
@@ -0,0 +1,233 @@
+/*
+ * Copyright (c) 2022, Andrey Kunitsyn
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+// ----------------------------------------------------------------------------
+#pragma once
+
+#include <modm/math/algorithm/prescaler.hpp>
+#include <hardware/structs/pio.h>
+
+namespace modm::platform::pio
+{
+	
+	template <typename Pio,size_t SM>
+	class StateMachine {
+	private:
+		static_assert(SM<NUM_PIO_STATE_MACHINES,"Invalid SM");
+		static inline pio_sm_hw& sm() {
+			return Pio::pio().sm[SM];
+		}
+		struct Encoder {
+			constexpr uint16_t encodeSideDelay(const pio::OptionalValue& /*side*/,uint16_t /*delay*/) const {
+				return 0;
+			}
+		};
+		template <class Pin,bool isOut>
+		static inline void addPin() {
+			sm().pinctrl =
+                (1u << PIO_SM0_PINCTRL_SET_COUNT_LSB) |
+                (uint32_t(Pin::pin) << PIO_SM0_PINCTRL_SET_BASE_LSB);
+            exec(pio::Set().pindirs<isOut?1:0>().encode(Encoder()));
+		}
+		static inline void exec(const pio::Instruction& instr) {
+			sm().instr = instr.value;
+		}
+		static constexpr uint32_t STALL_MASK = 1u << (PIO_FDEBUG_TXSTALL_LSB + SM);
+	public:
+		template < class... Pins > 
+		static void addOutput() {
+			auto pinctrlSaved = sm().pinctrl;
+			(addPin<Pins,true>(),...);
+			sm().pinctrl = pinctrlSaved;
+		}
+		struct Config {
+			uint32_t clkdiv{0};
+		    uint32_t execctrl{0};
+		    uint32_t shiftctrl{0};
+		    uint32_t pinctrl{0};
+		    template <  uint16_t div_int, uint8_t div_frac >
+		    constexpr Config& setClkDiv() {
+				clkdiv = 	(uint32_t(div_frac) << PIO_SM0_CLKDIV_FRAC_LSB) |
+            				(uint32_t(div_int) << PIO_SM0_CLKDIV_INT_LSB);
+            	return *this;
+			}
+			constexpr Config& setWrap(uint32_t wrap_target,uint32_t wrap) {
+				execctrl = (execctrl & ~(PIO_SM0_EXECCTRL_WRAP_TOP_BITS | PIO_SM0_EXECCTRL_WRAP_BOTTOM_BITS)) |
+                  (wrap_target << PIO_SM0_EXECCTRL_WRAP_BOTTOM_LSB) |
+                  (wrap << PIO_SM0_EXECCTRL_WRAP_TOP_LSB);
+                return *this;
+			}
+			template <bool shiftRight,bool autoPush,size_t pushThreshold>
+			constexpr Config& setInShift() {
+				shiftctrl = (shiftctrl &
+                    ~(PIO_SM0_SHIFTCTRL_IN_SHIFTDIR_BITS |
+                      PIO_SM0_SHIFTCTRL_AUTOPUSH_BITS |
+                      PIO_SM0_SHIFTCTRL_PUSH_THRESH_BITS)) |
+                   ((shiftRight?1:0) << PIO_SM0_SHIFTCTRL_IN_SHIFTDIR_LSB) |
+                   ((autoPush?1:0) << PIO_SM0_SHIFTCTRL_AUTOPUSH_LSB) |
+                   ((pushThreshold & 0x1fu) << PIO_SM0_SHIFTCTRL_PUSH_THRESH_LSB);
+                return *this;
+			}
+			template <bool shiftRight,bool autoPull,size_t pullThreshold>
+			constexpr Config& setOutShift() {
+				shiftctrl = (shiftctrl &
+                    ~(PIO_SM0_SHIFTCTRL_OUT_SHIFTDIR_BITS |
+                      PIO_SM0_SHIFTCTRL_AUTOPULL_BITS |
+                      PIO_SM0_SHIFTCTRL_PULL_THRESH_BITS)) |
+                   ((shiftRight?1:0) << PIO_SM0_SHIFTCTRL_OUT_SHIFTDIR_LSB) |
+                   ((autoPull?1:0) << PIO_SM0_SHIFTCTRL_AUTOPULL_LSB) |
+                   ((pullThreshold & 0x1fu) << PIO_SM0_SHIFTCTRL_PULL_THRESH_LSB);
+                return *this;
+			}
+			template < class StartPin , size_t count, bool optional, bool pindirs>
+			constexpr Config& setSidesetPins() {
+				static_assert((count+(optional?1:0)) <= 5,"too many sideset pins");
+				pinctrl = (pinctrl & ~(PIO_SM0_PINCTRL_SIDESET_COUNT_BITS | PIO_SM0_PINCTRL_SIDESET_BASE_BITS)) |
+								((count+(optional?1:0)) << PIO_SM0_PINCTRL_SIDESET_COUNT_LSB) | 
+								(uint32_t(StartPin::pin) << PIO_SM0_PINCTRL_SIDESET_BASE_LSB);
+				execctrl = (execctrl & ~(PIO_SM0_EXECCTRL_SIDE_EN_BITS | PIO_SM0_EXECCTRL_SIDE_PINDIR_BITS)) |
+                  ((optional?1:0) << PIO_SM0_EXECCTRL_SIDE_EN_LSB) |
+                  ((pindirs?1:0) << PIO_SM0_EXECCTRL_SIDE_PINDIR_LSB);
+                return *this;
+			}
+			template < class StartPin , size_t count>
+			constexpr Config& setOutPins() {
+				pinctrl = (pinctrl & ~(PIO_SM0_PINCTRL_OUT_BASE_BITS | PIO_SM0_PINCTRL_OUT_COUNT_BITS)) |
+                	(uint32_t(StartPin::pin) << PIO_SM0_PINCTRL_OUT_BASE_LSB) |
+                	(count << PIO_SM0_PINCTRL_OUT_COUNT_LSB);
+                return *this;
+			}
+			template < class StartPin , size_t count>
+			constexpr Config& setSetPins() {
+				pinctrl = (pinctrl & ~(PIO_SM0_PINCTRL_SET_BASE_BITS | PIO_SM0_PINCTRL_SET_COUNT_BITS)) |
+                	(uint32_t(StartPin::pin) << PIO_SM0_PINCTRL_SET_BASE_LSB) |
+                	(count << PIO_SM0_PINCTRL_SET_COUNT_LSB);
+                return *this;
+			}
+			template < class StartPin>
+			constexpr Config& setInPins() {
+				pinctrl = (pinctrl & ~PIO_SM0_PINCTRL_IN_BASE_BITS) |
+                	(uint32_t(StartPin::pin) << PIO_SM0_PINCTRL_IN_BASE_LSB);
+                return *this;
+			}
+			constexpr Config& setFifoJoinTx() {
+				shiftctrl = (shiftctrl & ~(PIO_SM0_SHIFTCTRL_FJOIN_TX_BITS | PIO_SM0_SHIFTCTRL_FJOIN_RX_BITS)) |
+                   PIO_SM0_SHIFTCTRL_FJOIN_TX_BITS;
+                return *this;
+			}
+			constexpr Config& setFifoJoinRx() {
+				shiftctrl = (shiftctrl & ~(PIO_SM0_SHIFTCTRL_FJOIN_TX_BITS | PIO_SM0_SHIFTCTRL_FJOIN_RX_BITS)) |
+                   PIO_SM0_SHIFTCTRL_FJOIN_RX_BITS;
+                return *this;
+			}
+			constexpr Config& setFifoJoinNone() {
+				shiftctrl = (shiftctrl & ~(PIO_SM0_SHIFTCTRL_FJOIN_TX_BITS | PIO_SM0_SHIFTCTRL_FJOIN_RX_BITS));
+				return *this;
+			}
+			template <typename Program>
+			constexpr Config& setup(size_t offset,const Program& prg) {
+				setWrap(prg.wrap_target+offset,prg.wrap+offset);
+				return *this;
+			}
+			void init(uint16_t startPC) {
+				StateMachine::setEnabled(false);
+
+				StateMachine::sm().clkdiv = clkdiv;
+				StateMachine::sm().execctrl = execctrl;
+				StateMachine::sm().shiftctrl = shiftctrl;
+				StateMachine::sm().pinctrl = pinctrl;
+
+				StateMachine::clearFifos();
+				// Clear FIFO debug flags
+			    constexpr uint32_t fdebug_sm_mask =
+			            (1u << PIO_FDEBUG_TXOVER_LSB) |
+			            (1u << PIO_FDEBUG_RXUNDER_LSB) |
+			            (1u << PIO_FDEBUG_TXSTALL_LSB) |
+			            (1u << PIO_FDEBUG_RXSTALL_LSB);
+			    Pio::pio().fdebug = fdebug_sm_mask << SM;
+			    StateMachine::restart();
+			    StateMachine::clkDivRestart();
+			    StateMachine::jump(startPC);
+			}
+
+			template< class SystemClock, frequency_t freq, percent_t tolerance=pct(1) >
+			constexpr Config&
+			setFrequency()
+			{
+				static_assert(freq <= SystemClock::PeriFrequency and
+							  SystemClock::PeriFrequency <= freq  * 65535ull,
+						"SystemClock::PeriFrequency must be in the range [freq, 65535 x freq].");
+				// 16.8 fractional freq generator 
+				constexpr uint64_t min = (1ul << 8);
+				constexpr uint64_t max = (1ul << 24) - 1ul;
+
+				constexpr uint64_t prescaler = std::max(min,std::min(max,(uint64_t(SystemClock::SysFrequency)<<8) / freq));
+				constexpr uint64_t res_freq = (uint64_t(SystemClock::SysFrequency)<<8) / prescaler;
+
+				//constexpr auto result = Prescaler::from_linear(SystemClock::SysFrequency<<8, freq, min, max);
+				modm::PeripheralDriver::assertBaudrateInTolerance< res_freq, uint64_t(freq), tolerance >();
+
+				clkdiv = 	(uint32_t(prescaler & 0xff) << PIO_SM0_CLKDIV_FRAC_LSB) |
+            				(uint32_t(prescaler>>8) << PIO_SM0_CLKDIV_INT_LSB);
+
+				return *this;
+			}
+		};
+		static constexpr Config config() {
+			return Config()
+				.template setClkDiv<1,0>()
+				.setWrap(0,31)
+				.template setInShift<true,false,32>()
+				.template setOutShift<true,false,32>();
+		}
+		
+		
+		static void clearFifos() {
+			// changing the FIFO join state clears the fifo
+			hw_xor_bits(&sm().shiftctrl, PIO_SM0_SHIFTCTRL_FJOIN_RX_BITS);
+			hw_xor_bits(&sm().shiftctrl, PIO_SM0_SHIFTCTRL_FJOIN_RX_BITS);
+		}
+		static void setEnabled(bool enabled) {
+			Pio::pio().ctrl = (Pio::pio().ctrl & ~(1u << SM)) | ((enabled?1:0) << SM);
+		}
+		static void restart() {
+			Pio::pio().ctrl |= 1u << (PIO_CTRL_SM_RESTART_LSB + SM);
+		}
+		static void clkDivRestart() {
+			Pio::pio().ctrl |= 1u << (PIO_CTRL_CLKDIV_RESTART_LSB + SM);
+		}
+		
+		
+		static void jump(uint16_t pc) {
+			exec(pio::Jmp().encode(Encoder())|pc);
+		}
+
+		static inline bool txFifoFull() {
+			return (Pio::pio().fstat & (1u << (PIO_FSTAT_TXFULL_LSB + SM))) != 0;
+		}
+		static inline bool txFifoEmpty() {
+			return (Pio::pio().fstat & (1u << (PIO_FSTAT_TXEMPTY_LSB + SM))) != 0;
+		}
+
+		static inline void write(uint32_t val) {
+			Pio::pio().txf[SM] = val;
+		}
+		static inline uint32_t read() {
+			return Pio::pio().rxf[SM];
+		}
+		
+		static void resetStall() {
+			Pio::pio().fdebug = STALL_MASK;
+		}
+		static void waitStall() {
+			while ((Pio::pio().fdebug & STALL_MASK)==0) {__NOP();}
+		}
+		
+	};
+}
\ No newline at end of file