M7350/external/bluetooth/bluez/tools/parser/att.c

/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2011  André Dieb Martins <andre.dieb@gmail.com>
 *
 *
 *  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 <errno.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>

#include "parser.h"

#define GATT_PRIM_SVC_UUID		0x2800
#define GATT_SND_SVC_UUID		0x2801
#define GATT_INCLUDE_UUID		0x2802
#define GATT_CHARAC_UUID		0x2803

#define GATT_CHARAC_DEVICE_NAME			0x2A00
#define GATT_CHARAC_APPEARANCE			0x2A01
#define GATT_CHARAC_PERIPHERAL_PRIV_FLAG	0x2A02
#define GATT_CHARAC_RECONNECTION_ADDRESS	0x2A03
#define GATT_CHARAC_PERIPHERAL_PREF_CONN	0x2A04
#define GATT_CHARAC_SERVICE_CHANGED		0x2A05

#define GATT_CHARAC_EXT_PROPER_UUID	0x2900
#define GATT_CHARAC_USER_DESC_UUID	0x2901
#define GATT_CLIENT_CHARAC_CFG_UUID	0x2902
#define GATT_SERVER_CHARAC_CFG_UUID	0x2903
#define GATT_CHARAC_FMT_UUID		0x2904
#define GATT_CHARAC_AGREG_FMT_UUID	0x2905



/* Attribute Protocol Opcodes */
#define ATT_OP_ERROR			0x01
#define ATT_OP_MTU_REQ			0x02
#define ATT_OP_MTU_RESP			0x03
#define ATT_OP_FIND_INFO_REQ		0x04
#define ATT_OP_FIND_INFO_RESP		0x05
#define ATT_OP_FIND_BY_TYPE_REQ		0x06
#define ATT_OP_FIND_BY_TYPE_RESP	0x07
#define ATT_OP_READ_BY_TYPE_REQ		0x08
#define ATT_OP_READ_BY_TYPE_RESP	0x09
#define ATT_OP_READ_REQ			0x0A
#define ATT_OP_READ_RESP		0x0B
#define ATT_OP_READ_BLOB_REQ		0x0C
#define ATT_OP_READ_BLOB_RESP		0x0D
#define ATT_OP_READ_MULTI_REQ		0x0E
#define ATT_OP_READ_MULTI_RESP		0x0F
#define ATT_OP_READ_BY_GROUP_REQ	0x10
#define ATT_OP_READ_BY_GROUP_RESP	0x11
#define ATT_OP_WRITE_REQ		0x12
#define ATT_OP_WRITE_RESP		0x13
#define ATT_OP_WRITE_CMD		0x52
#define ATT_OP_PREP_WRITE_REQ		0x16
#define ATT_OP_PREP_WRITE_RESP		0x17
#define ATT_OP_EXEC_WRITE_REQ		0x18
#define ATT_OP_EXEC_WRITE_RESP		0x19
#define ATT_OP_HANDLE_NOTIFY		0x1B
#define ATT_OP_HANDLE_IND		0x1D
#define ATT_OP_HANDLE_CNF		0x1E
#define ATT_OP_SIGNED_WRITE_CMD		0xD2

/* Error codes for Error response PDU */
#define ATT_ECODE_INVALID_HANDLE		0x01
#define ATT_ECODE_READ_NOT_PERM			0x02
#define ATT_ECODE_WRITE_NOT_PERM		0x03
#define ATT_ECODE_INVALID_PDU			0x04
#define ATT_ECODE_INSUFF_AUTHEN			0x05
#define ATT_ECODE_REQ_NOT_SUPP			0x06
#define ATT_ECODE_INVALID_OFFSET		0x07
#define ATT_ECODE_INSUFF_AUTHO			0x08
#define ATT_ECODE_PREP_QUEUE_FULL		0x09
#define ATT_ECODE_ATTR_NOT_FOUND		0x0A
#define ATT_ECODE_ATTR_NOT_LONG			0x0B
#define ATT_ECODE_INSUFF_ENCR_KEY_SIZE		0x0C
#define ATT_ECODE_INVAL_ATTR_VALUE_LEN		0x0D
#define ATT_ECODE_UNLIKELY			0x0E
#define ATT_ECODE_INSUFF_ENC			0x0F
#define ATT_ECODE_UNSUPP_GRP_TYPE		0x10
#define ATT_ECODE_INSUFF_RESOURCES		0x11
#define ATT_ECODE_IO				0xFF


