From e3aaf5e9269661f1bda894af7e90e9ec284549db Mon Sep 17 00:00:00 2001 From: Johan Hedberg Date: Wed, 4 Oct 2006 13:37:35 +0000 Subject: L2CAP info request support for AuditRemoteDevice method --- hcid/dbus-test.c | 184 ++++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 181 insertions(+), 3 deletions(-) (limited to 'hcid/dbus-test.c') diff --git a/hcid/dbus-test.c b/hcid/dbus-test.c index 9e4ad690..c74adf54 100644 --- a/hcid/dbus-test.c +++ b/hcid/dbus-test.c @@ -30,9 +30,19 @@ #include +#include +#include + #include "hcid.h" #include "dbus.h" +#define L2INFO_TIMEOUT (2 * 1000) + +enum { + AUDIT_STATE_MTU = 0, + AUDIT_STATE_FEATURES +}; + struct audit { bdaddr_t addr; @@ -47,6 +57,16 @@ struct audit { GIOChannel *io; guint io_id; + + guint timeout; + + int state; + + gboolean got_mtu; + gboolean got_mask; + + uint16_t mtu; + uint32_t mask; }; static struct slist *audits = NULL; @@ -91,6 +111,8 @@ static void audit_requestor_exited(const char *name, struct audit *audit) audits = slist_remove(audits, audit); if (audit->io) g_io_channel_close(audit->io); + if (audit->timeout) + g_timeout_remove(audit->timeout); audit_free(audit); } @@ -115,8 +137,129 @@ static gboolean audit_in_progress(void) return FALSE; } +static gboolean l2raw_input_timer(struct audit *audit) +{ + error("l2raw_input_timer: Timed out while waiting for input"); + + g_io_channel_close(audit->io); + audits = slist_remove(audits, audit); + name_listener_remove(audit->conn, audit->requestor, + (name_cb_t) audit_requestor_exited, audit); + audit_free(audit); + + return FALSE; +} + +static void handle_mtu_response(struct audit *audit, const l2cap_info_rsp *rsp) +{ + switch (btohs(rsp->result)) { + case 0x0000: + audit->mtu = btohs(bt_get_unaligned((uint16_t *) rsp->data)); + audit->got_mtu = TRUE; + debug("Connectionless MTU size is %d", audit->mtu); + break; + case 0x0001: + debug("Connectionless MTU is not supported"); + break; + } +} + +static void handle_features_response(struct audit *audit, const l2cap_info_rsp *rsp) +{ + switch (btohs(rsp->result)) { + case 0x0000: + audit->mask = btohl(bt_get_unaligned((uint32_t *) rsp->data)); + audit->got_mask = TRUE; + debug("Extended feature mask is 0x%04x", audit->mask); + if (audit->mask & 0x01) + debug(" Flow control mode"); + if (audit->mask & 0x02) + debug(" Retransmission mode"); + if (audit->mask & 0x04) + debug(" Bi-directional QoS"); + break; + case 0x0001: + debug("Extended feature mask is not supported"); + break; + } +} + +static gboolean l2raw_data_callback(GIOChannel *io, GIOCondition cond, struct audit *audit) +{ + unsigned char buf[48]; + l2cap_cmd_hdr *cmd = (l2cap_cmd_hdr *) buf; + l2cap_info_req *req = (l2cap_info_req *) (buf + L2CAP_CMD_HDR_SIZE); + l2cap_info_rsp *rsp = (l2cap_info_rsp *) (buf + L2CAP_CMD_HDR_SIZE); + int sk; + + if (cond & G_IO_NVAL) { + g_io_channel_unref(io); + return FALSE; + } + + if (audit->timeout) { + g_timeout_remove(audit->timeout); + audit->timeout = 0; + } + + if (cond & (G_IO_ERR | G_IO_HUP)) + goto failed; + + sk = g_io_channel_unix_get_fd(io); + + memset(buf, 0, sizeof(buf)); + + if (recv(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + 2, 0) < 0) { + error("Can't receive info response: %s (%d)", strerror(errno), errno); + goto failed; + } + + switch (audit->state) { + case AUDIT_STATE_MTU: + handle_mtu_response(audit, rsp); + + memset(buf, 0, sizeof(buf)); + cmd->code = L2CAP_INFO_REQ; + cmd->ident = 42; + cmd->len = htobs(2); + req->type = htobs(0x0002); + + if (send(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_REQ_SIZE, 0) < 0) { + error("Can't send info request:", strerror(errno), errno); + goto failed; + } + + audit->timeout = g_timeout_add(L2INFO_TIMEOUT, (GSourceFunc) + l2raw_input_timer, audit); + + audit->state = AUDIT_STATE_FEATURES; + + return TRUE; + + case AUDIT_STATE_FEATURES: + handle_features_response(audit, rsp); + break; + } + +failed: + g_io_channel_close(io); + g_io_channel_unref(io); + audits = slist_remove(audits, audit); + name_listener_remove(audit->conn, audit->requestor, + (name_cb_t) audit_requestor_exited, audit); + audit_free(audit); + + return FALSE; +} + static gboolean l2raw_connect_complete(GIOChannel *io, GIOCondition cond, struct audit *audit) { + unsigned char buf[48]; + l2cap_cmd_hdr *cmd = (l2cap_cmd_hdr *) buf; + l2cap_info_req *req = (l2cap_info_req *) (buf + L2CAP_CMD_HDR_SIZE); + socklen_t len; + int sk, ret; + if (cond & G_IO_NVAL) { g_io_channel_unref(io); return FALSE; @@ -124,13 +267,46 @@ static gboolean l2raw_connect_complete(GIOChannel *io, GIOCondition cond, struct if (cond & (G_IO_ERR | G_IO_HUP)) { error("Error on raw l2cap socket"); - g_io_channel_close(io); - return FALSE; + goto failed; + } + + sk = g_io_channel_unix_get_fd(io); + + len = sizeof(ret); + if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &ret, &len) < 0) { + error("Can't get socket error: %s (%d)", strerror(errno), errno); + goto failed; + } + + if (ret != 0) { + error("l2raw_connect failed: %s (%d)", strerror(ret), ret); + goto failed; } debug("AuditRemoteDevice: connected"); - /* FIXME: Instead of closing and unrefing we should proceed with the audit */ + /* Send L2CAP info request */ + memset(buf, 0, sizeof(buf)); + cmd->code = L2CAP_INFO_REQ; + cmd->ident = 42; + cmd->len = htobs(2); + req->type = htobs(0x0001); + + if (send(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_REQ_SIZE, 0) < 0) { + error("Can't send info request: %s (%d)", strerror(errno), errno); + goto failed; + } + + audit->timeout = g_timeout_add(L2INFO_TIMEOUT, (GSourceFunc) + l2raw_input_timer, audit); + + audit->io_id = g_io_add_watch(audit->io, + G_IO_IN | G_IO_NVAL | G_IO_HUP | G_IO_ERR, + (GIOFunc) l2raw_data_callback, audit); + + return FALSE; + +failed: g_io_channel_close(io); g_io_channel_unref(io); audits = slist_remove(audits, audit); @@ -248,6 +424,8 @@ static DBusHandlerResult cancel_audit_remote_device(DBusConnection *conn, if (audit->io) g_io_channel_close(audit->io); + if (audit->timeout) + g_timeout_remove(audit->timeout); audits = slist_remove(audits, audit); name_listener_remove(audit->conn, audit->requestor, -- cgit