From cf441bd4e69d6a4973ce417f7c77a34d04cf080e Mon Sep 17 00:00:00 2001 From: Beyley Cardellio Date: Thu, 12 Feb 2026 23:14:03 -0500 Subject: [PATCH] d/rift_sensor: Add sensor initialization code Part-of: --- CMakeLists.txt | 5 +- src/xrt/drivers/CMakeLists.txt | 27 ++ src/xrt/drivers/rift_sensor/ar0134.c | 308 ++++++++++++++ src/xrt/drivers/rift_sensor/ar0134.h | 29 ++ src/xrt/drivers/rift_sensor/esp570.c | 157 +++++++ src/xrt/drivers/rift_sensor/esp570.h | 67 +++ src/xrt/drivers/rift_sensor/esp770u.c | 569 ++++++++++++++++++++++++++ src/xrt/drivers/rift_sensor/esp770u.h | 59 +++ src/xrt/drivers/rift_sensor/mt9v034.c | 164 ++++++++ src/xrt/drivers/rift_sensor/mt9v034.h | 41 ++ 10 files changed, 1425 insertions(+), 1 deletion(-) create mode 100644 src/xrt/drivers/rift_sensor/ar0134.c create mode 100644 src/xrt/drivers/rift_sensor/ar0134.h create mode 100644 src/xrt/drivers/rift_sensor/esp570.c create mode 100644 src/xrt/drivers/rift_sensor/esp570.h create mode 100644 src/xrt/drivers/rift_sensor/esp770u.c create mode 100644 src/xrt/drivers/rift_sensor/esp770u.h create mode 100644 src/xrt/drivers/rift_sensor/mt9v034.c create mode 100644 src/xrt/drivers/rift_sensor/mt9v034.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 94ca894fe..f22327fd9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -332,7 +332,7 @@ option_with_deps(XRT_HAVE_LIBUSB "Enable libusb (used for most drivers)" DEPENDS option_with_deps(XRT_HAVE_LIBUVC "Enable libuvc video driver" DEPENDS LIBUVC_FOUND XRT_HAVE_LIBUSB) option_with_deps(XRT_HAVE_REALSENSE "Enable RealSense support" DEPENDS realsense2_FOUND) option_with_deps(XRT_HAVE_OPENVR "Enable OpenVR support" DEPENDS OPENVR_FOUND) -option_with_deps(XRT_BUILD_DRIVER_UVC "Enable userspace UVC driver" OFF DEPENDS XRT_HAVE_LIBUSB) +option_with_deps(XRT_BUILD_DRIVER_UVC "Enable userspace UVC driver" DEPENDS XRT_HAVE_LIBUSB) # Drivers to build (sorted) option_with_deps(XRT_BUILD_DRIVER_ANDROID "Enable Android sensors driver" DEPENDS ANDROID) @@ -357,6 +357,7 @@ option_with_deps(XRT_BUILD_DRIVER_QWERTY "Enable Qwerty driver" DEPENDS XRT_HAVE option_with_deps(XRT_BUILD_DRIVER_REALSENSE "Enable RealSense device driver" DEPENDS XRT_HAVE_REALSENSE) option_with_deps(XRT_BUILD_DRIVER_REMOTE "Enable remote debugging driver" DEPENDS "XRT_HAVE_LINUX OR ANDROID OR WIN32") option_with_deps(XRT_BUILD_DRIVER_RIFT "Enable Oculus Rift (DK2/CV1) device driver" DEPENDS XRT_HAVE_INTERNAL_HID) +option_with_deps(XRT_BUILD_DRIVER_RIFT_SENSOR "Enable Oculus Rift Sensor (DK2/CV1) device driver" DEPENDS XRT_BUILD_DRIVER_RIFT XRT_BUILD_DRIVER_UVC XRT_HAVE_LIBUSB) option_with_deps(XRT_BUILD_DRIVER_RIFT_S "Enable Oculus Rift S device driver" DEPENDS XRT_HAVE_V4L2) option_with_deps(XRT_BUILD_DRIVER_ROKID "Enable Rokid driver" DEPENDS "XRT_HAVE_LIBUSB") option_with_deps(XRT_BUILD_DRIVER_STEAMVR_LIGHTHOUSE "Enable SteamVR Lighthouse driver" DEPENDS XRT_HAVE_LINUX XRT_MODULE_AUX_VIVE) @@ -467,6 +468,7 @@ list( "REALSENSE" "REMOTE" "RIFT" + "RIFT_SENSOR" "RIFT_S" "ROKID" "SURVIVE" @@ -715,6 +717,7 @@ message(STATUS "# DRIVER_QWERTY: ${XRT_BUILD_DRIVER_QWERTY}") message(STATUS "# DRIVER_REALSENSE: ${XRT_BUILD_DRIVER_REALSENSE}") message(STATUS "# DRIVER_REMOTE: ${XRT_BUILD_DRIVER_REMOTE}") message(STATUS "# DRIVER_RIFT: ${XRT_BUILD_DRIVER_RIFT}") +message(STATUS "# DRIVER_RIFT_SENSOR: ${XRT_BUILD_DRIVER_RIFT_SENSOR}") message(STATUS "# DRIVER_RIFT_S: ${XRT_BUILD_DRIVER_RIFT_S}") message(STATUS "# DRIVER_ROKID: ${XRT_BUILD_DRIVER_ROKID}") message(STATUS "# DRIVER_SIMULATED: ${XRT_BUILD_DRIVER_SIMULATED}") diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index 575c9d6da..76422c39d 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -310,6 +310,33 @@ if(XRT_BUILD_DRIVER_RIFT) list(APPEND ENABLED_HEADSET_DRIVERS rift) endif() +if(XRT_BUILD_DRIVER_RIFT_SENSOR) + add_library( + drv_rift_sensor STATIC + rift_sensor/ar0134.c + rift_sensor/ar0134.h + rift_sensor/esp570.c + rift_sensor/esp570.h + rift_sensor/esp770u.c + rift_sensor/esp770u.h + rift_sensor/mt9v034.c + rift_sensor/mt9v034.h + ) + target_include_directories(drv_rift_sensor SYSTEM PRIVATE) + target_link_libraries( + drv_rift_sensor + PRIVATE + xrt-interfaces + aux_util + aux_util_sink + aux_math + drv_includes + drv_uvc + ${LIBUSB1_LIBRARIES} + ) + target_include_directories(drv_rift_sensor PUBLIC ${LIBUSB1_INCLUDE_DIRS}) +endif() + if(XRT_BUILD_DRIVER_RIFT_S) add_library( drv_rift_s STATIC diff --git a/src/xrt/drivers/rift_sensor/ar0134.c b/src/xrt/drivers/rift_sensor/ar0134.c new file mode 100644 index 000000000..3eca13c73 --- /dev/null +++ b/src/xrt/drivers/rift_sensor/ar0134.c @@ -0,0 +1,308 @@ +// Copyright 2017, Philipp Zabel +// Copyright 2019-2025, Jan Schmidt +// Copyright 2025-2026, Beyley Cardellio +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Implementation of ar0134 sensor initialization + * @author Philipp Zabel + * @author Jan Schmidt + * @author Beyley Cardellio + * @ingroup drv_rift_sensor + */ + +#include "os/os_time.h" + +#include "util/u_logging.h" + +#include "uvc/uvc_interface.h" + +#include "ar0134.h" + + +#define XU_ENTITY 4 +#define SENSOR_REG_SEL 2 +#define PIXEL_CLOCKS_PER_LINE 1388 +#define COARSE_INTEGRATION_TIME 26 +#define FINE_INTEGRATION_TIME 646 + +/* + * Internal functions + */ + +static int +ar0134_read_reg(libusb_device_handle *dev, uint16_t reg, uint16_t *val) +{ + uint8_t buf[6] = {0x86, 0x20, reg >> 8, reg & 0xff, 0x00, 0x00}; + int ret; + + ret = uvc_set_cur(dev, 0, XU_ENTITY, SENSOR_REG_SEL, buf, sizeof(buf)); + if (ret < 0) { + return ret; + } + + ret = uvc_get_cur(dev, 0, XU_ENTITY, SENSOR_REG_SEL, buf, 3); + if (ret < 0) { + return ret; + } + + if (buf[0] != 0x86) { + U_LOG_D("(0x%x): %02x %02x %02x", reg, buf[0], buf[1], buf[2]); + } + *val = (buf[2] << 8) | buf[1]; + return ret; +} + +static int +ar0134_write_reg(libusb_device_handle *dev, uint16_t reg, uint16_t val) +{ + uint8_t buf[64] = {0x06, 0x20, reg >> 8, reg & 0xff, val >> 8, val & 0xff}; + int ret; + + ret = uvc_set_cur(dev, 0, XU_ENTITY, SENSOR_REG_SEL, buf, 6); + if (ret < 0) { + return ret; + } + + ret = uvc_get_cur(dev, 0, XU_ENTITY, SENSOR_REG_SEL, buf, sizeof buf); + if (ret < 0) { + return ret; + } + if (buf[0] != 0x06 || buf[1] != 0x20 || buf[2] != (reg >> 8) || buf[3] != (reg & 0xff) || + buf[4] != (val >> 8) || buf[5] != (val & 0xff)) { + U_LOG_D("(0x%04x, 0x%04x): %02x %02x %02x %02x %02x %02x", reg, val, buf[0], buf[1], buf[2], buf[3], + buf[4], buf[5]); + } + return ret; +} + +/* + * Exported functions + */ + +int +rift_sensor_ar0134_init(libusb_device_handle *devh, bool usb2_mode) +{ + uint16_t val; + int ret; + +#if 0 // Debug: dump all registers + for (int i = 0x3000; i < 0x3100; i += 2) { + if (i % 16 == 0) + printf("%04x: ", i); + ret = ar0134_read_reg(devh, i, &val); + if (ret < 0) return ret; + printf("%04x ", val); + if (i % 16 == 14) + printf("\n"); + } +#endif + + // Read chip version and revision number registers + for (int i = 0; i < 10; i++) { + ret = ar0134_read_reg(devh, 0x3000, &val); + if (ret < 0 || val != 0x2406) { + // 5000us sleep between retries + os_nanosleep(OS_NS_PER_USEC * 5000); + continue; + } + break; + } + if (ret < 0) { + return ret; + } + if (val != 0x2406) { + U_LOG_E("This is not an AR0134 sensor: 0x%04x", val); + } + + ret = ar0134_read_reg(devh, 0x300e, &val); + if (ret < 0) { + return ret; + } + if (val != 0x1300) { + U_LOG_W("Unexpected revision number: 0x%04x", val); + } + + ret = ar0134_read_reg(devh, 0x30b0, &val); + if (ret < 0) { + return ret; + } + if (val != 0x0080) { + U_LOG_W("Expected monochrome mode instead of 0x%04x", val); + } + + // Disable embedded register data and statistics. For now we can't use this anyway. + ret = ar0134_read_reg(devh, 0x3064, &val); + if (ret < 0) { + return ret; + } + if (val != 0x1882 && val != 0x1982) { + U_LOG_W("Unexpected embedded data control: %04x", val); + } + ret = ar0134_write_reg(devh, 0x3064, val & ~0x0180); + if (ret < 0) { + return ret; + } + + // Set data pedestal (black level) to zero + ret = ar0134_write_reg(devh, 0x301e, 0); + if (ret < 0) { + return ret; + } + + // Disable binning + ret = ar0134_write_reg(devh, 0x3032, 0x0000); + if (ret < 0) { + return ret; + } + + // Set all gain values to the default, in USB2 mode 0x0007 is used instead. + ret = ar0134_write_reg(devh, 0x305e, usb2_mode ? 0x0007 : 0x0020); + if (ret < 0) { + return ret; + } + + // This changes nothing, probably clear some already cleared bits. + ret = ar0134_read_reg(devh, 0x30b0, &val); + if (ret < 0) { + return ret; + } + val = 0x0080; // val &= ~(?); + ret = ar0134_write_reg(devh, 0x30b0, val); + if (ret < 0) { + return ret; + } + + // Set vertical and horizontal capture ranges to maximum (0-959, 0-1279). + ret = ar0134_write_reg(devh, 0x3002, 0); + if (ret < 0) { + return ret; + } + ret = ar0134_write_reg(devh, 0x3006, 959); + if (ret < 0) { + return ret; + } + ret = ar0134_write_reg(devh, 0x3004, 0); + if (ret < 0) { + return ret; + } + ret = ar0134_write_reg(devh, 0x3008, 1279); + if (ret < 0) { + return ret; + } + + // Set minimum supported pixel clocks per line + ret = ar0134_write_reg(devh, 0x300c, PIXEL_CLOCKS_PER_LINE); + if (ret < 0) { + return ret; + } + + // Set a short line bit, needed for the 1388 pclk line length above. + ret = ar0134_read_reg(devh, 0x30b0, &val); + if (ret < 0) { + return ret; + } + val |= (1 << 10); + ret = ar0134_write_reg(devh, 0x30b0, val); + if (ret < 0) { + return ret; + } + + // Set total number of lines, 37 lines vertical blanking + ret = ar0134_write_reg(devh, 0x300a, 997); + if (ret < 0) { + return ret; + } + + // Read number of pixel clocks per line, should be changed above. + ret = ar0134_read_reg(devh, 0x300c, &val); + if (ret < 0) { + return ret; + } + + if (val != PIXEL_CLOCKS_PER_LINE) { + U_LOG_E("Too many pixel clocks per line: %u, wanted %u", val, PIXEL_CLOCKS_PER_LINE); + } + + // Set coarse integration time (in multiples of lines) and + // fine integration time (in multiples of the pixel clock). + ret = ar0134_write_reg(devh, 0x3012, COARSE_INTEGRATION_TIME); // 26 lines + if (ret < 0) { + return ret; + } + ret = ar0134_write_reg(devh, 0x3014, FINE_INTEGRATION_TIME); + if (ret < 0) { + return ret; + } + ret = ar0134_read_reg(devh, 0x3012, &val); + if (ret < 0) { + return ret; + } + if (val != COARSE_INTEGRATION_TIME) { + U_LOG_E("Failed to set coarse integration time: %u", val); + } + ret = ar0134_read_reg(devh, 0x3014, &val); + if (ret < 0) { + return ret; + } + if (val != FINE_INTEGRATION_TIME) { + U_LOG_E("Failed to set fine integration time: %u", val); + } + + // Stop streaming, trigger external exposure from nRF51288 + ret = ar0134_read_reg(devh, 0x301a, &val); + if (ret < 0) { + return ret; + } + if (val != 0x10dc) { + U_LOG_W("Unexpected reset register value: 0x%04x", val); + } + // Force PLL always enabled + val |= 1 << 11; + // Enable trigger input pin + val |= 1 << 8; + // Disable streaming (switch to externally triggered mode) + val &= ~(1 << 2); + ret = ar0134_write_reg(devh, 0x301a, val); + if (ret < 0) { + return ret; + } + + // Enable junction temperature sensor + ret = ar0134_read_reg(devh, 0x30b4, &val); + if (ret < 0) { + return ret; + } + // Temperature sensor power + val |= 1 << 0; + // Start conversion + val |= 1 << 4; + ret = ar0134_write_reg(devh, 0x30b4, val); + if (ret < 0) { + return ret; + } + + // Read 70 °C calibration point + uint16_t calib_70c; + ret = ar0134_read_reg(devh, 0x30c6, &calib_70c); + if (ret < 0) { + return ret; + } + + // Read 50 °C calibration point + uint16_t calib_50c; + ret = ar0134_read_reg(devh, 0x30c8, &calib_50c); + if (ret < 0) { + return ret; + } + + // Read temperature sensor + ret = ar0134_read_reg(devh, 0x30b2, &val); + if (ret < 0) { + return ret; + } + + U_LOG_D("Temperature: %.1f °C", 50.0 + 20.0 * (val - calib_50c) / (calib_70c - calib_50c)); + + return 0; +} diff --git a/src/xrt/drivers/rift_sensor/ar0134.h b/src/xrt/drivers/rift_sensor/ar0134.h new file mode 100644 index 000000000..f64d5dddf --- /dev/null +++ b/src/xrt/drivers/rift_sensor/ar0134.h @@ -0,0 +1,29 @@ +// Copyright 2017, Philipp Zabel +// Copyright 2019-2025, Jan Schmidt +// Copyright 2025-2026, Beyley Cardellio +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Implementation of ar0134 sensor initialization + * @author Philipp Zabel + * @author Jan Schmidt + * @author Beyley Cardellio + * @ingroup drv_rift_sensor + */ + +#pragma once + +#include "xrt/xrt_defines.h" + +#include + + +/*! + * Sensor setup after stream start + * + * @param[in] devh libusb device handle + * @param[in] usb2_mode Whether to configure for USB2 mode + * @return 0 on success, negative error code on failure + */ +int +rift_sensor_ar0134_init(libusb_device_handle *devh, bool usb2_mode); \ No newline at end of file diff --git a/src/xrt/drivers/rift_sensor/esp570.c b/src/xrt/drivers/rift_sensor/esp570.c new file mode 100644 index 000000000..cbd31c421 --- /dev/null +++ b/src/xrt/drivers/rift_sensor/esp570.c @@ -0,0 +1,157 @@ +// Copyright 2014-2020, Philipp Zabel +// Copyright 2017, TheOnlyJoey +// Copyright 2025, Jan Schmidt +// Copyright 2025-2026, Beyley Cardellio +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Implementation of esp570 sensor initialization + * @author Philipp Zabel + * @author TheOnlyJoey + * @author Jan Schmidt + * @author Beyley Cardellio + * @ingroup drv_rift_sensor + */ + +#include "util/u_logging.h" + +#include "uvc/uvc_interface.h" + +#include "esp570.h" + +#include +#include + + +#define ESP570_EXTENSION_UNIT 4 + +#define ESP570_SELECTOR_I2C 2 +#define ESP570_SELECTOR_UNKNOWN_3 3 +#define ESP570_SELECTOR_EEPROM 5 + +/* + * Internal functions + */ + +/*! + * Calls SET_CUR and then GET_CUR on a given selector of the eSP570 UVC + * extension unit. + * + * @param[in] devh libusb device handle + * @param[in] selector Selector to call on + * @param[in] buf Buffer to send with SET_CUR, and receive with GET_CUR + * @param[in] len Length of the buffer + * @return 0 on success, negative error code on failure + */ +static int +uvc_xu_set_get_cur(libusb_device_handle *devh, int selector, uint8_t *buf, uint8_t len) +{ + int ret = uvc_set_cur(devh, 0, ESP570_EXTENSION_UNIT, selector, buf, len); + if (ret < 0) { + U_LOG_E("uvc_set_cur failed: %d", ret); + return ret; + } + + return uvc_get_cur(devh, 0, ESP570_EXTENSION_UNIT, selector, buf, len); +} + +/* + * Exported functions + */ + +int +esp570_eeprom_read(libusb_device_handle *devh, uint16_t addr, uint8_t *data, uint8_t len) +{ + uint8_t buf[59] = {0x82, 0xa0, addr >> 8, addr & 0xff, len}; + + if (len > 32) { + return -EINVAL; + } + + int ret = uvc_xu_set_get_cur(devh, ESP570_SELECTOR_EEPROM, buf, sizeof buf); + if (ret < 0) { + U_LOG_E("uvc_xu_set_get_cur failed: %d", ret); + return ret; + } + + if (buf[0] != 0x82 || buf[1] != len) { + U_LOG_E("read_buf: error: 0x%02x 0x%02x", buf[0], buf[1]); + return -1; + } + + memcpy(data, buf + 2, len); + return len; +} + +int +esp570_i2c_read(libusb_device_handle *devh, uint8_t addr, uint8_t reg, uint16_t *val) +{ + uint8_t buf[6] = {0x84, addr, reg}; + + int ret = uvc_xu_set_get_cur(devh, ESP570_SELECTOR_I2C, buf, sizeof buf); + if (ret < 0) { + return ret; + } + + if (buf[0] != 0x84 || buf[4] != 0x00 || buf[5] != 0x00) { + U_LOG_W("(%02x): %02x %02x %02x %02x %02x %02x", reg, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]); + return -1; + } + + *val = (buf[1] << 8) | buf[2]; + + return 0; +} + +int +esp570_i2c_write(libusb_device_handle *devh, uint8_t addr, uint8_t reg, uint16_t val) +{ + uint8_t buf[6] = {0x04, addr, reg, val >> 8, val & 0xff}; + + int ret = uvc_xu_set_get_cur(devh, ESP570_SELECTOR_I2C, buf, sizeof buf); + if (ret < 0) { + return ret; + } + + if (buf[0] != 0x04 || buf[1] != addr || buf[2] != reg || buf[5] != 0) { + U_LOG_W("(%02x): %02x %02x %02x %02x %02x %02x", reg, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]); + return -1; + } + + if (buf[3] != (val >> 8) || buf[4] != (val & 0xff)) { + U_LOG_W("(%02x, %04x): read back mismatch 0x%04x", reg, val, (buf[3] << 8) | buf[4]); + } + + return 0; +} + +int +esp570_setup_unknown_3(libusb_device_handle *devh) +{ + uint8_t buf[3]; + int ret; + + buf[0] = 0x80; + buf[1] = 0x14; + buf[2] = 0x00; + ret = uvc_xu_set_get_cur(devh, ESP570_SELECTOR_UNKNOWN_3, buf, 3); + if (ret < 0) { + return ret; + } + if (buf[0] != 0x80 || buf[1] != 0xdc || buf[2] != 0x00) { + U_LOG_W("(80 14 00): %02x %02x %02x", buf[0], buf[1], buf[2]); + } + + buf[0] = 0xa0; + buf[1] = 0xf0; + buf[2] = 0x00; + ret = uvc_xu_set_get_cur(devh, ESP570_SELECTOR_UNKNOWN_3, buf, 3); + if (ret < 0) { + return ret; + } + if (buf[0] != 0xa0 || buf[1] != 0x98 || buf[2] != 0x00) { + U_LOG_W("(80 14 00): %02x %02x %02x", buf[0], buf[1], buf[2]); + } + + return 0; +} diff --git a/src/xrt/drivers/rift_sensor/esp570.h b/src/xrt/drivers/rift_sensor/esp570.h new file mode 100644 index 000000000..c9a7ae6ce --- /dev/null +++ b/src/xrt/drivers/rift_sensor/esp570.h @@ -0,0 +1,67 @@ +// Copyright 2014-2020, Philipp Zabel +// Copyright 2017, TheOnlyJoey +// Copyright 2025, Jan Schmidt +// Copyright 2025-2026, Beyley Cardellio +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Implementation of esp570 sensor initialization + * @author Philipp Zabel + * @author TheOnlyJoey + * @author Jan Schmidt + * @author Beyley Cardellio + * @ingroup drv_rift_sensor + */ + +#pragma once + +#include "xrt/xrt_defines.h" + +#include + + +/*! + * Performs a 16-bit read operation on the I2C bus. + * + * @param[in] devh libusb device handle + * @param[in] addr I2C address + * @param[in] reg Register to read from + * @param[out] val Value read + * @return 0 on success, negative error code on failure + */ +int +esp570_i2c_read(libusb_device_handle *devh, uint8_t addr, uint8_t reg, uint16_t *val); + +/*! + * Performs a 16-bit write operation on the I2C bus. + * + * @param[in] devh libusb device handle + * @param[in] addr I2C address + * @param[in] reg Register to write to + * @param[in] val Value to write + * @return 0 on success, negative error code on failure + */ +int +esp570_i2c_write(libusb_device_handle *devh, uint8_t addr, uint8_t reg, uint16_t val); + +/*! + * Reads a buffer from the Microchip 24AA128 EEPROM. + * + * @param[in] devh libusb device handle + * @param[in] addr Address to read from + * @param[out] buf Buffer to read into + * @param[in] len Number of bytes to read (max 32) + * @return 0 on success, negative error code on failure + */ +int +esp570_eeprom_read(libusb_device_handle *devh, uint16_t addr, uint8_t *buf, uint8_t len); + +/*! + * Calls SET_CUR and GET_CUR on the extension unit's selector 3 with values + * captured from the Oculus Windows drivers to setup the device. I have no idea what these mean. + * + * @param[in] devh libusb device handle + * @return 0 on success, negative error code on failure + */ +int +esp570_setup_unknown_3(libusb_device_handle *devh); diff --git a/src/xrt/drivers/rift_sensor/esp770u.c b/src/xrt/drivers/rift_sensor/esp770u.c new file mode 100644 index 000000000..aea8c1c85 --- /dev/null +++ b/src/xrt/drivers/rift_sensor/esp770u.c @@ -0,0 +1,569 @@ +// Copyright 2017, Philipp Zabel +// Copyright 2025, Jan Schmidt +// Copyright 2025-2026, Beyley Cardellio +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Implementation of esp770u sensor initialization + * @author Philipp Zabel + * @author Jan Schmidt + * @author Beyley Cardellio + * @ingroup drv_rift_sensor + */ + +#include "util/u_logging.h" + +#include "uvc/uvc_interface.h" + +#include "esp770u.h" + +#include +#include + + +#define ESP770U_EXTENSION_UNIT 4 + +#define ESP770U_SELECTOR_I2C 2 +#define ESP770U_SELECTOR_REG 3 +#define ESP770U_SELECTOR_COUNTER 10 +#define ESP770U_SELECTOR_CONTROL 11 +#define ESP770U_SELECTOR_DATA 12 + +#define XU_ENTITY ESP770U_EXTENSION_UNIT +#define REG_SEL ESP770U_SELECTOR_REG +#define CONTROL_SEL ESP770U_SELECTOR_CONTROL +#define DATA_SEL ESP770U_SELECTOR_DATA + +#define ESP_DEBUG 0 + +#define esp770u_read_reg(d, r, v) rift_sensor_esp770u_read_reg((d), 0xf0, (r), (v)) +#define esp770u_read_reg_f1(d, r, v) rift_sensor_esp770u_read_reg((d), 0xf1, (r), (v)) +#define esp770u_write_reg(d, r, v) rift_sensor_esp770u_write_reg((d), 0xf0, (r), (v)) +#define esp770u_write_reg_f1(d, r, v) rift_sensor_esp770u_write_reg((d), 0xf1, (r), (v)) + +/* + * Internal functions + */ + +static int +set_get_verify_a0(libusb_device_handle *dev, uint8_t val, uint8_t retval) +{ + uint8_t buf[4] = {0xa0, val, 0x00, 0x00}; + int ret; + + ret = uvc_set_cur(dev, 0, XU_ENTITY, REG_SEL, buf, sizeof buf); + if (ret < 0) { + return ret; + } + memset(buf, 0, sizeof buf); + ret = uvc_get_cur(dev, 0, XU_ENTITY, REG_SEL, buf, sizeof buf); + if (ret < 0) { + return ret; + } + if (buf[0] != 0xa0 || buf[1] != retval || buf[2] != 0x00) { + U_LOG_W("response, should be a0 %02x 00: %02x %02x %02x", retval, buf[0], buf[1], buf[2]); + return -1; + } + + return 0; +} + +/* + * Read self-incrementing counter. + */ +static int +esp770u_get_counter(libusb_device_handle *devh, uint8_t *count) +{ + return uvc_get_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_COUNTER, count, 1); +} + +/* + * Write back self-incrementing counter. + */ +static int +esp770u_set_counter(libusb_device_handle *devh, uint8_t count) +{ + return uvc_set_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_COUNTER, &count, 1); +} + +static int +esp770u_spi_set_control(libusb_device_handle *devh, uint8_t a, size_t len) +{ + // a is alternating, 0x81 or 0x41 + uint8_t control[16] = {0x00, a, 0x80, 0x01, [9] = len}; + + return uvc_set_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_CONTROL, control, sizeof control); +} + +static int +esp770u_spi_set_data(libusb_device_handle *devh, uint8_t *data, size_t len) +{ + return uvc_set_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_DATA, data, len); +} + +static int +esp770u_spi_get_data(libusb_device_handle *devh, uint8_t *data, size_t len) +{ + return uvc_get_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_DATA, data, len); +} + +static int +radio_write(libusb_device_handle *devhandle, const uint8_t *buf, size_t len) +{ + uint8_t data[127]; + int ret; + size_t i; + + if (len > 126) { + return -EINVAL; + } + + memset(data, 0, sizeof data); + for (i = 0; i < len; i++) { + data[i] = buf[i]; + data[126] -= buf[i]; // calculate checksum + } + + ret = esp770u_spi_set_control(devhandle, 0x81, sizeof data); + if (ret < 0) { + return ret; + } + + // send data + ret = esp770u_spi_set_data(devhandle, data, sizeof data); + if (ret < 0) { + return ret; + } + + ret = esp770u_spi_set_control(devhandle, 0x41, sizeof data); + if (ret < 0) { + return ret; + } + + // expect all zeros + ret = esp770u_spi_get_data(devhandle, data, sizeof data); + if (ret < 0) { + return ret; + } + + ret = esp770u_spi_set_control(devhandle, 0x81, sizeof data); + if (ret < 0) { + return ret; + } + + // clear + memset(data, 0, sizeof data); + ret = esp770u_spi_set_data(devhandle, data, sizeof data); + if (ret < 0) { + return ret; + } + + ret = esp770u_spi_set_control(devhandle, 0x41, sizeof data); + if (ret < 0) { + return ret; + } + + ret = esp770u_spi_get_data(devhandle, data, sizeof data); + if (ret < 0) { + return ret; + } + + // Expect zeros + for (i = 2; i < 126; i++) { + if (data[i]) { + U_LOG_W("Unexpected byte (%02x at %02x):", data[i], (uint8_t)i); + break; + } + } + + if (data[0] != buf[0] || data[1] != buf[1]) { + U_LOG_W("Unexpected read (%02x %02x):", buf[0], buf[1]); + for (i = 0; i < 127; i++) { + U_LOG_D("%02x ", data[i]); + } + } + + uint8_t checksum = 0; + for (i = 0; i < 127; i++) { + checksum += data[i]; + } + if (checksum) { + U_LOG_W("Checksum mismatch: %02x", checksum); + for (i = 0; i < 127; i++) { + U_LOG_D("%02x ", data[i]); + } + } + + return 0; +} + +static int +rift_sensor_esp770u_read_reg(libusb_device_handle *dev, uint8_t reg_block, uint8_t reg, uint8_t *val) +{ + uint8_t buf[4] = {0x82, reg_block, reg}; + int ret; + + ret = uvc_set_cur(dev, 0, XU_ENTITY, REG_SEL, buf, sizeof buf); + if (ret < 0) { + return ret; + } + ret = uvc_get_cur(dev, 0, XU_ENTITY, REG_SEL, buf, 3); + if (ret < 0) { + return ret; + } + if (buf[0] != 0x82 || buf[2] != 0x00) { + U_LOG_W("read_reg(0x%x,0x%x): %02x %02x %02x", reg_block, reg, buf[0], buf[1], buf[2]); + } + *val = buf[1]; + return ret; +} + +static int +rift_sensor_esp770u_write_reg(libusb_device_handle *dev, uint8_t reg_block, uint8_t reg, uint8_t val) +{ + uint8_t buf[4] = {0x02, reg_block, reg, val}; + int ret; + + ret = uvc_set_cur(dev, 0, XU_ENTITY, REG_SEL, buf, sizeof buf); + if (ret < 0) { + return ret; + } + ret = uvc_get_cur(dev, 0, XU_ENTITY, REG_SEL, buf, sizeof buf); + if (ret < 0) { + return ret; + } + if (buf[0] != 0x02 || buf[1] != reg_block || buf[2] != reg || buf[3] != val) { + U_LOG_W("write_reg(0x%x,0x%x,0x%x): %02x %02x %02x %02x", reg_block, reg, val, buf[0], buf[1], buf[2], + buf[3]); + } + return ret; +} + +/* + * Exported functions + */ + +int +rift_sensor_esp770u_flash_read(libusb_device_handle *devh, uint32_t addr, uint8_t *data, uint16_t len) +{ + uint8_t control[16]; + uint8_t count; + int ret; + + ret = esp770u_get_counter(devh, &count); + if (ret < 0) { + return ret; + } + + memset(control, 0, sizeof control); + control[0] = count; + control[1] = 0x41; + control[2] = 0x03; + control[3] = 0x01; + + control[5] = (addr >> 16) & 0xff; + control[6] = (addr >> 8) & 0xff; + control[7] = addr & 0xff; + + control[8] = len >> 8; + control[9] = len & 0xff; + + ret = uvc_set_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_CONTROL, control, sizeof control); + if (ret < 0) { + return ret; + } + + memset(data, 0, len); + ret = uvc_get_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_DATA, data, len); + if (ret < 0) { + return ret; + } + + return esp770u_set_counter(devh, count); +} + +int +rift_sensor_esp770u_setup_radio(libusb_device_handle *devhandle, const uint8_t radio_id[5]) +{ + int ret; + + const uint8_t cmd1[7] = {0x40, 0x10, radio_id[0], radio_id[1], radio_id[2], radio_id[3], radio_id[4]}; + ret = radio_write(devhandle, cmd1, sizeof cmd1); + if (ret < 0) { + return ret; + } + + const uint8_t cmd2[10] = {0x50, 0x11, 0xf4, 0x01, 0x00, 0x00, 0x67, 0xff, 0xff, 0xff}; + ret = radio_write(devhandle, cmd2, sizeof cmd2); + if (ret < 0) { + return ret; + } + + const uint8_t cmd3[2] = {0x61, 0x12}; + ret = radio_write(devhandle, cmd3, sizeof cmd3); + if (ret < 0) { + return ret; + } + + const uint8_t cmd4[2] = {0x71, 0x85}; + ret = radio_write(devhandle, cmd4, sizeof cmd4); + if (ret < 0) { + return ret; + } + + const uint8_t cmd5[2] = {0x81, 0x86}; + return radio_write(devhandle, cmd5, sizeof cmd5); +} + +int +rift_sensor_esp770u_init_regs(libusb_device_handle *devhandle) +{ + int ret; + uint8_t val; + + ret = set_get_verify_a0(devhandle, 0x03, 0xb2); + if (ret < 0) { + return ret; + } + + ret = esp770u_read_reg(devhandle, 0x5a, &val); + if (ret < 0) { + return ret; + } + if (val != 0x01 && val != 0x03) { + U_LOG_W("unexpected 5a value: %02x", val); + } + + ret = esp770u_write_reg(devhandle, 0x5a, 0x01); /* &= 0x02? */ + if (ret < 0) { + return ret; + } + ret = esp770u_read_reg(devhandle, 0x5a, &val); + if (ret < 0) { + return ret; + } + if (val != 0x01) { + U_LOG_W("unexpected 5a value: %02x", val); + } + + ret = esp770u_read_reg(devhandle, 0x18, &val); /* |= 0x01 ? */ + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg(devhandle, 0x18, 0x0f); + if (ret < 0) { + return ret; + } + ret = esp770u_read_reg(devhandle, 0x17, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg(devhandle, 0x17, 0xed); /* |= 0x01 ? */ + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg(devhandle, 0x17, 0xec); /* &= ~0x01 ? */ + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg(devhandle, 0x18, 0x0e); /* &= ~0x01 ? */ + if (ret < 0) { + return ret; + } + ret = esp770u_read_reg(devhandle, 0x14, &val); + if (ret < 0) { + return ret; + } + +#if ESP_DEBUG + for (int i = 0; i < 256; i += 1) { + uint8_t rval; + if (i % 16 == 0) { + printf("%02x: ", i); + } + ret = esp770u_read_reg(devhandle, i, &rval); + if (ret < 0) { + return ret; + } + printf("%02x ", rval); + if (i % 16 == 15) { + printf("\n"); + } + } + + printf("--------------------------------------\n"); + + for (int i = 0; i < 256; i += 1) { + uint8_t rval; + if (i % 16 == 0) { + printf("%02x: ", i); + } + ret = esp770u_read_reg_f1(devhandle, i, &rval); + if (ret < 0) { + return ret; + } + printf("%02x ", rval); + if (i % 16 == 15) { + printf("\n"); + } + } + printf("--------------------------------------\n"); +#endif + + return ret; +} + +int +rift_sensor_esp770u_init_jpeg(libusb_device_handle *devhandle) +{ + int ret = 0; + uint8_t val; + + /* 0xf11f = 0x04 */ + ret = esp770u_read_reg_f1(devhandle, 0x1f, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x1f, 0x04); + if (ret < 0) { + return ret; + } + + /* 0xf102 = 0x7f */ + ret = esp770u_read_reg_f1(devhandle, 0x02, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x02, 0x7f); + if (ret < 0) { + return ret; + } + + /* 0xf101 = 0xff */ + ret = esp770u_read_reg_f1(devhandle, 0x01, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x01, 0xff); + if (ret < 0) { + return ret; + } + + /* 0xf103 = 0xc0 */ + ret = esp770u_read_reg_f1(devhandle, 0x03, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x03, 0xc0); + if (ret < 0) { + return ret; + } + + /* 0xf101 = 0xff, 2 times */ + ret = esp770u_read_reg_f1(devhandle, 0x01, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x01, 0xff); + if (ret < 0) { + return ret; + } + ret = esp770u_read_reg_f1(devhandle, 0x01, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x01, 0xff); + if (ret < 0) { + return ret; + } + + /* 0xf11e = 0x01 */ + ret = esp770u_read_reg_f1(devhandle, 0x1e, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x1e, 0x01); + if (ret < 0) { + return ret; + } + + /* 0xf103 = 0xc0 */ + ret = esp770u_read_reg_f1(devhandle, 0x03, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x03, 0xc0); + if (ret < 0) { + return ret; + } + + /* 0xf101 = 0xff, once */ + ret = esp770u_read_reg_f1(devhandle, 0x01, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x01, 0xff); + if (ret < 0) { + return ret; + } + + /* 0xf102 = 0x7f again */ + ret = esp770u_read_reg_f1(devhandle, 0x02, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x02, 0x7f); + if (ret < 0) { + return ret; + } + + /* 0xf101 = 0xff, 2 more times */ + ret = esp770u_read_reg_f1(devhandle, 0x01, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x01, 0xff); + if (ret < 0) { + return ret; + } + + // @todo is this early return intentional? + return ret; + + + // https://discord.com/channels/556527313823596604/1140622730018967623/1471716745164230807 +#if 0 + // 0xf102 = 0x7f, 2 more times + ret = esp770u_read_reg_f1(devhandle, 0x02, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x02, 0x7f); + if (ret < 0) { + return ret; + } + + ret = esp770u_read_reg_f1(devhandle, 0x02, &val); + if (ret < 0) { + return ret; + } + ret = esp770u_write_reg_f1(devhandle, 0x02, 0x7f); + if (ret < 0) { + return ret; + } +#endif + + /* + * Extra 64-byte fw writes, not sent for now - doesn't seem to matter: + * Data Fragment: 1681030100012ed10040000000000000 + * Data Fragment: 070707070707070707070707070707070707070707070707070707070707070707070707… + * Data Fragment: 16020100000000000000000000000000 + * Data Fragment: 16 + * Data Fragment: 1781030100012f160040000000000000 + * Data Fragment: 010101010101010101010101010101010101010101010101010101010101010101010101… + * Data Fragment: 17020100000000000000000000000000 + * Data Fragment: 17 + */ + + return ret; +} diff --git a/src/xrt/drivers/rift_sensor/esp770u.h b/src/xrt/drivers/rift_sensor/esp770u.h new file mode 100644 index 000000000..82c54a68c --- /dev/null +++ b/src/xrt/drivers/rift_sensor/esp770u.h @@ -0,0 +1,59 @@ +// Copyright 2017, Philipp Zabel +// Copyright 2025, Jan Schmidt +// Copyright 2025-2026, Beyley Cardellio +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Implementation of esp770u sensor initialization + * @author Philipp Zabel + * @author Jan Schmidt + * @author Beyley Cardellio + * @ingroup drv_rift_sensor + */ + +#pragma once + +#include "xrt/xrt_defines.h" + +#include + + +/*! + * Reads a buffer from the flash storage. + * + * @param[in] devh libusb device handle + * @param[in] addr Address to read from + * @param[out] data Buffer to read into + * @param[in] len Number of bytes to read + * @return 0 on success, negative error code on failure + */ +int +rift_sensor_esp770u_flash_read(libusb_device_handle *devh, uint32_t addr, uint8_t *data, uint16_t len); + +/*! + * Sets up the radio with the given ID. + * + * @param[in] devhandle libusb device handle + * @param[in] radio_id 5-byte radio ID + * @return 0 on success, negative error code on failure + */ +int +rift_sensor_esp770u_setup_radio(libusb_device_handle *devhandle, const uint8_t radio_id[5]); + +/*! + * Initial register setup, only after camera plugin + * + * @param[in] devhandle libusb device handle + * @return 0 on success, negative error code on failure + */ +int +rift_sensor_esp770u_init_regs(libusb_device_handle *devhandle); + +/*! + * Extra initialisation sent after UVC config when in USB2 / MJPEG mode + * + * @param[in] devhandle libusb device handle + * @return 0 on success, negative error code on failure + */ +int +rift_sensor_esp770u_init_jpeg(libusb_device_handle *devhandle); diff --git a/src/xrt/drivers/rift_sensor/mt9v034.c b/src/xrt/drivers/rift_sensor/mt9v034.c new file mode 100644 index 000000000..495b4082a --- /dev/null +++ b/src/xrt/drivers/rift_sensor/mt9v034.c @@ -0,0 +1,164 @@ +// Copyright 2014-2020, Philipp Zabel +// Copyright 2017, TheOnlyJoey +// Copyright 2025, Jan Schmidt +// Copyright 2025-2026, Beyley Cardellio +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Implementation of mt9v034 sensor initialization + * @author Philipp Zabel + * @author TheOnlyJoey + * @author Jan Schmidt + * @author Beyley Cardellio + * @ingroup drv_rift_sensor + */ + +#include "util/u_logging.h" + +#include "esp570.h" +#include "mt9v034.h" + +#include +#include + + +#define MT9V034_CHIP_VERSION 0x00 +#define MT9V034_WINDOW_HEIGHT 0x03 +#define MT9V034_WINDOW_WIDTH 0x04 +#define MT9V034_HORIZONTAL_BLANKING 0x05 +#define MT9V034_VERTICAL_BLANKING 0x06 +#define MT9V034_CHIP_CONTROL 0x07 +#define MT9V034_COARSE_SHUTTER_WIDTH_TOTAL 0x0b +#define MT9V034_READ_MODE 0x0d +#define MT9V034_LED_OUT_CONTROL 0x1b +#define MT9V034_ANALOG_GAIN 0x35 +#define MT9V034_BLACK_LEVEL_CALIB_CTRL 0x47 +#define MT9V034_BLACK_LEVEL_CALIB_VALUE 0x48 +#define MT9V034_AEC_AGC_ENABLE 0xaf +#define MT9V034_MAX_TOTAL_SHUTTER_WIDTH 0xbd +#define MT9V034_FINE_SHUTTER_WIDTH_TOTAL 0xd5 + +#define MT9V034_ANALOG_GAIN_MIN 16 + +#define MT9V034_CHIP_CONTROL_MASTER_MODE (1 << 3) +#define MT9V034_CHIP_CONTROL_SNAPSHOT_MODE (3 << 3) +#define MT9V034_CHIP_CONTROL_DOUT_ENABLE (1 << 7) +#define MT9V034_CHIP_CONTROL_SEQUENTIAL (1 << 8) + +#define MT9V032_READ_MODE_RESERVED 0x300 +#define MT9V034_READ_MODE_ROW_FLIP (1 << 4) +#define MT9V034_READ_MODE_COLUMN_FLIP (1 << 5) + +#define MT9V034_BLACK_LEVEL_CALIB_OVERRIDE (1 << 0) + +#define MT9V034_LED_OUT_DISABLE (1 << 0) +#define MT9V034_LED_OUT_INVERT (1 << 1) + +#define MT9V034_I2C_ADDR 0x4c + +/* + * Internal functions + */ + +static inline int +mt9v034_read_reg(libusb_device_handle *devh, uint8_t reg, uint16_t *val) +{ + return esp570_i2c_read(devh, MT9V034_I2C_ADDR << 1, reg, val); +} + +static inline int +mt9v034_write_reg(libusb_device_handle *devh, uint8_t reg, uint16_t val) +{ + return esp570_i2c_write(devh, MT9V034_I2C_ADDR << 1, reg, val); +} + +static int +mt9v034_enable_sync(libusb_device_handle *devh) +{ + uint16_t chip_control; + + // Disable AEC/AGC + mt9v034_write_reg(devh, MT9V034_AEC_AGC_ENABLE, 0); + // Raise black level with manual black level calibration override + mt9v034_write_reg(devh, MT9V034_BLACK_LEVEL_CALIB_CTRL, MT9V034_BLACK_LEVEL_CALIB_OVERRIDE); + mt9v034_write_reg(devh, MT9V034_BLACK_LEVEL_CALIB_VALUE, 0x81); // -127 + // Reduce analog gain to minimum + mt9v034_write_reg(devh, MT9V034_ANALOG_GAIN, MT9V034_ANALOG_GAIN_MIN); + mt9v034_write_reg(devh, MT9V034_VERTICAL_BLANKING, 5); + mt9v034_write_reg(devh, MT9V034_HORIZONTAL_BLANKING, 94); + mt9v034_write_reg(devh, MT9V034_CHIP_CONTROL, + MT9V034_CHIP_CONTROL_MASTER_MODE | MT9V034_CHIP_CONTROL_DOUT_ENABLE | + MT9V034_CHIP_CONTROL_SEQUENTIAL); + + // Set integration time in number of rows + number of clock cycles + mt9v034_write_reg(devh, MT9V034_COARSE_SHUTTER_WIDTH_TOTAL, 11); + mt9v034_write_reg(devh, MT9V034_FINE_SHUTTER_WIDTH_TOTAL, 111); + // Switch to snapshot mode, exposure controlled by Rift DK2 HMD + mt9v034_read_reg(devh, MT9V034_CHIP_CONTROL, &chip_control); + if (chip_control != + (MT9V034_CHIP_CONTROL_MASTER_MODE | MT9V034_CHIP_CONTROL_DOUT_ENABLE | MT9V034_CHIP_CONTROL_SEQUENTIAL)) { + U_LOG_W("Unexpected chip control value: 0x%04x", chip_control); + } + mt9v034_write_reg(devh, MT9V034_CHIP_CONTROL, + MT9V034_CHIP_CONTROL_SNAPSHOT_MODE | MT9V034_CHIP_CONTROL_DOUT_ENABLE | + MT9V034_CHIP_CONTROL_SEQUENTIAL); + // Enable LED? + mt9v034_write_reg(devh, MT9V034_LED_OUT_CONTROL, MT9V034_LED_OUT_INVERT); + return 0; +} + +static int +mt9v034_disable_sync(libusb_device_handle *devh) +{ + // Disable LED? + mt9v034_write_reg(devh, MT9V034_LED_OUT_CONTROL, 0); + + // Disable AEC/AGC + mt9v034_write_reg(devh, MT9V034_AEC_AGC_ENABLE, 0); + // Raise black level with manual black level calibration override + mt9v034_write_reg(devh, MT9V034_BLACK_LEVEL_CALIB_CTRL, 0x80); + // Set analog gain to default + mt9v034_write_reg(devh, MT9V034_ANALOG_GAIN, 0x20); + mt9v034_write_reg(devh, MT9V034_VERTICAL_BLANKING, 57); + mt9v034_write_reg(devh, MT9V034_HORIZONTAL_BLANKING, 94); + mt9v034_write_reg(devh, MT9V034_CHIP_CONTROL, + MT9V034_CHIP_CONTROL_MASTER_MODE | MT9V034_CHIP_CONTROL_DOUT_ENABLE | + MT9V034_CHIP_CONTROL_SEQUENTIAL); + // Set integration time in number of rows + number of clock cycles + mt9v034_write_reg(devh, MT9V034_COARSE_SHUTTER_WIDTH_TOTAL, 0xf0); + mt9v034_write_reg(devh, MT9V034_FINE_SHUTTER_WIDTH_TOTAL, 0); + + return 0; +} + +/* + * Exported functions + */ + +int +mt9v034_setup(libusb_device_handle *devh) +{ + uint16_t version, read_mode; + + mt9v034_read_reg(devh, MT9V034_CHIP_VERSION, &version); + if (version != 0x1324) { + return -1; + } + + // Enable horizontal and vertical flip + mt9v034_read_reg(devh, MT9V034_READ_MODE, &read_mode); + mt9v034_write_reg(devh, MT9V034_READ_MODE, + read_mode | MT9V034_READ_MODE_ROW_FLIP | MT9V034_READ_MODE_COLUMN_FLIP); + + return 0; +} + +int +mt9v034_set_sync(libusb_device_handle *devh, bool enabled) +{ + if (enabled) { + return mt9v034_enable_sync(devh); + } else { + return mt9v034_disable_sync(devh); + } +} diff --git a/src/xrt/drivers/rift_sensor/mt9v034.h b/src/xrt/drivers/rift_sensor/mt9v034.h new file mode 100644 index 000000000..88e2d8448 --- /dev/null +++ b/src/xrt/drivers/rift_sensor/mt9v034.h @@ -0,0 +1,41 @@ +// Copyright 2014-2020, Philipp Zabel +// Copyright 2017, TheOnlyJoey +// Copyright 2025, Jan Schmidt +// Copyright 2025-2026, Beyley Cardellio +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Implementation of mt9v034 sensor initialization + * @author Philipp Zabel + * @author TheOnlyJoey + * @author Jan Schmidt + * @author Beyley Cardellio + * @ingroup drv_rift_sensor + */ + +#pragma once + +#include "xrt/xrt_defines.h" + +#include + + +/*! + * Sets up the MT9V034 sensor for synchronized exposure, with minimal gain + * and raised black level calibration. + * + * @param[in] devh libusb device handle + * @return 0 on success, negative error code on failure + */ +int +mt9v034_setup(libusb_device_handle *devh); + +/*! + * Enables or disables the MT9V034's sync output + * + * @param[in] devh libusb device handle + * @param[in] enabled Whether to enable or disable the sync output + * @return 0 on success, negative error code on failure + */ +int +mt9v034_set_sync(libusb_device_handle *devh, bool enabled); -- 2.51.2