static int brf_send_command_socket(int fd, struct bts_action_send *send_action)
{
char response[1024] = {0};
hci_command_hdr *cmd = (hci_command_hdr *) send_action->data;
uint16_t opcode = cmd->opcode;
struct hci_request rq;
memset(&rq, 0, sizeof(rq));
rq.ogf = cmd_opcode_ogf(opcode);
rq.ocf = cmd_opcode_ocf(opcode);
rq.event = EVT_CMD_COMPLETE;
rq.cparam = &send_action->data[3];
rq.clen = send_action->data[2];
rq.rparam = response;
rq.rlen = sizeof(response);
if (hci_send_req(fd, &rq, 15) < 0) {
perror("Cannot send hci command to socket");
return -1;
}
/* verify success */
if (response[0]) {
errno = EIO;
return -1;
}
return 0;
}
static void hci_command(uint8_t *data)
{
hci_command_hdr *ch;
uint8_t *ptr = data;
uint16_t ogf, ocf;
ch = (hci_command_hdr *) ptr;
ptr += HCI_COMMAND_HDR_SIZE;
ch->opcode = btohs(ch->opcode);
ogf = cmd_opcode_ogf(ch->opcode);
ocf = cmd_opcode_ocf(ch->opcode);
switch (ogf) {
case OGF_LINK_CTL:
hci_link_control(ocf, ch->plen, ptr);
break;
case OGF_LINK_POLICY:
hci_link_policy(ocf, ch->plen, ptr);
break;
case OGF_HOST_CTL:
hci_host_control(ocf, ch->plen, ptr);
break;
case OGF_INFO_PARAM:
hci_info_param(ocf, ch->plen, ptr);
break;
}
}
void packet_hci_command(struct timeval *tv, uint16_t index,
const void *data, uint16_t size)
{
const hci_command_hdr *hdr = data;
uint16_t opcode = btohs(hdr->opcode);
uint16_t ogf = cmd_opcode_ogf(opcode);
uint16_t ocf = cmd_opcode_ocf(opcode);
btsnoop_write(tv, index, 0x02, data, size);
print_header(tv, index);
if (size < HCI_COMMAND_HDR_SIZE) {
printf("* Malformed HCI Command packet\n");
return;
}
printf("< HCI Command: %s (0x%2.2x|0x%4.4x) plen %d\n",
opcode2str(opcode), ogf, ocf, hdr->plen);
data += HCI_COMMAND_HDR_SIZE;
size -= HCI_COMMAND_HDR_SIZE;
packet_hexdump(data, size);
}
static int hci_sock_sendmsg(struct socket *sock, struct msghdr *msg, int len,
struct scm_cookie *scm)
{
struct sock *sk = sock->sk;
struct hci_dev *hdev;
struct sk_buff *skb;
int err;
BT_DBG("sock %p sk %p", sock, sk);
if (msg->msg_flags & MSG_OOB)
return -EOPNOTSUPP;
if (msg->msg_flags & ~(MSG_DONTWAIT|MSG_NOSIGNAL|MSG_ERRQUEUE))
return -EINVAL;
if (len < 4)
return -EINVAL;
lock_sock(sk);
if (!(hdev = hci_pi(sk)->hdev)) {
err = -EBADFD;
goto done;
}
if (!(skb = bluez_skb_send_alloc(sk, len, msg->msg_flags & MSG_DONTWAIT, &err)))
goto done;
if (memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len)) {
err = -EFAULT;
goto drop;
}
skb->pkt_type = *((unsigned char *) skb->data);
skb_pull(skb, 1);
skb->dev = (void *) hdev;
if (skb->pkt_type == HCI_COMMAND_PKT) {
u16 opcode = __le16_to_cpu(get_unaligned((u16 *)skb->data));
u16 ogf = cmd_opcode_ogf(opcode);
u16 ocf = cmd_opcode_ocf(opcode);
if (((ogf > HCI_SFLT_MAX_OGF) ||
!hci_test_bit(ocf & HCI_FLT_OCF_BITS, &hci_sec_filter.ocf_mask[ogf])) &&
!capable(CAP_NET_RAW)) {
err = -EPERM;
goto drop;
}
if (test_bit(HCI_RAW, &hdev->flags) || (ogf == OGF_VENDOR_CMD)) {
skb_queue_tail(&hdev->raw_q, skb);
hci_sched_tx(hdev);
} else {
skb_queue_tail(&hdev->cmd_q, skb);
hci_sched_cmd(hdev);
}
} else {
if (!capable(CAP_NET_RAW)) {
err = -EPERM;
goto drop;
}
skb_queue_tail(&hdev->raw_q, skb);
hci_sched_tx(hdev);
}
err = len;
done:
release_sock(sk);
return err;
drop:
kfree_skb(skb);
goto done;
}
static void csrhci_in_packet(struct csrhci_s *s, uint8_t *pkt)
{
uint8_t *rpkt;
int opc;
switch (*pkt ++) {
case H4_CMD_PKT:
opc = le16_to_cpu(((struct hci_command_hdr *) pkt)->opcode);
if (cmd_opcode_ogf(opc) == OGF_VENDOR_CMD) {
csrhci_in_packet_vendor(s, cmd_opcode_ocf(opc),
pkt + sizeof(struct hci_command_hdr),
s->in_len - sizeof(struct hci_command_hdr) - 1);
return;
}
/* TODO: if the command is OCF_READ_LOCAL_COMMANDS or the likes,
* we need to send it to the HCI layer and then add our supported
* commands to the returned mask (such as OGF_VENDOR_CMD). With
* bt-hci.c we could just have hooks for this kind of commands but
* we can't with bt-host.c. */
s->hci->cmd_send(s->hci, pkt, s->in_len - 1);
break;
case H4_EVT_PKT:
goto bad_pkt;
case H4_ACL_PKT:
s->hci->acl_send(s->hci, pkt, s->in_len - 1);
break;
case H4_SCO_PKT:
s->hci->sco_send(s->hci, pkt, s->in_len - 1);
break;
case H4_NEG_PKT:
if (s->in_hdr != sizeof(csrhci_neg_packet) ||
memcmp(pkt - 1, csrhci_neg_packet, s->in_hdr)) {
fprintf(stderr, "%s: got a bad NEG packet\n", __FUNCTION__);
return;
}
pkt += 2;
rpkt = csrhci_out_packet_csr(s, H4_NEG_PKT, 10);
*rpkt ++ = 0x20; /* Operational settings negotation Ok */
memcpy(rpkt, pkt, 7); rpkt += 7;
*rpkt ++ = 0xff;
*rpkt ++ = 0xff;
break;
case H4_ALIVE_PKT:
if (s->in_hdr != 4 || pkt[1] != 0x55 || pkt[2] != 0x00) {
fprintf(stderr, "%s: got a bad ALIVE packet\n", __FUNCTION__);
return;
}
rpkt = csrhci_out_packet_csr(s, H4_ALIVE_PKT, 2);
*rpkt ++ = 0xcc;
*rpkt ++ = 0x00;
break;
default:
bad_pkt:
/* TODO: error out */
fprintf(stderr, "%s: got a bad packet\n", __FUNCTION__);
break;
}
csrhci_fifo_wake(s);
}
static struct scope_list *get_scope_type(uint8_t type, void *filter1,
void *filter2)
{
struct scope_list *list = NULL;
struct scope_node *scope_node;
struct hciseq_node *seq_node = seq->current;
uint16_t opcode, node_opcode;
uint8_t node_ogf, ogf = 0x00;
uint16_t node_ocf, ocf = 0x0000;
uint8_t node_evt, evt = 0x00;
bool match;
int pos = 1;
struct hciseq_attr *attr;
if (type == BT_H4_CMD_PKT) {
ogf = *((uint8_t *) filter1);
ocf = *((uint16_t *) filter2);
opcode = cmd_opcode_pack(ogf, ocf);
if (opcode > 0x2FFF) {
attr = NULL;
} else {
attr = type_cfg->cmd[opcode];
if (attr == NULL) {
attr = malloc(sizeof(*attr));
type_cfg->cmd[opcode] = attr;
}
}
} else if (type == BT_H4_EVT_PKT) {
evt = *((uint8_t *) filter1);
attr = type_cfg->evt[evt];
if (attr == NULL) {
attr = malloc(sizeof(*attr));
type_cfg->evt[evt] = attr;
}
} else if (type == BT_H4_ACL_PKT) {
attr = type_cfg->acl;
if (attr == NULL) {
attr = malloc(sizeof(*attr));
type_cfg->acl = attr;
}
} else {
attr = NULL;
}
/* add matching packets in sequence */
while (seq_node != NULL) {
match = false;
if (((uint8_t *) seq_node->frame->data)[0] == type) {
if (type == BT_H4_CMD_PKT) {
node_opcode = *((uint16_t *)
(seq_node->frame->data + 1));
node_ogf = cmd_opcode_ogf(node_opcode);
node_ocf = cmd_opcode_ocf(node_opcode);
if (node_ogf == ogf && node_ocf == ocf)
match = true;
} else if (type == BT_H4_EVT_PKT) {
node_evt = ((uint8_t *)
seq_node->frame->data)[1];
if (evt == node_evt)
match = true;
} else if (type == BT_H4_ACL_PKT) {
match = true;
}
}
if (match) {
if (verbose)
printf("\tadd packet [%d]\n", pos);
if (list == NULL) {
list = malloc(sizeof(*list));
scope_node = malloc(sizeof(*scope_node));
list->head = scope_node;
} else {
scope_node->next = malloc(sizeof(*scope_node));
scope_node = scope_node->next;
}
scope_node->attr = seq_node->attr;
scope_node->pos = pos;
scope_node->next = NULL;
}
seq_node = seq_node->next;
pos++;
}
/* add type config */
if (attr != NULL) {
if (list == NULL) {
list = malloc(sizeof(*list));
scope_node = malloc(sizeof(*scope_node));
list->head = scope_node;
} else {
scope_node->next = malloc(sizeof(*scope_node));
scope_node = scope_node->next;
}
scope_node->attr = attr;
scope_node->pos = 0;
scope_node->next = NULL;
}
return list;
}
void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
{
hci_event_hdr *he = (hci_event_hdr *) skb->data;
evt_cmd_status *cs;
evt_cmd_complete *ec;
__u16 opcode, ocf, ogf;
skb_pull(skb, HCI_EVENT_HDR_SIZE);
BT_DBG("%s evt 0x%x", hdev->name, he->evt);
switch (he->evt) {
case EVT_NUM_COMP_PKTS:
hci_num_comp_pkts_evt(hdev, skb);
break;
case EVT_INQUIRY_COMPLETE:
hci_inquiry_complete_evt(hdev, skb);
break;
case EVT_INQUIRY_RESULT:
hci_inquiry_result_evt(hdev, skb);
break;
case EVT_INQUIRY_RESULT_WITH_RSSI:
hci_inquiry_result_with_rssi_evt(hdev, skb);
break;
case EVT_CONN_REQUEST:
hci_conn_request_evt(hdev, skb);
break;
case EVT_CONN_COMPLETE:
hci_conn_complete_evt(hdev, skb);
break;
case EVT_DISCONN_COMPLETE:
hci_disconn_complete_evt(hdev, skb);
break;
case EVT_ROLE_CHANGE:
hci_role_change_evt(hdev, skb);
break;
case EVT_AUTH_COMPLETE:
hci_auth_complete_evt(hdev, skb);
break;
case EVT_ENCRYPT_CHANGE:
hci_encrypt_change_evt(hdev, skb);
break;
case EVT_CMD_STATUS:
cs = (evt_cmd_status *) skb->data;
skb_pull(skb, EVT_CMD_STATUS_SIZE);
opcode = __le16_to_cpu(cs->opcode);
ogf = cmd_opcode_ogf(opcode);
ocf = cmd_opcode_ocf(opcode);
switch (ogf) {
case OGF_INFO_PARAM:
hci_cs_info_param(hdev, ocf, cs->status);
break;
case OGF_HOST_CTL:
hci_cs_host_ctl(hdev, ocf, cs->status);
break;
case OGF_LINK_CTL:
hci_cs_link_ctl(hdev, ocf, cs->status);
break;
case OGF_LINK_POLICY:
hci_cs_link_policy(hdev, ocf, cs->status);
break;
default:
BT_DBG("%s Command Status OGF %x", hdev->name, ogf);
break;
};
if (cs->ncmd) {
atomic_set(&hdev->cmd_cnt, 1);
if (!skb_queue_empty(&hdev->cmd_q))
hci_sched_cmd(hdev);
}
break;
case EVT_CMD_COMPLETE:
ec = (evt_cmd_complete *) skb->data;
skb_pull(skb, EVT_CMD_COMPLETE_SIZE);
opcode = __le16_to_cpu(ec->opcode);
ogf = cmd_opcode_ogf(opcode);
ocf = cmd_opcode_ocf(opcode);
switch (ogf) {
case OGF_INFO_PARAM:
hci_cc_info_param(hdev, ocf, skb);
break;
case OGF_HOST_CTL:
hci_cc_host_ctl(hdev, ocf, skb);
break;
case OGF_LINK_CTL:
hci_cc_link_ctl(hdev, ocf, skb);
break;
case OGF_LINK_POLICY:
hci_cc_link_policy(hdev, ocf, skb);
break;
default:
BT_DBG("%s Command Completed OGF %x", hdev->name, ogf);
break;
};
if (ec->ncmd) {
atomic_set(&hdev->cmd_cnt, 1);
if (!skb_queue_empty(&hdev->cmd_q))
hci_sched_cmd(hdev);
}
break;
};
kfree_skb(skb);
hdev->stat.evt_rx++;
}
static int hci_sock_sendmsg(struct socket *sock, struct msghdr *msg, int len,
struct scm_cookie *scm)
{
struct sock *sk = sock->sk;
struct hci_dev *hdev;
struct sk_buff *skb;
int err;
BT_DBG("sock %p sk %p", sock, sk);
if (msg->msg_flags & MSG_OOB)
return -EOPNOTSUPP;
if (msg->msg_flags & ~(MSG_DONTWAIT|MSG_NOSIGNAL|MSG_ERRQUEUE))
return -EINVAL;
if (len < 4)
return -EINVAL;
lock_sock(sk);
if (!(hdev = hci_pi(sk)->hdev)) {
err = -EBADFD;
goto done;
}
if (!(skb = bluez_skb_send_alloc(sk, len, msg->msg_flags & MSG_DONTWAIT, &err)))
goto done;
if (memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len)) {
err = -EFAULT;
goto drop;
}
skb->pkt_type = *((unsigned char *) skb->data);
skb_pull(skb, 1);
if (!capable(CAP_NET_RAW)) {
err = -EPERM;
if (skb->pkt_type == HCI_COMMAND_PKT) {
__u16 opcode = __le16_to_cpu(*(__u16 *)skb->data);
__u16 ogf = cmd_opcode_ogf(opcode) - 1;
__u16 ocf = cmd_opcode_ocf(opcode) & HCI_FLT_OCF_BITS;
if (ogf > HCI_SFLT_MAX_OGF ||
!hci_test_bit(ocf, &hci_sec_filter.ocf_mask[ogf]))
goto drop;
} else
goto drop;
}
/* Send frame to HCI core */
skb->dev = (void *) hdev;
hci_send_raw(skb);
err = len;
done:
release_sock(sk);
return err;
drop:
kfree_skb(skb);
goto done;
}