sdp.c

Linux下蓝牙探测工具

static inline void print_de(int level, struct frame *frm, int *split, uint16_t *psm, uint8_t *channel)
{
	int n = 0;
	uint8_t de_type = parse_de_hdr(frm, &n);

	switch (de_type) {
	case SDP_DE_NULL:
		printf(" null");
		break;
	case SDP_DE_UINT:
	case SDP_DE_INT:
	case SDP_DE_BOOL:
		print_int(de_type, level, n, frm, psm, channel);
		break;
	case SDP_DE_UUID:
		if (split) {
			/* Split output by uuids.
			 * Used for printing Protocol Desc List */
			if (*split) {
				printf("\n");
				p_indent(level, NULL);
			}
			++*split;
		}
		print_uuid(n, frm, psm, channel);
		break;
	case SDP_DE_URL:
	case SDP_DE_STRING:
		print_string(n, frm, de_type == SDP_DE_URL? "url": "str");
		break;
	case SDP_DE_SEQ:
		printf(" <");
		print_des(de_type, level, n, frm, split, psm, channel);
		printf(" >");
		break;
	case SDP_DE_ALT:
		printf(" [");
		print_des(de_type, level, n, frm, split, psm, channel);
		printf(" ]");
		break;
	}
}

static inline void print_srv_srch_pat(int level, struct frame *frm)
{
	int len, n1 = 0, n2 = 0;

	p_indent(level, frm);
	printf("pat");

	if (parse_de_hdr(frm, &n1) == SDP_DE_SEQ) {
		len = frm->len;
		while (len - frm->len < n1 && frm->len > 0) {
			if (parse_de_hdr(frm, &n2) == SDP_DE_UUID) {
				print_uuid(n2, frm, NULL, NULL);
			} else {
				printf("\nERROR: Unexpected syntax (UUID)\n");
				raw_dump(level, frm);
			}
		}
		printf("\n");
	} else {
		printf("\nERROR: Unexpected syntax (SEQ)\n");
		raw_dump(level, frm);
	}
}

static inline void print_attr_id_list(int level, struct frame *frm)
{
	uint16_t attr_id;
	uint32_t attr_id_range;
	int len, n1 = 0, n2 = 0;

	p_indent(level, frm);
	printf("aid(s)");

	if (parse_de_hdr(frm, &n1) == SDP_DE_SEQ) {
		len = frm->len;
		while (len - frm->len < n1 && frm->len > 0) {
			/* Print AttributeID */
			if (parse_de_hdr(frm, &n2) == SDP_DE_UINT) {
				char *name;
				switch(n2) {
				case 2:
					attr_id = get_u16(frm);
					name = get_attr_id_name(attr_id);
					if (!name)
						name = "unknown";
					printf(" 0x%04x (%s)", attr_id, name);
					break;
				case 4:
					attr_id_range = get_u32(frm);
					printf(" 0x%04x - 0x%04x",
							(attr_id_range >> 16),
							(attr_id_range & 0xFFFF));
					break;
				}
			} else {
				printf("\nERROR: Unexpected syntax\n");
				raw_dump(level, frm);
			}
		}
		printf("\n");
	} else {
		printf("\nERROR: Unexpected syntax\n");
		raw_dump(level, frm);
	}
}

static inline void print_attr_list(int level, struct frame *frm)
{
	uint16_t attr_id, psm;
	uint8_t channel;
	int len, split, n1 = 0, n2 = 0;

	if (parse_de_hdr(frm, &n1) == SDP_DE_SEQ) {
		len = frm->len;
		while (len - frm->len < n1 && frm->len > 0) {
			/* Print AttributeID */
			if (parse_de_hdr(frm, &n2) == SDP_DE_UINT && n2 == sizeof(attr_id)) {
				char *name;
				attr_id = get_u16(frm);
				p_indent(level, 0);
				name = get_attr_id_name(attr_id);
				if (!name)
					name = "unknown";
				printf("aid 0x%04x (%s)\n", attr_id, name);
				split = (attr_id != SDP_ATTR_ID_PROTOCOL_DESCRIPTOR_LIST);
				psm = 0;
				channel = 0;

				/* Print AttributeValue */
				p_indent(level + 1, 0);
				print_de(level + 1, frm, split ? NULL: &split,
					attr_id == SDP_ATTR_ID_PROTOCOL_DESCRIPTOR_LIST ? &psm : NULL,
					attr_id == SDP_ATTR_ID_PROTOCOL_DESCRIPTOR_LIST ? &channel : NULL);
				printf("\n");
			} else {
				printf("\nERROR: Unexpected syntax\n");
				raw_dump(level, frm);
				break;
			}
		}
	} else {
		printf("\nERROR: Unexpected syntax\n");
		raw_dump(level, frm);
	}
}