/* Attribute Protocol Opcodes */
static const char *attop2str(uint8_t op)
{
	switch (op) {
	case ATT_OP_ERROR:
		return "Error";
	case ATT_OP_MTU_REQ:
		return "MTU req";
	case ATT_OP_MTU_RESP:
		return "MTU resp";
	case ATT_OP_FIND_INFO_REQ:
		return "Find Information req";
	case ATT_OP_FIND_INFO_RESP:
		return "Find Information resp";
	case ATT_OP_FIND_BY_TYPE_REQ:
		return "Find By Type req";
	case ATT_OP_FIND_BY_TYPE_RESP:
		return "Find By Type resp";
	case ATT_OP_READ_BY_TYPE_REQ:
		return "Read By Type req";
	case ATT_OP_READ_BY_TYPE_RESP:
		return "Read By Type resp";
	case ATT_OP_READ_REQ:
		return "Read req";
	case ATT_OP_READ_RESP:
		return "Read resp";
	case ATT_OP_READ_BLOB_REQ:
		return "Read Blob req";
	case ATT_OP_READ_BLOB_RESP:
		return "Read Blob resp";
	case ATT_OP_READ_MULTI_REQ:
		return "Read Multi req";
	case ATT_OP_READ_MULTI_RESP:
		return "Read Multi resp";
	case ATT_OP_READ_BY_GROUP_REQ:
		return "Read By Group req";
	case ATT_OP_READ_BY_GROUP_RESP:
		return "Read By Group resp";
	case ATT_OP_WRITE_REQ:
		return "Write req";
	case ATT_OP_WRITE_RESP:
		return "Write resp";
	case ATT_OP_WRITE_CMD:
		return "Write cmd";
	case ATT_OP_PREP_WRITE_REQ:
		return "Prepare Write req";
	case ATT_OP_PREP_WRITE_RESP:
		return "Prepare Write resp";
	case ATT_OP_EXEC_WRITE_REQ:
		return "Exec Write req";
	case ATT_OP_EXEC_WRITE_RESP:
		return "Exec Write resp";
	case ATT_OP_HANDLE_NOTIFY:
		return "Handle notify";
	case ATT_OP_HANDLE_IND:
		return "Handle indicate";
	case ATT_OP_HANDLE_CNF:
		return "Handle CNF";
	case ATT_OP_SIGNED_WRITE_CMD:
		return "Signed Write Cmd";
	default:
		return "Unknown";
	}
}

static const char * atterror2str(uint8_t err)
{
	switch (err) {
	case ATT_ECODE_INVALID_HANDLE:
		return "Invalid handle";
	case ATT_ECODE_READ_NOT_PERM:
		return "Read not permitted";
	case ATT_ECODE_WRITE_NOT_PERM:
		return "Write not permitted";
	case ATT_ECODE_INVALID_PDU:
		return "Invalid PDU";
	case ATT_ECODE_INSUFF_AUTHEN:
		return "Insufficient authentication";
	case ATT_ECODE_REQ_NOT_SUPP:
		return "Request not supported";
	case ATT_ECODE_INVALID_OFFSET:
		return "Invalid offset";
	case ATT_ECODE_INSUFF_AUTHO:
		return "Insufficient authorization";
	case ATT_ECODE_PREP_QUEUE_FULL:
		return "Prepare queue full";
	case ATT_ECODE_ATTR_NOT_FOUND:
		return "Attribute not found";
	case ATT_ECODE_ATTR_NOT_LONG:
		return "Attribute not long";
	case ATT_ECODE_INSUFF_ENCR_KEY_SIZE:
		return "Insufficient encryption key size";
	case ATT_ECODE_INVAL_ATTR_VALUE_LEN:
		return "Invalid attribute value length";
	case ATT_ECODE_UNLIKELY:
		return "Unlikely error";
	case ATT_ECODE_INSUFF_ENC:
		return "Insufficient encryption";
	case ATT_ECODE_UNSUPP_GRP_TYPE:
		return "Unsupported group type";
	case ATT_ECODE_INSUFF_RESOURCES:
		return "Insufficient resources";
	case ATT_ECODE_IO:
		return "Application Error";
	default:
		return "Reserved";
	}
}

