Skip to content
This repository was archived by the owner on Dec 14, 2025. It is now read-only.

Commit ddfc31b

Browse files
darthclouddarthcloud
committed
[BT] Add Switch 2 joysticks calibration support
1 parent 8039c1c commit ddfc31b

3 files changed

Lines changed: 282 additions & 122 deletions

File tree

‎main/adapter/wireless/sw2.c‎

Lines changed: 158 additions & 81 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
#include "zephyr/types.h"
99
#include "tools/util.h"
1010
#include "adapter/config.h"
11+
#include "adapter/mapping_quirks.h"
1112
#include "tests/cmds.h"
1213
#include "bluetooth/mon.h"
1314
#include "bluetooth/hidp/sw2.h"
@@ -44,11 +45,11 @@ enum {
4445
SW2_GL,
4546
};
4647

47-
// static const uint8_t sw2_axes_idx[ADAPTER_MAX_AXES] =
48-
// {
49-
// /* AXIS_LX, AXIS_LY, AXIS_RX, AXIS_RY, TRIG_L, TRIG_R */
50-
// 0, 1, 2, 3, 4, 5
51-
// };
48+
static const uint8_t sw2_axes_idx[ADAPTER_MAX_AXES] =
49+
{
50+
/* AXIS_LX, AXIS_LY, AXIS_RX, AXIS_RY, TRIG_L, TRIG_R */
51+
0, 1, 2, 3, 4, 5
52+
};
5253