static inline void print_attr_lists(int level, struct frame *frm)
{
	int n = 0, cnt = 0;
	int count = frm->len;

	if (parse_de_hdr(frm, &n) == SDP_DE_SEQ) {
		while (count - frm->len < n && frm->len > 0) {
			p_indent(level, 0);
			printf("record #%d\n", cnt++);
			print_attr_list(level + 2, frm);
		}
	} else {
		printf("\nERROR: Unexpected syntax\n");
		raw_dump(level, frm);
	}
}

static inline void print_cont_state(int level, unsigned char *buf)
{
	uint8_t cont = buf[0];
	int i;

	p_indent(level, 0);
	printf("cont");
	for (i = 0; i < cont + 1; i++)
		printf(" %2.2X", buf[i]);
	printf("\n");
}

static char *pid2str(uint8_t pid)
{
	switch (pid) {
	case SDP_ERROR_RSP:
		return "Error Rsp";
	case SDP_SERVICE_SEARCH_REQ:
		return "SS Req";
	case SDP_SERVICE_SEARCH_RSP:
		return "SS Rsp";
	case SDP_SERVICE_ATTR_REQ:
		return "SA Req";
	case SDP_SERVICE_ATTR_RSP:
		return "SA Rsp";
	case SDP_SERVICE_SEARCH_ATTR_REQ:
		return "SSA Req";
	case SDP_SERVICE_SEARCH_ATTR_RSP:
		return "SSA Rsp";
	default:
		return "Unknown";
	}
}

#define FRAME_TABLE_SIZE 10

static struct frame frame_table[FRAME_TABLE_SIZE];

static int frame_add(struct frame *frm, int count)
{
	register struct frame *fr;
	register unsigned char *data;
	register int i, len = 0, pos = -1;

	for (i = 0; i < FRAME_TABLE_SIZE; i++) {
		if (frame_table[i].handle == frm->handle &&
				frame_table[i].cid == frm->cid) {
			pos = i;
			len = frame_table[i].data_len;
			break;
		}
		if (pos < 0 && !frame_table[i].handle)
			pos = i;
	}

	if (pos < 0 || count <= 0)
		return -EIO;

	data = malloc(len + count);
	if (!data)
		return -ENOMEM;

	fr = &frame_table[pos];

	if (len > 0) {
		memcpy(data, fr->data, len);
		memcpy(data + len, frm->ptr, count);
	} else
		memcpy(data, frm->ptr, count);

	if (fr->data)
		free(fr->data);

	fr->data       = data;
	fr->data_len   = len + count;
	fr->len        = fr->data_len;
	fr->ptr        = fr->data;
	fr->dev_id     = frm->dev_id;
	fr->in         = frm->in;
	fr->ts         = frm->ts;
	fr->handle     = frm->handle;
	fr->cid        = frm->cid;
	fr->num        = frm->num;
	fr->channel    = frm->channel;
	fr->pppdump_fd = frm->pppdump_fd;
	fr->audio_fd   = frm->audio_fd;

	return pos;
}

static struct frame *frame_get(struct frame *frm, int count)
{
	register int pos;

	pos = frame_add(frm, count);
	if (pos < 0)
		return frm;

	frame_table[pos].handle = 0;

	return &frame_table[pos];
}

