/*

 *

 *  BlueZ - Bluetooth protocol stack for Linux

 *

 *  Copyright (C) 2011-2014  Intel Corporation

 *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>

 *

 *

 *  This library is free software; you can redistribute it and/or

 *  modify it under the terms of the GNU Lesser General Public

 *  License as published by the Free Software Foundation; either

 *  version 2.1 of the License, or (at your option) any later version.

 *

 *  This library 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

 *  Lesser General Public License for more details.

 *

 *  You should have received a copy of the GNU Lesser General Public

 *  License along with this library; 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 <stdio.h>

#include <stdlib.h>

#include <string.h>

#include <ctype.h>

#include <inttypes.h>

#include "lib/bluetooth.h"

#include "lib/uuid.h"

#include "src/shared/util.h"

#include "bt.h"

#include "packet.h"

#include "display.h"

#include "l2cap.h"

#include "keys.h"

#include "sdp.h"

#include "bnep.h"

#define GET_PKT_TYPE(type) (type & 0x7f)

#define GET_EXTENSION(type) (type & 0x80)

/* BNEP Extension Type */

#define BNEP_EXTENSION_CONTROL		0x00

#define BNEP_CONTROL			0x01

uint16_t proto = 0x0000;

struct bnep_frame {

	uint8_t type;

	int extension;

	struct l2cap_frame l2cap_frame;

};

static bool get_macaddr(struct bnep_frame *bnep_frame, char *str)

{

	uint8_t addr[6];

	struct l2cap_frame *frame = &bnep_frame->l2cap_frame;

	int i;

	for (i = 0; i < 6; i++)

		if (!l2cap_frame_get_u8(frame, &addr[i]))

			return false;

	sprintf(str, "%02x:%02x:%02x:%02x:%02x:%02x",

		addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);

	return true;

}

static bool bnep_general(struct bnep_frame *bnep_frame,

					uint8_t indent,	int hdr_len)

{

	struct l2cap_frame *frame;

	char src_addr[20], dest_addr[20];

	if (!get_macaddr(bnep_frame, dest_addr))

		return false;

	if (!get_macaddr(bnep_frame, src_addr))

		return false;

	frame = &bnep_frame->l2cap_frame;

	if (!l2cap_frame_get_be16(frame, &proto))

		return false;

	print_field("%*cdst %s src %s [proto 0x%04x] ", indent,

					' ', dest_addr, src_addr, proto);

	return true;

}

static bool cmd_nt_understood(struct bnep_frame *bnep_frame, uint8_t indent)

{

	struct l2cap_frame *frame = &bnep_frame->l2cap_frame;

	uint8_t ptype;

	if (!l2cap_frame_get_u8(frame, &ptype))

		return false;

	print_field("%*cType: 0x%02x ", indent, ' ', ptype);

	return true;

}

static bool setup_conn_req(struct bnep_frame *bnep_frame, uint8_t indent)

{

	struct l2cap_frame *frame = &bnep_frame->l2cap_frame;

	uint8_t uuid_size;

	uint32_t src_uuid = 0, dst_uuid = 0;

	if (!l2cap_frame_get_u8(frame, &uuid_size))

		return false;

	print_field("%*cSize: 0x%02x ", indent, ' ', uuid_size);

	switch (uuid_size) {

	case 2:

		if (!l2cap_frame_get_be16(frame, (uint16_t *) &dst_uuid))

			return false;

		if (!l2cap_frame_get_be16(frame, (uint16_t *) &src_uuid))

			return false;

		break;

	case 4:

		if (!l2cap_frame_get_be32(frame, &dst_uuid))

			return false;

		if (!l2cap_frame_get_be32(frame, &src_uuid))

			return false;

		break;

	case 16:

		if (!l2cap_frame_get_be32(frame, &dst_uuid))

			return false;

		l2cap_frame_pull(frame, frame, 12);

		if (!l2cap_frame_get_be32(frame, &src_uuid))

			return false;

		l2cap_frame_pull(frame, frame, 12);

		break;

	default:

		l2cap_frame_pull(frame, frame, (uuid_size * 2));

		return true;

	}

	print_field("%*cDst: 0x%x(%s)", indent, ' ', dst_uuid,

						bt_uuid32_to_str(dst_uuid));

	print_field("%*cSrc: 0x%x(%s)", indent, ' ', src_uuid,

						bt_uuid32_to_str(src_uuid));

	return true;

}

static const char *value2str(uint16_t value)

