Adds a library with pair of functions to put load on CPU.

[can-benchmark.git] / rtems / gw / libs / load.c

diff --git a/rtems/gw/libs/load.c b/rtems/gw/libs/load.c
new file mode 100644 (file)
index 0000000..ba7fac4
--- /dev/null
+++ b/rtems/gw/libs/load.c
@@ -0,0 +1,116 @@
+/*\r
+* Implements simple producer/consumer thread pair to cause load on the CPU.\r
+* \r
+* Used in benchmarking the CAN gateway.\r
+*\r
+* Co-opted from http://support.dce.felk.cvut.cz/pos/cv3/src/semaphore.html. \r
+*/\r
+\r
+#include <stdio.h>\r
+\r
+#include <pthread.h>\r
+#include <semaphore.h>\r
+\r
+#include "load.h"\r
+\r
+\r
+static void* produce(void* arg);\r
+static void* consume(void* arg);\r
+\r
+/* semaphores for consumer/producer pair */\r
+static sem_t produced, consumed;\r
+/* pthread handles for the loader */\r
+static pthread_t consumer, producer;\r
+static int n = 0;\r
+static char running = 0;\r
+\r
+static void* produce(void* arg){\r
+    while (1) {\r
+            sem_wait(&consumed);\r
+            n++;  /* increment n by 1 */\r
+            sem_post(&produced);\r
+            pthread_testcancel();\r
+    }\r
+    return NULL;\r
+}       \r
+\r
+static void* consume(void* arg){\r
+    while (1) {\r
+            sem_wait(&produced);\r
+            n--; /* aaand decrement */\r
+            sem_post(&consumed);\r
+            pthread_testcancel();\r
+    }\r
+    return NULL;\r
+}\r
+\r
+int start_thread_load(){\r
+    if (running == 0){\r
+        printf("Attempting to start load.\n");\r
+        int res;\r
+        running = 1;\r
+        res = sem_init(&consumed, 0, 0);\r
+        if (res < 0){\r
+            printf("Couldn't initialize consumed semaphore.\n");\r
+            return 1;\r
+        }\r
+        res = sem_init(&produced, 0, 1);\r
+        if (res < 0){\r
+            printf("Couldn't initialize produced semaphore.\n");\r
+            return 1;\r
+        }\r
+        \r
+        res = pthread_create(&producer, NULL, produce, NULL);\r
+        if (res < 0){\r
+            printf("Couldn't create producer thread.\n");\r
+            return 1;\r
+        }\r
+        \r
+        res = pthread_create(&consumer, NULL, consume, NULL);\r
+        if (res < 0){\r
+            printf("Couldn't create consumer thread.\n");\r
+            return 1;\r
+        }\r
+    \r
+        pthread_detach(producer);\r
+        pthread_detach(consumer);\r
+        printf("Load started succesfully.\n");\r
+        return 0;\r
+    } else {\r
+        printf("Load is already running.\n");\r
+        return 0;\r
+    }\r
+}\r
+\r
+/*\r
+* This function stops threads loading the CPU and destroys associated semaphores. \r
+* \r
+* No error handling currently, only tries to report errors.\r
+* \r
+*/\r
+int end_thread_load(){\r
+    if (running == 1){\r
+        int res;\r
+        printf("Attempting to cancel producer thread.\n");\r
+        res = pthread_cancel(producer);\r
+        if (res != 0){\r
+            printf("Failed.\n");\r
+            /* Not sure what to do with error here, will have to figure out later. */\r
+        }\r
+        printf("Attempting to cancel consumer thread.\n");\r
+        res = pthread_cancel(consumer);\r
+        if (res != 0){\r
+            printf("Failed.\n");\r
+        }\r
+        printf("Preparing to destroy semaphores.\n");\r
+        sleep(1);\r
+        sem_destroy(&produced);\r
+        sem_destroy(&consumed);\r
+        running = 0;\r
+        printf("Finished.\n");\r
+        return 0;\r
+    } else {\r
+        printf("Load is not running.\n");\r
+        return 0;\r
+    }\r
+}
\ No newline at end of file