|
| 1 | +/* |
| 2 | + * This file is part of the Micro Python project, http://micropython.org/ |
| 3 | + * |
| 4 | + * The MIT License (MIT) |
| 5 | + * |
| 6 | + * Copyright (c) 2024 Jeff Epler for Adafruit Industries |
| 7 | + * |
| 8 | + * Permission is hereby granted, free of charge, to any person obtaining a copy |
| 9 | + * of this software and associated documentation files (the "Software"), to deal |
| 10 | + * in the Software without restriction, including without limitation the rights |
| 11 | + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
| 12 | + * copies of the Software, and to permit persons to whom the Software is |
| 13 | + * furnished to do so, subject to the following conditions: |
| 14 | + * |
| 15 | + * The above copyright notice and this permission notice shall be included in |
| 16 | + * all copies or substantial portions of the Software. |
| 17 | + * |
| 18 | + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
| 19 | + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
| 20 | + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
| 21 | + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
| 22 | + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| 23 | + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
| 24 | + * THE SOFTWARE. |
| 25 | + */ |
| 26 | + |
| 27 | +#include "bindings/rp2pio/StateMachine.h" |
| 28 | +#include "py/runtime.h" |
| 29 | +#include "shared-bindings/digitalio/DigitalInOut.h" |
| 30 | +#include "shared-bindings/floppyio/__init__.h" |
| 31 | +#include "common-hal/floppyio/__init__.h" |
| 32 | +#include "shared-bindings/time/__init__.h" |
| 33 | +#include "supervisor/shared/tick.h" |
| 34 | + |
| 35 | +static const uint16_t fluxread_program[] = { |
| 36 | + // ; Count flux pulses and watch for index pin |
| 37 | + // ; flux input is the 'jmp pin'. index is "pin zero". |
| 38 | + // ; Counts are in units 3 / F_pio, so e.g., at 30MHz 1 count = 0.1us |
| 39 | + // ; Count down while waiting for the counter to go HIGH |
| 40 | + // ; The only counting is down, so C code will just have to negate the |
| 41 | + // count! |
| 42 | + // ; Each 'wait one' loop takes 3 instruction-times |
| 43 | + // wait_one: |
| 44 | + 0x0041, // jmp x--, wait_one_next ; acts as a non-conditional decrement |
| 45 | + // of x |
| 46 | + // wait_one_next: |
| 47 | + 0x00c3, // jmp pin wait_zero |
| 48 | + 0x0000, // jmp wait_one |
| 49 | + // ; Each 'wait zero' loop takes 3 instruction-times, needing one |
| 50 | + // instruction delay |
| 51 | + // ; (it has to match the 'wait one' timing exactly) |
| 52 | + // wait_zero: |
| 53 | + 0x0044, // jmp x--, wait_zero_next ; acts as a non-conditional decrement |
| 54 | + // of x |
| 55 | + // wait_zero_next: |
| 56 | + 0x01c3, // jmp pin wait_zero [1] |
| 57 | + // ; Top bit is index status, bottom 15 bits are inverse of counts |
| 58 | + // ; Combined FIFO gives 16 entries (8 32-bit entries) so with the |
| 59 | + // ; smallest plausible pulse of 2us there are 250 CPU cycles available |
| 60 | + // @125MHz |
| 61 | + 0x4001, // in pins, 1 |
| 62 | + 0x402f, // in x, 15 |
| 63 | + // ; Three cycles for the end of loop, so we need to decrement x to make |
| 64 | + // everything |
| 65 | + // ; come out right. This has constant timing whether we actually jump back |
| 66 | + // vs wrapping. |
| 67 | + 0x0040, // jmp x--, wait_one |
| 68 | +}; |
| 69 | + |
| 70 | +typedef struct { |
| 71 | + PIO pio; |
| 72 | + uint8_t sm; |
| 73 | + bool word_available; |
| 74 | + uint16_t half; |
| 75 | +} floppy_reader; |
| 76 | + |
| 77 | +static bool data_available(floppy_reader *reader) { |
| 78 | + return reader->word_available || !pio_sm_is_rx_fifo_empty(reader->pio, reader->sm); |
| 79 | +} |
| 80 | + |
| 81 | +static uint16_t read_fifo(floppy_reader *reader) { |
| 82 | + if (reader->word_available) { |
| 83 | + reader->word_available = false; |
| 84 | + return reader->half; |
| 85 | + } |
| 86 | + uint32_t value = pio_sm_get_blocking(reader->pio, reader->sm); |
| 87 | + reader->half = value >> 16; |
| 88 | + reader->word_available = true; |
| 89 | + return value & 0xffff; |
| 90 | +} |
| 91 | + |
| 92 | + |
| 93 | +int common_hal_floppyio_flux_readinto(void *buf, size_t len, digitalio_digitalinout_obj_t *data, digitalio_digitalinout_obj_t *index, mp_int_t index_wait_ms) { |
| 94 | +#define READ_INDEX() (!!(*index_port & index_mask)) |
| 95 | + uint32_t index_mask; |
| 96 | + volatile uint32_t *index_port = common_hal_digitalio_digitalinout_get_reg(index, DIGITALINOUT_REG_READ, &index_mask); |
| 97 | + |
| 98 | + memset(buf, 0, len); |
| 99 | + |
| 100 | + uint32_t pins_we_use = 1 << data->pin->number; |
| 101 | + |
| 102 | + rp2pio_statemachine_obj_t state_machine; |
| 103 | + bool ok = rp2pio_statemachine_construct(&state_machine, |
| 104 | + fluxread_program, MP_ARRAY_SIZE(fluxread_program), |
| 105 | + FLOPPYIO_SAMPLERATE * 3, // 3 PIO cycles per sample count |
| 106 | + NULL, 0, // init program |
| 107 | + NULL, 0, // out |
| 108 | + index->pin, 1, // in |
| 109 | + 1, 0, // in pull up/down |
| 110 | + NULL, 0, // set |
| 111 | + NULL, 0, // sideset |
| 112 | + 0, 0, // initial pin state |
| 113 | + data->pin, // jump pin |
| 114 | + pins_we_use, false, true, |
| 115 | + true, 32, false, // TX setting we don't use |
| 116 | + true, // Wait for txstall. If we don't, then we'll deinit too quickly. |
| 117 | + true, 32, true, // move 32 bits at a time |
| 118 | + false, // claim pins |
| 119 | + false, // Not user-interruptible. |
| 120 | + false, // No sideset enable |
| 121 | + 0, -1, // wrap |
| 122 | + PIO_ANY_OFFSET // offset |
| 123 | + ); |
| 124 | + if (!ok) { |
| 125 | + mp_raise_RuntimeError(MP_ERROR_TEXT("All state machines in use")); |
| 126 | + } |
| 127 | + |
| 128 | + floppy_reader reader = { .pio = state_machine.pio, .sm = state_machine.state_machine, }; |
| 129 | + |
| 130 | + uint8_t *ptr = buf, *end = ptr + len; |
| 131 | + |
| 132 | + uint64_t index_deadline_us = time_us_64() + index_wait_ms * 1000; |
| 133 | + |
| 134 | + common_hal_mcu_disable_interrupts(); |
| 135 | + |
| 136 | + // check if flux is arriving |
| 137 | + uint64_t flux_deadline_us = time_us_64() + 20; |
| 138 | + while (pio_sm_is_rx_fifo_empty(reader.pio, reader.sm)) { |
| 139 | + if (time_us_64() > flux_deadline_us) { |
| 140 | + common_hal_mcu_enable_interrupts(); |
| 141 | + common_hal_rp2pio_statemachine_deinit(&state_machine); |
| 142 | + mp_raise_RuntimeError(MP_ERROR_TEXT("timeout waiting for flux")); |
| 143 | + } |
| 144 | + } |
| 145 | + |
| 146 | + // wait for index pulse low |
| 147 | + while (READ_INDEX()) { |
| 148 | + if (time_us_64() > index_deadline_us) { |
| 149 | + common_hal_mcu_enable_interrupts(); |
| 150 | + common_hal_rp2pio_statemachine_deinit(&state_machine); |
| 151 | + mp_raise_RuntimeError(MP_ERROR_TEXT("timeout waiting for index pulse")); |
| 152 | + } |
| 153 | + } |
| 154 | + |
| 155 | + pio_sm_clear_fifos(reader.pio, reader.sm); |
| 156 | + |
| 157 | + // if another index doesn't show up ... |
| 158 | + index_deadline_us = time_us_64() + index_wait_ms * 1000; |
| 159 | + |
| 160 | + int last = read_fifo(&reader); |
| 161 | + bool last_index = READ_INDEX(); |
| 162 | + while (ptr != end) { |
| 163 | + |
| 164 | + /* Handle index */ |
| 165 | + bool now_index = READ_INDEX(); |
| 166 | + |
| 167 | + if (!now_index && last_index) { |
| 168 | + break; |
| 169 | + } |
| 170 | + last_index = now_index; |
| 171 | + |
| 172 | + if (!data_available(&reader)) { |
| 173 | + // no flux is arriving? is ANY flux arriving or has a full revoulution gone by? |
| 174 | + if (time_us_64() > index_deadline_us) { |
| 175 | + break; |
| 176 | + } |
| 177 | + continue; |
| 178 | + } |
| 179 | + |
| 180 | + int timestamp = read_fifo(&reader); |
| 181 | + int delta = last - timestamp; |
| 182 | + if (delta < 0) { |
| 183 | + delta += 65536; |
| 184 | + } |
| 185 | + delta /= 2; |
| 186 | + |
| 187 | + last = timestamp; |
| 188 | + *ptr++ = delta > 255 ? 255 : delta; |
| 189 | + } |
| 190 | + |
| 191 | + common_hal_mcu_enable_interrupts(); |
| 192 | + common_hal_rp2pio_statemachine_deinit(&state_machine); |
| 193 | + |
| 194 | + return ptr - (uint8_t *)buf; |
| 195 | +} |
0 commit comments