diff --git a/monitor/l2cap.c b/monitor/l2cap.c
index 87212af..e982bdd 100644
--- a/monitor/l2cap.c
+++ b/monitor/l2cap.c
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 {
uint16_t cid, const void *data, uint16_t size)
{
struct l2cap_frame frame;
- uint16_t psm;
+ uint16_t psm, chan;
uint8_t mode;
switch (cid) {
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]",
- cid, size, psm, mode);
+ " %d len %d [PSM %d mode %d] {chan %d}",
+ cid, size, psm, mode, chan);
switch (psm) {
case 0x0001:
- sdp_packet(&frame);
+ sdp_packet(&frame, chan);
break;
default:
packet_hexdump(data, size);
diff --git a/monitor/sdp.c b/monitor/sdp.c
index 805e37e..55991e6 100644
--- a/monitor/sdp.c
+++ b/monitor/sdp.c
#define _GNU_SOURCE
#include <stdio.h>
+#include <stdlib.h>
#include <string.h>
+#include <inttypes.h>
#include <bluetooth/bluetooth.h>
#include "uuid.h"
#include "sdp.h"
-#define SIZES(args...) ((uint8_t[]) { args, 0xff } )
+#define MAX_TID 16
+
+struct tid_data {
+ bool inuse;
+ uint16_t tid;
+ uint16_t channel;
+ uint8_t cont[17];
+};
+
+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)
{
case 4:
print_field("%*c0x%8.8x", indent, ' ', bt_get_be32(data));
break;
+ case 8:
+ print_field("%*c0x%16.16" PRIx64, indent, ' ',
+ bt_get_be64(data));
+ break;
default:
packet_hexdump(data, size);
break;
print_field("%*c%s", indent, ' ', data[0] ? "true" : "false");
}
+#define SIZES(args...) ((uint8_t[]) { args, 0xff } )
+
static struct {
uint8_t value;
uint8_t *sizes;
decode_data_elements(indent, data, size);
}
+static uint16_t get_bytes(const uint8_t *data, uint16_t size)
+{
+ switch (data[0] & 0x07) {
+ case 5:
+ return 2 + data[1];
+ case 6:
+ return 3 + bt_get_be16(data + 1);
+ case 7:
+ return 5 + bt_get_be32(data + 1);
+ }
+
+ return 0;
+}
+
static void print_continuation(const uint8_t *data, uint16_t size)
{
if (data[0] != size - 1) {
packet_hexdump(data + 1, size - 1);
}
-static uint16_t get_bytes(const uint8_t *data, uint16_t size)
+static void store_continuation(struct tid_data *tid,
+ const uint8_t *data, uint16_t size)
{
- switch (data[0] & 0x07) {
- case 5:
- return 2 + data[1];
- case 6:
- return 3 + bt_get_be16(data + 1);
- case 7:
- return 5 + bt_get_be32(data + 1);
+ 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, 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;
}
- return 0;
+ if (tid->cont[0] == 0x00 && data[bytes] == 0x00) {
+ decode_data_elements(2, data, bytes);
+ 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(2, cont_list[n].data, cont_list[n].size);
+ 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 void error_rsp(const struct l2cap_frame *frame)
+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 = bt_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 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);
print_field("Error code: 0x%2.2x", error);
}
-static void service_req(const struct l2cap_frame *frame)
+static void service_req(const struct l2cap_frame *frame, struct tid_data *tid)
{
uint16_t search_bytes;
frame->size - search_bytes - 2);
}
-static void service_rsp(const struct l2cap_frame *frame)
+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);
frame->size - 4 - (count * 4));
}
-static void attr_req(const struct l2cap_frame *frame)
+static void attr_req(const struct l2cap_frame *frame, struct tid_data *tid)
{
uint16_t attr_bytes;
decode_data_elements(2, frame->data + 6, attr_bytes);
- print_continuation(frame->data + 6 + attr_bytes,
+ store_continuation(tid, frame->data + 6 + attr_bytes,
frame->size - 6 - attr_bytes);
}
-static void attr_rsp(const struct l2cap_frame *frame)
+static void attr_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;
- }
+ bytes = common_rsp(frame, tid);
- bytes = bt_get_be16(frame->data);
- print_field("Attribute bytes: %d", bytes);
+ handle_continuation(tid, bytes, frame->data + 2, frame->size - 2);
- if (bytes > frame->size - 2) {
- print_text(COLOR_ERROR, "invalid attribute size");
- return;
- }
-
- decode_data_elements(2, frame->data + 2, bytes);
-
- print_continuation(frame->data + 2 + bytes, frame->size - 2 - bytes);
+ clear_tid(tid);
}
-static void search_attr_req(const struct l2cap_frame *frame)
+static void search_attr_req(const struct l2cap_frame *frame,
+ struct tid_data *tid)
{
uint16_t search_bytes, attr_bytes;
decode_data_elements(2, frame->data + search_bytes + 2, attr_bytes);
- print_continuation(frame->data + search_bytes + 2 + attr_bytes,
+ 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)
+static void search_attr_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;
- }
-
- bytes = bt_get_be16(frame->data);
- print_field("Attribute list bytes: %d", bytes);
+ bytes = common_rsp(frame, tid);
- decode_data_elements(2, frame->data + 2, bytes);
+ handle_continuation(tid, bytes, frame->data + 2, frame->size - 2);
- print_continuation(frame->data + 2 + bytes, frame->size - 2 - bytes);
+ clear_tid(tid);
}
struct sdp_data {
uint8_t pdu;
const char *str;
- void (*func) (const struct l2cap_frame *frame);
+ void (*func) (const struct l2cap_frame *frame, struct tid_data *tid);
};
static const struct sdp_data sdp_table[] = {
{ }
};
-void sdp_packet(const struct l2cap_frame *frame)
+void sdp_packet(const struct l2cap_frame *frame, uint16_t channel)
{
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;
return;
}
+ tid_info = get_tid(tid, channel);
+
l2cap_frame_pull(&sdp_frame, frame, 5);
- sdp_data->func(&sdp_frame);
+ sdp_data->func(&sdp_frame, tid_info);
}
diff --git a/monitor/sdp.h b/monitor/sdp.h
index 5287bb1..c339772 100644
--- a/monitor/sdp.h
+++ b/monitor/sdp.h
*
*/
-void sdp_packet(const struct l2cap_frame *frame);
+void sdp_packet(const struct l2cap_frame *frame, uint16_t channel);