Date: Tue, 10 Mar 2020 11:20:19 -0400
Subject: [PATCH] Mesh timer scheduler fix (#159)

* FIXES Mesh timer incorrect loading Capture compare register

See Github pull request #158

* ADDS Recovery procedure when message fail to publish over MESH

This works by tracking how many publication failure occur and reschedule the timer with the oldest event.
Related to issue PR #158

* ADDS Patch file for Issue #158

* REFACTOR Reschedule condition to check wrap around condition

* ADDS New Patch file to reflect changes applies under review

---
 .../3.1.0/src/mesh/core/include/timer.h       |  8 ++++
 .../src/mesh/core/include/timer_scheduler.h   |  7 +++
 .../3.1.0/src/mesh/core/src/core_tx_adv.c     | 33 ++++++++++++-
 .../3.1.0/src/mesh/core/src/timer.c           | 47 ++++++++++++-------
 .../3.1.0/src/mesh/core/src/timer_scheduler.c | 11 +++++
 5 files changed, 87 insertions(+), 19 deletions(-)

diff --git a/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/include/timer.h b/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/include/timer.h
index 7bdaa4d6..a80eb3d6 100644
--- a/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/include/timer.h
+++ b/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/include/timer.h
@@ -119,6 +119,14 @@ void timer_start(timestamp_t timestamp, timer_callback_t cb);
  */
 void timer_stop(void);

+/**
+ * @brief Calculate the number of ticks until
+ *        capture compare register 1 will fire.
+ *
+ * @return Ticks
+ */
+uint32_t timer_ticks_until_next_event(void);
+
 /** @} */

 #endif /* MESH_TIMER_H__ */
diff --git a/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/include/timer_scheduler.h b/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/include/timer_scheduler.h
index 6f853525..46f42950 100644
--- a/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/include/timer_scheduler.h
+++ b/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/include/timer_scheduler.h
@@ -128,6 +128,13 @@ void timer_sch_reschedule(timer_event_t* p_timer_evt, timestamp_t new_timestamp)
  */
 bool timer_sch_is_scheduled(const timer_event_t * p_timer_evt);

+/**
+ * @brief In the attempt that the scheduler fails to fire it's oldest event.
+ *        Perform a recovery by scheduling the oldest event to fire.
+ *        This meant to be used as a recovery attempt it things keep failing
+ */
+void timer_sch_recover(void);
+
 /** @} */

 #endif /* TIMER_SCHEDULER_H__ */
diff --git a/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/src/core_tx_adv.c b/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/src/core_tx_adv.c
index 71f8849b..1927a6f1 100644
--- a/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/src/core_tx_adv.c
+++ b/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/src/core_tx_adv.c
@@ -45,8 +45,9 @@

 typedef struct
 {
-    uint32_t adv_tx_count;
-    advertiser_t advertiser;
+    uint32_t        adv_tx_count;
+    uint8_t         alloc_fail_count;
+    advertiser_t    advertiser;
 } adv_bearer_role_t;

 static adv_bearer_role_t m_bearer_roles[CORE_TX_ROLE_COUNT];
@@ -68,6 +69,9 @@ static core_tx_alloc_result_t packet_alloc(core_tx_bearer_t * p_bearer, const co
 static void packet_send(core_tx_bearer_t * p_bearer, const uint8_t * p_packet, uint32_t packet_length);
 static void packet_discard(core_tx_bearer_t * p_bearer);

+
+static const uint32_t MAX_TICKS_TO_NEXT_EVENT = RTC_COUNTER_COUNTER_Msk / 2;
+
 static const core_tx_bearer_interface_t m_interface = {packet_alloc,
                                                        packet_send,
                                                        packet_discard};
@@ -94,8 +98,31 @@ static core_tx_alloc_result_t packet_alloc(core_tx_bearer_t * p_bearer, const co
         advertiser_packet_alloc(&m_bearer_roles[p_params->role].advertiser,
                                 sizeof(ble_ad_header_t) + p_params->net_packet_len);

+    adv_bearer_role_t *bearer_role = &m_bearer_roles[p_params->role];
     if (m_current_alloc.p_packet == NULL)
     {
+        static const typeof (bearer_role->alloc_fail_count) ALLOC_FAIL_STATURATION = ~0;
+
+        if(ALLOC_FAIL_STATURATION != bearer_role->alloc_fail_count) {
+            bearer_role->alloc_fail_count++;
+        }
+
+        if (p_params->role == CORE_TX_ROLE_ORIGINATOR) {
+
+            const advertiser_t *p_adv = &bearer_role->advertiser;
+
+            /*Called via core_tx_bearer_interface_t thus log it at that level since there no module leve for these logs*/
+            __LOG(LOG_SRC_BEARER, LOG_LEVEL_ERROR, "packet_alloc: cnt:%lu, tId:0x%lx, ts:0x%x\n",
+                bearer_role->alloc_fail_count, &p_adv->timer, p_adv->timer.timestamp);
+
+            if (bearer_role->alloc_fail_count > 0 &&
+                timer_ticks_until_next_event() >= MAX_TICKS_TO_NEXT_EVENT) {
+
+                bearer_role->alloc_fail_count = 0;
+                timer_sch_recover();
+            }
+        }
+
         return CORE_TX_ALLOC_FAIL_NO_MEM;
     }
     else
@@ -104,6 +131,8 @@ static core_tx_alloc_result_t packet_alloc(core_tx_bearer_t * p_bearer, const co
         m_current_alloc.p_packet->config.repeats = m_bearer_roles[p_params->role].adv_tx_count;
         m_current_alloc.role                     = p_params->role;

+        bearer_role->alloc_fail_count = 0;
+
         return CORE_TX_ALLOC_SUCCESS;
     }
 }
diff --git a/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/src/timer.c b/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/src/timer.c
index a7d57749..9dffafbe 100644
--- a/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/src/timer.c
+++ b/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/src/timer.c
@@ -48,8 +48,8 @@
 #define IS_OVERFLOW_PENDING() (NRF_RTC1->EVENTS_OVRFLW == 1)
 /* Margin is required to prevent situation when written CC value is equal to COUNTER.
  * Situation with equality will cause losing interrupt for the tail counting until next overflow. */
-#define PROTECTION_MARGIN_FOR_TIMER_START   2ul
-#define PROTECTION_MARGIN_FOR_OVFW_HANDLER  1ul
+#define PROTECTION_MARGIN_FOR_TIMER_START   3ul
+#define PROTECTION_MARGIN_FOR_OVFW_HANDLER  2ul

 #define TIMER_US_TO_TICKS(US)                              \
             ((uint32_t)ROUNDED_DIV(                        \
@@ -81,10 +81,11 @@ void nrf_mesh_timer_tail_handle(void)
 void nrf_mesh_timer_ovfw_handle(void)
 {
     uint32_t was_masked;
+    bool expired = false;
+
     _DISABLE_IRQS(was_masked);
     NRF_RTC1->EVENTS_OVRFLW = 0;
     m_ovfw_counter++;
-    _ENABLE_IRQS(was_masked);

     if (m_ovfw_timer_counter != 0)
     {
@@ -94,20 +95,24 @@ void nrf_mesh_timer_ovfw_handle(void)
         {
             if (m_tail_timer_counter > NRF_RTC1->COUNTER)
             {
-                _DISABLE_IRQS(was_masked);
-                NRF_RTC1->CC[1] = m_tail_timer_counter > NRF_RTC1->COUNTER + PROTECTION_MARGIN_FOR_OVFW_HANDLER ?
-                        m_tail_timer_counter : NRF_RTC1->COUNTER + PROTECTION_MARGIN_FOR_TIMER_START;
-                _ENABLE_IRQS(was_masked);
+                NRF_RTC1->CC[1] = (m_tail_timer_counter > NRF_RTC1->COUNTER + PROTECTION_MARGIN_FOR_OVFW_HANDLER) ?
+                        m_tail_timer_counter : (NRF_RTC1->COUNTER + PROTECTION_MARGIN_FOR_TIMER_START);
                 NRF_RTC1->EVTENSET = RTC_EVTEN_COMPARE1_Msk;
                 NRF_RTC1->INTENSET = RTC_INTENSET_COMPARE1_Msk;
             }
             else
             {
-                NRF_MESH_ASSERT(mp_cb != NULL);
-                mp_cb(timer_now());
+                expired = true;
             }
         }
     }
+    _ENABLE_IRQS(was_masked);
+
+    if (expired)
+    {
+        NRF_MESH_ASSERT(mp_cb != NULL);
+        mp_cb(timer_now());
+    }
 }

 void timer_init(void)
@@ -126,12 +131,11 @@ timestamp_t timer_now(void)

     _DISABLE_IRQS(was_masked);
     ovfw_counter = m_ovfw_counter;
-    sample = NRF_RTC1->COUNTER;
     if (IS_OVERFLOW_PENDING())
     {
-        sample = NRF_RTC1->COUNTER;
         ovfw_counter++;
     }
+    sample = NRF_RTC1->COUNTER;
     _ENABLE_IRQS(was_masked);

     ovfw_counter <<= RTC_RESOLUTION;
@@ -153,14 +157,12 @@ void timer_start(timestamp_t timestamp, timer_callback_t cb)
     }

     uint32_t timeout_ticks = TIMER_US_TO_TICKS(time_diff);
+    timeout_ticks = MAX(PROTECTION_MARGIN_FOR_TIMER_START, timeout_ticks);

     _DISABLE_IRQS(was_masked);
-    uint32_t ticks_now = NRF_RTC1->COUNTER;
-
-    timeout_ticks = MAX(PROTECTION_MARGIN_FOR_TIMER_START, timeout_ticks);
-    if (timeout_ticks <= APP_TIMER_MAX_CNT_VAL - ticks_now)
+    if (timeout_ticks <= APP_TIMER_MAX_CNT_VAL - NRF_RTC1->COUNTER)
     {
-        NRF_RTC1->CC[1] = ticks_now + timeout_ticks;
+        NRF_RTC1->CC[1] = NRF_RTC1->COUNTER + timeout_ticks;
         NRF_RTC1->EVTENSET = RTC_EVTEN_COMPARE1_Msk;
         NRF_RTC1->INTENSET = RTC_INTENSET_COMPARE1_Msk;
         m_ovfw_timer_counter = 0;
@@ -168,7 +170,7 @@ void timer_start(timestamp_t timestamp, timer_callback_t cb)
     }
     else
     {
-        timeout_ticks -= (APP_TIMER_MAX_CNT_VAL - ticks_now);
+        timeout_ticks -= (APP_TIMER_MAX_CNT_VAL - NRF_RTC1->COUNTER);
         m_ovfw_timer_counter = (timeout_ticks >> RTC_RESOLUTION) + 1;
         m_tail_timer_counter = timeout_ticks & APP_TIMER_MAX_CNT_VAL;
     }
@@ -188,3 +190,14 @@ void timer_stop(void)
     m_ovfw_counter = 0;
 #endif
 }
+
+uint32_t timer_ticks_until_next_event(void)
+{
+    volatile uint32_t was_masked = 0;
+    _DISABLE_IRQS(was_masked);
+    const uint32_t cc = NRF_RTC1->CC[1];
+    const uint32_t ctr = NRF_RTC1->COUNTER;
+    _ENABLE_IRQS(was_masked);
+
+    return app_timer_cnt_diff_compute(cc, ctr);
+}
diff --git a/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/src/timer_scheduler.c b/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/src/timer_scheduler.c
index 3ea30ca6..f77372af 100644
--- a/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/src/timer_scheduler.c
+++ b/Dependencies/nRF5_Mesh_SDK/3.1.0/src/mesh/core/src/timer_scheduler.c
@@ -40,6 +40,7 @@
 #include "nrf_mesh_assert.h"
 #include "bearer_event.h"
 #include "toolchain.h"
+#include <log.h>

 /** Time in us to regard as immediate when firing several timers at once */
 #define TIMER_MARGIN    (100)
@@ -225,3 +226,13 @@ bool timer_sch_is_scheduled(const timer_event_t * p_timer_evt)
     return (p_timer_evt->state == TIMER_EVENT_STATE_ADDED ||
             p_timer_evt->state == TIMER_EVENT_STATE_IN_CALLBACK);
 }
+
+void timer_sch_recover(void)
+{
+    timer_event_t * head = m_scheduler.p_head;
+    if (head) {
+        timer_sch_reschedule(head, head->timestamp);
+    }
+
+    __LOG(LOG_SRC_TIMER_SCHEDULER, LOG_LEVEL_ERROR, "timer_sch_recover. tIdHead:0x%lx, ts:0x%lx\n", head, head ? head->timestamp : 0);
+}
--
2.21.1 (Apple Git-122.3)
