static void bfun(long t)
{
	RT_MSGQ *smbx, **rmbx;
	int *msg, mtype, i, n;

	rmbx = rt_malloc(NTASKS*sizeof(int));
	msg  = rt_malloc((MAXSIZ + 1)*sizeof(int));

	smbx = rt_named_msgq_init("SMSG", 0, 0);
	for (i = 0; i < NTASKS; i++) {
		char mname[6] = "RMBX";
		mname[4] = i + '0';
		mname[5] = 0;
		rmbx[i] = rt_named_msgq_init(mname, 0, 0);
	}

	rt_sem_wait_barrier(&barrier);
	while (end < NTASKS) {
		rt_msg_receive(smbx, msg, sizeof(int)*(MAXSIZ + 1), &mtype);
		n = 0;
		for (i = 1; i < MAXSIZ; i++) {
			n += msg[i];
		}
		if (msg[MAXSIZ] != n) {
			rt_printk("SERVER RECEIVED AN UNKNOWN MSG.\n");
			goto prem;
		}
		msg[1] = 0xFFFFFFFF;
		rt_msg_send(rmbx[msg[0]], msg, 2*sizeof(int), 1);
	}
prem:
	rt_free(rmbx);
	rt_free(msg);
	rt_printk("SERVER TASK ENDS.\n");
}

int main(void)
{
  RT_TASK *task;
  RTIME now;
  int cnt=0;

  // make main thread LXRT soft realtime
  task = rt_task_init_schmod(nam2num("MYTASK"), 9, 0, 0, SCHED_FIFO, 0xF);
  mlockall(MCL_CURRENT | MCL_FUTURE);

  // start realtime timer and scheduler
  //rt_set_oneshot_mode();
  rt_set_periodic_mode();
  start_rt_timer(0);

  now = rt_get_time() + 10*PERIOD;
  rt_task_make_periodic(task, now, PERIOD);

  printf("Init mutex and cond.\n");
  mutex = rt_sem_init(nam2num("MUTEX"), 1);

  if (mutex==0)
    printf("Error init mutex\n");

  cond = rt_cond_init(nam2num("CONDITION"));

  if (cond==0)
    printf("Error init cond\n");

  thread0 = rt_thread_create(fun0, NULL, 10000);
  //thread1 = rt_thread_create(fun1, NULL, 20000);

  //rt_sleep(PERIOD);

  while (cnt < THRESHOLD) {
    rt_task_wait_period();
    rt_printk("main: Hello World %d!\n",cnt);
    rt_sem_wait(mutex); //now the mutex should have value 0

    if (instance_cnt==0) {
      rt_sem_signal(mutex); //now the mutex should have vaule 1
      rt_printk("worker thread busy!\n");
    } else {
      instance_cnt++;
      rt_cond_signal(cond);
      rt_sem_signal(mutex); //now the mutex should have vaule 1
      rt_printk("signaling worker thread to start!\n");
    }

    cnt++;
  }

  // wait for end of program
  printf("TYPE <ENTER> TO TERMINATE\n");
  getchar();

  // cleanup
  stop_rt_timer();
  return 0;
}

void send_msg(void *arg)
{
    int ret;
    struct msghdr msg;
    struct iovec iov[2];
    unsigned short msgsize = size;


    while(1)
    {
        iov[0].iov_base = &msgsize;
        iov[0].iov_len  = sizeof(msgsize);
        iov[1].iov_base = buffer_out;
        iov[1].iov_len  = size;

        memset(&msg, 0, sizeof(msg));
        msg.msg_name    = &dest_addr;
        msg.msg_namelen = sizeof(dest_addr);
        msg.msg_iov     = iov;
        msg.msg_iovlen  = 2;

        rt_printk("Sending message of %d+2 bytes\n", size);
        ret = rt_socket_sendmsg(sock, &msg, 0);
        if (ret != (int)(sizeof(msgsize) + size))
            rt_printk(" rt_socket_sendmsg() = %d!\n", ret);

        rt_task_wait_period();
    }
}

