Пример #1
0
void connecter(void *arg) {
    DMessage *message = d_new_message();
    int status;

    rt_printf("tconnect : Debut de l'exécution de tconnect\n");

    while (1) {
        rt_printf("tconnect : Attente du sémaphore semConnecterRobot\n");
        rt_sem_p(&semConnecterRobot, TM_INFINITE);
        rt_printf("tconnect : Ouverture de la communication avec le robot\n");
        status = robot->open_device(robot);
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        etatCommRobot = status;

        if (status == STATUS_OK) {
            cptCommErr = 0;
            //robot->start_insecurely(robot);
            status = robot->start(robot);
            if (status == STATUS_OK) { /* Demarrage du robot */
                rt_printf("tconnect : Robot démarrer\n");
                rt_sem_v(&semDeplacer);
                rt_sem_v(&semRechargerWatchdog);
                rt_sem_v(&semVerifierBatterie);
            } else { /* Impossible de demarrer le robot, tentative de reinitialisation */
                robot->stop(robot);
                robot->close_com(robot);
            }
        }
        
        rt_mutex_release(&mutexEtat);
        message->put_state(message, status);
        serveur->send(serveur, message);
    }
}
Пример #2
0
void task_H(void* data)
{
	rt_sem_p(&syncsem,TM_INFINITE);
	rt_task_sleep_ms(1);
	rt_sem_p(&semB,TM_INFINITE);
	rt_printf("H : I got B\n");
	busy_wait_ms(1);
	rt_sem_p(&semA,TM_INFINITE);
	busy_wait_ms(2);
	print_pri(NULL,(char) data);
	rt_sem_v(&semB);
	rt_sem_v(&semA);
}
Пример #3
0
void task_L(void* data)
{
	rt_sem_p(&syncsem,TM_INFINITE);
	
	rt_sem_p(&semA,TM_INFINITE);
	int prio = rt_task_set_priority(NULL,99);
	rt_printf("L : I got A\n");
	busy_wait_ms(3);
	rt_sem_p(&semB,TM_INFINITE);
	busy_wait_ms(3);
	print_pri(NULL,(char) data);
	rt_sem_v(&semB);
	rt_sem_v(&semA);
}
Пример #4
0
/**
 * Create the CAN Receiver Task
 * @param fd0 CAN port
 * @param *ReceiveLoop_task CAN receiver task
 * @param *ReceiveLoop_task_proc CAN receiver function
 */
