[v5,5/5] HID: joycon: add rumble support
diff mbox series

Message ID 20190603060644.10338-6-djogorchock@gmail.com
State New
Delegated to: Jiri Kosina
Headers show
Series
  • HID: joycon
Related show

Commit Message

Daniel J. Ogorchock June 3, 2019, 6:06 a.m. UTC
This patch adds support for controller rumble.

It also switches the report parser to using the joycon_input_report
struct, which makes accessing the fields clearer.

The ff_effect weak magnitude is associated with the pro controller's
right motor (or with a right joy-con). The strong magnitude is
associated with the pro's left motor (or a left joy-con).

The rumble data is sent periodically (currently configured for every 300
milliseconds). If the controller receives no rumble data for too long a
time period, it will stop vibrating. The data is also sent every time
joycon_set_rumble is called to avoid latency of up to 300ms.

Signed-off-by: Daniel J. Ogorchock <djogorchock@gmail.com>
---
 drivers/hid/Kconfig      |  10 ++
 drivers/hid/hid-joycon.c | 347 ++++++++++++++++++++++++++++++++++++++-
 2 files changed, 348 insertions(+), 9 deletions(-)

Comments

Roderick Colenbrander June 4, 2019, 7:29 p.m. UTC | #1
Hi Daniel,

I quickly glanced over a few parts of this patch. I most likely won't
have time for a detailed review this time around (I'm in a very busy
period and travelling a lot the next few weeks).

In any case what caught my eyes was the "also" part of this patch.
Ideally patches should be "atomic" and only really do one thing. In
the build up phase of a driver that's a bit harder of course. However
as of this part in the series you are.

Since none of the driver has been merged, I would honestly suggest to
move those changes to the hid parsing logic to earlier in the patch
series. There is no need for code, which gets immediately wiped away
again.

One little style thing, which I saw as part of the refactor was:
> -               raw_y = hid_field_extract(ctlr->hdev, (data + 10), 4, 12);
> +               raw_x = hid_field_extract(ctlr->hdev,
> +                                         rep->right_stick, 0, 12);

I would keep that on a single line. It still stays within 80
characters and even if it went a few over it is probably fine.

Thanks,
Roderick

