l2cap.c

/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2011-2012  Intel Corporation
 *  Copyright (C) 2004-2010  Marcel Holtmann <[email protected]>
 *
 *
 *  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

#include <stdio.h>
#include <stdlib.h>
#include <inttypes.h>

#include <bluetooth/bluetooth.h>

#include "bt.h"
#include "packet.h"
#include "display.h"
#include "l2cap.h"
#include "uuid.h"
#include "sdp.h"

#define MAX_CHAN 64

struct chan_data {
	uint16_t index;
	uint16_t handle;
	uint16_t scid;
	uint16_t dcid;
	uint16_t psm;
	uint8_t  ctrlid;
	uint8_t  mode;
};

static struct chan_data chan_list[MAX_CHAN];

static void assign_scid(const struct l2cap_frame *frame,
				uint16_t scid, uint16_t psm, uint8_t ctrlid)
{
	int i, n = -1;

	for (i = 0; i < MAX_CHAN; i++) {
		if (n < 0 && chan_list[i].handle == 0x0000)
			n = i;

		if (chan_list[i].index != frame->index)
			continue;

		if (chan_list[i].handle != frame->handle)
			continue;

		if (frame->in) {
			if (chan_list[i].dcid == scid) {
				n = i;
				break;
			}
		} else {
			if (chan_list[i].scid == scid) {
				n = i;
				break;
			}
		}
	}

	if (n < 0)
		return;

	memset(&chan_list[n], 0, sizeof(chan_list[n]));
	chan_list[n].index = frame->index;
	chan_list[n].handle = frame->handle;

	if (frame->in)
		chan_list[n].dcid = scid;
	else
		chan_list[n].scid = scid;

	chan_list[n].psm = psm;
	chan_list[n].ctrlid = ctrlid;
	chan_list[n].mode = 0;
}

static void release_scid(const struct l2cap_frame *frame, uint16_t scid)
{
	int i;

	for (i = 0; i < MAX_CHAN; i++) {
		if (chan_list[i].index != frame->index)
			continue;

		if (chan_list[i].handle != frame->handle)
			continue;

		if (frame->in) {
			if (chan_list[i].scid == scid) {
				chan_list[i].handle = 0;
				break;
			}
		} else {
			if (chan_list[i].dcid == scid) {
				chan_list[i].handle = 0;
				break;
			}
		}
	}
}

static void assign_dcid(const struct l2cap_frame *frame,
					uint16_t dcid, uint16_t scid)
{
	int i;

	for (i = 0; i < MAX_CHAN; i++) {
		if (chan_list[i].index != frame->index)
			continue;

		if (chan_list[i].handle != frame->handle)
			continue;

		if (frame->in) {
			if (chan_list[i].scid == scid) {
				chan_list[i].dcid = dcid;
				break;
			}
		} else {
			if (chan_list[i].dcid == scid) {
				chan_list[i].scid = dcid;
				break;
			}
		}
	}
}

static void assign_mode(const struct l2cap_frame *frame,
					uint8_t mode, uint16_t dcid)
{
	int i;

	for (i = 0; i < MAX_CHAN; i++) {
		if (chan_list[i].index != frame->index)
			continue;

		if (chan_list[i].handle != frame->handle)
			continue;

		if (frame->in) {
			if (chan_list[i].scid == dcid) {
				chan_list[i].mode = mode;
				break;
			}
		} else {
			if (chan_list[i].dcid == dcid) {
				chan_list[i].mode = mode;
				break;
			}
		}
	}
}

static uint16_t get_psm(const struct l2cap_frame *frame)
{
	int i;

	for (i = 0; i < MAX_CHAN; i++) {
		if (chan_list[i].index != frame->index &&
					chan_list[i].ctrlid == 0)
			continue;

		if (chan_list[i].handle != frame->handle &&
					chan_list[i].ctrlid != frame->index)
			continue;

		if (frame->in) {
			if (chan_list[i].scid == frame->cid)
				return chan_list[i].psm;
		} else {
			if (chan_list[i].dcid == frame->cid)
				return chan_list[i].psm;
		}
	}

	return 0;
}

static uint8_t get_mode(const struct l2cap_frame *frame)
{
	int i;

	for (i = 0; i < MAX_CHAN; i++) {
		if (chan_list[i].index != frame->index &&
					chan_list[i].ctrlid == 0)
			continue;

		if (chan_list[i].handle != frame->handle &&
					chan_list[i].ctrlid != frame->index)
			continue;

		if (frame->in) {
			if (chan_list[i].scid == frame->cid)
				return chan_list[i].mode;
		} else {
			if (chan_list[i].dcid == frame->cid)
				return chan_list[i].mode;
		}
	}

	return 0;
}

static uint16_t get_chan(const struct l2cap_frame *frame)
{
	int i;

	for (i = 0; i < MAX_CHAN; i++) {
		if (chan_list[i].index != frame->index &&
					chan_list[i].ctrlid == 0)
			continue;

		if (chan_list[i].handle != frame->handle &&
					chan_list[i].ctrlid != frame->index)
			continue;

		if (frame->in) {
			if (chan_list[i].scid == frame->cid)
				return i;
		} else {
			if (chan_list[i].dcid == frame->cid)
				return i;
		}
	}

	return 0;
}

#define MAX_INDEX 16

struct index_data {
	void *frag_buf;
	uint16_t frag_pos;
	uint16_t frag_len;
	uint16_t frag_cid;
};

static struct index_data index_list[MAX_INDEX];

static void clear_fragment_buffer(uint16_t index)
{
	free(index_list[index].frag_buf);
	index_list[index].frag_buf = NULL;
	index_list[index].frag_pos = 0;
	index_list[index].frag_len = 0;
}

static void print_psm(uint16_t psm)
{
	print_field("PSM: %d (0x%4.4x)", btohs(psm), btohs(psm));
}

static void print_cid(const char *type, uint16_t cid)
{
	print_field("%s CID: %d", type, btohs(cid));
}

