/*

 *

 *  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

 *

 */

#ifdef HAVE_CONFIG_H

#include <config.h>

#endif

#define _GNU_SOURCE

#include <errno.h>

#include <ctype.h>

#include <stdlib.h>

#include <stdint.h>

#include <stdbool.h>

#include <glib.h>

#include "lib/bluetooth.h"

#include "lib/hci.h"

#include "lib/sdp.h"

#include "src/shared/util.h"

#include "uuid-helper.h"

#include "eir.h"

#define EIR_OOB_MIN (2 + 6)

static void sd_free(void *data)

{

	struct eir_sd *sd = data;

	free(sd->uuid);

	g_free(sd);

}

static void data_free(void *data)

{

	struct eir_ad *ad = data;

	g_free(ad->data);

	g_free(ad);

}

void eir_data_free(struct eir_data *eir)

{

	g_slist_free_full(eir->services, free);

	eir->services = NULL;

	g_free(eir->name);

	eir->name = NULL;

	g_free(eir->hash);

	eir->hash = NULL;

	g_free(eir->randomizer);

	eir->randomizer = NULL;

	g_slist_free_full(eir->msd_list, g_free);

	eir->msd_list = NULL;

	g_slist_free_full(eir->sd_list, sd_free);

	eir->sd_list = NULL;

	g_slist_free_full(eir->data_list, data_free);

	eir->data_list = NULL;

}

static void eir_parse_uuid16(struct eir_data *eir, const void *data,

								uint8_t len)

{

	const uint16_t *uuid16 = data;

	uuid_t service;

	char *uuid_str;

	unsigned int i;

	service.type = SDP_UUID16;

	for (i = 0; i < len / 2; i++, uuid16++) {

		service.value.uuid16 = get_le16(uuid16);

		uuid_str = bt_uuid2string(&service);

		if (!uuid_str)

			continue;

		eir->services = g_slist_append(eir->services, uuid_str);

	}

}

static void eir_parse_uuid32(struct eir_data *eir, const void *data,

								uint8_t len)

{

	const uint32_t *uuid32 = data;

	uuid_t service;

	char *uuid_str;

	unsigned int i;

	service.type = SDP_UUID32;

	for (i = 0; i < len / 4; i++, uuid32++) {

		service.value.uuid32 = get_le32(uuid32);

		uuid_str = bt_uuid2string(&service);

		if (!uuid_str)

			continue;

		eir->services = g_slist_append(eir->services, uuid_str);

	}

}

static void eir_parse_uuid128(struct eir_data *eir, const uint8_t *data,

								uint8_t len)

{

	const uint8_t *uuid_ptr = data;

	uuid_t service;

	char *uuid_str;

	unsigned int i;

	int k;

	service.type = SDP_UUID128;

	for (i = 0; i < len / 16; i++) {

		for (k = 0; k < 16; k++)

			service.value.uuid128.data[k] = uuid_ptr[16 - k - 1];

		uuid_str = bt_uuid2string(&service);

		if (!uuid_str)

			continue;

		eir->services = g_slist_append(eir->services, uuid_str);

		uuid_ptr += 16;

	}

}

static char *name2utf8(const uint8_t *name, uint8_t len)

{

	char utf8_name[HCI_MAX_NAME_LENGTH + 2];

	int i;

	if (g_utf8_validate((const char *) name, len, NULL))

		return g_strndup((char *) name, len);

	memset(utf8_name, 0, sizeof(utf8_name));

	strncpy(utf8_name, (char *) name, len);

	/* Assume ASCII, and replace all non-ASCII with spaces */

	for (i = 0; utf8_name[i] != '\0'; i++) {

		if (!isascii(utf8_name[i]))

			utf8_name[i] = ' ';

	}

	/* Remove leading and trailing whitespace characters */

	g_strstrip(utf8_name);

	return g_strdup(utf8_name);

}

static void eir_parse_msd(struct eir_data *eir, const uint8_t *data,

								uint8_t len)

{

	struct eir_msd *msd;

	if (len < 2 || len > 2 + sizeof(msd->data))

		return;

	msd = g_malloc(sizeof(*msd));

	msd->company = get_le16(data);

	msd->data_len = len - 2;

	memcpy(&msd->data, data + 2, msd->data_len);

	eir->msd_list = g_slist_append(eir->msd_list, msd);

}

static void eir_parse_sd(struct eir_data *eir, uuid_t *service,

					const uint8_t *data, uint8_t len)

{

	struct eir_sd *sd;

	char *uuid;

	uuid = bt_uuid2string(service);

	if (!uuid)

		return;

	sd = g_malloc(sizeof(*sd));

	sd->uuid = uuid;

	sd->data_len = len;

	memcpy(&sd->data, data, sd->data_len);

	eir->sd_list = g_slist_append(eir->sd_list, sd);

}

