comedi_rt_timer.c

rtlinux-3.2-pre3.tar.bz2 rtlinux3.2-pre3的源代码

	// every comedi_cmd causes one execution of while loop
	while(1){
		devpriv->rt_task_active = 1;
		devpriv->scan_task_active = 1;
		devpriv->start = rt_get_time();

		for(n = 0; n < cmd->stop_arg || cmd->stop_src == TRIG_NONE; n++){
			// scan timing
			if(n)
				rt_task_wait_period();
			if(devpriv->scan_task_active == 0){
				goto cleanup;
			}
			ret = rt_task_make_periodic(devpriv->scan_task,
				devpriv->start + devpriv->scan_period * n,
				devpriv->convert_period);
			if(ret < 0){
				comedi_error(dev, "bug!");
			}
		}

cleanup:

		devpriv->rt_task_active = 0;
		// suspend until next comedi_cmd
		rt_task_suspend(devpriv->rt_task);
	}
}

static int timer_insn(comedi_device *dev,comedi_subdevice *s,
	comedi_insn *insn,lsampl_t *data)
{
	comedi_insn xinsn = *insn;

	xinsn.data = data;
	xinsn.subdev = devpriv->subd;

	return comedi_do_insn(devpriv->device,&xinsn);
}

static int cmdtest_helper(comedi_cmd *cmd,
	unsigned int start_src,
	unsigned int scan_begin_src,
	unsigned int convert_src,
	unsigned int scan_end_src,
	unsigned int stop_src)
{
	int err = 0;
	int tmp;

	tmp = cmd->start_src;
	cmd->start_src &= start_src;
	if(!cmd->start_src || tmp!=cmd->start_src)err++;

	tmp = cmd->scan_begin_src;
	cmd->scan_begin_src &= scan_begin_src;
	if(!cmd->scan_begin_src || tmp!=cmd->scan_begin_src)err++;

	tmp = cmd->convert_src;
	cmd->convert_src &= convert_src;
	if(!cmd->convert_src || tmp!=cmd->convert_src)err++;

	tmp = cmd->scan_end_src;
	cmd->scan_end_src &= scan_end_src;
	if(!cmd->scan_end_src || tmp!=cmd->scan_end_src)err++;

	tmp = cmd->stop_src;
	cmd->stop_src &= stop_src;
	if(!cmd->stop_src || tmp!=cmd->stop_src)err++;

	return err;
}

static int timer_cmdtest(comedi_device *dev,comedi_subdevice *s,comedi_cmd *cmd)
{
	int err = 0;
	unsigned int start_src = 0;

	if(s->type == COMEDI_SUBD_AO)
		start_src = TRIG_INT;
	else
		start_src = TRIG_NOW;

	err = cmdtest_helper(cmd,
		start_src,	/* start_src */
		TRIG_TIMER | TRIG_FOLLOW,	/* scan_begin_src */
		TRIG_NOW | TRIG_TIMER,	/* convert_src */
		TRIG_COUNT,	/* scan_end_src */
		TRIG_COUNT | TRIG_NONE);	/* stop_src */
	if(err)return 1;

	/* step 2: make sure trigger sources are unique and mutually
	 * compatible */

	if(cmd->start_src != TRIG_NOW &&
		cmd->start_src != TRIG_INT)
		err++;
	if(cmd->scan_begin_src != TRIG_TIMER &&
		cmd->scan_begin_src != TRIG_FOLLOW)
		err++;
	if(cmd->convert_src != TRIG_TIMER &&
		cmd->convert_src != TRIG_NOW)
		err++;
	if(cmd->stop_src != TRIG_COUNT &&
		cmd->stop_src != TRIG_NONE)
		err++;
	if(cmd->scan_begin_src == TRIG_FOLLOW && cmd->convert_src != TRIG_TIMER)
		err++;
	if(cmd->convert_src == TRIG_NOW && cmd->scan_begin_src != TRIG_TIMER)
		err++;

	if(err)return 2;

	/* step 3: make sure arguments are trivially compatible */
	// limit frequency, this is fairly arbitrary
	if(cmd->scan_begin_src == TRIG_TIMER){
		if(cmd->scan_begin_arg<SPEED_LIMIT){
			cmd->scan_begin_arg=SPEED_LIMIT;
			err++;
		}
	}
	if(cmd->convert_src == TRIG_TIMER){
		if(cmd->convert_arg<SPEED_LIMIT){
			cmd->convert_arg=SPEED_LIMIT;
			err++;
		}
	}
	// make sure conversion and scan frequencies are compatible
	if(cmd->convert_src == TRIG_TIMER && cmd->scan_begin_src == TRIG_TIMER){
		if(cmd->convert_arg * cmd->scan_end_arg > cmd->scan_begin_arg){
			cmd->scan_begin_arg = cmd->convert_arg * cmd->scan_end_arg;
			err++;
		}
	}
	if(err)return 3;

	/* step 4: fix up and arguments */
	if(err)return 4;

	return 0;
}

static int timer_cmd(comedi_device *dev,comedi_subdevice *s)
{
	int ret;
	comedi_cmd *cmd = &s->async->cmd;

	/* hack attack: drivers are not supposed to do this: */
	dev->rt = 1;

	// make sure tasks have finished cleanup of last comedi_cmd
	if(devpriv->rt_task_active || devpriv->scan_task_active)
		return -EBUSY;

	ret = comedi_lock(devpriv->device,devpriv->subd);
	if(ret<0)
	{
		comedi_error(dev, "failed to obtain lock");
		return ret;
	}
	switch(cmd->scan_begin_src){
		case TRIG_TIMER:
			devpriv->scan_period = nano2count(cmd->scan_begin_arg);
			break;
		case TRIG_FOLLOW:
			devpriv->scan_period = nano2count(cmd->convert_arg * cmd->scan_end_arg);
			break;
		default:
			comedi_error(dev, "bug setting scan period!");
			return -1;
			break;
	}
	switch(cmd->convert_src){
		case TRIG_TIMER:
			devpriv->convert_period = nano2count(cmd->convert_arg);
			break;
		case TRIG_NOW:
			devpriv->convert_period = 1;
			break;
		default:
			comedi_error(dev, "bug setting conversion period!");
			return -1;
			break;
	}

	if(cmd->start_src == TRIG_NOW)
		return timer_start_cmd(dev, s);

	s->async->inttrig = timer_inttrig;

	return 0;
}