static void print_reject_reason(uint16_t reason)
{
	const char *str;

	switch (btohs(reason)) {
	case 0x0000:
		str = "Command not understood";
		break;
	case 0x0001:
		str = "Signaling MTU exceeded";
		break;
	case 0x0002:
		str = "Invalid CID in request";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Reason: %s (0x%4.4x)", str, btohs(reason));
}

static void print_conn_result(uint16_t result)
{
	const char *str;

	switch (btohs(result)) {
	case 0x0000:
		str = "Connection successful";
		break;
	case 0x0001:
		str = "Connection pending";
		break;
	case 0x0002:
		str = "Connection refused - PSM not supported";
		break;
	case 0x0003:
		str = "Connection refused - security block";
		break;
	case 0x0004:
		str = "Connection refused - no resources available";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Result: %s (0x%4.4x)", str, btohs(result));
}

static void print_conn_status(uint16_t status)
{
        const char *str;

	switch (btohs(status)) {
	case 0x0000:
		str = "No further information available";
		break;
	case 0x0001:
		str = "Authentication pending";
		break;
	case 0x0002:
		str = "Authorization pending";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Status: %s (0x%4.4x)", str, btohs(status));
}

static void print_config_flags(uint16_t flags)
{
	const char *str;

	if (btohs(flags) & 0x0001)
		str = " (continuation)";
	else
		str = "";

	print_field("Flags: 0x%4.4x%s", btohs(flags), str);
}

static void print_config_result(uint16_t result)
{
	const char *str;

	switch (btohs(result)) {
	case 0x0000:
		str = "Success";
		break;
	case 0x0001:
		str = "Failure - unacceptable parameters";
		break;
	case 0x0002:
		str = "Failure - rejected";
		break;
	case 0x0003:
		str = "Failure - unknown options";
		break;
	case 0x0004:
		str = "Pending";
		break;
	case 0x0005:
		str = "Failure - flow spec rejected";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Result: %s (0x%4.4x)", str, btohs(result));
}

static struct {
	uint8_t type;
	uint8_t len;
	const char *str;
} options_table[] = {
	{ 0x01,  2, "Maximum Transmission Unit"		},
	{ 0x02,  2, "Flush Timeout"			},
	{ 0x03, 22, "Quality of Service"		},
	{ 0x04,  9, "Retransmission and Flow Control"	},
	{ 0x05,  1, "Frame Check Sequence"		},
	{ 0x06, 16, "Extended Flow Specification"	},
	{ 0x07,  2, "Extended Window Size"		},
        { }
};

static void print_config_options(const struct l2cap_frame *frame,
				uint8_t offset, uint16_t dcid, bool response)
{
	const uint8_t *data = frame->data + offset;
	uint16_t size = frame->size - offset;
	uint16_t consumed = 0;

	while (consumed < size - 2) {
		const char *str = "Unknown";
		uint8_t type = data[consumed];
		uint8_t len = data[consumed + 1];
		uint8_t expect_len = 0;
		int i;

		for (i = 0; options_table[i].str; i++) {
			if (options_table[i].type == type) {
				str = options_table[i].str;
				expect_len = options_table[i].len;
				break;
			}
		}

		print_field("Option: %s (0x%2.2x)", str, type);

		if (expect_len == 0) {
			consumed += 2;
			break;
		}

		if (len != expect_len) {
			print_text(COLOR_ERROR, "wrong option size (%d != %d)",
							len, expect_len);
			consumed += 2;
			break;
		}

		switch (type) {
		case 0x01:
			print_field("  MTU: %d",
					bt_get_le16(data + consumed + 2));
			break;
		case 0x02:
			print_field("  Flush timeout: %d",
					bt_get_le16(data + consumed + 2));
			break;
		case 0x03:
			switch (data[consumed + 3]) {
			case 0x00:
				str = "No Traffic";
				break;
			case 0x01:
				str = "Best Effort";
				break;
			case 0x02:
				str = "Guaranteed";
				break;
			default:
				str = "Reserved";
				break;
			}
			print_field("  Flags: 0x%2.2x", data[consumed + 2]);
			print_field("  Service type: %s (0x%2.2x)",
						str, data[consumed + 3]);
			print_field("  Token rate: 0x%8.8x",
					bt_get_le32(data + consumed + 4));
			print_field("  Token bucket size: 0x%8.8x",
					bt_get_le32(data + consumed + 8));
			print_field("  Peak bandwidth: 0x%8.8x",
					bt_get_le32(data + consumed + 12));
			print_field("  Latency: 0x%8.8x",
					bt_get_le32(data + consumed + 16));
			print_field("  Delay variation: 0x%8.8x",
					bt_get_le32(data + consumed + 20));
                        break;
		case 0x04:
			if (response)
				assign_mode(frame, data[consumed + 2], dcid);

			switch (data[consumed + 2]) {
			case 0x00:
				str = "Basic";
				break;
			case 0x01:
				str = "Retransmission";
				break;
			case 0x02:
				str = "Flow control";
				break;
			case 0x03:
				str = "Enhanced retransmission";
				break;
			case 0x04:
				str = "Streaming";
				break;
			default:
				str = "Reserved";
				break;
			}
			print_field("  Mode: %s (0x%2.2x)",
						str, data[consumed + 2]);
			print_field("  TX window size: %d", data[consumed + 3]);
			print_field("  Max transmit: %d", data[consumed + 4]);
			print_field("  Retransmission timeout: %d",
					bt_get_le16(data + consumed + 5));
			print_field("  Monitor timeout: %d",
					bt_get_le16(data + consumed + 7));
			print_field("  Maximum PDU size: %d",
					bt_get_le16(data + consumed + 9));
			break;
		case 0x05:
			switch (data[consumed + 2]) {
			case 0x00:
				str = "No FCS";
				break;
			case 0x01:
				str = "16-bit FCS";
				break;
			default:
				str = "Reserved";
				break;
			}
			print_field("  FCS: %s (0x%2.2d)",
						str, data[consumed + 2]);
			break;
		case 0x06:
			switch (data[consumed + 3]) {
			case 0x00:
				str = "No traffic";
				break;
			case 0x01:
				str = "Best effort";
				break;
			case 0x02:
				str = "Guaranteed";
				break;
			default:
				str = "Reserved";
				break;
			}
			print_field("  Identifier: 0x%2.2x",
						data[consumed + 2]);
			print_field("  Service type: %s (0x%2.2x)",
						str, data[consumed + 3]);
			print_field("  Maximum SDU size: 0x%4.4x",
					bt_get_le16(data + consumed + 4));
			print_field("  SDU inter-arrival time: 0x%8.8x",
					bt_get_le32(data + consumed + 6));
			print_field("  Access latency: 0x%8.8x",
					bt_get_le32(data + consumed + 10));
			print_field("  Flush timeout: 0x%8.8x",
					bt_get_le32(data + consumed + 14));
			break;
		case 0x07:
			print_field("  Max window size: %d",
					bt_get_le16(data + consumed + 2));
			break;
		default:
			packet_hexdump(data + consumed + 2, len);
			break;
		}

		consumed += len + 2;
	}

	if (consumed < size)
		packet_hexdump(data + consumed, size - consumed);
}

static void print_info_type(uint16_t type)
{
	const char *str;

	switch (btohs(type)) {
	case 0x0001:
		str = "Connectionless MTU";
		break;
	case 0x0002:
		str = "Extended features supported";
		break;
	case 0x0003:
		str = "Fixed channels supported";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Type: %s (0x%4.4x)", str, btohs(type));
}

static void print_info_result(uint16_t result)
{
	const char *str;

	switch (btohs(result)) {
	case 0x0000:
		str = "Success";
		break;
	case 0x0001:
		str = "Not supported";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Result: %s (0x%4.4x)", str, btohs(result));
}

static struct {
	uint8_t bit;
	const char *str;
} features_table[] = {
	{  0, "Flow control mode"			},
	{  1, "Retransmission mode"			},
	{  2, "Bi-directional QoS"			},
	{  3, "Enhanced Retransmission Mode"		},
	{  4, "Streaming Mode"				},
	{  5, "FCS Option"				},
	{  6, "Extended Flow Specification for BR/EDR"	},
	{  7, "Fixed Channels"				},
	{  8, "Extended Window Size"			},
	{  9, "Unicast Connectionless Data Reception"	},
	{ 31, "Reserved for feature mask extension"	},
	{ }
};

static void print_features(uint32_t features)
{
	uint32_t mask = features;
	int i;

	print_field("Features: 0x%8.8x", features);

	for (i = 0; features_table[i].str; i++) {
		if (features & (1 << features_table[i].bit)) {
			print_field("  %s", features_table[i].str);
			mask &= ~(1 << features_table[i].bit);
		}
	}

	if (mask)
		print_field("  Unknown features (0x%8.8x)", mask);
}

static struct {
	uint16_t cid;
	const char *str;
} channels_table[] = {
	{ 0x0000, "Null identifier"		},
	{ 0x0001, "L2CAP Signaling (BR/EDR)"	},
	{ 0x0002, "Connectionless reception"	},
	{ 0x0003, "AMP Manager Protocol"	},
	{ 0x0004, "Attribute Protocol"		},
	{ 0x0005, "L2CAP Signaling (LE)"	},
	{ 0x0006, "Security Manager"		},
	{ 0x003f, "AMP Test Manager"		},
	{ }
};

static void print_channels(uint64_t channels)
{
	uint64_t mask = channels;
	int i;

	print_field("Channels: 0x%16.16" PRIx64, channels);

	for (i = 0; channels_table[i].str; i++) {
		if (channels & (1 << channels_table[i].cid)) {
			print_field("  %s", channels_table[i].str);
			mask &= ~(1 << channels_table[i].cid);
		}
	}

	if (mask)
		print_field("  Unknown channels (0x%8.8" PRIx64 ")", mask);
}

static void print_move_result(uint16_t result)
{
	const char *str;

	switch (btohs(result)) {
	case 0x0000:
		str = "Move success";
		break;
	case 0x0001:
		str = "Move pending";
		break;
	case 0x0002:
		str = "Move refused - Controller ID not supported";
		break;
	case 0x0003:
		str = "Move refused - new Controller ID is same";
		break;
	case 0x0004:
		str = "Move refused - Configuration not supported";
		break;
	case 0x0005:
		str = "Move refused - Move Channel collision";
		break;
	case 0x0006:
		str = "Move refused - Channel not allowed to be moved";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Result: %s (0x%4.4x)", str, btohs(result));
}

static void print_move_cfm_result(uint16_t result)
{
	const char *str;

	switch (btohs(result)) {
	case 0x0000:
		str = "Move success - both sides succeed";
		break;
	case 0x0001:
		str = "Move failure - one or both sides refuse";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Result: %s (0x%4.4x)", str, btohs(result));
}

static void print_conn_param_result(uint16_t result)
{
	const char *str;

	switch (btohs(result)) {
	case 0x0000:
		str = "Connection Parameters accepted";
		break;
	case 0x0001:
		str = "Connection Parameters rejected";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Result: %s (0x%4.4x)", str, btohs(result));
}

static void sig_cmd_reject(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_cmd_reject *pdu = frame->data;
	const void *data = frame->data;
	uint16_t size = frame->size;
	uint16_t scid, dcid;

	print_reject_reason(pdu->reason);

	data += sizeof(*pdu);
	size -= sizeof(*pdu);

	switch (btohs(pdu->reason)) {
	case 0x0000:
		if (size != 0) {
			print_text(COLOR_ERROR, "invalid data size");
			packet_hexdump(data, size);
			break;
		}
		break;
	case 0x0001:
		if (size != 2) {
			print_text(COLOR_ERROR, "invalid data size");
			packet_hexdump(data, size);
			break;
		}
		print_field("MTU: %d", bt_get_le16(data));
		break;
	case 0x0002:
		if (size != 4) {
			print_text(COLOR_ERROR, "invalid data size");
			packet_hexdump(data, size);
			break;
		}
		dcid = bt_get_le16(data);
		scid = bt_get_le16(data + 2);
		print_cid("Destination", htobs(dcid));
		print_cid("Source", htobs(scid));
		break;
	default:
		packet_hexdump(data, size);
		break;
	}
}

static void sig_conn_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_conn_req *pdu = frame->data;

	print_psm(pdu->psm);
	print_cid("Source", pdu->scid);

	assign_scid(frame, btohs(pdu->scid), btohs(pdu->psm), 0);
}

static void sig_conn_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_conn_rsp *pdu = frame->data;

	print_cid("Destination", pdu->dcid);
	print_cid("Source", pdu->scid);
	print_conn_result(pdu->result);
	print_conn_status(pdu->status);

	assign_dcid(frame, btohs(pdu->dcid), btohs(pdu->scid));
}

static void sig_config_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_config_req *pdu = frame->data;

	print_cid("Destination", pdu->dcid);
	print_config_flags(pdu->flags);
	print_config_options(frame, 4, btohs(pdu->dcid), false);
}

static void sig_config_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_config_rsp *pdu = frame->data;

	print_cid("Source", pdu->scid);
	print_config_flags(pdu->flags);
	print_config_result(pdu->result);
	print_config_options(frame, 6, btohs(pdu->scid), true);
}

static void sig_disconn_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_disconn_req *pdu = frame->data;

	print_cid("Destination", pdu->dcid);
	print_cid("Source", pdu->scid);
}

static void sig_disconn_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_disconn_rsp *pdu = frame->data;

	print_cid("Destination", pdu->dcid);
	print_cid("Source", pdu->scid);

	release_scid(frame, btohs(pdu->scid));
}

static void sig_echo_req(const struct l2cap_frame *frame)
{
	packet_hexdump(frame->data, frame->size);
}

static void sig_echo_rsp(const struct l2cap_frame *frame)
{
	packet_hexdump(frame->data, frame->size);
}

static void sig_info_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_info_req *pdu = frame->data;

	print_info_type(pdu->type);
}

static void sig_info_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_info_rsp *pdu = frame->data;
	const void *data = frame->data;
	uint16_t size = frame->size;

	print_info_type(pdu->type);
	print_info_result(pdu->result);

	data += sizeof(*pdu);
	size -= sizeof(*pdu);

	if (btohs(pdu->result) != 0x0000) {
		if (size > 0) {
			print_text(COLOR_ERROR, "invalid data size");
			packet_hexdump(data, size);
		}
		return;
	}

	switch (btohs(pdu->type)) {
	case 0x0001:
		if (size != 2) {
			print_text(COLOR_ERROR, "invalid data size");
			packet_hexdump(data, size);
			break;
		}
		print_field("MTU: %d", bt_get_le16(data));
		break;
	case 0x0002:
		if (size != 4) {
			print_text(COLOR_ERROR, "invalid data size");
			packet_hexdump(data, size);
			break;
		}
		print_features(bt_get_le32(data));
		break;
	case 0x0003:
		if (size != 8) {
			print_text(COLOR_ERROR, "invalid data size");
			packet_hexdump(data, size);
			break;
		}
		print_channels(bt_get_le64(data));
		break;
	default:
		packet_hexdump(data, size);
		break;
	}
}

static void sig_create_chan_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_create_chan_req *pdu = frame->data;

	print_psm(pdu->psm);
	print_cid("Source", pdu->scid);
	print_field("Controller ID: %d", pdu->ctrlid);

	assign_scid(frame, btohs(pdu->scid), btohs(pdu->psm), pdu->ctrlid);
}

