wiimote.c

linux下wii的鼠标驱动

/*
 * File that defines the wiimote functions declared in wiimote.h
 * Kendall Merriman
 */

#include "wiimote.h"
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>
#include <bluetooth/l2cap.h>
#include <sys/socket.h>
#include <unistd.h>
#include <stdlib.h>

#define CTL_PSM 17
#define INT_PSM 19

int bt_dev_id = 0;
int bt_main_sock = 0;

int initialize_wiimote_support()
{
    bt_dev_id = hci_get_route(NULL);
    bt_main_sock = hci_open_dev(bt_dev_id);
    if(bt_dev_id < 0 || bt_main_sock < 0)
        return -1;
    return 0;
}

void cleanup_wiimote_support()
{
    close(bt_main_sock);
}

int is_wiimote_name(char* name)
{
    if(!strcmp(name, "Nintendo RVL-CNT-01"))
        return 1;
    return 0;
}

int find_wiimotes(wiimote_t* wiimotes, int num, int timeout)
{
    inquiry_info* ii = NULL;
    int max_rsp = num * 3;
    int num_rsp = 0;
    ii = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info));

    num_rsp = hci_inquiry(bt_dev_id, 8, max_rsp, NULL, &ii, IREQ_CACHE_FLUSH);
    if(num_rsp < 0)
    {
        //...uhhhh.....crap!
        return -1;
    }

    char name[248];
    int num_wiimotes = 0;
    int i;
    for(i = 0; i < num_rsp; i++)
    {
        memset(name, 0, sizeof(name));
        if(hci_read_remote_name(bt_main_sock, &(ii+i)->bdaddr, sizeof(name), name, 0) < 0)
            continue;
        if(is_wiimote_name(name))
        {
            //we found a wiimote!!!
            memcpy(&wiimotes[num_wiimotes].addr, &(ii+i)->bdaddr, sizeof((ii+i)->bdaddr));
            num_wiimotes++;
        }
    }
    return num_wiimotes;
}

int connect_wiimotes(wiimote_t* wiimotes, int num)
{
    int i;
    for(i = 0; i < num; i++)
    {
        struct sockaddr_l2 ctl_remote_addr, int_remote_addr;
        memset(&ctl_remote_addr, 0, sizeof(struct sockaddr_l2));
        memset(&int_remote_addr, 0, sizeof(struct sockaddr_l2));
        ctl_remote_addr.l2_family = AF_BLUETOOTH;
        ctl_remote_addr.l2_bdaddr = wiimotes[i].addr;
        ctl_remote_addr.l2_psm = htobs(CTL_PSM);

        int_remote_addr.l2_family = AF_BLUETOOTH;
        int_remote_addr.l2_bdaddr = wiimotes[i].addr;
        int_remote_addr.l2_psm = htobs(INT_PSM);

        wiimotes[i].int_sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
        wiimotes[i].ctl_sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
        if(wiimotes[i].int_sock == -1 || wiimotes[i].ctl_sock == -1)
        {
            return -1;
        }
        if(connect(wiimotes[i].ctl_sock, (struct sockaddr*)&ctl_remote_addr, sizeof(struct sockaddr_l2)) ||
           connect(wiimotes[i].int_sock, (struct sockaddr*)&int_remote_addr, sizeof(struct sockaddr_l2)))
        {
            close(wiimotes[i].ctl_sock);
            close(wiimotes[i].int_sock);
            return -1;
        }
     }
    return i;
}

void disconnect_wiimotes(wiimote_t* wiimotes, int num)
{
    int i = 0;
    for(; i < num; i++)
    {
        close(wiimotes[i].ctl_sock);
        close(wiimotes[i].int_sock);
    }
}

int wiimote_send(wiimote_t* mote, unsigned char* bytes, int len, int flags)
{
    unsigned char* tmp = bytes;
    bytes = (unsigned char*) malloc(len + 1);
    bytes[0] = 0x52;
    memcpy(bytes + 1, tmp, len);
    if(flags & WIIMOTE_COMM_INTERUPT)
    {
        int retval = write(mote->int_sock, bytes, len + 1);
        free(bytes);
        return retval;
    }
    else
    {
        int retval = write(mote->ctl_sock, bytes, len + 1);
        free(bytes);
        return retval;
    }
    return -1;
}

int wiimote_recv(wiimote_t* mote, unsigned char* bytes, int len, int flags)
{
    if(flags & WIIMOTE_COMM_INTERUPT)
    {
        int retval = read(mote->int_sock, bytes, len);
        if(retval > 0)
            memcpy(bytes, bytes + 1, retval);
        return retval - 1;
    }
    else
    {
        int retval = read(mote->ctl_sock, bytes, len);
        if(retval > 0)
            memcpy(bytes, bytes + 1, retval);
        return retval - 1;
    }
    return -1;
}


int wiimote_check_data(wiimote_t* mote, int flags)
{
    fd_set fdset;
    FD_ZERO(&fdset);
    struct timeval timeout;
    timeout.tv_sec = 0;
    timeout.tv_usec = 0;
    if(flags & WIIMOTE_COMM_INTERUPT)
    {
        FD_SET(mote->int_sock, &fdset);
        return select(mote->int_sock + 1, &fdset, NULL, NULL, &timeout) != 0;
    }
    else
    {
        FD_SET(mote->ctl_sock, &fdset);
        return select(mote->ctl_sock + 1, &fdset, NULL, NULL, &timeout) != 0;
    }
    return -1;
}