1 /*
2 * Oculus Rift CV1 Radio
3 * Copyright 2016 Philipp Zabel
4 * Copyright 2019 Jan Schmidt
5 * SPDX-License-Identifier: BSL-1.0
6 */
7 #include <stdint.h>
8 #include <string.h>
9
10 #include "rift-hmd-radio.h"
11 #include "../ext_deps/nxjson.h"
12
get_feature_report(hid_device * handle,rift_sensor_feature_cmd cmd,unsigned char * buf)13 static int get_feature_report(hid_device *handle, rift_sensor_feature_cmd cmd, unsigned char* buf)
14 {
15 memset(buf, 0, FEATURE_BUFFER_SIZE);
16 buf[0] = (unsigned char)cmd;
17 return hid_get_feature_report(handle, buf, FEATURE_BUFFER_SIZE);
18 }
19
send_feature_report(hid_device * handle,unsigned char * buf,int length)20 static int send_feature_report(hid_device *handle, unsigned char* buf, int length)
21 {
22 return hid_send_feature_report(handle, buf, length);
23 }
24
rift_hmd_radio_send_cmd(hid_device * handle,uint8_t a,uint8_t b,uint8_t c)25 static bool rift_hmd_radio_send_cmd(hid_device *handle, uint8_t a, uint8_t b, uint8_t c)
26 {
27 unsigned char buffer[FEATURE_BUFFER_SIZE];
28 int cmd_size = encode_radio_control_cmd(buffer, a, b, c);
29 int ret_size;
30
31 if (send_feature_report(handle, buffer, cmd_size) < 0)
32 return false;
33
34 do {
35 ret_size = get_feature_report(handle, RIFT_CMD_RADIO_CONTROL, buffer);
36 if (ret_size < 1) {
37 LOGE("HMD radio command 0x%02x/%02x/%02x failed - response too small", a, b, c);
38 return false;
39 }
40 } while (buffer[3] & 0x80);
41
42 /* 0x08 means the device isn't responding */
43 if (buffer[3] & 0x08)
44 return false;
45
46 return true;
47 }
48
rift_radio_read_flash(hid_device * handle,uint8_t device_type,uint16_t offset,uint16_t length,uint8_t * flash_data)49 static int rift_radio_read_flash(hid_device *handle, uint8_t device_type,
50 uint16_t offset, uint16_t length, uint8_t *flash_data)
51 {
52 int ret;
53 unsigned char buffer[FEATURE_BUFFER_SIZE];
54 int cmd_size = encode_radio_data_read_cmd(buffer, offset, length);
55
56 ret = send_feature_report(handle, buffer, cmd_size);
57 if (ret < 0)
58 goto done;
59
60 if (!rift_hmd_radio_send_cmd (handle, 0x03, RIFT_HMD_RADIO_READ_FLASH_CONTROL,
61 device_type)) {
62 ret = -1;
63 goto done;
64 }
65
66 ret = get_feature_report(handle, RIFT_CMD_RADIO_READ_DATA, buffer);
67 if (ret < 0)
68 goto done;
69
70 memcpy (flash_data, buffer+7, length);
71
72 done:
73 return ret;
74 }
75
rift_radio_read_calibration_hash(hid_device * handle,uint8_t device_type,uint8_t hash[16])76 static int rift_radio_read_calibration_hash(hid_device *handle, uint8_t device_type,
77 uint8_t hash[16])
78 {
79 return rift_radio_read_flash(handle, device_type, 0x1bf0, 16, hash);
80 }
81
rift_radio_read_calibration(hid_device * handle,uint8_t device_type,char ** json_out,uint16_t * length)82 static int rift_radio_read_calibration(hid_device *handle, uint8_t device_type,
83 char **json_out, uint16_t *length)
84 {
85 char *json;
86 int ret;
87 uint8_t flash_data[20];
88 uint16_t json_length;
89 uint16_t offset;
90
91 ret = rift_radio_read_flash(handle, device_type, 0, 20, flash_data);
92 if (ret < 0)
93 return ret;
94
95 if (flash_data[0] != 1 || flash_data[1] != 0)
96 return -1; /* Invalid data */
97 json_length = (flash_data[3] << 8) | flash_data[2];
98
99 json = calloc(1, json_length + 1);
100 memcpy(json, flash_data + 4, 16);
101
102 for (offset = 20; offset < json_length + 4; offset += 20) {
103 uint16_t json_offset = offset - 4;
104
105 ret = rift_radio_read_flash(handle, device_type, offset, 20, flash_data);
106 if (ret < 0) {
107 free(json);
108 return ret;
109 }
110
111 memcpy(json + json_offset, flash_data, OHMD_MIN (20, json_length - json_offset));
112 }
113
114 *json_out = json;
115 *length = json_length;
116
117 return 0;
118 }
119
json_read_vec3(const nx_json * nxj,const char * key,vec3f * out)120 static bool json_read_vec3(const nx_json *nxj, const char *key, vec3f *out)
121 {
122 const nx_json *member = nx_json_get (nxj, key);
123
124 if (member->type != NX_JSON_ARRAY)
125 return false;
126
127 out->x = nx_json_item (member, 0)->dbl_value;
128 out->y = nx_json_item (member, 1)->dbl_value;
129 out->z = nx_json_item (member, 2)->dbl_value;
130
131 return true;
132 }
133
rift_touch_parse_calibration(char * json,rift_touch_calibration * c)134 static int rift_touch_parse_calibration(char *json,
135 rift_touch_calibration *c)
136 {
137 const nx_json* nxj, *obj, *version, *array;
138 int version_number = -1;
139 unsigned int i;
140
141 nxj = nx_json_parse (json, 0);
142 if (nxj == NULL)
143 return -1;
144
145 obj = nx_json_get(nxj, "TrackedObject");
146 if (obj->type == NX_JSON_NULL)
147 goto fail;
148
149 version = nx_json_get (obj, "JsonVersion");
150 if (version->type != NX_JSON_INTEGER || version->int_value != 2) {
151 version_number = version->int_value;
152 goto fail;
153 }
154 version_number = version->int_value;
155
156 if (!json_read_vec3 (obj, "ImuPosition", &c->imu_position))
157 goto fail;
158
159 c->joy_x_range_min = nx_json_get (obj, "JoyXRangeMin")->int_value;
160 c->joy_x_range_max = nx_json_get (obj, "JoyXRangeMax")->int_value;
161 c->joy_x_dead_min = nx_json_get (obj, "JoyXDeadMin")->int_value;
162 c->joy_x_dead_max = nx_json_get (obj, "JoyXDeadMax")->int_value;
163 c->joy_y_range_min = nx_json_get (obj, "JoyYRangeMin")->int_value;
164 c->joy_y_range_max = nx_json_get (obj, "JoyYRangeMax")->int_value;
165 c->joy_y_dead_min = nx_json_get (obj, "JoyYDeadMin")->int_value;
166 c->joy_y_dead_max = nx_json_get (obj, "JoyYDeadMax")->int_value;
167
168 c->trigger_min_range = nx_json_get (obj, "TriggerMinRange")->int_value;
169 c->trigger_mid_range = nx_json_get (obj, "TriggerMidRange")->int_value;
170 c->trigger_max_range = nx_json_get (obj, "TriggerMaxRange")->int_value;
171
172 array = nx_json_get (obj, "GyroCalibration");
173 for (i = 0; i < 12; i++)
174 c->gyro_calibration[i] = nx_json_item (array, i)->dbl_value;
175
176 c->middle_min_range = nx_json_get (obj, "MiddleMinRange")->int_value;
177 c->middle_mid_range = nx_json_get (obj, "MiddleMidRange")->int_value;
178 c->middle_max_range = nx_json_get (obj, "MiddleMaxRange")->int_value;
179
180 c->middle_flipped = nx_json_get (obj, "MiddleFlipped")->int_value;
181
182 array = nx_json_get (obj, "AccCalibration");
183 for (i = 0; i < 12; i++)
184 c->acc_calibration[i] = nx_json_item (array, i)->dbl_value;
185
186 array = nx_json_get (obj, "CapSenseMin");
187 for (i = 0; i < 8; i++)
188 c->cap_sense_min[i] = nx_json_item (array, i)->int_value;
189
190 array = nx_json_get (obj, "CapSenseTouch");
191 for (i = 0; i < 8; i++)
192 c->cap_sense_touch[i] = nx_json_item (array, i)->int_value;
193
194 nx_json_free (nxj);
195 return 0;
196 fail:
197 LOGW ("Unrecognised Touch Controller JSON data version %d\n%s\n", version_number, json);
198 nx_json_free (nxj);
199 return -1;
200 }
201
rift_touch_get_calibration(hid_device * handle,int device_id,rift_touch_calibration * calibration)202 int rift_touch_get_calibration(hid_device *handle, int device_id,
203 rift_touch_calibration *calibration)
204 {
205 uint8_t hash[16];
206 uint16_t length;
207 char *json = NULL;
208 int ret = -1;
209
210 /* If the controller isn't on yet, we might fail to read the calibration data */
211 ret = rift_radio_read_calibration_hash(handle, device_id, hash);
212 if (ret < 0) {
213 LOGV ("Failed to read calibration hash from device %d", device_id);
214 return ret;
215 }
216
217 /* TODO: We need a persistent store for the calibration data to
218 * save time - we only need to re-read the calibration data from the
219 * device if the hash changes. */
220
221 ret = rift_radio_read_calibration(handle, device_id, &json, &length);
222 if (ret < 0)
223 return ret;
224
225 rift_touch_parse_calibration(json, calibration);
226
227 free(json);
228 return 0;
229 }
230
rift_hmd_radio_get_address(hid_device * handle,uint8_t radio_address[5])231 bool rift_hmd_radio_get_address(hid_device *handle, uint8_t radio_address[5])
232 {
233 unsigned char buf[FEATURE_BUFFER_SIZE];
234 int ret_size;
235
236 if (!rift_hmd_radio_send_cmd (handle, 0x05, 0x03, 0x05))
237 return false;
238
239 ret_size = get_feature_report(handle, RIFT_CMD_RADIO_READ_DATA, buf);
240 if (ret_size < 0)
241 return false;
242
243 if (!decode_radio_address (radio_address, buf, ret_size)) {
244 LOGE("Failed to decode received radio address");
245 return false;
246 }
247
248 return true;
249 }
250
251