static void sig_create_chan_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_create_chan_rsp *pdu = frame->data;

	print_cid("Destination", pdu->dcid);
	print_cid("Source", pdu->scid);
	print_conn_result(pdu->result);
	print_conn_status(pdu->status);

	assign_dcid(frame, btohs(pdu->dcid), btohs(pdu->scid));
}

static void sig_move_chan_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_move_chan_req *pdu = frame->data;

	print_cid("Initiator", pdu->icid);
	print_field("Controller ID: %d", pdu->ctrlid);
}

static void sig_move_chan_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_move_chan_rsp *pdu = frame->data;

	print_cid("Initiator", pdu->icid);
	print_move_result(pdu->result);
}

static void sig_move_chan_cfm(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_move_chan_cfm *pdu = frame->data;

	print_cid("Initiator", pdu->icid);
	print_move_cfm_result(pdu->result);
}

static void sig_move_chan_cfm_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_move_chan_cfm_rsp *pdu = frame->data;

	print_cid("Initiator", pdu->icid);
}

static void sig_conn_param_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_conn_param_req *pdu = frame->data;

	print_field("Min interval: %d", btohs(pdu->min_interval));
	print_field("Max interval: %d", btohs(pdu->max_interval));
	print_field("Slave latency: %d", btohs(pdu->latency));
	print_field("Timeout multiplier: %d", btohs(pdu->timeout));
}

static void sig_conn_param_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_pdu_conn_param_rsp *pdu = frame->data;

	print_conn_param_result(pdu->result);
}

struct sig_opcode_data {
	uint8_t opcode;
	const char *str;
	void (*func) (const struct l2cap_frame *frame);
	uint16_t size;
	bool fixed;
};

static const struct sig_opcode_data bredr_sig_opcode_table[] = {
	{ 0x01, "Command Reject",
			sig_cmd_reject, 2, false },
	{ 0x02, "Connection Request",
			sig_conn_req, 4, true },
	{ 0x03, "Connection Response",
			sig_conn_rsp, 8, true },
	{ 0x04, "Configure Request",
			sig_config_req, 4, false },
	{ 0x05, "Configure Response",
			sig_config_rsp, 6, false },
	{ 0x06, "Disconnection Request",
			sig_disconn_req, 4, true },
	{ 0x07, "Disconnection Response",
			sig_disconn_rsp, 4, true },
	{ 0x08, "Echo Request",
			sig_echo_req, 0, false },
	{ 0x09, "Echo Response",
			sig_echo_rsp, 0, false },
	{ 0x0a, "Information Request",
			sig_info_req, 2, true },
	{ 0x0b, "Information Response",
			sig_info_rsp, 4, false },
	{ 0x0c, "Create Channel Request",
			sig_create_chan_req, 5, true },
	{ 0x0d, "Create Channel Response",
			sig_create_chan_rsp, 8, true },
	{ 0x0e, "Move Channel Request",
			sig_move_chan_req, 3, true },
	{ 0x0f, "Move Channel Response",
			sig_move_chan_rsp, 4, true },
	{ 0x10, "Move Channel Confirmation",
			sig_move_chan_cfm, 4, true },
	{ 0x11, "Move Channel Confirmation Response",
			sig_move_chan_cfm_rsp, 2, true },
	{ },
};

static const struct sig_opcode_data le_sig_opcode_table[] = {
	{ 0x01, "Command Reject",
			sig_cmd_reject, 2, false },
	{ 0x12, "Connection Parameter Update Request",
			sig_conn_param_req, 8, true },
	{ 0x13, "Connection Parameter Update Response",
			sig_conn_param_rsp, 2, true },
	{ },
};

