From 03ebfde089585fef66539058c666a30ab5940f55 Mon Sep 17 00:00:00 2001
From: Raphael Assenat <raph@raphnet.net>
Date: Tue, 10 Apr 2018 15:10:02 -0400
Subject: [PATCH] Force feedback: Don't ignore the loop count

The PID Effect Operation output report contains a 'loop count'
field that has an influence on how long the effect runs. Ignoring
it can lead to never stopping vibration for software that use
the loop count to let effects stop automatically instead of
issuing a stop command.
---
 main.c   | 11 +++++++++++
 usbpad.c | 33 +++++++++++++++++++++++++++------
 usbpad.h |  1 +
 3 files changed, 39 insertions(+), 6 deletions(-)

diff --git a/main.c b/main.c
index 86c20bd..74ab1f5 100644
--- a/main.c
+++ b/main.c
@@ -36,6 +36,7 @@
 #include "hiddata.h"
 #include "usbstrings.h"
 #include "intervaltimer.h"
+#include "intervaltimer2.h"
 #include "requests.h"
 #include "stkchk.h"
 
@@ -453,6 +454,7 @@ int main(void)
 	usart1_init();
 	eeprom_init();
 	intervaltimer_init();
+	intervaltimer2_init();
 	stkchk_init();
 
 	switch (g_eeprom_data.cfg.mode)
@@ -510,6 +512,9 @@ int main(void)
 	sei();
 	usb_init(&usb_params);
 
+	// Timebase for force feedback 'loop count'
+	intervaltimer2_set16ms();
+
 	while (1)
 	{
 		static char last_v[MAX_PLAYERS] = { };
@@ -520,6 +525,12 @@ int main(void)
 
 		usb_doTasks();
 		hiddata_doTask(&hiddata_ops);
+		// Run vibration tasks
+		if (intervaltimer2_get()) {
+			for (channel=0; channel < num_players; channel++) {
+				usbpad_vibrationTask(&usbpads[channel]);
+			}
+		}
 
 		switch(state)
 		{
diff --git a/usbpad.c b/usbpad.c
index 0a3b6be..c49c73d 100644
--- a/usbpad.c
+++ b/usbpad.c
@@ -17,6 +17,7 @@
 #include <stdint.h>
 #include <stdio.h>
 #include <string.h>
+#include <avr/interrupt.h>
 #include "usb.h"
 #include "gamepads.h"
 #include "usbpad.h"
@@ -236,6 +237,18 @@ void usbpad_forceVibrate(struct usbpad *pad, char force)
 	pad->force_vibrate = force;
 }
 
+void usbpad_vibrationTask(struct usbpad *pad)
+{
+	uint8_t sreg;
+
+	sreg = SREG;
+	cli();
+	if (pad->_loop_count) {
+		pad->_loop_count--;
+	}
+	SREG = sreg;
+}
+
 char usbpad_mustVibrate(struct usbpad *pad)
 {
 	if (pad->force_vibrate) {
@@ -245,11 +258,16 @@ char usbpad_mustVibrate(struct usbpad *pad)
 	if (!pad->vibration_on) {
 		pad->gamepad_vibrate = 0;
 	} else {
-		if (pad->constant_force > 0x7f) {
-			pad->gamepad_vibrate = 1;
-		} else if (pad->periodic_magnitude > 0x7f) {
-			pad->gamepad_vibrate = 1;
+		if (pad->_loop_count > 0) {
+			if (pad->constant_force > 0x7f) {
+				pad->gamepad_vibrate = 1;
+			} else if (pad->periodic_magnitude > 0x7f) {
+				pad->gamepad_vibrate = 1;
+			} else {
+				pad->gamepad_vibrate = 0;
+			}
 		} else {
+			// Loop count = 0 -> Stop
 			pad->gamepad_vibrate = 0;
 		}
 	}
@@ -380,9 +398,12 @@ uint8_t usbpad_hid_set_report(struct usbpad *pad, const struct usb_request *rq,
 				 * Byte 1 : bit 7=rom flag, bits 6-0=effect block index
 				 * Byte 2 : Effect operation
 				 * Byte 3 : Loop count */
-				pad->_loop_count = data[3]<<3;
 
-				printf_P(PSTR("EFFECT OP: rom=%s, idx=0x%02x"), data[1] & 0x80 ? "Yes":"No", data[1] & 0x7F);
+				 // scale this up so vibration stays on longer.
+				 // main.c uses a 16ms interval timer for vibration "loops"
+				pad->_loop_count = (data[3]+1)*3;
+
+				printf_P(PSTR("EFFECT OP: rom=%s, idx=0x%02x : "), data[1] & 0x80 ? "Yes":"No", data[1] & 0x7F);
 
 				switch(data[1] & 0x7F) // Effect block index
 				{
diff --git a/usbpad.h b/usbpad.h
index 12a6ac3..aecccfc 100644
--- a/usbpad.h
+++ b/usbpad.h
@@ -25,6 +25,7 @@ int usbpad_getReportSize(void);
 unsigned char *usbpad_getReportBuffer(struct usbpad *pad);
 
 void usbpad_update(struct usbpad *pad, const gamepad_data *pad_data);
+void usbpad_vibrationTask(struct usbpad *pad);
 char usbpad_mustVibrate(struct usbpad *pad);
 void usbpad_forceVibrate(struct usbpad *pad, char force);