int cwiid_get_mesg(cwiid_wiimote_t *wiimote, int *mesg_count,
union cwiid_mesg *mesg[], struct timespec *timestamp)
{
struct mesg_array ma;
if (read_mesg_array(wiimote->mesg_pipe[0], &ma)) {
if (errno == EAGAIN) {
return -1;
}
else {
cwiid_err(wiimote, "Pipe read error (mesg_pipe)");
return -1;
}
}
*mesg_count = ma.count;
*timestamp = ma.timestamp;
if ((*mesg = malloc(ma.count * sizeof ma.array[0])) == NULL) {
cwiid_err(wiimote, "Memory allocation error (mesg array)");
return -1;
}
memcpy(*mesg, &ma.array, ma.count * sizeof (*mesg)[0]);
return 0;
}
int write_mesg_array(struct wiimote *wiimote, struct mesg_array *ma)
{
ssize_t len = (void *)&ma->array[ma->count] - (void *)ma;
int ret = 0;
/* This must remain a single write operation to ensure atomicity,
* which is required to avoid mutexes and cancellation issues */
if (write(wiimote->mesg_pipe[1], ma, len) != len) {
if (errno == EAGAIN) {
cwiid_err(wiimote, "Mesg pipe overflow");
if (fcntl(wiimote->mesg_pipe[1], F_SETFL, 0)) {
cwiid_err(wiimote, "File control error (mesg pipe)");
ret = -1;
}
else {
if (write(wiimote->mesg_pipe[1], ma, len) != len) {
cwiid_err(wiimote, "Pipe write error (mesg pipe)");
ret = -1;
}
if (fcntl(wiimote->mesg_pipe[1], F_SETFL, O_NONBLOCK)) {
cwiid_err(wiimote, "File control error (mesg pipe");
}
}
}
else {
cwiid_err(wiimote, "Pipe write error (mesg pipe)");
ret = -1;
}
}
return ret;
}
int cwiid_send_rpt(cwiid_wiimote_t *wiimote, uint8_t flags, uint8_t report,
size_t len, const void *data)
{
unsigned char buf[32];
if (len+2 > sizeof(buf)) {
cwiid_err( wiimote, "cwiid_send_prt: %d bytes over maximum", len+2-sizeof(buf) );
return -1;
}
buf[0] = BT_TRANS_SET_REPORT | BT_PARAM_OUTPUT;
buf[1] = report;
if (len > 0) {
memcpy( &buf[2], data, len );
}
if (!(flags & CWIID_SEND_RPT_NO_RUMBLE)) {
buf[2] |= wiimote->state.rumble;
}
if (write(wiimote->ctl_socket, buf, len+2) != (ssize_t)(len+2)) {
cwiid_err(wiimote, "cwiid_send_rpt: write: %s", strerror(errno));
return -1;
}
else if (verify_handshake(wiimote)) {
return -1;
}
return 0;
}
void *mesg_callback_thread(struct wiimote *wiimote)
{
int mesg_pipe = wiimote->mesg_pipe[0];
cwiid_mesg_callback_t *callback = wiimote->mesg_callback;
struct mesg_array ma;
int cancelstate;
while (1) {
if (read_mesg_array(mesg_pipe, &ma)) {
cwiid_err(wiimote, "Mesg pipe read error");
continue;
}
/* TODO: The callback can still be called once after disconnect,
* although it's very unlikely. User must keep track and avoid
* accessing the wiimote struct after disconnect. */
if (pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &cancelstate)) {
cwiid_err(wiimote, "Cancel state disable error (callback thread)");
}
callback(wiimote, ma.count, ma.array, &ma.timestamp);
if (pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, &cancelstate)) {
cwiid_err(wiimote, "Cancel state restore error (callback thread)");
}
}
return NULL;
}
int cwiid_beep(cwiid_wiimote_t *wiimote)
{
/* unsigned char buf[SOUND_BUF_LEN] = { 0xA0, 0xCC, 0x33, 0xCC, 0x33,
0xCC, 0x33, 0xCC, 0x33, 0xCC, 0x33, 0xCC, 0x33, 0xCC, 0x33, 0xCC, 0x33,
0xCC, 0x33, 0xCC, 0x33}; */
unsigned char buf[SOUND_BUF_LEN] = { 0xA0, 0xC3, 0xC3, 0xC3, 0xC3,
0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3,
0xC3, 0xC3, 0xC3, 0xC3
};
int i;
int ret = 0;
pthread_mutex_t timer_mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_cond_t timer_cond = PTHREAD_COND_INITIALIZER;
struct timespec t;
if (exec_write_seq(wiimote, SEQ_LEN(speaker_enable_seq),
speaker_enable_seq)) {
cwiid_err(wiimote, "Speaker enable error");
ret = -1;
}
pthread_mutex_lock(&timer_mutex);
for (i=0; i<100; i++) {
clock_gettime(CLOCK_REALTIME, &t);
t.tv_nsec += 10204081;
/* t.tv_nsec += 7000000; */
if (cwiid_send_rpt(wiimote, 0, RPT_SPEAKER_DATA, SOUND_BUF_LEN, buf)) {
printf("%d\n", i);
cwiid_err(wiimote, "Report send error (speaker data)");
ret = -1;
break;
}
/* TODO: I should be shot for this, but hey, it works.
* longterm - find a better wait */
pthread_cond_timedwait(&timer_cond, &timer_mutex, &t);
}
pthread_mutex_unlock(&timer_mutex);
if (exec_write_seq(wiimote, SEQ_LEN(speaker_disable_seq),
speaker_disable_seq)) {
cwiid_err(wiimote, "Speaker disable error");
ret = -1;
}
return ret;
}
int cwiid_find_wiimote(bdaddr_t *bdaddr, int timeout)
{
struct cwiid_bdinfo *bdinfo;
int bdinfo_count;
if (timeout == -1) {
while ((bdinfo_count = cwiid_get_bdinfo_array(-1, 2, 1, &bdinfo, 0))
== 0);
if (bdinfo_count == -1) {
return -1;
}
}
else {
bdinfo_count = cwiid_get_bdinfo_array(-1, timeout, 1, &bdinfo, 0);
if (bdinfo_count == -1) {
return -1;
}
else if (bdinfo_count == 0) {
cwiid_err(NULL, "No wiimotes found");
return -1;
}
}
bacpy(bdaddr, &bdinfo[0].bdaddr);
free(bdinfo);
return 0;
}
int cancel_mesg_callback(struct wiimote *wiimote)
{
int ret = 0;
if (pthread_cancel(wiimote->mesg_callback_thread)) {
cwiid_err(wiimote, "Thread cancel error (callback thread)");
ret = -1;
}
if (pthread_detach(wiimote->mesg_callback_thread)) {
cwiid_err(wiimote, "Thread detach error (callback thread)");
ret = -1;
}
return ret;
}
int process_status(struct wiimote *wiimote, const unsigned char *data,
struct mesg_array *ma)
{
struct cwiid_status_mesg status_mesg;
(void)ma;
status_mesg.type = CWIID_MESG_STATUS;
status_mesg.battery = data[5];
if (data[2] & 0x02) {
/* status_thread will figure out what it is */
status_mesg.ext_type = CWIID_EXT_UNKNOWN;
}
else {
status_mesg.ext_type = CWIID_EXT_NONE;
}
if (write(wiimote->status_pipe[1], &status_mesg, sizeof status_mesg)
!= sizeof status_mesg) {
cwiid_err(wiimote, "Status pipe write error");
return -1;
}
return 0;
}
int cwiid_get_state(cwiid_wiimote_t *wiimote, struct cwiid_state *state)
{
if (pthread_mutex_lock(&wiimote->state_mutex)) {
cwiid_err(wiimote, "Mutex lock error (state mutex)");
return -1;
}
memcpy(state, &wiimote->state, sizeof *state);
if (pthread_mutex_unlock(&wiimote->state_mutex)) {
cwiid_err(wiimote, "Mutex unlock error (state mutex) - "
"deadlock warning");
return -1;
}
return 0;
}
int verify_handshake(struct wiimote *wiimote)
{
unsigned char handshake;
if (read(wiimote->ctl_socket, &handshake, 1) != 1) {
cwiid_err(wiimote, "Socket read error (handshake)");
return -1;
}
else if ((handshake & BT_TRANS_MASK) != BT_TRANS_HANDSHAKE) {
cwiid_err(wiimote, "Handshake expected, non-handshake received");
return -1;
}
else if ((handshake & BT_PARAM_MASK) != BT_PARAM_SUCCESSFUL) {
cwiid_err(wiimote, "Non-successful handshake");
return -1;
}
return 0;
}
int cwiid_request_status(cwiid_wiimote_t *wiimote)
{
unsigned char data;
data = 0;
if (cwiid_send_rpt(wiimote, 0, RPT_STATUS_REQ, 1, &data)) {
cwiid_err(wiimote, "Status request error");
return -1;
}
return 0;
}
int cancel_mesg_callback(struct wiimote *wiimote)
{
int err;
err = pthread_cancel(wiimote->mesg_callback_thread);
if (err) {
cwiid_err(wiimote, "Thread cancel error (callback thread): %s", strerror(err));
return -1;
}
return 0;
}
int cancel_rw(struct wiimote *wiimote)
{
struct rw_mesg rw_mesg;
rw_mesg.type = RW_CANCEL;
if (write(wiimote->rw_pipe[1], &rw_mesg, sizeof rw_mesg) !=
sizeof rw_mesg) {
cwiid_err(wiimote, "Pipe write error (rw)");
return -1;
}
return 0;
}
int cwiid_set_rumble(cwiid_wiimote_t *wiimote, uint8_t rumble)
{
unsigned char data;
/* TODO: assumption: char assignments are atomic, no mutex lock needed */
wiimote->state.rumble = rumble ? 1 : 0;
data = wiimote->state.led << 4;
if (cwiid_send_rpt(wiimote, 0, RPT_LED_RUMBLE, 1, &data)) {
cwiid_err(wiimote, "Report send error (led)");
return -1;
}
return 0;
}
int cwiid_get_acc_cal(cwiid_wiimote_t *wiimote, enum cwiid_ext_type ext_type,
struct acc_cal *acc_cal)
{
uint8_t flags;
uint32_t offset;
unsigned char buf[7];
char *err_str;
switch (ext_type) {
case CWIID_EXT_NONE:
flags = CWIID_RW_EEPROM;
offset = 0x16;
err_str = "";
break;
case CWIID_EXT_NUNCHUK:
flags = CWIID_RW_REG;
offset = 0xA40020;
err_str = "nunchuk ";
break;
default:
cwiid_err(wiimote, "Unsupported calibration request");
return -1;
}
if (cwiid_read(wiimote, flags, offset, 7, buf)) {
cwiid_err(wiimote, "Read error (%scal)", err_str);
return -1;
}
acc_cal->zero[CWIID_X] = buf[0];
acc_cal->zero[CWIID_Y] = buf[1];
acc_cal->zero[CWIID_Z] = buf[2];
acc_cal->one[CWIID_X] = buf[4];
acc_cal->one[CWIID_Y] = buf[5];
acc_cal->one[CWIID_Z] = buf[6];
return 0;
}
int cwiid_enable(cwiid_wiimote_t *wiimote, int flags)
{
unsigned char data;
if ((flags & CWIID_FLAG_NONBLOCK) &&
!(wiimote->flags & CWIID_FLAG_NONBLOCK)) {
if (fcntl(wiimote->mesg_pipe[0], F_SETFL, O_NONBLOCK)) {
cwiid_err(wiimote, "File control error (mesg pipe)");
return -1;
}
}
if (flags & CWIID_FLAG_MOTIONPLUS) {
data = 0x04;
cwiid_write(wiimote, CWIID_RW_REG, 0xA600FE, 1, &data);
cwiid_request_status(wiimote);
}
wiimote->flags |= flags;
return 0;
}
int process_error(struct wiimote *wiimote, ssize_t len, struct mesg_array *ma)
{
struct cwiid_error_mesg *error_mesg;
error_mesg = &ma->array[ma->count++].error_mesg;
error_mesg->type = CWIID_MESG_ERROR;
if (len == 0) {
error_mesg->error = CWIID_ERROR_DISCONNECT;
}
else {
error_mesg->error = CWIID_ERROR_COMM;
}
if (cancel_rw(wiimote)) {
cwiid_err(wiimote, "RW cancel error");
}
return 0;
}
/* TODO: fix error reporting - this is public now and
* should report its own errors */
int cwiid_send_rpt(cwiid_wiimote_t *wiimote, uint8_t flags, uint8_t report,
size_t len, const void *data)
{
unsigned char *buf;
if ((buf = malloc((len*2) * sizeof *buf)) == NULL) {
cwiid_err(wiimote, "Memory allocation error (mesg array)");
return -1;
}
if (wiimote->type == WIIMOTE_NEW)
buf[0] = BT_TRANS_DATA | BT_PARAM_OUTPUT;
else
buf[0] = BT_TRANS_SET_REPORT | BT_PARAM_OUTPUT;
buf[1] = report;
memcpy(buf+2, data, len);
if (!(flags & CWIID_SEND_RPT_NO_RUMBLE)) {
buf[2] |= wiimote->state.rumble;
}
// if this is a new version of the wiimote
if (wiimote->type == WIIMOTE_NEW)
{
if (write(wiimote->int_socket, buf, len+2) != (ssize_t)(len+2)) {
free(buf);
return -1;
}
}
// otherwise it is WIIMOTE_OLD which also includes Wii Board
else
{
if (write(wiimote->ctl_socket, buf, len+2) != (ssize_t)(len+2)) {
free(buf);
return -1;
}
else if (verify_handshake(wiimote)) {
free(buf);
return -1;
}
}
return 0;
}
int cwiid_set_mesg_callback(cwiid_wiimote_t *wiimote,
cwiid_mesg_callback_t *callback)
{
if (wiimote->mesg_callback) {
if (cancel_mesg_callback(wiimote)) {
/* prints it's own errors */
return -1;
}
}
wiimote->mesg_callback = callback;
if (wiimote->mesg_callback) {
if (pthread_create(&wiimote->mesg_callback_thread, NULL,
(void *(*)(void *))&mesg_callback_thread, wiimote)) {
cwiid_err(wiimote, "Thread creation error (callback thread)");
return -1;
}
}
return 0;
}
int cwiid_get_balance_cal(cwiid_wiimote_t *wiimote,
struct balance_cal *balance_cal)
{
unsigned char buf[24];
if (cwiid_read(wiimote, CWIID_RW_REG, 0xa40024, 24, buf)) {
cwiid_err(wiimote, "Read error (balancecal)");
return -1;
}
balance_cal->right_top[0] = ((uint16_t)buf[0]<<8 | (uint16_t)buf[1]);
balance_cal->right_bottom[0] = ((uint16_t)buf[2]<<8 | (uint16_t)buf[3]);
balance_cal->left_top[0] = ((uint16_t)buf[4]<<8 | (uint16_t)buf[5]);
balance_cal->left_bottom[0] = ((uint16_t)buf[6]<<8 | (uint16_t)buf[7]);
balance_cal->right_top[1] = ((uint16_t)buf[8]<<8 | (uint16_t)buf[9]);
balance_cal->right_bottom[1] = ((uint16_t)buf[10]<<8 | (uint16_t)buf[11]);
balance_cal->left_top[1] = ((uint16_t)buf[12]<<8 | (uint16_t)buf[13]);
balance_cal->left_bottom[1] = ((uint16_t)buf[14]<<8 | (uint16_t)buf[15]);
balance_cal->right_top[2] = ((uint16_t)buf[16]<<8 | (uint16_t)buf[17]);
balance_cal->right_bottom[2] = ((uint16_t)buf[18]<<8 | (uint16_t)buf[19]);
balance_cal->left_top[2] = ((uint16_t)buf[20]<<8 | (uint16_t)buf[21]);
balance_cal->left_bottom[2] = ((uint16_t)buf[22]<<8 | (uint16_t)buf[23]);
return 0;
}
int process_ext(struct wiimote *wiimote, unsigned char *data,
unsigned char len, struct mesg_array *ma)
{
struct cwiid_nunchuk_mesg *nunchuk_mesg;
struct cwiid_classic_mesg *classic_mesg;
struct cwiid_balance_mesg *balance_mesg;
struct cwiid_motionplus_mesg *motionplus_mesg;
int i;
switch (wiimote->state.ext_type) {
case CWIID_EXT_NONE:
cwiid_err(wiimote, "Received unexpected extension report");
break;
case CWIID_EXT_UNKNOWN:
break;
case CWIID_EXT_NUNCHUK:
if (wiimote->state.rpt_mode & CWIID_RPT_NUNCHUK) {
nunchuk_mesg = &ma->array[ma->count++].nunchuk_mesg;
nunchuk_mesg->type = CWIID_MESG_NUNCHUK;
nunchuk_mesg->stick[CWIID_X] = data[0];
nunchuk_mesg->stick[CWIID_Y] = data[1];
nunchuk_mesg->acc[CWIID_X] = data[2];
nunchuk_mesg->acc[CWIID_Y] = data[3];
nunchuk_mesg->acc[CWIID_Z] = data[4];
nunchuk_mesg->buttons = ~data[5] & NUNCHUK_BTN_MASK;
}
break;
case CWIID_EXT_CLASSIC:
if (wiimote->state.rpt_mode & CWIID_RPT_CLASSIC) {
classic_mesg = &ma->array[ma->count++].classic_mesg;
classic_mesg->type = CWIID_MESG_CLASSIC;
for (i=0; i < 6; i++) {
data[i] = data[i];
}
classic_mesg->l_stick[CWIID_X] = data[0] & 0x3F;
classic_mesg->l_stick[CWIID_Y] = data[1] & 0x3F;
classic_mesg->r_stick[CWIID_X] = (data[0] & 0xC0)>>3 |
(data[1] & 0xC0)>>5 |
(data[2] & 0x80)>>7;
classic_mesg->r_stick[CWIID_Y] = data[2] & 0x1F;
classic_mesg->l = (data[2] & 0x60)>>2 |
(data[3] & 0xE0)>>5;
classic_mesg->r = data[3] & 0x1F;
classic_mesg->buttons = ~((uint16_t)data[4]<<8 |
(uint16_t)data[5]);
}
break;
case CWIID_EXT_BALANCE:
if (wiimote->state.rpt_mode & CWIID_RPT_BALANCE) {
balance_mesg = &ma->array[ma->count++].balance_mesg;
balance_mesg->type = CWIID_MESG_BALANCE;
balance_mesg->right_top = ((uint16_t)data[0]<<8 |
(uint16_t)data[1]);
balance_mesg->right_bottom = ((uint16_t)data[2]<<8 |
(uint16_t)data[3]);
balance_mesg->left_top = ((uint16_t)data[4]<<8 |
(uint16_t)data[5]);
balance_mesg->left_bottom = ((uint16_t)data[6]<<8 |
(uint16_t)data[7]);
}
case CWIID_EXT_MOTIONPLUS:
if (wiimote->state.rpt_mode & CWIID_RPT_MOTIONPLUS) {
motionplus_mesg = &ma->array[ma->count++].motionplus_mesg;
motionplus_mesg->type = CWIID_MESG_MOTIONPLUS;
motionplus_mesg->angle_rate[CWIID_PHI] = ((uint16_t)data[5] & 0xFC)<<6 |
(uint16_t)data[2];
motionplus_mesg->angle_rate[CWIID_THETA] = ((uint16_t)data[4] & 0xFC)<<6 |
(uint16_t)data[1];
motionplus_mesg->angle_rate[CWIID_PSI] = ((uint16_t)data[3] & 0xFC)<<6 |
(uint16_t)data[0];
motionplus_mesg->low_speed[CWIID_PHI] = ((uint8_t)data[3] & 0x01);
motionplus_mesg->low_speed[CWIID_THETA] = ((uint8_t)data[4] & 0x02)>>1;
motionplus_mesg->low_speed[CWIID_PSI] = ((uint8_t)data[3] & 0x02)>>1;
}
cwiid_wiimote_t *cwiid_open_timeout(bdaddr_t *bdaddr, int flags, int timeout)
{
struct sockaddr_l2 remote_addr;
int ctl_socket = -1, int_socket = -1;
struct wiimote *wiimote = NULL;
bdaddr_t any_bdaddr;
/* Treat a null bdaddr as BDADDR_ANY */
if (bdaddr == NULL) {
any_bdaddr = *BDADDR_ANY;
bdaddr = &any_bdaddr;
}
/* If BDADDR_ANY is given, find available wiimote */
if (bacmp(bdaddr, BDADDR_ANY) == 0) {
if (cwiid_find_wiimote(bdaddr, timeout)) {
goto ERR_HND;
}
sleep(1);
}
/* Connect to Wiimote */
/* Control Channel */
memset(&remote_addr, 0, sizeof remote_addr);
remote_addr.l2_family = AF_BLUETOOTH;
bacpy( &remote_addr.l2_bdaddr, bdaddr );
remote_addr.l2_psm = htobs(CTL_PSM);
if ((ctl_socket =
socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) == -1) {
cwiid_err(NULL, "Socket creation error (control socket): %s", strerror(errno));
goto ERR_HND;
}
if (connect(ctl_socket, (struct sockaddr *)&remote_addr,
sizeof remote_addr)) {
cwiid_err(NULL, "Socket connect error (control socket): %s", strerror(errno));
goto ERR_HND;
}
/* Interrupt Channel */
remote_addr.l2_psm = htobs(INT_PSM);
if ((int_socket =
socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) == -1) {
cwiid_err(NULL, "Socket creation error (interrupt socket): %s", strerror(errno));
goto ERR_HND;
}
if (connect(int_socket, (struct sockaddr *)&remote_addr,
sizeof remote_addr)) {
cwiid_err(NULL, "Socket connect error (interrupt socket): %s", strerror(errno));
goto ERR_HND;
}
if ((wiimote = cwiid_new(ctl_socket, int_socket, flags)) == NULL) {
/* Raises its own error */
goto ERR_HND;
}
return wiimote;
ERR_HND:
/* Close Sockets */
if (ctl_socket != -1) {
if (close(ctl_socket)) {
cwiid_err(NULL, "Socket close error (control socket): %s", strerror(errno));
}
}
if (int_socket != -1) {
if (close(int_socket)) {
cwiid_err(NULL, "Socket close error (interrupt socket): %s", strerror(errno));
}
}
return NULL;
}
int cwiid_close(cwiid_wiimote_t *wiimote)
{
void *pthread_ret;
int err;
/* Stop rumbling, otherwise wiimote continues to rumble for
few seconds after closing the connection! There should be no
need to check if stopping fails: we are closing the connection
in any case. */
if (wiimote->state.rumble) {
cwiid_set_rumble(wiimote, 0);
}
/* Cancel and join router_thread and status_thread */
if (pthread_cancel(wiimote->router_thread)) {
/* if thread quit abnormally, would have printed it's own error */
}
err = pthread_join(wiimote->router_thread, &pthread_ret);
if (err) {
cwiid_err(wiimote, "Thread join error (router thread): %s", strerror(err));
}
else if (!((pthread_ret == PTHREAD_CANCELED) || (pthread_ret == NULL))) {
cwiid_err(wiimote, "Bad return value from router thread");
}
if (pthread_cancel(wiimote->status_thread)) {
/* if thread quit abnormally, would have printed it's own error */
}
err = pthread_join(wiimote->status_thread, &pthread_ret);
if (err) {
cwiid_err(wiimote, "Thread join error (status thread): %s", strerror(err));
}
else if (!((pthread_ret == PTHREAD_CANCELED) || (pthread_ret == NULL))) {
cwiid_err(wiimote, "Bad return value from status thread");
}
if (wiimote->mesg_callback) {
if (cancel_mesg_callback(wiimote)) {
/* prints it's own errors */
}
}
if (cancel_rw(wiimote)) {
/* prints it's own errors */
}
/* Close sockets */
if (close(wiimote->int_socket)) {
cwiid_err(wiimote, "Socket close error (interrupt socket): %s", strerror(errno));
}
if (close(wiimote->ctl_socket)) {
cwiid_err(wiimote, "Socket close error (control socket): %s", strerror(errno));
}
/* Close Pipes */
if (close(wiimote->mesg_pipe[0]) || close(wiimote->mesg_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (mesg pipe): %s", strerror(errno));
}
if (close(wiimote->status_pipe[0]) || close(wiimote->status_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (status pipe): %s", strerror(errno));
}
if (close(wiimote->rw_pipe[0]) || close(wiimote->rw_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (rw pipe): %s", strerror(errno));
}
/* Destroy mutexes */
err = pthread_mutex_destroy(&wiimote->state_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (state): %s", strerror(err));
}
err = pthread_mutex_destroy(&wiimote->rw_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (rw): %s", strerror(err));
}
err = pthread_mutex_destroy(&wiimote->rpt_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (rpt): %s", strerror(err));
}
free(wiimote);
return 0;
}
cwiid_wiimote_t *cwiid_new(int ctl_socket, int int_socket, int flags)
{
struct wiimote *wiimote = NULL;
char mesg_pipe_init = 0, status_pipe_init = 0, rw_pipe_init = 0,
state_mutex_init = 0, rw_mutex_init = 0, rpt_mutex_init = 0,
router_thread_init = 0, status_thread_init = 0;
void *pthread_ret;
int err;
/* Allocate wiimote */
if ((wiimote = malloc(sizeof *wiimote)) == NULL) {
cwiid_err(NULL, "Memory allocation error (cwiid_wiimote_t)");
goto ERR_HND;
}
/* set sockets and flags */
wiimote->ctl_socket = ctl_socket;
wiimote->int_socket = int_socket;
wiimote->flags = flags;
/* Global Lock, Store and Increment wiimote_id */
err = pthread_mutex_lock(&global_mutex);
if (err) {
cwiid_err(NULL, "Mutex lock error (global mutex): %s", strerror(err));
goto ERR_HND;
}
wiimote->id = wiimote_id++;
err = pthread_mutex_unlock(&global_mutex);
if (err) {
cwiid_err(wiimote, "Mutex unlock error (global mutex) - "
"deadlock warning: %s", strerror(err));
goto ERR_HND;
}
/* Create pipes */
if (pipe(wiimote->mesg_pipe)) {
cwiid_err(wiimote, "Pipe creation error (mesg pipe): %s", strerror(errno));
goto ERR_HND;
}
mesg_pipe_init = 1;
if (pipe(wiimote->status_pipe)) {
cwiid_err(wiimote, "Pipe creation error (status pipe): %s", strerror(errno));
goto ERR_HND;
}
status_pipe_init = 1;
if (pipe(wiimote->rw_pipe)) {
cwiid_err(wiimote, "Pipe creation error (rw pipe): %s", strerror(errno));
goto ERR_HND;
}
rw_pipe_init = 1;
/* Setup blocking */
if (fcntl(wiimote->mesg_pipe[1], F_SETFL, O_NONBLOCK)) {
cwiid_err(wiimote, "File control error (mesg write pipe): %s", strerror(errno));
goto ERR_HND;
}
if (wiimote->flags & CWIID_FLAG_NONBLOCK) {
if (fcntl(wiimote->mesg_pipe[0], F_SETFL, O_NONBLOCK)) {
cwiid_err(wiimote, "File control error (mesg read pipe): %s", strerror(errno));
goto ERR_HND;
}
}
/* Init mutexes */
err = pthread_mutex_init(&wiimote->state_mutex, NULL);
if (err) {
cwiid_err(wiimote, "Mutex initialization error (state mutex): %s", strerror(err));
goto ERR_HND;
}
state_mutex_init = 1;
err = pthread_mutex_init(&wiimote->rw_mutex, NULL);
if (err) {
cwiid_err(wiimote, "Mutex initialization error (rw mutex): %s", strerror(err));
goto ERR_HND;
}
rw_mutex_init = 1;
err = pthread_mutex_init(&wiimote->rpt_mutex, NULL);
if (err) {
cwiid_err(wiimote, "Mutex initialization error (rpt mutex): %s", strerror(err));
goto ERR_HND;
}
rpt_mutex_init = 1;
/* Set rw_status before starting router thread */
wiimote->rw_status = RW_IDLE;
/* Launch interrupt socket listener and dispatch threads */
err = pthread_create(&wiimote->router_thread, NULL,
(void *(*)(void *))&router_thread, wiimote);
if (err) {
cwiid_err(wiimote, "Thread creation error (router thread): %s", strerror(err));
goto ERR_HND;
}
router_thread_init = 1;
err = pthread_create(&wiimote->status_thread, NULL,
(void *(*)(void *))&status_thread, wiimote);
if (err) {
cwiid_err(wiimote, "Thread creation error (status thread): %s", strerror(err));
goto ERR_HND;
}
status_thread_init = 1;
/* Success! Update state */
memset(&wiimote->state, 0, sizeof wiimote->state);
wiimote->mesg_callback = NULL;
cwiid_set_led(wiimote, 0);
cwiid_request_status(wiimote);
return wiimote;
ERR_HND:
if (wiimote) {
/* Close threads */
if (router_thread_init) {
pthread_cancel(wiimote->router_thread);
err = pthread_join(wiimote->router_thread, &pthread_ret);
if (err) {
cwiid_err(wiimote, "Thread join error (router thread): %s", strerror(err));
}
else if (!((pthread_ret == PTHREAD_CANCELED) &&
(pthread_ret == NULL))) {
cwiid_err(wiimote, "Bad return value from router thread");
}
}
if (status_thread_init) {
pthread_cancel(wiimote->status_thread);
err = pthread_join(wiimote->status_thread, &pthread_ret);
if (err) {
cwiid_err(wiimote, "Thread join error (status thread): %s", strerror(err));
}
else if (!((pthread_ret == PTHREAD_CANCELED) && (pthread_ret == NULL))) {
cwiid_err(wiimote, "Bad return value from status thread");
}
}
/* Close Pipes */
if (mesg_pipe_init) {
if (close(wiimote->mesg_pipe[0]) || close(wiimote->mesg_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (mesg pipe): %s", strerror(errno));
}
}
if (status_pipe_init) {
if (close(wiimote->status_pipe[0]) ||
close(wiimote->status_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (status pipe): %s", strerror(errno));
}
}
if (rw_pipe_init) {
if (close(wiimote->rw_pipe[0]) || close(wiimote->rw_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (rw pipe): %s", strerror(errno));
}
}
/* Destroy Mutexes */
if (state_mutex_init) {
err = pthread_mutex_destroy(&wiimote->state_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (state mutex): %s", strerror(err));
}
}
if (rw_mutex_init) {
err = pthread_mutex_destroy(&wiimote->rw_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (rw mutex): %s", strerror(err));
}
}
if (rpt_mutex_init) {
err = pthread_mutex_destroy(&wiimote->rpt_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (rpt mutex): %s", strerror(err));
}
}
free(wiimote);
}
return NULL;
}
/* timeout in 2 second units */
int cwiid_get_bdinfo_array(int dev_id, unsigned int timeout, int max_bdinfo,
struct cwiid_bdinfo **bdinfo, uint8_t flags)
{
inquiry_info *dev_list = NULL;
int max_inquiry;
int dev_count;
int sock = -1;
int bdinfo_count;
int i, j;
int err = 0;
int ret;
/* NULLify for the benefit of error handling */
*bdinfo = NULL;
/* If not given (=-1), get the first available Bluetooth interface */
if (dev_id == -1) {
if ((dev_id = hci_get_route(NULL)) == -1) {
cwiid_err(NULL, "No Bluetooth interface found");
return -1;
}
}
/* Get Bluetooth Device List */
if ((flags & BT_NO_WIIMOTE_FILTER) && (max_bdinfo != -1)) {
max_inquiry = max_bdinfo;
}
else {
max_inquiry = BT_MAX_INQUIRY;
}
if ((dev_count = hci_inquiry(dev_id, timeout, max_inquiry, NULL,
&dev_list, IREQ_CACHE_FLUSH)) == -1) {
cwiid_err(NULL, "Bluetooth device inquiry error");
err = 1;
goto CODA;
}
if (dev_count == 0) {
bdinfo_count = 0;
goto CODA;
}
/* Open connection to Bluetooth Interface */
if ((sock = hci_open_dev(dev_id)) == -1) {
cwiid_err(NULL, "Bluetooth interface open error");
err = 1;
goto CODA;
}
/* Allocate info list */
if (max_bdinfo == -1) {
max_bdinfo = dev_count;
}
if ((*bdinfo = malloc(max_bdinfo * sizeof **bdinfo)) == NULL) {
cwiid_err(NULL, "Memory allocation error (bdinfo array)");
err = 1;
goto CODA;
}
/* Copy dev_list to bdinfo */
for (bdinfo_count=i=0; (i < dev_count) && (bdinfo_count < max_bdinfo);
i++) {
/* Filter by class */
if (!(flags & BT_NO_WIIMOTE_FILTER) &&
((dev_list[i].dev_class[0] != WIIMOTE_CLASS_0) ||
(dev_list[i].dev_class[1] != WIIMOTE_CLASS_1) ||
(dev_list[i].dev_class[2] != WIIMOTE_CLASS_2))) {
continue;
}
/* timeout (10000) in milliseconds */
if (hci_read_remote_name(sock, &dev_list[i].bdaddr, BT_NAME_LEN,
(*bdinfo)[bdinfo_count].name, 10000)) {
cwiid_err(NULL, "Bluetooth name read error");
err = 1;
goto CODA;
}
/* Filter by name */
if (!(flags & BT_NO_WIIMOTE_FILTER) &&
strncmp((*bdinfo)[bdinfo_count].name, WIIMOTE_NAME, BT_NAME_LEN) &&
strncmp((*bdinfo)[bdinfo_count].name, WIIBALANCE_NAME, BT_NAME_LEN)) {
continue;
}
/* Passed filter, add to bdinfo */
bacpy(&(*bdinfo)[bdinfo_count].bdaddr, &dev_list[i].bdaddr);
for (j=0; j<3; j++) {
(*bdinfo)[bdinfo_count].btclass[j] =
dev_list[i].dev_class[j];
}
bdinfo_count++;
}
if (bdinfo_count == 0) {
free(*bdinfo);
}
else if (bdinfo_count < max_bdinfo) {
if ((*bdinfo = realloc(*bdinfo, bdinfo_count * sizeof **bdinfo))
== NULL) {
cwiid_err(NULL, "Memory reallocation error (bdinfo array)");
err = 1;
goto CODA;
}
}
CODA:
if (dev_list) free(dev_list);
if (sock != -1) hci_close_dev(sock);
if (err) {
if (*bdinfo) free(*bdinfo);
ret = -1;
}
else {
ret = bdinfo_count;
}
return ret;
}
void *router_thread(struct wiimote *wiimote)
{
unsigned char buf[READ_BUF_LEN];
ssize_t len;
struct mesg_array ma;
char err, print_clock_err = 1;
while (1) {
/* Read packet */
len = read(wiimote->int_socket, buf, READ_BUF_LEN);
ma.count = 0;
if (clock_gettime(CLOCK_REALTIME, &ma.timestamp)) {
if (print_clock_err) {
cwiid_err(wiimote, "clock_gettime error");
print_clock_err = 0;
}
}
err = 0;
if ((len == -1) || (len == 0)) {
process_error(wiimote, len, &ma);
write_mesg_array(wiimote, &ma);
/* Quit! */
break;
}
else {
/* Verify first byte (DATA/INPUT) */
if (buf[0] != (BT_TRANS_DATA | BT_PARAM_INPUT)) {
cwiid_err(wiimote, "Invalid packet type");
}
/* Main switch */
/* printf("%.2X %.2X %.2X %.2X %.2X %.2X %.2X %.2X\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]);
printf("%.2X %.2X %.2X %.2X %.2X %.2X %.2X %.2X\n", buf[8], buf[9], buf[10], buf[11], buf[12], buf[13], buf[14], buf[15]);
printf("%.2X %.2X %.2X %.2X %.2X %.2X %.2X %.2X\n", buf[16], buf[17], buf[18], buf[19], buf[20], buf[21], buf[22], buf[23]);
printf("\n"); */
switch (buf[1]) {
case RPT_STATUS:
err = process_status(wiimote, &buf[2], &ma);
break;
case RPT_BTN:
err = process_btn(wiimote, &buf[2], &ma);
break;
case RPT_BTN_ACC:
err = process_btn(wiimote, &buf[2], &ma) ||
process_acc(wiimote, &buf[4], &ma);
break;
case RPT_BTN_EXT8:
err = process_btn(wiimote, &buf[2], &ma) ||
process_ext(wiimote, &buf[4], 8, &ma);
break;
case RPT_BTN_ACC_IR12:
err = process_btn(wiimote, &buf[2], &ma) ||
process_acc(wiimote, &buf[4], &ma) ||
process_ir12(wiimote, &buf[7], &ma);
break;
case RPT_BTN_EXT19:
err = process_btn(wiimote, &buf[2], &ma) ||
process_ext(wiimote, &buf[4], 19, &ma);
break;
case RPT_BTN_ACC_EXT16:
err = process_btn(wiimote, &buf[2], &ma) ||
process_acc(wiimote, &buf[4], &ma) ||
process_ext(wiimote, &buf[7], 16, &ma);
break;
case RPT_BTN_IR10_EXT9:
err = process_btn(wiimote, &buf[2], &ma) ||
process_ir10(wiimote, &buf[4], &ma) ||
process_ext(wiimote, &buf[14], 9, &ma);
break;
case RPT_BTN_ACC_IR10_EXT6:
err = process_btn(wiimote, &buf[2], &ma) ||
process_acc(wiimote, &buf[4], &ma) ||
process_ir10(wiimote, &buf[7], &ma) ||
process_ext(wiimote, &buf[17], 6, &ma);
break;
case RPT_EXT21:
err = process_ext(wiimote, &buf[2], 21, &ma);
break;
case RPT_BTN_ACC_IR36_1:
case RPT_BTN_ACC_IR36_2:
cwiid_err(wiimote, "Unsupported report type received "
"(interleaved data)");
err = 1;
break;
case RPT_READ_DATA:
err = process_read(wiimote, &buf[4]) ||
process_btn(wiimote, &buf[2], &ma);
break;
case RPT_WRITE_ACK:
err = process_write(wiimote, &buf[2]);
break;
default:
cwiid_err(wiimote, "Unknown message type");
err = 1;
break;
}
if (!err && (ma.count > 0)) {
if (update_state(wiimote, &ma)) {
cwiid_err(wiimote, "State update error");
}
if (wiimote->flags & CWIID_FLAG_MESG_IFC) {
/* prints its own errors */
write_mesg_array(wiimote, &ma);
}
}
}
}
return NULL;
}
int update_state(struct wiimote *wiimote, struct mesg_array *ma)
{
int i;
union cwiid_mesg *mesg;
if (pthread_mutex_lock(&wiimote->state_mutex)) {
cwiid_err(wiimote, "Mutex lock error (state mutex)");
return -1;
}
for (i=0; i < ma->count; i++) {
mesg = &ma->array[i];
switch (mesg->type) {
case CWIID_MESG_STATUS:
wiimote->state.battery = mesg->status_mesg.battery;
if (wiimote->state.ext_type != mesg->status_mesg.ext_type) {
memset(&wiimote->state.ext, 0, sizeof wiimote->state.ext);
wiimote->state.ext_type = mesg->status_mesg.ext_type;
}
break;
case CWIID_MESG_BTN:
wiimote->state.buttons = mesg->btn_mesg.buttons;
break;
case CWIID_MESG_ACC:
memcpy(wiimote->state.acc, mesg->acc_mesg.acc,
sizeof wiimote->state.acc);
break;
case CWIID_MESG_IR:
memcpy(wiimote->state.ir_src, mesg->ir_mesg.src,
sizeof wiimote->state.ir_src);
break;
case CWIID_MESG_NUNCHUK:
memcpy(wiimote->state.ext.nunchuk.stick,
mesg->nunchuk_mesg.stick,
sizeof wiimote->state.ext.nunchuk.stick);
memcpy(wiimote->state.ext.nunchuk.acc,
mesg->nunchuk_mesg.acc,
sizeof wiimote->state.ext.nunchuk.acc);
wiimote->state.ext.nunchuk.buttons = mesg->nunchuk_mesg.buttons;
break;
case CWIID_MESG_CLASSIC:
memcpy(wiimote->state.ext.classic.l_stick,
mesg->classic_mesg.l_stick,
sizeof wiimote->state.ext.classic.l_stick);
memcpy(wiimote->state.ext.classic.r_stick,
mesg->classic_mesg.r_stick,
sizeof wiimote->state.ext.classic.r_stick);
wiimote->state.ext.classic.l = mesg->classic_mesg.l;
wiimote->state.ext.classic.r = mesg->classic_mesg.r;
wiimote->state.ext.classic.buttons = mesg->classic_mesg.buttons;
break;
case CWIID_MESG_BALANCE:
wiimote->state.ext.balance.right_top = mesg->balance_mesg.right_top;
wiimote->state.ext.balance.right_bottom = mesg->balance_mesg.right_bottom;
wiimote->state.ext.balance.left_top = mesg->balance_mesg.left_top;
wiimote->state.ext.balance.left_bottom = mesg->balance_mesg.left_bottom;
break;
case CWIID_MESG_ERROR:
wiimote->state.error = mesg->error_mesg.error;
break;
case CWIID_MESG_UNKNOWN:
/* do nothing, error has already been printed */
break;
}
}
if (pthread_mutex_unlock(&wiimote->state_mutex)) {
cwiid_err(wiimote, "Mutex unlock error (state mutex) - "
"deadlock warning");
return -1;
}
return 0;
}
void *status_thread(struct wiimote *wiimote)
{
struct mesg_array ma;
struct cwiid_status_mesg *status_mesg;
unsigned char buf[2];
unsigned char data[2];
ma.count = 1;
status_mesg = &ma.array[0].status_mesg;
while (1) {
if (full_read(wiimote->status_pipe[0], status_mesg,
sizeof *status_mesg)) {
cwiid_err(wiimote, "Pipe read error (status)");
/* Quit! */
break;
}
if (status_mesg->type != CWIID_MESG_STATUS) {
cwiid_err(wiimote, "Bad message on status pipe");
continue;
}
if (status_mesg->ext_type == CWIID_EXT_UNKNOWN) {
/* Read extension ID */
if (cwiid_read(wiimote, CWIID_RW_REG, 0xA400FE, 2, &buf)) {
cwiid_err(wiimote, "Read error (extension error)");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
/* If we have just disabled passthrough override passthrough with just plain motionplus */
/*if ((!wiimote->passthrough_activate_flag && ((buf[0] << 8) | buf[1]) == EXT_NUNCHUK_MOTIONPLUS) ||
(!wiimote->passthrough_activate_flag && ((buf[0] << 8) | buf[1]) == EXT_CLASSIC_MOTIONPLUS)) {
fprintf(stderr, "status_thread: reverting to motionplus\n");
buf[0] = EXT_MOTIONPLUS;
}*/
/* If the extension didn't change, or if the extension is a
* MotionPlus, no init necessary */
switch ((buf[0] << 8) | buf[1]) {
case EXT_NONE:
status_mesg->ext_type = CWIID_EXT_NONE;
break;
case EXT_NUNCHUK:
if (wiimote->ext == MOTIONPLUS_PRESENT && wiimote->passthrough_activate_flag){
data[0] = 0x05;
cwiid_write(wiimote, CWIID_RW_REG, 0xA600FE, 1, &data[0]);
cwiid_read(wiimote, CWIID_RW_REG, 0xA400FE, 1, &data[1]);
if (data[1] == 0x05) {
status_mesg->ext_type = CWIID_EXT_NUNCHUK_MPLUS_PASSTHROUGH;
wiimote->ext = NUNCHUK_MOTIONPLUS_PRESENT;
break;
}
}else{
status_mesg->ext_type = CWIID_EXT_NUNCHUK;
wiimote->ext = EXT_PRESENT_NOT_MOTIONPLUS;
break;
}
case EXT_NUNCHUK_MOTIONPLUS:
status_mesg->ext_type = CWIID_EXT_NUNCHUK_MPLUS_PASSTHROUGH;
wiimote->ext = NUNCHUK_MOTIONPLUS_PRESENT;
break;
case EXT_CLASSIC:
if (wiimote->ext == MOTIONPLUS_PRESENT && wiimote->passthrough_activate_flag){
data[0] = 0x07;
cwiid_write(wiimote, CWIID_RW_REG, 0xA600FE, 1, &data[0]);
cwiid_read(wiimote, CWIID_RW_REG, 0xA400FE, 1, &data[1]);
if (data[1] == 0x07) {
status_mesg->ext_type = CWIID_EXT_CLASSIC_MPLUS_PASSTHROUGH;
wiimote->ext = CLASSIC_MOTIONPLUS_PRESENT;
break;
}
}else{
status_mesg->ext_type = CWIID_EXT_CLASSIC;
wiimote->ext = EXT_PRESENT_NOT_MOTIONPLUS;
break;
}
case EXT_CLASSIC_MOTIONPLUS:
status_mesg->ext_type = CWIID_EXT_CLASSIC_MPLUS_PASSTHROUGH;
wiimote->ext = CLASSIC_MOTIONPLUS_PRESENT;
break;
case EXT_BALANCE:
status_mesg->ext_type = CWIID_EXT_BALANCE;
wiimote->ext = EXT_PRESENT_NOT_MOTIONPLUS;
break;
case EXT_MOTIONPLUS:
status_mesg->ext_type = CWIID_EXT_MOTIONPLUS;
wiimote->ext = MOTIONPLUS_PRESENT;
break;
case EXT_PARTIAL:
/* Everything (but MotionPlus) shows up as partial until initialized */
buf[0] = 0x55;
buf[1] = 0x00;
/* Initialize extension register space */
if (cwiid_write(wiimote, CWIID_RW_REG, 0xA400F0, 1, &buf[0])) {
cwiid_err(wiimote, "Extension initialization error");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
else if (cwiid_write(wiimote, CWIID_RW_REG, 0xA400FB, 1, &buf[1])) {
cwiid_err(wiimote, "Extension initialization error");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
/* Read extension ID */
else if (cwiid_read(wiimote, CWIID_RW_REG, 0xA400FE, 2, &buf)) {
cwiid_err(wiimote, "Read error (extension error)");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
else {
switch ((buf[0] << 8) | buf[1]) {
case EXT_NONE:
case EXT_PARTIAL:
status_mesg->ext_type = CWIID_EXT_NONE;
break;
case EXT_NUNCHUK:
status_mesg->ext_type = CWIID_EXT_NUNCHUK;
break;
case EXT_CLASSIC:
status_mesg->ext_type = CWIID_EXT_CLASSIC;
break;
case EXT_BALANCE:
status_mesg->ext_type = CWIID_EXT_BALANCE;
break;
case EXT_NUNCHUK_MOTIONPLUS:
status_mesg->ext_type = CWIID_EXT_NUNCHUK_MPLUS_PASSTHROUGH;
break;
case EXT_CLASSIC_MOTIONPLUS:
status_mesg->ext_type = CWIID_EXT_CLASSIC_MPLUS_PASSTHROUGH;
break;
default:
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
break;
}
}
break;
}
}
else {
//fprintf(stderr,"ext_type=%d ext=%d\n", status_mesg->ext_type, wiimote->ext);
if (!(wiimote->passthrough_activate_flag && wiimote->ext == MOTIONPLUS_PRESENT) ||
!wiimote->passthrough_activate_flag) {
//fprintf(stderr,"NO EXTENSION\n");
wiimote->ext = NO_EXTENSION;
}
pthread_cond_signal(&wiimote->condition_var);
}
if (update_state(wiimote, &ma)) {
cwiid_err(wiimote, "State update error");
}
if (update_rpt_mode(wiimote, -1)) {
cwiid_err(wiimote, "Error reseting report mode");
}
if ((wiimote->state.rpt_mode & CWIID_RPT_STATUS) &&
(wiimote->flags & CWIID_FLAG_MESG_IFC)) {
if (write_mesg_array(wiimote, &ma)) {
/* prints its own errors */
}
}
}
return NULL;
}
cwiid_wiimote_t *cwiid_listen(int flags)
{
struct sockaddr_l2 local_addr;
struct sockaddr_l2 remote_addr;
socklen_t socklen;
int ctl_server_socket = -1, int_server_socket = -1,
ctl_socket = -1, int_socket = -1;
struct wiimote *wiimote = NULL;
/* Connect to Wiimote */
/* Control Channel */
memset(&local_addr, 0, sizeof local_addr);
local_addr.l2_family = AF_BLUETOOTH;
local_addr.l2_bdaddr = *BDADDR_ANY;
local_addr.l2_psm = htobs(CTL_PSM);
if ((ctl_server_socket =
socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) == -1) {
cwiid_err(NULL, "Socket creation error (control socket): %s", strerror(errno));
goto ERR_HND;
}
if (bind(ctl_server_socket, (struct sockaddr *)&local_addr,
sizeof local_addr)) {
cwiid_err(NULL, "Socket bind error (control socket): %s", strerror(errno));
goto ERR_HND;
}
if (listen(ctl_server_socket, 1)) {
cwiid_err(NULL, "Socket listen error (control socket): %s", strerror(errno));
goto ERR_HND;
}
/* Interrupt Channel */
local_addr.l2_psm = htobs(INT_PSM);
if ((int_server_socket =
socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) == -1) {
cwiid_err(NULL, "Socket creation error (interrupt socket): %s", strerror(errno));
goto ERR_HND;
}
if (bind(int_server_socket, (struct sockaddr *)&local_addr,
sizeof local_addr)) {
cwiid_err(NULL, "Socket bind error (interrupt socket): %s", strerror(errno));
goto ERR_HND;
}
if (listen(int_server_socket, 1)) {
cwiid_err(NULL, "Socket listen error (interrupt socket): %s", strerror(errno));
goto ERR_HND;
}
/* Block for Connections */
if ((ctl_socket = accept(ctl_server_socket, (struct sockaddr *)&remote_addr, &socklen)) < 0) {
cwiid_err(NULL, "Socket accept error (control socket): %s", strerror(errno));
goto ERR_HND;
}
if ((int_socket = accept(int_server_socket, (struct sockaddr *)&remote_addr, &socklen)) < 0) {
cwiid_err(NULL, "Socket accept error (interrupt socket): %s", strerror(errno));
goto ERR_HND;
}
/* Close server sockets */
if (close(ctl_server_socket)) {
cwiid_err(NULL, "Socket close error (control socket): %s", strerror(errno));
}
if (close(int_server_socket)) {
cwiid_err(NULL, "Socket close error (interrupt socket): %s", strerror(errno));
}
if ((wiimote = cwiid_new(ctl_socket, int_socket, flags)) == NULL) {
/* Raises its own error */
goto ERR_HND;
}
return wiimote;
ERR_HND:
/* Close Sockets */
if (ctl_server_socket != -1) {
if (close(ctl_server_socket)) {
cwiid_err(NULL, "Socket close error (control server socket): %s", strerror(errno));
}
}
if (int_server_socket != -1) {
if (close(int_server_socket)) {
cwiid_err(NULL, "Socket close error (interrupt server socket): %s", strerror(errno));
}
}
if (ctl_socket != -1) {
if (close(ctl_socket)) {
cwiid_err(NULL, "Socket close error (control socket): %s", strerror(errno));
}
}
if (int_socket != -1) {
if (close(int_socket)) {
cwiid_err(NULL, "Socket close error (interrupt socket): %s", strerror(errno));
}
}
return NULL;
}
void *status_thread(struct wiimote *wiimote)
{
struct mesg_array ma;
struct cwiid_status_mesg *status_mesg;
unsigned char buf[2];
ma.count = 1;
status_mesg = &ma.array[0].status_mesg;
while (1) {
if (full_read(wiimote->status_pipe[0], status_mesg,
sizeof *status_mesg)) {
cwiid_err(wiimote, "Pipe read error (status)");
/* Quit! */
break;
}
if (status_mesg->type != CWIID_MESG_STATUS) {
cwiid_err(wiimote, "Bad message on status pipe");
continue;
}
if (status_mesg->ext_type == CWIID_EXT_UNKNOWN) {
/* Read extension ID */
if (cwiid_read(wiimote, CWIID_RW_REG, 0xA400FE, 1, &buf[0])) {
cwiid_err(wiimote, "Read error (extension error)");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
/* If the extension didn't change, or if the extension is a
* MotionPlus, no init necessary */
switch (buf[0]) {
case EXT_NONE:
status_mesg->ext_type = CWIID_EXT_NONE;
break;
case EXT_NUNCHUK:
status_mesg->ext_type = CWIID_EXT_NUNCHUK;
break;
case EXT_CLASSIC:
status_mesg->ext_type = CWIID_EXT_CLASSIC;
break;
case EXT_BALANCE:
status_mesg->ext_type = CWIID_EXT_BALANCE;
break;
case EXT_MOTIONPLUS:
status_mesg->ext_type = CWIID_EXT_MOTIONPLUS;
break;
case EXT_PARTIAL:
/* Everything (but MotionPlus) shows up as partial until initialized */
buf[0] = 0x55;
buf[1] = 0x00;
/* Initialize extension register space */
if (cwiid_write(wiimote, CWIID_RW_REG, 0xA400F0, 1, &buf[0])) {
cwiid_err(wiimote, "Extension initialization error");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
else if (cwiid_write(wiimote, CWIID_RW_REG, 0xA400FB, 1, &buf[1])) {
cwiid_err(wiimote, "Extension initialization error");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
/* Read extension ID */
else if (cwiid_read(wiimote, CWIID_RW_REG, 0xA400FE, 1, &buf[0])) {
cwiid_err(wiimote, "Read error (extension error)");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
else {
switch (buf[0]) {
case EXT_NONE:
case EXT_PARTIAL:
status_mesg->ext_type = CWIID_EXT_NONE;
break;
case EXT_NUNCHUK:
status_mesg->ext_type = CWIID_EXT_NUNCHUK;
break;
case EXT_CLASSIC:
status_mesg->ext_type = CWIID_EXT_CLASSIC;
break;
case EXT_BALANCE:
status_mesg->ext_type = CWIID_EXT_BALANCE;
break;
default:
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
break;
}
}
break;
}
}
if (update_state(wiimote, &ma)) {
cwiid_err(wiimote, "State update error");
}
if (update_rpt_mode(wiimote, -1)) {
cwiid_err(wiimote, "Error reseting report mode");
}
if ((wiimote->state.rpt_mode & CWIID_RPT_STATUS) &&
(wiimote->flags & CWIID_FLAG_MESG_IFC)) {
if (write_mesg_array(wiimote, &ma)) {
/* prints its own errors */
}
}
}
return NULL;
}
