From 53baf73a54381cd4131a353ee2b5d1b0ecb52efb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Carlos=20Guimar=C3=A3es?= <carlos.em.guimaraes@gmail.com>
Date: Wed, 1 Feb 2023 10:36:09 +0100
Subject: [PATCH] Improve sleep resilience considering only their granularity

---
 src/system/arduino/esp32/system.c  | 8 +++++++-
 src/system/arduino/opencr/system.c | 8 +++++++-
 src/system/emscripten/system.c     | 8 +++++++-
 src/system/espidf/system.c         | 8 +++++++-
 src/system/mbed/system.cpp         | 3 ++-
 src/system/unix/system.c           | 8 +++++++-
 src/system/zephyr/system.c         | 8 +++++++-
 7 files changed, 44 insertions(+), 7 deletions(-)

diff --git a/src/system/arduino/esp32/system.c b/src/system/arduino/esp32/system.c
index 7d68ccaf8..a632600a7 100644
--- a/src/system/arduino/esp32/system.c
+++ b/src/system/arduino/esp32/system.c
@@ -117,7 +117,13 @@ int8_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { return pthread_cond_wa
 /*------------------ Sleep ------------------*/
 int z_sleep_us(unsigned int time) { return usleep(time); }
 
-int z_sleep_ms(unsigned int time) { return usleep(time * 1000U); }
+int z_sleep_ms(unsigned int time) {
+    for (unsigned int i = 0; i < time; i++) {
+        z_sleep_us(1000);
+    }
+
+    return 0;
+}
 
 int z_sleep_s(unsigned int time) { return sleep(time); }
 
diff --git a/src/system/arduino/opencr/system.c b/src/system/arduino/opencr/system.c
index 953dd0d1a..607a3e450 100644
--- a/src/system/arduino/opencr/system.c
+++ b/src/system/arduino/opencr/system.c
@@ -107,7 +107,13 @@ int z_sleep_ms(unsigned int time) {
     return 0;
 }
 
-int z_sleep_s(unsigned int time) { return z_sleep_ms(time * 1000U); }
+int z_sleep_s(unsigned int time)  {
+    for (unsigned int i = 0; i < time; i++) {
+        z_sleep_ms(1000);
+    }
+
+    return 0;
+}
 
 /*------------------ Instant ------------------*/
 void __z_clock_gettime(z_clock_t *ts) {
diff --git a/src/system/emscripten/system.c b/src/system/emscripten/system.c
index 85a78384f..0f8c45f6e 100644
--- a/src/system/emscripten/system.c
+++ b/src/system/emscripten/system.c
@@ -89,7 +89,13 @@ int z_sleep_ms(unsigned int time) {
     return 0;
 }
 
-int z_sleep_s(unsigned int time) { return z_sleep_ms(time * 1000); }
+int z_sleep_s(unsigned int time)  {
+    for (unsigned int i = 0; i < time; i++) {
+        z_sleep_ms(1000);
+    }
+
+    return 0;
+}
 
 /*------------------ Instant ------------------*/
 z_clock_t z_clock_now(void) { return z_time_now(); }
diff --git a/src/system/espidf/system.c b/src/system/espidf/system.c
index d7fe81f01..44b314560 100644
--- a/src/system/espidf/system.c
+++ b/src/system/espidf/system.c
@@ -118,7 +118,13 @@ int8_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { return pthread_cond_wa
 /*------------------ Sleep ------------------*/
 int z_sleep_us(unsigned int time) { return usleep(time); }
 
-int z_sleep_ms(unsigned int time) { return usleep(time * 1000U); }
+int z_sleep_ms(unsigned int time) {
+    for (unsigned int i = 0; i < time; i++) {
+        z_sleep_us(1000);
+    }
+
+    return 0;
+}
 
 int z_sleep_s(unsigned int time) { return sleep(time); }
 
diff --git a/src/system/mbed/system.cpp b/src/system/mbed/system.cpp
index 4fcdf8c50..eacf6ed35 100644
--- a/src/system/mbed/system.cpp
+++ b/src/system/mbed/system.cpp
@@ -110,7 +110,8 @@ int8_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) {
 
 /*------------------ Sleep ------------------*/
 int z_sleep_us(unsigned int time) {
-    return -1;  // Not supported
+    ThisThread::sleep_for(chrono::milliseconds(((time / 1000) + (time % 1000 == 0 ? 0 : 1))));
+    return 0;
 }
 
 int z_sleep_ms(unsigned int time) {
diff --git a/src/system/unix/system.c b/src/system/unix/system.c
index 2eea8a5a1..83380b81b 100644
--- a/src/system/unix/system.c
+++ b/src/system/unix/system.c
@@ -137,7 +137,13 @@ int8_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { return pthread_cond_wa
 /*------------------ Sleep ------------------*/
 int z_sleep_us(unsigned int time) { return usleep(time); }
 
-int z_sleep_ms(unsigned int time) { return z_sleep_us(time * 1000U); }
+int z_sleep_ms(unsigned int time) {
+    for (unsigned int i = 0; i < time; i++) {
+        z_sleep_us(1000);
+    }
+
+    return 0;
+}
 
 int z_sleep_s(unsigned int time) { return sleep(time); }
 
diff --git a/src/system/zephyr/system.c b/src/system/zephyr/system.c
index 5b124e549..4c2bdc0f7 100644
--- a/src/system/zephyr/system.c
+++ b/src/system/zephyr/system.c
@@ -103,7 +103,13 @@ int8_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { return pthread_cond_wa
 /*------------------ Sleep ------------------*/
 int z_sleep_us(unsigned int time) { return usleep(time); }
 
-int z_sleep_ms(unsigned int time) { return usleep(time * 1000U); }
+int z_sleep_ms(unsigned int time) {
+    for (unsigned int i = 0; i < time; i++) {
+        z_sleep_us(1000);
+    }
+
+    return 0;
+}
 
 int z_sleep_s(unsigned int time) { return sleep(time); }