/*
*
* 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 <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 "sdp.h"
#define MAX_TID 16
#define MAX_CONT_SIZE 17
struct tid_data {
bool inuse;
uint16_t tid;
uint16_t channel;
uint8_t cont[MAX_CONT_SIZE];
};
static struct tid_data tid_list[MAX_TID];
static struct tid_data *get_tid(uint16_t tid, uint16_t channel)
{
int i, n = -1;
for (i = 0; i < MAX_TID; i++) {
if (!tid_list[i].inuse) {
if (n < 0)
n = i;
continue;
}
if (tid_list[i].tid == tid && tid_list[i].channel == channel)
return &tid_list[i];
}
if (n < 0)
return NULL;
tid_list[n].inuse = true;
tid_list[n].tid = tid;
tid_list[n].channel = channel;
return &tid_list[n];
}
static void clear_tid(struct tid_data *tid)
{
if (tid)
tid->inuse = false;
}
static void print_uint(uint8_t indent, const uint8_t *data, uint32_t size)
{
switch (size) {
case 1:
print_field("%*c0x%2.2x", indent, ' ', data[0]);
break;
case 2:
print_field("%*c0x%4.4x", indent, ' ', get_be16(data));
break;
case 4:
print_field("%*c0x%8.8x", indent, ' ', get_be32(data));
break;
case 8:
print_field("%*c0x%16.16" PRIx64, indent, ' ', get_be64(data));
break;
default:
packet_hexdump(data, size);
break;
}
}
static void print_sint(uint8_t indent, const uint8_t *data, uint32_t size)
{
packet_hexdump(data, size);
}
static void print_uuid(uint8_t indent, const uint8_t *data, uint32_t size)
{
switch (size) {
case 2:
print_field("%*c%s (0x%4.4x)", indent, ' ',
bt_uuid16_to_str(get_be16(data)), get_be16(data));
break;
case 4:
print_field("%*c%s (0x%8.8x)", indent, ' ',
bt_uuid32_to_str(get_be32(data)), get_be32(data));
break;
case 16:
/* BASE_UUID = 00000000-0000-1000-8000-00805F9B34FB */
print_field("%*c%8.8x-%4.4x-%4.4x-%4.4x-%4.4x%8.4x",
indent, ' ',
get_be32(data), get_be16(data + 4),
get_be16(data + 6), get_be16(data + 8),
get_be16(data + 10), get_be32(data + 12));
if (get_be16(data + 4) == 0x0000 &&
get_be16(data + 6) == 0x1000 &&
get_be16(data + 8) == 0x8000 &&
get_be16(data + 10) == 0x0080 &&
get_be32(data + 12) == 0x5F9B34FB)
print_field("%*c%s", indent, ' ',
bt_uuid32_to_str(get_be32(data)));
break;
default:
packet_hexdump(data, size);
break;
}
}
static void print_string(uint8_t indent, const uint8_t *data, uint32_t size)
{
char *str = alloca(size + 1);
str[size] = '\0';
strncpy(str, (const char *) data, size);
print_field("%*c%s [len %d]", indent, ' ', str, size);
}
static void print_boolean(uint8_t indent, const uint8_t *data, uint32_t size)
{
print_field("%*c%s", indent, ' ', data[0] ? "true" : "false");
}
#define SIZES(args...) ((uint8_t[]) { args, 0xff } )
static struct {
uint8_t value;
uint8_t *sizes;
bool recurse;
const char *str;
void (*print) (uint8_t indent, const uint8_t *data, uint32_t size);
} type_table[] = {
{ 0, SIZES(0), false, "Nil" },
{ 1, SIZES(0, 1, 2, 3, 4), false, "Unsigned Integer", print_uint },
{ 2, SIZES(0, 1, 2, 3, 4), false, "Signed Integer", print_sint },
{ 3, SIZES(1, 2, 4), false, "UUID", print_uuid },
{ 4, SIZES(5, 6, 7), false, "String", print_string },
{ 5, SIZES(0), false, "Boolean", print_boolean },
{ 6, SIZES(5, 6, 7), true, "Sequence" },
{ 7, SIZES(5, 6, 7), true, "Alternative" },
{ 8, SIZES(5, 6, 7), false, "URL", print_string },
{ }
};
static struct {
uint8_t index;
uint8_t bits;
uint8_t size;
const char *str;
} size_table[] = {
{ 0, 0, 1, "1 byte" },
{ 1, 0, 2, "2 bytes" },
{ 2, 0, 4, "4 bytes" },
{ 3, 0, 8, "8 bytes" },
{ 4, 0, 16, "16 bytes" },
{ 5, 8, 0, "8 bits" },
{ 6, 16, 0, "16 bits" },
{ 7, 32, 0, "32 bits" },
{ }
};
static bool valid_size(uint8_t size, uint8_t *sizes)
{
int i;
for (i = 0; sizes[i] != 0xff; i++) {
if (sizes[i] == size)
return true;
}
return false;
}
static uint8_t get_bits(const uint8_t *data, uint32_t size)
{
int i;
for (i = 0; size_table[i].str; i++) {
if (size_table[i].index == (data[0] & 0x07))
return size_table[i].bits;
}
return 0;
}
static uint32_t get_size(const uint8_t *data, uint32_t size)
{
int i;
for (i = 0; size_table[i].str; i++) {
if (size_table[i].index == (data[0] & 0x07)) {
switch (size_table[i].bits) {
case 0:
if ((data[0] & 0xf8) == 0)
return 0;
else
return size_table[i].size;
case 8:
return data[1];
case 16:
return get_be16(data + 1);
case 32:
return get_be32(data + 1);
default:
return 0;
}
}
}
return 0;
}
static void decode_data_elements(uint32_t position, uint8_t indent,
const uint8_t *data, uint32_t size,
void (*print_func) (uint32_t, uint8_t, uint8_t,
const uint8_t *, uint32_t))
{
uint32_t datalen, elemlen, extrabits;
int i;
if (!size)
return;
extrabits = get_bits(data, size);
if (size < 1 + (extrabits / 8)) {
print_text(COLOR_ERROR, "data element descriptor too short");
packet_hexdump(data, size);
return;
}
datalen = get_size(data, size);
if (size < 1 + (extrabits / 8) + datalen) {
print_text(COLOR_ERROR, "data element size too short");
packet_hexdump(data, size);
return;
}
elemlen = 1 + (extrabits / 8) + datalen;
for (i = 0; type_table[i].str; i++) {
uint8_t type = (data[0] & 0xf8) >> 3;
if (type_table[i].value != type)
continue;
if (print_func) {
print_func(position, indent, type,
data + 1 + (extrabits / 8), datalen);
break;
}
print_field("%*c%s (%d) with %u byte%s [%u extra bits] len %u",
indent, ' ', type_table[i].str, type,
datalen, datalen == 1 ? "" : "s",
extrabits, elemlen);
if (!valid_size(data[0] & 0x07, type_table[i].sizes)) {
print_text(COLOR_ERROR, "invalid data element size");
packet_hexdump(data + 1 + (extrabits / 8), datalen);
break;
}
if (type_table[i].recurse)
decode_data_elements(0, indent + 2,
data + 1 + (extrabits / 8), datalen,
print_func);
else if (type_table[i].print)
type_table[i].print(indent + 2,
data + 1 + (extrabits / 8), datalen);
break;
}
if (elemlen > size) {
print_text(COLOR_ERROR, "invalid data element size");
return;
}
data += elemlen;
size -= elemlen;
decode_data_elements(position + 1, indent, data, size, print_func);
}
static uint32_t get_bytes(const uint8_t *data, uint32_t size)
{
switch (data[0] & 0x07) {
case 5:
return 2 + data[1];
case 6:
return 3 + get_be16(data + 1);
case 7:
return 5 + get_be32(data + 1);
}
return 0;
}
static struct {
uint16_t id;
const char *str;
} attribute_table[] = {
{ 0x0000, "Service Record Handle" },
{ 0x0001, "Service Class ID List" },
{ 0x0002, "Service Record State" },
{ 0x0003, "Service ID" },
{ 0x0004, "Protocol Descriptor List" },
{ 0x0005, "Browse Group List" },
{ 0x0006, "Language Base Attribute ID List" },
{ 0x0007, "Service Info Time To Live" },
{ 0x0008, "Service Availability" },
{ 0x0009, "Bluetooth Profile Descriptor List" },
{ 0x000a, "Documentation URL" },
{ 0x000b, "Client Executable URL" },
{ 0x000c, "Icon URL" },
{ 0x000d, "Additional Protocol Descriptor List" },
{ }
};
static void print_attr(uint32_t position, uint8_t indent, uint8_t type,
const uint8_t *data, uint32_t size)
{
int i;
if ((position % 2) == 0) {
uint16_t id = get_be16(data);
const char *str = "Unknown";
for (i = 0; attribute_table[i].str; i++) {
if (attribute_table[i].id == id)
str = attribute_table[i].str;
}
print_field("%*cAttribute: %s (0x%4.4x) [len %d]",
indent, ' ', str, id, size);
return;
}
for (i = 0; type_table[i].str; i++) {
if (type_table[i].value != type)
continue;
if (type_table[i].recurse)
decode_data_elements(0, indent + 2, data, size, NULL);
else if (type_table[i].print)
type_table[i].print(indent + 2, data, size);
break;
}
}
static void print_attr_list(uint32_t position, uint8_t indent, uint8_t type,
const uint8_t *data, uint32_t size)
{
print_field("%*cAttribute list: [len %d] {position %d}",
indent, ' ', size, position);
decode_data_elements(0, indent + 2, data, size, print_attr);
}
static void print_attr_lists(uint32_t position, uint8_t indent, uint8_t type,
const uint8_t *data, uint32_t size)
{
decode_data_elements(0, indent, data, size, print_attr_list);
}
static void print_continuation(const uint8_t *data, uint16_t size)
{
if (data[0] != size - 1) {
print_text(COLOR_ERROR, "invalid continuation state");
packet_hexdump(data, size);
return;
}
print_field("Continuation state: %d", data[0]);
packet_hexdump(data + 1, size - 1);
}
static void store_continuation(struct tid_data *tid,
const uint8_t *data, uint16_t size)
{
if (size > MAX_CONT_SIZE) {
print_text(COLOR_ERROR, "invalid continuation size");
return;
}
memcpy(tid->cont, data, size);
print_continuation(data, size);
}
#define MAX_CONT 8
struct cont_data {
uint16_t channel;
uint8_t cont[17];
void *data;
uint32_t size;
};
static struct cont_data cont_list[MAX_CONT];
static void handle_continuation(struct tid_data *tid, bool nested,
uint16_t bytes, const uint8_t *data, uint16_t size)
{
uint8_t *newdata;
int i, n = -1;
if (bytes + 1 > size) {
print_text(COLOR_ERROR, "missing continuation state");
return;
}
if (tid->cont[0] == 0x00 && data[bytes] == 0x00) {
decode_data_elements(0, 2, data, bytes,
nested ? print_attr_lists : print_attr_list);
print_continuation(data + bytes, size - bytes);
return;
}
for (i = 0; i < MAX_CONT; i++) {
if (cont_list[i].cont[0] == 0x00) {
if (n < 0)
n = i;
continue;
}
if (cont_list[i].channel != tid->channel)
continue;
if (cont_list[i].cont[0] != tid->cont[0])
continue;
if (!memcmp(cont_list[i].cont + 1,
tid->cont + 1, tid->cont[0])) {
n = i;
break;
}
}
print_continuation(data + bytes, size - bytes);
if (n < 0)
return;
newdata = realloc(cont_list[n].data, cont_list[n].size + bytes);
if (!newdata) {
print_text(COLOR_ERROR, "failed buffer allocation");
free(cont_list[n].data);
cont_list[n].data = NULL;
cont_list[n].size = 0;
return;
}
cont_list[n].channel = tid->channel;
cont_list[n].data = newdata;
if (bytes > 0) {
memcpy(cont_list[n].data + cont_list[n].size, data, bytes);
cont_list[n].size += bytes;
}
if (data[bytes] == 0x00) {
print_field("Combined attribute bytes: %d", cont_list[n].size);
decode_data_elements(0, 2, cont_list[n].data, cont_list[n].size,
nested ? print_attr_lists : print_attr_list);
free(cont_list[n].data);
cont_list[n].data = NULL;
cont_list[n].size = 0;
} else
memcpy(cont_list[i].cont, data + bytes, data[bytes] + 1);
}
static uint16_t common_rsp(const struct l2cap_frame *frame,
struct tid_data *tid)
{
uint16_t bytes;
if (frame->size < 2) {
print_text(COLOR_ERROR, "invalid size");
packet_hexdump(frame->data, frame->size);
return 0;
}
bytes = get_be16(frame->data);
print_field("Attribute bytes: %d", bytes);
if (bytes > frame->size - 2) {
print_text(COLOR_ERROR, "invalid attribute size");
packet_hexdump(frame->data + 2, frame->size - 2);
return 0;
}
return bytes;
}
static const char *error_str(uint16_t code)
{
switch (code) {
case 0x0001:
return "Invalid Version";
case 0x0002:
return "Invalid Record Handle";
case 0x0003:
return "Invalid Syntax";
case 0x0004:
return "Invalid PDU Size";
case 0x0005:
return "Invalid Continuation State";
default:
return "Unknown";
}
}
static void error_rsp(const struct l2cap_frame *frame, struct tid_data *tid)
{
uint16_t error;
clear_tid(tid);
if (frame->size < 2) {
print_text(COLOR_ERROR, "invalid size");
packet_hexdump(frame->data, frame->size);
return;
}
error = get_be16(frame->data);
print_field("Error code: %s (0x%4.4x)", error_str(error), error);
}
static void service_req(const struct l2cap_frame *frame, struct tid_data *tid)
{
uint32_t search_bytes;
search_bytes = get_bytes(frame->data, frame->size);
print_field("Search pattern: [len %d]", search_bytes);
if (search_bytes + 2 > frame->size) {
print_text(COLOR_ERROR, "invalid search list length");
packet_hexdump(frame->data, frame->size);
return;
}
decode_data_elements(0, 2, frame->data, search_bytes, NULL);
print_field("Max record count: %d",
get_be16(frame->data + search_bytes));
print_continuation(frame->data + search_bytes + 2,
frame->size - search_bytes - 2);
}
static void service_rsp(const struct l2cap_frame *frame, struct tid_data *tid)
{
uint16_t count;
int i;
clear_tid(tid);
if (frame->size < 4) {
print_text(COLOR_ERROR, "invalid size");
packet_hexdump(frame->data, frame->size);
return;
}
count = get_be16(frame->data + 2);
if (count * 4 > frame->size) {
print_text(COLOR_ERROR, "invalid record count");
return;
}
print_field("Total record count: %d", get_be16(frame->data));
print_field("Current record count: %d", count);
for (i = 0; i < count; i++)
print_field("Record handle: 0x%4.4x",
get_be32(frame->data + 4 + (i * 4)));
print_continuation(frame->data + 4 + (count * 4),
frame->size - 4 - (count * 4));
}
static void attr_req(const struct l2cap_frame *frame, struct tid_data *tid)
{
uint32_t attr_bytes;
if (frame->size < 6) {
print_text(COLOR_ERROR, "invalid size");
packet_hexdump(frame->data, frame->size);
return;
}
print_field("Record handle: 0x%4.4x", get_be32(frame->data));
print_field("Max attribute bytes: %d", get_be16(frame->data + 4));
attr_bytes = get_bytes(frame->data + 6, frame->size - 6);
print_field("Attribute list: [len %d]", attr_bytes);
if (attr_bytes + 6 > frame->size) {
print_text(COLOR_ERROR, "invalid attribute list length");
packet_hexdump(frame->data, frame->size);
return;
}
decode_data_elements(0, 2, frame->data + 6, attr_bytes, NULL);
store_continuation(tid, frame->data + 6 + attr_bytes,
frame->size - 6 - attr_bytes);
}
static void attr_rsp(const struct l2cap_frame *frame, struct tid_data *tid)
{
uint16_t bytes;
bytes = common_rsp(frame, tid);
handle_continuation(tid, false, bytes,
frame->data + 2, frame->size - 2);
clear_tid(tid);
}
static void search_attr_req(const struct l2cap_frame *frame,
struct tid_data *tid)
{
uint32_t search_bytes, attr_bytes;
search_bytes = get_bytes(frame->data, frame->size);
print_field("Search pattern: [len %d]", search_bytes);
if (search_bytes + 2 > frame->size) {
print_text(COLOR_ERROR, "invalid search list length");
packet_hexdump(frame->data, frame->size);
return;
}
decode_data_elements(0, 2, frame->data, search_bytes, NULL);
print_field("Max record count: %d",
get_be16(frame->data + search_bytes));
attr_bytes = get_bytes(frame->data + search_bytes + 2,
frame->size - search_bytes - 2);
print_field("Attribute list: [len %d]", attr_bytes);
if (search_bytes + attr_bytes > frame->size) {
print_text(COLOR_ERROR, "invalid attribute list length");
return;
}
decode_data_elements(0, 2, frame->data + search_bytes + 2,
attr_bytes, NULL);
store_continuation(tid, frame->data + search_bytes + 2 + attr_bytes,
frame->size - search_bytes - 2 - attr_bytes);
}
static void search_attr_rsp(const struct l2cap_frame *frame,
struct tid_data *tid)
{
uint16_t bytes;
bytes = common_rsp(frame, tid);
handle_continuation(tid, true, bytes, frame->data + 2, frame->size - 2);
clear_tid(tid);
}
struct sdp_data {
uint8_t pdu;
const char *str;
void (*func) (const struct l2cap_frame *frame, struct tid_data *tid);
};
static const struct sdp_data sdp_table[] = {
{ 0x01, "Error Response", error_rsp },
{ 0x02, "Service Search Request", service_req },
{ 0x03, "Service Search Response", service_rsp },
{ 0x04, "Service Attribute Request", attr_req },
{ 0x05, "Service Attribute Response", attr_rsp },
{ 0x06, "Service Search Attribute Request", search_attr_req },
{ 0x07, "Service Search Attribute Response", search_attr_rsp },
{ }
};
void sdp_packet(const struct l2cap_frame *frame)
{
uint8_t pdu;
uint16_t tid, plen;
struct l2cap_frame sdp_frame;
struct tid_data *tid_info;
const struct sdp_data *sdp_data = NULL;
const char *pdu_color, *pdu_str;
int i;
l2cap_frame_pull(&sdp_frame, frame, 0);
if (!l2cap_frame_get_u8(&sdp_frame, &pdu) ||
!l2cap_frame_get_be16(&sdp_frame, &tid) ||
!l2cap_frame_get_be16(&sdp_frame, &plen)) {
print_text(COLOR_ERROR, "frame too short");
packet_hexdump(frame->data, frame->size);
return;
}
if (sdp_frame.size != plen) {
print_text(COLOR_ERROR, "invalid frame size");
packet_hexdump(sdp_frame.data, sdp_frame.size);
return;
}
for (i = 0; sdp_table[i].str; i++) {
if (sdp_table[i].pdu == pdu) {
sdp_data = &sdp_table[i];
break;
}
}
if (sdp_data) {
if (sdp_data->func) {
if (frame->in)
pdu_color = COLOR_MAGENTA;
else
pdu_color = COLOR_BLUE;
} else
pdu_color = COLOR_WHITE_BG;
pdu_str = sdp_data->str;
} else {
pdu_color = COLOR_WHITE_BG;
pdu_str = "Unknown";
}
print_indent(6, pdu_color, "SDP: ", pdu_str, COLOR_OFF,
" (0x%2.2x) tid %d len %d", pdu, tid, plen);
tid_info = get_tid(tid, frame->chan);
if (!sdp_data || !sdp_data->func || !tid_info) {
packet_hexdump(sdp_frame.data, sdp_frame.size);
return;
}
sdp_data->func(&sdp_frame, tid_info);
}