ssize_t hs_hid_send_feature_report(hs_handle *h, const uint8_t *buf, size_t size)
{
assert(h);
assert(h->dev->type == HS_DEVICE_TYPE_HID);
assert(buf);
if (size < 2)
return 0;
ssize_t r;
restart:
r = ioctl(h->fd, HIDIOCSFEATURE(size), (const char *)buf);
if (r < 0) {
switch (errno) {
case EINTR:
goto restart;
case EIO:
case ENXIO:
return hs_error(HS_ERROR_IO, "I/O error while writing to '%s'", h->dev->path);
}
return hs_error(HS_ERROR_SYSTEM, "ioctl('%s', HIDIOCSFEATURE) failed: %s", h->dev->path,
strerror(errno));
}
return r;
}
int HID_API_EXPORT hid_send_feature_report(hid_device *dev, const unsigned char *data, size_t length)
{
int res;
res = ioctl(dev->device_handle, HIDIOCSFEATURE(length), data);
if (res < 0)
perror("ioctl (SFEATURE)");
return res;
}
void Device::writeFeatureReport(const Device::Byte* report,size_t reportSize)
{
int ioctlResult=ioctl(fd,HIDIOCSFEATURE(reportSize),report);
if(ioctlResult<0)
{
int err=errno;
Misc::throwStdErr("RawHID::Device::writeFeatureReport: Error %s",strerror(err));
}
else if(size_t(ioctlResult)!=reportSize)
Misc::throwStdErr("RawHID::Device::writeFeatureReport: Short write, %u instead of %u",size_t(ioctlResult),reportSize);
}
void HidInterface::setReport(unsigned char report, unsigned char value) {
unsigned char buf[2];
/* buf[0] is Report ID, buf[1] is value */
buf[0] = report;
buf[1] = value;
/* TODO: check return value */
int ret = ioctl(*fd_, HIDIOCSFEATURE(sizeof(buf)), buf);
if (ret < 0) {
std::cerr << "Error setting HID feature report." << std::endl;
}
}
int HID_API_EXPORT hid_send_feature_report(hid_device *dev, const unsigned char *data, size_t length)
{
#ifdef FEATURE_REPORT
int res;
res = ioctl(dev->device_handle, HIDIOCSFEATURE(length), data);
if (res < 0)
perror("ioctl (SFEATURE)");
return res;
#else
perror("ioctl HIDIOCSFEATURE not yet supported in mainline kernel");
return -1;
#endif
}
static int set_master_bdaddr(int fd, const bdaddr_t *bdaddr)
{
uint8_t buf[8];
int ret;
buf[0] = 0xf5;
buf[1] = 0x01;
baswap((bdaddr_t *) (buf + 2), bdaddr);
ret = ioctl(fd, HIDIOCSFEATURE(sizeof(buf)), buf);
if (ret < 0)
error("sixaxis: failed to write master address (%s)",
strerror(errno));
return ret;
}
RazerHydra::~RazerHydra()
{
if (hidraw_fd >= 0)
{
ROS_INFO("releasing hydra");
uint8_t buf[256];
memset(buf, 0, sizeof(buf));
buf[6] = 1;
buf[8] = 4;
buf[89] = 5;
int res = ioctl(hidraw_fd, HIDIOCSFEATURE(91), buf);
if (res < 0)
{
ROS_ERROR("unable to stop streaming");
perror("HIDIOCSFEATURE");
}
else
ROS_INFO("stopped streaming");
close(hidraw_fd);
}
}
void da2013_cmd_send(int fd)
{
int res;
// Pretty unreliable unless you do it a few times
for (int i = 0; i < 3; i++)
{
res = ioctl(fd, HIDIOCSFEATURE(sizeof(da2013_cmd)), (char *) &da2013_cmd);
if (res < 0)
perror("HIDIOCSFEATURE");
else
{
for (int j = 0; j < 3; j++)
{
res = ioctl(fd, HIDIOCGFEATURE(sizeof(da2013_cmd)), (char *) &da2013_ret);
if (res == sizeof(da2013_cmd))
break;
if (res < 0)
perror("HIDIOCGFEATURE");
}
}
switch (da2013_cmd.status)
{
case 0:
case 1:
case 2:
case 3:
// Command succeeded
break;
default:
fprintf(stderr, "Command %04X/%04X failed with %02X\n",
da2013_cmd.command,
da2013_cmd.request,
da2013_cmd.status);
}
usleep(50 * 1000);
}
}
/*
* @return number of bytes read if function succeeds otherwise -1 if error occurs.
* @throws SerialComException if any JNI function, system call or C function fails.
*/
jint linux_get_feature_report(JNIEnv *env, jlong fd, jbyte reportID, jbyteArray report, jint length) {
int ret = -1;
jbyte* buffer = NULL;;
/* allocate and clear buffer */
buffer = (jbyte *) calloc((length), sizeof(unsigned char));
if(buffer == NULL) {
throw_serialcom_exception(env, 3, 0, E_CALLOCSTR);
return -1;
}
if(reportID < 0) {
/* set 0x00 as 1st byte if device does not support report ID */
buffer[0] = 0x00;
}else {
/* The first byte of SFEATURE and GFEATURE is the report number */
buffer[0] = reportID;
}
errno = 0;
/* this ioctl returns number of bytes read from the device */
ret = ioctl(fd, HIDIOCSFEATURE(length), buffer);
if(ret < 0) {
free(buffer);
throw_serialcom_exception(env, 1, errno, NULL);
return -1;
}
/* copy data from native buffer to Java buffer */
(*env)->SetByteArrayRegion(env, report, 0, ret, buffer);
if((*env)->ExceptionOccurred(env)) {
free(buffer);
throw_serialcom_exception(env, 3, 0, E_SETBYTEARRREGIONSTR);
return -1;
}
free(buffer);
return ret;
}
bool RazerHydra::init(const char *device)
{
int res;
uint8_t buf[256];
struct hidraw_report_descriptor rpt_desc;
struct hidraw_devinfo info;
hidraw_fd = open(device, O_RDWR | O_NONBLOCK);
if (hidraw_fd < 0)
{
ROS_ERROR("couldn't open hidraw device");
return false;
}
ROS_INFO("opened hydra");
memset(&rpt_desc, 0x0, sizeof(rpt_desc));
memset(&info, 0x0, sizeof(info));
memset(buf, 0x0, sizeof(buf));
/*
// Get Report Descriptor Size
ROS_DEBUG("Getting Report Descriptor Size");
res = ioctl(hidraw_fd, HIDIOCGRDESCSIZE, &desc_size);
if (res < 0)
perror("HIDIOCGRDESCSIZE");
else
printf("Report Descriptor Size: %d\n", desc_size);
// Get Report Descriptor
ROS_DEBUG("Getting Report Descriptor");
rpt_desc.size = desc_size;
res = ioctl(hidraw_fd, HIDIOCGRDESC, &rpt_desc);
if (res < 0) {
perror("HIDIOCGRDESC");
} else {
printf("Report Descriptor:\n");
for (size_t i = 0; i < rpt_desc.size; i++)
printf("%hhx ", rpt_desc.value[i]);
puts("\n");
}
*/
/* Get Raw Name */
ROS_DEBUG("Getting Raw Name");
res = ioctl(hidraw_fd, HIDIOCGRAWNAME(256), buf);
if (res < 0)
perror("HIDIOCGRAWNAME");
else
printf("Raw Name: %s\n", buf);
// set feature to start it streaming
memset(buf, 0x0, sizeof(buf));
buf[6] = 1;
buf[8] = 4;
buf[9] = 3;
buf[89] = 6;
int attempt = 0;
for (; attempt < 50; attempt++)
{
res = ioctl(hidraw_fd, HIDIOCSFEATURE(91), buf);
if (res < 0)
{
ROS_ERROR("unable to start streaming");
perror("HIDIOCSFEATURE");
usleep(50000);
}
else
{
ROS_INFO("started streaming");
break;
}
}
ROS_INFO("%d attempts", attempt);
return attempt < 60;
}
static int send_report(int fd, char *buf, size_t len) {
int res = ioctl(fd, HIDIOCSFEATURE(len), buf);
if (res < 0)
perror("HIDIOCSFEATURE failed");
return res;
}
int
main(int argc, char **argv)
{
int fd;
int i, res, desc_size = 0;
char buf[256], *dev;
struct hidraw_report_descriptor rpt_desc;
struct hidraw_devinfo info;
/* Usage check */
if (argc != 3)
{
fprintf(stderr, "Usage: %s <hidraw-dev> <SITL IP>\n", argv[0]);
return 1;
}
/* Device is argv[1] */
dev = argv[1];
/* IP addr is argv[2] */
pwm_init(argv[2]);
/* Open the Device with non-blocking reads. In real life,
don't use a hard coded path; use libudev instead. */
fd = open(dev, O_RDWR|O_NONBLOCK);
if (fd < 0)
{
perror("Unable to open device");
return 1;
}
memset(&rpt_desc, 0x0, sizeof(rpt_desc));
memset(&info, 0x0, sizeof(info));
memset(buf, 0x0, sizeof(buf));
/* Get Report Descriptor Size */
res = ioctl(fd, HIDIOCGRDESCSIZE, &desc_size);
if (res < 0)
{
perror("HIDIOCGRDESCSIZE");
}
else
{
printf("Report Descriptor Size: %d\n", desc_size);
}
/* Get Report Descriptor */
rpt_desc.size = desc_size;
res = ioctl(fd, HIDIOCGRDESC, &rpt_desc);
if (res < 0)
{
perror("HIDIOCGRDESC");
}
else
{
printf("Report Descriptor:\n");
for (i = 0; i < rpt_desc.size; i++)
{
printf("%hhx ", rpt_desc.value[i]);
}
puts("\n");
}
/* Get Raw Name */
res = ioctl(fd, HIDIOCGRAWNAME(256), buf);
if (res < 0)
{
perror("HIDIOCGRAWNAME");
}
else
{
printf("Raw Name: %s\n", buf);
}
/* Get Physical Location */
res = ioctl(fd, HIDIOCGRAWPHYS(256), buf);
if (res < 0)
{
perror("HIDIOCGRAWPHYS");
}
else
{
printf("Raw Phys: %s\n", buf);
}
/* Get Raw Info */
res = ioctl(fd, HIDIOCGRAWINFO, &info);
if (res < 0)
{
perror("HIDIOCGRAWINFO");
}
else
{
printf("Raw Info:\n");
printf("\tbustype: %d (%s)\n",
info.bustype, bus_str(info.bustype));
printf("\tvendor: 0x%04hx\n", info.vendor);
printf("\tproduct: 0x%04hx\n", info.product);
}
/* Set Feature */
buf[0] = 0x9; /* Report Number */
buf[1] = 0xff;
buf[2] = 0xff;
buf[3] = 0xff;
res = ioctl(fd, HIDIOCSFEATURE(4), buf);
if (res < 0)
{
perror("HIDIOCSFEATURE");
}
else
{
printf("ioctl HIDIOCGFEATURE returned: %d\n", res);
}
/* Get Feature */
buf[0] = 0x9; /* Report Number */
res = ioctl(fd, HIDIOCGFEATURE(256), buf);
if (res < 0)
{
perror("HIDIOCGFEATURE");
}
else
{
printf("ioctl HIDIOCGFEATURE returned: %d\n", res);
printf("Report data (not containing the report number):\n\t");
for (i = 0; i < res; i++)
{
printf("%hhx ", buf[i]);
}
puts("\n");
}
/* Send a Report to the Device */
buf[0] = 0x1; /* Report Number */
buf[1] = 0x77;
res = write(fd, buf, 2);
if (res < 0)
{
printf("Error: %d\n", errno);
perror("write");
}
else
{
printf("write() wrote %d bytes\n", res);
}
/* Get a report from the device */
res = read(fd, buf, 16);
if (res < 0)
{
perror("read");
}
else
{
printf("read() read %d bytes:\n\t", res);
for (i = 0; i < res; i++)
{
printf("%hhx ", buf[i]);
}
puts("\n");
}
/* OP loop */
while (1)
{
/* Send a Report to the Device */
buf[0] = 0x0; /* Report Number */
buf[1] = 0x0;
res = write(fd, buf, 2);
if (res < 0)
{
printf("Error: %d\n", errno);
perror("write");
}
/* Get a report from the device */
res = read(fd, buf, 16);
if (res < 0)
{
perror("read");
}
else
{
pwm_send(buf, res);
}
usleep(20 * 1000);
}
close(fd);
return 0;
}
