static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
__u8 code, __u8 ident, __u16 dlen, void *data)
{
struct sk_buff *skb, **frag;
l2cap_cmd_hdr *cmd;
l2cap_hdr *lh;
int len, count;
BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", conn, code, ident, dlen);
len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
count = MIN(conn->mtu, len);
skb = bluez_skb_alloc(count, GFP_ATOMIC);
if (!skb)
return NULL;
lh = (l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
lh->len = __cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
lh->cid = __cpu_to_le16(0x0001);
cmd = (l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
cmd->code = code;
cmd->ident = ident;
cmd->len = __cpu_to_le16(dlen);
if (dlen) {
count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
memcpy(skb_put(skb, count), data, count);
data += count;
}
len -= skb->len;
/* Continuation fragments (no L2CAP header) */
frag = &skb_shinfo(skb)->frag_list;
while (len) {
count = MIN(conn->mtu, len);
*frag = bluez_skb_alloc(count, GFP_ATOMIC);
if (!*frag)
goto fail;
memcpy(skb_put(*frag, count), data, count);
len -= count;
data += count;
frag = &(*frag)->next;
}
return skb;
fail:
kfree_skb(skb);
return NULL;
}
/* General internal stack event */
void hci_si_event(struct hci_dev *hdev, int type, int dlen, void *data)
{
hci_event_hdr *eh;
evt_stack_internal *si;
struct sk_buff *skb;
int size;
void *ptr;
size = HCI_EVENT_HDR_SIZE + EVT_STACK_INTERNAL_SIZE + dlen;
skb = bluez_skb_alloc(size, GFP_ATOMIC);
if (!skb)
return;
ptr = skb_put(skb, size);
eh = ptr;
eh->evt = EVT_STACK_INTERNAL;
eh->plen = EVT_STACK_INTERNAL_SIZE + dlen;
ptr += HCI_EVENT_HDR_SIZE;
si = ptr;
si->type = type;
memcpy(si->data, data, dlen);
skb->pkt_type = HCI_EVENT_PKT;
skb->dev = (void *) hdev;
hci_send_to_sock(hdev, skb);
kfree_skb(skb);
}
static void hci_usb_bulk_read(struct urb *urb)
{
struct hci_usb *husb = (struct hci_usb *) urb->context;
unsigned char *data = urb->transfer_buffer;
int count = urb->actual_length, status;
struct sk_buff *skb;
hci_acl_hdr *ah;
register __u16 dlen;
if (!husb)
return;
DBG("%s status %d, count %d, flags %x", husb->hdev.name, urb->status, count, urb->transfer_flags);
if (urb->status) {
/* Do not re-submit URB on critical errors */
switch (urb->status) {
case -ENOENT:
return;
default:
goto resubmit;
};
}
if (!count)
goto resubmit;
DMP(data, count);
ah = (hci_acl_hdr *) data;
dlen = le16_to_cpu(ah->dlen);
/* Verify frame len and completeness */
if ((count - HCI_ACL_HDR_SIZE) != dlen) {
ERR("%s corrupted ACL packet: count %d, plen %d", husb->hdev.name, count, dlen);
goto resubmit;
}
/* Allocate packet */
if (!(skb = bluez_skb_alloc(count, GFP_ATOMIC))) {
ERR("Can't allocate mem for new packet");
goto resubmit;
}
memcpy(skb_put(skb, count), data, count);
skb->dev = (void *) &husb->hdev;
skb->pkt_type = HCI_ACLDATA_PKT;
husb->hdev.stat.byte_rx += skb->len;
hci_recv_frame(skb);
resubmit:
husb->read_urb->dev = husb->udev;
if ((status = usb_submit_urb(husb->read_urb)))
DBG("%s read URB submit failed %d", husb->hdev.name, status);
DBG("%s read URB re-submited", husb->hdev.name);
}
static int bluecard_hci_set_baud_rate(struct hci_dev *hdev, int baud)
{
bluecard_info_t *info = (bluecard_info_t *)(hdev->driver_data);
struct sk_buff *skb;
/* Ericsson baud rate command */
unsigned char cmd[] = { HCI_COMMAND_PKT, 0x09, 0xfc, 0x01, 0x03 };
if (!(skb = bluez_skb_alloc(HCI_MAX_FRAME_SIZE, GFP_ATOMIC))) {
printk(KERN_WARNING "bluecard_cs: Can't allocate mem for new packet.\n");
return -1;
}
switch (baud) {
case 460800:
cmd[4] = 0x00;
skb->pkt_type = PKT_BAUD_RATE_460800;
break;
case 230400:
cmd[4] = 0x01;
skb->pkt_type = PKT_BAUD_RATE_230400;
break;
case 115200:
cmd[4] = 0x02;
skb->pkt_type = PKT_BAUD_RATE_115200;
break;
case 57600:
/* Fall through... */
default:
cmd[4] = 0x03;
skb->pkt_type = PKT_BAUD_RATE_57600;
break;
}
memcpy(skb_put(skb, sizeof(cmd)), cmd, sizeof(cmd));
skb_queue_tail(&(info->txq), skb);
bluecard_write_wakeup(info);
return 0;
}
/* Get packet from user space buffer(already verified) */
static inline ssize_t hci_vhci_get_user(struct hci_vhci_struct *hci_vhci, const char *buf, size_t count)
{
struct sk_buff *skb;
if (count > HCI_MAX_FRAME_SIZE)
return -EINVAL;
if (!(skb = bluez_skb_alloc(count, GFP_KERNEL)))
return -ENOMEM;
copy_from_user(skb_put(skb, count), buf, count);
skb->dev = (void *) &hci_vhci->hdev;
skb->pkt_type = *((__u8 *) skb->data);
skb_pull(skb, 1);
hci_recv_frame(skb);
return count;
}
static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, __u16 flags)
{
struct l2cap_conn *conn = hcon->l2cap_data;
if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
goto drop;
BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
if (flags & ACL_START) {
l2cap_hdr *hdr;
int len;
if (conn->rx_len) {
BT_ERR("Unexpected start frame (len %d)", skb->len);
kfree_skb(conn->rx_skb);
conn->rx_skb = NULL;
conn->rx_len = 0;
l2cap_conn_unreliable(conn, ECOMM);
}
if (skb->len < 2) {
BT_ERR("Frame is too short (len %d)", skb->len);
l2cap_conn_unreliable(conn, ECOMM);
goto drop;
}
hdr = (l2cap_hdr *) skb->data;
len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
if (len == skb->len) {
/* Complete frame received */
l2cap_recv_frame(conn, skb);
return 0;
}
BT_DBG("Start: total len %d, frag len %d", len, skb->len);
if (skb->len > len) {
BT_ERR("Frame is too long (len %d, expected len %d)",
skb->len, len);
l2cap_conn_unreliable(conn, ECOMM);
goto drop;
}
/* Allocate skb for the complete frame including header */
conn->rx_skb = bluez_skb_alloc(len, GFP_ATOMIC);
if (!conn->rx_skb)
goto drop;
memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
conn->rx_len = len - skb->len;
} else {
BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
if (!conn->rx_len) {
BT_ERR("Unexpected continuation frame (len %d)", skb->len);
l2cap_conn_unreliable(conn, ECOMM);
goto drop;
}
if (skb->len > conn->rx_len) {
BT_ERR("Fragment is too long (len %d, expected %d)",
skb->len, conn->rx_len);
kfree_skb(conn->rx_skb);
conn->rx_skb = NULL;
conn->rx_len = 0;
l2cap_conn_unreliable(conn, ECOMM);
goto drop;
}
memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
conn->rx_len -= skb->len;
if (!conn->rx_len) {
/* Complete frame received */
l2cap_recv_frame(conn, conn->rx_skb);
conn->rx_skb = NULL;
}
}
drop:
kfree_skb(skb);
return 0;
}
static void hci_usb_intr(struct urb *urb)
{
struct hci_usb *husb = (struct hci_usb *) urb->context;
unsigned char *data = urb->transfer_buffer;
register int count = urb->actual_length;
register struct sk_buff *skb = husb->intr_skb;
hci_event_hdr *eh;
register int len;
if (!husb)
return;
DBG("%s count %d", husb->hdev.name, count);
if (urb->status || !count) {
DBG("%s intr status %d, count %d", husb->hdev.name, urb->status, count);
return;
}
/* Do we really have to handle continuations here ? */
if (!skb) {
/* New frame */
if (count < HCI_EVENT_HDR_SIZE) {
DBG("%s bad frame len %d", husb->hdev.name, count);
return;
}
eh = (hci_event_hdr *) data;
len = eh->plen + HCI_EVENT_HDR_SIZE;
if (count > len) {
DBG("%s corrupted frame, len %d", husb->hdev.name, count);
return;
}
/* Allocate skb */
if (!(skb = bluez_skb_alloc(len, GFP_ATOMIC))) {
ERR("Can't allocate mem for new packet");
return;
}
skb->dev = (void *) &husb->hdev;
skb->pkt_type = HCI_EVENT_PKT;
husb->intr_skb = skb;
husb->intr_count = len;
} else {
/* Continuation */
if (count > husb->intr_count) {
ERR("%s bad frame len %d (expected %d)", husb->hdev.name, count, husb->intr_count);
kfree_skb(skb);
husb->intr_skb = NULL;
husb->intr_count = 0;
return;
}
}
memcpy(skb_put(skb, count), data, count);
husb->intr_count -= count;
DMP(data, count);
if (!husb->intr_count) {
/* Got complete frame */
husb->hdev.stat.byte_rx += skb->len;
hci_recv_frame(skb);
husb->intr_skb = NULL;
}
}
/* Recv data */
static int h4_recv(struct hci_uart *hu, void *data, int count)
{
struct h4_struct *h4 = hu->priv;
register char *ptr;
hci_event_hdr *eh;
hci_acl_hdr *ah;
hci_sco_hdr *sh;
register int len, type, dlen;
BT_DBG("hu %p count %d rx_state %ld rx_count %ld",
hu, count, h4->rx_state, h4->rx_count);
ptr = data;
while (count) {
if (h4->rx_count) {
len = MIN(h4->rx_count, count);
memcpy(skb_put(h4->rx_skb, len), ptr, len);
h4->rx_count -= len; count -= len; ptr += len;
if (h4->rx_count)
continue;
switch (h4->rx_state) {
case H4_W4_DATA:
BT_DBG("Complete data");
BT_DMP(h4->rx_skb->data, h4->rx_skb->len);
hci_recv_frame(h4->rx_skb);
h4->rx_state = H4_W4_PACKET_TYPE;
h4->rx_skb = NULL;
continue;
case H4_W4_EVENT_HDR:
eh = (hci_event_hdr *) h4->rx_skb->data;
BT_DBG("Event header: evt 0x%2.2x plen %d", eh->evt, eh->plen);
h4_check_data_len(h4, eh->plen);
continue;
case H4_W4_ACL_HDR:
ah = (hci_acl_hdr *) h4->rx_skb->data;
dlen = __le16_to_cpu(ah->dlen);
BT_DBG("ACL header: dlen %d", dlen);
h4_check_data_len(h4, dlen);
continue;
case H4_W4_SCO_HDR:
sh = (hci_sco_hdr *) h4->rx_skb->data;
BT_DBG("SCO header: dlen %d", sh->dlen);
h4_check_data_len(h4, sh->dlen);
continue;
}
}
/* H4_W4_PACKET_TYPE */
switch (*ptr) {
case HCI_EVENT_PKT:
BT_DBG("Event packet");
h4->rx_state = H4_W4_EVENT_HDR;
h4->rx_count = HCI_EVENT_HDR_SIZE;
type = HCI_EVENT_PKT;
break;
case HCI_ACLDATA_PKT:
BT_DBG("ACL packet");
h4->rx_state = H4_W4_ACL_HDR;
h4->rx_count = HCI_ACL_HDR_SIZE;
type = HCI_ACLDATA_PKT;
break;
case HCI_SCODATA_PKT:
BT_DBG("SCO packet");
h4->rx_state = H4_W4_SCO_HDR;
h4->rx_count = HCI_SCO_HDR_SIZE;
type = HCI_SCODATA_PKT;
break;
default:
BT_ERR("Unknown HCI packet type %2.2x", (__u8)*ptr);
hu->hdev.stat.err_rx++;
ptr++; count--;
continue;
};
ptr++; count--;
/* Allocate packet */
h4->rx_skb = bluez_skb_alloc(HCI_MAX_FRAME_SIZE, GFP_ATOMIC);
if (!h4->rx_skb) {
BT_ERR("Can't allocate mem for new packet");
h4->rx_state = H4_W4_PACKET_TYPE;
h4->rx_count = 0;
return 0;
}
h4->rx_skb->dev = (void *) &hu->hdev;
h4->rx_skb->pkt_type = type;
}
return count;
}
static inline void n_hci_rx(struct n_hci *n_hci, const __u8 * data, char *flags, int count)
{
register const char *ptr;
hci_event_hdr *eh;
hci_acl_hdr *ah;
hci_sco_hdr *sh;
register int len, type, dlen;
DBG("count %d state %ld rx_count %ld", count, n_hci->rx_state, n_hci->rx_count);
n_hci->hdev.stat.byte_rx += count;
ptr = data;
while (count) {
if (n_hci->rx_count) {
len = MIN(n_hci->rx_count, count);
memcpy(skb_put(n_hci->rx_skb, len), ptr, len);
n_hci->rx_count -= len; count -= len; ptr += len;
if (n_hci->rx_count)
continue;
switch (n_hci->rx_state) {
case WAIT_DATA:
DBG("Complete data");
DMP(n_hci->rx_skb->data, n_hci->rx_skb->len);
hci_recv_frame(n_hci->rx_skb);
n_hci->rx_state = WAIT_PACKET_TYPE;
n_hci->rx_skb = NULL;
continue;
case WAIT_EVENT_HDR:
eh = (hci_event_hdr *) n_hci->rx_skb->data;
DBG("Event header: evt 0x%2.2x plen %d", eh->evt, eh->plen);
n_hci_check_data_len(n_hci, eh->plen);
continue;
case WAIT_ACL_HDR:
ah = (hci_acl_hdr *) n_hci->rx_skb->data;
dlen = __le16_to_cpu(ah->dlen);
DBG("ACL header: dlen %d", dlen);
n_hci_check_data_len(n_hci, dlen);
continue;
case WAIT_SCO_HDR:
sh = (hci_sco_hdr *) n_hci->rx_skb->data;
DBG("SCO header: dlen %d", sh->dlen);
n_hci_check_data_len(n_hci, sh->dlen);
continue;
};
}
/* WAIT_PACKET_TYPE */
switch (*ptr) {
case HCI_EVENT_PKT:
DBG("Event packet");
n_hci->rx_state = WAIT_EVENT_HDR;
n_hci->rx_count = HCI_EVENT_HDR_SIZE;
type = HCI_EVENT_PKT;
break;
case HCI_ACLDATA_PKT:
DBG("ACL packet");
n_hci->rx_state = WAIT_ACL_HDR;
n_hci->rx_count = HCI_ACL_HDR_SIZE;
type = HCI_ACLDATA_PKT;
break;
case HCI_SCODATA_PKT:
DBG("SCO packet");
n_hci->rx_state = WAIT_SCO_HDR;
n_hci->rx_count = HCI_SCO_HDR_SIZE;
type = HCI_SCODATA_PKT;
break;
default:
ERR("Unknown HCI packet type %2.2x", (__u8)*ptr);
n_hci->hdev.stat.err_rx++;
ptr++; count--;
continue;
};
ptr++; count--;
/* Allocate packet */
if (!(n_hci->rx_skb = bluez_skb_alloc(HCI_MAX_FRAME_SIZE, GFP_ATOMIC))) {
ERR("Can't allocate mem for new packet");
n_hci->rx_state = WAIT_PACKET_TYPE;
n_hci->rx_count = 0;
return;
}
n_hci->rx_skb->dev = (void *) &n_hci->hdev;
n_hci->rx_skb->pkt_type = type;
}
}
static void bluecard_receive(bluecard_info_t *info, unsigned int offset)
{
unsigned int iobase;
unsigned char buf[31];
int i, len;
if (!info) {
printk(KERN_WARNING "bluecard_cs: Call of receive for unknown device.\n");
return;
}
iobase = info->link.io.BasePort1;
if (test_bit(XMIT_SENDING_READY, &(info->tx_state)))
bluecard_enable_activity_led(info);
len = bluecard_read(iobase, offset, buf, sizeof(buf));
for (i = 0; i < len; i++) {
/* Allocate packet */
if (info->rx_skb == NULL) {
info->rx_state = RECV_WAIT_PACKET_TYPE;
info->rx_count = 0;
if (!(info->rx_skb = bluez_skb_alloc(HCI_MAX_FRAME_SIZE, GFP_ATOMIC))) {
printk(KERN_WARNING "bluecard_cs: Can't allocate mem for new packet.\n");
return;
}
}
if (info->rx_state == RECV_WAIT_PACKET_TYPE) {
info->rx_skb->dev = (void *)&(info->hdev);
info->rx_skb->pkt_type = buf[i];
switch (info->rx_skb->pkt_type) {
case 0x00:
/* init packet */
if (offset != 0x00) {
set_bit(XMIT_BUF_ONE_READY, &(info->tx_state));
set_bit(XMIT_BUF_TWO_READY, &(info->tx_state));
set_bit(XMIT_SENDING_READY, &(info->tx_state));
bluecard_write_wakeup(info);
}
kfree_skb(info->rx_skb);
info->rx_skb = NULL;
break;
case HCI_EVENT_PKT:
info->rx_state = RECV_WAIT_EVENT_HEADER;
info->rx_count = HCI_EVENT_HDR_SIZE;
break;
case HCI_ACLDATA_PKT:
info->rx_state = RECV_WAIT_ACL_HEADER;
info->rx_count = HCI_ACL_HDR_SIZE;
break;
case HCI_SCODATA_PKT:
info->rx_state = RECV_WAIT_SCO_HEADER;
info->rx_count = HCI_SCO_HDR_SIZE;
break;
default:
/* unknown packet */
printk(KERN_WARNING "bluecard_cs: Unknown HCI packet with type 0x%02x received.\n", info->rx_skb->pkt_type);
info->hdev.stat.err_rx++;
kfree_skb(info->rx_skb);
info->rx_skb = NULL;
break;
}
} else {
*skb_put(info->rx_skb, 1) = buf[i];
info->rx_count--;
if (info->rx_count == 0) {
int dlen;
hci_event_hdr *eh;
hci_acl_hdr *ah;
hci_sco_hdr *sh;
switch (info->rx_state) {
case RECV_WAIT_EVENT_HEADER:
eh = (hci_event_hdr *)(info->rx_skb->data);
info->rx_state = RECV_WAIT_DATA;
info->rx_count = eh->plen;
break;
case RECV_WAIT_ACL_HEADER:
ah = (hci_acl_hdr *)(info->rx_skb->data);
dlen = __le16_to_cpu(ah->dlen);
info->rx_state = RECV_WAIT_DATA;
info->rx_count = dlen;
break;
case RECV_WAIT_SCO_HEADER:
sh = (hci_sco_hdr *)(info->rx_skb->data);
info->rx_state = RECV_WAIT_DATA;
info->rx_count = sh->dlen;
break;
case RECV_WAIT_DATA:
hci_recv_frame(info->rx_skb);
info->rx_skb = NULL;
break;
}
}
}
}
info->hdev.stat.byte_rx += len;
}