static void sender(long nothing)
{
	int diff = 0, warmup = 1000000000/WORKCYCLE;
	RTIME t, tb;
	struct sample { unsigned long cnt; RTIME tx, rx; } samp = { 0, 0, 0 };
	rt_printk("RtnetTest: Transmitter task initialised\n");

	tb = t = rt_get_real_time_ns();
	while(!end) {
		slen = sprintf(buffer_out, "%lld", t);
		slen = rt_dev_sendto(sock, buffer_out, slen, 0, (struct sockaddr*)&tx_addr, sizeof(tx_addr));

		if (slen < 0) {
			rt_printk("RtnetTest: Packet send failed! Errno %d\n", -slen);
			return;
		}
		rt_task_wait_period();
		t = rt_get_real_time_ns();
		if (!warmup) {
			diff = abs((int)(t - tb - WORKCYCLE));
			samp.cnt++;
			tb = t;
			if (diff > samp.rx) samp.rx = diff;
			rt_mbx_send_if(mbx, &samp, sizeof(samp));
		} else {
			tb = rt_get_real_time_ns();
			warmup--;
		}
	}
}

/***
 *	rt_unregister_rtnetdev:	unregister a rtnet_device
 *	@rtdev:			the device
 */
int rt_unregister_rtnetdev(struct rtnet_device *rtdev)
{
	struct net_device *d, **dp, *dev = dev_get_by_rtdev(rtdev);

	rt_printk("RTnet: rt_unregister_netdevice(%s)\n", dev->name);

	/* If device is running, close it first. */
	if (dev->flags & IFF_UP)
		rtdev_close(rtdev);

	dev->deadbeaf = 1;

	/* And unlink it from device chain. */
	for (dp = &dev_base; (d=*dp)!=NULL; dp=&d->next) {
		if (d==dev) {
			write_lock(&dev_base_lock);
			*dp = d->next;
			write_unlock(&dev_base_lock);
			break;
		}
	}

	if ( !d ) {
		rt_printk("RTnet: device %s/%p never was registered\n", dev->name, dev);
		return -ENODEV;
	}

	clear_bit(__LINK_STATE_PRESENT, &dev->state);


	if ( dev->uninit ) 
		dev->uninit(dev);

	return 0;
}

static void timer_tick(void)
{
	rt_times.tick_time = rt_times.intr_time;
	rt_times.intr_time = rt_times.tick_time + rt_times.periodic_tick;
	rt_set_timer_delay(0);
	if (rt_times.tick_time >= rt_times.linux_time) {
		rt_times.linux_time += rt_times.linux_tick;
		rt_pend_linux_irq(TIMER_8254_IRQ);
	}
	if (run) {
		if (rt_waiting_return(tasknode, taskport)) {
			overuns++;
		}
		switch(run) {
			case 1: RT_sem_signal(tasknode, -taskport, rmt_sem);
				rt_printk("SEM SIGNAL %d\n", ++cnt);
				break;
			case 2:
				RT_task_resume(tasknode, -taskport, rmt_task);
				rt_printk("TASK RESUME %d\n", ++cnt);
				break;
			case 3:
				RT_send_if(tasknode, -taskport, rmt_task, run);
				rt_printk("TASK SEND %d\n", ++cnt);
				break;
		}
	}
}

static void bfun(long t)
{
	unsigned long long msg;
	unsigned long long name = 0xccccccccccccccccLL;

	while (1) {
		cpu_used[hard_cpu_id()]++;

		msg = 0LL;
		bstat = 'r';
		rt_mbx_receive(&smbx, &msg, sizeof(msg));
		if (msg == 0x1122334455667788LL) {
			t = 0;
		} else {
			if (msg == 0x99aabbccddeeff00LL) {
				t = 1;
			} else {
				rt_printk("SERVER RECEIVED AN UNKNOWN MSG: %x%x, STAT: %c %c %c.\n", ((int *)&msg)[1], ((int *)&msg)[0], mstat[0], mstat[1], bstat);
				t = 0;
				goto prem;
			}
		}
		bstat = '0' + t;
		rt_mbx_send(&rmbx[t], &name, sizeof(name));
	}
prem:
	premature = PREMATURE;
	rt_printk("SERVER TASK ENDS PREMATURELY.\n");
}

