//**********************************************************
//!
//! Closes connection to the currently open device
//!
//! @return ICP_RC_OK => success (ignores errors)
//!
ICP_ErrorType bdm_usb_close( void ) {
int rc;
// print("bdm_usb_close()\n");
if (usbDeviceHandle == NULL) {
print("bdm_usb_close() - device not open - no action\n");
return ICP_RC_OK;
}
rc = libusb_release_interface(usbDeviceHandle, 0);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_close() - libusb_release_interface() failed, rc = %s\n", libusb_strerror((libusb_error)rc));
}
int configValue;
rc = libusb_get_configuration(usbDeviceHandle, &configValue);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_close() - libusb_get_configuration() failed, rc = %s\n", libusb_strerror((libusb_error)rc));
}
// Unconfigure BDM
// I know the libusb documentation says to use -1 but this ends up being passed
// to the USB device WHICH IS NOT A GOOD THING!
#ifdef WIN32
rc = libusb_set_configuration(usbDeviceHandle, 0);
#else
rc = libusb_set_configuration(usbDeviceHandle, -1);
#endif
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_close() - libusb_set_configuration(0) failed, rc = %s\n", libusb_strerror((libusb_error)rc));
}
libusb_close(usbDeviceHandle);
usbDeviceHandle = NULL;
return ICP_RC_OK;
}
static int set_hackrf_configuration(libusb_device_handle* usb_device, int config)
{
int result, curr_config;
result = libusb_get_configuration(usb_device, &curr_config);
if( result != 0 )
{
return HACKRF_ERROR_LIBUSB;
}
if(curr_config != config)
{
result = detach_kernel_drivers(usb_device);
if( result != 0 )
{
return result;
}
result = libusb_set_configuration(usb_device, config);
if( result != 0 )
{
return HACKRF_ERROR_LIBUSB;
}
}
result = detach_kernel_drivers(usb_device);
if( result != 0 )
{
return result;
}
return LIBUSB_SUCCESS;
}
int jtag_libusb_set_configuration(jtag_libusb_device_handle *devh,
int configuration)
{
struct jtag_libusb_device *udev = jtag_libusb_get_device(devh);
int retCode = -99;
struct libusb_config_descriptor *config = NULL;
int current_config = -1;
retCode = libusb_get_configuration(devh, ¤t_config);
if (retCode != 0)
return retCode;
retCode = libusb_get_config_descriptor(udev, configuration, &config);
if (retCode != 0 || config == NULL)
return retCode;
/* Only change the configuration if it is not already set to the
same one. Otherwise this issues a lightweight reset and hangs
LPC-Link2 with JLink firmware. */
if (current_config != config->bConfigurationValue)
retCode = libusb_set_configuration(devh, config->bConfigurationValue);
libusb_free_config_descriptor(config);
return retCode;
}
static bool claim_tir(int config, unsigned int interface)
{
ltr_int_log_message("Trying to claim TrackIR interface.\n");
if(libusb_claim_interface(handle, interface)){
ltr_int_log_message("Couldn't claim interface!\n");
interface_claimed = false;
return false;
}
ltr_int_log_message("TrackIR interface claimed.\n");
interface_claimed = true;
// if(libusb_reset_device(handle)){
// log_message("Couldn't reset device!\n");
// return false;
// }
int cfg;
ltr_int_log_message("Requesting TrackIR configuration.\n");
if(libusb_get_configuration(handle, &cfg)){
ltr_int_log_message("Can't get device configuration!\n");
return false;
}
ltr_int_log_message("TrackIR configuration received.\n");
if(cfg != config){
ltr_int_log_message("Device configuration is wrong!\n");
return false;
}
ltr_int_log_message("Device configuration is OK.\n");
return true;
}
void USBThread::ConnectToUSBDevice()
{
printf("Connect to the USB device -> THREAD: %d\n",QThread::currentThreadId());
//initilize usb
libusb_init(&sensor_cxt);
libusb_set_debug(sensor_cxt, 1);
bool usbSetUp = false;
while (!usbSetUp) {
usbSetUp = true;
printf("going to try and get handle for USB\n");
handle = libusb_open_device_with_vid_pid(sensor_cxt,MY_VID,MY_PID);
if (handle == 0) {
printf("failed to get handle\n");
usbSetUp = false;
((GenericExecThread*)(this->thread()))->forceSleep(1);
continue;
}
printf("got handle!\n");
//printf("Reseting device\n");
//libusb_reset_device(handle);
//printf("Device Reset\n");
int usbConfig;
libusb_get_configuration(handle, &usbConfig);
if (usbConfig == 0)
{
printf("Setting Configuration\n");
if (!libusb_set_configuration(handle, 1)) {
printf("Failed to set configuration\n");
libusb_close(handle);
FailedToConnect();
usbSetUp = false;
((GenericExecThread*)(this->thread()))->forceSleep(10);
continue;
}
printf("Set Configuration\n");
}
printf("Claiming Interface\n");
int claimResults = libusb_claim_interface(handle, 0);
if (claimResults != 0) {
printf("Failed to claim Interface\n");
libusb_close(handle);
usbSetUp = false;
FailedToConnect();
if (LIBUSB_ERROR_NOT_FOUND == claimResults)
printf("Requested Interface does not exist.\n");
else if (LIBUSB_ERROR_BUSY == claimResults)
printf("Another program or driver has claimed the interface.\n");
else if (LIBUSB_ERROR_NO_DEVICE == claimResults)
printf("The device has been disconnected .\n");
((GenericExecThread*)(this->thread()))->forceSleep(10);
continue;
}
printf("Claimed Interface\n");
}
// done connecting and configuring, should be able to
// send bulk packets now
}
int LibusbDevice::ReleaseAllInterfacesForCurrentConfig() const
{
int config_num;
const int get_config_ret = libusb_get_configuration(m_handle, &config_num);
if (get_config_ret < 0)
return get_config_ret;
return ReleaseAllInterfaces(config_num);
}
//**********************************************************
//!
//! Open connection to device enumerated by bdm_usb_find_devices()
//!
//! @param device_no Device number to open
//!
//! @return \n
//! == ICP_RC_OK (0) => Success\n
//! == ICP_RC_USB_ERROR => USB failure
//!
ICP_ErrorType bdm_usb_open( unsigned int device_no ) {
// print("bdm_usb_open( %d )\n", device_no);
if (!initialised) {
print("bdm_usb_open() - Not initialised! \n");
return ICP_RC_USB_ERROR;
}
if (device_no >= deviceCount) {
print("bdm_usb_open() - Illegal device #\n");
return ICP_RC_ILLEGAL_PARAMS;
}
if (usbDeviceHandle != NULL) {
print("bdm_usb_open() - Closing previous device\n");
bdm_usb_close();
}
int rc = libusb_open(bdmDevices[device_no], &usbDeviceHandle);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_open() - libusb_open() failed, rc = %s\n", libusb_strerror((libusb_error)rc));
usbDeviceHandle = NULL;
return ICP_RC_USB_ERROR;
}
int configuration = 0;
rc = libusb_get_configuration(usbDeviceHandle, &configuration);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_open() - libusb_get_configuration() failed, rc = %s\n", libusb_strerror((libusb_error)rc));
}
if (configuration != 1) {
rc = libusb_set_configuration(usbDeviceHandle, 1);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_open() - libusb_set_configuration(1) failed, rc = %s\n", libusb_strerror((libusb_error)rc));
// Release the device
libusb_close(usbDeviceHandle);
usbDeviceHandle = NULL;
return ICP_RC_USB_ERROR;
}
}
rc = libusb_claim_interface(usbDeviceHandle, 0);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_open() - libusb_claim_interface(0) failed, rc = %s\n", libusb_strerror((libusb_error)rc));
// Release the device
libusb_set_configuration(usbDeviceHandle, 0);
libusb_close(usbDeviceHandle);
usbDeviceHandle = NULL;
return ICP_RC_USB_ERROR;
}
rc = libusb_clear_halt(usbDeviceHandle, 0x01);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_open() - libusb_clear_halt(...,0x01) failed, rc = %s\n", libusb_strerror((libusb_error)rc));
}
rc = libusb_clear_halt(usbDeviceHandle, 0x82);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_open() - libusb_clear_halt(...,0x82) failed, rc = %s\n", libusb_strerror((libusb_error)rc));
}
return (ICP_RC_OK);
}
/**
* \brief Get the number of the current configuration.
*
* \return configuration number
*/
int Device::getConfiguration() throw(USBError) {
int result;
int rc = libusb_get_configuration(dev_handle, &result);
if (rc != LIBUSB_SUCCESS) {
debug(LOG_ERR, DEBUG_LOG, 0, "cannot get configuration: %s",
libusb_error_name(rc));
throw USBError(libusb_error_name(rc));
}
return result;
}
void irecv_usb_get_configuration(libusb_device_handle *usb_handle, int *config) {
libusbip_error_t error = LIBUSBIP_E_SUCCESS;
if (libirecovery_usage_context == IRECV_CTX_LOCAL) {
libusb_get_configuration(usb_handle, config);
return;
}
error =
libusbip_get_configuration(&libirecovery_connection_info,
&libirecovery_device_handle, config);
if (error < 0) {
debug("libusbip_get_configuration failed\n");
}
}
int
usb_init (void)
{
libusb_device_handle *udev;
// XXX: error handling
libusb_init (NULL);
udev = libusb_open_device_with_vid_pid(NULL, 0x04A9, 0x3042);
int configuration = 0;
libusb_get_configuration(udev, &configuration);
libusb_set_configuration(udev, configuration);
usb.g.pUdev = udev;
usb.g.byInterfaceNumber = 0; // originally dev->config[0].interface[0].altsetting[0].bInterfaceNumber;
libusb_claim_interface(udev, usb.g.byInterfaceNumber);
return 0;
}
static int MSP_get_endpoints(HANDLE h, uint8_t *int_in, uint8_t *bulk_in, uint8_t *bulk_out) {
int r;
int c;
int j, k, m;
struct libusb_config_descriptor *config;
const struct libusb_endpoint_descriptor *eps;
libusb_device *dev = libusb_get_device(h);
libusb_get_configuration(h, &c);
if (c == 0) {
printf("Device is unconfigured\n");
r = libusb_set_configuration(h, 1);
if (r == 0) {
printf("Successfully configured device for configuration 1\n");
} else {
MSP_libusb_error(r);
}
}
libusb_get_active_config_descriptor(dev, &config);
for (j = 0; j < config->bNumInterfaces; j++) {
const struct libusb_interface *intf = &config->interface[j];
for (k = 0; k < intf->num_altsetting; k++) {
const struct libusb_interface_descriptor *intf_desc;
intf_desc = &intf->altsetting[k];
if (intf_desc->bInterfaceClass == LIBUSB_CLASS_COMM) {
// This is the interface we are interested in
eps = intf_desc->endpoint;
for (m = 0; m < intf_desc->bNumEndpoints; m++) {
if ((eps[m].bmAttributes & LIBUSB_TRANSFER_TYPE_MASK) == LIBUSB_TRANSFER_TYPE_INTERRUPT) {
if ((eps[m].bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK) == LIBUSB_ENDPOINT_IN) {
printf("Found INTERRUPT IN endpoint (0x%x)\n",eps[m].bEndpointAddress);
*int_in = eps[m].bEndpointAddress;
}
} else if (eps[m].bmAttributes & LIBUSB_TRANSFER_TYPE_BULK) {
if ((eps[m].bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK) == LIBUSB_ENDPOINT_IN) {
printf("Found BULK IN endpoint (0x%x)\n",eps[m].bEndpointAddress);
*bulk_in = eps[m].bEndpointAddress;
} else if ((eps[m].bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK) == LIBUSB_ENDPOINT_OUT) {
printf("Found BULK OUT endpoint (0x%x)\n",eps[m].bEndpointAddress);
*bulk_out = eps[m].bEndpointAddress;
}
}
}
}
}
}
return 0;
}
irecv_error_t irecv_set_configuration(irecv_client_t client, int configuration) {
if (client == NULL || client->handle == NULL) {
return IRECV_E_NO_DEVICE;
}
debug("Setting to configuration %d\n", configuration);
int current = 0;
libusb_get_configuration(client->handle, ¤t);
if (current != configuration) {
if (libusb_set_configuration(client->handle, configuration) < 0) {
return IRECV_E_USB_CONFIGURATION;
}
}
client->config = configuration;
return IRECV_E_SUCCESS;
}
irecv_error_t irecv_set_configuration(irecv_client_t client, int configuration) {
if (check_context(client) != IRECV_E_SUCCESS) return IRECV_E_NO_DEVICE;
#ifndef WIN32
debug("Setting to configuration %d\n", configuration);
int current = 0;
libusb_get_configuration(client->handle, ¤t);
if (current != configuration) {
if (libusb_set_configuration(client->handle, configuration) < 0) {
return IRECV_E_USB_CONFIGURATION;
}
}
client->config = configuration;
#endif
return IRECV_E_SUCCESS;
}
UsbControl::ResultCode UsbControl::setConfiguration()
{
UsbControl::ResultCode code = Success;
int desired_config_id = 1;
int current_config_id = -1;
int r;
r = libusb_get_configuration(handle_, ¤t_config_id);
CHECK_LIBUSB_RESULT(code, r) << "failed to get configuration! " << WRITE_LIBUSB_ERROR(r);
if(code == Success)
{
if(current_config_id != desired_config_id)
{
r = libusb_set_configuration(handle_, desired_config_id);
CHECK_LIBUSB_RESULT(code, r) << "failed to set configuration! " << WRITE_LIBUSB_ERROR(r);
}
}
return code;
}
struct hwstub_device_t *hwstub_open(libusb_device_handle *handle)
{
struct hwstub_device_t *dev = malloc(sizeof(struct hwstub_device_t));
memset(dev, 0, sizeof(struct hwstub_device_t));
dev->handle = handle;
libusb_device *mydev = libusb_get_device(dev->handle);
int config_id;
libusb_get_configuration(dev->handle, &config_id);
struct libusb_device_descriptor dev_desc;
libusb_get_device_descriptor(mydev, &dev_desc);
if(dev_desc.bDeviceClass != HWSTUB_CLASS ||
dev_desc.bDeviceSubClass != HWSTUB_SUBCLASS ||
dev_desc.bDeviceProtocol != HWSTUB_PROTOCOL)
goto Lerr;
return dev;
Lerr:
free(dev);
return NULL;
}
static bool configure_tir(int config)
{
int cfg = -1;
ltr_int_log_message("Requesting TrackIR configuration.\n");
if(libusb_get_configuration(handle, &cfg)){
ltr_int_log_message("Can't get device configuration!\n");
return false;
}
ltr_int_log_message("TrackIR configuration received.\n");
if(cfg != config){
ltr_int_log_message("Trying to set TrackIR configuration.\n");
if(libusb_set_configuration(handle, config)){
ltr_int_log_message("Can't set device configuration!\n");
return false;
}
ltr_int_log_message("TrackIR configured.\n");
}else{
ltr_int_log_message("TrackIR already in requested configuration.\n");
}
return true;
}
static void describe_handle(HANDLE h) {
int c;
int j, k, m;
struct libusb_config_descriptor *config;
const struct libusb_endpoint_descriptor *eps;
libusb_device *dev = libusb_get_device(h);
libusb_get_configuration(h, &c);
if (c == 0) {
printf("Device is unconfigured\n");
} else {
// We expect the configuration to be 1
printf("Currently active configuration: %d\n",c);
}
libusb_get_active_config_descriptor(dev, &config);
for (j = 0; j < config->bNumInterfaces; j++) {
printf("Looking at interface #%d\n",j);
const struct libusb_interface *intf = &config->interface[j];
for (k = 0; k < intf->num_altsetting; k++) {
printf("Looking at interface #%d, altsetting #%d\n",j,k);
const struct libusb_interface_descriptor *intf_desc;
intf_desc = &intf->altsetting[k];
printf("Interface class: %d\n", intf_desc->bInterfaceClass);
if (intf_desc->bInterfaceClass == LIBUSB_CLASS_COMM) {
// This is the interface we are interested in
printf(" Communications Device Class\n");
// Enumerate endpoints
eps = intf_desc->endpoint;
for (m = 0; m < intf_desc->bNumEndpoints; m++) {
printf("Endpoint: %d, address: %d\n",m, eps[m].bEndpointAddress);
printf(" polling interval: %d\n", eps[m].bInterval);
}
} else if (intf_desc->bInterfaceClass == LIBUSB_CLASS_HID) {
printf(" Human Interface Device Class\n");
}
}
}
}
void
irpc_send_usb_get_configuration(struct irpc_connection_info *ci)
{
tpl_node *tn = NULL;
irpc_retval_t retval = IRPC_SUCCESS;
irpc_device_handle handle;
int sock = ci->client_sock;
int config;
// Read irpc_device_handle and config from client.
tn = tpl_map(IRPC_DEV_HANDLE_INT_FMT, &handle, &config);
tpl_load(tn, TPL_FD, sock);
tpl_unpack(tn, 0);
tpl_free(tn);
if (libusb_get_configuration(irpc_handle, &config) != 0)
retval = IRPC_FAILURE;
// Send libusb_get_configuration packet.
tn = tpl_map(IRPC_INT_FMT, &retval);
tpl_pack(tn, 0);
tpl_dump(tn, TPL_FD, sock);
tpl_free(tn);
}
int Protonect::openKinect(std::string binpath){
if( bOpened ){
closeKinect();
}
cmd_seq = 0;
uint16_t vid = 0x045E;
uint16_t pid[2] = {0x02d8, 0x02C4};
uint16_t mi = 0x00;
bool debug_mode = false;
uint8_t bus;
int r;
const struct libusb_version* version;
version = libusb_get_version();
printf("Using libusbx v%d.%d.%d.%d\n\n", version->major, version->minor, version->micro, version->nano);
r = libusb_init(NULL);
if (r < 0) return r;
libusb_set_debug(NULL, debug_mode ? LIBUSB_LOG_LEVEL_DEBUG : LIBUSB_LOG_LEVEL_INFO);
for(int i = 0; i < 2; i++){
printf("Trying to open device %04X:%04X...\n", vid, pid[i]);
handle = NULL;
int tryCount = 4;
if (handle == NULL){
while(tryCount > 0 && handle == NULL){
handle = libusb_open_device_with_vid_pid(NULL, vid, pid[i]);
tryCount--;
usleep(100);
if( handle ){
libusb_reset_device(handle);
usleep(100);
handle = libusb_open_device_with_vid_pid(NULL, vid, pid[i]);
}
}
}
if(handle != NULL){
break;
}
}
if( handle == NULL ){
perr("Protonect::openKinect Failed. - handle is NULL\n");
return -1;
}
dev = libusb_get_device(handle);
bus = libusb_get_bus_number(dev);
/*
struct libusb_device_descriptor dev_desc;
printf("\nReading device descriptor:\n");
CALL_CHECK(libusb_get_device_descriptor(dev, &dev_desc));
printf(" length: %d\n", dev_desc.bLength);
printf(" device class: %d\n", dev_desc.bDeviceClass);
printf(" S/N: %d\n", dev_desc.iSerialNumber);
printf(" VID:PID: %04X:%04X\n", dev_desc.idVendor, dev_desc.idProduct);
printf(" bcdDevice: %04X\n", dev_desc.bcdDevice);
printf(" iMan:iProd:iSer: %d:%d:%d\n", dev_desc.iManufacturer, dev_desc.iProduct, dev_desc.iSerialNumber);
printf(" nb confs: %d\n", dev_desc.bNumConfigurations);
*/
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
int active_cfg = -5;
r = libusb_get_configuration(handle, &active_cfg);
printf("active configuration: %d, err: %d", active_cfg, r);
int configId = 1;
if (active_cfg != configId)
{
printf("Setting config: %d\n", configId);
r = libusb_set_configuration(handle, configId);
if (r != LIBUSB_SUCCESS)
{
perr(" Can't set configuration. Error code: %d (%s)\n", r, libusb_error_name(r));
}
}
int iface = 0;
printf("\nClaiming interface %d...\n", iface);
r = libusb_claim_interface(handle, iface);
if (r != LIBUSB_SUCCESS)
{
perr(" Failed: %d.\n", r);
}
iface = 1;
printf("\nClaiming interface %d...\n", iface);
r = libusb_claim_interface(handle, iface);
if (r != LIBUSB_SUCCESS)
{
perr(" Failed: %d.\n", r);
}
InitKinect(handle);
// install signal handler now
signal(SIGINT,sigint_handler);
shutdown = false;
usb_loop.start();
//INITIALIZE OBJECTS
// frame_listener = new libfreenect2::FrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
rgb_bulk_transfers = new libfreenect2::usb::BulkTransferPool(handle, 0x83);
//rgb_processor = new libfreenect2::ofRGBPacketProcessor();
// rgb_packet_stream_parser = new libfreenect2::RgbPacketStreamParser(rgb_processor);
// rgb_processor->setFrameListener(frame_listener);
rgb_bulk_transfers->allocate(50, 0x4000);
rgb_bulk_transfers->setCallback(rgb_packet_stream_parser);
rgb_bulk_transfers->enableSubmission();
rgb_bulk_transfersPtr = rgb_bulk_transfers;
depth_iso_transfers = new libfreenect2::usb::IsoTransferPool(handle, 0x84);
depth_processor = new libfreenect2::CpuDepthPacketProcessor();
depth_processor->setFrameListener(frame_listener);
depth_processor->load11To16LutFromFile((binpath + "11to16.bin").c_str());
depth_processor->loadXTableFromFile((binpath + "xTable.bin").c_str());
depth_processor->loadZTableFromFile((binpath + "zTable.bin").c_str());
depth_packet_stream_parser = new libfreenect2::DepthPacketStreamParser(depth_processor);
size_t max_packet_size = libusb_get_max_iso_packet_size(dev, 0x84);
std::cout << "iso max_packet_size: " << max_packet_size << std::endl;
depth_iso_transfers = new libfreenect2::usb::IsoTransferPool(handle, 0x84);
depth_iso_transfers->allocate(80, 8, max_packet_size);
depth_iso_transfers->setCallback(depth_packet_stream_parser);
depth_iso_transfers->enableSubmission();
depth_iso_transfersPtr = depth_iso_transfers;
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
RunKinect(handle, *depth_processor);
rgb_bulk_transfers->submit(10);
depth_iso_transfers->submit(60);
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
bOpened = true;
return 0;
}
// Find the device, and set up to read it periodically, then send
// the data out stdout so another process can pick it up and do
// something reasonable with it.
//
int main(int argc, char **argv)
{
char *usage = {"usage: %s -u -n\n"};
int libusbDebug = 0; //This will turn on the DEBUG for libusb
int noisy = 0; //This will print the packets as they come in
libusb_device **devs;
int r, err, c;
ssize_t cnt;
while ((c = getopt (argc, argv, "unh")) != -1)
switch (c){
case 'u':
libusbDebug = 1;
break;
case 'n':
noisy = 1;
break;
case 'h':
fprintf(stderr, usage, argv[0]);
case '?':
exit(1);
default:
exit(1);
}
fprintf(stderr,"%s Starting ... ",argv[0]);
fprintf(stderr,"libusbDebug = %d, noisy = %d\n", libusbDebug, noisy);
// The Pi linker can give you fits. If you get a linker error on
// the next line, set LD_LIBRARY_PATH to /usr/local/lib
fprintf(stderr,"This is not an error!! Checking linker, %s\n", libusb_strerror(0));
if (signal(SIGINT, sig_handler) == SIG_ERR)
fprintf(stderr,"Couldn't set up signal handler\n");
err = libusb_init(NULL);
if (err < 0){
fprintf(stderr,"Couldn't init usblib, %s\n", libusb_strerror(err));
exit(1);
}
// This is where you can get debug output from libusb.
// just set it to LIBUSB_LOG_LEVEL_DEBUG
if (libusbDebug)
libusb_set_debug(NULL, LIBUSB_LOG_LEVEL_DEBUG);
else
libusb_set_debug(NULL, LIBUSB_LOG_LEVEL_INFO);
cnt = libusb_get_device_list(NULL, &devs);
if (cnt < 0){
fprintf(stderr,"Couldn't get device list, %s\n", libusb_strerror(err));
exit(1);
}
// go get the device; the device handle is saved in weatherStation struct.
if (!findDevice(devs)){
fprintf(stderr,"Couldn't find the device\n");
exit(1);
}
// Now I've found the weather station and can start to try stuff
// So, I'll get the device descriptor
struct libusb_device_descriptor deviceDesc;
err = libusb_get_device_descriptor(weatherStation.device, &deviceDesc);
if (err){
fprintf(stderr,"Couldn't get device descriptor, %s\n", libusb_strerror(err));
exit(1);
}
fprintf(stderr,"got the device descriptor back\n");
// Open the device and save the handle in the weatherStation struct
err = libusb_open(weatherStation.device, &weatherStation.handle);
if (err){
fprintf(stderr,"Open failed, %s\n", libusb_strerror(err));
closeUpAndLeave(); }
fprintf(stderr,"I was able to open it\n");
// Now that it's opened, I can free the list of all devices
libusb_free_device_list(devs, 1); // Documentation says to get rid of the list
// Once I have the device I need
fprintf(stderr,"Released the device list\n");
err = libusb_set_auto_detach_kernel_driver(weatherStation.handle, 1);
if(err){
fprintf(stderr,"Some problem with detach %s\n", libusb_strerror(err));
closeUpAndLeave();
}
int activeConfig;
err =libusb_get_configuration(weatherStation.handle, &activeConfig);
if (err){
fprintf(stderr,"Can't get current active configuration, %s\n", libusb_strerror(err));;
closeUpAndLeave();
}
fprintf(stderr,"Currently active configuration is %d\n", activeConfig);
if(activeConfig != 1){
err = libusb_set_configuration (weatherStation.handle, 1);
if (err){
fprintf(stderr,"Cannot set configuration, %s\n", libusb_strerror(err));;
closeUpAndLeave();
}
fprintf(stderr,"Just did the set configuration\n");
}
err = libusb_claim_interface(weatherStation.handle, 0); //claim interface 0 (the first) of device (mine had just 1)
if(err) {
fprintf(stderr,"Cannot claim interface, %s\n", libusb_strerror(err));
closeUpAndLeave();
}
fprintf(stderr,"Claimed Interface\n");
// I don't want to just hang up and read the reports as fast as I can, so
// I'll space them out a bit. It's weather, and it doesn't change very fast.
sleep(1);
fprintf(stderr,"Setting the device 'idle' before starting reads\n");
err = libusb_control_transfer(weatherStation.handle,
0x21, 0x0a, 0, 0, 0, 0, 1000);
sleep(1);
int tickcounter= 0;
fprintf(stderr,"Starting reads\n");
while(1){
sleep(1);
if(tickcounter++ % 10 == 0){
getit(1, noisy);
}
if(tickcounter % 30 == 0){
getit(2, noisy);
}
if (tickcounter % 15 == 0){
showit();
}
}
}
int usb_init(int usb_number, e_controller_type type)
{
int ret = -1;
int dev_i;
struct usb_state* state = usb_states+usb_number;
if(!controller[type].vendor || !controller[type].product)
{
gprintf(_("no pass-through device is needed\n"));
return 0;
}
usb_state_indexes[usb_number] = usb_number;
memset(state, 0x00, sizeof(*state));
state->joystick_id = -1;
state->type = type;
state->ack = 1;
if(!ctx)
{
ret = libusb_init(&ctx);
if(ret != LIBUSB_SUCCESS)
{
fprintf(stderr, "libusb_init: %s.\n", libusb_strerror(ret));
return -1;
}
}
if(!devs)
{
cnt = libusb_get_device_list(ctx, &devs);
if(cnt < 0)
{
fprintf(stderr, "libusb_get_device_list: %s.\n", libusb_strerror(cnt));
return -1;
}
}
for(dev_i=0; dev_i<cnt; ++dev_i)
{
struct libusb_device_descriptor desc;
ret = libusb_get_device_descriptor(devs[dev_i], &desc);
if(!ret)
{
if(desc.idVendor == controller[type].vendor && desc.idProduct == controller[type].product)
{
libusb_device_handle* devh;
ret = libusb_open(devs[dev_i], &devh);
if(ret != LIBUSB_SUCCESS)
{
fprintf(stderr, "libusb_open: %s.\n", libusb_strerror(ret));
return -1;
}
else
{
ret = libusb_reset_device(devh);
if(ret != LIBUSB_SUCCESS)
{
fprintf(stderr, "libusb_reset_device: %s.\n", libusb_strerror(ret));
libusb_close(devh);
return -1;
}
#if defined(LIBUSB_API_VERSION) || defined(LIBUSBX_API_VERSION)
libusb_set_auto_detach_kernel_driver(devh, 1);
#else
#ifndef WIN32
ret = libusb_kernel_driver_active(devh, 0);
if(ret == 1)
{
ret = libusb_detach_kernel_driver(devh, 0);
if(ret != LIBUSB_SUCCESS)
{
fprintf(stderr, "libusb_detach_kernel_driver: %s.\n", libusb_strerror(ret));
libusb_close(devh);
return -1;
}
}
else if(ret != LIBUSB_SUCCESS)
{
fprintf(stderr, "libusb_kernel_driver_active: %s.\n", libusb_strerror(ret));
libusb_close(devh);
return -1;
}
#endif
#endif
int config;
ret = libusb_get_configuration(devh, &config);
if(ret != LIBUSB_SUCCESS)
{
fprintf(stderr, "libusb_get_configuration: %s.\n", libusb_strerror(ret));
libusb_close(devh);
return -1;
}
if(config != controller[state->type].configuration)
{
ret = libusb_set_configuration(devh, controller[state->type].configuration);
if(ret != LIBUSB_SUCCESS)
{
fprintf(stderr, "libusb_set_configuration: %s.\n", libusb_strerror(ret));
libusb_close(devh);
return -1;
}
}
ret = libusb_claim_interface(devh, controller[state->type].interface);
if(ret != LIBUSB_SUCCESS)
{
fprintf(stderr, "libusb_claim_interface: %s.\n", libusb_strerror(ret));
libusb_close(devh);
}
else
{
state->devh = devh;
++nb_opened;
#ifndef WIN32
const struct libusb_pollfd** pfd_usb = libusb_get_pollfds(ctx);
int poll_i;
for (poll_i=0; pfd_usb[poll_i] != NULL; ++poll_i)
{
GE_AddSource(pfd_usb[poll_i]->fd, usb_number, usb_handle_events, usb_handle_events, usb_close);
}
free(pfd_usb);
#endif
if(state->type == C_TYPE_XONE_PAD && adapter_get(usb_number)->status)
{
//if the authentication was already performed, activate the controller
//warning: make sure not to make any libusb async io before this!
unsigned char activate[] = { 0x05, 0x20, 0x00, 0x01, 0x00 };
usb_send_interrupt_out_sync(usb_number, activate, sizeof(activate));
unsigned char activate_rumble[] = { 0x09, 0x00, 0x00, 0x09, 0x00, 0x0f, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x00 };
usb_send_interrupt_out_sync(usb_number, activate_rumble, sizeof(activate_rumble));
}
// register joystick
state->joystick_id = GE_RegisterJoystick(controller[state->type].name, NULL);
int i;
for(i = 0; i < controller[state->type].endpoints.in.reports.nb; ++i)
{
usb_states[usb_number].reports[i].report_id = controller[state->type].endpoints.in.reports.elements[i].report_id;
}
return 0;
}
}
}
}
}
return -1;
}
/**
* Open the device and init it
* You should call b96_init_usb() first before using this function
*
* @return the handler of the device
*/
libusb_device_handle* b96_init_device(void) {
libusb_device_handle *handler=NULL;
int r1;
int c1;
struct libusb_device_descriptor desc;
int i;
handler = libusb_open_device_with_vid_pid(libusb_ctx, 0x10d6, 0x10d6);
if (handler == NULL) {
error_at_line(0,0,__FILE__,__LINE__,"Error: cannot open device 10d6:10d6");
return handler;
}
r1 = libusb_get_device_descriptor(libusb_get_device(handler), &desc);
if (r1 != 0) {
error_at_line(0,0,__FILE__,__LINE__,"Error: cannot get device descriptor");
}
printf ("bDescriptorType: %d\n", desc.bDescriptorType);
printf ("bNumConfigurations: %d\n", desc.bNumConfigurations);
printf ("iManufacturer: %d\n", desc.iManufacturer);
for (i=1; i<=desc.bNumConfigurations; i++) {
struct libusb_config_descriptor *config=NULL;
int r2=0;
int j;
r2 = libusb_get_config_descriptor_by_value(libusb_get_device(handler), i, &config);
if (r2 != 0) {
error_at_line(0,0,__FILE__,__LINE__,"Error: cannot get configuration %d descriptor", i);
continue;
}
printf ("bNumInterfaces: %d\n", config->bNumInterfaces);
for (j=0; j<config->bNumInterfaces; j++) {
}
if (config != NULL) {
libusb_free_config_descriptor(config);
config=NULL;
}
}
r1 = libusb_detach_kernel_driver(handler, 0);
if (r1 != 0) {
const char *errorStr="unknown";
if (r1 == LIBUSB_ERROR_NOT_FOUND) {
errorStr = "LIBUSB_ERROR_NOT_FOUND (no worry)";
} else if (r1 == LIBUSB_ERROR_INVALID_PARAM) {
errorStr = "LIBUSB_ERROR_INVALID_PARAM";
} else if (r1 == LIBUSB_ERROR_NO_DEVICE) {
errorStr = "LIBUSB_ERROR_NO_DEVICE";
} else if (r1 == LIBUSB_ERROR_NOT_SUPPORTED) {
errorStr = "LIBUSB_ERROR_NOT_SUPPORTED";
}
error_at_line(0,0,__FILE__,__LINE__,"Info: cannot detach kernel driver: %s", errorStr);
}
c1 = 0;
r1 = libusb_get_configuration(handler, &c1);
if (r1 != 0) {
error_at_line(0,0,__FILE__,__LINE__,"Error: cannot get device configuration");
}
printf ("Configuiration: %d\n",c1);
r1 = libusb_set_configuration(handler, c1);
if (r1 != 0) {
error_at_line(0,0,__FILE__,__LINE__,"Error: cannot set device configuration");
}
r1 = libusb_claim_interface(handler, 0);
if (r1 != 0) {
error_at_line(0,0,__FILE__,__LINE__,"Error: cannot claim device interface");
}
return handler;
}
int cyusb_get_configuration(cyusb_handle *h, int *config)
{
return ( libusb_get_configuration(h, config) );
}
//**********************************************************
//!
//! Open connection to device enumerated by bdm_usb_find_devices()
//!
//! @param device_no Device number to open
//!
//! @return \n
//! == BDM_RC_OK (0) => Success\n
//! == BDM_RC_USB_ERROR => USB failure
//!
USBDM_ErrorCode bdm_usb_open( unsigned int device_no ) {
// print("bdm_usb_open( %d )\n", device_no);
if (!initialised) {
print("bdm_usb_open() - Not initialised! \n");
return PROGRAMMING_RC_ERROR_INTERNAL_CHECK_FAILED;
}
if (device_no >= deviceCount) {
print("bdm_usb_open() - Illegal device #\n");
return BDM_RC_ILLEGAL_PARAMS;
}
if (usbDeviceHandle != NULL) {
print("bdm_usb_open() - Closing previous device\n");
bdm_usb_close();
}
int rc = libusb_open(bdmDevices[device_no], &usbDeviceHandle);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_open() - libusb_open() failed, rc = %s\n", libusb_strerror((libusb_error)rc));
usbDeviceHandle = NULL;
return BDM_RC_DEVICE_OPEN_FAILED;
}
int configuration = 0;
rc = libusb_get_configuration(usbDeviceHandle, &configuration);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_open() - libusb_get_configuration() failed, rc = %s\n", libusb_strerror((libusb_error)rc));
}
if (configuration != 1) {
rc = libusb_set_configuration(usbDeviceHandle, 1);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_open() - libusb_set_configuration(1) failed, rc = %s\n", libusb_strerror((libusb_error)rc));
// Release the device
libusb_close(usbDeviceHandle);
usbDeviceHandle = NULL;
return BDM_RC_DEVICE_OPEN_FAILED;
}
}
rc = libusb_claim_interface(usbDeviceHandle, 0);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_open() - libusb_claim_interface(0) failed, rc = %s\n", libusb_strerror((libusb_error)rc));
// Release the device
libusb_set_configuration(usbDeviceHandle, 0);
libusb_close(usbDeviceHandle);
usbDeviceHandle = NULL;
return BDM_RC_DEVICE_OPEN_FAILED;
}
rc = libusb_clear_halt(usbDeviceHandle, 0x01);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_open() - libusb_clear_halt(...,0x01) failed, rc = %s\n", libusb_strerror((libusb_error)rc));
}
rc = libusb_clear_halt(usbDeviceHandle, 0x82);
if (rc != LIBUSB_SUCCESS) {
print("bdm_usb_open() - libusb_clear_halt(...,0x82) failed, rc = %s\n", libusb_strerror((libusb_error)rc));
}
// libusb_set_debug(NULL, 4);
// int maxTransferSize = libusb_get_max_packet_size(libusb_get_device(usbDeviceHandle), EP_OUT);
// if (maxTransferSize<0) {
// print("bdm_usb_open() - libusb_get_max_packet_size(EP_OUT) failed, rc = %d\n", maxTransferSize);
// }
// else {
// print("bdm_usb_open() - libusb_get_max_packet_size(EP_OUT) => %d\n", maxTransferSize);
// }
//
// maxTransferSize = libusb_get_max_packet_size(libusb_get_device(usbDeviceHandle), EP_IN);
// if (maxTransferSize<0) {
// print("bdm_usb_open() - libusb_get_max_packet_size(EP_IN) failed, rc = %d\n", maxTransferSize);
// }
// else {
// print("bdm_usb_open() - libusb_get_max_packet_size(EP_IN) => %d\n", maxTransferSize);
// }
return (BDM_RC_OK);
}
/* Helper to open a libusb device that matches vid, pid, product string and/or serial string.
* Set any field to 0 as a wildcard. If the device is found true is returned, with ctx containing
* the already opened handle. ctx->interface must be set to the desired interface (channel) number
* prior to calling this function. */
static bool open_matching_device(struct mpsse_ctx *ctx, const uint16_t *vid, const uint16_t *pid,
const char *product, const char *serial, const char *location)
{
libusb_device **list;
struct libusb_device_descriptor desc;
struct libusb_config_descriptor *config0;
int err;
bool found = false;
ssize_t cnt = libusb_get_device_list(ctx->usb_ctx, &list);
if (cnt < 0)
LOG_ERROR("libusb_get_device_list() failed with %s", libusb_error_name(cnt));
for (ssize_t i = 0; i < cnt; i++) {
libusb_device *device = list[i];
err = libusb_get_device_descriptor(device, &desc);
if (err != LIBUSB_SUCCESS) {
LOG_ERROR("libusb_get_device_descriptor() failed with %s", libusb_error_name(err));
continue;
}
if (vid && *vid != desc.idVendor)
continue;
if (pid && *pid != desc.idProduct)
continue;
err = libusb_open(device, &ctx->usb_dev);
if (err != LIBUSB_SUCCESS) {
LOG_ERROR("libusb_open() failed with %s",
libusb_error_name(err));
continue;
}
if (location && !device_location_equal(device, location)) {
libusb_close(ctx->usb_dev);
continue;
}
if (product && !string_descriptor_equal(ctx->usb_dev, desc.iProduct, product)) {
libusb_close(ctx->usb_dev);
continue;
}
if (serial && !string_descriptor_equal(ctx->usb_dev, desc.iSerialNumber, serial)) {
libusb_close(ctx->usb_dev);
continue;
}
found = true;
break;
}
libusb_free_device_list(list, 1);
if (!found) {
LOG_ERROR("no device found");
return false;
}
err = libusb_get_config_descriptor(libusb_get_device(ctx->usb_dev), 0, &config0);
if (err != LIBUSB_SUCCESS) {
LOG_ERROR("libusb_get_config_descriptor() failed with %s", libusb_error_name(err));
libusb_close(ctx->usb_dev);
return false;
}
/* Make sure the first configuration is selected */
int cfg;
err = libusb_get_configuration(ctx->usb_dev, &cfg);
if (err != LIBUSB_SUCCESS) {
LOG_ERROR("libusb_get_configuration() failed with %s", libusb_error_name(err));
goto error;
}
if (desc.bNumConfigurations > 0 && cfg != config0->bConfigurationValue) {
err = libusb_set_configuration(ctx->usb_dev, config0->bConfigurationValue);
if (err != LIBUSB_SUCCESS) {
LOG_ERROR("libusb_set_configuration() failed with %s", libusb_error_name(err));
goto error;
}
}
/* Try to detach ftdi_sio kernel module */
err = libusb_detach_kernel_driver(ctx->usb_dev, ctx->interface);
if (err != LIBUSB_SUCCESS && err != LIBUSB_ERROR_NOT_FOUND
&& err != LIBUSB_ERROR_NOT_SUPPORTED) {
LOG_ERROR("libusb_detach_kernel_driver() failed with %s", libusb_error_name(err));
goto error;
}
err = libusb_claim_interface(ctx->usb_dev, ctx->interface);
if (err != LIBUSB_SUCCESS) {
LOG_ERROR("libusb_claim_interface() failed with %s", libusb_error_name(err));
goto error;
}
/* Reset FTDI device */
err = libusb_control_transfer(ctx->usb_dev, FTDI_DEVICE_OUT_REQTYPE,
SIO_RESET_REQUEST, SIO_RESET_SIO,
ctx->index, NULL, 0, ctx->usb_write_timeout);
if (err < 0) {
LOG_ERROR("failed to reset FTDI device: %s", libusb_error_name(err));
goto error;
}
switch (desc.bcdDevice) {
case 0x500:
ctx->type = TYPE_FT2232C;
break;
case 0x700:
ctx->type = TYPE_FT2232H;
break;
case 0x800:
ctx->type = TYPE_FT4232H;
break;
case 0x900:
ctx->type = TYPE_FT232H;
break;
default:
LOG_ERROR("unsupported FTDI chip type: 0x%04x", desc.bcdDevice);
goto error;
}
/* Determine maximum packet size and endpoint addresses */
if (!(desc.bNumConfigurations > 0 && ctx->interface < config0->bNumInterfaces
&& config0->interface[ctx->interface].num_altsetting > 0))
goto desc_error;
const struct libusb_interface_descriptor *descriptor;
descriptor = &config0->interface[ctx->interface].altsetting[0];
if (descriptor->bNumEndpoints != 2)
goto desc_error;
ctx->in_ep = 0;
ctx->out_ep = 0;
for (int i = 0; i < descriptor->bNumEndpoints; i++) {
if (descriptor->endpoint[i].bEndpointAddress & 0x80) {
ctx->in_ep = descriptor->endpoint[i].bEndpointAddress;
ctx->max_packet_size =
descriptor->endpoint[i].wMaxPacketSize;
} else {
ctx->out_ep = descriptor->endpoint[i].bEndpointAddress;
}
}
if (ctx->in_ep == 0 || ctx->out_ep == 0)
goto desc_error;
libusb_free_config_descriptor(config0);
return true;
desc_error:
LOG_ERROR("unrecognized USB device descriptor");
error:
libusb_free_config_descriptor(config0);
libusb_close(ctx->usb_dev);
return false;
}
int main(int argc, char *argv[])
{
std::string program_path(argv[0]);
size_t executable_name_idx = program_path.rfind("Protonect");
std::string binpath = "/";
if(executable_name_idx != std::string::npos)
{
binpath = program_path.substr(0, executable_name_idx);
}
uint16_t vid = 0x045E;
uint16_t pid = 0x02C4;
uint16_t mi = 0x00;
bool debug_mode = false;
libusb_device_handle *handle;
libusb_device *dev;
uint8_t bus;
const char* speed_name[5] =
{ "Unknown", "1.5 Mbit/s (USB LowSpeed)", "12 Mbit/s (USB FullSpeed)", "480 Mbit/s (USB HighSpeed)", "5000 Mbit/s (USB SuperSpeed)" };
int r;
const struct libusb_version* version;
version = libusb_get_version();
printf("Using libusbx v%d.%d.%d.%d\n\n", version->major, version->minor, version->micro, version->nano);
r = libusb_init(NULL);
if (r < 0)
return r;
libusb_set_debug(NULL, debug_mode ? LIBUSB_LOG_LEVEL_DEBUG : LIBUSB_LOG_LEVEL_INFO);
printf("Opening device %04X:%04X...\n", vid, pid);
handle = libusb_open_device_with_vid_pid(NULL, vid, pid);
if (handle == NULL)
{
perr(" Failed.\n");
//system("PAUSE");
return -1;
}
dev = libusb_get_device(handle);
bus = libusb_get_bus_number(dev);
/*
struct libusb_device_descriptor dev_desc;
printf("\nReading device descriptor:\n");
CALL_CHECK(libusb_get_device_descriptor(dev, &dev_desc));
printf(" length: %d\n", dev_desc.bLength);
printf(" device class: %d\n", dev_desc.bDeviceClass);
printf(" S/N: %d\n", dev_desc.iSerialNumber);
printf(" VID:PID: %04X:%04X\n", dev_desc.idVendor, dev_desc.idProduct);
printf(" bcdDevice: %04X\n", dev_desc.bcdDevice);
printf(" iMan:iProd:iSer: %d:%d:%d\n", dev_desc.iManufacturer, dev_desc.iProduct, dev_desc.iSerialNumber);
printf(" nb confs: %d\n", dev_desc.bNumConfigurations);
*/
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
int active_cfg = -5;
r = libusb_get_configuration(handle, &active_cfg);
printf("active configuration: %d, err: %d", active_cfg, r);
int configId = 1;
if (active_cfg != configId)
{
printf("Setting config: %d\n", configId);
r = libusb_set_configuration(handle, configId);
if (r != LIBUSB_SUCCESS)
{
perr(" Can't set configuration. Error code: %d (%s)\n", r, libusb_error_name(r));
}
}
int iface = 0;
printf("\nClaiming interface %d...\n", iface);
r = libusb_claim_interface(handle, iface);
if (r != LIBUSB_SUCCESS)
{
perr(" Failed: %d.\n", r);
}
iface = 1;
printf("\nClaiming interface %d...\n", iface);
r = libusb_claim_interface(handle, iface);
if (r != LIBUSB_SUCCESS)
{
perr(" Failed: %d.\n", r);
}
InitKinect(handle);
// install signal handler now
signal(SIGINT,sigint_handler);
shutdown = false;
libfreenect2::usb::EventLoop usb_loop;
usb_loop.start();
libfreenect2::FrameMap frames;
libfreenect2::FrameListener frame_listener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
//libfreenect2::DumpRgbPacketProcessor rgb_processor;
libfreenect2::TurboJpegRgbPacketProcessor rgb_processor;
rgb_processor.setFrameListener(&frame_listener);
libfreenect2::RgbPacketStreamParser rgb_packet_stream_parser(&rgb_processor);
libfreenect2::usb::BulkTransferPool rgb_bulk_transfers(handle, 0x83);
rgb_bulk_transfers.allocate(50, 0x4000);
rgb_bulk_transfers.setCallback(&rgb_packet_stream_parser);
rgb_bulk_transfers.enableSubmission();
libfreenect2::CpuDepthPacketProcessor depth_processor;
depth_processor.setFrameListener(&frame_listener);
depth_processor.load11To16LutFromFile((binpath + "../11to16.bin").c_str());
depth_processor.loadXTableFromFile((binpath + "../xTable.bin").c_str());
depth_processor.loadZTableFromFile((binpath + "../zTable.bin").c_str());
libfreenect2::DepthPacketStreamParser depth_packet_stream_parser(&depth_processor);
size_t max_packet_size = libusb_get_max_iso_packet_size(dev, 0x84);
std::cout << "iso max_packet_size: " << max_packet_size << std::endl;
libfreenect2::usb::IsoTransferPool depth_iso_transfers(handle, 0x84);
depth_iso_transfers.allocate(80, 8, max_packet_size);
depth_iso_transfers.setCallback(&depth_packet_stream_parser);
depth_iso_transfers.enableSubmission();
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
RunKinect(handle, depth_processor);
rgb_bulk_transfers.submit(10);
depth_iso_transfers.submit(60);
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
while(!shutdown)
{
frame_listener.waitForNewFrame(frames);
libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
cv::imshow("rgb", cv::Mat(rgb->height, rgb->width, CV_8UC3, rgb->data));
cv::imshow("ir", cv::Mat(ir->height, ir->width, CV_32FC1, ir->data) / 20000.0f);
cv::imshow("depth", cv::Mat(depth->height, depth->width, CV_32FC1, depth->data) / 4500.0f);
cv::waitKey(1);
frame_listener.release(frames);
}
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
rgb_bulk_transfers.disableSubmission();
depth_iso_transfers.disableSubmission();
CloseKinect(handle);
rgb_bulk_transfers.cancel();
depth_iso_transfers.cancel();
// wait for all transfers to cancel
// TODO: better implementation
libfreenect2::this_thread::sleep_for(libfreenect2::chrono::seconds(2));
rgb_bulk_transfers.deallocate();
depth_iso_transfers.deallocate();
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
iface = 0;
printf("Releasing interface %d...\n", iface);
libusb_release_interface(handle, iface);
iface = 1;
printf("Releasing interface %d...\n", iface);
libusb_release_interface(handle, iface);
printf("Closing device...\n");
libusb_close(handle);
usb_loop.stop();
libusb_exit(NULL);
//system("PAUSE");
return 0;
}
int main(int argc, char * const argv[])
{
struct sdp_dev *p_id;
struct mach_id *mach;
libusb_device **devs;
libusb_device *dev;
int r;
int err;
ssize_t cnt;
libusb_device_handle *h = NULL;
int config = 0;
int verify = 0;
struct sdp_work *curr;
struct sdp_work *cmd_head = NULL;
char const *conf;
char const *base_path = get_base_path(argv[0]);
char const *conf_path = "/etc/imx-loader.d/";
err = parse_opts(argc, argv, &conf_path, &verify, &cmd_head);
if (err < 0)
return -1;
// Get list of machines...
conf = conf_file_name("imx_usb.conf", base_path, conf_path);
if (conf == NULL)
return -1;
struct mach_id *list = parse_imx_conf(conf);
if (!list)
return -1;
r = libusb_init(NULL);
if (r < 0)
goto out;
cnt = libusb_get_device_list(NULL, &devs);
if (cnt < 0)
goto out;
// print_devs(devs);
dev = find_imx_dev(devs, &mach, list);
if (dev) {
err = libusb_open(dev, &h);
if (err)
printf("%s:Could not open device vid=0x%x pid=0x%x err=%d\n", __func__, mach->vid, mach->pid, err);
}
libusb_free_device_list(devs, 1);
if (!h)
goto out;
// Get machine specific configuration file..
conf = conf_file_name(mach->file_name, base_path, conf_path);
if (conf == NULL)
goto out;
p_id = parse_conf(conf);
if (!p_id)
goto out;
if (p_id->mode == MODE_HID)
p_id->transfer = &transfer_hid;
if (p_id->mode == MODE_BULK)
p_id->transfer = &transfer_bulk;
// USB private pointer is libusb device handle...
p_id->priv = h;
libusb_get_configuration(h, &config);
printf("%04x:%04x(%s) bConfigurationValue =%x\n",
mach->vid, mach->pid, p_id->name, config);
if (libusb_kernel_driver_active(h, 0))
libusb_detach_kernel_driver(h, 0);
err = libusb_claim_interface(h, 0);
if (err) {
printf("Claim failed\n");
goto out;
}
printf("Interface 0 claimed\n");
err = do_status(p_id);
if (err) {
printf("status failed\n");
goto out;
}
// By default, use work from config file...
curr = p_id->work;
if (cmd_head != NULL)
curr = cmd_head;
if (curr == NULL) {
printf("no job found\n");
goto out;
}
while (curr) {
if (curr->mem)
perform_mem_work(p_id, curr->mem);
// printf("jump_mode %x\n", curr->jump_mode);
if (curr->filename[0]) {
err = DoIRomDownload(p_id, curr, verify);
}
if (err) {
err = do_status(p_id);
break;
}
if (!curr->next && (!curr->plug || curr != cmd_head))
break;
err = do_status(p_id);
printf("jump_mode %x plug=%i err=%i\n", curr->jump_mode, curr->plug, err);
if (err) {
int retry;
/* Rediscovers device */
libusb_release_interface(h, 0);
libusb_close(h);
libusb_exit(NULL);
for (retry = 0; retry < 10; retry++) {
printf("sleeping\n");
sleep(3);
printf("done sleeping\n");
h = open_vid_pid(mach, p_id);
if (h)
break;
}
if (!h)
goto out;
}
if (curr == cmd_head && curr->plug) {
curr->plug = 0;
continue;
}
curr = curr->next;
}
exit:
libusb_release_interface(h, 0);
out:
if (h)
libusb_close(h);
libusb_exit(NULL);
return 0;
}
stlink_t* stlink_open_usb(const int verbose) {
stlink_t* sl = NULL;
struct stlink_libusb* slu = NULL;
int error = -1;
libusb_device** devs = NULL;
libusb_device* dev;
ssize_t i;
ssize_t count;
int config;
char *iSerial = NULL;
sl = malloc(sizeof (stlink_t));
slu = malloc(sizeof (struct stlink_libusb));
if (sl == NULL) goto on_error;
if (slu == NULL) goto on_error;
memset(sl, 0, sizeof (stlink_t));
memset(slu, 0, sizeof (struct stlink_libusb));
ugly_init(verbose);
sl->backend = &_stlink_usb_backend;
sl->backend_data = slu;
sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
/* flash memory settings */
sl->flash_base = STM32_FLASH_BASE;
sl->flash_size = STM32_FLASH_SIZE;
sl->flash_pgsz = STM32_FLASH_PGSZ;
/* system memory */
sl->sys_base = STM32_SYSTEM_BASE;
sl->sys_size = STM32_SYSTEM_SIZE;
/* sram memory settings */
sl->sram_base = STM32_SRAM_BASE;
sl->sram_size = STM32L_SRAM_SIZE;
if (libusb_init(&(slu->libusb_ctx))) {
WLOG("failed to init libusb context, wrong version of libraries?\n");
goto on_error;
}
slu->usb_handle = libusb_open_device_with_vid_pid(slu->libusb_ctx, USB_ST_VID, USB_STLINK_32L_PID);
if (slu->usb_handle == NULL) {
// TODO - free usb context too...
free(slu);
WLOG("Couldn't find any ST-Link/V2 devices");
return NULL;
}
if (libusb_kernel_driver_active(slu->usb_handle, 0) == 1) {
int r;
r = libusb_detach_kernel_driver(slu->usb_handle, 0);
if (r<0) {
WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-r));
goto on_libusb_error;
}
}
if (libusb_get_configuration(slu->usb_handle, &config)) {
/* this may fail for a previous configured device */
WLOG("libusb_get_configuration()\n");
goto on_libusb_error;
}
if (config != 1) {
printf("setting new configuration (%d -> 1)\n", config);
if (libusb_set_configuration(slu->usb_handle, 1)) {
/* this may fail for a previous configured device */
WLOG("libusb_set_configuration() failed\n");
goto on_libusb_error;
}
}
if (libusb_claim_interface(slu->usb_handle, 0)) {
WLOG("libusb_claim_interface() failed\n");
goto on_libusb_error;
}
slu->req_trans = libusb_alloc_transfer(0);
if (slu->req_trans == NULL) {
WLOG("libusb_alloc_transfer failed\n");
goto on_libusb_error;
}
slu->rep_trans = libusb_alloc_transfer(0);
if (slu->rep_trans == NULL) {
WLOG("libusb_alloc_transfer failed\n");
goto on_libusb_error;
}
// TODO - could use the scanning techniq from stm8 code here...
slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
slu->sg_transfer_idx = 0;
// TODO - never used at the moment, always CMD_SIZE
slu->cmd_len = (slu->protocoll == 1)? STLINK_SG_SIZE: STLINK_CMD_SIZE;
/* success */
if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
ILOG("-- exit_dfu_mode\n");
stlink_exit_dfu_mode(sl);
}
stlink_version(sl);
error = 0;
on_libusb_error:
if (devs != NULL) {
libusb_free_device_list(devs, 1);
}
if (error == -1) {
stlink_close(sl);
return NULL;
}
/* success */
return sl;
on_error:
if (sl != NULL) free(sl);
if (slu != NULL) free(slu);
return 0;
}
/**
* Wraps a CDB mass storage command in the appropriate gunk to get it down
* @param handle
* @param endpoint
* @param cdb
* @param cdb_length
* @param lun
* @param flags
* @param expected_rx_size
* @return
*/
int send_usb_mass_storage_command(libusb_device_handle *handle, uint8_t endpoint_out,
uint8_t *cdb, uint8_t cdb_length,
uint8_t lun, uint8_t flags, uint32_t expected_rx_size) {
DLOG("Sending usb m-s cmd: cdblen:%d, rxsize=%d\n", cdb_length, expected_rx_size);
dump_CDB_command(cdb, cdb_length);
static uint32_t tag;
if (tag == 0) {
tag = 1;
}
int try = 0;
int ret = 0;
int real_transferred;
int i = 0;
uint8_t c_buf[STLINK_SG_SIZE];
// tag is allegedly ignored... TODO - verify
c_buf[i++] = 'U';
c_buf[i++] = 'S';
c_buf[i++] = 'B';
c_buf[i++] = 'C';
write_uint32(&c_buf[i], tag);
uint32_t this_tag = tag++;
write_uint32(&c_buf[i+4], expected_rx_size);
i+= 8;
c_buf[i++] = flags;
c_buf[i++] = lun;
c_buf[i++] = cdb_length;
// Now the actual CDB request
assert(cdb_length <= CDB_SL);
memcpy(&(c_buf[i]), cdb, cdb_length);
int sending_length = STLINK_SG_SIZE;
// send....
do {
ret = libusb_bulk_transfer(handle, endpoint_out, c_buf, sending_length,
&real_transferred, SG_TIMEOUT_MSEC);
if (ret == LIBUSB_ERROR_PIPE) {
libusb_clear_halt(handle, endpoint_out);
}
try++;
} while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
if (ret != LIBUSB_SUCCESS) {
WLOG("sending failed: %d\n", ret);
return -1;
}
return this_tag;
}
/**
* Straight from stm8 stlink code...
* @param handle
* @param endpoint_in
* @param endpoint_out
*/
static void
get_sense(libusb_device_handle *handle, uint8_t endpoint_in, uint8_t endpoint_out)
{
DLOG("Fetching sense...\n");
uint8_t cdb[16];
memset(cdb, 0, sizeof(cdb));
#define REQUEST_SENSE 0x03
#define REQUEST_SENSE_LENGTH 18
cdb[0] = REQUEST_SENSE;
cdb[4] = REQUEST_SENSE_LENGTH;
uint32_t tag = send_usb_mass_storage_command(handle, endpoint_out, cdb, sizeof(cdb), 0,
LIBUSB_ENDPOINT_IN, REQUEST_SENSE_LENGTH);
if (tag == 0) {
WLOG("refusing to send request sense with tag 0\n");
return;
}
unsigned char sense[REQUEST_SENSE_LENGTH];
int transferred;
int ret;
int try = 0;
do {
ret = libusb_bulk_transfer(handle, endpoint_in, sense, sizeof(sense),
&transferred, SG_TIMEOUT_MSEC);
if (ret == LIBUSB_ERROR_PIPE) {
libusb_clear_halt(handle, endpoint_in);
}
try++;
} while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
if (ret != LIBUSB_SUCCESS) {
WLOG("receiving sense failed: %d\n", ret);
return;
}
if (transferred != sizeof(sense)) {
WLOG("received unexpected amount of sense: %d != %d\n", transferred, sizeof(sense));
}
uint32_t received_tag;
int status = get_usb_mass_storage_status(handle, endpoint_in, &received_tag);
if (status != 0) {
WLOG("receiving sense failed with status: %02x\n", status);
return;
}
if (sense[0] != 0x70 && sense[0] != 0x71) {
WLOG("No sense data\n");
} else {
WLOG("Sense KCQ: %02X %02X %02X\n", sense[2] & 0x0f, sense[12], sense[13]);
}
}
/**
* Just send a buffer on an endpoint, no questions asked.
* Handles repeats, and time outs. Also handles reading status reports and sense
* @param handle libusb device *
* @param endpoint_out sends
* @param endpoint_in used to read status reports back in
* @param cbuf what to send
* @param length how much to send
* @return number of bytes actually sent, or -1 for failures.
*/
int send_usb_data_only(libusb_device_handle *handle, unsigned char endpoint_out,
unsigned char endpoint_in, unsigned char *cbuf, unsigned int length) {
int ret;
int real_transferred;
int try = 0;
do {
ret = libusb_bulk_transfer(handle, endpoint_out, cbuf, length,
&real_transferred, SG_TIMEOUT_MSEC);
if (ret == LIBUSB_ERROR_PIPE) {
libusb_clear_halt(handle, endpoint_out);
}
try++;
} while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
if (ret != LIBUSB_SUCCESS) {
WLOG("sending failed: %d\n", ret);
return -1;
}
// now, swallow up the status, so that things behave nicely...
uint32_t received_tag;
// -ve is for my errors, 0 is good, +ve is libusb sense status bytes
int status = get_usb_mass_storage_status(handle, endpoint_in, &received_tag);
if (status < 0) {
WLOG("receiving status failed: %d\n", status);
return -1;
}
if (status != 0) {
WLOG("receiving status not passed :(: %02x\n", status);
}
if (status == 1) {
get_sense(handle, endpoint_in, endpoint_out);
return -1;
}
return real_transferred;
}
int stlink_q(stlink_t *sl) {
struct stlink_libsg* sg = sl->backend_data;
//uint8_t cdb_len = 6; // FIXME varies!!!
uint8_t cdb_len = 10; // FIXME varies!!!
uint8_t lun = 0; // always zero...
uint32_t tag = send_usb_mass_storage_command(sg->usb_handle, sg->ep_req,
sg->cdb_cmd_blk, cdb_len, lun, LIBUSB_ENDPOINT_IN, sl->q_len);
// now wait for our response...
// length copied from stlink-usb...
int rx_length = sl->q_len;
int try = 0;
int real_transferred;
int ret;
if (rx_length > 0) {
do {
ret = libusb_bulk_transfer(sg->usb_handle, sg->ep_rep, sl->q_buf, rx_length,
&real_transferred, SG_TIMEOUT_MSEC);
if (ret == LIBUSB_ERROR_PIPE) {
libusb_clear_halt(sg->usb_handle, sg->ep_req);
}
try++;
} while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
if (ret != LIBUSB_SUCCESS) {
WLOG("Receiving failed: %d\n", ret);
return -1;
}
if (real_transferred != rx_length) {
WLOG("received unexpected amount: %d != %d\n", real_transferred, rx_length);
}
}
uint32_t received_tag;
// -ve is for my errors, 0 is good, +ve is libusb sense status bytes
int status = get_usb_mass_storage_status(sg->usb_handle, sg->ep_rep, &received_tag);
if (status < 0) {
WLOG("receiving status failed: %d\n", status);
return -1;
}
if (status != 0) {
WLOG("receiving status not passed :(: %02x\n", status);
}
if (status == 1) {
get_sense(sg->usb_handle, sg->ep_rep, sg->ep_req);
return -1;
}
if (received_tag != tag) {
WLOG("received tag %d but expected %d\n", received_tag, tag);
//return -1;
}
if (rx_length > 0 && real_transferred != rx_length) {
return -1;
}
return 0;
}
// TODO thinking, cleanup
void stlink_stat(stlink_t *stl, char *txt) {
if (stl->q_len <= 0)
return;
stlink_print_data(stl);
switch (stl->q_buf[0]) {
case STLINK_OK:
DLOG(" %s: ok\n", txt);
return;
case STLINK_FALSE:
DLOG(" %s: false\n", txt);
return;
default:
DLOG(" %s: unknown\n", txt);
}
}
int _stlink_sg_version(stlink_t *stl) {
struct stlink_libsg *sl = stl->backend_data;
clear_cdb(sl);
sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
stl->q_len = 6;
sl->q_addr = 0;
return stlink_q(stl);
}
// Get stlink mode:
// STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE
// usb dfu || usb mass || jtag or swd
int _stlink_sg_current_mode(stlink_t *stl) {
struct stlink_libsg *sl = stl->backend_data;
clear_cdb(sl);
sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
stl->q_len = 2;
sl->q_addr = 0;
if (stlink_q(stl))
return -1;
return stl->q_buf[0];
}
// Exit the mass mode and enter the swd debug mode.
int _stlink_sg_enter_swd_mode(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD;
sl->q_len = 0; // >0 -> aboard
return stlink_q(sl);
}
// Exit the mass mode and enter the jtag debug mode.
// (jtag is disabled in the discovery's stlink firmware)
int _stlink_sg_enter_jtag_mode(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
DLOG("\n*** stlink_enter_jtag_mode ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
sl->q_len = 0;
return stlink_q(sl);
}
// XXX kernel driver performs reset, the device temporally disappears
// Suspect this is no longer the case when we have ignore on? RECHECK
int _stlink_sg_exit_dfu_mode(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
DLOG("\n*** stlink_exit_dfu_mode ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
sg->cdb_cmd_blk[1] = STLINK_DFU_EXIT;
sl->q_len = 0; // ??
return stlink_q(sl);
/*
[135121.844564] sd 19:0:0:0: [sdb] Unhandled error code
[135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK
[135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00
[135121.844584] end_request: I/O error, dev sdb, sector 4096
[135121.844590] Buffer I/O error on device sdb, logical block 512
[135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7
[135130.274551] usb 6-1: device firmware changed
[135130.274618] usb 6-1: USB disconnect, address 7
[135130.275186] VFS: busy inodes on changed media or resized disk sdb
[135130.275424] VFS: busy inodes on changed media or resized disk sdb
[135130.286758] VFS: busy inodes on changed media or resized disk sdb
[135130.292796] VFS: busy inodes on changed media or resized disk sdb
[135130.301481] VFS: busy inodes on changed media or resized disk sdb
[135130.304316] VFS: busy inodes on changed media or resized disk sdb
[135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8
[135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1
[135130.629492] scsi20 : usb-storage 6-1:1.0
[135131.625600] scsi 20:0:0:0: Direct-Access STM32 PQ: 0 ANSI: 0
[135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0
[135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB)
[135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled
[135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through
[135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled
[135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through
[135131.640609] sdb:
[135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled
[135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through
[135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk
[135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
[135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
[135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
[135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
[135131.671570] end_request: I/O error, dev sdb, sector 63872
[135131.671575] Buffer I/O error on device sdb, logical block 7984
[135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
[135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
[135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
[135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
[135131.678551] end_request: I/O error, dev sdb, sector 63872
...
[135131.853565] end_request: I/O error, dev sdb, sector 4096
*/
}
int _stlink_sg_core_id(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
int ret;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
sl->q_len = 4;
sg->q_addr = 0;
ret = stlink_q(sl);
if (ret)
return ret;
sl->core_id = read_uint32(sl->q_buf, 0);
return 0;
}
// Arm-core reset -> halted state.
int _stlink_sg_reset(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
sl->q_len = 2;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "core reset");
return 0;
}
// Arm-core reset -> halted state.
int _stlink_sg_jtag_reset(stlink_t *sl, int value) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_JTAG_DRIVE_NRST;
sg->cdb_cmd_blk[2] = (value)?0:1;
sl->q_len = 3;
sg->q_addr = 2;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "core reset");
return 0;
}
// Arm-core status: halted or running.
int _stlink_sg_status(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
sl->q_len = 2;
sg->q_addr = 0;
return stlink_q(sl);
}
// Force the core into the debug mode -> halted state.
int _stlink_sg_force_debug(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
sl->q_len = 2;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "force debug");
return 0;
}
// Read all arm-core registers.
int _stlink_sg_read_all_regs(stlink_t *sl, reg *regp) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
sl->q_len = 84;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_print_data(sl);
// TODO - most of this should be re-extracted up....
// 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71 | 72-75 | 76-79 | 80-83
// r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2
for (int i = 0; i < 16; i++) {
regp->r[i] = read_uint32(sl->q_buf, 4 * i);
if (sl->verbose > 1)
DLOG("r%2d = 0x%08x\n", i, regp->r[i]);
}
regp->xpsr = read_uint32(sl->q_buf, 64);
regp->main_sp = read_uint32(sl->q_buf, 68);
regp->process_sp = read_uint32(sl->q_buf, 72);
regp->rw = read_uint32(sl->q_buf, 76);
regp->rw2 = read_uint32(sl->q_buf, 80);
if (sl->verbose < 2)
return 0;
DLOG("xpsr = 0x%08x\n", regp->xpsr);
DLOG("main_sp = 0x%08x\n", regp->main_sp);
DLOG("process_sp = 0x%08x\n", regp->process_sp);
DLOG("rw = 0x%08x\n", regp->rw);
DLOG("rw2 = 0x%08x\n", regp->rw2);
return 0;
}
// Read an arm-core register, the index must be in the range 0..20.
// 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20
// r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2
int _stlink_sg_read_reg(stlink_t *sl, int r_idx, reg *regp) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
sg->cdb_cmd_blk[2] = r_idx;
sl->q_len = 4;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
// 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20
// 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71 | 72-75 | 76-79 | 80-83
// r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2
stlink_print_data(sl);
uint32_t r = read_uint32(sl->q_buf, 0);
DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
switch (r_idx) {
case 16:
regp->xpsr = r;
break;
case 17:
regp->main_sp = r;
break;
case 18:
regp->process_sp = r;
break;
case 19:
regp->rw = r; //XXX ?(primask, basemask etc.)
break;
case 20:
regp->rw2 = r; //XXX ?(primask, basemask etc.)
break;
default:
regp->r[r_idx] = r;
}
return 0;
}
// Write an arm-core register. Index:
// 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20
// r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2
int _stlink_sg_write_reg(stlink_t *sl, uint32_t reg, int idx) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
// 2: reg index
// 3-6: reg content
sg->cdb_cmd_blk[2] = idx;
write_uint32(sg->cdb_cmd_blk + 3, reg);
sl->q_len = 2;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "write reg");
return 0;
}
// Write a register of the debug module of the core.
// XXX ?(atomic writes)
// TODO test
void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
struct stlink_libsg *sg = sl->backend_data;
DLOG("\n*** stlink_write_dreg ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
// 2-5: address of reg of the debug module
// 6-9: reg content
write_uint32(sg->cdb_cmd_blk + 2, addr);
write_uint32(sg->cdb_cmd_blk + 6, reg);
sl->q_len = 2;
sg->q_addr = addr;
stlink_q(sl);
stlink_stat(sl, "write debug reg");
}
// Force the core exit the debug mode.
int _stlink_sg_run(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
sl->q_len = 2;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "run core");
return 0;
}
// Step the arm-core.
int _stlink_sg_step(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
sl->q_len = 2;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "step core");
return 0;
}
// TODO test
// see Cortex-M3 Technical Reference Manual
// TODO make delegate!
void stlink_set_hw_bp(stlink_t *sl, int fp_nr, uint32_t addr, int fp) {
DLOG("\n*** stlink_set_hw_bp ***\n");
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
// 2:The number of the flash patch used to set the breakpoint
// 3-6: Address of the breakpoint (LSB)
// 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
sl->q_buf[2] = fp_nr;
write_uint32(sl->q_buf, addr);
sl->q_buf[7] = fp;
sl->q_len = 2;
stlink_q(sl);
stlink_stat(sl, "set flash breakpoint");
}
// TODO test
// TODO make delegate!
void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) {
struct stlink_libsg *sg = sl->backend_data;
DLOG("\n*** stlink_clr_hw_bp ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
sg->cdb_cmd_blk[2] = fp_nr;
sl->q_len = 2;
stlink_q(sl);
stlink_stat(sl, "clear flash breakpoint");
}
// Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)
int _stlink_sg_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
// 2-5: addr
// 6-7: len
write_uint32(sg->cdb_cmd_blk + 2, addr);
write_uint16(sg->cdb_cmd_blk + 6, len);
// data_in 0-0x40-len
// !!! len _and_ q_len must be max 6k,
// i.e. >1024 * 6 = 6144 -> aboard)
// !!! if len < q_len: 64*k, 1024*n, n=1..5 -> aboard
// (broken residue issue)
sl->q_len = len;
sg->q_addr = addr;
if (stlink_q(sl))
return -1;
stlink_print_data(sl);
return 0;
}
// Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
int _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
struct stlink_libsg *sg = sl->backend_data;
int ret;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
// 2-5: addr
// 6-7: len (>0x40 (64) -> aboard)
write_uint32(sg->cdb_cmd_blk + 2, addr);
write_uint16(sg->cdb_cmd_blk + 6, len);
// this sends the command...
ret = send_usb_mass_storage_command(sg->usb_handle,
sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
if (ret == -1)
return ret;
// This sends the data...
ret = send_usb_data_only(sg->usb_handle,
sg->ep_req, sg->ep_rep, sl->q_buf, len);
if (ret == -1)
return ret;
stlink_print_data(sl);
return 0;
}
// Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
int _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
struct stlink_libsg *sg = sl->backend_data;
int ret;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT;
// 2-5: addr
// 6-7: len "unlimited"
write_uint32(sg->cdb_cmd_blk + 2, addr);
write_uint16(sg->cdb_cmd_blk + 6, len);
// this sends the command...
ret = send_usb_mass_storage_command(sg->usb_handle,
sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
if (ret == -1)
return ret;
// This sends the data...
ret = send_usb_data_only(sg->usb_handle,
sg->ep_req, sg->ep_rep, sl->q_buf, len);
if (ret == -1)
return ret;
stlink_print_data(sl);
return 0;
}
// Write one DWORD data to memory
int _stlink_sg_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_JTAG_WRITEDEBUG_32BIT;
// 2-5: addr
write_uint32(sg->cdb_cmd_blk + 2, addr);
write_uint32(sg->cdb_cmd_blk + 6, data);
sl->q_len = 2;
return stlink_q(sl);
}
// Read one DWORD data from memory
int _stlink_sg_read_debug32(stlink_t *sl, uint32_t addr, uint32_t *data) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_JTAG_READDEBUG_32BIT;
// 2-5: addr
write_uint32(sg->cdb_cmd_blk + 2, addr);
sl->q_len = 8;
if (stlink_q(sl))
return -1;
*data = read_uint32(sl->q_buf, 4);
return 0;
}
// Exit the jtag or swd mode and enter the mass mode.
int _stlink_sg_exit_debug_mode(stlink_t *stl)
{
if (stl) {
struct stlink_libsg* sl = stl->backend_data;
clear_cdb(sl);
sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
stl->q_len = 0; // >0 -> aboard
return stlink_q(stl);
}
return 0;
}
// 1) open a sg device, switch the stlink from dfu to mass mode
// 2) wait 5s until the kernel driver stops reseting the broken device
// 3) reopen the device
// 4) the device driver is now ready for a switch to jtag/swd mode
// TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
stlink_backend_t _stlink_sg_backend = {
_stlink_sg_close,
_stlink_sg_exit_debug_mode,
_stlink_sg_enter_swd_mode,
_stlink_sg_enter_jtag_mode,
_stlink_sg_exit_dfu_mode,
_stlink_sg_core_id,
_stlink_sg_reset,
_stlink_sg_jtag_reset,
_stlink_sg_run,
_stlink_sg_status,
_stlink_sg_version,
_stlink_sg_read_debug32,
_stlink_sg_read_mem32,
_stlink_sg_write_debug32,
_stlink_sg_write_mem32,
_stlink_sg_write_mem8,
_stlink_sg_read_all_regs,
_stlink_sg_read_reg,
NULL, /* read_all_unsupported_regs */
NULL, /* read_unsupported_regs */
NULL, /* write_unsupported_regs */
_stlink_sg_write_reg,
_stlink_sg_step,
_stlink_sg_current_mode,
_stlink_sg_force_debug,
NULL
};
static stlink_t* stlink_open(const int verbose) {
stlink_t *sl = malloc(sizeof (stlink_t));
memset(sl, 0, sizeof(stlink_t));
struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
if (sl == NULL || slsg == NULL) {
WLOG("Couldn't malloc stlink and stlink_sg structures out of memory!\n");
return NULL;
}
if (libusb_init(&(slsg->libusb_ctx))) {
WLOG("failed to init libusb context, wrong version of libraries?\n");
free(sl);
free(slsg);
return NULL;
}
libusb_set_debug(slsg->libusb_ctx, 3);
slsg->usb_handle = libusb_open_device_with_vid_pid(slsg->libusb_ctx, USB_ST_VID, USB_STLINK_PID);
if (slsg->usb_handle == NULL) {
WLOG("Failed to find an stlink v1 by VID:PID\n");
libusb_close(slsg->usb_handle);
libusb_exit(slsg->libusb_ctx);
free(sl);
free(slsg);
return NULL;
}
// TODO
// Could read the interface config descriptor, and assert lots of the assumptions
// assumption: numInterfaces is always 1...
if (libusb_kernel_driver_active(slsg->usb_handle, 0) == 1) {
int r = libusb_detach_kernel_driver(slsg->usb_handle, 0);
if (r < 0) {
WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-r));
libusb_close(slsg->usb_handle);
libusb_exit(slsg->libusb_ctx);
free(sl);
free(slsg);
return NULL;
}
DLOG("Kernel driver was successfully detached\n");
}
int config;
if (libusb_get_configuration(slsg->usb_handle, &config)) {
/* this may fail for a previous configured device */
WLOG("libusb_get_configuration()\n");
libusb_close(slsg->usb_handle);
libusb_exit(slsg->libusb_ctx);
free(sl);
free(slsg);
return NULL;
}
// assumption: bConfigurationValue is always 1
if (config != 1) {
WLOG("Your stlink got into a real weird configuration, trying to fix it!\n");
DLOG("setting new configuration (%d -> 1)\n", config);
if (libusb_set_configuration(slsg->usb_handle, 1)) {
/* this may fail for a previous configured device */
WLOG("libusb_set_configuration() failed\n");
libusb_close(slsg->usb_handle);
libusb_exit(slsg->libusb_ctx);
free(sl);
free(slsg);
return NULL;
}
}
if (libusb_claim_interface(slsg->usb_handle, 0)) {
WLOG("libusb_claim_interface() failed\n");
libusb_close(slsg->usb_handle);
libusb_exit(slsg->libusb_ctx);
free(sl);
free(slsg);
return NULL;
}
// assumption: endpoint config is fixed mang. really.
slsg->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
slsg->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
DLOG("Successfully opened stlinkv1 by libusb :)\n");
sl->verbose = verbose;
sl->backend_data = slsg;
sl->backend = &_stlink_sg_backend;
sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
slsg->q_addr = 0;
return sl;
}
int do_work(struct sdp_dev *p_id, struct sdp_work **work, int verify)
{
struct sdp_work *curr = *work;
int config = 0;
int err = 0;
libusb_device_handle *h = p_id->priv;
libusb_get_configuration(h, &config);
dbg_printf("bConfigurationValue = 0x%x\n", config);
if (libusb_kernel_driver_active(h, 0))
libusb_detach_kernel_driver(h, 0);
err = libusb_claim_interface(h, 0);
if (err) {
fprintf(stderr, "claim interface failed\n");
return err;
}
printf("Interface 0 claimed\n");
err = do_status(p_id);
if (err) {
fprintf(stderr, "status failed\n");
goto err_release_interface;
}
while (curr) {
/* Do current job */
if (curr->mem)
perform_mem_work(p_id, curr->mem);
if (curr->filename[0])
err = DoIRomDownload(p_id, curr, verify);
if (err) {
fprintf(stderr, "DoIRomDownload failed, err=%d\n", err);
do_status(p_id);
break;
}
/* Check if more work is to do... */
if (!curr->next) {
/*
* If only one job, but with a plug-in is specified
* reexecute the same job, but this time download the
* image. This allows to specify a single file with
* plugin and image, and imx_usb will download & run
* the plugin first and then the image.
* NOTE: If the file does not contain a plugin,
* DoIRomDownload->process_header will set curr->plug
* to 0, so we won't download the same image twice...
*/
if (curr->plug) {
curr->plug = 0;
} else {
curr = NULL;
break;
}
} else {
curr = curr->next;
}
/*
* Check if device is still here, otherwise return
* with work (retry)
*/
err = do_status(p_id);
if (err < 0) {
err = 0;
break;
}
}
*work = curr;
err_release_interface:
libusb_release_interface(h, 0);
return err;
}