static const char *uuid2str(uint16_t uuid)
{
	switch (uuid) {
	case GATT_PRIM_SVC_UUID:
		return "GATT Primary Service";
	case GATT_SND_SVC_UUID:
		return "GATT Secondary Service";
	case GATT_INCLUDE_UUID:
		return "GATT Include";
	case GATT_CHARAC_UUID:
		return "GATT Characteristic";
	case GATT_CHARAC_DEVICE_NAME:
		return "GATT(type) Device Name";
	case GATT_CHARAC_APPEARANCE:
		return "GATT(type) Appearance";
	case GATT_CHARAC_PERIPHERAL_PRIV_FLAG:
		return "GATT(type) Peripheral Privacy Flag";
	case GATT_CHARAC_RECONNECTION_ADDRESS:
		return "GATT(type) Characteristic Reconnection Address";
	case GATT_CHARAC_PERIPHERAL_PREF_CONN:
		return "GATT(type) Characteristic Preferred Connection Parameters";
	case GATT_CHARAC_SERVICE_CHANGED:
		return "GATT(type) Characteristic Service Changed";
	case GATT_CHARAC_EXT_PROPER_UUID:
		return "GATT(desc) Characteristic Extended Properties";
	case GATT_CHARAC_USER_DESC_UUID:
		return "GATT(desc) User Description";
	case GATT_CLIENT_CHARAC_CFG_UUID:
		return "GATT(desc) Client Characteristic Configuration";
	case GATT_SERVER_CHARAC_CFG_UUID:
		return "GATT(desc) Server Characteristic Configuration";
	case GATT_CHARAC_FMT_UUID:
		return "GATT(desc) Format";
	case GATT_CHARAC_AGREG_FMT_UUID:
		return "GATT(desc) Aggregate Format";
	default:
		return "Unknown";
	}
}

static void att_error_dump(int level, struct frame *frm)
{
	uint8_t op = get_u8(frm);
	uint16_t handle = btohs(htons(get_u16(frm)));
	uint8_t err = get_u8(frm);

	p_indent(level, frm);
	printf("Error: %s (%d)\n", atterror2str(err), err);

	p_indent(level, frm);
	printf("%s (0x%.2x) on handle 0x%4.4x\n", attop2str(op), op, handle);
}

static void att_mtu_req_dump(int level, struct frame *frm)
{
	uint16_t client_rx_mtu = btohs(htons(get_u16(frm)));

	p_indent(level, frm);
	printf("client rx mtu %d\n", client_rx_mtu);
}

static void att_mtu_resp_dump(int level, struct frame *frm)
{
	uint16_t server_rx_mtu = btohs(htons(get_u16(frm)));

	p_indent(level, frm);
	printf("server rx mtu %d\n", server_rx_mtu);
}

static void att_find_info_req_dump(int level, struct frame *frm)
{
	uint16_t start = btohs(htons(get_u16(frm)));
	uint16_t end = btohs(htons(get_u16(frm)));

	p_indent(level, frm);
	printf("start 0x%4.4x, end 0x%4.4x\n", start, end);
}

static void print_uuid128(struct frame *frm)
{
	uint8_t uuid[16];
	int i;

	for (i = 0; i < 16; i++)
		uuid[15 - i] = get_u8(frm);

	for (i = 0; i < 16; i++) {
		printf("%02x", uuid[i]);
		if (i == 3 || i == 5 || i == 7 || i == 9)
			printf("-");
	}
}

static void att_find_info_resp_dump(int level, struct frame *frm)
{
	uint8_t fmt = get_u8(frm);

	p_indent(level, frm);

	if (fmt == 0x01) {
		printf("format: uuid-16\n");

		while (frm->len > 0) {
			uint16_t handle = btohs(htons(get_u16(frm)));
			uint16_t uuid = btohs(htons(get_u16(frm)));
			p_indent(level + 1, frm);
			printf("handle 0x%4.4x, uuid 0x%4.4x (%s)\n", handle, uuid,
					uuid2str(uuid));
		}
	} else {
		printf("format: uuid-128\n");

		while (frm->len > 0) {
			uint16_t handle = btohs(htons(get_u16(frm)));

			p_indent(level + 1, frm);
			printf("handle 0x%4.4x, uuid ", handle);
			print_uuid128(frm);
			printf("\n");
		}
	}
}

static void att_find_by_type_req_dump(int level, struct frame *frm)
{
	uint16_t start = btohs(htons(get_u16(frm)));
	uint16_t end = btohs(htons(get_u16(frm)));
	uint16_t uuid = btohs(htons(get_u16(frm)));

	p_indent(level, frm);
	printf("start 0x%4.4x, end 0x%4.4x, uuid 0x%4.4x\n", start, end, uuid);

	p_indent(level, frm);
	printf("value");
	while (frm->len > 0)
		printf(" 0x%2.2x", get_u8(frm));
	printf("\n");
}

