static int usb_string_sub(struct usb_device *dev, unsigned int langid,
unsigned int index, unsigned char *buf)
{
int rc;
/* Try to read the string descriptor by asking for the maximum
* possible number of bytes */
rc = usb_get_string(dev, langid, index, buf, 255);
/* If that failed try to read the descriptor length, then
* ask for just that many bytes */
if (rc < 2) {
rc = usb_get_string(dev, langid, index, buf, 2);
if (rc == 2)
rc = usb_get_string(dev, langid, index, buf, buf[0]);
}
if (rc >= 2) {
if (!buf[0] && !buf[1])
usb_try_string_workarounds(buf, &rc);
/* There might be extra junk at the end of the descriptor */
if (buf[0] < rc)
rc = buf[0];
rc = rc - (rc & 1); /* force a multiple of two */
}
if (rc < 2)
rc = (rc < 0 ? rc : -EINVAL);
return rc;
}
static int usb_string_sub(struct usb_device *dev, unsigned int langid,
unsigned int index, unsigned char *buf)
{
int rc;
/* Try to read the string descriptor by asking for the maximum
* possible number of bytes */
rc = usb_get_string(dev, langid, index, buf, 255);
/* If that failed try to read the descriptor length, then
* ask for just that many bytes */
if (rc < 0) {
rc = usb_get_string(dev, langid, index, buf, 2);
if (rc == 2)
rc = usb_get_string(dev, langid, index, buf, buf[0]);
}
if (rc >= 0) {
/* There might be extra junk at the end of the descriptor */
if (buf[0] < rc)
rc = buf[0];
if (rc < 2)
rc = -EINVAL;
}
return rc;
}
int usb_get_string_simple(usb_dev_handle *dev, int index, char *buf, size_t buflen)
{
char tbuf[255]; /* Some devices choke on size > 255 */
int ret, langid, si, di;
/*
* Asking for the zero'th index is special - it returns a string
* descriptor that contains all the language IDs supported by the
* device. Typically there aren't many - often only one. The
* language IDs are 16 bit numbers, and they start at the third byte
* in the descriptor. See USB 2.0 specification, section 9.6.7, for
* more information on this. */
ret = usb_get_string(dev, 0, 0, tbuf, sizeof(tbuf));
if (ret < 0)
return ret;
if (ret < 4)
return -EIO;
langid = tbuf[2] | (tbuf[3] << 8);
ret = usb_get_string(dev, index, langid, tbuf, sizeof(tbuf));
if (ret < 0)
return ret;
if (tbuf[1] != USB_DT_STRING)
return -EIO;
if (tbuf[0] > ret)
return -EFBIG;
for (di = 0, si = 2; si < tbuf[0]; si += 2) {
if (di >= (buflen - 1))
break;
if (tbuf[si + 1]) /* high byte */
buf[di++] = '?';
else
buf[di++] = tbuf[si];
}
buf[di] = 0;
return di;
}
/*
* Class: ch_ntb_usb_LibusbJava
* Method: usb_get_string
* Signature: (JII)Ljava/lang/String;
*/
JNIEXPORT jstring JNICALL Java_ch_ntb_usb_LibusbJava_usb_1get_1string
(JNIEnv *env, jclass obj, jlong dev_handle, jint index, jint langid)
{
clearLibusbJavaError();
char string[256];
int retVal = usb_get_string((usb_dev_handle *) dev_handle, index, langid, string, 256);
if (retVal > 0)
return (*env)->NewStringUTF(env, string);
return 0;
}
int Device::string(std::string &buf, int index, u_int16_t langID)
{
int retval;
char tmpBuff[256];
if (0 == langID) {
/* we want the first lang ID available, so find out what it is */
retval = usb_get_string(m_handle, 0, 0, tmpBuff, sizeof(tmpBuff));
if (retval < 0)
return retval;
if (retval < 4 || tmpBuff[1] != USB_DT_STRING)
return -EIO;
langID = tmpBuff[2] | (tmpBuff[3] << 8);
}
retval = usb_get_string(m_handle, index, langID, tmpBuff, sizeof(tmpBuff));
if (retval < 0)
return retval;
if (tmpBuff[1] != USB_DT_STRING)
return -EIO;
if (tmpBuff[0] > retval)
return -EFBIG;
/* FIXME: Handle unicode? */
#if 0
if (retval > 0) {
std::string.setUnicode((unsigned char *)&tmpBuff[2], tmpBuff[0] / 2 - 1);
}
#endif
return retval;
}
/* USB::DevHandle#usb_get_string(index, langid, buf) */
static VALUE
rusb_get_string(
VALUE v,
VALUE vindex,
VALUE vlangid,
VALUE vbuf)
{
usb_dev_handle *p = get_usb_devhandle(v);
int index = NUM2INT(vindex);
int langid = NUM2INT(vlangid);
char *buf;
int buflen;
int ret;
StringValue(vbuf);
rb_str_modify(vbuf);
buf = RSTRING_PTR(vbuf);
buflen = RSTRING_LEN(vbuf);
ret = usb_get_string(p, index, langid, buf, buflen);
check_usb_error("usb_get_string", ret);
return INT2NUM(ret);
}
void debug(void)
{
robot_information_t robot;
target_cam_t target_cam;
unsigned char str[10] = "\0";
unsigned short old_sw = 0,
start_sw =0,
old_limit_sw = 0;
float target_degree = 0.00;
transmit_usb("[ r : reset] \n\r");
transmit_usb("[ 1 : encoder]\n\r");
transmit_usb("[ 2 : AD]\n\r");
transmit_usb("[ 3 : coordinate]\n\r");
transmit_usb("[ 4 : sonic_waves]\n\r");
transmit_usb("[ 5 : encoder reset]\n\r");
transmit_usb("[ 6 : cam_inf]\n\r");
transmit_usb("[ 7 : target_cam]\n\r");
transmit_usb("[ 8 : inf&target]\n\r");
transmit_usb("[ 9 : start_switch]\n\r");
transmit_usb("[10 : limit_switch]\n\r");
transmit_usb("[11 : motor]\n\r");
transmit_usb("[12 : reverse motor]\n\r");
transmit_usb("[13 : stop motor]\n\r");
transmit_usb("[14 : arm motor]\n\r");
transmit_usb("[15 : reverse arm motor]\n\r");
//transmit_usb("[p : p gain adjustment]\n\r");
//transmit_usb("[d : d gain adjustment]\n\r");
GPIO_ResetBits(ENC_RESET);
while(strcmp(str, "r") != 0){
if(usb_available() != 0){
usb_get_string(str);
}
buzzer_stop();
//sonic_waves(&robot);
if(count_time(3) >= INTERRUPT_TIME){
reset_count_time(3);
get_robot_inf( &robot );
cam_data(&target_cam, &robot);
start_sw =positive_chattering(START_SW,1);
robot.sw.limit_sw = negative_chattering(LIMIT_SW,2);
robot.ad = get_ADC1_value(0);
}
if(strcmp(str, "1") == 0){
f_print(PC,"ENCL",robot.enc_cnt.l);
f_print(PC,"ENCR",robot.enc_cnt.r);
f_print(PC,"ENCF",robot.enc_cnt.f);
put_enter(PC);
}else if(strcmp(str, "2") == 0){
f_print(PC,"AD",robot.ad);
put_enter(PC);
}else if(strcmp(str, "3") == 0){
f2_print(PC,"now_coord",robot.coord.c_x, robot.coord.c_y);
f_print(PC,"deg",robot.angle.degree);
put_enter(PC);
}else if(strcmp(str, "4") == 0){
f_print(PC,"time",count_time(2));
f_print(PC,"dis",robot.waves);
put_enter(PC);
}else if(strcmp(str, "5") == 0){
GPIO_SetBits(ENC_RESET);
//str[0] = 'r';
}else if(strcmp(str, "6") == 0){
f2_print(PC, "under", target_cam.under_x, target_cam.under_y );
f2_print(PC, "over", target_cam.over_x, target_cam.over_y );
put_enter(PC);
}else if(strcmp(str, "7") == 0){
f2_print(PC, "target_cam", (target_cam.x) * cos(D_TO_R(robot.angle.degree))+robot.coord.c_x, (target_cam.y)*sin(D_TO_R(robot.angle.degree))+robot.coord.c_y);
put_enter(PC);
}else if(strcmp(str, "8") == 0){
f2_print(PC, "under", target_cam.under_x, target_cam.under_y);
f2_print(PC, "over", target_cam.over_x, target_cam.over_y );
target_degree = get_target_degree(ROBO_TO_CENTER - robot.coord.c_x, robot.coord.c_y);
f2_print(PC, "target_cam", (target_cam.x) * cos(D_TO_R(target_degree)) + robot.coord.c_x, (target_cam.y) * sin(D_TO_R(target_degree)) + robot.coord.c_y);
put_enter(PC);
}else if(strcmp(str, "9") == 0){
if(robot.sw.start_sw != old_sw ){
f_print(PC,"sw",start_sw);
put_enter(PC);
}
}else if(strcmp(str, "10") == 0){
if(robot.sw.limit_sw != old_limit_sw ){
f_print(PC,"limit_sw",robot.sw.limit_sw);
put_enter(PC);
}
}else if(strcmp(str, "11") == 0){
move(50, 50, 50);
}else if(strcmp(str, "12") == 0){
move(-50, -50, -50);
}else if(strcmp(str, "13") == 0){
move(0, 0, 0);
move_arm(0);
}else if(strcmp(str, "14") == 0){
move_arm(100);
}else if(strcmp(str, "15") == 0){
move_arm(-100);
}
old_limit_sw = robot.sw.start_sw;
old_sw = start_sw;
}
}
void upsdrv_initups(void)
{
#ifndef TESTING
const struct {
const char *name;
int (*command)(const char *cmd, char *buf, size_t buflen);
} subdriver[] = {
{ "cypress", &cypress_command },
{ "phoenix", &phoenix_command },
{ "ippon", &ippon_command },
{ "krauler", &krauler_command },
{ NULL }
};
int ret, langid;
char tbuf[255]; /* Some devices choke on size > 255 */
char *regex_array[6];
char *subdrv = getval("subdriver");
regex_array[0] = getval("vendorid");
regex_array[1] = getval("productid");
regex_array[2] = getval("vendor");
regex_array[3] = getval("product");
regex_array[4] = getval("serial");
regex_array[5] = getval("bus");
/* check for language ID workaround (#1) */
if (getval("langid_fix")) {
/* skip "0x" prefix and set back to hexadecimal */
if (sscanf(getval("langid_fix") + 2, "%x", &langid_fix) != 1) {
upslogx(LOG_NOTICE, "Error enabling language ID workaround");
}
else {
upsdebugx(2, "language ID workaround enabled (using '0x%x')", langid_fix);
}
}
/* pick up the subdriver name if set explicitly */
if (subdrv) {
int i;
if (!regex_array[0] || !regex_array[1]) {
fatalx(EXIT_FAILURE, "When specifying a subdriver, 'vendorid' and 'productid' are mandatory.");
}
for (i = 0; subdriver[i].name; i++) {
if (strcasecmp(subdrv, subdriver[i].name)) {
continue;
}
subdriver_command = subdriver[i].command;
break;
}
if (!subdriver_command) {
fatalx(EXIT_FAILURE, "Subdriver \"%s\" not found!", subdrv);
}
}
ret = USBNewRegexMatcher(®ex_matcher, regex_array, REG_ICASE | REG_EXTENDED);
switch (ret)
{
case -1:
fatal_with_errno(EXIT_FAILURE, "USBNewRegexMatcher");
case 0:
break; /* all is well */
default:
fatalx(EXIT_FAILURE, "invalid regular expression: %s", regex_array[ret]);
}
/* link the matchers */
regex_matcher->next = &device_matcher;
ret = usb->open(&udev, &usbdevice, regex_matcher, NULL);
if (ret < 0) {
fatalx(EXIT_FAILURE,
"No supported devices found. Please check your device availability with 'lsusb'\n"
"and make sure you have an up-to-date version of NUT. If this does not help,\n"
"try running the driver with at least 'subdriver', 'vendorid' and 'productid'\n"
"options specified. Please refer to the man page for details about these options\n"
"(man 8 blazer_usb).\n");
}
if (!subdriver_command) {
fatalx(EXIT_FAILURE, "No subdriver selected");
}
/* create a new matcher for later reopening */
ret = USBNewExactMatcher(&reopen_matcher, &usbdevice);
if (ret) {
fatal_with_errno(EXIT_FAILURE, "USBNewExactMatcher");
}
/* link the matchers */
reopen_matcher->next = regex_matcher;
dstate_setinfo("ups.vendorid", "%04x", usbdevice.VendorID);
dstate_setinfo("ups.productid", "%04x", usbdevice.ProductID);
/* check for language ID workaround (#2) */
if (langid_fix != -1) {
/* Future improvement:
* Asking for the zero'th index is special - it returns a string
* descriptor that contains all the language IDs supported by the
* device. Typically there aren't many - often only one. The
* language IDs are 16 bit numbers, and they start at the third byte
* in the descriptor. See USB 2.0 specification, section 9.6.7, for
* more information on this.
* This should allow automatic application of the workaround */
ret = usb_get_string(udev, 0, 0, tbuf, sizeof(tbuf));
if (ret >= 4) {
langid = tbuf[2] | (tbuf[3] << 8);
upsdebugx(1, "First supported language ID: 0x%x (please report to the NUT maintainer!)", langid);
}
}
#endif
blazer_initups();
}
static int krauler_command(const char *cmd, char *buf, size_t buflen)
{
/*
* Still not implemented:
* 0x6 T<n> (don't know how to pass the parameter)
* 0x68 and 0x69 both cause shutdown after an undefined interval
*/
const struct {
const char *str; /* Megatec command */
const int index; /* Krauler string index for this command */
const char prefix; /* character to replace the first byte in reply */
} command[] = {
{ "Q1\r", 0x03, '(' },
{ "F\r", 0x0d, '#' },
{ "I\r", 0x0c, '#' },
{ "T\r", 0x04, '\r' },
{ "TL\r", 0x05, '\r' },
{ "Q\r", 0x07, '\r' },
{ "C\r", 0x0b, '\r' },
{ "CT\r", 0x0b, '\r' },
{ NULL }
};
int i;
upsdebugx(3, "send: %.*s", (int)strcspn(cmd, "\r"), cmd);
for (i = 0; command[i].str; i++) {
int retry;
if (strcmp(cmd, command[i].str)) {
continue;
}
for (retry = 0; retry < 10; retry++) {
int ret;
if (langid_fix != -1) {
/* Apply langid_fix value */
ret = usb_get_string(udev, command[i].index, langid_fix, buf, buflen);
}
else {
ret = usb_get_string_simple(udev, command[i].index, buf, buflen);
}
if (ret <= 0) {
upsdebugx(3, "read: %s", ret ? usb_strerror() : "timeout");
return ret;
}
/* this may serve in the future */
upsdebugx(1, "received %d (%d)", ret, buf[0]);
if (langid_fix != -1) {
/* Limit this check, at least for now */
/* Invalid receive size - message corrupted */
if (ret != buf[0])
{
upsdebugx(1, "size mismatch: %d / %d", ret, buf[0]);
continue;
}
/* Simple unicode -> ASCII inplace conversion
* FIXME: this code is at least shared with mge-shut/libshut
* Create a common function? */
unsigned int di, si, size = buf[0];
for (di = 0, si = 2; si < size; si += 2) {
if (di >= (buflen - 1))
break;
if (buf[si + 1]) /* high byte */
buf[di++] = '?';
else
buf[di++] = buf[si];
}
buf[di] = 0;
ret = di;
}
/* "UPS No Ack" has a special meaning */
if (!strcasecmp(buf, "UPS No Ack")) {
upsdebugx(3, "read: %.*s", (int)strcspn(buf, "\r"), buf);
continue;
}
/* Replace the first byte of what we received with the correct one */
buf[0] = command[i].prefix;
upsdebugx(3, "read: %.*s", (int)strcspn(buf, "\r"), buf);
return ret;
}
return 0;
}
/* echo the unknown command back */
upsdebugx(3, "read: %.*s", (int)strcspn(cmd, "\r"), cmd);
return snprintf(buf, buflen, "%s", cmd);
}
