int wiiuse_io_write(struct wiimote_t* wm, byte* buf, int len) {
DWORD bytes;
int i;
if (!wm || !WIIMOTE_IS_CONNECTED(wm))
return 0;
switch (wm->stack) {
case WIIUSE_STACK_UNKNOWN:
{
/* try to auto-detect the stack type */
if (i = WriteFile(wm->dev_handle, buf, 22, &bytes, &wm->hid_overlap)) {
/* bluesoleil will always return 1 here, even if it's not connected */
wm->stack = WIIUSE_STACK_BLUESOLEIL;
return i;
}
if (i = HidD_SetOutputReport(wm->dev_handle, buf, len)) {
wm->stack = WIIUSE_STACK_MS;
return i;
}
WIIUSE_ERROR("Unable to determine bluetooth stack type.");
return 0;
}
case WIIUSE_STACK_MS:
return HidD_SetOutputReport(wm->dev_handle, buf, len);
case WIIUSE_STACK_BLUESOLEIL:
return WriteFile(wm->dev_handle, buf, 22, &bytes, &wm->hid_overlap);
}
return 0;
}
int wiiuse_os_read(struct wiimote_t* wm, byte* buf, int len) {
int rc;
int i;
rc = read(wm->in_sock, buf, len);
if (rc == -1) {
/* error reading data */
WIIUSE_ERROR("Receiving wiimote data (id %i).", wm->unid);
perror("Error Details");
if (errno == ENOTCONN) {
/* this can happen if the bluetooth dongle is disconnected */
WIIUSE_ERROR("Bluetooth appears to be disconnected. Wiimote unid %i will be disconnected.", wm->unid);
wiiuse_os_disconnect(wm);
wiiuse_disconnected(wm);
}
} else if (rc == 0) {
/* remote disconnect */
wiiuse_disconnected(wm);
} else {
/* read successful */
/* on *nix we ignore the first byte */
memmove(buf, buf + 1, len - 1);
/* log the received data */
#ifdef WITH_WIIUSE_DEBUG
{
int i;
printf("[DEBUG] (id %i) RECV: (%.2x) ", wm->unid, buf[0]);
for (i = 1; i < rc; i++) {
printf("%.2x ", buf[i]);
}
printf("\n");
}
#endif
}
return rc;
}
/**
* @brief Write data to the wiimote.
*
* @param wm Pointer to a wiimote_t structure.
* @param addr The address to write to.
* @param data The data to be written to the memory location.
* @param len The length of the block to be written.
*/
int wiiuse_write_data(struct wiimote_t* wm, unsigned int addr, const byte* data, byte len) {
byte buf[21] = {0}; /* the payload is always 23 */
byte * bufPtr = buf;
if (!wm || !WIIMOTE_IS_CONNECTED(wm)) {
WIIUSE_ERROR("Attempt to write, but no wiimote available or not connected!");
return 0;
}
if (!data || !len) {
WIIUSE_ERROR("Attempt to write, but no data or length == 0");
return 0;
}
WIIUSE_DEBUG("Writing %i bytes to memory location 0x%x...", len, addr);
#ifdef WITH_WIIUSE_DEBUG
{
int i = 0;
printf("Write data is: ");
for (; i < len; ++i) {
printf("%x ", data[i]);
}
printf("\n");
}
#endif
/* the offset is in big endian */
buffer_big_endian_uint32_t(&bufPtr, (uint32_t)addr);
/* length */
buffer_big_endian_uint8_t(&bufPtr, len);
/* data */
memcpy(bufPtr, data, len);
wiiuse_send(wm, WM_CMD_WRITE_DATA, buf, 21);
return 1;
}
/**
* @brief Set if the wiimote should track IR targets.
*
* @param wm Pointer to a wiimote_t structure.
* @param status 1 to enable, 0 to disable.
*/
void wiiuse_set_ir(struct wiimote_t* wm, int status) {
byte buf;
char* block1 = NULL;
char* block2 = NULL;
int ir_level;
if (!wm)
return;
/*
* Wait for the handshake to finish first.
* When it handshake finishes and sees that
* IR is enabled, it will call this function
* again to actually enable IR.
*/
if (!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE)) {
if(status) {
WIIUSE_DEBUG("Tried to enable IR, will wait until handshake finishes.");
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR);
} // else ignoring request to turn off, since it's turned off by default
return;
}
/*
* Check to make sure a sensitivity setting is selected.
*/
ir_level = get_ir_sens(wm, &block1, &block2);
if (!ir_level) {
WIIUSE_ERROR("No IR sensitivity setting selected.");
return;
}
if (status) {
/* if already enabled then stop */
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR))
return;
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR);
} else {
/* if already disabled then stop */
if (!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR))
return;
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
}
/* set camera 1 and 2 */
buf = (status ? 0x04 : 0x00);
wiiuse_send(wm, WM_CMD_IR, &buf, 1);
wiiuse_send(wm, WM_CMD_IR_2, &buf, 1);
if (!status) {
WIIUSE_DEBUG("Disabled IR cameras for wiimote id %i.", wm->unid);
wiiuse_set_report_type(wm);
return;
}
/* enable IR, set sensitivity */
buf = 0x08;
wiiuse_write_data(wm, WM_REG_IR, &buf, 1);
/* wait for the wiimote to catch up */
#ifndef WIN32
usleep(50000);
#else
Sleep(50);
#endif
/* write sensitivity blocks */
wiiuse_write_data(wm, WM_REG_IR_BLOCK1, (byte*)block1, 9);
wiiuse_write_data(wm, WM_REG_IR_BLOCK2, (byte*)block2, 2);
/* set the IR mode */
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_EXP))
buf = WM_IR_TYPE_BASIC;
else
buf = WM_IR_TYPE_EXTENDED;
wiiuse_write_data(wm, WM_REG_IR_MODENUM, &buf, 1);
#ifndef WIN32
usleep(50000);
#else
Sleep(50);
#endif
/* set the wiimote report type */
wiiuse_set_report_type(wm);
WIIUSE_DEBUG("Enabled IR camera for wiimote id %i (sensitivity level %i).", wm->unid, ir_level);
}
int wiiuse_os_find(struct wiimote_t** wm, int max_wiimotes, int timeout) {
int device_id;
int device_sock;
inquiry_info scan_info_arr[128];
inquiry_info* scan_info = scan_info_arr;
int found_devices;
int found_wiimotes;
int i = 0;
/* reset all wiimote bluetooth device addresses */
for (found_wiimotes = 0; found_wiimotes < max_wiimotes; ++found_wiimotes) {
/* bacpy(&(wm[found_wiimotes]->bdaddr), BDADDR_ANY); */
memset(&(wm[found_wiimotes]->bdaddr), 0, sizeof(bdaddr_t));
}
found_wiimotes = 0;
/* get the id of the first bluetooth device. */
device_id = hci_get_route(NULL);
if (device_id < 0) {
if (errno == ENODEV) {
WIIUSE_ERROR("Could not detect a Bluetooth adapter!");
} else {
perror("hci_get_route");
}
return 0;
}
/* create a socket to the device */
device_sock = hci_open_dev(device_id);
if (device_sock < 0) {
perror("hci_open_dev");
return 0;
}
memset(&scan_info_arr, 0, sizeof(scan_info_arr));
/* scan for bluetooth devices for 'timeout' seconds */
found_devices = hci_inquiry(device_id, timeout, 128, NULL, &scan_info, IREQ_CACHE_FLUSH);
if (found_devices < 0) {
perror("hci_inquiry");
return 0;
}
WIIUSE_INFO("Found %i bluetooth device(s).", found_devices);
/* display discovered devices */
for (i = 0; (i < found_devices) && (found_wiimotes < max_wiimotes); ++i) {
if ((scan_info[i].dev_class[0] == WM_DEV_CLASS_0) &&
(scan_info[i].dev_class[1] == WM_DEV_CLASS_1) &&
(scan_info[i].dev_class[2] == WM_DEV_CLASS_2)) {
/* found a device */
ba2str(&scan_info[i].bdaddr, wm[found_wiimotes]->bdaddr_str);
WIIUSE_INFO("Found wiimote (%s) [id %i].", wm[found_wiimotes]->bdaddr_str, wm[found_wiimotes]->unid);
wm[found_wiimotes]->bdaddr = scan_info[i].bdaddr;
WIIMOTE_ENABLE_STATE(wm[found_wiimotes], WIIMOTE_STATE_DEV_FOUND);
++found_wiimotes;
}
}
close(device_sock);
return found_wiimotes;
}
int wiiuse_os_poll(struct wiimote_t** wm, int wiimotes) {
int evnt;
struct timeval tv;
fd_set fds;
int r;
int i;
byte read_buffer[MAX_PAYLOAD];
int highest_fd = -1;
evnt = 0;
if (!wm) {
return 0;
}
/* block select() for 1/2000th of a second */
tv.tv_sec = 0;
tv.tv_usec = 500;
FD_ZERO(&fds);
for (i = 0; i < wiimotes; ++i) {
/* only poll it if it is connected */
if (WIIMOTE_IS_SET(wm[i], WIIMOTE_STATE_CONNECTED)) {
FD_SET(wm[i]->in_sock, &fds);
/* find the highest fd of the connected wiimotes */
if (wm[i]->in_sock > highest_fd) {
highest_fd = wm[i]->in_sock;
}
}
wm[i]->event = WIIUSE_NONE;
}
if (highest_fd == -1)
/* nothing to poll */
{
return 0;
}
if (select(highest_fd + 1, &fds, NULL, NULL, &tv) == -1) {
WIIUSE_ERROR("Unable to select() the wiimote interrupt socket(s).");
perror("Error Details");
return 0;
}
/* check each socket for an event */
for (i = 0; i < wiimotes; ++i) {
/* if this wiimote is not connected, skip it */
if (!WIIMOTE_IS_CONNECTED(wm[i])) {
continue;
}
if (FD_ISSET(wm[i]->in_sock, &fds)) {
/* clear out the event buffer */
memset(read_buffer, 0, sizeof(read_buffer));
/* clear out any old read data */
clear_dirty_reads(wm[i]);
/* read the pending message into the buffer */
r = wiiuse_os_read(wm[i], read_buffer, sizeof(read_buffer));
if (r > 0) {
/* propagate the event */
propagate_event(wm[i], read_buffer[0], read_buffer + 1);
evnt += (wm[i]->event != WIIUSE_NONE);
}
} else {
/* send out any waiting writes */
wiiuse_send_next_pending_write_request(wm[i]);
idle_cycle(wm[i]);
}
}
return evnt;
}
