rewrite cpu_port.c (but still has bug)

1 files changed, 126 insertions, 26 deletions

diff --git a/libcpu/sim/posix/cpu_port.c b/libcpu/sim/posix/cpu_port.c
index 622a3bdc29..70dc3572e1 100644
--- a/libcpu/sim/posix/cpu_port.c
+++ b/libcpu/sim/posix/cpu_port.c
@@ -24,21 +24,29 @@ typedef struct _thread
     void (*exit)(void);
     sem_t sem;
     rt_thread_t rtthread;
+    int status;
     void *data;
 } thread_t;
 
 #define THREAD_T(thread)  ((thread_t *)thread)
 
 #define MSG_SUSPEND  SIGUSR1    /* 10 */
-/* #define MSG_RESUME   SIGUSR2 */
+#define MSG_RESUME   SIGUSR2
 #define MSG_TICK     SIGALRM    /* 14 */
 #define TIMER_TYPE   ITIMER_REAL
 #define MAX_INTERRUPT_NUM ((unsigned int)sizeof(unsigned int) * 8)
 
 #define INTERRUPT_ENABLE   0
 #define INTERRUPT_DISABLE  1
+
+/* 线程挂起状态，共两种取值 */
+#define SUSPEND_LOCK      0
+#define SUSPEND_SIGWAIT   1
+#define THREAD_RUNNING    2
+
 /* interrupt flag, if 1, disable, if 0, enable */
 static long interrupt_disable_flag;
+static int systick_signal_flag;
 
 /* flag in interrupt handling */
 rt_uint32_t rt_interrupt_from_thread, rt_interrupt_to_thread;
@@ -78,8 +86,9 @@ int signal_mask(void)
     sigaddset(&sigmask, SIGALRM);
     pthread_sigmask(SIG_BLOCK, &sigmask, &oldmask);
 }
-static void thread_switch_handler(int sig)
+static void thread_suspend_signal_handler(int sig)
 {
+    sigset_t sigmask;
     pthread_t pid = pthread_self();
     thread_t *thread_from;
     thread_t *thread_to;
@@ -100,10 +109,44 @@ static void thread_switch_handler(int sig)
     RT_ASSERT((thread_t *)(tid->sp) == thread_to);
 
     TRACE("signal: SIGSUSPEND suspend <%s>\n", thread_from->rtthread->name);
-    sem_wait(&thread_from->sem);
+
+    /* 使用sigwait或者sigsuspend来挂起from线程  */
+    //sem_wait(&thread_from->sem);
+    sigemptyset(&sigmask);
+    sigaddset(&sigmask, MSG_RESUME);
+
+    /* Beginnig Linux Programming上说，当信号处理函数运行中，此信号就会被屏蔽，
+     * 以防止重复执行信号处理函数
+     */
+    thread_from->status = SUSPEND_SIGWAIT;
+    if (sigwait(&sigmask, &sig) != 0)
+    {
+        printf("sigwait faild, %d\n", sig);
+    }
+    thread_to = (thread_t *) rt_interrupt_to_thread;
+    RT_ASSERT(thread_to == thread_from);
+    thread_to->status = THREAD_RUNNING;
     TRACE("signal: SIGSUSPEND resume  <%s>\n", thread_from->rtthread->name);
 }
 
