static void iso_callback(struct libusb_transfer *xfer) { int i; fnusb_isoc_stream *strm = (fnusb_isoc_stream*)xfer->user_data; if (strm->dead) { freenect_context *ctx = strm->parent->parent->parent; strm->dead_xfers++; FN_SPEW("EP %02x transfer complete, %d left\n", xfer->endpoint, strm->num_xfers - strm->dead_xfers); return; } switch(xfer->status) { case LIBUSB_TRANSFER_COMPLETED: // Normal operation. { uint8_t *buf = (uint8_t*)xfer->buffer; for (i=0; i<strm->pkts; i++) { strm->cb(strm->parent->parent, buf, xfer->iso_packet_desc[i].actual_length); buf += strm->len; } libusb_submit_transfer(xfer); break; } case LIBUSB_TRANSFER_NO_DEVICE: { // We lost the device we were talking to. This is a large problem, // and one that we should eventually come up with a way to // properly propagate up to the caller. freenect_context *ctx = strm->parent->parent->parent; FN_ERROR("USB device disappeared, cancelling stream :(\n"); strm->dead_xfers++; fnusb_stop_iso(strm->parent, strm); break; } case LIBUSB_TRANSFER_CANCELLED: { freenect_context *ctx = strm->parent->parent->parent; FN_SPEW("EP %02x transfer cancelled\n", xfer->endpoint); strm->dead_xfers++; break; } default: { // On other errors, resubmit the transfer - in particular, libusb // on OSX tends to hit random errors a lot. If we don't resubmit // the transfers, eventually all of them die and then we don't get // any more data from the Kinect. freenect_context *ctx = strm->parent->parent->parent; FN_WARNING("Isochronous transfer error: %d\n", xfer->status); libusb_submit_transfer(xfer); break; } } }
int fnusb_start_iso(fnusb_dev *dev, fnusb_isoc_stream *strm, fnusb_iso_cb cb, int ep, int xfers, int pkts, int len) { freenect_context *ctx = dev->parent->parent; int ret, i; strm->parent = dev; strm->cb = cb; strm->num_xfers = xfers; strm->pkts = pkts; strm->len = len; strm->buffer = (uint8_t*)malloc(xfers * pkts * len); strm->xfers = (struct libusb_transfer**)malloc(sizeof(struct libusb_transfer*) * xfers); strm->dead = 0; strm->dead_xfers = 0; uint8_t *bufp = strm->buffer; for (i=0; i<xfers; i++) { FN_SPEW("Creating EP %02x transfer #%d\n", ep, i); strm->xfers[i] = libusb_alloc_transfer(pkts); libusb_fill_iso_transfer(strm->xfers[i], dev->dev, ep, bufp, pkts * len, pkts, iso_callback, strm, 0); libusb_set_iso_packet_lengths(strm->xfers[i], len); ret = libusb_submit_transfer(strm->xfers[i]); if (ret < 0) FN_WARNING("Failed to submit isochronous transfer %d: %d\n", i, ret); bufp += pkts*len; } return 0; }
static void iso_callback(struct libusb_transfer *xfer) { int i; fnusb_isoc_stream *strm = (fnusb_isoc_stream*)xfer->user_data; if (strm->dead) { freenect_context *ctx = strm->parent->parent->parent; strm->dead_xfers++; FN_SPEW("EP %02x transfer complete, %d left\n", xfer->endpoint, strm->num_xfers - strm->dead_xfers); return; } if(xfer->status == LIBUSB_TRANSFER_COMPLETED) { uint8_t *buf = (uint8_t*)xfer->buffer; for (i=0; i<strm->pkts; i++) { strm->cb(strm->parent->parent, buf, xfer->iso_packet_desc[i].actual_length); buf += strm->len; } libusb_submit_transfer(xfer); } else { freenect_context *ctx = strm->parent->parent->parent; // using the suggestion at https://groups.google.com/group/openkinect/msg/9d78e24063033a6c FN_WARNING("Isochronous transfer error: %d\n", xfer->status); libusb_submit_transfer(xfer); //strm->dead_xfers++; } }
static void iso_callback(struct libusb_transfer *xfer) { int i; fnusb_isoc_stream *strm = xfer->user_data; if (strm->dead) { freenect_context *ctx = strm->parent->parent->parent; strm->dead_xfers++; FN_SPEW("EP %02x transfer complete, %d left\n", xfer->endpoint, strm->num_xfers - strm->dead_xfers); return; } if(xfer->status == LIBUSB_TRANSFER_COMPLETED) { uint8_t *buf = (void*)xfer->buffer; for (i=0; i<strm->pkts; i++) { strm->cb(strm->parent->parent, buf, xfer->iso_packet_desc[i].actual_length); buf += strm->len; } libusb_submit_transfer(xfer); } else { freenect_context *ctx = strm->parent->parent->parent; FN_WARNING("Isochronous transfer error: %d\n", xfer->status); strm->dead_xfers++; } }
int update_tilt_state_alt(freenect_device *dev) { freenect_context *ctx = dev->parent; if (dev->usb_audio.dev == NULL) { FN_WARNING("Motor control failed: audio device missing"); return -1; } int transferred = 0; int res = 0; fn_alt_motor_command cmd; cmd.magic = fn_le32(0x06022009); cmd.tag = fn_le32(tag_seq++); cmd.arg1 = fn_le32(0x68); // 104. Incidentally, the number of bytes that we expect in the reply. cmd.cmd = fn_le32(0x8032); unsigned char buffer[256]; memcpy(buffer, &cmd, 16); res = libusb_bulk_transfer(dev->usb_audio.dev, 0x01, buffer, 16, &transferred, 250); if (res != 0) { return res; } res = libusb_bulk_transfer(dev->usb_audio.dev, 0x81, buffer, 256, &transferred, 250); // 104 bytes if (res != 0) { return res; } struct { int32_t x; int32_t y; int32_t z; int32_t tilt; } accel_and_tilt; memcpy(&accel_and_tilt, buffer + 16, sizeof(accel_and_tilt)); FN_SPEW("Accelerometer state: X == %d \t Y == %d \t Z == %d \t Tilt == %d\n", accel_and_tilt.x, accel_and_tilt.y, accel_and_tilt.z, accel_and_tilt.tilt); dev->raw_state.accelerometer_x = (int16_t)accel_and_tilt.x; dev->raw_state.accelerometer_y = (int16_t)accel_and_tilt.y; dev->raw_state.accelerometer_z = (int16_t)accel_and_tilt.z; // this is multiplied by 2 as the older 1414 device reports angles doubled and freenect takes this into account dev->raw_state.tilt_angle = (int8_t)accel_and_tilt.tilt * 2; // Reply: skip four uint32_t, then you have three int32_t that give you acceleration in that direction, it seems. // Units still to be worked out. return get_reply(dev->usb_audio.dev, ctx); }
int fnusb_open_subdevices(freenect_device *dev, int index) { freenect_context *ctx = dev->parent; dev->usb_cam.parent = dev; dev->usb_cam.dev = NULL; dev->usb_motor.parent = dev; dev->usb_motor.dev = NULL; #ifdef BUILD_AUDIO dev->usb_audio.parent = dev; dev->usb_audio.dev = NULL; #endif libusb_device **devs; //pointer to pointer of device, used to retrieve a list of devices ssize_t cnt = libusb_get_device_list (dev->parent->usb.ctx, &devs); //get the list of devices if (cnt < 0) return -1; int i = 0, nr_cam = 0, nr_mot = 0; #ifdef BUILD_AUDIO int nr_audio = 0; #endif int res; struct libusb_device_descriptor desc; for (i = 0; i < cnt; i++) { int r = libusb_get_device_descriptor (devs[i], &desc); if (r < 0) continue; if (desc.idVendor != VID_MICROSOFT) continue; // Search for the camera if ((ctx->enabled_subdevices & FREENECT_DEVICE_CAMERA) && !dev->usb_cam.dev && desc.idProduct == PID_NUI_CAMERA) { // If the index given by the user matches our camera index if (nr_cam == index) { res = libusb_open (devs[i], &dev->usb_cam.dev); if (res < 0 || !dev->usb_cam.dev) { FN_ERROR("Could not open camera: %d\n", res); dev->usb_cam.dev = NULL; break; } #ifndef _WIN32 // Detach an existing kernel driver for the device res = libusb_kernel_driver_active(dev->usb_cam.dev, 0); if (res == 1) { res = libusb_detach_kernel_driver(dev->usb_cam.dev, 0); if (res < 0) { FN_ERROR("Could not detach kernel driver for camera: %d\n", res); libusb_close(dev->usb_cam.dev); dev->usb_cam.dev = NULL; break; } } #endif res = libusb_claim_interface (dev->usb_cam.dev, 0); if (res < 0) { FN_ERROR("Could not claim interface on camera: %d\n", res); libusb_close(dev->usb_cam.dev); dev->usb_cam.dev = NULL; break; } } else { nr_cam++; } } // Search for the motor if ((ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR) && !dev->usb_motor.dev && desc.idProduct == PID_NUI_MOTOR) { // If the index given by the user matches our camera index if (nr_mot == index) { res = libusb_open (devs[i], &dev->usb_motor.dev); if (res < 0 || !dev->usb_motor.dev) { FN_ERROR("Could not open motor: %d\n", res); dev->usb_motor.dev = NULL; break; } res = libusb_claim_interface (dev->usb_motor.dev, 0); if (res < 0) { FN_ERROR("Could not claim interface on motor: %d\n", res); libusb_close(dev->usb_motor.dev); dev->usb_motor.dev = NULL; break; } } else { nr_mot++; } } #ifdef BUILD_AUDIO // TODO: check that the firmware has already been loaded; if not, upload firmware. // Search for the audio if ((ctx->enabled_subdevices & FREENECT_DEVICE_AUDIO) && !dev->usb_audio.dev && desc.idProduct == PID_NUI_AUDIO) { // If the index given by the user matches our audio index if (nr_audio == index) { res = libusb_open (devs[i], &dev->usb_audio.dev); if (res < 0 || !dev->usb_audio.dev) { FN_ERROR("Could not open audio: %d\n", res); dev->usb_audio.dev = NULL; break; } res = libusb_claim_interface (dev->usb_audio.dev, 0); if (res < 0) { FN_ERROR("Could not claim interface on audio: %d\n", res); libusb_close(dev->usb_audio.dev); dev->usb_audio.dev = NULL; break; } // Using the device handle that we've claimed, see if this // device has already uploaded firmware (has 2 interfaces). If // not, save the serial number (by reading the appropriate // descriptor), upload the firmware, and then enter a loop // waiting for a device with the same serial number to // reappear. int num_interfaces = fnusb_num_interfaces(&dev->usb_audio); if (num_interfaces == 1) { // Read the serial number from the string descriptor and save it. unsigned char string_desc[256]; // String descriptors are at most 256 bytes res = libusb_get_string_descriptor_ascii(dev->usb_audio.dev, desc.iSerialNumber, string_desc, 256); if (res < 0) { FN_ERROR("Failed to retrieve serial number for audio device in bootloader state\n"); break; } char* audio_serial = strdup((char*)string_desc); FN_SPEW("Uploading firmware to audio device in bootloader state.\n"); res = upload_firmware(&dev->usb_audio); if (res < 0) { FN_ERROR("upload_firmware failed: %d\n", res); break; } libusb_close(dev->usb_audio.dev); dev->usb_audio.dev = NULL; // Wait for the device to reappear. int loops = 0; for (loops = 0; loops < 10; loops++) { // Loop for at most 10 tries. FN_SPEW("Try %d: Looking for new audio device matching serial %s\n", loops, audio_serial); // Scan devices. libusb_device **new_dev_list; int dev_index; ssize_t num_new_devs = libusb_get_device_list(ctx->usb.ctx, &new_dev_list); for (dev_index = 0; dev_index < num_new_devs; ++dev_index) { struct libusb_device_descriptor new_dev_desc; int r; r = libusb_get_device_descriptor (new_dev_list[dev_index], &new_dev_desc); if (r < 0) continue; // If this dev is a Kinect audio device, open device, read serial, and compare. if (new_dev_desc.idVendor == VID_MICROSOFT && new_dev_desc.idProduct == PID_NUI_AUDIO) { FN_SPEW("Matched VID/PID!\n"); libusb_device_handle* new_dev_handle; // Open device r = libusb_open(new_dev_list[dev_index], &new_dev_handle); if (r < 0) continue; // Read serial r = libusb_get_string_descriptor_ascii(new_dev_handle, new_dev_desc.iSerialNumber, string_desc, 256); if (r < 0) { FN_SPEW("Lost new audio device while fetching serial number.\n"); libusb_close(new_dev_handle); continue; } // Compare to expected serial if (r == strlen(audio_serial) && strcmp((char*)string_desc, audio_serial) == 0) { // We found it! r = libusb_claim_interface(new_dev_handle, 0); if (r != 0) { // Ouch, found the device but couldn't claim the interface. FN_SPEW("Device with serial %s reappeared but couldn't claim interface 0\n", audio_serial); libusb_close(new_dev_handle); continue; } // Save the device handle. dev->usb_audio.dev = new_dev_handle; // Verify that we've actually found a device running the right firmware. if (fnusb_num_interfaces(&dev->usb_audio) != 2) { FN_SPEW("Opened audio with matching serial but too few interfaces.\n"); dev->usb_audio.dev = NULL; libusb_close(new_dev_handle); continue; } break; } else { FN_SPEW("Got serial %s, expected serial %s\n", (char*)string_desc, audio_serial); } } } libusb_free_device_list(new_dev_list, 1); // If we found the right device, break out of this loop. if (dev->usb_audio.dev) break; // Sleep for a second to give the device more time to reenumerate. sleep(1); } free(audio_serial); } } else { nr_audio++; } } #endif } libusb_free_device_list (devs, 1); // free the list, unref the devices in it // Check that each subdevice is either opened or not enabled. if ( (dev->usb_cam.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_CAMERA)) && (dev->usb_motor.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR)) #ifdef BUILD_AUDIO && (dev->usb_audio.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_AUDIO)) #endif ) { return 0; } else { if (dev->usb_cam.dev) { libusb_release_interface(dev->usb_cam.dev, 0); libusb_close(dev->usb_cam.dev); } if (dev->usb_motor.dev) { libusb_release_interface(dev->usb_motor.dev, 0); libusb_close(dev->usb_motor.dev); } #ifdef BUILD_AUDIO if (dev->usb_audio.dev) { libusb_release_interface(dev->usb_audio.dev, 0); libusb_close(dev->usb_audio.dev); } #endif return -1; } }
static void iso_callback(struct libusb_transfer *xfer) { int i; fnusb_isoc_stream *strm = (fnusb_isoc_stream*)xfer->user_data; freenect_context *ctx = strm->parent->parent->parent; if (strm->dead) { strm->dead_xfers++; FN_SPEW("EP %02x transfer complete, %d left\n", xfer->endpoint, strm->num_xfers - strm->dead_xfers); return; } switch(xfer->status) { case LIBUSB_TRANSFER_COMPLETED: // Normal operation. { uint8_t *buf = (uint8_t*)xfer->buffer; for (i=0; i<strm->pkts; i++) { strm->cb(strm->parent->parent, buf, xfer->iso_packet_desc[i].actual_length); buf += strm->len; } int res; res = libusb_submit_transfer(xfer); if (res != 0) { FN_ERROR("iso_callback(): failed to resubmit transfer after successful completion: %d\n", res); strm->dead_xfers++; if (res == LIBUSB_ERROR_NO_DEVICE) { strm->parent->device_dead = 1; } } break; } case LIBUSB_TRANSFER_NO_DEVICE: { // We lost the device we were talking to. This is a large problem, // and one that we should eventually come up with a way to // properly propagate up to the caller. if(!strm->parent->device_dead) { FN_ERROR("USB device disappeared, cancelling stream %02x :(\n", xfer->endpoint); } strm->dead_xfers++; strm->parent->device_dead = 1; break; } case LIBUSB_TRANSFER_CANCELLED: { if(strm->dead) { FN_SPEW("EP %02x transfer cancelled\n", xfer->endpoint); } else { // This seems to be a libusb bug on OSX - instead of completing // the transfer with LIBUSB_TRANSFER_NO_DEVICE, the transfers // simply come back cancelled by the OS. We can detect this, // though - the stream should be marked dead if we're // intentionally cancelling transfers. if(!strm->parent->device_dead) { FN_ERROR("Got cancelled transfer, but we didn't request it - device disconnected?\n"); } strm->parent->device_dead = 1; } strm->dead_xfers++; break; } default: { // On other errors, resubmit the transfer - in particular, libusb // on OSX tends to hit random errors a lot. If we don't resubmit // the transfers, eventually all of them die and then we don't get // any more data from the Kinect. FN_WARNING("Isochronous transfer error: %d\n", xfer->status); int res; res = libusb_submit_transfer(xfer); if (res != 0) { FN_ERROR("Isochronous transfer resubmission failed after unknown error: %d\n", res); strm->dead_xfers++; if (res == LIBUSB_ERROR_NO_DEVICE) { strm->parent->device_dead = 1; } } break; } } }
FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) { freenect_context *ctx = dev->parent; dev->device_does_motor_control_with_audio = 0; dev->motor_control_with_audio_enabled = 0; dev->usb_cam.parent = dev; dev->usb_cam.dev = NULL; dev->usb_motor.parent = dev; dev->usb_motor.dev = NULL; #ifdef BUILD_AUDIO dev->usb_audio.parent = dev; dev->usb_audio.dev = NULL; #endif libusb_device **devs; //pointer to pointer of device, used to retrieve a list of devices ssize_t cnt = libusb_get_device_list (dev->parent->usb.ctx, &devs); //get the list of devices if (cnt < 0) return -1; int i = 0, nr_cam = 0, nr_mot = 0; #ifdef BUILD_AUDIO int nr_audio = 0; #endif int res; struct libusb_device_descriptor desc; for (i = 0; i < cnt; i++) { int r = libusb_get_device_descriptor (devs[i], &desc); if (r < 0) continue; if (desc.idVendor != VID_MICROSOFT) continue; res = 0; // Search for the camera if ((ctx->enabled_subdevices & FREENECT_DEVICE_CAMERA) && !dev->usb_cam.dev && (desc.idProduct == PID_NUI_CAMERA || desc.idProduct == PID_K4W_CAMERA)) { // If the index given by the user matches our camera index if (nr_cam == index) { res = libusb_open (devs[i], &dev->usb_cam.dev); if (res < 0 || !dev->usb_cam.dev) { FN_ERROR("Could not open camera: %d\n", res); dev->usb_cam.dev = NULL; break; } if (desc.idProduct == PID_K4W_CAMERA || desc.bcdDevice != fn_le32(267)) { freenect_device_flags requested_devices = ctx->enabled_subdevices; // Not the old kinect so we only set up the camera ctx->enabled_subdevices = FREENECT_DEVICE_CAMERA; ctx->zero_plane_res = 334; dev->device_does_motor_control_with_audio = 1; //lets also set the LED ON //this keeps the camera alive for some systems which get freezes if( desc.idProduct == PID_K4W_CAMERA ){ freenect_extra_keep_alive(PID_K4W_AUDIO); }else{ freenect_extra_keep_alive(PID_NUI_AUDIO); } #ifdef BUILD_AUDIO //for newer devices we need to enable the audio device for motor control //we only do this though if motor has been requested. if ((requested_devices & FREENECT_DEVICE_MOTOR) && (requested_devices & FREENECT_DEVICE_AUDIO) == 0) { ctx->enabled_subdevices = (freenect_device_flags)(ctx->enabled_subdevices | FREENECT_DEVICE_AUDIO); } #endif }else{ /* The good old kinect that tilts and tweets */ ctx->zero_plane_res = 322; } #ifndef _WIN32 // Detach an existing kernel driver for the device res = libusb_kernel_driver_active(dev->usb_cam.dev, 0); if (res == 1) { res = libusb_detach_kernel_driver(dev->usb_cam.dev, 0); if (res < 0) { FN_ERROR("Could not detach kernel driver for camera: %d\n", res); libusb_close(dev->usb_cam.dev); dev->usb_cam.dev = NULL; break; } } #endif res = libusb_claim_interface (dev->usb_cam.dev, 0); if (res < 0) { FN_ERROR("Could not claim interface on camera: %d\n", res); libusb_close(dev->usb_cam.dev); dev->usb_cam.dev = NULL; break; } if(desc.idProduct == PID_K4W_CAMERA){ res = libusb_set_interface_alt_setting(dev->usb_cam.dev, 0, 1); if (res != 0) { FN_ERROR("Failed to set alternate interface #1 for K4W: %d\n", res); libusb_close(dev->usb_cam.dev); dev->usb_cam.dev = NULL; break; } } } else { nr_cam++; } } } if(ctx->enabled_subdevices == FREENECT_DEVICE_CAMERA || res < 0) cnt = 0; // Search for the motor for (i = 0; i < cnt; i++) { int r = libusb_get_device_descriptor (devs[i], &desc); if (r < 0) continue; if (desc.idVendor != VID_MICROSOFT) continue; if ((ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR) && !dev->usb_motor.dev && desc.idProduct == PID_NUI_MOTOR) { // If the index given by the user matches our camera index if (nr_mot == index) { res = libusb_open (devs[i], &dev->usb_motor.dev); if (res < 0 || !dev->usb_motor.dev) { FN_ERROR("Could not open motor: %d\n", res); dev->usb_motor.dev = NULL; break; } res = libusb_claim_interface (dev->usb_motor.dev, 0); if (res < 0) { FN_ERROR("Could not claim interface on motor: %d\n", res); libusb_close(dev->usb_motor.dev); dev->usb_motor.dev = NULL; break; } } else { nr_mot++; } } #ifdef BUILD_AUDIO // TODO: check that the firmware has already been loaded; if not, upload firmware. // Search for the audio if ((ctx->enabled_subdevices & FREENECT_DEVICE_AUDIO) && !dev->usb_audio.dev && (desc.idProduct == PID_NUI_AUDIO || fnusb_is_pid_k4w_audio(desc.idProduct))) { // If the index given by the user matches our audio index if (nr_audio == index) { res = libusb_open (devs[i], &dev->usb_audio.dev); if (res < 0 || !dev->usb_audio.dev) { FN_ERROR("Could not open audio: %d\n", res); dev->usb_audio.dev = NULL; break; } res = libusb_claim_interface (dev->usb_audio.dev, 0); if (res < 0) { FN_ERROR("Could not claim interface on audio: %d\n", res); libusb_close(dev->usb_audio.dev); dev->usb_audio.dev = NULL; break; } // Using the device handle that we've claimed, see if this // device has already uploaded firmware (has 2 interfaces). If // not, save the serial number (by reading the appropriate // descriptor), upload the firmware, and then enter a loop // waiting for a device with the same serial number to // reappear. int num_interfaces = fnusb_num_interfaces(&dev->usb_audio); if( num_interfaces >= 2 ){ if( dev->device_does_motor_control_with_audio ){ dev->motor_control_with_audio_enabled = 1; } }else{ // Read the serial number from the string descriptor and save it. unsigned char string_desc[256]; // String descriptors are at most 256 bytes res = libusb_get_string_descriptor_ascii(dev->usb_audio.dev, desc.iSerialNumber, string_desc, 256); if (res < 0) { FN_ERROR("Failed to retrieve serial number for audio device in bootloader state\n"); break; } char* audio_serial = strdup((char*)string_desc); FN_SPEW("Uploading firmware to audio device in bootloader state.\n"); // Check if we can load from memory - otherwise load from disk if( desc.idProduct == PID_NUI_AUDIO && ctx->fn_fw_nui_ptr && ctx->fn_fw_nui_size > 0){ FN_SPEW("loading firmware from memory\n"); res = upload_firmware_from_memory(&dev->usb_audio, ctx->fn_fw_nui_ptr, ctx->fn_fw_nui_size); } else if( desc.idProduct == PID_K4W_AUDIO && ctx->fn_fw_k4w_ptr && ctx->fn_fw_k4w_size > 0 ){ FN_SPEW("loading firmware from memory\n"); res = upload_firmware_from_memory(&dev->usb_audio, ctx->fn_fw_k4w_ptr, ctx->fn_fw_k4w_size); } else{ res = upload_firmware(&dev->usb_audio, "audios.bin"); } if (res < 0) { FN_ERROR("upload_firmware failed: %d\n", res); break; } libusb_close(dev->usb_audio.dev); dev->usb_audio.dev = NULL; // Wait for the device to reappear. int loops = 0; for (loops = 0; loops < 10; loops++) { // Loop for at most 10 tries. FN_SPEW("Try %d: Looking for new audio device matching serial %s\n", loops, audio_serial); // Scan devices. libusb_device **new_dev_list; int dev_index; ssize_t num_new_devs = libusb_get_device_list(ctx->usb.ctx, &new_dev_list); for (dev_index = 0; dev_index < num_new_devs; ++dev_index) { struct libusb_device_descriptor new_dev_desc; int r; r = libusb_get_device_descriptor (new_dev_list[dev_index], &new_dev_desc); if (r < 0) continue; // If this dev is a Kinect audio device, open device, read serial, and compare. if (new_dev_desc.idVendor == VID_MICROSOFT && (new_dev_desc.idProduct == PID_NUI_AUDIO || fnusb_is_pid_k4w_audio(desc.idProduct))) { FN_SPEW("Matched VID/PID!\n"); libusb_device_handle* new_dev_handle; // Open device r = libusb_open(new_dev_list[dev_index], &new_dev_handle); if (r < 0) continue; // Read serial r = libusb_get_string_descriptor_ascii(new_dev_handle, new_dev_desc.iSerialNumber, string_desc, 256); if (r < 0) { FN_SPEW("Lost new audio device while fetching serial number.\n"); libusb_close(new_dev_handle); continue; } // Compare to expected serial if (r == strlen(audio_serial) && strcmp((char*)string_desc, audio_serial) == 0) { // We found it! r = libusb_claim_interface(new_dev_handle, 0); if (r != 0) { // Ouch, found the device but couldn't claim the interface. FN_SPEW("Device with serial %s reappeared but couldn't claim interface 0\n", audio_serial); libusb_close(new_dev_handle); continue; } // Save the device handle. dev->usb_audio.dev = new_dev_handle; // Verify that we've actually found a device running the right firmware. num_interfaces = fnusb_num_interfaces(&dev->usb_audio); if( num_interfaces >= 2 ){ if( dev->device_does_motor_control_with_audio ){ dev->motor_control_with_audio_enabled = 1; } }else{ FN_SPEW("Opened audio with matching serial but too few interfaces.\n"); dev->usb_audio.dev = NULL; libusb_close(new_dev_handle); continue; } break; } else { FN_SPEW("Got serial %s, expected serial %s\n", (char*)string_desc, audio_serial); } } } libusb_free_device_list(new_dev_list, 1); // If we found the right device, break out of this loop. if (dev->usb_audio.dev) break; // Sleep for a second to give the device more time to reenumerate. sleep(1); } free(audio_serial); } } else { nr_audio++; } } #endif } libusb_free_device_list (devs, 1); // free the list, unref the devices in it // Check that each subdevice is either opened or not enabled. if ( (dev->usb_cam.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_CAMERA)) && (dev->usb_motor.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR)) #ifdef BUILD_AUDIO && (dev->usb_audio.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_AUDIO)) #endif ) { return 0; } else { if (dev->usb_cam.dev) { libusb_release_interface(dev->usb_cam.dev, 0); libusb_close(dev->usb_cam.dev); } else { FN_ERROR("Failed to open camera subdevice or it is not disabled."); } if (dev->usb_motor.dev) { libusb_release_interface(dev->usb_motor.dev, 0); libusb_close(dev->usb_motor.dev); } else { FN_ERROR("Failed to open motor subddevice or it is not disabled."); } #ifdef BUILD_AUDIO if (dev->usb_audio.dev) { libusb_release_interface(dev->usb_audio.dev, 0); libusb_close(dev->usb_audio.dev); } else { FN_ERROR("Failed to open audio subdevice or it is not disabled."); } #endif return -1; } }
FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) { freenect_context *ctx = dev->parent; dev->device_does_motor_control_with_audio = 0; dev->motor_control_with_audio_enabled = 0; dev->usb_cam.parent = dev; dev->usb_cam.dev = NULL; dev->usb_motor.parent = dev; dev->usb_motor.dev = NULL; dev->usb_audio.parent = dev; dev->usb_audio.dev = NULL; libusb_device **devs; // pointer to pointer of device, used to retrieve a list of devices ssize_t cnt = libusb_get_device_list (dev->parent->usb.ctx, &devs); //get the list of devices if (cnt < 0) return -1; int i = 0, nr_cam = 0, nr_mot = 0; int nr_audio = 0; int res; struct libusb_device_descriptor desc; for (i = 0; i < cnt; i++) { int r = libusb_get_device_descriptor (devs[i], &desc); if (r < 0) continue; if (desc.idVendor != VID_MICROSOFT) continue; res = 0; // Search for the camera if ((ctx->enabled_subdevices & FREENECT_DEVICE_CAMERA) && !dev->usb_cam.dev && (desc.idProduct == PID_NUI_CAMERA || desc.idProduct == PID_K4W_CAMERA)) { // If the index given by the user matches our camera index if (nr_cam == index) { dev->usb_cam.VID = desc.idVendor; dev->usb_cam.PID = desc.idProduct; res = libusb_open (devs[i], &dev->usb_cam.dev); if (res < 0 || !dev->usb_cam.dev) { FN_ERROR("Could not open camera: %d\n", res); dev->usb_cam.dev = NULL; break; } if (desc.idProduct == PID_K4W_CAMERA || desc.bcdDevice != fn_le32(267)) { freenect_device_flags requested_devices = ctx->enabled_subdevices; // Not the 1414 kinect so remove the motor flag, this should preserve the audio flag if set ctx->enabled_subdevices = (freenect_device_flags)(ctx->enabled_subdevices & ~FREENECT_DEVICE_MOTOR); ctx->zero_plane_res = 334; dev->device_does_motor_control_with_audio = 1; // set the LED for non 1414 devices to keep the camera alive for some systems which get freezes libusb_device * audioDevice = fnusb_find_connected_audio_device(devs[i], devs, cnt); if (audioDevice != NULL) { libusb_device_handle * audioHandle = NULL; res = libusb_open(audioDevice, &audioHandle); if (res != 0) { FN_ERROR("Failed to set the LED of K4W or 1473 device: %d\n", res); } else { // we need to do this as it is possible that the device was not closed properly in a previous session // if we don't do this and the device wasn't closed properly - it can cause infinite hangs on LED and TILT functions libusb_reset_device(audioHandle); libusb_close(audioHandle); res = libusb_open(audioDevice, &audioHandle); if (res == 0) { res = libusb_claim_interface(audioHandle, 0); if (res != 0) { FN_ERROR("Unable to claim interface %d\n", res); } else { fnusb_set_led_alt(audioHandle, ctx, LED_GREEN); libusb_release_interface(audioHandle, 0); } libusb_close(audioHandle); } } } // for newer devices we need to enable the audio device for motor control // we only do this though if motor has been requested. if ((requested_devices & FREENECT_DEVICE_MOTOR) && (requested_devices & FREENECT_DEVICE_AUDIO) == 0) { ctx->enabled_subdevices = (freenect_device_flags)(ctx->enabled_subdevices | FREENECT_DEVICE_AUDIO); } } else { // The good old kinect that tilts and tweets ctx->zero_plane_res = 322; } res = fnusb_claim_camera(dev); if (res < 0) { break; } } else { nr_cam++; } } } if (ctx->enabled_subdevices == FREENECT_DEVICE_CAMERA || res < 0) cnt = 0; // Search for the motor for (i = 0; i < cnt; i++) { int r = libusb_get_device_descriptor (devs[i], &desc); if (r < 0) continue; if (desc.idVendor != VID_MICROSOFT) continue; if ((ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR) && !dev->usb_motor.dev && desc.idProduct == PID_NUI_MOTOR) { // If the index given by the user matches our camera index if (nr_mot == index) { dev->usb_motor.VID = desc.idVendor; dev->usb_motor.PID = desc.idProduct; res = libusb_open (devs[i], &dev->usb_motor.dev); if (res < 0 || !dev->usb_motor.dev) { FN_ERROR("Could not open motor: %d\n", res); dev->usb_motor.dev = NULL; break; } res = libusb_claim_interface (dev->usb_motor.dev, 0); if (res < 0) { FN_ERROR("Could not claim interface on motor: %d\n", res); libusb_close(dev->usb_motor.dev); dev->usb_motor.dev = NULL; break; } } else { nr_mot++; } } // Search for the audio if ((ctx->enabled_subdevices & FREENECT_DEVICE_AUDIO) && !dev->usb_audio.dev && (desc.idProduct == PID_NUI_AUDIO || fnusb_is_pid_k4w_audio(desc.idProduct))) { // If the index given by the user matches our audio index if (nr_audio == index) { dev->usb_audio.VID = desc.idVendor; dev->usb_audio.PID = desc.idProduct; res = libusb_open (devs[i], &dev->usb_audio.dev); if (res < 0 || !dev->usb_audio.dev) { FN_ERROR("Could not open audio: %d\n", res); dev->usb_audio.dev = NULL; break; } res = libusb_claim_interface (dev->usb_audio.dev, 0); if (res < 0) { FN_ERROR("Could not claim interface on audio: %d\n", res); libusb_close(dev->usb_audio.dev); dev->usb_audio.dev = NULL; break; } // Using the device handle that we've claimed, see if this // device has already uploaded firmware (has 2 interfaces). // If not, save the serial number (by reading the appropriate // descriptor), upload the firmware, and then enter a loop // waiting for a device with the same serial number to // reappear. int num_interfaces = fnusb_num_interfaces(&dev->usb_audio); if (num_interfaces >= 2) { if (dev->device_does_motor_control_with_audio) { dev->motor_control_with_audio_enabled = 1; } } else { // Read the serial number from the string descriptor and save it. unsigned char string_desc[256]; // String descriptors are at most 256 bytes res = libusb_get_string_descriptor_ascii(dev->usb_audio.dev, desc.iSerialNumber, string_desc, 256); if (res < 0) { FN_ERROR("Failed to retrieve serial number for audio device in bootloader state\n"); break; } char* audio_serial = strdup((char*)string_desc); FN_SPEW("Uploading firmware to audio device in bootloader state.\n"); // Check if we can load from memory - otherwise load from disk if (desc.idProduct == PID_NUI_AUDIO && ctx->fn_fw_nui_ptr && ctx->fn_fw_nui_size > 0) { FN_SPEW("loading firmware from memory\n"); res = upload_firmware_from_memory(&dev->usb_audio, ctx->fn_fw_nui_ptr, ctx->fn_fw_nui_size); } else if (desc.idProduct == PID_K4W_AUDIO && ctx->fn_fw_k4w_ptr && ctx->fn_fw_k4w_size > 0) { FN_SPEW("loading firmware from memory\n"); res = upload_firmware_from_memory(&dev->usb_audio, ctx->fn_fw_k4w_ptr, ctx->fn_fw_k4w_size); } else { res = upload_firmware(&dev->usb_audio, "audios.bin"); } if (res < 0) { FN_ERROR("upload_firmware failed: %d\n", res); break; } libusb_close(dev->usb_audio.dev); dev->usb_audio.dev = NULL; // Wait for the device to reappear. int loops = 0; for (loops = 0; loops < 10; loops++) { FN_SPEW("Try %d: Looking for new audio device matching serial %s\n", loops, audio_serial); // Scan devices. libusb_device **new_dev_list; int dev_index; ssize_t num_new_devs = libusb_get_device_list(ctx->usb.ctx, &new_dev_list); for (dev_index = 0; dev_index < num_new_devs; ++dev_index) { struct libusb_device_descriptor new_dev_desc; int r; r = libusb_get_device_descriptor (new_dev_list[dev_index], &new_dev_desc); if (r < 0) continue; // If this dev is a Kinect audio device, open device, read serial, and compare. if (new_dev_desc.idVendor == VID_MICROSOFT && (new_dev_desc.idProduct == PID_NUI_AUDIO || fnusb_is_pid_k4w_audio(desc.idProduct))) { FN_SPEW("Matched VID/PID!\n"); libusb_device_handle* new_dev_handle; // Open device r = libusb_open(new_dev_list[dev_index], &new_dev_handle); if (r < 0) continue; // Read serial r = libusb_get_string_descriptor_ascii(new_dev_handle, new_dev_desc.iSerialNumber, string_desc, 256); if (r < 0) { FN_SPEW("Lost new audio device while fetching serial number.\n"); libusb_close(new_dev_handle); continue; } // Compare to expected serial if (r == strlen(audio_serial) && strcmp((char*)string_desc, audio_serial) == 0) { // We found it! r = libusb_claim_interface(new_dev_handle, 0); if (r != 0) { // Ouch, found the device but couldn't claim the interface. FN_SPEW("Device with serial %s reappeared but couldn't claim interface 0\n", audio_serial); libusb_close(new_dev_handle); continue; } // Save the device handle. dev->usb_audio.dev = new_dev_handle; // Verify that we've actually found a device running the right firmware. num_interfaces = fnusb_num_interfaces(&dev->usb_audio); if (num_interfaces >= 2) { if (dev->device_does_motor_control_with_audio) { dev->motor_control_with_audio_enabled = 1; } } else { FN_SPEW("Opened audio with matching serial but too few interfaces.\n"); dev->usb_audio.dev = NULL; libusb_close(new_dev_handle); continue; } break; } else { FN_SPEW("Got serial %s, expected serial %s\n", (char*)string_desc, audio_serial); } } } libusb_free_device_list(new_dev_list, 1); // If we found the right device, break out of this loop. if (dev->usb_audio.dev) break; // Sleep for a second to give the device more time to reenumerate. sleep(1); } free(audio_serial); } } else { nr_audio++; } } } libusb_free_device_list (devs, 1); // free the list, unref the devices in it if ((dev->usb_cam.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_CAMERA)) && (dev->usb_motor.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR))) //&& (dev->usb_audio.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_AUDIO))) { // Each requested subdevice is open. // Except audio, which may fail if firmware is missing (or because it hates us). return 0; } if (dev->usb_cam.dev != NULL) { libusb_release_interface(dev->usb_cam.dev, 0); libusb_close(dev->usb_cam.dev); } else { FN_ERROR("Failed to open camera subdevice or it is not disabled."); } if (dev->usb_motor.dev != NULL) { libusb_release_interface(dev->usb_motor.dev, 0); libusb_close(dev->usb_motor.dev); } else { FN_ERROR("Failed to open motor subddevice or it is not disabled."); } if (dev->usb_audio.dev != NULL) { libusb_release_interface(dev->usb_audio.dev, 0); libusb_close(dev->usb_audio.dev); } else { FN_ERROR("Failed to open audio subdevice or it is not disabled."); } return -1; }