From patchwork Thu May 26 20:59:34 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: 576995 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 E8BA7C433F5 for ; Thu, 26 May 2022 20:59:41 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S237599AbiEZU7l (ORCPT ); Thu, 26 May 2022 16:59:41 -0400 Received: from lindbergh.monkeyblade.net ([23.128.96.19]:44518 "EHLO lindbergh.monkeyblade.net" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S229569AbiEZU7k (ORCPT ); Thu, 26 May 2022 16:59:40 -0400 Received: from mail-pf1-x429.google.com (mail-pf1-x429.google.com [IPv6:2607:f8b0:4864:20::429]) by lindbergh.monkeyblade.net (Postfix) with ESMTPS id AC7CDE733A for ; Thu, 26 May 2022 13:59:39 -0700 (PDT) Received: by mail-pf1-x429.google.com with SMTP id f21so2674949pfa.3 for ; Thu, 26 May 2022 13:59:39 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gmail.com; s=20210112; h=from:to:subject:date:message-id:mime-version :content-transfer-encoding; bh=uKHRNfeN49vK2aKZDtzsKp50eqPBJNLL30heiyMDxb8=; b=Rw6g7/067gzdoouYUVCu38cw15itKLw/+M5N5Elveg8IBmi2rExryPNi8Uusf7XlOd eIKbMfejiJ7sBU1jKKLcSgsQyqG8fvZgHKvz296jqZqoeCyuWGBx9dnM4qTGA/KyBci9 Bjb3nyMjQ46Efm01UPY4OnHDyuL80yw9JIAW2/cj8FU05u74ppfalw2DoSWmCDtILqxB esRWJWJ1GottTZig/93yAKgF+eIVfmzTr61sj3fJdXqVFs0P2d9Kb/yRRyeFvuLKBoYX Lsy3vyTg+oXNSbO0zDYa8NqrNF/nRBllnl+OIMBLjtfeKbLwGNOO70Ua8NA0YhVtwoa3 vh3A== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20210112; h=x-gm-message-state:from:to:subject:date:message-id:mime-version :content-transfer-encoding; bh=uKHRNfeN49vK2aKZDtzsKp50eqPBJNLL30heiyMDxb8=; b=lHA/8U0ezdUE9pk7fiAVEUJbQu3kEyoM7I4lm0YgAkAj/CXHQaKz5244BRKeC2CtSG z8EZs20zMOGui+LsLTqgIYPxqy356LEbRaRY87gdw8m/2gDV8/M9jfIXt2Vvtr/TWBr6 SK3IS2/sYr2ljwenGfKrd/870O8vAm4Buz9zPXMuSTVflZ2BFi9WQxwuB8C9eQFGgPEA mkQh53CVUnL4kCuaFOMWG5QKSHq0jaHhkwzDsoZy/dE7iISoIPkb9F3hof4uq+6AT7Zk 7p3Iurm+dhkC1ZYZ28u6Vq5rfD6sN2wFkH8v7xdc81JUtKAC0pZAnvi1d3fC2lfED7Yt FUng== X-Gm-Message-State: AOAM531jtbnTn4UFZI5jQzLtzxtJM2wJDgb7fDbfsIO3cjjDNruXIWWk vHq9dMQiB8xptlYZRuLbfB3ucSE8ASQ= X-Google-Smtp-Source: ABdhPJyKpWOWqN6CUah7uXU/xC90Q6aco6OyCUSbxuUstlymKjP/CqPc68zKvDUOWIIyTA4iBgYUBQ== X-Received: by 2002:a05:6a00:23ce:b0:50d:823f:981 with SMTP id g14-20020a056a0023ce00b0050d823f0981mr41447994pfc.10.1653598778580; Thu, 26 May 2022 13:59:38 -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 z6-20020a170903018600b0015e8d4eb25bsm2080064plg.165.2022.05.26.13.59.37 for (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Thu, 26 May 2022 13:59:38 -0700 (PDT) From: Luiz Augusto von Dentz To: linux-bluetooth@vger.kernel.org Subject: [PATCH v2 1/4] monitor/att: Simplify CCC decoders Date: Thu, 26 May 2022 13:59:34 -0700 Message-Id: <20220526205937.4159665-1-luiz.dentz@gmail.com> X-Mailer: git-send-email 2.35.1 MIME-Version: 1.0 Precedence: bulk List-ID: X-Mailing-List: linux-bluetooth@vger.kernel.org From: Luiz Augusto von Dentz This simplify callbacks moving the decoding of the value to print_ccc_value. --- monitor/att.c | 35 +++++++++++++---------------------- 1 file changed, 13 insertions(+), 22 deletions(-) diff --git a/monitor/att.c b/monitor/att.c index df3e65057..0223af210 100644 --- a/monitor/att.c +++ b/monitor/att.c @@ -221,9 +221,15 @@ static const struct bitfield_data ccc_value_table[] = { { } }; -static void print_ccc_value(uint8_t value) +static void print_ccc_value(const struct l2cap_frame *frame) { - uint8_t mask = value; + uint8_t value; + uint8_t mask; + + if (!l2cap_frame_get_u8((void *)frame, &value)) { + print_text(COLOR_ERROR, "invalid size"); + return; + } mask = print_bitfield(4, value, ccc_value_table); if (mask) @@ -231,28 +237,14 @@ static void print_ccc_value(uint8_t value) mask); } -static void gatt_ccc_read(const struct l2cap_frame *frame) +static void ccc_read(const struct l2cap_frame *frame) { - uint8_t value; - - if (!l2cap_frame_get_u8((void *)frame, &value)) { - print_text(COLOR_ERROR, "invalid size"); - return; - } - - print_ccc_value(value); + print_ccc_value(frame); } -static void gatt_ccc_write(const struct l2cap_frame *frame) +static void ccc_write(const struct l2cap_frame *frame) { - uint8_t value; - - if (!l2cap_frame_get_u8((void *)frame, &value)) { - print_text(COLOR_ERROR, "invalid size"); - return; - } - - print_ccc_value(value); + print_ccc_value(frame); } #define GATT_HANDLER(_uuid, _read, _write, _notify) \ @@ -272,8 +264,7 @@ struct gatt_handler { void (*write)(const struct l2cap_frame *frame); void (*notify)(const struct l2cap_frame *frame); } gatt_handlers[] = { - GATT_HANDLER(GATT_CLIENT_CHARAC_CFG_UUID, gatt_ccc_read, - gatt_ccc_write, NULL) + GATT_HANDLER(0x2902, ccc_read, ccc_write, NULL), }; static struct gatt_handler *get_handler(struct gatt_db_attribute *attr) From patchwork Thu May 26 20:59:35 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: 576272 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 F1E6CC433EF for ; Thu, 26 May 2022 20:59:42 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S237671AbiEZU7m (ORCPT ); Thu, 26 May 2022 16:59:42 -0400 Received: from lindbergh.monkeyblade.net ([23.128.96.19]:44520 "EHLO lindbergh.monkeyblade.net" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S232749AbiEZU7l (ORCPT ); Thu, 26 May 2022 16:59:41 -0400 Received: from mail-pf1-x42d.google.com (mail-pf1-x42d.google.com [IPv6:2607:f8b0:4864:20::42d]) by lindbergh.monkeyblade.net (Postfix) with ESMTPS id 13279E7328 for ; Thu, 26 May 2022 13:59:40 -0700 (PDT) Received: by mail-pf1-x42d.google.com with SMTP id c65so2099772pfb.1 for ; Thu, 26 May 2022 13:59:40 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gmail.com; s=20210112; h=from:to:subject:date:message-id:in-reply-to:references:mime-version :content-transfer-encoding; bh=rt575DNIJ8qNZ0PTyIh3GV4R6BBDs4YiRzQg+HefwxM=; b=Ujxepr8IfTq4CHX1390sYEBxbnKxdhJixw42orkd+QKWJJ5CQ0PiLEaJ+cl6ugJFqa 4N/Y3MLuFoZ+QWozc0j8VlxXuf6ad8EIXPoquDMxrXfPkxBLOba4myyScsvd2OgnruSE ttteZVwl3FYeS8g0qtd/lfJA0mayZE/2AYlIG7Pr9S/Mo/EbyupVaQlZGN3wGgC5BzuA I6Nm01eEHGHpxTAdN9N8zVNGJMHNC1HVBa6keHu/SAdgjo2ilw4S4UaRS9nq1pRpQ1iH 56agk3h2wejG+ZQFkDo/HzqyiKRYCQRKTQArMcNuNXKm39CZLtoFeMZpOBeStUmzlpYo sOTA== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20210112; h=x-gm-message-state:from:to:subject:date:message-id:in-reply-to :references:mime-version:content-transfer-encoding; bh=rt575DNIJ8qNZ0PTyIh3GV4R6BBDs4YiRzQg+HefwxM=; b=d47raXqGdze7DXH4UP8tVneGhab8fymnWXExnVoHeFeqrRtCfSu79enggL9OYt28Nx AGgv9ocIiXV7KPuEt8NnSoMGc4wUYnQDKPoxy2aSTat9iFMG8f1SAdb3YegUTEsAiZPa jyrd7w8/kR6URrvuOpPlxFcHIigXByfHS+Dx6Co46Y18Gz5oxwRj0bUQZGWCE1tP116b BAtL2hJ02//B/nASktC4KTgd2YdEMAb0HIIE6AdMqdC6BZLX729fFY26SJPu6lKDz0OV gd11r3fqu3K9li+KGD/bPiTjk9XHwQVvkM7eRKzJTva8TltzaocJAt0Y4ylD0sWvqaMe GyPw== X-Gm-Message-State: AOAM531VpJ68zHj1iPAzs5RKubpGVdiKZoYTSgtwreLc+AQIufX2kfwq lDybBjeB3dmSGhZd7VvRPxt5DSjLNpY= X-Google-Smtp-Source: ABdhPJyWOuF3TbcGXM0HjCfA3f+KmWdWjuqWZaPwbU7nQl3AlY4NAIKJSwJdTuBuj5PdmukBYcJ2KQ== X-Received: by 2002:a65:6c08:0:b0:3f2:6a6a:98d with SMTP id y8-20020a656c08000000b003f26a6a098dmr34222035pgu.30.1653598779147; Thu, 26 May 2022 13:59:39 -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 z6-20020a170903018600b0015e8d4eb25bsm2080064plg.165.2022.05.26.13.59.38 for (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Thu, 26 May 2022 13:59:38 -0700 (PDT) From: Luiz Augusto von Dentz To: linux-bluetooth@vger.kernel.org Subject: [PATCH v2 2/4] monitor/att: Add decoding support for PAC Sink/Source Date: Thu, 26 May 2022 13:59:35 -0700 Message-Id: <20220526205937.4159665-2-luiz.dentz@gmail.com> X-Mailer: git-send-email 2.35.1 In-Reply-To: <20220526205937.4159665-1-luiz.dentz@gmail.com> References: <20220526205937.4159665-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 decoding support for PAC Sink/Source attributes: < ACL Data TX: Handle 42 flags 0x00 dlen 9 Channel: 64 len 5 sdu 3 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Read Request (0x0a) len 2 Handle: 0x0017 Type: Sink PAC (0x2bc9) > ACL Data RX: Handle 42 flags 0x02 dlen 31 Channel: 65 len 27 sdu 25 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} Value: 010600000000100301ff0002020302030305041e00f00000 Number of PAC(s): 1 PAC #0: Codec: LC3 (0x06) Codec Specific Configuration #0: len 0x03 type 0x01 Codec Specific Configuration: ff00 Codec Specific Configuration #1: len 0x02 type 0x02 Codec Specific Configuration: 03 Codec Specific Configuration #2: len 0x02 type 0x03 Codec Specific Configuration: 03 Codec Specific Configuration #3: len 0x05 type 0x04 Codec Specific Configuration: 1e00f000 --- monitor/att.c | 92 ++++++++++++++++++++++++++++++++++++++++++++++++ monitor/l2cap.h | 10 +++++- monitor/packet.c | 10 ++++++ monitor/packet.h | 2 ++ 4 files changed, 113 insertions(+), 1 deletion(-) diff --git a/monitor/att.c b/monitor/att.c index 0223af210..de27ee42c 100644 --- a/monitor/att.c +++ b/monitor/att.c @@ -247,6 +247,96 @@ static void ccc_write(const struct l2cap_frame *frame) print_ccc_value(frame); } +static void print_pac(const struct l2cap_frame *frame) +{ + uint8_t num = 0, i; + + if (!l2cap_frame_get_u8((void *)frame, &num)) { + print_text(COLOR_ERROR, "Number of PAC(s): invalid size"); + goto done; + } + + print_field(" Number of PAC(s): %u", num); + + for (i = 0; i < num; i++) { + uint8_t codec_id; + uint16_t codec_cid, codec_vid; + struct bt_hci_lv_data *cc; + struct bt_hci_lv_data *meta; + + print_field(" PAC #%u:", i); + + if (!l2cap_frame_get_u8((void *)frame, &codec_id)) { + print_text(COLOR_ERROR, "Codec: invalid size"); + goto done; + } + + packet_print_codec_id(" Codec", codec_id); + + if (!l2cap_frame_get_le16((void *)frame, &codec_cid)) { + print_text(COLOR_ERROR, + "Codec Company ID: invalid size"); + goto done; + } + + if (!l2cap_frame_get_le16((void *)frame, &codec_vid)) { + print_text(COLOR_ERROR, + "Codec Vendor ID: invalid size"); + goto done; + } + + if (codec_id == 0xff) { + print_field(" Codec Company ID: %s (0x%04x)", + bt_compidtostr(codec_cid), + codec_cid); + print_field(" Codec Vendor ID: 0x%04x", codec_vid); + } + + cc = l2cap_frame_pull((void *)frame, frame, sizeof(*cc)); + if (!cc) { + print_text(COLOR_ERROR, + "Codec Specific Configuration: invalid size"); + goto done; + } + + if (!l2cap_frame_pull((void *)frame, frame, cc->len)) { + print_text(COLOR_ERROR, + "Codec Specific Configuration: invalid size"); + goto done; + } + + packet_print_ltv(" Codec Specific Configuration", cc->data, + cc->len); + + meta = l2cap_frame_pull((void *)frame, frame, sizeof(*meta)); + if (!meta) { + print_text(COLOR_ERROR, "Metadata: invalid size"); + goto done; + } + + if (!l2cap_frame_pull((void *)frame, frame, meta->len)) { + print_text(COLOR_ERROR, "Metadata: invalid size"); + goto done; + } + + packet_print_ltv(" Metadata", meta->data, meta->len); + } + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void pac_read(const struct l2cap_frame *frame) +{ + print_pac(frame); +} + +static void pac_notify(const struct l2cap_frame *frame) +{ + print_pac(frame); +} + #define GATT_HANDLER(_uuid, _read, _write, _notify) \ { \ .uuid = { \ @@ -265,6 +355,8 @@ struct gatt_handler { void (*notify)(const struct l2cap_frame *frame); } gatt_handlers[] = { GATT_HANDLER(0x2902, ccc_read, ccc_write, NULL), + GATT_HANDLER(0x2bc9, pac_read, NULL, pac_notify), + GATT_HANDLER(0x2bcb, pac_read, NULL, pac_notify), }; static struct gatt_handler *get_handler(struct gatt_db_attribute *attr) diff --git a/monitor/l2cap.h b/monitor/l2cap.h index 1daeb69be..c33d4c57f 100644 --- a/monitor/l2cap.h +++ b/monitor/l2cap.h @@ -48,13 +48,21 @@ static inline void l2cap_frame_clone(struct l2cap_frame *frame, } } -static inline void l2cap_frame_pull(struct l2cap_frame *frame, +static inline void *l2cap_frame_pull(struct l2cap_frame *frame, const struct l2cap_frame *source, uint16_t len) { + void *data; + l2cap_frame_clone(frame, source); + if (source->size < len) + return NULL; + + data = (void *)frame->data; frame->data = source->data + len; frame->size = source->size - len; + + return data; } static inline bool l2cap_frame_get_u8(struct l2cap_frame *frame, uint8_t *value) diff --git a/monitor/packet.c b/monitor/packet.c index e854c1a8e..c7739fba5 100644 --- a/monitor/packet.c +++ b/monitor/packet.c @@ -1377,6 +1377,11 @@ static void print_codec_id(const char *label, uint8_t codec) print_field("%s: %s (0x%2.2x)", label, str, codec); } +void packet_print_codec_id(const char *label, uint8_t codec) +{ + print_codec_id(label, codec); +} + static const struct bitfield_data codec_transport_table[] = { { 0, "Codec supported over BR/EDR ACL" }, { 1, "Codec supported over BR/EDR SCO and eSCO"}, @@ -3368,6 +3373,11 @@ static void print_ltv(const char *label, const uint8_t *data, uint8_t len) print_hex_field(label, iov.iov_base, iov.iov_len); } +void packet_print_ltv(const char *label, const uint8_t *data, uint8_t len) +{ + print_ltv(label, data, len); +} + static void print_base_annoucement(const uint8_t *data, uint8_t data_len) { struct iovec iov; diff --git a/monitor/packet.h b/monitor/packet.h index a00975eb3..97d683e3a 100644 --- a/monitor/packet.h +++ b/monitor/packet.h @@ -61,6 +61,8 @@ void packet_print_channel_map_lmp(const uint8_t *map); void packet_print_channel_map_ll(const uint8_t *map); void packet_print_io_capability(uint8_t capability); void packet_print_io_authentication(uint8_t authentication); +void packet_print_codec_id(const char *label, uint8_t codec); +void packet_print_ltv(const char *label, const uint8_t *data, uint8_t len); void packet_control(struct timeval *tv, struct ucred *cred, uint16_t index, uint16_t opcode, From patchwork Thu May 26 20:59:36 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: 576994 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 9569AC433F5 for ; Thu, 26 May 2022 20:59:44 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S242657AbiEZU7o (ORCPT ); Thu, 26 May 2022 16:59:44 -0400 Received: from lindbergh.monkeyblade.net ([23.128.96.19]:44540 "EHLO lindbergh.monkeyblade.net" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S232749AbiEZU7n (ORCPT ); Thu, 26 May 2022 16:59:43 -0400 Received: from mail-pf1-x436.google.com (mail-pf1-x436.google.com [IPv6:2607:f8b0:4864:20::436]) by lindbergh.monkeyblade.net (Postfix) with ESMTPS id 3B263E64EC for ; Thu, 26 May 2022 13:59:41 -0700 (PDT) Received: by mail-pf1-x436.google.com with SMTP id j6so2637637pfe.13 for ; Thu, 26 May 2022 13:59:41 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gmail.com; s=20210112; h=from:to:subject:date:message-id:in-reply-to:references:mime-version :content-transfer-encoding; bh=FYghBkb6lP6/U7Ej2tHgZ6W4jSqYTjy8ELLaRflX2b4=; b=P9Wio+nZ7obmIcUMq+qJV+k3xNwXx7Jo6XSgtR4M55ymYM6c39Pozkzqd8U5RAwfVX qQdU4DF1p3y0lvHNx1+gEqO+pBxsHgfVqZS7A316sYkl37tHI2287BqY9KTx40hY7xit LJcdsytp7BOQtqn3tLTz165xfRLf4vN/NyH6N67voZCMNB4bT2PsLdSVbIC6VJ1ZPinm Su29A/Hw25FZFAJ+amIIpzx9hkSRYAAafVCYItZBkygV7C95P4cVrrYnyfxzUiIR7dLk thSTm5d2i7WYjr6K7xshcEm0QpZQaA94CimfD3Jl9yzP+xSpFNb/ydwJFZVOMRLAIlWw 0S1g== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20210112; h=x-gm-message-state:from:to:subject:date:message-id:in-reply-to :references:mime-version:content-transfer-encoding; bh=FYghBkb6lP6/U7Ej2tHgZ6W4jSqYTjy8ELLaRflX2b4=; b=vifzlqnwPUOZFVCYau2N0128QxXMjw50LUXQLy15Wp6jY0Ve6t/S/iVtAzzHSr2B6v rRVHrrQti5Ce4p7a7aKuOOhbr2+q1DPpvWoZk8f3H2ws+oKkT4ISRn022nwmaeljmwsv yEtsQCFGQEAErMS9ot/xd1fsVV3Xxt6h1FQ9JaSYG4QEWLPt4+DCkxrXstZ8t43gCc9f 1bQBJyGaR9h3+1CXIPPIHXVlHYXQkYjXrbTIRTHTAMQx81icCTA+gRQqF0zBitZ1A1iO uQ/MbYejMEtdjAyYinHonBuk54Lw6dJytM39JN8Blq6IzV+8JJKy4oz40fzih3wNDjwl pkcg== X-Gm-Message-State: AOAM532NsZCHxH7w+F+qAyfxVW4lRB2+2c7f73ZclVt37OMgT2uXlQB9 n+V5ujwYCl714n8qK993njQJxehGpsE= X-Google-Smtp-Source: ABdhPJwFPA5rXSyOBYkKqcg7r/OCApmpvq8jFSjG0AbH9lcTgbFlftM4aBERffJpbSfPFGdnKGsy7w== X-Received: by 2002:a63:90ca:0:b0:3fa:3e63:15f4 with SMTP id a193-20020a6390ca000000b003fa3e6315f4mr19884247pge.362.1653598780121; Thu, 26 May 2022 13:59:40 -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 z6-20020a170903018600b0015e8d4eb25bsm2080064plg.165.2022.05.26.13.59.39 for (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Thu, 26 May 2022 13:59:39 -0700 (PDT) From: Luiz Augusto von Dentz To: linux-bluetooth@vger.kernel.org Subject: [PATCH v2 3/4] monitor/att: Add decoding support for ASE Sink/Source Date: Thu, 26 May 2022 13:59:36 -0700 Message-Id: <20220526205937.4159665-3-luiz.dentz@gmail.com> X-Mailer: git-send-email 2.35.1 In-Reply-To: <20220526205937.4159665-1-luiz.dentz@gmail.com> References: <20220526205937.4159665-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 decoding support for ASE Sink/Source attributes: > ACL Data RX: Handle 42 flags 0x02 dlen 9 Channel: 65 len 5 sdu 3 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Read Request (0x0a) len 2 Handle: 0x002a Type: Sink ASE (0x2bc4) < ACL Data TX: Handle 42 flags 0x00 dlen 9 Channel: 64 len 5 sdu 3 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Read Response (0x0b) len 2 Value: 0300 ASE ID: 1 State: Idle (0x00) < ACL Data TX: Handle 42 flags 0x00 dlen 55 Channel: 64 len 51 sdu 49 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Handle Multiple Value Notification (0x23) len 48 Length: 0x0023 Handle: 0x0024 Type: Sink ASE (0x2bc4) Data: 01010000000a00204e00409c00204e00409c0006000000000a02010302020103042800 ASE ID: 1 State: Codec Configured (0x01) Framing: Unframed PDUs supported (0x00) PHY: 0x00 RTN: 0 Max Transport Latency: 10 Presentation Delay Min: 20000 us Presentation Delay Max: 40000 us Preferred Presentation Delay Min: 20000 us Preferred Presentation Delay Max: 40000 us Codec: LC3 (0x06) Codec Specific Configuration #0: len 0x02 type 0x01 Codec Specific Configuration: 03 Codec Specific Configuration #1: len 0x02 type 0x02 Codec Specific Configuration: 01 Codec Specific Configuration #2: len 0x03 type 0x04 Codec Specific Configuration: 2800 < ACL Data TX: Handle 42 flags 0x00 dlen 37 Channel: 64 len 33 sdu 31 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Handle Multiple Value Notification (0x23) len 30 Length: 0x0011 Handle: 0x0024 Type: Sink ASE (0x2bc4) Data: 0102000010270000022800020a00409c00 ASE ID: 1 State: QoS Configured (0x02) CIG ID: 0x00 CIS ID: 0x00 SDU Interval: 10000 usec Framing: Unframed (0x00) PHY: 0x02 LE 2M PHY (0x02) Max SDU: 40 RTN: 2 Max Transport Latency: 10 Presentation Delay: 40000 us < ACL Data TX: Handle 42 flags 0x00 dlen 33 Channel: 64 len 29 sdu 27 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Handle Multiple Value Notification (0x23) len 26 Length: 0x000d Handle: 0x002a Type: Source ASE (0x2bc5) Data: 03030000060304030202000000 ASE ID: 3 State: Enabling (0x03) CIG ID: 0x00 CIS ID: 0x00 Metadata #0: len 0x03 type 0x04 Metadata: 0302 Metadata #1: len 0x02 type 0x00 < ACL Data TX: Handle 42 flags 0x00 dlen 39 Channel: 64 len 35 sdu 33 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Handle Multiple Value Notification (0x23) len 32 Length: 0x000d Handle: 0x002a Type: Source ASE (0x2bc5) Data: 03040000060304030202000000 ASE ID: 3 State: Streaming (0x04) CIG ID: 0x00 CIS ID: 0x00 Metadata #0: len 0x03 type 0x04 Metadata: 0302 Metadata #1: len 0x02 type 0x00 < ACL Data TX: Handle 42 flags 0x00 dlen 33 Channel: 64 len 29 sdu 27 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Handle Multiple Value Notification (0x23) len 26 Length: 0x000d Handle: 0x002a Type: Source ASE (0x2bc5) Data: 03050000060304030202000000 ASE ID: 3 State: Disabling (0x05) CIG ID: 0x00 CIS ID: 0x00 Metadata #0: len 0x03 type 0x04 Metadata: 0302 Metadata #1: len 0x02 type 0x00 --- lib/bluetooth.h | 4 + monitor/att.c | 365 ++++++++++++++++++++++++++++++++++++++++++++---- monitor/l2cap.h | 163 +++++++++++++++++++++ 3 files changed, 506 insertions(+), 26 deletions(-) diff --git a/lib/bluetooth.h b/lib/bluetooth.h index 0fcf412c6..e6171cef0 100644 --- a/lib/bluetooth.h +++ b/lib/bluetooth.h @@ -377,6 +377,10 @@ void bt_free(void *ptr); int bt_error(uint16_t code); const char *bt_compidtostr(int id); +typedef struct { + uint8_t data[3]; +} uint24_t; + typedef struct { uint8_t data[16]; } uint128_t; diff --git a/monitor/att.c b/monitor/att.c index de27ee42c..b39ac5a49 100644 --- a/monitor/att.c +++ b/monitor/att.c @@ -247,6 +247,38 @@ static void ccc_write(const struct l2cap_frame *frame) print_ccc_value(frame); } +static bool print_codec(const struct l2cap_frame *frame) +{ + uint8_t codec_id; + uint16_t codec_cid, codec_vid; + + if (!l2cap_frame_get_u8((void *)frame, &codec_id)) { + print_text(COLOR_ERROR, "Codec: invalid size"); + return false; + } + + packet_print_codec_id(" Codec", codec_id); + + if (!l2cap_frame_get_le16((void *)frame, &codec_cid)) { + print_text(COLOR_ERROR, "Codec Company ID: invalid size"); + return false; + } + + if (!l2cap_frame_get_le16((void *)frame, &codec_vid)) { + print_text(COLOR_ERROR, "Codec Vendor ID: invalid size"); + return false; + } + + if (codec_id == 0xff) { + print_field(" Codec Company ID: %s (0x%04x)", + bt_compidtostr(codec_cid), + codec_cid); + print_field(" Codec Vendor ID: 0x%04x", codec_vid); + } + + return true; +} + static void print_pac(const struct l2cap_frame *frame) { uint8_t num = 0, i; @@ -259,38 +291,13 @@ static void print_pac(const struct l2cap_frame *frame) print_field(" Number of PAC(s): %u", num); for (i = 0; i < num; i++) { - uint8_t codec_id; - uint16_t codec_cid, codec_vid; struct bt_hci_lv_data *cc; struct bt_hci_lv_data *meta; print_field(" PAC #%u:", i); - if (!l2cap_frame_get_u8((void *)frame, &codec_id)) { - print_text(COLOR_ERROR, "Codec: invalid size"); + if (!print_codec(frame)) goto done; - } - - packet_print_codec_id(" Codec", codec_id); - - if (!l2cap_frame_get_le16((void *)frame, &codec_cid)) { - print_text(COLOR_ERROR, - "Codec Company ID: invalid size"); - goto done; - } - - if (!l2cap_frame_get_le16((void *)frame, &codec_vid)) { - print_text(COLOR_ERROR, - "Codec Vendor ID: invalid size"); - goto done; - } - - if (codec_id == 0xff) { - print_field(" Codec Company ID: %s (0x%04x)", - bt_compidtostr(codec_cid), - codec_cid); - print_field(" Codec Vendor ID: 0x%04x", codec_vid); - } cc = l2cap_frame_pull((void *)frame, frame, sizeof(*cc)); if (!cc) { @@ -337,6 +344,310 @@ static void pac_notify(const struct l2cap_frame *frame) print_pac(frame); } +static void print_prefer_framing(uint8_t value) +{ + switch (value) { + case 0x00: + print_field(" Framing: Unframed ISOAL PDUs supported " + "(0x%2.2x)", value); + return; + case 0x01: + print_field(" Framing: Unframed ISOAL PDUs not supported " + "(0x%2.2x)", value); + return; + default: + print_field(" Framing: Reserved (0x%2.2x)", value); + } +} + +static const struct bitfield_data prefer_phy_table[] = { + { 0, "LE 1M PHY preffered (0x01)" }, + { 1, "LE 2M PHY preffered (0x02)" }, + { 2, "LE Codec PHY preffered (0x04)" }, + { } +}; + +static void print_prefer_phy(uint8_t phy) +{ + uint8_t mask; + + mask = print_bitfield(4, phy, prefer_phy_table); + if (mask) + print_text(COLOR_WHITE_BG, " Unknown fields (0x%2.2x)", + mask); +} + +static void print_ase_config(const struct l2cap_frame *frame) +{ + uint8_t framing, phy, rtn; + uint16_t latency; + uint32_t pd_min, pd_max, ppd_min, ppd_max; + struct bt_hci_lv_data *cc; + + if (!l2cap_frame_get_u8((void *)frame, &framing)) { + print_text(COLOR_ERROR, "Framing: invalid size"); + return; + } + + print_prefer_framing(framing); + + if (!l2cap_frame_get_u8((void *)frame, &phy)) { + print_text(COLOR_ERROR, "PHY: invalid size"); + return; + } + + print_prefer_phy(phy); + + if (!l2cap_frame_get_u8((void *)frame, &rtn)) { + print_text(COLOR_ERROR, "RTN: invalid size"); + return; + } + + print_field(" RTN: %u", rtn); + + if (!l2cap_frame_get_le16((void *)frame, &latency)) { + print_text(COLOR_ERROR, "RTN: invalid size"); + return; + } + + print_field(" Max Transport Latency: %u ms", latency); + + if (!l2cap_frame_get_le24((void *)frame, &pd_min)) { + print_text(COLOR_ERROR, "Presentation Delay Min: invalid size"); + return; + } + + print_field(" Presentation Delay Min: %u us", pd_min); + + if (!l2cap_frame_get_le24((void *)frame, &pd_max)) { + print_text(COLOR_ERROR, "Presentation Delay Max: invalid size"); + return; + } + + print_field(" Presentation Delay Max: %u us", pd_max); + + if (!l2cap_frame_get_le24((void *)frame, &ppd_min)) { + print_text(COLOR_ERROR, + "Preferred Presentation Delay Min: invalid size"); + return; + } + + print_field(" Preferred Presentation Delay Min: %u us", ppd_min); + + if (!l2cap_frame_get_le24((void *)frame, &ppd_max)) { + print_text(COLOR_ERROR, + "Preferred Presentation Delay Max: invalid size"); + return; + } + + print_field(" Preferred Presentation Delay Max: %u us", ppd_max); + + if (!print_codec(frame)) + return; + + cc = l2cap_frame_pull((void *)frame, frame, sizeof(*cc)); + if (!cc) { + print_text(COLOR_ERROR, + "Codec Specific Configuration: invalid size"); + return; + } + + if (!l2cap_frame_pull((void *)frame, frame, cc->len)) { + print_text(COLOR_ERROR, + "Codec Specific Configuration: invalid size"); + return; + } + + packet_print_ltv(" Codec Specific Configuration", cc->data, cc->len); +} + +static void print_framing(uint8_t value) +{ + switch (value) { + case 0x00: + print_field(" Framing: Unframed (0x%2.2x)", value); + break; + case 0x01: + print_field(" Framing: Framed (0x%2.2x)", value); + break; + default: + print_field(" Framing: Reserved (0x%2.2x)", value); + } +} + +static const struct bitfield_data phy_table[] = { + { 0, "LE 1M PHY (0x01)" }, + { 1, "LE 2M PHY (0x02)" }, + { 2, "LE Codec PHY (0x04)" }, + { } +}; + +static void print_phy(uint8_t phy) +{ + uint8_t mask; + + mask = print_bitfield(4, phy, phy_table); + if (mask) + print_text(COLOR_WHITE_BG, " Unknown fields (0x%2.2x)", + mask); +} + +static void print_ase_qos(const struct l2cap_frame *frame) +{ + uint8_t framing, phy, rtn; + uint16_t sdu, latency; + uint32_t interval, pd; + + if (!l2cap_frame_print_u8((void *)frame, " CIG ID")) + return; + + if (!l2cap_frame_print_u8((void *)frame, " CIS ID")) + return; + + if (!l2cap_frame_get_le24((void *)frame, &interval)) { + print_text(COLOR_ERROR, "SDU Interval: invalid size"); + return; + } + + print_field(" SDU Interval: %u us", interval); + + if (!l2cap_frame_get_u8((void *)frame, &framing)) { + print_text(COLOR_ERROR, "Framing: invalid size"); + return; + } + + print_framing(framing); + + if (!l2cap_frame_get_u8((void *)frame, &phy)) { + print_text(COLOR_ERROR, "PHY: invalid size"); + return; + } + + print_phy(phy); + + if (!l2cap_frame_get_le16((void *)frame, &sdu)) { + print_text(COLOR_ERROR, "Max SDU: invalid size"); + return; + } + + print_field(" Max SDU: %u", sdu); + + if (!l2cap_frame_get_u8((void *)frame, &rtn)) { + print_text(COLOR_ERROR, "RTN: invalid size"); + return; + } + + print_field(" RTN: %u", rtn); + + if (!l2cap_frame_get_le16((void *)frame, &latency)) { + print_text(COLOR_ERROR, "Max Transport Latency: invalid size"); + return; + } + + print_field(" Max Transport Latency: %u", sdu); + + if (!l2cap_frame_get_le24((void *)frame, &pd)) { + print_text(COLOR_ERROR, "Presentation Delay: invalid size"); + return; + } + + print_field(" Presentation Delay: %u us", pd); +} + +static void print_ase_metadata(const struct l2cap_frame *frame) +{ + struct bt_hci_lv_data *meta; + + if (!l2cap_frame_print_u8((void *)frame, " CIG ID")) + return; + + if (!l2cap_frame_print_u8((void *)frame, " CIS ID")) + return; + + meta = l2cap_frame_pull((void *)frame, frame, sizeof(*meta)); + if (!meta) { + print_text(COLOR_ERROR, "Metadata: invalid size"); + return; + } + + if (!l2cap_frame_pull((void *)frame, frame, meta->len)) { + print_text(COLOR_ERROR, "Metadata: invalid size"); + return; + } + + packet_print_ltv(" Metadata", meta->data, meta->len); +} + +static void print_ase_status(const struct l2cap_frame *frame) +{ + uint8_t id, state; + + if (!l2cap_frame_get_u8((void *)frame, &id)) { + print_text(COLOR_ERROR, "ASE ID: invalid size"); + goto done; + } + + print_field(" ASE ID: %u", id); + + if (!l2cap_frame_get_u8((void *)frame, &state)) { + print_text(COLOR_ERROR, "ASE State: invalid size"); + goto done; + } + + switch (state) { + /* ASE_State = 0x00 (Idle) */ + case 0x00: + print_field(" State: Idle (0x00)"); + break; + /* ASE_State = 0x01 (Codec Configured) */ + case 0x01: + print_field(" State: Codec Configured (0x01)"); + print_ase_config(frame); + break; + /* ASE_State = 0x02 (QoS Configured) */ + case 0x02: + print_field(" State: QoS Configured (0x02)"); + print_ase_qos(frame); + break; + /* ASE_Status = 0x03 (Enabling) */ + case 0x03: + print_field(" State: Enabling (0x03)"); + print_ase_metadata(frame); + break; + /* ASE_Status = 0x04 (Streaming) */ + case 0x04: + print_field(" State: Streaming (0x04)"); + print_ase_metadata(frame); + break; + /* ASE_Status = 0x05 (Disabling) */ + case 0x05: + print_field(" State: Disabling (0x05)"); + print_ase_metadata(frame); + break; + /* ASE_Status = 0x06 (Releasing) */ + case 0x06: + print_field(" State: Releasing (0x06)"); + break; + default: + print_field(" State: Reserved (0x%2.2x)", state); + break; + } + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void ase_read(const struct l2cap_frame *frame) +{ + print_ase_status(frame); +} + +static void ase_notify(const struct l2cap_frame *frame) +{ + print_ase_status(frame); +} + #define GATT_HANDLER(_uuid, _read, _write, _notify) \ { \ .uuid = { \ @@ -355,6 +666,8 @@ struct gatt_handler { void (*notify)(const struct l2cap_frame *frame); } gatt_handlers[] = { GATT_HANDLER(0x2902, ccc_read, ccc_write, NULL), + GATT_HANDLER(0x2bc4, ase_read, NULL, ase_notify), + GATT_HANDLER(0x2bc5, ase_read, NULL, ase_notify), GATT_HANDLER(0x2bc9, pac_read, NULL, pac_notify), GATT_HANDLER(0x2bcb, pac_read, NULL, pac_notify), }; diff --git a/monitor/l2cap.h b/monitor/l2cap.h index c33d4c57f..00a8ffbbd 100644 --- a/monitor/l2cap.h +++ b/monitor/l2cap.h @@ -78,6 +78,21 @@ static inline bool l2cap_frame_get_u8(struct l2cap_frame *frame, uint8_t *value) return true; } +static inline bool l2cap_frame_print_u8(struct l2cap_frame *frame, + const char *label) +{ + uint8_t u8; + + if (!l2cap_frame_get_u8(frame, &u8)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: 0x%2.2x", label, u8); + + return true; +} + static inline bool l2cap_frame_get_be16(struct l2cap_frame *frame, uint16_t *value) { @@ -92,6 +107,21 @@ static inline bool l2cap_frame_get_be16(struct l2cap_frame *frame, return true; } +static inline bool l2cap_frame_print_be16(struct l2cap_frame *frame, + const char *label) +{ + uint16_t u16; + + if (!l2cap_frame_get_be16(frame, &u16)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: 0x%4.4x", label, u16); + + return true; +} + static inline bool l2cap_frame_get_le16(struct l2cap_frame *frame, uint16_t *value) { @@ -106,6 +136,79 @@ static inline bool l2cap_frame_get_le16(struct l2cap_frame *frame, return true; } +static inline bool l2cap_frame_print_le16(struct l2cap_frame *frame, + const char *label) +{ + uint16_t u16; + + if (!l2cap_frame_get_le16(frame, &u16)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: 0x%4.4x", label, u16); + + return true; +} + +static inline bool l2cap_frame_get_be24(struct l2cap_frame *frame, + uint32_t *value) +{ + if (frame->size < sizeof(uint24_t)) + return false; + + if (value) + *value = get_be24(frame->data); + + l2cap_frame_pull(frame, frame, sizeof(uint24_t)); + + return true; +} + +static inline bool l2cap_frame_print_be24(struct l2cap_frame *frame, + const char *label) +{ + uint32_t u24; + + if (!l2cap_frame_get_be24(frame, &u24)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: 0x%6.6x", label, u24); + + return true; +} + +static inline bool l2cap_frame_get_le24(struct l2cap_frame *frame, + uint32_t *value) +{ + if (frame->size < sizeof(uint24_t)) + return false; + + if (value) + *value = get_le24(frame->data); + + l2cap_frame_pull(frame, frame, sizeof(uint24_t)); + + return true; +} + +static inline bool l2cap_frame_print_le24(struct l2cap_frame *frame, + const char *label) +{ + uint32_t u24; + + if (!l2cap_frame_get_le24(frame, &u24)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: 0x%6.6x", label, u24); + + return true; +} + static inline bool l2cap_frame_get_be32(struct l2cap_frame *frame, uint32_t *value) { @@ -120,6 +223,21 @@ static inline bool l2cap_frame_get_be32(struct l2cap_frame *frame, return true; } +static inline bool l2cap_frame_print_be32(struct l2cap_frame *frame, + const char *label) +{ + uint32_t u32; + + if (!l2cap_frame_get_be32(frame, &u32)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: 0x%8.8x", label, u32); + + return true; +} + static inline bool l2cap_frame_get_le32(struct l2cap_frame *frame, uint32_t *value) { @@ -134,6 +252,21 @@ static inline bool l2cap_frame_get_le32(struct l2cap_frame *frame, return true; } +static inline bool l2cap_frame_print_le32(struct l2cap_frame *frame, + const char *label) +{ + uint32_t u32; + + if (!l2cap_frame_get_le32(frame, &u32)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: 0x%8.8x", label, u32); + + return true; +} + static inline bool l2cap_frame_get_be64(struct l2cap_frame *frame, uint64_t *value) { @@ -148,6 +281,21 @@ static inline bool l2cap_frame_get_be64(struct l2cap_frame *frame, return true; } +static inline bool l2cap_frame_print_be64(struct l2cap_frame *frame, + const char *label) +{ + uint64_t u64; + + if (!l2cap_frame_get_be64(frame, &u64)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: 0x%zx", label, u64); + + return true; +} + static inline bool l2cap_frame_get_le64(struct l2cap_frame *frame, uint64_t *value) { @@ -162,6 +310,21 @@ static inline bool l2cap_frame_get_le64(struct l2cap_frame *frame, return true; } +static inline bool l2cap_frame_print_le64(struct l2cap_frame *frame, + const char *label) +{ + uint64_t u64; + + if (!l2cap_frame_get_le64(frame, &u64)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: 0x%zx", label, u64); + + return true; +} + static inline bool l2cap_frame_get_be128(struct l2cap_frame *frame, uint64_t *lvalue, uint64_t *rvalue) { From patchwork Thu May 26 20:59:37 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: 576271 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 EA49FC433FE for ; Thu, 26 May 2022 20:59:45 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S243351AbiEZU7p (ORCPT ); Thu, 26 May 2022 16:59:45 -0400 Received: from lindbergh.monkeyblade.net ([23.128.96.19]:44558 "EHLO lindbergh.monkeyblade.net" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S232749AbiEZU7o (ORCPT ); Thu, 26 May 2022 16:59:44 -0400 Received: from mail-pl1-x630.google.com (mail-pl1-x630.google.com [IPv6:2607:f8b0:4864:20::630]) by lindbergh.monkeyblade.net (Postfix) with ESMTPS id 6D67BE7328 for ; Thu, 26 May 2022 13:59:42 -0700 (PDT) Received: by mail-pl1-x630.google.com with SMTP id a17so2448688plb.4 for ; Thu, 26 May 2022 13:59:42 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gmail.com; s=20210112; h=from:to:subject:date:message-id:in-reply-to:references:mime-version :content-transfer-encoding; bh=X65r6hR3vAP/IZaizNOL3lQnjdFm2a7gMNwJvuzqbK0=; b=XP0JC8x/88oMicKngrikucMjUQuYD5DrlViLJVuvJq9dLdb6K9k27UFwQ1vMqD65k5 op+7FZq+OtGtQad30P/r4mHeyMk6s9/tCwzZ0Q+R3iXHOIcfeZ55X17BVMRxgtAqlXeV nQa8VIOls2gom6+pi55ZGBFHJuDUPbjNnRpubzCb3m49uug2f3JH3EgBwCk4A4qNOXXQ rkzwc4GK4WuC/gLh7wvM+GyUBOMPjCxzpW/c+UkhU3uSGAAIy5gLlsnyQvEkX8+fCCT0 +5oRbPPRZrV+yKAp3+4vyHg27o/b33XoWn5Wp7sVgQcDD3LM1jj8G+AxC08ukYY+xxmj nHkw== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20210112; h=x-gm-message-state:from:to:subject:date:message-id:in-reply-to :references:mime-version:content-transfer-encoding; bh=X65r6hR3vAP/IZaizNOL3lQnjdFm2a7gMNwJvuzqbK0=; b=Ia5D5FpNvJI1ML3W4v7BGrsG32oTmhekd85fJk6+h7mV4iSQ8LZZmuBW/ob0zWuLea eoS8eXKpVHCZ4XDa2JoBArWQIaJqDWufavkAyW1MhNCn6kmKvZ4h0zp5csAtI/7j82ZS 0iwtOv811kbVYR21/KAHP18wzguTXjRNCjCOA5wMaGmuSHd6CImxUQmzwfHo21sSb+y3 qKawpYAqmgC4jgF/IhRO68XobFo1KlOqYAAgbtM6QaFfDSVrhPXvUWauVf8awfTxSWaO mZoG/HjQudmUGfuKtSGi+jB0Z852lq9kV3XutSLRau7JMjAcefqZIvtD9R/Rar4sBmPi 1kqg== X-Gm-Message-State: AOAM531vBdZvOHLPLNwEZoFWIW+dSeEyW655i469OGnrVb/LIS5EA7FE rW41pyaw3CzEGoa4Uux8vB5fRsc9ZJg= X-Google-Smtp-Source: ABdhPJxbtM19/r3+1DQfgOh4iymTNalkSdX13dBAg2qeSgUuYhw0sw7tnEI1lmeKw0FTaIWbtsdRiw== X-Received: by 2002:a17:902:e741:b0:163:62e2:6f04 with SMTP id p1-20020a170902e74100b0016362e26f04mr8532919plf.0.1653598781336; Thu, 26 May 2022 13:59:41 -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 z6-20020a170903018600b0015e8d4eb25bsm2080064plg.165.2022.05.26.13.59.40 for (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Thu, 26 May 2022 13:59:40 -0700 (PDT) From: Luiz Augusto von Dentz To: linux-bluetooth@vger.kernel.org Subject: [PATCH v2 4/4] monitor/att: Add decoding support for ASE Control Point Date: Thu, 26 May 2022 13:59:37 -0700 Message-Id: <20220526205937.4159665-4-luiz.dentz@gmail.com> X-Mailer: git-send-email 2.35.1 In-Reply-To: <20220526205937.4159665-1-luiz.dentz@gmail.com> References: <20220526205937.4159665-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 decoding support for ASE Control Point attribute: > ACL Data RX: Handle 42 flags 0x02 dlen 30 Channel: 64 len 26 sdu 24 [PSM 39 mode Enhanced Credit (0x81)] {chan 1} ATT: Write Command (0x52) len 23 Handle: 0x0030 Type: ASE Control Point (0x2bc6) Data: 010103020206000000000a02010302020103042800 Opcode: Codec Configuration (0x01) Number of ASE(s): 1 ASE: #0 ASE ID: 0x03 Target Latency: Balance Latency/Reliability (0x02) PHY: 0x02 LE 2M PHY (0x02) Codec: LC3 (0x06) Codec Specific Configuration #0: len 0x02 type 0x01 Codec Specific Configuration: 03 Codec Specific Configuration #1: len 0x02 type 0x02 Codec Specific Configuration: 01 Codec Specific Configuration #2: len 0x03 type 0x04 Codec Specific Configuration: 2800 < ACL Data TX: Handle 42 flags 0x00 dlen 55 Channel: 64 len 51 sdu 49 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Handle Multiple Value Notification (0x23) len 48 Length: 0x0005 Handle: 0x0030 Type: ASE Control Point (0x2bc6) Data: 0101030000 Opcode: Codec Configuration (0x01) Number of ASE(s): 1 ASE: #0 ASE ID: 0x03 ASE Response Code: Success (0x00) ASE Response Reason: None (0x00) > ACL Data RX: Handle 42 flags 0x02 dlen 27 Channel: 64 len 23 sdu 21 [PSM 39 mode Enhanced Credit (0x81)] {chan 1} ATT: Write Command (0x52) len 20 Handle: 0x0030 Type: ASE Control Point (0x2bc6) Data: 020103000010270000022800020a00409c00 Opcode: QoS Configuration (0x02) Number of ASE(s): 1 ASE: #0 ASE ID: 0x03 CIG ID: 0x00 CIS ID: 0x00 SDU Interval: 10000 usec Framing: Unframed (0x00) PHY: 0x02 LE 2M PHY (0x02) Max SDU: 40 RTN: 2 Max Transport Latency: 10 Presentation Delay: 40000 us < ACL Data TX: Handle 42 flags 0x00 dlen 37 Channel: 64 len 33 sdu 31 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Handle Multiple Value Notification (0x23) len 30 Length: 0x0005 Handle: 0x0030 Type: ASE Control Point (0x2bc6) Data: 0201030000 Opcode: QoS Configuration (0x02) Number of ASE(s): 1 ASE: #0 ASE ID: 0x03 ASE Response Code: Success (0x00) ASE Response Reason: None (0x00) > ACL Data RX: Handle 42 flags 0x02 dlen 17 Channel: 64 len 13 sdu 11 [PSM 39 mode Enhanced Credit (0x81)] {chan 1} ATT: Write Command (0x52) len 10 Handle: 0x0030 Type: ASE Control Point (0x2bc6) Data: 0301030403020200 Opcode: Enable (0x03) Number of ASE(s): 1 ASE: #0 ASE ID: 0x03 Metadata #0: len 0x03 type 0x02 Metadata: 0200 < ACL Data TX: Handle 42 flags 0x00 dlen 33 Channel: 64 len 29 sdu 27 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Handle Multiple Value Notification (0x23) len 26 Length: 0x0005 Handle: 0x0030 Type: ASE Control Point (0x2bc6) Data: 0301030000 Opcode: Enable (0x03) Number of ASE(s): 1 ASE: #0 ASE ID: 0x03 ASE Response Code: Success (0x00) ASE Response Reason: None (0x00) > ACL Data RX: Handle 42 flags 0x02 dlen 12 Channel: 64 len 8 sdu 6 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Write Command (0x52) len 5 Handle: 0x0030 Type: ASE Control Point (0x2bc6) Data: 050101 Opcode: Disable (0x05) Number of ASE(s): 1 --- monitor/att.c | 773 ++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 588 insertions(+), 185 deletions(-) diff --git a/monitor/att.c b/monitor/att.c index b39ac5a49..152a5f320 100644 --- a/monitor/att.c +++ b/monitor/att.c @@ -247,7 +247,7 @@ static void ccc_write(const struct l2cap_frame *frame) print_ccc_value(frame); } -static bool print_codec(const struct l2cap_frame *frame) +static bool print_ase_codec(const struct l2cap_frame *frame) { uint8_t codec_id; uint16_t codec_cid, codec_vid; @@ -279,6 +279,36 @@ static bool print_codec(const struct l2cap_frame *frame) return true; } +static bool print_ase_lv(const struct l2cap_frame *frame, const char *label) +{ + struct bt_hci_lv_data *lv; + + lv = l2cap_frame_pull((void *)frame, frame, sizeof(*lv)); + if (!lv) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + if (!l2cap_frame_pull((void *)frame, frame, lv->len)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + packet_print_ltv(label, lv->data, lv->len); + + return true; +} + +static bool print_ase_cc(const struct l2cap_frame *frame) +{ + return print_ase_lv(frame, " Codec Specific Configuration"); +} + +static bool print_ase_metadata(const struct l2cap_frame *frame) +{ + return print_ase_lv(frame, " Metadata"); +} + static void print_pac(const struct l2cap_frame *frame) { uint8_t num = 0, i; @@ -291,42 +321,16 @@ static void print_pac(const struct l2cap_frame *frame) print_field(" Number of PAC(s): %u", num); for (i = 0; i < num; i++) { - struct bt_hci_lv_data *cc; - struct bt_hci_lv_data *meta; - print_field(" PAC #%u:", i); - if (!print_codec(frame)) + if (!print_ase_codec(frame)) goto done; - cc = l2cap_frame_pull((void *)frame, frame, sizeof(*cc)); - if (!cc) { - print_text(COLOR_ERROR, - "Codec Specific Configuration: invalid size"); - goto done; - } + if (!print_ase_cc(frame)) + break; - if (!l2cap_frame_pull((void *)frame, frame, cc->len)) { - print_text(COLOR_ERROR, - "Codec Specific Configuration: invalid size"); - goto done; - } - - packet_print_ltv(" Codec Specific Configuration", cc->data, - cc->len); - - meta = l2cap_frame_pull((void *)frame, frame, sizeof(*meta)); - if (!meta) { - print_text(COLOR_ERROR, "Metadata: invalid size"); - goto done; - } - - if (!l2cap_frame_pull((void *)frame, frame, meta->len)) { - print_text(COLOR_ERROR, "Metadata: invalid size"); - goto done; - } - - packet_print_ltv(" Metadata", meta->data, meta->len); + if (!print_ase_metadata(frame)) + break; } done: @@ -344,20 +348,28 @@ static void pac_notify(const struct l2cap_frame *frame) print_pac(frame); } -static void print_prefer_framing(uint8_t value) +static bool print_prefer_framing(const struct l2cap_frame *frame) { - switch (value) { - case 0x00: - print_field(" Framing: Unframed ISOAL PDUs supported " - "(0x%2.2x)", value); - return; - case 0x01: - print_field(" Framing: Unframed ISOAL PDUs not supported " - "(0x%2.2x)", value); - return; - default: - print_field(" Framing: Reserved (0x%2.2x)", value); + uint8_t framing; + + if (!l2cap_frame_get_u8((void *)frame, &framing)) { + print_text(COLOR_ERROR, " Framing: invalid size"); + return false; } + + switch (framing) { + case 0x00: + print_field(" Framing: Unframed PDUs supported (0x00)"); + break; + case 0x01: + print_field(" Framing: Unframed PDUs not supported (0x01)"); + break; + default: + print_field(" Framing: Reserved (0x%2.2x)", framing); + break; + } + + return true; } static const struct bitfield_data prefer_phy_table[] = { @@ -367,112 +379,122 @@ static const struct bitfield_data prefer_phy_table[] = { { } }; -static void print_prefer_phy(uint8_t phy) +static bool print_prefer_phy(const struct l2cap_frame *frame) { - uint8_t mask; + uint8_t phy, mask; + + if (!l2cap_frame_get_u8((void *)frame, &phy)) { + print_text(COLOR_ERROR, "PHY: invalid size"); + return false; + } + + print_field(" PHY: 0x%2.2x", phy); mask = print_bitfield(4, phy, prefer_phy_table); if (mask) print_text(COLOR_WHITE_BG, " Unknown fields (0x%2.2x)", mask); + + return true; +} + +static bool print_ase_rtn(const struct l2cap_frame *frame, const char *label) +{ + uint8_t rtn; + + if (!l2cap_frame_get_u8((void *)frame, &rtn)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: %u", label, rtn); + + return true; +} + +static bool print_ase_latency(const struct l2cap_frame *frame, + const char *label) +{ + uint16_t latency; + + if (!l2cap_frame_get_le16((void *)frame, &latency)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: %u", label, latency); + + return true; +} + +static bool print_ase_pd(const struct l2cap_frame *frame, const char *label) +{ + uint32_t pd; + + if (!l2cap_frame_get_le24((void *)frame, &pd)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: %u us", label, pd); + + return true; } static void print_ase_config(const struct l2cap_frame *frame) { - uint8_t framing, phy, rtn; - uint16_t latency; - uint32_t pd_min, pd_max, ppd_min, ppd_max; - struct bt_hci_lv_data *cc; - - if (!l2cap_frame_get_u8((void *)frame, &framing)) { - print_text(COLOR_ERROR, "Framing: invalid size"); - return; - } - - print_prefer_framing(framing); - - if (!l2cap_frame_get_u8((void *)frame, &phy)) { - print_text(COLOR_ERROR, "PHY: invalid size"); - return; - } - - print_prefer_phy(phy); - - if (!l2cap_frame_get_u8((void *)frame, &rtn)) { - print_text(COLOR_ERROR, "RTN: invalid size"); - return; - } - - print_field(" RTN: %u", rtn); - - if (!l2cap_frame_get_le16((void *)frame, &latency)) { - print_text(COLOR_ERROR, "RTN: invalid size"); - return; - } - - print_field(" Max Transport Latency: %u ms", latency); - - if (!l2cap_frame_get_le24((void *)frame, &pd_min)) { - print_text(COLOR_ERROR, "Presentation Delay Min: invalid size"); - return; - } - - print_field(" Presentation Delay Min: %u us", pd_min); - - if (!l2cap_frame_get_le24((void *)frame, &pd_max)) { - print_text(COLOR_ERROR, "Presentation Delay Max: invalid size"); - return; - } - - print_field(" Presentation Delay Max: %u us", pd_max); - - if (!l2cap_frame_get_le24((void *)frame, &ppd_min)) { - print_text(COLOR_ERROR, - "Preferred Presentation Delay Min: invalid size"); - return; - } - - print_field(" Preferred Presentation Delay Min: %u us", ppd_min); - - if (!l2cap_frame_get_le24((void *)frame, &ppd_max)) { - print_text(COLOR_ERROR, - "Preferred Presentation Delay Max: invalid size"); - return; - } - - print_field(" Preferred Presentation Delay Max: %u us", ppd_max); - - if (!print_codec(frame)) + if (!print_prefer_framing(frame)) return; - cc = l2cap_frame_pull((void *)frame, frame, sizeof(*cc)); - if (!cc) { - print_text(COLOR_ERROR, - "Codec Specific Configuration: invalid size"); + if (!print_prefer_phy(frame)) return; - } - if (!l2cap_frame_pull((void *)frame, frame, cc->len)) { - print_text(COLOR_ERROR, - "Codec Specific Configuration: invalid size"); + if (!print_ase_rtn(frame, " RTN")) return; - } - packet_print_ltv(" Codec Specific Configuration", cc->data, cc->len); + if (!print_ase_latency(frame, " Max Transport Latency")) + return; + + if (!print_ase_pd(frame, " Presentation Delay Min")) + return; + + if (!print_ase_pd(frame, " Presentation Delay Max")) + return; + + if (!print_ase_pd(frame, " Preferred Presentation Delay Min")) + return; + + if (!print_ase_pd(frame, " Preferred Presentation Delay Max")) + return; + + if (!print_ase_codec(frame)) + return; + + print_ase_cc(frame); } -static void print_framing(uint8_t value) +static bool print_ase_framing(const struct l2cap_frame *frame, + const char *label) { - switch (value) { + uint8_t framing; + + if (!l2cap_frame_get_u8((void *)frame, &framing)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + switch (framing) { case 0x00: - print_field(" Framing: Unframed (0x%2.2x)", value); + print_field("%s: Unframed (0x00)", label); break; case 0x01: - print_field(" Framing: Framed (0x%2.2x)", value); + print_field("%s: Framed (0x01)", label); break; default: - print_field(" Framing: Reserved (0x%2.2x)", value); + print_field("%s: Reserved (0x%2.2x)", label, framing); } + + return true; } static const struct bitfield_data phy_table[] = { @@ -482,100 +504,92 @@ static const struct bitfield_data phy_table[] = { { } }; -static void print_phy(uint8_t phy) +static bool print_ase_phy(const struct l2cap_frame *frame, const char *label) { - uint8_t mask; + uint8_t phy, mask; + + if (!l2cap_frame_get_u8((void *)frame, &phy)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: 0x%2.2x", label, phy); mask = print_bitfield(4, phy, phy_table); if (mask) print_text(COLOR_WHITE_BG, " Unknown fields (0x%2.2x)", mask); + + return true; +} + +static bool print_ase_interval(const struct l2cap_frame *frame, + const char *label) +{ + uint32_t interval; + + if (!l2cap_frame_get_le24((void *)frame, &interval)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: %u usec", label, interval); + + return true; +} + +static bool print_ase_sdu(const struct l2cap_frame *frame, const char *label) +{ + uint16_t sdu; + + if (!l2cap_frame_get_le16((void *)frame, &sdu)) { + print_text(COLOR_ERROR, "%s: invalid size", label); + return false; + } + + print_field("%s: %u", label, sdu); + + return true; } static void print_ase_qos(const struct l2cap_frame *frame) { - uint8_t framing, phy, rtn; - uint16_t sdu, latency; - uint32_t interval, pd; - if (!l2cap_frame_print_u8((void *)frame, " CIG ID")) return; if (!l2cap_frame_print_u8((void *)frame, " CIS ID")) return; - if (!l2cap_frame_get_le24((void *)frame, &interval)) { - print_text(COLOR_ERROR, "SDU Interval: invalid size"); + if (!print_ase_interval(frame, " SDU Interval")) return; - } - print_field(" SDU Interval: %u us", interval); - - if (!l2cap_frame_get_u8((void *)frame, &framing)) { - print_text(COLOR_ERROR, "Framing: invalid size"); + if (!print_ase_framing(frame, " Framing")) return; - } - print_framing(framing); - - if (!l2cap_frame_get_u8((void *)frame, &phy)) { - print_text(COLOR_ERROR, "PHY: invalid size"); + if (!print_ase_phy(frame, " PHY")) return; - } - print_phy(phy); - - if (!l2cap_frame_get_le16((void *)frame, &sdu)) { - print_text(COLOR_ERROR, "Max SDU: invalid size"); + if (!print_ase_sdu(frame, " Max SDU")) return; - } - print_field(" Max SDU: %u", sdu); - - if (!l2cap_frame_get_u8((void *)frame, &rtn)) { - print_text(COLOR_ERROR, "RTN: invalid size"); + if (!print_ase_rtn(frame, " RTN")) return; - } - print_field(" RTN: %u", rtn); - - if (!l2cap_frame_get_le16((void *)frame, &latency)) { - print_text(COLOR_ERROR, "Max Transport Latency: invalid size"); + if (!print_ase_latency(frame, " Max Transport Latency")) return; - } - print_field(" Max Transport Latency: %u", sdu); - - if (!l2cap_frame_get_le24((void *)frame, &pd)) { - print_text(COLOR_ERROR, "Presentation Delay: invalid size"); - return; - } - - print_field(" Presentation Delay: %u us", pd); + print_ase_pd(frame, " Presentation Delay"); } -static void print_ase_metadata(const struct l2cap_frame *frame) +static void print_ase_metadata_status(const struct l2cap_frame *frame) { - struct bt_hci_lv_data *meta; - if (!l2cap_frame_print_u8((void *)frame, " CIG ID")) return; if (!l2cap_frame_print_u8((void *)frame, " CIS ID")) return; - meta = l2cap_frame_pull((void *)frame, frame, sizeof(*meta)); - if (!meta) { - print_text(COLOR_ERROR, "Metadata: invalid size"); - return; - } - - if (!l2cap_frame_pull((void *)frame, frame, meta->len)) { - print_text(COLOR_ERROR, "Metadata: invalid size"); - return; - } - - packet_print_ltv(" Metadata", meta->data, meta->len); + print_ase_metadata(frame); } static void print_ase_status(const struct l2cap_frame *frame) @@ -612,17 +626,17 @@ static void print_ase_status(const struct l2cap_frame *frame) /* ASE_Status = 0x03 (Enabling) */ case 0x03: print_field(" State: Enabling (0x03)"); - print_ase_metadata(frame); + print_ase_metadata_status(frame); break; /* ASE_Status = 0x04 (Streaming) */ case 0x04: print_field(" State: Streaming (0x04)"); - print_ase_metadata(frame); + print_ase_metadata_status(frame); break; /* ASE_Status = 0x05 (Disabling) */ case 0x05: print_field(" State: Disabling (0x05)"); - print_ase_metadata(frame); + print_ase_metadata_status(frame); break; /* ASE_Status = 0x06 (Releasing) */ case 0x06: @@ -648,6 +662,394 @@ static void ase_notify(const struct l2cap_frame *frame) print_ase_status(frame); } +static bool print_ase_target_latency(const struct l2cap_frame *frame) +{ + uint8_t latency; + + if (!l2cap_frame_get_u8((void *)frame, &latency)) { + print_text(COLOR_ERROR, " Target Latency: invalid size"); + return false; + } + + switch (latency) { + case 0x01: + print_field(" Target Latency: Low Latency (0x01)"); + break; + case 0x02: + print_field(" Target Latency: Balance Latency/Reliability " + "(0x02)"); + break; + case 0x03: + print_field(" Target Latency: High Reliability (0x03)"); + break; + default: + print_field(" Target Latency: Reserved (0x%2.2x)", latency); + break; + } + + return true; +} + +static bool ase_config_cmd(const struct l2cap_frame *frame) +{ + if (!l2cap_frame_print_u8((void *)frame, " ASE ID")) + return false; + + if (!print_ase_target_latency(frame)) + return false; + + if (!print_ase_phy(frame, " PHY")) + return false; + + if (!print_ase_codec(frame)) + return false; + + if (!print_ase_cc(frame)) + return false; + + return true; +} + +static bool ase_qos_cmd(const struct l2cap_frame *frame) +{ + if (!l2cap_frame_print_u8((void *)frame, " ASE ID")) + return false; + + if (!l2cap_frame_print_u8((void *)frame, " CIG ID")) + return false; + + if (!l2cap_frame_print_u8((void *)frame, " CIS ID")) + return false; + + if (!print_ase_interval(frame, " SDU Interval")) + return false; + + if (!print_ase_framing(frame, " Framing")) + return false; + + if (!print_ase_phy(frame, " PHY")) + return false; + + if (!print_ase_sdu(frame, " Max SDU")) + return false; + + if (!print_ase_rtn(frame, " RTN")) + return false; + + if (!print_ase_latency(frame, " Max Transport Latency")) + return false; + + if (!print_ase_pd(frame, " Presentation Delay")) + return false; + + return true; +} + +static bool ase_enable_cmd(const struct l2cap_frame *frame) +{ + if (!l2cap_frame_print_u8((void *)frame, " ASE ID")) + return false; + + if (!print_ase_metadata(frame)) + return false; + + return true; +} + +static bool ase_start_cmd(const struct l2cap_frame *frame) +{ + if (!l2cap_frame_print_u8((void *)frame, " ASE ID")) + return false; + + return true; +} + +static bool ase_disable_cmd(const struct l2cap_frame *frame) +{ + if (!l2cap_frame_print_u8((void *)frame, " ASE ID")) + return false; + + return true; +} + +static bool ase_stop_cmd(const struct l2cap_frame *frame) +{ + if (!l2cap_frame_print_u8((void *)frame, " ASE ID")) + return false; + + return true; +} + +static bool ase_metadata_cmd(const struct l2cap_frame *frame) +{ + if (!l2cap_frame_print_u8((void *)frame, " ASE ID")) + return false; + + if (!print_ase_metadata(frame)) + return false; + + return true; +} + +static bool ase_release_cmd(const struct l2cap_frame *frame) +{ + if (!l2cap_frame_print_u8((void *)frame, " ASE ID")) + return false; + + return true; +} + +#define ASE_CMD(_op, _desc, _func) \ +[_op] = { \ + .desc = _desc, \ + .func = _func, \ +} + +struct ase_cmd { + const char *desc; + bool (*func)(const struct l2cap_frame *frame); +} ase_cmd_table[] = { + /* Opcode = 0x01 (Codec Configuration) */ + ASE_CMD(0x01, "Codec Configuration", ase_config_cmd), + /* Opcode = 0x02 (QoS Configuration) */ + ASE_CMD(0x02, "QoS Configuration", ase_qos_cmd), + /* Opcode = 0x03 (Enable) */ + ASE_CMD(0x03, "Enable", ase_enable_cmd), + /* Opcode = 0x04 (Receiver Start Ready) */ + ASE_CMD(0x04, "Receiver Start Ready", ase_start_cmd), + /* Opcode = 0x05 (Disable) */ + ASE_CMD(0x05, "Disable", ase_disable_cmd), + /* Opcode = 0x06 (Receiver Stop Ready) */ + ASE_CMD(0x06, "Receiver Stop Ready", ase_stop_cmd), + /* Opcode = 0x07 (Update Metadata) */ + ASE_CMD(0x07, "Update Metadata", ase_metadata_cmd), + /* Opcode = 0x08 (Release) */ + ASE_CMD(0x08, "Release", ase_release_cmd), +}; + +static struct ase_cmd *ase_get_cmd(uint8_t op) +{ + if (op > ARRAY_SIZE(ase_cmd_table)) + return NULL; + + return &ase_cmd_table[op]; +} + +static void print_ase_cmd(const struct l2cap_frame *frame) +{ + uint8_t op, num, i; + struct ase_cmd *cmd; + + if (!l2cap_frame_get_u8((void *)frame, &op)) { + print_text(COLOR_ERROR, "opcode: invalid size"); + goto done; + } + + if (!l2cap_frame_get_u8((void *)frame, &num)) { + print_text(COLOR_ERROR, "num: invalid size"); + goto done; + } + + cmd = ase_get_cmd(op); + if (!cmd) { + print_field(" Opcode: Reserved (0x%2.2x)", op); + goto done; + } + + print_field(" Opcode: %s (0x%2.2x)", cmd->desc, op); + print_field(" Number of ASE(s): %u", num); + + for (i = 0; i < num && frame->size; i++) { + print_field(" ASE: #%u", i); + + if (!cmd->func(frame)) + break; + } + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void ase_cp_write(const struct l2cap_frame *frame) +{ + print_ase_cmd(frame); +} + +static bool print_ase_cp_rsp_code(const struct l2cap_frame *frame) +{ + uint8_t code; + + if (!l2cap_frame_get_u8((void *)frame, &code)) { + print_text(COLOR_ERROR, " ASE Response Code: invalid size"); + return false; + } + + switch (code) { + case 0x00: + print_field(" ASE Response Code: Success (0x00)"); + break; + case 0x01: + print_field(" ASE Response Code: Unsupported Opcode (0x01)"); + break; + case 0x02: + print_field(" ASE Response Code: Invalid Length (0x02)"); + break; + case 0x03: + print_field(" ASE Response Code: Invalid ASE ID (0x03)"); + break; + case 0x04: + print_field(" ASE Response Code: Invalid ASE State (0x04)"); + break; + case 0x05: + print_field(" ASE Response Code: Invalid ASE Direction " + "(0x05)"); + break; + case 0x06: + print_field(" ASE Response Code: Unsupported Audio " + "Capabilities (0x06)"); + break; + case 0x07: + print_field(" ASE Response Code: Unsupported Configuration " + "(0x07)"); + break; + case 0x08: + print_field(" ASE Response Code: Rejected Configuration " + "(0x08)"); + break; + case 0x09: + print_field(" ASE Response Code: Invalid Configuration " + "(0x09)"); + break; + case 0x0a: + print_field(" ASE Response Code: Unsupported Metadata " + "(0x0a)"); + break; + case 0x0b: + print_field(" ASE Response Code: Rejected Metadata (0x0b)"); + break; + case 0x0c: + print_field(" ASE Response Code: Invalid Metadata (0x0c)"); + break; + case 0x0d: + print_field(" ASE Response Code: Insufficient Resources " + "(0x0d)"); + break; + case 0x0e: + print_field(" ASE Response Code: Unspecified Error (0x0e)"); + break; + default: + print_field(" ASE Response Code: Reserved (0x%2.2x)", code); + break; + } + + return true; +} + +static bool print_ase_cp_rsp_reason(const struct l2cap_frame *frame) +{ + uint8_t reason; + + if (!l2cap_frame_get_u8((void *)frame, &reason)) { + print_text(COLOR_ERROR, + " ASE Response Reason: invalid size"); + return false; + } + + switch (reason) { + case 0x00: + print_field(" ASE Response Reason: None (0x00)"); + break; + case 0x01: + print_field(" ASE Response Reason: ASE ID (0x01)"); + break; + case 0x02: + print_field(" ASE Response Reason: Codec Specific " + "Configuration (0x02)"); + break; + case 0x03: + print_field(" ASE Response Reason: SDU Interval (0x03)"); + break; + case 0x04: + print_field(" ASE Response Reason: Framing (0x04)"); + break; + case 0x05: + print_field(" ASE Response Reason: PHY (0x05)"); + break; + case 0x06: + print_field(" ASE Response Reason: Max SDU (0x06)"); + break; + case 0x07: + print_field(" ASE Response Reason: RTN (0x07)"); + break; + case 0x08: + print_field(" ASE Response Reason: Max Transport Latency " + "(0x08)"); + break; + case 0x09: + print_field(" ASE Response Reason: Presentation Delay " + "(0x09)"); + break; + case 0x0a: + print_field(" ASE Response Reason: Invalid ASE/CIS Mapping " + "(0x0a)"); + break; + default: + print_field(" ASE Response Reason: Reserved (0x%2.2x)", + reason); + break; + } + + return true; +} + +static void print_ase_cp_rsp(const struct l2cap_frame *frame) +{ + uint8_t op, num, i; + struct ase_cmd *cmd; + + if (!l2cap_frame_get_u8((void *)frame, &op)) { + print_text(COLOR_ERROR, " opcode: invalid size"); + goto done; + } + + if (!l2cap_frame_get_u8((void *)frame, &num)) { + print_text(COLOR_ERROR, " Number of ASE(s): invalid size"); + goto done; + } + + cmd = ase_get_cmd(op); + if (!cmd) { + print_field(" Opcode: Reserved (0x%2.2x)", op); + goto done; + } + + print_field(" Opcode: %s (0x%2.2x)", cmd->desc, op); + print_field(" Number of ASE(s): %u", num); + + for (i = 0; i < num && frame->size; i++) { + print_field(" ASE: #%u", i); + + if (!l2cap_frame_print_u8((void *)frame, " ASE ID")) + break; + + if (!print_ase_cp_rsp_code(frame)) + break; + + if (!print_ase_cp_rsp_reason(frame)) + break; + } + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void ase_cp_notify(const struct l2cap_frame *frame) +{ + print_ase_cp_rsp(frame); +} + #define GATT_HANDLER(_uuid, _read, _write, _notify) \ { \ .uuid = { \ @@ -668,6 +1070,7 @@ struct gatt_handler { GATT_HANDLER(0x2902, ccc_read, ccc_write, NULL), GATT_HANDLER(0x2bc4, ase_read, NULL, ase_notify), GATT_HANDLER(0x2bc5, ase_read, NULL, ase_notify), + GATT_HANDLER(0x2bc6, NULL, ase_cp_write, ase_cp_notify), GATT_HANDLER(0x2bc9, pac_read, NULL, pac_notify), GATT_HANDLER(0x2bcb, pac_read, NULL, pac_notify), };