475 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * kinect sensor device camera, gspca driver4 *5 * Copyright (C) 2011 Antonio Ospite <ospite@studenti.unina.it>6 *7 * Based on the OpenKinect project and libfreenect8 * http://openkinect.org/wiki/Init_Analysis9 *10 * Special thanks to Steven Toth and kernellabs.com for sponsoring a Kinect11 * sensor device which I tested the driver on.12 */13 14#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt15 16#define MODULE_NAME "kinect"17 18#include "gspca.h"19 20#define CTRL_TIMEOUT 50021 22MODULE_AUTHOR("Antonio Ospite <ospite@studenti.unina.it>");23MODULE_DESCRIPTION("GSPCA/Kinect Sensor Device USB Camera Driver");24MODULE_LICENSE("GPL");25 26static bool depth_mode;27 28struct 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 39struct cam_hdr {40 uint8_t magic[2];41 __le16 len;42 __le16 cmd;43 __le16 tag;44};45 46/* specific webcam descriptor */47struct 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 0x000156#define MODE_640x488 0x000257#define MODE_1280x1024 0x000458 59#define FORMAT_BAYER 0x001060#define FORMAT_UYVY 0x002061#define FORMAT_Y10B 0x004062 63#define FPS_HIGH 0x010064 65static 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 73static 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 101static 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 111static 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 120static 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 198static 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 */220static 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 0238 /* Setting those values is not needed for video stream */239 cam->npkt = 15;240 gspca_dev->pkt_size = 960 * 2;241#endif242 243 return 0;244}245 246static 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 */267static 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 274static 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 else300 fmt_val = 0x00;301 302 if (mode & MODE_1280x1024)303 res_val = 0x02;304 else305 res_val = 0x01;306 307 if (mode & FPS_HIGH)308 fps_val = 0x1e;309 else310 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 start320 * and stop the depth stream before the camera will hand us 1280x1024321 * IR. This is a stupid workaround, but we've yet to find a better322 * 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 346static 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 367static void sd_stopN_video(struct gspca_dev *gspca_dev)368{369 /* reset video stream */370 write_register(gspca_dev, 0x05, 0x00);371}372 373static void sd_stopN_depth(struct gspca_dev *gspca_dev)374{375 /* reset depth stream */376 write_register(gspca_dev, 0x06, 0x00);377}378 379static 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 else410 pr_warn("Packet type not recognized...\n");411}412 413/* sub-driver description */414static 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};426static 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 -- */440static const struct usb_device_id device_table[] = {441 {USB_DEVICE(0x045e, 0x02ae)},442 {USB_DEVICE(0x045e, 0x02bf)},443 {}444};445 446MODULE_DEVICE_TABLE(usb, device_table);447 448/* -- device connect -- */449static 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 else455 return gspca_dev_probe(intf, id, &sd_desc_video,456 sizeof(struct sd), THIS_MODULE);457}458 459static 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_PM465 .suspend = gspca_suspend,466 .resume = gspca_resume,467 .reset_resume = gspca_resume,468#endif469};470 471module_usb_driver(sd_driver);472 473module_param(depth_mode, bool, 0644);474MODULE_PARM_DESC(depth_mode, "0=video 1=depth");475