84 lines
3.4 KiB
C
84 lines
3.4 KiB
C
// 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;
|
|
}
|