diff --git a/nimble/controller/include/controller/ble_ll_resolv.h b/nimble/controller/include/controller/ble_ll_resolv.h index 2f02e848a..e1dbbf316 100644 --- a/nimble/controller/include/controller/ble_ll_resolv.h +++ b/nimble/controller/include/controller/ble_ll_resolv.h @@ -59,9 +59,10 @@ int ble_ll_resolv_list_rmv(uint8_t *cmdbuf); /* Address resolution enable command */ int ble_ll_resolv_enable_cmd(uint8_t *cmdbuf); -/* XXX: implement */ -int ble_ll_resolv_peer_addr_rd(uint8_t *cmdbuf); -void ble_ll_resolv_local_addr_rd(uint8_t *cmdbuf); +int ble_ll_resolv_peer_addr_rd(uint8_t *cmdbuf, uint8_t *rspbuf, + uint8_t *rsplen); +int ble_ll_resolv_local_addr_rd(uint8_t *cmdbuf, uint8_t *rspbuf, + uint8_t *rsplen); /* Finds 'addr' in resolving list. Doesnt check if address resolution enabled */ struct ble_ll_resolv_entry * diff --git a/nimble/controller/src/ble_ll_hci.c b/nimble/controller/src/ble_ll_hci.c index df852d700..91ceb228d 100644 --- a/nimble/controller/src/ble_ll_hci.c +++ b/nimble/controller/src/ble_ll_hci.c @@ -851,10 +851,10 @@ ble_ll_hci_le_cmd_proc(uint8_t *cmdbuf, uint16_t ocf, uint8_t *rsplen, rc = ble_ll_resolv_list_read_size(rspbuf, rsplen); break; case BLE_HCI_OCF_LE_RD_PEER_RESOLV_ADDR: - rc = ble_ll_resolv_peer_addr_rd(cmdbuf); + rc = ble_ll_resolv_peer_addr_rd(cmdbuf, rspbuf, rsplen); break; case BLE_HCI_OCF_LE_RD_LOCAL_RESOLV_ADDR: - ble_ll_resolv_local_addr_rd(cmdbuf); + rc = ble_ll_resolv_local_addr_rd(cmdbuf, rspbuf, rsplen); break; case BLE_HCI_OCF_LE_SET_ADDR_RES_EN: rc = ble_ll_resolv_enable_cmd(cmdbuf); diff --git a/nimble/controller/src/ble_ll_resolv.c b/nimble/controller/src/ble_ll_resolv.c index d67ad56af..b7af9fd70 100644 --- a/nimble/controller/src/ble_ll_resolv.c +++ b/nimble/controller/src/ble_ll_resolv.c @@ -406,15 +406,51 @@ ble_ll_resolv_enable_cmd(uint8_t *cmdbuf) } int -ble_ll_resolv_peer_addr_rd(uint8_t *cmdbuf) +ble_ll_resolv_peer_addr_rd(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen) { - /* XXX */ - return 0; + struct ble_ll_resolv_entry *rl; + uint8_t addr_type; + uint8_t *ident_addr; + int rc; + + addr_type = cmdbuf[0]; + ident_addr = cmdbuf + 1; + + rl = ble_ll_resolv_list_find(ident_addr, addr_type); + if (rl) { + memcpy(rspbuf, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN); + rc = BLE_ERR_SUCCESS; + } else { + memset(rspbuf, 0, BLE_DEV_ADDR_LEN); + rc = BLE_ERR_UNK_CONN_ID; + } + + *rsplen = BLE_DEV_ADDR_LEN; + return rc; } -void -ble_ll_resolv_local_addr_rd(uint8_t *cmdbuf) +int +ble_ll_resolv_local_addr_rd(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen) { + struct ble_ll_resolv_entry *rl; + uint8_t addr_type; + uint8_t *ident_addr; + int rc; + + addr_type = cmdbuf[0]; + ident_addr = cmdbuf + 1; + + rl = ble_ll_resolv_list_find(ident_addr, addr_type); + if (rl) { + memcpy(rspbuf, rl->rl_local_rpa, BLE_DEV_ADDR_LEN); + rc = BLE_ERR_SUCCESS; + } else { + memset(rspbuf, 0, BLE_DEV_ADDR_LEN); + rc = BLE_ERR_UNK_CONN_ID; + } + + *rsplen = BLE_DEV_ADDR_LEN; + return rc; } /**