HID: nintendo: prevent rumble queue overruns

Make sure that we never throw out the most recent rumble setting,
opting to overwrite the prior queue head instead. This prevents
instances where we could get rumble stuck on if there were an overrun at
the wrong time.
Signed-off-by: Daniel J. Ogorchock <djogorchock@gmail.com>
Reviewed-by: Silvan Jegen <s.jegen@gmail.com
Signed-off-by: Jiri Kosina <jkosina@suse.cz>

drivers/hid/hid-nintendo.c

@@ -1527,6 +1527,7 @@ static int joycon_set_rumble(struct joycon_ctlr *ctlr, u16 amp_r, u16 amp_l,
 	u16 freq_l_low;
 	u16 freq_l_high;
 	unsigned long flags;
+	int next_rq_head;
 
 	spin_lock_irqsave(&ctlr->lock, flags);
 	freq_r_low = ctlr->rumble_rl_freq;
@@ -1547,8 +1548,21 @@ static int joycon_set_rumble(struct joycon_ctlr *ctlr, u16 amp_r, u16 amp_l,
 	joycon_encode_rumble(data, freq_l_low, freq_l_high, amp);
 
 	spin_lock_irqsave(&ctlr->lock, flags);
-	if (++ctlr->rumble_queue_head >= JC_RUMBLE_QUEUE_SIZE)
-		ctlr->rumble_queue_head = 0;
+
+	next_rq_head = ctlr->rumble_queue_head + 1;
+	if (next_rq_head >= JC_RUMBLE_QUEUE_SIZE)
+		next_rq_head = 0;
+
+	/* Did we overrun the circular buffer?
+	 * If so, be sure we keep the latest intended rumble state.
+	 */
+	if (next_rq_head == ctlr->rumble_queue_tail) {
+		hid_dbg(ctlr->hdev, "rumble queue is full");
+		/* overwrite the prior value at the end of the circular buf */
+		next_rq_head = ctlr->rumble_queue_head;
+	}
+
+	ctlr->rumble_queue_head = next_rq_head;
 	memcpy(ctlr->rumble_data[ctlr->rumble_queue_head], data,
 	       JC_RUMBLE_DATA_SIZE);
 
@@ -2128,7 +2142,7 @@ static int nintendo_hid_probe(struct hid_device *hdev,
 
 	ctlr->hdev = hdev;
 	ctlr->ctlr_state = JOYCON_CTLR_STATE_INIT;
-	ctlr->rumble_queue_head = JC_RUMBLE_QUEUE_SIZE - 1;
+	ctlr->rumble_queue_head = 0;
 	ctlr->rumble_queue_tail = 0;
 	hid_set_drvdata(hdev, ctlr);
 	mutex_init(&ctlr->output_mutex);