Diff: platform/DeepSleepLock.h

Revision:

176:af195413fb11

Parent:

175:b96e65c34a4d

--- a/platform/DeepSleepLock.h	Mon Oct 02 15:33:19 2017 +0100
+++ b/platform/DeepSleepLock.h	Wed Oct 11 12:45:49 2017 +0100
@@ -16,7 +16,9 @@
 #ifndef MBED_DEEPSLEEPLOCK_H
 #define MBED_DEEPSLEEPLOCK_H
 
+#include <limits.h>
 #include "platform/mbed_sleep.h"
+#include "platform/mbed_critical.h"
 
 namespace mbed {
 
@@ -36,29 +38,47 @@
   * @endcode
   */
 class DeepSleepLock {
+private:
+    uint16_t _lock_count;
+
 public:
-    DeepSleepLock()
+    DeepSleepLock(): _lock_count(1)
     {
         sleep_manager_lock_deep_sleep();
     }
 
     ~DeepSleepLock()
     {
-        sleep_manager_unlock_deep_sleep();
+        if (_lock_count) {
+            sleep_manager_unlock_deep_sleep();
+        }
     }
 
     /** Mark the start of a locked deep sleep section
      */
     void lock()
     {
-        sleep_manager_lock_deep_sleep();
+        uint16_t count = core_util_atomic_incr_u16(&_lock_count, 1);
+        if (1 == count) {
+            sleep_manager_lock_deep_sleep();
+        }
+        if (0 == count) {
+            error("DeepSleepLock overflow (> USHRT_MAX)");
+        }
     }
 
     /** Mark the end of a locked deep sleep section
      */
     void unlock()
     {
-        sleep_manager_unlock_deep_sleep();
+        uint16_t count = core_util_atomic_decr_u16(&_lock_count, 1);
+        if (count == 0) {
+            sleep_manager_unlock_deep_sleep();
+        }
+        if (count == USHRT_MAX) {
+            core_util_critical_section_exit();
+            error("DeepSleepLock underflow (< 0)");
+        }
     }
 };