btdev: Add resolving list support
Luiz Augusto von Dentz
maintainerPATCH 1/1 btdev: Add resolving list support
From: Luiz Augusto von Dentz <[email protected]>
This adds support for commands related to resolving list.
--- emulator/btdev.c | 211 ++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 210 insertions(+), 1 deletion(-)
emulator/btdev.c +210 -1
@@ -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)
Signature · 1 line
2.30.2
Luiz Augusto von Dentz
maintainerHi,
Quoted 266 lines · Luiz Augusto von Dentz
From: Luiz Augusto von Dentz <[email protected]>
This adds support for commands related to resolving list.
--- emulator/btdev.c | 211 ++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 210 insertions(+), 1 deletion(-)emulator/btdev.c +210
-2@@ -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) --
Quoted 1 line
2.30.2
Pushed.
Signature · 1 line
Luiz Augusto von Dentz