/** * 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); } }