add support for TSPDRV vibration
* Needed for the LG V30, G7, V35, V40
* Use TW_HAPTICS_TSPDRV in your BoardConfig.mk to enable it.
Change-Id: I0970ed5c046e851f7e6f562283523c7214c8d2b5
diff --git a/minuitwrp/tspdrv.cpp b/minuitwrp/tspdrv.cpp
new file mode 100644
index 0000000..6967cd5
--- /dev/null
+++ b/minuitwrp/tspdrv.cpp
@@ -0,0 +1,193 @@
+#include <stdio.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <cinttypes>
+#include <cmath>
+#include <iostream>
+#include <sys/ioctl.h>
+
+#include "tspdrv.h"
+
+int tspdrv_initialized = 0;
+int tspdrv_file_desc;
+int tspdrv_numActuators = 0;
+
+int initialize_tspdrv()
+{
+ // Open device file as read/write for ioctl and write
+ tspdrv_file_desc = open(TSPDRV, O_RDWR);
+ if(tspdrv_file_desc < 0)
+ {
+ printf("Failed to open device file: %s", TSPDRV);
+ return -1;
+ }
+
+ // create default device parameters
+ device_parameter dev_param1 { 0, VIBE_KP_CFG_FREQUENCY_PARAM1, 0};
+ device_parameter dev_param2 { 0, VIBE_KP_CFG_FREQUENCY_PARAM2, 0};
+ device_parameter dev_param3 { 0, VIBE_KP_CFG_FREQUENCY_PARAM3, 0};
+ device_parameter dev_param4 { 0, VIBE_KP_CFG_FREQUENCY_PARAM4, 400};
+ device_parameter dev_param5 { 0, VIBE_KP_CFG_FREQUENCY_PARAM5, 13435};
+ device_parameter dev_param6 { 0, VIBE_KP_CFG_FREQUENCY_PARAM6, 0};
+ device_parameter dev_param_update_rate {0, VIBE_KP_CFG_UPDATE_RATE_MS, 5};
+
+ // Set magic number for vibration driver, wont allow us to write data without!
+ int ret = ioctl(tspdrv_file_desc, TSPDRV_SET_MAGIC_NUMBER, TSPDRV_MAGIC_NUMBER);
+ if(ret != 0)
+ {
+ printf("Failed to set magic number");
+ return -ret;
+ }
+
+ // Set default device parameter 1
+ ret = ioctl(tspdrv_file_desc, TSPDRV_SET_DEVICE_PARAMETER, &dev_param1);
+ if(ret != 0)
+ {
+ printf("Failed to set device parameter 1");
+ return -ret;
+ }
+
+ // Set default device parameter 2
+ ret = ioctl(tspdrv_file_desc, TSPDRV_SET_DEVICE_PARAMETER, &dev_param2);
+ if(ret != 0)
+ {
+ printf("Failed to set device parameter 2");
+ return -ret;
+ }
+
+ // Set default device parameter 3
+ ret = ioctl(tspdrv_file_desc, TSPDRV_SET_DEVICE_PARAMETER, &dev_param3);
+ if(ret != 0)
+ {
+ printf("Failed to set device parameter 3");
+ return -ret;
+ }
+
+ // Set default device parameter 4
+ ret = ioctl(tspdrv_file_desc, TSPDRV_SET_DEVICE_PARAMETER, &dev_param4);
+ if(ret != 0)
+ {
+ printf("Failed to set device parameter 4");
+ return -ret;
+ }
+
+ // Set default device parameter 5
+ ret = ioctl(tspdrv_file_desc, TSPDRV_SET_DEVICE_PARAMETER, &dev_param5);
+ if(ret != 0)
+ {
+ printf("Failed to set device parameter 5");
+ return -ret;
+ }
+
+ // Set default device parameter 6
+ ret = ioctl(tspdrv_file_desc, TSPDRV_SET_DEVICE_PARAMETER, &dev_param6);
+ if(ret != 0)
+ {
+ printf("Failed to set device parameter 6");
+ return -ret;
+ }
+
+ // Set default device parameter update rate
+ ret = ioctl(tspdrv_file_desc, TSPDRV_SET_DEVICE_PARAMETER, &dev_param_update_rate);
+ if(ret != 0)
+ {
+ printf("Failed to set device parameter update rate");
+ return -ret;
+ }
+
+ // Get number of actuators the device has
+ ret = ioctl(tspdrv_file_desc, TSPDRV_GET_NUM_ACTUATORS, 0);
+ if(ret == 0)
+ {
+ printf("No actuators found!");
+ return -2;
+ }
+
+ tspdrv_numActuators = ret;
+ tspdrv_initialized = 1;
+ return 0;
+}
+
+int tspdrv_off() {
+
+ for(int32_t i = 0; i < tspdrv_numActuators; i++)
+ {
+ int32_t ret = ioctl(tspdrv_file_desc, TSPDRV_DISABLE_AMP, i);
+ if(ret != 0)
+ {
+ printf("Failed to deactivate Actuator with index %d", i);
+ return -1;
+ }
+ }
+
+ return 0;
+}
+
+int vibrate(int timeout_ms)
+{
+ double BUFFER_ENTRIES_PER_MS = 8.21;
+ uint8_t DEFAULT_AMPLITUDE = 127;
+ int32_t OUTPUT_BUFFER_SIZE = 40;
+
+ if(!tspdrv_initialized)
+ {
+ printf("Initializing TSPDRV\n");
+ if(initialize_tspdrv() == 0)
+ {
+ printf("TSPDRV initialized\n");
+ }
+ }
+
+ // Calculate needed buffer entries
+ int32_t bufferSize = (int32_t) round(BUFFER_ENTRIES_PER_MS * timeout_ms);
+ VibeUInt8 fullBuffer[bufferSize];
+
+ // turn previous vibrations off
+ tspdrv_off();
+
+ for(int32_t i = 0; i < bufferSize; i++)
+ {
+ // The vibration is a sine curve, the negative parts are 255 + negative value
+ fullBuffer[i] = (VibeUInt8) (DEFAULT_AMPLITUDE * sin(i/BUFFER_ENTRIES_PER_MS));
+ }
+
+ // Amount of buffer arrays with size of OUTPUT_BUFFER_SIZE
+ int32_t numBuffers = (int32_t) ceil((double)bufferSize / (double)OUTPUT_BUFFER_SIZE);
+ VibeUInt8 outputBuffers[numBuffers][OUTPUT_BUFFER_SIZE];
+ memset(outputBuffers, 0, sizeof(outputBuffers)); // zero the array before we fill it with values
+
+ for(int32_t i = 0; i < bufferSize; i++)
+ {
+ // split fullBuffer into multiple smaller buffers with size OUTPUT_BUFFER_SIZE
+ outputBuffers[i/OUTPUT_BUFFER_SIZE][i%OUTPUT_BUFFER_SIZE] = fullBuffer[i];
+ }
+
+ for(int32_t i = 0; i < tspdrv_numActuators; i++)
+ {
+ for(int32_t j = 0; j < numBuffers; j++)
+ {
+ char output[OUTPUT_BUFFER_SIZE + SPI_HEADER_SIZE];
+ memset(output, 0, sizeof(output));
+ output[0] = i; // first byte is actuator index
+ output[1] = 8; // per definition has to be 8
+ output[2] = OUTPUT_BUFFER_SIZE; // size of the following output buffer
+ for(int32_t k = 3; k < OUTPUT_BUFFER_SIZE+3; k++)
+ {
+ output[k] = outputBuffers[j][k-3];
+ }
+ // write the buffer to the device
+ write(tspdrv_file_desc, output, sizeof(output));
+ if((j+1) % 4 == 0)
+ {
+ // every 4 buffers, but not the first if theres only 1, we send an ENABLE_AMP signal
+ int32_t ret = ioctl(tspdrv_file_desc, TSPDRV_ENABLE_AMP, i);
+ if(ret != 0)
+ {
+ printf("Failed to activate Actuator with index %d", i);
+ return -1;
+ }
+ }
+ }
+ }
+ return 0;
+}
\ No newline at end of file