+static void thread_resume_signal_handler(int sig)
+{
+    sigset_t sigmask;
+    pthread_t pid = pthread_self();
+    thread_t *thread_from;
+    thread_t *thread_to;
+    rt_thread_t tid;
+
+    thread_from = (thread_t *) rt_interrupt_from_thread;
+    thread_to = (thread_t *) rt_interrupt_to_thread;
+
+    /* 注意！此时 rt_thread_self的值是to线程的值！ */
+    tid = rt_thread_self();
+    RT_ASSERT((thread_t *)(tid->sp) == thread_to);
+
+    TRACE("signal: SIGRESUME resume  <%s>\n", thread_to->rtthread->name);
+}
+
 static void *thread_run(void *parameter)
 {
     rt_thread_t tid;
@@ -114,6 +157,7 @@ static void *thread_run(void *parameter)
     /* FIXME set signal mask, mask the timer! */
     signal_mask();
 
+    thread->status = SUSPEND_SIGWAIT;
     TRACE("pid <%08x> stop on sem...\n", (unsigned int)(thread->pthread));
     sem_wait(&thread->sem);
 
@@ -234,7 +278,8 @@ void rt_hw_interrupt_enable(rt_base_t level)
     tid = rt_thread_self();
     pid = pthread_self();
 
-    if (pid != mainthread_pid && thread_from->pthread == pid)
+    //pid != mainthread_pid &&
+    if (thread_from->pthread == pid)
     {
         /* 注意这段代码是在RTT普通线程函数总函数中执行的，
          * from线程就是当前rtt线程 */
@@ -245,19 +290,36 @@ void rt_hw_interrupt_enable(rt_base_t level)
               thread_to->rtthread->name);
 
         /* 唤醒被挂起的线程 */
-        sem_post(& thread_to ->sem);
+        if (thread_to->status == SUSPEND_SIGWAIT)
+        {
+            pthread_kill(thread_to->pthread, MSG_RESUME);
+        }
+        else if (thread_to->status == SUSPEND_LOCK)
+        {
+            sem_post(& thread_to->sem);
+        }
+        else
+        {
+            printf("conswitch: should not be here! %d\n", __LINE__);
+            exit(EXIT_FAILURE);
+        }
         cpu_pending_interrupts --;
-
+        thread_from->status = SUSPEND_LOCK;
         pthread_mutex_unlock(ptr_int_mutex);
 
         /* 挂起当前的线程 */
         sem_wait(& thread_from->sem);
+        //TRACE("rttask:%s suspend!\n", thread_from->rtthread->name);
+        pthread_mutex_lock(ptr_int_mutex);
+        thread_from->status = THREAD_RUNNING;
+        pthread_mutex_unlock(ptr_int_mutex);
+        //TRACE("rttask:%s resume!\n", thread_to->rtthread->name);
     }
     else
     {
         /* 注意这段代码可能在多种情况下运行：
-		 * 1. 在system tick中执行， 即主线程的SIGALRM信号处理函数中执行
-		 * 2. 其他线程中调用，比如用于获取按键输入的线程中调用
+         * 1. 在system tick中执行， 即主线程的SIGALRM信号处理函数中执行
+         * 2. 其他线程中调用，比如用于获取按键输入的线程中调用
          */
         TRACE("conswitch: S in pid<%x>  ,suspend <%s>, resume <%s>!\n",
               (unsigned int)pid,
@@ -267,11 +329,22 @@ void rt_hw_interrupt_enable(rt_base_t level)
         pthread_kill(thread_from->pthread, MSG_SUSPEND);
         cpu_pending_interrupts --;
 
-        pthread_mutex_unlock(ptr_int_mutex);
-
         /* 唤醒to线程 */
-        sem_post(& thread_to->sem);
+        if (thread_to->status == SUSPEND_SIGWAIT)
+        {
+            pthread_kill(thread_to->pthread, MSG_RESUME);
+        }
+        else if (thread_to->status == SUSPEND_LOCK)
+        {
+            sem_post(& thread_to->sem);
+        }
+        else
+        {
+            printf("conswitch: should not be here! %d\n", __LINE__);
+            exit(EXIT_FAILURE);
+        }
 
+        pthread_mutex_unlock(ptr_int_mutex);
     }
     /*TODO: It may need to unmask the signal */
 }
@@ -302,13 +375,13 @@ void rt_hw_context_switch(rt_uint32_t from,
         rt_interrupt_from_thread = *((rt_uint32_t *)from);
     }
 #endif
