usb/gspca/kinect.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * kinect sensor device camera, gspca driver
0004  *
0005  * Copyright (C) 2011  Antonio Ospite <ospite@studenti.unina.it>
0006  *
0007  * Based on the OpenKinect project and libfreenect
0008  * http://openkinect.org/wiki/Init_Analysis
0009  *
0010  * Special thanks to Steven Toth and kernellabs.com for sponsoring a Kinect
0011  * sensor device which I tested the driver on.
0012  */
0013
0014 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
0015
0016 #define MODULE_NAME "kinect"
0017
0018 #include "gspca.h"
0019
0020 #define CTRL_TIMEOUT 500
0021
0022 MODULE_AUTHOR("Antonio Ospite <ospite@studenti.unina.it>");
0023 MODULE_DESCRIPTION("GSPCA/Kinect Sensor Device USB Camera Driver");
0024 MODULE_LICENSE("GPL");
0025
0026 static bool depth_mode;
0027
0028 struct pkt_hdr {
0029     uint8_t magic[2];
0030     uint8_t pad;
0031     uint8_t flag;
0032     uint8_t unk1;
0033     uint8_t seq;
0034     uint8_t unk2;
0035     uint8_t unk3;
0036     uint32_t timestamp;
0037 };
0038
0039 struct cam_hdr {
0040     uint8_t magic[2];
0041     __le16 len;
0042     __le16 cmd;
0043     __le16 tag;
0044 };
0045
0046 /* specific webcam descriptor */
0047 struct sd {
0048     struct gspca_dev gspca_dev; /* !! must be the first item */
0049     uint16_t cam_tag;           /* a sequence number for packets */
0050     uint8_t stream_flag;        /* to identify different stream types */
0051     uint8_t obuf[0x400];        /* output buffer for control commands */
0052     uint8_t ibuf[0x200];        /* input buffer for control commands */
0053 };
0054
0055 #define MODE_640x480   0x0001
0056 #define MODE_640x488   0x0002
0057 #define MODE_1280x1024 0x0004
0058
0059 #define FORMAT_BAYER   0x0010
0060 #define FORMAT_UYVY    0x0020
0061 #define FORMAT_Y10B    0x0040
0062
0063 #define FPS_HIGH       0x0100
0064
0065 static const struct v4l2_pix_format depth_camera_mode[] = {
0066     {640, 480, V4L2_PIX_FMT_Y10BPACK, V4L2_FIELD_NONE,
0067      .bytesperline = 640 * 10 / 8,
0068      .sizeimage =  640 * 480 * 10 / 8,
0069      .colorspace = V4L2_COLORSPACE_SRGB,
0070      .priv = MODE_640x488 | FORMAT_Y10B},
0071 };
0072
0073 static const struct v4l2_pix_format video_camera_mode[] = {
0074     {640, 480, V4L2_PIX_FMT_SGRBG8, V4L2_FIELD_NONE,
0075      .bytesperline = 640,
0076      .sizeimage = 640 * 480,
0077      .colorspace = V4L2_COLORSPACE_SRGB,
0078      .priv = MODE_640x480 | FORMAT_BAYER | FPS_HIGH},
0079     {640, 480, V4L2_PIX_FMT_UYVY, V4L2_FIELD_NONE,
0080      .bytesperline = 640 * 2,
0081      .sizeimage = 640 * 480 * 2,
0082      .colorspace = V4L2_COLORSPACE_SRGB,
0083      .priv = MODE_640x480 | FORMAT_UYVY},
0084     {1280, 1024, V4L2_PIX_FMT_SGRBG8, V4L2_FIELD_NONE,
0085      .bytesperline = 1280,
0086      .sizeimage = 1280 * 1024,
0087      .colorspace = V4L2_COLORSPACE_SRGB,
0088      .priv = MODE_1280x1024 | FORMAT_BAYER},
0089     {640, 488, V4L2_PIX_FMT_Y10BPACK, V4L2_FIELD_NONE,
0090      .bytesperline = 640 * 10 / 8,
0091      .sizeimage =  640 * 488 * 10 / 8,
0092      .colorspace = V4L2_COLORSPACE_SRGB,
0093      .priv = MODE_640x488 | FORMAT_Y10B | FPS_HIGH},
0094     {1280, 1024, V4L2_PIX_FMT_Y10BPACK, V4L2_FIELD_NONE,
0095      .bytesperline = 1280 * 10 / 8,
0096      .sizeimage =  1280 * 1024 * 10 / 8,
0097      .colorspace = V4L2_COLORSPACE_SRGB,
0098      .priv = MODE_1280x1024 | FORMAT_Y10B},
0099 };
0100
0101 static int kinect_write(struct usb_device *udev, uint8_t *data,
0102             uint16_t wLength)
0103 {
0104     return usb_control_msg(udev,
0105                   usb_sndctrlpipe(udev, 0),
0106                   0x00,
0107                   USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
0108                   0, 0, data, wLength, CTRL_TIMEOUT);
0109 }
0110
0111 static int kinect_read(struct usb_device *udev, uint8_t *data, uint16_t wLength)
0112 {
0113     return usb_control_msg(udev,
0114                   usb_rcvctrlpipe(udev, 0),
0115                   0x00,
0116                   USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
0117                   0, 0, data, wLength, CTRL_TIMEOUT);
0118 }
0119
0120 static int send_cmd(struct gspca_dev *gspca_dev, uint16_t cmd, void *cmdbuf,
0121         unsigned int cmd_len, void *replybuf, unsigned int reply_len)
0122 {
0123     struct sd *sd = (struct sd *) gspca_dev;
0124     struct usb_device *udev = gspca_dev->dev;
0125     int res, actual_len;
0126     uint8_t *obuf = sd->obuf;
0127     uint8_t *ibuf = sd->ibuf;
0128     struct cam_hdr *chdr = (void *)obuf;
0129     struct cam_hdr *rhdr = (void *)ibuf;
0130
0131     if (cmd_len & 1 || cmd_len > (0x400 - sizeof(*chdr))) {
0132         pr_err("send_cmd: Invalid command length (0x%x)\n", cmd_len);
0133         return -1;
0134     }
0135
0136     chdr->magic[0] = 0x47;
0137     chdr->magic[1] = 0x4d;
0138     chdr->cmd = cpu_to_le16(cmd);
0139     chdr->tag = cpu_to_le16(sd->cam_tag);
0140     chdr->len = cpu_to_le16(cmd_len / 2);
0141
0142     memcpy(obuf+sizeof(*chdr), cmdbuf, cmd_len);
0143
0144     res = kinect_write(udev, obuf, cmd_len + sizeof(*chdr));
0145     gspca_dbg(gspca_dev, D_USBO, "Control cmd=%04x tag=%04x len=%04x: %d\n",
0146           cmd,
0147           sd->cam_tag, cmd_len, res);
0148     if (res < 0) {
0149         pr_err("send_cmd: Output control transfer failed (%d)\n", res);
0150         return res;
0151     }
0152
0153     do {
0154         actual_len = kinect_read(udev, ibuf, 0x200);
0155     } while (actual_len == 0);
0156     gspca_dbg(gspca_dev, D_USBO, "Control reply: %d\n", actual_len);
0157     if (actual_len < (int)sizeof(*rhdr)) {
0158         pr_err("send_cmd: Input control transfer failed (%d)\n",
0159                actual_len);
0160         return actual_len < 0 ? actual_len : -EREMOTEIO;
0161     }
0162     actual_len -= sizeof(*rhdr);
0163
0164     if (rhdr->magic[0] != 0x52 || rhdr->magic[1] != 0x42) {
0165         pr_err("send_cmd: Bad magic %02x %02x\n",
0166                rhdr->magic[0], rhdr->magic[1]);
0167         return -1;
0168     }
0169     if (rhdr->cmd != chdr->cmd) {
0170         pr_err("send_cmd: Bad cmd %02x != %02x\n",
0171                rhdr->cmd, chdr->cmd);
0172         return -1;
0173     }
0174     if (rhdr->tag != chdr->tag) {
0175         pr_err("send_cmd: Bad tag %04x != %04x\n",
0176                rhdr->tag, chdr->tag);
0177         return -1;
0178     }
0179     if (le16_to_cpu(rhdr->len) != (actual_len/2)) {
0180         pr_err("send_cmd: Bad len %04x != %04x\n",
0181                le16_to_cpu(rhdr->len), (int)(actual_len/2));
0182         return -1;
0183     }
0184
0185     if (actual_len > reply_len) {
0186         pr_warn("send_cmd: Data buffer is %d bytes long, but got %d bytes\n",
0187             reply_len, actual_len);
0188         memcpy(replybuf, ibuf+sizeof(*rhdr), reply_len);
0189     } else {
0190         memcpy(replybuf, ibuf+sizeof(*rhdr), actual_len);
0191     }
0192
0193     sd->cam_tag++;
0194
0195     return actual_len;
0196 }
0197
0198 static int write_register(struct gspca_dev *gspca_dev, uint16_t reg,
0199             uint16_t data)
0200 {
0201     uint16_t reply[2];
0202     __le16 cmd[2];
0203     int res;
0204
0205     cmd[0] = cpu_to_le16(reg);
0206     cmd[1] = cpu_to_le16(data);
0207
0208     gspca_dbg(gspca_dev, D_USBO, "Write Reg 0x%04x <= 0x%02x\n", reg, data);
0209     res = send_cmd(gspca_dev, 0x03, cmd, 4, reply, 4);
0210     if (res < 0)
0211         return res;
0212     if (res != 2) {
0213         pr_warn("send_cmd returned %d [%04x %04x], 0000 expected\n",
0214             res, reply[0], reply[1]);
0215     }
0216     return 0;
0217 }
0218
0219 /* this function is called at probe time */
0220 static int sd_config_video(struct gspca_dev *gspca_dev,
0221              const struct usb_device_id *id)
0222 {
0223     struct sd *sd = (struct sd *) gspca_dev;
0224     struct cam *cam;
0225
0226     sd->cam_tag = 0;
0227
0228     sd->stream_flag = 0x80;
0229
0230     cam = &gspca_dev->cam;
0231
0232     cam->cam_mode = video_camera_mode;
0233     cam->nmodes = ARRAY_SIZE(video_camera_mode);
0234
0235     gspca_dev->xfer_ep = 0x81;
0236
0237 #if 0
0238     /* Setting those values is not needed for video stream */
0239     cam->npkt = 15;
0240     gspca_dev->pkt_size = 960 * 2;
0241 #endif
0242
0243     return 0;
0244 }
0245
0246 static int sd_config_depth(struct gspca_dev *gspca_dev,
0247              const struct usb_device_id *id)
0248 {
0249     struct sd *sd = (struct sd *) gspca_dev;
0250     struct cam *cam;
0251
0252     sd->cam_tag = 0;
0253
0254     sd->stream_flag = 0x70;
0255
0256     cam = &gspca_dev->cam;
0257
0258     cam->cam_mode = depth_camera_mode;
0259     cam->nmodes = ARRAY_SIZE(depth_camera_mode);
0260
0261     gspca_dev->xfer_ep = 0x82;
0262
0263     return 0;
0264 }
0265
0266 /* this function is called at probe and resume time */
0267 static int sd_init(struct gspca_dev *gspca_dev)
0268 {
0269     gspca_dbg(gspca_dev, D_PROBE, "Kinect Camera device.\n");
0270
0271     return 0;
0272 }
0273
0274 static int sd_start_video(struct gspca_dev *gspca_dev)
0275 {
0276     int mode;
0277     uint8_t fmt_reg, fmt_val;
0278     uint8_t res_reg, res_val;
0279     uint8_t fps_reg, fps_val;
0280     uint8_t mode_val;
0281
0282     mode = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv;
0283
0284     if (mode & FORMAT_Y10B) {
0285         fmt_reg = 0x19;
0286         res_reg = 0x1a;
0287         fps_reg = 0x1b;
0288         mode_val = 0x03;
0289     } else {
0290         fmt_reg = 0x0c;
0291         res_reg = 0x0d;
0292         fps_reg = 0x0e;
0293         mode_val = 0x01;
0294     }
0295
0296     /* format */
0297     if (mode & FORMAT_UYVY)
0298         fmt_val = 0x05;
0299     else
0300         fmt_val = 0x00;
0301
0302     if (mode & MODE_1280x1024)
0303         res_val = 0x02;
0304     else
0305         res_val = 0x01;
0306
0307     if (mode & FPS_HIGH)
0308         fps_val = 0x1e;
0309     else
0310         fps_val = 0x0f;
0311
0312
0313     /* turn off IR-reset function */
0314     write_register(gspca_dev, 0x105, 0x00);
0315
0316     /* Reset video stream */
0317     write_register(gspca_dev, 0x05, 0x00);
0318
0319     /* Due to some ridiculous condition in the firmware, we have to start
0320      * and stop the depth stream before the camera will hand us 1280x1024
0321      * IR.  This is a stupid workaround, but we've yet to find a better
0322      * solution.
0323      *
0324      * Thanks to Drew Fisher for figuring this out.
0325      */
0326     if (mode & (FORMAT_Y10B | MODE_1280x1024)) {
0327         write_register(gspca_dev, 0x13, 0x01);
0328         write_register(gspca_dev, 0x14, 0x1e);
0329         write_register(gspca_dev, 0x06, 0x02);
0330         write_register(gspca_dev, 0x06, 0x00);
0331     }
0332
0333     write_register(gspca_dev, fmt_reg, fmt_val);
0334     write_register(gspca_dev, res_reg, res_val);
0335     write_register(gspca_dev, fps_reg, fps_val);
0336
0337     /* Start video stream */
0338     write_register(gspca_dev, 0x05, mode_val);
0339
0340     /* disable Hflip */
0341     write_register(gspca_dev, 0x47, 0x00);
0342
0343     return 0;
0344 }
0345
0346 static int sd_start_depth(struct gspca_dev *gspca_dev)
0347 {
0348     /* turn off IR-reset function */
0349     write_register(gspca_dev, 0x105, 0x00);
0350
0351     /* reset depth stream */
0352     write_register(gspca_dev, 0x06, 0x00);
0353     /* Depth Stream Format 0x03: 11 bit stream | 0x02: 10 bit */
0354     write_register(gspca_dev, 0x12, 0x02);
0355     /* Depth Stream Resolution 1: standard (640x480) */
0356     write_register(gspca_dev, 0x13, 0x01);
0357     /* Depth Framerate / 0x1e (30): 30 fps */
0358     write_register(gspca_dev, 0x14, 0x1e);
0359     /* Depth Stream Control  / 2: Open Depth Stream */
0360     write_register(gspca_dev, 0x06, 0x02);
0361     /* disable depth hflip / LSB = 0: Smoothing Disabled */
0362     write_register(gspca_dev, 0x17, 0x00);
0363
0364     return 0;
0365 }
0366
0367 static void sd_stopN_video(struct gspca_dev *gspca_dev)
0368 {
0369     /* reset video stream */
0370     write_register(gspca_dev, 0x05, 0x00);
0371 }
0372
0373 static void sd_stopN_depth(struct gspca_dev *gspca_dev)
0374 {
0375     /* reset depth stream */
0376     write_register(gspca_dev, 0x06, 0x00);
0377 }
0378
0379 static void sd_pkt_scan(struct gspca_dev *gspca_dev, u8 *__data, int len)
0380 {
0381     struct sd *sd = (struct sd *) gspca_dev;
0382
0383     struct pkt_hdr *hdr = (void *)__data;
0384     uint8_t *data = __data + sizeof(*hdr);
0385     int datalen = len - sizeof(*hdr);
0386
0387     uint8_t sof = sd->stream_flag | 1;
0388     uint8_t mof = sd->stream_flag | 2;
0389     uint8_t eof = sd->stream_flag | 5;
0390
0391     if (len < 12)
0392         return;
0393
0394     if (hdr->magic[0] != 'R' || hdr->magic[1] != 'B') {
0395         pr_warn("[Stream %02x] Invalid magic %02x%02x\n",
0396             sd->stream_flag, hdr->magic[0], hdr->magic[1]);
0397         return;
0398     }
0399
0400     if (hdr->flag == sof)
0401         gspca_frame_add(gspca_dev, FIRST_PACKET, data, datalen);
0402
0403     else if (hdr->flag == mof)
0404         gspca_frame_add(gspca_dev, INTER_PACKET, data, datalen);
0405
0406     else if (hdr->flag == eof)
0407         gspca_frame_add(gspca_dev, LAST_PACKET, data, datalen);
0408
0409     else
0410         pr_warn("Packet type not recognized...\n");
0411 }
0412
0413 /* sub-driver description */
0414 static const struct sd_desc sd_desc_video = {
0415     .name      = MODULE_NAME,
0416     .config    = sd_config_video,
0417     .init      = sd_init,
0418     .start     = sd_start_video,
0419     .stopN     = sd_stopN_video,
0420     .pkt_scan  = sd_pkt_scan,
0421     /*
0422     .get_streamparm = sd_get_streamparm,
0423     .set_streamparm = sd_set_streamparm,
0424     */
0425 };
0426 static const struct sd_desc sd_desc_depth = {
0427     .name      = MODULE_NAME,
0428     .config    = sd_config_depth,
0429     .init      = sd_init,
0430     .start     = sd_start_depth,
0431     .stopN     = sd_stopN_depth,
0432     .pkt_scan  = sd_pkt_scan,
0433     /*
0434     .get_streamparm = sd_get_streamparm,
0435     .set_streamparm = sd_set_streamparm,
0436     */
0437 };
0438
0439 /* -- module initialisation -- */
0440 static const struct usb_device_id device_table[] = {
0441     {USB_DEVICE(0x045e, 0x02ae)},
0442     {USB_DEVICE(0x045e, 0x02bf)},
0443     {}
0444 };
0445
0446 MODULE_DEVICE_TABLE(usb, device_table);
0447
0448 /* -- device connect -- */
0449 static int sd_probe(struct usb_interface *intf, const struct usb_device_id *id)
0450 {
0451     if (depth_mode)
0452         return gspca_dev_probe(intf, id, &sd_desc_depth,
0453                        sizeof(struct sd), THIS_MODULE);
0454     else
0455         return gspca_dev_probe(intf, id, &sd_desc_video,
0456                        sizeof(struct sd), THIS_MODULE);
0457 }
0458
0459 static struct usb_driver sd_driver = {
0460     .name       = MODULE_NAME,
0461     .id_table   = device_table,
0462     .probe      = sd_probe,
0463     .disconnect = gspca_disconnect,
0464 #ifdef CONFIG_PM
0465     .suspend    = gspca_suspend,
0466     .resume     = gspca_resume,
0467     .reset_resume = gspca_resume,
0468 #endif
0469 };
0470
0471 module_usb_driver(sd_driver);
0472
0473 module_param(depth_mode, bool, 0644);
0474 MODULE_PARM_DESC(depth_mode, "0=video 1=depth");