/***
 *	rtskb_pool_release
 */
int rtskb_pool_release(void) 
{
	int err = 0;

	if ( rt_free_srq (dec_pool_srq)<0 ) {
		rt_printk("RTnet: deallocating 'dec_pool_srq=%d' failed.\n", dec_pool_srq);
		return dec_pool_srq;
	}

	if ( rt_free_srq (inc_pool_srq)<0 ) {
		rt_printk("RTnet: deallocating 'inc_pool_srq=%d' failed.\n", inc_pool_srq);
		return inc_pool_srq;
	}

	if ( rtskb_pool.qlen>0 ) {
		int i;		
		for (i=rtskb_pool.qlen; i>0; i--)
			dispose_rtskb(__rtskb_dequeue(&rtskb_pool));
	}

	if ( rtskb_data_cache && kmem_cache_destroy (rtskb_data_cache) ) {
		rt_printk("RTnet: deallocating 'rtskb_data_cache' failed.\n");
	}
	
	if ( rtskb_cache && kmem_cache_destroy (rtskb_cache) ) {
		rt_printk("RTnet: deallocating 'rtnet_skb_cache' failed.\n");
	}

	return err;
}

void taskh_func(long tid)
{
	RTIME time;
	unsigned int msg = 0, wait;
	rt_send(&taskm, msg);
	rt_send(&taskl, msg);
	while (1) {
		rt_receive(&taskm, &msg);
		time = rt_get_time_ns();
		if (MUTEX_LOCK(&mutex) <= 1) {
			if ((wait = (int)(rt_get_time_ns() - time)) > 250000) {
				rt_printk("PRIORITY INVERSION, WAITED FOR %d us\n", wait/1000);
			} else {
				rt_printk("NO PRIORITY INVERSION, WAITED FOR %d us\n", wait/1000);
			}
			if (SemType) {
				MUTEX_LOCK(&mutex);
				MUTEX_LOCK(&mutex);
				rt_busy_sleep(100000);
				MUTEX_LOCK(&mutex);
			}
			rt_busy_sleep(100000);
			if (SemType) {
				rt_sem_signal(&mutex);
				rt_busy_sleep(100000);
				rt_sem_signal(&mutex);
				rt_sem_signal(&mutex);
			}
			rt_sem_signal(&mutex);
		} else {
			rt_task_suspend(0);
		}
	}
}

/***
 *	rt_unregister_rtnetdev:	unregister a rtnet_device
 *	@rtdev:			the device
 */
int rt_unregister_rtnetdev(struct rtnet_device *rtdev)
{
	struct rtnet_device *d, **dp;
	unsigned long flags;

	rt_printk("RTnet: rt_unregister_netdevice(%s)\n", rtdev->name);

	/* If device is running, close it first. */
	if (rtdev->flags & IFF_UP)
		rtdev_close(rtdev);

	hard_save_flags_and_cli(flags);
	write_lock(&rtnet_devices_lock);
	/* Unlink it from device chain. */
	for (dp = &rtnet_devices; (d=*dp)!=NULL; dp=&d->next) {
		if (d==rtdev) {
			*dp = d->next;
			break;
		}
	}
	write_unlock(&rtnet_devices_lock);
	hard_restore_flags(flags);

	if ( !d ) {
		rt_printk("RTnet: device %s/%p never was registered\n", rtdev->name, rtdev);
		return -ENODEV;
	}
	clear_bit(__LINK_STATE_PRESENT, &rtdev->state);

	return 0;
}

