@@ -810,10 +810,13 @@ static bool tx_cancel(struct mesh_io *io, const uint8_t *data, uint8_t len)
static bool find_by_filter(const void *a, const void *b)
{
- const struct pvt_rx_reg *rx_reg = a;
- const uint8_t *filter = b;
+ const struct pvt_rx_reg *rx_reg_old = a;
+ const struct pvt_rx_reg *rx_reg = b;
+
+ if (rx_reg_old->len != rx_reg->len)
+ return false;
- return !memcmp(rx_reg->filter, filter, rx_reg->len);
+ return !memcmp(rx_reg_old->filter, rx_reg->filter, rx_reg->len);
}
static bool recv_register(struct mesh_io *io, const uint8_t *filter,
@@ -821,16 +824,13 @@ static bool recv_register(struct mesh_io *io, const uint8_t *filter,
{
struct bt_hci_cmd_le_set_scan_enable cmd;
struct mesh_io_private *pvt = io->pvt;
- struct pvt_rx_reg *rx_reg;
+ struct pvt_rx_reg *rx_reg, *rx_reg_old;
bool already_scanning;
bool active = false;
if (!cb || !filter || !len)
return false;
- rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter, filter);
-
- l_free(rx_reg);
rx_reg = l_malloc(sizeof(*rx_reg) + len);
memcpy(rx_reg->filter, filter, len);
@@ -838,6 +838,10 @@ static bool recv_register(struct mesh_io *io, const uint8_t *filter,
rx_reg->cb = cb;
rx_reg->user_data = user_data;
+ rx_reg_old = l_queue_remove_if(pvt->rx_regs, find_by_filter, rx_reg);
+
+ l_free(rx_reg_old);
+
already_scanning = !l_queue_isempty(pvt->rx_regs);
l_queue_push_head(pvt->rx_regs, rx_reg);
@@ -863,14 +867,20 @@ static bool recv_deregister(struct mesh_io *io, const uint8_t *filter,
{
struct bt_hci_cmd_le_set_scan_enable cmd = {0, 0};
struct mesh_io_private *pvt = io->pvt;
- struct pvt_rx_reg *rx_reg;
+ struct pvt_rx_reg *rx_reg, *rx_reg_tmp;
bool active = false;
- rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter, filter);
+ rx_reg_tmp = l_malloc(sizeof(*rx_reg_tmp) + len);
+ memcpy(&rx_reg_tmp->filter, filter, len);
+ rx_reg_tmp->len = len;
+
+ rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter, rx_reg_tmp);
if (rx_reg)
l_free(rx_reg);
+ l_free(rx_reg_tmp);
+
/* Look for any AD types requiring Active Scanning */
if (l_queue_find(pvt->rx_regs, find_active, NULL))
active = true;
@@ -488,24 +488,24 @@ static bool tx_cancel(struct mesh_io *io, const uint8_t *data, uint8_t len)
static bool find_by_filter(const void *a, const void *b)
{
- const struct pvt_rx_reg *rx_reg = a;
- const uint8_t *filter = b;
+ const struct pvt_rx_reg *rx_reg_old = a;
+ const struct pvt_rx_reg *rx_reg = b;
- return !memcmp(rx_reg->filter, filter, rx_reg->len);
+ if (rx_reg_old->len != rx_reg->len)
+ return false;
+
+ return !memcmp(rx_reg_old->filter, rx_reg->filter, rx_reg->len);
}
static bool recv_register(struct mesh_io *io, const uint8_t *filter,
uint8_t len, mesh_io_recv_func_t cb, void *user_data)
{
struct mesh_io_private *pvt = io->pvt;
- struct pvt_rx_reg *rx_reg;
+ struct pvt_rx_reg *rx_reg, *rx_reg_old;
if (!cb || !filter || !len)
return false;
- rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter, filter);
-
- l_free(rx_reg);
rx_reg = l_malloc(sizeof(*rx_reg) + len);
memcpy(rx_reg->filter, filter, len);
@@ -513,6 +513,10 @@ static bool recv_register(struct mesh_io *io, const uint8_t *filter,
rx_reg->cb = cb;
rx_reg->user_data = user_data;
+ rx_reg_old = l_queue_remove_if(pvt->rx_regs, find_by_filter, rx_reg);
+
+ l_free(rx_reg_old);
+
l_queue_push_head(pvt->rx_regs, rx_reg);
return true;