diff --git a/drivers/bluetooth/nble/gatt.c b/drivers/bluetooth/nble/gatt.c index 614c21d074b..ee8aa1f9cd2 100644 --- a/drivers/bluetooth/nble/gatt.c +++ b/drivers/bluetooth/nble/gatt.c @@ -40,7 +40,7 @@ struct nble_gatt_service { static struct nble_gatt_service svc_db[BLE_GATTS_MAX_SERVICES]; static uint8_t svc_count; -static struct bt_gatt_subscribe_params *subscriptions; +static sys_slist_t subscriptions; /** * Copy a UUID in a buffer using the smallest memory length @@ -1190,32 +1190,31 @@ static void gatt_subscription_add(struct bt_conn *conn, bt_addr_le_copy(¶ms->_peer, &conn->dst); /* Prepend subscription */ - params->_next = subscriptions; - subscriptions = params; + sys_slist_prepend(&subscriptions, ¶ms->node); } -static void gatt_subscription_remove(struct bt_conn *conn, - struct bt_gatt_subscribe_params *prev, +static void gatt_subscription_remove(struct bt_conn *conn, sys_snode_t *prev, struct bt_gatt_subscribe_params *params) { /* Remove subscription from the list*/ - if (!prev) { - subscriptions = params->_next; - } else { - prev->_next = params->_next; - } + sys_slist_remove(&subscriptions, prev, ¶ms->node); params->notify(conn, params, NULL, 0); } static void remove_subscriptions(struct bt_conn *conn) { - struct bt_gatt_subscribe_params *params, *prev; + sys_snode_t *node, *tmp, *prev = NULL; /* Lookup existing subscriptions */ - for (params = subscriptions, prev = NULL; params; - prev = params, params = params->_next) { + SYS_SLIST_FOR_EACH_NODE_SAFE(&subscriptions, node, tmp) { + struct bt_gatt_subscribe_params *params; + + params = CONTAINER_OF(node, struct bt_gatt_subscribe_params, + node); + if (bt_addr_le_cmp(¶ms->_peer, &conn->dst)) { + prev = node; continue; } @@ -1247,7 +1246,7 @@ static int gatt_write_ccc(struct bt_conn *conn, int bt_gatt_subscribe(struct bt_conn *conn, struct bt_gatt_subscribe_params *params) { - struct bt_gatt_subscribe_params *tmp; + sys_snode_t *node; bool has_subscription = false; if (!conn || !params || !params->notify || @@ -1263,7 +1262,12 @@ int bt_gatt_subscribe(struct bt_conn *conn, conn, params->value_handle, params->ccc_handle, params->value); /* Lookup existing subscriptions */ - for (tmp = subscriptions; tmp; tmp = tmp->_next) { + SYS_SLIST_FOR_EACH_NODE(&subscriptions, node) { + struct bt_gatt_subscribe_params *tmp; + + tmp = CONTAINER_OF(node, struct bt_gatt_subscribe_params, + node); + /* Fail if entry already exists */ if (tmp == params) { return -EALREADY; @@ -1299,7 +1303,7 @@ int bt_gatt_subscribe(struct bt_conn *conn, void on_nble_gattc_value_evt(const struct nble_gattc_value_evt *ev, uint8_t *data, uint8_t length) { - struct bt_gatt_subscribe_params *params; + sys_snode_t *node, *tmp, *prev = NULL; struct bt_conn *conn; conn = bt_conn_lookup_handle(ev->conn_handle); @@ -1311,14 +1315,22 @@ void on_nble_gattc_value_evt(const struct nble_gattc_value_evt *ev, BT_DBG("conn %p value handle 0x%04x status %d data len %u", conn, ev->handle, ev->status, length); - for (params = subscriptions; params; params = params->_next) { + SYS_SLIST_FOR_EACH_NODE_SAFE(&subscriptions, node, tmp) { + struct bt_gatt_subscribe_params *params; + + params = CONTAINER_OF(node, struct bt_gatt_subscribe_params, + node); + if (ev->handle != params->value_handle) { + prev = node; continue; } if (params->notify(conn, params, data, length) == BT_GATT_ITER_STOP) { bt_gatt_unsubscribe(conn, params); + } else { + prev = node; } } @@ -1328,7 +1340,8 @@ void on_nble_gattc_value_evt(const struct nble_gattc_value_evt *ev, int bt_gatt_unsubscribe(struct bt_conn *conn, struct bt_gatt_subscribe_params *params) { - struct bt_gatt_subscribe_params *tmp, *found = NULL; + sys_snode_t *node, *next, *prev = NULL; + struct bt_gatt_subscribe_params *found = NULL; bool has_subscription = false; if (!conn || !params) { @@ -1342,18 +1355,19 @@ int bt_gatt_unsubscribe(struct bt_conn *conn, BT_DBG("conn %p value_handle 0x%04x ccc_handle 0x%04x value 0x%04x", conn, params->value_handle, params->ccc_handle, params->value); - /* Check head */ - if (subscriptions == params) { - subscriptions = params->_next; - found = params; - } - /* Lookup existing subscriptions */ - for (tmp = subscriptions; tmp; tmp = tmp->_next) { + SYS_SLIST_FOR_EACH_NODE_SAFE(&subscriptions, node, next) { + struct bt_gatt_subscribe_params *tmp; + + tmp = CONTAINER_OF(node, struct bt_gatt_subscribe_params, + node); + /* Remove subscription */ - if (tmp->_next == params) { - tmp->_next = params->_next; + if (params == tmp) { found = params; + sys_slist_remove(&subscriptions, prev, node); + } else { + prev = node; } /* Check if there still remains any other subscription */ diff --git a/include/bluetooth/gatt.h b/include/bluetooth/gatt.h index 4e074ca8650..8d36363530f 100644 --- a/include/bluetooth/gatt.h +++ b/include/bluetooth/gatt.h @@ -972,7 +972,7 @@ struct bt_gatt_subscribe_params { uint16_t ccc_handle; /** Subscribe value */ uint16_t value; - struct bt_gatt_subscribe_params *_next; + sys_snode_t node; }; /** @brief Subscribe Attribute Value Notification diff --git a/subsys/bluetooth/host/gatt.c b/subsys/bluetooth/host/gatt.c index 711aca5a100..12c0c3296a7 100644 --- a/subsys/bluetooth/host/gatt.c +++ b/subsys/bluetooth/host/gatt.c @@ -33,7 +33,7 @@ static struct bt_gatt_attr *db; #if defined(CONFIG_BLUETOOTH_GATT_CLIENT) -static struct bt_gatt_subscribe_params *subscriptions; +static sys_slist_t subscriptions; #endif /* CONFIG_BLUETOOTH_GATT_CLIENT */ #if !defined(CONFIG_BLUETOOTH_GATT_DYNAMIC_DB) @@ -697,48 +697,53 @@ static uint8_t disconnected_cb(const struct bt_gatt_attr *attr, void *user_data) void bt_gatt_notification(struct bt_conn *conn, uint16_t handle, const void *data, uint16_t length) { - struct bt_gatt_subscribe_params *params; + sys_snode_t *node, *tmp, *prev = NULL; BT_DBG("handle 0x%04x length %u", handle, length); - for (params = subscriptions; params; params = params->_next) { - if (bt_conn_addr_le_cmp(conn, ¶ms->_peer)) { - continue; - } + SYS_SLIST_FOR_EACH_NODE_SAFE(&subscriptions, node, tmp) { + struct bt_gatt_subscribe_params *params; - if (handle != params->value_handle) { + params = CONTAINER_OF(node, struct bt_gatt_subscribe_params, + node); + + if (bt_conn_addr_le_cmp(conn, ¶ms->_peer) || + handle != params->value_handle) { + prev = node; continue; } if (params->notify(conn, params, data, length) == BT_GATT_ITER_STOP) { bt_gatt_unsubscribe(conn, params); + } else { + prev = node; } } } -static void gatt_subscription_remove(struct bt_conn *conn, - struct bt_gatt_subscribe_params *prev, +static void gatt_subscription_remove(struct bt_conn *conn, sys_snode_t *prev, struct bt_gatt_subscribe_params *params) { /* Remove subscription from the list*/ - if (!prev) { - subscriptions = params->_next; - } else { - prev->_next = params->_next; - } + sys_slist_remove(&subscriptions, prev, ¶ms->node); params->notify(conn, params, NULL, 0); } static void remove_subscriptions(struct bt_conn *conn) { - struct bt_gatt_subscribe_params *params, *prev; + sys_snode_t *node, *tmp, *prev = NULL; /* Lookup existing subscriptions */ - for (params = subscriptions, prev = NULL; params; - prev = params, params = params->_next) { + SYS_SLIST_FOR_EACH_NODE_SAFE(&subscriptions, node, tmp) { + struct bt_gatt_subscribe_params *params; + + params = CONTAINER_OF(node, struct bt_gatt_subscribe_params, + node); + if (bt_conn_addr_le_cmp(conn, ¶ms->_peer)) { + prev = node; continue; } @@ -1629,8 +1634,7 @@ static void gatt_subscription_add(struct bt_conn *conn, bt_addr_le_copy(¶ms->_peer, &conn->le.dst); /* Prepend subscription */ - params->_next = subscriptions; - subscriptions = params; + sys_slist_prepend(&subscriptions, ¶ms->node); } static void gatt_write_ccc_rsp(struct bt_conn *conn, uint8_t err, @@ -1643,15 +1647,15 @@ static void gatt_write_ccc_rsp(struct bt_conn *conn, uint8_t err, /* if write to CCC failed we remove subscription and notify app */ if (err) { - struct bt_gatt_subscribe_params *cur, *prev; + sys_snode_t *node, *tmp, *prev = NULL; - for (cur = subscriptions, prev = NULL; cur; - prev = cur, cur = cur->_next) { - - if (cur == params) { - gatt_subscription_remove(conn, prev, params); + SYS_SLIST_FOR_EACH_NODE_SAFE(&subscriptions, node, tmp) { + if (node == ¶ms->node) { + gatt_subscription_remove(conn, tmp, params); break; } + + prev = node; } } } @@ -1681,7 +1685,7 @@ static int gatt_write_ccc(struct bt_conn *conn, uint16_t handle, uint16_t value, int bt_gatt_subscribe(struct bt_conn *conn, struct bt_gatt_subscribe_params *params) { - struct bt_gatt_subscribe_params *tmp; + sys_snode_t *node; bool has_subscription = false; if (!conn || !params || !params->notify || @@ -1694,7 +1698,12 @@ int bt_gatt_subscribe(struct bt_conn *conn, } /* Lookup existing subscriptions */ - for (tmp = subscriptions; tmp; tmp = tmp->_next) { + SYS_SLIST_FOR_EACH_NODE(&subscriptions, node) { + struct bt_gatt_subscribe_params *tmp; + + tmp = CONTAINER_OF(node, struct bt_gatt_subscribe_params, + node); + /* Fail if entry already exists */ if (tmp == params) { return -EALREADY; @@ -1731,7 +1740,7 @@ int bt_gatt_subscribe(struct bt_conn *conn, int bt_gatt_unsubscribe(struct bt_conn *conn, struct bt_gatt_subscribe_params *params) { - struct bt_gatt_subscribe_params *tmp; + sys_snode_t *node, *next, *prev = NULL; bool has_subscription = false, found = false; if (!conn || !params) { @@ -1742,18 +1751,19 @@ int bt_gatt_unsubscribe(struct bt_conn *conn, return -ENOTCONN; } - /* Check head */ - if (subscriptions == params) { - subscriptions = params->_next; - found = true; - } - /* Lookup existing subscriptions */ - for (tmp = subscriptions; tmp; tmp = tmp->_next) { + SYS_SLIST_FOR_EACH_NODE_SAFE(&subscriptions, node, next) { + struct bt_gatt_subscribe_params *tmp; + + tmp = CONTAINER_OF(node, struct bt_gatt_subscribe_params, + node); + /* Remove subscription */ - if (tmp->_next == params) { - tmp->_next = params->_next; + if (params == tmp) { found = true; + sys_slist_remove(&subscriptions, prev, node); + } else { + prev = node; } /* Check if there still remains any other subscription */ @@ -1781,11 +1791,15 @@ void bt_gatt_cancel(struct bt_conn *conn, void *params) static void add_subscriptions(struct bt_conn *conn) { - struct bt_gatt_subscribe_params *params, *prev; + sys_snode_t *node; /* Lookup existing subscriptions */ - for (params = subscriptions, prev = NULL; params; - prev = params, params = params->_next) { + SYS_SLIST_FOR_EACH_NODE(&subscriptions, node) { + struct bt_gatt_subscribe_params *params; + + params = CONTAINER_OF(node, struct bt_gatt_subscribe_params, + node); + if (bt_conn_addr_le_cmp(conn, ¶ms->_peer)) { continue; } @@ -1800,11 +1814,15 @@ static void add_subscriptions(struct bt_conn *conn) static void update_subscriptions(struct bt_conn *conn) { - struct bt_gatt_subscribe_params *params, *prev; + sys_snode_t *node; /* Update existing subscriptions */ - for (params = subscriptions, prev = NULL; params; - prev = params, params = params->_next) { + SYS_SLIST_FOR_EACH_NODE(&subscriptions, node) { + struct bt_gatt_subscribe_params *params; + + params = CONTAINER_OF(node, struct bt_gatt_subscribe_params, + node); + if (params->_peer.type == BT_ADDR_LE_PUBLIC) { continue; }