Bluetooth HIDP emulation on top of usb-hid.c and L2CAP and SDP.

git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@5347 c046a42c-6fe2-441c-8c8c-71466251a162

3 files changed, 606 insertions, 8 deletions

diff --git a/hw/bt-hid.c b/hw/bt-hid.c
new file mode 100644
index 0000000000..4602d9e06e
--- /dev/null
+++ b/hw/bt-hid.c
@@ -0,0 +1,571 @@
+/*
+ * QEMU Bluetooth HID Profile wrapper for USB HID.
+ *
+ * Copyright (C) 2007-2008 OpenMoko, Inc.
+ * Written by Andrzej Zaborowski <andrew@openedhand.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 or
+ * (at your option) version 3 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include "qemu-common.h"
+#include "usb.h"
+#include "bt.h"
+
+enum hid_transaction_req {
+    BT_HANDSHAKE			= 0x0,
+    BT_HID_CONTROL			= 0x1,
+    BT_GET_REPORT			= 0x4,
+    BT_SET_REPORT			= 0x5,
+    BT_GET_PROTOCOL			= 0x6,
+    BT_SET_PROTOCOL			= 0x7,
+    BT_GET_IDLE				= 0x8,
+    BT_SET_IDLE				= 0x9,
+    BT_DATA				= 0xa,
+    BT_DATC				= 0xb,
+};
+
+enum hid_transaction_handshake {
+    BT_HS_SUCCESSFUL			= 0x0,
+    BT_HS_NOT_READY			= 0x1,
+    BT_HS_ERR_INVALID_REPORT_ID		= 0x2,
+    BT_HS_ERR_UNSUPPORTED_REQUEST	= 0x3,
+    BT_HS_ERR_INVALID_PARAMETER		= 0x4,
+    BT_HS_ERR_UNKNOWN			= 0xe,
+    BT_HS_ERR_FATAL			= 0xf,
+};
+
+enum hid_transaction_control {
+    BT_HC_NOP				= 0x0,
+    BT_HC_HARD_RESET			= 0x1,
+    BT_HC_SOFT_RESET			= 0x2,
+    BT_HC_SUSPEND			= 0x3,
+    BT_HC_EXIT_SUSPEND			= 0x4,
+    BT_HC_VIRTUAL_CABLE_UNPLUG		= 0x5,
+};
+
+enum hid_protocol {
+    BT_HID_PROTO_BOOT			= 0,
+    BT_HID_PROTO_REPORT			= 1,
+};
+
+enum hid_boot_reportid {
+    BT_HID_BOOT_INVALID			= 0,
+    BT_HID_BOOT_KEYBOARD,
+    BT_HID_BOOT_MOUSE,
+};
+
+enum hid_data_pkt {
+    BT_DATA_OTHER			= 0,
+    BT_DATA_INPUT,
+    BT_DATA_OUTPUT,
+    BT_DATA_FEATURE,
+};
+
+#define BT_HID_MTU			48
+
+/* HID interface requests */
+#define GET_REPORT			0xa101
+#define GET_IDLE			0xa102
+#define GET_PROTOCOL			0xa103
+#define SET_REPORT			0x2109
+#define SET_IDLE			0x210a
+#define SET_PROTOCOL			0x210b
+
+struct bt_hid_device_s {
+    struct bt_l2cap_device_s btdev;
+    struct bt_l2cap_conn_params_s *control;
+    struct bt_l2cap_conn_params_s *interrupt;
+    USBDevice *usbdev;
+
+    int proto;
+    int connected;
+    int data_type;
+    int intr_state;
+    struct {
+        int len;
+        uint8_t buffer[1024];
+    } dataother, datain, dataout, feature, intrdataout;
+    enum {
+        bt_state_ready,
+        bt_state_transaction,
+        bt_state_suspend,
+    } state;
+};
+
+static void bt_hid_reset(struct bt_hid_device_s *s)
+{
+    struct bt_scatternet_s *net = s->btdev.device.net;
+
+    /* Go as far as... */
+    bt_l2cap_device_done(&s->btdev);
+    bt_l2cap_device_init(&s->btdev, net);
+
+    s->usbdev->handle_reset(s->usbdev);
+    s->proto = BT_HID_PROTO_REPORT;
+    s->state = bt_state_ready;
+    s->dataother.len = 0;
+    s->datain.len = 0;
+    s->dataout.len = 0;
+    s->feature.len = 0;
+    s->intrdataout.len = 0;
+    s->intr_state = 0;
+}
+
+static int bt_hid_out(struct bt_hid_device_s *s)
+{
+    USBPacket p;
+
+    if (s->data_type == BT_DATA_OUTPUT) {
+        p.pid = USB_TOKEN_OUT;
+        p.devep = 1;
+        p.data = s->dataout.buffer;
+        p.len = s->dataout.len;
+        s->dataout.len = s->usbdev->handle_data(s->usbdev, &p);
+
+        return s->dataout.len;
+    }
+
+    if (s->data_type == BT_DATA_FEATURE) {
+        /* XXX:
+         * does this send a USB_REQ_CLEAR_FEATURE/USB_REQ_SET_FEATURE
+         * or a SET_REPORT? */
+        p.devep = 0;
+    }
+
+    return -1;
+}
+
+static int bt_hid_in(struct bt_hid_device_s *s)
+{
+    USBPacket p;
+
+    p.pid = USB_TOKEN_IN;
+    p.devep = 1;
+    p.data = s->datain.buffer;
+    p.len = sizeof(s->datain.buffer);
+    s->datain.len = s->usbdev->handle_data(s->usbdev, &p);
+
+    return s->datain.len;
+}
+
+static void bt_hid_send_handshake(struct bt_hid_device_s *s, int result)
+{
+    *s->control->sdu_out(s->control, 1) =
+            (BT_HANDSHAKE << 4) | result;
+    s->control->sdu_submit(s->control);
+}
+
+static void bt_hid_send_control(struct bt_hid_device_s *s, int operation)
+{
+    *s->control->sdu_out(s->control, 1) =
+            (BT_HID_CONTROL << 4) | operation;
+    s->control->sdu_submit(s->control);
+}
+
+static void bt_hid_disconnect(struct bt_hid_device_s *s)
+{
+    /* Disconnect s->control and s->interrupt */
+}
+
+static void bt_hid_send_data(struct bt_l2cap_conn_params_s *ch, int type,
+                const uint8_t *data, int len)
+{
+    uint8_t *pkt, hdr = (BT_DATA << 4) | type;
+    int plen;
+
+    do {
+        plen = MIN(len, ch->remote_mtu - 1);
+        pkt = ch->sdu_out(ch, plen + 1);
+
+        pkt[0] = hdr;
+        if (plen)
+            memcpy(pkt + 1, data, plen);
+        ch->sdu_submit(ch);
+
+        len -= plen;
+        data += plen;
+        hdr = (BT_DATC << 4) | type;
+    } while (plen == ch->remote_mtu - 1);
+}
+
+static void bt_hid_control_transaction(struct bt_hid_device_s *s,
+                const uint8_t *data, int len)
+{
+    uint8_t type, parameter;
+    int rlen, ret = -1;
+    if (len < 1)
+        return;
+
+    type = data[0] >> 4;
+    parameter = data[0] & 0xf;
+
+    switch (type) {
+    case BT_HANDSHAKE:
+    case BT_DATA:
+        switch (parameter) {
+        default:
+            /* These are not expected to be sent this direction.  */
+            ret = BT_HS_ERR_INVALID_PARAMETER;
+        }
+        break;
+
+    case BT_HID_CONTROL:
+        if (len != 1 || (parameter != BT_HC_VIRTUAL_CABLE_UNPLUG &&
+                                s->state == bt_state_transaction)) {
+            ret = BT_HS_ERR_INVALID_PARAMETER;
+            break;
+        }
+        switch (parameter) {
+        case BT_HC_NOP:
+            break;
+        case BT_HC_HARD_RESET:
+        case BT_HC_SOFT_RESET:
+            bt_hid_reset(s);
+            break;
+        case BT_HC_SUSPEND:
+            if (s->state == bt_state_ready)
+                s->state = bt_state_suspend;
+            else
+                ret = BT_HS_ERR_INVALID_PARAMETER;
+            break;
+        case BT_HC_EXIT_SUSPEND:
+            if (s->state == bt_state_suspend)
+                s->state = bt_state_ready;
+            else
+                ret = BT_HS_ERR_INVALID_PARAMETER;
+            break;
+        case BT_HC_VIRTUAL_CABLE_UNPLUG:
+            bt_hid_disconnect(s);
+            break;
+        default:
+            ret = BT_HS_ERR_INVALID_PARAMETER;
+        }
+        break;
+
+    case BT_GET_REPORT:
+        /* No ReportIDs declared.  */
+        if (((parameter & 8) && len != 3) ||
+                        (!(parameter & 8) && len != 1) ||
+                        s->state != bt_state_ready) {
+            ret = BT_HS_ERR_INVALID_PARAMETER;
+            break;
+        }
+        if (parameter & 8)
+            rlen = data[2] | (data[3] << 8);
+        else
+            rlen = INT_MAX;
+        switch (parameter & 3) {
+        case BT_DATA_OTHER:
+            ret = BT_HS_ERR_INVALID_PARAMETER;
+            break;
+        case BT_DATA_INPUT:
+            /* Here we can as well poll s->usbdev */
+            bt_hid_send_data(s->control, BT_DATA_INPUT,
+                            s->datain.buffer, MIN(rlen, s->datain.len));
+            break;
+        case BT_DATA_OUTPUT:
+            bt_hid_send_data(s->control, BT_DATA_OUTPUT,
+                            s->dataout.buffer, MIN(rlen, s->dataout.len));
+            break;
+        case BT_DATA_FEATURE:
+            bt_hid_send_data(s->control, BT_DATA_FEATURE,
+                            s->feature.buffer, MIN(rlen, s->feature.len));
+            break;
+        }
+        break;
+
+    case BT_SET_REPORT:
+        if (len < 2 || len > BT_HID_MTU || s->state != bt_state_ready ||
+                        (parameter & 3) == BT_DATA_OTHER ||
+                        (parameter & 3) == BT_DATA_INPUT) {
+            ret = BT_HS_ERR_INVALID_PARAMETER;
+            break;
+        }
+        s->data_type = parameter & 3;
+        if (s->data_type == BT_DATA_OUTPUT) {
+            s->dataout.len = len - 1;
+            memcpy(s->dataout.buffer, data + 1, s->dataout.len);
+        } else {
+            s->feature.len = len - 1;
+            memcpy(s->feature.buffer, data + 1, s->feature.len);
+        }
+        if (len == BT_HID_MTU)
+            s->state = bt_state_transaction;
+        else
+            bt_hid_out(s);
+        break;
+
+    case BT_GET_PROTOCOL:
+        if (len != 1 || s->state == bt_state_transaction) {
+            ret = BT_HS_ERR_INVALID_PARAMETER;
+            break;
+        }
+        *s->control->sdu_out(s->control, 1) = s->proto;
+        s->control->sdu_submit(s->control);
+        break;
+
+    case BT_SET_PROTOCOL:
+        if (len != 1 || s->state == bt_state_transaction ||
+                        (parameter != BT_HID_PROTO_BOOT &&
+                         parameter != BT_HID_PROTO_REPORT)) {
+            ret = BT_HS_ERR_INVALID_PARAMETER;
+            break;
+        }
+        s->proto = parameter;
+        s->usbdev->handle_control(s->usbdev, SET_PROTOCOL, s->proto, 0, 0, 0);
+        ret = BT_HS_SUCCESSFUL;
+        break;
+
+    case BT_GET_IDLE:
+        if (len != 1 || s->state == bt_state_transaction) {
+            ret = BT_HS_ERR_INVALID_PARAMETER;
+            break;
+        }
+        s->usbdev->handle_control(s->usbdev, GET_IDLE, 0, 0, 1,
+                        s->control->sdu_out(s->control, 1));
+        s->control->sdu_submit(s->control);
+        break;
+
+    case BT_SET_IDLE:
+        if (len != 2 || s->state == bt_state_transaction) {
+            ret = BT_HS_ERR_INVALID_PARAMETER;
+            break;
+        }
+
+        /* We don't need to know about the Idle Rate here really,
+         * so just pass it on to the device.  */
+        ret = s->usbdev->handle_control(s->usbdev,
+                        SET_IDLE, data[1], 0, 0, 0) ?
+                BT_HS_SUCCESSFUL : BT_HS_ERR_INVALID_PARAMETER;
+        /* XXX: Does this generate a handshake? */
+        break;
+
+    case BT_DATC:
+        if (len > BT_HID_MTU || s->state != bt_state_transaction) {
+            ret = BT_HS_ERR_INVALID_PARAMETER;
+            break;
+        }
+        if (s->data_type == BT_DATA_OUTPUT) {
+            memcpy(s->dataout.buffer + s->dataout.len, data + 1, len - 1);
+            s->dataout.len += len - 1;
+        } else {
+            memcpy(s->feature.buffer + s->feature.len, data + 1, len - 1);
+            s->feature.len += len - 1;
+        }
+        if (len < BT_HID_MTU) {
+            bt_hid_out(s);
+            s->state = bt_state_ready;
+        }
+        break;
+
+    default:
+        ret = BT_HS_ERR_UNSUPPORTED_REQUEST;
+    }
+
+    if (ret != -1)
+        bt_hid_send_handshake(s, ret);
+}
+
+static void bt_hid_control_sdu(void *opaque, const uint8_t *data, int len)
+{
+    struct bt_hid_device_s *hid = opaque;
+
+    return bt_hid_control_transaction(hid, data, len);
+}
+
+static void bt_hid_datain(void *opaque)
+{
+    struct bt_hid_device_s *hid = opaque;
+
+    /* If suspended, wake-up and send a wake-up event first.  We might
+     * want to also inspect the input report and ignore event like
+     * mouse movements until a button event occurs.  */
+    if (hid->state == bt_state_suspend) {
+        hid->state = bt_state_ready;
+    }
+
+    if (bt_hid_in(hid) > 0)
+        /* TODO: when in boot-mode precede any Input reports with the ReportID
+         * byte, here and in GetReport/SetReport on the Control channel.  */
+        bt_hid_send_data(hid->interrupt, BT_DATA_INPUT,
+                        hid->datain.buffer, hid->datain.len);
+}
+
+static void bt_hid_interrupt_sdu(void *opaque, const uint8_t *data, int len)
+{
+    struct bt_hid_device_s *hid = opaque;
+
+    if (len > BT_HID_MTU || len < 1)
+        goto bad;
+    if ((data[0] & 3) != BT_DATA_OUTPUT)
+        goto bad;
+    if ((data[0] >> 4) == BT_DATA) {
+        if (hid->intr_state)
+            goto bad;
+
+        hid->data_type = BT_DATA_OUTPUT;
+        hid->intrdataout.len = 0;
+    } else if ((data[0] >> 4) == BT_DATC) {
+        if (!hid->intr_state)
+            goto bad;
+    } else
+        goto bad;
+
+    memcpy(hid->intrdataout.buffer + hid->intrdataout.len, data + 1, len - 1);
+    hid->intrdataout.len += len - 1;
+    hid->intr_state = (len == BT_HID_MTU);
+    if (!hid->intr_state) {
+        memcpy(hid->dataout.buffer, hid->intrdataout.buffer,
+                        hid->dataout.len = hid->intrdataout.len);
+        bt_hid_out(hid);
+    }
+
+    return;
+bad:
+    fprintf(stderr, "%s: bad transaction on Interrupt channel.\n",
+                    __FUNCTION__);
+}
+
+/* "Virtual cable" plug/unplug event.  */
+static void bt_hid_connected_update(struct bt_hid_device_s *hid)
+{
+    int prev = hid->connected;
+
+    hid->connected = hid->control && hid->interrupt;
+
+    /* Stop page-/inquiry-scanning when a host is connected.  */
+    hid->btdev.device.page_scan = !hid->connected;
+    hid->btdev.device.inquiry_scan = !hid->connected;
+
+    if (hid->connected && !prev) {
+        hid->usbdev->handle_reset(hid->usbdev);
+        hid->proto = BT_HID_PROTO_REPORT;
+    }
+
+    /* Should set HIDVirtualCable in SDP (possibly need to check that SDP
+     * isn't destroyed yet, in case we're being called from handle_destroy) */
+}
+
+static void bt_hid_close_control(void *opaque)
+{
+    struct bt_hid_device_s *hid = opaque;
+
+    hid->control = 0;
+    bt_hid_connected_update(hid);
+}
+
+static void bt_hid_close_interrupt(void *opaque)
+{
+    struct bt_hid_device_s *hid = opaque;
+
+    hid->interrupt = 0;
+    bt_hid_connected_update(hid);
+}
+
+static int bt_hid_new_control_ch(struct bt_l2cap_device_s *dev,
+                struct bt_l2cap_conn_params_s *params)
+{
+    struct bt_hid_device_s *hid = (struct bt_hid_device_s *) dev;
+
+    if (hid->control)
+        return 1;
+
+    hid->control = params;
+    hid->control->opaque = hid;
+    hid->control->close = bt_hid_close_control;
+    hid->control->sdu_in = bt_hid_control_sdu;
+
+    bt_hid_connected_update(hid);
+
+    return 0;
+}
+
+static int bt_hid_new_interrupt_ch(struct bt_l2cap_device_s *dev,
+                struct bt_l2cap_conn_params_s *params)
+{
+    struct bt_hid_device_s *hid = (struct bt_hid_device_s *) dev;
+
+    if (hid->interrupt)
+        return 1;
+
+    hid->interrupt = params;
+    hid->interrupt->opaque = hid;
+    hid->interrupt->close = bt_hid_close_interrupt;
+    hid->interrupt->sdu_in = bt_hid_interrupt_sdu;
+
+    bt_hid_connected_update(hid);
+
+    return 0;
+}
+
+static void bt_hid_destroy(struct bt_device_s *dev)
+{
+    struct bt_hid_device_s *hid = (struct bt_hid_device_s *) dev;
+
+    if (hid->connected)
+        bt_hid_send_control(hid, BT_HC_VIRTUAL_CABLE_UNPLUG);
+    bt_l2cap_device_done(&hid->btdev);
+
+    hid->usbdev->handle_destroy(hid->usbdev);
+
+    qemu_free(hid);
+}
+
+enum peripheral_minor_class {
+    class_other		= 0 << 4,
+    class_keyboard	= 1 << 4,
+    class_pointing	= 2 << 4,
+    class_combo		= 3 << 4,
+};
+
+static struct bt_device_s *bt_hid_init(struct bt_scatternet_s *net,
+                USBDevice *dev, enum peripheral_minor_class minor)
+{
+    struct bt_hid_device_s *s = qemu_mallocz(sizeof(*s));
+    uint32_t class =
+            /* Format type */
+            (0 << 0) |
+            /* Device class */
+            (minor << 2) |
+            (5 << 8) |  /* "Peripheral" */
+            /* Service classes */
+            (1 << 13) | /* Limited discoverable mode */
+            (1 << 19);  /* Capturing device (?) */
+
+    bt_l2cap_device_init(&s->btdev, net);
+    bt_l2cap_sdp_init(&s->btdev);
+    bt_l2cap_psm_register(&s->btdev, BT_PSM_HID_CTRL,
+                    BT_HID_MTU, bt_hid_new_control_ch);
+    bt_l2cap_psm_register(&s->btdev, BT_PSM_HID_INTR,
+                    BT_HID_MTU, bt_hid_new_interrupt_ch);
+
+    s->usbdev = dev;
+    s->btdev.device.lmp_name = s->usbdev->devname;
+    usb_hid_datain_cb(s->usbdev, s, bt_hid_datain);
+
+    s->btdev.device.handle_destroy = bt_hid_destroy;
+
+    s->btdev.device.class[0] = (class >>  0) & 0xff;
+    s->btdev.device.class[1] = (class >>  8) & 0xff;
+    s->btdev.device.class[2] = (class >> 16) & 0xff;
+
+    return &s->btdev.device;
+}
+
+struct bt_device_s *bt_keyboard_init(struct bt_scatternet_s *net)
+{
+    return bt_hid_init(net, usb_keyboard_init(), class_keyboard);
+}
diff --git a/hw/usb-hid.c b/hw/usb-hid.c
index 406c9ab839..972543f282 100644
--- a/hw/usb-hid.c
+++ b/hw/usb-hid.c
@@ -67,6 +67,8 @@ typedef struct USBHIDState {
     int protocol;
     int idle;
     int changed;
+    void *datain_opaque;
+    void (*datain)(void *);
 } USBHIDState;
 
 /* mostly the same values as the Bochs USB Mouse device */
