Beispiel #1
0
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);
	}
}
Beispiel #2
0
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;
}
Beispiel #3
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 {
Beispiel #4
0
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;
}
Beispiel #5
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);
}
Beispiel #6
0
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);
	}
}
Beispiel #7
0
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);
}
Beispiel #8
0
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);
}
Beispiel #9
0
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;
}
Beispiel #11
0
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);
}
Beispiel #12
0
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(")");
}
Beispiel #13
0
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;
}
Beispiel #14
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);
}
Beispiel #15
0
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);
}
Beispiel #16
0
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);
}
Beispiel #17
0
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;
}
Beispiel #18
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);
}
Beispiel #19
0
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;
	}
}
Beispiel #21
0
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);
}
Beispiel #22
0
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);
}
Beispiel #23
0
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;
}
Beispiel #25
0
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;
	}
}
Beispiel #26
0
/*
 * 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;
}
Beispiel #27
0
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 {
Beispiel #28
0
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);
}
Beispiel #29
0
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;
}
Beispiel #30
0
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;
}