void sdp_dump(int level, struct frame *frm)
{
	sdp_pdu_hdr *hdr = frm->ptr;
	uint16_t tid = ntohs(hdr->tid);
	uint16_t len = ntohs(hdr->len);
	uint16_t total, count;
	uint8_t cont;

	frm->ptr += SDP_PDU_HDR_SIZE;
	frm->len -= SDP_PDU_HDR_SIZE;

	p_indent(level, frm);
	printf("SDP %s: tid 0x%x len 0x%x\n", pid2str(hdr->pid), tid, len);

	switch (hdr->pid) {
	case SDP_ERROR_RSP:
		p_indent(level + 1, frm);
		printf("code 0x%x info ", get_u16(frm));
		if (frm->len > 0)
			hex_dump(0, frm, frm->len);
		else
			printf("none\n");
		break;

	case SDP_SERVICE_SEARCH_REQ:
		/* Parse ServiceSearchPattern */
		print_srv_srch_pat(level + 1, frm);

		/* Parse MaximumServiceRecordCount */
		p_indent(level + 1, frm);
		printf("max %d\n", get_u16(frm));

		/* Parse ContinuationState */
		print_cont_state(level + 1, frm->ptr);
		break;

	case SDP_SERVICE_SEARCH_RSP:
		/* Parse TotalServiceRecordCount */
		total = get_u16(frm);

		/* Parse CurrentServiceRecordCount */
		count = get_u16(frm);
		p_indent(level + 1, frm);
		if (count < total)
			printf("count %d of %d\n", count, total);
		else
			printf("count %d\n", count);

		/* Parse service record handle(s) */
		if (count > 0) {
			int i;
			p_indent(level + 1, frm);
			printf("handle%s", count > 1 ? "s" : "");
			for (i = 0; i < count; i++)
				printf(" 0x%x", get_u32(frm));
			printf("\n");
		}

		/* Parse ContinuationState */
		print_cont_state(level + 1, frm->ptr);
		break;

	case SDP_SERVICE_ATTR_REQ:
		/* Parse ServiceRecordHandle */
		p_indent(level + 1, frm);
		printf("handle 0x%x\n", get_u32(frm));

		/* Parse MaximumAttributeByteCount */
		p_indent(level + 1, frm);
		printf("max %d\n", get_u16(frm));

		/* Parse ServiceSearchPattern */
		print_attr_id_list(level + 1, frm);

		/* Parse ContinuationState */
		print_cont_state(level + 1, frm->ptr);
		break;

	case SDP_SERVICE_ATTR_RSP:
		/* Parse AttributeByteCount */
		count = get_u16(frm);
		p_indent(level + 1, frm);
		printf("count %d\n", count);

		/* Parse ContinuationState */
		cont = *(unsigned char *)(frm->ptr + count);

		if (cont == 0) {
			/* Parse AttributeList */
			print_attr_list(level + 1, frame_get(frm, count));
		} else
			frame_add(frm, count);

		print_cont_state(level + 1, frm->ptr + count);
		break;

	case SDP_SERVICE_SEARCH_ATTR_REQ:
		/* Parse ServiceSearchPattern */
		print_srv_srch_pat(level + 1, frm);

		/* Parse MaximumAttributeByteCount */
		p_indent(level + 1, frm);
		printf("max %d\n", get_u16(frm));

		/* Parse AttributeList */
		print_attr_id_list(level + 1, frm);

		/* Parse ContinuationState */
		print_cont_state(level + 1, frm->ptr);
		break;

	case SDP_SERVICE_SEARCH_ATTR_RSP:
		/* Parse AttributeByteCount */
		count = get_u16(frm);
		p_indent(level + 1, frm);
		printf("count %d\n", count);

		/* Parse ContinuationState */
		cont = *(unsigned char *)(frm->ptr + count);

		if (cont == 0) {
			/* Parse AttributeLists */
			print_attr_lists(level + 1, frame_get(frm, count));
		} else
			frame_add(frm, count);

		print_cont_state(level + 1, frm->ptr + count);
		break;

	default:
		raw_dump(level + 1, frm);
		break;
	}
}