Bluetooth: controller: Fix soft latencies in periodic ticker

Fix, add the missing code for the removal of any accumulated
soft latencies or negative drift ticks when scheduling next
interval expiry with added laziness.

Typically a first interval would accumulate soft latencies
and this has to be removed if the interval is rescheduled
with any added laziness (scheduled to the next soft real
time interval).

Example, scan windows block any new scheduling until the end
of the window, adding latencies to any soft real time ticker
expiry which should try to execute as early as possible after
the scan window.

Fixes: zephyrproject-rtos#6083

Signed-off-by: Vinayak Kariappa Chettimada <[email protected]>

Author: cvinayak

Diff for: subsys/bluetooth/controller/ticker/ticker.c

@@ -834,6 +834,20 @@ static inline u32_t ticker_job_insert(struct ticker_instance *instance,
 			ticker->ticks_to_expire += ticker->ticks_periodic +
 						   ticker_remainder_inc(ticker);
 			ticker->lazy_current++;
+
+			/* Remove any accumulated drift (possibly added due to
+			 * ticker job execution latencies).
+			 */
+			if (ticker->ticks_to_expire >
+			    ticker->ticks_to_expire_minus) {
+				ticker->ticks_to_expire -=
+					ticker->ticks_to_expire_minus;
+				ticker->ticks_to_expire_minus = 0;
+			} else {
+				ticker->ticks_to_expire_minus -=
+					ticker->ticks_to_expire;
+				ticker->ticks_to_expire = 0;
+			}
 		} else {
 			return TICKER_STATUS_FAILURE;
 		}