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