static void att_find_by_type_resp_dump(int level, struct frame *frm)
{
	while (frm->len > 0) {
		uint16_t uuid = btohs(htons(get_u16(frm)));
		uint16_t end = btohs(htons(get_u16(frm)));

		p_indent(level, frm);
		printf("Found attr 0x%4.4x, group end handle 0x%4.4x\n",
								uuid, end);
	}
}

static void att_read_by_type_req_dump(int level, struct frame *frm)
{
	uint16_t start = btohs(htons(get_u16(frm)));
	uint16_t end = btohs(htons(get_u16(frm)));

	p_indent(level, frm);
	printf("start 0x%4.4x, end 0x%4.4x\n", start, end);

	p_indent(level, frm);
	if (frm->len == 2) {
		printf("type-uuid 0x%4.4x\n", btohs(htons(get_u16(frm))));
	} else if (frm->len == 16) {
		printf("type-uuid ");
		print_uuid128(frm);
		printf("\n");
	} else {
		printf("malformed uuid (expected 2 or 16 octets)\n");
		p_indent(level, frm);
		raw_dump(level, frm);
	}
}

static void att_read_by_type_resp_dump(int level, struct frame *frm)
{
	uint8_t length = get_u8(frm);

	p_indent(level, frm);
	printf("length: %d\n", length);

	while (frm->len > 0) {
		uint16_t handle = btohs(htons(get_u16(frm)));
		int val_len = length - 2;
		int i;

		p_indent(level + 1, frm);
		printf("handle 0x%4.4x, value ", handle);
		for (i = 0; i < val_len; i++) {
			printf("0x%.2x ", get_u8(frm));
		}
		printf("\n");
	}
}

static void att_read_req_dump(int level, struct frame *frm)
{
	uint16_t handle = btohs(htons(get_u16(frm)));

	p_indent(level, frm);
	printf("handle 0x%4.4x\n", handle);
}

static void att_read_blob_req_dump(int level, struct frame *frm)
{
	uint16_t handle = btohs(htons(get_u16(frm)));
	uint16_t offset = btohs(htons(get_u16(frm)));

	p_indent(level, frm);
	printf("handle 0x%4.4x offset 0x%4.4x\n", handle, offset);
}

static void att_read_blob_resp_dump(int level, struct frame *frm)
{
	p_indent(level, frm);
	printf("value");

	while (frm->len > 0)
		printf(" 0x%2.2x", get_u8(frm));
	printf("\n");
}

static void att_read_multi_req_dump(int level, struct frame *frm)
{
	p_indent(level, frm);
	printf("Handles\n");

	while (frm->len > 0) {
		p_indent(level, frm);
		printf("handle 0x%4.4x\n", btohs(htons(get_u16(frm))));
	}
}

static void att_read_multi_resp_dump(int level, struct frame *frm)
{
	p_indent(level, frm);
	printf("values");

	while (frm->len > 0)
		printf(" 0x%2.2x", get_u8(frm));
	printf("\n");
}

static void att_read_by_group_resp_dump(int level, struct frame *frm)
{
	uint8_t length = get_u8(frm);

	while (frm->len > 0) {
		uint16_t attr_handle = btohs(htons(get_u16(frm)));
		uint16_t end_grp_handle = btohs(htons(get_u16(frm)));
		uint8_t remaining = length - 4;

		p_indent(level, frm);
		printf("attr handle 0x%4.4x, end group handle 0x%4.4x\n",
						attr_handle, end_grp_handle);

		p_indent(level, frm);
		printf("value");
		while (remaining > 0) {
			printf(" 0x%2.2x", get_u8(frm));
			remaining--;
		}
		printf("\n");
	}
}

static void att_write_req_dump(int level, struct frame *frm)
{
	uint16_t handle = btohs(htons(get_u16(frm)));

	p_indent(level, frm);
	printf("handle 0x%4.4x value ", handle);

	while (frm->len > 0)
		printf(" 0x%2.2x", get_u8(frm));
	printf("\n");
}

static void att_signed_write_dump(int level, struct frame *frm)
{
	uint16_t handle = btohs(htons(get_u16(frm)));
	int value_len = frm->len - 12; /* handle:2 already accounted, sig: 12 */

	p_indent(level, frm);
	printf("handle 0x%4.4x value ", handle);

	while (value_len--)
		printf(" 0x%2.2x", get_u8(frm));
	printf("\n");

	p_indent(level, frm);
	printf("auth signature ");
	while (frm->len > 0)
		printf(" 0x%2.2x", get_u8(frm));
	printf("\n");
}

