diff --git a/.buildkite/pipeline.yml b/.buildkite/pipeline.yml index b1f338d..50f730f 100644 --- a/.buildkite/pipeline.yml +++ b/.buildkite/pipeline.yml @@ -1,2 +1,2 @@ steps: - - command: zig build + - command: zig build test diff --git a/.gitignore b/.gitignore index 4c82b07..c8366d4 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,5 @@ zig-cache zig-out +.DS_Store +.gdbinit +.lldbinit diff --git a/build.zig b/build.zig index 5144b36..21a91ff 100644 --- a/build.zig +++ b/build.zig @@ -39,8 +39,19 @@ pub fn addPiPicoExecutable( // project requires multiple HALs, it accepts microzig as a param pub fn build(b: *Builder) !void { const optimize = b.standardOptimizeOption(.{}); - var examples = Examples.init(b, optimize); - examples.install(); + //var examples = Examples.init(b, optimize); + //examples.install(); + + const pio_tests = b.addTest(.{ + .root_source_file = .{ + .path = "src/hal/pio.zig", + }, + .optimize = optimize, + }); + pio_tests.addIncludePath("src/hal/pio/assembler"); + + const test_step = b.step("test", "run unit tests"); + test_step.dependOn(&pio_tests.run().step); } fn root() []const u8 { @@ -55,6 +66,7 @@ pub const Examples = struct { pwm: *microzig.EmbeddedExecutable, spi_master: *microzig.EmbeddedExecutable, uart: *microzig.EmbeddedExecutable, + squarewave: *microzig.EmbeddedExecutable, //uart_pins: microzig.EmbeddedExecutable, flash_program: *microzig.EmbeddedExecutable, diff --git a/deps/microzig b/deps/microzig index 23482a6..dabc932 160000 --- a/deps/microzig +++ b/deps/microzig @@ -1 +1 @@ -Subproject commit 23482a6986252e0eeff54a04abc0aac8a08d25d7 +Subproject commit dabc9325cdee394ff66e28c91803cb814954b157 diff --git a/examples/squarewave.zig b/examples/squarewave.zig new file mode 100644 index 0000000..b320681 --- /dev/null +++ b/examples/squarewave.zig @@ -0,0 +1,53 @@ +//! Hello world for the PIO module: generating a square wave +const std = @import("std"); +const microzig = @import("microzig"); +const rp2040 = microzig.hal; +const gpio = rp2040.gpio; +const Pio = rp2040.pio.Pio; +const StateMachine = rp2040.pio.StateMachine; + +const squarewave_program = (rp2040.pio.assemble( + \\.program squarewave + \\ set pindirs, 1 ; Set pin to output + \\again: + \\ set pins, 1 [1] ; Drive pin high and then delay for one cycle + \\ set pins, 0 ; Drive pin low + \\ jmp again ; Set PC to label `again` +) catch + @panic("failed to assemble program")) + .get_program_by_name("squarewave"); + +pub fn main() void { + // Pick one PIO instance arbitrarily. We're also arbitrarily picking state + // machine 0 on this PIO instance (the state machines are numbered 0 to 3 + // inclusive). + const pio: Pio = .pio0; + const sm: StateMachine = .sm0; + + // Load the assembled program directly into the PIO's instruction memory. + // Each PIO instance has a 32-slot instruction memory, which all 4 state + // machines can see. The system has write-only access. + for (squarewave_program.instructions, 0..) |insn, i| + pio.get_instruction_memory()[i] = insn; + + // Configure state machine 0 to run at sysclk/2.5. The state machines can + // run as fast as one instruction per clock cycle, but we can scale their + // speed down uniformly to meet some precise frequency target, e.g. for a + // UART baud rate. This register has 16 integer divisor bits and 8 + // fractional divisor bits. + pio.set_clkdiv_int_frac(sm, 2, 0x80); + + // There are five pin mapping groups (out, in, set, side-set, jmp pin) + // which are used by different instructions or in different circumstances. + // Here we're just using SET instructions. Configure state machine 0 SETs + // to affect GPIO 0 only; then configure GPIO0 to be controlled by PIO0, + // as opposed to e.g. the processors. + pio.set_out_pins(sm, 0, 1); + gpio.set_function(0, .pio0); + + // Set the state machine running. The PIO CTRL register is global within a + // PIO instance, so you can start/stop multiple state machines + // simultaneously. We're using the register's hardware atomic set alias to + // make one bit high without doing a read-modify-write on the register. + pio.set_enabled(sm, true); +} diff --git a/src/hal.zig b/src/hal.zig index f8a74d2..5813cf9 100644 --- a/src/hal.zig +++ b/src/hal.zig @@ -1,5 +1,6 @@ +const std = @import("std"); const microzig = @import("microzig"); -const regs = microzig.chip.registers; +const SIO = microzig.chip.peripherals.SIO; pub const adc = @import("hal/adc.zig"); pub const pins = @import("hal/pins.zig"); @@ -14,6 +15,7 @@ pub const resets = @import("hal/resets.zig"); pub const irq = @import("hal/irq.zig"); pub const rom = @import("hal/rom.zig"); pub const flash = @import("hal/flash.zig"); +pub const pio = @import("hal/pio.zig"); pub const clock_config = clocks.GlobalConfiguration.init(.{ .ref = .{ .source = .src_xosc }, @@ -32,5 +34,5 @@ pub fn init() void { } pub fn get_cpu_id() u32 { - return regs.SIO.CPUID.*; + return SIO.CPUID.*; } diff --git a/src/hal/gpio.zig b/src/hal/gpio.zig index 1e333b1..c0b6bae 100644 --- a/src/hal/gpio.zig +++ b/src/hal/gpio.zig @@ -87,7 +87,7 @@ pub const PullUpDown = enum { pub inline fn set_pull(comptime gpio: u32, mode: ?PullUpDown) void { const gpio_name = comptime std.fmt.comptimePrint("GPIO{d}", .{gpio}); - const gpio_regs = @field(PADS_BANK0, gpio_name); + const gpio_regs = &@field(PADS_BANK0, gpio_name); if (mode == null) { gpio_regs.modify(.{ .PUE = 0, .PDE = 0 }); diff --git a/src/hal/hw.zig b/src/hal/hw.zig new file mode 100644 index 0000000..a44aff1 --- /dev/null +++ b/src/hal/hw.zig @@ -0,0 +1,11 @@ +pub const Lock = struct { + impl: u32, + + pub fn claim() Lock { + + } + + pub fn unlock(lock: Lock) void { + + } +}; diff --git a/src/hal/irq.zig b/src/hal/irq.zig index f3502be..2ff9a13 100644 --- a/src/hal/irq.zig +++ b/src/hal/irq.zig @@ -1,5 +1,5 @@ const microzig = @import("microzig"); -const regs = microzig.chip.registers; +const NVIC = microzig.chip.peripherals.NVIC; // TODO: the register definitions are improved now, use them instead of raw // writes/reads @@ -10,11 +10,11 @@ fn get_interrupt_mask(comptime interrupt_name: []const u8) u32 { } pub fn enable(comptime interrupt_name: []const u8) void { const mask = comptime get_interrupt_mask(interrupt_name); - regs.SCS.NVIC.ICPR.raw = mask; - regs.SCS.NVIC.ISER.raw = mask; + NVIC.ICPR.raw = mask; + NVIC.ISER.raw = mask; } pub fn disable(comptime interrupt_name: []const u8) void { const mask = comptime get_interrupt_mask(interrupt_name); - regs.SCS.NVIC.ICER.raw = mask; + NVIC.ICER.raw = mask; } diff --git a/src/hal/pio.zig b/src/hal/pio.zig new file mode 100644 index 0000000..e37d713 --- /dev/null +++ b/src/hal/pio.zig @@ -0,0 +1,358 @@ +//! A PIO instance can load a single `Bytecode`, it has to be loaded into memory +const std = @import("std"); + +const microzig = @import("microzig"); +const PIO = microzig.chip.types.peripherals.PIO0; +const PIO0 = microzig.chip.peripherals.PIO0; +const PIO1 = microzig.chip.peripherals.PIO1; + +const gpio = @import("gpio.zig"); +const assembler = @import("pio/assembler.zig"); +pub const Bytecode = Bytecode; +pub const Program = assembler.Program; +pub const assemble = assembler.assemble; + +var used_instruction_space: [2]u32 = [_]u32{ 0, 0 }; +var claimed_state_machines: [2]u4 = [_]u4{ 0, 0 }; + +pub const Join = enum { + none, + rx, + tx, +}; + +pub const Status = enum { + rx_lessthan, + tx_lessthan, +}; + +pub const Configuration = struct { + pin: u32, +}; + +pub const StateMachine = enum(u2) { + sm0, + sm1, + sm2, + sm3, +}; + +pub const Pio = enum(u1) { + pio0 = 0, + pio1 = 1, + + fn get_regs(self: Pio) *volatile PIO { + return switch (self) { + .pio0 => PIO0, + .pio1 => PIO1, + }; + } + + pub fn get_instruction_memory(self: Pio) *volatile [32]u32 { + const regs = self.get_regs(); + return @ptrCast(*volatile [32]u32, ®s.INSTR_MEM0); + } + + pub fn gpio_init(self: Pio, comptime pin: u5) void { + gpio.set_function(pin, switch (self) { + .pio0 => .pio0, + .pio1 => .pio1, + }); + } + + pub fn load(self: Pio, bytecode: Bytecode) !void { + _ = self; + _ = bytecode; + } + + fn can_add_program_at_offset(self: Pio, program: Program, offset: u5) bool { + if (program.origin) |origin| + if (origin != offset) + return false; + + const used_mask = used_instruction_space[@enumToInt(self)]; + const program_mask = program.get_mask(); + + // We can add the program if the masks don't overlap, if there is + // overlap the result of a bitwise AND will have a non-zero result + return (used_mask & program_mask) == 0; + } + + fn find_offset_for_program(self: Pio, program: Program) !u5 { + return if (program.origin) |origin| + if (self.can_add_program_at_offset(program, origin)) + origin + else + error.NoSpace + else for (0..32 - program.isntruction.len) |i| { + if (self.can_add_program_at_offset(program, i)) + break i; + } else error.NoSpace; + } + + fn add_program_at_offset_unlocked(self: Pio, program: Program, offset: u5) !void { + if (!self.can_add_program_at_offset(program, offset)) + return error.NoSpace; + + const instruction_memory = self.get_instruction_memory(); + for (program.instructions, offset..) |insn, i| + instruction_memory[i] = insn; + + const program_mask = program.get_mask(); + used_instruction_space[@enumToInt(self)] |= program_mask << offset; + } + + /// Public functions will need to lock independently, so only exposing this function for now + pub fn add_program(self: Pio, program: Program) !void { + // TODO: const lock = hw.Lock.claim() + // defer lock.unlock(); + + const offset = try self.find_offset_for_program_unlocked(); + try self.add_program_at_offset(program, offset); + } + + pub fn claim_unused_state_machine(self: Pio) !StateMachine { + // TODO: const lock = hw.Lock.claim() + // defer lock.unlock(); + + const claimed_mask = claimed_state_machines[@enumToInt(self)]; + return for (0..4) |i| { + const sm_mask = (@as(u4, 1) << @intCast(u2, i)); + if (0 == (claimed_mask & sm_mask)) { + claimed_state_machines[@enumToInt(self)] |= sm_mask; + break @intToEnum(StateMachine, i); + } + } else error.NoSpace; + } + + pub const StateMachineRegs = extern struct { + clkdiv: @TypeOf(PIO0.SM0_CLKDIV), + execctrl: @TypeOf(PIO0.SM0_EXECCTRL), + shiftctrl: @TypeOf(PIO0.SM0_SHIFTCTRL), + addr: @TypeOf(PIO0.SM0_ADDR), + instr: @TypeOf(PIO0.SM0_INSTR), + pinctrl: @TypeOf(PIO0.SM0_PINCTRL), + }; + + fn get_sm_regs(self: Pio, sm: StateMachine) *volatile StateMachineRegs { + const pio_regs = self.get_regs(); + return switch (sm) { + .sm0 => @ptrCast(*volatile StateMachineRegs, &pio_regs.SM0_CLKDIV), + .sm1 => @ptrCast(*volatile StateMachineRegs, &pio_regs.SM1_CLKDIV), + .sm2 => @ptrCast(*volatile StateMachineRegs, &pio_regs.SM2_CLKDIV), + .sm3 => @ptrCast(*volatile StateMachineRegs, &pio_regs.SM3_CLKDIV), + }; + } + + pub fn join_fifo(self: Pio, sm: StateMachine, join: Join) void { + const tx: u1 = switch (join) { + .tx => 1, + .rx => 0, + .none => 0, + }; + const rx: u1 = switch (join) { + .tx => 0, + .rx => 1, + .none => 0, + }; + + const sm_regs = self.get_sm_regs(sm); + sm_regs.shiftctrl.modify(.{ + .FJOIN_TX = tx, + .FJOIN_RX = rx, + }); + } + + pub fn set_clkdiv_int_frac(self: Pio, sm: StateMachine, div_int: u16, div_frac: u8) void { + if (div_int == 0 and div_frac != 0) + @panic("invalid params"); + + const sm_regs = self.get_sm_regs(sm); + sm_regs.clkdiv.write(.{ + .INT = div_int, + .FRAC = div_frac, + + .reserved8 = 0, + }); + } + + pub fn set_out_shift(self: Pio, sm: StateMachine, args: struct { + shift_right: bool, + autopull: bool, + pull_threshold: u5, + }) void { + const sm_regs = self.get_sm_regs(sm); + sm_regs.shiftctrl.modify(.{ + .OUT_SHIFTDIR = @boolToInt(args.shift_right), + .AUTOPULL = @boolToInt(args.autopull), + .PULL_THRESH = args.pull_threshold, + }); + } + + pub fn set_out_pins(self: Pio, sm: StateMachine, base: u5, count: u5) void { + const sm_regs = self.get_sm_regs(sm); + sm_regs.pinctrl.modify(.{ + .OUT_BASE = base, + .OUT_COUNT = count, + }); + } + + pub fn set_sideset_pins(self: Pio, sm: StateMachine, base: u5) void { + const sm_regs = self.get_sm_regs(sm); + sm_regs.pinctrl.modify(.{ .SIDESET_BASE = base }); + } + + pub fn is_tx_fifo_full(self: Pio, sm: StateMachine) bool { + const regs = self.get_regs(); + const txfull = regs.FSTAT.read().TXFULL; + return (txfull & (@as(u4, 1) << @enumToInt(sm))) != 0; + } + + pub fn get_tx_fifo(self: Pio, sm: StateMachine) *volatile u32 { + const regs = self.get_regs(); + return switch (sm) { + .sm0 => ®s.TXF0, + .sm1 => ®s.TXF1, + .sm2 => ®s.TXF2, + .sm3 => ®s.TXF3, + }; + } + + pub fn blocking_write(self: Pio, sm: StateMachine, value: u32) void { + while (self.is_tx_fifo_full(sm)) {} + + const fifo_ptr = self.get_tx_fifo(sm); + fifo_ptr.* = value; + } + + pub fn encode_jmp() void {} + + //static inline uint _pio_encode_instr_and_args(enum pio_instr_bits instr_bits, uint arg1, uint arg2) { + // valid_params_if(PIO_INSTRUCTIONS, arg1 <= 0x7); + //#if PARAM_ASSERTIONS_ENABLED(PIO_INSTRUCTIONS) + // uint32_t major = _pio_major_instr_bits(instr_bits); + // if (major == pio_instr_bits_in || major == pio_instr_bits_out) { + // assert(arg2 && arg2 <= 32); + // } else { + // assert(arg2 <= 31); + // } + //#endif + // return instr_bits | (arg1 << 5u) | (arg2 & 0x1fu); + //} + // + //static inline uint pio_encode_jmp(uint addr) { + // return _pio_encode_instr_and_args(pio_instr_bits_jmp, 0, addr); + //} + + pub fn set_enabled(self: Pio, sm: StateMachine, enabled: bool) void { + const regs = self.get_regs(); + + var value = regs.CTRL.read(); + if (enabled) + value.SM_ENABLE |= @as(u4, 1) << @enumToInt(sm) + else + value.SM_ENABLE &= ~(@as(u4, 1) << @enumToInt(sm)); + + regs.CTRL.write(value); + } + + pub fn sm_init(self: Pio, sm: StateMachine, initial_pc: u5, config: StateMachineRegs) void { + // Halt the machine, set some sensible defaults + self.set_enabled(sm, false); + + self.set_config(sm, config); + self.clear_fifos(sm); + + // Clear FIFO debug flags + //const 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->fdebug = fdebug_sm_mask << sm; + + // Finally, clear some internal SM state + self.restart(sm); + self.clkdiv_restart(sm); + self.exec(sm, encode_jmp(initial_pc)); + } + + // state machine configuration helpers: + // + // - set_out_pins + // - set_set_pins + // - set_in_pins + // - set_sideset_pins + // - set_sideset + // - calculate_clkdiv_from_float + // - set_clkdiv + // - set_wrap + // - set_jmp_pin + // - set_in_shift + // - set_out_shift + // - set_fifo_join + // - set_out_special + // - set_mov_status + // + // PIO: + // + // - can_add_program + // - add_program_at_offset + // - add_program + // - remove_program + // - clear_instruction_memory + // - sm_init + // - sm_set_enabled + // - sm_mask_enabled + // - sm_restart + // - restart_sm_mask + // - sm_clkdiv_restart + // - clkdiv_restart_sm_mask + // - enable_sm_mask_in_sync + // - set_irq0_source_enabled + // - set_irq1_source_enabled + // - set_irq0_source_mask_enabled + // - set_irq1_source_mask_enabled + // - set_irqn_source_enabled + // - set_irqn_source_mask_enabled + // - interrupt_get + // - interrupt_clear + // - sm_get_pc + // - sm_exec + // - sm_is_exec_stalled + // - sm_exec_wait_blocking + // - sm_set_wrap + // - sm_set_out_pins + // - sm_set_set_pins + // - sm_set_in_pins + // - sm_set_sideset_pins + // - sm_put + // - sm_get + // - sm_is_rx_fifo_full + // - sm_is_rx_fifo_empty + // - sm_is_rx_fifo_level + // - sm_is_tx_fifo_full + // - sm_is_tx_fifo_empty + // - sm_is_tx_fifo_level + // - sm_put_blocking + // - sm_get_blocking + // - sm_drain_tx_fifo + // - sm_set_clkdiv_int_frac + // - sm_set_clkdiv + // - sm_clear_fifos + // - sm_set_pins + // - sm_set_pins_with_mask + // - sm_set_pindirs_with_mask + // - sm_set_consecutive_pindirs + // - sm_claim + // - claim_sm_mask + // - sm_unclaim + // - claim_unused_sm + // - sm_is_claimed + // +}; + +test "pio" { + std.testing.refAllDecls(assembler); + //std.testing.refAllDecls(@import("pio/test.zig")); +} diff --git a/src/hal/pio/assembler.zig b/src/hal/pio/assembler.zig new file mode 100644 index 0000000..d3596fd --- /dev/null +++ b/src/hal/pio/assembler.zig @@ -0,0 +1,140 @@ +const std = @import("std"); +const assert = std.debug.assert; + +const tokenizer = @import("assembler/tokenizer.zig"); +const encoder = @import("assembler/encoder.zig"); + +pub const TokenizeOptions = tokenizer.Options; +pub const EncodeOptions = encoder.Options; + +pub const Define = struct { + name: []const u8, + value: i64, +}; + +pub const Program = struct { + name: []const u8, + defines: []const Define, + instructions: []const u16, + origin: ?u5, + side_set: ?encoder.SideSet, + wrap_target: ?u5, + wrap: ?u5, + + pub fn get_mask(program: Program) u32 { + return (1 << program.instructions.len) - 1; + } +}; + +pub const Output = struct { + defines: []const Define, + programs: []const Program, + + pub fn get_program_by_name( + comptime output: Output, + comptime name: []const u8, + ) Program { + return for (output.programs) |program| { + if (std.mem.eql(u8, program.name, program)) + break program; + } else @panic(std.fmt.comptimePrint("program '{s}' not found", .{name})); + } + + pub fn get_define_by_name( + comptime output: Output, + comptime name: []const u8, + ) u32 { + return for (output.defines) |define| { + if (std.mem.eql(u8, define.name, define)) + break define; + } else @panic(std.fmt.comptimePrint("define '{s}' not found", .{name})); + } +}; + +pub const AssembleOptions = struct { + tokenize: TokenizeOptions = .{}, + encode: EncodeOptions = .{}, +}; + +pub const Diagnostics = struct { + message: std.BoundedArray(u8, 256), + index: u32, + + pub fn init(index: u32, comptime fmt: []const u8, args: anytype) Diagnostics { + var ret = Diagnostics{ + .message = std.BoundedArray(u8, 256).init(0) catch unreachable, + .index = index, + }; + + ret.message.writer().print(fmt, args) catch unreachable; + return ret; + } +}; + +pub fn assemble_impl(comptime source: []const u8, diags: *?Diagnostics, options: AssembleOptions) !Output { + const tokens = try tokenizer.tokenize(source, diags, options.tokenize); + const encoder_output = try encoder.encode(tokens.slice(), diags, options.encode); + var programs = std.BoundedArray(Program, options.encode.max_programs).init(0) catch unreachable; + for (encoder_output.programs.slice()) |bounded| + try programs.append(bounded.to_exported_program()); + + return Output{ + .defines = blk: { + var tmp = std.BoundedArray(Define, options.encode.max_defines).init(0) catch unreachable; + for (encoder_output.global_defines.slice()) |define| + tmp.append(.{ + .name = define.name, + .value = define.value, + }) catch unreachable; + break :blk tmp.slice(); + }, + .programs = programs.slice(), + }; +} + +fn format_compile_error(comptime message: []const u8, comptime source: []const u8, comptime index: u32) []const u8 { + var line_str: []const u8 = ""; + var line_num: u32 = 1; + var column: u32 = 0; + var line_it = std.mem.tokenize(u8, source, "\n\r"); + while (line_it.next()) |line| : (line_num += 1) { + line_str = line_str ++ "\n" ++ line; + if (line_it.index > index) { + column = line.len - (line_it.index - index); + line_str = line; + break; + } + } + + return std.fmt.comptimePrint( + \\failed to assemble PIO code: + \\ + \\{s} + \\{s}^ + \\{s}{s} + \\ + , .{ + line_str, + [_]u8{' '} ** column, + [_]u8{' '} ** column, + message, + }); +} + +pub fn assemble(comptime source: []const u8, comptime options: AssembleOptions) !Output { + var diags: ?Diagnostics = null; + return assemble_impl(source, &diags, options) catch |err| if (diags) |d| + @compileError(format_compile_error(d.message.slice(), source, d.index)) + else + err; +} + +test "tokenizer and encoder" { + std.testing.refAllDecls(tokenizer); + std.testing.refAllDecls(@import("assembler/Expression.zig")); + std.testing.refAllDecls(encoder); +} + +test "comparison" { + //std.testing.refAllDecls(@import("assembler/comparison_tests.zig")); +} diff --git a/src/hal/pio/assembler/Expression.zig b/src/hal/pio/assembler/Expression.zig new file mode 100644 index 0000000..794b5c3 --- /dev/null +++ b/src/hal/pio/assembler/Expression.zig @@ -0,0 +1,706 @@ +//! Expressions for PIO are weird. The documentation states that an expression, +//! when used as a "value", requires parenthesis. However the official PIO +//! assembler allows for defines with a value of `::1` which is an expression. +//! +//! Annoyingly, looking at the parser, it seems that it supports a number of +//! other operations not covered in the documentation. +ops: BoundedOperations, +values: BoundedValues, + +const std = @import("std"); +const assert = std.debug.assert; + +const assembler = @import("../assembler.zig"); +const Diagnostics = assembler.Diagnostics; + +const encoder = @import("encoder.zig"); +const DefineWithIndex = encoder.DefineWithIndex; + +const Expression = @This(); +const BoundedOperations = std.BoundedArray(OperationWithIndex, 32); +const BoundedValues = std.BoundedArray(Value, 32); + +const Value = struct { + str: []const u8, + index: u32, +}; + +const OperationWithIndex = struct { + op: Operation, + index: u32, +}; + +const call_depth_max = 64; + +pub const Operation = enum { + add, + sub, + mul, + div, + negative, + bit_reverse, + value, + // operations shown in pioasm's parser: + // - OR + // - AND + // - XOR + + pub fn format( + op: Operation, + comptime fmt: []const u8, + options: std.fmt.FormatOptions, + writer: anytype, + ) !void { + _ = fmt; + _ = options; + try writer.print("{s}", .{switch (op) { + .add => "add", + .sub => "sub", + .mul => "mul", + .div => "div", + .negative => "neg", + .bit_reverse => "rev", + .value => "val", + }}); + } +}; + +pub fn tokenize( + str: []const u8, + /// starting index of the expression + index: u32, + diags: *?Diagnostics, +) !Expression { + var ops = BoundedOperations.init(0) catch unreachable; + var values = BoundedValues.init(0) catch unreachable; + + const call_depth: u32 = 0; + try recursive_tokenize(call_depth, &ops, &values, str, index, diags); + return Expression{ + .ops = ops, + .values = values, + }; +} + +const TrimResult = struct { + str: []const u8, + index: u32, + + fn default(str: []const u8) TrimResult { + return TrimResult{ + .str = str, + .index = 0, + }; + } +}; + +fn trim_outer_parenthesis(str: []const u8) TrimResult { + // if the outer characters (not including whitespace) are parenthesis, then include the inside string + + // scan the prefix + const start: usize = for (str, 0..) |c, i| { + switch (c) { + ' ', + '\t', + => {}, + '(' => break i + 1, + else => return TrimResult.default(str), + } + } else return TrimResult.default(str); + + const end: usize = blk: { + var i = str.len - 1; + break :blk while (i > 0) : (i -= 1) { + switch (str[i]) { + ' ', + '\t', + => {}, + ')' => break i, + else => return TrimResult.default(str), + } + } else return TrimResult.default(str); + }; + + return TrimResult{ + .str = str[start..end], + .index = @intCast(u32, start), + }; +} + +fn recursive_tokenize( + call_depth: u32, + ops: *BoundedOperations, + values: *BoundedValues, + str: []const u8, + index: u32, + diags: *?Diagnostics, +) !void { + assert(call_depth < call_depth_max); + const trim_result = trim_outer_parenthesis(str); + const expr_str = trim_result.str; + const expr_index = index + trim_result.index; + + var parenthesis_found = false; + var depth: u32 = 0; + var i = @intCast(i32, expr_str.len - 1); + outer: while (i >= 0) : (i -= 1) { + const idx = @intCast(u32, i); + // TODO: how about if the expression is fully enveloped in parenthesis? + switch (expr_str[idx]) { + ')' => { + depth += 1; + parenthesis_found = true; + continue :outer; + }, + '(' => { + if (depth == 0) { + diags.* = Diagnostics.init(expr_index + idx, "mismatched parenthesis", .{}); + return error.MismatchedParenthesis; + } + + depth -= 1; + parenthesis_found = true; + if (depth != 0) + continue :outer; + }, + else => if (depth > 0) + continue :outer, + } + + const op: Operation = switch (expr_str[idx]) { + '+' => .add, + // needs context to determine if it's a negative or subtraction + '-' => blk: { + // it's negative if we have nothing to the left. If an operator + // is found to the left we continue + const is_negative = (i == 0) or is_negative: { + var j = i - 1; + while (j >= 0) : (j -= 1) { + const jdx = @intCast(u32, j); + switch (expr_str[jdx]) { + ' ', '\t' => continue, + '+', '-', '*', '/' => continue :outer, + else => break :is_negative false, + } + } + + break :is_negative true; + }; + + if (is_negative) { + try ops.append(.{ + .op = .negative, + .index = expr_index + idx, + }); + try recursive_tokenize(call_depth + 1, ops, values, expr_str[idx + 1 ..], expr_index + idx + 1, diags); + return; + } + + break :blk .sub; + }, + '*' => .mul, + '/' => .div, + ':' => { + const is_bit_reverse = (i != 0) and expr_str[idx - 1] == ':'; + if (is_bit_reverse) { + try ops.append(.{ + .op = .bit_reverse, + .index = expr_index + idx - 1, + }); + try recursive_tokenize(call_depth + 1, ops, values, expr_str[idx + 1 ..], expr_index + idx + 1, diags); + i -= 1; + return; + } + + return error.InvalidBitReverse; + }, + else => continue, + }; + + try ops.append(.{ + .op = op, + .index = expr_index + idx, + }); + try recursive_tokenize(call_depth + 1, ops, values, expr_str[idx + 1 ..], expr_index + idx + 1, diags); + try recursive_tokenize(call_depth + 1, ops, values, expr_str[0..idx], expr_index, diags); + return; + } else if (parenthesis_found) { + try recursive_tokenize(call_depth + 1, ops, values, expr_str, expr_index, diags); + } else { + // if we hit this path, then the full string has been scanned, and no operators + const trimmed = std.mem.trim(u8, expr_str, " \t"); + const value_index = expr_index + @intCast(u32, std.mem.indexOf(u8, expr_str, trimmed).?); + try ops.append(.{ + .op = .value, + .index = value_index, + }); + + try values.append(.{ + .str = trimmed, + .index = value_index, + }); + } + + if (depth != 0) { + diags.* = Diagnostics.init(expr_index + @intCast(u32, i), "mismatched parenthesis", .{}); + return error.MismatchedParenthesis; + } +} + +const EvaluatedValue = struct { + num: i128, + index: u32, + + pub fn format( + eval_value: EvaluatedValue, + comptime fmt: []const u8, + options: std.fmt.FormatOptions, + writer: anytype, + ) !void { + _ = fmt; + _ = options; + try writer.print("{}", .{eval_value.num}); + } +}; + +pub fn evaluate( + self: Expression, + define_lists: []const []const DefineWithIndex, + diags: *?Diagnostics, +) !i128 { + var values = std.BoundedArray(EvaluatedValue, 32).init(0) catch unreachable; + // parse/extract values into numbers + for (self.values.slice()) |entry| { + const value: EvaluatedValue = if (std.fmt.parseInt(i128, entry.str, 0)) |num| .{ + .num = num, + .index = entry.index, + } else |_| blk: { + // if it fails, try looking up the strings in definitions + for (define_lists) |define_list| + for (define_list) |define| + if (std.mem.eql(u8, define.name, entry.str)) + break :blk .{ + .num = define.value, + .index = define.index, + }; + + diags.* = Diagnostics.init(entry.index, "value doesn't parse as an integer, or define not found", .{}); + return error.UnresolvedValue; + }; + + try values.append(value); + } + + return if (self.ops.len == 1) blk: { + assert(self.values.len == 1); + assert(self.ops.get(0).op == .value); + + break :blk values.get(0).num; + } else blk: { + const result = try recursive_evaluate(0, self.ops.slice(), values.slice(), diags); + assert(result.consumed.ops == self.ops.len); + assert(result.consumed.values == self.values.len); + break :blk result.value; + }; +} + +const RecursiveEvalResult = struct { + value: i128, + consumed: struct { + ops: u32, + values: u32, + }, + index: u32, +}; + +fn recursive_evaluate( + call_depth: u32, + owis: []const OperationWithIndex, + values: []const EvaluatedValue, + diags: *?Diagnostics, +) !RecursiveEvalResult { + assert(call_depth < call_depth_max); + assert(owis.len != 0); + assert(values.len != 0); + + return switch (owis[0].op) { + .value => .{ + .value = values[0].num, + .index = values[0].index, + .consumed = .{ + .ops = 1, + .values = 1, + }, + }, + .negative => .{ + .value = -values[0].num, + .index = values[0].index, + .consumed = .{ + .ops = 2, + .values = 1, + }, + }, + .bit_reverse => blk: { + if (values[0].num >= std.math.maxInt(u32) or + values[0].num < std.math.minInt(i32)) + { + diags.* = Diagnostics.init(owis[0].index, "Evaluated value does not fit in 32-bits: 0x{x}", .{values[0].num}); + return error.EvaluatedValueDoesntFit; + } + + break :blk .{ + .value = @bitCast(i128, @bitReverse(@bitCast(u128, values[0].num)) >> (128 - 32)), + .index = values[0].index, + .consumed = .{ + .ops = 2, + .values = 1, + }, + }; + }, + .add, .sub, .mul, .div => blk: { + const rhs = try recursive_evaluate(call_depth + 1, owis[1..], values, diags); + const lhs = try recursive_evaluate(call_depth + 1, owis[1 + rhs.consumed.ops ..], values[rhs.consumed.values..], diags); + break :blk .{ + .consumed = .{ + .ops = 1 + lhs.consumed.ops + rhs.consumed.ops, + .values = lhs.consumed.values + rhs.consumed.values, + }, + .index = lhs.index, + .value = switch (owis[0].op) { + .add => lhs.value + rhs.value, + .sub => lhs.value - rhs.value, + .mul => lhs.value * rhs.value, + .div => div: { + if (rhs.value == 0) { + diags.* = Diagnostics.init(owis[0].index, "divide by zero (denominator evaluates to zero)", .{}); + return error.DivideByZero; + } + + // TODO: other requirement for @divExact + break :div @divExact(lhs.value, rhs.value); + }, + else => unreachable, + }, + }; + }, + }; +} + +const expect = std.testing.expect; +const expectEqual = std.testing.expectEqual; +const expectEqualStrings = std.testing.expectEqualStrings; + +fn expect_equal_slices_of_values( + expected: []const Value, + actual: []const Value, +) !void { + for (expected, actual) |e, a| { + try expectEqualStrings(e.str, a.str); + try expectEqual(e.index, a.index); + } +} + +fn expect_equal_slices_of_ops( + expected: []const OperationWithIndex, + actual: []const OperationWithIndex, +) !void { + for (expected, actual) |e, a| { + try expectEqual(e.op, a.op); + try expectEqual(e.index, a.index); + } +} + +test "expr.tokenize.integer" { + var diags: ?Diagnostics = null; + const expr = try tokenize("1", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 0, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 0, .str = "1" }, + }, expr.values.slice()); +} + +test "expr.tokenize.integer.parenthesis" { + var diags: ?Diagnostics = null; + const expr = try tokenize("(1)", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 1, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 1, .str = "1" }, + }, expr.values.slice()); +} + +test "expr.tokenize.integer.double parenthesis" { + var diags: ?Diagnostics = null; + const expr = try tokenize("((1))", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 2, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 2, .str = "1" }, + }, expr.values.slice()); +} + +test "expr.tokenize.symbol" { + var diags: ?Diagnostics = null; + const expr = try tokenize("BAR", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 0, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 0, .str = "BAR" }, + }, expr.values.slice()); +} + +test "expr.tokenize.add" { + var diags: ?Diagnostics = null; + const expr = try tokenize("1 + 2", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 2, .op = .add }, + .{ .index = 4, .op = .value }, + .{ .index = 0, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 4, .str = "2" }, + .{ .index = 0, .str = "1" }, + }, expr.values.slice()); +} + +test "expr.tokenize.add.chain" { + var diags: ?Diagnostics = null; + const expr = try tokenize("1 + 2 + 3", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 6, .op = .add }, + .{ .index = 8, .op = .value }, + .{ .index = 2, .op = .add }, + .{ .index = 4, .op = .value }, + .{ .index = 0, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 8, .str = "3" }, + .{ .index = 4, .str = "2" }, + .{ .index = 0, .str = "1" }, + }, expr.values.slice()); +} + +test "expr.tokenize.sub" { + var diags: ?Diagnostics = null; + const expr = try tokenize("1 - 2", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 2, .op = .sub }, + .{ .index = 4, .op = .value }, + .{ .index = 0, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 4, .str = "2" }, + .{ .index = 0, .str = "1" }, + }, expr.values.slice()); +} + +test "expr.tokenize.sub.nospace" { + var diags: ?Diagnostics = null; + const expr = try tokenize("1-2", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 1, .op = .sub }, + .{ .index = 2, .op = .value }, + .{ .index = 0, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 2, .str = "2" }, + .{ .index = 0, .str = "1" }, + }, expr.values.slice()); +} + +test "expr.tokenize.sub.negative" { + var diags: ?Diagnostics = null; + const expr = try tokenize("1 - -2", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 2, .op = .sub }, + .{ .index = 4, .op = .negative }, + .{ .index = 5, .op = .value }, + .{ .index = 0, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 5, .str = "2" }, + .{ .index = 0, .str = "1" }, + }, expr.values.slice()); +} + +test "expr.tokenize.mul" { + var diags: ?Diagnostics = null; + const expr = try tokenize("1 * 2", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 2, .op = .mul }, + .{ .index = 4, .op = .value }, + .{ .index = 0, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 4, .str = "2" }, + .{ .index = 0, .str = "1" }, + }, expr.values.slice()); +} + +test "expr.tokenize.div" { + var diags: ?Diagnostics = null; + const expr = try tokenize("1 / 2", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 2, .op = .div }, + .{ .index = 4, .op = .value }, + .{ .index = 0, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 4, .str = "2" }, + .{ .index = 0, .str = "1" }, + }, expr.values.slice()); +} + +test "expr.tokenize.negative" { + var diags: ?Diagnostics = null; + const expr = try tokenize("-1", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 0, .op = .negative }, + .{ .index = 1, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 1, .str = "1" }, + }, expr.values.slice()); +} + +test "expr.tokenize.bit reverse" { + var diags: ?Diagnostics = null; + const expr = try tokenize("::1", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 0, .op = .bit_reverse }, + .{ .index = 2, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 2, .str = "1" }, + }, expr.values.slice()); +} + +test "expr.tokenzie.parenthesis" { + var diags: ?Diagnostics = null; + const expr = try tokenize("1 * (::2 + (12 / 3)) - 5", 0, &diags); + + try expect_equal_slices_of_ops(&.{ + .{ .index = 21, .op = .sub }, + .{ .index = 23, .op = .value }, + .{ .index = 2, .op = .mul }, + .{ .index = 9, .op = .add }, + .{ .index = 15, .op = .div }, + .{ .index = 17, .op = .value }, + .{ .index = 12, .op = .value }, + .{ .index = 5, .op = .bit_reverse }, + .{ .index = 7, .op = .value }, + .{ .index = 0, .op = .value }, + }, expr.ops.slice()); + + try expect_equal_slices_of_values(&.{ + .{ .index = 23, .str = "5" }, + .{ .index = 17, .str = "3" }, + .{ .index = 12, .str = "12" }, + .{ .index = 7, .str = "2" }, + .{ .index = 0, .str = "1" }, + }, expr.values.slice()); +} + +fn evaluate_test(expected: i128, str: []const u8, define_list: []const DefineWithIndex) !void { + var diags: ?Diagnostics = null; + const expr = tokenize(str, 0, &diags) catch |err| { + if (diags) |d| + std.log.err("{}: {s}", .{ err, d.message.slice() }); + + return err; + }; + + const actual = expr.evaluate(&.{define_list}, &diags) catch |err| { + if (diags) |d| + std.log.err("{}: {s}", .{ err, d.message.slice() }) + else + std.log.err("{}", .{err}); + + return err; + }; + + try expectEqual(expected, actual); +} + +test "expr.evaluate.integer" { + try evaluate_test(1, "1", &.{}); +} + +test "expr.evaluate.symbol" { + try evaluate_test(5, "BAR", &.{ + .{ + .name = "BAR", + .value = 5, + .index = 0, + }, + }); +} + +test "expr.evaluate.add" { + try evaluate_test(3, "1 + 2", &.{}); + try evaluate_test(6, "1 + 2 + 3", &.{}); +} + +test "expr.evaluate.sub" { + try evaluate_test(1, "2 - 1", &.{}); + try evaluate_test(1, "(NUM_CYCLES - 1)", &.{ + .{ + .name = "NUM_CYCLES", + .value = 2, + .index = 1, + }, + }); +} + +test "expr.evaluate.mul" { + try evaluate_test(9, "3 * 3", &.{}); +} + +test "expr.evaluate.div" { + try evaluate_test(3, "9 / 3", &.{}); + try evaluate_test(3, "9 / 3", &.{}); +} + +test "expr.evaluate.negative" { + try evaluate_test(-3, "-3", &.{}); +} + +test "expr.evaluate.bit reverse" { + try evaluate_test(0x80000000, "::1", &.{}); +} + +test "expr.evaluate.parenthesis" { + try evaluate_test(15, "5 * (1 + 2)", &.{}); + try evaluate_test(1 * (@bitReverse(@as(u32, 2)) + (12 / 3)) - 5, "1 * (::2 + (12 / 3)) - 5", &.{}); +} diff --git a/src/hal/pio/assembler/comparison_tests.zig b/src/hal/pio/assembler/comparison_tests.zig new file mode 100644 index 0000000..4a39465 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests.zig @@ -0,0 +1,168 @@ +const std = @import("std"); +const assembler = @import("../assembler.zig"); +const tokenizer = @import("tokenizer.zig"); +const c = @cImport({ + @cDefine("PICO_NO_HARDWARE", "1"); + @cInclude("stdint.h"); + @cInclude("comparison_tests/addition.pio.h"); + @cInclude("comparison_tests/apa102.pio.h"); + @cInclude("comparison_tests/blink.pio.h"); + @cInclude("comparison_tests/clocked_input.pio.h"); + @cInclude("comparison_tests/differential_manchester.pio.h"); + @cInclude("comparison_tests/hello.pio.h"); + @cInclude("comparison_tests/hub75.pio.h"); + @cInclude("comparison_tests/i2c.pio.h"); + @cInclude("comparison_tests/manchester_encoding.pio.h"); + @cInclude("comparison_tests/nec_carrier_burst.pio.h"); + @cInclude("comparison_tests/nec_carrier_control.pio.h"); + @cInclude("comparison_tests/nec_receive.pio.h"); + @cInclude("comparison_tests/pio_serialiser.pio.h"); + @cInclude("comparison_tests/pwm.pio.h"); + @cInclude("comparison_tests/quadrature_encoder.pio.h"); + @cInclude("comparison_tests/resistor_dac.pio.h"); + @cInclude("comparison_tests/spi.pio.h"); + @cInclude("comparison_tests/squarewave.pio.h"); + @cInclude("comparison_tests/squarewave_fast.pio.h"); + @cInclude("comparison_tests/squarewave_wrap.pio.h"); + @cInclude("comparison_tests/st7789_lcd.pio.h"); + @cInclude("comparison_tests/uart_rx.pio.h"); + @cInclude("comparison_tests/uart_tx.pio.h"); + @cInclude("comparison_tests/ws2812.pio.h"); +}); + +fn pio_comparison(comptime source: []const u8) !void { + const output = comptime assembler.assemble(source, .{}) catch unreachable; + try std.testing.expect(output.programs.len > 0); + + inline for (output.programs) |program| { + const expected_insns = @field(c, program.name ++ "_program_instructions"); + for (program.instructions, expected_insns) |actual, expected| { + std.log.debug("expected: 0x{x}", .{expected}); + std.log.debug(" actual: 0x{x}", .{actual}); + std.log.debug("", .{}); + } + + for (program.instructions, expected_insns) |actual, expected| + try std.testing.expectEqual(expected, actual); + } +} + +test "pio.comparison.addition" { + @setEvalBranchQuota(4000); + try pio_comparison(@embedFile("comparison_tests/addition.pio")); +} + +test "pio.comparison.apa102" { + @setEvalBranchQuota(10000); + try pio_comparison(@embedFile("comparison_tests/apa102.pio")); +} + +test "pio.comparison.blink" { + @setEvalBranchQuota(4000); + try pio_comparison(@embedFile("comparison_tests/blink.pio")); +} + +test "pio.comparison.clocked_input" { + @setEvalBranchQuota(5000); + try pio_comparison(@embedFile("comparison_tests/clocked_input.pio")); +} + +test "pio.comparison.differential_manchester" { + @setEvalBranchQuota(14000); + try pio_comparison(@embedFile("comparison_tests/differential_manchester.pio")); +} + +test "pio.comparison.hello" { + @setEvalBranchQuota(3000); + try pio_comparison(@embedFile("comparison_tests/hello.pio")); +} + +test "pio.comparison.hub75" { + @setEvalBranchQuota(17000); + try pio_comparison(@embedFile("comparison_tests/hub75.pio")); +} + +test "pio.comparison.i2c" { + @setEvalBranchQuota(17000); + try pio_comparison(@embedFile("comparison_tests/i2c.pio")); +} + +test "pio.comparison.manchester_encoding" { + @setEvalBranchQuota(11000); + try pio_comparison(@embedFile("comparison_tests/manchester_encoding.pio")); +} + +test "pio.comparison.nec_carrier_burst" { + @setEvalBranchQuota(6000); + try pio_comparison(@embedFile("comparison_tests/nec_carrier_burst.pio")); +} + +test "pio.comparison.nec_carrier_control" { + @setEvalBranchQuota(9000); + try pio_comparison(@embedFile("comparison_tests/nec_carrier_control.pio")); +} + +test "pio.comparison.nec_receive" { + @setEvalBranchQuota(11000); + try pio_comparison(@embedFile("comparison_tests/nec_receive.pio")); +} + +test "pio.comparison.pio_serialiser" { + @setEvalBranchQuota(3000); + try pio_comparison(@embedFile("comparison_tests/pio_serialiser.pio")); +} + +test "pio.comparison.pwm" { + @setEvalBranchQuota(4000); + try pio_comparison(@embedFile("comparison_tests/pwm.pio")); +} + +test "pio.comparison.quadrature_encoder" { + @setEvalBranchQuota(17000); + try pio_comparison(@embedFile("comparison_tests/quadrature_encoder.pio")); +} + +test "pio.comparison.resistor_dac" { + @setEvalBranchQuota(3000); + try pio_comparison(@embedFile("comparison_tests/resistor_dac.pio")); +} + +test "pio.comparison.spi" { + @setEvalBranchQuota(22000); + try pio_comparison(@embedFile("comparison_tests/spi.pio")); +} + +test "pio.comparison.squarewave" { + @setEvalBranchQuota(2000); + try pio_comparison(@embedFile("comparison_tests/squarewave.pio")); +} + +test "pio.comparison.squarewave_fast" { + @setEvalBranchQuota(2000); + try pio_comparison(@embedFile("comparison_tests/squarewave_fast.pio")); +} + +test "pio.comparison.squarewave_wrap" { + @setEvalBranchQuota(3000); + try pio_comparison(@embedFile("comparison_tests/squarewave_wrap.pio")); +} + +test "pio.comparison.st7789_lcd" { + @setEvalBranchQuota(5000); + try pio_comparison(@embedFile("comparison_tests/st7789_lcd.pio")); +} + +test "pio.comparison.uart_rx" { + @setEvalBranchQuota(10000); + try pio_comparison(@embedFile("comparison_tests/uart_rx.pio")); +} + +test "pio.comparison.uart_tx" { + @setEvalBranchQuota(6000); + try pio_comparison(@embedFile("comparison_tests/uart_tx.pio")); +} + +test "pio.comparison.ws2812" { + @setEvalBranchQuota(10000); + try pio_comparison(@embedFile("comparison_tests/ws2812.pio")); +} diff --git a/src/hal/pio/assembler/comparison_tests/README.adoc b/src/hal/pio/assembler/comparison_tests/README.adoc new file mode 100644 index 0000000..872e125 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/README.adoc @@ -0,0 +1,4 @@ += PIO example programs for testing + +These were all taken from https://github.com/raspberrypi/pico-examples[the official pico examples repo]. +The headers are generated using `pioasm`. diff --git a/src/hal/pio/assembler/comparison_tests/addition.pio b/src/hal/pio/assembler/comparison_tests/addition.pio new file mode 100644 index 0000000..8eddd0e --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/addition.pio @@ -0,0 +1,33 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program addition + +; Pop two 32 bit integers from the TX FIFO, add them together, and push the +; result to the TX FIFO. Autopush/pull should be disabled as we're using +; explicit push and pull instructions. +; +; This program uses the two's complement identity x + y == ~(~x - y) + + pull + mov x, ~osr + pull + mov y, osr + jmp test ; this loop is equivalent to the following C code: +incr: ; while (y--) + jmp x-- test ; x--; +test: ; This has the effect of subtracting y from x, eventually. + jmp y-- incr + mov isr, ~x + push + +% c-sdk { +static inline void addition_program_init(PIO pio, uint sm, uint offset) { + pio_sm_config c = addition_program_get_default_config(offset); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/addition.pio.h b/src/hal/pio/assembler/comparison_tests/addition.pio.h new file mode 100644 index 0000000..7621c58 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/addition.pio.h @@ -0,0 +1,52 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// -------- // +// addition // +// -------- // + +#define addition_wrap_target 0 +#define addition_wrap 8 + +static const uint16_t addition_program_instructions[] = { + // .wrap_target + 0x80a0, // 0: pull block + 0xa02f, // 1: mov x, !osr + 0x80a0, // 2: pull block + 0xa047, // 3: mov y, osr + 0x0006, // 4: jmp 6 + 0x0046, // 5: jmp x--, 6 + 0x0085, // 6: jmp y--, 5 + 0xa0c9, // 7: mov isr, !x + 0x8020, // 8: push block + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program addition_program = { + .instructions = addition_program_instructions, + .length = 9, + .origin = -1, +}; + +static inline pio_sm_config addition_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + addition_wrap_target, offset + addition_wrap); + return c; +} + +static inline void addition_program_init(PIO pio, uint sm, uint offset) { + pio_sm_config c = addition_program_get_default_config(offset); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/apa102.pio b/src/hal/pio/assembler/comparison_tests/apa102.pio new file mode 100644 index 0000000..5d76f08 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/apa102.pio @@ -0,0 +1,89 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program apa102_mini +.side_set 1 + +; This is really just a TX-only SPI. CLK is side-set pin 0, DIN is OUT pin 0. +; Autopull enabled, threshold 32. +; +; Every word (32 bits) written to the FIFO will be shifted out in its entirety, MSB-first. + + out pins, 1 side 0 ; Stall here when no data (still asserts clock low) + nop side 1 + +% c-sdk { +#include "hardware/clocks.h" +static inline void apa102_mini_program_init(PIO pio, uint sm, uint offset, + uint baud, uint pin_clk, uint pin_din) { + pio_sm_set_pins_with_mask(pio, sm, 0, (1u << pin_clk) | (1u << pin_din)); + pio_sm_set_pindirs_with_mask(pio, sm, ~0u, (1u << pin_clk) | (1u << pin_din)); + pio_gpio_init(pio, pin_clk); + pio_gpio_init(pio, pin_din); + + pio_sm_config c = apa102_mini_program_get_default_config(offset); + sm_config_set_out_pins(&c, pin_din, 1); + sm_config_set_sideset_pins(&c, pin_clk); + // Shift to right, autopull with threshold 32 + sm_config_set_out_shift(&c, false, true, 32); + // Deeper FIFO as we're not doing any RX + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + // We transmit 1 bit every 2 execution cycles + float div = (float)clock_get_hz(clk_sys) / (2 * baud); + sm_config_set_clkdiv(&c, div); + + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +%} + +.program apa102_rgb555 + +; Alternative program to unpack two RGB555 pixels from a FIFO word and transmit. +; This makes it easier to DMA large buffers without processor involvement. + +; OSR: shift to right +; ISR: shift to right + +; To set brightness, set ISR to bit-reverse of 5-bit brightness, +; followed by 111. (00...00_b0b1b2b3b4_111) + +; DMA pixel format is 0RRRRRGGGGGBBBBB x2 (15 bpp, 2px per FIFO word) + +; APA102 command structure: +; increasing time ---->> +; | byte 3 | byte 2 | byte 1 | byte 0 | +; |7 0|7 0|7 0|7 0| +; ------------------------------------- +; Pixel |111bbbbb|BBBBBBBB|GGGGGGGG|RRRRRRRR| +; Start Frame |00000000|00000000|00000000|00000000| +; Stop Frame |11111111|11111111|11111111|11111111| + +.wrap_target +public pixel_out: + ; pixel_out formats an APA102 colour command in the ISR. + ; bit_run shifts 32 bits out of the ISR, with clock. + pull ifempty + set x, 2 +colour_loop: + in osr, 5 + out null, 5 + in null, 3 + jmp x-- colour_loop + in y, 8 + mov isr, ::isr ; reverse for msb-first wire order + out null, 1 +public bit_run: + ; in isr, n rotates ISR by n bits (right rotation only) + ; Use this to perform out shifts from ISR, via mov pins + set x, 31 +bit_out: + set pins, 0 + mov pins, isr [6] + set pins, 1 + in isr, 1 [6] + jmp x-- bit_out +.wrap diff --git a/src/hal/pio/assembler/comparison_tests/apa102.pio.h b/src/hal/pio/assembler/comparison_tests/apa102.pio.h new file mode 100644 index 0000000..fd5a520 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/apa102.pio.h @@ -0,0 +1,105 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ----------- // +// apa102_mini // +// ----------- // + +#define apa102_mini_wrap_target 0 +#define apa102_mini_wrap 1 + +static const uint16_t apa102_mini_program_instructions[] = { + // .wrap_target + 0x6001, // 0: out pins, 1 side 0 + 0xb042, // 1: nop side 1 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program apa102_mini_program = { + .instructions = apa102_mini_program_instructions, + .length = 2, + .origin = -1, +}; + +static inline pio_sm_config apa102_mini_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + apa102_mini_wrap_target, offset + apa102_mini_wrap); + sm_config_set_sideset(&c, 1, false, false); + return c; +} + +#include "hardware/clocks.h" +static inline void apa102_mini_program_init(PIO pio, uint sm, uint offset, + uint baud, uint pin_clk, uint pin_din) { + pio_sm_set_pins_with_mask(pio, sm, 0, (1u << pin_clk) | (1u << pin_din)); + pio_sm_set_pindirs_with_mask(pio, sm, ~0u, (1u << pin_clk) | (1u << pin_din)); + pio_gpio_init(pio, pin_clk); + pio_gpio_init(pio, pin_din); + pio_sm_config c = apa102_mini_program_get_default_config(offset); + sm_config_set_out_pins(&c, pin_din, 1); + sm_config_set_sideset_pins(&c, pin_clk); + // Shift to right, autopull with threshold 32 + sm_config_set_out_shift(&c, false, true, 32); + // Deeper FIFO as we're not doing any RX + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + // We transmit 1 bit every 2 execution cycles + float div = (float)clock_get_hz(clk_sys) / (2 * baud); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + +// ------------- // +// apa102_rgb555 // +// ------------- // + +#define apa102_rgb555_wrap_target 0 +#define apa102_rgb555_wrap 14 + +#define apa102_rgb555_offset_pixel_out 0u +#define apa102_rgb555_offset_bit_run 9u + +static const uint16_t apa102_rgb555_program_instructions[] = { + // .wrap_target + 0x80e0, // 0: pull ifempty block + 0xe022, // 1: set x, 2 + 0x40e5, // 2: in osr, 5 + 0x6065, // 3: out null, 5 + 0x4063, // 4: in null, 3 + 0x0042, // 5: jmp x--, 2 + 0x4048, // 6: in y, 8 + 0xa0d6, // 7: mov isr, ::isr + 0x6061, // 8: out null, 1 + 0xe03f, // 9: set x, 31 + 0xe000, // 10: set pins, 0 + 0xa606, // 11: mov pins, isr [6] + 0xe001, // 12: set pins, 1 + 0x46c1, // 13: in isr, 1 [6] + 0x004a, // 14: jmp x--, 10 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program apa102_rgb555_program = { + .instructions = apa102_rgb555_program_instructions, + .length = 15, + .origin = -1, +}; + +static inline pio_sm_config apa102_rgb555_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + apa102_rgb555_wrap_target, offset + apa102_rgb555_wrap); + return c; +} +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/blink.pio b/src/hal/pio/assembler/comparison_tests/blink.pio new file mode 100644 index 0000000..ef30900 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/blink.pio @@ -0,0 +1,34 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +; SET pin 0 should be mapped to your LED GPIO + +.program blink + pull block + out y, 32 +.wrap_target + mov x, y + set pins, 1 ; Turn LED on +lp1: + jmp x-- lp1 ; Delay for (x + 1) cycles, x is a 32 bit number + mov x, y + set pins, 0 ; Turn LED off +lp2: + jmp x-- lp2 ; Delay for the same number of cycles again +.wrap ; Blink forever! + + +% c-sdk { +// this is a raw helper function for use by the user which sets up the GPIO output, and configures the SM to output on a particular pin + +void blink_program_init(PIO pio, uint sm, uint offset, uint pin) { + pio_gpio_init(pio, pin); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + pio_sm_config c = blink_program_get_default_config(offset); + sm_config_set_set_pins(&c, pin, 1); + pio_sm_init(pio, sm, offset, &c); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/blink.pio.h b/src/hal/pio/assembler/comparison_tests/blink.pio.h new file mode 100644 index 0000000..e466a28 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/blink.pio.h @@ -0,0 +1,54 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ----- // +// blink // +// ----- // + +#define blink_wrap_target 2 +#define blink_wrap 7 + +static const uint16_t blink_program_instructions[] = { + 0x80a0, // 0: pull block + 0x6040, // 1: out y, 32 + // .wrap_target + 0xa022, // 2: mov x, y + 0xe001, // 3: set pins, 1 + 0x0044, // 4: jmp x--, 4 + 0xa022, // 5: mov x, y + 0xe000, // 6: set pins, 0 + 0x0047, // 7: jmp x--, 7 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program blink_program = { + .instructions = blink_program_instructions, + .length = 8, + .origin = -1, +}; + +static inline pio_sm_config blink_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + blink_wrap_target, offset + blink_wrap); + return c; +} + +// this is a raw helper function for use by the user which sets up the GPIO output, and configures the SM to output on a particular pin +void blink_program_init(PIO pio, uint sm, uint offset, uint pin) { + pio_gpio_init(pio, pin); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + pio_sm_config c = blink_program_get_default_config(offset); + sm_config_set_set_pins(&c, pin, 1); + pio_sm_init(pio, sm, offset, &c); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/clocked_input.pio b/src/hal/pio/assembler/comparison_tests/clocked_input.pio new file mode 100644 index 0000000..51fa54c --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/clocked_input.pio @@ -0,0 +1,51 @@ +; +; Copyright (c) 2021 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program clocked_input + +; Sample bits using an external clock, and push groups of bits into the RX FIFO. +; - IN pin 0 is the data pin +; - IN pin 1 is the clock pin +; - Autopush is enabled, threshold 8 +; +; This program samples data with each rising clock edge (like mode 0 or mode 3 +; SPI). The data is actually sampled one system clock cycle after the rising +; edge is observed, so a clock ratio of at least input_clk < clk_sys / 6 is +; recommended for good sampling alignment. + + wait 0 pin 1 + wait 1 pin 1 + in pins, 1 + +% c-sdk { +static inline void clocked_input_program_init(PIO pio, uint sm, uint offset, uint pin) { + pio_sm_config c = clocked_input_program_get_default_config(offset); + + // Set the IN base pin to the provided `pin` parameter. This is the data + // pin, and the next-numbered GPIO is used as the clock pin. + sm_config_set_in_pins(&c, pin); + // Set the pin directions to input at the PIO + pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false); + // Connect these GPIOs to this PIO block + pio_gpio_init(pio, pin); + pio_gpio_init(pio, pin + 1); + + // Shifting to left matches the customary MSB-first ordering of SPI. + sm_config_set_in_shift( + &c, + false, // Shift-to-right = false (i.e. shift to left) + true, // Autopush enabled + 8 // Autopush threshold = 8 + ); + + // We only receive, so disable the TX FIFO to make the RX FIFO deeper. + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + + // Load our configuration, and start the program from the beginning + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/clocked_input.pio.h b/src/hal/pio/assembler/comparison_tests/clocked_input.pio.h new file mode 100644 index 0000000..277b939 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/clocked_input.pio.h @@ -0,0 +1,64 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ------------- // +// clocked_input // +// ------------- // + +#define clocked_input_wrap_target 0 +#define clocked_input_wrap 2 + +static const uint16_t clocked_input_program_instructions[] = { + // .wrap_target + 0x2021, // 0: wait 0 pin, 1 + 0x20a1, // 1: wait 1 pin, 1 + 0x4001, // 2: in pins, 1 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program clocked_input_program = { + .instructions = clocked_input_program_instructions, + .length = 3, + .origin = -1, +}; + +static inline pio_sm_config clocked_input_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + clocked_input_wrap_target, offset + clocked_input_wrap); + return c; +} + +static inline void clocked_input_program_init(PIO pio, uint sm, uint offset, uint pin) { + pio_sm_config c = clocked_input_program_get_default_config(offset); + // Set the IN base pin to the provided `pin` parameter. This is the data + // pin, and the next-numbered GPIO is used as the clock pin. + sm_config_set_in_pins(&c, pin); + // Set the pin directions to input at the PIO + pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false); + // Connect these GPIOs to this PIO block + pio_gpio_init(pio, pin); + pio_gpio_init(pio, pin + 1); + // Shifting to left matches the customary MSB-first ordering of SPI. + sm_config_set_in_shift( + &c, + false, // Shift-to-right = false (i.e. shift to left) + true, // Autopush enabled + 8 // Autopush threshold = 8 + ); + // We only receive, so disable the TX FIFO to make the RX FIFO deeper. + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + // Load our configuration, and start the program from the beginning + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/differential_manchester.pio b/src/hal/pio/assembler/comparison_tests/differential_manchester.pio new file mode 100644 index 0000000..a9e825d --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/differential_manchester.pio @@ -0,0 +1,104 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program differential_manchester_tx +.side_set 1 opt + +; Transmit one bit every 16 cycles. In each bit period: +; - A '0' is encoded as a transition at the start of the bit period +; - A '1' is encoded as a transition at the start *and* in the middle +; +; Side-set bit 0 must be mapped to the data output pin. +; Autopull must be enabled. + +public start: +initial_high: + out x, 1 ; Start of bit period: always assert transition + jmp !x high_0 side 1 [6] ; Test the data bit we just shifted out of OSR +high_1: + nop + jmp initial_high side 0 [6] ; For `1` bits, also transition in the middle +high_0: + jmp initial_low [7] ; Otherwise, the line is stable in the middle + +initial_low: + out x, 1 ; Always shift 1 bit from OSR to X so we can + jmp !x low_0 side 0 [6] ; branch on it. Autopull refills OSR for us. +low_1: + nop + jmp initial_low side 1 [6] ; If there are two transitions, return to +low_0: + jmp initial_high [7] ; the initial line state is flipped! + +% c-sdk { +static inline void differential_manchester_tx_program_init(PIO pio, uint sm, uint offset, uint pin, float div) { + pio_sm_set_pins_with_mask(pio, sm, 0, 1u << pin); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + pio_gpio_init(pio, pin); + + pio_sm_config c = differential_manchester_tx_program_get_default_config(offset); + sm_config_set_sideset_pins(&c, pin); + sm_config_set_out_shift(&c, true, true, 32); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset + differential_manchester_tx_offset_start, &c); + + // Execute a blocking pull so that we maintain the initial line state until data is available + pio_sm_exec(pio, sm, pio_encode_pull(false, true)); + pio_sm_set_enabled(pio, sm, true); +} +%} +.program differential_manchester_rx + +; Assumes line is idle low +; One bit is 16 cycles. In each bit period: +; - A '0' is encoded as a transition at time 0 +; - A '1' is encoded as a transition at time 0 and a transition at time T/2 +; +; The IN mapping and the JMP pin select must both be mapped to the GPIO used for +; RX data. Autopush must be enabled. + +public start: +initial_high: ; Find rising edge at start of bit period + wait 1 pin, 0 [11] ; Delay to eye of second half-period (i.e 3/4 of way + jmp pin high_0 ; through bit) and branch on RX pin high/low. +high_1: + in x, 1 ; Second transition detected (a `1` data symbol) + jmp initial_high +high_0: + in y, 1 [1] ; Line still high, no centre transition (data is `0`) + ; Fall-through + +.wrap_target +initial_low: ; Find falling edge at start of bit period + wait 0 pin, 0 [11] ; Delay to eye of second half-period + jmp pin low_1 +low_0: + in y, 1 ; Line still low, no centre transition (data is `0`) + jmp initial_high +low_1: ; Second transition detected (data is `1`) + in x, 1 [1] +.wrap + +% c-sdk { +static inline void differential_manchester_rx_program_init(PIO pio, uint sm, uint offset, uint pin, float div) { + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false); + pio_gpio_init(pio, pin); + + pio_sm_config c = differential_manchester_rx_program_get_default_config(offset); + sm_config_set_in_pins(&c, pin); // for WAIT + sm_config_set_jmp_pin(&c, pin); // for JMP + sm_config_set_in_shift(&c, true, true, 32); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + + // X and Y are set to 0 and 1, to conveniently emit these to ISR/FIFO. + pio_sm_exec(pio, sm, pio_encode_set(pio_x, 1)); + pio_sm_exec(pio, sm, pio_encode_set(pio_y, 0)); + pio_sm_set_enabled(pio, sm, true); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/differential_manchester.pio.h b/src/hal/pio/assembler/comparison_tests/differential_manchester.pio.h new file mode 100644 index 0000000..c8d76ca --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/differential_manchester.pio.h @@ -0,0 +1,120 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// -------------------------- // +// differential_manchester_tx // +// -------------------------- // + +#define differential_manchester_tx_wrap_target 0 +#define differential_manchester_tx_wrap 9 + +#define differential_manchester_tx_offset_start 0u + +static const uint16_t differential_manchester_tx_program_instructions[] = { + // .wrap_target + 0x6021, // 0: out x, 1 + 0x1e24, // 1: jmp !x, 4 side 1 [6] + 0xa042, // 2: nop + 0x1600, // 3: jmp 0 side 0 [6] + 0x0705, // 4: jmp 5 [7] + 0x6021, // 5: out x, 1 + 0x1629, // 6: jmp !x, 9 side 0 [6] + 0xa042, // 7: nop + 0x1e05, // 8: jmp 5 side 1 [6] + 0x0700, // 9: jmp 0 [7] + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program differential_manchester_tx_program = { + .instructions = differential_manchester_tx_program_instructions, + .length = 10, + .origin = -1, +}; + +static inline pio_sm_config differential_manchester_tx_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + differential_manchester_tx_wrap_target, offset + differential_manchester_tx_wrap); + sm_config_set_sideset(&c, 2, true, false); + return c; +} + +static inline void differential_manchester_tx_program_init(PIO pio, uint sm, uint offset, uint pin, float div) { + pio_sm_set_pins_with_mask(pio, sm, 0, 1u << pin); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + pio_gpio_init(pio, pin); + pio_sm_config c = differential_manchester_tx_program_get_default_config(offset); + sm_config_set_sideset_pins(&c, pin); + sm_config_set_out_shift(&c, true, true, 32); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset + differential_manchester_tx_offset_start, &c); + // Execute a blocking pull so that we maintain the initial line state until data is available + pio_sm_exec(pio, sm, pio_encode_pull(false, true)); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + +// -------------------------- // +// differential_manchester_rx // +// -------------------------- // + +#define differential_manchester_rx_wrap_target 5 +#define differential_manchester_rx_wrap 9 + +#define differential_manchester_rx_offset_start 0u + +static const uint16_t differential_manchester_rx_program_instructions[] = { + 0x2ba0, // 0: wait 1 pin, 0 [11] + 0x00c4, // 1: jmp pin, 4 + 0x4021, // 2: in x, 1 + 0x0000, // 3: jmp 0 + 0x4141, // 4: in y, 1 [1] + // .wrap_target + 0x2b20, // 5: wait 0 pin, 0 [11] + 0x00c9, // 6: jmp pin, 9 + 0x4041, // 7: in y, 1 + 0x0000, // 8: jmp 0 + 0x4121, // 9: in x, 1 [1] + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program differential_manchester_rx_program = { + .instructions = differential_manchester_rx_program_instructions, + .length = 10, + .origin = -1, +}; + +static inline pio_sm_config differential_manchester_rx_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + differential_manchester_rx_wrap_target, offset + differential_manchester_rx_wrap); + return c; +} + +static inline void differential_manchester_rx_program_init(PIO pio, uint sm, uint offset, uint pin, float div) { + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false); + pio_gpio_init(pio, pin); + pio_sm_config c = differential_manchester_rx_program_get_default_config(offset); + sm_config_set_in_pins(&c, pin); // for WAIT + sm_config_set_jmp_pin(&c, pin); // for JMP + sm_config_set_in_shift(&c, true, true, 32); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + // X and Y are set to 0 and 1, to conveniently emit these to ISR/FIFO. + pio_sm_exec(pio, sm, pio_encode_set(pio_x, 1)); + pio_sm_exec(pio, sm, pio_encode_set(pio_y, 0)); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/hello.pio b/src/hal/pio/assembler/comparison_tests/hello.pio new file mode 100644 index 0000000..9eac4de --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/hello.pio @@ -0,0 +1,34 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program hello + +; Repeatedly get one word of data from the TX FIFO, stalling when the FIFO is +; empty. Write the least significant bit to the OUT pin group. + +loop: + pull + out pins, 1 + jmp loop + +% c-sdk { +static inline void hello_program_init(PIO pio, uint sm, uint offset, uint pin) { + pio_sm_config c = hello_program_get_default_config(offset); + + // Map the state machine's OUT pin group to one pin, namely the `pin` + // parameter to this function. + sm_config_set_out_pins(&c, pin, 1); + // Set this pin's GPIO function (connect PIO to the pad) + pio_gpio_init(pio, pin); + // Set the pin direction to output at the PIO + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + + // Load our configuration, and jump to the start of the program + pio_sm_init(pio, sm, offset, &c); + // Set the state machine running + pio_sm_set_enabled(pio, sm, true); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/hello.pio.h b/src/hal/pio/assembler/comparison_tests/hello.pio.h new file mode 100644 index 0000000..415d1cc --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/hello.pio.h @@ -0,0 +1,55 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ----- // +// hello // +// ----- // + +#define hello_wrap_target 0 +#define hello_wrap 2 + +static const uint16_t hello_program_instructions[] = { + // .wrap_target + 0x80a0, // 0: pull block + 0x6001, // 1: out pins, 1 + 0x0000, // 2: jmp 0 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program hello_program = { + .instructions = hello_program_instructions, + .length = 3, + .origin = -1, +}; + +static inline pio_sm_config hello_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + hello_wrap_target, offset + hello_wrap); + return c; +} + +static inline void hello_program_init(PIO pio, uint sm, uint offset, uint pin) { + pio_sm_config c = hello_program_get_default_config(offset); + // Map the state machine's OUT pin group to one pin, namely the `pin` + // parameter to this function. + sm_config_set_out_pins(&c, pin, 1); + // Set this pin's GPIO function (connect PIO to the pad) + pio_gpio_init(pio, pin); + // Set the pin direction to output at the PIO + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + // Load our configuration, and jump to the start of the program + pio_sm_init(pio, sm, offset, &c); + // Set the state machine running + pio_sm_set_enabled(pio, sm, true); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/hub75.pio b/src/hal/pio/assembler/comparison_tests/hub75.pio new file mode 100644 index 0000000..a6fb619 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/hub75.pio @@ -0,0 +1,128 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program hub75_row + +; side-set pin 0 is LATCH +; side-set pin 1 is OEn +; OUT pins are row select A-E +; +; Each FIFO record consists of: +; - 5-bit row select (LSBs) +; - Pulse width - 1 (27 MSBs) +; +; Repeatedly select a row, pulse LATCH, and generate a pulse of a certain +; width on OEn. + +.side_set 2 + +.wrap_target + out pins, 5 [7] side 0x2 ; Deassert OEn, output row select + out x, 27 [7] side 0x3 ; Pulse LATCH, get OEn pulse width +pulse_loop: + jmp x-- pulse_loop side 0x0 ; Assert OEn for x+1 cycles +.wrap + +% c-sdk { +static inline void hub75_row_program_init(PIO pio, uint sm, uint offset, uint row_base_pin, uint n_row_pins, uint latch_base_pin) { + pio_sm_set_consecutive_pindirs(pio, sm, row_base_pin, n_row_pins, true); + pio_sm_set_consecutive_pindirs(pio, sm, latch_base_pin, 2, true); + for (uint i = row_base_pin; i < row_base_pin + n_row_pins; ++i) + pio_gpio_init(pio, i); + pio_gpio_init(pio, latch_base_pin); + pio_gpio_init(pio, latch_base_pin + 1); + + pio_sm_config c = hub75_row_program_get_default_config(offset); + sm_config_set_out_pins(&c, row_base_pin, n_row_pins); + sm_config_set_sideset_pins(&c, latch_base_pin); + sm_config_set_out_shift(&c, true, true, 32); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +static inline void hub75_wait_tx_stall(PIO pio, uint sm) { + uint32_t txstall_mask = 1u << (PIO_FDEBUG_TXSTALL_LSB + sm); + pio->fdebug = txstall_mask; + while (!(pio->fdebug & txstall_mask)) + tight_loop_contents(); +} +%} + +.program hub75_data_rgb888 +.side_set 1 + +; Each FIFO record consists of a RGB888 pixel. (This is ok for e.g. an RGB565 +; source which has been gamma-corrected) +; +; Even pixels are sent on R0, G0, B0 and odd pixels on R1, G1, B1 (typically +; these are for different parts of the screen, NOT for adjacent pixels, so the +; frame buffer must be interleaved before passing to PIO.) +; +; Each pass through, we take bit n, n + 8 and n + 16 from each pixel, for n in +; {0...7}. Therefore the pixels need to be transmitted 8 times (ouch) to build +; up the full 8 bit value for each channel, and perform bit-planed PWM by +; varying pulse widths on the other state machine, in ascending powers of 2. +; This avoids a lot of bit shuffling on the processors, at the cost of DMA +; bandwidth (which we have loads of). + +; Might want to close your eyes before you read this +public entry_point: +.wrap_target +public shift0: + pull side 0 ; gets patched to `out null, n` if n nonzero (otherwise the PULL is required for fencing) + in osr, 1 side 0 ; shuffle shuffle shuffle + out null, 8 side 0 + in osr, 1 side 0 + out null, 8 side 0 + in osr, 1 side 0 + out null, 32 side 0 ; Discard remainder of OSR contents +public shift1: + pull side 0 ; gets patched to out null, n if n is nonzero (otherwise PULL required) + in osr, 1 side 1 ; Note this posedge clocks in the data from the previous iteration + out null, 8 side 1 + in osr, 1 side 1 + out null, 8 side 1 + in osr, 1 side 1 + out null, 32 side 1 + in null, 26 side 1 ; Note we are just doing this little manoeuvre here to get GPIOs in the order + mov pins, ::isr side 1 ; R0, G0, B0, R1, G1, B1. Can go 1 cycle faster if reversed +.wrap +; Note that because the clock edge for pixel n is in the middle of pixel n + +; 1, a dummy pixel at the end is required to clock the last piece of genuine +; data. (Also 1 pixel of garbage is clocked out at the start, but this is +; harmless) + +% c-sdk { +static inline void hub75_data_rgb888_program_init(PIO pio, uint sm, uint offset, uint rgb_base_pin, uint clock_pin) { + pio_sm_set_consecutive_pindirs(pio, sm, rgb_base_pin, 6, true); + pio_sm_set_consecutive_pindirs(pio, sm, clock_pin, 1, true); + for (uint i = rgb_base_pin; i < rgb_base_pin + 6; ++i) + pio_gpio_init(pio, i); + pio_gpio_init(pio, clock_pin); + + pio_sm_config c = hub75_data_rgb888_program_get_default_config(offset); + sm_config_set_out_pins(&c, rgb_base_pin, 6); + sm_config_set_sideset_pins(&c, clock_pin); + sm_config_set_out_shift(&c, true, true, 24); + // ISR shift to left. R0 ends up at bit 5. We push it up to MSB and then flip the register. + sm_config_set_in_shift(&c, false, false, 32); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + pio_sm_init(pio, sm, offset, &c); + pio_sm_exec(pio, sm, offset + hub75_data_rgb888_offset_entry_point); + pio_sm_set_enabled(pio, sm, true); +} + +// Patch a data program at `offset` to preshift pixels by `shamt` +static inline void hub75_data_rgb888_set_shift(PIO pio, uint sm, uint offset, uint shamt) { + uint16_t instr; + if (shamt == 0) + instr = pio_encode_pull(false, true); // blocking PULL + else + instr = pio_encode_out(pio_null, shamt); + pio->instr_mem[offset + hub75_data_rgb888_offset_shift0] = instr; + pio->instr_mem[offset + hub75_data_rgb888_offset_shift1] = instr; +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/hub75.pio.h b/src/hal/pio/assembler/comparison_tests/hub75.pio.h new file mode 100644 index 0000000..4d6df69 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/hub75.pio.h @@ -0,0 +1,138 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// --------- // +// hub75_row // +// --------- // + +#define hub75_row_wrap_target 0 +#define hub75_row_wrap 2 + +static const uint16_t hub75_row_program_instructions[] = { + // .wrap_target + 0x7705, // 0: out pins, 5 side 2 [7] + 0x7f3b, // 1: out x, 27 side 3 [7] + 0x0042, // 2: jmp x--, 2 side 0 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program hub75_row_program = { + .instructions = hub75_row_program_instructions, + .length = 3, + .origin = -1, +}; + +static inline pio_sm_config hub75_row_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + hub75_row_wrap_target, offset + hub75_row_wrap); + sm_config_set_sideset(&c, 2, false, false); + return c; +} + +static inline void hub75_row_program_init(PIO pio, uint sm, uint offset, uint row_base_pin, uint n_row_pins, uint latch_base_pin) { + pio_sm_set_consecutive_pindirs(pio, sm, row_base_pin, n_row_pins, true); + pio_sm_set_consecutive_pindirs(pio, sm, latch_base_pin, 2, true); + for (uint i = row_base_pin; i < row_base_pin + n_row_pins; ++i) + pio_gpio_init(pio, i); + pio_gpio_init(pio, latch_base_pin); + pio_gpio_init(pio, latch_base_pin + 1); + pio_sm_config c = hub75_row_program_get_default_config(offset); + sm_config_set_out_pins(&c, row_base_pin, n_row_pins); + sm_config_set_sideset_pins(&c, latch_base_pin); + sm_config_set_out_shift(&c, true, true, 32); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +static inline void hub75_wait_tx_stall(PIO pio, uint sm) { + uint32_t txstall_mask = 1u << (PIO_FDEBUG_TXSTALL_LSB + sm); + pio->fdebug = txstall_mask; + while (!(pio->fdebug & txstall_mask)) + tight_loop_contents(); +} + +#endif + +// ----------------- // +// hub75_data_rgb888 // +// ----------------- // + +#define hub75_data_rgb888_wrap_target 0 +#define hub75_data_rgb888_wrap 15 + +#define hub75_data_rgb888_offset_entry_point 0u +#define hub75_data_rgb888_offset_shift0 0u +#define hub75_data_rgb888_offset_shift1 7u + +static const uint16_t hub75_data_rgb888_program_instructions[] = { + // .wrap_target + 0x80a0, // 0: pull block side 0 + 0x40e1, // 1: in osr, 1 side 0 + 0x6068, // 2: out null, 8 side 0 + 0x40e1, // 3: in osr, 1 side 0 + 0x6068, // 4: out null, 8 side 0 + 0x40e1, // 5: in osr, 1 side 0 + 0x6060, // 6: out null, 32 side 0 + 0x80a0, // 7: pull block side 0 + 0x50e1, // 8: in osr, 1 side 1 + 0x7068, // 9: out null, 8 side 1 + 0x50e1, // 10: in osr, 1 side 1 + 0x7068, // 11: out null, 8 side 1 + 0x50e1, // 12: in osr, 1 side 1 + 0x7060, // 13: out null, 32 side 1 + 0x507a, // 14: in null, 26 side 1 + 0xb016, // 15: mov pins, ::isr side 1 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program hub75_data_rgb888_program = { + .instructions = hub75_data_rgb888_program_instructions, + .length = 16, + .origin = -1, +}; + +static inline pio_sm_config hub75_data_rgb888_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + hub75_data_rgb888_wrap_target, offset + hub75_data_rgb888_wrap); + sm_config_set_sideset(&c, 1, false, false); + return c; +} + +static inline void hub75_data_rgb888_program_init(PIO pio, uint sm, uint offset, uint rgb_base_pin, uint clock_pin) { + pio_sm_set_consecutive_pindirs(pio, sm, rgb_base_pin, 6, true); + pio_sm_set_consecutive_pindirs(pio, sm, clock_pin, 1, true); + for (uint i = rgb_base_pin; i < rgb_base_pin + 6; ++i) + pio_gpio_init(pio, i); + pio_gpio_init(pio, clock_pin); + pio_sm_config c = hub75_data_rgb888_program_get_default_config(offset); + sm_config_set_out_pins(&c, rgb_base_pin, 6); + sm_config_set_sideset_pins(&c, clock_pin); + sm_config_set_out_shift(&c, true, true, 24); + // ISR shift to left. R0 ends up at bit 5. We push it up to MSB and then flip the register. + sm_config_set_in_shift(&c, false, false, 32); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + pio_sm_init(pio, sm, offset, &c); + pio_sm_exec(pio, sm, offset + hub75_data_rgb888_offset_entry_point); + pio_sm_set_enabled(pio, sm, true); +} +// Patch a data program at `offset` to preshift pixels by `shamt` +static inline void hub75_data_rgb888_set_shift(PIO pio, uint sm, uint offset, uint shamt) { + uint16_t instr; + if (shamt == 0) + instr = pio_encode_pull(false, true); // blocking PULL + else + instr = pio_encode_out(pio_null, shamt); + pio->instr_mem[offset + hub75_data_rgb888_offset_shift0] = instr; + pio->instr_mem[offset + hub75_data_rgb888_offset_shift1] = instr; +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/i2c.pio b/src/hal/pio/assembler/comparison_tests/i2c.pio new file mode 100644 index 0000000..65f3e78 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/i2c.pio @@ -0,0 +1,145 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program i2c +.side_set 1 opt pindirs + +; TX Encoding: +; | 15:10 | 9 | 8:1 | 0 | +; | Instr | Final | Data | NAK | +; +; If Instr has a value n > 0, then this FIFO word has no +; data payload, and the next n + 1 words will be executed as instructions. +; Otherwise, shift out the 8 data bits, followed by the ACK bit. +; +; The Instr mechanism allows stop/start/repstart sequences to be programmed +; by the processor, and then carried out by the state machine at defined points +; in the datastream. +; +; The "Final" field should be set for the final byte in a transfer. +; This tells the state machine to ignore a NAK: if this field is not +; set, then any NAK will cause the state machine to halt and interrupt. +; +; Autopull should be enabled, with a threshold of 16. +; Autopush should be enabled, with a threshold of 8. +; The TX FIFO should be accessed with halfword writes, to ensure +; the data is immediately available in the OSR. +; +; Pin mapping: +; - Input pin 0 is SDA, 1 is SCL (if clock stretching used) +; - Jump pin is SDA +; - Side-set pin 0 is SCL +; - Set pin 0 is SDA +; - OUT pin 0 is SDA +; - SCL must be SDA + 1 (for wait mapping) +; +; The OE outputs should be inverted in the system IO controls! +; (It's possible for the inversion to be done in this program, +; but costs 2 instructions: 1 for inversion, and one to cope +; with the side effect of the MOV on TX shift counter.) + +do_nack: + jmp y-- entry_point ; Continue if NAK was expected + irq wait 0 rel ; Otherwise stop, ask for help + +do_byte: + set x, 7 ; Loop 8 times +bitloop: + out pindirs, 1 [7] ; Serialise write data (all-ones if reading) + nop side 1 [2] ; SCL rising edge + wait 1 pin, 1 [4] ; Allow clock to be stretched + in pins, 1 [7] ; Sample read data in middle of SCL pulse + jmp x-- bitloop side 0 [7] ; SCL falling edge + + ; Handle ACK pulse + out pindirs, 1 [7] ; On reads, we provide the ACK. + nop side 1 [7] ; SCL rising edge + wait 1 pin, 1 [7] ; Allow clock to be stretched + jmp pin do_nack side 0 [2] ; Test SDA for ACK/NAK, fall through if ACK + +public entry_point: +.wrap_target + out x, 6 ; Unpack Instr count + out y, 1 ; Unpack the NAK ignore bit + jmp !x do_byte ; Instr == 0, this is a data record. + out null, 32 ; Instr > 0, remainder of this OSR is invalid +do_exec: + out exec, 16 ; Execute one instruction per FIFO word + jmp x-- do_exec ; Repeat n + 1 times +.wrap + +% c-sdk { + +#include "hardware/clocks.h" +#include "hardware/gpio.h" + + +static inline void i2c_program_init(PIO pio, uint sm, uint offset, uint pin_sda, uint pin_scl) { + assert(pin_scl == pin_sda + 1); + pio_sm_config c = i2c_program_get_default_config(offset); + + // IO mapping + sm_config_set_out_pins(&c, pin_sda, 1); + sm_config_set_set_pins(&c, pin_sda, 1); + sm_config_set_in_pins(&c, pin_sda); + sm_config_set_sideset_pins(&c, pin_scl); + sm_config_set_jmp_pin(&c, pin_sda); + + sm_config_set_out_shift(&c, false, true, 16); + sm_config_set_in_shift(&c, false, true, 8); + + float div = (float)clock_get_hz(clk_sys) / (32 * 100000); + sm_config_set_clkdiv(&c, div); + + // Try to avoid glitching the bus while connecting the IOs. Get things set + // up so that pin is driven down when PIO asserts OE low, and pulled up + // otherwise. + gpio_pull_up(pin_scl); + gpio_pull_up(pin_sda); + uint32_t both_pins = (1u << pin_sda) | (1u << pin_scl); + pio_sm_set_pins_with_mask(pio, sm, both_pins, both_pins); + pio_sm_set_pindirs_with_mask(pio, sm, both_pins, both_pins); + pio_gpio_init(pio, pin_sda); + gpio_set_oeover(pin_sda, GPIO_OVERRIDE_INVERT); + pio_gpio_init(pio, pin_scl); + gpio_set_oeover(pin_scl, GPIO_OVERRIDE_INVERT); + pio_sm_set_pins_with_mask(pio, sm, 0, both_pins); + + // Clear IRQ flag before starting, and make sure flag doesn't actually + // assert a system-level interrupt (we're using it as a status flag) + pio_set_irq0_source_enabled(pio, pis_interrupt0 + sm, false); + pio_set_irq1_source_enabled(pio, pis_interrupt0 + sm, false); + pio_interrupt_clear(pio, sm); + + // Configure and start SM + pio_sm_init(pio, sm, offset + i2c_offset_entry_point, &c); + pio_sm_set_enabled(pio, sm, true); +} + +%} + + +.program set_scl_sda +.side_set 1 opt + +; Assemble a table of instructions which software can select from, and pass +; into the FIFO, to issue START/STOP/RSTART. This isn't intended to be run as +; a complete program. + + set pindirs, 0 side 0 [7] ; SCL = 0, SDA = 0 + set pindirs, 1 side 0 [7] ; SCL = 0, SDA = 1 + set pindirs, 0 side 1 [7] ; SCL = 1, SDA = 0 + set pindirs, 1 side 1 [7] ; SCL = 1, SDA = 1 + +% c-sdk { +// Define order of our instruction table +enum { + I2C_SC0_SD0 = 0, + I2C_SC0_SD1, + I2C_SC1_SD0, + I2C_SC1_SD1 +}; +%} diff --git a/src/hal/pio/assembler/comparison_tests/i2c.pio.h b/src/hal/pio/assembler/comparison_tests/i2c.pio.h new file mode 100644 index 0000000..5c47039 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/i2c.pio.h @@ -0,0 +1,136 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// --- // +// i2c // +// --- // + +#define i2c_wrap_target 12 +#define i2c_wrap 17 + +#define i2c_offset_entry_point 12u + +static const uint16_t i2c_program_instructions[] = { + 0x008c, // 0: jmp y--, 12 + 0xc030, // 1: irq wait 0 rel + 0xe027, // 2: set x, 7 + 0x6781, // 3: out pindirs, 1 [7] + 0xba42, // 4: nop side 1 [2] + 0x24a1, // 5: wait 1 pin, 1 [4] + 0x4701, // 6: in pins, 1 [7] + 0x1743, // 7: jmp x--, 3 side 0 [7] + 0x6781, // 8: out pindirs, 1 [7] + 0xbf42, // 9: nop side 1 [7] + 0x27a1, // 10: wait 1 pin, 1 [7] + 0x12c0, // 11: jmp pin, 0 side 0 [2] + // .wrap_target + 0x6026, // 12: out x, 6 + 0x6041, // 13: out y, 1 + 0x0022, // 14: jmp !x, 2 + 0x6060, // 15: out null, 32 + 0x60f0, // 16: out exec, 16 + 0x0050, // 17: jmp x--, 16 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program i2c_program = { + .instructions = i2c_program_instructions, + .length = 18, + .origin = -1, +}; + +static inline pio_sm_config i2c_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + i2c_wrap_target, offset + i2c_wrap); + sm_config_set_sideset(&c, 2, true, true); + return c; +} + +#include "hardware/clocks.h" +#include "hardware/gpio.h" +static inline void i2c_program_init(PIO pio, uint sm, uint offset, uint pin_sda, uint pin_scl) { + assert(pin_scl == pin_sda + 1); + pio_sm_config c = i2c_program_get_default_config(offset); + // IO mapping + sm_config_set_out_pins(&c, pin_sda, 1); + sm_config_set_set_pins(&c, pin_sda, 1); + sm_config_set_in_pins(&c, pin_sda); + sm_config_set_sideset_pins(&c, pin_scl); + sm_config_set_jmp_pin(&c, pin_sda); + sm_config_set_out_shift(&c, false, true, 16); + sm_config_set_in_shift(&c, false, true, 8); + float div = (float)clock_get_hz(clk_sys) / (32 * 100000); + sm_config_set_clkdiv(&c, div); + // Try to avoid glitching the bus while connecting the IOs. Get things set + // up so that pin is driven down when PIO asserts OE low, and pulled up + // otherwise. + gpio_pull_up(pin_scl); + gpio_pull_up(pin_sda); + uint32_t both_pins = (1u << pin_sda) | (1u << pin_scl); + pio_sm_set_pins_with_mask(pio, sm, both_pins, both_pins); + pio_sm_set_pindirs_with_mask(pio, sm, both_pins, both_pins); + pio_gpio_init(pio, pin_sda); + gpio_set_oeover(pin_sda, GPIO_OVERRIDE_INVERT); + pio_gpio_init(pio, pin_scl); + gpio_set_oeover(pin_scl, GPIO_OVERRIDE_INVERT); + pio_sm_set_pins_with_mask(pio, sm, 0, both_pins); + // Clear IRQ flag before starting, and make sure flag doesn't actually + // assert a system-level interrupt (we're using it as a status flag) + pio_set_irq0_source_enabled(pio, pis_interrupt0 + sm, false); + pio_set_irq1_source_enabled(pio, pis_interrupt0 + sm, false); + pio_interrupt_clear(pio, sm); + // Configure and start SM + pio_sm_init(pio, sm, offset + i2c_offset_entry_point, &c); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + +// ----------- // +// set_scl_sda // +// ----------- // + +#define set_scl_sda_wrap_target 0 +#define set_scl_sda_wrap 3 + +static const uint16_t set_scl_sda_program_instructions[] = { + // .wrap_target + 0xf780, // 0: set pindirs, 0 side 0 [7] + 0xf781, // 1: set pindirs, 1 side 0 [7] + 0xff80, // 2: set pindirs, 0 side 1 [7] + 0xff81, // 3: set pindirs, 1 side 1 [7] + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program set_scl_sda_program = { + .instructions = set_scl_sda_program_instructions, + .length = 4, + .origin = -1, +}; + +static inline pio_sm_config set_scl_sda_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + set_scl_sda_wrap_target, offset + set_scl_sda_wrap); + sm_config_set_sideset(&c, 2, true, false); + return c; +} + +// Define order of our instruction table +enum { + I2C_SC0_SD0 = 0, + I2C_SC0_SD1, + I2C_SC1_SD0, + I2C_SC1_SD1 +}; + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/manchester_encoding.pio b/src/hal/pio/assembler/comparison_tests/manchester_encoding.pio new file mode 100644 index 0000000..0117d2a --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/manchester_encoding.pio @@ -0,0 +1,94 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program manchester_tx +.side_set 1 opt + +; Transmit one bit every 12 cycles. a '0' is encoded as a high-low sequence +; (each part lasting half a bit period, or 6 cycles) and a '1' is encoded as a +; low-high sequence. +; +; Side-set bit 0 must be mapped to the GPIO used for TX. +; Autopull must be enabled -- this program does not care about the threshold. +; The program starts at the public label 'start'. + +.wrap_target +do_1: + nop side 0 [5] ; Low for 6 cycles (5 delay, +1 for nop) + jmp get_bit side 1 [3] ; High for 4 cycles. 'get_bit' takes another 2 cycles +do_0: + nop side 1 [5] ; Output high for 6 cycles + nop side 0 [3] ; Output low for 4 cycles +public start: +get_bit: + out x, 1 ; Always shift out one bit from OSR to X, so we can + jmp !x do_0 ; branch on it. Autopull refills the OSR when empty. +.wrap + +% c-sdk { +static inline void manchester_tx_program_init(PIO pio, uint sm, uint offset, uint pin, float div) { + pio_sm_set_pins_with_mask(pio, sm, 0, 1u << pin); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + pio_gpio_init(pio, pin); + + pio_sm_config c = manchester_tx_program_get_default_config(offset); + sm_config_set_sideset_pins(&c, pin); + sm_config_set_out_shift(&c, true, true, 32); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset + manchester_tx_offset_start, &c); + + pio_sm_set_enabled(pio, sm, true); +} +%} + +.program manchester_rx + +; Assumes line is idle low, first bit is 0 +; One bit is 12 cycles +; a '0' is encoded as 10 +; a '1' is encoded as 01 +; +; Both the IN base and the JMP pin mapping must be pointed at the GPIO used for RX. +; Autopush must be enabled. +; Before enabling the SM, it should be placed in a 'wait 1, pin` state, so that +; it will not start sampling until the initial line idle state ends. + +start_of_0: ; We are 0.25 bits into a 0 - signal is high + wait 0 pin 0 ; Wait for the 1->0 transition - at this point we are 0.5 into the bit + in y, 1 [8] ; Emit a 0, sleep 3/4 of a bit + jmp pin start_of_0 ; If signal is 1 again, it's another 0 bit, otherwise it's a 1 + +.wrap_target +start_of_1: ; We are 0.25 bits into a 1 - signal is 1 + wait 1 pin 0 ; Wait for the 0->1 transition - at this point we are 0.5 into the bit + in x, 1 [8] ; Emit a 1, sleep 3/4 of a bit + jmp pin start_of_0 ; If signal is 0 again, it's another 1 bit otherwise it's a 0 +.wrap + +% c-sdk { +static inline void manchester_rx_program_init(PIO pio, uint sm, uint offset, uint pin, float div) { + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false); + pio_gpio_init(pio, pin); + + pio_sm_config c = manchester_rx_program_get_default_config(offset); + sm_config_set_in_pins(&c, pin); // for WAIT + sm_config_set_jmp_pin(&c, pin); // for JMP + sm_config_set_in_shift(&c, true, true, 32); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + + // X and Y are set to 0 and 1, to conveniently emit these to ISR/FIFO. + pio_sm_exec(pio, sm, pio_encode_set(pio_x, 1)); + pio_sm_exec(pio, sm, pio_encode_set(pio_y, 0)); + // Assume line is idle low, and first transmitted bit is 0. Put SM in a + // wait state before enabling. RX will begin once the first 0 symbol is + // detected. + pio_sm_exec(pio, sm, pio_encode_wait_pin(1, 0) | pio_encode_delay(2)); + pio_sm_set_enabled(pio, sm, true); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/manchester_encoding.pio.h b/src/hal/pio/assembler/comparison_tests/manchester_encoding.pio.h new file mode 100644 index 0000000..ee00886 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/manchester_encoding.pio.h @@ -0,0 +1,112 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ------------- // +// manchester_tx // +// ------------- // + +#define manchester_tx_wrap_target 0 +#define manchester_tx_wrap 5 + +#define manchester_tx_offset_start 4u + +static const uint16_t manchester_tx_program_instructions[] = { + // .wrap_target + 0xb542, // 0: nop side 0 [5] + 0x1b04, // 1: jmp 4 side 1 [3] + 0xbd42, // 2: nop side 1 [5] + 0xb342, // 3: nop side 0 [3] + 0x6021, // 4: out x, 1 + 0x0022, // 5: jmp !x, 2 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program manchester_tx_program = { + .instructions = manchester_tx_program_instructions, + .length = 6, + .origin = -1, +}; + +static inline pio_sm_config manchester_tx_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + manchester_tx_wrap_target, offset + manchester_tx_wrap); + sm_config_set_sideset(&c, 2, true, false); + return c; +} + +static inline void manchester_tx_program_init(PIO pio, uint sm, uint offset, uint pin, float div) { + pio_sm_set_pins_with_mask(pio, sm, 0, 1u << pin); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + pio_gpio_init(pio, pin); + pio_sm_config c = manchester_tx_program_get_default_config(offset); + sm_config_set_sideset_pins(&c, pin); + sm_config_set_out_shift(&c, true, true, 32); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset + manchester_tx_offset_start, &c); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + +// ------------- // +// manchester_rx // +// ------------- // + +#define manchester_rx_wrap_target 3 +#define manchester_rx_wrap 5 + +static const uint16_t manchester_rx_program_instructions[] = { + 0x2020, // 0: wait 0 pin, 0 + 0x4841, // 1: in y, 1 [8] + 0x00c0, // 2: jmp pin, 0 + // .wrap_target + 0x20a0, // 3: wait 1 pin, 0 + 0x4821, // 4: in x, 1 [8] + 0x00c0, // 5: jmp pin, 0 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program manchester_rx_program = { + .instructions = manchester_rx_program_instructions, + .length = 6, + .origin = -1, +}; + +static inline pio_sm_config manchester_rx_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + manchester_rx_wrap_target, offset + manchester_rx_wrap); + return c; +} + +static inline void manchester_rx_program_init(PIO pio, uint sm, uint offset, uint pin, float div) { + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false); + pio_gpio_init(pio, pin); + pio_sm_config c = manchester_rx_program_get_default_config(offset); + sm_config_set_in_pins(&c, pin); // for WAIT + sm_config_set_jmp_pin(&c, pin); // for JMP + sm_config_set_in_shift(&c, true, true, 32); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + // X and Y are set to 0 and 1, to conveniently emit these to ISR/FIFO. + pio_sm_exec(pio, sm, pio_encode_set(pio_x, 1)); + pio_sm_exec(pio, sm, pio_encode_set(pio_y, 0)); + // Assume line is idle low, and first transmitted bit is 0. Put SM in a + // wait state before enabling. RX will begin once the first 0 symbol is + // detected. + pio_sm_exec(pio, sm, pio_encode_wait_pin(1, 0) | pio_encode_delay(2)); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/nec_carrier_burst.pio b/src/hal/pio/assembler/comparison_tests/nec_carrier_burst.pio new file mode 100644 index 0000000..499e892 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/nec_carrier_burst.pio @@ -0,0 +1,61 @@ +; +; Copyright (c) 2021 mjcross +; +; SPDX-License-Identifier: BSD-3-Clause +; + + +.program nec_carrier_burst + +; Generate bursts of carrier. +; +; Repeatedly wait for an IRQ to be set then clear it and generate 21 cycles of +; carrier with 25% duty cycle +; +.define NUM_CYCLES 21 ; how many carrier cycles to generate +.define BURST_IRQ 7 ; which IRQ should trigger a carrier burst +.define public TICKS_PER_LOOP 4 ; the number of instructions in the loop (for timing) + +.wrap_target + set X, (NUM_CYCLES - 1) ; initialise the loop counter + wait 1 irq BURST_IRQ ; wait for the IRQ then clear it +cycle_loop: + set pins, 1 ; set the pin high (1 cycle) + set pins, 0 [1] ; set the pin low (2 cycles) + jmp X--, cycle_loop ; (1 more cycle) +.wrap + + +% c-sdk { +static inline void nec_carrier_burst_program_init(PIO pio, uint sm, uint offset, uint pin, float freq) { + // Create a new state machine configuration + // + pio_sm_config c = nec_carrier_burst_program_get_default_config (offset); + + // Map the SET pin group to one pin, namely the `pin` + // parameter to this function. + // + sm_config_set_set_pins (&c, pin, 1); + + // Set the GPIO function of the pin (connect the PIO to the pad) + // + pio_gpio_init (pio, pin); + + // Set the pin direction to output at the PIO + // + pio_sm_set_consecutive_pindirs (pio, sm, pin, 1, true); + + // Set the clock divider to generate the required frequency + // + float div = clock_get_hz (clk_sys) / (freq * nec_carrier_burst_TICKS_PER_LOOP); + sm_config_set_clkdiv (&c, div); + + // Apply the configuration to the state machine + // + pio_sm_init (pio, sm, offset, &c); + + // Set the state machine running + // + pio_sm_set_enabled (pio, sm, true); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/nec_carrier_burst.pio.h b/src/hal/pio/assembler/comparison_tests/nec_carrier_burst.pio.h new file mode 100644 index 0000000..5fa0014 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/nec_carrier_burst.pio.h @@ -0,0 +1,70 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ----------------- // +// nec_carrier_burst // +// ----------------- // + +#define nec_carrier_burst_wrap_target 0 +#define nec_carrier_burst_wrap 4 + +#define nec_carrier_burst_TICKS_PER_LOOP 4 + +static const uint16_t nec_carrier_burst_program_instructions[] = { + // .wrap_target + 0xe034, // 0: set x, 20 + 0x20c7, // 1: wait 1 irq, 7 + 0xe001, // 2: set pins, 1 + 0xe100, // 3: set pins, 0 [1] + 0x0042, // 4: jmp x--, 2 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program nec_carrier_burst_program = { + .instructions = nec_carrier_burst_program_instructions, + .length = 5, + .origin = -1, +}; + +static inline pio_sm_config nec_carrier_burst_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + nec_carrier_burst_wrap_target, offset + nec_carrier_burst_wrap); + return c; +} + +static inline void nec_carrier_burst_program_init(PIO pio, uint sm, uint offset, uint pin, float freq) { + // Create a new state machine configuration + // + pio_sm_config c = nec_carrier_burst_program_get_default_config (offset); + // Map the SET pin group to one pin, namely the `pin` + // parameter to this function. + // + sm_config_set_set_pins (&c, pin, 1); + // Set the GPIO function of the pin (connect the PIO to the pad) + // + pio_gpio_init (pio, pin); + // Set the pin direction to output at the PIO + // + pio_sm_set_consecutive_pindirs (pio, sm, pin, 1, true); + // Set the clock divider to generate the required frequency + // + float div = clock_get_hz (clk_sys) / (freq * nec_carrier_burst_TICKS_PER_LOOP); + sm_config_set_clkdiv (&c, div); + // Apply the configuration to the state machine + // + pio_sm_init (pio, sm, offset, &c); + // Set the state machine running + // + pio_sm_set_enabled (pio, sm, true); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/nec_carrier_control.pio b/src/hal/pio/assembler/comparison_tests/nec_carrier_control.pio new file mode 100644 index 0000000..0733afe --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/nec_carrier_control.pio @@ -0,0 +1,79 @@ +; +; Copyright (c) 2021 mjcross +; +; SPDX-License-Identifier: BSD-3-Clause +; + + +.program nec_carrier_control + +; Transmit an encoded 32-bit frame in NEC IR format. +; +; Accepts 32-bit words from the transmit FIFO and sends them least-significant bit first +; using pulse position modulation. +; +; Carrier bursts are generated using the nec_carrier_burst program, which is expected to be +; running on a separate state machine. +; +; This program expects there to be 2 state machine ticks per 'normal' 562.5us +; burst period. +; +.define BURST_IRQ 7 ; the IRQ used to trigger a carrier burst +.define NUM_INITIAL_BURSTS 16 ; how many bursts to transmit for a 'sync burst' + +.wrap_target + pull ; fetch a data word from the transmit FIFO into the + ; output shift register, blocking if the FIFO is empty + + set X, (NUM_INITIAL_BURSTS - 1) ; send a sync burst (9ms) +long_burst: + irq BURST_IRQ + jmp X-- long_burst + + nop [15] ; send a 4.5ms space + irq BURST_IRQ [1] ; send a 562.5us burst to begin the first data bit + +data_bit: + out X, 1 ; shift the least-significant bit from the OSR + jmp !X burst ; send a short delay for a '0' bit + nop [3] ; send an additional delay for a '1' bit +burst: + irq BURST_IRQ ; send a 562.5us burst to end the data bit + +jmp !OSRE data_bit ; continue sending bits until the OSR is empty + +.wrap ; fetch another data word from the FIFO + + +% c-sdk { +static inline void nec_carrier_control_program_init (PIO pio, uint sm, uint offset, float tick_rate, int bits_per_frame) { + + // create a new state machine configuration + // + pio_sm_config c = nec_carrier_control_program_get_default_config(offset); + + // configure the output shift register + // + sm_config_set_out_shift (&c, + true, // shift right + false, // disable autopull + bits_per_frame); + + // join the FIFOs to make a single large transmit FIFO + // + sm_config_set_fifo_join (&c, PIO_FIFO_JOIN_TX); + + // configure the clock divider + // + float div = clock_get_hz (clk_sys) / tick_rate; + sm_config_set_clkdiv (&c, div); + + // apply the configuration to the state machine + // + pio_sm_init(pio, sm, offset, &c); + + // set the state machine running + // + pio_sm_set_enabled(pio, sm, true); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/nec_carrier_control.pio.h b/src/hal/pio/assembler/comparison_tests/nec_carrier_control.pio.h new file mode 100644 index 0000000..8c9a305 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/nec_carrier_control.pio.h @@ -0,0 +1,73 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ------------------- // +// nec_carrier_control // +// ------------------- // + +#define nec_carrier_control_wrap_target 0 +#define nec_carrier_control_wrap 10 + +static const uint16_t nec_carrier_control_program_instructions[] = { + // .wrap_target + 0x80a0, // 0: pull block + 0xe02f, // 1: set x, 15 + 0xc007, // 2: irq nowait 7 + 0x0042, // 3: jmp x--, 2 + 0xaf42, // 4: nop [15] + 0xc107, // 5: irq nowait 7 [1] + 0x6021, // 6: out x, 1 + 0x0029, // 7: jmp !x, 9 + 0xa342, // 8: nop [3] + 0xc007, // 9: irq nowait 7 + 0x00e6, // 10: jmp !osre, 6 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program nec_carrier_control_program = { + .instructions = nec_carrier_control_program_instructions, + .length = 11, + .origin = -1, +}; + +static inline pio_sm_config nec_carrier_control_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + nec_carrier_control_wrap_target, offset + nec_carrier_control_wrap); + return c; +} + +static inline void nec_carrier_control_program_init (PIO pio, uint sm, uint offset, float tick_rate, int bits_per_frame) { + // create a new state machine configuration + // + pio_sm_config c = nec_carrier_control_program_get_default_config(offset); + // configure the output shift register + // + sm_config_set_out_shift (&c, + true, // shift right + false, // disable autopull + bits_per_frame); + // join the FIFOs to make a single large transmit FIFO + // + sm_config_set_fifo_join (&c, PIO_FIFO_JOIN_TX); + // configure the clock divider + // + float div = clock_get_hz (clk_sys) / tick_rate; + sm_config_set_clkdiv (&c, div); + // apply the configuration to the state machine + // + pio_sm_init(pio, sm, offset, &c); + // set the state machine running + // + pio_sm_set_enabled(pio, sm, true); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/nec_receive.pio b/src/hal/pio/assembler/comparison_tests/nec_receive.pio new file mode 100644 index 0000000..a2c5f5e --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/nec_receive.pio @@ -0,0 +1,96 @@ +; +; Copyright (c) 2021 mjcross +; +; SPDX-License-Identifier: BSD-3-Clause +; + + +.program nec_receive + +; Decode IR frames in NEC format and push 32-bit words to the input FIFO. +; +; The input pin should be connected to an IR detector with an 'active low' output. +; +; This program expects there to be 10 state machine clock ticks per 'normal' 562.5us burst period +; in order to permit timely detection of start of a burst. The initailisation function below sets +; the correct divisor to achive this relative to the system clock. +; +; Within the 'NEC' protocol frames consists of 32 bits sent least-siginificant bit first; so the +; Input Shift Register should be configured to shift right and autopush after 32 bits, as in the +; initialisation function below. +; +.define BURST_LOOP_COUNTER 30 ; the detection threshold for a 'frame sync' burst +.define BIT_SAMPLE_DELAY 15 ; how long to wait after the end of the burst before sampling + +.wrap_target + +next_burst: + set X, BURST_LOOP_COUNTER + wait 0 pin 0 ; wait for the next burst to start + +burst_loop: + jmp pin data_bit ; the burst ended before the counter expired + jmp X-- burst_loop ; wait for the burst to end + + ; the counter expired - this is a sync burst + mov ISR, NULL ; reset the Input Shift Register + wait 1 pin 0 ; wait for the sync burst to finish + jmp next_burst ; wait for the first data bit + +data_bit: + nop [ BIT_SAMPLE_DELAY - 1 ] ; wait for 1.5 burst periods before sampling the bit value + in PINS, 1 ; if the next burst has started then detect a '0' (short gap) + ; otherwise detect a '1' (long gap) + ; after 32 bits the ISR will autopush to the receive FIFO +.wrap + + +% c-sdk { +static inline void nec_receive_program_init (PIO pio, uint sm, uint offset, uint pin) { + + // Set the GPIO function of the pin (connect the PIO to the pad) + // + pio_gpio_init(pio, pin); + + // Set the pin direction to `input` at the PIO + // + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false); + + // Create a new state machine configuration + // + pio_sm_config c = nec_receive_program_get_default_config (offset); + + // configure the Input Shift Register + // + sm_config_set_in_shift (&c, + true, // shift right + true, // enable autopush + 32); // autopush after 32 bits + + // join the FIFOs to make a single large receive FIFO + // + sm_config_set_fifo_join (&c, PIO_FIFO_JOIN_RX); + + // Map the IN pin group to one pin, namely the `pin` + // parameter to this function. + // + sm_config_set_in_pins (&c, pin); + + // Map the JMP pin to the `pin` parameter of this function. + // + sm_config_set_jmp_pin (&c, pin); + + // Set the clock divider to 10 ticks per 562.5us burst period + // + float div = clock_get_hz (clk_sys) / (10.0 / 562.5e-6); + sm_config_set_clkdiv (&c, div); + + // Apply the configuration to the state machine + // + pio_sm_init (pio, sm, offset, &c); + + // Set the state machine running + // + pio_sm_set_enabled (pio, sm, true); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/nec_receive.pio.h b/src/hal/pio/assembler/comparison_tests/nec_receive.pio.h new file mode 100644 index 0000000..e9611b6 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/nec_receive.pio.h @@ -0,0 +1,84 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ----------- // +// nec_receive // +// ----------- // + +#define nec_receive_wrap_target 0 +#define nec_receive_wrap 8 + +static const uint16_t nec_receive_program_instructions[] = { + // .wrap_target + 0xe03e, // 0: set x, 30 + 0x2020, // 1: wait 0 pin, 0 + 0x00c7, // 2: jmp pin, 7 + 0x0042, // 3: jmp x--, 2 + 0xa0c3, // 4: mov isr, null + 0x20a0, // 5: wait 1 pin, 0 + 0x0000, // 6: jmp 0 + 0xae42, // 7: nop [14] + 0x4001, // 8: in pins, 1 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program nec_receive_program = { + .instructions = nec_receive_program_instructions, + .length = 9, + .origin = -1, +}; + +static inline pio_sm_config nec_receive_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + nec_receive_wrap_target, offset + nec_receive_wrap); + return c; +} + +static inline void nec_receive_program_init (PIO pio, uint sm, uint offset, uint pin) { + // Set the GPIO function of the pin (connect the PIO to the pad) + // + pio_gpio_init(pio, pin); + // Set the pin direction to `input` at the PIO + // + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false); + // Create a new state machine configuration + // + pio_sm_config c = nec_receive_program_get_default_config (offset); + // configure the Input Shift Register + // + sm_config_set_in_shift (&c, + true, // shift right + true, // enable autopush + 32); // autopush after 32 bits + // join the FIFOs to make a single large receive FIFO + // + sm_config_set_fifo_join (&c, PIO_FIFO_JOIN_RX); + // Map the IN pin group to one pin, namely the `pin` + // parameter to this function. + // + sm_config_set_in_pins (&c, pin); + // Map the JMP pin to the `pin` parameter of this function. + // + sm_config_set_jmp_pin (&c, pin); + // Set the clock divider to 10 ticks per 562.5us burst period + // + float div = clock_get_hz (clk_sys) / (10.0 / 562.5e-6); + sm_config_set_clkdiv (&c, div); + // Apply the configuration to the state machine + // + pio_sm_init (pio, sm, offset, &c); + // Set the state machine running + // + pio_sm_set_enabled (pio, sm, true); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/pio_serialiser.pio b/src/hal/pio/assembler/comparison_tests/pio_serialiser.pio new file mode 100644 index 0000000..67a6866 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/pio_serialiser.pio @@ -0,0 +1,27 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program pio_serialiser + +; Just serialise a stream of bits. Take 32 bits from each FIFO record. LSB-first. + +.wrap_target + out pins, 1 +.wrap + +% c-sdk { +static inline void pio_serialiser_program_init(PIO pio, uint sm, uint offset, uint data_pin, float clk_div) { + pio_gpio_init(pio, data_pin); + pio_sm_set_consecutive_pindirs(pio, sm, data_pin, 1, true); + pio_sm_config c = pio_serialiser_program_get_default_config(offset); + sm_config_set_out_pins(&c, data_pin, 1); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + sm_config_set_clkdiv(&c, clk_div); + sm_config_set_out_shift(&c, true, true, 32); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/pio_serialiser.pio.h b/src/hal/pio/assembler/comparison_tests/pio_serialiser.pio.h new file mode 100644 index 0000000..0627585 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/pio_serialiser.pio.h @@ -0,0 +1,50 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// -------------- // +// pio_serialiser // +// -------------- // + +#define pio_serialiser_wrap_target 0 +#define pio_serialiser_wrap 0 + +static const uint16_t pio_serialiser_program_instructions[] = { + // .wrap_target + 0x6001, // 0: out pins, 1 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program pio_serialiser_program = { + .instructions = pio_serialiser_program_instructions, + .length = 1, + .origin = -1, +}; + +static inline pio_sm_config pio_serialiser_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + pio_serialiser_wrap_target, offset + pio_serialiser_wrap); + return c; +} + +static inline void pio_serialiser_program_init(PIO pio, uint sm, uint offset, uint data_pin, float clk_div) { + pio_gpio_init(pio, data_pin); + pio_sm_set_consecutive_pindirs(pio, sm, data_pin, 1, true); + pio_sm_config c = pio_serialiser_program_get_default_config(offset); + sm_config_set_out_pins(&c, data_pin, 1); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + sm_config_set_clkdiv(&c, clk_div); + sm_config_set_out_shift(&c, true, true, 32); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/pwm.pio b/src/hal/pio/assembler/comparison_tests/pwm.pio new file mode 100644 index 0000000..d0f2bcb --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/pwm.pio @@ -0,0 +1,31 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +; Side-set pin 0 is used for PWM output + +.program pwm +.side_set 1 opt + + pull noblock side 0 ; Pull from FIFO to OSR if available, else copy X to OSR. + mov x, osr ; Copy most-recently-pulled value back to scratch X + mov y, isr ; ISR contains PWM period. Y used as counter. +countloop: + jmp x!=y noset ; Set pin high if X == Y, keep the two paths length matched + jmp skip side 1 +noset: + nop ; Single dummy cycle to keep the two paths the same length +skip: + jmp y-- countloop ; Loop until Y hits 0, then pull a fresh PWM value from FIFO + +% c-sdk { +static inline void pwm_program_init(PIO pio, uint sm, uint offset, uint pin) { + pio_gpio_init(pio, pin); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + pio_sm_config c = pwm_program_get_default_config(offset); + sm_config_set_sideset_pins(&c, pin); + pio_sm_init(pio, sm, offset, &c); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/pwm.pio.h b/src/hal/pio/assembler/comparison_tests/pwm.pio.h new file mode 100644 index 0000000..7d738a0 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/pwm.pio.h @@ -0,0 +1,53 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// --- // +// pwm // +// --- // + +#define pwm_wrap_target 0 +#define pwm_wrap 6 + +static const uint16_t pwm_program_instructions[] = { + // .wrap_target + 0x9080, // 0: pull noblock side 0 + 0xa027, // 1: mov x, osr + 0xa046, // 2: mov y, isr + 0x00a5, // 3: jmp x != y, 5 + 0x1806, // 4: jmp 6 side 1 + 0xa042, // 5: nop + 0x0083, // 6: jmp y--, 3 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program pwm_program = { + .instructions = pwm_program_instructions, + .length = 7, + .origin = -1, +}; + +static inline pio_sm_config pwm_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + pwm_wrap_target, offset + pwm_wrap); + sm_config_set_sideset(&c, 2, true, false); + return c; +} + +static inline void pwm_program_init(PIO pio, uint sm, uint offset, uint pin) { + pio_gpio_init(pio, pin); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + pio_sm_config c = pwm_program_get_default_config(offset); + sm_config_set_sideset_pins(&c, pin); + pio_sm_init(pio, sm, offset, &c); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/quadrature_encoder.pio b/src/hal/pio/assembler/comparison_tests/quadrature_encoder.pio new file mode 100644 index 0000000..d245d4b --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/quadrature_encoder.pio @@ -0,0 +1,165 @@ +; +; Copyright (c) 2021 pmarques-dev @ github +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program quadrature_encoder + +; this code must be loaded into address 0, but at 29 instructions, it probably +; wouldn't be able to share space with other programs anyway +.origin 0 + + +; the code works by running a loop that continuously shifts the 2 phase pins into +; ISR and looks at the lower 4 bits to do a computed jump to an instruction that +; does the proper "do nothing" | "increment" | "decrement" action for that pin +; state change (or no change) + +; ISR holds the last state of the 2 pins during most of the code. The Y register +; keeps the current encoder count and is incremented / decremented according to +; the steps sampled + +; writing any non zero value to the TX FIFO makes the state machine push the +; current count to RX FIFO between 6 to 18 clocks afterwards. The worst case +; sampling loop takes 14 cycles, so this program is able to read step rates up +; to sysclk / 14 (e.g., sysclk 125MHz, max step rate = 8.9 Msteps/sec) + + +; 00 state + JMP update ; read 00 + JMP decrement ; read 01 + JMP increment ; read 10 + JMP update ; read 11 + +; 01 state + JMP increment ; read 00 + JMP update ; read 01 + JMP update ; read 10 + JMP decrement ; read 11 + +; 10 state + JMP decrement ; read 00 + JMP update ; read 01 + JMP update ; read 10 + JMP increment ; read 11 + +; to reduce code size, the last 2 states are implemented in place and become the +; target for the other jumps + +; 11 state + JMP update ; read 00 + JMP increment ; read 01 +decrement: + ; note: the target of this instruction must be the next address, so that + ; the effect of the instruction does not depend on the value of Y. The + ; same is true for the "JMP X--" below. Basically "JMP Y--, " + ; is just a pure "decrement Y" instruction, with no other side effects + JMP Y--, update ; read 10 + + ; this is where the main loop starts +.wrap_target +update: + ; we start by checking the TX FIFO to see if the main code is asking for + ; the current count after the PULL noblock, OSR will have either 0 if + ; there was nothing or the value that was there + SET X, 0 + PULL noblock + + ; since there are not many free registers, and PULL is done into OSR, we + ; have to do some juggling to avoid losing the state information and + ; still place the values where we need them + MOV X, OSR + MOV OSR, ISR + + ; the main code did not ask for the count, so just go to "sample_pins" + JMP !X, sample_pins + + ; if it did ask for the count, then we push it + MOV ISR, Y ; we trash ISR, but we already have a copy in OSR + PUSH + +sample_pins: + ; we shift into ISR the last state of the 2 input pins (now in OSR) and + ; the new state of the 2 pins, thus producing the 4 bit target for the + ; computed jump into the correct action for this state + MOV ISR, NULL + IN OSR, 2 + IN PINS, 2 + MOV PC, ISR + + ; the PIO does not have a increment instruction, so to do that we do a + ; negate, decrement, negate sequence +increment: + MOV X, !Y + JMP X--, increment_cont +increment_cont: + MOV Y, !X +.wrap ; the .wrap here avoids one jump instruction and saves a cycle too + + + +% c-sdk { + +#include "hardware/clocks.h" +#include "hardware/gpio.h" + +// max_step_rate is used to lower the clock of the state machine to save power +// if the application doesn't require a very high sampling rate. Passing zero +// will set the clock to the maximum, which gives a max step rate of around +// 8.9 Msteps/sec at 125MHz + +static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate) +{ + pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false); + gpio_pull_up(pin); + gpio_pull_up(pin + 1); + + pio_sm_config c = quadrature_encoder_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 left, autopull disabled + sm_config_set_in_shift(&c, false, false, 32); + // don't join FIFO's + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE); + + // passing "0" as the sample frequency, + if (max_step_rate == 0) { + sm_config_set_clkdiv(&c, 1.0); + } else { + // one state machine loop takes at most 14 cycles + float div = (float)clock_get_hz(clk_sys) / (14 * max_step_rate); + sm_config_set_clkdiv(&c, div); + } + + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + + +// When requesting the current count we may have to wait a few cycles (average +// ~11 sysclk cycles) for the state machine to reply. If we are reading multiple +// encoders, we may request them all in one go and then fetch them all, thus +// avoiding doing the wait multiple times. If we are reading just one encoder, +// we can use the "get_count" function to request and wait + +static inline void quadrature_encoder_request_count(PIO pio, uint sm) +{ + pio->txf[sm] = 1; +} + +static inline int32_t quadrature_encoder_fetch_count(PIO pio, uint sm) +{ + while (pio_sm_is_rx_fifo_empty(pio, sm)) + tight_loop_contents(); + return pio->rxf[sm]; +} + +static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm) +{ + quadrature_encoder_request_count(pio, sm); + return quadrature_encoder_fetch_count(pio, sm); +} + +%} + diff --git a/src/hal/pio/assembler/comparison_tests/quadrature_encoder.pio.h b/src/hal/pio/assembler/comparison_tests/quadrature_encoder.pio.h new file mode 100644 index 0000000..4cc4bb3 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/quadrature_encoder.pio.h @@ -0,0 +1,116 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ------------------ // +// quadrature_encoder // +// ------------------ // + +#define quadrature_encoder_wrap_target 15 +#define quadrature_encoder_wrap 28 + +static const uint16_t quadrature_encoder_program_instructions[] = { + 0x000f, // 0: jmp 15 + 0x000e, // 1: jmp 14 + 0x001a, // 2: jmp 26 + 0x000f, // 3: jmp 15 + 0x001a, // 4: jmp 26 + 0x000f, // 5: jmp 15 + 0x000f, // 6: jmp 15 + 0x000e, // 7: jmp 14 + 0x000e, // 8: jmp 14 + 0x000f, // 9: jmp 15 + 0x000f, // 10: jmp 15 + 0x001a, // 11: jmp 26 + 0x000f, // 12: jmp 15 + 0x001a, // 13: jmp 26 + 0x008f, // 14: jmp y--, 15 + // .wrap_target + 0xe020, // 15: set x, 0 + 0x8080, // 16: pull noblock + 0xa027, // 17: mov x, osr + 0xa0e6, // 18: mov osr, isr + 0x0036, // 19: jmp !x, 22 + 0xa0c2, // 20: mov isr, y + 0x8020, // 21: push block + 0xa0c3, // 22: mov isr, null + 0x40e2, // 23: in osr, 2 + 0x4002, // 24: in pins, 2 + 0xa0a6, // 25: mov pc, isr + 0xa02a, // 26: mov x, !y + 0x005c, // 27: jmp x--, 28 + 0xa049, // 28: mov y, !x + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program quadrature_encoder_program = { + .instructions = quadrature_encoder_program_instructions, + .length = 29, + .origin = 0, +}; + +static inline pio_sm_config quadrature_encoder_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + quadrature_encoder_wrap_target, offset + quadrature_encoder_wrap); + return c; +} + +#include "hardware/clocks.h" +#include "hardware/gpio.h" +// max_step_rate is used to lower the clock of the state machine to save power +// if the application doesn't require a very high sampling rate. Passing zero +// will set the clock to the maximum, which gives a max step rate of around +// 8.9 Msteps/sec at 125MHz +static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate) +{ + pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false); + gpio_pull_up(pin); + gpio_pull_up(pin + 1); + pio_sm_config c = quadrature_encoder_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 left, autopull disabled + sm_config_set_in_shift(&c, false, false, 32); + // don't join FIFO's + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE); + // passing "0" as the sample frequency, + if (max_step_rate == 0) { + sm_config_set_clkdiv(&c, 1.0); + } else { + // one state machine loop takes at most 14 cycles + float div = (float)clock_get_hz(clk_sys) / (14 * max_step_rate); + sm_config_set_clkdiv(&c, div); + } + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +// When requesting the current count we may have to wait a few cycles (average +// ~11 sysclk cycles) for the state machine to reply. If we are reading multiple +// encoders, we may request them all in one go and then fetch them all, thus +// avoiding doing the wait multiple times. If we are reading just one encoder, +// we can use the "get_count" function to request and wait +static inline void quadrature_encoder_request_count(PIO pio, uint sm) +{ + pio->txf[sm] = 1; +} +static inline int32_t quadrature_encoder_fetch_count(PIO pio, uint sm) +{ + while (pio_sm_is_rx_fifo_empty(pio, sm)) + tight_loop_contents(); + return pio->rxf[sm]; +} +static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm) +{ + quadrature_encoder_request_count(pio, sm); + return quadrature_encoder_fetch_count(pio, sm); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/resistor_dac.pio b/src/hal/pio/assembler/comparison_tests/resistor_dac.pio new file mode 100644 index 0000000..2dca1f1 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/resistor_dac.pio @@ -0,0 +1,38 @@ +; +; Copyright (c) 2021 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program resistor_dac_5bit + +; Drive one of the 5-bit resistor DACs on the VGA reference board. (this isn't +; a good way to do VGA -- just want a nice sawtooth for the ADC example!) + + out pins, 5 + + + +% c-sdk { +#include "hardware/clocks.h" +static inline void resistor_dac_5bit_program_init(PIO pio, uint sm, uint offset, + uint sample_rate_hz, uint pin_base) { + + pio_sm_set_pins_with_mask(pio, sm, 0, 0x1fu << pin_base); + pio_sm_set_pindirs_with_mask(pio, sm, ~0u, 0x1fu << pin_base); + for (int i = 0; i < 5; ++i) + pio_gpio_init(pio, pin_base + i); + + pio_sm_config c = resistor_dac_5bit_program_get_default_config(offset); + sm_config_set_out_pins(&c, pin_base, 5); + // Shift to right, autopull threshold 5 + sm_config_set_out_shift(&c, true, true, 5); + // Deeper FIFO as we're not doing any RX + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + float div = (float)clock_get_hz(clk_sys) / sample_rate_hz; + sm_config_set_clkdiv(&c, div); + + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/resistor_dac.pio.h b/src/hal/pio/assembler/comparison_tests/resistor_dac.pio.h new file mode 100644 index 0000000..df80791 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/resistor_dac.pio.h @@ -0,0 +1,57 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ----------------- // +// resistor_dac_5bit // +// ----------------- // + +#define resistor_dac_5bit_wrap_target 0 +#define resistor_dac_5bit_wrap 0 + +static const uint16_t resistor_dac_5bit_program_instructions[] = { + // .wrap_target + 0x6005, // 0: out pins, 5 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program resistor_dac_5bit_program = { + .instructions = resistor_dac_5bit_program_instructions, + .length = 1, + .origin = -1, +}; + +static inline pio_sm_config resistor_dac_5bit_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + resistor_dac_5bit_wrap_target, offset + resistor_dac_5bit_wrap); + return c; +} + +#include "hardware/clocks.h" +static inline void resistor_dac_5bit_program_init(PIO pio, uint sm, uint offset, + uint sample_rate_hz, uint pin_base) { + pio_sm_set_pins_with_mask(pio, sm, 0, 0x1fu << pin_base); + pio_sm_set_pindirs_with_mask(pio, sm, ~0u, 0x1fu << pin_base); + for (int i = 0; i < 5; ++i) + pio_gpio_init(pio, pin_base + i); + pio_sm_config c = resistor_dac_5bit_program_get_default_config(offset); + sm_config_set_out_pins(&c, pin_base, 5); + // Shift to right, autopull threshold 5 + sm_config_set_out_shift(&c, true, true, 5); + // Deeper FIFO as we're not doing any RX + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + float div = (float)clock_get_hz(clk_sys) / sample_rate_hz; + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/spi.pio b/src/hal/pio/assembler/comparison_tests/spi.pio new file mode 100644 index 0000000..eba785e --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/spi.pio @@ -0,0 +1,168 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +; These programs implement full-duplex SPI, with a SCK period of 4 clock +; cycles. A different program is provided for each value of CPHA, and CPOL is +; achieved using the hardware GPIO inversion available in the IO controls. +; +; Transmit-only SPI can go twice as fast -- see the ST7789 example! + + +.program spi_cpha0 +.side_set 1 + +; Pin assignments: +; - SCK is side-set pin 0 +; - MOSI is OUT pin 0 +; - MISO is IN pin 0 +; +; Autopush and autopull must be enabled, and the serial frame size is set by +; configuring the push/pull threshold. Shift left/right is fine, but you must +; justify the data yourself. This is done most conveniently for frame sizes of +; 8 or 16 bits by using the narrow store replication and narrow load byte +; picking behaviour of RP2040's IO fabric. + +; Clock phase = 0: data is captured on the leading edge of each SCK pulse, and +; transitions on the trailing edge, or some time before the first leading edge. + + out pins, 1 side 0 [1] ; Stall here on empty (sideset proceeds even if + in pins, 1 side 1 [1] ; instruction stalls, so we stall with SCK low) + +.program spi_cpha1 +.side_set 1 + +; Clock phase = 1: data transitions on the leading edge of each SCK pulse, and +; is captured on the trailing edge. + + out x, 1 side 0 ; Stall here on empty (keep SCK deasserted) + mov pins, x side 1 [1] ; Output data, assert SCK (mov pins uses OUT mapping) + in pins, 1 side 0 ; Input data, deassert SCK + +% c-sdk { +#include "hardware/gpio.h" +static inline void pio_spi_init(PIO pio, uint sm, uint prog_offs, uint n_bits, + float clkdiv, bool cpha, bool cpol, uint pin_sck, uint pin_mosi, uint pin_miso) { + pio_sm_config c = cpha ? spi_cpha1_program_get_default_config(prog_offs) : spi_cpha0_program_get_default_config(prog_offs); + sm_config_set_out_pins(&c, pin_mosi, 1); + sm_config_set_in_pins(&c, pin_miso); + sm_config_set_sideset_pins(&c, pin_sck); + // Only support MSB-first in this example code (shift to left, auto push/pull, threshold=nbits) + sm_config_set_out_shift(&c, false, true, n_bits); + sm_config_set_in_shift(&c, false, true, n_bits); + sm_config_set_clkdiv(&c, clkdiv); + + // MOSI, SCK output are low, MISO is input + pio_sm_set_pins_with_mask(pio, sm, 0, (1u << pin_sck) | (1u << pin_mosi)); + pio_sm_set_pindirs_with_mask(pio, sm, (1u << pin_sck) | (1u << pin_mosi), (1u << pin_sck) | (1u << pin_mosi) | (1u << pin_miso)); + pio_gpio_init(pio, pin_mosi); + pio_gpio_init(pio, pin_miso); + pio_gpio_init(pio, pin_sck); + + // The pin muxes can be configured to invert the output (among other things + // and this is a cheesy way to get CPOL=1 + gpio_set_outover(pin_sck, cpol ? GPIO_OVERRIDE_INVERT : GPIO_OVERRIDE_NORMAL); + // SPI is synchronous, so bypass input synchroniser to reduce input delay. + hw_set_bits(&pio->input_sync_bypass, 1u << pin_miso); + + pio_sm_init(pio, sm, prog_offs, &c); + pio_sm_set_enabled(pio, sm, true); +} +%} + +; SPI with Chip Select +; ----------------------------------------------------------------------------- +; +; For your amusement, here are some SPI programs with an automatic chip select +; (asserted once data appears in TX FIFO, deasserts when FIFO bottoms out, has +; a nice front/back porch). +; +; The number of bits per FIFO entry is configured via the Y register +; and the autopush/pull threshold. From 2 to 32 bits. +; +; Pin assignments: +; - SCK is side-set bit 0 +; - CSn is side-set bit 1 +; - MOSI is OUT bit 0 (host-to-device) +; - MISO is IN bit 0 (device-to-host) +; +; This program only supports one chip select -- use GPIO if more are needed +; +; Provide a variation for each possibility of CPHA; for CPOL we can just +; invert SCK in the IO muxing controls (downstream from PIO) + + +; CPHA=0: data is captured on the leading edge of each SCK pulse (including +; the first pulse), and transitions on the trailing edge + +.program spi_cpha0_cs +.side_set 2 + +.wrap_target +bitloop: + out pins, 1 side 0x0 [1] + in pins, 1 side 0x1 + jmp x-- bitloop side 0x1 + + out pins, 1 side 0x0 + mov x, y side 0x0 ; Reload bit counter from Y + in pins, 1 side 0x1 + jmp !osre bitloop side 0x1 ; Fall-through if TXF empties + + nop side 0x0 [1] ; CSn back porch +public entry_point: ; Must set X,Y to n-2 before starting! + pull ifempty side 0x2 [1] ; Block with CSn high (minimum 2 cycles) +.wrap ; Note ifempty to avoid time-of-check race + +; CPHA=1: data transitions on the leading edge of each SCK pulse, and is +; captured on the trailing edge + +.program spi_cpha1_cs +.side_set 2 + +.wrap_target +bitloop: + out pins, 1 side 0x1 [1] + in pins, 1 side 0x0 + jmp x-- bitloop side 0x0 + + out pins, 1 side 0x1 + mov x, y side 0x1 + in pins, 1 side 0x0 + jmp !osre bitloop side 0x0 + +public entry_point: ; Must set X,Y to n-2 before starting! + pull ifempty side 0x2 [1] ; Block with CSn high (minimum 2 cycles) + nop side 0x0 [1]; CSn front porch +.wrap + +% c-sdk { +#include "hardware/gpio.h" +static inline void pio_spi_cs_init(PIO pio, uint sm, uint prog_offs, uint n_bits, float clkdiv, bool cpha, bool cpol, + uint pin_sck, uint pin_mosi, uint pin_miso) { + pio_sm_config c = cpha ? spi_cpha1_cs_program_get_default_config(prog_offs) : spi_cpha0_cs_program_get_default_config(prog_offs); + sm_config_set_out_pins(&c, pin_mosi, 1); + sm_config_set_in_pins(&c, pin_miso); + sm_config_set_sideset_pins(&c, pin_sck); + sm_config_set_out_shift(&c, false, true, n_bits); + sm_config_set_in_shift(&c, false, true, n_bits); + sm_config_set_clkdiv(&c, clkdiv); + + pio_sm_set_pins_with_mask(pio, sm, (2u << pin_sck), (3u << pin_sck) | (1u << pin_mosi)); + pio_sm_set_pindirs_with_mask(pio, sm, (3u << pin_sck) | (1u << pin_mosi), (3u << pin_sck) | (1u << pin_mosi) | (1u << pin_miso)); + pio_gpio_init(pio, pin_mosi); + pio_gpio_init(pio, pin_miso); + pio_gpio_init(pio, pin_sck); + pio_gpio_init(pio, pin_sck + 1); + gpio_set_outover(pin_sck, cpol ? GPIO_OVERRIDE_INVERT : GPIO_OVERRIDE_NORMAL); + hw_set_bits(&pio->input_sync_bypass, 1u << pin_miso); + + uint entry_point = prog_offs + (cpha ? spi_cpha1_cs_offset_entry_point : spi_cpha0_cs_offset_entry_point); + pio_sm_init(pio, sm, entry_point, &c); + pio_sm_exec(pio, sm, pio_encode_set(pio_x, n_bits - 2)); + pio_sm_exec(pio, sm, pio_encode_set(pio_y, n_bits - 2)); + pio_sm_set_enabled(pio, sm, true); +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/spi.pio.h b/src/hal/pio/assembler/comparison_tests/spi.pio.h new file mode 100644 index 0000000..22aa805 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/spi.pio.h @@ -0,0 +1,198 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// --------- // +// spi_cpha0 // +// --------- // + +#define spi_cpha0_wrap_target 0 +#define spi_cpha0_wrap 1 + +static const uint16_t spi_cpha0_program_instructions[] = { + // .wrap_target + 0x6101, // 0: out pins, 1 side 0 [1] + 0x5101, // 1: in pins, 1 side 1 [1] + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program spi_cpha0_program = { + .instructions = spi_cpha0_program_instructions, + .length = 2, + .origin = -1, +}; + +static inline pio_sm_config spi_cpha0_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + spi_cpha0_wrap_target, offset + spi_cpha0_wrap); + sm_config_set_sideset(&c, 1, false, false); + return c; +} +#endif + +// --------- // +// spi_cpha1 // +// --------- // + +#define spi_cpha1_wrap_target 0 +#define spi_cpha1_wrap 2 + +static const uint16_t spi_cpha1_program_instructions[] = { + // .wrap_target + 0x6021, // 0: out x, 1 side 0 + 0xb101, // 1: mov pins, x side 1 [1] + 0x4001, // 2: in pins, 1 side 0 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program spi_cpha1_program = { + .instructions = spi_cpha1_program_instructions, + .length = 3, + .origin = -1, +}; + +static inline pio_sm_config spi_cpha1_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + spi_cpha1_wrap_target, offset + spi_cpha1_wrap); + sm_config_set_sideset(&c, 1, false, false); + return c; +} + +#include "hardware/gpio.h" +static inline void pio_spi_init(PIO pio, uint sm, uint prog_offs, uint n_bits, + float clkdiv, bool cpha, bool cpol, uint pin_sck, uint pin_mosi, uint pin_miso) { + pio_sm_config c = cpha ? spi_cpha1_program_get_default_config(prog_offs) : spi_cpha0_program_get_default_config(prog_offs); + sm_config_set_out_pins(&c, pin_mosi, 1); + sm_config_set_in_pins(&c, pin_miso); + sm_config_set_sideset_pins(&c, pin_sck); + // Only support MSB-first in this example code (shift to left, auto push/pull, threshold=nbits) + sm_config_set_out_shift(&c, false, true, n_bits); + sm_config_set_in_shift(&c, false, true, n_bits); + sm_config_set_clkdiv(&c, clkdiv); + // MOSI, SCK output are low, MISO is input + pio_sm_set_pins_with_mask(pio, sm, 0, (1u << pin_sck) | (1u << pin_mosi)); + pio_sm_set_pindirs_with_mask(pio, sm, (1u << pin_sck) | (1u << pin_mosi), (1u << pin_sck) | (1u << pin_mosi) | (1u << pin_miso)); + pio_gpio_init(pio, pin_mosi); + pio_gpio_init(pio, pin_miso); + pio_gpio_init(pio, pin_sck); + // The pin muxes can be configured to invert the output (among other things + // and this is a cheesy way to get CPOL=1 + gpio_set_outover(pin_sck, cpol ? GPIO_OVERRIDE_INVERT : GPIO_OVERRIDE_NORMAL); + // SPI is synchronous, so bypass input synchroniser to reduce input delay. + hw_set_bits(&pio->input_sync_bypass, 1u << pin_miso); + pio_sm_init(pio, sm, prog_offs, &c); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + +// ------------ // +// spi_cpha0_cs // +// ------------ // + +#define spi_cpha0_cs_wrap_target 0 +#define spi_cpha0_cs_wrap 8 + +#define spi_cpha0_cs_offset_entry_point 8u + +static const uint16_t spi_cpha0_cs_program_instructions[] = { + // .wrap_target + 0x6101, // 0: out pins, 1 side 0 [1] + 0x4801, // 1: in pins, 1 side 1 + 0x0840, // 2: jmp x--, 0 side 1 + 0x6001, // 3: out pins, 1 side 0 + 0xa022, // 4: mov x, y side 0 + 0x4801, // 5: in pins, 1 side 1 + 0x08e0, // 6: jmp !osre, 0 side 1 + 0xa142, // 7: nop side 0 [1] + 0x91e0, // 8: pull ifempty block side 2 [1] + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program spi_cpha0_cs_program = { + .instructions = spi_cpha0_cs_program_instructions, + .length = 9, + .origin = -1, +}; + +static inline pio_sm_config spi_cpha0_cs_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + spi_cpha0_cs_wrap_target, offset + spi_cpha0_cs_wrap); + sm_config_set_sideset(&c, 2, false, false); + return c; +} +#endif + +// ------------ // +// spi_cpha1_cs // +// ------------ // + +#define spi_cpha1_cs_wrap_target 0 +#define spi_cpha1_cs_wrap 8 + +#define spi_cpha1_cs_offset_entry_point 7u + +static const uint16_t spi_cpha1_cs_program_instructions[] = { + // .wrap_target + 0x6901, // 0: out pins, 1 side 1 [1] + 0x4001, // 1: in pins, 1 side 0 + 0x0040, // 2: jmp x--, 0 side 0 + 0x6801, // 3: out pins, 1 side 1 + 0xa822, // 4: mov x, y side 1 + 0x4001, // 5: in pins, 1 side 0 + 0x00e0, // 6: jmp !osre, 0 side 0 + 0x91e0, // 7: pull ifempty block side 2 [1] + 0xa142, // 8: nop side 0 [1] + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program spi_cpha1_cs_program = { + .instructions = spi_cpha1_cs_program_instructions, + .length = 9, + .origin = -1, +}; + +static inline pio_sm_config spi_cpha1_cs_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + spi_cpha1_cs_wrap_target, offset + spi_cpha1_cs_wrap); + sm_config_set_sideset(&c, 2, false, false); + return c; +} + +#include "hardware/gpio.h" +static inline void pio_spi_cs_init(PIO pio, uint sm, uint prog_offs, uint n_bits, float clkdiv, bool cpha, bool cpol, + uint pin_sck, uint pin_mosi, uint pin_miso) { + pio_sm_config c = cpha ? spi_cpha1_cs_program_get_default_config(prog_offs) : spi_cpha0_cs_program_get_default_config(prog_offs); + sm_config_set_out_pins(&c, pin_mosi, 1); + sm_config_set_in_pins(&c, pin_miso); + sm_config_set_sideset_pins(&c, pin_sck); + sm_config_set_out_shift(&c, false, true, n_bits); + sm_config_set_in_shift(&c, false, true, n_bits); + sm_config_set_clkdiv(&c, clkdiv); + pio_sm_set_pins_with_mask(pio, sm, (2u << pin_sck), (3u << pin_sck) | (1u << pin_mosi)); + pio_sm_set_pindirs_with_mask(pio, sm, (3u << pin_sck) | (1u << pin_mosi), (3u << pin_sck) | (1u << pin_mosi) | (1u << pin_miso)); + pio_gpio_init(pio, pin_mosi); + pio_gpio_init(pio, pin_miso); + pio_gpio_init(pio, pin_sck); + pio_gpio_init(pio, pin_sck + 1); + gpio_set_outover(pin_sck, cpol ? GPIO_OVERRIDE_INVERT : GPIO_OVERRIDE_NORMAL); + hw_set_bits(&pio->input_sync_bypass, 1u << pin_miso); + uint entry_point = prog_offs + (cpha ? spi_cpha1_cs_offset_entry_point : spi_cpha0_cs_offset_entry_point); + pio_sm_init(pio, sm, entry_point, &c); + pio_sm_exec(pio, sm, pio_encode_set(pio_x, n_bits - 2)); + pio_sm_exec(pio, sm, pio_encode_set(pio_y, n_bits - 2)); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/squarewave.pio b/src/hal/pio/assembler/comparison_tests/squarewave.pio new file mode 100644 index 0000000..405c899 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/squarewave.pio @@ -0,0 +1,13 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program squarewave + set pindirs, 1 ; Set pin to output +again: + set pins, 1 [1] ; Drive pin high and then delay for one cycle + set pins, 0 ; Drive pin low + jmp again ; Set PC to label `again` + diff --git a/src/hal/pio/assembler/comparison_tests/squarewave.pio.h b/src/hal/pio/assembler/comparison_tests/squarewave.pio.h new file mode 100644 index 0000000..9ec7ea6 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/squarewave.pio.h @@ -0,0 +1,40 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ---------- // +// squarewave // +// ---------- // + +#define squarewave_wrap_target 0 +#define squarewave_wrap 3 + +static const uint16_t squarewave_program_instructions[] = { + // .wrap_target + 0xe081, // 0: set pindirs, 1 + 0xe101, // 1: set pins, 1 [1] + 0xe000, // 2: set pins, 0 + 0x0001, // 3: jmp 1 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program squarewave_program = { + .instructions = squarewave_program_instructions, + .length = 4, + .origin = -1, +}; + +static inline pio_sm_config squarewave_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + squarewave_wrap_target, offset + squarewave_wrap); + return c; +} +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/squarewave_fast.pio b/src/hal/pio/assembler/comparison_tests/squarewave_fast.pio new file mode 100644 index 0000000..26162fa --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/squarewave_fast.pio @@ -0,0 +1,19 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +; Note that if you modify squarewave.c to include this program, you'll also +; need to set the wrap registers yourself. This would be handled for you by +; squarewave_program_get_default_config(). + + +.program squarewave_fast +; Like squarewave_wrap, but remove the delay cycles so we can run twice as fast. + set pindirs, 1 ; Set pin to output +.wrap_target + set pins, 1 ; Drive pin high + set pins, 0 ; Drive pin low +.wrap + diff --git a/src/hal/pio/assembler/comparison_tests/squarewave_fast.pio.h b/src/hal/pio/assembler/comparison_tests/squarewave_fast.pio.h new file mode 100644 index 0000000..686209c --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/squarewave_fast.pio.h @@ -0,0 +1,39 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// --------------- // +// squarewave_fast // +// --------------- // + +#define squarewave_fast_wrap_target 1 +#define squarewave_fast_wrap 2 + +static const uint16_t squarewave_fast_program_instructions[] = { + 0xe081, // 0: set pindirs, 1 + // .wrap_target + 0xe001, // 1: set pins, 1 + 0xe000, // 2: set pins, 0 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program squarewave_fast_program = { + .instructions = squarewave_fast_program_instructions, + .length = 3, + .origin = -1, +}; + +static inline pio_sm_config squarewave_fast_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + squarewave_fast_wrap_target, offset + squarewave_fast_wrap); + return c; +} +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/squarewave_test.pio b/src/hal/pio/assembler/comparison_tests/squarewave_test.pio new file mode 100644 index 0000000..1b3e290 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/squarewave_test.pio @@ -0,0 +1,12 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program squarewave +set pindirs, 1 ; Set pin to output +again: set pins, 1 [1] ; Drive pin high and then delay for one cycle + set pins, 0 ; Drive pin low + jmp again ; Set PC to label `again` + diff --git a/src/hal/pio/assembler/comparison_tests/squarewave_wrap.pio b/src/hal/pio/assembler/comparison_tests/squarewave_wrap.pio new file mode 100644 index 0000000..100f09c --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/squarewave_wrap.pio @@ -0,0 +1,19 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +; Note that if you modify squarewave.c to include this program, you'll also +; need to set the wrap registers yourself. This would be handled for you by +; squarewave_program_get_default_config(). + +.program squarewave_wrap +; Like squarewave, but use the state machine's .wrap hardware instead of an +; explicit jmp. This is a free (0-cycle) unconditional jump. + + set pindirs, 1 ; Set pin to output +.wrap_target + set pins, 1 [1] ; Drive pin high and then delay for one cycle + set pins, 0 [1] ; Drive pin low and then delay for one cycle +.wrap diff --git a/src/hal/pio/assembler/comparison_tests/squarewave_wrap.pio.h b/src/hal/pio/assembler/comparison_tests/squarewave_wrap.pio.h new file mode 100644 index 0000000..ff90703 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/squarewave_wrap.pio.h @@ -0,0 +1,39 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// --------------- // +// squarewave_wrap // +// --------------- // + +#define squarewave_wrap_wrap_target 1 +#define squarewave_wrap_wrap 2 + +static const uint16_t squarewave_wrap_program_instructions[] = { + 0xe081, // 0: set pindirs, 1 + // .wrap_target + 0xe101, // 1: set pins, 1 [1] + 0xe100, // 2: set pins, 0 [1] + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program squarewave_wrap_program = { + .instructions = squarewave_wrap_program_instructions, + .length = 3, + .origin = -1, +}; + +static inline pio_sm_config squarewave_wrap_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + squarewave_wrap_wrap_target, offset + squarewave_wrap_wrap); + return c; +} +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/st7789_lcd.pio b/src/hal/pio/assembler/comparison_tests/st7789_lcd.pio new file mode 100644 index 0000000..aa35c68 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/st7789_lcd.pio @@ -0,0 +1,57 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program st7789_lcd +.side_set 1 + +; This is just a simple clocked serial TX. At 125 MHz system clock we can +; sustain up to 62.5 Mbps. +; Data on OUT pin 0 +; Clock on side-set pin 0 + +.wrap_target + out pins, 1 side 0 ; stall here if no data (clock low) + nop side 1 +.wrap + +% c-sdk { +// For optimal use of DMA bandwidth we would use an autopull threshold of 32, +// but we are using a threshold of 8 here (consume 1 byte from each FIFO entry +// and discard the remainder) to make things easier for software on the other side + +static inline void st7789_lcd_program_init(PIO pio, uint sm, uint offset, uint data_pin, uint clk_pin, float clk_div) { + pio_gpio_init(pio, data_pin); + pio_gpio_init(pio, clk_pin); + pio_sm_set_consecutive_pindirs(pio, sm, data_pin, 1, true); + pio_sm_set_consecutive_pindirs(pio, sm, clk_pin, 1, true); + pio_sm_config c = st7789_lcd_program_get_default_config(offset); + sm_config_set_sideset_pins(&c, clk_pin); + sm_config_set_out_pins(&c, data_pin, 1); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + sm_config_set_clkdiv(&c, clk_div); + sm_config_set_out_shift(&c, false, true, 8); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +// Making use of the narrow store replication behaviour on RP2040 to get the +// data left-justified (as we are using shift-to-left to get MSB-first serial) + +static inline void st7789_lcd_put(PIO pio, uint sm, uint8_t x) { + while (pio_sm_is_tx_fifo_full(pio, sm)) + ; + *(volatile uint8_t*)&pio->txf[sm] = x; +} + +// SM is done when it stalls on an empty FIFO + +static inline void st7789_lcd_wait_idle(PIO pio, uint sm) { + uint32_t sm_stall_mask = 1u << (sm + PIO_FDEBUG_TXSTALL_LSB); + pio->fdebug = sm_stall_mask; + while (!(pio->fdebug & sm_stall_mask)) + ; +} +%} diff --git a/src/hal/pio/assembler/comparison_tests/st7789_lcd.pio.h b/src/hal/pio/assembler/comparison_tests/st7789_lcd.pio.h new file mode 100644 index 0000000..c77167e --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/st7789_lcd.pio.h @@ -0,0 +1,72 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ---------- // +// st7789_lcd // +// ---------- // + +#define st7789_lcd_wrap_target 0 +#define st7789_lcd_wrap 1 + +static const uint16_t st7789_lcd_program_instructions[] = { + // .wrap_target + 0x6001, // 0: out pins, 1 side 0 + 0xb042, // 1: nop side 1 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program st7789_lcd_program = { + .instructions = st7789_lcd_program_instructions, + .length = 2, + .origin = -1, +}; + +static inline pio_sm_config st7789_lcd_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + st7789_lcd_wrap_target, offset + st7789_lcd_wrap); + sm_config_set_sideset(&c, 1, false, false); + return c; +} + +// For optimal use of DMA bandwidth we would use an autopull threshold of 32, +// but we are using a threshold of 8 here (consume 1 byte from each FIFO entry +// and discard the remainder) to make things easier for software on the other side +static inline void st7789_lcd_program_init(PIO pio, uint sm, uint offset, uint data_pin, uint clk_pin, float clk_div) { + pio_gpio_init(pio, data_pin); + pio_gpio_init(pio, clk_pin); + pio_sm_set_consecutive_pindirs(pio, sm, data_pin, 1, true); + pio_sm_set_consecutive_pindirs(pio, sm, clk_pin, 1, true); + pio_sm_config c = st7789_lcd_program_get_default_config(offset); + sm_config_set_sideset_pins(&c, clk_pin); + sm_config_set_out_pins(&c, data_pin, 1); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + sm_config_set_clkdiv(&c, clk_div); + sm_config_set_out_shift(&c, false, true, 8); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +// Making use of the narrow store replication behaviour on RP2040 to get the +// data left-justified (as we are using shift-to-left to get MSB-first serial) +static inline void st7789_lcd_put(PIO pio, uint sm, uint8_t x) { + while (pio_sm_is_tx_fifo_full(pio, sm)) + ; + *(volatile uint8_t*)&pio->txf[sm] = x; +} +// SM is done when it stalls on an empty FIFO +static inline void st7789_lcd_wait_idle(PIO pio, uint sm) { + uint32_t sm_stall_mask = 1u << (sm + PIO_FDEBUG_TXSTALL_LSB); + pio->fdebug = sm_stall_mask; + while (!(pio->fdebug & sm_stall_mask)) + ; +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/uart_rx.pio b/src/hal/pio/assembler/comparison_tests/uart_rx.pio new file mode 100644 index 0000000..54a6577 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/uart_rx.pio @@ -0,0 +1,94 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program uart_rx_mini + +; Minimum viable 8n1 UART receiver. Wait for the start bit, then sample 8 bits +; with the correct timing. +; IN pin 0 is mapped to the GPIO used as UART RX. +; Autopush must be enabled, with a threshold of 8. + + wait 0 pin 0 ; Wait for start bit + set x, 7 [10] ; Preload bit counter, delay until eye of first data bit +bitloop: ; Loop 8 times + in pins, 1 ; Sample data + jmp x-- bitloop [6] ; Each iteration is 8 cycles + +% c-sdk { +#include "hardware/clocks.h" +#include "hardware/gpio.h" + +static inline void uart_rx_mini_program_init(PIO pio, uint sm, uint offset, uint pin, uint baud) { + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false); + pio_gpio_init(pio, pin); + gpio_pull_up(pin); + + pio_sm_config c = uart_rx_mini_program_get_default_config(offset); + sm_config_set_in_pins(&c, pin); // for WAIT, IN + // Shift to right, autopush enabled + sm_config_set_in_shift(&c, true, true, 8); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + // SM transmits 1 bit per 8 execution cycles. + float div = (float)clock_get_hz(clk_sys) / (8 * baud); + sm_config_set_clkdiv(&c, div); + + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +%} + +.program uart_rx + +; Slightly more fleshed-out 8n1 UART receiver which handles framing errors and +; break conditions more gracefully. +; IN pin 0 and JMP pin are both mapped to the GPIO used as UART RX. + +start: + wait 0 pin 0 ; Stall until start bit is asserted + set x, 7 [10] ; Preload bit counter, then delay until halfway through +bitloop: ; the first data bit (12 cycles incl wait, set). + in pins, 1 ; Shift data bit into ISR + jmp x-- bitloop [6] ; Loop 8 times, each loop iteration is 8 cycles + jmp pin good_stop ; Check stop bit (should be high) + + irq 4 rel ; Either a framing error or a break. Set a sticky flag, + wait 1 pin 0 ; and wait for line to return to idle state. + jmp start ; Don't push data if we didn't see good framing. + +good_stop: ; No delay before returning to start; a little slack is + push ; important in case the TX clock is slightly too fast. + + +% c-sdk { +static inline void uart_rx_program_init(PIO pio, uint sm, uint offset, uint pin, uint baud) { + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false); + pio_gpio_init(pio, pin); + gpio_pull_up(pin); + + pio_sm_config c = uart_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, autopush disabled + sm_config_set_in_shift(&c, true, false, 32); + // Deeper FIFO as we're not doing any TX + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + // SM transmits 1 bit per 8 execution cycles. + float div = (float)clock_get_hz(clk_sys) / (8 * baud); + sm_config_set_clkdiv(&c, div); + + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +static inline char uart_rx_program_getc(PIO pio, uint sm) { + // 8-bit read from the uppermost byte of the FIFO, as data is left-justified + io_rw_8 *rxfifo_shift = (io_rw_8*)&pio->rxf[sm] + 3; + while (pio_sm_is_rx_fifo_empty(pio, sm)) + tight_loop_contents(); + return (char)*rxfifo_shift; +} + +%} diff --git a/src/hal/pio/assembler/comparison_tests/uart_rx.pio.h b/src/hal/pio/assembler/comparison_tests/uart_rx.pio.h new file mode 100644 index 0000000..b6b5da6 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/uart_rx.pio.h @@ -0,0 +1,120 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ------------ // +// uart_rx_mini // +// ------------ // + +#define uart_rx_mini_wrap_target 0 +#define uart_rx_mini_wrap 3 + +static const uint16_t uart_rx_mini_program_instructions[] = { + // .wrap_target + 0x2020, // 0: wait 0 pin, 0 + 0xea27, // 1: set x, 7 [10] + 0x4001, // 2: in pins, 1 + 0x0642, // 3: jmp x--, 2 [6] + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program uart_rx_mini_program = { + .instructions = uart_rx_mini_program_instructions, + .length = 4, + .origin = -1, +}; + +static inline pio_sm_config uart_rx_mini_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + uart_rx_mini_wrap_target, offset + uart_rx_mini_wrap); + return c; +} + +#include "hardware/clocks.h" +#include "hardware/gpio.h" +static inline void uart_rx_mini_program_init(PIO pio, uint sm, uint offset, uint pin, uint baud) { + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false); + pio_gpio_init(pio, pin); + gpio_pull_up(pin); + pio_sm_config c = uart_rx_mini_program_get_default_config(offset); + sm_config_set_in_pins(&c, pin); // for WAIT, IN + // Shift to right, autopush enabled + sm_config_set_in_shift(&c, true, true, 8); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + // SM transmits 1 bit per 8 execution cycles. + float div = (float)clock_get_hz(clk_sys) / (8 * baud); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +#endif + +// ------- // +// uart_rx // +// ------- // + +#define uart_rx_wrap_target 0 +#define uart_rx_wrap 8 + +static const uint16_t uart_rx_program_instructions[] = { + // .wrap_target + 0x2020, // 0: wait 0 pin, 0 + 0xea27, // 1: set x, 7 [10] + 0x4001, // 2: in pins, 1 + 0x0642, // 3: jmp x--, 2 [6] + 0x00c8, // 4: jmp pin, 8 + 0xc014, // 5: irq nowait 4 rel + 0x20a0, // 6: wait 1 pin, 0 + 0x0000, // 7: jmp 0 + 0x8020, // 8: push block + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program uart_rx_program = { + .instructions = uart_rx_program_instructions, + .length = 9, + .origin = -1, +}; + +static inline pio_sm_config uart_rx_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + uart_rx_wrap_target, offset + uart_rx_wrap); + return c; +} + +static inline void uart_rx_program_init(PIO pio, uint sm, uint offset, uint pin, uint baud) { + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false); + pio_gpio_init(pio, pin); + gpio_pull_up(pin); + pio_sm_config c = uart_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, autopush disabled + sm_config_set_in_shift(&c, true, false, 32); + // Deeper FIFO as we're not doing any TX + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + // SM transmits 1 bit per 8 execution cycles. + float div = (float)clock_get_hz(clk_sys) / (8 * baud); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +static inline char uart_rx_program_getc(PIO pio, uint sm) { + // 8-bit read from the uppermost byte of the FIFO, as data is left-justified + io_rw_8 *rxfifo_shift = (io_rw_8*)&pio->rxf[sm] + 3; + while (pio_sm_is_rx_fifo_empty(pio, sm)) + tight_loop_contents(); + return (char)*rxfifo_shift; +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/uart_tx.pio b/src/hal/pio/assembler/comparison_tests/uart_tx.pio new file mode 100644 index 0000000..b1320f6 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/uart_tx.pio @@ -0,0 +1,61 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program uart_tx +.side_set 1 opt + +; An 8n1 UART transmit program. +; OUT pin 0 and side-set pin 0 are both mapped to UART TX pin. + + pull side 1 [7] ; Assert stop bit, or stall with line in idle state + set x, 7 side 0 [7] ; Preload bit counter, assert start bit for 8 clocks +bitloop: ; This loop will run 8 times (8n1 UART) + out pins, 1 ; Shift 1 bit from OSR to the first OUT pin + jmp x-- bitloop [6] ; Each loop iteration is 8 cycles. + + +% c-sdk { +#include "hardware/clocks.h" + +static inline void uart_tx_program_init(PIO pio, uint sm, uint offset, uint pin_tx, uint baud) { + // 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 = uart_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); + + // SM transmits 1 bit per 8 execution cycles. + float div = (float)clock_get_hz(clk_sys) / (8 * baud); + sm_config_set_clkdiv(&c, div); + + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +static inline void uart_tx_program_putc(PIO pio, uint sm, char c) { + pio_sm_put_blocking(pio, sm, (uint32_t)c); +} + +static inline void uart_tx_program_puts(PIO pio, uint sm, const char *s) { + while (*s) + uart_tx_program_putc(pio, sm, *s++); +} + +%} diff --git a/src/hal/pio/assembler/comparison_tests/uart_tx.pio.h b/src/hal/pio/assembler/comparison_tests/uart_tx.pio.h new file mode 100644 index 0000000..ff9b7d2 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/uart_tx.pio.h @@ -0,0 +1,73 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ------- // +// uart_tx // +// ------- // + +#define uart_tx_wrap_target 0 +#define uart_tx_wrap 3 + +static const uint16_t uart_tx_program_instructions[] = { + // .wrap_target + 0x9fa0, // 0: pull block side 1 [7] + 0xf727, // 1: set x, 7 side 0 [7] + 0x6001, // 2: out pins, 1 + 0x0642, // 3: jmp x--, 2 [6] + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program uart_tx_program = { + .instructions = uart_tx_program_instructions, + .length = 4, + .origin = -1, +}; + +static inline pio_sm_config uart_tx_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + uart_tx_wrap_target, offset + uart_tx_wrap); + sm_config_set_sideset(&c, 2, true, false); + return c; +} + +#include "hardware/clocks.h" +static inline void uart_tx_program_init(PIO pio, uint sm, uint offset, uint pin_tx, uint baud) { + // 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 = uart_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); + // SM transmits 1 bit per 8 execution cycles. + float div = (float)clock_get_hz(clk_sys) / (8 * baud); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +static inline void uart_tx_program_putc(PIO pio, uint sm, char c) { + pio_sm_put_blocking(pio, sm, (uint32_t)c); +} +static inline void uart_tx_program_puts(PIO pio, uint sm, const char *s) { + while (*s) + uart_tx_program_putc(pio, sm, *s++); +} + +#endif + diff --git a/src/hal/pio/assembler/comparison_tests/ws2812.pio b/src/hal/pio/assembler/comparison_tests/ws2812.pio new file mode 100644 index 0000000..3c31fd6 --- /dev/null +++ b/src/hal/pio/assembler/comparison_tests/ws2812.pio @@ -0,0 +1,85 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program ws2812 +.side_set 1 + +.define public T1 2 +.define public T2 5 +.define public T3 3 + +.lang_opt python sideset_init = pico.PIO.OUT_HIGH +.lang_opt python out_init = pico.PIO.OUT_HIGH +.lang_opt python out_shiftdir = 1 + +.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 + +% c-sdk { +#include "hardware/clocks.h" + +static inline void ws2812_program_init(PIO pio, uint sm, uint offset, uint pin, float freq, bool rgbw) { + + pio_gpio_init(pio, pin); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + + pio_sm_config c = ws2812_program_get_default_config(offset); + sm_config_set_sideset_pins(&c, pin); + sm_config_set_out_shift(&c, false, true, rgbw ? 32 : 24); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + + int cycles_per_bit = ws2812_T1 + ws2812_T2 + ws2812_T3; + float div = clock_get_hz(clk_sys) / (freq * cycles_per_bit); + sm_config_set_clkdiv(&c, div); + + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +%} + +.program ws2812_parallel + +.define public T1 2 +.define public T2 5 +.define public T3 3 + +.wrap_target + out x, 32 + mov pins, !null [T1-1] + mov pins, x [T2-1] + mov pins, null [T3-2] +.wrap + +% c-sdk { +#include "hardware/clocks.h" + +static inline void ws2812_parallel_program_init(PIO pio, uint sm, uint offset, uint pin_base, uint pin_count, float freq) { + for(uint i=pin_base; i |int| @intCast(T, int), + .string => |str| outer: for (define_lists) |defines| { + for (defines) |define| { + if (std.mem.eql(u8, str, define.name)) { + if (define.value > std.math.maxInt(T)) { + diags.* = Diagnostics.init( + index, + "{s}'s value ({}) is too large to fit in {} bits", + .{ str, define.value, @bitSizeOf(T) }, + ); + + break :outer error.TooBig; + } + + break :outer @intCast(T, define.value); + } + } + } else { + diags.* = Diagnostics.init(index, "define '{s}' not found", .{ + str, + }); + + break :outer error.DefineNotFound; + }, + .expression => |expr_str| { + const expr = try Expression.tokenize(expr_str, index, diags); + const result = try expr.evaluate(define_lists, diags); + if (result < 0 or result > std.math.maxInt(T)) { + diags.* = Diagnostics.init( + index, + "value of {} does not fit in a u{}", + .{ + result, @bitSizeOf(T), + }, + ); + } + + return @intCast(T, result); + }, + }; + } + + fn evaluate( + self: *Self, + comptime T: type, + program: BoundedProgram, + value: Value, + index: u32, + diags: *?Diagnostics, + ) !T { + return evaluate_impl(T, &.{ + self.output.global_defines.slice(), + self.output.private_defines.slice(), + program.defines.slice(), + program.private_defines.slice(), + }, value, index, diags); + } + + fn evaluate_target( + self: *Self, + program: BoundedProgram, + target: []const u8, + index: u32, + diags: *?Diagnostics, + ) !u5 { + return for (program.labels.slice()) |label| { + if (std.mem.eql(u8, target, label.name)) + break label.index; + } else try self.evaluate(u5, program, try Value.from_string(target), index, diags); + } + + fn evaluate_global( + self: *Self, + comptime T: type, + value: Value, + index: u32, + diags: *?Diagnostics, + ) !T { + return evaluate_impl(T, &.{ + self.output.global_defines.slice(), + self.output.private_defines.slice(), + }, value, index, diags); + } + + fn encode_globals(self: *Self, diags: *?Diagnostics) !void { + assert(self.index == 0); + + // read defines until program is had + while (self.peek_token()) |token| switch (token.data) { + .define => |define| { + const result = DefineWithIndex{ + .name = define.name, + .value = try self.evaluate_global(i128, define.value, token.index, diags), + .index = define.index, + }; + + if (define.public) + try self.output.global_defines.append(result) + else + try self.output.private_defines.append(result); + + self.consume(1); + }, + .program => break, + else => return error.InvalidTokenInGlobalSpace, + }; + } + + fn encode_program_init(self: *Self, program: *BoundedProgram, diags: *?Diagnostics) !void { + while (self.peek_token()) |token| { + switch (token.data) { + .program, .label, .instruction, .word, .wrap_target => break, + .define => |define| { + const result = DefineWithIndex{ + .name = define.name, + .value = try self.evaluate(i128, program.*, define.value, token.index, diags), + .index = define.index, + }; + + if (define.public) + try program.defines.append(result) + else + try program.private_defines.append(result); + + self.consume(1); + }, + .origin => |value| { + program.origin = try self.evaluate(u5, program.*, value, token.index, diags); + self.consume(1); + }, + .side_set => |side_set| { + program.side_set = .{ + .count = try self.evaluate(u3, program.*, side_set.count, token.index, diags), + .optional = side_set.opt, + .pindirs = side_set.pindirs, + }; + self.consume(1); + }, + // ignore + .lang_opt => self.consume(1), + .wrap => unreachable, + } + } + } + + fn encode_instruction( + self: *Self, + program: *BoundedProgram, + token: Token.Instruction, + token_index: u32, + diags: *?Diagnostics, + ) !void { + // guaranteed to be an instruction variant + const payload: Instruction.Payload = switch (token.payload) { + .nop => .{ + .mov = .{ + .destination = .y, + .operation = .none, + .source = .y, + }, + }, + .jmp => |jmp| .{ + .jmp = .{ + .condition = jmp.condition, + .address = try self.evaluate_target(program.*, jmp.target, token_index, diags), + }, + }, + .wait => |wait| .{ + .wait = .{ + .polarity = wait.polarity, + .source = wait.source, + .index = try self.evaluate(u5, program.*, wait.num, token_index, diags), + }, + }, + .in => |in| .{ + .in = .{ + .source = in.source, + .bit_count = in.bit_count, + }, + }, + .out => |out| .{ + .out = .{ + .destination = out.destination, + .bit_count = out.bit_count, + }, + }, + .push => |push| .{ + .push = .{ + .if_full = @boolToInt(push.iffull), + .block = @boolToInt(push.block), + }, + }, + .pull => |pull| .{ + .pull = .{ + .if_empty = @boolToInt(pull.ifempty), + .block = @boolToInt(pull.block), + }, + }, + .mov => |mov| .{ + .mov = .{ + .destination = mov.destination, + .operation = mov.operation, + .source = mov.source, + }, + }, + .irq => |irq| blk: { + const irq_num = try self.evaluate(u5, program.*, irq.num, token_index, diags); + break :blk .{ + .irq = .{ + .clear = @boolToInt(irq.clear), + .wait = @boolToInt(irq.wait), + .index = if (irq.rel) + @as(u5, 0x10) | irq_num + else + irq_num, + }, + }; + }, + .set => |set| .{ + .set = .{ + .destination = set.destination, + .data = try self.evaluate(u5, program.*, set.value, token_index, diags), + }, + }, + }; + + const tag: Instruction.Tag = switch (token.payload) { + .nop => .mov, + .jmp => .jmp, + .wait => .wait, + .in => .in, + .out => .out, + .push => .push_pull, + .pull => .push_pull, + .mov => .mov, + .irq => .irq, + .set => .set, + }; + + if (program.side_set) |side_set| { + if (!side_set.optional and token.side_set == null) { + diags.* = Diagnostics.init(token_index, "'side' must be specified for this instruction because 'opt' was not specied in the .side_set directive", .{}); + return error.InvalidSideSet; + } + } else { + if (token.side_set != null) { + diags.* = Diagnostics.init(token_index, ".side_set directive must be specified for program to use side_set", .{}); + return error.InvalidSideSet; + } + } + + const side_set: ?u5 = if (token.side_set) |s| + try self.evaluate(u5, program.*, s, token_index, diags) + else + null; + + const delay: ?u5 = if (token.delay) |d| + try self.evaluate(u5, program.*, d, token_index, diags) + else + null; + + const delay_side_set = try calc_delay_side_set( + program.side_set, + side_set, + delay, + ); + + try program.instructions.append(Instruction{ + .tag = tag, + .payload = payload, + .delay_side_set = delay_side_set, + }); + } + + fn calc_delay_side_set( + program_settings: ?SideSet, + side_set_opt: ?u5, + delay_opt: ?u5, + ) !u5 { + // TODO: error for side_set/delay collision + const delay: u5 = if (delay_opt) |delay| delay else 0; + return if (program_settings) |settings| + if (settings.optional) + if (side_set_opt) |side_set| + 0x10 | (side_set << @as(u3, 4) - settings.count) | delay + else + delay + else + (side_set_opt.? << @as(u3, 5) - settings.count) | delay + else + delay; + } + + fn encode_instruction_body(self: *Self, program: *BoundedProgram, diags: *?Diagnostics) !void { + // first scan through body for labels + var instr_index: u5 = 0; + for (self.tokens[self.index..]) |token| { + switch (token.data) { + .label => |label| try program.labels.append(.{ + .name = label.name, + .public = label.public, + .index = instr_index, + }), + .instruction, .word => instr_index += 1, + .wrap_target => { + if (program.wrap_target != null) { + diags.* = Diagnostics.init(token.index, "wrap_target already set for this program", .{}); + return error.WrapTargetAlreadySet; + } + + program.wrap_target = instr_index; + }, + .wrap => { + if (program.wrap != null) { + diags.* = Diagnostics.init(token.index, "wrap already set for this program", .{}); + return error.WrapAlreadySet; + } + + program.wrap = instr_index - 1; + }, + .program => break, + else => unreachable, // invalid + } + } + + // encode instructions, labels will be populated + for (self.tokens[self.index..], self.index..) |token, i| { + switch (token.data) { + .instruction => |instr| try self.encode_instruction(program, instr, token.index, diags), + .word => |word| try program.instructions.append( + @bitCast(Instruction, try self.evaluate(u16, program.*, word, token.index, diags)), + ), + // already processed + .label, .wrap_target, .wrap => {}, + .program => { + self.index = @intCast(u32, i); + break; + }, + + else => unreachable, // invalid + } + } else if (self.tokens.len > 0) + self.index = @intCast(u32, self.tokens.len); + } + + fn encode_program(self: *Self, diags: *?Diagnostics) !?BoundedProgram { + const program_token = self.get_token() orelse return null; + if (program_token.data != .program) + return error.ExpectedProgramToken; + + var program = BoundedProgram{ + .name = program_token.data.program, + .defines = BoundedDefines.init(0) catch unreachable, + .private_defines = BoundedDefines.init(0) catch unreachable, + .instructions = BoundedInstructions.init(0) catch unreachable, + .labels = BoundedLabels.init(0) catch unreachable, + .side_set = null, + .origin = null, + .wrap_target = null, + .wrap = null, + }; + + try self.encode_program_init(&program, diags); + try self.encode_instruction_body(&program, diags); + + return program; + } + + fn encode_output(self: *Self, diags: *?Diagnostics) !Self.Output { + try self.encode_globals(diags); + + while (try self.encode_program(diags)) |program| + try self.output.programs.append(program); + + return self.output; + } + }; +} + +pub const Instruction = packed struct(u16) { + payload: Payload, + delay_side_set: u5, + tag: Tag, + + pub const Payload = packed union { + jmp: Jmp, + wait: Wait, + in: In, + out: Out, + push: Push, + pull: Pull, + mov: Mov, + irq: Irq, + set: Set, + }; + + pub const Tag = enum(u3) { + jmp, + wait, + in, + out, + push_pull, + mov, + irq, + set, + }; + + pub const Jmp = packed struct(u8) { + address: u5, + condition: Token.Instruction.Jmp.Condition, + }; + + pub const Wait = packed struct(u8) { + index: u5, + source: Token.Instruction.Wait.Source, + polarity: u1, + }; + + pub const In = packed struct(u8) { + bit_count: u5, + source: Token.Instruction.In.Source, + }; + + pub const Out = packed struct(u8) { + bit_count: u5, + destination: Token.Instruction.Out.Destination, + }; + + pub const Push = packed struct(u8) { + _reserved0: u5 = 0, + block: u1, + if_full: u1, + _reserved1: u1 = 0, + }; + + pub const Pull = packed struct(u8) { + _reserved0: u5 = 0, + block: u1, + if_empty: u1, + _reserved1: u1 = 1, + }; + + pub const Mov = packed struct(u8) { + source: Token.Instruction.Mov.Source, + operation: Token.Instruction.Mov.Operation, + destination: Token.Instruction.Mov.Destination, + }; + + pub const Irq = packed struct(u8) { + index: u5, + wait: u1, + clear: u1, + reserved: u1 = 0, + }; + + pub const Set = packed struct(u8) { + data: u5, + destination: Token.Instruction.Set.Destination, + }; +}; + +//============================================================================== +// Encoder Tests +//============================================================================== + +const expect = std.testing.expect; +const expectEqual = std.testing.expectEqual; +const expectEqualStrings = std.testing.expectEqualStrings; + +fn encode_bounded_output_impl(source: []const u8, diags: *?assembler.Diagnostics) !Encoder(.{}).Output { + const tokens = try tokenizer.tokenize(source, diags, .{}); + var encoder = Encoder(.{}).init(tokens.slice()); + return try encoder.encode_output(diags); +} + +fn encode_bounded_output(source: []const u8) !Encoder(.{}).Output { + var diags: ?assembler.Diagnostics = null; + return encode_bounded_output_impl(source, &diags) catch |err| if (diags) |d| blk: { + std.log.err("error at index {}: {s}", .{ d.index, d.message.slice() }); + break :blk err; + } else err; +} + +test "encode.define" { + const output = try encode_bounded_output(".define foo 5"); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 1), output.private_defines.len); + try expectEqual(@as(usize, 0), output.programs.len); + + try expectEqualStrings("foo", output.private_defines.get(0).name); + try expectEqual(@as(i128, 5), output.private_defines.get(0).value); +} + +test "encode.define.public" { + const output = try encode_bounded_output(".define PUBLIC foo 5"); + + try expectEqual(@as(usize, 1), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 0), output.programs.len); +} + +test "encode.program.empty" { + const output = try encode_bounded_output(".program arst"); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + try expectEqualStrings("arst", output.programs.get(0).name); + try expectEqual(@as(usize, 0), output.programs.get(0).instructions.len); +} + +test "encode.program.define" { + const output = try encode_bounded_output( + \\.program arst + \\.define bruh 7 + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + const program = output.programs.get(0); + try expectEqualStrings("arst", program.name); + try expectEqual(@as(usize, 0), program.instructions.len); + + const define = program.private_defines.get(0); + try expectEqualStrings("bruh", define.name); + try expectEqual(@as(i128, 7), define.value); +} + +test "encode.program.define.public" { + const output = try encode_bounded_output( + \\.program arst + \\.define public bruh 7 + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + const program = output.programs.get(0); + try expectEqualStrings("arst", program.name); + try expectEqual(@as(usize, 0), program.instructions.len); + + const define = program.defines.get(0); + try expectEqualStrings("bruh", define.name); + try expectEqual(@as(i128, 7), define.value); +} + +test "encode.program.define.namespaced" { + const output = try encode_bounded_output( + \\.program arst + \\.define public bruh 7 + \\.program what + \\.define public hi 8 + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 2), output.programs.len); + + const program_arst = output.programs.get(0); + try expectEqualStrings("arst", program_arst.name); + try expectEqual(@as(usize, 0), program_arst.instructions.len); + + const define_bruh = program_arst.defines.get(0); + try expectEqualStrings("bruh", define_bruh.name); + try expectEqual(@as(i128, 7), define_bruh.value); + + const program_what = output.programs.get(1); + try expectEqualStrings("what", program_what.name); + try expectEqual(@as(usize, 0), program_what.instructions.len); + + const define_hi = program_what.defines.get(0); + try expectEqualStrings("hi", define_hi.name); + try expectEqual(@as(i128, 8), define_hi.value); +} + +test "encode.origin" { + const output = try encode_bounded_output( + \\.program arst + \\.origin 0 + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + const program = output.programs.get(0); + try expectEqualStrings("arst", program.name); + try expectEqual(@as(usize, 0), program.instructions.len); + + try expectEqual(@as(?u5, 0), program.origin); +} + +test "encode.wrap_target" { + const output = try encode_bounded_output( + \\.program arst + \\nop + \\.wrap_target + \\nop + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + const program = output.programs.get(0); + try expectEqualStrings("arst", program.name); + try expectEqual(@as(usize, 2), program.instructions.len); + + try expectEqual(@as(?u5, 1), program.wrap_target); +} + +test "encode.wrap" { + const output = try encode_bounded_output( + \\.program arst + \\nop + \\.wrap + \\nop + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + const program = output.programs.get(0); + try expectEqualStrings("arst", program.name); + try expectEqual(@as(usize, 2), program.instructions.len); + + try expectEqual(@as(?u5, 0), program.wrap); +} + +test "encode.side_set" { + const output = try encode_bounded_output( + \\.program arst + \\.side_set 1 + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + const program = output.programs.get(0); + try expectEqual(@as(?u5, 1), program.side_set.?.count); +} + +test "encode.side_set.opt" { + const output = try encode_bounded_output( + \\.program arst + \\.side_set 1 opt + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + const program = output.programs.get(0); + try expectEqual(@as(?u5, 1), program.side_set.?.count); + try expect(program.side_set.?.optional); +} + +test "encode.side_set.pindirs" { + const output = try encode_bounded_output( + \\.program arst + \\.side_set 1 pindirs + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + const program = output.programs.get(0); + try expectEqual(@as(?u5, 1), program.side_set.?.count); + try expect(program.side_set.?.pindirs); +} + +test "encode.label" { + const output = try encode_bounded_output( + \\.program arst + \\nop + \\my_label: + \\nop + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + const program = output.programs.get(0); + try expectEqual(@as(usize, 1), program.labels.len); + + const label = program.labels.get(0); + try expectEqualStrings("my_label", label.name); + try expectEqual(@as(u32, 1), label.index); + try expectEqual(false, label.public); +} + +test "encode.label.public" { + const output = try encode_bounded_output( + \\.program arst + \\nop + \\nop + \\public my_label: + \\nop + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + const program = output.programs.get(0); + try expectEqual(@as(usize, 1), program.labels.len); + + const label = program.labels.get(0); + try expectEqualStrings("my_label", label.name); + try expectEqual(@as(u32, 2), label.index); + try expectEqual(true, label.public); +} + +test "encode.side_set.bits" { + const output = try encode_bounded_output( + \\.program arst + \\.side_set 1 opt + \\nop side 1 + \\nop [1] + \\nop side 0 [1] + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + const program = output.programs.get(0); + + const instr0 = program.instructions.get(0); + try expectEqual(@as(u5, 0x18), instr0.delay_side_set); + + const instr1 = program.instructions.get(1); + try expectEqual(@as(u5, 0x1), instr1.delay_side_set); + + const instr2 = program.instructions.get(2); + try expectEqual(@as(u5, 0x11), instr2.delay_side_set); +} + +test "encode.evaluate.global" { + const output = try encode_bounded_output( + \\.define NUM 5 + \\.define public FOO NUM + ); + + try expectEqual(@as(usize, 1), output.private_defines.len); + try expectEqual(@as(usize, 1), output.global_defines.len); + try expectEqualStrings("FOO", output.global_defines.get(0).name); + try expectEqual(@as(i128, 5), output.global_defines.get(0).value); +} + +test "encode.evaluate.addition" { + const output = try encode_bounded_output( + \\.define public FOO (1+5) + ); + + try expectEqual(@as(usize, 1), output.global_defines.len); + try expectEqualStrings("FOO", output.global_defines.get(0).name); + try expectEqual(@as(i128, 6), output.global_defines.get(0).value); +} + +test "encode.evaluate.subtraction" { + const output = try encode_bounded_output( + \\.define public FOO (5-1) + ); + + try expectEqual(@as(usize, 1), output.global_defines.len); + try expectEqualStrings("FOO", output.global_defines.get(0).name); + try expectEqual(@as(i128, 4), output.global_defines.get(0).value); +} + +test "encode.evaluate.multiplication" { + const output = try encode_bounded_output( + \\.define public FOO (5*2) + ); + + try expectEqual(@as(usize, 1), output.global_defines.len); + try expectEqualStrings("FOO", output.global_defines.get(0).name); + try expectEqual(@as(i128, 10), output.global_defines.get(0).value); +} + +test "encode.evaluate.division" { + const output = try encode_bounded_output( + \\.define public FOO (6/2) + ); + + try expectEqual(@as(usize, 1), output.global_defines.len); + try expectEqualStrings("FOO", output.global_defines.get(0).name); + try expectEqual(@as(i128, 3), output.global_defines.get(0).value); +} + +test "encode.evaluate.bit reversal" { + const output = try encode_bounded_output( + \\.define public FOO ::1 + ); + + try expectEqual(@as(usize, 1), output.global_defines.len); + try expectEqualStrings("FOO", output.global_defines.get(0).name); + try expectEqual(@as(i128, 0x80000000), output.global_defines.get(0).value); +} + +test "encode.jmp.label" { + const output = try encode_bounded_output( + \\.program arst + \\nop + \\my_label: + \\nop + \\nop + \\jmp my_label + ); + + try expectEqual(@as(usize, 0), output.global_defines.len); + try expectEqual(@as(usize, 0), output.private_defines.len); + try expectEqual(@as(usize, 1), output.programs.len); + + const program = output.programs.get(0); + try expectEqual(@as(usize, 1), program.labels.len); + + const label = program.labels.get(0); + try expectEqualStrings("my_label", label.name); + try expectEqual(@as(u32, 1), label.index); + try expectEqual(false, label.public); + + const instr = program.instructions.get(3); + try expectEqual(Instruction.Tag.jmp, instr.tag); + try expectEqual(@as(u5, 0), instr.delay_side_set); + try expectEqual(Token.Instruction.Jmp.Condition.always, instr.payload.jmp.condition); + try expectEqual(@as(u5, 1), instr.payload.jmp.address); +} + +//test "encode.error.duplicated program name" {} +//test "encode.error.duplicated define" {} +//test "encode.error.multiple side_set" {} +//test "encode.error.label with no instruction" {} +//test "encode.error.label with no instruction" {} + +// Test Plan +// ========= +// +// - .program name validation +// - .origin in program init and in program body +// - .side_set must be in the program init +// - .wrap_target must come before an instruction, defaults to start of a program +// - .wrap_target must only be used once in a program +// - .wrap must be placed after an instruction, defaults to end of a program +// - .wrap must only be used once in a program +// - +// diff --git a/src/hal/pio/assembler/tokenizer.zig b/src/hal/pio/assembler/tokenizer.zig new file mode 100644 index 0000000..cc360f5 --- /dev/null +++ b/src/hal/pio/assembler/tokenizer.zig @@ -0,0 +1,1947 @@ +const std = @import("std"); +const assert = std.debug.assert; + +const assembler = @import("../assembler.zig"); +const Diagnostics = assembler.Diagnostics; + +const Expression = @import("Expression.zig"); + +pub const Options = struct { + capacity: u32 = 256, +}; + +pub fn tokenize( + source: []const u8, + diags: *?assembler.Diagnostics, + comptime options: Options, +) !std.BoundedArray(Token, options.capacity) { + var tokens = std.BoundedArray(Token, options.capacity).init(0) catch unreachable; + var tokenizer = Tokenizer.init(source); + while (try tokenizer.next(diags)) |token| + try tokens.append(token); + + return tokens; +} + +pub const Value = union(enum) { + // integer, hex, binary + integer: u32, + // either a symbol or label + string: []const u8, + expression: []const u8, + + pub fn format( + value: Value, + comptime fmt: []const u8, + options: std.fmt.FormatOptions, + writer: anytype, + ) !void { + _ = fmt; + _ = options; + switch (value) { + .string => |str| try writer.print("\"{s}\"", .{str}), + .expression => |expr| try writer.print("{s}", .{expr}), + .integer => |int| try writer.print("{}", .{int}), + } + } + + pub fn from_string(str: []const u8) !Value { + return Value{ + .integer = std.fmt.parseInt(u32, str, 0) catch { + return Value{ + .string = str, + }; + }, + }; + } +}; + +// the characters we're interested in are: +// ';' -> line comment +// '/' -> '/' -> line comment +// '/' -> '*' -> block comment +// '%' -> -> -> -> '{' -> code block +// '.' -> directive +pub const Tokenizer = struct { + source: []const u8, + index: u32, + + pub fn format( + self: Tokenizer, + comptime fmt: []const u8, + options: std.fmt.FormatOptions, + writer: anytype, + ) !void { + _ = fmt; + _ = options; + + try writer.print( + \\parser: + \\ index: {} + \\ + \\ + , .{self.index}); + + var printed_cursor = false; + var line_it = std.mem.tokenize(u8, self.source, "\n\r"); + while (line_it.next()) |line| { + try writer.print("{s}\n", .{line}); + if (!printed_cursor and line_it.index > self.index) { + try writer.writeByteNTimes(' ', line.len - (line_it.index - self.index)); + try writer.writeAll("\x1b[30;42;1m^\x1b[0m\n"); + printed_cursor = true; + } + } + } + + fn init(source: []const u8) Tokenizer { + return Tokenizer{ + .source = source, + .index = 0, + }; + } + + fn consume(self: *Tokenizer, count: u32) void { + assert(self.index < self.source.len); + self.index += count; + } + + fn peek(self: Tokenizer) ?u8 { + return if (self.index < self.source.len) + self.source[self.index] + else + null; + } + + fn get(self: *Tokenizer) ?u8 { + return if (self.index < self.source.len) blk: { + defer self.index += 1; + break :blk self.source[self.index]; + } else null; + } + + fn skip_line(self: *Tokenizer) void { + while (self.get()) |c| + if (c == '\n') + return; + } + + fn skip_until_end_of_comment_block(self: *Tokenizer) void { + while (self.get()) |c| { + if (c == '*') { + if (self.peek()) |p| { + self.consume(1); + if (p == '/') { + return; + } + } + } + } + } + + fn skip_until_end_of_code_block(self: *Tokenizer) void { + // TODO: assert we have the code identifier and open curly bracket + while (self.get()) |c| { + if (c == '%') { + if (self.peek()) |p| { + self.consume(1); + if (p == '}') { + return; + } + } + } + } + } + + fn read_until_whitespace_or_end(self: *Tokenizer) ![]const u8 { + const start = self.index; + var end: ?u32 = null; + while (self.peek()) |p| { + switch (p) { + ' ', '\n', '\r', '\t', ',' => { + end = self.index; + break; + }, + else => self.consume(1), + } + } else end = self.index; + + return self.source[start .. end orelse return error.EndOfStream]; + } + + fn skip_whitespace(self: *Tokenizer) void { + while (self.peek()) |p| { + switch (p) { + ' ', '\t', '\r', '\n', ',' => self.consume(1), + else => return, + } + } + } + + /// returns array of args + fn get_args(self: *Tokenizer, comptime num: u32, diags: *?Diagnostics) TokenizeError![num]?[]const u8 { + var args: [num]?[]const u8 = undefined; + for (&args) |*arg| + arg.* = try self.get_arg(diags); + + return args; + } + + const PeekResult = struct { + str: []const u8, + start: u32, + }; + + fn peek_arg(self: *Tokenizer, diags: *?Diagnostics) TokenizeError!?PeekResult { + var tmp_index = self.index; + return self.peek_arg_impl(&tmp_index, diags); + } + + fn consume_peek(self: *Tokenizer, result: PeekResult) void { + assert(self.index <= result.start); + self.index = result.start + @intCast(u32, result.str.len); + } + + /// gets next arg without consuming the stream + fn peek_arg_impl( + self: *Tokenizer, + index: *u32, + diags: *?Diagnostics, + ) TokenizeError!?PeekResult { + + // skip whitespace + while (index.* < self.source.len) { + switch (self.source[index.*]) { + ' ', '\t', ',' => index.* += 1, + else => break, + } + } + + if (index.* == self.source.len) + return null; + + const start = index.*; + const end = end: { + break :end switch (self.source[start]) { + '(' => blk: { + var depth: u32 = 0; + break :blk while (index.* < self.source.len) : (index.* += 1) { + switch (self.source[index.*]) { + '(' => depth += 1, + ')' => { + depth -= 1; + + if (depth == 0) { + index.* += 1; + break index.*; + } + }, + else => {}, + } + } else { + diags.* = Diagnostics.init(start, "mismatched parenthesis", .{}); + return error.InvalidExpression; + }; + }, + '[' => while (index.* < self.source.len) : (index.* += 1) { + if (self.source[index.*] == ']') { + index.* += 1; + break index.*; + } + } else { + diags.* = Diagnostics.init(start, "mismatched parenthesis", .{}); + return error.InvalidExpression; + }, + else => while (index.* < self.source.len) { + switch (self.source[index.*]) { + // ; and / are to stop at comments + ' ', '\t', '\r', '\n', ',', ';', '/' => break index.*, + else => index.* += 1, + } + } else index.*, + }; + }; + + return if (start != end) + PeekResult{ + .str = self.source[start..end], + .start = start, + } + else + null; + } + + fn get_arg(self: *Tokenizer, diags: *?Diagnostics) TokenizeError!?[]const u8 { + return if (try self.peek_arg_impl(&self.index, diags)) |result| + result.str + else + null; + } + + const Identifier = struct { + index: u32, + str: []const u8, + }; + + fn get_identifier(self: *Tokenizer) TokenizeError!Identifier { + self.skip_whitespace(); + return Identifier{ + .index = self.index, + .str = try self.read_until_whitespace_or_end(), + }; + } + + const TokenizeError = error{ + EndOfStream, + NoValue, + NotAnExpression, + Overflow, + InvalidCharacter, + InvalidSource, + InvalidCondition, + MissingArg, + InvalidDestination, + InvalidOperation, + InvalidExpression, + TooBig, + }; + + fn get_program(self: *Tokenizer, index: u32, diags: *?Diagnostics) TokenizeError!Token { + const name = (try self.get_arg(diags)) orelse { + diags.* = Diagnostics.init(index, "missing program name", .{}); + return error.MissingArg; + }; + return Token{ + .index = index, + .data = .{ .program = name }, + }; + } + + fn assert_is_lower(str: []const u8) void { + for (str) |c| + assert(std.ascii.isLower(c)); + } + + fn eql_lower(comptime lhs: []const u8, rhs: []const u8) bool { + assert_is_lower(lhs); + if (lhs.len != rhs.len) + return false; + + var buf: [lhs.len]u8 = undefined; + for (&buf, rhs) |*b, r| + b.* = std.ascii.toLower(r); + + return std.mem.eql(u8, &buf, lhs); + } + + fn get_define(self: *Tokenizer, index: u32, diags: *?Diagnostics) TokenizeError!Token { + const maybe_public = try self.get_identifier(); + var is_public = eql_lower("public", maybe_public.str); + + const name = if (is_public) + try self.get_identifier() + else + maybe_public; + + return Token{ + .index = index, + .data = .{ + .define = .{ + .name = name.str, + .value = Value{ + .expression = (try self.get_arg(diags)) orelse { + diags.* = Diagnostics.init(index, "failed to get expression", .{}); + return error.InvalidExpression; + }, + }, + .public = is_public, + .index = name.index, + }, + }, + }; + } + + fn get_expression(self: *Tokenizer) TokenizeError!Value { + const start = self.index; + var count: u32 = 1; + + if (self.get()) |c| + if (c != '(') + return error.NotAnExpression; + + while (self.get()) |c| { + switch (c) { + '(' => count += 1, + ')' => { + count -= 1; + }, + else => {}, + } + + if (count == 0) { + return Value{ + .expression = self.source[start..self.index], + }; + } + } else { + return error.NotAnExpression; + } + } + + fn get_value(self: *Tokenizer) TokenizeError!Value { + self.skip_whitespace(); + + if (self.peek()) |p| + if (p == '(') + return try self.get_expression() + else { + const identifier = try self.get_identifier(); + return try Value.from_string(identifier.str); + } + else + return error.NoValue; + } + + fn get_origin(self: *Tokenizer, index: u32, diags: *?Diagnostics) TokenizeError!Token { + _ = diags; + return Token{ + .index = index, + .data = .{ + .origin = try self.get_value(), + }, + }; + } + + fn get_side_set(self: *Tokenizer, index: u32, diags: *?Diagnostics) TokenizeError!Token { + const args = try self.get_args(3, diags); + const count = try Value.from_string(args[0] orelse { + diags.* = Diagnostics.init(index, "missing count", .{}); + return error.MissingArg; + }); + var opt = false; + var pindirs = false; + + if (args[1]) |arg| { + if (std.mem.eql(u8, "opt", arg)) + opt = true + else if (std.mem.eql(u8, "pindirs", arg)) + pindirs = true; + } + + if (args[2]) |arg| { + if (std.mem.eql(u8, "pindirs", arg)) + pindirs = true; + } + + return Token{ + .index = index, + .data = .{ + .side_set = .{ + .count = count, + .opt = opt, + .pindirs = pindirs, + }, + }, + }; + } + + fn get_wrap_target(_: *Tokenizer, index: u32, _: *?Diagnostics) TokenizeError!Token { + return Token{ + .index = index, + .data = .{ .wrap_target = {} }, + }; + } + + fn get_wrap(_: *Tokenizer, index: u32, _: *?Diagnostics) TokenizeError!Token { + return Token{ + .index = index, + .data = .{ .wrap = {} }, + }; + } + + fn get_lang_opt(self: *Tokenizer, index: u32, diags: *?Diagnostics) TokenizeError!Token { + _ = diags; + return Token{ + .index = index, + .data = .{ + .lang_opt = .{ + .lang = (try self.get_identifier()).str, + .name = (try self.get_identifier()).str, + .option = (try self.get_identifier()).str, + }, + }, + }; + } + + fn get_word(self: *Tokenizer, index: u32, diags: *?Diagnostics) TokenizeError!Token { + _ = diags; + return Token{ + .index = index, + .data = .{ .word = try self.get_value() }, + }; + } + + const directives = std.ComptimeStringMap(*const fn (*Tokenizer, u32, *?Diagnostics) TokenizeError!Token, .{ + .{ "program", get_program }, + .{ "define", get_define }, + .{ "origin", get_origin }, + .{ "side_set", get_side_set }, + .{ "wrap_target", get_wrap_target }, + .{ "wrap", get_wrap }, + .{ "lang_opt", get_lang_opt }, + .{ "word", get_word }, + }); + + fn get_directive(self: *Tokenizer, diags: *?Diagnostics) !Token { + const index = self.index; + const identifier = try self.read_until_whitespace_or_end(); + return if (directives.get(identifier)) |handler| ret: { + const ret = try handler(self, index, diags); + self.skip_line(); + break :ret ret; + } else error.InvalidDirective; + } + + fn get_nop(_: *Tokenizer, _: *?Diagnostics) TokenizeError!Token.Instruction.Payload { + return Token.Instruction.Payload{ + .nop = {}, + }; + } + + fn target_from_string(str: []const u8) TokenizeError!Token.Instruction.Jmp.Target { + const value = Value.from_string(str); + return Token.Instruction.Payload{ + .jmp = .{ + .condition = .always, + .target = switch (value) { + .string => |label| Token.Instruction.Jmp.Target{ + .label = label, + }, + else => Token.Instruction.Jmp.Target{ + .value = value, + }, + }, + }, + }; + } + + fn get_jmp(self: *Tokenizer, diags: *?Diagnostics) TokenizeError!Token.Instruction.Payload { + const Condition = Token.Instruction.Jmp.Condition; + const conditions = std.ComptimeStringMap(Condition, .{ + .{ "!x", .x_is_zero }, + .{ "x--", .x_dec }, + .{ "!y", .y_is_zero }, + .{ "y--", .y_dec }, + .{ "x!=y", .x_is_not_y }, + .{ "pin", .pin }, + .{ "!osre", .osre_not_empty }, + }); + + const maybe_cond = (try self.get_arg(diags)) orelse return error.MissingArg; + const maybe_cond_lower = try lowercase_bounded(256, maybe_cond); + const cond: Condition = conditions.get(maybe_cond_lower.slice()) orelse .always; + const target_str = if (cond == .always) + maybe_cond + else + (try self.get_arg(diags)) orelse return error.MissingArg; + + return Token.Instruction.Payload{ + .jmp = .{ .condition = cond, .target = target_str }, + }; + } + + fn get_wait(self: *Tokenizer, diags: *?Diagnostics) TokenizeError!Token.Instruction.Payload { + const polarity = try std.fmt.parseInt(u1, (try self.get_arg(diags)) orelse return error.MissingArg, 0); + const source_str = (try self.get_arg(diags)) orelse return error.MissingArg; + const pin = try Value.from_string((try self.get_arg(diags)) orelse return error.MissingArg); + + var buf: [8]u8 = undefined; + for (source_str, 0..) |c, i| + buf[i] = std.ascii.toLower(c); + + const source_lower = buf[0..source_str.len]; + const source: Token.Instruction.Wait.Source = + if (std.mem.eql(u8, "gpio", source_lower)) + .gpio + else if (std.mem.eql(u8, "pin", source_lower)) + .pin + else if (std.mem.eql(u8, "irq", source_lower)) + .irq + else + return error.InvalidSource; + + const rel: bool = if (source == .irq) + if (try self.peek_arg(diags)) |rel_result| blk: { + const is_rel = std.mem.eql(u8, "rel", rel_result.str); + if (is_rel) + self.consume_peek(rel_result); + + break :blk is_rel; + } else false + else + false; + + return Token.Instruction.Payload{ + .wait = .{ + .polarity = polarity, + .source = source, + .num = pin, + .rel = rel, + }, + }; + } + + /// get the lowercase of a string, returns an error if it's too big + fn lowercase_bounded(comptime max_size: usize, str: []const u8) TokenizeError!std.BoundedArray(u8, max_size) { + if (str.len > max_size) + return error.TooBig; + + var ret = std.BoundedArray(u8, max_size).init(0) catch unreachable; + for (str) |c| + try ret.append(std.ascii.toLower(c)); + + return ret; + } + + // TODO: I need to take a break. There is no rush to finish this. The thing + // I need to keep in mind with `get_args()` is that I must only consume the + // args that are used. side set and delay may be on the same line + + fn get_in(self: *Tokenizer, diags: *?Diagnostics) TokenizeError!Token.Instruction.Payload { + const source_str = (try self.get_arg(diags)) orelse return error.MissingArg; + const bit_count_str = (try self.get_arg(diags)) orelse return error.MissingArg; + + const source_lower = try lowercase_bounded(256, source_str); + const bit_count_tmp = try std.fmt.parseInt(u6, bit_count_str, 0); + const bit_count = if (bit_count_tmp == 32) + @as(u5, 0) + else + @intCast(u5, bit_count_tmp); + + return Token.Instruction.Payload{ + .in = .{ + .source = std.meta.stringToEnum(Token.Instruction.In.Source, source_lower.slice()) orelse return error.InvalidSource, + .bit_count = bit_count, + }, + }; + } + + fn get_out(self: *Tokenizer, diags: *?Diagnostics) TokenizeError!Token.Instruction.Payload { + const dest_src = (try self.get_arg(diags)) orelse return error.MissingArg; + const bit_count_str = (try self.get_arg(diags)) orelse return error.MissingArg; + + const dest_lower = try lowercase_bounded(256, dest_src); + const bit_count_tmp = try std.fmt.parseInt(u6, bit_count_str, 0); + const bit_count = if (bit_count_tmp == 32) + @as(u5, 0) + else + @intCast(u5, bit_count_tmp); + + return Token.Instruction.Payload{ + .out = .{ + .destination = std.meta.stringToEnum(Token.Instruction.Out.Destination, dest_lower.slice()) orelse return error.InvalidDestination, + .bit_count = bit_count, + }, + }; + } + + fn block_from_peek(self: *Tokenizer, result: PeekResult) TokenizeError!bool { + const block_lower = try lowercase_bounded(256, result.str); + const is_block = std.mem.eql(u8, "block", block_lower.slice()); + const is_noblock = std.mem.eql(u8, "noblock", block_lower.slice()); + + if (is_block or is_noblock) + self.consume_peek(result); + + return if (is_block) + true + else if (is_noblock) + false + else + true; + } + + fn get_push(self: *Tokenizer, diags: *?Diagnostics) TokenizeError!Token.Instruction.Payload { + return if (try self.peek_arg(diags)) |first_result| ret: { + const lower = try lowercase_bounded(256, first_result.str); + const iffull = std.mem.eql(u8, "iffull", lower.slice()); + + const block: bool = if (iffull) blk: { + self.consume_peek(first_result); + break :blk if (try self.peek_arg(diags)) |block_result| + try self.block_from_peek(block_result) + else + true; + } else try self.block_from_peek(first_result); + + break :ret Token.Instruction.Payload{ + .push = .{ + .iffull = iffull, + .block = block, + }, + }; + } else Token.Instruction.Payload{ + .push = .{ + .iffull = false, + .block = true, + }, + }; + } + + fn get_pull(self: *Tokenizer, diags: *?Diagnostics) TokenizeError!Token.Instruction.Payload { + return if (try self.peek_arg(diags)) |first_result| ret: { + const lower = try lowercase_bounded(256, first_result.str); + const ifempty = std.mem.eql(u8, "ifempty", lower.slice()); + + const block: bool = if (ifempty) blk: { + self.consume_peek(first_result); + break :blk if (try self.peek_arg(diags)) |block_result| + try self.block_from_peek(block_result) + else + true; + } else try self.block_from_peek(first_result); + + break :ret Token.Instruction.Payload{ + .pull = .{ + .ifempty = ifempty, + .block = block, + }, + }; + } else Token.Instruction.Payload{ + .pull = .{ + .ifempty = false, + .block = true, + }, + }; + } + + fn get_mov(self: *Tokenizer, diags: *?Diagnostics) TokenizeError!Token.Instruction.Payload { + const dest_str = (try self.get_arg(diags)) orelse return error.MissingArg; + const dest_lower = try lowercase_bounded(256, dest_str); + const destination = std.meta.stringToEnum(Token.Instruction.Mov.Destination, dest_lower.slice()) orelse return error.InvalidDestination; + + const second = try self.get_arg(diags) orelse return error.MissingArg; + const op_prefixed: ?[]const u8 = if (std.mem.startsWith(u8, second, "!")) + "!" + else if (std.mem.startsWith(u8, second, "~")) + "~" + else if (std.mem.startsWith(u8, second, "::")) + "::" + else + null; + + const source_str = if (op_prefixed) |op_str| + if (second.len == op_str.len) + (try self.get_arg(diags)) orelse return error.MissingArg + else + second[op_str.len..] + else + second; + + const source_lower = try lowercase_bounded(256, source_str); + const source = std.meta.stringToEnum(Token.Instruction.Mov.Source, source_lower.slice()) orelse return error.InvalidSource; + const operation: Token.Instruction.Mov.Operation = if (op_prefixed) |op_str| + if (std.mem.eql(u8, "!", op_str)) + .invert + else if (std.mem.eql(u8, "~", op_str)) + .invert + else if (std.mem.eql(u8, "::", op_str)) + .bit_reverse + else + return error.InvalidOperation + else + .none; + + return Token.Instruction.Payload{ + .mov = .{ + .destination = destination, + .source = source, + .operation = operation, + }, + }; + } + + fn get_irq(self: *Tokenizer, diags: *?Diagnostics) TokenizeError!Token.Instruction.Payload { + const first = (try self.get_arg(diags)) orelse return error.MissingArg; + + var clear = false; + var wait = false; + var has_mode = false; + const first_lower = try lowercase_bounded(256, first); + if (std.mem.eql(u8, "set", first_lower.slice())) { + has_mode = true; + // do nothing + } else if (std.mem.eql(u8, "nowait", first_lower.slice())) { + has_mode = true; + // do nothing + } else if (std.mem.eql(u8, "wait", first_lower.slice())) { + has_mode = true; + wait = true; + } else if (std.mem.eql(u8, "clear", first_lower.slice())) { + has_mode = true; + clear = true; + } + + const num = Value{ + .expression = if (has_mode) + (try self.get_arg(diags)) orelse { + diags.* = Diagnostics.init(self.index, "irq (mode) (rel): failed to get num argument", .{}); + return error.MissingArg; + } + else + first, + }; + + const rel: bool = if (try self.peek_arg(diags)) |result| blk: { + const rel_lower = try lowercase_bounded(256, result.str); + const is_rel = std.mem.eql(u8, "rel", rel_lower.slice()); + if (is_rel) + self.consume_peek(result); + + break :blk is_rel; + } else false; + + return Token.Instruction.Payload{ + .irq = .{ + .clear = clear, + .wait = wait, + .num = num, + .rel = rel, + }, + }; + } + + fn get_set(self: *Tokenizer, diags: *?Diagnostics) TokenizeError!Token.Instruction.Payload { + const dest_str = (try self.get_arg(diags)) orelse { + diags.* = Diagnostics.init(0, "missing destination", .{}); + return error.MissingArg; + }; + const value = try self.get_value(); + + const dest_lower = try lowercase_bounded(256, dest_str); + + return Token.Instruction.Payload{ + .set = .{ + .destination = std.meta.stringToEnum(Token.Instruction.Set.Destination, dest_lower.slice()) orelse return error.InvalidDestination, + .value = value, + }, + }; + } + + const instructions = std.ComptimeStringMap(*const fn (*Tokenizer, *?Diagnostics) TokenizeError!Token.Instruction.Payload, .{ + .{ "nop", get_nop }, + .{ "jmp", get_jmp }, + .{ "wait", get_wait }, + .{ "in", get_in }, + .{ "out", get_out }, + .{ "push", get_push }, + .{ "pull", get_pull }, + .{ "mov", get_mov }, + .{ "irq", get_irq }, + .{ "set", get_set }, + }); + + fn get_instruction(self: *Tokenizer, name: Identifier, diags: *?Diagnostics) !Token { + const name_lower = try lowercase_bounded(256, name.str); + const payload = if (instructions.get(name_lower.slice())) |handler| + try handler(self, diags) + else { + diags.* = Diagnostics.init(name.index, "invalid instruction", .{}); + return error.InvalidInstruction; + }; + + var side_set: ?Value = null; + var delay: ?Value = null; + + if (try self.peek_arg(diags)) |result| { + if (eql_lower("side", result.str)) { + self.consume_peek(result); + + const side_set_str = (try self.get_arg(diags)) orelse return error.MissingArg; + side_set = Value{ .expression = side_set_str }; + } else if (std.mem.startsWith(u8, result.str, "[") and std.mem.endsWith(u8, result.str, "]")) { + self.consume_peek(result); + delay = Value{ .expression = result.str[1 .. result.str.len - 1] }; + } + } + + if (try self.peek_arg(diags)) |result| { + if (eql_lower("side", result.str)) { + self.consume_peek(result); + + const side_set_str = (try self.get_arg(diags)) orelse return error.MissingArg; + assert(side_set == null); + side_set = Value{ .expression = side_set_str }; + } else if (std.mem.startsWith(u8, result.str, "[") and std.mem.endsWith(u8, result.str, "]")) { + self.consume_peek(result); + assert(delay == null); + delay = Value{ + .expression = result.str[1 .. result.str.len - 1], + }; + } + } + + self.skip_line(); + return Token{ + .index = name.index, + .data = .{ + .instruction = .{ + .payload = payload, + .side_set = side_set, + .delay = delay, + }, + }, + }; + } + + fn next(self: *Tokenizer, diags: *?assembler.Diagnostics) !?Token { + while (self.peek()) |p| { + switch (p) { + ' ', '\t', '\n', '\r', ',' => self.consume(1), + ';' => self.skip_line(), + '/' => { + self.consume(1); + if (self.peek()) |p2| { + self.consume(1); + switch (p2) { + '/' => self.skip_line(), + '*' => self.skip_until_end_of_comment_block(), + else => unreachable, + } + } else return null; + }, + '%' => { + self.consume(1); + self.skip_until_end_of_code_block(); + }, + '.' => { + self.consume(1); + return try self.get_directive(diags); + }, + 'a'...'z', 'A'...'Z', '0'...'9', '_' => { + const first = try self.get_identifier(); + + // definitely a label + return if (eql_lower("public", first.str)) + Token{ + .index = first.index, + .data = .{ + .label = .{ + .public = true, + .name = blk: { + const tmp = (try self.get_identifier()).str; + break :blk tmp[0 .. tmp.len - 1]; + }, + }, + }, + } + else if (std.mem.endsWith(u8, first.str, ":")) + Token{ + .index = first.index, + .data = .{ + .label = .{ + .name = first.str[0 .. first.str.len - 1], + }, + }, + } + else + try self.get_instruction(first, diags); + }, + else => return error.Unhandled, + } + } + + return null; + } +}; + +pub const Token = struct { + index: u32, + data: union(enum) { + program: []const u8, + define: Token.Define, + origin: Value, + side_set: SideSet, + wrap_target: void, + wrap: void, + lang_opt: LangOpt, + word: Value, + label: Label, + instruction: Instruction, + }, + + pub const Tag = std.meta.Tag(std.meta.FieldType(Token, .data)); + + pub const Label = struct { + name: []const u8, + public: bool = false, + }; + + // TODO: use Value instead of numbers + pub const Instruction = struct { + payload: Payload, + side_set: ?Value = null, + // TODO: delay can look like [T1-1], so we could consider the square + // brackets to be an expression + delay: ?Value = null, + + pub const Payload = union(enum) { + nop: void, + jmp: Jmp, + wait: Wait, + in: In, + out: Out, + push: Push, + pull: Pull, + mov: Mov, + irq: Irq, + set: Set, + }; + + pub const Jmp = struct { + condition: Condition, + target: []const u8, + + pub const Condition = enum(u3) { + always = 0b000, + x_is_zero = 0b001, // !X + x_dec = 0b010, // X-- + y_is_zero = 0b011, // !Y + y_dec = 0b100, // Y-- + x_is_not_y = 0b101, //X!=Y + pin = 0b110, // PIN + osre_not_empty = 0b111, // !OSRE + }; + }; + + pub const Wait = struct { + polarity: u1, + source: Source, + num: Value, + rel: bool, + + pub const Source = enum(u2) { + gpio = 0b00, + pin = 0b01, + irq = 0b10, + }; + }; + + pub const In = struct { + source: Source, + bit_count: u5, + + pub const Source = enum(u3) { + pins = 0b00, + x = 0b001, + y = 0b010, + null = 0b011, + isr = 0b110, + osr = 0b111, + }; + }; + + pub const Out = struct { + destination: Destination, + bit_count: u5, + + pub const Destination = enum(u3) { + pins = 0b000, + x = 0b001, + y = 0b010, + null = 0b011, + pindirs = 0b100, + pc = 0b101, + isr = 0b110, + exec = 0b111, + }; + }; + + pub const Push = struct { + block: bool, + iffull: bool, + }; + + pub const Pull = struct { + block: bool, + ifempty: bool, + }; + + pub const Mov = struct { + destination: Destination, + operation: Operation, + source: Source, + + pub const Destination = enum(u3) { + pins = 0b000, + x = 0b001, + y = 0b010, + exec = 0b100, + pc = 0b101, + isr = 0b110, + osr = 0b111, + }; + + pub const Operation = enum(u2) { + none = 0b00, + invert = 0b01, + bit_reverse = 0b10, + }; + + pub const Source = enum(u3) { + pins = 0b00, + x = 0b001, + y = 0b010, + null = 0b011, + status = 0b101, + isr = 0b110, + osr = 0b111, + }; + }; + + pub const Irq = struct { + clear: bool, + wait: bool, + num: Value, + rel: bool, + }; + + pub const Set = struct { + destination: Destination, + value: Value, + + pub const Destination = enum(u3) { + pins = 0b000, + x = 0b001, + y = 0b010, + pindirs = 0b100, + }; + }; + }; + + pub const Define = struct { + name: []const u8, + value: Value, + public: bool = false, + index: u32, + }; + + pub const SideSet = struct { + count: Value, + opt: bool = false, + pindirs: bool = false, + }; + + pub const LangOpt = struct { + lang: []const u8, + name: []const u8, + option: []const u8, + }; +}; + +//============================================================================== +// Tokenization Tests +//============================================================================== + +const expect = std.testing.expect; +const expectEqual = std.testing.expectEqual; +const expectEqualStrings = std.testing.expectEqualStrings; + +const DirectiveTag = @typeInfo(Token.Directive).Union.tag_type.?; +const PayloadTag = @typeInfo(Token.Instruction.Payload).Union.tag_type.?; + +fn expect_program(expected: []const u8, actual: Token) !void { + try expectEqual(Token.Tag.program, actual.data); + try expectEqualStrings(expected, actual.data.program); +} + +fn expect_value(expected: Value, actual: Value) !void { + switch (expected) { + .integer => |int| try expectEqual(int, actual.integer), + .string => |str| try expectEqualStrings(str, actual.string), + .expression => |expr| try expectEqualStrings(expr, actual.expression), + } +} + +fn expect_opt_value(expected: ?Value, actual: ?Value) !void { + if (expected != null) + switch (expected.?) { + .integer => |int| try expectEqual(int, actual.?.integer), + .string => |str| try expectEqualStrings(str, actual.?.string), + .expression => |expr| try expectEqualStrings(expr, actual.?.expression), + }; +} + +fn expect_define(expected: Token.Define, actual: Token) !void { + try expectEqual(Token.Tag.define, actual.data); + + const define = actual.data.define; + try expectEqualStrings(expected.name, define.name); + try expect_value(expected.value, define.value); +} + +fn expect_origin(expected: Value, actual: Token) !void { + try expectEqual(Token.Tag.origin, actual.data); + try expect_value(expected, actual.data.origin); +} + +fn expect_side_set(expected: Token.SideSet, actual: Token) !void { + try expectEqual(Token.Tag.side_set, actual.data); + + const side_set = actual.data.side_set; + try expect_value(expected.count, side_set.count); + try expectEqual(expected.opt, side_set.opt); + try expectEqual(expected.pindirs, side_set.pindirs); +} + +fn expect_wrap_target(actual: Token) !void { + try expectEqual(Token.Tag.wrap_target, actual.data); +} + +fn expect_wrap(actual: Token) !void { + try expectEqual(Token.Tag.wrap, actual.data); +} + +fn expect_lang_opt(expected: Token.LangOpt, actual: Token) !void { + try expectEqual(Token.Tag.lang_opt, actual.data); + + const lang_opt = actual.data.lang_opt; + try expectEqualStrings(expected.lang, lang_opt.lang); + try expectEqualStrings(expected.name, lang_opt.name); + try expectEqualStrings(expected.option, lang_opt.option); +} + +fn expect_word(expected: Value, actual: Token) !void { + try expectEqual(Token.Tag.word, actual.data); + try expect_value(expected, actual.data.word); +} + +fn expect_label(expected: Token.Label, actual: Token) !void { + try expectEqual(Token.Tag.label, actual.data); + + const label = actual.data.label; + try expectEqual(expected.public, label.public); + try expectEqualStrings(expected.name, label.name); +} + +const ExpectedNopInstr = struct { + delay: ?Value = null, + side_set: ?Value = null, +}; + +fn expect_instr_nop(expected: ExpectedNopInstr, actual: Token) !void { + try expectEqual(Token.Tag.instruction, actual.data); + try expectEqual(PayloadTag.nop, actual.data.instruction.payload); + + const instr = actual.data.instruction; + try expect_opt_value(expected.delay, instr.delay); + try expect_opt_value(expected.side_set, instr.side_set); +} + +const ExpectedSetInstr = struct { + dest: Token.Instruction.Set.Destination, + value: Value, + delay: ?Value = null, + side_set: ?Value = null, +}; + +fn expect_instr_set(expected: ExpectedSetInstr, actual: Token) !void { + try expectEqual(Token.Tag.instruction, actual.data); + try expectEqual(PayloadTag.set, actual.data.instruction.payload); + + const instr = actual.data.instruction; + try expect_opt_value(expected.delay, instr.delay); + try expect_opt_value(expected.side_set, instr.side_set); + + const set = instr.payload.set; + try expectEqual(expected.dest, set.destination); + try expect_value(expected.value, set.value); +} + +const ExpectedJmpInstr = struct { + cond: Token.Instruction.Jmp.Condition = .always, + target: []const u8, + delay: ?Value = null, + side_set: ?Value = null, +}; + +fn expect_instr_jmp(expected: ExpectedJmpInstr, actual: Token) !void { + try expectEqual(Token.Tag.instruction, actual.data); + try expectEqual(PayloadTag.jmp, actual.data.instruction.payload); + + const instr = actual.data.instruction; + try expect_opt_value(expected.delay, instr.delay); + try expect_opt_value(expected.side_set, instr.side_set); + + const jmp = instr.payload.jmp; + try expectEqual(expected.cond, jmp.condition); + try expectEqualStrings(expected.target, jmp.target); +} + +const ExpectedWaitInstr = struct { + polarity: u1, + source: Token.Instruction.Wait.Source, + num: Value, + // only valid for irq source + rel: bool = false, + delay: ?Value = null, + side_set: ?Value = null, +}; + +fn expect_instr_wait(expected: ExpectedWaitInstr, actual: Token) !void { + try expectEqual(Token.Tag.instruction, actual.data); + try expectEqual(PayloadTag.wait, actual.data.instruction.payload); + + const instr = actual.data.instruction; + try expect_opt_value(expected.delay, instr.delay); + try expect_opt_value(expected.side_set, instr.side_set); + + const wait = instr.payload.wait; + try expectEqual(expected.polarity, wait.polarity); + try expectEqual(expected.source, wait.source); + try expect_value(expected.num, wait.num); +} + +const ExpectedInInstr = struct { + source: Token.Instruction.In.Source, + bit_count: u5, + delay: ?Value = null, + side_set: ?Value = null, +}; + +fn expect_instr_in(expected: ExpectedInInstr, actual: Token) !void { + try expectEqual(Token.Tag.instruction, actual.data); + try expectEqual(PayloadTag.in, actual.data.instruction.payload); + + const instr = actual.data.instruction; + try expect_opt_value(expected.delay, instr.delay); + try expect_opt_value(expected.side_set, instr.side_set); + + const in = instr.payload.in; + try expectEqual(expected.source, in.source); + try expectEqual(expected.bit_count, in.bit_count); +} + +const ExpectedOutInstr = struct { + destination: Token.Instruction.Out.Destination, + bit_count: u5, + delay: ?Value = null, + side_set: ?Value = null, +}; + +fn expect_instr_out(expected: ExpectedOutInstr, actual: Token) !void { + try expectEqual(Token.Tag.instruction, actual.data); + try expectEqual(PayloadTag.out, actual.data.instruction.payload); + + const instr = actual.data.instruction; + try expect_opt_value(expected.delay, instr.delay); + try expect_opt_value(expected.side_set, instr.side_set); + + const out = instr.payload.out; + try expectEqual(expected.destination, out.destination); + try expectEqual(expected.bit_count, out.bit_count); +} + +const ExpectedPushInstr = struct { + block: bool = true, + iffull: bool = false, + delay: ?Value = null, + side_set: ?Value = null, +}; + +fn expect_instr_push(expected: ExpectedPushInstr, actual: Token) !void { + try expectEqual(Token.Tag.instruction, actual.data); + try expectEqual(PayloadTag.push, actual.data.instruction.payload); + + const instr = actual.data.instruction; + try expect_opt_value(expected.delay, instr.delay); + try expect_opt_value(expected.side_set, instr.side_set); + + const push = instr.payload.push; + try expectEqual(expected.block, push.block); + try expectEqual(expected.iffull, push.iffull); +} + +const ExpectedPullInstr = struct { + block: bool = true, + ifempty: bool = false, + delay: ?Value = null, + side_set: ?Value = null, +}; + +fn expect_instr_pull(expected: ExpectedPullInstr, actual: Token) !void { + try expectEqual(Token.Tag.instruction, actual.data); + try expectEqual(PayloadTag.pull, actual.data.instruction.payload); + + const instr = actual.data.instruction; + try expect_opt_value(expected.delay, instr.delay); + try expect_opt_value(expected.side_set, instr.side_set); + + const pull = instr.payload.pull; + try expectEqual(expected.block, pull.block); + try expectEqual(expected.ifempty, pull.ifempty); +} + +const ExpectedMovInstr = struct { + source: Token.Instruction.Mov.Source, + destination: Token.Instruction.Mov.Destination, + operation: Token.Instruction.Mov.Operation = .none, + delay: ?Value = null, + side_set: ?Value = null, +}; + +fn expect_instr_mov(expected: ExpectedMovInstr, actual: Token) !void { + try expectEqual(Token.Tag.instruction, actual.data); + try expectEqual(PayloadTag.mov, actual.data.instruction.payload); + + const instr = actual.data.instruction; + try expect_opt_value(expected.delay, instr.delay); + try expect_opt_value(expected.side_set, instr.side_set); + + const mov = instr.payload.mov; + try expectEqual(expected.source, mov.source); + try expectEqual(expected.operation, mov.operation); + try expectEqual(expected.destination, mov.destination); +} + +const ExpectedIrqInstr = struct { + clear: bool, + wait: bool, + num: u5, + rel: bool = false, + delay: ?Value = null, + side_set: ?Value = null, +}; + +fn expect_instr_irq(expected: ExpectedIrqInstr, actual: Token) !void { + try expectEqual(Token.Tag.instruction, actual.data); + try expectEqual(PayloadTag.irq, actual.data.instruction.payload); + + const instr = actual.data.instruction; + try expect_opt_value(expected.delay, instr.delay); + try expect_opt_value(expected.side_set, instr.side_set); + + const irq = instr.payload.irq; + try expectEqual(expected.clear, irq.clear); + try expectEqual(expected.wait, irq.wait); + try expectEqual(expected.rel, irq.rel); +} + +fn bounded_tokenize(source: []const u8) !std.BoundedArray(Token, 256) { + var diags: ?assembler.Diagnostics = null; + return tokenize(source, &diags, .{}) catch |err| if (diags) |d| blk: { + std.log.err("error at index {}: {s}", .{ d.index, d.message.slice() }); + break :blk err; + } else err; +} + +test "tokenize.empty string" { + const tokens = try bounded_tokenize(""); + try expectEqual(@as(usize, 0), tokens.len); +} + +test "tokenize.whitespace" { + const tokens = try bounded_tokenize(" \t\r\n"); + try expectEqual(@as(usize, 0), tokens.len); +} + +test "tokenize.comma line comment" { + const tokens = try bounded_tokenize("; this is a line comment"); + + try expectEqual(@as(usize, 0), tokens.len); +} + +test "tokenize.slash line comment" { + const tokens = try bounded_tokenize("// this is a line comment"); + + try expectEqual(@as(usize, 0), tokens.len); +} + +test "tokenize.block comment" { + const tokens = try bounded_tokenize( + \\/* this is + \\ a block comment */ + ); + + try expectEqual(@as(usize, 0), tokens.len); +} + +test "tokenize.code block" { + const tokens = try bounded_tokenize( + \\% c-sdk { + \\ int foo; + \\%} + ); + + try expectEqual(@as(usize, 0), tokens.len); +} + +test "tokenize.directive.program" { + const tokens = try bounded_tokenize(".program arst"); + try expect_program("arst", tokens.get(0)); +} + +test "tokenize.directive.define" { + const tokens = try bounded_tokenize(".define symbol_name 1"); + + try expect_define(.{ + .name = "symbol_name", + .value = .{ .expression = "1" }, + .index = 8, + }, tokens.get(0)); +} + +test "tokenize.directive.define.public" { + const tokens = try bounded_tokenize(".define public symbol_name 0x1"); + + try expect_define(.{ + .name = "symbol_name", + .value = .{ .expression = "0x1" }, + .public = true, + .index = 15, + }, tokens.get(0)); +} + +test "tokenize.directive.define.with expression" { + const tokens = try bounded_tokenize( + \\.define symbol_name 0x1 + \\.define something (symbol_name * 2) + ); + + try expect_define(.{ + .name = "symbol_name", + .value = .{ .expression = "0x1" }, + .index = 8, + }, tokens.get(0)); + + try expect_define(.{ + .name = "something", + .value = .{ .expression = "(symbol_name * 2)" }, + .index = 32, + }, tokens.get(1)); +} + +test "tokenize.directive.origin" { + const tokens = try bounded_tokenize(".origin 0x10"); + try expect_origin(.{ .integer = 0x10 }, tokens.get(0)); +} + +test "tokenize.directive.side_set" { + const tokens = try bounded_tokenize(".side_set 1"); + try expect_side_set(.{ .count = .{ .integer = 1 } }, tokens.get(0)); +} + +test "tokenize.directive.side_set.opt" { + const tokens = try bounded_tokenize(".side_set 1 opt"); + try expect_side_set(.{ .count = .{ .integer = 1 }, .opt = true }, tokens.get(0)); +} + +test "tokenize.directive.side_set.pindirs" { + const tokens = try bounded_tokenize(".side_set 1 pindirs"); + try expect_side_set(.{ .count = .{ .integer = 1 }, .pindirs = true }, tokens.get(0)); +} + +test "tokenize.directive.wrap_target" { + const tokens = try bounded_tokenize(".wrap_target"); + try expect_wrap_target(tokens.get(0)); +} + +test "tokenize.directive.wrap" { + const tokens = try bounded_tokenize(".wrap"); + try expect_wrap(tokens.get(0)); +} + +test "tokenize.directive.lang_opt" { + const tokens = try bounded_tokenize(".lang_opt c flag foo"); + try expect_lang_opt(.{ .lang = "c", .name = "flag", .option = "foo" }, tokens.get(0)); +} + +test "tokenize.directive.word" { + const tokens = try bounded_tokenize(".word 0xaaaa"); + try expect_word(.{ .integer = 0xaaaa }, tokens.get(0)); +} + +test "tokenize.label" { + const tokens = try bounded_tokenize("my_label:"); + try expect_label(.{ .name = "my_label" }, tokens.get(0)); +} + +test "tokenize.label.public" { + const tokens = try bounded_tokenize("public my_label:"); + try expect_label(.{ .name = "my_label", .public = true }, tokens.get(0)); +} + +test "tokenize.instr.nop" { + const tokens = try bounded_tokenize("nop"); + try expect_instr_nop(.{}, tokens.get(0)); +} + +test "tokenize.instr.jmp.label" { + const tokens = try bounded_tokenize("jmp my_label"); + try expect_instr_jmp(.{ .target = "my_label" }, tokens.get(0)); +} + +test "tokenize.instr.jmp.value" { + const tokens = try bounded_tokenize("jmp 0x2"); + try expect_instr_jmp(.{ .target = "0x2" }, tokens.get(0)); +} + +test "tokenize.instr.jmp.conditions" { + const Condition = Token.Instruction.Jmp.Condition; + const cases = std.ComptimeStringMap(Condition, .{ + .{ "!x", .x_is_zero }, + .{ "x--", .x_dec }, + .{ "!y", .y_is_zero }, + .{ "y--", .y_dec }, + .{ "x!=y", .x_is_not_y }, + .{ "pin", .pin }, + .{ "!osre", .osre_not_empty }, + }); + + inline for (cases.kvs) |case| { + const op = case.key; + const cond = case.value; + const tokens = try bounded_tokenize(std.fmt.comptimePrint("jmp {s} my_label", .{op})); + + try expect_instr_jmp(.{ .cond = cond, .target = "my_label" }, tokens.get(0)); + } +} + +test "tokenize.instr.wait" { + inline for (.{ "gpio", "pin", "irq" }) |source| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("wait 0 {s} 1", .{source})); + try expect_instr_wait(.{ + .polarity = 0, + .source = @field(Token.Instruction.Wait.Source, source), + .num = .{ .integer = 1 }, + }, tokens.get(0)); + } +} + +test "tokenize.instr.wait.irq.rel" { + const tokens = try bounded_tokenize("wait 1 irq 1 rel"); + try expect_instr_wait(.{ + .polarity = 1, + .source = .irq, + .num = .{ .integer = 1 }, + .rel = true, + }, tokens.get(0)); +} + +test "tokenize.instr.in" { + inline for (.{ + "pins", + "x", + "y", + "null", + "isr", + "osr", + }, 1..) |source, bit_count| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("in {s}, {}", .{ + source, + bit_count, + })); + + try expect_instr_in(.{ + .source = @field(Token.Instruction.In.Source, source), + .bit_count = @intCast(u5, bit_count), + }, tokens.get(0)); + } +} + +test "tokenize.instr.out" { + inline for (.{ + "pins", + "x", + "y", + "null", + "pindirs", + "pc", + "isr", + "exec", + }, 1..) |destination, bit_count| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("out {s}, {}", .{ + destination, + bit_count, + })); + + try expect_instr_out(.{ + .destination = @field(Token.Instruction.Out.Destination, destination), + .bit_count = @intCast(u5, bit_count), + }, tokens.get(0)); + } +} + +test "tokenize.instr.push" { + const tokens = try bounded_tokenize("push"); + try expect_instr_push(.{}, tokens.get(0)); +} + +test "tokenize.instr.push.block" { + const tokens = try bounded_tokenize("push block"); + try expect_instr_push(.{ + .block = true, + }, tokens.get(0)); +} + +test "tokenize.instr.push.noblock" { + const tokens = try bounded_tokenize("push noblock"); + try expect_instr_push(.{ + .block = false, + }, tokens.get(0)); +} + +test "tokenize.instr.push.iffull" { + const tokens = try bounded_tokenize("push iffull noblock"); + try expect_instr_push(.{ + .block = false, + .iffull = true, + }, tokens.get(0)); +} + +test "tokenize.instr.pull" { + const tokens = try bounded_tokenize("pull"); + try expect_instr_pull(.{}, tokens.get(0)); +} + +test "tokenize.instr.pull.block" { + const tokens = try bounded_tokenize("pull block"); + try expect_instr_pull(.{ + .block = true, + }, tokens.get(0)); +} + +test "tokenize.instr.pull.noblock" { + const tokens = try bounded_tokenize("pull noblock"); + try expect_instr_pull(.{ + .block = false, + }, tokens.get(0)); +} + +test "tokenize.instr.pull.ifempty" { + const tokens = try bounded_tokenize("pull ifempty noblock"); + try expect_instr_pull(.{ + .block = false, + .ifempty = true, + }, tokens.get(0)); +} + +test "tokenize.instr.mov" { + inline for (.{ + "pins", + "x", + "y", + "null", + "status", + "isr", + "osr", + }) |source| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("mov x {s}", .{source})); + + try expect_instr_mov(.{ + .source = @field(Token.Instruction.Mov.Source, source), + .destination = .x, + }, tokens.get(0)); + } + + inline for (.{ + "pins", + "x", + "y", + "exec", + "pc", + "isr", + "osr", + }) |dest| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("mov {s} x", .{dest})); + + try expect_instr_mov(.{ + .source = .x, + .destination = @field(Token.Instruction.Mov.Destination, dest), + }, tokens.get(0)); + } + + const Operation = Token.Instruction.Mov.Operation; + const operations = std.ComptimeStringMap(Operation, .{ + .{ "!", .invert }, + .{ "~", .invert }, + .{ "::", .bit_reverse }, + }); + + inline for (.{ "", " " }) |space| { + inline for (operations.kvs) |kv| { + const str = kv.key; + const operation = kv.value; + const tokens = try bounded_tokenize(std.fmt.comptimePrint("mov x {s}{s}y", .{ + str, + space, + })); + + try expect_instr_mov(.{ + .destination = .x, + .operation = operation, + .source = .y, + }, tokens.get(0)); + } + } +} + +test "tokenize.instr.irq" { + const ClearWait = struct { + clear: bool, + wait: bool, + }; + + const modes = std.ComptimeStringMap(ClearWait, .{ + .{ "", .{ .clear = false, .wait = false } }, + .{ "set", .{ .clear = false, .wait = false } }, + .{ "nowait", .{ .clear = false, .wait = false } }, + .{ "wait", .{ .clear = false, .wait = true } }, + .{ "clear", .{ .clear = true, .wait = false } }, + }); + + inline for (modes.kvs, 0..) |kv, num| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("irq {s} {}", .{ + kv.key, + num, + })); + + try expect_instr_irq(.{ + .clear = kv.value.clear, + .wait = kv.value.wait, + .num = num, + }, tokens.get(0)); + } +} + +test "tokenize.instr.irq.rel" { + const tokens = try bounded_tokenize("irq set 2 rel"); + try expect_instr_irq(.{ + .clear = false, + .wait = false, + .num = 2, + .rel = true, + }, tokens.get(0)); +} + +test "tokenize.instr.set" { + inline for (.{ + "pins", + "x", + "y", + "pindirs", + }) |dest| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("set {s}, 2", .{dest})); + try expect_instr_set(.{ + .dest = @field(Token.Instruction.Set.Destination, dest), + .value = .{ .integer = 2 }, + }, tokens.get(0)); + } +} + +test "tokenize.instr.set.with expression including define" { + const tokens = try bounded_tokenize("set X, (NUM_CYCLES - 1) ; initialise the loop counter"); + try expect_instr_set(.{ + .dest = .x, + .value = .{ .expression = "(NUM_CYCLES - 1)" }, + }, tokens.get(0)); +} + +const instruction_examples = .{ + "nop", + "jmp arst", + "wait 0 gpio 1", + "in pins, 2", + "out pc, 1", + "push", + "pull", + "mov x y", + "irq 1", + "set pins 2", +}; + +test "tokenize.instr.label prefixed" { + inline for (instruction_examples) |instr| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("my_label: {s}", .{instr})); + try expectEqual(@as(usize, 2), tokens.len); + try expect_label(.{ .name = "my_label" }, tokens.get(0)); + } +} + +test "tokenize.instr.side_set" { + inline for (instruction_examples) |instr| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("{s} side 0", .{instr})); + const token = tokens.get(0); + try expect_value(.{ + .expression = "0", + }, token.data.instruction.side_set.?); + try expectEqual(@as(?Value, null), token.data.instruction.delay); + } +} + +test "tokenize.instr.delay" { + inline for (instruction_examples) |instr| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("{s} [1]", .{instr})); + const token = tokens.get(0); + try expectEqual(@as(?Value, null), token.data.instruction.side_set); + try expect_value(.{ + .expression = "1", + }, token.data.instruction.delay.?); + } +} + +test "tokenize.instr.delay.expression" { + inline for (instruction_examples) |instr| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("{s} [T-1]", .{instr})); + const token = tokens.get(0); + try expectEqual(@as(?Value, null), token.data.instruction.side_set); + try expect_value(.{ + .expression = "T-1", + }, token.data.instruction.delay.?); + } +} + +test "tokenize.instr.side_set.expression" { + inline for (instruction_examples) |instr| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("{s} side (N-1)", .{instr})); + const token = tokens.get(0); + try expect_value(.{ + .expression = "(N-1)", + }, token.data.instruction.side_set.?); + try expectEqual(@as(?Value, null), token.data.instruction.delay); + } +} + +test "tokenize.instr.side_set and delay" { + inline for (instruction_examples) |instr| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("{s} side 1 [2]", .{instr})); + const token = tokens.get(0); + try expect_value(.{ + .expression = "1", + }, token.data.instruction.side_set.?); + try expect_value(.{ + .expression = "2", + }, token.data.instruction.delay.?); + } +} + +test "tokenize.instr.side_set and delay reversed" { + inline for (instruction_examples) |instr| { + const tokens = try bounded_tokenize(std.fmt.comptimePrint("{s} [2] side 1", .{instr})); + const token = tokens.get(0); + try expect_value(.{ + .expression = "1", + }, token.data.instruction.side_set.?); + try expect_value(.{ + .expression = "2", + }, token.data.instruction.delay.?); + } +} + +test "tokenize.instr.comment with no whitespace" { + const tokens = try bounded_tokenize("nop side 0x0 [1]; CSn front porch"); + try expect_instr_nop(.{ + .side_set = .{ .expression = "0x0" }, + .delay = .{ .expression = "1" }, + }, tokens.get(0)); +}