emulator/clock.c

   1 #include <stdlib.h>
   2 #include <time.h>
   3 #include <pthread.h>
   4 #include <limits.h>
   5 #include <errno.h>
   6
   7 #include "tools.h"
   8 #include "clock.h"
   9
  10 static int exit_loop;
  11 static pthread_t cpu_thread_id;
  12
  13 struct clock_handler {
  14     struct slist l;
  15     clock_func_t *handler;
  16 };
  17
  18 static struct clock_handler *handler_list;
  19
  20 #define NANOMAX (1000000000 - 1)
  21
  22 static void* cpu_clock_loop(void* arg) {
  23     struct clock_handler *ch;
  24     struct timespec begin;
  25     struct timespec end;
  26
  27     dprint("cpu clock started.\n");
  28
  29     while (!exit_loop) {
  30         long sec;
  31         long nsec;
  32
  33         clock_gettime(CLOCK_REALTIME, &begin);
  34         //dprint("-----------------\nclock ");
  35         ch = handler_list;
  36         while (ch != NULL) {
  37             if (!ch->handler())
  38                 return NULL;
  39             ch = (struct clock_handler*)ch->l.next;
  40         }
  41
  42         clock_gettime(CLOCK_REALTIME, &end);
  43         //calcurate sleep time.
  44         if (end.tv_sec < begin.tv_sec )
  45             sec = LONG_MAX - begin.tv_sec + end.tv_sec + 1;
  46         else
  47             sec = end.tv_sec - begin.tv_sec;
  48
  49         if (end.tv_nsec < begin.tv_nsec)
  50             nsec = NANOMAX - begin.tv_nsec + end.tv_nsec + 1;
  51         else
  52             nsec = end.tv_nsec - begin.tv_nsec;
  53
  54         //dprint("sec:%d, nsec:%d\n", sec, nsec);
  55         if (sec < CPU_CLOCK_SEC || nsec < CPU_CLOCK_NSEC) {
  56             struct timespec ts;
  57             int ret;
  58             ts.tv_sec = sec > CPU_CLOCK_SEC ? 0 : CPU_CLOCK_SEC - sec;
  59             ts.tv_nsec = nsec > CPU_CLOCK_NSEC ? 0 : CPU_CLOCK_NSEC - nsec;
  60
  61             ret = nanosleep(&ts, NULL);
  62             //dprint("sleep %d sec:%d, nsec:%d, err:%d\n", ret, ts.tv_sec, ts.tv_nsec, errno);
  63         }
  64     }
  65
  66     return NULL;
  67 }
  68
  69 int start_clock(void) {
  70     int ret;
  71     pthread_attr_t attr;
  72
  73     ret = pthread_attr_init(&attr);
  74     if (ret != RT_OK)
  75         return FALSE;
  76
  77     cpu_thread_id = 0;
  78     ret = pthread_create(&cpu_thread_id, &attr, cpu_clock_loop, NULL);
  79     if (ret != RT_OK)
  80         return FALSE;
  81
  82     return TRUE;
  83 }
  84
  85 static void end_loop(void) {
  86     void* ret;
  87     exit_loop = TRUE;
  88
  89     //join the running thread.
  90     pthread_join(cpu_thread_id, &ret);
  91
  92     dprint("clock thread joined.\n");
  93
  94 }
  95
  96 int register_clock_hander(clock_func_t *handler) {
  97     struct clock_handler *ch;
  98
  99     ch = malloc(sizeof(struct clock_handler));
 100     ch->l.next = NULL;
 101     ch->handler = handler;
 102
 103     if (handler_list == NULL) {
 104         handler_list = ch;
 105     }
 106     else {
 107         slist_add_tail(&handler_list->l, &ch->l);
 108     }
 109     return TRUE;
 110 }
 111
 112 int init_clock(void) {
 113     exit_loop = FALSE;
 114     handler_list = NULL;
 115     return TRUE;
 116 }
 117
 118 void clean_clock(void) {
 119     struct clock_handler *ch = handler_list;
 120
 121     end_loop();
 122
 123     while (ch != NULL) {
 124         struct clock_handler *pp = ch;
 125         ch = (struct clock_handler*)ch->l.next;
 126         free(pp);
 127     }
 128     //dprint("clean_clock done.\n");
 129 }
 130