feat: preserve native HD rumble on Switch 2 controllers

tests/switch2_haptics_test.c

@@ -0,0 +1,70 @@
+#include <assert.h>
+#include <stdint.h>
+#include "parser/uni_switch2_haptics.h"
+
+static uint64_t unpack(const uint8_t sample[5]) {
+    uint64_t result = 0;
+    for (unsigned byte = 0; byte < 5; ++byte) result |= (uint64_t)sample[byte] << (byte * 8);
+    return result;
+}
+
+int main(void) {
+    uni_switch2_haptics_frame_t frame;
+    uni_switch2_haptics_silence(&frame);
+    assert(uni_switch2_haptics_valid(&frame));
+    assert(uni_switch2_haptics_is_stop(&frame));
+    uint64_t sample = unpack(frame.sides[0].samples[0]);
+    assert((sample & 1023) == 385); // Measured160Hz anchor.
+    assert(((sample >> 20) & 1023) == 481); // Measured320Hz anchor.
+    assert(((sample >> 10) & 1023) == 0 && ((sample >> 30) & 1023) == 0);
+
+    uni_switch2_haptics_encode_sample(frame.sides[0].samples[0], 96, 96, 32767, 16384);
+    sample = unpack(frame.sides[0].samples[0]);
+    assert((sample & 1023) == 481);
+    assert(((sample >> 20) & 1023) == 577); // Upper frequency bit is frequency, not tone mode.
+    assert(((sample >> 10) & 1023) == 453 && ((sample >> 30) & 1023) == 226);
+    assert(!uni_switch2_haptics_is_stop(&frame));
+
+    frame.sides[0].count = 3;
+    uni_switch2_haptics_encode_sample(frame.sides[0].samples[1], 32, 64, 8192, 0);
+    uni_switch2_haptics_encode_sample(frame.sides[0].samples[2], 64, 127, 0, 32767);
+    uint8_t block[16];
+    assert(uni_switch2_haptics_write_block(block, &frame.sides[0], 0xab));
+    assert(block[0] == 0x7b);
+    assert((unpack(block + 1) & 1023) == 481);
+    assert((unpack(block + 6) & 1023) == 289);
+    assert(((unpack(block + 6) >> 10) & 1023) == 113);
+    assert(((unpack(block + 11) >> 20) & 1023) == 670);
+    assert(((unpack(block + 11) >> 10) & 1023) == 0);
+    assert(((unpack(block + 11) >> 30) & 1023) == 453);
+
+    frame.sides[0].count = 2;
+    assert(uni_switch2_haptics_write_block(block, &frame.sides[0], 15));
+    assert(block[0] == 0x6f);
+    for (unsigned i = 11; i < 16; ++i) assert(block[i] == 0);
+    frame.sides[0].count = 1;
+    assert(uni_switch2_haptics_write_block(block, &frame.sides[0], 16));
+    assert(block[0] == 0x50);
+    for (unsigned i = 6; i < 16; ++i) assert(block[i] == 0);
+
+    uni_switch2_haptics_encode_sample(frame.sides[0].samples[0], 0, 255, 65535, 65535);
+    sample = unpack(frame.sides[0].samples[0]);
+    assert((sample & 1023) == 196 && ((sample >> 20) & 1023) == 670);
+    assert(((sample >> 10) & 1023) == 453 && ((sample >> 30) & 1023) == 453);
+    assert(uni_switch2_haptics_valid(&frame));
+    // A malformed internal native frame cannot bypass the amplitude envelope.
+    frame.sides[0].samples[0][2] |= 0x0f;
+    assert(!uni_switch2_haptics_valid(&frame));
+
+    uni_switch2_haptics_silence(&frame);
+    frame.sides[1].count = 0;
+    assert(uni_switch2_haptics_valid(&frame));
+    assert(!uni_switch2_haptics_is_stop(&frame)); // A side-only mute must not clear its partner.
+    frame.sides[0].count = 0;
+    assert(!uni_switch2_haptics_valid(&frame));
+    assert(!uni_switch2_haptics_write_block(block, &frame.sides[0], 0));
+    frame.sides[0].count = 4;
+    assert(!uni_switch2_haptics_valid(&frame));
+    assert(!uni_switch2_haptics_write_block(block, &frame.sides[0], 0));
+    return 0;
+}