static void bredr_sig_packet(uint16_t index, bool in, uint16_t handle,
				uint16_t cid, const void *data, uint16_t size)
{
	struct l2cap_frame frame;

	while (size > 0) {
		const struct bt_l2cap_hdr_sig *hdr = data;
		const struct sig_opcode_data *opcode_data = NULL;
		const char *opcode_color, *opcode_str;
		uint16_t len;
		int i;

		if (size < 4) {
			print_text(COLOR_ERROR, "malformed signal packet");
			packet_hexdump(data, size);
			return;
		}

		len = btohs(hdr->len);

		data += 4;
		size -= 4;

		if (size < len) {
			print_text(COLOR_ERROR, "invalid signal packet size");
			packet_hexdump(data, size);
			return;
		}

		for (i = 0; bredr_sig_opcode_table[i].str; i++) {
			if (bredr_sig_opcode_table[i].opcode == hdr->code) {
				opcode_data = &bredr_sig_opcode_table[i];
				break;
			}
		}

		if (opcode_data) {
			if (opcode_data->func) {
				if (in)
					opcode_color = COLOR_MAGENTA;
				else
					opcode_color = COLOR_BLUE;
			} else
				opcode_color = COLOR_WHITE_BG;
			opcode_str = opcode_data->str;
		} else {
			opcode_color = COLOR_WHITE_BG;
			opcode_str = "Unknown";
		}

		print_indent(6, opcode_color, "L2CAP: ", opcode_str,
					COLOR_OFF,
					" (0x%2.2x) ident %d len %d",
					hdr->code, hdr->ident, len);

		if (!opcode_data || !opcode_data->func) {
			packet_hexdump(data, len);
			data += len;
			size -= len;
			return;
		}

		if (opcode_data->fixed) {
			if (len != opcode_data->size) {
				print_text(COLOR_ERROR, "invalid size");
				packet_hexdump(data, len);
				data += len;
				size -= len;
				continue;
			}
		} else {
			if (len < opcode_data->size) {
				print_text(COLOR_ERROR, "too short packet");
				packet_hexdump(data, size);
				data += len;
				size -= len;
				continue;
			}
		}

		l2cap_frame_init(&frame, index, in, handle, cid, data, len);
		opcode_data->func(&frame);

		data += len;
		size -= len;
	}

	packet_hexdump(data, size);
}

static void le_sig_packet(uint16_t index, bool in, uint16_t handle,
				uint16_t cid, const void *data, uint16_t size)
{
	struct l2cap_frame frame;
	const struct bt_l2cap_hdr_sig *hdr = data;
	const struct sig_opcode_data *opcode_data = NULL;
	const char *opcode_color, *opcode_str;
	uint16_t len;
	int i;

	if (size < 4) {
		print_text(COLOR_ERROR, "malformed signal packet");
		packet_hexdump(data, size);
		return;
	}

	len = btohs(hdr->len);

	data += 4;
	size -= 4;

	if (size != len) {
		print_text(COLOR_ERROR, "invalid signal packet size");
		packet_hexdump(data, size);
		return;
	}

	for (i = 0; le_sig_opcode_table[i].str; i++) {
		if (le_sig_opcode_table[i].opcode == hdr->code) {
			opcode_data = &le_sig_opcode_table[i];
			break;
		}
	}

	if (opcode_data) {
		if (opcode_data->func) {
			if (in)
				opcode_color = COLOR_MAGENTA;
			else
				opcode_color = COLOR_BLUE;
		} else
			opcode_color = COLOR_WHITE_BG;
		opcode_str = opcode_data->str;
	} else {
		opcode_color = COLOR_WHITE_BG;
		opcode_str = "Unknown";
	}

	print_indent(6, opcode_color, "LE L2CAP: ", opcode_str, COLOR_OFF,
					" (0x%2.2x) ident %d len %d",
					hdr->code, hdr->ident, len);

	if (!opcode_data || !opcode_data->func) {
		packet_hexdump(data, len);
		return;
	}

	if (opcode_data->fixed) {
		if (len != opcode_data->size) {
			print_text(COLOR_ERROR, "invalid size");
			packet_hexdump(data, len);
			return;
		}
	} else {
		if (len < opcode_data->size) {
			print_text(COLOR_ERROR, "too short packet");
			packet_hexdump(data, size);
			return;
		}
	}

	l2cap_frame_init(&frame, index, in, handle, cid, data, len);
	opcode_data->func(&frame);
}

static void connless_packet(uint16_t index, bool in, uint16_t handle,
				uint16_t cid, const void *data, uint16_t size)
{
	struct l2cap_frame frame;
	const struct bt_l2cap_hdr_connless *hdr = data;
	uint16_t psm;

	if (size < 2) {
		print_text(COLOR_ERROR, "malformed connectionless packet");
		packet_hexdump(data, size);
		return;
	}

	psm = btohs(hdr->psm);

	data += 2;
	size -= 2;

	print_indent(6, COLOR_CYAN, "L2CAP: Connectionless", "", COLOR_OFF,
						" len %d [PSM %d]", size, psm);

	switch (psm) {
	default:
		packet_hexdump(data, size);
		break;
	}

	l2cap_frame_init(&frame, index, in, handle, cid, data, size);
}

static void print_controller_list(const uint8_t *data, uint16_t size)
{
	while (size > 2) {
		const char *str;

		print_field("Controller ID: %d", data[0]);

		switch (data[1]) {
		case 0x00:
			str = "Primary BR/EDR Controller";
			break;
		case 0x01:
			str = "802.11 AMP Controller";
			break;
		default:
			str = "Reserved";
			break;
		}

		print_field("  Type: %s (0x%2.2x)", str, data[1]);

		switch (data[2]) {
		case 0x00:
			str = "Present";
			break;
		case 0x01:
			str = "Bluetooth only";
			break;
		case 0x02:
			str = "No capacity";
			break;
		case 0x03:
			str = "Low capacity";
			break;
		case 0x04:
			str = "Medium capacity";
			break;
		case 0x05:
			str = "High capacity";
			break;
		case 0x06:
			str = "Full capacity";
			break;
		default:
			str = "Reserved";
			break;
		}

		print_field("  Status: %s (0x%2.2x)", str, data[2]);

		data += 3;
		size -= 3;
	}

	packet_hexdump(data, size);
}

static void amp_cmd_reject(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_amp_cmd_reject *pdu = frame->data;

	print_field("Reason: 0x%4.4x", btohs(pdu->reason));
}

static void amp_discover_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_amp_discover_req *pdu = frame->data;

	print_field("MTU/MPS size: %d", btohs(pdu->size));
	print_field("Extended feature mask: 0x%4.4x", btohs(pdu->features));
}

static void amp_discover_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_amp_discover_rsp *pdu = frame->data;

	print_field("MTU/MPS size: %d", btohs(pdu->size));
	print_field("Extended feature mask: 0x%4.4x", btohs(pdu->features));

	print_controller_list(frame->data + 4, frame->size - 4);
}

static void amp_change_notify(const struct l2cap_frame *frame)
{
	print_controller_list(frame->data, frame->size);
}

static void amp_change_response(const struct l2cap_frame *frame)
{
}

static void amp_get_info_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_amp_get_info_req *pdu = frame->data;

	print_field("Controller ID: %d", pdu->ctrlid);
}

static void amp_get_info_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_amp_get_info_rsp *pdu = frame->data;
	const char *str;

	print_field("Controller ID: %d", pdu->ctrlid);

	switch (pdu->status) {
	case 0x00:
		str = "Success";
		break;
	case 0x01:
		str = "Invalid Controller ID";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Status: %s (0x%2.2x)", str, pdu->status);

	print_field("Total bandwidth: %d kbps", btohl(pdu->total_bw));
	print_field("Max guaranteed bandwidth: %d kbps", btohl(pdu->max_bw));
	print_field("Min latency: %d", btohl(pdu->min_latency));

	print_field("PAL capabilities: 0x%4.4x", btohs(pdu->pal_cap));
	print_field("Max ASSOC length: %d", btohs(pdu->max_assoc_len));
}

static void amp_get_assoc_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_amp_get_assoc_req *pdu = frame->data;

	print_field("Controller ID: %d", pdu->ctrlid);
}

static void amp_get_assoc_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_amp_get_assoc_rsp *pdu = frame->data;
	const char *str;

	print_field("Controller ID: %d", pdu->ctrlid);

	switch (pdu->status) {
	case 0x00:
		str = "Success";
		break;
	case 0x01:
		str = "Invalid Controller ID";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Status: %s (0x%2.2x)", str, pdu->status);

	packet_hexdump(frame->data + 2, frame->size - 2);
}

static void amp_create_phy_link_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_amp_create_phy_link_req *pdu = frame->data;

	print_field("Local controller ID: %d", pdu->local_ctrlid);
	print_field("Remote controller ID: %d", pdu->remote_ctrlid);

	packet_hexdump(frame->data + 2, frame->size - 2);
}

