ps2_vendor.c (8079B)
1 // Copyright 2022 Marek Kraus (@gamelaster) 2 // SPDX-License-Identifier: GPL-2.0-or-later 3 4 #include "gpio.h" 5 #include "hardware/pio.h" 6 #include "hardware/clocks.h" 7 #include "ps2.h" 8 #include "debug.h" 9 10 #if !defined(MCU_RP) 11 # error PIO Driver is only available for Raspberry Pi 2040 MCUs! 12 #endif 13 14 #if defined(PS2_ENABLE) 15 # if defined(PS2_MOUSE_ENABLE) 16 # if !defined(PS2_MOUSE_USE_REMOTE_MODE) 17 # define BUFFERED_MODE_ENABLE 18 # endif 19 # else // PS2 Keyboard 20 # define BUFFERED_MODE_ENABLE 21 # endif 22 #endif 23 24 #if PS2_DATA_PIN + 1 != PS2_CLOCK_PIN 25 # error PS/2 clock pin must be data pin + 1! 26 #endif 27 28 static inline void pio_serve_interrupt(void); 29 30 #if defined(PS2_PIO_USE_PIO1) 31 static const PIO pio = pio1; 32 33 OSAL_IRQ_HANDLER(RP_PIO1_IRQ_0_HANDLER) { 34 OSAL_IRQ_PROLOGUE(); 35 pio_serve_interrupt(); 36 OSAL_IRQ_EPILOGUE(); 37 } 38 #else 39 static const PIO pio = pio0; 40 41 OSAL_IRQ_HANDLER(RP_PIO0_IRQ_0_HANDLER) { 42 OSAL_IRQ_PROLOGUE(); 43 pio_serve_interrupt(); 44 OSAL_IRQ_EPILOGUE(); 45 } 46 #endif 47 48 #define PS2_WRAP_TARGET 0 49 #define PS2_WRAP 20 50 51 // clang-format off 52 static const uint16_t ps2_program_instructions[] = { 53 // .wrap_target 54 0x00c7, // 0: jmp pin, 7 55 0xe02a, // 1: set x, 10 56 0x2021, // 2: wait 0 pin, 1 57 0x4001, // 3: in pins, 1 58 0x20a1, // 4: wait 1 pin, 1 59 0x0042, // 5: jmp x--, 2 60 0x0000, // 6: jmp 0 61 0x00e9, // 7: jmp !osre, 9 62 0x0000, // 8: jmp 0 63 0xff81, // 9: set pindirs, 1 [31] 64 0xe280, // 10: set pindirs, 0 [2] 65 0xe082, // 11: set pindirs, 2 66 0x2021, // 12: wait 0 pin, 1 67 0xe029, // 13: set x, 9 68 0x6081, // 14: out pindirs, 1 69 0x20a1, // 15: wait 1 pin, 1 70 0x2021, // 16: wait 0 pin, 1 71 0x004e, // 17: jmp x--, 14 72 0xe083, // 18: set pindirs, 3 73 0x2021, // 19: wait 0 pin, 1 74 0x20a1, // 20: wait 1 pin, 1 75 // .wrap 76 }; 77 // clang-format on 78 79 static const struct pio_program ps2_program = { 80 .instructions = ps2_program_instructions, 81 .length = 21, 82 .origin = -1, 83 }; 84 85 static int state_machine = -1; 86 static thread_reference_t tx_thread = NULL; 87 88 #define BUFFER_SIZE 32 89 static input_buffers_queue_t pio_rx_queue; 90 static __attribute__((aligned(4))) uint8_t pio_rx_buffer[BQ_BUFFER_SIZE(BUFFER_SIZE, sizeof(uint32_t))]; 91 92 uint8_t ps2_error = PS2_ERR_NONE; 93 94 void pio_serve_interrupt(void) { 95 uint32_t irqs = pio->ints0; 96 97 if (irqs & (PIO_IRQ0_INTF_SM0_RXNEMPTY_BITS << state_machine)) { 98 osalSysLockFromISR(); 99 uint32_t* frame_buffer = (uint32_t*)ibqGetEmptyBufferI(&pio_rx_queue); 100 if (frame_buffer == NULL) { 101 osalSysUnlockFromISR(); 102 return; 103 } 104 *frame_buffer = pio_sm_get(pio, state_machine); 105 ibqPostFullBufferI(&pio_rx_queue, sizeof(uint32_t)); 106 osalSysUnlockFromISR(); 107 } 108 109 if (irqs & (PIO_IRQ0_INTF_SM0_TXNFULL_BITS << state_machine)) { 110 pio_set_irq0_source_enabled(pio, pis_sm0_tx_fifo_not_full + state_machine, false); 111 osalSysLockFromISR(); 112 osalThreadResumeI(&tx_thread, MSG_OK); 113 osalSysUnlockFromISR(); 114 } 115 } 116 117 void ps2_host_init(void) { 118 ibqObjectInit(&pio_rx_queue, false, pio_rx_buffer, sizeof(uint32_t), BUFFER_SIZE, NULL, NULL); 119 uint pio_idx = pio_get_index(pio); 120 121 hal_lld_peripheral_unreset(pio_idx == 0 ? RESETS_ALLREG_PIO0 : RESETS_ALLREG_PIO1); 122 123 state_machine = pio_claim_unused_sm(pio, true); 124 if (state_machine < 0) { 125 dprintln("ERROR: Failed to acquire state machine for PS/2!"); 126 ps2_error = PS2_ERR_NODATA; 127 return; 128 } 129 130 uint offset = pio_add_program(pio, &ps2_program); 131 132 pio_sm_config c = pio_get_default_sm_config(); 133 sm_config_set_wrap(&c, offset + PS2_WRAP_TARGET, offset + PS2_WRAP); 134 135 // Set pindirs to input (output enable is inverted below) 136 pio_sm_set_consecutive_pindirs(pio, state_machine, PS2_DATA_PIN, 2, true); 137 sm_config_set_clkdiv(&c, (float)clock_get_hz(clk_sys) / (200.0f * KHZ)); 138 sm_config_set_set_pins(&c, PS2_DATA_PIN, 2); 139 sm_config_set_out_pins(&c, PS2_DATA_PIN, 1); 140 sm_config_set_out_shift(&c, true, true, 10); 141 sm_config_set_in_shift(&c, true, true, 11); 142 sm_config_set_jmp_pin(&c, PS2_CLOCK_PIN); 143 sm_config_set_in_pins(&c, PS2_DATA_PIN); 144 145 // clang-format off 146 iomode_t pin_mode = PAL_RP_PAD_IE | 147 PAL_RP_GPIO_OE | 148 PAL_RP_PAD_SLEWFAST | 149 PAL_RP_PAD_DRIVE12 | 150 // Invert output enable so that pindirs=1 means input 151 // and indirs=0 means output. This way, out pindirs 152 // works correctly with the open-drain PS/2 interface. 153 // Setting pindirs=1 effectively pulls the line high, 154 // due to the pull-up resistor, while pindirs=0 pulls 155 // the line low. 156 PAL_RP_IOCTRL_OEOVER_DRVINVPERI | 157 (pio_idx == 0 ? PAL_MODE_ALTERNATE_PIO0 : PAL_MODE_ALTERNATE_PIO1); 158 // clang-format on 159 160 palSetLineMode(PS2_DATA_PIN, pin_mode); 161 palSetLineMode(PS2_CLOCK_PIN, pin_mode); 162 163 pio_set_irq0_source_enabled(pio, pis_sm0_rx_fifo_not_empty + state_machine, true); 164 pio_sm_init(pio, state_machine, offset, &c); 165 166 #if defined(PS2_PIO_USE_PIO1) 167 nvicEnableVector(RP_PIO1_IRQ_0_NUMBER, CORTEX_MAX_KERNEL_PRIORITY); 168 #else 169 nvicEnableVector(RP_PIO0_IRQ_0_NUMBER, CORTEX_MAX_KERNEL_PRIORITY); 170 #endif 171 172 pio_sm_set_enabled(pio, state_machine, true); 173 } 174 175 static int bit_parity(int x) { 176 return !__builtin_parity(x); 177 } 178 179 uint8_t ps2_host_send(uint8_t data) { 180 uint32_t frame = 0b1000000000; 181 frame = frame | data; 182 183 if (bit_parity(data)) { 184 frame = frame | (1 << 8); 185 } 186 187 pio_sm_put(pio, state_machine, frame); 188 189 msg_t msg = MSG_OK; 190 osalSysLock(); 191 while (pio_sm_is_tx_fifo_full(pio, state_machine)) { 192 pio_set_irq0_source_enabled(pio, pis_sm0_tx_fifo_not_full + state_machine, true); 193 msg = osalThreadSuspendTimeoutS(&tx_thread, TIME_MS2I(100)); 194 if (msg < MSG_OK) { 195 pio_set_irq0_source_enabled(pio, pis_sm0_tx_fifo_not_full + state_machine, false); 196 ps2_error = PS2_ERR_NODATA; 197 osalSysUnlock(); 198 return 0; 199 } 200 } 201 osalSysUnlock(); 202 203 return ps2_host_recv_response(); 204 } 205 206 static uint8_t ps2_get_data_from_frame(uint32_t frame) { 207 uint8_t data = (frame >> 22) & 0xFF; 208 uint32_t start_bit = (frame & 0b00000000001000000000000000000000) ? 1 : 0; 209 uint32_t parity_bit = (frame & 0b01000000000000000000000000000000) ? 1 : 0; 210 uint32_t stop_bit = (frame & 0b10000000001000000000000000000000) ? 1 : 0; 211 212 if (start_bit != 0) { 213 ps2_error = PS2_ERR_STARTBIT1; 214 return 0; 215 } 216 217 if (parity_bit != bit_parity(data)) { 218 ps2_error = PS2_ERR_PARITY; 219 return 0; 220 } 221 222 if (stop_bit != 1) { 223 ps2_error = PS2_ERR_STARTBIT2; 224 return 0; 225 } 226 227 return data; 228 } 229 230 uint8_t ps2_host_recv_response(void) { 231 uint32_t frame = 0; 232 msg_t msg = MSG_OK; 233 234 msg = ibqReadTimeout(&pio_rx_queue, (uint8_t*)&frame, sizeof(uint32_t), TIME_MS2I(100)); 235 if (msg < MSG_OK) { 236 ps2_error = PS2_ERR_NODATA; 237 return 0; 238 } 239 240 return ps2_get_data_from_frame(frame); 241 } 242 243 #ifdef BUFFERED_MODE_ENABLE 244 245 bool pbuf_has_data(void) { 246 osalSysLock(); 247 bool has_data = !ibqIsEmptyI(&pio_rx_queue); 248 osalSysUnlock(); 249 return has_data; 250 } 251 252 uint8_t ps2_host_recv(void) { 253 uint32_t frame = 0; 254 msg_t msg = MSG_OK; 255 256 uint8_t has_data = pbuf_has_data(); 257 if (has_data) { 258 msg = ibqReadTimeout(&pio_rx_queue, (uint8_t*)&frame, sizeof(uint32_t), TIME_MS2I(100)); 259 if (msg < MSG_OK) { 260 ps2_error = PS2_ERR_NODATA; 261 return 0; 262 } 263 } else { 264 ps2_error = PS2_ERR_NODATA; 265 } 266 267 return frame != 0 ? ps2_get_data_from_frame(frame) : 0; 268 } 269 270 #endif