Split find/insert sleep tracker function into two seperate functions.

Throw an error when trying to unlock driver that was not previously locked.

hal/mbed_sleep_manager.c

@@ -75,9 +75,15 @@ static size_t sleep_tracker_find_index(const char *const filename)
         }
     }
 
-    // If no driver was found currently in the structure, find the first array
+    return -1;
-    // index that hasn't been used by a driver and fill it, then return the
+}
-    // index.
+
+static size_t sleep_tracker_add(const char* const filename)
+{
+    char temp[IDENTIFIER_WIDTH];
+    strncpy(temp, filename, IDENTIFIER_WIDTH);
+    temp[IDENTIFIER_WIDTH - 1] = '\0';
+
     for (int i = 0; i < STATISTIC_COUNT; ++i) {
         if (sleep_stats[i].identifier[0] == '\0') {
             core_util_critical_section_enter();
@@ -113,8 +119,14 @@ static void sleep_tracker_print_stats(void)
 void sleep_tracker_lock(const char* const filename, int line)
 {
     const char* const stripped_path = strip_path(filename);
+
     size_t index = sleep_tracker_find_index(stripped_path);
 
+    // Entry for this driver does not exist, create one.
+    if (index == -1) {
+        index = sleep_tracker_add(filename);
+    }
+
     core_util_atomic_incr_u8(&sleep_stats[index].count, 1);
 
     printf("LOCK: %s, ln: %i, lock count: %u\r\n", stripped_path, line, deep_sleep_lock);
@@ -125,6 +137,11 @@ void sleep_tracker_unlock(const char* const filename, int line)
     const char* const stripped_path = strip_path(filename);
     size_t index = sleep_tracker_find_index(stripped_path);
 
+    // Entry for this driver does not exist, something went wrong.
+    if (index == -1) {
+        error("Unlocking sleep for driver that was not previously locked.");
+    }
+
     core_util_atomic_decr_u8(&sleep_stats[index].count, 1);
 
     printf("UNLOCK: %s, ln: %i, lock count: %u\r\n", stripped_path, line, deep_sleep_lock);