5354
static const struct ctrl_meta sw2_pro_axes_meta[ADAPTER_MAX_AXES] =
5455
{
@@ -93,16 +94,131 @@ static const uint32_t sw2_pro_btns_mask[32] = {
9394
BIT(SW2_ZR), BIT(SW2_R), BIT(SW2_GR), BIT(SW2_RJ),
9495
};
9596

96-
static void sw2_pro_to_generic(struct bt_data *bt_data, struct wireless_ctrl *ctrl_data) {
97+
static const uint32_t sw2_gc_mask[4] = {0x77FF0FFF, 0x00000000, 0x00000000, 0x00000000};
98+
static const uint32_t sw2_gc_desc[4] = {0x110000FF, 0x00000000, 0x00000000, 0x00000000};
99+
static const uint32_t sw2_gc_btns_mask[32] = {
100+
0, 0, 0, 0,
101+
0, 0, 0, 0,
102+
BIT(SW2_LEFT), BIT(SW2_RIGHT), BIT(SW2_DOWN), BIT(SW2_UP),
103+
0, 0, 0, 0,
104+
BIT(SW2_B), BIT(SW2_X), BIT(SW2_A), BIT(SW2_Y),
105+
BIT(SW2_PLUS), BIT(SW2_C), BIT(SW2_HOME), BIT(SW2_CAPTURE),
106+
0, BIT(SW2_ZL), BIT(SW2_L), 0,
107+
0, BIT(SW2_ZR), BIT(SW2_R), 0,
108+
};
109+
110+
static int32_t sw2_pad_init(struct bt_data *bt_data) {
111+
struct bt_hid_sw2_ctrl_calib *calib = NULL;
112+
const uint8_t *axes_idx = sw2_axes_idx;
113+
struct ctrl_meta *meta = bt_data->raw_src_mappings[PAD].meta;
114+
const struct ctrl_meta *sw2_axes_meta = sw2_pro_axes_meta;
115+
116+
mapping_quirks_apply(bt_data);
117+
118+
bt_hid_sw2_get_calib(bt_data->base.pids->id, &calib);
119+
120+
switch (bt_data->base.pid) {
121+
case SW2_LJC_PID:
122+
{
123+
// memcpy(bt_data->raw_src_mappings[PAD].btns_mask, &sw_jc_btns_mask[report_type],
124+
// sizeof(bt_data->raw_src_mappings[PAD].btns_mask));
125+
126+
// meta[0].polarity = 1;
127+
// meta[1].polarity = 0;
128+
// axes_idx = sw_jc_axes_idx;
129+
// memcpy(bt_data->raw_src_mappings[PAD].mask, sw_jc_mask,
130+
// sizeof(bt_data->raw_src_mappings[PAD].mask));
131+
// memcpy(bt_data->raw_src_mappings[PAD].desc, desc,
132+
// sizeof(bt_data->raw_src_mappings[PAD].desc));
133+
break;
134+
}
135+
case SW2_RJC_PID:
136+
{
137+
// memcpy(bt_data->raw_src_mappings[PAD].btns_mask, &sw_jc_btns_mask[report_type],
138+
// sizeof(bt_data->raw_src_mappings[PAD].btns_mask));
139+
140+
// meta[0].polarity = 0;
141+
// meta[1].polarity = 1;
142+
// axes_idx = sw_jc_axes_idx;
143+
// memcpy(bt_data->raw_src_mappings[PAD].mask, sw_jc_mask,
144+
// sizeof(bt_data->raw_src_mappings[PAD].mask));
145+
// memcpy(bt_data->raw_src_mappings[PAD].desc, desc,
146+
// sizeof(bt_data->raw_src_mappings[PAD].desc));
147+
break;
148+
}
149+
case SW2_GC_PID:
150+
{
151+
memcpy(bt_data->raw_src_mappings[PAD].btns_mask, sw2_gc_btns_mask,
152+
sizeof(bt_data->raw_src_mappings[PAD].btns_mask));
153+
meta[0].polarity = 0;
154+
meta[1].polarity = 0;
155+
meta[2].polarity = 0;
156+
meta[3].polarity = 0;
157+
axes_idx = sw2_axes_idx;
158+
sw2_axes_meta = sw2_gc_axes_meta;
159+
memcpy(bt_data->raw_src_mappings[PAD].mask, sw2_gc_mask,
160+
sizeof(bt_data->raw_src_mappings[PAD].mask));
161+
memcpy(bt_data->raw_src_mappings[PAD].desc, sw2_gc_desc,
162+
sizeof(bt_data->raw_src_mappings[PAD].desc));
163+
164+
meta[TRIG_L].neutral = sw2_gc_axes_meta[TRIG_L].neutral;
165+
meta[TRIG_L].abs_max = sw2_gc_axes_meta[TRIG_L].abs_max;
166+
meta[TRIG_L].abs_min = sw2_gc_axes_meta[TRIG_L].abs_min;
167+
meta[TRIG_R].neutral = sw2_gc_axes_meta[TRIG_R].neutral;
168+
meta[TRIG_R].abs_max = sw2_gc_axes_meta[TRIG_R].abs_max;
169+
meta[TRIG_R].abs_min = sw2_gc_axes_meta[TRIG_R].abs_min;
170+
break;
171+
}
172+
case SW2_PRO2_PID:
173+
default:
174+
{
175+
memcpy(bt_data->raw_src_mappings[PAD].btns_mask, sw2_pro_btns_mask,
176+
sizeof(bt_data->raw_src_mappings[PAD].btns_mask));
177+
meta[0].polarity = 0;
178+
meta[1].polarity = 0;
179+
meta[2].polarity = 0;
180+
meta[3].polarity = 0;
181+
axes_idx = sw2_axes_idx;
182+
memcpy(bt_data->raw_src_mappings[PAD].mask, sw2_pro_mask,
183+
sizeof(bt_data->raw_src_mappings[PAD].mask));
184+
memcpy(bt_data->raw_src_mappings[PAD].desc, sw2_pro_desc,
185+
sizeof(bt_data->raw_src_mappings[PAD].desc));
186+
break;
187+
}
188+
}
189+
190+
for (uint32_t i = 0; i < SW2_AXES_MAX; i++) {
191+
if (calib && calib->sticks[i / 2].axes[i % 2].neutral) {
192+
meta[axes_idx[i]].neutral = calib->sticks[i / 2].axes[i % 2].neutral;
193+
meta[axes_idx[i]].abs_max = calib->sticks[i / 2].axes[i % 2].rel_max * MAX_PULL_BACK;
194+
meta[axes_idx[i]].abs_min = calib->sticks[i / 2].axes[i % 2].rel_min * MAX_PULL_BACK;
195+
meta[axes_idx[i]].deadzone = calib->sticks[i / 2].deadzone;
196+
printf("# %s: controller calib loaded\n", __FUNCTION__);
197+
}
198+
else {
199+
meta[axes_idx[i]].neutral = sw2_axes_meta[i].neutral;
200+
meta[axes_idx[i]].abs_max = sw2_axes_meta[i].abs_max * MAX_PULL_BACK;
201+
meta[axes_idx[i]].abs_min = sw2_axes_meta[i].abs_min * MAX_PULL_BACK;
202+
meta[axes_idx[i]].deadzone = sw2_axes_meta[i].deadzone;
203+
printf("# %s: no calib, using default\n", __FUNCTION__);
204+
}
205+
bt_data->base.axes_cal[i] = 0;
206+
}
207+
208+
atomic_set_bit(&bt_data->base.flags[PAD], BT_INIT);
209+
return 0;
210+
}
211+
212+
static int32_t sw2_pro_to_generic(struct bt_data *bt_data, struct wireless_ctrl *ctrl_data) {
97213
struct sw2_map *map = (struct sw2_map *)bt_data->base.input;
98214
struct ctrl_meta *meta = bt_data->raw_src_mappings[PAD].meta;
99215
uint16_t axes[4];
100216

101-
// TESTS_CMDS_LOG("\"wireless_input\": {\"report_id\": %ld, \"axes\": [%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]], \"btns\": %lu},\n",
102-
// bt_data->base.report_id,
103-
// map->axes[0], map->axes[1], map->axes[2], map->axes[3], map->axes[4], map->axes[5],
104-
// map->axes[6], map->axes[7], map->axes[8], map->axes[9], map->axes[10], map->axes[11],
105-
// map->buttons);
217+
if (!atomic_test_bit(&bt_data->base.flags[PAD], BT_INIT)) {
218+
if (sw2_pad_init(bt_data)) {
219+
return -1;
220+
}
221+
}
106222

107223
memset((void *)ctrl_data, 0, sizeof(*ctrl_data));
108224

@@ -120,52 +236,27 @@ static void sw2_pro_to_generic(struct bt_data *bt_data, struct wireless_ctrl *ct
120236
axes[2] = map->axes[3] | ((map->axes[4] & 0xF) << 8);
121237
axes[3] = (map->axes[4] >> 4) | (map->axes[5] << 4);
122238

123-
if (!atomic_test_bit(&bt_data->base.flags[PAD], BT_INIT)) {
124-
memcpy(meta, sw2_pro_axes_meta, sizeof(sw2_pro_axes_meta));
125-
// bt_mon_log(false, "%s: axes_cal: [", __FUNCTION__);
126-
// for (uint32_t i = 0; i < ADAPTER_MAX_AXES; i++) {
127-
// meta[i].abs_max *= MAX_PULL_BACK;
128-
// meta[i].abs_min *= MAX_PULL_BACK;
129-
// bt_data->base.axes_cal[i] = -(map->axes[sw2_pro_axes_idx[i]] - sw2_pro_axes_meta[i].neutral);
130-
// if (i) {
131-
// bt_mon_log(false, ", ");
132-
// }
133-
// bt_mon_log(false, "%d", bt_data->base.axes_cal[i]);
134-
// }
135-
atomic_set_bit(&bt_data->base.flags[PAD], BT_INIT);
136-
// bt_mon_log(true, "]");
137-
}
239+
TESTS_CMDS_LOG("\"wireless_input\": {\"axes\": [%u, %u, %u, %u], \"btns\": %lu},\n",
240+
axes[0], axes[1], axes[2], axes[3], map->buttons);
138241

139242
for (uint32_t i = 0; i < SW2_AXES_MAX; i++) {
140243
ctrl_data->axes[i].meta = &meta[i];
141244
ctrl_data->axes[i].value = axes[i] - meta[i].neutral;
142245
}
246+
return 0;
143247
}
144248

145-
static const uint32_t sw2_gc_mask[4] = {0x77FF0FFF, 0x00000000, 0x00000000, 0x00000000};
146-
static const uint32_t sw2_gc_desc[4] = {0x110000FF, 0x00000000, 0x00000000, 0x00000000};
147-
static const uint32_t sw2_gc_btns_mask[32] = {
148-
0, 0, 0, 0,
149-
0, 0, 0, 0,
150-
BIT(SW2_LEFT), BIT(SW2_RIGHT), BIT(SW2_DOWN), BIT(SW2_UP),
151-
0, 0, 0, 0,
152-
BIT(SW2_B), BIT(SW2_X), BIT(SW2_A), BIT(SW2_Y),
153-
BIT(SW2_PLUS), BIT(SW2_C), BIT(SW2_HOME), BIT(SW2_CAPTURE),
154-
0, BIT(SW2_ZL), BIT(SW2_L), 0,
155-
0, BIT(SW2_ZR), BIT(SW2_R), 0,
156-
};
157-
158-
static void sw2_gc_to_generic(struct bt_data *bt_data, struct wireless_ctrl *ctrl_data) {
249+
static int32_t sw2_gc_to_generic(struct bt_data *bt_data, struct wireless_ctrl *ctrl_data) {
159250

160251
struct sw2_map *map = (struct sw2_map *)bt_data->base.input;
161252
struct ctrl_meta *meta = bt_data->raw_src_mappings[PAD].meta;
162253
uint16_t axes[4];
163254

164-
// TESTS_CMDS_LOG("\"wireless_input\": {\"report_id\": %ld, \"axes\": [%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]], \"btns\": %lu},\n",
165-
// bt_data->base.report_id,
166-
// map->axes[0], map->axes[1], map->axes[2], map->axes[3], map->axes[4], map->axes[5],
167-
// map->axes[6], map->axes[7], map->axes[8], map->axes[9], map->axes[10], map->axes[11],
168-
// map->buttons);
255+
if (!atomic_test_bit(&bt_data->base.flags[PAD], BT_INIT)) {
256+
if (sw2_pad_init(bt_data)) {
257+
return -1;
258+
}
259+
}
169260

170261
memset((void *)ctrl_data, 0, sizeof(*ctrl_data));
171262

@@ -183,21 +274,8 @@ static void sw2_gc_to_generic(struct bt_data *bt_data, struct wireless_ctrl *ctr
183274
axes[2] = map->axes[3] | ((map->axes[4] & 0xF) << 8);
184275
axes[3] = (map->axes[4] >> 4) | (map->axes[5] << 4);
185276

186-
if (!atomic_test_bit(&bt_data->base.flags[PAD], BT_INIT)) {
187-
memcpy(meta, sw2_gc_axes_meta, sizeof(sw2_gc_axes_meta));
188-
// bt_mon_log(false, "%s: axes_cal: [", __FUNCTION__);
189-
// for (uint32_t i = 0; i < ADAPTER_MAX_AXES; i++) {
190-
// meta[i].abs_max *= MAX_PULL_BACK;
191-
// meta[i].abs_min *= MAX_PULL_BACK;
192-
// bt_data->base.axes_cal[i] = -(map->axes[sw2_pro_axes_idx[i]] - sw2_pro_axes_meta[i].neutral);
193-
// if (i) {
194-
// bt_mon_log(false, ", ");
195-
// }
196-
// bt_mon_log(false, "%d", bt_data->base.axes_cal[i]);
197-
// }
198-
atomic_set_bit(&bt_data->base.flags[PAD], BT_INIT);
199-
// bt_mon_log(true, "]");
200-
}
277+
TESTS_CMDS_LOG("\"wireless_input\": {\"axes\": [%u, %u, %u, %u], \"btns\": %lu},\n",
278+
axes[0], axes[1], axes[2], axes[3], map->buttons);
201279

202280
for (uint32_t i = 0; i < SW2_AXES_MAX; i++) {
203281
ctrl_data->axes[i].meta = &meta[i];
@@ -207,18 +285,17 @@ static void sw2_gc_to_generic(struct bt_data *bt_data, struct wireless_ctrl *ctr
207285
ctrl_data->axes[i].meta = &meta[i];
208286
ctrl_data->axes[i].value = map->triggers[i - 4] - meta[i].neutral;
209287
}
288+
return 0;
210289
}
211290

212291
int32_t sw2_to_generic(struct bt_data *bt_data, struct wireless_ctrl *ctrl_data) {
213292
switch (bt_data->base.pid) {
214-
case 0x2066:
215-
case 0x2067:
216-
case 0x2069:
217-
sw2_pro_to_generic(bt_data, ctrl_data);
218-
break;
219-
case 0x2073:
220-
sw2_gc_to_generic(bt_data, ctrl_data);
221-
break;
293+
case SW2_LJC_PID:
294+
case SW2_RJC_PID:
295+
case SW2_PRO2_PID:
296+
return sw2_pro_to_generic(bt_data, ctrl_data);
297+
case SW2_GC_PID:
298+
return sw2_gc_to_generic(bt_data, ctrl_data);
222299
default:
223300
printf("# Unknown pid : %04X\n", bt_data->base.pid);
224301
return -1;
@@ -230,38 +307,38 @@ void sw2_fb_from_generic(struct generic_fb *fb_data, struct bt_data *bt_data) {
230307
struct bt_hidp_sw2_out *out = (struct bt_hidp_sw2_out *)bt_data->base.output;
231308

232309
switch (bt_data->base.pid) {
233-
case 0x2066:
234-
case 0x2067:
235-
case 0x2069:
310+
case SW2_LJC_PID:
311+
case SW2_RJC_PID:
312+
case SW2_PRO2_PID:
236313
switch (fb_data->type) {
237314
case FB_TYPE_RUMBLE:
238315
if (fb_data->hf_pwr || fb_data->lf_pwr) {
239-
out->r_lra.ops[0].hf_freq = 0x1e1;
316+
out->r_lra.ops[0].hf_freq = BT_HIDP_SW2_LRA_R_HF_FREQ;
240317
out->r_lra.ops[0].hf_amp = (uint8_t)((float)fb_data->hf_pwr / 2.68);
241-
out->r_lra.ops[0].lf_freq = 0x180;
318+
out->r_lra.ops[0].lf_freq = BT_HIDP_SW2_LRA_R_LF_FREQ;
242319
out->r_lra.ops[0].lf_amp = (uint16_t)((float)fb_data->hf_pwr / 0.3156);
243320
out->r_lra.ops[0].enable = 1;
244321

245-
out->l_lra.ops[0].hf_freq = 0xe1;
322+
out->l_lra.ops[0].hf_freq = BT_HIDP_SW2_LRA_L_HF_FREQ;
246323
out->l_lra.ops[0].hf_amp = (uint8_t)((float)fb_data->lf_pwr / 2.68);
247-
out->l_lra.ops[0].lf_freq = 0x100;
324+
out->l_lra.ops[0].lf_freq = BT_HIDP_SW2_LRA_L_LF_FREQ;
248325
out->l_lra.ops[0].lf_amp = (uint16_t)((float)fb_data->lf_pwr / 0.3156);
249326
out->l_lra.ops[0].enable = 1;
250327
}
251328
else {
252-
out->l_lra.ops[0].val = 0x1e100000;
253-
out->l_lra.ops[0].hf_amp = 0x00;
254-
out->r_lra.ops[0].val = 0x1e100000;
255-
out->r_lra.ops[0].hf_amp = 0x00;
329+
out->l_lra.ops[0].val = BT_HIDP_SW2_LRA_IDLE_32;
330+
out->l_lra.ops[0].hf_amp = BT_HIDP_SW2_LRA_IDLE_8;
331+
out->r_lra.ops[0].val = BT_HIDP_SW2_LRA_IDLE_32;
332+
out->r_lra.ops[0].hf_amp = BT_HIDP_SW2_LRA_IDLE_8;
256333
}
257-
printf("%08lX %02X %08lX %02X\n", out->l_lra.ops[0].val, out->l_lra.ops[0].hf_amp, out->r_lra.ops[0].val, out->r_lra.ops[0].hf_amp);
334+
//printf("%08lX %02X %08lX %02X\n", out->l_lra.ops[0].val, out->l_lra.ops[0].hf_amp, out->r_lra.ops[0].val, out->r_lra.ops[0].hf_amp);
258335
break;
259336
case FB_TYPE_PLAYER_LED:
260337
bt_data->base.output[41] = bt_hid_led_dev_id_map[bt_data->base.pids->out_idx];
261338
break;
262339
}
263340
break;
264-
case 0x2073:
341+
case SW2_GC_PID:
265342
switch (fb_data->type) {
266343
case FB_TYPE_RUMBLE:
267344
if (fb_data->state) {

0 commit comments

Comments
 (0)