USB-Servo/commandline/usbdrv.c
2006-10-28 12:40:42 +00:00

208 lines
6.5 KiB
C

/**
* \file usbdrv.c
* \brief USB-driver-parts for implementing a client.
* \author Ronald Schaten
* \version $Id: usbdrv.c,v 1.1 2006/10/28 12:40:42 rschaten Exp $
*
* License: See documentation.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <usb.h> /* this is libusb, see
http://libusb.sourceforge.net/ */
#include "usbdrv.h"
#include "usbservo.h"
int usbGetStringAscii(usb_dev_handle * dev, int index, int langid,
char *buf, int buflen) {
char buffer[256];
int rval, i;
if ((rval = usb_control_msg(dev, USB_ENDPOINT_IN, USB_REQ_GET_DESCRIPTOR,
(USB_DT_STRING << 8) + index, langid, buffer,
sizeof(buffer), 1000)) < 0) {
return rval;
}
if (buffer[1] != USB_DT_STRING) {
return 0;
}
if ((unsigned char)buffer[0] < rval) {
rval = (unsigned char)buffer[0];
}
rval /= 2;
/* lossy conversion to ISO Latin1 */
for (i = 1; i < rval; i++) {
if (i > buflen) {
/* destination buffer overflow */
break;
}
buf[i - 1] = buffer[2 * i];
if (buffer[2 * i + 1] != 0) {
/* outside of ISO Latin1 range */
buf[i - 1] = '?';
}
}
buf[i - 1] = 0;
return i - 1;
}
int usbOpenDevice(usb_dev_handle ** device, int vendor, char *vendorName,
int product, char *productName) {
struct usb_bus *bus;
struct usb_device *dev;
usb_dev_handle *handle = NULL;
int errorCode = USB_ERROR_NOTFOUND;
static int didUsbInit = 0;
if (!didUsbInit) {
didUsbInit = 1;
usb_init();
}
usb_find_busses();
usb_find_devices();
for (bus = usb_get_busses(); bus; bus = bus->next) {
for (dev = bus->devices; dev; dev = dev->next) {
if (dev->descriptor.idVendor == vendor
&& dev->descriptor.idProduct == product) {
char string[256];
int len;
handle = usb_open(dev); /* we need to open the device in
order to query strings */
if (!handle) {
errorCode = USB_ERROR_ACCESS;
fprintf(stderr,
"Warning: cannot open USB device: %s\n",
usb_strerror());
continue;
}
if (vendorName == NULL && productName == NULL) {
/* name does not matter */
break;
}
/* now check whether the names match: */
len = usbGetStringAscii(handle,
dev->descriptor.iManufacturer,
0x0409, string, sizeof(string));
if (len < 0) {
errorCode = USB_ERROR_IO;
fprintf(stderr,
"Warning: cannot query manufacturer for device: %s\n",
usb_strerror());
} else {
errorCode = USB_ERROR_NOTFOUND;
if (strcmp(string, vendorName) == 0) {
len = usbGetStringAscii(handle,
dev->descriptor.iProduct,
0x0409, string,
sizeof(string));
if (len < 0) {
errorCode = USB_ERROR_IO;
fprintf(stderr,
"Warning: cannot query product for device: %s\n",
usb_strerror());
} else {
errorCode = USB_ERROR_NOTFOUND;
if (strcmp(string, productName) == 0) {
break;
}
}
}
}
usb_close(handle);
handle = NULL;
}
}
if (handle) {
break;
}
}
if (handle != NULL) {
errorCode = 0;
*device = handle;
}
return errorCode;
}
int dev_test(usb_dev_handle * handle, int argc, char **argv) {
unsigned char buffer[8];
int nBytes;
int i, v, r;
if (argc != 2) {
return 1;
}
for (i = 0; i < 1000; i++) {
v = rand() & 0xffff;
nBytes = usb_control_msg(handle,
USB_TYPE_VENDOR | USB_RECIP_DEVICE |
USB_ENDPOINT_IN, CMD_ECHO, v, 0,
(char *)buffer, sizeof(buffer), 5000);
if (nBytes < 2) {
if (nBytes < 0) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
}
fprintf(stderr, "only %d bytes received in iteration %d\n",
nBytes, i);
exit(1);
}
r = buffer[0] | (buffer[1] << 8);
if (r != v) {
fprintf(stderr,
"data error: received 0x%x instead of 0x%x in iteration %d\n",
r, v, i);
exit(1);
}
}
printf("test succeeded\n");
return 0;
}
int dev_set(usb_dev_handle * handle, int argc, char **argv) {
unsigned char buffer[8];
int nBytes;
if (argc != 3) {
return 1;
}
int angle = atoi(argv[2]);
if ((angle < 0) || (angle > 255)) {
fprintf(stderr, "invalid angle: %d\n", angle);
exit(1);
}
nBytes = usb_control_msg(handle,
USB_TYPE_VENDOR | USB_RECIP_DEVICE |
USB_ENDPOINT_OUT, CMD_SET, angle, 0,
(char *)buffer, sizeof(buffer), 5000);
if (nBytes < 0) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
exit(1);
}
return 0;
}
int dev_status(usb_dev_handle * handle, int argc, char **argv) {
int nBytes;
unsigned char buffer[8];
if (argc != 2) {
return 1;
}
nBytes = usb_control_msg(handle,
USB_TYPE_VENDOR | USB_RECIP_DEVICE |
USB_ENDPOINT_IN, CMD_GET, 0, 0, (char *)buffer,
sizeof(buffer), 5000);
if (nBytes < 0) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
exit(1);
}
if (nBytes < 1) {
fprintf(stderr, "only %d bytes status received\n", nBytes);
exit(1);
}
printf("Current servo angle: %d\n", buffer[0]);
return 0;
}