TSP: The Transport Sample Protocol



Main Page | Modules | Alphabetical List | Data Structures | File List | Data Fields | Globals | Related Pages

stub_loopOnRtc.c

00001 /*
00002  * Buzy Loop on VME A24 Top synchro
00003  * Read Time, when 100ms, add in histogram
00004  */
00005 
00006 /* POSIX.1 */
00007 #include <stdlib.h>
00008 #include <stdio.h>
00009 #include <unistd.h>
00010 #include <math.h>
00011 
00012 /* POSIX.4 */
00013 #include <signal.h>
00014 
00015 /* Boue */
00016 
00017 #include <linux/rtc.h>
00018 #include <sys/ioctl.h>
00019 #include <fcntl.h>
00020 #include <unistd.h>
00021 #include <errno.h>
00022 #include <math.h>
00023 #include <stdlib.h>
00024 #include <stdio.h>
00025 #include <sys/time.h>
00026 #include <sys/mman.h>
00027 #include <sched.h>
00028 #include <sys/signal.h>
00029 #include <string.h>
00030 
00031 /* Temporary include, wait for libc patch of setaffinity */
00032 #include "affinity.h"
00033 
00034 /* Projet */
00035 #include "histogram.h"
00036 #include "tsp_provider_init.h"
00037 #include "tsp_datapool.h"
00038 #include "tsp_sys_headers.h"
00039 #include "glue_sserver.h"
00040 #include "tsp_ringbuf.h"
00041 #include "tsp_time.h"
00042 #include "calc_func.h"
00043 
00044 
00045 /* TSP glue server defines */
00046 #define TSP_STUB_FREQ 128 /*Hz, must be a 2 power for RTC*/
00047 #define TSP_PERIOD_US (1000000/TSP_STUB_FREQ) /*given in µS, value 10ms*/
00048 #define GLU_MAX_SYMBOLS 100
00049 /* Glue server ringbuf size */
00050 
00051 
00052 /* Nasty static variables */
00053 static TSP_sample_symbol_info_t *X_sample_symbol_info_list_val;
00054 static glu_ringbuf* glu_ring = 0;
00055 
00056 // My Globals : Bouh
00057 int stopit;
00058 int rt_mode=1; /* 0 = NRT, 1  RT mode */
00059 int it_mode=0; /* 0 = Polling on date, 1=IT , 2=IT blocking IO */
00060 unsigned long affinity_mask=0x1; /* Run on proc #0 */
00061 
00062 void fatal(char *txt)
00063 {
00064   fprintf (stderr, "ERROR : %s\n", txt);
00065   exit (-1);
00066 }
00067 
00068 double second(void) {
00069         struct timeval tv;
00070         gettimeofday(&tv,0);
00071         return tv.tv_sec + 1e-6 * tv.tv_usec;
00072 }
00073 
00074 typedef unsigned long long u64;
00075 
00076 /* return in nanosecds */
00077 u64 rdtsc(void) {
00078         u64 tsc;
00079         __asm__ __volatile__("rdtsc" : "=A" (tsc));
00080         return tsc;
00081 }
00082 
00083 
00084 void rtc_action(int toDo)
00085 {
00086   static int fd;
00087   int flags;
00088   char * dev_file_name = "/dev/rtc";
00089   long long data[3];
00090 
00091   switch (toDo)
00092     {
00093     case 0 : /* Open */
00094         flags = O_RDONLY;
00095         if (it_mode !=2) flags |= O_NONBLOCK; 
00096         fd = open(dev_file_name,flags);
00097         if (fd == -1) 
00098                 fatal(dev_file_name);
00099 
00100         if (ioctl(fd, RTC_IRQP_SET, TSP_STUB_FREQ) == -1) {
00101             fatal("ioctl(RTC_IRQP_SET) failed");
00102         }
00103         printf("Configured RTC for %d Hz, IRQ 8, it_mode=%d\n", TSP_STUB_FREQ, it_mode);
00104 
00105         /* Enable periodic interrupts */
00106         if (ioctl(fd, RTC_PIE_ON, 0) == -1)
00107           fatal("ioctl(RTC_PIE_ON) failed");
00108         break;
00109 
00110     case 1: /* Close */
00111         if (ioctl(fd, RTC_PIE_OFF, 0) == -1)
00112                 fatal("ioctl(RTC_PIE_OFF) failed");
00113         break;
00114 
00115     case 2: /* Poll*/
00116       if (it_mode!=2)
00117         while (read(fd,&data[0],sizeof(data)) == -1 && errno == EAGAIN);
00118       else
00119         read(fd, &data, sizeof(data));
00120 
00121       if (errno != EAGAIN && errno != 0)
00122         fatal("blocking read failed");
00123       break;
00124     }
00125 }
00126 
00127 void rtc_init(void)
00128 {
00129   rtc_action(0);
00130 }
00131  
00132 
00133 void rtc_close(void)
00134 {
00135   rtc_action(1);
00136 }
00137 
00138 /* in micro seconds */
00139 u64 rtc_read_time(void)
00140 {
00141   return (u64)(rdtsc()/1e3);
00142 }
00143 
00144 void rtc_wait_next_it(void)
00145 {
00146   rtc_action(2);
00147 }
00148 
00149 
00150 void signalled(int sig)
00151 {
00152   stopit = 1;
00153 }
00154 
00155 void usage(char *name)
00156 {
00157   printf ("USAGE: %s [-r rtMode(0|1) ]  [-i itMode(0|1|2) ] [-m affinityMask ]\n", name);
00158   exit (-1);
00159 }
00160 
00161 
00162 int rt_memory_lock(int pid)
00163 {
00164         if(mlockall(MCL_CURRENT | MCL_FUTURE)!=0){
00165                 fatal("ERROR : rt_memory_lock : mlockall error : ");
00166         }
00167         return 0;
00168 }
00169 
00170 int rt_memory_unlock(int pid)
00171 {
00172         if(munlockall()!=0){
00173                 fatal("rt_memory_unlock : munlockall error : ");
00174         }
00175         return 0;
00176 }
00177 
00178 
00179 int set_real_time_priority(int pid, unsigned long mask)
00180 {
00181         struct sched_param schp;
00182         unsigned int len; 
00183 
00184         /* Affinity to CPUs describe in mask  */
00185         len = sizeof(mask);
00186 
00187         if (sched_setaffinity(pid, len, &mask)) {
00188                 fatal ("could not set pid's affinity.\n");
00189         }
00190 
00191         /*
00192          * set the process to realtime privs
00193          */
00194         memset(&schp, 0, sizeof(schp));
00195         schp.sched_priority = sched_get_priority_max(SCHED_FIFO);
00196         
00197         if (sched_setscheduler(pid, SCHED_FIFO, &schp) != 0) {
00198                 fatal("sched_setscheduler");
00199         }
00200 
00201         return 0;
00202 }
00203  
00204 
00205 int main(int argc, char *argv[])
00206 {
00207   char myopt; /* Options */
00208 
00209   /* Init server */
00210   if(!TSP_provider_init(&argc, &argv))
00211     {
00212       exit (-1);
00213     }
00214   
00215   while ((myopt = getopt(argc, argv, "hr:m:i:")) != -1) {
00216     switch(myopt) {
00217     case 'r':   rt_mode = atoi(optarg);         break;
00218     case 'i':   it_mode = atoi(optarg);         break;
00219     case 'm':   affinity_mask = atoi(optarg);   break;
00220     case 'h':   usage(argv[0]);                 break;
00221     default:    break;
00222     }
00223   }
00224 
00225   printf ("#============================================================#\n");
00226   printf ("# Launching <LinuxRTServer> for %d Symbols at %dHz #\n", GLU_MAX_SYMBOLS, TSP_STUB_FREQ);
00227   printf ("# RTmode=%d  Mask=0x%lX \n", rt_mode, affinity_mask);
00228   printf ("#============================================================#\n");
00229 
00230   /* Open the logical device */
00231   printf ("=====================================\n");
00232   rtc_init();
00233   tsp_histo_init();
00234   tsp_histo_set_delta (10); /* Jitter of 10us*/
00235   if (rt_mode) {
00236       rt_memory_lock(0);
00237     }
00238   
00239   /* Can use kill -2 for stopping prog */
00240   signal(SIGINT, signalled);
00241   
00242   TSP_provider_run(TRUE);
00243   
00244   return 0;
00245 }
00246    
00247 
00248 
00249 /* ===================================== TSP PART ==================================== */
00250 
00251 char* GLU_get_server_name(void)
00252 {
00253   if (rt_mode)
00254     return "LinuxRT";
00255   else
00256     return "LinuxNRT";
00257 }
00258  
00259 int  GLU_get_symbol_number(void)
00260 
00261 {
00262   int i = 0;
00263   TSP_sample_symbol_info_t* p  = X_sample_symbol_info_list_val;
00264   for( p=X_sample_symbol_info_list_val; p->name!=0 ; p++) i++;
00265   return i;
00266 }
00267 
00268 static void* GLU_thread(void* arg)
00269 {
00270   
00271   static int last_missed = 0;
00272   int i, symbols_nb, my_time=0;
00273   glu_item_t item;
00274   unsigned long  last_us, now_us, t0_us, jitter, jitter_max=0;
00275   long delay;
00276   double retard, simtime, rtctime;
00277 
00278   printf (" GLU_thread :Last Time in NO RT mode !!!!\n");
00279   fflush (stdout);
00280   if (rt_mode) {
00281     set_real_time_priority (0 /*myself*/, affinity_mask);
00282   }
00283 
00284   symbols_nb  = GLU_get_symbol_number();
00285 
00286   /* Must synchronise to first IT then RAZ  handler */
00287   rtc_wait_next_it();
00288   last_us = now_us = rtc_read_time();
00289   rtc_wait_next_it();
00290   t0_us = rtc_read_time();
00291 
00292   /* infinite loop for symbols generation */
00293   stopit = 0;
00294   while(!stopit)
00295     {
00296       delay = 0;
00297 
00298       /* Wait to a new Period of time */
00299       switch (it_mode) 
00300         {
00301         case 0: /* Polling like a beast on time */
00302           while (delay < TSP_PERIOD_US)
00303             {
00304               now_us = rtc_read_time();
00305               delay =  (now_us - last_us); /* in us  on 64 bits */
00306             }
00307           break;
00308         case 1: /* Blocking / Non blocking IT before read */
00309           rtc_wait_next_it();
00310           now_us = rtc_read_time();
00311           delay =  (now_us - last_us); /* in us  on 64 bits */
00312           if (delay<0 || delay >= 1.5* TSP_PERIOD_US) printf ("Warning : delay=%ld, now=%lu, last=%lu\n", delay, now_us, last_us);
00313           break;
00314         default: break;
00315         }
00316           
00317 
00318       /* We got a new period, must calculate the data */
00319       my_time++;    
00320       last_us = now_us;
00321       jitter =  delay-TSP_PERIOD_US; 
00322       simtime = (double)(my_time) / (double)(TSP_STUB_FREQ);
00323       rtctime =  ((now_us-t0_us)/1e6);
00324       retard = rtctime - simtime;
00325       if (jitter>jitter_max) jitter_max = jitter;
00326       tsp_histo_enter_with_date(jitter, rtctime);
00327 
00328       for(i = 0 ; i <  symbols_nb ; i++)
00329         {
00330           item.time = my_time;
00331           item.provider_global_index = i;
00332           switch (i) 
00333             {
00334             case 0: item.value = simtime; break; /* s */
00335             case 1: item.value = rtctime; break;  /* s */
00336             case 2: item.value = now_us; break;  /* s */
00337             case 3: item.value = (double)delay/1e3; break;   /* ms */
00338             case 4: item.value = jitter; break; /* jitter us */
00339             case 5: item.value = (jitter)/1e3; break; /* jitter ms */
00340             case 6: item.value = (jitter_max)/1e3; break; /* jitter ms */
00341             case 7: item.value = retard; break; /* jitter ms */
00342             default :
00343                 item.value = calc_func(i, my_time); break;
00344             }
00345           TSP_datapool_push_next_item(&item);
00346         }
00347       
00348       
00349       /* Finalize the datapool state with new time : Ready to send */
00350       TSP_datapool_push_commit(my_time, GLU_GET_NEW_ITEM);
00351 
00352       if (rt_mode==0 && !(my_time%10))  
00353          printf("TOP %d : %s=%g \t%s=%g \t%s=%g \t%s=%g", my_time,
00354                 X_sample_symbol_info_list_val[0].name, simtime,
00355                 X_sample_symbol_info_list_val[1].name, rtctime,
00356                 X_sample_symbol_info_list_val[7].name, retard/1e3,
00357                 X_sample_symbol_info_list_val[6].name, jitter_max/1e3 );
00358     }
00359  
00360   /* Exit the program  */
00361   printf ("Exiting from GLU_Thread loop because stopit=%d\n", stopit);
00362 
00363             
00364   tsp_histo_dump(stdout , "LOOP on RTC");
00365  
00366   printf ("simtime\t= %gs\n", simtime);
00367   printf ("rtctime\t= %gs\n", rtctime);
00368   printf ("retard\t= %gs\n", retard);
00369   printf ("delay\t= %gms\n", (double)delay/1e3);
00370   printf ("max jit\t= %gms\n", jitter_max/1e3);
00371 
00372   exit (-1);
00373   return arg;
00374 
00375 }
00376 
00377 int GLU_init(int fallback_argc, char* fallback_argv[])
00378 {
00379   int i;
00380   char symbol_buf[50];
00381   
00382   X_sample_symbol_info_list_val = calloc (GLU_MAX_SYMBOLS+1, sizeof (TSP_sample_symbol_info_t)) ;
00383   for (i=0; i<GLU_MAX_SYMBOLS; i++)
00384     {      
00385       sprintf(symbol_buf, "Symbol%d",i);
00386       X_sample_symbol_info_list_val[i].name = strdup(symbol_buf);
00387       X_sample_symbol_info_list_val[i].provider_global_index = i;
00388       X_sample_symbol_info_list_val[i].period = 1;
00389     }  
00390 
00391   /*overide name*/
00392   i=0;
00393   X_sample_symbol_info_list_val[0].name = strdup("simtime");
00394   X_sample_symbol_info_list_val[1].name = strdup("rtime");
00395   X_sample_symbol_info_list_val[2].name = strdup("btime");
00396   X_sample_symbol_info_list_val[3].name = strdup("delay");
00397   X_sample_symbol_info_list_val[4].name = strdup("jitter");
00398   X_sample_symbol_info_list_val[5].name = strdup("jitter_ms");
00399   X_sample_symbol_info_list_val[6].name = strdup("jitter_max");
00400   X_sample_symbol_info_list_val[7].name = strdup("retard");
00401 
00402   return TRUE;
00403 }
00404 
00405 int GLU_start(void)
00406 {
00407   static pthread_t thread_id = 0;       
00408   return pthread_create(&thread_id, NULL, GLU_thread, NULL);
00409 }
00410 
00411 GLU_handle_t GLU_get_instance(int argc, char* argv[], char** error_info)
00412 {
00413   if(error_info)
00414     *error_info = "";
00415 
00416   return GLU_GLOBAL_HANDLE;
00417 }
00418 
00419 double GLU_get_base_frequency(void)
00420 {
00421   /* Calculate base frequency */
00422   return TSP_STUB_FREQ;
00423 }
00424 
00425 int  GLU_get_sample_symbol_info_list(GLU_handle_t h_glu,TSP_sample_symbol_info_list_t* symbol_list)
00426 {
00427   symbol_list->TSP_sample_symbol_info_list_t_len = GLU_get_symbol_number();
00428   symbol_list->TSP_sample_symbol_info_list_t_val = X_sample_symbol_info_list_val;
00429   return TRUE;
00430 }
00431 
00432 GLU_server_type_t GLU_get_server_type(void)
00433 {
00434   return GLU_SERVER_TYPE_ACTIVE;
00435 }
00436 
Framework Home Page.

Beware !! TSP wave is coming...