+    pthread_mutex_lock(ptr_int_mutex);
     rt_interrupt_from_thread = *((rt_uint32_t *)from);
     rt_interrupt_to_thread = *((rt_uint32_t *)to);
 
     /* 这个函数只是并不会真正执行中断处理函数，而只是简单的
      * 设置一下中断挂起标志位
      */
-    pthread_mutex_lock(ptr_int_mutex);
     cpu_pending_interrupts ++;
     pthread_mutex_unlock(ptr_int_mutex);
 }
@@ -337,20 +410,31 @@ void rt_hw_context_switch_to(rt_uint32_t to)
 
 static int mainthread_scheduler(void)
 {
-    int i, res;
+    int i, res, sig;
     thread_t *thread_from;
     thread_t *thread_to;
     pthread_mutex_t mutex;
     pthread_mutexattr_t mutexattr;
+    sigset_t  sigmask, oldmask;
 
     /* save the main thread id */
     mainthread_pid = pthread_self();
     TRACE("pid <%08x> mainthread\n", (unsigned int)(mainthread_pid));
 
+    /* 屏蔽suspend信号和resume信号 */
+    sigemptyset(&sigmask);
+    sigaddset(&sigmask, MSG_SUSPEND);
+    sigaddset(&sigmask, MSG_RESUME);
+    pthread_sigmask(SIG_BLOCK, &sigmask, &oldmask);
+
+    sigemptyset(&sigmask);
+    sigaddset(&sigmask, SIGALRM);
+
     /* install signal handler of system tick */
     signal_install(SIGALRM, mthread_signal_tick);
-    /* install signal handler used to suspend itself */
-    signal_install(MSG_SUSPEND, thread_switch_handler);
+    /* install signal handler used to suspend/resume threads */
+    signal_install(MSG_SUSPEND, thread_suspend_signal_handler);
+    signal_install(MSG_RESUME, thread_resume_signal_handler);
 
     /* create a mutex and condition val, used to indicate interrupts occrue */
     ptr_int_mutex = &mutex;
@@ -365,10 +449,34 @@ static int mainthread_scheduler(void)
     /* trigger_interrupt(CPU_INTERRUPT_YIELD); */
     thread_to = (thread_t *) rt_interrupt_to_thread;
     thread_resume(thread_to);
-
     for (;;)
     {
-        sleep(1);
+#if 1
+        if (sigwait(&sigmask, &sig) != 0)
+        {
+            printf("mthread: sigwait get unexpected sig %d\n", sig);
+        }
+#else
+        pause();
+#endif
+        TRACE("mthread:got sig %d\n", sig);
+        /* signal mask sigalrm  屏蔽SIGALRM信号 */
+        pthread_sigmask(SIG_BLOCK, &sigmask, &oldmask);
+
+//		if (systick_signal_flag != 0)
+        if (pthread_mutex_trylock(ptr_int_mutex) == 0)
+        {
+            tick_interrupt_isr();
+            systick_signal_flag = 0;
+            pthread_mutex_unlock(ptr_int_mutex);
+        }
+        else
+        {
+            TRACE("try lock failed.\n");
+        }
+
+        /* 开启SIGALRM信号 */
+        pthread_sigmask(SIG_UNBLOCK, &sigmask, &oldmask);
     }
 
     return 0;
@@ -418,15 +526,7 @@ static void mthread_signal_tick(int sig)
     if (sig == SIGALRM)
     {
         TRACE("pid <%x> signal: SIGALRM enter!\n", (unsigned int)pid);
-
-        if (pthread_mutex_trylock(ptr_int_mutex) != 0)
-        {
-            TRACE("try lock failed.\n");
-            return;
-        }
-        tick_interrupt_isr();
-        pthread_mutex_unlock(ptr_int_mutex);
-
+        //systick_signal_flag = 1;
         TRACE("pid <%x> signal: SIGALRM leave!\n", (unsigned int)pid);
     }
     else