From 2ab1e6a46241cd76b78cf59001836d77d23601ae Mon Sep 17 00:00:00 2001 From: Antonio Ospite Date: Sun, 5 Dec 2010 17:08:09 +0100 Subject: [PATCH] Initial import Signed-off-by: Antonio Ospite --- BUGS.txt | 9 ++ Makefile | 20 ++++ kinect.c | 323 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 352 insertions(+) create mode 100644 BUGS.txt create mode 100644 Makefile create mode 100644 kinect.c diff --git a/BUGS.txt b/BUGS.txt new file mode 100644 index 0000000..267c9a4 --- /dev/null +++ b/BUGS.txt @@ -0,0 +1,9 @@ +When gspca_resume is called with a wrong intf value it may pass a useless +ep... + +BUG: unable to handle kernel NULL pointer dereference at 0000000000000b0e +[ 2431.484253] IP: [] gspca_resume+0xd/0x3f [gspca_main] +[ 2431.484270] PGD 3cb60067 PUD 376b7067 PMD 0 +[ 2431.484278] Oops: 0002 [#1] SMP +[ 2431.484284] last sysfs file: /sys/devices/pci0000:00/0000:00:09.0/host2/target2:0:0/2:0:0:0/block/sda/sda6/stat + diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..fc47380 --- /dev/null +++ b/Makefile @@ -0,0 +1,20 @@ +obj-m := kinect.o + +KDIR := /lib/modules/$(shell uname -r)/build/ + +EXTRA_CFLAGS := -I$(KDIR)/drivers/media/video/gspca/ + +all: + $(MAKE) -C $(KDIR) M=$(PWD) modules + +clean: + $(MAKE) -C $(KDIR) M=$(PWD) clean + +unload: + -sudo rmmod kinect + -sudo rmmod gspca_main +load: + sudo modprobe gspca_main + sudo insmod ./kinect.ko + +reload: unload load diff --git a/kinect.c b/kinect.c new file mode 100644 index 0000000..c646490 --- /dev/null +++ b/kinect.c @@ -0,0 +1,323 @@ +/* + * kinect sensor device camera, gspca driver + * + * Copyright (C) 2010 Antonio Ospite + * + * Based on the OpenKinect project and libfreenect by Ector Martin + * http://openkinect.org/wiki/Init_Analysis + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#define MODULE_NAME "kinect" + +#include "gspca.h" + +#define CTRL_TIMEOUT 500 + +MODULE_AUTHOR("Antonio Ospite "); +MODULE_DESCRIPTION("GSPCA/Kinect Sensor Device USB Camera Driver"); +MODULE_LICENSE("GPL"); + +struct pkt_hdr { + uint8_t magic[2]; + uint8_t pad; + uint8_t flag; + uint8_t unk1; + uint8_t seq; + uint8_t unk2; + uint8_t unk3; + uint32_t timestamp; +}; + +typedef struct { + uint8_t magic[2]; + uint16_t len; + uint16_t cmd; + uint16_t tag; +} cam_hdr; + +int gspca_debug = D_ERR | D_PROBE | D_CONF | D_STREAM | D_FRAM | D_PACK | + D_USBI | D_USBO | D_V4L2; + +/* specific webcam descriptor */ +struct sd { + struct gspca_dev gspca_dev; /* !! must be the first item */ + uint16_t cam_tag; +}; + +/* V4L2 controls supported by the driver */ +/* controls prototypes here */ + +static const struct ctrl sd_ctrls[] = { +}; + +static const struct v4l2_pix_format color_camera_mode[] = { + {640, 480, V4L2_PIX_FMT_SGRBG8, V4L2_FIELD_NONE, + .bytesperline = 640, + .sizeimage = 640 * 480, + .colorspace = V4L2_COLORSPACE_SRGB, + .priv = 0}, +}; + +static int kinect_write(struct usb_device *udev, uint8_t *data, uint16_t wLength) +{ + return usb_control_msg(udev, + usb_sndctrlpipe(udev, 0), + 0x00, + USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE, + 0, 0, data, wLength, CTRL_TIMEOUT); +} + +static int kinect_read(struct usb_device *udev, uint8_t *data, uint16_t wLength) +{ + return usb_control_msg(udev, + usb_rcvctrlpipe(udev, 0), + 0x00, + USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE, + 0, 0, data, wLength, CTRL_TIMEOUT); +} + +static int send_cmd(struct gspca_dev *gspca_dev, uint16_t cmd, void *cmdbuf, + unsigned int cmd_len, void *replybuf, unsigned int reply_len) +{ + struct sd *sd = (struct sd *) gspca_dev; + struct usb_device *udev = gspca_dev->dev; + int res, actual_len; + uint8_t obuf[0x400]; + uint8_t ibuf[0x200]; + cam_hdr *chdr = (void*)obuf; + cam_hdr *rhdr = (void*)ibuf; + + if (cmd_len & 1 || cmd_len > (0x400 - sizeof(*chdr))) { + err("send_cmd: Invalid command length (0x%x)", cmd_len); + return -1; + } + + chdr->magic[0] = 0x47; + chdr->magic[1] = 0x4d; + chdr->cmd = cpu_to_le16(cmd); + chdr->tag = cpu_to_le16(sd->cam_tag); + chdr->len = cpu_to_le16(cmd_len / 2); + + memcpy(obuf+sizeof(*chdr), cmdbuf, cmd_len); + + res = kinect_write(udev, obuf, cmd_len + sizeof(*chdr)); + info("Control cmd=%04x tag=%04x len=%04x: %d", cmd, sd->cam_tag, cmd_len, res); + if (res < 0) { + err("send_cmd: Output control transfer failed (%d)", res); + return res; + } + + do { + actual_len = kinect_read(udev, ibuf, 0x200); + } while (actual_len == 0); + info("Control reply: %d", res); + if (actual_len < sizeof(*rhdr)) { + err("send_cmd: Input control transfer failed (%d)", res); + return res; + } + actual_len -= sizeof(*rhdr); + + if (rhdr->magic[0] != 0x52 || rhdr->magic[1] != 0x42) { + err("send_cmd: Bad magic %02x %02x", rhdr->magic[0], rhdr->magic[1]); + return -1; + } + if (rhdr->cmd != chdr->cmd) { + err("send_cmd: Bad cmd %02x != %02x", rhdr->cmd, chdr->cmd); + return -1; + } + if (rhdr->tag != chdr->tag) { + err("send_cmd: Bad tag %04x != %04x", rhdr->tag, chdr->tag); + return -1; + } + if (cpu_to_le16(rhdr->len) != (actual_len/2)) { + err("send_cmd: Bad len %04x != %04x", + cpu_to_le16(rhdr->len), (int)(actual_len/2)); + return -1; + } + + if (actual_len > reply_len) { + warn("send_cmd: Data buffer is %d bytes long, but got %d bytes", + reply_len, actual_len); + memcpy(replybuf, ibuf+sizeof(*rhdr), reply_len); + } else { + memcpy(replybuf, ibuf+sizeof(*rhdr), actual_len); + } + + sd->cam_tag++; + + return actual_len; +} + +static int write_register(struct gspca_dev *gspca_dev, uint16_t reg, uint16_t data) +{ + uint16_t reply[2]; + uint16_t cmd[2]; + int res; + + cmd[0] = cpu_to_le16(reg); + cmd[1] = cpu_to_le16(data); + + PDEBUG(D_USBO, "Write Reg 0x%04x <= 0x%02x", reg, data); + res = send_cmd(gspca_dev, 0x03, cmd, 4, reply, 4); + if (res < 0) + return res; + if (res != 2) { + warn("send_cmd returned %d [%04x %04x], 0000 expected", + res, reply[0], reply[1]); + } + return 0; +} + +/* this function is called at probe time */ +static int sd_config(struct gspca_dev *gspca_dev, + const struct usb_device_id *id) +{ + struct sd *sd = (struct sd *) gspca_dev; + struct cam *cam; + + sd->cam_tag = 0; + + cam = &gspca_dev->cam; + + cam->cam_mode = color_camera_mode; + cam->nmodes = ARRAY_SIZE(color_camera_mode); + + //cam->npkt = 15; + //gspca_dev->pkt_size = 960 * 2; + + return 0; +} + +/* this function is called at probe and resume time */ +static int sd_init(struct gspca_dev *gspca_dev) +{ + PDEBUG(D_PROBE, "Kinect Camera device."); + + return 0; +} + +static int sd_start(struct gspca_dev *gspca_dev) +{ + write_register(gspca_dev, 0x05, 0x00); // reset video stream + write_register(gspca_dev, 0x0c, 0x00); + write_register(gspca_dev, 0x0d, 0x01); + write_register(gspca_dev, 0x0e, 0x1e); // 30Hz bayer + + /* Video Format: Raw bayer data */ + write_register(gspca_dev, 0x05, 0x01); // start video stream + + write_register(gspca_dev, 0x47, 0x00); // disable Hflip + + return 0; +} + +static void sd_stopN(struct gspca_dev *gspca_dev) +{ + write_register(gspca_dev, 0x05, 0x00); // reset video stream +} + +static void sd_pkt_scan(struct gspca_dev *gspca_dev, u8 *__data, int len) +{ + //struct sd *sd = (struct sd *) gspca_dev; + + struct pkt_hdr *hdr = (void*)__data; + uint8_t *data = __data + sizeof(*hdr); + int datalen = len - sizeof(*hdr); + + /* strm->flag == 0x80 for the color camera stream */ + uint8_t sof = 0x80 | 1; + uint8_t mof = 0x80 | 2; + uint8_t eof = 0x80 | 5; + + if (len < 12) + return; + + if (hdr->magic[0] != 'R' || hdr->magic[1] != 'B') { + warn("[Stream %02x] Invalid magic %02x%02x", 0x80, //strm->flag + hdr->magic[0], hdr->magic[1]); + return; + } + + if (hdr->flag == sof) + gspca_frame_add(gspca_dev, FIRST_PACKET, data, datalen); + + else if (hdr->flag == mof) + gspca_frame_add(gspca_dev, INTER_PACKET, data, datalen); + + else if(hdr->flag == eof) + gspca_frame_add(gspca_dev, LAST_PACKET, data, datalen); + + else + warn("Not recognized..."); +} + +/* sub-driver description */ +static const struct sd_desc sd_desc = { + .name = MODULE_NAME, + .ctrls = sd_ctrls, + .nctrls = ARRAY_SIZE(sd_ctrls), + .config = sd_config, + .init = sd_init, + .start = sd_start, + .stopN = sd_stopN, + .pkt_scan = sd_pkt_scan, + /* + .querymenu = sd_querymenu, + .get_streamparm = sd_get_streamparm, + .set_streamparm = sd_set_streamparm, + */ +}; + +/* -- module initialisation -- */ +static const __devinitdata struct usb_device_id device_table[] = { + {USB_DEVICE(0x045e, 0x02ae)}, + {} +}; + +MODULE_DEVICE_TABLE(usb, device_table); + +/* -- device connect -- */ +static int sd_probe(struct usb_interface *intf, const struct usb_device_id *id) +{ + return gspca_dev_probe(intf, id, &sd_desc, sizeof(struct sd), + THIS_MODULE); +} + +static struct usb_driver sd_driver = { + .name = MODULE_NAME, + .id_table = device_table, + .probe = sd_probe, + .disconnect = gspca_disconnect, +#ifdef CONFIG_PM + .suspend = gspca_suspend, + .resume = gspca_resume, +#endif +}; + +/* -- module insert / remove -- */ +static int __init sd_mod_init(void) +{ + return usb_register(&sd_driver); +} + +static void __exit sd_mod_exit(void) +{ + usb_deregister(&sd_driver); +} + +module_init(sd_mod_init); +module_exit(sd_mod_exit); -- 2.1.4