static void amp_create_phy_link_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_amp_create_phy_link_rsp *pdu = frame->data;
	const char *str;

	print_field("Local controller ID: %d", pdu->local_ctrlid);
	print_field("Remote controller ID: %d", pdu->remote_ctrlid);

	switch (pdu->status) {
	case 0x00:
		str = "Success";
		break;
	case 0x01:
		str = "Invalid Controller ID";
		break;
	case 0x02:
		str = "Failed - Unable to start link creation";
		break;
	case 0x03:
		str = "Failed - Collision occurred";
		break;
	case 0x04:
		str = "Failed - Disconnected link packet received";
		break;
	case 0x05:
		str = "Failed - Link already exists";
		break;
	case 0x06:
		str = "Failed - Security violation";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Status: %s (0x%2.2x)", str, pdu->status);
}

static void amp_disconn_phy_link_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_amp_disconn_phy_link_req *pdu = frame->data;

	print_field("Local controller ID: %d", pdu->local_ctrlid);
	print_field("Remote controller ID: %d", pdu->remote_ctrlid);
}

static void amp_disconn_phy_link_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_amp_disconn_phy_link_rsp *pdu = frame->data;
	const char *str;

	print_field("Local controller ID: %d", pdu->local_ctrlid);
	print_field("Remote controller ID: %d", pdu->remote_ctrlid);

	switch (pdu->status) {
	case 0x00:
		str = "Success";
		break;
	case 0x01:
		str = "Invalid Controller ID";
		break;
	case 0x02:
		str = "Failed - No link exists";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Status: %s (0x%2.2x)", str, pdu->status);
}

struct amp_opcode_data {
	uint8_t opcode;
	const char *str;
	void (*func) (const struct l2cap_frame *frame);
	uint16_t size;
	bool fixed;
};

static const struct amp_opcode_data amp_opcode_table[] = {
	{ 0x01, "Command Reject",
			amp_cmd_reject, 2, false },
	{ 0x02, "Discover Request",
			amp_discover_req, 4, true },
	{ 0x03, "Discover Response",
			amp_discover_rsp, 7, false },
	{ 0x04, "Change Notify",
			amp_change_notify, 3, false },
	{ 0x05, "Change Response",
			amp_change_response, 0, true },
	{ 0x06, "Get Info Request",
			amp_get_info_req, 1, true },
	{ 0x07, "Get Info Response",
			amp_get_info_rsp, 18, true },
	{ 0x08, "Get Assoc Request",
			amp_get_assoc_req, 1, true },
	{ 0x09, "Get Assoc Response",
			amp_get_assoc_rsp, 2, false },
	{ 0x0a, "Create Physical Link Request",
			amp_create_phy_link_req, 2, false },
	{ 0x0b, "Create Physical Link Response",
			amp_create_phy_link_rsp, 3, true },
	{ 0x0c, "Disconnect Physical Link Request",
			amp_disconn_phy_link_req, 2, true },
	{ 0x0d, "Disconnect Physical Link Response",
			amp_disconn_phy_link_rsp, 3, true },
	{ },
};

static void amp_packet(uint16_t index, bool in, uint16_t handle,
			uint16_t cid, const void *data, uint16_t size)
{
	struct l2cap_frame frame;
	uint16_t control, fcs, len;
	uint8_t opcode, ident;
	const struct amp_opcode_data *opcode_data = NULL;
	const char *opcode_color, *opcode_str;
	int i;

	if (size < 4) {
		print_text(COLOR_ERROR, "malformed info frame packet");
		packet_hexdump(data, size);
		return;
	}

	control = bt_get_le16(data);
	fcs = bt_get_le16(data + size - 2);

	print_indent(6, COLOR_CYAN, "Channel:", "", COLOR_OFF,
				" %d dlen %d control 0x%4.4x fcs 0x%4.4x",
						3, size, control, fcs);

	if (control & 0x01)
		return;

	if (size < 8) {
		print_text(COLOR_ERROR, "malformed manager packet");
		packet_hexdump(data, size);
		return;
	}

	opcode = *((const uint8_t *) (data + 2));
	ident = *((const uint8_t *) (data + 3));
	len = bt_get_le16(data + 4);

	if (len != size - 8) {
		print_text(COLOR_ERROR, "invalid manager packet size");
		packet_hexdump(data +  2, size - 4);
		return;
	}

	for (i = 0; amp_opcode_table[i].str; i++) {
		if (amp_opcode_table[i].opcode == opcode) {
			opcode_data = &amp_opcode_table[i];
			break;
		}
	}

	if (opcode_data) {
		if (opcode_data->func) {
			if (in)
				opcode_color = COLOR_MAGENTA;
			else
				opcode_color = COLOR_BLUE;
		} else
			opcode_color = COLOR_WHITE_BG;
		opcode_str = opcode_data->str;
	} else {
		opcode_color = COLOR_WHITE_BG;
		opcode_str = "Unknown";
	}

	print_indent(6, opcode_color, "AMP: ", opcode_str, COLOR_OFF,
			" (0x%2.2x) ident %d len %d", opcode, ident, len);

	if (!opcode_data || !opcode_data->func) {
		packet_hexdump(data + 6, size - 8);
		return;
	}

	if (opcode_data->fixed) {
		if (len != opcode_data->size) {
			print_text(COLOR_ERROR, "invalid size");
			packet_hexdump(data + 6, size - 8);
			return;
		}
	} else {
		if (len < opcode_data->size) {
			print_text(COLOR_ERROR, "too short packet");
			packet_hexdump(data + 6, size - 8);
			return;
		}
	}

	l2cap_frame_init(&frame, index, in, handle, cid, data + 6, len);
	opcode_data->func(&frame);
}

static void print_hex_field(const char *label, const uint8_t *data,
								uint8_t len)
{
	char str[len * 2 + 1];
	uint8_t i;

	str[0] = '\0';

	for (i = 0; i < len; i++)
		sprintf(str + (i * 2), "%2.2x", data[i]);

	print_field("%s: %s", label, str);
}

static void print_uuid(const char *label, const void *data, uint16_t size)
{
	const char *str;

	switch (size) {
	case 2:
		str = uuid16_to_str(bt_get_le16(data));
		print_field("%s: %s (0x%4.4x)", label, str, bt_get_le16(data));
		break;
	case 4:
		str = uuid32_to_str(bt_get_le32(data));
		print_field("%s: %s (0x%8.8x)", label, str, bt_get_le32(data));
		break;
	case 16:
		str = uuid128_to_str(data);
		print_field("%s: %s (%8.8x-%4.4x-%4.4x-%4.4x-%8.8x%4.4x)",
				label, str,
				bt_get_le32(data + 12), bt_get_le16(data + 10),
				bt_get_le16(data + 8), bt_get_le16(data + 6),
				bt_get_le32(data + 2), bt_get_le16(data + 0));
		break;
	default:
		packet_hexdump(data, size);
		break;
	}
}

static void print_handle_range(const char *label, const void *data)
{
	print_field("%s: 0x%4.4x-0x%4.4x", label,
				bt_get_le16(data), bt_get_le16(data + 2));
}

static void print_data_list(const char *label, uint8_t length,
					const void *data, uint16_t size)
{
	uint8_t count;

	if (length == 0)
		return;

	count = size / length;

	print_field("%s: %u entr%s", label, count, count == 1 ? "y" : "ies");

	while (size >= length) {
		print_field("Handle: 0x%4.4x", bt_get_le16(data));
		print_hex_field("Value", data + 2, length - 2);

		data += length;
		size -= length;
	}

	packet_hexdump(data, size);
}

static void print_attribute_info(uint16_t type, const void *data, uint16_t len)
{
	const char *str = uuid16_to_str(type);

	print_field("%s: %s (0x%4.4x)", "Attribute type", str, type);

	switch (type) {
	case 0x2800:	/* Primary Service */
	case 0x2801:	/* Secondary Service */
		print_uuid("  UUID", data, len);
		break;
	case 0x2802:	/* Include */
		if (len < 4) {
			print_hex_field("  Value", data, len);
			break;
		}
		print_handle_range("  Handle range", data);
		print_uuid("  UUID", data + 4, len - 4);
		break;
	case 0x2803:	/* Characteristic */
		if (len < 3) {
			print_hex_field("  Value", data, len);
			break;
		}
		print_field("  Properties: 0x%2.2x", *((uint8_t *) data));
		print_field("  Handle: 0x%2.2x", bt_get_le16(data + 1));
		print_uuid("  UUID", data + 3, len - 3);
		break;
	default:
		print_hex_field("Value", data, len);
		break;
	}
}

static const char *att_opcode_to_str(uint8_t opcode);

