src/eir.c - platform/external/bluetooth/bluez - Git at Google

 /*
  *
  *  BlueZ - Bluetooth protocol stack for Linux
  *
  *  Copyright (C) 2011  Nokia Corporation
  *  Copyright (C) 2011  Marcel Holtmann <marcel@holtmann.org>
  *
  *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
  *  (at your option) any later version.
  *
  *  This program is distributed in the hope that it will be useful,
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
  *
  *  You should have received a copy of the GNU General Public License
  *  along with this program; if not, write to the Free Software
  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  *
  */
 #include <errno.h>
 #include <stdlib.h>
 #include <stdint.h>
 #include <glib.h>

 #include <bluetooth/bluetooth.h>
 #include <bluetooth/hci.h>
 #include <bluetooth/sdp.h>

 #include "glib-helper.h"
 #include "eir.h"

 #define EIR_FLAGS                   0x01  /* flags */
 #define EIR_UUID16_SOME             0x02  /* 16-bit UUID, more available */
 #define EIR_UUID16_ALL              0x03  /* 16-bit UUID, all listed */
 #define EIR_UUID32_SOME             0x04  /* 32-bit UUID, more available */
 #define EIR_UUID32_ALL              0x05  /* 32-bit UUID, all listed */
 #define EIR_UUID128_SOME            0x06  /* 128-bit UUID, more available */
 #define EIR_UUID128_ALL             0x07  /* 128-bit UUID, all listed */
 #define EIR_NAME_SHORT              0x08  /* shortened local name */
 #define EIR_NAME_COMPLETE           0x09  /* complete local name */
 #define EIR_TX_POWER                0x0A  /* transmit power level */
 #define EIR_DEVICE_ID               0x10  /* device ID */

 void eir_data_free(struct eir_data *eir)
 {
 	g_slist_foreach(eir->services, (GFunc) g_free, NULL);
 	g_slist_free(eir->services);
 	g_free(eir->name);
 }

 int eir_parse(struct eir_data *eir, uint8_t *eir_data)
 {
 	uint16_t len = 0;
 	size_t total;
 	size_t uuid16_count = 0;
 	size_t uuid32_count = 0;
 	size_t uuid128_count = 0;
 	uint8_t *uuid16 = NULL;
 	uint8_t *uuid32 = NULL;
 	uint8_t *uuid128 = NULL;
 	uuid_t service;
 	char *uuid_str;
 	unsigned int i;

 	eir->flags = -1;

 	/* No EIR data to parse */
 	if (eir_data == NULL)
 		return 0;

 	while (len < HCI_MAX_EIR_LENGTH - 1) {
 		uint8_t field_len = eir_data[0];

 		/* Check for the end of EIR */
 		if (field_len == 0)
 			break;

 		switch (eir_data[1]) {
 		case EIR_UUID16_SOME:
 		case EIR_UUID16_ALL:
 			uuid16_count = field_len / 2;
 			uuid16 = &eir_data[2];
 			break;
 		case EIR_UUID32_SOME:
 		case EIR_UUID32_ALL:
 			uuid32_count = field_len / 4;
 			uuid32 = &eir_data[2];
 			break;
 		case EIR_UUID128_SOME:
 		case EIR_UUID128_ALL:
 			uuid128_count = field_len / 16;
 			uuid128 = &eir_data[2];
 			break;
 		case EIR_FLAGS:
 			eir->flags = eir_data[2];
 			break;
 		case EIR_NAME_SHORT:
 		case EIR_NAME_COMPLETE:
 			if (g_utf8_validate((char *) &eir_data[2],
 							field_len - 1, NULL))
 				eir->name = g_strndup((char *) &eir_data[2],
 								field_len - 1);
 			else
 				eir->name = g_strdup("");
 			eir->name_complete = eir_data[1] == EIR_NAME_COMPLETE;
 			break;
 		}

 		len += field_len + 1;
 		eir_data += field_len + 1;
 	}

 	/* Bail out if got incorrect length */
 	if (len > HCI_MAX_EIR_LENGTH)
 		return -EINVAL;

 	total = uuid16_count + uuid32_count + uuid128_count;

 	/* No UUIDs were parsed, so skip code below */
 	if (!total)
 		return 0;

 	/* Generate uuids in SDP format (EIR data is Little Endian) */
 	service.type = SDP_UUID16;
 	for (i = 0; i < uuid16_count; i++) {
 		uint16_t val16 = uuid16[1];

 		val16 = (val16 << 8) + uuid16[0];
 		service.value.uuid16 = val16;
 		uuid_str = bt_uuid2string(&service);
 		eir->services = g_slist_append(eir->services, uuid_str);
 		uuid16 += 2;
 	}

 	service.type = SDP_UUID32;
 	for (i = uuid16_count; i < uuid32_count + uuid16_count; i++) {
 		uint32_t val32 = uuid32[3];
 		int k;

 		for (k = 2; k >= 0; k--)
 			val32 = (val32 << 8) + uuid32[k];

 		service.value.uuid32 = val32;
 		uuid_str = bt_uuid2string(&service);
 		eir->services = g_slist_append(eir->services, uuid_str);
 		uuid32 += 4;
 	}

 	service.type = SDP_UUID128;
 	for (i = uuid32_count + uuid16_count; i < total; i++) {
 		int k;

 		for (k = 0; k < 16; k++)
 			service.value.uuid128.data[k] = uuid128[16 - k - 1];

 		uuid_str = bt_uuid2string(&service);
 		eir->services = g_slist_append(eir->services, uuid_str);
 		uuid128 += 16;
 	}

 	return 0;
 }

 #define SIZEOF_UUID128 16

 static void eir_generate_uuid128(GSList *list, uint8_t *ptr, uint16_t *eir_len)
 {
 	int i, k, uuid_count = 0;
 	uint16_t len = *eir_len;
 	uint8_t *uuid128;
 	gboolean truncated = FALSE;

 	/* Store UUIDs in place, skip 2 bytes to write type and length later */
 	uuid128 = ptr + 2;

 	for (; list; list = list->next) {
 		struct uuid_info *uuid = list->data;
 		uint8_t *uuid128_data = uuid->uuid.value.uuid128.data;

 		if (uuid->uuid.type != SDP_UUID128)
 			continue;

 		/* Stop if not enough space to put next UUID128 */
 		if ((len + 2 + SIZEOF_UUID128) > HCI_MAX_EIR_LENGTH) {
 			truncated = TRUE;
 			break;
 		}

 		/* Check for duplicates, EIR data is Little Endian */
 		for (i = 0; i < uuid_count; i++) {
 			for (k = 0; k < SIZEOF_UUID128; k++) {
 				if (uuid128[i * SIZEOF_UUID128 + k] !=
 					uuid128_data[SIZEOF_UUID128 - 1 - k])
 					break;
 			}
 			if (k == SIZEOF_UUID128)
 				break;
 		}

 		if (i < uuid_count)
 			continue;

 		/* EIR data is Little Endian */
 		for (k = 0; k < SIZEOF_UUID128; k++)
 			uuid128[uuid_count * SIZEOF_UUID128 + k] =
 				uuid128_data[SIZEOF_UUID128 - 1 - k];

 		len += SIZEOF_UUID128;
 		uuid_count++;
 	}

 	if (uuid_count > 0 || truncated) {
 		/* EIR Data length */
 		ptr[0] = (uuid_count * SIZEOF_UUID128) + 1;
 		/* EIR Data type */
 		ptr[1] = truncated ? EIR_UUID128_SOME : EIR_UUID128_ALL;
 		len += 2;
 		*eir_len = len;
 	}
 }

 void eir_create(const char *name, int8_t tx_power, uint16_t did_vendor,
 			uint16_t did_product, uint16_t did_version,
 			GSList *uuids, uint8_t *data)
 {
 	GSList *l;
 	uint8_t *ptr = data;
 	uint16_t eir_len = 0;
 	uint16_t uuid16[HCI_MAX_EIR_LENGTH / 2];
 	int i, uuid_count = 0;
 	gboolean truncated = FALSE;
 	size_t name_len;

 	name_len = strlen(name);

 	if (name_len > 0) {
 		/* EIR Data type */
 		if (name_len > 48) {
 			name_len = 48;
 			ptr[1] = EIR_NAME_SHORT;
 		} else
 			ptr[1] = EIR_NAME_COMPLETE;

 		/* EIR Data length */
 		ptr[0] = name_len + 1;

 		memcpy(ptr + 2, name, name_len);

 		eir_len += (name_len + 2);
 		ptr += (name_len + 2);
 	}

 	if (tx_power != 0) {
 		*ptr++ = 2;
 		*ptr++ = EIR_TX_POWER;
 		*ptr++ = (uint8_t) tx_power;
 		eir_len += 3;
 	}

 	if (did_vendor != 0x0000) {
 		uint16_t source = 0x0002;
 		*ptr++ = 9;
 		*ptr++ = EIR_DEVICE_ID;
 		*ptr++ = (source & 0x00ff);
 		*ptr++ = (source & 0xff00) >> 8;
 		*ptr++ = (did_vendor & 0x00ff);
 		*ptr++ = (did_vendor & 0xff00) >> 8;
 		*ptr++ = (did_product & 0x00ff);
 		*ptr++ = (did_product & 0xff00) >> 8;
 		*ptr++ = (did_version & 0x00ff);
 		*ptr++ = (did_version & 0xff00) >> 8;
 		eir_len += 10;
 	}

 	/* Group all UUID16 types */
 	for (l = uuids; l != NULL; l = g_slist_next(l)) {
 		struct uuid_info *uuid = l->data;

 		if (uuid->uuid.type != SDP_UUID16)
 			continue;

 		if (uuid->uuid.value.uuid16 < 0x1100)
 			continue;

 		if (uuid->uuid.value.uuid16 == PNP_INFO_SVCLASS_ID)
 			continue;

 		/* Stop if not enough space to put next UUID16 */
 		if ((eir_len + 2 + sizeof(uint16_t)) > HCI_MAX_EIR_LENGTH) {
 			truncated = TRUE;
 			break;
 		}

 		/* Check for duplicates */
 		for (i = 0; i < uuid_count; i++)
 			if (uuid16[i] == uuid->uuid.value.uuid16)
 				break;

 		if (i < uuid_count)
 			continue;

 		uuid16[uuid_count++] = uuid->uuid.value.uuid16;
 		eir_len += sizeof(uint16_t);
 	}

 	if (uuid_count > 0) {
 		/* EIR Data length */
 		ptr[0] = (uuid_count * sizeof(uint16_t)) + 1;
 		/* EIR Data type */
 		ptr[1] = truncated ? EIR_UUID16_SOME : EIR_UUID16_ALL;

 		ptr += 2;
 		eir_len += 2;

 		for (i = 0; i < uuid_count; i++) {
 			*ptr++ = (uuid16[i] & 0x00ff);
 			*ptr++ = (uuid16[i] & 0xff00) >> 8;
 		}
 	}

 	/* Group all UUID128 types */
 	if (eir_len <= HCI_MAX_EIR_LENGTH - 2)
 		eir_generate_uuid128(uuids, ptr, &eir_len);
 }
	/*
	*
	* BlueZ - Bluetooth protocol stack for Linux
	*
	* Copyright (C) 2011 Nokia Corporation
	* Copyright (C) 2011 Marcel Holtmann <marcel@holtmann.org>
	*
	*
	* This program is free software; you can redistribute it and/or modify
	* it under the terms of the GNU General Public License as published by
	* the Free Software Foundation; either version 2 of the License, or
	* (at your option) any later version.
	*
	* This program is distributed in the hope that it will be useful,
	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	* GNU General Public License for more details.
	*
	* You should have received a copy of the GNU General Public License
	* along with this program; if not, write to the Free Software
	* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
	*
	*/
	#include <errno.h>
	#include <stdlib.h>
	#include <stdint.h>
	#include <glib.h>

	#include <bluetooth/bluetooth.h>
	#include <bluetooth/hci.h>
	#include <bluetooth/sdp.h>

	#include "glib-helper.h"
	#include "eir.h"

	#define EIR_FLAGS 0x01 /* flags */
	#define EIR_UUID16_SOME 0x02 /* 16-bit UUID, more available */
	#define EIR_UUID16_ALL 0x03 /* 16-bit UUID, all listed */
	#define EIR_UUID32_SOME 0x04 /* 32-bit UUID, more available */
	#define EIR_UUID32_ALL 0x05 /* 32-bit UUID, all listed */
	#define EIR_UUID128_SOME 0x06 /* 128-bit UUID, more available */
	#define EIR_UUID128_ALL 0x07 /* 128-bit UUID, all listed */
	#define EIR_NAME_SHORT 0x08 /* shortened local name */
	#define EIR_NAME_COMPLETE 0x09 /* complete local name */
	#define EIR_TX_POWER 0x0A /* transmit power level */
	#define EIR_DEVICE_ID 0x10 /* device ID */

	void eir_data_free(struct eir_data *eir)
	{
	g_slist_foreach(eir->services, (GFunc) g_free, NULL);
	g_slist_free(eir->services);
	g_free(eir->name);
	}

	int eir_parse(struct eir_data eir, uint8_t eir_data)
	{
	uint16_t len = 0;
	size_t total;
	size_t uuid16_count = 0;
	size_t uuid32_count = 0;
	size_t uuid128_count = 0;
	uint8_t *uuid16 = NULL;
	uint8_t *uuid32 = NULL;
	uint8_t *uuid128 = NULL;
	uuid_t service;
	char *uuid_str;
	unsigned int i;

	eir->flags = -1;

	/* No EIR data to parse */
	if (eir_data == NULL)
	return 0;

	while (len < HCI_MAX_EIR_LENGTH - 1) {
	uint8_t field_len = eir_data[0];

	/* Check for the end of EIR */
	if (field_len == 0)
	break;

	switch (eir_data[1]) {
	case EIR_UUID16_SOME:
	case EIR_UUID16_ALL:
	uuid16_count = field_len / 2;
	uuid16 = &eir_data[2];
	break;
	case EIR_UUID32_SOME:
	case EIR_UUID32_ALL:
	uuid32_count = field_len / 4;
	uuid32 = &eir_data[2];
	break;
	case EIR_UUID128_SOME:
	case EIR_UUID128_ALL:
	uuid128_count = field_len / 16;
	uuid128 = &eir_data[2];
	break;
	case EIR_FLAGS:
	eir->flags = eir_data[2];
	break;
	case EIR_NAME_SHORT:
	case EIR_NAME_COMPLETE:
	if (g_utf8_validate((char *) &eir_data[2],
	field_len - 1, NULL))
	eir->name = g_strndup((char *) &eir_data[2],
	field_len - 1);
	else
	eir->name = g_strdup("");
	eir->name_complete = eir_data[1] == EIR_NAME_COMPLETE;
	break;
	}

	len += field_len + 1;
	eir_data += field_len + 1;
	}

	/* Bail out if got incorrect length */
	if (len > HCI_MAX_EIR_LENGTH)
	return -EINVAL;

	total = uuid16_count + uuid32_count + uuid128_count;

	/* No UUIDs were parsed, so skip code below */
	if (!total)
	return 0;

	/* Generate uuids in SDP format (EIR data is Little Endian) */
	service.type = SDP_UUID16;
	for (i = 0; i < uuid16_count; i++) {
	uint16_t val16 = uuid16[1];

	val16 = (val16 << 8) + uuid16[0];
	service.value.uuid16 = val16;
	uuid_str = bt_uuid2string(&service);
	eir->services = g_slist_append(eir->services, uuid_str);
	uuid16 += 2;
	}

	service.type = SDP_UUID32;
	for (i = uuid16_count; i < uuid32_count + uuid16_count; i++) {
	uint32_t val32 = uuid32[3];
	int k;

	for (k = 2; k >= 0; k--)
	val32 = (val32 << 8) + uuid32[k];

	service.value.uuid32 = val32;
	uuid_str = bt_uuid2string(&service);
	eir->services = g_slist_append(eir->services, uuid_str);
	uuid32 += 4;
	}

	service.type = SDP_UUID128;
	for (i = uuid32_count + uuid16_count; i < total; i++) {
	int k;

	for (k = 0; k < 16; k++)
	service.value.uuid128.data[k] = uuid128[16 - k - 1];

	uuid_str = bt_uuid2string(&service);
	eir->services = g_slist_append(eir->services, uuid_str);
	uuid128 += 16;
	}

	return 0;
	}

	#define SIZEOF_UUID128 16

	static void eir_generate_uuid128(GSList list, uint8_t ptr, uint16_t *eir_len)
	{
	int i, k, uuid_count = 0;
	uint16_t len = *eir_len;
	uint8_t *uuid128;
	gboolean truncated = FALSE;

	/* Store UUIDs in place, skip 2 bytes to write type and length later */
	uuid128 = ptr + 2;

	for (; list; list = list->next) {
	struct uuid_info *uuid = list->data;
	uint8_t *uuid128_data = uuid->uuid.value.uuid128.data;

	if (uuid->uuid.type != SDP_UUID128)
	continue;

	/* Stop if not enough space to put next UUID128 */
	if ((len + 2 + SIZEOF_UUID128) > HCI_MAX_EIR_LENGTH) {
	truncated = TRUE;
	break;
	}

	/* Check for duplicates, EIR data is Little Endian */
	for (i = 0; i < uuid_count; i++) {
	for (k = 0; k < SIZEOF_UUID128; k++) {
	if (uuid128[i * SIZEOF_UUID128 + k] !=
	uuid128_data[SIZEOF_UUID128 - 1 - k])
	break;
	}
	if (k == SIZEOF_UUID128)
	break;
	}

	if (i < uuid_count)
	continue;

	/* EIR data is Little Endian */
	for (k = 0; k < SIZEOF_UUID128; k++)
	uuid128[uuid_count * SIZEOF_UUID128 + k] =
	uuid128_data[SIZEOF_UUID128 - 1 - k];

	len += SIZEOF_UUID128;
	uuid_count++;
	}

	if (uuid_count > 0 \|\| truncated) {
	/* EIR Data length */
	ptr[0] = (uuid_count * SIZEOF_UUID128) + 1;
	/* EIR Data type */
	ptr[1] = truncated ? EIR_UUID128_SOME : EIR_UUID128_ALL;
	len += 2;
	*eir_len = len;
	}
	}

	void eir_create(const char *name, int8_t tx_power, uint16_t did_vendor,
	uint16_t did_product, uint16_t did_version,
	GSList uuids, uint8_t data)
	{
	GSList *l;
	uint8_t *ptr = data;
	uint16_t eir_len = 0;
	uint16_t uuid16[HCI_MAX_EIR_LENGTH / 2];
	int i, uuid_count = 0;
	gboolean truncated = FALSE;
	size_t name_len;

	name_len = strlen(name);

	if (name_len > 0) {
	/* EIR Data type */
	if (name_len > 48) {
	name_len = 48;
	ptr[1] = EIR_NAME_SHORT;
	} else
	ptr[1] = EIR_NAME_COMPLETE;

	/* EIR Data length */
	ptr[0] = name_len + 1;

	memcpy(ptr + 2, name, name_len);

	eir_len += (name_len + 2);
	ptr += (name_len + 2);
	}

	if (tx_power != 0) {
	*ptr++ = 2;
	*ptr++ = EIR_TX_POWER;
	*ptr++ = (uint8_t) tx_power;
	eir_len += 3;
	}

	if (did_vendor != 0x0000) {
	uint16_t source = 0x0002;
	*ptr++ = 9;
	*ptr++ = EIR_DEVICE_ID;
	*ptr++ = (source & 0x00ff);
	*ptr++ = (source & 0xff00) >> 8;
	*ptr++ = (did_vendor & 0x00ff);
	*ptr++ = (did_vendor & 0xff00) >> 8;
	*ptr++ = (did_product & 0x00ff);
	*ptr++ = (did_product & 0xff00) >> 8;
	*ptr++ = (did_version & 0x00ff);
	*ptr++ = (did_version & 0xff00) >> 8;
	eir_len += 10;
	}

	/* Group all UUID16 types */
	for (l = uuids; l != NULL; l = g_slist_next(l)) {
	struct uuid_info *uuid = l->data;

	if (uuid->uuid.type != SDP_UUID16)
	continue;

	if (uuid->uuid.value.uuid16 < 0x1100)
	continue;

	if (uuid->uuid.value.uuid16 == PNP_INFO_SVCLASS_ID)
	continue;

	/* Stop if not enough space to put next UUID16 */
	if ((eir_len + 2 + sizeof(uint16_t)) > HCI_MAX_EIR_LENGTH) {
	truncated = TRUE;
	break;
	}

	/* Check for duplicates */
	for (i = 0; i < uuid_count; i++)
	if (uuid16[i] == uuid->uuid.value.uuid16)
	break;

	if (i < uuid_count)
	continue;

	uuid16[uuid_count++] = uuid->uuid.value.uuid16;
	eir_len += sizeof(uint16_t);
	}

	if (uuid_count > 0) {
	/* EIR Data length */
	ptr[0] = (uuid_count * sizeof(uint16_t)) + 1;
	/* EIR Data type */
	ptr[1] = truncated ? EIR_UUID16_SOME : EIR_UUID16_ALL;

	ptr += 2;
	eir_len += 2;

	for (i = 0; i < uuid_count; i++) {
	*ptr++ = (uuid16[i] & 0x00ff);
	*ptr++ = (uuid16[i] & 0xff00) >> 8;
	}
	}

	/* Group all UUID128 types */
	if (eir_len <= HCI_MAX_EIR_LENGTH - 2)
	eir_generate_uuid128(uuids, ptr, &eir_len);
	}