{

	switch (value) {

	case 0x00:

		return "Operation Successful";

	case 0x01:

		return "Operation Failed - Invalid Dst Srv UUID";

	case 0x02:

		return "Operation Failed - Invalid Src Srv UUID";

	case 0x03:

		return "Operation Failed - Invalid Srv UUID size";

	case 0x04:

		return "Operation Failed - Conn not allowed";

	default:

		return "Unknown";

	}

}

static bool print_rsp_msg(struct bnep_frame *bnep_frame, uint8_t indent)

{

	struct l2cap_frame *frame = &bnep_frame->l2cap_frame;

	uint16_t rsp_msg;

	if (!l2cap_frame_get_be16(frame, &rsp_msg))

		return false;

	print_field("%*cRsp msg: %s(0x%04x) ", indent, ' ',

					value2str(rsp_msg), rsp_msg);

	return true;

}

static bool filter_nettype_req(struct bnep_frame *bnep_frame, uint8_t indent)

{

	struct l2cap_frame *frame = &bnep_frame->l2cap_frame;

	uint16_t length, start_range, end_range;

	int i;

	if (!l2cap_frame_get_be16(frame, &length))

		return false;

	print_field("%*cLength: 0x%04x", indent, ' ', length);

	for (i = 0; i < length / 4; i++) {

		if (!l2cap_frame_get_be16(frame, &start_range))

			return false;

		if (!l2cap_frame_get_be16(frame, &end_range))

			return false;

		print_field("%*c0x%04x - 0x%04x", indent, ' ',

						start_range, end_range);

	}

	return true;

}

static bool filter_multaddr_req(struct bnep_frame *bnep_frame, uint8_t indent)

{

	struct l2cap_frame *frame = &bnep_frame->l2cap_frame;

	uint16_t length;

	char start_addr[20], end_addr[20];

	int i;

	if (!l2cap_frame_get_be16(frame, &length))

		return false;

	print_field("%*cLength: 0x%04x", indent, ' ', length);

	for (i = 0; i < length / 12; i++) {

		if (!get_macaddr(bnep_frame, start_addr))

			return false;

		if (!get_macaddr(bnep_frame, end_addr))

			return false;

		print_field("%*c%s - %s", indent, ' ', start_addr, end_addr);

	}

	return true;

}

struct bnep_control_data {

	uint8_t type;

	const char *str;

	bool (*func) (struct bnep_frame *frame, uint8_t indent);

};

static const struct bnep_control_data bnep_control_table[] = {

	{ 0x00, "Command Not Understood",	cmd_nt_understood	},

	{ 0x01, "Setup Conn Req",		setup_conn_req		},

	{ 0x02, "Setup Conn Rsp",		print_rsp_msg		},

	{ 0x03, "Filter NetType Set",		filter_nettype_req	},

	{ 0x04, "Filter NetType Rsp",		print_rsp_msg		},

	{ 0x05, "Filter MultAddr Set",		filter_multaddr_req	},

	{ 0x06, "Filter MultAddr Rsp",		print_rsp_msg		},

	{ }

};

static bool bnep_control(struct bnep_frame *bnep_frame,

					uint8_t indent,	int hdr_len)

{

	uint8_t ctype;

	struct l2cap_frame *frame = &bnep_frame->l2cap_frame;

	const struct bnep_control_data *bnep_control_data = NULL;

	const char *type_str;

	int i;

	if (!l2cap_frame_get_u8(frame, &ctype))

		return false;

	for (i = 0; bnep_control_table[i].str; i++) {

		if (bnep_control_table[i].type == ctype) {

			bnep_control_data = &bnep_control_table[i];

			break;

		}

	}

	if (bnep_control_data)

		type_str = bnep_control_data->str;

	else

		type_str = "Unknown control type";

	print_field("%*c%s (0x%02x) ", indent, ' ', type_str, ctype);

	if (!bnep_control_data || !bnep_control_data->func) {

		packet_hexdump(frame->data, hdr_len - 1);

		l2cap_frame_pull(frame, frame, hdr_len - 1);

		goto done;

	}

	if (!bnep_control_data->func(bnep_frame, indent+2))

		return false;

done:

	return true;

}

static bool bnep_compressed(struct bnep_frame *bnep_frame,

					uint8_t indent,	int hdr_len)

{

	struct l2cap_frame *frame = &bnep_frame->l2cap_frame;

	if (!l2cap_frame_get_be16(frame, &proto))

		return false;

	print_field("%*c[proto 0x%04x] ", indent, ' ', proto);

	return true;

}

static bool bnep_src_only(struct bnep_frame *bnep_frame,

					uint8_t indent,	int hdr_len)

