diff --git a/subsys/bluetooth/controller/ll_sw/ull_conn.c b/subsys/bluetooth/controller/ll_sw/ull_conn.c index c9561825edf..e83a25a2bbc 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_conn.c +++ b/subsys/bluetooth/controller/ll_sw/ull_conn.c @@ -49,8 +49,7 @@ static void conn_cleanup(struct ll_conn *conn); static void ctrl_tx_enqueue(struct ll_conn *conn, struct node_tx *tx); static inline void event_fex_prep(struct ll_conn *conn); static inline void event_vex_prep(struct ll_conn *conn); -static inline int event_conn_upd_prep(struct ll_conn *conn, - u16_t event_counter, +static inline int event_conn_upd_prep(struct ll_conn *conn, u16_t lazy, u32_t ticks_at_expire); static inline void event_ch_map_prep(struct ll_conn *conn, u16_t event_counter); @@ -761,14 +760,7 @@ int ull_conn_llcp(struct ll_conn *conn, u32_t ticks_at_expire, u16_t lazy) switch (conn->llcp_type) { case LLCP_CONN_UPD: { - struct lll_conn *lll = &conn->lll; - u16_t event_counter; - - /* Calculate current event counter */ - event_counter = lll->event_counter + - lll->latency_prepare + lazy; - - if (event_conn_upd_prep(conn, event_counter, + if (event_conn_upd_prep(conn, lazy, ticks_at_expire) == 0) { return -ECANCELED; } @@ -1726,12 +1718,14 @@ static inline void event_conn_upd_init(struct ll_conn *conn, #endif /* !CONFIG_BT_CTLR_SCHED_ADVANCED */ } -static inline int event_conn_upd_prep(struct ll_conn *conn, - u16_t event_counter, +static inline int event_conn_upd_prep(struct ll_conn *conn, u16_t lazy, u32_t ticks_at_expire) { + struct lll_conn *lll = &conn->lll; struct ll_conn *conn_upd; u16_t instant_latency; + u16_t event_counter; + conn_upd = conn_upd_curr; @@ -1740,6 +1734,9 @@ static inline int event_conn_upd_prep(struct ll_conn *conn, conn_upd_curr = conn; } + /* Calculate current event counter */ + event_counter = lll->event_counter + lll->latency_prepare + lazy; + instant_latency = (event_counter - conn->llcp.conn_upd.instant) & 0xffff; if (conn->llcp.conn_upd.state != LLCP_CUI_STATE_INPROG) { @@ -1805,7 +1802,6 @@ static inline int event_conn_upd_prep(struct ll_conn *conn, u32_t ticks_win_offset; u32_t conn_interval_us; struct node_rx_pdu *rx; - struct lll_conn *lll; u8_t ticker_id_conn; u32_t ticker_status; u32_t periodic_us; @@ -1889,6 +1885,7 @@ static inline int event_conn_upd_prep(struct ll_conn *conn, ticks_at_expire -= HAL_TICKER_US_TO_TICKS( (conn_interval_old - conn_interval_new) * 1250U); } + lll->latency_prepare += lazy; lll->latency_prepare -= (instant_latency - latency); /* calculate the offset, window widening and interval */