/**
* Create a scan response PDU
*
* @param advsm
*/
static struct os_mbuf *
ble_ll_adv_scan_rsp_pdu_make(struct ble_ll_adv_sm *advsm)
{
uint8_t scan_rsp_len;
uint8_t *dptr;
uint8_t pdulen;
uint8_t hdr;
struct os_mbuf *m;
/* Obtain scan response buffer */
m = os_msys_get_pkthdr(BLE_SCAN_RSP_DATA_MAX_LEN + BLE_DEV_ADDR_LEN,
sizeof(struct ble_mbuf_hdr));
if (!m) {
return NULL;
}
/* Make sure that the length is valid */
scan_rsp_len = advsm->scan_rsp_len;
assert(scan_rsp_len <= BLE_SCAN_RSP_DATA_MAX_LEN);
/* Set BLE transmit header */
pdulen = BLE_DEV_ADDR_LEN + scan_rsp_len;
hdr = BLE_ADV_PDU_TYPE_SCAN_RSP;
if (advsm->adv_txadd) {
hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
}
ble_ll_mbuf_init(m, pdulen, hdr);
/*
* XXX: Am I sure this is correct? The adva in this packet will be the
* same one that was being advertised and is based on the peer identity
* address in the set advertising parameters. If a different peer sends
* us a scan request (for some reason) we will reply with an adva that
* was not generated based on the local irk of the peer sending the scan
* request.
*/
/* Construct scan response */
dptr = m->om_data;
memcpy(dptr, advsm->adva, BLE_DEV_ADDR_LEN);
if (scan_rsp_len != 0) {
memcpy(dptr + BLE_DEV_ADDR_LEN, advsm->scan_rsp_data, scan_rsp_len);
}
return m;
}
/**
* Create a scan response PDU
*
* @param advsm
*/
static void
ble_ll_adv_scan_rsp_pdu_make(struct ble_ll_adv_sm *advsm)
{
uint8_t scan_rsp_len;
uint8_t *dptr;
uint8_t *addr;
uint8_t pdulen;
uint8_t hdr;
struct os_mbuf *m;
/* Make sure that the length is valid */
scan_rsp_len = advsm->scan_rsp_len;
assert(scan_rsp_len <= BLE_SCAN_RSP_DATA_MAX_LEN);
/* Set PDU payload length */
pdulen = BLE_DEV_ADDR_LEN + scan_rsp_len;
/* Get the advertising PDU */
m = advsm->scan_rsp_pdu;
assert(m != NULL);
/* Construct scan response */
if (advsm->own_addr_type == BLE_HCI_ADV_OWN_ADDR_PUBLIC) {
hdr = BLE_ADV_PDU_TYPE_SCAN_RSP;
addr = g_dev_addr;
} else if (advsm->own_addr_type == BLE_HCI_ADV_OWN_ADDR_RANDOM) {
hdr = BLE_ADV_PDU_TYPE_SCAN_RSP | BLE_ADV_PDU_HDR_TXADD_RAND;
addr = g_random_addr;
} else {
/* XXX: unsupported for now */
hdr = 0;
addr = NULL;
assert(0);
}
/* Set BLE transmit header */
ble_ll_mbuf_init(m, pdulen, hdr);
/* Put our address into buffer */
dptr = m->om_data;
memcpy(dptr, addr, BLE_DEV_ADDR_LEN);
/* Copy in scan response data, if any */
if (scan_rsp_len != 0) {
memcpy(dptr + BLE_DEV_ADDR_LEN, advsm->scan_rsp_data, scan_rsp_len);
}
}
/**
* Create the advertising PDU
*
* @param advsm Pointer to advertisement state machine
*/
static void
ble_ll_adv_pdu_make(struct ble_ll_adv_sm *advsm, struct os_mbuf *m)
{
uint8_t adv_data_len;
uint8_t *dptr;
uint8_t pdulen;
uint8_t pdu_type;
/* assume this is not a direct ind */
adv_data_len = advsm->adv_len;
pdulen = BLE_DEV_ADDR_LEN + adv_data_len;
/* Must be an advertising type! */
switch (advsm->adv_type) {
case BLE_HCI_ADV_TYPE_ADV_IND:
pdu_type = BLE_ADV_PDU_TYPE_ADV_IND;
break;
case BLE_HCI_ADV_TYPE_ADV_NONCONN_IND:
pdu_type = BLE_ADV_PDU_TYPE_ADV_NONCONN_IND;
break;
case BLE_HCI_ADV_TYPE_ADV_SCAN_IND:
pdu_type = BLE_ADV_PDU_TYPE_ADV_SCAN_IND;
break;
case BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_HD:
case BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_LD:
pdu_type = BLE_ADV_PDU_TYPE_ADV_DIRECT_IND;
adv_data_len = 0;
pdulen = BLE_ADV_DIRECT_IND_LEN;
if (advsm->adv_rxadd) {
pdu_type |= BLE_ADV_PDU_HDR_RXADD_RAND;
}
break;
/* Set these to avoid compiler warnings */
default:
pdulen = 0;
pdu_type = 0;
adv_data_len = 0xFF;
break;
}
/* An invalid advertising data length indicates a memory overwrite */
assert(adv_data_len <= BLE_ADV_DATA_MAX_LEN);
/* Set the PDU length in the state machine (includes header) */
advsm->adv_pdu_len = pdulen + BLE_LL_PDU_HDR_LEN;
/* Set TxAdd to random if needed. */
if (advsm->adv_txadd) {
pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND;
}
/* Get the advertising PDU and initialize it*/
ble_ll_mbuf_init(m, pdulen, pdu_type);
/* Construct advertisement */
dptr = m->om_data;
memcpy(dptr, advsm->adva, BLE_DEV_ADDR_LEN);
dptr += BLE_DEV_ADDR_LEN;
/* For ADV_DIRECT_IND add inita */
if (advsm->adv_directed) {
memcpy(dptr, advsm->initiator_addr, BLE_DEV_ADDR_LEN);
}
/* Copy in advertising data, if any */
if (adv_data_len != 0) {
memcpy(dptr, advsm->adv_data, adv_data_len);
}
}
/**
* Create the advertising PDU
*
* @param advsm Pointer to advertisement state machine
*/
static void
ble_ll_adv_pdu_make(struct ble_ll_adv_sm *advsm, struct os_mbuf *m)
{
int is_direct_adv;
uint8_t adv_data_len;
uint8_t *dptr;
uint8_t pdulen;
uint8_t pdu_type;
uint8_t *addr;
/* assume this is not a direct ind */
adv_data_len = advsm->adv_len;
pdulen = BLE_DEV_ADDR_LEN + adv_data_len;
is_direct_adv = 0;
/* Must be an advertising type! */
switch (advsm->adv_type) {
case BLE_HCI_ADV_TYPE_ADV_IND:
pdu_type = BLE_ADV_PDU_TYPE_ADV_IND;
break;
case BLE_HCI_ADV_TYPE_ADV_NONCONN_IND:
pdu_type = BLE_ADV_PDU_TYPE_ADV_NONCONN_IND;
break;
case BLE_HCI_ADV_TYPE_ADV_SCAN_IND:
pdu_type = BLE_ADV_PDU_TYPE_ADV_SCAN_IND;
break;
case BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_HD:
case BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_LD:
is_direct_adv = 1;
pdu_type = BLE_ADV_PDU_TYPE_ADV_DIRECT_IND;
adv_data_len = 0;
pdulen = BLE_ADV_DIRECT_IND_LEN;
if (advsm->peer_addr_type == BLE_HCI_ADV_PEER_ADDR_RANDOM) {
pdu_type |= BLE_ADV_PDU_HDR_RXADD_RAND;
}
break;
/* Set these to avoid compiler warnings */
default:
pdulen = 0;
pdu_type = 0;
adv_data_len = 0xFF;
break;
}
/* An invalid advertising data length indicates a memory overwrite */
assert(adv_data_len <= BLE_ADV_DATA_MAX_LEN);
/* Set the PDU length in the state machine (includes header) */
advsm->adv_pdu_len = pdulen + BLE_LL_PDU_HDR_LEN;
/* Construct scan response */
if (advsm->own_addr_type == BLE_HCI_ADV_OWN_ADDR_PUBLIC) {
addr = g_dev_addr;
} else if (advsm->own_addr_type == BLE_HCI_ADV_OWN_ADDR_RANDOM) {
pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND;
addr = g_random_addr;
} else {
/* XXX: unsupported for now. Should never happen */
addr = NULL;
assert(0);
}
/* Get the advertising PDU and initialize it*/
ble_ll_mbuf_init(m, pdulen, pdu_type);
/* Construct advertisement */
dptr = m->om_data;
memcpy(dptr, addr, BLE_DEV_ADDR_LEN);
dptr += BLE_DEV_ADDR_LEN;
/* For ADV_DIRECT_IND, we need to put initiators address in there */
if (is_direct_adv) {
memcpy(dptr, advsm->initiator_addr, BLE_DEV_ADDR_LEN);
}
/* Copy in advertising data, if any */
if (adv_data_len != 0) {
memcpy(dptr, advsm->adv_data, adv_data_len);
}
}
