From 32a3591b706af613552b425fecc58ee2463ae724 Mon Sep 17 00:00:00 2001
From: Iulia Tanasescu <iulia.tanasescu@nxp.com>
Date: Tue, 2 Apr 2024 14:43:25 +0300
Subject: [PATCH] bthost: Add support for Set PA data

This adds bthost_set_pa_data.
---
 emulator/bthost.c | 49 ++++++++++++++++++++++++++++++++++++++++++++++-
 emulator/bthost.h |  4 +++-
 src/shared/ad.h   |  1 +
 3 files changed, 52 insertions(+), 2 deletions(-)

diff --git a/emulator/bthost.c b/emulator/bthost.c
index 8c40fce90..627b771ea 100644
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
@@ -5,7 +5,7 @@
  *
  *  Copyright (C) 2011-2012  Intel Corporation
  *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
- *  Copyright 2023 NXP
+ *  Copyright 2023-2024 NXP
  *
  *
  */
@@ -27,6 +27,7 @@
 #include "src/shared/util.h"
 #include "src/shared/tester.h"
 #include "src/shared/queue.h"
+#include "src/shared/ad.h"
 #include "monitor/bt.h"
 #include "monitor/rfcomm.h"
 #include "bthost.h"
@@ -3127,6 +3128,52 @@ void bthost_set_pa_params(struct bthost *bthost)
 	send_command(bthost, BT_HCI_CMD_LE_SET_PA_PARAMS, &cp, sizeof(cp));
 }
 
+static void set_pa_data(struct bthost *bthost, const uint8_t *data,
+				uint8_t len, uint8_t offset)
+{
+	struct bt_hci_cmd_le_set_pa_data *cp;
+	uint8_t buf[sizeof(*cp) + BT_PA_MAX_DATA_LEN];
+
+	cp = (void *)buf;
+
+	memset(cp, 0, sizeof(*cp));
+	memset(cp->data, 0, BT_PA_MAX_DATA_LEN);
+
+	cp->handle = 1;
+
+	if (len - offset > BT_PA_MAX_DATA_LEN) {
+		cp->data_len = BT_PA_MAX_DATA_LEN;
+
+		if (!offset)
+			cp->operation = 0x01;
+		else
+			cp->operation = 0x00;
+	} else {
+		cp->data_len = len - offset;
+
+		if (!offset)
+			cp->operation = 0x03;
+		else
+			cp->operation = 0x02;
+	}
+
+	memcpy(cp->data, data + offset, cp->data_len);
+
+	send_command(bthost, BT_HCI_CMD_LE_SET_PA_DATA, buf,
+					sizeof(*cp) + cp->data_len);
+
+	if (cp->operation == 0x01 || cp->operation == 0x00) {
+		offset += cp->data_len;
+		set_pa_data(bthost, data, len, offset);
+	}
+}
+
+void bthost_set_pa_data(struct bthost *bthost, const uint8_t *data,
+							uint8_t len)
+{
+	set_pa_data(bthost, data, len, 0);
+}
+
 void bthost_set_pa_enable(struct bthost *bthost, uint8_t enable)
 {
 	struct bt_hci_cmd_le_set_pa_enable cp;
diff --git a/emulator/bthost.h b/emulator/bthost.h
index 46781365b..f03262d46 100644
--- a/emulator/bthost.h
+++ b/emulator/bthost.h
@@ -5,7 +5,7 @@
  *
  *  Copyright (C) 2011-2012  Intel Corporation
  *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
- *  Copyright 2023 NXP
+ *  Copyright 2023-2024 NXP
  *
  *
  */
@@ -102,6 +102,8 @@ void bthost_set_ext_adv_data(struct bthost *bthost, const uint8_t *data,
 void bthost_set_ext_adv_params(struct bthost *bthost);
 void bthost_set_ext_adv_enable(struct bthost *bthost, uint8_t enable);
 void bthost_set_pa_params(struct bthost *bthost);
+void bthost_set_pa_data(struct bthost *bthost, const uint8_t *data,
+								uint8_t len);
 void bthost_set_pa_enable(struct bthost *bthost, uint8_t enable);
 void bthost_create_big(struct bthost *bthost, uint8_t num_bis, uint8_t enc,
 				const uint8_t *bcode);
diff --git a/src/shared/ad.h b/src/shared/ad.h
index 87b3401a3..820b91775 100644
--- a/src/shared/ad.h
+++ b/src/shared/ad.h
@@ -15,6 +15,7 @@
 #include "lib/uuid.h"
 
 #define BT_AD_MAX_DATA_LEN		31
+#define BT_PA_MAX_DATA_LEN		252
 
 #define BT_AD_FLAGS			0x01
 #define BT_AD_UUID16_SOME		0x02
-- 
2.47.3