static void att_error_response(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_att_error_response *pdu = frame->data;
	const char *str;

	switch (pdu->error) {
	case 0x01:
		str = "Invalid Handle";
		break;
	case 0x02:
		str = "Read Not Permitted";
		break;
	case 0x03:
		str = "Write Not Permitted";
		break;
	case 0x04:
		str = "Invalid PDU";
		break;
	case 0x05:
		str = "Insufficient Authentication";
		break;
	case 0x06:
		str = "Request Not Supported";
		break;
	case 0x07:
		str = "Invalid Offset";
		break;
	case 0x08:
		str = "Insufficient Authorization";
		break;
	case 0x09:
		str = "Prepare Queue Full";
		break;
	case 0x0a:
		str = "Attribute Not Found";
		break;
	case 0x0b:
		str = "Attribute Not Long";
		break;
	case 0x0c:
		str = "Insufficient Encryption Key Size";
		break;
	case 0x0d:
		str = "Invalid Attribute Value Length";
		break;
	case 0x0e:
		str = "Unlikely Error";
		break;
	case 0x0f:
		str = "Insufficient Encryption";
		break;
	case 0x10:
		str = "Unsupported Group Type";
		break;
	case 0x11:
		str = "Insufficient Resources";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("%s (0x%2.2x)", att_opcode_to_str(pdu->request),
							pdu->request);
	print_field("Handle: 0x%4.4x", btohs(pdu->handle));
	print_field("Error: %s (0x%2.2x)", str, pdu->error);
}

static void att_exchange_mtu_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_att_exchange_mtu_req *pdu = frame->data;

	print_field("Client RX MTU: %d", btohs(pdu->mtu));
}

static void att_exchange_mtu_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_att_exchange_mtu_rsp *pdu = frame->data;

	print_field("Server RX MTU: %d", btohs(pdu->mtu));
}

static void att_find_info_req(const struct l2cap_frame *frame)
{
	print_handle_range("Handle range", frame->data);
}

static const char *att_format_str(uint8_t format)
{
	switch (format) {
	case 0x01:
		return "UUID-16";
	case 0x02:
		return "UUID-128";
	default:
		return "unknown";
	}
}

static uint16_t print_info_data_16(const uint16_t *data, uint16_t len)
{
	while (len >= 4) {
		print_field("Handle: 0x%4.4x", bt_get_le16(data));
		print_uuid("UUID", data + 2, 2);
		data += 4;
		len -= 4;
	}

	return len;
}

static uint16_t print_info_data_128(const uint16_t *data, uint16_t len)
{
	while (len >= 18) {
		print_field("Handle: 0x%4.4x", bt_get_le16(data));
		print_uuid("UUID", data + 2, 16);
		data += 18;
		len -= 18;
	}

	return len;
}

static void att_find_info_rsp(const struct l2cap_frame *frame)
{
	const uint8_t *format = frame->data;
	uint16_t len;

	print_field("Format: %s (0x%2.2x)", att_format_str(*format), *format);

	if (*format == 0x01)
		len = print_info_data_16(frame->data + 1, frame->size - 1);
	else if (*format == 0x02)
		len = print_info_data_128(frame->data + 1, frame->size - 1);
	else
		len = frame->size - 1;

	packet_hexdump(frame->data + (frame->size - len), len);
}

static void att_find_by_type_val_req(const struct l2cap_frame *frame)
{
	uint16_t type;

	print_handle_range("Handle range", frame->data);

	type = bt_get_le16(frame->data + 4);
	print_attribute_info(type, frame->data + 6, frame->size - 6);
}

static void att_find_by_type_val_rsp(const struct l2cap_frame *frame)
{
	const uint8_t *ptr = frame->data;
	uint16_t len = frame->size;

	while (len >= 4) {
		print_handle_range("Handle range", ptr);
		ptr += 4;
		len -= 4;
	}

	packet_hexdump(ptr, len);
}

static void att_read_type_req(const struct l2cap_frame *frame)
{
	print_handle_range("Handle range", frame->data);
	print_uuid("Attribute type", frame->data + 4, frame->size - 4);
}

static void att_read_type_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_att_read_group_type_rsp *pdu = frame->data;

	print_field("Attribute data length: %d", pdu->length);
	print_data_list("Attribute data list", pdu->length,
					frame->data + 1, frame->size - 1);
}

static void att_read_req(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_att_read_req *pdu = frame->data;

	print_field("Handle: 0x%4.4x", btohs(pdu->handle));
}

static void att_read_rsp(const struct l2cap_frame *frame)
{
	print_hex_field("Value", frame->data, frame->size);
}

static void att_read_blob_req(const struct l2cap_frame *frame)
{
	print_field("Handle: 0x%4.4x", bt_get_le16(frame->data));
	print_field("Offset: 0x%4.4x", bt_get_le16(frame->data + 2));
}

static void att_read_blob_rsp(const struct l2cap_frame *frame)
{
	packet_hexdump(frame->data, frame->size);
}

static void att_read_multiple_req(const struct l2cap_frame *frame)
{
	int i, count;

	count = frame->size / 2;

	for (i = 0; i < count; i++)
		print_field("Handle: 0x%4.4x",
					bt_get_le16(frame->data + (i * 2)));
}

static void att_read_group_type_req(const struct l2cap_frame *frame)
{
	print_handle_range("Handle range", frame->data);
	print_uuid("Attribute group type", frame->data + 4, frame->size - 4);
}

static void att_read_group_type_rsp(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_att_read_group_type_rsp *pdu = frame->data;

	print_field("Attribute data length: %d", pdu->length);
	print_data_list("Attribute data list", pdu->length,
					frame->data + 1, frame->size - 1);
}

static void att_write_req(const struct l2cap_frame *frame)
{
	print_field("Handle: 0x%4.4x", bt_get_le16(frame->data));
	print_hex_field("  Data", frame->data + 2, frame->size - 2);
}

static void att_write_rsp(const struct l2cap_frame *frame)
{
}

static void att_prepare_write_req(const struct l2cap_frame *frame)
{
	print_field("Handle: 0x%4.4x", bt_get_le16(frame->data));
	print_field("Offset: 0x%4.4x", bt_get_le16(frame->data + 2));
	print_hex_field("  Data", frame->data + 4, frame->size - 4);
}

static void att_prepare_write_rsp(const struct l2cap_frame *frame)
{
	print_field("Handle: 0x%4.4x", bt_get_le16(frame->data));
	print_field("Offset: 0x%4.4x", bt_get_le16(frame->data + 2));
	print_hex_field("  Data", frame->data + 4, frame->size - 4);
}

static void att_execute_write_req(const struct l2cap_frame *frame)
{
	uint8_t flags = *(uint8_t *) frame->data;
	const char *flags_str;

	switch (flags) {
	case 0x00:
		flags_str = "Cancel all prepared writes";
		break;
	case 0x01:
		flags_str = "Immediately write all pending values";
		break;
	default:
		flags_str = "Unknown";
		break;
	}

	print_field("Flags: %s (0x%02x)", flags_str, flags);
}

static void att_handle_value_notify(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_att_handle_value_notify *pdu = frame->data;

	print_field("Handle: 0x%4.4x", btohs(pdu->handle));
	print_hex_field("  Data", frame->data + 2, frame->size - 2);
}

static void att_handle_value_ind(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_att_handle_value_ind *pdu = frame->data;

	print_field("Handle: 0x%4.4x", btohs(pdu->handle));
	print_hex_field("  Data", frame->data + 2, frame->size - 2);
}

static void att_handle_value_conf(const struct l2cap_frame *frame)
{
}

static void att_write_command(const struct l2cap_frame *frame)
{
	print_field("Handle: 0x%4.4x", bt_get_le16(frame->data));
	print_hex_field("  Data", frame->data + 2, frame->size - 2);
}

struct att_opcode_data {
	uint8_t opcode;
	const char *str;
	void (*func) (const struct l2cap_frame *frame);
	uint8_t size;
	bool fixed;
};

