bluez5_utils-5.62/monitor/bnep.c

// SPDX-License-Identifier: LGPL-2.1-or-later
/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2011-2014  Intel Corporation
 *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
 *
 *
 */

#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);
}