GCC Code Coverage Report


Directory: main/
File: adapter/wired/ps.c
Date: 2025-10-04 14:03:00
Exec Total Coverage
Lines: 0 172 0.0%
Functions: 0 9 0.0%
Branches: 0 89 0.0%

Line Branch Exec Source
1 /*
2 * Copyright (c) 2019-2025, Jacques Gagnon
3 * SPDX-License-Identifier: Apache-2.0
4 */
5
6 #include <string.h>
7 #include "zephyr/types.h"
8 #include "tools/util.h"
9 #include "adapter/config.h"
10 #include "adapter/kb_monitor.h"
11 #include "adapter/wired/wired.h"
12 #include "tests/cmds.h"
13 #include "bluetooth/mon.h"
14 #include "ps.h"
15
16 #define PS_JOYSTICK_AXES_CNT 4
17
18 enum {
19 PS_SELECT = 0,
20 PS_L3,
21 PS_R3,
22 PS_START,
23 PS_D_UP,
24 PS_D_RIGHT,
25 PS_D_DOWN,
26 PS_D_LEFT,
27 PS_L2,
28 PS_R2,
29 PS_L1,
30 PS_R1,
31 PS_T,
32 PS_C,
33 PS_X,
34 PS_S,
35 };
36
37 static DRAM_ATTR const uint8_t ps_axes_idx[ADAPTER_PS2_MAX_AXES] =
38 {
39 /* AXIS_LX, AXIS_LY, AXIS_RX, AXIS_RY, TRIG_L, TRIG_R */
40 2, 3, 0, 1, 14, 15,
41 /* TRIG_LS, TRIG_RS, DPAD_L, DPAD_R, DPAD_D, DPAD_U */
42 12, 13, 5, 4, 7, 6,
43 /* BTN_L, BTN_R, BTN_D, BTN_U */
44 11, 9, 10, 8
45 };
46
47 static DRAM_ATTR const uint8_t ps_mouse_axes_idx[ADAPTER_MAX_AXES] =
48 {
49 /* AXIS_LX, AXIS_LY, AXIS_RX, AXIS_RY, TRIG_L, TRIG_R */
50 0, 1, 0, 1, 0, 1
51 };
52
53 static DRAM_ATTR const struct ctrl_meta ps_axes_meta[ADAPTER_PS2_MAX_AXES] =
54 {
55 {.size_min = -128, .size_max = 127, .neutral = 0x80, .abs_max = 0xB0, .abs_min = 0xB0},
56 {.size_min = -128, .size_max = 127, .neutral = 0x80, .abs_max = 0xB0, .abs_min = 0xB0, .polarity = 1},
57 {.size_min = -128, .size_max = 127, .neutral = 0x80, .abs_max = 0xB0, .abs_min = 0xB0},
58 {.size_min = -128, .size_max = 127, .neutral = 0x80, .abs_max = 0xB0, .abs_min = 0xB0, .polarity = 1},
59 {.size_min = 0, .size_max = 255, .neutral = 0x00, .abs_max = 0x100, .abs_min = 0x00},
60 {.size_min = 0, .size_max = 255, .neutral = 0x00, .abs_max = 0x100, .abs_min = 0x00},
61 {.size_min = 0, .size_max = 255, .neutral = 0x00, .abs_max = 0x100, .abs_min = 0x00},
62 {.size_min = 0, .size_max = 255, .neutral = 0x00, .abs_max = 0x100, .abs_min = 0x00},
63 {.size_min = 0, .size_max = 255, .neutral = 0x00, .abs_max = 0x100, .abs_min = 0x00},
64 {.size_min = 0, .size_max = 255, .neutral = 0x00, .abs_max = 0x100, .abs_min = 0x00},
65 {.size_min = 0, .size_max = 255, .neutral = 0x00, .abs_max = 0x100, .abs_min = 0x00},
66 {.size_min = 0, .size_max = 255, .neutral = 0x00, .abs_max = 0x100, .abs_min = 0x00},
67 {.size_min = 0, .size_max = 255, .neutral = 0x00, .abs_max = 0x100, .abs_min = 0x00},
68 {.size_min = 0, .size_max = 255, .neutral = 0x00, .abs_max = 0x100, .abs_min = 0x00},
69 {.size_min = 0, .size_max = 255, .neutral = 0x00, .abs_max = 0x100, .abs_min = 0x00},
70 {.size_min = 0, .size_max = 255, .neutral = 0x00, .abs_max = 0x100, .abs_min = 0x00},
71 };
72
73 static DRAM_ATTR const struct ctrl_meta ps_mouse_axes_meta[ADAPTER_MAX_AXES] =
74 {
75 {.size_min = -128, .size_max = 127, .neutral = 0x00, .abs_max = 127, .abs_min = 128},
76 {.size_min = -128, .size_max = 127, .neutral = 0x00, .abs_max = 127, .abs_min = 128, .polarity = 1},
77 {.size_min = -128, .size_max = 127, .neutral = 0x00, .abs_max = 127, .abs_min = 128},
78 {.size_min = -128, .size_max = 127, .neutral = 0x00, .abs_max = 127, .abs_min = 128, .polarity = 1},
79 {.size_min = -128, .size_max = 127, .neutral = 0x00, .abs_max = 127, .abs_min = 128},
80 {.size_min = -128, .size_max = 127, .neutral = 0x00, .abs_max = 127, .abs_min = 128},
81 };
82
83 struct ps_map {
84 uint16_t buttons;
85 union {
86 uint8_t axes[16];
87 struct {
88 uint32_t sticks;
89 uint8_t pressure[12];
90 };
91 };
92 uint8_t analog_btn;
93 } __packed;
94
95 struct ps_mouse_map {
96 uint16_t buttons;
97 uint8_t relative[2];
98 int32_t raw_axes[2];
99 } __packed;
100
101 #ifndef CONFIG_BLUERETRO_ADAPTER_INPUT_MAP_DBG
102 static const uint32_t ps_mask[4] = {0xBB7F0FFF, 0x00000000, 0x00000000, BR_COMBO_MASK};
103 static const uint32_t ps_desc[4] = {0x330F0FFF, 0x00000000, 0x00000000, 0x00000000};
104 #else
105 static const uint32_t ps_mask[4] = {0xFFFFFFFF, 0x00000000, 0x00000000, BR_COMBO_MASK};
106 static const uint32_t ps_desc[4] = {0x110000FF, 0x00000000, 0x00000000, 0x00000000};
107 #endif
108 static DRAM_ATTR const uint32_t ps_btns_mask[32] = {
109 0, 0, 0, 0,
110 0, 0, 0, 0,
111 BIT(PS_D_LEFT), BIT(PS_D_RIGHT), BIT(PS_D_DOWN), BIT(PS_D_UP),
112 0, 0, 0, 0,
113 BIT(PS_S), BIT(PS_C), BIT(PS_X), BIT(PS_T),
114 BIT(PS_START), BIT(PS_SELECT), 0, 0,
115 BIT(PS_L2), BIT(PS_L1), 0, BIT(PS_L3),
116 BIT(PS_R2), BIT(PS_R1), 0, BIT(PS_R3),
117 };
118 static DRAM_ATTR const uint8_t ps_btns_idx[32] = {
119 0, 0, 0, 0,
120 0, 0, 0, 0,
121 5, 4, 7, 6,
122 0, 0, 0, 0,
123 11, 9, 10, 8,
124 0, 0, 0, 0,
125 14, 12, 0, 0,
126 15, 13, 0, 0,
127 };
128
129 static const uint32_t ps_mouse_mask[4] = {0x110000F0, 0x00000000, 0x00000000, BR_COMBO_MASK};
130 static const uint32_t ps_mouse_desc[4] = {0x000000F0, 0x00000000, 0x00000000, 0x00000000};
131 static const uint32_t ps_mouse_btns_mask[32] = {
132 0, 0, 0, 0,
133 0, 0, 0, 0,
134 0, 0, 0, 0,
135 0, 0, 0, 0,
136 0, 0, 0, 0,
137 0, 0, 0, 0,
138 BIT(PS_L1), 0, 0, 0,
139 BIT(PS_R1), 0, 0, 0,
140 };
141
142 static const uint32_t ps_kb_mask[4] = {0xE6FF0F0F, 0xFFFFFFFF, 0xFFFFFFFF, 0x0007FFFF | BR_COMBO_MASK};
143 static const uint32_t ps_kb_desc[4] = {0x00000000, 0x00000000, 0x00000000, 0x00000000};
144 static const uint8_t ps_kb_scancode[KBM_MAX] = {
145 /* KB_A, KB_D, KB_S, KB_W, MOUSE_X_LEFT, MOUSE_X_RIGHT, MOUSE_Y_DOWN MOUSE_Y_UP */
146 0x1C, 0x23, 0x1B, 0x1D, 0x00, 0x00, 0x00, 0x00,
147 /* KB_LEFT, KB_RIGHT, KB_DOWN, KB_UP, MOUSE_WX_LEFT, MOUSE_WX_RIGHT, MOUSE_WY_DOWN, MOUSE_WY_UP */
148 0x86, 0x8D, 0x8A, 0x89, 0x00, 0x00, 0x00, 0x00,
149 /* KB_Q, KB_R, KB_E, KB_F, KB_ESC, KB_ENTER, KB_LWIN, KB_HASH */
150 0x15, 0x2D, 0x24, 0x2B, 0x76, 0x5A, 0x1F, 0x0E,
151 /* MOUSE_RIGHT, KB_Z, KB_LCTRL, MOUSE_MIDDLE, MOUSE_LEFT, KB_X, KB_LSHIFT, KB_SPACE */
152 0x00, 0x1A, 0x14, 0x00, 0x00, 0x22, 0x12, 0x29,
153
154 /* KB_B, KB_C, KB_G, KB_H, KB_I, KB_J, KB_K, KB_L */
155 0x32, 0x21, 0x34, 0x33, 0x43, 0x3B, 0x42, 0x4B,
156 /* KB_M, KB_N, KB_O, KB_P, KB_T, KB_U, KB_V, KB_Y */
157 0x3A, 0x31, 0x44, 0x4D, 0x2C, 0x3C, 0x2A, 0x35,
158 /* KB_1, KB_2, KB_3, KB_4, KB_5, KB_6, KB_7, KB_8 */
159 0x16, 0x1E, 0x26, 0x25, 0x2E, 0x36, 0x3D, 0x3E,
160 /* KB_9, KB_0, KB_BACKSPACE, KB_TAB, KB_MINUS, KB_EQUAL, KB_LEFTBRACE, KB_RIGHTBRACE */
161 0x46, 0x45, 0x66, 0x0D, 0x4E, 0x55, 0x54, 0x5B,
162
163 /* KB_BACKSLASH, KB_SEMICOLON, KB_APOSTROPHE, KB_GRAVE, KB_COMMA, KB_DOT, KB_SLASH, KB_CAPSLOCK */
164 0x5D, 0x4C, 0x51, 0x00, 0x41, 0x49, 0x4A, 0x58,
165 /* KB_F1, KB_F2, KB_F3, KB_F4, KB_F5, KB_F6, KB_F7, KB_F8 */
166 0x05, 0x06, 0x04, 0x0C, 0x03, 0x0B, 0x83, 0x0A,
167 /* KB_F9, KB_F10, KB_F11, KB_F12, KB_PSCREEN, KB_SCROLL, KB_PAUSE, KB_INSERT */
168 0x01, 0x09, 0x78, 0x07, 0x84, 0x7E, 0x82, 0x81,
169 /* KB_HOME, KB_PAGEUP, KB_DEL, KB_END, KB_PAGEDOWN, KB_NUMLOCK, KB_KP_DIV, KB_KP_MULTI */
170 0x87, 0x8B, 0x85, 0x88, 0x8C, 0x77, 0x80, 0x7C,
171
172 /* KB_KP_MINUS, KB_KP_PLUS, KB_KP_ENTER, KB_KP_1, KB_KP_2, KB_KP_3, KB_KP_4, KB_KP_5 */
173 0x7B, 0x79, 0x19, 0x69, 0x72, 0x7A, 0x6B, 0x73,
174 /* KB_KP_6, KB_KP_7, KB_KP_8, KB_KP_9, KB_KP_0, KB_KP_DOT, KB_LALT, KB_RCTRL */
175 0x74, 0x6C, 0x75, 0x7D, 0x70, 0x71, 0x11, 0x18,
176 /* KB_RSHIFT, KB_RALT, KB_RWIN */
177 0x59, 0x17, 0x00,
178 };
179
180 void IRAM_ATTR ps_init_buffer(int32_t dev_mode, struct wired_data *wired_data) {
181 switch (dev_mode) {
182 case DEV_KB:
183 {
184 memset(wired_data->output, 0x00, 12);
185 break;
186 }
187 case DEV_MOUSE:
188 {
189 struct ps_mouse_map *map = (struct ps_mouse_map *)wired_data->output;
190
191 map->buttons = 0xFCFF;
192 for (uint32_t i = 0; i < 2; i++) {
193 map->raw_axes[i] = 0;
194 map->relative[i] = 1;
195 }
196 break;
197 }
198 default:
199 {
200 struct ps_map *map = (struct ps_map *)wired_data->output;
201 struct ps_map *map_mask = (struct ps_map *)wired_data->output_mask;
202
203 memset((void *)map, 0, sizeof(*map));
204 map->buttons = 0xFFFF;
205 map->analog_btn = 0x00;
206 for (uint32_t i = 0; i < PS_JOYSTICK_AXES_CNT; i++) {
207 map->axes[ps_axes_idx[i]] = ps_axes_meta[i].neutral;
208 }
209 memset(map->pressure, 0x00, sizeof(map->pressure));
210
211 map_mask->buttons = 0x0000;
212 map_mask->sticks = 0x00000000;
213 memset(map_mask->pressure, 0xFF, sizeof(map_mask->pressure));
214 break;
215 }
216 }
217 }
218
219 void ps_meta_init(struct wired_ctrl *ctrl_data) {
220 memset((void *)ctrl_data, 0, sizeof(*ctrl_data)*WIRED_MAX_DEV);
221
222 for (uint32_t i = 0; i < WIRED_MAX_DEV; i++) {
223 for (uint32_t j = 0; j < ADAPTER_PS2_MAX_AXES; j++) {
224 switch (config.out_cfg[i].dev_mode) {
225 case DEV_KB:
226 ctrl_data[i].mask = ps_kb_mask;
227 ctrl_data[i].desc = ps_kb_desc;
228 goto exit_axes_loop;
229 case DEV_MOUSE:
230 if (j >= PS_JOYSTICK_AXES_CNT) {
231 goto exit_axes_loop;
232 }
233 ctrl_data[i].mask = ps_mouse_mask;
234 ctrl_data[i].desc = ps_mouse_desc;
235 ctrl_data[i].axes[j].meta = &ps_mouse_axes_meta[j];
236 break;
237 default:
238 ctrl_data[i].mask = ps_mask;
239 ctrl_data[i].desc = ps_desc;
240 ctrl_data[i].axes[j].meta = &ps_axes_meta[j];
241 break;
242 }
243 }
244 exit_axes_loop:
245 ;
246 }
247 }
248
249 static void ps_ctrl_from_generic(struct wired_ctrl *ctrl_data, struct wired_data *wired_data) {
250 struct ps_map map_tmp;
251
252 memcpy((void *)&map_tmp, wired_data->output, sizeof(map_tmp));
253
254 for (uint32_t i = 0; i < ARRAY_SIZE(generic_btns_mask); i++) {
255 if (ctrl_data->map_mask[0] & BIT(i)) {
256 if (ctrl_data->btns[0].value & generic_btns_mask[i]) {
257 map_tmp.buttons &= ~ps_btns_mask[i];
258 wired_data->cnt_mask[i] = ctrl_data->btns[0].cnt_mask[i];
259 }
260 else {
261 map_tmp.buttons |= ps_btns_mask[i];
262 wired_data->cnt_mask[i] = 0;
263 }
264 }
265 }
266
267 if (ctrl_data->map_mask[0] & generic_btns_mask[PAD_MT]) {
268 if (ctrl_data->btns[0].value & generic_btns_mask[PAD_MT]) {
269 map_tmp.analog_btn |= 0x01;
270 }
271 else {
272 map_tmp.analog_btn &= ~0x01;
273 }
274 }
275
276 for (uint32_t i = 0; i < ADAPTER_PS2_MAX_AXES; i++) {
277 uint32_t btn_id = axis_to_btn_id(i);
278 uint32_t btn_mask = axis_to_btn_mask(i);
279 if (ctrl_data->map_mask[0] & (btn_mask & ps_desc[0])) {
280 if (ctrl_data->axes[i].value > ctrl_data->axes[i].meta->size_max) {
281 map_tmp.axes[ps_axes_idx[i]] = 255;
282 }
283 else if (ctrl_data->axes[i].value < ctrl_data->axes[i].meta->size_min) {
284 map_tmp.axes[ps_axes_idx[i]] = 0;
285 }
286 else {
287 map_tmp.axes[ps_axes_idx[i]] = (uint8_t)(ctrl_data->axes[i].value + ctrl_data->axes[i].meta->neutral);
288 }
289
290 if (i >= PS_JOYSTICK_AXES_CNT) {
291 if (map_tmp.axes[ps_axes_idx[i]]) {
292 map_tmp.buttons &= ~ps_btns_mask[btn_id];;
293 wired_data->cnt_mask[btn_id] = ctrl_data->btns[0].cnt_mask[btn_id];
294 }
295 else {
296 map_tmp.buttons |= ps_btns_mask[btn_id];
297 wired_data->cnt_mask[btn_id] = 0;
298 }
299 }
300 }
301 wired_data->cnt_mask[btn_id] = ctrl_data->axes[i].cnt_mask;
302 }
303
304 memcpy(wired_data->output, (void *)&map_tmp, sizeof(map_tmp));
305
306 TESTS_CMDS_LOG("\"wired_output\": {\"axes\": [%d, %d, %d, %d], \"btns\": [%d, %d]},\n",
307 map_tmp.axes[ps_axes_idx[0]], map_tmp.axes[ps_axes_idx[1]],
308 map_tmp.axes[ps_axes_idx[2]], map_tmp.axes[ps_axes_idx[3]],
309 map_tmp.buttons, map_tmp.analog_btn);
310 BT_MON_LOG("\"wired_output\": {\"axes\": [%02X, %02X, %02X, %02X], \"btns\": [%04X, %02X]},\n",
311 map_tmp.axes[ps_axes_idx[0]], map_tmp.axes[ps_axes_idx[1]],
312 map_tmp.axes[ps_axes_idx[2]], map_tmp.axes[ps_axes_idx[3]],
313 map_tmp.buttons, map_tmp.analog_btn);
314 }
315
316 static void ps_mouse_from_generic(struct wired_ctrl *ctrl_data, struct wired_data *wired_data) {
317 struct ps_mouse_map map_tmp;
318 int32_t *raw_axes = (int32_t *)(wired_data->output + 4);
319
320 memcpy((void *)&map_tmp, wired_data->output, sizeof(map_tmp));
321
322 for (uint32_t i = 0; i < ARRAY_SIZE(generic_btns_mask); i++) {
323 if (ctrl_data->map_mask[0] & BIT(i)) {
324 if (ctrl_data->btns[0].value & generic_btns_mask[i]) {
325 map_tmp.buttons &= ~ps_mouse_btns_mask[i];
326 }
327 else {
328 map_tmp.buttons |= ps_mouse_btns_mask[i];
329 }
330 }
331 }
332
333 for (uint32_t i = 2; i < PS_JOYSTICK_AXES_CNT; i++) {
334 if (ctrl_data->map_mask[0] & (axis_to_btn_mask(i) & ps_mouse_desc[0])) {
335 if (ctrl_data->axes[i].relative) {
336 map_tmp.relative[ps_mouse_axes_idx[i]] = 1;
337 atomic_add(&raw_axes[ps_mouse_axes_idx[i]], ctrl_data->axes[i].value);
338 }
339 else {
340 map_tmp.relative[ps_mouse_axes_idx[i]] = 0;
341 raw_axes[ps_mouse_axes_idx[i]] = ctrl_data->axes[i].value;
342 }
343 }
344 }
345
346 memcpy(wired_data->output, (void *)&map_tmp, sizeof(map_tmp) - 8);
347 }
348
349 static void ps_kb_from_generic(struct wired_ctrl *ctrl_data, struct wired_data *wired_data) {
350 if (!atomic_test_bit(&wired_data->flags, WIRED_KBMON_INIT)) {
351 kbmon_init(ctrl_data->index, ps_kb_id_to_scancode);
352 kbmon_set_typematic(ctrl_data->index, 1, 500000, 90000);
353 atomic_set_bit(&wired_data->flags, WIRED_KBMON_INIT);
354 }
355 kbmon_update(ctrl_data->index, ctrl_data);
356 }
357
358 void ps_kb_id_to_scancode(uint32_t dev_id, uint8_t type, uint8_t id) {
359 uint8_t kb_buf[12] = {0};
360 uint8_t scancode = ps_kb_scancode[id];
361 uint32_t idx = 0;
362
363 if (scancode) {
364 switch (type) {
365 case KBMON_TYPE_MAKE:
366 kb_buf[idx++] = 0x01;
367 break;
368 case KBMON_TYPE_BREAK:
369 kb_buf[idx++] = 0x02;
370 kb_buf[idx++] = 0xF0;
371 break;
372 }
373 if (id < KBM_MAX) {
374 kb_buf[idx++] = scancode;
375 }
376 kbmon_set_code(dev_id, kb_buf, 12);
377 }
378 }
379
380 void ps_from_generic(int32_t dev_mode, struct wired_ctrl *ctrl_data, struct wired_data *wired_data) {
381 switch (dev_mode) {
382 case DEV_KB:
383 ps_kb_from_generic(ctrl_data, wired_data);
384 break;
385 case DEV_MOUSE:
386 ps_mouse_from_generic(ctrl_data, wired_data);
387 break;
388 default:
389 ps_ctrl_from_generic(ctrl_data, wired_data);
390 break;
391 }
392 }
393
394 void ps_fb_to_generic(int32_t dev_mode, struct raw_fb *raw_fb_data, struct generic_fb *fb_data) {
395 fb_data->wired_id = raw_fb_data->header.wired_id;
396 fb_data->type = raw_fb_data->header.type;
397
398 /* This stop rumble when BR timeout trigger */
399 if (raw_fb_data->header.data_len == 0) {
400 fb_data->state = 0;
401 fb_data->lf_pwr = fb_data->hf_pwr = 0;
402 }
403 else {
404 switch (fb_data->type) {
405 case FB_TYPE_RUMBLE:
406 fb_data->state = (raw_fb_data->data[0] || raw_fb_data->data[1] ? 1 : 0);
407 fb_data->lf_pwr = raw_fb_data->data[1];
408 fb_data->hf_pwr = (raw_fb_data->data[0] == 0x01) ? 0xFF : 0x00;
409 break;
410 case FB_TYPE_STATUS_LED:
411 fb_data->led = raw_fb_data->data[0];
412 break;
413 }
414 }
415 }
416
417 void IRAM_ATTR ps_gen_turbo_mask(struct wired_data *wired_data) {
418 struct ps_map *map_mask = (struct ps_map *)wired_data->output_mask;
419
420 map_mask->buttons = 0x0000;
421 map_mask->sticks = 0x00000000;
422 memset(map_mask->pressure, 0xFF, sizeof(map_mask->pressure));
423
424 for (uint32_t i = 0; i < ARRAY_SIZE(ps_btns_mask); i++) {
425 uint8_t mask = wired_data->cnt_mask[i] >> 1;
426
427 if (ps_btns_mask[i] && mask) {
428 if (wired_data->cnt_mask[i] & 1) {
429 if (!(mask & wired_data->frame_cnt)) {
430 map_mask->buttons |= ps_btns_mask[i];
431 if (ps_btns_idx[i]) {
432 map_mask->axes[ps_btns_idx[i]] = 0x00;
433 }
434 }
435 }
436 else {
437 if (!((mask & wired_data->frame_cnt) == mask)) {
438 map_mask->buttons |= ps_btns_mask[i];
439 if (ps_btns_idx[i]) {
440 map_mask->axes[ps_btns_idx[i]] = 0x00;
441 }
442 }
443 }
444 }
445 }
446
447 wired_gen_turbo_mask_axes8(wired_data, map_mask->axes, 4, ps_axes_idx, ps_axes_meta);
448 }
449