From patchwork Fri Nov 4 21:36:48 2022 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Luiz Augusto von Dentz X-Patchwork-Id: 13032619 Return-Path: X-Spam-Checker-Version: SpamAssassin 3.4.0 (2014-02-07) on aws-us-west-2-korg-lkml-1.web.codeaurora.org Received: from vger.kernel.org (vger.kernel.org [23.128.96.18]) by smtp.lore.kernel.org (Postfix) with ESMTP id 58399C433FE for ; Fri, 4 Nov 2022 21:36:57 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S229886AbiKDVgz (ORCPT ); Fri, 4 Nov 2022 17:36:55 -0400 Received: from lindbergh.monkeyblade.net ([23.128.96.19]:43064 "EHLO lindbergh.monkeyblade.net" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S229883AbiKDVgx (ORCPT ); Fri, 4 Nov 2022 17:36:53 -0400 Received: from mail-pj1-x1035.google.com (mail-pj1-x1035.google.com [IPv6:2607:f8b0:4864:20::1035]) by lindbergh.monkeyblade.net (Postfix) with ESMTPS id 8FC9649B77 for ; Fri, 4 Nov 2022 14:36:52 -0700 (PDT) Received: by mail-pj1-x1035.google.com with SMTP id r61-20020a17090a43c300b00212f4e9cccdso9356317pjg.5 for ; Fri, 04 Nov 2022 14:36:52 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gmail.com; s=20210112; h=content-transfer-encoding:mime-version:message-id:date:subject:to :from:from:to:cc:subject:date:message-id:reply-to; bh=XMD5vQCV8GDZ7SOShRSqUlhWC9SWNwH1MWaerbCB1yU=; b=C/lKptZ6WtSqChBVVbrAOaw8jfJHOqFI8B2oT9qMVVdIa809aLmNet7GRFRtHk4UCQ cpXGOYSqW+VITueJv5/yDZpLKO0ZL1GzS0YxhCVGJhV/EMvavrc5gB6aoPLUjzpln1oD LROv4/BBPWAZak0/KWOgT1a6jNpE48vjlG3wl6O0WlAKB9j6qpXZkyHCcDaG++BbseRz 71GcPlpgW/VwjCZb0PFtcA16XMmaJd7+4yCK7TNY6dHKLdgTaM9kdswkTnFPtLkXaqI1 joxrvtPCVeQTCPOS/lYxf9jMxTWSBXeQHqX15l4wW0Jt7nOum/U9JzOs0S0VwAVNaO6C l8pQ== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20210112; h=content-transfer-encoding:mime-version:message-id:date:subject:to :from:x-gm-message-state:from:to:cc:subject:date:message-id:reply-to; bh=XMD5vQCV8GDZ7SOShRSqUlhWC9SWNwH1MWaerbCB1yU=; b=4FRXYUekzMZYydrDiQPbTBNEN2mzcPBgxVjVXaTqwtrMIQsD0+xeukWmO/OiiL1IAD t7Ep2qsQ5BEB6aBSUz89Yg3h2x1CIXIJMLFUy3KPMiRvg2K0zX1zcvbsKN6A2XO23dOK UN+l05Zv3Hzfp33t053DNPYFdbWN5HoO8IzUd2xHxCysD7u1PfjXwCMSCEb/BIfnVuog vudy2OYh9nzhl/+Hi9CukNclPLhrh3fm2IpYLS6LVvCdVNXCnCisfMbrFW3990Pglvsw RpMxSw/VEK6fgW41jRBTrrI10I3i2VcaQU82H6xTCm7V4pRxnJ5d54Aqxetdd/A4497T arQg== X-Gm-Message-State: ACrzQf14Y4xauxp6OPKv7S1Vd6+QF4ZIMLU/NcdOKaOX3idbf0/pAO/y 5dyHT5WxeLUdeBqyM7hEtRQMkddjAyw= X-Google-Smtp-Source: AMsMyM5eLWGll7BymPn+rBma/IVXfPozc4gI/08FIUYIgmIa65MtTwLFGTzpkqvhGps0WNk+mEtPog== X-Received: by 2002:a17:90a:8808:b0:213:c7d0:2217 with SMTP id s8-20020a17090a880800b00213c7d02217mr33648842pjn.231.1667597811347; Fri, 04 Nov 2022 14:36:51 -0700 (PDT) Received: from lvondent-mobl4.. (c-71-56-157-77.hsd1.or.comcast.net. [71.56.157.77]) by smtp.gmail.com with ESMTPSA id 15-20020a17090a08cf00b001efa9e83927sm2041027pjn.51.2022.11.04.14.36.50 for (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Fri, 04 Nov 2022 14:36:50 -0700 (PDT) From: Luiz Augusto von Dentz To: linux-bluetooth@vger.kernel.org Subject: [PATCH BlueZ 1/2] monitor/att: Add handler support for Read by Type Date: Fri, 4 Nov 2022 14:36:48 -0700 Message-Id: <20221104213649.36925-1-luiz.dentz@gmail.com> X-Mailer: git-send-email 2.37.3 MIME-Version: 1.0 Precedence: bulk List-ID: X-Mailing-List: linux-bluetooth@vger.kernel.org From: Luiz Augusto von Dentz This adds handler support for Read by Type so it can further decode the values when the procedure is used. --- monitor/att.c | 206 ++++++++++++++++++++++++++++++++++-------------- monitor/l2cap.h | 13 ++- 2 files changed, 155 insertions(+), 64 deletions(-) diff --git a/monitor/att.c b/monitor/att.c index fbd75db03b83..289f4fc04d9a 100644 --- a/monitor/att.c +++ b/monitor/att.c @@ -43,6 +43,21 @@ #include "l2cap.h" #include "att.h" +struct att_read { + struct gatt_db_attribute *attr; + bool in; + uint16_t chan; + void (*func)(const struct l2cap_frame *frame); +}; + +struct att_conn_data { + struct gatt_db *ldb; + struct timespec ldb_mtim; + struct gatt_db *rdb; + struct timespec rdb_mtim; + struct queue *reads; +}; + static void print_uuid(const char *label, const void *data, uint16_t size) { const char *str; @@ -77,27 +92,66 @@ static void print_handle_range(const char *label, const void *data) get_le16(data), get_le16(data + 2)); } -static void print_data_list(const char *label, uint8_t length, - const void *data, uint16_t size) +static bool match_read_frame(const void *data, const void *match_data) { + const struct att_read *read = data; + const struct l2cap_frame *frame = match_data; + + /* Read frame and response frame shall be in the opposite direction to + * match. + */ + if (read->in == frame->in) + return false; + + return read->chan == frame->chan; +} + +static void print_data_list(const char *label, uint8_t length, + const struct l2cap_frame *frame) +{ + struct packet_conn_data *conn; + struct att_conn_data *data; + struct att_read *read; uint8_t count; if (length == 0) return; - count = size / length; + conn = packet_get_conn_data(frame->handle); + if (conn) { + data = conn->data; + if (data) + read = queue_remove_if(data->reads, match_read_frame, + (void *)frame); + else + read = NULL; + } else + read = NULL; + + count = frame->size / length; print_field("%s: %u entr%s", label, count, count == 1 ? "y" : "ies"); - while (size >= length) { - print_field("Handle: 0x%4.4x", get_le16(data)); - print_hex_field("Value", data + 2, length - 2); + while (frame->size >= length) { + if (!l2cap_frame_print_le16((void *)frame, "Handle")) + break; - data += length; - size -= length; + print_hex_field("Value", frame->data, length - 2); + + if (read) { + struct l2cap_frame f; + + l2cap_frame_clone_size(&f, frame, length - 2); + + read->func(&f); + } + + if (!l2cap_frame_pull((void *)frame, frame, length - 2)) + break; } - packet_hexdump(data, size); + packet_hexdump(frame->data, frame->size); + free(read); } static void print_attribute_info(uint16_t type, const void *data, uint16_t len) @@ -2292,9 +2346,8 @@ struct gatt_handler { GATT_HANDLER(0x2bba, content_control_id_read, NULL, NULL), }; -static struct gatt_handler *get_handler(struct gatt_db_attribute *attr) +static struct gatt_handler *get_handler_uuid(const bt_uuid_t *uuid) { - const bt_uuid_t *uuid = gatt_db_attribute_get_type(attr); size_t i; for (i = 0; i < ARRAY_SIZE(gatt_handlers); i++) { @@ -2307,6 +2360,11 @@ static struct gatt_handler *get_handler(struct gatt_db_attribute *attr) return NULL; } +static struct gatt_handler *get_handler(struct gatt_db_attribute *attr) +{ + return get_handler_uuid(gatt_db_attribute_get_type(attr)); +} + static void att_exchange_mtu_req(const struct l2cap_frame *frame) { const struct bt_l2cap_att_exchange_mtu_req *pdu = frame->data; @@ -2403,36 +2461,23 @@ static void att_find_by_type_val_rsp(const struct l2cap_frame *frame) packet_hexdump(ptr, len); } -static void att_read_type_req(const struct l2cap_frame *frame) +static int bt_uuid_from_data(bt_uuid_t *uuid, const void *data, uint16_t size) { - print_handle_range("Handle range", frame->data); - print_uuid("Attribute type", frame->data + 4, frame->size - 4); + uint128_t u128; + + switch (size) { + case 2: + return bt_uuid16_create(uuid, get_le16(data)); + case 4: + return bt_uuid32_create(uuid, get_le32(data)); + case 16: + memcpy(u128.data, data, sizeof(u128.data)); + return bt_uuid128_create(uuid, u128); + } + + return -EINVAL; } -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); -} - -struct att_read { - struct gatt_db_attribute *attr; - bool in; - uint16_t chan; - void (*func)(const struct l2cap_frame *frame); -}; - -struct att_conn_data { - struct gatt_db *ldb; - struct timespec ldb_mtim; - struct gatt_db *rdb; - struct timespec rdb_mtim; - struct queue *reads; -}; - static void att_conn_data_free(void *data) { struct att_conn_data *att_data = data; @@ -2443,6 +2488,67 @@ static void att_conn_data_free(void *data) free(att_data); } +static struct att_conn_data *att_get_conn_data(struct packet_conn_data *conn) +{ + struct att_conn_data *data = conn->data; + + if (data) + return data; + + data = new0(struct att_conn_data, 1); + data->rdb = gatt_db_new(); + data->ldb = gatt_db_new(); + conn->data = data; + conn->destroy = att_conn_data_free; + + return data; +} + +static void att_read_type_req(const struct l2cap_frame *frame) +{ + bt_uuid_t uuid; + struct packet_conn_data *conn; + struct att_conn_data *data; + struct att_read *read; + struct gatt_handler *handler; + + print_handle_range("Handle range", frame->data); + print_uuid("Attribute type", frame->data + 4, frame->size - 4); + + if (bt_uuid_from_data(&uuid, frame->data + 4, frame->size - 4)) + return; + + handler = get_handler_uuid(&uuid); + if (!handler || !handler->read) + return; + + conn = packet_get_conn_data(frame->handle); + data = att_get_conn_data(conn); + + if (!data->reads) + data->reads = queue_new(); + + read = new0(struct att_read, 1); + read->in = frame->in; + read->chan = frame->chan; + read->func = handler->read; + + queue_push_tail(data->reads, read); +} + +static void att_read_type_rsp(const struct l2cap_frame *frame) +{ + uint8_t len; + + if (!l2cap_frame_get_u8((void *)frame, &len)) { + print_text(COLOR_ERROR, "invalid size"); + return; + } + + print_field("Attribute data length: %d", len); + print_data_list("Attribute data list", len, frame); +} + static void gatt_load_db(struct gatt_db *db, const char *filename, struct timespec *mtim) { @@ -2467,19 +2573,11 @@ static void gatt_load_db(struct gatt_db *db, const char *filename, static void load_gatt_db(struct packet_conn_data *conn) { - struct att_conn_data *data = conn->data; + struct att_conn_data *data = att_get_conn_data(conn); char filename[PATH_MAX]; char local[18]; char peer[18]; - if (!data) { - data = new0(struct att_conn_data, 1); - data->rdb = gatt_db_new(); - data->ldb = gatt_db_new(); - conn->data = data; - conn->destroy = att_conn_data_free; - } - ba2str((bdaddr_t *)conn->src, local); ba2str((bdaddr_t *)conn->dst, peer); @@ -2605,20 +2703,6 @@ static void att_read_req(const struct l2cap_frame *frame) queue_push_tail(data->reads, read); } -static bool match_read_frame(const void *data, const void *match_data) -{ - const struct att_read *read = data; - const struct l2cap_frame *frame = match_data; - - /* Read frame and response frame shall be in the opposite direction to - * match. - */ - if (read->in == frame->in) - return false; - - return read->chan == frame->chan; -} - static void att_read_rsp(const struct l2cap_frame *frame) { struct packet_conn_data *conn; diff --git a/monitor/l2cap.h b/monitor/l2cap.h index e63377857cf7..935066e6e7dc 100644 --- a/monitor/l2cap.h +++ b/monitor/l2cap.h @@ -31,8 +31,9 @@ void l2cap_frame_init(struct l2cap_frame *frame, uint16_t index, bool in, uint16_t cid, uint16_t psm, const void *data, uint16_t size); -static inline void l2cap_frame_clone(struct l2cap_frame *frame, - const struct l2cap_frame *source) +static inline void l2cap_frame_clone_size(struct l2cap_frame *frame, + const struct l2cap_frame *source, + uint16_t size) { if (frame != source) { frame->index = source->index; @@ -44,10 +45,16 @@ static inline void l2cap_frame_clone(struct l2cap_frame *frame, frame->chan = source->chan; frame->mode = source->mode; frame->data = source->data; - frame->size = source->size; + frame->size = size; } } +static inline void l2cap_frame_clone(struct l2cap_frame *frame, + const struct l2cap_frame *source) +{ + l2cap_frame_clone_size(frame, source, source->size); +} + static inline void *l2cap_frame_pull(struct l2cap_frame *frame, const struct l2cap_frame *source, uint16_t len) { From patchwork Fri Nov 4 21:36:49 2022 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Luiz Augusto von Dentz X-Patchwork-Id: 13032620 Return-Path: X-Spam-Checker-Version: SpamAssassin 3.4.0 (2014-02-07) on aws-us-west-2-korg-lkml-1.web.codeaurora.org Received: from vger.kernel.org (vger.kernel.org [23.128.96.18]) by smtp.lore.kernel.org (Postfix) with ESMTP id 5D506C4332F for ; Fri, 4 Nov 2022 21:36:58 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S229772AbiKDVg5 (ORCPT ); Fri, 4 Nov 2022 17:36:57 -0400 Received: from lindbergh.monkeyblade.net ([23.128.96.19]:43070 "EHLO lindbergh.monkeyblade.net" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S229767AbiKDVgy (ORCPT ); Fri, 4 Nov 2022 17:36:54 -0400 Received: from mail-pf1-x433.google.com (mail-pf1-x433.google.com [IPv6:2607:f8b0:4864:20::433]) by lindbergh.monkeyblade.net (Postfix) with ESMTPS id CEBC145EE7 for ; Fri, 4 Nov 2022 14:36:53 -0700 (PDT) Received: by mail-pf1-x433.google.com with SMTP id k15so5608519pfg.2 for ; Fri, 04 Nov 2022 14:36:53 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gmail.com; s=20210112; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:to:from:from:to:cc:subject:date:message-id :reply-to; bh=XQNvCYq4DWT5O6YSnww9WUDLXDRvhBKnzKNmfjCKHWI=; b=FuyFRIRoq70EhTgAJbJeWjgK9BvT2wa28YAobvPQkzLiYRPJStW6oy2d2ypQeXk4Yy jZ8LQ3yM1IOhjJo5g4olNv59dTM53Bu4D+Y+9i42ih1cHMqNm9dLB7Jf5wO8IN9vW5mU BmWmb80g82N1NhLsathnzpD1Mq7VQgVP9UKceMl4fRq89Egmgt/IiDv1qFu8RGWQkqtg bBQ6w/5CcoRA2+LXhUdLhvft44OYf4F0ZfBcDNLYg7dy2tVPdqPGSIwrG+MZaLyKSVkF g25857yZ0yMMOl9tzMc2oYNp2Y1TUi/LDRNs9HVSYYf7xaNTW1cBMAGsGQHeXwFIB/Es z56w== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20210112; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:to:from:x-gm-message-state:from:to:cc :subject:date:message-id:reply-to; bh=XQNvCYq4DWT5O6YSnww9WUDLXDRvhBKnzKNmfjCKHWI=; b=GulKUfPnM67px/kyQ7QB0DVCgmpnvho24qywrOOyWCXZC5XlRsB7k6y8cPdPibXRLr G2Vy4ZbsxszFsYQy/e5SD5Nm9/dBHouVFdkjaBKb2b98slhXh3bAiM/vlktOD6fXoEKX hG9k+nPwX71fQ0W9tDdBQgH3a+DozX5+og0Kvx/gTYnTcxEjFJhTpJikJRD7spe9fzO5 PoH3hhbDDxtjaTmHJTDMmkogc4+p4URb9GtWV35RsEmhp6pyLBi5n49wTHXzj269lBor 8t3+ISseGxPwa2Idf2HiJvgLAtvUB/GpNEFQ4c1RnB7bOR0R+hYNHCRf7rvcWIW4dD9U E+Vw== X-Gm-Message-State: ACrzQf2K6rFt0RPoOb/wuerzTp047ioer8DgGtsA57YSuM0h7KOyanJC GD3t6eO9oGgWeGwOWW4BRsBUHFtMe4U= X-Google-Smtp-Source: AMsMyM54+uznMMvE5Es5IF8kcxOFMAWz9uzo+Hvv/D2ksLZ/yrv7TeY+YRcCkOrxDUqnaaPsoAXIgA== X-Received: by 2002:a63:5fd6:0:b0:46f:fdc1:4086 with SMTP id t205-20020a635fd6000000b0046ffdc14086mr16415064pgb.154.1667597812768; Fri, 04 Nov 2022 14:36:52 -0700 (PDT) Received: from lvondent-mobl4.. (c-71-56-157-77.hsd1.or.comcast.net. [71.56.157.77]) by smtp.gmail.com with ESMTPSA id 15-20020a17090a08cf00b001efa9e83927sm2041027pjn.51.2022.11.04.14.36.51 for (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Fri, 04 Nov 2022 14:36:51 -0700 (PDT) From: Luiz Augusto von Dentz To: linux-bluetooth@vger.kernel.org Subject: [PATCH BlueZ 2/2] monitor/att: Add support for decoding Characteristic Declaration Date: Fri, 4 Nov 2022 14:36:49 -0700 Message-Id: <20221104213649.36925-2-luiz.dentz@gmail.com> X-Mailer: git-send-email 2.37.3 In-Reply-To: <20221104213649.36925-1-luiz.dentz@gmail.com> References: <20221104213649.36925-1-luiz.dentz@gmail.com> MIME-Version: 1.0 Precedence: bulk List-ID: X-Mailing-List: linux-bluetooth@vger.kernel.org From: Luiz Augusto von Dentz This adds supports for decoding the so called Characteristic Declaration (0x2803): > ACL Data RX: Handle 3585 flags 0x02 dlen 76 ATT: Read By Type Response (0x09) len 71 Attribute data length: 7 Attribute data list: 10 entries Handle: 0x0002 Value: 200300052a Properties: 0x20 Indicate (0x20) Value Handle: 0x0003 Value UUID: Service Changed (0x2a05) Handle: 0x0015 Value: 021600002a Properties: 0x02 Read (0x02) Value Handle: 0x0016 Value UUID: Device Name (0x2a00) Handle: 0x0017 Value: 021800012a Properties: 0x02 Read (0x02) Value Handle: 0x0018 Value UUID: Appearance (0x2a01) Handle: 0x0019 Value: 021a00a62a Properties: 0x02 Read (0x02) Value Handle: 0x001a Value UUID: Central Address Resolution (0x2aa6) Handle: 0x0029 Value: 102a00372a Properties: 0x10 Notify (0x10) Value Handle: 0x002a Value UUID: Heart Rate Measurement (0x2a37) Handle: 0x002c Value: 022d00382a Properties: 0x02 Read (0x02) Value Handle: 0x002d Value UUID: Body Sensor Location (0x2a38) Handle: 0x002e Value: 082f00392a Properties: 0x08 Write (0x08) Value Handle: 0x002f Value UUID: Heart Rate Control Point (0x2a39) Handle: 0x0031 Value: 0a32008a2a Properties: 0x0a Read (0x02) Write (0x08) Value Handle: 0x0032 Value UUID: First Name (0x2a8a) Handle: 0x0033 Value: 0a3400902a Properties: 0x0a Read (0x02) Write (0x08) Value Handle: 0x0034 Value UUID: Last Name (0x2a90) Handle: 0x0035 Value: 0a36008c2a Properties: 0x0a Read (0x02) Write (0x08) Value Handle: 0x0036 Value UUID: Gender (0x2a8c) --- monitor/att.c | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/monitor/att.c b/monitor/att.c index 289f4fc04d9a..efd840d51961 100644 --- a/monitor/att.c +++ b/monitor/att.c @@ -273,6 +273,46 @@ static void att_error_response(const struct l2cap_frame *frame) print_field("Error: %s (0x%2.2x)", str, pdu->error); } +static const struct bitfield_data chrc_prop_table[] = { + { 0, "Broadcast (0x01)" }, + { 1, "Read (0x02)" }, + { 2, "Write Without Response (0x04)" }, + { 3, "Write (0x08)" }, + { 4, "Notify (0x10)" }, + { 5, "Indicate (0x20)" }, + { 6, "Authorize (0x40)" }, + { 6, "Extended Properties (0x80)" }, + { } +}; + +static void print_chrc(const struct l2cap_frame *frame) +{ + uint8_t prop; + uint8_t mask; + + if (!l2cap_frame_get_u8((void *)frame, &prop)) { + print_text(COLOR_ERROR, "Property: invalid size"); + return; + } + + print_field(" Properties: 0x%2.2x", prop); + + mask = print_bitfield(6, prop, chrc_prop_table); + if (mask) + print_text(COLOR_WHITE_BG, " Unknown fields (0x%2.2x)", + mask); + + if (!l2cap_frame_print_le16((void *)frame, " Value Handle")) + return; + + print_uuid(" Value UUID", frame->data, frame->size); +} + +static void chrc_read(const struct l2cap_frame *frame) +{ + print_chrc(frame); +} + static const struct bitfield_data ccc_value_table[] = { { 0, "Notification (0x01)" }, { 1, "Indication (0x02)" }, @@ -2314,6 +2354,7 @@ struct gatt_handler { void (*write)(const struct l2cap_frame *frame); void (*notify)(const struct l2cap_frame *frame); } gatt_handlers[] = { + GATT_HANDLER(0x2803, chrc_read, NULL, NULL), GATT_HANDLER(0x2902, ccc_read, ccc_write, NULL), GATT_HANDLER(0x2bc4, ase_read, NULL, ase_notify), GATT_HANDLER(0x2bc5, ase_read, NULL, ase_notify),