static void bfun(int t)
{
	int smbx, *rmbx, *msg, mtype, i, n;

	rmbx = rt_malloc(NTASKS*sizeof(int));
	msg  = rt_malloc((MAXSIZ + 1)*sizeof(int));

	smbx = rt_msgget(nam2num("SMSG"), 0);
	for (i = 0; i < NTASKS; i++) {
		char mname[6] = "RMBX";
		mname[4] = i + '0';
		mname[5] = 0;
		rmbx[i] = rt_msgget(nam2num(mname), 0);
	}

	rt_sem_wait_barrier(&barrier);
	while (end < NTASKS) {
		rt_msgrcv_nu(smbx, &mtype, msg, sizeof(int)*(MAXSIZ + 1), 1, 0);
		n = 0;
		for (i = 1; i < MAXSIZ; i++) {
			n += msg[i];
		}
		if (msg[MAXSIZ] != n) {
			rt_printk("SERVER RECEIVED AN UNKNOWN MSG.\n");
			goto prem;
		}
		msg[1] = 0xFFFFFFFF;
		rt_msgsnd_nu(rmbx[msg[0]], 1, msg, 2*sizeof(int), 0);
	}
prem:
	rt_free(rmbx);
	rt_free(msg);
	rt_printk("SERVER TASK ENDS.\n");
}

static RTAI_SYSCALL_MODE int _comedi_get_krange(void *dev, unsigned int subdev, unsigned int chan, unsigned int range, comedi_krange *krange)
{
	rt_printk("COMEDI GET KRANGE: dev = %p, subdev = %u, chan = %u, range = %u.\n", dev, subdev, chan, range);
	rt_printk("COMEDI GET KRANGE RETURNS %d, %d, %u.\n", RANGE_MIN, RANGE_MAX, RANGE_FLAGS);
	*krange = (comedi_krange){ RANGE_MIN, RANGE_MAX, RANGE_FLAGS};
	return 0;
}

RTAI_SYSCALL_MODE char *rt_comedi_get_driver_name(void *dev, char *name)
{
	rt_printk("COMEDI GET DRIVER NAME: dev = %x.\n", dev);
	strncpy(name, DRIVER_NAME, COMEDI_NAMELEN);
	rt_printk("COMEDI GET DRIVER NAME RETURNS %s.\n", DRIVER_NAME);
	return name;
}

/***
 *      do_stacktask
 */
static void do_stacktask(int mgr_id)
{
        struct rtnet_msg msg;
	struct rtnet_mgr *mgr = (struct rtnet_mgr *)mgr_id;

        rt_printk("RTnet: stack-mgr started\n");
        while(1) {
                rt_mbx_receive(&(mgr->mbx), &msg, sizeof(struct rtnet_msg));
                if ( (msg.rtdev) && (msg.msg_type==Rx_PACKET) ) {
			while ( !rtskb_queue_empty(&msg.rtdev->rxqueue) ) {
	                        struct rtskb *skb = rtskb_dequeue(&msg.rtdev->rxqueue);
		                if ( skb ) {
			                unsigned short hash = ntohs(skb->protocol) & (MAX_RT_PROTOCOLS-1);
				        struct rtpacket_type *pt = rt_packets[hash];
					skb->nh.raw = skb->data;
	                                if (pt) {
		                                pt->handler (skb, skb->rtdev, pt);
			                } else {
						rt_printk("RTnet: undefined Layer-3-Protokoll\n");
						kfree_rtskb(skb);
					}
				}
                        }
                }

        }
}

int main(void)
{
	int i, srq, count = 0, nextcount = 0, repeat;

	rt_thread_init(nam2num("MNTSK"), 100, 0, SCHED_FIFO, 0x1);
	rt_printk("\nTESTING THE SCHEDULER WITH SRQs [%d LOOPs].\n", LOOPS);
	repeat = 1000000000LL/((long long)DELAY*(long long)PRINT_FREQ);
	srq = rtai_open_srq(0xcacca);
	start_rt_timer(0);
	rt_grow_and_lock_stack(100000);
#ifdef MAKE_HARD
	MAKE_HARD();
#endif

	for (i = 0; i < LOOPS; i++) {
		rtai_srq(srq, (unsigned long)nano2count(DELAY));
		if (++count > nextcount) {
			nextcount += repeat;
			rt_printk(">>> %d.\n", nextcount);
		}
	}

	rt_make_soft_real_time();
	stop_rt_timer();
        rt_task_delete(NULL);
	rt_printk("END SCHEDULER TEST WITH SRQs.\n\n");
	return 0;
}