@@ -402,6 +404,14 @@ static const uint8_t usb_hid_usage_keys[0x100] = {
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 };
 
+static void usb_hid_changed(USBHIDState *hs)
+{
+    hs->changed = 1;
+
+    if (hs->datain)
+        hs->datain(hs->datain_opaque);
+}
+
 static void usb_mouse_event(void *opaque,
                             int dx1, int dy1, int dz1, int buttons_state)
 {
@@ -412,7 +422,8 @@ static void usb_mouse_event(void *opaque,
     s->dy += dy1;
     s->dz += dz1;
     s->buttons_state = buttons_state;
-    hs->changed = 1;
+
+    usb_hid_changed(hs);
 }
 
 static void usb_tablet_event(void *opaque,
@@ -425,7 +436,8 @@ static void usb_tablet_event(void *opaque,
     s->y = y;
     s->dz += dz;
     s->buttons_state = buttons_state;
-    hs->changed = 1;
+
+    usb_hid_changed(hs);
 }
 
 static void usb_keyboard_event(void *opaque, int keycode)
@@ -439,8 +451,6 @@ static void usb_keyboard_event(void *opaque, int keycode)
     hid_code = usb_hid_usage_keys[key | ((s->modifiers >> 1) & (1 << 7))];
     s->modifiers &= ~(1 << 8);
 
-    hs->changed = 1;
-
     switch (hid_code) {
     case 0x00:
         return;
@@ -465,15 +475,23 @@ static void usb_keyboard_event(void *opaque, int keycode)
             if (s->key[i] == hid_code) {
                 s->key[i] = s->key[-- s->keys];
                 s->key[s->keys] = 0x00;
-                return;
+                usb_hid_changed(hs);
+                break;
             }
+        if (i < 0)
+            return;
     } else {
         for (i = s->keys - 1; i >= 0; i --)
             if (s->key[i] == hid_code)
-                return;
-        if (s->keys < sizeof(s->key))
-            s->key[s->keys ++] = hid_code;
+                break;
+        if (i < 0) {
+            if (s->keys < sizeof(s->key))
+                s->key[s->keys ++] = hid_code;
+        } else
+            return;
     }
+
+    usb_hid_changed(hs);
 }
 
 static inline int int_clamp(int val, int vmin, int vmax)
@@ -894,3 +912,11 @@ USBDevice *usb_keyboard_init(void)
 
     return (USBDevice *) s;
 }
+
+void usb_hid_datain_cb(USBDevice *dev, void *opaque, void (*datain)(void *))
+{
+    USBHIDState *s = (USBHIDState *)dev;
+
+    s->datain_opaque = opaque;
+    s->datain = datain;
+}
diff --git a/hw/usb.h b/hw/usb.h
index 1a353bbc44..137488d206 100644
--- a/hw/usb.h
+++ b/hw/usb.h
@@ -247,6 +247,7 @@ void usb_host_info(void);
 USBDevice *usb_mouse_init(void);
 USBDevice *usb_tablet_init(void);
 USBDevice *usb_keyboard_init(void);
+void usb_hid_datain_cb(USBDevice *dev, void *opaque, void (*datain)(void *));
 
 /* usb-msd.c */
 USBDevice *usb_msd_init(const char *filename);