The open source OpenXR runtime
d/pssense: Fix parsing of calibration data and check CRC
Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2301>
+50
-21
+50
-21
src/xrt/drivers/pssense/pssense_driver.c
+50
-21
src/xrt/drivers/pssense/pssense_driver.c
···
200
200
};
201
201
static_assert(sizeof(struct pssense_output_report) == OUTPUT_REPORT_LENGTH, "Incorrect output report struct length");
202
202
203
+
#define FEATURE_REPORT_LENGTH 64
204
+
#define CALIBRATION_DATA_LENGTH 116
205
+
/**
206
+
* HID output report data packet.
207
+
*/
208
+
struct pssense_feature_report
209
+
{
210
+
uint8_t report_id;
211
+
uint8_t part_id;
212
+
uint8_t data[CALIBRATION_DATA_LENGTH / 2];
213
+
struct pssense_i32_le crc;
214
+
};
215
+
static_assert(sizeof(struct pssense_feature_report) == FEATURE_REPORT_LENGTH, "Incorrect feature report struct length");
216
+
203
217
/*!
204
218
* PlayStation Sense state parsed from a data packet.
205
219
*/
···
706
720
pssense_get_calibration_data(struct pssense_device *pssense)
707
721
{
708
722
int ret;
709
-
uint8_t buffer[64];
710
-
uint8_t data[(sizeof(buffer) - 2) * 2];
711
-
for (int i = 0; i < 2; i++) {
712
-
ret = os_hid_get_feature(pssense->hid, CALIBRATION_DATA_FEATURE_REPORT_ID, buffer, sizeof(buffer));
713
-
if (ret < 0) {
714
-
PSSENSE_ERROR(pssense, "Failed to retrieve calibration report: %d", ret);
715
-
return false;
716
-
}
717
-
if (ret != sizeof(buffer)) {
718
-
PSSENSE_ERROR(pssense, "Invalid byte count transferred, expected %zu got %d\n", sizeof(buffer),
719
-
ret);
720
-
return false;
721
-
}
722
-
if (buffer[1] == CALIBRATION_DATA_PART_ID_1) {
723
-
memcpy(data, buffer + 2, sizeof(buffer) - 2);
724
-
} else if (buffer[1] == CALIBRATION_DATA_PART_ID_2) {
725
-
memcpy(data + sizeof(buffer) - 2, buffer + 2, sizeof(buffer) - 2);
726
-
} else {
727
-
PSSENSE_ERROR(pssense, "Unknown calibration data part ID %u", buffer[1]);
728
-
return false;
723
+
uint8_t buffer[sizeof(struct pssense_feature_report)];
724
+
uint8_t data[CALIBRATION_DATA_LENGTH] = {0};
725
+
bool invalid_crc;
726
+
do {
727
+
invalid_crc = false;
728
+
for (int i = 0; i < 2; i++) {
729
+
ret = os_hid_get_feature(pssense->hid, CALIBRATION_DATA_FEATURE_REPORT_ID, buffer,
730
+
sizeof(buffer));
731
+
if (ret < 0) {
732
+
PSSENSE_ERROR(pssense, "Failed to retrieve calibration report: %d", ret);
733
+
return false;
734
+
}
735
+
if (ret != sizeof(buffer)) {
736
+
PSSENSE_ERROR(pssense, "Invalid byte count transferred, expected %zu got %d",
737
+
sizeof(buffer), ret);
738
+
return false;
739
+
}
740
+
struct pssense_feature_report *report = (struct pssense_feature_report *)buffer;
741
+
if (report->part_id == CALIBRATION_DATA_PART_ID_1) {
742
+
memcpy(data, report->data, sizeof(report->data));
743
+
} else if (report->part_id == CALIBRATION_DATA_PART_ID_2) {
744
+
memcpy(data + sizeof(report->data), report->data, sizeof(report->data));
745
+
} else {
746
+
PSSENSE_ERROR(pssense, "Unknown calibration data part ID %u", report->part_id);
747
+
return false;
748
+
}
749
+
750
+
uint32_t crc = crc32_le(0, &FEATURE_REPORT_CRC32_SEED, 1);
751
+
crc = crc32_le(crc, (uint8_t *)&buffer, sizeof(buffer) - 4);
752
+
uint32_t expected_crc = pssense_i32_le_to_u32(&report->crc);
753
+
if (crc != expected_crc) {
754
+
PSSENSE_WARN(pssense, "Invalid feature report CRC. Expected 0x%08X, actual 0x%08X",
755
+
expected_crc, crc);
756
+
invalid_crc = true;
757
+
}
729
758
}
730
-
}
759
+
} while (invalid_crc);
731
760
732
761
// TODO: Parse calibration data into prefiler
733
762