diff --git a/drivers/hid/hid-nintendo.c b/drivers/hid/hid-nintendo.c index f7fc3b1ec582..f62cc55def38 100644 --- a/drivers/hid/hid-nintendo.c +++ b/drivers/hid/hid-nintendo.c @@ -2,7 +2,7 @@ /* * HID driver for Nintendo Switch Joy-Cons and Pro Controllers * - * Copyright (c) 2019 Daniel J. Ogorchock + * Copyright (c) 2019-2020 Daniel J. Ogorchock * * The following resources/projects were referenced for this driver: * https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering @@ -114,6 +114,15 @@ static const u16 JC_CAL_USR_RIGHT_DATA_ADDR = 0x801D; static const u16 JC_CAL_FCT_DATA_LEFT_ADDR = 0x603d; static const u16 JC_CAL_FCT_DATA_RIGHT_ADDR = 0x6046; +/* SPI storage addresses of IMU factory calibration data */ +static const u16 JC_IMU_CAL_FCT_DATA_ADDR = 0x6020; +static const u16 JC_IMU_CAL_FCT_DATA_END = 0x6037; +#define JC_IMU_CAL_DATA_SIZE \ + (JC_IMU_CAL_FCT_DATA_END - JC_IMU_CAL_FCT_DATA_ADDR + 1) +/* SPI storage addresses of IMU user calibration data */ +static const u16 JC_IMU_CAL_USR_MAGIC_ADDR = 0x8026; +static const u16 JC_IMU_CAL_USR_DATA_ADDR = 0x8028; + /* The raw analog joystick values will be mapped in terms of this magnitude */ static const u16 JC_MAX_STICK_MAG = 32767; static const u16 JC_STICK_FUZZ = 250; @@ -124,6 +133,18 @@ static const u16 JC_MAX_DPAD_MAG = 1; static const u16 JC_DPAD_FUZZ /*= 0*/; static const u16 JC_DPAD_FLAT /*= 0*/; +/* The accel axes will be mapped in terms of this magnitude */ +static const u16 JC_MAX_ACCEL_MAG = 32767; +static const u16 JC_ACCEL_RES = 4096; +static const u16 JC_ACCEL_FUZZ = 10; +static const u16 JC_ACCEL_FLAT /*= 0*/; + +/* The gyro axes will be mapped in terms of this magnitude */ +static const u16 JC_MAX_GYRO_MAG = 32767; +static const u16 JC_GYRO_RES = 13371 / 936; /* 14 (14.285) */ +static const u16 JC_GYRO_FUZZ = 10; +static const u16 JC_GYRO_FLAT /*= 0*/; + /* frequency/amplitude tables for rumble */ struct joycon_rumble_freq_data { u16 high; @@ -258,6 +279,11 @@ struct joycon_stick_cal { s32 center; }; +struct joycon_imu_cal { + s16 offset[3]; + s16 scale[3]; +}; + /* * All the controller's button values are stored in a u32. * They can be accessed with bitwise ANDs. @@ -305,6 +331,19 @@ struct joycon_subcmd_reply { u8 data[0]; /* will be at most 35 bytes */ } __packed; +struct joycon_imu_data { + s16 accel_x; + s16 accel_y; + s16 accel_z; + s16 gyro_x; + s16 gyro_y; + s16 gyro_z; +} __packed; + +struct joycon_imu_report { + struct joycon_imu_data data[3]; +} __packed; + struct joycon_input_report { u8 id; u8 timer; @@ -314,11 +353,10 @@ struct joycon_input_report { u8 right_stick[3]; u8 vibrator_report; - /* - * If support for firmware updates, gyroscope data, and/or NFC/IR - * are added in the future, this can be swapped for a union. - */ - struct joycon_subcmd_reply reply; + union { + struct joycon_subcmd_reply subcmd_reply; + struct joycon_imu_report imu_report; + }; } __packed; #define JC_MAX_RESP_SIZE (sizeof(struct joycon_input_report) + 35) @@ -359,6 +397,9 @@ struct joycon_ctlr { struct joycon_stick_cal right_stick_cal_x; struct joycon_stick_cal right_stick_cal_y; + struct joycon_imu_cal accel_cal; + struct joycon_imu_cal gyro_cal; + /* power supply data */ struct power_supply *battery; struct power_supply_desc battery_desc; @@ -378,6 +419,10 @@ struct joycon_ctlr { u16 rumble_rl_freq; u16 rumble_rh_freq; bool rumble_zero_amp; + + /* imu */ + struct input_dev *imu_input; + int timestamp; }; /* Helper macros for checking controller type */ @@ -565,7 +610,7 @@ static int joycon_request_spi_flash_read(struct joycon_ctlr *ctlr, } else { report = (struct joycon_input_report *)ctlr->input_buf; /* The read data starts at the 6th byte */ - *reply = &report->reply.data[5]; + *reply = &report->subcmd_reply.data[5]; } return ret; } @@ -725,6 +770,78 @@ static int joycon_request_calibration(struct joycon_ctlr *ctlr) return 0; } +static const s16 DFLT_ACCEL_OFFSET /*= 0*/; +static const s16 DFLT_ACCEL_SCALE = 16384; +static const s16 DFLT_GYRO_OFFSET /*= 0*/; +static const s16 DFLT_GYRO_SCALE = 13371; +static int joycon_request_imu_calibration(struct joycon_ctlr *ctlr) +{ + u16 imu_cal_addr = JC_IMU_CAL_FCT_DATA_ADDR; + u8 *raw_cal; + int ret; + int i; + + /* check if user calibration exists */ + if (!joycon_check_for_cal_magic(ctlr, JC_IMU_CAL_USR_MAGIC_ADDR)) { + imu_cal_addr = JC_IMU_CAL_USR_DATA_ADDR; + hid_info(ctlr->hdev, "using user cal for IMU\n"); + } else { + hid_info(ctlr->hdev, "using factory cal for IMU\n"); + } + + /* request IMU calibration data */ + hid_dbg(ctlr->hdev, "requesting IMU cal data\n"); + ret = joycon_request_spi_flash_read(ctlr, imu_cal_addr, + JC_IMU_CAL_DATA_SIZE, &raw_cal); + if (ret) { + hid_warn(ctlr->hdev, + "Failed to read IMU cal, using defaults; ret=%d\n", + ret); + + for (i = 0; i < 3; i++) { + ctlr->accel_cal.offset[i] = DFLT_ACCEL_OFFSET; + ctlr->accel_cal.scale[i] = DFLT_ACCEL_SCALE; + ctlr->gyro_cal.offset[i] = DFLT_GYRO_OFFSET; + ctlr->gyro_cal.scale[i] = DFLT_GYRO_SCALE; + } + return ret; + } + + /* IMU calibration parsing */ + for (i = 0; i < 3; i++) { + int j = i * 2; + + ctlr->accel_cal.offset[i] = raw_cal[j + 0] | + ((s16)raw_cal[j + 1] << 8); + ctlr->accel_cal.scale[i] = raw_cal[j + 6] | + ((s16)raw_cal[j + 7] << 8); + ctlr->gyro_cal.offset[i] = raw_cal[j + 12] | + ((s16)raw_cal[j + 13] << 8); + ctlr->gyro_cal.scale[i] = raw_cal[j + 18] | + ((s16)raw_cal[j + 19] << 8); + } + + hid_dbg(ctlr->hdev, "IMU calibration:\n" + "a_o[0]=%d a_o[1]=%d a_o[2]=%d\n" + "a_s[0]=%d a_s[1]=%d a_s[2]=%d\n" + "g_o[0]=%d g_o[1]=%d g_o[2]=%d\n" + "g_s[0]=%d g_s[1]=%d g_s[2]=%d\n", + ctlr->accel_cal.offset[0], + ctlr->accel_cal.offset[1], + ctlr->accel_cal.offset[2], + ctlr->accel_cal.scale[0], + ctlr->accel_cal.scale[1], + ctlr->accel_cal.scale[2], + ctlr->gyro_cal.offset[0], + ctlr->gyro_cal.offset[1], + ctlr->gyro_cal.offset[2], + ctlr->gyro_cal.scale[0], + ctlr->gyro_cal.scale[1], + ctlr->gyro_cal.scale[2]); + + return 0; +} + static int joycon_set_report_mode(struct joycon_ctlr *ctlr) { struct joycon_subcmd_request *req; @@ -751,6 +868,19 @@ static int joycon_enable_rumble(struct joycon_ctlr *ctlr, bool enable) return joycon_send_subcmd(ctlr, req, 1, HZ/4); } +static int joycon_enable_imu(struct joycon_ctlr *ctlr, bool enable) +{ + struct joycon_subcmd_request *req; + u8 buffer[sizeof(*req) + 1] = { 0 }; + + req = (struct joycon_subcmd_request *)buffer; + req->subcmd_id = JC_SUBCMD_ENABLE_IMU; + req->data[0] = enable ? 0x01 : 0x00; + + hid_dbg(ctlr->hdev, "%s IMU\n", enable ? "enabling" : "disabling"); + return joycon_send_subcmd(ctlr, req, 1, HZ); +} + static s32 joycon_map_stick_val(struct joycon_stick_cal *cal, s32 val) { s32 center = cal->center; @@ -919,6 +1049,54 @@ static void joycon_parse_report(struct joycon_ctlr *ctlr, spin_unlock_irqrestore(&ctlr->lock, flags); wake_up(&ctlr->wait); } + + /* parse IMU data if present */ + if (rep->id == JC_INPUT_IMU_DATA) { + struct joycon_imu_data *imu_data = rep->imu_report.data; + struct input_dev *idev = ctlr->imu_input; + int i; + int value[6]; + + for (i = 0; i < 3; i++) { /* there are 3 reports */ + ctlr->timestamp += 5000; /* each is 5 ms apart */ + input_event(idev, EV_MSC, MSC_TIMESTAMP, + ctlr->timestamp); + + /* + * Rather than convert to floats, we adjust by + * calibration factors and then multiply by the default + * scaling values. This way, the consumer only needs to + * divide by the default scale values. + */ + value[0] = (imu_data[i].gyro_x - + ctlr->gyro_cal.offset[0]) * + DFLT_GYRO_SCALE / ctlr->gyro_cal.scale[0]; + value[1] = (imu_data[i].gyro_y - + ctlr->gyro_cal.offset[1]) * + DFLT_GYRO_SCALE / ctlr->gyro_cal.scale[1]; + value[2] = (imu_data[i].gyro_z - + ctlr->gyro_cal.offset[2]) * + DFLT_GYRO_SCALE / ctlr->gyro_cal.scale[2]; + + value[3] = (imu_data[i].accel_x - + ctlr->accel_cal.offset[0]) * + DFLT_ACCEL_SCALE / ctlr->accel_cal.scale[0]; + value[4] = (imu_data[i].accel_y - + ctlr->accel_cal.offset[1]) * + DFLT_ACCEL_SCALE / ctlr->accel_cal.scale[1]; + value[5] = (imu_data[i].accel_z - + ctlr->accel_cal.offset[2]) * + DFLT_ACCEL_SCALE / ctlr->accel_cal.scale[2]; + + input_report_abs(idev, ABS_RX, value[0]); + input_report_abs(idev, ABS_RY, value[1]); + input_report_abs(idev, ABS_RZ, value[2]); + input_report_abs(idev, ABS_X, value[3]); + input_report_abs(idev, ABS_Y, value[4]); + input_report_abs(idev, ABS_Z, value[5]); + input_sync(idev); + } + } } static void joycon_rumble_worker(struct work_struct *work) @@ -1104,6 +1282,7 @@ static int joycon_input_create(struct joycon_ctlr *ctlr) { struct hid_device *hdev; const char *name; + const char *imu_name; int ret; int i; @@ -1112,18 +1291,24 @@ static int joycon_input_create(struct joycon_ctlr *ctlr) switch (hdev->product) { case USB_DEVICE_ID_NINTENDO_PROCON: name = "Nintendo Switch Pro Controller"; + imu_name = "Nintendo Switch Pro Controller IMU"; break; case USB_DEVICE_ID_NINTENDO_CHRGGRIP: - if (jc_type_has_left(ctlr)) + if (jc_type_has_left(ctlr)) { name = "Nintendo Switch Left Joy-Con (Grip)"; - else + imu_name = "Nintendo Switch Left Joy-Con IMU (Grip)"; + } else { name = "Nintendo Switch Right Joy-Con (Grip)"; + imu_name = "Nintendo Switch Right Joy-Con IMU (Grip)"; + } break; case USB_DEVICE_ID_NINTENDO_JOYCONL: name = "Nintendo Switch Left Joy-Con"; + imu_name = "Nintendo Switch Left Joy-Con IMU"; break; case USB_DEVICE_ID_NINTENDO_JOYCONR: name = "Nintendo Switch Right Joy-Con"; + imu_name = "Nintendo Switch Right Joy-Con IMU"; break; default: /* Should be impossible */ hid_err(hdev, "Invalid hid product\n"); @@ -1207,6 +1392,55 @@ static int joycon_input_create(struct joycon_ctlr *ctlr) if (ret) return ret; + /* configure the imu input device */ + ctlr->imu_input = devm_input_allocate_device(&hdev->dev); + if (!ctlr->imu_input) + return -ENOMEM; + + ctlr->imu_input->id.bustype = hdev->bus; + ctlr->imu_input->id.vendor = hdev->vendor; + ctlr->imu_input->id.product = hdev->product; + ctlr->imu_input->id.version = hdev->version; + ctlr->imu_input->uniq = ctlr->mac_addr_str; + ctlr->imu_input->name = imu_name; + input_set_drvdata(ctlr->imu_input, ctlr); + + /* configure imu axes */ + input_set_abs_params(ctlr->imu_input, ABS_X, + -JC_MAX_ACCEL_MAG, JC_MAX_ACCEL_MAG, + JC_ACCEL_FUZZ, JC_ACCEL_FLAT); + input_set_abs_params(ctlr->imu_input, ABS_Y, + -JC_MAX_ACCEL_MAG, JC_MAX_ACCEL_MAG, + JC_ACCEL_FUZZ, JC_ACCEL_FLAT); + input_set_abs_params(ctlr->imu_input, ABS_Z, + -JC_MAX_ACCEL_MAG, JC_MAX_ACCEL_MAG, + JC_ACCEL_FUZZ, JC_ACCEL_FLAT); + input_abs_set_res(ctlr->imu_input, ABS_X, JC_ACCEL_RES); + input_abs_set_res(ctlr->imu_input, ABS_Y, JC_ACCEL_RES); + input_abs_set_res(ctlr->imu_input, ABS_Z, JC_ACCEL_RES); + + input_set_abs_params(ctlr->imu_input, ABS_RX, + -JC_MAX_GYRO_MAG, JC_MAX_GYRO_MAG, + JC_GYRO_FUZZ, JC_GYRO_FLAT); + input_set_abs_params(ctlr->imu_input, ABS_RY, + -JC_MAX_GYRO_MAG, JC_MAX_GYRO_MAG, + JC_GYRO_FUZZ, JC_GYRO_FLAT); + input_set_abs_params(ctlr->imu_input, ABS_RZ, + -JC_MAX_GYRO_MAG, JC_MAX_GYRO_MAG, + JC_GYRO_FUZZ, JC_GYRO_FLAT); + + input_abs_set_res(ctlr->imu_input, ABS_RX, JC_GYRO_RES); + input_abs_set_res(ctlr->imu_input, ABS_RY, JC_GYRO_RES); + input_abs_set_res(ctlr->imu_input, ABS_RZ, JC_GYRO_RES); + + __set_bit(EV_MSC, ctlr->imu_input->evbit); + __set_bit(MSC_TIMESTAMP, ctlr->imu_input->mscbit); + __set_bit(INPUT_PROP_ACCELEROMETER, ctlr->imu_input->propbit); + + ret = input_register_device(ctlr->imu_input); + if (ret) + return ret; + return 0; } @@ -1467,7 +1701,7 @@ static int joycon_read_info(struct joycon_ctlr *ctlr) report = (struct joycon_input_report *)ctlr->input_buf; for (i = 4, j = 0; j < 6; i++, j++) - ctlr->mac_addr[j] = report->reply.data[i]; + ctlr->mac_addr[j] = report->subcmd_reply.data[i]; ctlr->mac_addr_str = devm_kasprintf(&ctlr->hdev->dev, GFP_KERNEL, "%02X:%02X:%02X:%02X:%02X:%02X", @@ -1482,7 +1716,7 @@ static int joycon_read_info(struct joycon_ctlr *ctlr) hid_info(ctlr->hdev, "controller MAC = %s\n", ctlr->mac_addr_str); /* Retrieve the type so we can distinguish for charging grip */ - ctlr->ctlr_type = report->reply.data[2]; + ctlr->ctlr_type = report->subcmd_reply.data[2]; return 0; } @@ -1525,7 +1759,7 @@ static int joycon_ctlr_handle_event(struct joycon_ctlr *ctlr, u8 *data, data[0] != JC_INPUT_SUBCMD_REPLY) break; report = (struct joycon_input_report *)data; - if (report->reply.id == ctlr->subcmd_ack_match) + if (report->subcmd_reply.id == ctlr->subcmd_ack_match) match = true; break; default: @@ -1655,6 +1889,16 @@ static int nintendo_hid_probe(struct hid_device *hdev, hid_warn(hdev, "Analog stick positions may be inaccurate\n"); } + /* get IMU calibration data, and parse it */ + ret = joycon_request_imu_calibration(ctlr); + if (ret) { + /* + * We can function with default calibration, but it may be + * inaccurate. Provide a warning, and continue on. + */ + hid_warn(hdev, "Unable to read IMU calibration data\n"); + } + /* Set the reporting mode to 0x30, which is the full report mode */ ret = joycon_set_report_mode(ctlr); if (ret) { @@ -1669,6 +1913,13 @@ static int nintendo_hid_probe(struct hid_device *hdev, goto err_mutex; } + /* Enable the IMU */ + ret = joycon_enable_imu(ctlr, true); + if (ret) { + hid_err(hdev, "Failed to enable the IMU; ret=%d\n", ret); + goto err_mutex; + } + ret = joycon_read_info(ctlr); if (ret) { hid_err(hdev, "Failed to retrieve controller info; ret=%d\n", @@ -1761,3 +2012,4 @@ module_hid_driver(nintendo_hid_driver); MODULE_LICENSE("GPL"); MODULE_AUTHOR("Daniel J. Ogorchock "); MODULE_DESCRIPTION("Driver for Nintendo Switch Controllers"); + diff --git a/drivers/input/joydev.c b/drivers/input/joydev.c index a2b5fbba2d3b..c5ec22e4a17e 100644 --- a/drivers/input/joydev.c +++ b/drivers/input/joydev.c @@ -756,6 +756,12 @@ static void joydev_cleanup(struct joydev *joydev) #define USB_VENDOR_ID_THQ 0x20d6 #define USB_DEVICE_ID_THQ_PS3_UDRAW 0xcb17 +#define USB_VENDOR_ID_NINTENDO 0x057e +#define USB_DEVICE_ID_NINTENDO_JOYCONL 0x2006 +#define USB_DEVICE_ID_NINTENDO_JOYCONR 0x2007 +#define USB_DEVICE_ID_NINTENDO_PROCON 0x2009 +#define USB_DEVICE_ID_NINTENDO_CHRGGRIP 0x200E + #define ACCEL_DEV(vnd, prd) \ { \ .flags = INPUT_DEVICE_ID_MATCH_VENDOR | \ @@ -787,6 +793,10 @@ static const struct input_device_id joydev_blacklist[] = { ACCEL_DEV(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS4_CONTROLLER_2), ACCEL_DEV(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS4_CONTROLLER_DONGLE), ACCEL_DEV(USB_VENDOR_ID_THQ, USB_DEVICE_ID_THQ_PS3_UDRAW), + ACCEL_DEV(USB_VENDOR_ID_NINTENDO, USB_DEVICE_ID_NINTENDO_PROCON), + ACCEL_DEV(USB_VENDOR_ID_NINTENDO, USB_DEVICE_ID_NINTENDO_CHRGGRIP), + ACCEL_DEV(USB_VENDOR_ID_NINTENDO, USB_DEVICE_ID_NINTENDO_JOYCONL), + ACCEL_DEV(USB_VENDOR_ID_NINTENDO, USB_DEVICE_ID_NINTENDO_JOYCONR), { /* sentinel */ } };