static const struct att_opcode_data att_opcode_table[] = {
	{ 0x01, "Error Response",
			att_error_response, 4, true },
	{ 0x02, "Exchange MTU Request",
			att_exchange_mtu_req, 2, true },
	{ 0x03, "Exchange MTU Response",
			att_exchange_mtu_rsp, 2, true },
	{ 0x04, "Find Information Request",
			att_find_info_req, 4, true },
	{ 0x05, "Find Information Response",
			att_find_info_rsp, 5, false },
	{ 0x06, "Find By Type Value Request",
			att_find_by_type_val_req, 6, false },
	{ 0x07, "Find By Type Value Response",
			att_find_by_type_val_rsp, 4, false },
	{ 0x08, "Read By Type Request",
			att_read_type_req, 6, false },
	{ 0x09, "Read By Type Response",
			att_read_type_rsp, 3, false },
	{ 0x0a, "Read Request",
			att_read_req, 2, true },
	{ 0x0b, "Read Response",
			att_read_rsp, 0, false },
	{ 0x0c, "Read Blob Request",
			att_read_blob_req, 4, true },
	{ 0x0d, "Read Blob Response",
			att_read_blob_rsp, 0, false },
	{ 0x0e, "Read Multiple Request",
			att_read_multiple_req, 4, false },
	{ 0x0f, "Read Multiple Response"	},
	{ 0x10, "Read By Group Type Request",
			att_read_group_type_req, 6, false },
	{ 0x11, "Read By Group Type Response",
			att_read_group_type_rsp, 4, false },
	{ 0x12, "Write Request"	,
			att_write_req, 2, false	},
	{ 0x13, "Write Response",
			att_write_rsp, 0, true	},
	{ 0x16, "Prepare Write Request",
			att_prepare_write_req, 4, false },
	{ 0x17, "Prepare Write Response",
			att_prepare_write_rsp, 4, false },
	{ 0x18, "Execute Write Request",
			att_execute_write_req, 1, true },
	{ 0x19, "Execute Write Response"	},
	{ 0x1b, "Handle Value Notification",
			att_handle_value_notify, 2, false },
	{ 0x1d, "Handle Value Indication",
			att_handle_value_ind, 2, false },
	{ 0x1e, "Handle Value Confirmation",
			att_handle_value_conf, 0, true },
	{ 0x52, "Write Command",
			att_write_command, 2, false },
	{ 0xd2, "Signed Write Command"		},
	{ }
};

static const char *att_opcode_to_str(uint8_t opcode)
{
	int i;

	for (i = 0; att_opcode_table[i].str; i++) {
		if (att_opcode_table[i].opcode == opcode)
			return att_opcode_table[i].str;
	}

	return "Unknown";
}

static void att_packet(uint16_t index, bool in, uint16_t handle,
			uint16_t cid, const void *data, uint16_t size)
{
	struct l2cap_frame frame;
	uint8_t opcode = *((const uint8_t *) data);
	const struct att_opcode_data *opcode_data = NULL;
	const char *opcode_color, *opcode_str;
	int i;

	if (size < 1) {
		print_text(COLOR_ERROR, "malformed attribute packet");
		packet_hexdump(data, size);
		return;
	}

	for (i = 0; att_opcode_table[i].str; i++) {
		if (att_opcode_table[i].opcode == opcode) {
			opcode_data = &att_opcode_table[i];
			break;
		}
	}

	if (opcode_data) {
		if (opcode_data->func) {
			if (in)
				opcode_color = COLOR_MAGENTA;
			else
				opcode_color = COLOR_BLUE;
		} else
			opcode_color = COLOR_WHITE_BG;
		opcode_str = opcode_data->str;
	} else {
		opcode_color = COLOR_WHITE_BG;
		opcode_str = "Unknown";
	}

	print_indent(6, opcode_color, "ATT: ", opcode_str, COLOR_OFF,
				" (0x%2.2x) len %d", opcode, size - 1);

	if (!opcode_data || !opcode_data->func) {
		packet_hexdump(data + 1, size - 1);
		return;
	}

	if (opcode_data->fixed) {
		if (size - 1 != opcode_data->size) {
			print_text(COLOR_ERROR, "invalid size");
			packet_hexdump(data + 1, size - 1);
			return;
		}
	} else {
		if (size - 1 < opcode_data->size) {
			print_text(COLOR_ERROR, "too short packet");
			packet_hexdump(data + 1, size - 1);
			return;
		}
	}

	l2cap_frame_init(&frame, index, in, handle, cid, data + 1, size - 1);
	opcode_data->func(&frame);
}

static void print_addr(const uint8_t *addr, uint8_t addr_type)
{
	const char *str;

	switch (addr_type) {
	case 0x00:
		print_field("Address: %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
						addr[5], addr[4], addr[3],
						addr[2], addr[1], addr[0]);
		break;
	case 0x01:
		switch ((addr[5] & 0xc0) >> 6) {
		case 0x00:
			str = "Non-Resolvable";
			break;
		case 0x01:
			str = "Resolvable";
			break;
		case 0x03:
			str = "Static";
			break;
		default:
			str = "Reserved";
			break;
		}

		print_field("Address: %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X"
					" (%s)", addr[5], addr[4], addr[3],
					addr[2], addr[1], addr[0], str);
		break;
	default:
		print_field("Address: %2.2X-%2.2X-%2.2X-%2.2X-%2.2X-%2.2X",
						addr[5], addr[4], addr[3],
						addr[2], addr[1], addr[0]);
		break;
	}
}

