bthidctl.c

affix是一个Open Source的蓝牙协议栈

		default:
		case sdp_data_nil:
			s = malloc (sizeof *s);
			memset (s, 0, sizeof *s);
			s->type = tcode;
			s->bytes = len;
			*idx += len;
			break;
		case sdp_data_uint:
		case sdp_data_sint:
		case sdp_data_bool:
			s = malloc (sizeof *s);
			memset (s, 0, sizeof *s);
			s->type = tcode;
			s->bytes = len;
			for (i = 0; i < len; i++)
				s->u.uint = s->u.uint << 8 | data [*idx + i];
			*idx += len;
			break;
		case sdp_data_uuid:
		case sdp_data_str:
		case sdp_data_url:
			s = malloc (sizeof *s + len + 1);
			memset (s, 0, sizeof *s + len + 1);
			s->type = tcode;
			s->bytes = len;
			for (i = 0; i < len; i++)
				s->u.ch [i] = data [*idx + i];
			*idx += len;
			break;
		case sdp_data_seq:
		case sdp_data_alt:
			s = malloc (sizeof *s);
			memset (s, 0, sizeof *s);
			s->type = tcode;
			s->bytes = len;
			lim = *idx + len;
			while (*idx < lim) {
				it = sdp_scan (data, idx);
				sdp_add_to_seq (s, it);
			}
			break;
	}

	return s;
}



int sdp_resp_pnp (unsigned char *data, int nbytes) {
	// Response handler for the 1st (PNP) SDP query
	int     id, parmlen;
	int     total_ct, curr_ct, attr_bytes;
	int     i;
	unsigned short tid;
        struct sdp_data *s;
	
	id = data [0];
	tid = data [1] << 8 | data [2];         /* SDP is big-endian */
	parmlen = data [3] << 8 | data [4];
	data += 5;
	if (nbytes < 5) {
		//fprintf ( stdout, "NOTICE: Device without PNP record (short read at %d bytes)", nbytes);
		return 2;
	}
	if (parmlen > nbytes - 5) {
		//fprintf( stdout, "NOTICE: PNP-record invalid (bad param " 
		//		"length %d; PDU size %d, id 0x%x, tid 0x%x)\n",
		//		parmlen, nbytes, id, tid);
		return 2;
	}
	if (id == 0x01 /* SDP_ErrorResponse */) {
		//if (parmlen < 2)
		//	fprintf ( stdout, "NOTICE: Retrieval of PNP info record yielded unspecific error!\n");
		//else
		//	fprintf ( stdout, "NOTICE: Condition 0x%x during PNP info record retrieval!\n", data [0] << 8 | data [1]);
		return 2;
	}
	if (id == 0x03 /* SDP_ServiceSearchResponse */) {
		if (parmlen < 9) {
			//fprintf ( stdout, "NOTICE: No PNP record (SDP_ServiceSearchResponse too short: %d bytes, parm len %d)!\n", nbytes, parmlen);
			return 2;
		}
		if (tid != sdp_tid) {
			fprintf ( stdout, "NOTICE: PNP record transaction has wrong ID (0x%x != 0x%x) - ignore PNP info!\n",
					tid, sdp_tid);
			return 2;
		}
		total_ct = data [0] << 8 | data [1];
		curr_ct = data [2] << 8 | data [3];
		if (total_ct < 1) {
			//fprintf ( stdout, "NOTICE: No PNP information for this device could be retrieved!\n");
			return 2;
		}
		if (total_ct > 1 || curr_ct != 1) {
			fprintf ( stdout, "NOTICE: Invalid PNP SDP response: too many handles (total %d, curr %d)!\n", total_ct, curr_ct);
			return 2;
		}
		sdp_handle = (unsigned long) data [4] << 24 |
			(unsigned long) data [5] << 16 |
			(unsigned long) data [6] <<  8 |
			(unsigned long) data [7] <<  0;
		sdp_getattr (NULL);
		return	-2;
	}
	if (id != 0x05 /* SDP_ServiceAttributeResponse */) {
		fprintf ( stderr, "The SDP query for PNP info gave an unexpected SDP response id=0x%x!\n", id);
		return 1;
	}
	attr_bytes = data [0] << 8 | data [1];
	data += 2;
	sdp_attr = realloc (sdp_attr, sdp_attr_bytes + attr_bytes);
	memcpy (sdp_attr + sdp_attr_bytes, data, attr_bytes);
	sdp_attr_bytes += attr_bytes;

	if (data [attr_bytes]) {
		/* There's a continuation, go get it */
		sdp_getattr (&data [attr_bytes]);
		i = recv (sdp_sock, data, 1024, 0); // 1024 == hardcoded length of "data" buffer
		return sdp_resp_pnp ( data, i );
	}
	i = 0;
	s = sdp_scan (sdp_attr, &i);
	free (sdp_attr);
	sdp_attr = NULL;
	
	if (i != sdp_attr_bytes) {
		fprintf ( stderr, "The PNP-Info (SDP attribute data element sequence) " \
				"has bad length (%d != %d)!\n", i, sdp_attr_bytes);
		sdp_free (s);
		return 1;
	}

	if (!s) {
		fprintf ( stdout, "NOTICE: PNP info: Invalid SDP record ignored!\n");
		return 2;
	}
	if (s->type != sdp_data_seq) {
		fprintf ( stderr, "The SDP attributes returned (answering the query for PNP info) are not "
				"a data element sequence!\n");
		sdp_free (s);
		return 1;
	}
	if (s->u.seq.items & 1) {
		fprintf ( stderr, "The SDP attribute data element sequence (answering the query for PNP info) " \
				"has an odd number of elements!\n");
	}
        for (i = 0; i < s->u.seq.items - 1; i += 2) {
		if (s->u.seq.item [i]->type == sdp_data_uint) {
				hid_attr_pnp (s->u.seq.item [i]->u.uint,
						s->u.seq.item [i+1]);
		} else {
			fprintf ( stderr, "The SDP attribute data element "
					"sequence (answering the query for PNP info) has non-integer value in "
					"item[%d]!\n", i);
		}
	}
	sdp_free (s);
	return	0;
}

