diff --git a/mcs/CMakeLists.txt b/mcs/CMakeLists.txt
index bce56863132822dd926a0471a928b1f228f47fd4..3f80ebb92eca0fc28436372253e322fb8429399d 100644
--- a/mcs/CMakeLists.txt
+++ b/mcs/CMakeLists.txt
@@ -29,6 +29,7 @@ set(SRC_FILE
 )
 
 if(DEMO_TARGET MATCHES "openamp_demo")
+elseif(DEMO_TARGET MATCHES "latency_demo")
 elseif(DEMO_TARGET MATCHES "rpmsg_pty_demo")
 	list(APPEND SRC_FILE "./${DEMO_TARGET}/rpmsg_pty.c")
 else()
diff --git a/mcs/latency_demo/rpmsg_main.c b/mcs/latency_demo/rpmsg_main.c
new file mode 100644
index 0000000000000000000000000000000000000000..44c3ad81a134a1324fc9d06cc705c8abe0eee6d4
--- /dev/null
+++ b/mcs/latency_demo/rpmsg_main.c
@@ -0,0 +1,86 @@
+#include <stdio.h>
+#include <stdarg.h>
+#include "openamp_module.h"
+
+char *cpu_id;
+char *target_binfile;
+char *target_binaddr;
+
+static void cleanup(int sig)
+{
+    openamp_deinit();
+}
+
+int rpmsg_app_master(void)
+{
+    int ret;
+    char buf[2048] = {0};
+    int len;
+    int i;
+
+    printf("start latency measure demo...\n");
+
+    /* linux send messsage to clientos first */
+    ret = send_message("latency_demo\n", sizeof("latency_demo\n"));
+    if (ret < 0) {
+        printf("send_message failed with status %d\n", ret);
+        return ret;
+    }
+
+    while (1) {
+        ret = receive_message(buf, sizeof(buf), &len);
+        if (ret < 0) {
+            printf("receive_message failed with status %d\n", ret);
+            return ret;
+        }
+
+        /* show message */
+        for (i = 0; i < len; i++)
+            printf("%c", buf[i]);
+    }
+
+    return 0;
+}
+
+int main(int argc, char **argv)
+{
+    int ret;
+    int opt;
+
+    /* ctrl+c signal, do cleanup before program exit */
+    signal(SIGINT, cleanup);
+
+    while ((opt = getopt(argc, argv, "c:t:a:")) != -1) {
+        switch (opt) {
+        case 'c':
+            cpu_id = optarg;
+            break;
+        case 't':
+            target_binfile = optarg;
+            break;
+        case 'a':
+            target_binaddr = optarg;
+            break;
+        default:
+            break;
+        }
+    }
+
+    ret = openamp_init();
+    if (ret) {
+        printf("openamp init failed: %d\n", ret);
+        openamp_deinit();
+        return ret;
+    }
+
+    ret = rpmsg_app_master();
+    if (ret) {
+        printf("rpmsg app master failed: %d\n", ret);
+        openamp_deinit();
+        return ret;
+    }
+
+    openamp_deinit();
+
+    return 0;
+}
diff --git a/mcs/latency_demo/zephyr_rpi.bin b/mcs/latency_demo/zephyr_rpi.bin
new file mode 100644
index 0000000000000000000000000000000000000000..d665b60013137677d86358ebb1c8abfa1babbbf4
Binary files /dev/null and b/mcs/latency_demo/zephyr_rpi.bin differ
diff --git a/mcs/openamp_demo/rpmsg_main.c b/mcs/openamp_demo/rpmsg_main.c
index 443894046e1ae4156152d0da2a88de0ec710caed..2087650ddf825ae33b98d80541bfe38becd03bf7 100644
--- a/mcs/openamp_demo/rpmsg_main.c
+++ b/mcs/openamp_demo/rpmsg_main.c
@@ -6,6 +6,11 @@ char *cpu_id;
 char *target_binfile;
 char *target_binaddr;
 