void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_task, void* ReceiveLoop_task_proc)
{
	int ret;
	static int id = 0;
	char taskname[32];
	snprintf(taskname, sizeof(taskname), "canloop%d-%d", id, current->pid);
	id++;

	/* create ReceiveLoop_task */
	ret = rt_task_create(ReceiveLoop_task,taskname,0,50,0); /* T_JOINABLE only in user space */
	if (ret) {
		printk("Failed to create ReceiveLoop_task number %d, code %d\n", id, ret);
		return;
	}

	/* periodic task for Xenomai kernel realtime */
	rt_task_set_periodic(ReceiveLoop_task, 0, 1 * 1000 * 1000); /* 1ms */

	/* start ReceiveLoop_task */
	ret = rt_task_start(ReceiveLoop_task, ReceiveLoop_task_proc,(void*)fd0);
	if (ret) {
		printk("Failed to start ReceiveLoop_task number %d, code %d\n", id, ret);
		return;
	}
	rt_sem_v(&control_task);
}
Пример #5
0
void event(void *cookie)
{
	int err;

	err = rt_task_set_periodic(NULL,
				  TM_NOW,
				  rt_timer_ns2ticks(sampling_period));
	if (err) {
	       fprintf(stderr,"switch: failed to set periodic, code %d\n", err);
	       return;
	}

	for (;;) {
	       err = rt_task_wait_period(NULL);
	       if (err) {
		       if (err != -ETIMEDOUT) {
			       /* Timer stopped. */
			       rt_task_delete(NULL);
		       }
	       }

	       switch_count++;
	       switch_tsc = rt_timer_tsc();

	       rt_sem_v(&switch_sem);
	}
}
Пример #6
0
void prioHigh(void *arg){
    int err = 0;

    rt_task_sleep(WAIT);

    int i = 0;
    while(i<3) {
      
      rt_printf("High priority task tries to lock semaphore\n");
      err = rt_sem_p(&mysync,TM_INFINITE);
      if(err < 0) rt_printf("Failed pending semaphore; error: %d: %s", err, strerror(-err)); 
        err = 0;
      rt_printf("High priority task locks semaphore\n");

      rt_timer_spin(SPINTIME);  // spin cpu doing nothing

      i++;

      rt_printf("High priority task unlocks semaphore\n");
      err = rt_sem_v(&mysync);
      if(err < 0) rt_printf("Failed signaling semaphore; error: %d: %s", err, strerror(-err)); 
        err = 0;
    }
    rt_printf("..........................................High priority task ends\n");
}
Пример #7
0
void taskTwo(void *arg){
	int i;
	for(i=0;i<ITER;i++){
		rt_sem_p(&semGlobal2,0);
		rt_printf("I'm taskTwo and global = %d---\n",--global);
		rt_sem_v(&semGlobal1);
	}
}
Пример #8
0
void taskOne(void *arg){
	int i;
	for(i=0;i<ITER;i++){
		rt_sem_p(&semGlobal1,0);
		rt_printf("I'm taskOne and global = %d...\n",++global);
		rt_sem_v(&semGlobal2);
	}
}
Пример #9
0
void computationTask(long arg)
{
	usleep(1);
	while(1){
		int i = 0;
		rt_mutex_acquire(&mutex_donnee,TM_INFINITE);	

		while(tabDonnePris==1){
			rt_mutex_release(&mutex_donnee);
			rt_sem_p(&sem_donnee,TM_INFINITE);
			rt_mutex_acquire(&mutex_donnee,TM_INFINITE);
		}
		tabDonnePris = 1;
		
		rt_mutex_acquire(&mutex_obs,TM_INFINITE);	
		while(tabObstaclePris==1){
		  rt_mutex_release(&mutex_obs);
		  rt_sem_p(&sem_obs,TM_INFINITE);
		  rt_mutex_acquire(&mutex_obs,TM_INFINITE);
		}
		tabObstaclePris = 1;
		printf("***************** COMPUTATION TASK ******************\n");
		
		for(i = 0; i<SENSOR_SIZE; i++){
			//If the obstacle is farther than 7meters
			if(sensorArray[i] > 7){
				
				obstacle* newObstacle = malloc(sizeof(obstacle));
				newObstacle->distance = sensorArray[i];
				
				//Add the new obstacle at the end of the list
				newObstacle->nxt = NULL;
				
				if(obstacleList == NULL) obstacleList = newObstacle;
				else{
					obstacle* temp = obstacleList;
					if(temp->distance==0)temp->distance=newObstacle->distance;
					else{
						while(temp->nxt != NULL)temp = temp->nxt;
						temp->nxt = newObstacle;
					}
				}
			}			
		}
		tabDonnePris = 0;
		tabObstaclePris = 0;
		
		rt_mutex_release(&mutex_obs);
		rt_mutex_release(&mutex_donnee);
		rt_sem_v(&sem_obs);		
		

		usleep(200000);
		printf("fin compute\n");
	}
}
Пример #10
0
void sense(void *args){
	rt_task_set_periodic(NULL,TM_NOW,PLCperiod);
	while(!stopped){
		/*
		 *	Read inputs status
		*/
		sensors = readInputs();
		rt_sem_v(&readDone);
		rt_task_wait_period(NULL);
	}
}
Пример #11
0
void demo(void *arg)
{
	int num = * (int *)arg;
	RT_TASK *curtask;
	RT_TASK_INFO curtaskinfo;
	curtask=rt_task_self();
	rt_task_inquire(curtask, &curtaskinfo);
	rt_sem_p(&sem, TM_INFINITE);
	rt_printf("Task name: %s - Argument %d\n", curtaskinfo.name, num);
	rt_sem_v(&sem);
}
Пример #12
0
void act(void *args){
	rt_task_set_periodic(NULL,TM_NOW,PLCperiod);
	while(!stopped){
		rt_sem_p(&executionDone,0);
		/*
		 *	Write outputs status
		 */
		writeOutputs(actuators);
		rt_sem_v(&writeDone);
		rt_task_wait_period(NULL);
	}
}
Пример #13
0
void connecter(void * arg) {
    int status;
    DMessage *message;
 
    rt_printf("tconnect : Debut de l'exécution de tconnect\n");
 
    while (1) {
        rt_printf("tconnect : Attente du sémarphore semConnecterRobot\n");
        rt_sem_p(&semConnecterRobot, TM_INFINITE);
        rt_printf("tconnect : Ouverture de la communication avec le robot\n");
        status = robot->open_device(robot);
 
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        etatCommRobot = status;
        rt_mutex_release(&mutexEtat);
 
        if (status == STATUS_OK) {
            status = robot->start_insecurely(robot);
            if (status == STATUS_OK){
                rt_printf("tconnect : Robot démarrer\n");
                
                                
                rt_sem_v(&semConnectedRobot); 
                rt_sem_v(&semConnectedRobot); 
            }
        }
 
        message = d_new_message();
        message->put_state(message, status);
        

 
        rt_printf("tconnecter : Envoi message\n");
        message->print(message, 100);
 
        if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
            message->free(message);
        }
    }
}
void taskOne(void *arg)
{
	uint8_t i;
	for (i = 0; i < ITER; i++) {
		// Wait for the semaphore
		rt_sem_p_timed(&sem_inc, NULL);

		rt_printf("I am taskOne and global = %d................\n", ++global);

		// Release Global
		rt_sem_v(&sem_dec);
	}
}
void taskTwo(void *arg)
{
	uint8_t i;
	for (i = 0; i < ITER; i++) {
		// Wait for the semaphore
		rt_sem_p_timed(&sem_dec, NULL);

		rt_printf("I am taskTwo and global = %d----------------\n", --global);

		// Release Global
		rt_sem_v(&sem_inc);
	}
}
Пример #16
0
void high(){
	rt_sem_p(&synca, TM_INFINITE);
	rt_task_sleep_ms(200);
	rt_printf("HIGH RUNNING\n");
	rt_sem_p(&semaphore, TM_INFINITE);
	//rt_mutex_acquire(&mutex, TM_INFINITE);
	rt_printf("HIGH AQUIRED LCOK\n");
	busy_wait_ms(100);
	busy_wait_ms(100);
	rt_printf("HIGH FINISHED\n");
	rt_sem_v(&semaphore);
	//rt_mutex_release(&mutex);
	
}
Пример #17
0
void low(){
	rt_sem_p(&synca, TM_INFINITE);
	rt_printf("LOW RUNNING\n");
	rt_sem_p(&semaphore, TM_INFINITE);
	//rt_mutex_acquire(&mutex, TM_INFINITE);
	rt_printf("LOW AQUIRED LOCK\n");
	busy_wait_ms(100);
	busy_wait_ms(100);
	busy_wait_ms(100);
	rt_printf("LOW FINISHED\n");
	rt_sem_v(&semaphore);
	//rt_mutex_release(&mutex);
	
}
Пример #18
0
static void release_resource(int method, int id){
    if(method == METHOD_SEMAPHORE){
        if(rt_sem_v(&sem) == SUCCESS){
            rt_printf("Task %i unlocked a resource\n", id);
        }else{
            rt_printf("Task %i failed to unlock a resource \n", id); 
        }
    }else if(method == METHOD_MUTEX){
        if(rt_mutex_release(&mut) == SUCCESS){
            rt_printf("Task %i unlocked a resource \n", id); 
        }else{
            rt_printf("Task %i failed to unlock a resource \n", id); 
        }
    }
}
Пример #19
0
void communiquer(void *arg) {
    DMessage *msg = d_new_message();
    int var1 = 1;
    int num_msg = 0;
 
    rt_printf("tserver : Début de l'exécution de serveur\n");
    serveur->open(serveur, "8000");
    rt_printf("tserver : Connexion\n");
 
    rt_mutex_acquire(&mutexEtat, TM_INFINITE);
    etatCommMoniteur = 0;
    rt_mutex_release(&mutexEtat);
 
    while (var1 > 0) {
        rt_printf("tserver : Attente d'un message\n");
        var1 = serveur->receive(serveur, msg);
        num_msg++;
        if (var1 > 0) {
            switch (msg->get_type(msg)) {
                case MESSAGE_TYPE_ACTION:
                    rt_printf("tserver : Le message %d reçu est une action\n",
                            num_msg);
                    DAction *action = d_new_action();
                    action->from_message(action, msg);
                    switch (action->get_order(action)) {
                        case ACTION_CONNECT_ROBOT:
                            rt_printf("tserver : Action connecter robot\n");
                            rt_sem_v(&semConnecterRobot);
                            break;
                    }
                    break;
                case MESSAGE_TYPE_MOVEMENT:
                    rt_printf("tserver : Le message reçu %d est un mouvement\n",
                            num_msg);
                    rt_mutex_acquire(&mutexMove, TM_INFINITE);
                    move->from_message(move, msg);
                    move->print(move);
                    rt_mutex_release(&mutexMove);
                    break;
            }
        }
    }
}
Пример #20
0
void rt_task_body(void *cookie)
{
    RTIME end;
    int err;

    rt_task_thread = pthread_self();

    rt_printf("syscall\n");
    setup_checkdebug(SIGDEBUG_MIGRATE_SYSCALL);
    sched_yield();
    check_sigdebug_received("SIGDEBUG_MIGRATE_SYSCALL");

    rt_printf("signal\n");
    setup_checkdebug(SIGDEBUG_MIGRATE_SIGNAL);
    err = rt_sem_v(&send_signal);
    check_no_error("rt_sem_v", err);
    rt_task_sleep(rt_timer_ns2ticks(10000000LL));
    check_sigdebug_received("SIGDEBUG_MIGRATE_SIGNAL");

    rt_printf("relaxed mutex owner\n");
    setup_checkdebug(SIGDEBUG_MIGRATE_PRIOINV);
    err = rt_mutex_acquire(&prio_invert, TM_INFINITE);
    check("rt_mutex_acquire", err, -EINTR);
    check_sigdebug_received("SIGDEBUG_MIGRATE_PRIOINV");

    rt_printf("page fault\n");
    setup_checkdebug(SIGDEBUG_MIGRATE_FAULT);
    rt_task_sleep(0);
    *mem ^= 0xFF;
    check_sigdebug_received("SIGDEBUG_MIGRATE_FAULT");

    if (wd) {
        rt_printf("watchdog\n");
        rt_print_flush_buffers();
        setup_checkdebug(SIGDEBUG_WATCHDOG);
        end = rt_timer_tsc() + rt_timer_ns2tsc(2100000000ULL);
        rt_task_sleep(0);
        while (rt_timer_tsc() < end && !sigdebug_received)
            /* busy loop */;
        check_sigdebug_received("SIGDEBUG_WATCHDOG");
    }
}
Пример #21
0
void executor(void *args){
	rt_task_set_periodic(NULL,TM_NOW,PLCperiod);
	short step = (short)*args;
	actuators = 0;	// how to initialize the temporary value of actuators?
	while(!stopped){
		rt_sem_p(&readDone,0);
		int c;
		for(c=0;c<nStep;c++){
			if(stepStatus[c])
				step[c](sensors);	// or equivalent
		}
		rt_sem_v(&executionDone);
		rt_sem_p(&writeDone,0);
		for(c=0;c<nStep;c++){
			if(stepStatus[c])
				condition[c](sensor,actuators);
		}
		rt_task_wait_period(NULL);
	}
	return;
}
Пример #22
0
int main(int argc, char *const argv[])
{
	int ret;

	traceobj_init(&trobj, argv[0], sizeof(tseq) / sizeof(int));

	ret = rt_sem_create(&sem, "SEMA", 0, S_FIFO);
	traceobj_check(&trobj, ret, 0);

	ret = rt_task_create(&t_test, "test_task", 0, 10, 0);
	traceobj_check(&trobj, ret, 0);

	traceobj_mark(&trobj, 1);

	ret = rt_task_start(&t_test, test_task, NULL);
	traceobj_check(&trobj, ret, 0);

	traceobj_mark(&trobj, 2);

	ret = rt_task_suspend(&t_test);
	traceobj_check(&trobj, ret, 0);

	traceobj_mark(&trobj, 3);

	ret = rt_sem_v(&sem);
	traceobj_check(&trobj, ret, 0);

	traceobj_mark(&trobj, 4);

	ret = rt_task_resume(&t_test);
	traceobj_check(&trobj, ret, 0);

	traceobj_mark(&trobj, 5);

	traceobj_join(&trobj);

	traceobj_verify(&trobj, tseq, sizeof(tseq) / sizeof(int));

	exit(0);
}
Пример #23
0
void demo (void *arg){
	sleep(1);
	RT_TASK *curtask;
	RT_TASK_INFO curtaskinfo;
	//inquire current task
	curtask = rt_task_self();
	int retval;
	retval = rt_task_inquire(curtask,&curtaskinfo);
	if(retval<0){
		rt_printf("inquiring error %d:%s\n",-retval,strerror(-retval));
	}
	//print task name
	retval = * (int *)arg;
	RTIME p = 1e9;
	p*=retval;
	rt_task_set_periodic(NULL,TM_NOW,p);
	while(1){
		rt_sem_p(&s,0);
		rt_printf("[%s] %d s periodic\n", curtaskinfo.name,retval);
		rt_sem_v(&s);
		rt_task_wait_period(NULL);
	}
}
Пример #24
0
static void taskB(
    void *              arg) {

    (void)arg;

    taskPrintInfo();
    B = 0ULL;

    while (1) {
        LOG_DBG("B signal");
        rt_sem_v(
            &Sem);
        LOG_DBG("B signaled");
        B++;
        printf("B: %d\n", B);
        rt_task_sleep(MS_TO_NS(10));

        if (B == 10) {

            return;
        }
    }
}
Пример #25
0
void prioLow(void *arg)
{
    int err = 0;
    RTIME runtime;

    runtime = 0;
    while(runtime < EXECTIMELOW) {
      
      err = rt_sem_p(&mysync,TM_INFINITE);
      if(err < 0) rt_printf("Failed pending semaphore; error: %d: %s", err, strerror(-err)); 
        err = 0;
      rt_printf("Low priority task locks semaphore\n");

      rt_timer_spin(SPINTIME);  // spin cpu doing nothing

      runtime = runtime + SPINTIME;

      rt_printf("Low priority task unlocks semaphore\n");
      err = rt_sem_v(&mysync);
      if(err < 0) rt_printf("Failed signaling semaphore; error: %d: %s", err, strerror(-err)); 
        err = 0;
    }
    rt_printf("..........................................Low priority task ends\n");
}
Пример #26
0
void latency (void *cookie)
{
    int err, count, nsamples, warmup = 1;
    RTIME expected_tsc, period_tsc, start_ticks;
    RT_TIMER_INFO timer_info;
    RT_QUEUE q;

    rt_queue_create(&q, "queue", 0, 100, 0);

    if (!(hard_timer_running = rt_is_hard_timer_running())) {
	err = rt_timer_start(TM_ONESHOT);
    	if (err)
	   {
	   fprintf(stderr,"latency: cannot start timer, code %d\n",err);
	   return;
	   }
    }

    err = rt_timer_inquire(&timer_info);

    if (err)
	{
	fprintf(stderr,"latency: rt_timer_inquire, code %d\n",err);
	return;
	}

    nsamples = ONE_BILLION / period_ns / 1;
    period_tsc = rt_timer_ns2tsc(period_ns);
    /* start time: one millisecond from now. */
    start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
    expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);

    err = rt_task_set_periodic(NULL,start_ticks,period_ns);

    if (err)
	{
	fprintf(stderr,"latency: failed to set periodic, code %d\n",err);
	return;
	}

    for (;;)
	{
	long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj;
	long overrun = 0;
	test_loops++;

	for (count = sumj = 0; count < nsamples; count++)
	    {
	    expected_tsc += period_tsc;
	    err = rt_task_wait_period(NULL);

	    if (err)
		{
		if (err != -ETIMEDOUT) {
		    rt_queue_delete(&q);
		    rt_task_delete(NULL); /* Timer stopped. */
		}
		overrun++;
		}

	    dt = (long)(rt_timer_tsc() - expected_tsc);
	    if (dt > maxj) maxj = dt;
	    if (dt < minj) minj = dt;
	    sumj += dt;

	    if (!(finished || warmup) && (do_histogram || do_stats))
		add_histogram(histogram_avg, dt);
	    }

	if(!warmup)
	    {
	    if (!finished && (do_histogram || do_stats))
		{
		add_histogram(histogram_max, maxj);
		add_histogram(histogram_min, minj);
		}

	    minjitter = minj;
	    if(minj < gminjitter)
		gminjitter = minj;

	    maxjitter = maxj;
	    if(maxj > gmaxjitter)
		gmaxjitter = maxj;

	    avgjitter = sumj / nsamples;
	    gavgjitter += avgjitter;
	    goverrun += overrun;
	    rt_sem_v(&display_sem);

	struct smpl_t { long minjitter, avgjitter, maxjitter, overrun; } *smpl;
	smpl = rt_queue_alloc(&q, sizeof(struct smpl_t));
#if 1
	smpl->minjitter = rt_timer_tsc2ns(minj);
	smpl->maxjitter = rt_timer_tsc2ns(maxj);
	smpl->avgjitter = rt_timer_tsc2ns(sumj / nsamples);
	smpl->overrun   = goverrun;
	rt_queue_send(&q, smpl, sizeof(struct smpl_t), TM_NONBLOCK);
#endif

	    }

	if(warmup && test_loops == WARMUP_TIME)
	    {
	    test_loops = 0;
	    warmup = 0;
	    }
	}
}
Пример #27
0
int main(int argc, char *argv[])
{
	
    mlockall(MCL_CURRENT | MCL_FUTURE);
    
    int sock, connected, bytes_recieved;  
   char send_data [1024] , recv_data[1024]; 
   char sysc_call[1000];
   struct sockaddr_in server_addr,client_addr;    
   int sin_size;
  
   int port_data = 1029;
   int viewport_data = 2029;

   int err;

    /* Create a semaphore; we could also have attempted to bind to
       some pre-existing object, using rt_sem_bind() instead of
       creating it. */

    err = rt_sem_create(&sem_desc,"sendSem",SEM_INIT,SEM_MODE);


   if ((sock = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
            perror("Socket");
            exit(1);
     }

   
        
      server_addr.sin_family = AF_INET;         
      server_addr.sin_port = htons(1026);     
      server_addr.sin_addr.s_addr = INADDR_ANY; 
      bzero(&(server_addr.sin_zero),8);       /* Initial semaphore count */


      if (bind(sock, (struct sockaddr *)&server_addr, sizeof(struct sockaddr))
                                                                       == -1) {
            perror("Unable to bind");
            exit(1);
        }

        if (listen(sock, 5) == -1) {
            perror("Listen");

            exit(1);
        }
		pid = fork();
			if (pid == 0){
                 	      
			}        
   		 
        while(1)
        {  
            if (pid != 0){
	
            	sin_size = sizeof(struct sockaddr_in);
            	connected = accept(sock, (struct sockaddr *)&client_addr,&sin_size);
            	

            	connection++;

			

            	printf("\n I got a connection from (%s , %d)",
            	       inet_ntoa(client_addr.sin_addr),ntohs(client_addr.sin_port));
    				    
            	sprintf(send_data, "%d", port_data);
            	rt_sem_p(&sem_desc,TM_INFINITE);
	    		send(connected, send_data,strlen(send_data), 0);  
            	rt_sem_v(&sem_desc);
                if (connection==1){
                    pid = fork();
     		 	 if (pid == 0){
                             sleep(3);
                 	     sprintf(sysc_call, "./Model -p %d -b 5 -v %d &",port_data,viewport_data);
       		 	     system(sysc_call);
			}
		} 
                else if(connection %2 ==0){
            	    port_data++;
                    viewport_data++;
            	    pid = fork();
     		 	 if (pid == 0){
                             sleep(3);
                 	      sprintf(sysc_call, "./Model -p %d -b 5 -v %d &",port_data,viewport_data);
       		 	     system(sysc_call);
			}
                    
            	}
	    }
        }       

      close(sock);

}
Пример #28
0
void latency (void *cookie)
{
    int err, count, nsamples;
    RTIME expected, period;

    err = rt_timer_start(TM_ONESHOT);

    if (err)
      {
	fprintf(stderr,"latency: cannot start timer, code %d\n",err);
	return;
      }

    nsamples = ONE_BILLION / sampling_period;
    period = rt_timer_ns2ticks(sampling_period);
    expected = rt_timer_tsc();
    err = rt_task_set_periodic(NULL,TM_NOW,sampling_period);

    if (err)
      {
	fprintf(stderr,"latency: failed to set periodic, code %d\n",err);
	return;
      }

    for (;;)
      {
	long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj;
	overrun = 0;
 	test_loops++;

	for (count = sumj = 0; count < nsamples; count++)
	  {
	    unsigned long ov;

	    expected += period;
	    err = rt_task_wait_period(&ov);

	    if (err)
	      {
		if (err != -ETIMEDOUT)
		  rt_task_delete(NULL); /* Timer stopped. */

		overrun += ov;
	      }

	    dt = (long)(rt_timer_tsc() - expected);
	    if (dt > maxj) maxj = dt;
	    if (dt < minj) minj = dt;
	    sumj += dt;

	    if (!finished && (do_histogram || do_stats))
		add_histogram(histogram_avg, dt);
	  }

	if (!finished && (do_histogram || do_stats))
	  {
	    add_histogram(histogram_max, maxj);
	    add_histogram(histogram_min, minj);
	  }

	minjitter = rt_timer_ticks2ns(minj);
	maxjitter = rt_timer_ticks2ns(maxj);
	avgjitter = rt_timer_ticks2ns(sumj / nsamples);
	rt_sem_v(&display_sem);
      }
}
Пример #29
0
/**
 * Signaling a semaphore
 */
void LeaveMutex(void)
{
	rt_sem_v(&CanFestival_mutex);
}
Пример #30
0
void latency(void *cookie)
{
	int err, count, nsamples, warmup = 1;
	RTIME expected_tsc, period_tsc, start_ticks, fault_threshold;
	RT_TIMER_INFO timer_info;
	unsigned old_relaxed = 0;

	err = rt_timer_inquire(&timer_info);

	if (err) {
		fprintf(stderr, "latency: rt_timer_inquire, code %d\n", err);
		return;
	}

	fault_threshold = rt_timer_ns2tsc(CONFIG_XENO_DEFAULT_PERIOD);
	nsamples = ONE_BILLION / period_ns / 1000;
	period_tsc = rt_timer_ns2tsc(period_ns);
	/* start time: one millisecond from now. */
	start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
	expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);

	err =
	    rt_task_set_periodic(NULL, start_ticks,
				 rt_timer_ns2ticks(period_ns));

	if (err) {
		fprintf(stderr, "latency: failed to set periodic, code %d\n",
			err);
		return;
	}

	for (;;) {
		long minj = TEN_MILLION, maxj = -TEN_MILLION, dt;
		long overrun = 0;
		long long sumj;
		test_loops++;

		for (count = sumj = 0; count < nsamples; count++) {
			unsigned new_relaxed;
			unsigned long ov;

			expected_tsc += period_tsc;
			err = rt_task_wait_period(&ov);

			dt = (long)(rt_timer_tsc() - expected_tsc);
			new_relaxed = sampling_relaxed;
			if (dt > maxj) {
				if (new_relaxed != old_relaxed
				    && dt > fault_threshold)
					max_relaxed +=
						new_relaxed - old_relaxed;
				maxj = dt;
			}
			old_relaxed = new_relaxed;
			if (dt < minj)
				minj = dt;
			sumj += dt;

			if (err) {
				if (err != -ETIMEDOUT) {
					fprintf(stderr,
						"latency: wait period failed, code %d\n",
						err);
					exit(EXIT_FAILURE); /* Timer stopped. */
				}

				overrun += ov;
				expected_tsc += period_tsc * ov;
			}

			if (freeze_max && (dt > gmaxjitter)
			    && !(finished || warmup)) {
				xntrace_user_freeze(rt_timer_tsc2ns(dt), 0);
				gmaxjitter = dt;
			}

			if (!(finished || warmup) && need_histo())
				add_histogram(histogram_avg, dt);
		}

		if (!warmup) {
			if (!finished && need_histo()) {
				add_histogram(histogram_max, maxj);
				add_histogram(histogram_min, minj);
			}

			minjitter = minj;
			if (minj < gminjitter)
				gminjitter = minj;

			maxjitter = maxj;
			if (maxj > gmaxjitter)
				gmaxjitter = maxj;

			avgjitter = sumj / nsamples;
			gavgjitter += avgjitter;
			goverrun += overrun;
			rt_sem_v(&display_sem);
		}

		if (warmup && test_loops == WARMUP_TIME) {
			test_loops = 0;
			warmup = 0;
		}
	}
}