From b0457ee0ac02ddc79949da8ddf1b6f910ba56c8d Mon Sep 17 00:00:00 2001
From: Konstantin Kondrashov <konstantin@espressif.com>
Date: Tue, 21 May 2024 23:26:49 +0300
Subject: [PATCH] feat(newlib): Adds flockfile/funlockfile for safe multi-line
 printing

for printf, vprintf, etc.

Closes https://github.com/espressif/esp-idf/issues/2565
---
 components/newlib/CMakeLists.txt              |  1 +
 components/newlib/flockfile.c                 | 32 ++++++++++
 .../test_apps/newlib/main/CMakeLists.txt      |  1 +
 .../test_apps/newlib/main/test_printf.c       | 60 +++++++++++++++++++
 4 files changed, 94 insertions(+)
 create mode 100644 components/newlib/flockfile.c
 create mode 100644 components/newlib/test_apps/newlib/main/test_printf.c

diff --git a/components/newlib/CMakeLists.txt b/components/newlib/CMakeLists.txt
index eb62582e9b4..21cb9eb2acd 100644
--- a/components/newlib/CMakeLists.txt
+++ b/components/newlib/CMakeLists.txt
@@ -19,6 +19,7 @@ set(srcs
     "abort.c"
     "assert.c"
     "heap.c"
+    "flockfile.c"
     "locks.c"
     "poll.c"
     "pthread.c"
diff --git a/components/newlib/flockfile.c b/components/newlib/flockfile.c
new file mode 100644
index 00000000000..1820d58a443
--- /dev/null
+++ b/components/newlib/flockfile.c
@@ -0,0 +1,32 @@
+/*
+ * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <stdio.h>
+
+#include <sys/lock.h>
+#include <sys/reent.h>
+
+void flockfile(FILE *fp)
+{
+    if (fp && !(fp->_flags & __SSTR)) {
+        __lock_acquire_recursive(fp->_lock);
+    }
+}
+
+int ftrylockfile(FILE *fp)
+{
+    if (fp && !(fp->_flags & __SSTR)) {
+        return __lock_try_acquire_recursive(fp->_lock);
+    }
+    return 0;
+}
+
+void funlockfile(FILE *fp)
+{
+    if (fp && !(fp->_flags & __SSTR)) {
+        __lock_release_recursive(fp->_lock);
+    }
+}
diff --git a/components/newlib/test_apps/newlib/main/CMakeLists.txt b/components/newlib/test_apps/newlib/main/CMakeLists.txt
index afd52fc0473..34ce489cd0b 100644
--- a/components/newlib/test_apps/newlib/main/CMakeLists.txt
+++ b/components/newlib/test_apps/newlib/main/CMakeLists.txt
@@ -5,6 +5,7 @@ idf_component_register(SRCS
                        "test_locks.c"
                        "test_misc.c"
                        "test_newlib.c"
+                       "test_printf.c"
                        "test_setjmp.c"
                        "test_stdatomic.c"
                        "test_time.c"
diff --git a/components/newlib/test_apps/newlib/main/test_printf.c b/components/newlib/test_apps/newlib/main/test_printf.c
new file mode 100644
index 00000000000..447d554999a
--- /dev/null
+++ b/components/newlib/test_apps/newlib/main/test_printf.c
@@ -0,0 +1,60 @@
+/*
+ * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#include <stdio.h>
+#include <string.h>
+#include <stdarg.h>
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "freertos/semphr.h"
+#include "unity.h"
+#include "esp_log.h"
+#include "sdkconfig.h"
+
+#if !CONFIG_FREERTOS_UNICORE
+static int my_printf(const char *fmt, ...)
+{
+    va_list list;
+    va_start(list, fmt);
+    int result = vprintf(fmt, list);
+    va_end(list);
+    return result;
+}
+
+static void test_task(void* arg)
+{
+    for (unsigned i = 0; i < 5; i++) {
+        printf("00000000%d\n", i);
+    }
+    SemaphoreHandle_t * p_done = (SemaphoreHandle_t *) arg;
+    xSemaphoreGive(*p_done);
+    vTaskDelay(1);
+    vTaskDelete(NULL);
+}
+
+TEST_CASE("Test flockfile/funlockfile", "[newlib]")
+{
+    SemaphoreHandle_t done = xSemaphoreCreateBinary();
+    flockfile(stdout);
+    {
+        xTaskCreatePinnedToCore(&test_task, "test_task", 4096, &done, 5, NULL, 1);
+        // make sure test_task is already running and is actually blocked by flockfile
+        TEST_ASSERT_FALSE(xSemaphoreTake(done, 100 / portTICK_PERIOD_MS));
+        for (unsigned i = 0; i < 5; i++) {
+            my_printf(LOG_ANSI_COLOR_BOLD_BACKGROUND(LOG_ANSI_COLOR_BLACK, LOG_ANSI_COLOR_BG_RED) "I ");
+            my_printf("(%d) ", 737);
+            my_printf("[%s]: ", "TAG");
+            my_printf("%s %d ", "message ", i);
+            my_printf("%s\n", LOG_ANSI_COLOR_RESET);
+        }
+        // test_task is still blocked by flockfile
+        TEST_ASSERT_FALSE(xSemaphoreTake(done, 100 / portTICK_PERIOD_MS));
+    }
+    funlockfile(stdout);
+    // after funlockfile, test_task can print a msg and complete test_task
+    TEST_ASSERT_TRUE(xSemaphoreTake(done, portMAX_DELAY));
+    vSemaphoreDelete(done);
+}
+#endif // !CONFIG_FREERTOS_UNICORE