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;
}
