HMD.swift (11159B)
1 // Protocol adaptation from Monado wmr_hmd.c (BSL-1.0). 2 // Copyright 2018, Philipp Zabel. 3 // Copyright 2020-2021, N Madsen. 4 // Copyright 2020-2023, Collabora, Ltd. 5 // SPDX-License-Identifier: BSL-1.0 6 // HP Reverb G1 over plain IOKit HID: wakes the display and streams the IMU. 7 // Protocol ported from Monado (src/xrt/drivers/wmr/wmr_hmd.c, wmr_protocol.c). 8 // Requires boot-arg hdcp-fuse-mask=0, otherwise macOS starts HDCP and the headset blanks. 9 import Foundation 10 import IOKit.hid 11 import simd 12 13 struct IMUSample { 14 var time: Double // seconds, device clock 15 var gyro: SIMD3<Float> // rad/s, raw device axes 16 var accel: SIMD3<Float> // m/s^2, raw device axes 17 } 18 19 final class HMD { 20 enum Mode { case observe, activate } 21 var onIMU: ((IMUSample) -> Void)? 22 var onStatus: ((String) -> Void)? 23 /// Display-ready flag inferred from 0x05. Physical panel illumination needs user confirmation. 24 var onPanel: ((Bool) -> Void)? 25 var onDisconnected: (() -> Void)? 26 private(set) var imuSamples = 0 27 private(set) var statusReports = 0 28 private(set) var lastIMUAt: Date? 29 30 private var managers: [IOHIDManager] = [] // must outlive the devices, or they get closed 31 private let ioLock = NSRecursiveLock() 32 private var control: IOHIDDevice? 33 private var sensors: IOHIDDevice? 34 var isConnected: Bool { 35 ioLock.lock(); defer { ioLock.unlock() } 36 return control != nil && sensors != nil 37 } 38 private var generation = 0 39 private var configReply: [UInt8]? 40 private let controlBuf = UnsafeMutablePointer<UInt8>.allocate(capacity: 64) 41 private let sensorsBuf = UnsafeMutablePointer<UInt8>.allocate(capacity: 512) 42 43 struct NotFound: Error, CustomStringConvertible { 44 let name: String 45 var description: String { "\(name) not found (plugged in, and accessory allowed?)" } 46 } 47 48 /// Observe registers callbacks only. Activate additionally wakes the panel and starts the IMU. 49 /// Callbacks fire on the main run loop. 50 func start(mode: Mode) throws { 51 ioLock.lock(); defer { ioLock.unlock() } 52 stop() 53 do { 54 control = try open(0x03f0, 0x0c6a, "QHMD control device") 55 sensors = try open(0x045e, 0x0659, "HoloLens Sensors") 56 let me = Unmanaged.passUnretained(self).toOpaque() 57 IOHIDDeviceRegisterInputReportCallback(control!, controlBuf, 64, { ctx, _, _, _, _, r, n in 58 Unmanaged<HMD>.fromOpaque(ctx!).takeUnretainedValue().handleControl(r, n) 59 }, me) 60 IOHIDDeviceRegisterInputReportCallback(sensors!, sensorsBuf, 512, { ctx, _, _, _, _, r, n in 61 Unmanaged<HMD>.fromOpaque(ctx!).takeUnretainedValue().handleSensors(r, n) 62 }, me) 63 for d in [control!, sensors!] { 64 IOHIDDeviceRegisterRemovalCallback(d, { ctx, _, _ in 65 let hmd = Unmanaged<HMD>.fromOpaque(ctx!).takeUnretainedValue() 66 // Leave the IOKit callback before closing its manager. 67 let generation = hmd.generation 68 onMainRunLoop { 69 if hmd.generation == generation { hmd.onDisconnected?() } 70 } 71 }, me) 72 IOHIDDeviceScheduleWithRunLoop(d, CFRunLoopGetMain(), CFRunLoopMode.defaultMode.rawValue) 73 } 74 75 guard mode == .activate else { return } 76 usleep(300_000) 77 for _ in 0..<4 { 78 var c = [UInt8](repeating: 0, count: 64); c[0] = 0x50; c[1] = 0x01 79 try setFeature(c); _ = getFeature(0x50); usleep(10_000) 80 } 81 _ = getFeature(0x09) // Preserve wake sequence; do not log device serials. 82 _ = getFeature(0x08); _ = getFeature(0x06) 83 try screenOn() 84 var imuOn = [UInt8](repeating: 0, count: 64); imuOn[0] = 0x02; imuOn[1] = 0x07 85 let result = IOHIDDeviceSetReport(sensors!, kIOHIDReportTypeOutput, 0x02, imuOn, imuOn.count) 86 guard result == kIOReturnSuccess else { throw hidError("IMU start", result) } 87 } catch { 88 stop() 89 throw error 90 } 91 } 92 93 func screenOn() throws { 94 ioLock.lock(); defer { ioLock.unlock() } 95 guard control != nil else { throw NotFound(name: "QHMD control device") } 96 try setFeature([0x04, 0x01]) 97 } 98 99 /// Read the factory store using Monado's WMR config protocol. This changes only 100 /// the sensor device's read cursor; it does not write calibration or toggle the panel. 101 /// Run on main after start(.observe), before a rendering session owns this device. 102 func readFactoryConfiguration() throws -> Data { 103 precondition(Thread.isMainThread) 104 let deadline = Date().addingTimeInterval(15) 105 func failure(_ message: String) -> NSError { 106 NSError(domain: "HMD calibration", code: 1, userInfo: [NSLocalizedDescriptionKey: message]) 107 } 108 func command(_ type: UInt8) throws -> [UInt8] { 109 guard Date() < deadline else { throw failure("factory read exceeded 15 seconds") } 110 configReply = nil 111 let request = WMRConfigProtocol.request(type) 112 ioLock.lock() 113 guard let sensors else { ioLock.unlock(); throw NotFound(name: "HoloLens Sensors") } 114 let result = IOHIDDeviceSetReport(sensors, kIOHIDReportTypeOutput, 0x02, request, request.count) 115 ioLock.unlock() 116 guard result == kIOReturnSuccess else { throw hidError("factory read command", result) } 117 let replyDeadline = min(deadline, Date().addingTimeInterval(0.5)) 118 while configReply == nil && Date() < replyDeadline { 119 RunLoop.main.run(until: min(replyDeadline, Date().addingTimeInterval(0.002))) 120 } 121 guard let reply = configReply, WMRConfigProtocol.isAck(reply) else { 122 throw failure(String(format: "no valid reply to config command 0x%02x", type)) 123 } 124 return reply 125 } 126 func part(_ type: UInt8, limit: Int) throws -> [UInt8] { 127 _ = try command(WMRConfigProtocol.selectCommand) 128 _ = try command(type) 129 var bytes: [UInt8] = [] 130 for _ in 0..<4096 { 131 switch WMRConfigProtocol.parseChunk(try command(WMRConfigProtocol.readChunkCommand)) { 132 case .end: return bytes 133 case .data(let payload): 134 guard bytes.count + payload.count <= limit else { throw failure("oversized factory config") } 135 bytes.append(contentsOf: payload) 136 case .invalid: throw failure("malformed factory config chunk") 137 } 138 } 139 throw failure("too many factory config chunks") 140 } 141 let metadata = try part(WMRConfigProtocol.metadataPart, limit: 84) 142 guard let length = WMRConfigProtocol.dataSize(fromMetadata: metadata) else { throw failure("short factory metadata") } 143 guard length > 0 else { throw failure("empty factory configuration") } 144 let bytes = try part(WMRConfigProtocol.dataPart, limit: length) 145 guard bytes.count == length else { throw failure("incomplete factory configuration") } 146 log("read factory configuration: \(length) bytes") 147 return Data(bytes) 148 } 149 150 /// Close host handles without sending panel-off, which can strand the DP link. 151 func stop() { 152 ioLock.lock(); defer { ioLock.unlock() } 153 generation += 1 154 for d in [control, sensors].compactMap({ $0 }) { 155 IOHIDDeviceRegisterRemovalCallback(d, nil, nil) 156 IOHIDDeviceRegisterInputReportCallback(d, d == control ? controlBuf : sensorsBuf, d == control ? 64 : 512, nil, nil) 157 IOHIDDeviceUnscheduleFromRunLoop(d, CFRunLoopGetMain(), CFRunLoopMode.defaultMode.rawValue) 158 } 159 for m in managers { IOHIDManagerClose(m, IOOptionBits(kIOHIDOptionsTypeNone)) } 160 control = nil; sensors = nil; managers.removeAll() 161 lastIMUAt = nil 162 } 163 164 deinit { 165 stop() 166 controlBuf.deallocate(); sensorsBuf.deallocate() 167 } 168 169 private func open(_ vid: Int, _ pid: Int, _ name: String) throws -> IOHIDDevice { 170 let m = IOHIDManagerCreate(kCFAllocatorDefault, IOOptionBits(kIOHIDOptionsTypeNone)) 171 managers.append(m) 172 IOHIDManagerSetDeviceMatching(m, [kIOHIDVendorIDKey: vid, kIOHIDProductIDKey: pid] as CFDictionary) 173 guard IOHIDManagerOpen(m, IOOptionBits(kIOHIDOptionsTypeNone)) == kIOReturnSuccess, 174 let d = (IOHIDManagerCopyDevices(m) as? Set<IOHIDDevice>)?.first else { throw NotFound(name: name) } 175 return d 176 } 177 178 private func hidError(_ action: String, _ result: IOReturn) -> NSError { 179 NSError(domain: "HMD", code: Int(result), userInfo: [NSLocalizedDescriptionKey: String(format: "%@ failed: 0x%08x", action, result)]) 180 } 181 182 private func setFeature(_ b: [UInt8]) throws { 183 let r = IOHIDDeviceSetReport(control!, kIOHIDReportTypeFeature, CFIndex(b[0]), b, b.count) 184 if r != kIOReturnSuccess { throw hidError(String(format: "HID set 0x%02x", b[0]), r) } 185 } 186 187 private func getFeature(_ id: UInt8) -> [UInt8]? { 188 var b = [UInt8](repeating: 0, count: 64); b[0] = id; var n = CFIndex(64) 189 return IOHIDDeviceGetReport(control!, kIOHIDReportTypeFeature, CFIndex(id), &b, &n) == kIOReturnSuccess ? Array(b[0..<n]) : nil 190 } 191 192 private func handleControl(_ p: UnsafeMutablePointer<UInt8>, _ n: Int) { 193 guard n >= 6 && p[0] == 0x05 else { return } 194 statusReports += 1 195 let raw = (0..<n).map { String(format: "%02x", p[$0]) }.joined(separator: " ") 196 onStatus?("display ready=\(p[1] == 1); raw \(raw)") 197 onPanel?(p[1] == 1) 198 } 199 200 private func handleSensors(_ p: UnsafeMutablePointer<UInt8>, _ n: Int) { 201 if n == 33, p[0] == 0x02 { 202 configReply = Array(UnsafeBufferPointer(start: p, count: n)) 203 return 204 } 205 guard (n == 381 || n == 497), p[0] == 0x01 else { return } 206 lastIMUAt = Date() 207 func i16(_ o: Int) -> Int32 { Int32(Int16(bitPattern: UInt16(p[o]) | UInt16(p[o + 1]) << 8)) } 208 func i32(_ o: Int) -> Int32 { Int32(bitPattern: UInt32(p[o]) | UInt32(p[o + 1]) << 8 | UInt32(p[o + 2]) << 16 | UInt32(p[o + 3]) << 24) } 209 func u64(_ o: Int) -> UInt64 { (0..<8).reduce(0) { $0 | UInt64(p[o + $1]) << (8 * UInt64($1)) } } 210 // Layout: id(1) temp[4](8) gyro_ts[4](32) gyro[3][32](192) accel_ts[4](32) accel[3][4](48) video_ts[4](32) 211 let gyroTS = 9, gyro = 41, accel = 41 + 192 + 32 212 for i in 0..<4 { 213 var g = SIMD3<Float>() 214 var acc = SIMD3<Float>() 215 for a in 0..<3 { 216 var sum: Int32 = 0 217 for k in 0..<8 { sum += i16(gyro + (a * 32 + 8 * i + k) * 2) } // 8 sub-samples per sample 218 g[a] = Float(sum) * 0.001 / 8 219 acc[a] = Float(i32(accel + (a * 4 + i) * 4)) * 0.001 220 } 221 imuSamples += 1 222 onIMU?(IMUSample(time: Double(u64(gyroTS + i * 8)) * 100e-9, gyro: g, accel: acc)) 223 } 224 } 225 }