On Sun, Jun 2, 2019 at 11:07 PM Daniel J. Ogorchock
<djogorchock@gmail.com> wrote:
>
> This patch adds support for controller rumble.
>
> It also switches the report parser to using the joycon_input_report
> struct, which makes accessing the fields clearer.
>
> The ff_effect weak magnitude is associated with the pro controller's
> right motor (or with a right joy-con). The strong magnitude is
> associated with the pro's left motor (or a left joy-con).
>
> The rumble data is sent periodically (currently configured for every 300
> milliseconds). If the controller receives no rumble data for too long a
> time period, it will stop vibrating. The data is also sent every time
> joycon_set_rumble is called to avoid latency of up to 300ms.
>
> Signed-off-by: Daniel J. Ogorchock <djogorchock@gmail.com>
> ---
>  drivers/hid/Kconfig      |  10 ++
>  drivers/hid/hid-joycon.c | 347 ++++++++++++++++++++++++++++++++++++++-
>  2 files changed, 348 insertions(+), 9 deletions(-)
>
> diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
> index 2ab2fa378eda..d8f7f9b7853a 100644
> --- a/drivers/hid/Kconfig
> +++ b/drivers/hid/Kconfig
> @@ -466,6 +466,16 @@ config HID_JOYCON
>         To compile this driver as a module, choose M here: the
>         module will be called hid-joycon.
>
> +config JOYCON_FF
> +       bool "Nintendo Switch controller force feedback support"
> +       depends on HID_JOYCON
> +       select INPUT_FF_MEMLESS
> +       help
> +       Say Y here if you have a Nintendo Switch controller and want to enable
> +       force feedback support for it. This works for both joy-cons and the pro
> +       controller. For the pro controller, both rumble motors can be controlled
> +       individually.
> +
>  config HID_TWINHAN
>         tristate "Twinhan IR remote control"
>         depends on HID
> diff --git a/drivers/hid/hid-joycon.c b/drivers/hid/hid-joycon.c
> index 277d706ce5f8..6a298f4470c6 100644
> --- a/drivers/hid/hid-joycon.c
> +++ b/drivers/hid/hid-joycon.c
> @@ -9,6 +9,7 @@
>   *   https://gitlab.com/pjranki/joycon-linux-kernel (Peter Rankin)
>   *   https://github.com/FrotBot/SwitchProConLinuxUSB
>   *   https://github.com/MTCKC/ProconXInput
> + *   https://github.com/Davidobot/BetterJoyForCemu
>   *   hid-wiimote kernel hid driver
>   *   hid-logitech-hidpp driver
>   *   hid-sony driver
> @@ -26,6 +27,7 @@
>  #include <linux/device.h>
>  #include <linux/hid.h>
>  #include <linux/input.h>
> +#include <linux/jiffies.h>
>  #include <linux/leds.h>
>  #include <linux/module.h>
>  #include <linux/power_supply.h>
> @@ -105,6 +107,121 @@
>  #define JC_STICK_FUZZ          250
>  #define JC_STICK_FLAT          500
>
> +/* frequency/amplitude tables for rumble */
> +struct joycon_rumble_freq_data {
> +       u16 high;
> +       u8 low;
> +       u16 freq; /* Hz*/
> +};
> +
> +struct joycon_rumble_amp_data {
> +       u8 high;
> +       u16 low;
> +       u16 amp;
> +};
> +
> +/*
> + * These tables are from
> + * https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering/blob/master/rumble_data_table.md
> + */
> +static const struct joycon_rumble_freq_data joycon_rumble_frequencies[] = {
> +       /* high, low, freq */
> +       { 0x0000, 0x01,   41 }, { 0x0000, 0x02,   42 }, { 0x0000, 0x03,   43 },
> +       { 0x0000, 0x04,   44 }, { 0x0000, 0x05,   45 }, { 0x0000, 0x06,   46 },
> +       { 0x0000, 0x07,   47 }, { 0x0000, 0x08,   48 }, { 0x0000, 0x09,   49 },
> +       { 0x0000, 0x0A,   50 }, { 0x0000, 0x0B,   51 }, { 0x0000, 0x0C,   52 },
> +       { 0x0000, 0x0D,   53 }, { 0x0000, 0x0E,   54 }, { 0x0000, 0x0F,   55 },
> +       { 0x0000, 0x10,   57 }, { 0x0000, 0x11,   58 }, { 0x0000, 0x12,   59 },
> +       { 0x0000, 0x13,   60 }, { 0x0000, 0x14,   62 }, { 0x0000, 0x15,   63 },
> +       { 0x0000, 0x16,   64 }, { 0x0000, 0x17,   66 }, { 0x0000, 0x18,   67 },
> +       { 0x0000, 0x19,   69 }, { 0x0000, 0x1A,   70 }, { 0x0000, 0x1B,   72 },
> +       { 0x0000, 0x1C,   73 }, { 0x0000, 0x1D,   75 }, { 0x0000, 0x1e,   77 },
> +       { 0x0000, 0x1f,   78 }, { 0x0000, 0x20,   80 }, { 0x0400, 0x21,   82 },
> +       { 0x0800, 0x22,   84 }, { 0x0c00, 0x23,   85 }, { 0x1000, 0x24,   87 },
> +       { 0x1400, 0x25,   89 }, { 0x1800, 0x26,   91 }, { 0x1c00, 0x27,   93 },
> +       { 0x2000, 0x28,   95 }, { 0x2400, 0x29,   97 }, { 0x2800, 0x2a,   99 },
> +       { 0x2c00, 0x2b,  102 }, { 0x3000, 0x2c,  104 }, { 0x3400, 0x2d,  106 },
> +       { 0x3800, 0x2e,  108 }, { 0x3c00, 0x2f,  111 }, { 0x4000, 0x30,  113 },
> +       { 0x4400, 0x31,  116 }, { 0x4800, 0x32,  118 }, { 0x4c00, 0x33,  121 },
> +       { 0x5000, 0x34,  123 }, { 0x5400, 0x35,  126 }, { 0x5800, 0x36,  129 },
> +       { 0x5c00, 0x37,  132 }, { 0x6000, 0x38,  135 }, { 0x6400, 0x39,  137 },
> +       { 0x6800, 0x3a,  141 }, { 0x6c00, 0x3b,  144 }, { 0x7000, 0x3c,  147 },
> +       { 0x7400, 0x3d,  150 }, { 0x7800, 0x3e,  153 }, { 0x7c00, 0x3f,  157 },
> +       { 0x8000, 0x40,  160 }, { 0x8400, 0x41,  164 }, { 0x8800, 0x42,  167 },
> +       { 0x8c00, 0x43,  171 }, { 0x9000, 0x44,  174 }, { 0x9400, 0x45,  178 },
> +       { 0x9800, 0x46,  182 }, { 0x9c00, 0x47,  186 }, { 0xa000, 0x48,  190 },
> +       { 0xa400, 0x49,  194 }, { 0xa800, 0x4a,  199 }, { 0xac00, 0x4b,  203 },
> +       { 0xb000, 0x4c,  207 }, { 0xb400, 0x4d,  212 }, { 0xb800, 0x4e,  217 },
> +       { 0xbc00, 0x4f,  221 }, { 0xc000, 0x50,  226 }, { 0xc400, 0x51,  231 },
> +       { 0xc800, 0x52,  236 }, { 0xcc00, 0x53,  241 }, { 0xd000, 0x54,  247 },
> +       { 0xd400, 0x55,  252 }, { 0xd800, 0x56,  258 }, { 0xdc00, 0x57,  263 },
> +       { 0xe000, 0x58,  269 }, { 0xe400, 0x59,  275 }, { 0xe800, 0x5a,  281 },
> +       { 0xec00, 0x5b,  287 }, { 0xf000, 0x5c,  293 }, { 0xf400, 0x5d,  300 },
> +       { 0xf800, 0x5e,  306 }, { 0xfc00, 0x5f,  313 }, { 0x0001, 0x60,  320 },
> +       { 0x0401, 0x61,  327 }, { 0x0801, 0x62,  334 }, { 0x0c01, 0x63,  341 },
> +       { 0x1001, 0x64,  349 }, { 0x1401, 0x65,  357 }, { 0x1801, 0x66,  364 },
> +       { 0x1c01, 0x67,  372 }, { 0x2001, 0x68,  381 }, { 0x2401, 0x69,  389 },
> +       { 0x2801, 0x6a,  397 }, { 0x2c01, 0x6b,  406 }, { 0x3001, 0x6c,  415 },
> +       { 0x3401, 0x6d,  424 }, { 0x3801, 0x6e,  433 }, { 0x3c01, 0x6f,  443 },
> +       { 0x4001, 0x70,  453 }, { 0x4401, 0x71,  462 }, { 0x4801, 0x72,  473 },
> +       { 0x4c01, 0x73,  483 }, { 0x5001, 0x74,  494 }, { 0x5401, 0x75,  504 },
> +       { 0x5801, 0x76,  515 }, { 0x5c01, 0x77,  527 }, { 0x6001, 0x78,  538 },
> +       { 0x6401, 0x79,  550 }, { 0x6801, 0x7a,  562 }, { 0x6c01, 0x7b,  574 },
> +       { 0x7001, 0x7c,  587 }, { 0x7401, 0x7d,  600 }, { 0x7801, 0x7e,  613 },
> +       { 0x7c01, 0x7f,  626 }, { 0x8001, 0x00,  640 }, { 0x8401, 0x00,  654 },
> +       { 0x8801, 0x00,  668 }, { 0x8c01, 0x00,  683 }, { 0x9001, 0x00,  698 },
> +       { 0x9401, 0x00,  713 }, { 0x9801, 0x00,  729 }, { 0x9c01, 0x00,  745 },
> +       { 0xa001, 0x00,  761 }, { 0xa401, 0x00,  778 }, { 0xa801, 0x00,  795 },
> +       { 0xac01, 0x00,  812 }, { 0xb001, 0x00,  830 }, { 0xb401, 0x00,  848 },
> +       { 0xb801, 0x00,  867 }, { 0xbc01, 0x00,  886 }, { 0xc001, 0x00,  905 },
> +       { 0xc401, 0x00,  925 }, { 0xc801, 0x00,  945 }, { 0xcc01, 0x00,  966 },
> +       { 0xd001, 0x00,  987 }, { 0xd401, 0x00, 1009 }, { 0xd801, 0x00, 1031 },
> +       { 0xdc01, 0x00, 1053 }, { 0xe001, 0x00, 1076 }, { 0xe401, 0x00, 1100 },
> +       { 0xe801, 0x00, 1124 }, { 0xec01, 0x00, 1149 }, { 0xf001, 0x00, 1174 },
> +       { 0xf401, 0x00, 1199 }, { 0xf801, 0x00, 1226 }, { 0xfc01, 0x00, 1253 }
> +};
> +
> +static const struct joycon_rumble_amp_data joycon_rumble_amplitudes[] = {
> +       /* high, low, amp */
> +       { 0x00, 0x0040,    0 },
> +       { 0x02, 0x8040,   10 }, { 0x04, 0x0041,   12 }, { 0x06, 0x8041,   14 },
> +       { 0x08, 0x0042,   17 }, { 0x0a, 0x8042,   20 }, { 0x0c, 0x0043,   24 },
> +       { 0x0e, 0x8043,   28 }, { 0x10, 0x0044,   33 }, { 0x12, 0x8044,   40 },
> +       { 0x14, 0x0045,   47 }, { 0x16, 0x8045,   56 }, { 0x18, 0x0046,   67 },
> +       { 0x1a, 0x8046,   80 }, { 0x1c, 0x0047,   95 }, { 0x1e, 0x8047,  112 },
> +       { 0x20, 0x0048,  117 }, { 0x22, 0x8048,  123 }, { 0x24, 0x0049,  128 },
> +       { 0x26, 0x8049,  134 }, { 0x28, 0x004a,  140 }, { 0x2a, 0x804a,  146 },
> +       { 0x2c, 0x004b,  152 }, { 0x2e, 0x804b,  159 }, { 0x30, 0x004c,  166 },
> +       { 0x32, 0x804c,  173 }, { 0x34, 0x004d,  181 }, { 0x36, 0x804d,  189 },
> +       { 0x38, 0x004e,  198 }, { 0x3a, 0x804e,  206 }, { 0x3c, 0x004f,  215 },
> +       { 0x3e, 0x804f,  225 }, { 0x40, 0x0050,  230 }, { 0x42, 0x8050,  235 },
> +       { 0x44, 0x0051,  240 }, { 0x46, 0x8051,  245 }, { 0x48, 0x0052,  251 },
> +       { 0x4a, 0x8052,  256 }, { 0x4c, 0x0053,  262 }, { 0x4e, 0x8053,  268 },
> +       { 0x50, 0x0054,  273 }, { 0x52, 0x8054,  279 }, { 0x54, 0x0055,  286 },
> +       { 0x56, 0x8055,  292 }, { 0x58, 0x0056,  298 }, { 0x5a, 0x8056,  305 },
> +       { 0x5c, 0x0057,  311 }, { 0x5e, 0x8057,  318 }, { 0x60, 0x0058,  325 },
> +       { 0x62, 0x8058,  332 }, { 0x64, 0x0059,  340 }, { 0x66, 0x8059,  347 },
> +       { 0x68, 0x005a,  355 }, { 0x6a, 0x805a,  362 }, { 0x6c, 0x005b,  370 },
> +       { 0x6e, 0x805b,  378 }, { 0x70, 0x005c,  387 }, { 0x72, 0x805c,  395 },
> +       { 0x74, 0x005d,  404 }, { 0x76, 0x805d,  413 }, { 0x78, 0x005e,  422 },
> +       { 0x7a, 0x805e,  431 }, { 0x7c, 0x005f,  440 }, { 0x7e, 0x805f,  450 },
> +       { 0x80, 0x0060,  460 }, { 0x82, 0x8060,  470 }, { 0x84, 0x0061,  480 },
> +       { 0x86, 0x8061,  491 }, { 0x88, 0x0062,  501 }, { 0x8a, 0x8062,  512 },
> +       { 0x8c, 0x0063,  524 }, { 0x8e, 0x8063,  535 }, { 0x90, 0x0064,  547 },
> +       { 0x92, 0x8064,  559 }, { 0x94, 0x0065,  571 }, { 0x96, 0x8065,  584 },
> +       { 0x98, 0x0066,  596 }, { 0x9a, 0x8066,  609 }, { 0x9c, 0x0067,  623 },
> +       { 0x9e, 0x8067,  636 }, { 0xa0, 0x0068,  650 }, { 0xa2, 0x8068,  665 },
> +       { 0xa4, 0x0069,  679 }, { 0xa6, 0x8069,  694 }, { 0xa8, 0x006a,  709 },
> +       { 0xaa, 0x806a,  725 }, { 0xac, 0x006b,  741 }, { 0xae, 0x806b,  757 },
> +       { 0xb0, 0x006c,  773 }, { 0xb2, 0x806c,  790 }, { 0xb4, 0x006d,  808 },
> +       { 0xb6, 0x806d,  825 }, { 0xb8, 0x006e,  843 }, { 0xba, 0x806e,  862 },
> +       { 0xbc, 0x006f,  881 }, { 0xbe, 0x806f,  900 }, { 0xc0, 0x0070,  920 },
> +       { 0xc2, 0x8070,  940 }, { 0xc4, 0x0071,  960 }, { 0xc6, 0x8071,  981 },
> +       { 0xc8, 0x0072, 1003 }
> +};
> +static const u16 joycon_max_rumble_amp =
> +joycon_rumble_amplitudes[ARRAY_SIZE(joycon_rumble_amplitudes) - 1].amp;
> +
>  /* States for controller state machine */
>  enum joycon_ctlr_state {
>         JOYCON_CTLR_STATE_INIT,
> @@ -182,6 +299,9 @@ struct joycon_input_report {
>
>  #define JC_MAX_RESP_SIZE (sizeof(struct joycon_input_report) + 35)
>  #define JC_NUM_LEDS 4
> +#define JC_RUMBLE_DFLT_LOW_FREQ 160
> +#define JC_RUMBLE_DFLT_HIGH_FREQ 320
> +#define JC_RUMBLE_PERIOD_MS 300
>
>  /* Each physical controller is associated with a joycon_ctlr struct */
>  struct joycon_ctlr {
> @@ -214,6 +334,17 @@ struct joycon_ctlr {
>         u8 battery_capacity;
>         bool battery_charging;
>         bool host_powered;
> +
> +       /* rumble */
> +       u8 rumble_data[8];
> +       struct work_struct rumble_worker;
> +       unsigned int rumble_msecs;
> +       u16 rumble_ll_freq;
> +       u16 rumble_lh_freq;
> +       u16 rumble_rl_freq;
> +       u16 rumble_rh_freq;
> +       u16 rumble_l_amp;
> +       u16 rumble_r_amp;
>  };
>
>  static int __joycon_hid_send(struct hid_device *hdev, u8 *data, size_t len)
> @@ -270,6 +401,12 @@ static int joycon_send_subcmd(struct joycon_ctlr *ctlr,
>                               size_t data_len)
>  {
>         int ret;
> +       unsigned long flags;
> +
> +       spin_lock_irqsave(&ctlr->lock, flags);
> +       memcpy(subcmd->rumble_data, ctlr->rumble_data,
> +              sizeof(subcmd->rumble_data));
> +       spin_unlock_irqrestore(&ctlr->lock, flags);
>
>         subcmd->output_id = JC_OUTPUT_RUMBLE_AND_SUBCMD;
>         subcmd->packet_num = ctlr->subcmd_num;
> @@ -279,9 +416,11 @@ static int joycon_send_subcmd(struct joycon_ctlr *ctlr,
>         ctlr->msg_type = JOYCON_MSG_TYPE_SUBCMD;
>
>         ret = joycon_hid_send_sync(ctlr, (u8 *)subcmd,
> -                                     sizeof(*subcmd) + data_len);
> -       if (ret)
> +                                  sizeof(*subcmd) + data_len);
> +       if (ret < 0)
>                 hid_dbg(ctlr->hdev, "send subcommand failed; ret=%d\n", ret);
> +       else
> +               ret = 0;
>         return ret;
>  }
>
> @@ -396,6 +535,19 @@ static int joycon_set_report_mode(struct joycon_ctlr *ctlr)
>         return joycon_send_subcmd(ctlr, req, 1);
>  }
>
> +static int joycon_enable_rumble(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_VIBRATION;
> +       req->data[0] = enable ? 0x01 : 0x00;
> +
> +       hid_dbg(ctlr->hdev, "%s rumble\n", enable ? "enabling" : "disabling");
> +       return joycon_send_subcmd(ctlr, req, 1);
> +}
> +
>  static int joycon_map_stick_val(struct joycon_stick_cal *cal, s32 val)
>  {
>         s32 center = cal->center;
> @@ -421,10 +573,16 @@ static void joycon_parse_report(struct joycon_ctlr *ctlr, u8 *data)
>         u8 tmp;
>         u32 btns;
>         u32 id = ctlr->hdev->product;
> +       struct joycon_input_report *rep = (struct joycon_input_report *) data;
> +       unsigned long msecs = jiffies_to_msecs(jiffies);
>
> -       /* Parse the battery status */
> -       tmp = data[2];
>         spin_lock_irqsave(&ctlr->lock, flags);
> +       if (rep->vibrator_report &&
> +           (msecs - ctlr->rumble_msecs) >= JC_RUMBLE_PERIOD_MS)
> +               schedule_work(&ctlr->rumble_worker);
> +
> +       /* Parse the battery status */
> +       tmp = rep->bat_con;
>         ctlr->host_powered = tmp & BIT(0);
>         ctlr->battery_charging = tmp & BIT(4);
>         tmp = tmp >> 5;
> @@ -452,7 +610,7 @@ static void joycon_parse_report(struct joycon_ctlr *ctlr, u8 *data)
>         spin_unlock_irqrestore(&ctlr->lock, flags);
>
>         /* Parse the buttons and sticks */
> -       btns = hid_field_extract(ctlr->hdev, data + 3, 0, 24);
> +       btns = hid_field_extract(ctlr->hdev, rep->button_status, 0, 24);
>
>         if (id != USB_DEVICE_ID_NINTENDO_JOYCONR) {
>                 u16 raw_x;
> @@ -461,8 +619,10 @@ static void joycon_parse_report(struct joycon_ctlr *ctlr, u8 *data)
>                 int y;
>
>                 /* get raw stick values */
> -               raw_x = hid_field_extract(ctlr->hdev, (data + 6), 0, 12);
> -               raw_y = hid_field_extract(ctlr->hdev, (data + 7), 4, 12);
> +               raw_x = hid_field_extract(ctlr->hdev,
> +                                         rep->left_stick, 0, 12);
> +               raw_y = hid_field_extract(ctlr->hdev,
> +                                         rep->left_stick + 1, 4, 12);
>                 /* map the stick values */
>                 x = joycon_map_stick_val(&ctlr->left_stick_cal_x, raw_x);
>                 y = -joycon_map_stick_val(&ctlr->left_stick_cal_y, raw_y);
> @@ -493,8 +653,10 @@ static void joycon_parse_report(struct joycon_ctlr *ctlr, u8 *data)
>                 int y;
>
>                 /* get raw stick values */
> -               raw_x = hid_field_extract(ctlr->hdev, (data + 9), 0, 12);
> -               raw_y = hid_field_extract(ctlr->hdev, (data + 10), 4, 12);
> +               raw_x = hid_field_extract(ctlr->hdev,
> +                                         rep->right_stick, 0, 12);
> +               raw_y = hid_field_extract(ctlr->hdev,
> +                                         rep->right_stick + 1, 4, 12);
>                 /* map stick values */
>                 x = joycon_map_stick_val(&ctlr->right_stick_cal_x, raw_x);
>                 y = -joycon_map_stick_val(&ctlr->right_stick_cal_y, raw_y);
> @@ -523,6 +685,151 @@ static void joycon_parse_report(struct joycon_ctlr *ctlr, u8 *data)
>         input_sync(dev);
>  }
>
> +static void joycon_rumble_worker(struct work_struct *work)
> +{
> +       struct joycon_ctlr *ctlr = container_of(work, struct joycon_ctlr,
> +                                                       rumble_worker);
> +       unsigned long flags;
> +       int ret;
> +
> +       spin_lock_irqsave(&ctlr->lock, flags);
> +       ctlr->rumble_msecs = jiffies_to_msecs(jiffies);
> +       spin_unlock_irqrestore(&ctlr->lock, flags);
> +
> +       mutex_lock(&ctlr->output_mutex);
> +       ret = joycon_enable_rumble(ctlr, true);
> +       mutex_unlock(&ctlr->output_mutex);
> +       if (ret < 0)
> +               hid_warn(ctlr->hdev, "Failed to set rumble data; ret=%d", ret);
> +}
> +
> +static struct joycon_rumble_freq_data joycon_find_rumble_freq(u16 freq)
> +{
> +       const size_t length = ARRAY_SIZE(joycon_rumble_frequencies);
> +       const struct joycon_rumble_freq_data *data = joycon_rumble_frequencies;
> +       int i = 0;
> +
> +       if (freq > data[0].freq) {
> +               for (i = 1; i < length - 1; i++) {
> +                       if (freq > data[i - 1].freq && freq <= data[i].freq)
> +                               break;
> +               }
> +       }
> +
> +       return data[i];
> +}
> +
> +static struct joycon_rumble_amp_data joycon_find_rumble_amp(u16 amp)
> +{
> +       const size_t length = ARRAY_SIZE(joycon_rumble_amplitudes);
> +       const struct joycon_rumble_amp_data *data = joycon_rumble_amplitudes;
> +       int i = 0;
> +
> +       if (amp > data[0].amp) {
> +               for (i = 1; i < length - 1; i++) {
> +                       if (amp > data[i - 1].amp && amp <= data[i].amp)
> +                               break;
> +               }
> +       }
> +
> +       return data[i];
> +}
> +
> +static void joycon_encode_rumble(u8 *data, u16 freq_low, u16 freq_high, u16 amp)
> +{
> +       struct joycon_rumble_freq_data freq_data_low;
> +       struct joycon_rumble_freq_data freq_data_high;
> +       struct joycon_rumble_amp_data amp_data;
> +
> +       freq_data_low = joycon_find_rumble_freq(freq_low);
> +       freq_data_high = joycon_find_rumble_freq(freq_high);
> +       amp_data = joycon_find_rumble_amp(amp);
> +
> +       data[0] = (freq_data_high.high >> 8) & 0xFF;
> +       data[1] = (freq_data_high.high & 0xFF) + amp_data.high;
> +       data[2] = freq_data_low.low + ((amp_data.low >> 8) & 0xFF);
> +       data[3] = amp_data.low & 0xFF;
> +}
> +
> +#define JOYCON_MAX_RUMBLE_HIGH_FREQ    ((u16) 1253)
> +#define JOYCON_MIN_RUMBLE_HIGH_FREQ    ((u16) 82)
> +#define JOYCON_MAX_RUMBLE_LOW_FREQ     ((u16) 626)
> +#define JOYCON_MIN_RUMBLE_LOW_FREQ     ((u16) 41)
> +
> +static void joycon_clamp_rumble_freqs(struct joycon_ctlr *ctlr)
> +{
> +       unsigned long flags;
> +
> +       spin_lock_irqsave(&ctlr->lock, flags);
> +       ctlr->rumble_ll_freq = clamp(ctlr->rumble_ll_freq,
> +                                    JOYCON_MIN_RUMBLE_LOW_FREQ,
> +                                    JOYCON_MAX_RUMBLE_LOW_FREQ);
> +       ctlr->rumble_lh_freq = clamp(ctlr->rumble_lh_freq,
> +                                    JOYCON_MIN_RUMBLE_HIGH_FREQ,
> +                                    JOYCON_MAX_RUMBLE_HIGH_FREQ);
> +       ctlr->rumble_rl_freq = clamp(ctlr->rumble_rl_freq,
> +                                    JOYCON_MIN_RUMBLE_LOW_FREQ,
> +                                    JOYCON_MAX_RUMBLE_LOW_FREQ);
> +       ctlr->rumble_rh_freq = clamp(ctlr->rumble_rh_freq,
> +                                    JOYCON_MIN_RUMBLE_HIGH_FREQ,
> +                                    JOYCON_MAX_RUMBLE_HIGH_FREQ);
> +       spin_unlock_irqrestore(&ctlr->lock, flags);
> +}
> +
> +static int joycon_set_rumble(struct joycon_ctlr *ctlr, u16 amp_r, u16 amp_l,
> +                            bool schedule_now)
> +{
> +       u8 data[8];
> +       u16 amp;
> +       u16 freq_r_low;
> +       u16 freq_r_high;
> +       u16 freq_l_low;
> +       u16 freq_l_high;
> +       unsigned long flags;
> +
> +       spin_lock_irqsave(&ctlr->lock, flags);
> +       freq_r_low = ctlr->rumble_rl_freq;
> +       freq_r_high = ctlr->rumble_rh_freq;
> +       freq_l_low = ctlr->rumble_ll_freq;
> +       freq_l_high = ctlr->rumble_lh_freq;
> +       spin_unlock_irqrestore(&ctlr->lock, flags);
> +
> +       /* right joy-con */
> +       amp = amp_r * (u32)joycon_max_rumble_amp / 65535;
> +       ctlr->rumble_r_amp = amp;
> +       joycon_encode_rumble(data + 4, freq_r_low, freq_r_high, amp);
> +
> +       /* left joy-con */
> +       amp = amp_l * (u32)joycon_max_rumble_amp / 65535;
> +       ctlr->rumble_l_amp = amp;
> +       joycon_encode_rumble(data, freq_l_low, freq_l_high, amp);
> +
> +       spin_lock_irqsave(&ctlr->lock, flags);
> +       memcpy(ctlr->rumble_data, data, sizeof(ctlr->rumble_data));
> +       spin_unlock_irqrestore(&ctlr->lock, flags);
> +
> +       /* don't wait for the periodic send (reduces latency) */
> +       if (schedule_now)
> +               schedule_work(&ctlr->rumble_worker);
> +
> +       return 0;
> +}
> +
> +#ifdef CONFIG_JOYCON_FF
> +static int joycon_play_effect(struct input_dev *dev, void *data,
> +                                                    struct ff_effect *effect)
> +{
> +       struct joycon_ctlr *ctlr = input_get_drvdata(dev);
> +
> +       if (effect->type != FF_RUMBLE)
> +               return 0;
> +
> +       return joycon_set_rumble(ctlr,
> +                                effect->u.rumble.weak_magnitude,
> +                                effect->u.rumble.strong_magnitude,
> +                                true);
> +}
> +#endif
>
>  static const unsigned int joycon_button_inputs[] = {
>         BTN_SELECT, BTN_Z, BTN_THUMBL,
> @@ -590,6 +897,19 @@ static int joycon_input_create(struct joycon_ctlr *ctlr)
>                 input_set_capability(ctlr->input, EV_KEY,
>                                      joycon_button_inputs[i]);
>
> +#ifdef CONFIG_JOYCON_FF
> +       /* set up rumble */
> +       input_set_capability(ctlr->input, EV_FF, FF_RUMBLE);
> +       input_ff_create_memless(ctlr->input, NULL, joycon_play_effect);
> +       ctlr->rumble_ll_freq = JC_RUMBLE_DFLT_LOW_FREQ;
> +       ctlr->rumble_lh_freq = JC_RUMBLE_DFLT_HIGH_FREQ;
> +       ctlr->rumble_rl_freq = JC_RUMBLE_DFLT_LOW_FREQ;
> +       ctlr->rumble_rh_freq = JC_RUMBLE_DFLT_HIGH_FREQ;
> +       joycon_clamp_rumble_freqs(ctlr);
> +       joycon_set_rumble(ctlr, 0, 0, false);
> +       ctlr->rumble_msecs = jiffies_to_msecs(jiffies);
> +#endif
> +
>         ret = input_register_device(ctlr->input);
>         if (ret)
>                 goto err_input;
> @@ -956,6 +1276,7 @@ static int joycon_hid_probe(struct hid_device *hdev,
>         mutex_init(&ctlr->output_mutex);
>         init_waitqueue_head(&ctlr->wait);
>         spin_lock_init(&ctlr->lock);
> +       INIT_WORK(&ctlr->rumble_worker, joycon_rumble_worker);
>
>         ret = hid_parse(hdev);
>         if (ret) {
> @@ -1009,6 +1330,13 @@ static int joycon_hid_probe(struct hid_device *hdev,
>                 goto err_mutex;
>         }
>
> +       /* Enable rumble */
> +       ret = joycon_enable_rumble(ctlr, true);
> +       if (ret) {
> +               hid_err(hdev, "Failed to enable rumble; ret=%d\n", ret);
> +               goto err_mutex;
> +       }
> +
>         mutex_unlock(&ctlr->output_mutex);
>
>         ret = joycon_input_create(ctlr);
> @@ -1055,6 +1383,7 @@ static void joycon_hid_remove(struct hid_device *hdev)
>         hid_dbg(hdev, "remove\n");
>         hid_hw_close(hdev);
>         hid_hw_stop(hdev);
> +
>         joycon_ctlr_destroy(ctlr);
>  }
>
> --
> 2.21.0
>

Patch
diff mbox series

diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index 2ab2fa378eda..d8f7f9b7853a 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -466,6 +466,16 @@  config HID_JOYCON
 	To compile this driver as a module, choose M here: the
 	module will be called hid-joycon.
 
+config JOYCON_FF
+	bool "Nintendo Switch controller force feedback support"
+	depends on HID_JOYCON
+	select INPUT_FF_MEMLESS
+	help
+	Say Y here if you have a Nintendo Switch controller and want to enable
+	force feedback support for it. This works for both joy-cons and the pro
+	controller. For the pro controller, both rumble motors can be controlled
+	individually.
+
 config HID_TWINHAN
 	tristate "Twinhan IR remote control"
 	depends on HID
diff --git a/drivers/hid/hid-joycon.c b/drivers/hid/hid-joycon.c
index 277d706ce5f8..6a298f4470c6 100644
--- a/drivers/hid/hid-joycon.c
+++ b/drivers/hid/hid-joycon.c
@@ -9,6 +9,7 @@ 
  *   https://gitlab.com/pjranki/joycon-linux-kernel (Peter Rankin)
  *   https://github.com/FrotBot/SwitchProConLinuxUSB
  *   https://github.com/MTCKC/ProconXInput
+ *   https://github.com/Davidobot/BetterJoyForCemu
  *   hid-wiimote kernel hid driver
  *   hid-logitech-hidpp driver
  *   hid-sony driver
@@ -26,6 +27,7 @@ 
 #include <linux/device.h>
 #include <linux/hid.h>
 #include <linux/input.h>
+#include <linux/jiffies.h>
 #include <linux/leds.h>
 #include <linux/module.h>
 #include <linux/power_supply.h>
@@ -105,6 +107,121 @@ 
 #define JC_STICK_FUZZ		250
 #define JC_STICK_FLAT		500
 
+/* frequency/amplitude tables for rumble */
+struct joycon_rumble_freq_data {
+	u16 high;
+	u8 low;
+	u16 freq; /* Hz*/
+};
+
+struct joycon_rumble_amp_data {
+	u8 high;
+	u16 low;
+	u16 amp;
+};
+
+/*
+ * These tables are from
+ * https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering/blob/master/rumble_data_table.md
+ */
+static const struct joycon_rumble_freq_data joycon_rumble_frequencies[] = {
+	/* high, low, freq */
+	{ 0x0000, 0x01,   41 }, { 0x0000, 0x02,   42 }, { 0x0000, 0x03,   43 },
+	{ 0x0000, 0x04,   44 }, { 0x0000, 0x05,   45 }, { 0x0000, 0x06,   46 },
+	{ 0x0000, 0x07,   47 }, { 0x0000, 0x08,   48 }, { 0x0000, 0x09,   49 },
+	{ 0x0000, 0x0A,   50 }, { 0x0000, 0x0B,   51 }, { 0x0000, 0x0C,   52 },
+	{ 0x0000, 0x0D,   53 }, { 0x0000, 0x0E,   54 }, { 0x0000, 0x0F,   55 },
+	{ 0x0000, 0x10,   57 }, { 0x0000, 0x11,   58 }, { 0x0000, 0x12,   59 },
+	{ 0x0000, 0x13,   60 }, { 0x0000, 0x14,   62 }, { 0x0000, 0x15,   63 },
+	{ 0x0000, 0x16,   64 }, { 0x0000, 0x17,   66 }, { 0x0000, 0x18,   67 },
+	{ 0x0000, 0x19,   69 }, { 0x0000, 0x1A,   70 }, { 0x0000, 0x1B,   72 },
+	{ 0x0000, 0x1C,   73 }, { 0x0000, 0x1D,   75 }, { 0x0000, 0x1e,   77 },
+	{ 0x0000, 0x1f,   78 }, { 0x0000, 0x20,   80 }, { 0x0400, 0x21,   82 },
+	{ 0x0800, 0x22,   84 }, { 0x0c00, 0x23,   85 }, { 0x1000, 0x24,   87 },
+	{ 0x1400, 0x25,   89 }, { 0x1800, 0x26,   91 }, { 0x1c00, 0x27,   93 },
+	{ 0x2000, 0x28,   95 }, { 0x2400, 0x29,   97 }, { 0x2800, 0x2a,   99 },
+	{ 0x2c00, 0x2b,  102 }, { 0x3000, 0x2c,  104 }, { 0x3400, 0x2d,  106 },
+	{ 0x3800, 0x2e,  108 }, { 0x3c00, 0x2f,  111 }, { 0x4000, 0x30,  113 },
+	{ 0x4400, 0x31,  116 }, { 0x4800, 0x32,  118 }, { 0x4c00, 0x33,  121 },
+	{ 0x5000, 0x34,  123 }, { 0x5400, 0x35,  126 }, { 0x5800, 0x36,  129 },
+	{ 0x5c00, 0x37,  132 }, { 0x6000, 0x38,  135 }, { 0x6400, 0x39,  137 },
+	{ 0x6800, 0x3a,  141 }, { 0x6c00, 0x3b,  144 }, { 0x7000, 0x3c,  147 },
+	{ 0x7400, 0x3d,  150 }, { 0x7800, 0x3e,  153 }, { 0x7c00, 0x3f,  157 },
+	{ 0x8000, 0x40,  160 }, { 0x8400, 0x41,  164 }, { 0x8800, 0x42,  167 },
+	{ 0x8c00, 0x43,  171 }, { 0x9000, 0x44,  174 }, { 0x9400, 0x45,  178 },
+	{ 0x9800, 0x46,  182 }, { 0x9c00, 0x47,  186 }, { 0xa000, 0x48,  190 },
+	{ 0xa400, 0x49,  194 }, { 0xa800, 0x4a,  199 }, { 0xac00, 0x4b,  203 },
+	{ 0xb000, 0x4c,  207 }, { 0xb400, 0x4d,  212 }, { 0xb800, 0x4e,  217 },
+	{ 0xbc00, 0x4f,  221 }, { 0xc000, 0x50,  226 }, { 0xc400, 0x51,  231 },
+	{ 0xc800, 0x52,  236 }, { 0xcc00, 0x53,  241 }, { 0xd000, 0x54,  247 },
+	{ 0xd400, 0x55,  252 }, { 0xd800, 0x56,  258 }, { 0xdc00, 0x57,  263 },
+	{ 0xe000, 0x58,  269 }, { 0xe400, 0x59,  275 }, { 0xe800, 0x5a,  281 },
+	{ 0xec00, 0x5b,  287 }, { 0xf000, 0x5c,  293 }, { 0xf400, 0x5d,  300 },
+	{ 0xf800, 0x5e,  306 }, { 0xfc00, 0x5f,  313 }, { 0x0001, 0x60,  320 },
+	{ 0x0401, 0x61,  327 }, { 0x0801, 0x62,  334 }, { 0x0c01, 0x63,  341 },
+	{ 0x1001, 0x64,  349 }, { 0x1401, 0x65,  357 }, { 0x1801, 0x66,  364 },
+	{ 0x1c01, 0x67,  372 }, { 0x2001, 0x68,  381 }, { 0x2401, 0x69,  389 },
+	{ 0x2801, 0x6a,  397 }, { 0x2c01, 0x6b,  406 }, { 0x3001, 0x6c,  415 },
+	{ 0x3401, 0x6d,  424 }, { 0x3801, 0x6e,  433 }, { 0x3c01, 0x6f,  443 },
+	{ 0x4001, 0x70,  453 }, { 0x4401, 0x71,  462 }, { 0x4801, 0x72,  473 },
+	{ 0x4c01, 0x73,  483 }, { 0x5001, 0x74,  494 }, { 0x5401, 0x75,  504 },
+	{ 0x5801, 0x76,  515 }, { 0x5c01, 0x77,  527 }, { 0x6001, 0x78,  538 },
+	{ 0x6401, 0x79,  550 }, { 0x6801, 0x7a,  562 }, { 0x6c01, 0x7b,  574 },
+	{ 0x7001, 0x7c,  587 }, { 0x7401, 0x7d,  600 }, { 0x7801, 0x7e,  613 },
+	{ 0x7c01, 0x7f,  626 }, { 0x8001, 0x00,  640 }, { 0x8401, 0x00,  654 },
+	{ 0x8801, 0x00,  668 }, { 0x8c01, 0x00,  683 }, { 0x9001, 0x00,  698 },
+	{ 0x9401, 0x00,  713 }, { 0x9801, 0x00,  729 }, { 0x9c01, 0x00,  745 },
+	{ 0xa001, 0x00,  761 }, { 0xa401, 0x00,  778 }, { 0xa801, 0x00,  795 },
+	{ 0xac01, 0x00,  812 }, { 0xb001, 0x00,  830 }, { 0xb401, 0x00,  848 },
+	{ 0xb801, 0x00,  867 }, { 0xbc01, 0x00,  886 }, { 0xc001, 0x00,  905 },
+	{ 0xc401, 0x00,  925 }, { 0xc801, 0x00,  945 }, { 0xcc01, 0x00,  966 },
+	{ 0xd001, 0x00,  987 }, { 0xd401, 0x00, 1009 }, { 0xd801, 0x00, 1031 },
+	{ 0xdc01, 0x00, 1053 }, { 0xe001, 0x00, 1076 }, { 0xe401, 0x00, 1100 },
+	{ 0xe801, 0x00, 1124 }, { 0xec01, 0x00, 1149 }, { 0xf001, 0x00, 1174 },
+	{ 0xf401, 0x00, 1199 }, { 0xf801, 0x00, 1226 }, { 0xfc01, 0x00, 1253 }
+};
+
+static const struct joycon_rumble_amp_data joycon_rumble_amplitudes[] = {
+	/* high, low, amp */
+	{ 0x00, 0x0040,    0 },
+	{ 0x02, 0x8040,   10 }, { 0x04, 0x0041,   12 }, { 0x06, 0x8041,   14 },
+	{ 0x08, 0x0042,   17 }, { 0x0a, 0x8042,   20 }, { 0x0c, 0x0043,   24 },
+	{ 0x0e, 0x8043,   28 }, { 0x10, 0x0044,   33 }, { 0x12, 0x8044,   40 },
+	{ 0x14, 0x0045,   47 }, { 0x16, 0x8045,   56 }, { 0x18, 0x0046,   67 },
+	{ 0x1a, 0x8046,   80 }, { 0x1c, 0x0047,   95 }, { 0x1e, 0x8047,  112 },
+	{ 0x20, 0x0048,  117 }, { 0x22, 0x8048,  123 }, { 0x24, 0x0049,  128 },
+	{ 0x26, 0x8049,  134 }, { 0x28, 0x004a,  140 }, { 0x2a, 0x804a,  146 },
+	{ 0x2c, 0x004b,  152 }, { 0x2e, 0x804b,  159 }, { 0x30, 0x004c,  166 },
+	{ 0x32, 0x804c,  173 }, { 0x34, 0x004d,  181 }, { 0x36, 0x804d,  189 },
+	{ 0x38, 0x004e,  198 }, { 0x3a, 0x804e,  206 }, { 0x3c, 0x004f,  215 },
+	{ 0x3e, 0x804f,  225 }, { 0x40, 0x0050,  230 }, { 0x42, 0x8050,  235 },
+	{ 0x44, 0x0051,  240 }, { 0x46, 0x8051,  245 }, { 0x48, 0x0052,  251 },
+	{ 0x4a, 0x8052,  256 }, { 0x4c, 0x0053,  262 }, { 0x4e, 0x8053,  268 },
+	{ 0x50, 0x0054,  273 }, { 0x52, 0x8054,  279 }, { 0x54, 0x0055,  286 },
+	{ 0x56, 0x8055,  292 }, { 0x58, 0x0056,  298 }, { 0x5a, 0x8056,  305 },
+	{ 0x5c, 0x0057,  311 }, { 0x5e, 0x8057,  318 }, { 0x60, 0x0058,  325 },
+	{ 0x62, 0x8058,  332 }, { 0x64, 0x0059,  340 }, { 0x66, 0x8059,  347 },
+	{ 0x68, 0x005a,  355 }, { 0x6a, 0x805a,  362 }, { 0x6c, 0x005b,  370 },
+	{ 0x6e, 0x805b,  378 }, { 0x70, 0x005c,  387 }, { 0x72, 0x805c,  395 },
+	{ 0x74, 0x005d,  404 }, { 0x76, 0x805d,  413 }, { 0x78, 0x005e,  422 },
+	{ 0x7a, 0x805e,  431 }, { 0x7c, 0x005f,  440 }, { 0x7e, 0x805f,  450 },
+	{ 0x80, 0x0060,  460 }, { 0x82, 0x8060,  470 }, { 0x84, 0x0061,  480 },
+	{ 0x86, 0x8061,  491 }, { 0x88, 0x0062,  501 }, { 0x8a, 0x8062,  512 },
+	{ 0x8c, 0x0063,  524 }, { 0x8e, 0x8063,  535 }, { 0x90, 0x0064,  547 },
+	{ 0x92, 0x8064,  559 }, { 0x94, 0x0065,  571 }, { 0x96, 0x8065,  584 },
+	{ 0x98, 0x0066,  596 }, { 0x9a, 0x8066,  609 }, { 0x9c, 0x0067,  623 },
+	{ 0x9e, 0x8067,  636 }, { 0xa0, 0x0068,  650 }, { 0xa2, 0x8068,  665 },
+	{ 0xa4, 0x0069,  679 }, { 0xa6, 0x8069,  694 }, { 0xa8, 0x006a,  709 },
+	{ 0xaa, 0x806a,  725 }, { 0xac, 0x006b,  741 }, { 0xae, 0x806b,  757 },
+	{ 0xb0, 0x006c,  773 }, { 0xb2, 0x806c,  790 }, { 0xb4, 0x006d,  808 },
+	{ 0xb6, 0x806d,  825 }, { 0xb8, 0x006e,  843 }, { 0xba, 0x806e,  862 },
+	{ 0xbc, 0x006f,  881 }, { 0xbe, 0x806f,  900 }, { 0xc0, 0x0070,  920 },
+	{ 0xc2, 0x8070,  940 }, { 0xc4, 0x0071,  960 }, { 0xc6, 0x8071,  981 },
+	{ 0xc8, 0x0072, 1003 }
+};
+static const u16 joycon_max_rumble_amp =
+joycon_rumble_amplitudes[ARRAY_SIZE(joycon_rumble_amplitudes) - 1].amp;
+
 /* States for controller state machine */
 enum joycon_ctlr_state {
 	JOYCON_CTLR_STATE_INIT,
@@ -182,6 +299,9 @@  struct joycon_input_report {
 
 #define JC_MAX_RESP_SIZE (sizeof(struct joycon_input_report) + 35)
 #define JC_NUM_LEDS 4
+#define JC_RUMBLE_DFLT_LOW_FREQ 160
+#define JC_RUMBLE_DFLT_HIGH_FREQ 320
+#define JC_RUMBLE_PERIOD_MS 300
 
 /* Each physical controller is associated with a joycon_ctlr struct */
 struct joycon_ctlr {
@@ -214,6 +334,17 @@  struct joycon_ctlr {
 	u8 battery_capacity;
 	bool battery_charging;
 	bool host_powered;
+
+	/* rumble */
+	u8 rumble_data[8];
+	struct work_struct rumble_worker;
+	unsigned int rumble_msecs;
+	u16 rumble_ll_freq;
+	u16 rumble_lh_freq;
+	u16 rumble_rl_freq;
+	u16 rumble_rh_freq;
+	u16 rumble_l_amp;
+	u16 rumble_r_amp;
 };
 
 static int __joycon_hid_send(struct hid_device *hdev, u8 *data, size_t len)
@@ -270,6 +401,12 @@  static int joycon_send_subcmd(struct joycon_ctlr *ctlr,
 			      size_t data_len)
 {
 	int ret;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctlr->lock, flags);
+	memcpy(subcmd->rumble_data, ctlr->rumble_data,
+	       sizeof(subcmd->rumble_data));
+	spin_unlock_irqrestore(&ctlr->lock, flags);
 
 	subcmd->output_id = JC_OUTPUT_RUMBLE_AND_SUBCMD;
 	subcmd->packet_num = ctlr->subcmd_num;
@@ -279,9 +416,11 @@  static int joycon_send_subcmd(struct joycon_ctlr *ctlr,
 	ctlr->msg_type = JOYCON_MSG_TYPE_SUBCMD;
 
 	ret = joycon_hid_send_sync(ctlr, (u8 *)subcmd,
-				      sizeof(*subcmd) + data_len);
-	if (ret)
+				   sizeof(*subcmd) + data_len);
+	if (ret < 0)
 		hid_dbg(ctlr->hdev, "send subcommand failed; ret=%d\n", ret);
+	else
+		ret = 0;
 	return ret;
 }
 
@@ -396,6 +535,19 @@  static int joycon_set_report_mode(struct joycon_ctlr *ctlr)
 	return joycon_send_subcmd(ctlr, req, 1);
 }
 
+static int joycon_enable_rumble(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_VIBRATION;
+	req->data[0] = enable ? 0x01 : 0x00;
+
+	hid_dbg(ctlr->hdev, "%s rumble\n", enable ? "enabling" : "disabling");
+	return joycon_send_subcmd(ctlr, req, 1);
+}
+
 static int joycon_map_stick_val(struct joycon_stick_cal *cal, s32 val)
 {
 	s32 center = cal->center;
@@ -421,10 +573,16 @@  static void joycon_parse_report(struct joycon_ctlr *ctlr, u8 *data)
 	u8 tmp;
 	u32 btns;
 	u32 id = ctlr->hdev->product;
+	struct joycon_input_report *rep = (struct joycon_input_report *) data;
+	unsigned long msecs = jiffies_to_msecs(jiffies);
 
-	/* Parse the battery status */
-	tmp = data[2];
 	spin_lock_irqsave(&ctlr->lock, flags);
+	if (rep->vibrator_report &&
+	    (msecs - ctlr->rumble_msecs) >= JC_RUMBLE_PERIOD_MS)
+		schedule_work(&ctlr->rumble_worker);
+
+	/* Parse the battery status */
+	tmp = rep->bat_con;
 	ctlr->host_powered = tmp & BIT(0);
 	ctlr->battery_charging = tmp & BIT(4);
 	tmp = tmp >> 5;
@@ -452,7 +610,7 @@  static void joycon_parse_report(struct joycon_ctlr *ctlr, u8 *data)
 	spin_unlock_irqrestore(&ctlr->lock, flags);
 
 	/* Parse the buttons and sticks */
-	btns = hid_field_extract(ctlr->hdev, data + 3, 0, 24);
+	btns = hid_field_extract(ctlr->hdev, rep->button_status, 0, 24);
 
 	if (id != USB_DEVICE_ID_NINTENDO_JOYCONR) {
 		u16 raw_x;
@@ -461,8 +619,10 @@  static void joycon_parse_report(struct joycon_ctlr *ctlr, u8 *data)
 		int y;
 
 		/* get raw stick values */
-		raw_x = hid_field_extract(ctlr->hdev, (data + 6), 0, 12);
-		raw_y = hid_field_extract(ctlr->hdev, (data + 7), 4, 12);
+		raw_x = hid_field_extract(ctlr->hdev,
+					  rep->left_stick, 0, 12);
+		raw_y = hid_field_extract(ctlr->hdev,
+					  rep->left_stick + 1, 4, 12);
 		/* map the stick values */
 		x = joycon_map_stick_val(&ctlr->left_stick_cal_x, raw_x);
 		y = -joycon_map_stick_val(&ctlr->left_stick_cal_y, raw_y);
@@ -493,8 +653,10 @@  static void joycon_parse_report(struct joycon_ctlr *ctlr, u8 *data)
 		int y;
 
 		/* get raw stick values */
-		raw_x = hid_field_extract(ctlr->hdev, (data + 9), 0, 12);
-		raw_y = hid_field_extract(ctlr->hdev, (data + 10), 4, 12);
+		raw_x = hid_field_extract(ctlr->hdev,
+					  rep->right_stick, 0, 12);
+		raw_y = hid_field_extract(ctlr->hdev,
+					  rep->right_stick + 1, 4, 12);
 		/* map stick values */
 		x = joycon_map_stick_val(&ctlr->right_stick_cal_x, raw_x);
 		y = -joycon_map_stick_val(&ctlr->right_stick_cal_y, raw_y);
@@ -523,6 +685,151 @@  static void joycon_parse_report(struct joycon_ctlr *ctlr, u8 *data)
 	input_sync(dev);
 }
 
+static void joycon_rumble_worker(struct work_struct *work)
+{
+	struct joycon_ctlr *ctlr = container_of(work, struct joycon_ctlr,
+							rumble_worker);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&ctlr->lock, flags);
+	ctlr->rumble_msecs = jiffies_to_msecs(jiffies);
+	spin_unlock_irqrestore(&ctlr->lock, flags);
+
+	mutex_lock(&ctlr->output_mutex);
+	ret = joycon_enable_rumble(ctlr, true);
+	mutex_unlock(&ctlr->output_mutex);
+	if (ret < 0)
+		hid_warn(ctlr->hdev, "Failed to set rumble data; ret=%d", ret);
+}
+
+static struct joycon_rumble_freq_data joycon_find_rumble_freq(u16 freq)
+{
+	const size_t length = ARRAY_SIZE(joycon_rumble_frequencies);
+	const struct joycon_rumble_freq_data *data = joycon_rumble_frequencies;
+	int i = 0;
+
+	if (freq > data[0].freq) {
+		for (i = 1; i < length - 1; i++) {
+			if (freq > data[i - 1].freq && freq <= data[i].freq)
+				break;
+		}
+	}
+
+	return data[i];
+}
+
+static struct joycon_rumble_amp_data joycon_find_rumble_amp(u16 amp)
+{
+	const size_t length = ARRAY_SIZE(joycon_rumble_amplitudes);
+	const struct joycon_rumble_amp_data *data = joycon_rumble_amplitudes;
+	int i = 0;
+
+	if (amp > data[0].amp) {
+		for (i = 1; i < length - 1; i++) {
+			if (amp > data[i - 1].amp && amp <= data[i].amp)
+				break;
+		}
+	}
+
+	return data[i];
+}
+
+static void joycon_encode_rumble(u8 *data, u16 freq_low, u16 freq_high, u16 amp)
+{
+	struct joycon_rumble_freq_data freq_data_low;
+	struct joycon_rumble_freq_data freq_data_high;
+	struct joycon_rumble_amp_data amp_data;
+
+	freq_data_low = joycon_find_rumble_freq(freq_low);
+	freq_data_high = joycon_find_rumble_freq(freq_high);
+	amp_data = joycon_find_rumble_amp(amp);
+
+	data[0] = (freq_data_high.high >> 8) & 0xFF;
+	data[1] = (freq_data_high.high & 0xFF) + amp_data.high;
+	data[2] = freq_data_low.low + ((amp_data.low >> 8) & 0xFF);
+	data[3] = amp_data.low & 0xFF;
+}
+
+#define JOYCON_MAX_RUMBLE_HIGH_FREQ	((u16) 1253)
+#define JOYCON_MIN_RUMBLE_HIGH_FREQ	((u16) 82)
+#define JOYCON_MAX_RUMBLE_LOW_FREQ	((u16) 626)
+#define JOYCON_MIN_RUMBLE_LOW_FREQ	((u16) 41)
+
+static void joycon_clamp_rumble_freqs(struct joycon_ctlr *ctlr)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctlr->lock, flags);
+	ctlr->rumble_ll_freq = clamp(ctlr->rumble_ll_freq,
+				     JOYCON_MIN_RUMBLE_LOW_FREQ,
+				     JOYCON_MAX_RUMBLE_LOW_FREQ);
+	ctlr->rumble_lh_freq = clamp(ctlr->rumble_lh_freq,
+				     JOYCON_MIN_RUMBLE_HIGH_FREQ,
+				     JOYCON_MAX_RUMBLE_HIGH_FREQ);
+	ctlr->rumble_rl_freq = clamp(ctlr->rumble_rl_freq,
+				     JOYCON_MIN_RUMBLE_LOW_FREQ,
+				     JOYCON_MAX_RUMBLE_LOW_FREQ);
+	ctlr->rumble_rh_freq = clamp(ctlr->rumble_rh_freq,
+				     JOYCON_MIN_RUMBLE_HIGH_FREQ,
+				     JOYCON_MAX_RUMBLE_HIGH_FREQ);
+	spin_unlock_irqrestore(&ctlr->lock, flags);
+}
+
+static int joycon_set_rumble(struct joycon_ctlr *ctlr, u16 amp_r, u16 amp_l,
+			     bool schedule_now)
+{
+	u8 data[8];
+	u16 amp;
+	u16 freq_r_low;
+	u16 freq_r_high;
+	u16 freq_l_low;
+	u16 freq_l_high;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctlr->lock, flags);
+	freq_r_low = ctlr->rumble_rl_freq;
+	freq_r_high = ctlr->rumble_rh_freq;
+	freq_l_low = ctlr->rumble_ll_freq;
+	freq_l_high = ctlr->rumble_lh_freq;
+	spin_unlock_irqrestore(&ctlr->lock, flags);
+
+	/* right joy-con */
+	amp = amp_r * (u32)joycon_max_rumble_amp / 65535;
+	ctlr->rumble_r_amp = amp;
+	joycon_encode_rumble(data + 4, freq_r_low, freq_r_high, amp);
+
+	/* left joy-con */
+	amp = amp_l * (u32)joycon_max_rumble_amp / 65535;
+	ctlr->rumble_l_amp = amp;
+	joycon_encode_rumble(data, freq_l_low, freq_l_high, amp);
+
+	spin_lock_irqsave(&ctlr->lock, flags);
+	memcpy(ctlr->rumble_data, data, sizeof(ctlr->rumble_data));
+	spin_unlock_irqrestore(&ctlr->lock, flags);
+
+	/* don't wait for the periodic send (reduces latency) */
+	if (schedule_now)
+		schedule_work(&ctlr->rumble_worker);
+
+	return 0;
+}
+
+#ifdef CONFIG_JOYCON_FF
+static int joycon_play_effect(struct input_dev *dev, void *data,
+						     struct ff_effect *effect)
+{
+	struct joycon_ctlr *ctlr = input_get_drvdata(dev);
+
+	if (effect->type != FF_RUMBLE)
+		return 0;
+
+	return joycon_set_rumble(ctlr,
+				 effect->u.rumble.weak_magnitude,
+				 effect->u.rumble.strong_magnitude,
+				 true);
+}
+#endif
 
 static const unsigned int joycon_button_inputs[] = {
 	BTN_SELECT, BTN_Z, BTN_THUMBL,
@@ -590,6 +897,19 @@  static int joycon_input_create(struct joycon_ctlr *ctlr)
 		input_set_capability(ctlr->input, EV_KEY,
 				     joycon_button_inputs[i]);
 
+#ifdef CONFIG_JOYCON_FF
+	/* set up rumble */
+	input_set_capability(ctlr->input, EV_FF, FF_RUMBLE);
+	input_ff_create_memless(ctlr->input, NULL, joycon_play_effect);
+	ctlr->rumble_ll_freq = JC_RUMBLE_DFLT_LOW_FREQ;
+	ctlr->rumble_lh_freq = JC_RUMBLE_DFLT_HIGH_FREQ;
+	ctlr->rumble_rl_freq = JC_RUMBLE_DFLT_LOW_FREQ;
+	ctlr->rumble_rh_freq = JC_RUMBLE_DFLT_HIGH_FREQ;
+	joycon_clamp_rumble_freqs(ctlr);
+	joycon_set_rumble(ctlr, 0, 0, false);
+	ctlr->rumble_msecs = jiffies_to_msecs(jiffies);
+#endif
+
 	ret = input_register_device(ctlr->input);
 	if (ret)
 		goto err_input;
@@ -956,6 +1276,7 @@  static int joycon_hid_probe(struct hid_device *hdev,
 	mutex_init(&ctlr->output_mutex);
 	init_waitqueue_head(&ctlr->wait);
 	spin_lock_init(&ctlr->lock);
+	INIT_WORK(&ctlr->rumble_worker, joycon_rumble_worker);
 
 	ret = hid_parse(hdev);
 	if (ret) {
@@ -1009,6 +1330,13 @@  static int joycon_hid_probe(struct hid_device *hdev,
 		goto err_mutex;
 	}
 
+	/* Enable rumble */
+	ret = joycon_enable_rumble(ctlr, true);
+	if (ret) {
+		hid_err(hdev, "Failed to enable rumble; ret=%d\n", ret);
+		goto err_mutex;
+	}
+
 	mutex_unlock(&ctlr->output_mutex);
 
 	ret = joycon_input_create(ctlr);
@@ -1055,6 +1383,7 @@  static void joycon_hid_remove(struct hid_device *hdev)
 	hid_dbg(hdev, "remove\n");
 	hid_hw_close(hdev);
 	hid_hw_stop(hdev);
+
 	joycon_ctlr_destroy(ctlr);
 }