static void att_prep_write_dump(int level, struct frame *frm)
{
	uint16_t handle = btohs(htons(get_u16(frm)));
	uint16_t val_offset = btohs(htons(get_u16(frm)));

	p_indent(level, frm);
	printf("attr handle 0x%4.4x, value offset 0x%4.4x\n", handle,
								val_offset);

	p_indent(level, frm);
	printf("part attr value ");
	while (frm->len > 0)
		printf(" 0x%2.2x", get_u8(frm));
	printf("\n");
}

static void att_exec_write_req_dump(int level, struct frame *frm)
{
	uint8_t flags = get_u8(frm);

	p_indent(level, frm);
	if (flags == 0x00)
		printf("cancel all prepared writes ");
	else
		printf("immediatelly write all pending prepared values ");

	printf("(0x%2.2x)\n", flags);
}

static void att_handle_notify_dump(int level, struct frame *frm)
{
	uint16_t handle = btohs(htons(get_u16(frm)));

	p_indent(level, frm);
	printf("handle 0x%4.4x\n", handle);

	p_indent(level, frm);
	printf("value ");
	while (frm->len > 0)
		printf("0x%.2x ", get_u8(frm));
	printf("\n");
}

void att_dump(int level, struct frame *frm)
{
	uint8_t op;

	op = get_u8(frm);

	p_indent(level, frm);
	printf("ATT: %s (0x%.2x)\n", attop2str(op), op);

	switch (op) {
		case ATT_OP_ERROR:
			att_error_dump(level + 1, frm);
			break;
		case ATT_OP_MTU_REQ:
			att_mtu_req_dump(level + 1, frm);
			break;
		case ATT_OP_MTU_RESP:
			att_mtu_resp_dump(level + 1, frm);
			break;
		case ATT_OP_FIND_INFO_REQ:
			att_find_info_req_dump(level + 1, frm);
			break;
		case ATT_OP_FIND_INFO_RESP:
			att_find_info_resp_dump(level + 1, frm);
			break;
		case ATT_OP_FIND_BY_TYPE_REQ:
			att_find_by_type_req_dump(level + 1, frm);
			break;
		case ATT_OP_FIND_BY_TYPE_RESP:
			att_find_by_type_resp_dump(level + 1, frm);
			break;
		case ATT_OP_READ_BY_TYPE_REQ:
		case ATT_OP_READ_BY_GROUP_REQ: /* exact same parsing */
			att_read_by_type_req_dump(level + 1, frm);
			break;
		case ATT_OP_READ_BY_TYPE_RESP:
			att_read_by_type_resp_dump(level + 1, frm);
			break;
		case ATT_OP_READ_REQ:
			att_read_req_dump(level + 1, frm);
			break;
		case ATT_OP_READ_RESP:
			raw_dump(level + 1, frm);
			break;
		case ATT_OP_READ_BLOB_REQ:
			att_read_blob_req_dump(level + 1, frm);
			break;
		case ATT_OP_READ_BLOB_RESP:
			att_read_blob_resp_dump(level + 1, frm);
			break;
		case ATT_OP_READ_MULTI_REQ:
			att_read_multi_req_dump(level + 1, frm);
			break;
		case ATT_OP_READ_MULTI_RESP:
			att_read_multi_resp_dump(level + 1, frm);
			break;
		case ATT_OP_READ_BY_GROUP_RESP:
			att_read_by_group_resp_dump(level + 1, frm);
			break;
		case ATT_OP_WRITE_REQ:
		case ATT_OP_WRITE_CMD:
			att_write_req_dump(level + 1, frm);
			break;
		case ATT_OP_SIGNED_WRITE_CMD:
			att_signed_write_dump(level + 1, frm);
			break;
		case ATT_OP_PREP_WRITE_REQ:
		case ATT_OP_PREP_WRITE_RESP:
			att_prep_write_dump(level + 1, frm);
			break;
		case ATT_OP_EXEC_WRITE_REQ:
			att_exec_write_req_dump(level + 1, frm);
			break;
		case ATT_OP_HANDLE_NOTIFY:
			att_handle_notify_dump(level + 1, frm);
			break;
		default:
			raw_dump(level, frm);
			break;
	}
}