int sdp_resp (unsigned char *data, int nbytes) {
	// Response handler for the 2nd (HIDP) SDP query
	int     id, parmlen;
	int     total_ct, curr_ct, attr_bytes;
	int     i;
	unsigned short tid;
        struct sdp_data *s;
	
	id = data [0];
	tid = data [1] << 8 | data [2];         /* SDP is big-endian */
	parmlen = data [3] << 8 | data [4];
	data += 5;

	if (nbytes < 5) {
		fprintf ( stderr, "SDP response invalid (response too small, %d bytes)", nbytes);
		return 1;
	}
	if (parmlen > nbytes - 5) {
		fprintf( stderr, "SDP response invalid (has bad parameter " \
				"length %d (PDU size %d), id 0x%x, tid 0x%x)\n",
				parmlen, nbytes, id, tid);
		return 1;
	}
	if (id == 0x01 /* SDP_ErrorResponse */) {
		if (parmlen < 2)
			fprintf ( stderr, "SDP response claims unspecified error condition while retrieving HID info!\n");
		else
			fprintf ( stderr, "SDP response claims error condition 0x%x while retrieving HID info!\n", data [0] << 8 | data [1]);
		return 1;
	}
	if (id == 0x03 /* SDP_ServiceSearchResponse */) {
		if (parmlen < 9) {
			fprintf ( stderr, "Invalid SDP_ServiceSearchResponse (too short: %d bytes, parm len %d)!\n", nbytes, parmlen);
			return 1;
		}
		if (tid != sdp_tid) {
			fprintf ( stderr, "SPD ServiceSearchResponse for PNP has wrong transaction ID (0x%x != 0x%x)!\n",
					tid, sdp_tid);
			return 1;
		}
		total_ct = data [0] << 8 | data [1];
		curr_ct = data [2] << 8 | data [3];
		if (total_ct < 1) {
			fprintf ( stderr, "This device does not offer HIDP information (no HID descriptor available)!\n");
			return 1;
		}
		if (total_ct > 1 || curr_ct != 1) {
			fprintf ( stderr, "The SDP response for HIDP info contains too many handles (total %d, curr %d)!\n", total_ct, curr_ct);
			return 1;
		}
		sdp_handle = (unsigned long) data [4] << 24 |
			(unsigned long) data [5] << 16 |
			(unsigned long) data [6] <<  8 |
			(unsigned long) data [7] <<  0;
		//fprintf ( stdout, "get HID info.." );
		sdp_getattr (NULL);
		return	-2;
	}
	if (id != 0x05 /* SDP_ServiceAttributeResponse */) {
		fprintf ( stderr, "The SDP query for PNP info gave an unexpected SDP response id=0x%x!\n", id);
		return 1;
	}
	attr_bytes = data [0] << 8 | data [1];
	data += 2;
	sdp_attr = realloc (sdp_attr, sdp_attr_bytes + attr_bytes);
	memcpy (sdp_attr + sdp_attr_bytes, data, attr_bytes);
	sdp_attr_bytes += attr_bytes;

	if (data [attr_bytes]) {
		/* There's a continuation, go get it */
		//fprintf ( stdout, "." ); fflush ( stdout );
		sdp_getattr (&data [attr_bytes]);
		//fprintf ( stdout, "." ); fflush ( stdout );
		i = recv (sdp_sock, data, 1024, 0); // 1024 == hardcoded length of "data" buffer
		return sdp_resp ( data, i );
	}
	i = 0;
	s = sdp_scan (sdp_attr, &i);
	free (sdp_attr);
	sdp_attr = NULL;
	
	if (i != sdp_attr_bytes) {
		fprintf ( stderr, "The PNP-Info (SDP attribute data element sequence) " \
				"has bad length (%d != %d)!\n", i, sdp_attr_bytes);
		sdp_free (s);
		return 1;
	}

	if (!s) {
		fprintf ( stderr, "The SDP query for HIDP info returned no SDP attributes!\n");
		return 1;
	}
	if (s->type != sdp_data_seq) {
		fprintf ( stderr, "The SDP attributes returned (answering the query for HIDP info) are not "
				"a data element sequence!\n");
		sdp_free (s);
		return 1;
	}
	if (s->u.seq.items & 1) {
		fprintf ( stderr, "The SDP attribute data element sequence (answering the query for PNP info) " \
				"has an odd number of elements!\n");
	}
        for (i = 0; i < s->u.seq.items - 1; i += 2) {
		if (s->u.seq.item [i]->type == sdp_data_uint)
			hid_attr (s->u.seq.item [i]->u.uint,
					s->u.seq.item [i+1]);
		else
			fprintf ( stderr, "The SDP attribute data element " \
					"sequence (answering the query for HIDP info) has non-integer value in " \
					"item[%d]!\n", i);
	}
	sdp_free (s);
	return	0;
}

