From mboxrd@z Thu Jan 1 00:00:00 1970 Received: from mx0a-0031df01.pphosted.com (mx0a-0031df01.pphosted.com [205.220.168.131]) (using TLSv1.2 with cipher ECDHE-RSA-AES256-GCM-SHA384 (256/256 bits)) (No client certificate requested) by smtp.subspace.kernel.org (Postfix) with ESMTPS id BF51B224F3 for ; Thu, 16 Apr 2026 04:20:41 +0000 (UTC) Authentication-Results: smtp.subspace.kernel.org; arc=none smtp.client-ip=205.220.168.131 ARC-Seal:i=1; a=rsa-sha256; d=subspace.kernel.org; s=arc-20240116; t=1776313244; cv=none; b=EMzNzb/wY4BdKN9H1UnF+KMoqsa0rlSXM94nt7gWTp15Lm0bHX/wCfKz0nJZ/HyfreC4ZENjRScqo1+Fr1+QeRv5mXuMPx6/Dia7SZAxHpshE8f1X10dxgCG3SFh2Jf5rfsuVjeMf2YjWTGYHrtTBVnja/6IR8vDNnshBU/a5vY= ARC-Message-Signature:i=1; a=rsa-sha256; d=subspace.kernel.org; s=arc-20240116; t=1776313244; c=relaxed/simple; bh=+NXY6xBGv0kOZ4g2kNWukCfvvQAP5QA/urDcDIqZgSA=; h=From:To:Cc:Subject:Date:Message-Id:In-Reply-To:References: MIME-Version:Content-Type; b=Q3eHToo2gw3rR6b70iq25XZnQJglxkguyb9G/FP4Oon6QQQj4cApWfzCVqWBvd7IUXExWdOh/nxE6XmMOeNmDxGbd6MDH7jDg1bi6eJOas3q+zeqoqNqRRUMylIGCeWTh+PzbBaJXpNhgs5g5mGdX7c9rq5qWM2usT/WlVl9wR8= ARC-Authentication-Results:i=1; smtp.subspace.kernel.org; dmarc=pass (p=reject dis=none) header.from=oss.qualcomm.com; spf=pass smtp.mailfrom=oss.qualcomm.com; dkim=pass (2048-bit key) header.d=qualcomm.com header.i=@qualcomm.com header.b=gY3le0VS; dkim=pass (2048-bit key) header.d=oss.qualcomm.com header.i=@oss.qualcomm.com header.b=bL1eJfRk; arc=none smtp.client-ip=205.220.168.131 Authentication-Results: smtp.subspace.kernel.org; dmarc=pass (p=reject dis=none) header.from=oss.qualcomm.com Authentication-Results: smtp.subspace.kernel.org; spf=pass smtp.mailfrom=oss.qualcomm.com Authentication-Results: smtp.subspace.kernel.org; dkim=pass (2048-bit key) header.d=qualcomm.com header.i=@qualcomm.com header.b="gY3le0VS"; dkim=pass (2048-bit key) header.d=oss.qualcomm.com header.i=@oss.qualcomm.com header.b="bL1eJfRk" Received: from pps.filterd (m0279867.ppops.net [127.0.0.1]) by mx0a-0031df01.pphosted.com (8.18.1.11/8.18.1.11) with ESMTP id 63FNxGrC3733636 for ; Thu, 16 Apr 2026 04:20:41 GMT DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=qualcomm.com; h= cc:content-transfer-encoding:content-type:date:from:in-reply-to :message-id:mime-version:references:subject:to; s=qcppdkim1; bh= KgJPRb1ry0E4vRzORt7mo22rskoQ+YL5iOXPSNsv4Ss=; b=gY3le0VS03qzJiRl mqCcuAKYyO/eRjifbSo/p8lSsleNMOdKB6FNv09L/RgJIiToL0F2WMbrZQXgKOzQ 75V5yKLihNrj2F8PYQLAi8M4kIgzFhqphPgYWshMrExLf7Wksjnli7BhmczCulFr zOUVnUSZwbdn9L+pmulaX+K8MHH8l/GJLtXxkUuyhi1GHvpCXD6kmRWOG/ToaVS9 BcozzBnABbpbHjggmaY/pHf7VZ0O/7m8nztHMwWEqP69sWzoH7br0N0hlve/kniZ aWw3CAAJSEGZ8iocJpZw3vVqaEITS3LvcwVJ8ZZu01aqx6KJyn43d4Y9cfCdDhQH HKUrfg== Received: from mail-pj1-f71.google.com (mail-pj1-f71.google.com [209.85.216.71]) by mx0a-0031df01.pphosted.com (PPS) with ESMTPS id 4djcqwjajk-1 (version=TLSv1.3 cipher=TLS_AES_128_GCM_SHA256 bits=128 verify=NOT) for ; Thu, 16 Apr 2026 04:20:40 +0000 (GMT) Received: by mail-pj1-f71.google.com with SMTP id 98e67ed59e1d1-35449510446so8222402a91.0 for ; Wed, 15 Apr 2026 21:20:40 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=oss.qualcomm.com; s=google; t=1776313240; x=1776918040; darn=vger.kernel.org; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:cc:to:from:from:to:cc:subject:date :message-id:reply-to; bh=KgJPRb1ry0E4vRzORt7mo22rskoQ+YL5iOXPSNsv4Ss=; b=bL1eJfRkILmo/EehsZdaT6pp8+VXVGRaKzRMkZ9q7m/sXumGkQqHVs58BJhqxxzzxs pcoNY3Vx6jeZu72OuLP45j/4fcb5OzhJ57B299uEi3crKUtSTP19UrR69sAx5dZ+XKZW vHbOrhVpyP+tj/mhifgP1FkIiu16B3X+zoDrPdAErObmk9WPkMZGars7IQpDnpnv42+o AOhA/6HX44f0sp6h3lwhmF65WutoyL9WgEGBu+fMJcFb/26ISdEq4RxHwV/9H+dV6nld NWOenuRq+++wB1GwUtcSAqs5jLgXcfCDrwSbjXYNUlmbzNzgTYc/P7paoFJ7z89fJ9dg XHZQ== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20251104; t=1776313240; x=1776918040; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:cc:to:from:x-gm-gg:x-gm-message-state:from :to:cc:subject:date:message-id:reply-to; bh=KgJPRb1ry0E4vRzORt7mo22rskoQ+YL5iOXPSNsv4Ss=; b=iHM9eq6qcrQYWGdoOyQvN5sGOrzyLLJH/isFkCk/JnzTPSxqLu3CbDm6Y9mRh3t5Gv HBmCdJ2ItuQWRDmm5TTbqvwdr8oUOlV6h3XE1FLXJP1vYnkZ9bE1JByXxry7ITvvqTtT bXqaYcNzR06ywHaptkKm0LUkWG6EjFZl1pQZP1yc5e5fAsnvRlUrbACJ0xzmxksX+j4s 0tBLs3/+kRVs2R9xeBLyXnHyrYw5E4ThR9FF+ryODOrchEz8kbLgzVXunEyucsvqHNKu hXIS7gBYQ6faKwZW4dA80jgU9ih7B8s0HScNTIl6l5BDt6OzuvCXgx5dIqn1Umy8x98F eUVg== X-Gm-Message-State: AOJu0YyClGZ399UlFgj7MFpXIjpvP6GiaN1Rg0ccba3QtrduQ1oCOU7s 7QUyjJAxdOhzcwq3tby1qiNX2nX72jWosoC9NnIz3t1cY/UajL5c5eDzNtdtnnm7JHFRcFkp9jB 9s95jhDgHnyGIJYxMtzZ25MMnjB87a4uRwlXHH2yss7rM0nOOakeZkrIbX/o2OJiyoPNCH2Wt+6 /kKFY= X-Gm-Gg: AeBDietfNbvd34xoMMMoXSGi431xtfxtx5Wfe3zg5GJMiugqrDZ+p4Fo9mpetC/7/b5 CzXaL20U4/zgPBrx4VI7dPeKjPOQa+IZ3fBuDjZM8+P0LHsnaIC19v1Rcg4UQMyjopCxA3OScn0 KtAUeR9EMEPLCk9yVfbgjq5sqoAfo0usv+EcJHQB+WVNqoTFMN9IYBF3EVcSnoI7erQtU79GiRI AD+CgpxZroTDQ/PUNUspdQKNDGIadqpsZwbHQtS8rxyr9c3VkXbhL7Gpsh6trKK/7LN5UrCe7uv UAtB6mU94SSdeMtNsHhA2uR1Qv3TVKpHI0CHSKzRYHt3QCwkxsCbYWGNMy13n61h43hUvoRxMZi q591C/j4Kv4sP2gMBGvYW6/hFfen9oyBZAdqqCsIfDdn/jcmJbUg= X-Received: by 2002:a17:903:748:b0:2b4:5309:2c14 with SMTP id d9443c01a7336-2b453092d89mr126207625ad.31.1776313239392; Wed, 15 Apr 2026 21:20:39 -0700 (PDT) X-Received: by 2002:a17:903:748:b0:2b4:5309:2c14 with SMTP id d9443c01a7336-2b453092d89mr126207325ad.31.1776313238566; Wed, 15 Apr 2026 21:20:38 -0700 (PDT) Received: from hu-nakella-hyd.qualcomm.com ([202.46.23.25]) by smtp.gmail.com with ESMTPSA id d9443c01a7336-2b4782abd6csm37208465ad.63.2026.04.15.21.20.35 (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Wed, 15 Apr 2026 21:20:38 -0700 (PDT) From: Naga Bhavani Akella To: linux-bluetooth@vger.kernel.org Cc: luiz.dentz@gmail.com, quic_mohamull@quicinc.com, quic_hbandi@quicinc.com, quic_anubhavg@quicinc.com, prathibha.madugonde@oss.qualcomm.com, Naga Bhavani Akella Subject: [PATCH BlueZ v9 3/3] profiles: ranging: Add HCI LE Event Handling in Reflector role Date: Thu, 16 Apr 2026 09:50:12 +0530 Message-Id: <20260416042012.1204688-4-naga.akella@oss.qualcomm.com> X-Mailer: git-send-email 2.34.1 In-Reply-To: <20260416042012.1204688-1-naga.akella@oss.qualcomm.com> References: <20260416042012.1204688-1-naga.akella@oss.qualcomm.com> Precedence: bulk X-Mailing-List: linux-bluetooth@vger.kernel.org List-Id: List-Subscribe: List-Unsubscribe: MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit X-Proofpoint-ORIG-GUID: YV5oNr0nXtU11GCDIoyIOE5Bp5br3WtW X-Proofpoint-GUID: YV5oNr0nXtU11GCDIoyIOE5Bp5br3WtW X-Authority-Analysis: v=2.4 cv=XOIAjwhE c=1 sm=1 tr=0 ts=69e06398 cx=c_pps a=UNFcQwm+pnOIJct1K4W+Mw==:117 a=ZePRamnt/+rB5gQjfz0u9A==:17 a=IkcTkHD0fZMA:10 a=A5OVakUREuEA:10 a=s4-Qcg_JpJYA:10 a=VkNPw1HP01LnGYTKEx00:22 a=u7WPNUs3qKkmUXheDGA7:22 a=eoimf2acIAo5FJnRuUoq:22 a=-wXKPabHJcnhTZ4rfTYA:9 a=3ZKOabzyN94A:10 a=QEXdDO2ut3YA:10 a=uKXjsCUrEbL0IQVhDsJ9:22 X-Proofpoint-Spam-Details-Enc: AW1haW4tMjYwNDE2MDAzNyBTYWx0ZWRfXynz4Kx+r5fPG WHeGdq6RV7UtNF5ypJ3yhyfTfMP1WNzbm38p5Ax9hYH3D821HjuWmCQ63Tz4uRgCQweFpALCZu+ hQ00+nGUvkfFnMdFjKrBlaB6g6qn2RBt9F3kqBKPyC2uuh6iPFi2TSPItMH7KIbOH/UyiyyQlKI 5VUT3qo0TGhKBdL1cTgg7EmRmyD8aVHiP2S5JhRAJGcfiNMltf78vio+bFPwwrGaXnkAZl1LhAV RZXJ9Z+SawS4oGVLQtLNg9IIQ7skabBkaofwOVD47si9fIjQ2xhTognYxNumuJ+XBOXkJqQWmip HQvhMQM0Fs8sgwsBETt7HOpdeaAsHlbMUACweN6qscKsU1TpXRP4199wIrZ5vXFjLeG3LRWNgn6 Mpg6M58pb3ug0yIeG90uMG+Sv2coOMB2+A8uoOtvI8w+/QG79atXHAwfTE/7oswUc6k4Os2iNzu zdO6WwRPYtMgO/JzjFg== X-Proofpoint-Virus-Version: vendor=baseguard engine=ICAP:2.0.293,Aquarius:18.0.1143,Hydra:6.1.51,FMLib:17.12.100.49 definitions=2026-04-16_01,2026-04-13_04,2025-10-01_01 X-Proofpoint-Spam-Details: rule=outbound_notspam policy=outbound score=0 priorityscore=1501 adultscore=0 suspectscore=0 spamscore=0 malwarescore=0 bulkscore=0 impostorscore=0 phishscore=0 clxscore=1015 lowpriorityscore=0 classifier=typeunknown authscore=0 authtc= authcc= route=outbound adjust=0 reason=mlx scancount=1 engine=8.22.0-2604070000 definitions=main-2604160037 Open RAW HCI Channel for CS Event Handling Parse the following HCI LE CS Events in reflector role and route the events to RAP Profile. 1. HCI_EVT_LE_CS_READ_RMT_SUPP_CAP_COMPLETE 2. HCI_EVT_LE_CS_CONFIG_COMPLETE 3. HCI_EVT_LE_CS_SECURITY_ENABLE_COMPLETE 4. HCI_EVT_LE_CS_PROCEDURE_ENABLE_COMPLETE 5. HCI_EVT_LE_CS_SUBEVENT_RESULT 6. HCI_EVT_LE_CS_SUBEVENT_RESULT_CONTINUE Send HCI_OP_LE_CS_SET_DEFAULT_SETTINGS to the controller with default settings selected by the user. Map connection handle received to device connection --- Makefile.plugins | 3 +- profiles/ranging/rap.c | 52 ++ profiles/ranging/rap_hci.c | 1263 ++++++++++++++++++++++++++++++++++++ 3 files changed, 1317 insertions(+), 1 deletion(-) create mode 100644 profiles/ranging/rap_hci.c diff --git a/Makefile.plugins b/Makefile.plugins index c9efadb45..ac667beda 100644 --- a/Makefile.plugins +++ b/Makefile.plugins @@ -89,7 +89,8 @@ builtin_modules += battery builtin_sources += profiles/battery/battery.c builtin_modules += rap -builtin_sources += profiles/ranging/rap.c +builtin_sources += profiles/ranging/rap.c \ + profiles/ranging/rap_hci.c if SIXAXIS builtin_modules += sixaxis diff --git a/profiles/ranging/rap.c b/profiles/ranging/rap.c index f03454c72..a473ed632 100644 --- a/profiles/ranging/rap.c +++ b/profiles/ranging/rap.c @@ -17,6 +17,7 @@ #include "gdbus/gdbus.h" #include "bluetooth/bluetooth.h" +#include "bluetooth/l2cap.h" #include "bluetooth/uuid.h" #include "src/plugin.h" @@ -34,12 +35,14 @@ #include "src/shared/rap.h" #include "attrib/att.h" #include "src/log.h" +#include "src/btd.h" struct rap_data { struct btd_device *device; struct btd_service *service; struct bt_rap *rap; unsigned int ready_id; + struct bt_hci *hci; }; static struct queue *sessions; @@ -95,6 +98,14 @@ static void rap_data_free(struct rap_data *data) } bt_rap_ready_unregister(data->rap, data->ready_id); + + if (data->hci) { + bt_rap_hci_sm_cleanup(); + bt_hci_unref(data->hci); + } + + /* Clean up HCI connection mappings */ + bt_rap_detach_hci(data->rap); bt_rap_unref(data->rap); free(data); } @@ -173,6 +184,7 @@ static int rap_probe(struct btd_service *service) struct btd_gatt_database *database = btd_adapter_get_database(adapter); struct rap_data *data = btd_service_get_user_data(service); char addr[18]; + int16_t hci_index; ba2str(device_get_address(device), addr); DBG("%s", addr); @@ -195,6 +207,19 @@ static int rap_probe(struct btd_service *service) return -EINVAL; } + hci_index = btd_adapter_get_index(adapter); + + data->hci = bt_hci_new_raw_device(hci_index); + if (bt_rap_attach_hci(data->rap, data->hci)) { + DBG("HCI raw channel initialized, hci%d", hci_index); + bt_rap_hci_set_options( + btd_opts.defaults.bcs.role, + btd_opts.defaults.bcs.cs_sync_ant_sel, + btd_opts.defaults.bcs.max_tx_power); + } else { + error("HCI raw channel not available (may be in use)"); + } + rap_data_add(data); data->ready_id = bt_rap_ready_register(data->rap, rap_ready, service, @@ -228,6 +253,10 @@ static int rap_accept(struct btd_service *service) struct btd_device *device = btd_service_get_device(service); struct bt_gatt_client *client = btd_device_get_gatt_client(device); struct rap_data *data = btd_service_get_user_data(service); + struct bt_att *att; + const bdaddr_t *bdaddr; + uint8_t bdaddr_type; + uint16_t handle; char addr[18]; ba2str(device_get_address(device), addr); @@ -243,6 +272,29 @@ static int rap_accept(struct btd_service *service) return -EINVAL; } + /* Set up connection handle mapping for CS event routing */ + att = bt_rap_get_att(data->rap); + bdaddr = device_get_address(device); + bdaddr_type = device_get_le_address_type(device); + + if (att && data->hci) { + /* Use bt_hci_get_conn_handle to find the connection handle + * by bdaddr using HCIGETCONNLIST ioctl + */ + if (bt_hci_get_conn_handle(data->hci, + (const uint8_t *)bdaddr, &handle)) { + DBG("Found conn handle 0x%04X for %s", handle, addr); + DBG("Setting up handle mapping: handle=0x%04X", + handle); + bt_rap_set_conn_handle(data->rap, handle, + (const uint8_t *)bdaddr, + bdaddr_type); + } else { + error("Failed to find connection handle for device %s", + addr); + } + } + btd_service_connecting_complete(service, 0); return 0; diff --git a/profiles/ranging/rap_hci.c b/profiles/ranging/rap_hci.c new file mode 100644 index 000000000..86034b378 --- /dev/null +++ b/profiles/ranging/rap_hci.c @@ -0,0 +1,1263 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/* + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (c) Qualcomm Technologies, Inc. and/or its subsidiaries. + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include +#include +#include +#include +#include + +#include "lib/bluetooth/bluetooth.h" +#include "src/shared/util.h" +#include "src/shared/queue.h" +#include "src/shared/rap.h" +#include "src/log.h" +#include "monitor/bt.h" + +#ifndef MIN +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#endif + +/* Macro to sign-extend an N-bit value to 16-bit signed integer */ +#define SIGN_EXTEND_TO_16(val, bits) \ + ((int16_t)((int32_t)((val) << (32 - (bits))) >> (32 - (bits)))) + +/* CS State Definitions */ +enum cs_state_t { + CS_INIT, + CS_STOPPED, + CS_STARTED, + CS_WAIT_CONFIG_CMPLT, + CS_WAIT_SEC_CMPLT, + CS_WAIT_PROC_CMPLT, + CS_HOLD, + CS_UNSPECIFIED +}; + +const char *state_names[] = { + "CS_INIT", + "CS_STOPPED", + "CS_STARTED", + "CS_WAIT_CONFIG_CMPLT", + "CS_WAIT_SEC_CMPLT", + "CS_WAIT_PROC_CMPLT", + "CS_HOLD", + "CS_UNSPECIFIED" +}; + +/* Callback Function Type */ +typedef void (*cs_callback_t)(uint16_t length, + const void *param, void *user_data); + +/* State Machine Context */ +struct cs_state_machine_t { + enum cs_state_t current_state; + enum cs_state_t old_state; + struct bt_hci *hci; + struct bt_rap *rap; + unsigned int event_id; + bool initiator; + bool procedure_active; +}; + +struct cs_callback_map_t { + enum cs_state_t state; + cs_callback_t callback; +}; + +struct cs_callback_map_t cs_callback_map[] = { + { CS_WAIT_CONFIG_CMPLT, bt_rap_hci_cs_config_complete_callback }, + { CS_WAIT_SEC_CMPLT, bt_rap_hci_cs_sec_enable_complete_callback }, + { CS_WAIT_PROC_CMPLT, bt_rap_hci_cs_procedure_enable_complete_callback } +}; + +#define CS_CALLBACK_MAP_SIZE ARRAY_SIZE(cs_callback_map) + +struct bt_rap_hci_cs_options cs_opt; +struct cs_state_machine_t *sm; + +/* Connection Handle Mapping */ +struct rap_conn_mapping { + uint16_t handle; + uint8_t bdaddr[6]; + uint8_t bdaddr_type; + struct bt_att *att; + struct bt_rap *rap; +}; + +static struct queue *conn_mappings; + +/* Connection Mapping Helper Functions */ +static void mapping_free(void *data) +{ + struct rap_conn_mapping *mapping = data; + + if (!mapping) + return; + + free(mapping); +} + +static bool match_mapping_handle(const void *a, const void *b) +{ + const struct rap_conn_mapping *mapping = a; + uint16_t handle = PTR_TO_UINT(b); + + return mapping->handle == handle; +} + +static bool match_mapping_rap(const void *a, const void *b) +{ + const struct rap_conn_mapping *mapping = a; + const struct bt_rap *rap = b; + + return mapping->rap == rap; +} + +static struct rap_conn_mapping *find_mapping_by_handle(uint16_t handle) +{ + if (!conn_mappings) + return NULL; + + return queue_find(conn_mappings, match_mapping_handle, + UINT_TO_PTR(handle)); +} + +static bool add_conn_mapping(uint16_t handle, const uint8_t *bdaddr, + uint8_t bdaddr_type, struct bt_att *att, + struct bt_rap *rap) +{ + struct rap_conn_mapping *mapping; + + if (!conn_mappings) { + conn_mappings = queue_new(); + if (!conn_mappings) + return false; + } + + /* Check if mapping already exists */ + mapping = find_mapping_by_handle(handle); + if (mapping) { + /* Update existing mapping */ + if (bdaddr) + memcpy(mapping->bdaddr, bdaddr, 6); + mapping->bdaddr_type = bdaddr_type; + mapping->att = att; + mapping->rap = rap; + return true; + } + + /* Create new mapping */ + mapping = new0(struct rap_conn_mapping, 1); + if (!mapping) + return false; + + mapping->handle = handle; + if (bdaddr) + memcpy(mapping->bdaddr, bdaddr, 6); + mapping->bdaddr_type = bdaddr_type; + mapping->att = att; + mapping->rap = rap; + + return queue_push_tail(conn_mappings, mapping); +} + +static void remove_conn_mapping(uint16_t handle) +{ + struct rap_conn_mapping *mapping; + + if (!conn_mappings) + return; + + mapping = queue_remove_if(conn_mappings, match_mapping_handle, + UINT_TO_PTR(handle)); + if (mapping) + mapping_free(mapping); +} + +static void remove_rap_mappings(struct bt_rap *rap) +{ + if (!conn_mappings) + return; + + queue_remove_all(conn_mappings, match_mapping_rap, rap, + mapping_free); +} + +static struct bt_rap *resolve_handle_to_rap(uint16_t handle, + struct bt_hci *hci) +{ + struct rap_conn_mapping *mapping; + + /* Try to find in mapping cache */ + mapping = find_mapping_by_handle(handle); + if (mapping && mapping->rap) { + DBG("Found handle 0x%04X in mapping cache", handle); + return mapping->rap; + } + + /* Profile layer should have called bt_rap_set_conn_handle() during + * connection establishment. If we reach here, the mapping was not set. + */ + DBG("No mapping found for handle 0x%04X", handle); + DBG("Profile layer should call bt_rap_set_conn_handle() on connect"); + + return NULL; +} + +/* State Machine Functions */ +static void cs_state_machine_init(struct cs_state_machine_t *sm, + struct bt_rap *rap, struct bt_hci *hci) +{ + if (!sm) + return; + + memset(sm, 0, sizeof(struct cs_state_machine_t)); + sm->current_state = CS_UNSPECIFIED; + sm->rap = rap; + sm->hci = hci; + sm->initiator = false; + sm->procedure_active = false; +} + +void bt_rap_hci_sm_cleanup(void) +{ + if (!sm) + return; + + if (sm->event_id) + bt_hci_unregister(sm->hci, sm->event_id); + + sm->current_state = CS_UNSPECIFIED; + sm->rap = NULL; + sm->hci = NULL; + sm->procedure_active = false; + + free(sm); +} + +void bt_rap_hci_set_options(uint8_t role, uint8_t cs_sync_ant_sel, + int8_t max_tx_power) +{ + cs_opt.role = role; + cs_opt.cs_sync_ant_sel = cs_sync_ant_sel; + cs_opt.max_tx_power = max_tx_power; +} + +/* State Transition Logic */ +static void cs_set_state(struct cs_state_machine_t *sm, + enum cs_state_t new_state) +{ + if (!sm) + return; + + if (sm->current_state == new_state) + return; + + /* Validate state values before array access */ + if (sm->current_state > CS_UNSPECIFIED || new_state > CS_UNSPECIFIED) { + error("Invalid state transition attempted\n"); + return; + } + + DBG("[STATE] Transition: %s → %s\n", + state_names[sm->current_state], + state_names[new_state]); + + sm->old_state = sm->current_state; + sm->current_state = new_state; +} + +static enum cs_state_t cs_get_current_state(struct cs_state_machine_t *sm) +{ + return sm ? sm->current_state : CS_UNSPECIFIED; +} + +/* HCI Event Callbacks */ +static void rap_def_settings_done_cb(const void *data, uint8_t size, + void *user_data) +{ + struct bt_hci_rsp_le_cs_set_def_settings *rp; + struct cs_state_machine_t *sm = (struct cs_state_machine_t *)user_data; + + if (!sm || !data || + size < sizeof(struct bt_hci_rsp_le_cs_set_def_settings)) + return; + + DBG("size=0x%02X\n", size); + + rp = (struct bt_hci_rsp_le_cs_set_def_settings *)data; + + if (cs_get_current_state(sm) != CS_INIT) { + DBG("Event received in Wrong State!! Expected : CS_INIT"); + return; + } + + if (rp->status == 0) { + /* Success - proceed to configuration */ + cs_set_state(sm, CS_WAIT_CONFIG_CMPLT); + + /* Reflector role */ + DBG("Waiting for CS Config Completed event...\n"); + /* TODO: Initiator role - Send CS Config complete cmd */ + } else { + /* Error - transition to stopped */ + error("CS Set default setting failed with status 0x%02X\n", + rp->status); + cs_set_state(sm, CS_STOPPED); + } +} + +static void rap_send_hci_def_settings_command(struct cs_state_machine_t *sm, + struct bt_hci_evt_le_cs_rd_rem_supp_cap_complete *ev) +{ + struct bt_hci_cmd_le_cs_set_def_settings cp; + unsigned int status; + + memset(&cp, 0, sizeof(cp)); + + if (ev->handle) + cp.handle = ev->handle; + cp.role_enable = cs_opt.role; + cp.cs_sync_antenna_selection = cs_opt.cs_sync_ant_sel; + cp.max_tx_power = cs_opt.max_tx_power; + + if (!sm || !sm->hci) { + error("Set Def Settings: sm or hci is null"); + return; + } + + status = bt_hci_send(sm->hci, BT_HCI_CMD_LE_CS_SET_DEF_SETTINGS, + &cp, sizeof(cp), rap_def_settings_done_cb, + sm, NULL); + + DBG("sending set default settings case, status : %d", status); + if (!status) + error("Failed to send default settings cmd"); +} + +static void rap_rd_rmt_supp_cap_cmplt_evt(const uint8_t *data, uint8_t size, + void *user_data) +{ + struct cs_state_machine_t *sm = (struct cs_state_machine_t *)user_data; + const struct bt_hci_evt_le_cs_rd_rem_supp_cap_complete *evt; + struct bt_rap *rap; + struct iovec iov; + + if (!sm || !data || + size < sizeof(struct bt_hci_evt_le_cs_rd_rem_supp_cap_complete)) + return; + + /* Initialize iovec with the event data */ + iov.iov_base = (void *)data; + iov.iov_len = size; + + /* Pull the entire structure at once */ + evt = util_iov_pull_mem(&iov, sizeof(*evt)); + if (!evt) { + error("Failed to pull remote cap complete struct\n"); + return; + } + + DBG("status=0x%02X, handle=0x%04X\n", evt->status, evt->handle); + + /* Check status */ + if (evt->status != 0) { + error("Remote capabilities failed with status 0x%02X\n", + evt->status); + cs_set_state(sm, CS_STOPPED); + return; + } + + /* Resolve handle to RAP instance */ + rap = resolve_handle_to_rap(evt->handle, sm->hci); + if (!rap) { + DBG("[WARN] Could not resolve handle 0x%04X to RAP instance\n", + evt->handle); + /* Continue with state machine RAP for now */ + rap = sm->rap; + } + + DBG("num_config=%u, ", + evt->num_config_supported); + DBG("max_consecutive_proc=%u, num_antennas=%u, ", + evt->max_consecutive_procedures_supported, + evt->num_antennas_supported); + DBG("max_antenna_paths=%u, roles=0x%02X, modes=0x%02X\n", + evt->max_antenna_paths_supported, + evt->roles_supported, + evt->modes_supported); + + rap_send_hci_def_settings_command(sm, + (struct bt_hci_evt_le_cs_rd_rem_supp_cap_complete *)evt); + cs_set_state(sm, CS_INIT); +} + +static void rap_cs_config_cmplt_evt(const uint8_t *data, uint8_t size, + void *user_data) +{ + struct cs_state_machine_t *sm = (struct cs_state_machine_t *)user_data; + const struct bt_hci_evt_le_cs_config_complete *evt; + struct rap_ev_cs_config_cmplt rap_ev; + struct iovec iov; + + if (!sm || !data || + size < sizeof(struct bt_hci_evt_le_cs_config_complete)) + return; + + /* Initialize iovec with the event data */ + iov.iov_base = (void *)data; + iov.iov_len = size; + + DBG("size=0x%02X\n", size); + + /* State Check */ + if (cs_get_current_state(sm) != CS_WAIT_CONFIG_CMPLT) { + DBG("Event received in Wrong State!! "); + DBG("Expected : CS_WAIT_CONFIG_CMPLT"); + return; + } + + /* Pull the entire structure at once */ + evt = util_iov_pull_mem(&iov, sizeof(*evt)); + if (!evt) { + error("Failed to pull config complete struct\n"); + return; + } + + DBG("status=0x%02X, handle=0x%04X\n", evt->status, evt->handle); + + /* Check status */ + if (evt->status != 0) { + error("Configuration failed with status 0x%02X\n", + evt->status); + cs_set_state(sm, CS_STOPPED); + return; + } + + /* Copy fields to rap_ev structure */ + rap_ev.status = evt->status; + rap_ev.conn_hdl = cpu_to_le16(evt->handle); + rap_ev.config_id = evt->config_id; + rap_ev.action = evt->action; + rap_ev.main_mode_type = evt->main_mode_type; + rap_ev.sub_mode_type = evt->sub_mode_type; + rap_ev.min_main_mode_steps = evt->min_main_mode_steps; + rap_ev.max_main_mode_steps = evt->max_main_mode_steps; + rap_ev.main_mode_rep = evt->main_mode_repetition; + rap_ev.mode_0_steps = evt->mode_0_steps; + rap_ev.role = evt->role; + rap_ev.rtt_type = evt->rtt_type; + rap_ev.cs_sync_phy = evt->cs_sync_phy; + memcpy(rap_ev.channel_map, evt->channel_map, 10); + rap_ev.channel_map_rep = evt->channel_map_repetition; + rap_ev.channel_sel_type = evt->channel_selection_type; + rap_ev.ch3c_shape = evt->ch3c_shape; + rap_ev.ch3c_jump = evt->ch3c_jump; + rap_ev.reserved = evt->reserved; + rap_ev.t_ip1_time = evt->t_ip1_time; + rap_ev.t_ip2_time = evt->t_ip2_time; + rap_ev.t_fcs_time = evt->t_fcs_time; + rap_ev.t_pm_time = evt->t_pm_time; + + /* Store rtt_type in global options */ + cs_opt.rtt_type = rap_ev.rtt_type; + + DBG("config_id=%u, action=%u, ", + rap_ev.config_id, rap_ev.action); + DBG("main_mode=%u, sub_mode=%u, role=%u, rtt_type=%u\n", + rap_ev.main_mode_type, rap_ev.sub_mode_type, + rap_ev.role, rap_ev.rtt_type); + + /* Success - proceed to Security enable complete */ + cs_set_state(sm, CS_WAIT_SEC_CMPLT); + + /* Reflector role */ + DBG("Waiting for security enable event...\n"); + /* TODO: Initiator role - Send CS Security enable cmd */ + + /* Send Callback to RAP Profile */ + for (size_t i = 0; i < CS_CALLBACK_MAP_SIZE; i++) { + if (cs_callback_map[i].state == sm->old_state) { + cs_callback_map[i].callback(size, &rap_ev, sm->rap); + return; + } + } +} + +static void rap_cs_sec_enable_cmplt_evt(const uint8_t *data, uint8_t size, + void *user_data) +{ + struct cs_state_machine_t *sm = (struct cs_state_machine_t *)user_data; + struct rap_ev_cs_sec_enable_cmplt rap_ev; + struct iovec iov; + uint8_t status; + uint16_t handle; + + if (!sm || !data || + size < sizeof(struct bt_hci_evt_le_cs_sec_enable_complete)) + return; + + /* Initialize iovec with the event data */ + iov.iov_base = (void *)data; + iov.iov_len = size; + + DBG("size=0x%02X\n", size); + + /* State Check */ + if (cs_get_current_state(sm) != CS_WAIT_SEC_CMPLT) { + DBG("Event received in Wrong State!! "); + DBG("Expected : CS_WAIT_SEC_CMPLT"); + return; + } + + /* Parse all fields in order using iovec */ + if (!util_iov_pull_u8(&iov, &status)) { + error("Failed to parse Status\n"); + return; + } + + if (!util_iov_pull_le16(&iov, &handle)) { + error("Failed to parse Connection_Handle\n"); + return; + } + + rap_ev.status = status; + rap_ev.conn_hdl = cpu_to_le16(handle); + + DBG("status=0x%02X, handle=0x%04X\n", + rap_ev.status, handle); + + if (rap_ev.status == 0) { + /* Success - proceed to configuration */ + cs_set_state(sm, CS_WAIT_PROC_CMPLT); + + /* Reflector role */ + DBG("Waiting for CS Proc complete event...\n"); + /* TODO: Initiator - Send CS Proc Set Parameter and enable */ + } else { + /* Error - transition to stopped */ + error("Security enable failed with status 0x%02X\n", + rap_ev.status); + cs_set_state(sm, CS_STOPPED); + } + + /* Send Callback to RAP Profile */ + for (size_t i = 0; i < CS_CALLBACK_MAP_SIZE; i++) { + if (cs_callback_map[i].state == sm->old_state) { + cs_callback_map[i].callback(size, &rap_ev, sm->rap); + return; + } + } +} + +static void rap_cs_proc_enable_cmplt_evt(const uint8_t *data, uint8_t size, + void *user_data) +{ + struct cs_state_machine_t *sm = (struct cs_state_machine_t *)user_data; + const struct bt_hci_evt_le_cs_proc_enable_complete *evt; + struct rap_ev_cs_proc_enable_cmplt rap_ev; + struct iovec iov; + + if (!sm || !data || + size < sizeof(struct bt_hci_evt_le_cs_proc_enable_complete)) + return; + + /* Initialize iovec with the event data */ + iov.iov_base = (void *)data; + iov.iov_len = size; + + DBG("size=0x%02X\n", size); + + /* State Check */ + if (cs_get_current_state(sm) != CS_WAIT_PROC_CMPLT) { + DBG("Event received in Wrong State!! "); + DBG("Expected : CS_WAIT_PROC_CMPLT"); + return; + } + + /* Pull the entire structure at once */ + evt = util_iov_pull_mem(&iov, sizeof(*evt)); + if (!evt) { + error("Failed to pull proc enable complete struct\n"); + return; + } + + DBG("status=0x%02X, handle=0x%04X\n", evt->status, evt->handle); + + /* Check status */ + if (evt->status != 0) { + error("Procedure enable failed with status 0x%02X\n", + evt->status); + cs_set_state(sm, CS_STOPPED); + sm->procedure_active = false; + return; + } + + /* Copy fields to rap_ev structure */ + rap_ev.status = evt->status; + rap_ev.conn_hdl = cpu_to_le16(evt->handle); + rap_ev.config_id = evt->config_id; + rap_ev.state = evt->state; + rap_ev.tone_ant_config_sel = evt->tone_antenna_config_selection; + rap_ev.sel_tx_pwr = evt->selected_tx_power; + memcpy(rap_ev.sub_evt_len, evt->subevent_len, 3); + rap_ev.sub_evts_per_evt = evt->subevents_per_event; + rap_ev.sub_evt_intrvl = evt->subevent_interval; + rap_ev.evt_intrvl = evt->event_interval; + rap_ev.proc_intrvl = evt->procedure_interval; + rap_ev.proc_counter = evt->procedure_count; + rap_ev.max_proc_len = evt->max_procedure_len; + + DBG("config_id=%u, state=%u, ", + rap_ev.config_id, rap_ev.state); + DBG("sub_evts_per_evt=%u, evt_intrvl=%u, proc_intrvl=%u\n", + rap_ev.sub_evts_per_evt, rap_ev.evt_intrvl, + rap_ev.proc_intrvl); + + /* Success - procedure started */ + cs_set_state(sm, CS_STARTED); + sm->procedure_active = true; + + /* Send Callback to RAP Profile */ + for (size_t i = 0; i < CS_CALLBACK_MAP_SIZE; i++) { + if (cs_callback_map[i].state == sm->old_state) { + cs_callback_map[i].callback(size, &rap_ev, sm->rap); + return; + } + } +} + +static void parse_i_q_sample(struct iovec *iov, int16_t *i_sample, + int16_t *q_sample) +{ + uint32_t buffer; + uint32_t i12; + uint32_t q12; + + /* Pull 24-bit little-endian value from iovec */ + if (!util_iov_pull_le24(iov, &buffer)) { + *i_sample = 0; + *q_sample = 0; + return; + } + + /* Extract 12-bit I and Q values from 24-bit buffer */ + i12 = buffer & 0x0FFFU; /* bits 0..11 */ + q12 = (buffer >> 12) & 0x0FFFU; /* bits 12..23 */ + + /* Sign-extend 12-bit values to 16-bit using macro */ + *i_sample = SIGN_EXTEND_TO_16(i12, 12); + *q_sample = SIGN_EXTEND_TO_16(q12, 12); +} + +/* Parse CS Mode 0 step data */ +static void parse_mode_zero_data(struct iovec *iov, + struct cs_mode_zero_data *mode_data, + uint8_t cs_role) +{ + uint32_t freq_offset; + + if (iov->iov_len < 3) { + DBG("Mode 0: too short (<3)"); + return; + } + + util_iov_pull_u8(iov, &mode_data->packet_quality); + util_iov_pull_u8(iov, &mode_data->packet_rssi_dbm); + util_iov_pull_u8(iov, &mode_data->packet_ant); + DBG("CS Step mode 0"); + + if (cs_role == CS_INITIATOR && iov->iov_len >= 4) { + util_iov_pull_le32(iov, &freq_offset); + mode_data->init_measured_freq_offset = freq_offset; + } +} + +/* Parse CS Mode 1 step data */ +static void parse_mode_one_data(struct iovec *iov, + struct cs_mode_one_data *mode_data, + uint8_t cs_role, uint8_t cs_rtt_type) +{ + uint16_t time_val; + + if (iov->iov_len < 4) { + DBG("Mode 1: too short (<4)"); + return; + } + + DBG("CS Step mode 1"); + util_iov_pull_u8(iov, &mode_data->packet_quality); + util_iov_pull_u8(iov, &mode_data->packet_rssi_dbm); + util_iov_pull_u8(iov, &mode_data->packet_ant); + util_iov_pull_u8(iov, &mode_data->packet_nadm); + + if (iov->iov_len >= 2) { + util_iov_pull_le16(iov, &time_val); + if (cs_role == CS_REFLECTOR) + mode_data->tod_toa_refl = time_val; + else + mode_data->toa_tod_init = time_val; + } + + if ((cs_rtt_type == 0x01 || cs_rtt_type == 0x02) && + iov->iov_len >= 6) { + int16_t i_val, q_val; + + parse_i_q_sample(iov, &i_val, &q_val); + mode_data->packet_pct1.i_sample = i_val; + mode_data->packet_pct1.q_sample = q_val; + + parse_i_q_sample(iov, &i_val, &q_val); + mode_data->packet_pct2.i_sample = i_val; + mode_data->packet_pct2.q_sample = q_val; + } +} + +/* Parse CS Mode 2 step data */ +static void parse_mode_two_data(struct iovec *iov, + struct cs_mode_two_data *mode_data, + uint8_t max_paths) +{ + uint8_t k; + + if (iov->iov_len < 1) { + DBG("Mode 2: too short (<1)"); + return; + } + + util_iov_pull_u8(iov, &mode_data->ant_perm_index); + DBG("CS Step mode 2, max paths : %d", max_paths); + + for (k = 0; k < max_paths; k++) { + int16_t i_val, q_val; + + if (iov->iov_len < 4) { + DBG("Mode 2: insufficient PCT for path %u (rem=%zu)", + k, iov->iov_len); + break; + } + parse_i_q_sample(iov, &i_val, &q_val); + mode_data->tone_pct[k].i_sample = i_val; + mode_data->tone_pct[k].q_sample = q_val; + + util_iov_pull_u8(iov, &mode_data->tone_quality_indicator[k]); + DBG("tone_quality_indicator : %d", + mode_data->tone_quality_indicator[k]); + DBG("[i, q] : %d, %d", + mode_data->tone_pct[k].i_sample, + mode_data->tone_pct[k].q_sample); + } +} + +/* Parse CS Mode 3 step data */ +static void parse_mode_three_data(struct iovec *iov, + struct cs_mode_three_data *mode_data, + uint8_t cs_role, uint8_t cs_rtt_type, + uint8_t max_paths) +{ + uint8_t k; + struct cs_mode_one_data *mode_one = &mode_data->mode_one_data; + struct cs_mode_two_data *mode_two = &mode_data->mode_two_data; + + if (iov->iov_len < 4) { + DBG("Mode 3: mode1 too short (<4)"); + return; + } + + DBG("CS Step mode 3"); + + /* Parse Mode 1 portion */ + parse_mode_one_data(iov, mode_one, cs_role, cs_rtt_type); + + /* Parse Mode 2 portion */ + if (iov->iov_len >= 1) { + util_iov_pull_u8(iov, &mode_two->ant_perm_index); + for (k = 0; k < max_paths; k++) { + int16_t i_val, q_val; + + if (iov->iov_len < 4) + break; + parse_i_q_sample(iov, &i_val, &q_val); + mode_two->tone_pct[k].i_sample = i_val; + mode_two->tone_pct[k].q_sample = q_val; + + util_iov_pull_u8(iov, + &mode_two->tone_quality_indicator[k]); + } + } +} + +/* Parse a single CS step */ +static void parse_cs_step(struct iovec *iov, struct cs_step_data *step, + uint8_t cs_role, uint8_t cs_rtt_type, + uint8_t max_paths) +{ + uint8_t mode; + uint8_t chnl; + uint8_t length; + + /* Check if we have enough data for the 3-byte header */ + if (iov->iov_len < 3) { + DBG("Truncated header for step"); + return; + } + + /* Read mode, channel, and length (3-byte header) */ + if (!util_iov_pull_u8(iov, &mode) || + !util_iov_pull_u8(iov, &chnl) || + !util_iov_pull_u8(iov, &length)) { + DBG("Failed to read header for step"); + return; + } + + DBG("event->step_data_len : %d", length); + + step->step_mode = mode; + step->step_chnl = chnl; + step->step_data_length = length; + + DBG("Step: mode=%u chnl=%u data_len=%u", mode, chnl, length); + + if (iov->iov_len < length) { + DBG("Truncated payload for step (need %u, have %zu)", + length, iov->iov_len); + return; + } + + /* Parse step data based on mode */ + switch (mode) { + case CS_MODE_ZERO: + parse_mode_zero_data(iov, &step->step_mode_data.mode_zero_data, + cs_role); + break; + case CS_MODE_ONE: + parse_mode_one_data(iov, &step->step_mode_data.mode_one_data, + cs_role, cs_rtt_type); + break; + case CS_MODE_TWO: + parse_mode_two_data(iov, &step->step_mode_data.mode_two_data, + max_paths); + break; + case CS_MODE_THREE: + parse_mode_three_data(iov, + &step->step_mode_data.mode_three_data, + cs_role, cs_rtt_type, max_paths); + break; + default: + DBG("Unknown step mode %d", mode); + /* Skip the entire step data */ + util_iov_pull(iov, length); + break; + } +} + +static void rap_cs_subevt_result_evt(const uint8_t *data, uint8_t size, + void *user_data) +{ + struct cs_state_machine_t *sm = (struct cs_state_machine_t *)user_data; + struct rap_ev_cs_subevent_result *rap_ev; + struct iovec iov; + uint8_t cs_role; + uint8_t cs_rtt_type; + uint8_t max_paths; + uint8_t steps; + size_t send_len = 0; + uint16_t handle; + uint8_t config_id; + uint16_t start_acl_conn_evt_counter; + uint16_t proc_counter; + uint16_t freq_comp; + uint8_t ref_pwr_lvl; + uint8_t proc_done_status; + uint8_t subevt_done_status; + uint8_t abort_reason; + uint8_t num_ant_paths; + uint8_t num_steps_reported; + uint8_t i; + + if (!sm || !data || + size < sizeof(struct bt_hci_evt_le_cs_subevent_result)) + return; + + /* Initialize iovec with the event data */ + iov.iov_base = (void *)data; + iov.iov_len = size; + + /* Check if Procedure is active or not */ + if (!sm->procedure_active) { + DBG("Received Subevent event when Procedure is inactive!"); + return; + } + + /* Parse header fields using iovec */ + if (!util_iov_pull_le16(&iov, &handle)) { + error("Failed to parse Connection_Handle\n"); + return; + } + + if (!util_iov_pull_u8(&iov, &config_id) || + !util_iov_pull_le16(&iov, &start_acl_conn_evt_counter) || + !util_iov_pull_le16(&iov, &proc_counter) || + !util_iov_pull_le16(&iov, &freq_comp) || + !util_iov_pull_u8(&iov, &ref_pwr_lvl) || + !util_iov_pull_u8(&iov, &proc_done_status) || + !util_iov_pull_u8(&iov, &subevt_done_status) || + !util_iov_pull_u8(&iov, &abort_reason) || + !util_iov_pull_u8(&iov, &num_ant_paths) || + !util_iov_pull_u8(&iov, &num_steps_reported)) { + error("Failed to parse subevent fields\n"); + return; + } + + cs_role = cs_opt.role; + cs_rtt_type = cs_opt.rtt_type; + max_paths = MIN((num_ant_paths + 1), CS_MAX_ANT_PATHS); + steps = MIN(num_steps_reported, CS_MAX_STEPS); + send_len = offsetof(struct rap_ev_cs_subevent_result, step_data) + + steps * sizeof(struct cs_step_data); + rap_ev = (struct rap_ev_cs_subevent_result *)malloc(send_len); + if (!rap_ev) { + error("Failed to allocate memory for subevent result\n"); + return; + } + + DBG("length=%u\n", size); + rap_ev->conn_hdl = le16_to_cpu(handle); + rap_ev->config_id = config_id; + rap_ev->start_acl_conn_evt_counter = start_acl_conn_evt_counter; + rap_ev->proc_counter = proc_counter; + rap_ev->freq_comp = freq_comp; + rap_ev->ref_pwr_lvl = ref_pwr_lvl; + rap_ev->proc_done_status = proc_done_status; + rap_ev->subevt_done_status = subevt_done_status; + rap_ev->abort_reason = abort_reason; + rap_ev->num_ant_paths = num_ant_paths; + rap_ev->num_steps_reported = steps; + + if (num_steps_reported > CS_MAX_STEPS) { + DBG("Too many steps reported: %u (max %u)", + num_steps_reported, CS_MAX_STEPS); + goto send_event; + } + + /* Early exit for error conditions */ + if (rap_ev->subevt_done_status == 0xF || + rap_ev->proc_done_status == 0xF) { + DBG("CS Procedure/Subevent aborted: "); + DBG("sub evt status = %d, proc status = %d, reason = %d", + rap_ev->subevt_done_status, rap_ev->proc_done_status, + rap_ev->abort_reason); + goto send_event; + } + + /* Parse interleaved step data from remaining iovec data */ + for (i = 0; i < steps; i++) + parse_cs_step(&iov, &rap_ev->step_data[i], cs_role, cs_rtt_type, + max_paths); + +send_event: + DBG("CS subevent result processed: %zu bytes, ", send_len); + bt_rap_hci_cs_subevent_result_callback(send_len, rap_ev, sm->rap); + free(rap_ev); +} + +static void rap_cs_subevt_result_cont_evt(const uint8_t *data, uint8_t size, + void *user_data) +{ + struct cs_state_machine_t *sm = (struct cs_state_machine_t *)user_data; + struct rap_ev_cs_subevent_result_cont *rap_ev; + struct iovec iov; + uint8_t cs_role; + uint8_t cs_rtt_type; + uint8_t max_paths; + uint8_t steps; + size_t send_len = 0; + uint16_t handle; + uint8_t config_id; + uint8_t proc_done_status; + uint8_t subevt_done_status; + uint8_t abort_reason; + uint8_t num_ant_paths; + uint8_t num_steps_reported; + uint8_t i; + + if (!sm || !data || + size < sizeof(struct bt_hci_evt_le_cs_subevent_result_continue)) + return; + + /* Initialize iovec with the event data */ + iov.iov_base = (void *)data; + iov.iov_len = size; + + /* Check if Procedure is active or not */ + if (!sm->procedure_active) { + error("Received Subevent when CS Procedure is inactive!"); + return; + } + + /* Parse header fields using iovec */ + if (!util_iov_pull_le16(&iov, &handle)) { + error("Failed to parse Connection_Handle\n"); + return; + } + + if (!util_iov_pull_u8(&iov, &config_id) || + !util_iov_pull_u8(&iov, &proc_done_status) || + !util_iov_pull_u8(&iov, &subevt_done_status) || + !util_iov_pull_u8(&iov, &abort_reason) || + !util_iov_pull_u8(&iov, &num_ant_paths) || + !util_iov_pull_u8(&iov, &num_steps_reported)) { + error("Failed to parse subevent continue fields "); + return; + } + + cs_role = cs_opt.role; + cs_rtt_type = cs_opt.rtt_type; + max_paths = MIN((num_ant_paths + 1), CS_MAX_ANT_PATHS); + steps = MIN(num_steps_reported, CS_MAX_STEPS); + send_len = offsetof(struct rap_ev_cs_subevent_result_cont, step_data) + + steps * sizeof(struct cs_step_data); + rap_ev = (struct rap_ev_cs_subevent_result_cont *)malloc(send_len); + if (!rap_ev) { + error("Failed to allocate memory for subevent result\n"); + return; + } + + DBG("length=%u\n", size); + rap_ev->conn_hdl = le16_to_cpu(handle); + rap_ev->config_id = config_id; + rap_ev->proc_done_status = proc_done_status; + rap_ev->subevt_done_status = subevt_done_status; + rap_ev->abort_reason = abort_reason; + rap_ev->num_ant_paths = num_ant_paths; + rap_ev->num_steps_reported = steps; + + if (num_steps_reported > CS_MAX_STEPS) { + DBG("Too many steps reported: %u (max %u)", + num_steps_reported, CS_MAX_STEPS); + goto send_event; + } + + /* Early exit for error conditions */ + if (rap_ev->subevt_done_status == 0xF || + rap_ev->proc_done_status == 0xF) { + DBG("CS Procedure/Subevent aborted: "); + DBG("sub evt status = %d, proc status = %d, reason = %d", + rap_ev->subevt_done_status, rap_ev->proc_done_status, + rap_ev->abort_reason); + goto send_event; + } + + /* Parse interleaved step data from remaining iovec data */ + for (i = 0; i < steps; i++) + parse_cs_step(&iov, &rap_ev->step_data[i], cs_role, cs_rtt_type, + max_paths); + +send_event: + DBG("CS subevent result cont processed: %zu bytes, ", send_len); + bt_rap_hci_cs_subevent_result_cont_callback(send_len, rap_ev, sm->rap); + free(rap_ev); +} + +/* Subevent handler function type */ +typedef void (*subevent_handler_t)(const uint8_t *data, uint8_t size, + void *user_data); + +/* Subevent table entry */ +struct subevent_entry { + uint8_t opcode; + uint8_t min_len; + uint8_t max_len; + subevent_handler_t handler; + const char *name; +}; + +/* Macro to define HCI event entries */ +#define HCI_EVT(_opcode, _struct, _handler, _name) \ + { \ + .opcode = _opcode, \ + .min_len = sizeof(_struct), \ + .max_len = 0xFF, \ + .handler = _handler, \ + .name = _name \ + } + +/* Subevent dispatch table */ +static const struct subevent_entry subevent_table[] = { + HCI_EVT(BT_HCI_EVT_LE_CS_RD_REM_SUPP_CAP_COMPLETE, + struct bt_hci_evt_le_cs_rd_rem_supp_cap_complete, + rap_rd_rmt_supp_cap_cmplt_evt, + "CS Read Remote Supported Capabilities Complete"), + HCI_EVT(BT_HCI_EVT_LE_CS_CONFIG_COMPLETE, + struct bt_hci_evt_le_cs_config_complete, + rap_cs_config_cmplt_evt, + "CS Config Complete"), + HCI_EVT(BT_HCI_EVT_LE_CS_SEC_ENABLE_COMPLETE, + struct bt_hci_evt_le_cs_sec_enable_complete, + rap_cs_sec_enable_cmplt_evt, + "CS Security Enable Complete"), + HCI_EVT(BT_HCI_EVT_LE_CS_PROC_ENABLE_COMPLETE, + struct bt_hci_evt_le_cs_proc_enable_complete, + rap_cs_proc_enable_cmplt_evt, + "CS Procedure Enable Complete"), + HCI_EVT(BT_HCI_EVT_LE_CS_SUBEVENT_RESULT, + struct bt_hci_evt_le_cs_subevent_result, + rap_cs_subevt_result_evt, + "CS Subevent Result"), + HCI_EVT(BT_HCI_EVT_LE_CS_SUBEVENT_RESULT_CONTINUE, + struct bt_hci_evt_le_cs_subevent_result_continue, + rap_cs_subevt_result_cont_evt, + "CS Subevent Result Continue") +}; + +#undef HCI_EVT + +#define SUBEVENT_TABLE_SIZE ARRAY_SIZE(subevent_table) + +/* HCI Event Registration */ +static void rap_handle_hci_events(const void *data, uint8_t size, + void *user_data) +{ + struct iovec iov; + uint8_t subevent; + const struct subevent_entry *entry = NULL; + size_t i; + + /* Initialize iovec with the event data */ + iov.iov_base = (void *)data; + iov.iov_len = size; + + /* Pull the subevent code */ + if (!util_iov_pull_u8(&iov, &subevent)) { + DBG("Failed to parse subevent code"); + return; + } + + /* Find the subevent in the table */ + for (i = 0; i < SUBEVENT_TABLE_SIZE; i++) { + if (subevent_table[i].opcode == subevent) { + entry = &subevent_table[i]; + break; + } + } + + /* Check if subevent is supported */ + if (!entry) { + DBG("Unknown subevent: 0x%02X", subevent); + return; + } + + /* Validate payload length */ + if (iov.iov_len < entry->min_len) { + DBG("%s: payload too short (%zu < %u)", + entry->name, iov.iov_len, entry->min_len); + return; + } + + if (entry->max_len != 0xFF && iov.iov_len > entry->max_len) { + DBG("%s: payload too long (%zu > %u)", + entry->name, iov.iov_len, entry->max_len); + return; + } + + /* Call the handler */ + DBG("Handling %s (opcode=0x%02X, len=%zu)", + entry->name, subevent, iov.iov_len); + + entry->handler(iov.iov_base, iov.iov_len, user_data); +} + +static void bt_rap_hci_register_events(struct bt_rap *rap, + struct bt_hci *hci) +{ + if (!rap || !hci) + return; + + sm = new0(struct cs_state_machine_t, 1); + if (!sm) { + error("Failed to allocate state machine\n"); + return; + } + + cs_state_machine_init(sm, rap, hci); + sm->event_id = bt_hci_register(hci, BT_HCI_EVT_LE_META_EVENT, + rap_handle_hci_events, sm, NULL); + + DBG("bt_hci_register done, event_id : %d", sm->event_id); + + if (!sm->event_id) { + DBG("Error: Failed to register hci le meta events "); + DBG("event_id=0x%02X\n", sm->event_id); + free(sm); + return; + } +} + +bool bt_rap_attach_hci(struct bt_rap *rap, struct bt_hci *hci) +{ + if (!rap) + return false; + + if (!hci) { + DBG("Failed to create HCI RAW channel "); + bt_hci_unref(hci); + return false; + } + + bt_rap_hci_register_events(rap, hci); + + return true; +} + +bool bt_rap_set_conn_handle(struct bt_rap *rap, uint16_t handle, + const uint8_t *bdaddr, uint8_t bdaddr_type) +{ + struct bt_att *att; + + if (!rap) + return false; + + att = bt_rap_get_att(rap); + if (!att) + return false; + + DBG("Setting connection mapping: handle=0x%04X, ", handle); + if (bdaddr) { + DBG("bdaddr=%02x:%02x:%02x:%02x:%02x:%02x type=%u", + bdaddr[5], bdaddr[4], bdaddr[3], + bdaddr[2], bdaddr[1], bdaddr[0], bdaddr_type); + } + + return add_conn_mapping(handle, bdaddr, bdaddr_type, att, rap); +} + +void bt_rap_clear_conn_handle(struct bt_rap *rap, uint16_t handle) +{ + if (!rap) + return; + + DBG("Clearing connection mapping: handle=0x%04X", handle); + remove_conn_mapping(handle); +} + +void bt_rap_detach_hci(struct bt_rap *rap) +{ + if (!rap) + return; + + DBG("Detaching RAP from HCI, cleaning up mappings"); + + /* Remove all mappings associated with this RAP instance */ + remove_rap_mappings(rap); +} --