?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
treewide: remove redundant IS_ERR() before error code check
[linux/fpc-iii.git] / drivers / media / usb / gspca / kinect.c
blob57ea6586359129777ad5eda8f789d7826e8254bd
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * kinect sensor device camera, gspca driver
4 *
5 * Copyright (C) 2011 Antonio Ospite <ospite@studenti.unina.it>
6 *
7 * Based on the OpenKinect project and libfreenect
8 * http://openkinect.org/wiki/Init_Analysis
9 *
10 * Special thanks to Steven Toth and kernellabs.com for sponsoring a Kinect
11 * sensor device which I tested the driver on.
12 */
13
14 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
15
16 #define MODULE_NAME "kinect"
17
18 #include "gspca.h"
19
20 #define CTRL_TIMEOUT 500
21
22 MODULE_AUTHOR("Antonio Ospite <ospite@studenti.unina.it>");
23 MODULE_DESCRIPTION("GSPCA/Kinect Sensor Device USB Camera Driver");
24 MODULE_LICENSE("GPL");
25
26 static bool depth_mode;
27
28 struct pkt_hdr {
29 uint8_t magic[2];
30 uint8_t pad;
31 uint8_t flag;
32 uint8_t unk1;
33 uint8_t seq;
34 uint8_t unk2;
35 uint8_t unk3;
36 uint32_t timestamp;
37 };
38
39 struct cam_hdr {
40 uint8_t magic[2];
41 __le16 len;
42 __le16 cmd;
43 __le16 tag;
44 };
45
46 /* specific webcam descriptor */
47 struct sd {
48 struct gspca_dev gspca_dev; /* !! must be the first item */
49 uint16_t cam_tag; /* a sequence number for packets */
50 uint8_t stream_flag; /* to identify different stream types */
51 uint8_t obuf[0x400]; /* output buffer for control commands */
52 uint8_t ibuf[0x200]; /* input buffer for control commands */
53 };
54
55 #define MODE_640x480 0x0001
56 #define MODE_640x488 0x0002
57 #define MODE_1280x1024 0x0004
58
59 #define FORMAT_BAYER 0x0010
60 #define FORMAT_UYVY 0x0020
61 #define FORMAT_Y10B 0x0040
62
63 #define FPS_HIGH 0x0100
64
65 static const struct v4l2_pix_format depth_camera_mode[] = {
66 {640, 480, V4L2_PIX_FMT_Y10BPACK, V4L2_FIELD_NONE,
67 .bytesperline = 640 * 10 / 8,
68 .sizeimage = 640 * 480 * 10 / 8,
69 .colorspace = V4L2_COLORSPACE_SRGB,
70 .priv = MODE_640x488 | FORMAT_Y10B},
71 };
72
73 static const struct v4l2_pix_format video_camera_mode[] = {
74 {640, 480, V4L2_PIX_FMT_SGRBG8, V4L2_FIELD_NONE,
75 .bytesperline = 640,
76 .sizeimage = 640 * 480,
77 .colorspace = V4L2_COLORSPACE_SRGB,
78 .priv = MODE_640x480 | FORMAT_BAYER | FPS_HIGH},
79 {640, 480, V4L2_PIX_FMT_UYVY, V4L2_FIELD_NONE,
80 .bytesperline = 640 * 2,
81 .sizeimage = 640 * 480 * 2,
82 .colorspace = V4L2_COLORSPACE_SRGB,
83 .priv = MODE_640x480 | FORMAT_UYVY},
84 {1280, 1024, V4L2_PIX_FMT_SGRBG8, V4L2_FIELD_NONE,
85 .bytesperline = 1280,
86 .sizeimage = 1280 * 1024,
87 .colorspace = V4L2_COLORSPACE_SRGB,
88 .priv = MODE_1280x1024 | FORMAT_BAYER},
89 {640, 488, V4L2_PIX_FMT_Y10BPACK, V4L2_FIELD_NONE,
90 .bytesperline = 640 * 10 / 8,
91 .sizeimage = 640 * 488 * 10 / 8,
92 .colorspace = V4L2_COLORSPACE_SRGB,
93 .priv = MODE_640x488 | FORMAT_Y10B | FPS_HIGH},
94 {1280, 1024, V4L2_PIX_FMT_Y10BPACK, V4L2_FIELD_NONE,
95 .bytesperline = 1280 * 10 / 8,
96 .sizeimage = 1280 * 1024 * 10 / 8,
97 .colorspace = V4L2_COLORSPACE_SRGB,
98 .priv = MODE_1280x1024 | FORMAT_Y10B},
99 };
100
101 static int kinect_write(struct usb_device *udev, uint8_t *data,
102 uint16_t wLength)
103 {
104 return usb_control_msg(udev,
105 usb_sndctrlpipe(udev, 0),
106 0x00,
107 USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
108 0, 0, data, wLength, CTRL_TIMEOUT);
109 }
110
111 static int kinect_read(struct usb_device *udev, uint8_t *data, uint16_t wLength)
112 {
113 return usb_control_msg(udev,
114 usb_rcvctrlpipe(udev, 0),
115 0x00,
116 USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
117 0, 0, data, wLength, CTRL_TIMEOUT);
118 }
119
120 static int send_cmd(struct gspca_dev *gspca_dev, uint16_t cmd, void *cmdbuf,
121 unsigned int cmd_len, void *replybuf, unsigned int reply_len)
122 {
123 struct sd *sd = (struct sd *) gspca_dev;
124 struct usb_device *udev = gspca_dev->dev;
125 int res, actual_len;
126 uint8_t *obuf = sd->obuf;
127 uint8_t *ibuf = sd->ibuf;
128 struct cam_hdr *chdr = (void *)obuf;
129 struct cam_hdr *rhdr = (void *)ibuf;
130
131 if (cmd_len & 1 || cmd_len > (0x400 - sizeof(*chdr))) {
132 pr_err("send_cmd: Invalid command length (0x%x)\n", cmd_len);
133 return -1;
134 }
135
136 chdr->magic[0] = 0x47;
137 chdr->magic[1] = 0x4d;
138 chdr->cmd = cpu_to_le16(cmd);
139 chdr->tag = cpu_to_le16(sd->cam_tag);
140 chdr->len = cpu_to_le16(cmd_len / 2);
141
142 memcpy(obuf+sizeof(*chdr), cmdbuf, cmd_len);
143
144 res = kinect_write(udev, obuf, cmd_len + sizeof(*chdr));
145 gspca_dbg(gspca_dev, D_USBO, "Control cmd=%04x tag=%04x len=%04x: %d\n",
146 cmd,
147 sd->cam_tag, cmd_len, res);
148 if (res < 0) {
149 pr_err("send_cmd: Output control transfer failed (%d)\n", res);
150 return res;
151 }
152
153 do {
154 actual_len = kinect_read(udev, ibuf, 0x200);
155 } while (actual_len == 0);
156 gspca_dbg(gspca_dev, D_USBO, "Control reply: %d\n", actual_len);
157 if (actual_len < (int)sizeof(*rhdr)) {
158 pr_err("send_cmd: Input control transfer failed (%d)\n",
159 actual_len);
160 return actual_len < 0 ? actual_len : -EREMOTEIO;
161 }
162 actual_len -= sizeof(*rhdr);
163
164 if (rhdr->magic[0] != 0x52 || rhdr->magic[1] != 0x42) {
165 pr_err("send_cmd: Bad magic %02x %02x\n",
166 rhdr->magic[0], rhdr->magic[1]);
167 return -1;
168 }
169 if (rhdr->cmd != chdr->cmd) {
170 pr_err("send_cmd: Bad cmd %02x != %02x\n",
171 rhdr->cmd, chdr->cmd);
172 return -1;
173 }
174 if (rhdr->tag != chdr->tag) {
175 pr_err("send_cmd: Bad tag %04x != %04x\n",
176 rhdr->tag, chdr->tag);
177 return -1;
178 }
179 if (le16_to_cpu(rhdr->len) != (actual_len/2)) {
180 pr_err("send_cmd: Bad len %04x != %04x\n",
181 le16_to_cpu(rhdr->len), (int)(actual_len/2));
182 return -1;
183 }
184
185 if (actual_len > reply_len) {
186 pr_warn("send_cmd: Data buffer is %d bytes long, but got %d bytes\n",
187 reply_len, actual_len);
188 memcpy(replybuf, ibuf+sizeof(*rhdr), reply_len);
189 } else {
190 memcpy(replybuf, ibuf+sizeof(*rhdr), actual_len);
191 }
192
193 sd->cam_tag++;
194
195 return actual_len;
196 }
197
198 static int write_register(struct gspca_dev *gspca_dev, uint16_t reg,
199 uint16_t data)
200 {
201 uint16_t reply[2];
202 __le16 cmd[2];
203 int res;
204
205 cmd[0] = cpu_to_le16(reg);
206 cmd[1] = cpu_to_le16(data);
207
208 gspca_dbg(gspca_dev, D_USBO, "Write Reg 0x%04x <= 0x%02x\n", reg, data);
209 res = send_cmd(gspca_dev, 0x03, cmd, 4, reply, 4);
210 if (res < 0)
211 return res;
212 if (res != 2) {
213 pr_warn("send_cmd returned %d [%04x %04x], 0000 expected\n",
214 res, reply[0], reply[1]);
215 }
216 return 0;
217 }
218
219 /* this function is called at probe time */
220 static int sd_config_video(struct gspca_dev *gspca_dev,
221 const struct usb_device_id *id)
222 {
223 struct sd *sd = (struct sd *) gspca_dev;
224 struct cam *cam;
225
226 sd->cam_tag = 0;
227
228 sd->stream_flag = 0x80;
229
230 cam = &gspca_dev->cam;
231
232 cam->cam_mode = video_camera_mode;
233 cam->nmodes = ARRAY_SIZE(video_camera_mode);
234
235 gspca_dev->xfer_ep = 0x81;
236
237 #if 0
238 /* Setting those values is not needed for video stream */
239 cam->npkt = 15;
240 gspca_dev->pkt_size = 960 * 2;
241 #endif
242
243 return 0;
244 }
245
246 static int sd_config_depth(struct gspca_dev *gspca_dev,
247 const struct usb_device_id *id)
248 {
249 struct sd *sd = (struct sd *) gspca_dev;
250 struct cam *cam;
251
252 sd->cam_tag = 0;
253
254 sd->stream_flag = 0x70;
255
256 cam = &gspca_dev->cam;
257
258 cam->cam_mode = depth_camera_mode;
259 cam->nmodes = ARRAY_SIZE(depth_camera_mode);
260
261 gspca_dev->xfer_ep = 0x82;
262
263 return 0;
264 }
265
266 /* this function is called at probe and resume time */
267 static int sd_init(struct gspca_dev *gspca_dev)
268 {
269 gspca_dbg(gspca_dev, D_PROBE, "Kinect Camera device.\n");
270
271 return 0;
272 }
273
274 static int sd_start_video(struct gspca_dev *gspca_dev)
275 {
276 int mode;
277 uint8_t fmt_reg, fmt_val;
278 uint8_t res_reg, res_val;
279 uint8_t fps_reg, fps_val;
280 uint8_t mode_val;
281
282 mode = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv;
283
284 if (mode & FORMAT_Y10B) {
285 fmt_reg = 0x19;
286 res_reg = 0x1a;
287 fps_reg = 0x1b;
288 mode_val = 0x03;
289 } else {
290 fmt_reg = 0x0c;
291 res_reg = 0x0d;
292 fps_reg = 0x0e;
293 mode_val = 0x01;
294 }
295
296 /* format */
297 if (mode & FORMAT_UYVY)
298 fmt_val = 0x05;
299 else
300 fmt_val = 0x00;
301
302 if (mode & MODE_1280x1024)
303 res_val = 0x02;
304 else
305 res_val = 0x01;
306
307 if (mode & FPS_HIGH)
308 fps_val = 0x1e;
309 else
310 fps_val = 0x0f;
311
312
313 /* turn off IR-reset function */
314 write_register(gspca_dev, 0x105, 0x00);
315
316 /* Reset video stream */
317 write_register(gspca_dev, 0x05, 0x00);
318
319 /* Due to some ridiculous condition in the firmware, we have to start
320 * and stop the depth stream before the camera will hand us 1280x1024
321 * IR. This is a stupid workaround, but we've yet to find a better
322 * solution.
323 *
324 * Thanks to Drew Fisher for figuring this out.
325 */
326 if (mode & (FORMAT_Y10B | MODE_1280x1024)) {
327 write_register(gspca_dev, 0x13, 0x01);
328 write_register(gspca_dev, 0x14, 0x1e);
329 write_register(gspca_dev, 0x06, 0x02);
330 write_register(gspca_dev, 0x06, 0x00);
331 }
332
333 write_register(gspca_dev, fmt_reg, fmt_val);
334 write_register(gspca_dev, res_reg, res_val);
335 write_register(gspca_dev, fps_reg, fps_val);
336
337 /* Start video stream */
338 write_register(gspca_dev, 0x05, mode_val);
339
340 /* disable Hflip */
341 write_register(gspca_dev, 0x47, 0x00);
342
343 return 0;
344 }
345
346 static int sd_start_depth(struct gspca_dev *gspca_dev)
347 {
348 /* turn off IR-reset function */
349 write_register(gspca_dev, 0x105, 0x00);
350
351 /* reset depth stream */
352 write_register(gspca_dev, 0x06, 0x00);
353 /* Depth Stream Format 0x03: 11 bit stream | 0x02: 10 bit */
354 write_register(gspca_dev, 0x12, 0x02);
355 /* Depth Stream Resolution 1: standard (640x480) */
356 write_register(gspca_dev, 0x13, 0x01);
357 /* Depth Framerate / 0x1e (30): 30 fps */
358 write_register(gspca_dev, 0x14, 0x1e);
359 /* Depth Stream Control / 2: Open Depth Stream */
360 write_register(gspca_dev, 0x06, 0x02);
361 /* disable depth hflip / LSB = 0: Smoothing Disabled */
362 write_register(gspca_dev, 0x17, 0x00);
363
364 return 0;
365 }
366
367 static void sd_stopN_video(struct gspca_dev *gspca_dev)
368 {
369 /* reset video stream */
370 write_register(gspca_dev, 0x05, 0x00);
371 }
372
373 static void sd_stopN_depth(struct gspca_dev *gspca_dev)
374 {
375 /* reset depth stream */
376 write_register(gspca_dev, 0x06, 0x00);
377 }
378
379 static void sd_pkt_scan(struct gspca_dev *gspca_dev, u8 *__data, int len)
380 {
381 struct sd *sd = (struct sd *) gspca_dev;
382
383 struct pkt_hdr *hdr = (void *)__data;
384 uint8_t *data = __data + sizeof(*hdr);
385 int datalen = len - sizeof(*hdr);
386
387 uint8_t sof = sd->stream_flag | 1;
388 uint8_t mof = sd->stream_flag | 2;
389 uint8_t eof = sd->stream_flag | 5;
390
391 if (len < 12)
392 return;
393
394 if (hdr->magic[0] != 'R' || hdr->magic[1] != 'B') {
395 pr_warn("[Stream %02x] Invalid magic %02x%02x\n",
396 sd->stream_flag, hdr->magic[0], hdr->magic[1]);
397 return;
398 }
399
400 if (hdr->flag == sof)
401 gspca_frame_add(gspca_dev, FIRST_PACKET, data, datalen);
402
403 else if (hdr->flag == mof)
404 gspca_frame_add(gspca_dev, INTER_PACKET, data, datalen);
405
406 else if (hdr->flag == eof)
407 gspca_frame_add(gspca_dev, LAST_PACKET, data, datalen);
408
409 else
410 pr_warn("Packet type not recognized...\n");
411 }
412
413 /* sub-driver description */
414 static const struct sd_desc sd_desc_video = {
415 .name = MODULE_NAME,
416 .config = sd_config_video,
417 .init = sd_init,
418 .start = sd_start_video,
419 .stopN = sd_stopN_video,
420 .pkt_scan = sd_pkt_scan,
421 /*
422 .get_streamparm = sd_get_streamparm,
423 .set_streamparm = sd_set_streamparm,
424 */
425 };
426 static const struct sd_desc sd_desc_depth = {
427 .name = MODULE_NAME,
428 .config = sd_config_depth,
429 .init = sd_init,
430 .start = sd_start_depth,
431 .stopN = sd_stopN_depth,
432 .pkt_scan = sd_pkt_scan,
433 /*
434 .get_streamparm = sd_get_streamparm,
435 .set_streamparm = sd_set_streamparm,
436 */
437 };
438
439 /* -- module initialisation -- */
440 static const struct usb_device_id device_table[] = {
441 {USB_DEVICE(0x045e, 0x02ae)},
442 {USB_DEVICE(0x045e, 0x02bf)},
443 {}
444 };
445
446 MODULE_DEVICE_TABLE(usb, device_table);
447
448 /* -- device connect -- */
449 static int sd_probe(struct usb_interface *intf, const struct usb_device_id *id)
450 {
451 if (depth_mode)
452 return gspca_dev_probe(intf, id, &sd_desc_depth,
453 sizeof(struct sd), THIS_MODULE);
454 else
455 return gspca_dev_probe(intf, id, &sd_desc_video,
456 sizeof(struct sd), THIS_MODULE);
457 }
458
459 static struct usb_driver sd_driver = {
460 .name = MODULE_NAME,
461 .id_table = device_table,
462 .probe = sd_probe,
463 .disconnect = gspca_disconnect,
464 #ifdef CONFIG_PM
465 .suspend = gspca_suspend,
466 .resume = gspca_resume,
467 .reset_resume = gspca_resume,
468 #endif
469 };
470
471 module_usb_driver(sd_driver);
472
473 module_param(depth_mode, bool, 0644);
474 MODULE_PARM_DESC(depth_mode, "0=video 1=depth");
Linux with added FPC-III support