| Message ID | 20210517215223.612671-1-luiz.dentz@gmail.com (mailing list archive) |
|---|---|
| State | New, archived |
| Headers | show |
| Series | [BlueZ] btdev: Add resolving list support | expand |
Hi, On Mon, May 17, 2021 at 2:52 PM Luiz Augusto von Dentz <luiz.dentz@gmail.com> wrote: > > From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com> > > This adds support for commands related to resolving list. > --- > emulator/btdev.c | 211 ++++++++++++++++++++++++++++++++++++++++++++++- > 1 file changed, 210 insertions(+), 1 deletion(-) > > diff --git a/emulator/btdev.c b/emulator/btdev.c > index 2fd452970..692fabd71 100644 > --- a/emulator/btdev.c > +++ b/emulator/btdev.c > @@ -34,6 +34,7 @@ > #include "btdev.h" > > #define WL_SIZE 16 > +#define RL_SIZE 16 > > #define has_bredr(btdev) (!((btdev)->features[4] & 0x20)) > #define has_le(btdev) (!!((btdev)->features[4] & 0x40)) > @@ -157,6 +158,9 @@ struct btdev { > uint8_t le_iso_path[2]; > > uint8_t le_wl[WL_SIZE][7]; > + uint8_t le_rl[RL_SIZE][39]; > + uint8_t le_rl_enable; > + uint16_t le_rl_timeout; > > uint8_t le_local_sk256[32]; > > @@ -3440,6 +3444,191 @@ static int cmd_remove_wl(struct btdev *dev, const void *data, uint8_t len) > return 0; > } > > +static int cmd_add_rl(struct btdev *dev, const void *data, uint8_t len) > +{ > + const struct bt_hci_cmd_le_add_to_resolv_list *cmd = data; > + uint8_t status; > + bool exists = false; > + int i, pos = -1; > + > + /* Valid range for address type is 0x00 to 0x01 */ > + if (cmd->addr_type > 0x01) > + return -EINVAL; > + > + for (i = 0; i < RL_SIZE; i++) { > + if (dev->le_rl[i][0] == cmd->addr_type && > + !memcmp(&dev->le_rl[i][1], cmd->addr, 6)) { > + exists = true; > + break; > + } else if (pos < 0 && dev->le_rl[i][0] == 0xff) > + pos = i; > + } > + > + if (exists) > + return -EALREADY; > + > + if (pos < 0) { > + cmd_status(dev, BT_HCI_ERR_MEM_CAPACITY_EXCEEDED, > + BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST); > + return 0; > + } > + > + dev->le_rl[pos][0] = cmd->addr_type; > + memcpy(&dev->le_rl[pos][1], cmd->addr, 6); > + memcpy(&dev->le_rl[pos][7], cmd->peer_irk, 16); > + memcpy(&dev->le_rl[pos][23], cmd->local_irk, 16); > + > + status = BT_HCI_ERR_SUCCESS; > + cmd_complete(dev, BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST, > + &status, sizeof(status)); > + > + return 0; > +} > + > +static int cmd_remove_rl(struct btdev *dev, const void *data, uint8_t len) > +{ > + const struct bt_hci_cmd_le_remove_from_resolv_list *cmd = data; > + uint8_t status; > + int i, pos = -1; > + > + /* Valid range for address type is 0x00 to 0x01 */ > + if (cmd->addr_type > 0x01) > + return -EINVAL; > + > + for (i = 0; i < RL_SIZE; i++) { > + if (dev->le_rl[i][0] == cmd->addr_type && > + !memcmp(&dev->le_rl[i][1], cmd->addr, 6)) { > + pos = i; > + break; > + } > + } > + > + if (pos < 0) > + return -EINVAL; > + > + dev->le_rl[pos][0] = 0xff; > + memset(&dev->le_rl[pos][1], 0, 38); > + > + status = BT_HCI_ERR_SUCCESS; > + cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST, > + &status, sizeof(status)); > + > + return 0; > +} > + > +static void rl_clear(struct btdev *dev) > +{ > + int i; > + > + for (i = 0; i < RL_SIZE; i++) { > + dev->le_rl[i][0] = 0xff; > + memset(&dev->le_rl[i][1], 0, 38); > + } > +} > + > +static int cmd_clear_rl(struct btdev *dev, const void *data, uint8_t len) > +{ > + uint8_t status; > + > + rl_clear(dev); > + > + status = BT_HCI_ERR_SUCCESS; > + cmd_complete(dev, BT_HCI_CMD_LE_CLEAR_RESOLV_LIST, > + &status, sizeof(status)); > + > + return 0; > +} > + > +static int cmd_read_rl_size(struct btdev *dev, const void *data, uint8_t len) > +{ > + struct bt_hci_rsp_le_read_resolv_list_size rsp; > + > + rsp.status = BT_HCI_ERR_SUCCESS; > + rsp.size = RL_SIZE; > + > + cmd_complete(dev, BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE, > + &rsp, sizeof(rsp)); > + > + return 0; > +} > + > +static int cmd_read_peer_rl_addr(struct btdev *dev, const void *data, > + uint8_t size) > +{ > + const struct bt_hci_cmd_le_read_peer_resolv_addr *cmd = data; > + struct bt_hci_rsp_le_read_peer_resolv_addr rsp; > + > + /* Valid range for address type is 0x00 to 0x01 */ > + if (cmd->addr_type > 0x01) > + return -EINVAL; > + > + rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID; > + memset(rsp.addr, 0, 6); > + > + cmd_complete(dev, BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR, > + &rsp, sizeof(rsp)); > + > + return 0; > +} > + > +static int cmd_read_local_rl_addr(struct btdev *dev, const void *data, > + uint8_t size) > +{ > + const struct bt_hci_cmd_le_read_local_resolv_addr *cmd = data; > + struct bt_hci_rsp_le_read_local_resolv_addr rsp; > + > + /* Valid range for address type is 0x00 to 0x01 */ > + if (cmd->addr_type > 0x01) > + return -EINVAL; > + > + rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID; > + memset(rsp.addr, 0, 6); > + > + cmd_complete(dev, BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR, > + &rsp, sizeof(rsp)); > + > + return 0; > +} > + > +static int cmd_set_rl_enable(struct btdev *dev, const void *data, uint8_t len) > +{ > + const struct bt_hci_cmd_le_set_resolv_enable *cmd = data; > + uint8_t status; > + > + /* Valid range for address resolution enable is 0x00 to 0x01 */ > + if (cmd->enable > 0x01) > + return -EINVAL; > + > + dev->le_rl_enable = cmd->enable; > + > + status = BT_HCI_ERR_SUCCESS; > + cmd_complete(dev, BT_HCI_CMD_LE_SET_RESOLV_ENABLE, > + &status, sizeof(status)); > + > + return 0; > +} > + > +static int cmd_set_rl_timeout(struct btdev *dev, const void *data, uint8_t len) > +{ > + const struct bt_hci_cmd_le_set_resolv_timeout *cmd = data; > + uint16_t timeout; > + uint8_t status; > + > + timeout = le16_to_cpu(cmd->timeout); > + > + /* Valid range for RPA timeout is 0x0001 to 0xa1b8 */ > + if (timeout < 0x0001 || timeout > 0xa1b8) > + return -EINVAL; > + > + dev->le_rl_timeout = timeout; > + > + status = BT_HCI_ERR_SUCCESS; > + cmd_complete(dev, BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT, > + &status, sizeof(status)); > + > + return 0; > +} > + > static int cmd_conn_update(struct btdev *dev, const void *data, uint8_t len) > { > cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_CONN_UPDATE); > @@ -3828,7 +4017,16 @@ static int cmd_gen_dhkey(struct btdev *dev, const void *data, uint8_t len) > CMD(BT_HCI_CMD_LE_CONN_PARAM_REQ_NEG_REPLY, cmd_conn_param_neg_reply, \ > cmd_conn_param_neg_reply_complete), \ > CMD(BT_HCI_CMD_LE_READ_LOCAL_PK256, cmd_read_local_pk256, NULL), \ > - CMD(BT_HCI_CMD_LE_GENERATE_DHKEY, cmd_gen_dhkey, NULL) > + CMD(BT_HCI_CMD_LE_GENERATE_DHKEY, cmd_gen_dhkey, NULL), \ > + CMD(BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST, cmd_add_rl, NULL), \ > + CMD(BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST, cmd_remove_rl, NULL), \ > + CMD(BT_HCI_CMD_LE_CLEAR_RESOLV_LIST, cmd_clear_rl, NULL), \ > + CMD(BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE, cmd_read_rl_size, NULL), \ > + CMD(BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR, cmd_read_peer_rl_addr, NULL), \ > + CMD(BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR, cmd_read_local_rl_addr, \ > + NULL), \ > + CMD(BT_HCI_CMD_LE_SET_RESOLV_ENABLE, cmd_set_rl_enable, NULL), \ > + CMD(BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT, cmd_set_rl_timeout, NULL) > > static int cmd_set_default_phy(struct btdev *dev, const void *data, > uint8_t len) > @@ -5022,6 +5220,14 @@ static void set_le_commands(struct btdev *btdev) > /* Extra LE commands for >= 4.2 adapters */ > btdev->commands[34] |= 0x02; /* LE Read Local P-256 Public Key */ > btdev->commands[34] |= 0x04; /* LE Generate DHKey */ > + btdev->commands[34] |= 0x08; /* LE Add Device To Resolving List */ > + btdev->commands[34] |= 0x10; /* LE Remove Dev From Resolving List */ > + btdev->commands[34] |= 0x20; /* LE Clear Resolving List */ > + btdev->commands[34] |= 0x40; /* LE Read Resolving List Size */ > + btdev->commands[34] |= 0x80; /* LE Read Peer Resolvable Address */ > + btdev->commands[35] |= 0x01; /* LE Read Local Resolvable Address */ > + btdev->commands[35] |= 0x02; /* LE Set Address Resolution Enable */ > + btdev->commands[35] |= 0x04; /* LE Set RPA Timeout */ > > btdev->cmds = cmd_le; > > @@ -5302,6 +5508,9 @@ static void set_le_states(struct btdev *btdev) > btdev->le_states[5] = 0x03; > > wl_clear(btdev); > + rl_clear(btdev); > + btdev->le_rl_enable = 0x00; > + btdev->le_rl_timeout = 0x0384; /* 900 secs or 15 minutes */ > } > > static void set_amp_features(struct btdev *btdev) > -- > 2.30.2 Pushed.
diff --git a/emulator/btdev.c b/emulator/btdev.c index 2fd452970..692fabd71 100644 --- a/emulator/btdev.c +++ b/emulator/btdev.c @@ -34,6 +34,7 @@ #include "btdev.h" #define WL_SIZE 16 +#define RL_SIZE 16 #define has_bredr(btdev) (!((btdev)->features[4] & 0x20)) #define has_le(btdev) (!!((btdev)->features[4] & 0x40)) @@ -157,6 +158,9 @@ struct btdev { uint8_t le_iso_path[2]; uint8_t le_wl[WL_SIZE][7]; + uint8_t le_rl[RL_SIZE][39]; + uint8_t le_rl_enable; + uint16_t le_rl_timeout; uint8_t le_local_sk256[32]; @@ -3440,6 +3444,191 @@ static int cmd_remove_wl(struct btdev *dev, const void *data, uint8_t len) return 0; } +static int cmd_add_rl(struct btdev *dev, const void *data, uint8_t len) +{ + const struct bt_hci_cmd_le_add_to_resolv_list *cmd = data; + uint8_t status; + bool exists = false; + int i, pos = -1; + + /* Valid range for address type is 0x00 to 0x01 */ + if (cmd->addr_type > 0x01) + return -EINVAL; + + for (i = 0; i < RL_SIZE; i++) { + if (dev->le_rl[i][0] == cmd->addr_type && + !memcmp(&dev->le_rl[i][1], cmd->addr, 6)) { + exists = true; + break; + } else if (pos < 0 && dev->le_rl[i][0] == 0xff) + pos = i; + } + + if (exists) + return -EALREADY; + + if (pos < 0) { + cmd_status(dev, BT_HCI_ERR_MEM_CAPACITY_EXCEEDED, + BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST); + return 0; + } + + dev->le_rl[pos][0] = cmd->addr_type; + memcpy(&dev->le_rl[pos][1], cmd->addr, 6); + memcpy(&dev->le_rl[pos][7], cmd->peer_irk, 16); + memcpy(&dev->le_rl[pos][23], cmd->local_irk, 16); + + status = BT_HCI_ERR_SUCCESS; + cmd_complete(dev, BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST, + &status, sizeof(status)); + + return 0; +} + +static int cmd_remove_rl(struct btdev *dev, const void *data, uint8_t len) +{ + const struct bt_hci_cmd_le_remove_from_resolv_list *cmd = data; + uint8_t status; + int i, pos = -1; + + /* Valid range for address type is 0x00 to 0x01 */ + if (cmd->addr_type > 0x01) + return -EINVAL; + + for (i = 0; i < RL_SIZE; i++) { + if (dev->le_rl[i][0] == cmd->addr_type && + !memcmp(&dev->le_rl[i][1], cmd->addr, 6)) { + pos = i; + break; + } + } + + if (pos < 0) + return -EINVAL; + + dev->le_rl[pos][0] = 0xff; + memset(&dev->le_rl[pos][1], 0, 38); + + status = BT_HCI_ERR_SUCCESS; + cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST, + &status, sizeof(status)); + + return 0; +} + +static void rl_clear(struct btdev *dev) +{ + int i; + + for (i = 0; i < RL_SIZE; i++) { + dev->le_rl[i][0] = 0xff; + memset(&dev->le_rl[i][1], 0, 38); + } +} + +static int cmd_clear_rl(struct btdev *dev, const void *data, uint8_t len) +{ + uint8_t status; + + rl_clear(dev); + + status = BT_HCI_ERR_SUCCESS; + cmd_complete(dev, BT_HCI_CMD_LE_CLEAR_RESOLV_LIST, + &status, sizeof(status)); + + return 0; +} + +static int cmd_read_rl_size(struct btdev *dev, const void *data, uint8_t len) +{ + struct bt_hci_rsp_le_read_resolv_list_size rsp; + + rsp.status = BT_HCI_ERR_SUCCESS; + rsp.size = RL_SIZE; + + cmd_complete(dev, BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE, + &rsp, sizeof(rsp)); + + return 0; +} + +static int cmd_read_peer_rl_addr(struct btdev *dev, const void *data, + uint8_t size) +{ + const struct bt_hci_cmd_le_read_peer_resolv_addr *cmd = data; + struct bt_hci_rsp_le_read_peer_resolv_addr rsp; + + /* Valid range for address type is 0x00 to 0x01 */ + if (cmd->addr_type > 0x01) + return -EINVAL; + + rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID; + memset(rsp.addr, 0, 6); + + cmd_complete(dev, BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR, + &rsp, sizeof(rsp)); + + return 0; +} + +static int cmd_read_local_rl_addr(struct btdev *dev, const void *data, + uint8_t size) +{ + const struct bt_hci_cmd_le_read_local_resolv_addr *cmd = data; + struct bt_hci_rsp_le_read_local_resolv_addr rsp; + + /* Valid range for address type is 0x00 to 0x01 */ + if (cmd->addr_type > 0x01) + return -EINVAL; + + rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID; + memset(rsp.addr, 0, 6); + + cmd_complete(dev, BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR, + &rsp, sizeof(rsp)); + + return 0; +} + +static int cmd_set_rl_enable(struct btdev *dev, const void *data, uint8_t len) +{ + const struct bt_hci_cmd_le_set_resolv_enable *cmd = data; + uint8_t status; + + /* Valid range for address resolution enable is 0x00 to 0x01 */ + if (cmd->enable > 0x01) + return -EINVAL; + + dev->le_rl_enable = cmd->enable; + + status = BT_HCI_ERR_SUCCESS; + cmd_complete(dev, BT_HCI_CMD_LE_SET_RESOLV_ENABLE, + &status, sizeof(status)); + + return 0; +} + +static int cmd_set_rl_timeout(struct btdev *dev, const void *data, uint8_t len) +{ + const struct bt_hci_cmd_le_set_resolv_timeout *cmd = data; + uint16_t timeout; + uint8_t status; + + timeout = le16_to_cpu(cmd->timeout); + + /* Valid range for RPA timeout is 0x0001 to 0xa1b8 */ + if (timeout < 0x0001 || timeout > 0xa1b8) + return -EINVAL; + + dev->le_rl_timeout = timeout; + + status = BT_HCI_ERR_SUCCESS; + cmd_complete(dev, BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT, + &status, sizeof(status)); + + return 0; +} + static int cmd_conn_update(struct btdev *dev, const void *data, uint8_t len) { cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_CONN_UPDATE); @@ -3828,7 +4017,16 @@ static int cmd_gen_dhkey(struct btdev *dev, const void *data, uint8_t len) CMD(BT_HCI_CMD_LE_CONN_PARAM_REQ_NEG_REPLY, cmd_conn_param_neg_reply, \ cmd_conn_param_neg_reply_complete), \ CMD(BT_HCI_CMD_LE_READ_LOCAL_PK256, cmd_read_local_pk256, NULL), \ - CMD(BT_HCI_CMD_LE_GENERATE_DHKEY, cmd_gen_dhkey, NULL) + CMD(BT_HCI_CMD_LE_GENERATE_DHKEY, cmd_gen_dhkey, NULL), \ + CMD(BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST, cmd_add_rl, NULL), \ + CMD(BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST, cmd_remove_rl, NULL), \ + CMD(BT_HCI_CMD_LE_CLEAR_RESOLV_LIST, cmd_clear_rl, NULL), \ + CMD(BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE, cmd_read_rl_size, NULL), \ + CMD(BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR, cmd_read_peer_rl_addr, NULL), \ + CMD(BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR, cmd_read_local_rl_addr, \ + NULL), \ + CMD(BT_HCI_CMD_LE_SET_RESOLV_ENABLE, cmd_set_rl_enable, NULL), \ + CMD(BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT, cmd_set_rl_timeout, NULL) static int cmd_set_default_phy(struct btdev *dev, const void *data, uint8_t len) @@ -5022,6 +5220,14 @@ static void set_le_commands(struct btdev *btdev) /* Extra LE commands for >= 4.2 adapters */ btdev->commands[34] |= 0x02; /* LE Read Local P-256 Public Key */ btdev->commands[34] |= 0x04; /* LE Generate DHKey */ + btdev->commands[34] |= 0x08; /* LE Add Device To Resolving List */ + btdev->commands[34] |= 0x10; /* LE Remove Dev From Resolving List */ + btdev->commands[34] |= 0x20; /* LE Clear Resolving List */ + btdev->commands[34] |= 0x40; /* LE Read Resolving List Size */ + btdev->commands[34] |= 0x80; /* LE Read Peer Resolvable Address */ + btdev->commands[35] |= 0x01; /* LE Read Local Resolvable Address */ + btdev->commands[35] |= 0x02; /* LE Set Address Resolution Enable */ + btdev->commands[35] |= 0x04; /* LE Set RPA Timeout */ btdev->cmds = cmd_le; @@ -5302,6 +5508,9 @@ static void set_le_states(struct btdev *btdev) btdev->le_states[5] = 0x03; wl_clear(btdev); + rl_clear(btdev); + btdev->le_rl_enable = 0x00; + btdev->le_rl_timeout = 0x0384; /* 900 secs or 15 minutes */ } static void set_amp_features(struct btdev *btdev)
From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com> This adds support for commands related to resolving list. --- emulator/btdev.c | 211 ++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 210 insertions(+), 1 deletion(-)