int sdp_probe_pnp ( BD_ADDR * bda ) /* Search for PNP info */
{
	int	n = 0, plen;
	unsigned char	buf[32];
	unsigned char	lbuf[1024];
	if ( sdp_sock >= 0 ) {
		close ( sdp_sock );
		if ( sdp_attr ) free ( sdp_attr );
		sdp_attr = NULL;
	}
	if ( ( sdp_sock = bthid_channel_connect ( bda, PSM_SDP ) ) < 0 ) {
		fprintf ( stderr, "Failed to connect SDP channel!\n" );
		return	1;
	}
	sdp_tid = 0;	/* First query at all, so our TransactionID is 0 (increasing) */
	sdp_attr_bytes = n = 0;
	buf [n++] = 0x02;			/* SDP_ServiceSearchRequest */
	buf [n++] = sdp_tid >> 8;
	buf [n++] = sdp_tid;
	plen = n, n += 2;			/* 2 bytes for param len */
	buf [n++] = sdp_data_seq << 3 | 0x05;	/* Data element sequence */
	buf [n++] = 3;				/* 3 bytes in data element sequence */
	buf [n++] = (sdp_data_uuid << 3 | 0x01) & 0xff;	/* 2-byte UUID */
	buf [n++] = SDP_UUID_PNP_INFO >> 8;		/* PNPINFO service UUID */
	buf [n++] = SDP_UUID_PNP_INFO & 0xff;
	buf [n++] = 1 >> 8;			/* One handle max */
	buf [n++] = 1;
	buf [n++] = 0;				/* No continuation code */
	buf [plen] = ( n - (plen + 2) ) >> 8;
	buf [plen+1] = n - (plen + 2);
	if ( ( n = send (sdp_sock, buf, n, 0) ) < 0 ) {
		close ( sdp_sock );
		sdp_sock = -1;
		fprintf ( stderr, "Sending SDP request failed!\n" );
		return	1;
	}
	if ( 1 > ( n = recv (sdp_sock, lbuf, sizeof lbuf, 0) ) ) {
		fprintf ( stderr, "Failed to receive answer for ServiceSearchRequest!\n" );
		return	1;
	}
	n = sdp_resp_pnp ( lbuf, n );
	if ( n == 2 ) {
		// No PNP record retrieved, fake it
		// This is necessary e.g. for the Logitech Presenter - it does not offer
		// a valid PNP record. This info would be nice-to-have in /proc/bus/input/...
		// but if it cannot be retrieved, best-effort is to set it to 0x0000
		iocstruct.conn_info.vendor = 0;
		iocstruct.conn_info.product = 0;
		iocstruct.conn_info.version = 0;
		return	0;
	}
	if ( n != -2 ) {
		fprintf ( stderr, "Failed to send SDP attrib request!\n" );
		return	1;
	}
	if ( 1 > ( n = recv (sdp_sock, lbuf, sizeof lbuf, 0) ) ) {
		fprintf ( stderr, "Failed to receive answer for SDP AttributeRequest!\n" );
		return	1;
	}
	n = sdp_resp_pnp ( lbuf, n );
	return	0;
}
int sdp_probe ( BD_ADDR * bda )
{
	int	n = 0, plen;
	unsigned char	buf[32];
	unsigned char	lbuf[1024];
	if ( sdp_sock >= 0 ) {
		close ( sdp_sock );
		if ( sdp_attr ) free ( sdp_attr );
		sdp_attr = NULL;
	}
	if ( ( sdp_sock = bthid_channel_connect ( bda, PSM_SDP ) ) < 0 ) {
		fprintf ( stderr, "Failed to open a SDP connection to the HID device (bthid_channel_connect)!\n" );
		return	1;
	}
	sdp_attr_bytes = n = 0;
	buf [n++] = 0x02;			/* SDP_ServiceSearchRequest */
	buf [n++] = sdp_tid >> 8;
	buf [n++] = sdp_tid;
	plen = n, n += 2;			/* 2 bytes for param len */
	buf [n++] = sdp_data_seq << 3 | 0x05;	/* Data element sequence */
	buf [n++] = 3;				/* 3 bytes in data element sequence */
	buf [n++] = (sdp_data_uuid << 3 | 0x01) & 0xff;	/* 2-byte UUID */
	buf [n++] = 0x1124 >> 8;		/* HIDP service UUID - seems not to have a #define in AFFIX, so numeric constant used here */
	buf [n++] = 0x1124 & 0xff;		/* seems to be interchangeably usable to UUID PSM_HID_CONTROL (0x0011) */
	buf [n++] = 1 >> 8;			/* One handle max */
	buf [n++] = 1;
	buf [n++] = 0;				/* No continuation code */
	buf [plen] = ( n - (plen + 2) ) >> 8;
	buf [plen+1] = n - (plen + 2);
	if ( ( n = send (sdp_sock, buf, n, 0) ) < 0 ) {
		close ( sdp_sock );
		sdp_sock = -1;
		fprintf ( stderr, "Sending SDP request for HIDP info to the HID device failed!\n" );
		return	1;
	}
	// Now get the ServiceSearchRequest answer...
	if ( 1 > ( n = recv (sdp_sock, lbuf, sizeof lbuf, 0) ) ) {
		fprintf ( stderr, "ServiceSearchRequest for HIDP info of HID device failed!\n" );
		return	1;
	}
	n = sdp_resp ( lbuf, n );
	if ( n != -2 ) {
		fprintf ( stderr, "Failed to send request for HIDP info!\n" );
		return	1;
	}
	if ( 1 > ( n = recv (sdp_sock, lbuf, sizeof lbuf, 0) ) ) {
		fprintf ( stderr, "Failed to receive answer for SDP AttributeRequest from HID device!\n" );
		return	1;
	}
	n = sdp_resp ( lbuf, n );
	return	0;
}

int do_inquiry(int length)
{
	int		fd;
	int		err;
	__u8		num;
	if (!length) length = 8;
	fd = hci_open(btdev);
	if (fd < 0) {
		fprintf(stderr, "Unable to open device %s for inquiry: %s!\n", btdev, strerror(errno));
		return 1;
	}
	fprintf(stdout, "Performing device inquiry for %d seconds...", length); fflush ( stdout );
	err = HCI_Inquiry(fd, length, 20, devs, &num);