diff --git a/src/Baballonia.VFTCapture/VFTCapture.cs b/src/Baballonia.VFTCapture/VFTCapture.cs index 257144b3..98dd3bf9 100644 --- a/src/Baballonia.VFTCapture/VFTCapture.cs +++ b/src/Baballonia.VFTCapture/VFTCapture.cs @@ -33,6 +33,7 @@ public override async Task StartCapture() { try { + Logger.LogDebug("Trying to enable device."); // Open the VFT device and initialize it. SetTrackerState(setActive: true); @@ -97,25 +98,18 @@ private Task VideoCapture_UpdateLoop() private void SetTrackerState(bool setActive) { - // Prev: var fd = ViveFacialTracker.open(Url, ViveFacialTracker.FileOpenFlags.O_RDWR); - var vftFileStream = File.Open(Source, FileMode.Open, FileAccess.ReadWrite); - var fd = vftFileStream.SafeFileHandle.DangerousGetHandle(); - if (fd != IntPtr.Zero) + try { - try - { - // Activate the tracker and give it some time to warm up/cool down - if (setActive) - ViveFacialTracker.activate_tracker((int)fd); - else - ViveFacialTracker.deactivate_tracker((int)fd); - // await Task.Delay(1000); - } - finally - { - // Prev: ViveFacialTracker.close((int)fd); - vftFileStream.Close(); - } + // Leverage IDisposable for GC-less release of handle. + using var device = new ViveFacialTracker(Logger, Source); + if (!device.IsValid) + throw new NullReferenceException(); + + device.SetState(setActive); + } + catch(Exception e) + { + Logger.LogError(e.Message); } } diff --git a/src/Baballonia.VFTCapture/ViveFacialTracker.cs b/src/Baballonia.VFTCapture/ViveFacialTracker.cs index 32573df8..0e6ec2b9 100644 --- a/src/Baballonia.VFTCapture/ViveFacialTracker.cs +++ b/src/Baballonia.VFTCapture/ViveFacialTracker.cs @@ -1,9 +1,24 @@ using System.Runtime.InteropServices; +using Microsoft.Extensions.Logging; namespace Baballonia.VFTCapture; -public partial class ViveFacialTracker +public partial class ViveFacialTracker : IDisposable { + /// + /// Native interop functions for Linux. + /// + private sealed partial class LinuxNative + { + [LibraryImport("libc", EntryPoint = "close", SetLastError = true)] + public static partial int Close(IntPtr handle); + [LibraryImport("libc", EntryPoint = "ioctl", SetLastError = true)] + public static partial int Ioctl(IntPtr handle, int request, ref UvcXuControlQuery capability); + + [LibraryImport("libc", EntryPoint = "open", SetLastError = true)] + public static partial IntPtr Open([MarshalAs(UnmanagedType.LPStr)] string path, FileOpenFlags flags); + } + public enum FileOpenFlags { O_RDONLY = 0x00, @@ -13,58 +28,43 @@ public enum FileOpenFlags } [StructLayout(LayoutKind.Sequential)] - public struct uvc_xu_control_query + private struct UvcXuControlQuery + { + public byte unit; + public byte selector; + public UvcQuery query; + public ushort size; + public IntPtr data; + } + + public enum XuTask : byte { - public byte unit { get; set; } - public byte selector { get; set; } - public byte query { get; set; } - public ushort size { get; set; } - public IntPtr data { get; set; } + SET = 0x50, + GET = 0x51, } - const int _XU_TASK_SET = 0x50; - const int _XU_TASK_GET = 0x51; - const int _XU_REG_SENSOR = 0xab; - - const int _UVC_SET_CUR = 0x01; - const int _UVC_GET_CUR = 0x81; - const int _UVC_GET_MIN = 0x82; - const int _UVC_GET_MAX = 0x83; - const int _UVC_GET_RES = 0x84; - const int _UVC_GET_LEN = 0x85; - const int _UVC_GET_INFO = 0x86; - const int _UVC_GET_DEF = 0x87; - public static int _UVCIOC_CTRL_QUERY = _IOWR('u', 0x21); - - private const string LibcLibrary = "libc"; - - [LibraryImport(LibcLibrary, SetLastError = true)] - public static partial int read(int fd, IntPtr buf, int count); - [LibraryImport(LibcLibrary, SetLastError = true)] - public static partial int open([MarshalAs(UnmanagedType.LPStr)] string pathname, FileOpenFlags flags); - [LibraryImport(LibcLibrary)] - internal static partial int close(int fd); - [LibraryImport(LibcLibrary, SetLastError = true)] - public static partial int ioctl(int fd, uint request, IntPtr argp); - - [LibraryImport(LibcLibrary, SetLastError = true)] - public static partial int ioctl(int fd, int request, IntPtr argp); - - [LibraryImport(LibcLibrary, SetLastError = true)] - internal static partial int ioctl(int fd, uint request, ulong argp); - [LibraryImport(LibcLibrary, SetLastError = true)] - public static partial int write(int fd, IntPtr buf, int count); + public enum XuReg : byte + { + SENSOR = 0xab, + } + + public enum UvcQuery : byte + { + SET_CUR = 0x01, + GET_CUR = 0x81, + GET_MIN = 0x82, + GET_MAX = 0x83, + GET_RES = 0x84, + GET_LEN = 0x85, + GET_INFO = 0x86, + GET_DEF = 0x87, + } const int _IOC_NRBITS = 8; const int _IOC_TYPEBITS = 8; const int _IOC_SIZEBITS = 14; const int _IOC_DIRBITS = 2; - const int _IOC_NRMASK = (1 << _IOC_NRBITS) - 1; - const int _IOC_TYPEMASK = (1 << _IOC_TYPEBITS) - 1; - const int _IOC_SIZEMASK = (1 << _IOC_SIZEBITS) - 1; - const int _IOC_DIRMASK = (1 << _IOC_DIRBITS) - 1; - const int _IOC_NRSHIFT = 0; const int _IOC_TYPESHIFT = _IOC_NRSHIFT + _IOC_NRBITS; const int _IOC_SIZESHIFT = _IOC_TYPESHIFT + _IOC_TYPEBITS; @@ -74,237 +74,255 @@ public struct uvc_xu_control_query const int _IOC_WRITE = 1; const int _IOC_READ = 2; - private static int _IOC(int dir, int type, int nr, int size) - => ((dir) << _IOC_DIRSHIFT) | ((type) << _IOC_TYPESHIFT) | ((nr) << _IOC_NRSHIFT) | ((size) << _IOC_SIZESHIFT); + private static int Ioc(int dir, int type, int nr, int size) + => (dir << _IOC_DIRSHIFT) | (type << _IOC_TYPESHIFT) | (nr << _IOC_NRSHIFT) | (size << _IOC_SIZESHIFT); + private static int IocTypeCheck() where T : unmanaged + => Marshal.SizeOf(); + private static readonly int _UVC_IOC_CTRL_QUERY = Ioc(_IOC_READ | _IOC_WRITE, 'u', 0x21, IocTypeCheck()); + + /// + /// The file handle. Should not really be used outside of this clase. + /// + internal IntPtr Handle { get; private set; } = IntPtr.Zero; + + /// + /// Device control buffer size. For the Vive Face Tracker this + /// is either 384 or 64. + /// + public ushort BufferSize { get; private set; } = 0; + + /// + /// Check if the held handle is still valid. + /// + public bool IsValid { get => Handle != IntPtr.Zero; } + + private readonly ILogger log; + + /// + /// Opens a file and wraps the handle. It must point to a Vive Face Tracker device. + /// + /// + public ViveFacialTracker(ILogger logger, string path, FileOpenFlags flags = FileOpenFlags.O_RDWR) + { + log = logger; - internal static int _IO(int type, int nr) => _IOC(_IOC_NONE, type, nr, 0); - internal static int _IOR(int type, int nr) => _IOC(_IOC_READ, type, nr, _IOC_TYPECHECK()); - internal static int _IOW(int type, int nr) => _IOC(_IOC_WRITE, type, nr, _IOC_TYPECHECK()); - private static int _IOWR(int type, int nr) => _IOC(_IOC_READ | _IOC_WRITE, type, nr, _IOC_TYPECHECK()); - private static int _IOC_TYPECHECK() => Marshal.SizeOf(); + log.LogDebug($"VFT: opening '{path}' as '{flags}'"); + Handle = LinuxNative.Open(path, flags); + var err = Marshal.GetLastPInvokeError(); + if (err != 0) + throw new Exception($"Error while opening native file:\n{Marshal.GetLastPInvokeErrorMessage()}"); - private static void xu_set_cur(int fd, byte selector, byte[] data) + log.LogDebug("VFT: validating buffer size"); + var deviceBufferSz = GetLen(); + + log.LogDebug($"VFT: get buffer size: {deviceBufferSz}"); + if (deviceBufferSz != 384 && deviceBufferSz != 64) + throw new Exception($"Got unexpected device buffer size: {deviceBufferSz}"); + + BufferSize = deviceBufferSz; + } + + /// + /// Explicit closing of the handle without waiting for GC. + /// + public void Dispose() { - unsafe + // We must ensure we actually hold a valid pointer. + if (IsValid) + LinuxNative.Close(Handle); + // After closing we must ensure the handle cannot be used anymore! + Handle = IntPtr.Zero; + } + + ~ViveFacialTracker() + => Dispose(); + + private void XuQueryCur(byte unit, byte selector, UvcQuery query, byte[] data) + { + var dataHandle = GCHandle.Alloc(data, GCHandleType.Pinned); + try { - fixed (byte* dataptr = &data[0]) + UvcXuControlQuery c = new() + { + unit = unit, + selector = selector, + query = query, + size = (ushort)data.Length, + data = dataHandle.AddrOfPinnedObject() + }; + var res = LinuxNative.Ioctl(Handle, _UVC_IOC_CTRL_QUERY, ref c); + if (res != 0) { - var c = new uvc_xu_control_query() - { - unit = 4, - selector = selector, - query = _UVC_SET_CUR, - size = (ushort)data.Length, - data = (IntPtr)dataptr - }; - ioctl(fd, (uint)_UVCIOC_CTRL_QUERY, (IntPtr)(&c)); + int err = Marshal.GetLastPInvokeError(); + string msg = Marshal.GetLastPInvokeErrorMessage(); + throw new Exception( + $"Error in ioctl uvc_xu_control_query request:\n" + + $" {{q:{query},sz:{data.Length}}}\n" + + $" {{r:{res},e:{err}}}: {msg}"); } } + finally + { + dataHandle.Free(); + } } - private static byte[] xu_get_cur(int fd, byte selector, int len) + private void XuSetCur(byte selector, byte[] data) + => XuQueryCur(4, selector, UvcQuery.SET_CUR, data); + + private byte[] XuGetCur(byte selector, int len) { byte[] data = new byte[len]; - unsafe - { - fixed (byte* dataptr = &data[0]) - { - uvc_xu_control_query c = new uvc_xu_control_query() - { - unit = 4, - selector = selector, - query = _UVC_GET_CUR, - size = (ushort)data.Length, - data = (IntPtr)dataptr - }; - if (ioctl(fd, (uint)_UVCIOC_CTRL_QUERY, (IntPtr)(&c)) != 0) - return data; //TODO: maybe throw here instead? - } - } + XuQueryCur(4, selector, UvcQuery.GET_CUR, data); return data; } - public static void set_cur_no_resp(int fd, byte[] data) + private ushort GetLen(byte selector = 2) { - xu_set_cur(fd, 2, data); + byte[] data = new byte[2]; + XuQueryCur(4, selector, UvcQuery.GET_LEN, data); + return (ushort)(data[1] << 8 | data[0]); } - private static bool set_cur(int fd, byte[] data, int timeout = 1000) + private byte SetCur(byte[] data, int timeout = 1000) { - xu_set_cur(fd, 2, data); - CancellationTokenSource cts = new CancellationTokenSource(timeout); + if (data.Length != BufferSize) + throw new Exception($"Got incorrect buffer size: {data.Length} expected: {BufferSize}"); + + XuSetCur(2, data); + + if (timeout <= 0) + return 0; + + CancellationTokenSource cts = new(timeout); while (!cts.Token.IsCancellationRequested) { - byte[] rcvdata = xu_get_cur(fd, 2, 384); - switch (rcvdata[0]) + byte[] result = XuGetCur(2, BufferSize); + switch (result[0]) { - case 0x55: + case 0x55: // Not ready. + // Can do a lil' eep. + Thread.Sleep(1); break; case 0x56: - return Enumerable.SequenceEqual(data[0..16], rcvdata[1..17]); + if (Enumerable.SequenceEqual(data[0..16], result[1..17])) + { + return result[17]; + } + throw new Exception($"Invalid response sequence: {result[1..17]} expected: {data[0..16]}"); default: - return false; + throw new Exception($"Unexpected response from XU command: {result[0]}"); } } - return false; - } - - private static void set_register(int fd, int reg, int addr, int value) - { - byte[] data = new byte[384]; - - data[0] = _XU_TASK_SET; - data[1] = (byte)reg; - data[2] = 0x60; - data[3] = 1; // address len - data[4] = 1; // value len - // address - data[5] = (byte)((addr >> 24) & 0xFF); - data[6] = (byte)((addr >> 16) & 0xFF); - data[7] = (byte)((addr >> 8) & 0xFF); - data[8] = (byte)(addr & 0xFF); - // page address - data[9] = 0x90; - data[10] = 0x01; - data[11] = 0x00; - data[12] = 0x01; - // value - data[13] = (byte)((value >> 24) & 0xFF); - data[14] = (byte)((value >> 16) & 0xFF); - data[15] = (byte)((value >> 8) & 0xFF); - data[16] = (byte)(value & 0xFF); - - set_cur(fd, data); + throw new TimeoutException("Got no rersponse from device."); } - private static int get_register(int fd, int reg, int addr) + /// + /// Applies a task to a register. + /// + private byte RegisterTask(XuTask task, XuReg reg, byte addr, byte value = 0x00) { - byte[] data = new byte[384]; - - data[0] = _XU_TASK_GET; - data[1] = (byte)reg; - data[2] = 0x60; - data[3] = 1; // address len - data[4] = 1; // value len - // address - data[5] = (byte)((addr >> 24) & 0xFF); - data[6] = (byte)((addr >> 16) & 0xFF); - data[7] = (byte)((addr >> 8) & 0xFF); - data[8] = (byte)(addr & 0xFF); - // page address - data[9] = 0x90; + byte[] data = new byte[BufferSize]; + + // Because we only need to set bytes, we can hardcode + // most of the values here. + data[00] = (byte)task; + data[01] = (byte)reg; + data[02] = 0x60; + data[03] = 0x01; // 1 byte sized address. + data[04] = 0x01; // 1 byte sized value. + // Address. + data[05] = 0x00; + data[06] = 0x00; + data[07] = 0x00; + data[08] = addr; + // Page address. + data[09] = 0x90; data[10] = 0x01; data[11] = 0x00; data[12] = 0x01; - // value + // Value. data[13] = 0x00; data[14] = 0x00; data[15] = 0x00; - data[16] = 0x00; - data[254] = 0x53; - data[255] = 0x54; + data[16] = value; - set_cur(fd, data); - return 0; + return SetCur(data); } - private static void set_register_sensor(int fd, int addr, int value) - { - set_register(fd, _XU_REG_SENSOR, addr, value); - } + private void SetRegister(XuReg reg, byte addr, byte value) + => RegisterTask(XuTask.SET, reg, addr, value); - public static void get_register_sensor(int fd, int addr) - { - get_register(fd, _XU_REG_SENSOR, addr); - } + private byte GetRegister(XuReg reg, byte addr) + => RegisterTask(XuTask.GET, reg, addr); + + private void SetRegisterSensor(byte addr, byte value) + => SetRegister(XuReg.SENSOR, addr, value); + + private void GetRegisterSensor(byte addr) + => GetRegister(XuReg.SENSOR, addr); - private static void set_enable_stream(int fd, bool enable) + private void SetEnableStream(bool enable) { - byte[] data = new byte[384]; - data[0] = _XU_TASK_SET; - data[1] = 0x14; - data[2] = 0x00; + log.LogDebug($"VFT: set stream: {(enable ? "on" : "off")}"); + byte[] data = new byte[BufferSize]; + data[0] = (byte)XuTask.SET; + data[1] = 0x14; // Magic numbers, this does not have + data[2] = 0x00; // the same pattern as RegisterTask! data[3] = (byte)(enable ? 0x01 : 0x00); - data[254] = 0x53; - data[255] = 0x54; - set_cur(fd, data); + SetCur(data); } - public static uint get_len(int fd) + private void SendMagicPacket() { - uint length = 0; - unsafe + // I have no clue why we need to send this magic packet. + log.LogDebug("VFT: sending magic packet"); + byte[] data = new byte[BufferSize]; + data[0] = 0x51; // Magic numbers, this does not have + data[1] = 0x52; // the same pattern as RegisterTask! + if (BufferSize >= 256) { - uvc_xu_control_query c = new uvc_xu_control_query() - { - unit = 4, - selector = 2, - query = _UVC_GET_LEN, - size = 2, - data = (IntPtr)(&length) - }; - ioctl(fd, _UVCIOC_CTRL_QUERY, (IntPtr)(&c)); + // Need to set magic numbers on large buffers. + data[254] = 0x53; + data[255] = 0x54; } - return length; + SetCur(data); } - public static bool activate_tracker(int fd) + /// + /// Toggles the state of the camera. + /// + public void SetState(bool enabled) { - //uint l = get_len(fd); - byte[] data = new byte[384]; - data[0] = 0x51; - data[1] = 0x52; - data[254] = 0x53; - data[255] = 0x54; - - set_cur(fd, data); - set_enable_stream(fd, false); - set_cur(fd, data); - - // 0x02, 0x03 and 0x04 all control IR intensity. - set_register_sensor(fd, 0x00, 0x40); - set_register_sensor(fd, 0x08, 0x01); - set_register_sensor(fd, 0x70, 0x00); - set_register_sensor(fd, 0x02, 0xFF); //IR ON - set_register_sensor(fd, 0x03, 0xFF); // IR ON - set_register_sensor(fd, 0x04, 0xFF); // IR ON - set_register_sensor(fd, 0x0e, 0x00); - set_register_sensor(fd, 0x05, 0xb2); - set_register_sensor(fd, 0x06, 0xb2); - set_register_sensor(fd, 0x07, 0xb2); - set_register_sensor(fd, 0x0f, 0x03); - set_cur(fd, data); - set_enable_stream(fd, true); - return true; - } + SendMagicPacket(); + SetEnableStream(false); + + Thread.Sleep(100); + + // Set infra-red LED state. + byte irLedState = (byte)(enabled ? 0xff : 0x00); + log.LogDebug($"VFT: set camera: {(enabled ? "on" : "off")}"); + SendMagicPacket(); + SetRegisterSensor(0x00, 0x40); + SetRegisterSensor(0x08, 0x01); + SetRegisterSensor(0x70, 0x00); + SetRegisterSensor(0x02, irLedState); + SetRegisterSensor(0x03, irLedState); + SetRegisterSensor(0x04, irLedState); + SetRegisterSensor(0x0e, 0x00); + SetRegisterSensor(0x05, 0xb2); + SetRegisterSensor(0x06, 0xb2); + SetRegisterSensor(0x07, 0xb2); + SetRegisterSensor(0x0f, 0x03); + + // On enable, restore the stream. + // On disable, skip this. + if (enabled) + { + Thread.Sleep(100); - public static bool deactivate_tracker(int fd) - { - //uint l = get_len(fd); - byte[] data = new byte[384]; - data[0] = 0x51; - data[1] = 0x52; - data[254] = 0x53; - data[255] = 0x54; - - set_cur(fd, data); - set_enable_stream(fd, false); - set_cur(fd, data); - - // 0x02, 0x03 and 0x04 all control IR intensity. - set_register_sensor(fd, 0x00, 0x40); - set_register_sensor(fd, 0x08, 0x01); - set_register_sensor(fd, 0x70, 0x00); - set_register_sensor(fd, 0x02, 0x00); //IR Off - set_register_sensor(fd, 0x03, 0x00); // IR Off - set_register_sensor(fd, 0x04, 0x00); // IR Off - set_register_sensor(fd, 0x0e, 0x00); - set_register_sensor(fd, 0x05, 0xb2); - set_register_sensor(fd, 0x06, 0xb2); - set_register_sensor(fd, 0x07, 0xb2); - set_register_sensor(fd, 0x0f, 0x03); - set_cur(fd, data); - set_enable_stream(fd, true); - return true; + SendMagicPacket(); + SetEnableStream(true); + } } }