static int timer_inttrig(comedi_device *dev, comedi_subdevice *s, unsigned int trig_num)
{
	if(trig_num != 0)
		return -EINVAL;

	s->async->inttrig = NULL;

	return timer_start_cmd(dev, s);
}

static int timer_start_cmd(comedi_device *dev, comedi_subdevice *s)
{
	comedi_async *async = s->async;
	comedi_cmd *cmd = &async->cmd;
	RTIME now, delay, period;
	int ret;

	devpriv->stop = 0;
	s->async->events = 0;


	if(cmd->start_src == TRIG_NOW)
		delay = nano2count(cmd->start_arg);
	else
		delay = 0;

	now=rt_get_time();
	/* Using 'period' this way gets around some weird bug in gcc-2.95.2
	 * that generates the compile error 'internal error--unrecognizable insn'
	 * when rt_task_make_period() is called (observed with rtlinux-3.1, linux-2.2.19).
	 *  - fmhess */
	period = devpriv->scan_period;
	ret = rt_task_make_periodic(devpriv->rt_task, now
		+ delay, period);
	if(ret < 0)
	{
		comedi_error(dev, "error starting rt_task");
		return ret;
	}
	return 0;
}

static int timer_attach(comedi_device *dev,comedi_devconfig *it)
{
	int ret;
	comedi_subdevice *s, *emul_s;
	comedi_device *emul_dev;
	/* These should probably be devconfig options[] */
	const int timer_priority = 4;
	const int scan_priority = timer_priority + 1;
	char path[20];

	printk("comedi%d: timer: ",dev->minor);

	dev->board_name="timer";

	if((ret=alloc_subdevices(dev, 1))<0)
		return ret;
	if((ret=alloc_private(dev,sizeof(timer_private)))<0)
		return ret;

	sprintf(path,"/dev/comedi%d",it->options[0]);
	devpriv->device = comedi_open(path);
	devpriv->subd=it->options[1];

	printk("emulating commands for minor %i, subdevice %d\n", it->options[0], devpriv->subd);

	emul_dev = devpriv->device;
	emul_s = emul_dev->subdevices+devpriv->subd;

	// input or output subdevice
	s=dev->subdevices+0;
	s->type=emul_s->type;
	s->subdev_flags = emul_s->subdev_flags; /* SDF_GROUND (to fool check_driver) */
	s->n_chan=emul_s->n_chan;
	s->len_chanlist=1024;
	s->do_cmd=timer_cmd;
	s->do_cmdtest=timer_cmdtest;
	s->cancel=timer_cancel;
	s->maxdata=emul_s->maxdata;
	s->range_table=emul_s->range_table;
	s->range_table_list=emul_s->range_table_list;
	switch(emul_s->type){
	case COMEDI_SUBD_AI:
		s->insn_read=timer_insn;
		dev->read_subdev = s;
		devpriv->io_function = timer_data_read;
		break;
	case COMEDI_SUBD_AO:
		s->insn_write=timer_insn;
		s->insn_read=timer_insn;
		dev->write_subdev = s;
		devpriv->io_function = timer_data_write;
		break;
	case COMEDI_SUBD_DIO:
		s->insn_write=timer_insn;
		s->insn_read=timer_insn;
		s->insn_bits=timer_insn;
		dev->read_subdev = s;
		devpriv->io_function = timer_dio_read;
		break;
	default:
		comedi_error(dev, "failed to determine subdevice type!");
		return -EINVAL;
	}

	rt_set_oneshot_mode();
	start_rt_timer( 1 );
	devpriv->timer_running = 1;

	devpriv->rt_task = kmalloc(sizeof(RT_TASK),GFP_KERNEL);
	memset(devpriv->rt_task,0,sizeof(RT_TASK));

	// initialize real-time tasks
	ret = rt_task_init(devpriv->rt_task, timer_task_func,(int)dev, 3000,
		timer_priority, 0, 0);
	if(ret < 0) 	{
		comedi_error(dev, "error initalizing rt_task");
		kfree(devpriv->rt_task);
		devpriv->rt_task = 0;
		return ret;
	}

	devpriv->scan_task = kmalloc(sizeof(RT_TASK),GFP_KERNEL);
	memset(devpriv->scan_task,0,sizeof(RT_TASK));

	ret = rt_task_init(devpriv->scan_task, scan_task_func,
		(int)dev, 3000, scan_priority, 0, 0);
	if(ret < 0){
		comedi_error(dev, "error initalizing scan_task");
		kfree(devpriv->scan_task);
		devpriv->scan_task = 0;
		return ret;
	}

	return 1;
}

// free allocated resources
static int timer_detach(comedi_device *dev)
{
	printk("comedi%d: timer: remove\n",dev->minor);

	if(devpriv){
		if(devpriv->rt_task){
			rt_task_delete(devpriv->rt_task);
			kfree(devpriv->rt_task);
		}
		if(devpriv->scan_task){
			rt_task_delete(devpriv->scan_task);
			kfree(devpriv->scan_task);
		}
		if( devpriv->timer_running )
			stop_rt_timer();
	}

	return 0;
}