static void send_acl(struct bthost *bthost, uint16_t handle, uint16_t cid, const void *data, uint16_t len) { struct bt_hci_acl_hdr *acl_hdr; struct bt_l2cap_hdr *l2_hdr; uint16_t pkt_len; void *pkt_data; pkt_len = 1 + sizeof(*acl_hdr) + sizeof(*l2_hdr) + len; pkt_data = malloc(pkt_len); if (!pkt_data) return; ((uint8_t *) pkt_data)[0] = BT_H4_ACL_PKT; acl_hdr = pkt_data + 1; acl_hdr->handle = acl_handle_pack(handle, 0); acl_hdr->dlen = cpu_to_le16(len + sizeof(*l2_hdr)); l2_hdr = pkt_data + 1 + sizeof(*acl_hdr); l2_hdr->cid = cpu_to_le16(cid); l2_hdr->len = cpu_to_le16(len); if (len > 0) memcpy(pkt_data + 1 + sizeof(*acl_hdr) + sizeof(*l2_hdr), data, len); send_packet(bthost, pkt_data, pkt_len); free(pkt_data); }
static void io_acl_data(void *data) { struct vhci_conn *conn = data; unsigned char buf[HCI_MAX_FRAME_SIZE], *ptr; hci_acl_hdr *ah; uint16_t flags; int len; ptr = buf + 1; if (read_n(conn->fd, ptr, HCI_ACL_HDR_SIZE) <= 0) { close_connection(conn); return; } ah = (void *) ptr; ptr += HCI_ACL_HDR_SIZE; len = btohs(ah->dlen); if (read_n(conn->fd, ptr, len) <= 0) { close_connection(conn); return; } buf[0] = HCI_ACLDATA_PKT; flags = acl_flags(btohs(ah->handle)); ah->handle = htobs(acl_handle_pack(conn->handle, flags)); len += HCI_ACL_HDR_SIZE + 1; write_snoop(vdev.dd, HCI_ACLDATA_PKT, 1, buf, len); if (write(vdev.dev_fd, buf, len) < 0) syslog(LOG_ERR, "ACL data write error"); }
static gboolean io_acl_data(GIOChannel *chan, GIOCondition cond, gpointer data) { struct vhci_conn *conn = (struct vhci_conn *) data; unsigned char buf[HCI_MAX_FRAME_SIZE], *ptr; hci_acl_hdr *ah; uint16_t flags; int fd, err, len; if (cond & G_IO_NVAL) { g_io_channel_unref(chan); return FALSE; } if (cond & G_IO_HUP) { close_connection(conn); return FALSE; } fd = g_io_channel_unix_get_fd(chan); ptr = buf + 1; if (read_n(fd, ptr, HCI_ACL_HDR_SIZE) <= 0) { close_connection(conn); return FALSE; } ah = (void *) ptr; ptr += HCI_ACL_HDR_SIZE; len = btohs(ah->dlen); if (read_n(fd, ptr, len) <= 0) { close_connection(conn); return FALSE; } buf[0] = HCI_ACLDATA_PKT; flags = acl_flags(btohs(ah->handle)); ah->handle = htobs(acl_handle_pack(conn->handle, flags)); len += HCI_ACL_HDR_SIZE + 1; write_snoop(vdev.dd, HCI_ACLDATA_PKT, 1, buf, len); err = write(vdev.fd, buf, len); return TRUE; }
/* * This function can be used to bypass the l2cap outgoing MTU check of the Linux kernel. * If plen is higher than ACL_MTU, it sends a segmented packet. */ static int l2cap_bluez_acl_send_data (int channel, unsigned char *data, unsigned short plen) { int ret = -1, dd = -1; uint8_t type = HCI_ACLDATA_PKT; hci_acl_hdr acl_hdr; l2cap_hdr l2_hdr; struct iovec iv[4]; int ivn; unsigned short data_len; if ((dd = hci_open_dev(channels.channels[channel].devid)) < 0) { perror("hci_open_dev"); return -1; } data_len = ACL_MTU-1-HCI_ACL_HDR_SIZE-L2CAP_HDR_SIZE; if(plen < data_len) { data_len = plen; } iv[0].iov_base = &type; iv[0].iov_len = 1; acl_hdr.handle = htobs(acl_handle_pack(channels.channels[channel].handle, ACL_START)); acl_hdr.dlen = htobs(data_len+L2CAP_HDR_SIZE); iv[1].iov_base = &acl_hdr; iv[1].iov_len = HCI_ACL_HDR_SIZE; l2_hdr.cid = htobs(channels.channels[channel].cid); l2_hdr.len = htobs(plen); iv[2].iov_base = &l2_hdr; iv[2].iov_len = L2CAP_HDR_SIZE; ivn = 3; if (data_len) { iv[3].iov_base = data; iv[3].iov_len = htobs(data_len); ivn = 4; } while ((ret = writev(dd, iv, ivn)) < 0) { if (errno == EAGAIN || errno == EINTR) { continue; } perror("writev"); break; } if(ret != -1) { plen -= data_len; while(plen) { data += data_len; data_len = ACL_MTU-1-HCI_ACL_HDR_SIZE; if(plen < data_len) { data_len = plen; } iv[0].iov_base = &type; iv[0].iov_len = 1; acl_hdr.handle = htobs(acl_handle_pack(channels.channels[channel].handle, ACL_CONT)); acl_hdr.dlen = htobs(plen); iv[1].iov_base = &acl_hdr; iv[1].iov_len = HCI_ACL_HDR_SIZE; iv[2].iov_base = data; iv[2].iov_len = htobs(data_len); ivn = 3; while ((ret = writev(dd, iv, ivn)) < 0) { if (errno == EAGAIN || errno == EINTR) { continue; } perror("writev"); break; } if(ret == -1) { break; } plen -= data_len; } } hci_close_dev(dd); return ret; }