src/cameras.c: add read_register() function

Signed-off-by: Antonio Ospite <[email protected]>

src/cameras.c

@@ -735,6 +735,25 @@ static int write_register(freenect_device *dev, uint16_t reg, uint16_t data)
 	return 0;
 }
 
+static uint16_t read_register(freenect_device *dev, uint16_t reg)
+{
+	freenect_context *ctx = dev->parent;
+	uint16_t reply[2];
+	uint16_t cmd;
+	int res;
+
+	cmd = fn_le16(reg);
+
+	FN_DEBUG("Read Reg 0x%04x =>\n", reg);
+	res = send_cmd(dev, 0x02, &cmd, 2, reply, 4);
+	if (res < 0)
+		FN_ERROR("read_register: send_cmd() failed: %d\n", res);
+	if (res != 4)
+		FN_WARNING("send_cmd returned %d [%04x %04x], 0000 expected\n", res, reply[0], reply[1]);
+
+	return reply[1];
+}
+
 int freenect_start_depth(freenect_device *dev)
 {
 	freenect_context *ctx = dev->parent;