static void read_index_list_complete(int sk, void *buf, size_t len) { struct mgmt_rp_read_index_list *rp = buf; uint16_t num; int i; if (len < sizeof(*rp)) { error("Too small read index list complete event"); return; } num = btohs(bt_get_unaligned(&rp->num_controllers)); if (num * sizeof(uint16_t) + sizeof(*rp) != len) { error("Incorrect packet size for index list event"); return; } for (i = 0; i < num; i++) { uint16_t index; index = btohs(bt_get_unaligned(&rp->index[i])); add_controller(index); get_connections(sk, index); clear_uuids(index); } }
static int mgmt_read_evt(int sk, int eindex) { int ret; unsigned char buf[MGMT_BUF_SIZE]; struct mgmt_hdr *hdr = (void *) buf; struct pollfd pfd = { .fd = sk, .events = POLLIN }; if((ret = poll(&pfd, 1, 5000)) <= 0) { fprintf(stderr, "Unable to poll management socket: %s (%d)\n", strerror(errno), errno); return -1; } if(pfd.revents != POLLIN) { fprintf(stderr, "Bad event from management socket\n"); return -1; } if((ret = read(sk, buf, sizeof(buf))) < 0) { fprintf(stderr, "Unable to read from management socket: %s (%d)\n", strerror(errno), errno); return -1; } /*int i; for(i=0; i<ret; ++i) { if(!(i%16)) { printf("\n"); } printf("0x%02x ", buf[i]); } printf("\n");*/ uint16_t opcode = btohs(bt_get_unaligned(&hdr->opcode)); uint16_t len = btohs(bt_get_unaligned(&hdr->len)); uint16_t index = btohs(bt_get_unaligned(&hdr->index)); if (ret != MGMT_HDR_SIZE + len) { fprintf(stderr, "Packet length mismatch. ret %d len %u\n", ret, len); return -1; } switch (opcode) { case MGMT_EV_CMD_COMPLETE: return mgmt_cmd_complete(sk, index, buf + MGMT_HDR_SIZE, len); default: fprintf(stderr, "Unknown Management opcode %u (index %u)\n", opcode, index); break; } return 0; }
/* * Extract attribute identifiers from the request PDU. * Clients could request a subset of attributes (by id) * from a service record, instead of the whole set. The * requested identifiers are present in the PDU form of * the request */ static int extract_attrs(sdp_record_t *rec, sdp_list_t *seq, sdp_buf_t *buf) { sdp_buf_t pdu; if (!rec) return SDP_INVALID_RECORD_HANDLE; if (seq) { SDPDBG("Entries in attr seq : %d", sdp_list_len(seq)); } else { SDPDBG("NULL attribute descriptor"); } if (seq == NULL) { SDPDBG("Attribute sequence is NULL"); return 0; } sdp_gen_record_pdu(rec, &pdu); for (; seq; seq = seq->next) { struct attrid *aid = seq->data; SDPDBG("AttrDataType : %d", aid->dtd); if (aid->dtd == SDP_UINT16) { uint16_t attr = bt_get_unaligned((uint16_t *)&aid->uint16); sdp_data_t *a = (sdp_data_t *)sdp_data_get(rec, attr); if (a) sdp_append_to_pdu(buf, a); } else if (aid->dtd == SDP_UINT32) { uint32_t range = bt_get_unaligned((uint32_t *)&aid->uint32); uint16_t attr; uint16_t low = (0xffff0000 & range) >> 16; uint16_t high = 0x0000ffff & range; sdp_data_t *data; SDPDBG("attr range : 0x%x", range); SDPDBG("Low id : 0x%x", low); SDPDBG("High id : 0x%x", high); if (low == 0x0000 && high == 0xffff && pdu.data_size <= buf->buf_size) { /* copy it */ memcpy(buf->data, pdu.data, pdu.data_size); buf->data_size = pdu.data_size; break; } /* (else) sub-range of attributes */ for (attr = low; attr < high; attr++) { data = sdp_data_get(rec, attr); if (data) sdp_append_to_pdu(buf, data); } data = sdp_data_get(rec, high); if (data) sdp_append_to_pdu(buf, data); } else {
static uint32_t get_val(uint8_t *ptr, uint8_t len) { switch (len) { case 1: return *ptr; case 2: return btohs(bt_get_unaligned((uint16_t *) ptr)); case 4: return btohl(bt_get_unaligned((uint32_t *) ptr)); } return 0; }
static void mgmt_connect_failed(int sk, void *buf, size_t len) { struct mgmt_ev_connect_failed *ev = buf; struct controller_info *info; uint16_t index; char addr[18]; if (len < sizeof(*ev)) { error("Too small connect_failed event"); return; } index = btohs(bt_get_unaligned(&ev->index)); ba2str(&ev->bdaddr, addr); DBG("hci%u %s status %u", index, addr, ev->status); if (index > max_index) { error("Unexpected index %u in connect_failed event", index); return; } info = &controllers[index]; btd_event_conn_complete(&info->bdaddr, ev->status, &ev->bdaddr); }
static void mgmt_pin_code_request(int sk, void *buf, size_t len) { struct mgmt_ev_pin_code_request *ev = buf; struct controller_info *info; uint16_t index; char addr[18]; int err; if (len < sizeof(*ev)) { error("Too small pin_code_request event"); return; } index = btohs(bt_get_unaligned(&ev->index)); ba2str(&ev->bdaddr, addr); DBG("hci%u %s", index, addr); if (index > max_index) { error("Unexpected index %u in pin_code_request event", index); return; } info = &controllers[index]; err = btd_event_request_pin(&info->bdaddr, &ev->bdaddr); if (err < 0) { error("btd_event_request_pin: %s", strerror(-err)); mgmt_pincode_reply(index, &ev->bdaddr, NULL); } }
static void set_pairable_complete(int sk, void *buf, size_t len) { struct mgmt_mode *rp = buf; struct controller_info *info; struct btd_adapter *adapter; uint16_t index; if (len < sizeof(*rp)) { error("Too small set pairable complete event"); return; } index = btohs(bt_get_unaligned(&rp->index)); DBG("hci%d pairable %u", index, rp->val); if (index > max_index) { error("Unexpected index %u in pairable complete", index); return; } info = &controllers[index]; info->pairable = rp->val ? TRUE : FALSE; adapter = manager_find_adapter(&info->bdaddr); if (!adapter) return; btd_adapter_pairable_changed(adapter, info->pairable); }
static void disconnect_complete(int sk, void *buf, size_t len) { struct mgmt_rp_disconnect *rp = buf; struct controller_info *info; uint16_t index; char addr[18]; if (len < sizeof(*rp)) { error("Too small disconnect complete event"); return; } index = btohs(bt_get_unaligned(&rp->index)); ba2str(&rp->bdaddr, addr); DBG("hci%d %s disconnected", index, addr); if (index > max_index) { error("Unexpected index %u in disconnect complete", index); return; } info = &controllers[index]; btd_event_disconn_complete(&info->bdaddr, &rp->bdaddr); }
static void get_connections_complete(int sk, void *buf, size_t len) { struct mgmt_rp_get_connections *rp = buf; struct controller_info *info; uint16_t index; int i; if (len < sizeof(*rp)) { error("Too small get_connections complete event"); return; } if (len < (sizeof(*rp) + (rp->conn_count * sizeof(bdaddr_t)))) { error("Too small get_connections complete event"); return; } index = btohs(bt_get_unaligned(&rp->index)); if (index > max_index) { error("Unexpected index %u in get_connections complete", index); return; } info = &controllers[index]; for (i = 0; i < rp->conn_count; i++) { bdaddr_t *bdaddr = g_memdup(&rp->conn[i], sizeof(bdaddr_t)); info->connections = g_slist_append(info->connections, bdaddr); } read_info(sk, index); }
/* * Remove a registered service record */ int service_remove_req(sdp_req_t *req, sdp_buf_t *rsp) { uint8_t *p = req->buf + sizeof(sdp_pdu_hdr_t); uint32_t handle = ntohl(bt_get_unaligned((uint32_t *) p)); sdp_record_t *rec; int status = 0; /* extract service record handle */ rec = sdp_record_find(handle); if (rec) { sdp_svcdb_collect(rec); status = sdp_record_remove(handle); sdp_record_free(rec); if (status == 0) update_db_timestamp(); } else { status = SDP_INVALID_RECORD_HANDLE; SDPDBG("Could not find record : 0x%x", handle); } p = rsp->data; bt_put_unaligned(htons(status), (uint16_t *) p); rsp->data_size = sizeof(uint16_t); return status; }
static void mgmt_new_key(int sk, void *buf, size_t len) { struct mgmt_ev_new_key *ev = buf; struct controller_info *info; uint16_t index; if (len != sizeof(*ev)) { error("new_key event size mismatch (%zu != %zu)", len, sizeof(*ev)); return; } index = btohs(bt_get_unaligned(&ev->index)); DBG("Controller %u new key of type %u pin_len %u", index, ev->key.type, ev->key.pin_len); if (index > max_index) { error("Unexpected index %u in new_key event", index); return; } if (ev->key.pin_len > 16) { error("Invalid PIN length (%u) in new_key event", ev->key.pin_len); return; } info = &controllers[index]; btd_event_link_key_notify(&info->bdaddr, &ev->key.bdaddr, ev->key.val, ev->key.type, ev->key.pin_len, ev->old_key_type); }
static void conf_rfc(void *ptr, int len, int in, uint16_t cid) { uint8_t mode; mode = *((uint8_t *) ptr); set_mode(in, cid, mode); printf("RFC 0x%02x (%s", mode, mode2str(mode)); if (mode == 0x01 || mode == 0x02) { uint8_t txwin, maxtrans; uint16_t rto, mto, mps; txwin = *((uint8_t *) (ptr + 1)); maxtrans = *((uint8_t *) (ptr + 2)); rto = btohs(bt_get_unaligned((uint16_t *) (ptr + 3))); mto = btohs(bt_get_unaligned((uint16_t *) (ptr + 5))); mps = btohs(bt_get_unaligned((uint16_t *) (ptr + 7))); printf(", TxWin %d, MaxTx %d, RTo %d, MTo %d, MPS %d", txwin, maxtrans, rto, mto, mps); } printf(")"); }
static uint16_t bnep_setup_decode(struct bnep_setup_conn_req *req, uint16_t *dst_role, uint16_t *src_role) { uint8_t *dest, *source; dest = req->service; source = req->service + req->uuid_size; switch (req->uuid_size) { case 2: /* UUID16 */ *dst_role = ntohs(bt_get_unaligned((uint16_t *) dest)); *src_role = ntohs(bt_get_unaligned((uint16_t *) source)); break; case 4: /* UUID32 */ case 16: /* UUID128 */ /* * Check that the bytes in the UUID, except the service ID itself, are * correct. The service ID is checked in bnep_setup_chk(). */ if (memcmp(dest, bluetooth_base_uuid.data, 2)) return BNEP_CONN_INVALID_DST; if (memcmp(source, bluetooth_base_uuid.data, 2)) return BNEP_CONN_INVALID_SRC; if (req->uuid_size == 16) { if (memcmp(&dest[4], &bluetooth_base_uuid.data[4], 12)) return BNEP_CONN_INVALID_DST; if (memcmp(&source[4], &bluetooth_base_uuid.data[4], 12)) return BNEP_CONN_INVALID_SRC; } *dst_role = ntohl(bt_get_unaligned((uint32_t *) dest)); *src_role = ntohl(bt_get_unaligned((uint32_t *) source)); break; default: return BNEP_CONN_INVALID_SVC; } return 0; }
static void mgmt_controller_error(int sk, void *buf, size_t len) { struct mgmt_ev_controller_error *ev = buf; uint16_t index; if (len < sizeof(*ev)) { error("Too small management controller error event packet"); return; } index = btohs(bt_get_unaligned(&ev->index)); DBG("index %u error_code %u", index, ev->error_code); }
static void mgmt_cmd_status(int sk, void *buf, size_t len) { struct mgmt_ev_cmd_status *ev = buf; uint16_t opcode; if (len < sizeof(*ev)) { error("Too small management command status event packet"); return; } opcode = btohs(bt_get_unaligned(&ev->opcode)); DBG("status %u opcode %u", ev->status, opcode); }
static void mgmt_index_removed(int sk, void *buf, size_t len) { struct mgmt_ev_index_removed *ev = buf; uint16_t index; if (len < sizeof(*ev)) { error("Too small index removed event"); return; } index = btohs(bt_get_unaligned(&ev->index)); remove_controller(index); }
static uint16_t bnep_setup_decode(struct bnep_setup_conn_req *req, uint16_t *dst_role, uint16_t *src_role) { uint8_t *dest, *source; dest = req->service; source = req->service + req->uuid_size; switch (req->uuid_size) { case 2: /* UUID16 */ *dst_role = ntohs(bt_get_unaligned((uint16_t *) dest)); *src_role = ntohs(bt_get_unaligned((uint16_t *) source)); break; case 4: /* UUID32 */ case 16: /* UUID128 */ *dst_role = ntohl(bt_get_unaligned((uint32_t *) dest)); *src_role = ntohl(bt_get_unaligned((uint32_t *) source)); break; default: return BNEP_CONN_INVALID_SVC; } return 0; }
static void set_powered_complete(int sk, void *buf, size_t len) { struct mgmt_mode *rp = buf; uint16_t index; if (len < sizeof(*rp)) { error("Too small set powered complete event"); return; } index = btohs(bt_get_unaligned(&rp->index)); DBG("hci%d powered %u", index, rp->val); mgmt_update_powered(index, rp->val); }
static void mgmt_powered(int sk, void *buf, size_t len) { struct mgmt_mode *ev = buf; uint16_t index; if (len < sizeof(*ev)) { error("Too small powered event"); return; } index = btohs(bt_get_unaligned(&ev->index)); DBG("Controller %u powered %u", index, ev->val); mgmt_update_powered(index, ev->val); }
static void mgmt_cmd_status(int sk, uint16_t index, void *buf, size_t len) { struct mgmt_ev_cmd_status *ev = buf; uint16_t opcode; if (len < sizeof(*ev)) { error("Too small management command status event packet"); return; } opcode = btohs(bt_get_unaligned(&ev->opcode)); DBG("status %u opcode %u (index %u)", ev->status, opcode, index); switch (opcode) { case MGMT_OP_READ_LOCAL_OOB_DATA: read_local_oob_data_failed(sk, index); break; } }
static void read_version_complete(int sk, void *buf, size_t len) { struct mgmt_hdr hdr; struct mgmt_rp_read_version *rp = buf; if (len < sizeof(*rp)) { error("Too small read version complete event"); return; } mgmt_revision = btohs(bt_get_unaligned(&rp->revision)); mgmt_version = rp->version; DBG("version %u revision %u", mgmt_version, mgmt_revision); memset(&hdr, 0, sizeof(hdr)); hdr.opcode = htobs(MGMT_OP_READ_INDEX_LIST); if (write(sk, &hdr, sizeof(hdr)) < 0) error("Unable to read controller index list: %s (%d)", strerror(errno), errno); }
static void mgmt_connectable(int sk, void *buf, size_t len) { struct mgmt_mode *ev = buf; struct controller_info *info; struct btd_adapter *adapter; uint16_t index; uint8_t mode; if (len < sizeof(*ev)) { error("Too small connectable event"); return; } index = btohs(bt_get_unaligned(&ev->index)); DBG("Controller %u connectable %u", index, ev->val); if (index > max_index) { error("Unexpected index %u in connectable event", index); return; } info = &controllers[index]; info->connectable = ev->val ? TRUE : FALSE; adapter = manager_find_adapter(&info->bdaddr); if (!adapter) return; if (info->discoverable) mode = SCAN_INQUIRY; else mode = 0; if (info->connectable) mode |= SCAN_PAGE; adapter_mode_changed(adapter, mode); }
static void set_discoverable_complete(int sk, void *buf, size_t len) { struct mgmt_mode *rp = buf; struct controller_info *info; struct btd_adapter *adapter; uint16_t index; uint8_t mode; if (len < sizeof(*rp)) { error("Too small set discoverable complete event"); return; } index = btohs(bt_get_unaligned(&rp->index)); DBG("hci%d discoverable %u", index, rp->val); if (index > max_index) { error("Unexpected index %u in discoverable complete", index); return; } info = &controllers[index]; info->discoverable = rp->val ? TRUE : FALSE; adapter = manager_find_adapter(&info->bdaddr); if (!adapter) return; /* set_discoverable will always also change page scanning */ mode = SCAN_PAGE; if (info->discoverable) mode |= SCAN_INQUIRY; adapter_mode_changed(adapter, mode); }
/* * Update a service record */ int service_update_req(sdp_req_t *req, sdp_buf_t *rsp) { sdp_record_t *orec, *nrec; int status = 0, scanned = 0; uint8_t *p = req->buf + sizeof(sdp_pdu_hdr_t); int bufsize = req->len - sizeof(sdp_pdu_hdr_t); uint32_t handle = ntohl(bt_get_unaligned((uint32_t *) p)); SDPDBG("Svc Rec Handle: 0x%x", handle); p += sizeof(uint32_t); bufsize -= sizeof(uint32_t); orec = sdp_record_find(handle); SDPDBG("SvcRecOld: %p", orec); if (!orec) { status = SDP_INVALID_RECORD_HANDLE; goto done; } nrec = extract_pdu_server(BDADDR_ANY, p, bufsize, handle, &scanned); if (!nrec) { status = SDP_INVALID_SYNTAX; goto done; } assert(nrec == orec); update_db_timestamp(); done: p = rsp->data; bt_put_unaligned(htons(status), (uint16_t *) p); rsp->data_size = sizeof(uint16_t); return status; }
static void mgmt_cmd_complete(int sk, void *buf, size_t len) { struct mgmt_ev_cmd_complete *ev = buf; uint16_t opcode; DBG(""); if (len < sizeof(*ev)) { error("Too small management command complete event packet"); return; } opcode = btohs(bt_get_unaligned(&ev->opcode)); switch (opcode) { case MGMT_OP_READ_VERSION: read_version_complete(sk, ev->data, len - sizeof(*ev)); break; case MGMT_OP_READ_INDEX_LIST: read_index_list_complete(sk, ev->data, len - sizeof(*ev)); break; case MGMT_OP_READ_INFO: read_info_complete(sk, ev->data, len - sizeof(*ev)); break; case MGMT_OP_SET_POWERED: set_powered_complete(sk, ev->data, len - sizeof(*ev)); break; case MGMT_OP_SET_DISCOVERABLE: set_discoverable_complete(sk, ev->data, len - sizeof(*ev)); break; case MGMT_OP_SET_CONNECTABLE: set_connectable_complete(sk, ev->data, len - sizeof(*ev)); break; case MGMT_OP_SET_PAIRABLE: set_pairable_complete(sk, ev->data, len - sizeof(*ev)); break; case MGMT_OP_ADD_UUID: DBG("add_uuid complete"); break; case MGMT_OP_REMOVE_UUID: DBG("remove_uuid complete"); break; case MGMT_OP_SET_DEV_CLASS: DBG("set_dev_class complete"); break; case MGMT_OP_SET_SERVICE_CACHE: DBG("set_service_cache complete"); break; case MGMT_OP_LOAD_KEYS: DBG("load_keys complete"); break; case MGMT_OP_REMOVE_KEY: DBG("remove_key complete"); break; case MGMT_OP_DISCONNECT: DBG("disconnect complete"); disconnect_complete(sk, ev->data, len - sizeof(*ev)); break; case MGMT_OP_GET_CONNECTIONS: get_connections_complete(sk, ev->data, len - sizeof(*ev)); break; case MGMT_OP_PIN_CODE_REPLY: DBG("pin_code_reply complete"); break; case MGMT_OP_PIN_CODE_NEG_REPLY: DBG("pin_code_neg_reply complete"); break; case MGMT_OP_SET_IO_CAPABILITY: DBG("set_io_capability complete"); break; default: error("Unknown command complete for opcode %u", opcode); break; } }
/* * Service search request PDU. This method extracts the search pattern * (a sequence of UUIDs) and calls the matching function * to find matching services */ static int service_search_req(sdp_req_t *req, sdp_buf_t *buf) { int status = 0, i, plen, mlen, mtu, scanned; sdp_list_t *pattern = NULL; uint16_t expected, actual, rsp_count = 0; uint8_t dtd; sdp_cont_state_t *cstate = NULL; uint8_t *pCacheBuffer = NULL; int handleSize = 0; uint32_t cStateId = 0; short *pTotalRecordCount, *pCurrentRecordCount; uint8_t *pdata = req->buf + sizeof(sdp_pdu_hdr_t); size_t data_left = req->len - sizeof(sdp_pdu_hdr_t); scanned = extract_des(pdata, data_left, &pattern, &dtd, SDP_TYPE_UUID); if (scanned == -1) { status = SDP_INVALID_SYNTAX; goto done; } pdata += scanned; data_left -= scanned; plen = ntohs(((sdp_pdu_hdr_t *)(req->buf))->plen); mlen = scanned + sizeof(uint16_t) + 1; // ensure we don't read past buffer if (plen < mlen || plen != mlen + *(uint8_t *)(pdata+sizeof(uint16_t))) { status = SDP_INVALID_SYNTAX; goto done; } if (data_left < sizeof(uint16_t)) { status = SDP_INVALID_SYNTAX; goto done; } expected = ntohs(bt_get_unaligned((uint16_t *)pdata)); SDPDBG("Expected count: %d", expected); SDPDBG("Bytes scanned : %d", scanned); pdata += sizeof(uint16_t); data_left -= sizeof(uint16_t); /* * Check if continuation state exists, if yes attempt * to get rsp remainder from cache, else send error */ if (sdp_cstate_get(pdata, data_left, &cstate) < 0) { status = SDP_INVALID_SYNTAX; goto done; } mtu = req->mtu - sizeof(sdp_pdu_hdr_t) - sizeof(uint16_t) - sizeof(uint16_t) - SDP_CONT_STATE_SIZE; actual = MIN(expected, mtu >> 2); /* make space in the rsp buffer for total and current record counts */ pdata = buf->data; /* total service record count = 0 */ pTotalRecordCount = (short *)pdata; bt_put_unaligned(0, (uint16_t *)pdata); pdata += sizeof(uint16_t); buf->data_size += sizeof(uint16_t); /* current service record count = 0 */ pCurrentRecordCount = (short *)pdata; bt_put_unaligned(0, (uint16_t *)pdata); pdata += sizeof(uint16_t); buf->data_size += sizeof(uint16_t); if (cstate == NULL) { /* for every record in the DB, do a pattern search */ sdp_list_t *list = sdp_get_record_list(); handleSize = 0; for (; list && rsp_count < expected; list = list->next) { sdp_record_t *rec = (sdp_record_t *) list->data; SDPDBG("Checking svcRec : 0x%x", rec->handle); if (sdp_match_uuid(pattern, rec->pattern) > 0 && sdp_check_access(rec->handle, &req->device)) { rsp_count++; bt_put_unaligned(htonl(rec->handle), (uint32_t *)pdata); pdata += sizeof(uint32_t); handleSize += sizeof(uint32_t); } } SDPDBG("Match count: %d", rsp_count); buf->data_size += handleSize; bt_put_unaligned(htons(rsp_count), (uint16_t *)pTotalRecordCount); bt_put_unaligned(htons(rsp_count), (uint16_t *)pCurrentRecordCount); if (rsp_count > actual) { /* cache the rsp and generate a continuation state */ cStateId = sdp_cstate_alloc_buf(buf); /* * subtract handleSize since we now send only * a subset of handles */ buf->data_size -= handleSize; } else { /* NULL continuation state */ sdp_set_cstate_pdu(buf, NULL); } } /* under both the conditions below, the rsp buffer is not built yet */ if (cstate || cStateId > 0) { short lastIndex = 0; if (cstate) { /* * Get the previous sdp_cont_state_t and obtain * the cached rsp */ sdp_buf_t *pCache = sdp_get_cached_rsp(cstate); if (pCache) { pCacheBuffer = pCache->data; /* get the rsp_count from the cached buffer */ rsp_count = ntohs(bt_get_unaligned((uint16_t *)pCacheBuffer)); /* get index of the last sdp_record_t sent */ lastIndex = cstate->cStateValue.lastIndexSent; } else { status = SDP_INVALID_CSTATE; goto done; } } else { pCacheBuffer = buf->data; lastIndex = 0; } /* * Set the local buffer pointer to after the * current record count and increment the cached * buffer pointer to beyond the counters */ pdata = (uint8_t *) pCurrentRecordCount + sizeof(uint16_t); /* increment beyond the totalCount and the currentCount */ pCacheBuffer += 2 * sizeof(uint16_t); if (cstate) { handleSize = 0; for (i = lastIndex; (i - lastIndex) < actual && i < rsp_count; i++) { bt_put_unaligned(bt_get_unaligned((uint32_t *)(pCacheBuffer + i * sizeof(uint32_t))), (uint32_t *)pdata); pdata += sizeof(uint32_t); handleSize += sizeof(uint32_t); } } else { handleSize = actual << 2; i = actual; } buf->data_size += handleSize; bt_put_unaligned(htons(rsp_count), (uint16_t *)pTotalRecordCount); bt_put_unaligned(htons(i - lastIndex), (uint16_t *)pCurrentRecordCount); if (i == rsp_count) { /* set "null" continuationState */ sdp_set_cstate_pdu(buf, NULL); } else { /* * there's more: set lastIndexSent to * the new value and move on */ sdp_cont_state_t newState; SDPDBG("Setting non-NULL sdp_cstate_t"); if (cstate) memcpy(&newState, cstate, sizeof(sdp_cont_state_t)); else { memset(&newState, 0, sizeof(sdp_cont_state_t)); newState.timestamp = cStateId; } newState.cStateValue.lastIndexSent = i; sdp_set_cstate_pdu(buf, &newState); } } done: free(cstate); if (pattern) sdp_list_free(pattern, free); return status; }
static void l2cap_parse(int level, struct frame *frm) { l2cap_hdr *hdr = (void *)frm->ptr; uint16_t dlen = btohs(hdr->len); uint16_t cid = btohs(hdr->cid); uint16_t psm; frm->ptr += L2CAP_HDR_SIZE; frm->len -= L2CAP_HDR_SIZE; if (cid == 0x1) { /* Signaling channel */ while (frm->len >= L2CAP_CMD_HDR_SIZE) { l2cap_cmd_hdr *hdr = frm->ptr; frm->ptr += L2CAP_CMD_HDR_SIZE; frm->len -= L2CAP_CMD_HDR_SIZE; if (!p_filter(FILT_L2CAP)) { p_indent(level, frm); printf("L2CAP(s): "); } switch (hdr->code) { case L2CAP_COMMAND_REJ: command_rej(level, frm); break; case L2CAP_CONN_REQ: conn_req(level, frm); break; case L2CAP_CONN_RSP: conn_rsp(level, frm); break; case L2CAP_CONF_REQ: conf_req(level, hdr, frm); break; case L2CAP_CONF_RSP: conf_rsp(level, hdr, frm); break; case L2CAP_DISCONN_REQ: disconn_req(level, frm); break; case L2CAP_DISCONN_RSP: disconn_rsp(level, frm); break; case L2CAP_ECHO_REQ: echo_req(level, hdr, frm); break; case L2CAP_ECHO_RSP: echo_rsp(level, hdr, frm); break; case L2CAP_INFO_REQ: info_req(level, hdr, frm); break; case L2CAP_INFO_RSP: info_rsp(level, hdr, frm); break; default: if (p_filter(FILT_L2CAP)) break; printf("code 0x%2.2x ident %d len %d\n", hdr->code, hdr->ident, btohs(hdr->len)); raw_dump(level, frm); } if (frm->len > btohs(hdr->len)) { frm->len -= btohs(hdr->len); frm->ptr += btohs(hdr->len); } else frm->len = 0; } } else if (cid == 0x2) { /* Connectionless channel */ if (p_filter(FILT_L2CAP)) return; psm = btohs(bt_get_unaligned((uint16_t *) frm->ptr)); frm->ptr += 2; frm->len -= 2; p_indent(level, frm); printf("L2CAP(c): len %d psm %d\n", dlen, psm); raw_dump(level, frm); } else { /* Connection oriented channel */ uint8_t mode = get_mode(!frm->in, cid); uint16_t psm = get_psm(!frm->in, cid); uint16_t ctrl = 0, fcs = 0; uint32_t proto; frm->cid = cid; frm->num = get_num(!frm->in, cid); if (mode > 0) { ctrl = btohs(bt_get_unaligned((uint16_t *) frm->ptr)); frm->ptr += 2; frm->len -= 4; fcs = btohs(bt_get_unaligned((uint16_t *) (frm->ptr + frm->len))); } if (!p_filter(FILT_L2CAP)) { p_indent(level, frm); printf("L2CAP(d): cid 0x%4.4x len %d", cid, dlen); if (mode > 0) printf(" ctrl 0x%4.4x fcs 0x%4.4x", ctrl, fcs); printf(" [psm %d]\n", psm); level++; if (mode > 0) { p_indent(level, frm); printf("%s:", ctrl & 0x01 ? "S-frame" : "I-frame"); if (ctrl & 0x01) { printf(" %s", supervisory2str((ctrl & 0x0c) >> 2)); } else {
static void read_info_complete(int sk, void *buf, size_t len) { struct mgmt_rp_read_info *rp = buf; struct controller_info *info; struct btd_adapter *adapter; uint8_t mode; uint16_t index; char addr[18]; if (len < sizeof(*rp)) { error("Too small read info complete event"); return; } index = btohs(bt_get_unaligned(&rp->index)); if (index > max_index) { error("Unexpected index %u in read info complete", index); return; } mgmt_set_mode(index, MGMT_OP_SET_SERVICE_CACHE, 1); info = &controllers[index]; info->type = rp->type; info->enabled = rp->powered; info->connectable = rp->connectable; info->discoverable = rp->discoverable; info->pairable = rp->pairable; info->sec_mode = rp->sec_mode; bacpy(&info->bdaddr, &rp->bdaddr); memcpy(info->dev_class, rp->dev_class, 3); memcpy(info->features, rp->features, 8); info->manufacturer = btohs(bt_get_unaligned(&rp->manufacturer)); info->hci_ver = rp->hci_ver; info->hci_rev = btohs(bt_get_unaligned(&rp->hci_rev)); ba2str(&info->bdaddr, addr); DBG("hci%u type %u addr %s", index, info->type, addr); DBG("hci%u class 0x%02x%02x%02x", index, info->dev_class[2], info->dev_class[1], info->dev_class[0]); DBG("hci%u manufacturer %d HCI ver %d:%d", index, info->manufacturer, info->hci_ver, info->hci_rev); DBG("hci%u enabled %u discoverable %u pairable %u sec_mode %u", index, info->enabled, info->discoverable, info->pairable, info->sec_mode); adapter = btd_manager_register_adapter(index); if (adapter == NULL) { error("mgmtops: unable to register adapter"); return; } btd_adapter_get_mode(adapter, &mode, NULL, NULL); if (mode == MODE_OFF) { mgmt_set_powered(index, FALSE); return; } if (info->enabled) mgmt_update_powered(index, TRUE); else mgmt_set_powered(index, TRUE); btd_adapter_unref(adapter); }
static int mgmt_cmd_complete(int sk, uint16_t index, void *buf, size_t len) { struct mgmt_ev_cmd_complete *ev = buf; uint16_t opcode; int ret = 0; if (len < sizeof(*ev)) { fprintf(stderr, "Too small management command complete event packet\n"); return -1; } opcode = btohs(bt_get_unaligned(&ev->opcode)); len -= sizeof(*ev); switch (opcode) { case MGMT_OP_SET_POWERED: if(step != E_SET_POWERED) { fprintf(stderr, "unexpected set_powered complete\n"); } else if(ev->status) { fprintf(stderr, "set_powered failed\n"); ret = -1; } break; case MGMT_OP_SET_LOCAL_NAME: if(step != E_SET_LOCAL_NAME) { fprintf(stderr, "unexpected set_local_name complete\n"); } else if(ev->status) { fprintf(stderr, "set_local_name failed\n"); ret = -1; } break; case MGMT_OP_SET_CONNECTABLE: if(step != E_SET_CONNECTABLE) { fprintf(stderr, "unexpected set_connectable complete\n"); } else if(ev->status) { fprintf(stderr, "set_powered failed\n"); ret = -1; } break; /*case MGMT_OP_SET_DEV_CLASS: break;*/ case MGMT_OP_LOAD_LINK_KEYS: if(step != E_LOAD_LINK_KEYS) { fprintf(stderr, "unexpected load_link_keys complete\n"); } else if(ev->status) { fprintf(stderr, "load_link_keys failed\n"); ret = -1; } break; default: fprintf(stderr, "Unhandled command complete for opcode %u\n", opcode); ret = -1; break; } return ret; }
static gboolean mgmt_event(GIOChannel *io, GIOCondition cond, gpointer user_data) { char buf[MGMT_BUF_SIZE]; struct mgmt_hdr *hdr = (void *) buf; int sk; ssize_t ret; uint16_t len, opcode; DBG("cond %d", cond); if (cond & G_IO_NVAL) return FALSE; sk = g_io_channel_unix_get_fd(io); if (cond & (G_IO_ERR | G_IO_HUP)) { error("Error on management socket"); return FALSE; } ret = read(sk, buf, sizeof(buf)); if (ret < 0) { error("Unable to read from management socket: %s (%d)", strerror(errno), errno); return TRUE; } DBG("Received %zd bytes from management socket", ret); if (ret < MGMT_HDR_SIZE) { error("Too small Management packet"); return TRUE; } opcode = btohs(bt_get_unaligned(&hdr->opcode)); len = btohs(bt_get_unaligned(&hdr->len)); if (ret != MGMT_HDR_SIZE + len) { error("Packet length mismatch. ret %zd len %u", ret, len); return TRUE; } switch (opcode) { case MGMT_EV_CMD_COMPLETE: mgmt_cmd_complete(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_CMD_STATUS: mgmt_cmd_status(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_CONTROLLER_ERROR: mgmt_controller_error(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_INDEX_ADDED: mgmt_index_added(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_INDEX_REMOVED: mgmt_index_removed(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_POWERED: mgmt_powered(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_DISCOVERABLE: mgmt_discoverable(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_CONNECTABLE: mgmt_connectable(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_PAIRABLE: mgmt_pairable(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_NEW_KEY: mgmt_new_key(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_DEVICE_CONNECTED: mgmt_device_connected(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_DEVICE_DISCONNECTED: mgmt_device_disconnected(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_CONNECT_FAILED: mgmt_connect_failed(sk, buf + MGMT_HDR_SIZE, len); break; case MGMT_EV_PIN_CODE_REQUEST: mgmt_pin_code_request(sk, buf + MGMT_HDR_SIZE, len); break; default: error("Unknown Management opcode %u", opcode); break; } return TRUE; }