{

	struct l2cap_frame *frame;

	char src_addr[20];

	if (!get_macaddr(bnep_frame, src_addr))

		return false;

	frame = &bnep_frame->l2cap_frame;

	if (!l2cap_frame_get_be16(frame, &proto))

		return false;

	print_field("%*csrc %s [proto 0x%04x] ", indent,

					' ', src_addr, proto);

	return true;

}

static bool bnep_dst_only(struct bnep_frame *bnep_frame,

					uint8_t indent,	int hdr_len)

{

	struct l2cap_frame *frame;

	char dest_addr[20];

	if (!get_macaddr(bnep_frame, dest_addr))

		return false;

	frame = &bnep_frame->l2cap_frame;

	if (!l2cap_frame_get_be16(frame, &proto))

		return false;

	print_field("%*cdst %s [proto 0x%04x] ", indent,

					' ', dest_addr, proto);

	return true;

}

static bool bnep_eval_extension(struct bnep_frame *bnep_frame, uint8_t indent)

{

	struct l2cap_frame *frame = &bnep_frame->l2cap_frame;

	uint8_t type, length;

	int extension;

	if (!l2cap_frame_get_u8(frame, &type))

		return false;

	if (!l2cap_frame_get_u8(frame, &length))

		return false;

	extension = GET_EXTENSION(type);

	type = GET_PKT_TYPE(type);

	switch (type) {

	case BNEP_EXTENSION_CONTROL:

		print_field("%*cExt Control(0x%02x|%s) len 0x%02x", indent,

				' ', type, extension ? "1" : "0", length);

		if (!bnep_control(bnep_frame, indent+2, length))

			return false;

		break;

	default:

		print_field("%*cExt Unknown(0x%02x|%s) len 0x%02x", indent,

				' ', type, extension ? "1" : "0", length);

		packet_hexdump(frame->data, length);

		l2cap_frame_pull(frame, frame, length);

	}

	if (extension)

		if (!bnep_eval_extension(bnep_frame, indent))

			return false;

	return true;

}

struct bnep_data {

	uint8_t type;

	const char *str;

	bool (*func) (struct bnep_frame *frame, uint8_t indent, int hdr_len);

};

static const struct bnep_data bnep_table[] = {

	{ 0x00, "General Ethernet",		bnep_general	},

	{ 0x01, "Control",			bnep_control	},

	{ 0x02, "Compressed Ethernet",		bnep_compressed	},

	{ 0x03, "Compressed Ethernet SrcOnly",	bnep_src_only	},

	{ 0x04, "Compressed Ethernet DestOnly",	bnep_dst_only	},

	{ }

};

void bnep_packet(const struct l2cap_frame *frame)

{

	uint8_t type, indent = 1;

	struct bnep_frame bnep_frame;

	struct l2cap_frame *l2cap_frame;

	const struct bnep_data *bnep_data = NULL;

	const char *pdu_color, *pdu_str;

	int i;

	l2cap_frame_pull(&bnep_frame.l2cap_frame, frame, 0);

	l2cap_frame = &bnep_frame.l2cap_frame;

	if (!l2cap_frame_get_u8(l2cap_frame, &type))

		goto fail;

	bnep_frame.extension = GET_EXTENSION(type);

	bnep_frame.type = GET_PKT_TYPE(type);

	for (i = 0; bnep_table[i].str; i++) {

		if (bnep_table[i].type == bnep_frame.type) {

			bnep_data = &bnep_table[i];

			break;

		}

	}

	if (bnep_data) {

		if (bnep_data->func) {

			if (frame->in)

				pdu_color = COLOR_MAGENTA;

			else

				pdu_color = COLOR_BLUE;

		} else

			pdu_color = COLOR_WHITE_BG;

		pdu_str = bnep_data->str;

	} else {

		pdu_color = COLOR_WHITE_BG;

		pdu_str = "Unknown packet type";

	}

	print_indent(6, pdu_color, "BNEP: ", pdu_str, COLOR_OFF,

				" (0x%02x|%s)", bnep_frame.type,

				bnep_frame.extension ? "1" : "0");

	if (!bnep_data || !bnep_data->func) {

		packet_hexdump(l2cap_frame->data, l2cap_frame->size);

		return;

	}

	if (!bnep_data->func(&bnep_frame, indent, -1))

		goto fail;

	/* Extension info */

	if (bnep_frame.extension)

		if (!bnep_eval_extension(&bnep_frame, indent+2))

			goto fail;

	/* Control packet => No payload info */

	if (bnep_frame.type == BNEP_CONTROL)

		return;

	/* TODO: Handle BNEP IP packet */

	packet_hexdump(l2cap_frame->data, l2cap_frame->size);

	return;

fail:

	print_text(COLOR_ERROR, "frame too short");

	packet_hexdump(frame->data, frame->size);

}