static void print_addr_type(uint8_t addr_type)
{
	const char *str;

	switch (addr_type) {
	case 0x00:
		str = "Public";
		break;
	case 0x01:
		str = "Random";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Address type: %s (0x%2.2x)", str, addr_type);
}

static void print_smp_io_capa(uint8_t io_capa)
{
	const char *str;

	switch (io_capa) {
	case 0x00:
		str = "DisplayOnly";
		break;
	case 0x01:
		str = "DisplayYesNo";
		break;
	case 0x02:
		str = "KeyboardOnly";
		break;
	case 0x03:
		str = "NoInputNoOutput";
		break;
	case 0x04:
		str = "KeyboardDisplay";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("IO capability: %s (0x%2.2x)", str, io_capa);
}

static void print_smp_oob_data(uint8_t oob_data)
{
	const char *str;

	switch (oob_data) {
	case 0x00:
		str = "Authentication data not present";
		break;
	case 0x01:
		str = "Authentication data from remote device present";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("OOB data: %s (0x%2.2x)", str, oob_data);
}

static void print_smp_auth_req(uint8_t auth_req)
{
	const char *str;

	switch (auth_req & 0x03) {
	case 0x00:
		str = "No bonding";
		break;
	case 0x01:
		str = "Bonding";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Authentication requirement: %s - %s (0x%2.2x)",
			str, (auth_req & 0x04) ? "MITM" : "No MITM", auth_req);
}

static void smp_pairing_request(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_smp_pairing_request *pdu = frame->data;

	print_smp_io_capa(pdu->io_capa);
	print_smp_oob_data(pdu->oob_data);
	print_smp_auth_req(pdu->auth_req);

	print_field("Max encryption key size: %d", pdu->max_key_size);
	print_field("Initiator key distribution: 0x%2.2x", pdu->init_key_dist);
	print_field("Responder key distribution: 0x%2.2x", pdu->resp_key_dist);
}

static void smp_pairing_response(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_smp_pairing_response *pdu = frame->data;

	print_smp_io_capa(pdu->io_capa);
	print_smp_oob_data(pdu->oob_data);
	print_smp_auth_req(pdu->auth_req);

	print_field("Max encryption key size: %d", pdu->max_key_size);
	print_field("Initiator key distribution: 0x%2.2x", pdu->init_key_dist);
	print_field("Responder key distribution: 0x%2.2x", pdu->resp_key_dist);
}

static void smp_pairing_confirm(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_smp_pairing_confirm *pdu = frame->data;

	print_hex_field("Confim value", pdu->value, 16);
}

static void smp_pairing_random(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_smp_pairing_random *pdu = frame->data;

	print_hex_field("Random value", pdu->value, 16);
}

static void smp_pairing_failed(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_smp_pairing_failed *pdu = frame->data;
	const char *str;

	switch (pdu->reason) {
	case 0x01:
		str = "Passkey entry failed";
		break;
	case 0x02:
		str = "OOB not available";
		break;
	case 0x03:
		str = "Authentication requirements";
		break;
	case 0x04:
		str = "Confirm value failed";
		break;
	case 0x05:
		str = "Pairing not supported";
		break;
	case 0x06:
		str = "Encryption key size";
		break;
	case 0x07:
		str = "Command not supported";
		break;
	case 0x08:
		str = "Unspecified reason";
		break;
	case 0x09:
		str = "Repeated attempts";
		break;
	case 0x0a:
		str = "Invalid parameters";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("Reason: %s (0x%2.2x)", str, pdu->reason);
}

static void smp_encrypt_info(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_smp_encrypt_info *pdu = frame->data;

	print_hex_field("Long term key", pdu->ltk, 16);
}

static void smp_master_ident(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_smp_master_ident *pdu = frame->data;

	print_field("EDIV: 0x%4.4x", btohs(pdu->ediv));
	print_field("Rand: 0x%16.16" PRIx64, btohll(pdu->rand));
}

static void smp_ident_info(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_smp_ident_info *pdu = frame->data;

	print_hex_field("Identity resolving key", pdu->irk, 16);
}

static void smp_ident_addr_info(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_smp_ident_addr_info *pdu = frame->data;

	print_addr_type(pdu->addr_type);
	print_addr(pdu->addr, pdu->addr_type);
}

static void smp_signing_info(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_smp_signing_info *pdu = frame->data;

	print_hex_field("Signature key", pdu->csrk, 16);
}

static void smp_security_request(const struct l2cap_frame *frame)
{
	const struct bt_l2cap_smp_security_request *pdu = frame->data;

	print_smp_auth_req(pdu->auth_req);
}

struct smp_opcode_data {
	uint8_t opcode;
	const char *str;
	void (*func) (const struct l2cap_frame *frame);
	uint8_t size;
	bool fixed;
};

static const struct smp_opcode_data smp_opcode_table[] = {
	{ 0x01, "Pairing Request",
			smp_pairing_request, 6, true },
	{ 0x02, "Pairing Response",
			smp_pairing_response, 6, true },
	{ 0x03, "Pairing Confirm",
			smp_pairing_confirm, 16, true },
	{ 0x04, "Pairing Random",
			smp_pairing_random, 16, true },
	{ 0x05, "Pairing Failed",
			smp_pairing_failed, 1, true },
	{ 0x06, "Encryption Information",
			smp_encrypt_info, 16, true },
	{ 0x07, "Master Identification",
			smp_master_ident, 10, true },
	{ 0x08, "Identity Information",
			smp_ident_info, 16, true },
	{ 0x09, "Identity Address Information",
			smp_ident_addr_info, 7, true },
	{ 0x0a, "Signing Information",
			smp_signing_info, 16, true },
	{ 0x0b, "Security Request",
			smp_security_request, 1, true },
	{ }
};

static void smp_packet(uint16_t index, bool in, uint16_t handle,
			uint16_t cid, const void *data, uint16_t size)
{
	struct l2cap_frame frame;
	uint8_t opcode = *((const uint8_t *) data);
	const struct smp_opcode_data *opcode_data = NULL;
	const char *opcode_color, *opcode_str;
	int i;

	if (size < 1) {
		print_text(COLOR_ERROR, "malformed attribute packet");
		packet_hexdump(data, size);
		return;
	}

	for (i = 0; smp_opcode_table[i].str; i++) {
		if (smp_opcode_table[i].opcode == opcode) {
			opcode_data = &smp_opcode_table[i];
			break;
		}
	}

	if (opcode_data) {
		if (opcode_data->func) {
			if (in)
				opcode_color = COLOR_MAGENTA;
			else
				opcode_color = COLOR_BLUE;
		} else
			opcode_color = COLOR_WHITE_BG;
		opcode_str = opcode_data->str;
	} else {
		opcode_color = COLOR_WHITE_BG;
		opcode_str = "Unknown";
	}

	print_indent(6, opcode_color, "SMP: ", opcode_str, COLOR_OFF,
				" (0x%2.2x) len %d", opcode, size - 1);

	if (!opcode_data || !opcode_data->func) {
		packet_hexdump(data + 1, size - 1);
		return;
	}

	if (opcode_data->fixed) {
		if (size - 1 != opcode_data->size) {
			print_text(COLOR_ERROR, "invalid size");
			packet_hexdump(data + 1, size - 1);
			return;
		}
	} else {
		if (size - 1 < opcode_data->size) {
			print_text(COLOR_ERROR, "too short packet");
			packet_hexdump(data + 1, size - 1);
			return;
		}
	}

	l2cap_frame_init(&frame, index, in, handle, cid, data + 1, size - 1);
	opcode_data->func(&frame);
}

static void l2cap_frame(uint16_t index, bool in, uint16_t handle,
			uint16_t cid, const void *data, uint16_t size)
{
	struct l2cap_frame frame;
	uint16_t psm, chan;
	uint8_t mode;

	switch (cid) {
	case 0x0001:
		bredr_sig_packet(index, in, handle, cid, data, size);
		break;
	case 0x0002:
		connless_packet(index, in, handle, cid, data, size);
		break;
	case 0x0003:
		amp_packet(index, in, handle, cid, data, size);
		break;
	case 0x0004:
		att_packet(index, in, handle, cid, data, size);
		break;
	case 0x0005:
		le_sig_packet(index, in, handle, cid, data, size);
		break;
	case 0x0006:
		smp_packet(index, in, handle, cid, data, size);
		break;
	default:
		l2cap_frame_init(&frame, index, in, handle, cid, data, size);
		psm = get_psm(&frame);
		mode = get_mode(&frame);
		chan = get_chan(&frame);

		print_indent(6, COLOR_CYAN, "Channel:", "", COLOR_OFF,
				" %d len %d [PSM %d mode %d] {chan %d}",
						cid, size, psm, mode, chan);

		switch (psm) {
		case 0x0001:
			sdp_packet(&frame, chan);
			break;
		case 0x001f:
			att_packet(index, in, handle, cid, data, size);
			break;
		default:
			packet_hexdump(data, size);
			break;
		}
		break;
	}
}

void l2cap_packet(uint16_t index, bool in, uint16_t handle, uint8_t flags,
					const void *data, uint16_t size)
{
	const struct bt_l2cap_hdr *hdr = data;
	uint16_t len, cid;

	if (index > MAX_INDEX - 1) {
		print_text(COLOR_ERROR, "controller index too large");
		packet_hexdump(data, size);
		return;
	}

	switch (flags) {
	case 0x00:	/* start of a non-automatically-flushable PDU */
	case 0x02:	/* start of an automatically-flushable PDU */
		if (index_list[index].frag_len) {
			print_text(COLOR_ERROR, "unexpected start frame");
			packet_hexdump(data, size);
			clear_fragment_buffer(index);
			return;
		}

		if (size < sizeof(*hdr)) {
			print_text(COLOR_ERROR, "frame too short");
			packet_hexdump(data, size);
			return;
		}

		len = btohs(hdr->len);
		cid = btohs(hdr->cid);

		data += sizeof(*hdr);
		size -= sizeof(*hdr);

		if (len == size) {
			/* complete frame */
			l2cap_frame(index, in, handle, cid, data, len);
			return;
		}

		if (size > len) {
			print_text(COLOR_ERROR, "frame too long");
			packet_hexdump(data, size);
			return;
		}

		index_list[index].frag_buf = malloc(len);
		if (!index_list[index].frag_buf) {
			print_text(COLOR_ERROR, "failed buffer allocation");
			packet_hexdump(data, size);
			return;
		}

		memcpy(index_list[index].frag_buf, data, size);
		index_list[index].frag_pos = size;
		index_list[index].frag_len = len - size;
		index_list[index].frag_cid = cid;
		break;

	case 0x01:	/* continuing fragment */
		if (!index_list[index].frag_len) {
			print_text(COLOR_ERROR, "unexpected continuation");
			packet_hexdump(data, size);
			return;
		}

		if (size > index_list[index].frag_len) {
			print_text(COLOR_ERROR, "fragment too long");
			packet_hexdump(data, size);
			clear_fragment_buffer(index);
			return;
		}

		memcpy(index_list[index].frag_buf +
				index_list[index].frag_pos, data, size);
		index_list[index].frag_pos += size;
		index_list[index].frag_len -= size;

		if (!index_list[index].frag_len) {
			/* complete frame */
			l2cap_frame(index, in, handle,
					index_list[index].frag_cid,
					index_list[index].frag_buf,
					index_list[index].frag_pos);
			clear_fragment_buffer(index);
			return;
		}
		break;

	case 0x03:	/* complete automatically-flushable PDU */
		if (index_list[index].frag_len) {
			print_text(COLOR_ERROR, "unexpected complete frame");
			packet_hexdump(data, size);
			clear_fragment_buffer(index);
			return;
		}

		if (size < sizeof(*hdr)) {
			print_text(COLOR_ERROR, "frame too short");
			packet_hexdump(data, size);
			return;
		}

		len = btohs(hdr->len);
		cid = btohs(hdr->cid);

		data += sizeof(*hdr);
		size -= sizeof(*hdr);

		if (len != size) {
			print_text(COLOR_ERROR, "wrong frame size");
			packet_hexdump(data, size);
			return;
		}

		/* complete frame */
		l2cap_frame(index, in, handle, cid, data, len);
		break;

	default:
		print_text(COLOR_ERROR, "invalid packet flags (0x%2.2x)",
								flags);
		packet_hexdump(data, size);
		return;
	}
}