feat: preserve native HD rumble on Switch 2 controllers

bluepad32_config/parser/uni_switch2_haptics.c

@@ -0,0 +1,84 @@
+// SPDX-License-Identifier: Apache-2.0
+#include "parser/uni_switch2_haptics.h"
+#include <stddef.h>
+#include <string.h>
+
+static uint8_t frequency_index(uint8_t index) {
+    if (index < 1) return 1;
+    return index > 127 ? 127 : index;
+}
+
+static uint16_t amplitude_code(uint16_t q15) {
+    if (q15 > 32767) q15 = 32767;
+    // Match SDL's conservative native envelope while retaining a linear input
+    // curve. This is transport gain, not a claim of calibrated physical force.
+    return (uint16_t)(((uint32_t)q15 * 29000u / 32767u) >> 6);
+}
+
+static uint64_t load_sample(const uint8_t data[5]) {
+    uint64_t value = 0;
+    for (unsigned i = 0; i < 5; ++i) value |= (uint64_t)data[i] << (8 * i);
+    return value;
+}
+
+static bool valid_side(const uni_switch2_haptics_side_t* side) {
+    if (side->count > UNI_SWITCH2_HAPTICS_MAX_SAMPLES) return false;
+    for (unsigned i = 0; i < side->count; ++i) {
+        uint64_t value = load_sample(side->samples[i]);
+        unsigned first_frequency = value & 1023u;
+        unsigned second_frequency = (value >> 20) & 1023u;
+        if (first_frequency == 0 || first_frequency > 670 ||
+            second_frequency == 0 || second_frequency > 670 ||
+            ((value >> 10) & 1023u) > UNI_SWITCH2_HAPTICS_MAX_AMPLITUDE ||
+            ((value >> 30) & 1023u) > UNI_SWITCH2_HAPTICS_MAX_AMPLITUDE)
+            return false;
+    }
+    return true;
+}
+
+void uni_switch2_haptics_encode_sample(uint8_t out[5], uint8_t low_index, uint8_t high_index,
+                                      uint16_t low_q15, uint16_t high_q15) {
+    if (!out) return;
+    // The two physical frequency fields share one measured logarithmic scale.
+    // Original Switch indices have32 steps/octave; Switch2 has96 steps/octave.
+    uint64_t value = 193u + 3u * frequency_index(low_index);
+    value |= (uint64_t)amplitude_code(low_q15) << 10;
+    value |= (uint64_t)(289u + 3u * frequency_index(high_index)) << 20;
+    value |= (uint64_t)amplitude_code(high_q15) << 30;
+    for (unsigned i = 0; i < 5; ++i) out[i] = (uint8_t)(value >> (8 * i));
+}
+
+void uni_switch2_haptics_silence(uni_switch2_haptics_frame_t* frame) {
+    if (!frame) return;
+    memset(frame, 0, sizeof(*frame));
+    for (unsigned side = 0; side < 2; ++side) {
+        frame->sides[side].count = 1;
+        uni_switch2_haptics_encode_sample(frame->sides[side].samples[0], 64, 64, 0, 0);
+    }
+}
+
+bool uni_switch2_haptics_valid(const uni_switch2_haptics_frame_t* frame) {
+    return frame && (frame->sides[0].count || frame->sides[1].count) &&
+           valid_side(&frame->sides[0]) && valid_side(&frame->sides[1]);
+}
+
+bool uni_switch2_haptics_is_stop(const uni_switch2_haptics_frame_t* frame) {
+    if (!uni_switch2_haptics_valid(frame) || !frame->sides[0].count || !frame->sides[1].count)
+        return false;
+    for (unsigned side = 0; side < 2; ++side) {
+        for (unsigned i = 0; i < frame->sides[side].count; ++i) {
+            uint64_t value = load_sample(frame->sides[side].samples[i]);
+            if (((value >> 10) & 1023u) || ((value >> 30) & 1023u)) return false;
+        }
+    }
+    return true;
+}
+
+bool uni_switch2_haptics_write_block(uint8_t out[16], const uni_switch2_haptics_side_t* side,
+                                    uint8_t sequence) {
+    if (!out || !side || !side->count || side->count > UNI_SWITCH2_HAPTICS_MAX_SAMPLES) return false;
+    memset(out, 0, 16);
+    out[0] = (uint8_t)(0x40u | (side->count << 4) | (sequence & 15u));
+    memcpy(out + 1, side->samples, 5u * side->count);
+    return true;
+}