+static void cleanup(int sig)
+{
+    openamp_deinit();
+}
+
 int rpmsg_app_master(void)
 {
     int ret;
@@ -40,6 +45,9 @@ int main(int argc, char **argv)
     int ret;
     int opt;
 
+    /* ctrl+c signal, do cleanup before program exit */
+    signal(SIGINT, cleanup);
+
     while ((opt = getopt(argc, argv, "c:t:a:")) != -1) {
         switch (opt) {
         case 'c':
diff --git a/mcs/openamp_demo/zephyr_rpi.bin b/mcs/openamp_demo/zephyr_rpi.bin
index 5240ed73c05a25fdb5c07a30c220c7ac3748e8f6..68f8fda8483ece2f6fcd8c8846740fd745679858 100644
Binary files a/mcs/openamp_demo/zephyr_rpi.bin and b/mcs/openamp_demo/zephyr_rpi.bin differ
diff --git a/mcs/rpmsg_pty_demo/rpmsg_main.c b/mcs/rpmsg_pty_demo/rpmsg_main.c
index 0e63e859f57ffc1a93869eb68004829fdd5ebfc7..abd1a7c160349410120cfd733caf37fca0c9b9ff 100644
--- a/mcs/rpmsg_pty_demo/rpmsg_main.c
+++ b/mcs/rpmsg_pty_demo/rpmsg_main.c
@@ -8,12 +8,19 @@ char *cpu_id;
 char *target_binfile;
 char *target_binaddr;
 
+static void cleanup(int sig)
+{
+    openamp_deinit();
+    pthread_mutex_destroy(&mutex);
+}
+
 int rpmsg_app_master(void)
 {
     pthread_t tida, tidb, tidc;
 
     printf("Multi-thread processing user requests...\n");
 
+    /* init mutex as thread lock */
     pthread_mutex_init(&mutex, NULL);
 
     /* userA, zephyr shell, open with screen */
@@ -40,6 +47,9 @@ int main(int argc, char **argv)
     int ret;
     int opt;
 
+    /* ctrl+c signal, do cleanup before program exit */
+    signal(SIGINT, cleanup);
+
     while ((opt = getopt(argc, argv, "c:t:a:")) != -1) {
         switch (opt) {
         case 'c':
diff --git a/mcs/rpmsg_pty_demo/rpmsg_pty.c b/mcs/rpmsg_pty_demo/rpmsg_pty.c
index c7a21c58bdfda9e44bf2136f3be7515fff32c33d..2d06c762cff51534150171e15e2a2abe7a133bd8 100644
--- a/mcs/rpmsg_pty_demo/rpmsg_pty.c
+++ b/mcs/rpmsg_pty_demo/rpmsg_pty.c
@@ -11,6 +11,7 @@
 /* define the keys according to your terminfo */
 #define KEY_CTRL_D      4
 #define FILE_MODE       0644
+#define PTSNAME_LEN     20
 
 pthread_mutex_t mutex;
 
@@ -18,6 +19,7 @@ void open_pty(int *pfdm, int *pfds, const char *pty_name)
 {
     int ret;
     int fdm, fds;
+    char pts_name[PTSNAME_LEN] = {0};
 
     /* Open the master side of the PTY */
     fdm = posix_openpt(O_RDWR | O_NOCTTY);
@@ -33,8 +35,15 @@ void open_pty(int *pfdm, int *pfds, const char *pty_name)
         printf("Error %d on unlockpt()\n", errno);
 
     /* Open the slave side of the PTY */
-    fds = open(ptsname(fdm), O_RDWR | O_NOCTTY);
-    printf("open a new terminal, zephyr %s: screen %s\n", pty_name, ptsname(fdm));
+    ret = ptsname_r(fdm, pts_name, sizeof(pts_name));
+    if (ret != 0)
+        printf("Error %d on ptsname_r()\n", errno);
+
+    fds = open(pts_name, O_RDWR | O_NOCTTY);
+    if (ret != 0)
+        printf("Error %d on open()\n", errno);
+
+    printf("open a new terminal, zephyr %s: screen %s\n", pty_name, pts_name);
 
     *pfdm = fdm;
     *pfds = fds;
diff --git a/mcs/rpmsg_pty_demo/zephyr_qemu.bin b/mcs/rpmsg_pty_demo/zephyr_qemu.bin
index 3d3f85216423e18b262099e489f35b5fc8ea9969..6d566c0d5ca6202e6b8710761ae16aea691b8fca 100644
Binary files a/mcs/rpmsg_pty_demo/zephyr_qemu.bin and b/mcs/rpmsg_pty_demo/zephyr_qemu.bin differ
diff --git a/mcs/rpmsg_pty_demo/zephyr_rpi.bin b/mcs/rpmsg_pty_demo/zephyr_rpi.bin
index 981f9b4fe71377a2c6d642b181fd40da449a40b7..af94a220f6e547f4b27b068c3436856c4c19f03b 100644
Binary files a/mcs/rpmsg_pty_demo/zephyr_rpi.bin and b/mcs/rpmsg_pty_demo/zephyr_rpi.bin differ