void __do_atexit( void )
{
	func_ptr *p = atexit_function_tab;

	rt_printk("__do_atexit START\n");

	// we check if the pointer is NULL, this
	// can happen when a second C++ modules is loaded
	// and than unloaded. At unload time the second module
	// will call the atexit handlers and delete
	// the function table
	if( atexit_function_tab == 0 ){
		rt_printk("atexit_function_tab == NULL\n");
		return;
	}
	
	while(*p)
	{
		rt_printk("calling atexit function %p\n",p );
		(*(p))();
		p++;
		if( atexit_function_tab_len-- )
			break;	
	}

	vfree( atexit_function_tab );

 	atexit_function_tab_size = 0;
 	atexit_function_tab_len = 0;
 	atexit_function_tab = 0;
 			
	rt_printk("__do_atexit END\n");	
}

int __init hello_init(void)
{
	RTIME sampling;

	rt_printk(KERN_INFO "TESTE - *************************** INICIO *******************************\n");

	rt_task_init(&Task_1, signalIchi, 0, 3000, 0, 0, NULL);
	rt_task_init(&Task_2,   signalNi, 0, 3000, 0, 0, NULL);
	rt_task_init(&Task_3,  signalSan, 0, 3000, 0, 0, NULL);

	sampling = start_rt_timer(nano2count(TICK));

	rt_sem_init(&rMutex, 1);

	rt_task_make_periodic(&Task_1, rt_get_time() + sampling * 16, sampling * 16);
	rt_task_make_periodic(&Task_2, rt_get_time() + sampling * 18, sampling * 18);
	rt_task_make_periodic(&Task_3, rt_get_time() + sampling * 30, sampling * 30);

	rt_change_prio(&Task_1, 2);
	rt_change_prio(&Task_2, 1);
	rt_change_prio(&Task_3, 3);

	rt_printk(KERN_INFO "Init module function\n");
	return 0;
}

void __exit hello_clean(void)
{
  rt_task_delete(&Task_1);
  rt_task_delete(&Task_2);
  rt_task_delete(&Task_3);
  rt_printk(KERN_INFO "Cleanup module function\n");
  rt_printk(KERN_INFO "-----------------------\n");
}

static RTAI_SYSCALL_MODE int _comedi_data_read(void *dev, unsigned int subdev, unsigned int chan, unsigned int range, unsigned int aref, lsampl_t *data)
{
	RTAI_COMEDI_LOCK(dev, subdev);
	rt_printk("COMEDI DATA READ: dev = %p, subdev = %u, chan = %u, range = %u, aref = %x.\n", dev, subdev, chan, range, aref);
	data[0] = 0xabcdea;
	rt_printk("COMEDI DATA READ RETURNS %x.\n", data[0]);
	RTAI_COMEDI_UNLOCK(dev, subdev);
	return 1;
}

static RTAI_SYSCALL_MODE int _comedi_dio_bitfield(void *dev, unsigned int subdev, unsigned int write_mask, unsigned int *bits)
{
	RTAI_COMEDI_LOCK(dev, subdev);
	rt_printk("COMEDI DIO BITFIELD: dev = %p, subdev = %u, write_mask = %x, bits = %x.\n", dev, subdev, write_mask, bits[0]);
	bits[0] |= 0xFFFFFFFF;
	rt_printk("COMEDI DIO BITFIELD RETURNS %x.\n", bits[0]);
	RTAI_COMEDI_UNLOCK(dev, subdev);
	return 1;
}