static void eir_parse_uuid16_data(struct eir_data *eir, const uint8_t *data,

								uint8_t len)

{

	uuid_t service;

	if (len < 2 || len > EIR_SD_MAX_LEN)

		return;

	service.type = SDP_UUID16;

	service.value.uuid16 = get_le16(data);

	eir_parse_sd(eir, &service, data + 2, len - 2);

}

static void eir_parse_uuid32_data(struct eir_data *eir, const uint8_t *data,

								uint8_t len)

{

	uuid_t service;

	if (len < 4 || len > EIR_SD_MAX_LEN)

		return;

	service.type = SDP_UUID32;

	service.value.uuid32 = get_le32(data);

	eir_parse_sd(eir, &service, data + 4, len - 4);

}

static void eir_parse_uuid128_data(struct eir_data *eir, const uint8_t *data,

								uint8_t len)

{

	uuid_t service;

	int k;

	if (len < 16 || len > EIR_SD_MAX_LEN)

		return;

	service.type = SDP_UUID128;

	for (k = 0; k < 16; k++)

		service.value.uuid128.data[k] = data[16 - k - 1];

	eir_parse_sd(eir, &service, data + 16, len - 16);

}

static void eir_parse_data(struct eir_data *eir, uint8_t type,

					const uint8_t *data, uint8_t len)

{

	struct eir_ad *ad;

	ad = g_malloc(sizeof(*ad));

	ad->type = type;

	ad->len = len;

	ad->data = g_malloc(len);

	memcpy(ad->data, data, len);

	eir->data_list = g_slist_append(eir->data_list, ad);

}

void eir_parse(struct eir_data *eir, const uint8_t *eir_data, uint8_t eir_len)

{

	uint16_t len = 0;

	eir->flags = 0;

	eir->tx_power = 127;

	/* No EIR data to parse */

	if (eir_data == NULL)

		return;

	while (len < eir_len - 1) {

		uint8_t field_len = eir_data[0];

		const uint8_t *data;

		uint8_t data_len;

		/* Check for the end of EIR */

		if (field_len == 0)

			break;

		len += field_len + 1;

		/* Do not continue EIR Data parsing if got incorrect length */

		if (len > eir_len)

			break;

		data = &eir_data[2];

		data_len = field_len - 1;

		switch (eir_data[1]) {

		case EIR_UUID16_SOME:

		case EIR_UUID16_ALL:

			eir_parse_uuid16(eir, data, data_len);

			break;

		case EIR_UUID32_SOME:

		case EIR_UUID32_ALL:

			eir_parse_uuid32(eir, data, data_len);

			break;

		case EIR_UUID128_SOME:

		case EIR_UUID128_ALL:

			eir_parse_uuid128(eir, data, data_len);

			break;

		case EIR_FLAGS:

			if (data_len > 0)

				eir->flags = *data;

			break;

		case EIR_NAME_SHORT:

		case EIR_NAME_COMPLETE:

			/* Some vendors put a NUL byte terminator into

			 * the name */

			while (data_len > 0 && data[data_len - 1] == '\0')

				data_len--;

			g_free(eir->name);

			eir->name = name2utf8(data, data_len);

			eir->name_complete = eir_data[1] == EIR_NAME_COMPLETE;

			break;

		case EIR_TX_POWER:

			if (data_len < 1)

				break;

			eir->tx_power = (int8_t) data[0];

			break;

		case EIR_CLASS_OF_DEV:

			if (data_len < 3)

				break;

			eir->class = data[0] | (data[1] << 8) |

							(data[2] << 16);

			break;

		case EIR_GAP_APPEARANCE:

			if (data_len < 2)

				break;

			eir->appearance = get_le16(data);

			break;

		case EIR_SSP_HASH:

			if (data_len < 16)

				break;

			eir->hash = g_memdup(data, 16);

			break;

		case EIR_SSP_RANDOMIZER:

			if (data_len < 16)

				break;

			eir->randomizer = g_memdup(data, 16);

			break;

		case EIR_DEVICE_ID:

			if (data_len < 8)

				break;

			eir->did_source = data[0] | (data[1] << 8);

			eir->did_vendor = data[2] | (data[3] << 8);

			eir->did_product = data[4] | (data[5] << 8);

			eir->did_version = data[6] | (data[7] << 8);

			break;

		case EIR_SVC_DATA16:

			eir_parse_uuid16_data(eir, data, data_len);

			break;

		case EIR_SVC_DATA32:

			eir_parse_uuid32_data(eir, data, data_len);

			break;

		case EIR_SVC_DATA128:

			eir_parse_uuid128_data(eir, data, data_len);

			break;

		case EIR_MANUFACTURER_DATA:

			eir_parse_msd(eir, data, data_len);

			break;

		default:

			eir_parse_data(eir, eir_data[1], data, data_len);

			break;

		}

		eir_data += field_len + 1;

	}

}

