Prevent DeepSleepLock from leaving sleep locked

Add _lock_count to DeepSleepLock and use this to prevent deep sleep
from staying locked when the DeepSleepLock objected is destroyed after
an unbalanced number of calls to lock and unlock.

platform/DeepSleepLock.h

@@ -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 @@ namespace mbed {
   * @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)");
+        }
     }
 };