int eir_parse_oob(struct eir_data *eir, uint8_t *eir_data, uint16_t eir_len)

{

	if (eir_len < EIR_OOB_MIN)

		return -1;

	if (eir_len != get_le16(eir_data))

		return -1;

	eir_data += sizeof(uint16_t);

	eir_len -= sizeof(uint16_t);

	memcpy(&eir->addr, eir_data, sizeof(bdaddr_t));

	eir_data += sizeof(bdaddr_t);

	eir_len -= sizeof(bdaddr_t);

	/* optional OOB EIR data */

	if (eir_len > 0)

		eir_parse(eir, eir_data, eir_len);

	return 0;

}

#define SIZEOF_UUID128 16

static void eir_generate_uuid128(sdp_list_t *list, uint8_t *ptr,

							uint16_t *eir_len)

{

	int i, k, uuid_count = 0;

	uint16_t len = *eir_len;

	uint8_t *uuid128;

	bool truncated = false;

	/* Store UUIDs in place, skip 2 bytes to write type and length later */

	uuid128 = ptr + 2;

	for (; list; list = list->next) {

		sdp_record_t *rec = list->data;

		uuid_t *uuid = &rec->svclass;

		uint8_t *uuid128_data = uuid->value.uuid128.data;

		if (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;

	}

}

int eir_create_oob(const bdaddr_t *addr, const char *name, uint32_t cod,

			const uint8_t *hash, const uint8_t *randomizer,

			uint16_t did_vendor, uint16_t did_product,

			uint16_t did_version, uint16_t did_source,

			sdp_list_t *uuids, uint8_t *data)

{

	sdp_list_t *l;

	uint8_t *ptr = data;

	uint16_t eir_optional_len = 0;

	uint16_t eir_total_len;

	uint16_t uuid16[HCI_MAX_EIR_LENGTH / 2];

	int i, uuid_count = 0;

	bool truncated = false;

	size_t name_len;

	eir_total_len =  sizeof(uint16_t) + sizeof(bdaddr_t);

	ptr += sizeof(uint16_t);

	memcpy(ptr, addr, sizeof(bdaddr_t));

	ptr += sizeof(bdaddr_t);

	if (cod > 0) {

		uint8_t class[3];

		class[0] = (uint8_t) cod;

		class[1] = (uint8_t) (cod >> 8);

		class[2] = (uint8_t) (cod >> 16);

		*ptr++ = 4;

		*ptr++ = EIR_CLASS_OF_DEV;

		memcpy(ptr, class, sizeof(class));

		ptr += sizeof(class);

		eir_optional_len += sizeof(class) + 2;

	}

	if (hash) {

		*ptr++ = 17;

		*ptr++ = EIR_SSP_HASH;

		memcpy(ptr, hash, 16);

		ptr += 16;

		eir_optional_len += 16 + 2;

	}

	if (randomizer) {

		*ptr++ = 17;

		*ptr++ = EIR_SSP_RANDOMIZER;

		memcpy(ptr, randomizer, 16);

		ptr += 16;

		eir_optional_len += 16 + 2;

	}

	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_optional_len += (name_len + 2);

		ptr += (name_len + 2);

	}

	if (did_vendor != 0x0000) {

		*ptr++ = 9;

		*ptr++ = EIR_DEVICE_ID;

		*ptr++ = (did_source & 0x00ff);

		*ptr++ = (did_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_optional_len += 10;

	}

	/* Group all UUID16 types */

	for (l = uuids; l != NULL; l = l->next) {

		sdp_record_t *rec = l->data;

		uuid_t *uuid = &rec->svclass;

		if (uuid->type != SDP_UUID16)

			continue;

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

			continue;

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

			continue;

		/* Stop if not enough space to put next UUID16 */

		if ((eir_optional_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->value.uuid16)

				break;

		if (i < uuid_count)

			continue;

		uuid16[uuid_count++] = uuid->value.uuid16;

		eir_optional_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_optional_len += 2;

		for (i = 0; i < uuid_count; i++) {

			*ptr++ = (uuid16[i] & 0x00ff);

			*ptr++ = (uuid16[i] & 0xff00) >> 8;

		}

	}

	/* Group all UUID128 types */

	if (eir_optional_len <= HCI_MAX_EIR_LENGTH - 2)

		eir_generate_uuid128(uuids, ptr, &eir_optional_len);

	eir_total_len += eir_optional_len;

	/* store total length */

	put_le16(eir_total_len, data);

	return eir_total_len;

}