rt_task_wait_period hangs system
Paul Pfeiffer <[email protected]> Thu, 15 Mar 2018 12:46:52 +0100
| Newsgroups | gmane.linux.real-time.rtai |
|---|---|
| Message-ID | <[email protected]> |
This is a multi-part message in MIME format. --------------BA32ED73923B6B7D614D483C Content-Type: text/plain; charset=utf-8; format=flowed Content-Transfer-Encoding: quoted-printable Dear RTAI community, after installation of RTAI 5.1 and running the latency tests, I am=20 trying to run production code, which leads to a system crash. Debgugging=20 lead me to conclude that it is the call to rt_task_wait_period, that=20 produced the crash. System info: RTAi 5.1, Kernel 4.9.76, Intel i5-6600K In detail, the call happens in a custom kernel module, that first=20 chooses between oneshot and periodic mode (both lead to the same crash) ``` #ifdef ONESHOT_MODE =C2=A0 rt_set_oneshot_mode(); =C2=A0 start_rt_timer(1); #else =C2=A0 rt_set_periodic_mode(); =C2=A0 #endif ``` =C2=A0registers a real time task ``` rt_linux_use_fpu( usesFPU );=C2=A0=C2=A0=C2=A0=C2=A0=C2=A0 /* declare if = we use the FPU */ retVal =3D rt_task_init( &dynClampTask.rtTask, dynclamp_loop, dummy,=20 stackSize, priority, usesFPU, signal ); ``` and then makes it periodic ``` rt_task_make_periodic( &dynClampTask.rtTask, rt_get_time() +=20 periodTicks, periodTicks ) ``` Both of this calls are successful. In the task dynclamp_loop, the only calls to RTAI are ``` rt_get_cpu_time_in_ns() ``` which poses no problem and ``` rt_task_wait_period() ``` which kills the system.=C2=A0 This has worked in previous RTAI versions a= nd=20 on other machines, so we were wondering whether this a 5.1 specific=20 problem or related to our new machine? For completeness, I attach the=20 source code of the kernel module, the functions of interest are=20 dynclamp_loop, init_dynclamp_loop and cleanup_dynclamp_loop. Any ideas? Maybe related, I have recently been reporting my diffuculties installing=20 RTAI 5.1, especially the system hig up when rtai_sched was loaded.=20 Thanks to the help of Paolo, see thread "RTAI in debian 9", this problem=20 was fixed via manual generation of a calibration file. Sorry, for the long question, but we are really eager to use the latest=20 RTAI version and would like to understand this problem. Best, Paul --------------BA32ED73923B6B7D614D483C Content-Type: text/x-csrc; name="dynclampmodule.c" Content-Transfer-Encoding: 7bit Content-Disposition: attachment; filename="dynclampmodule.c" #include <linux/module.h> #include <linux/init.h> #include <linux/kernel.h> #include <linux/version.h> #include <asm/uaccess.h> #include <linux/kfifo.h> #include <linux/mutex.h> #include <linux/slab.h> #include <linux/fs.h> #include <linux/cdev.h> #include <linux/delay.h> #include <rtai.h> #include <rtai_sched.h> #include <linux/comedilib.h> #include "moduledef.h" #ifdef ENABLE_COMPUTATION #ifdef ENABLE_MATHH #include <rtai_math.h> #endif #endif MODULE_LICENSE( "GPL" ); MODULE_DESCRIPTION( "Dynamic clamp for RELACS" ); MODULE_AUTHOR( "Jan Benda <[email protected]>" ); /////////////////////////////////////////////////////////////////////////////// // *** TYPE DEFINITIONS *** /////////////////////////////////////////////////////////////////////////////// // DAQ-DEVICES: struct chanT { int subdev; unsigned int chan; int param; int modelIndex; int statusIndex; int isUsed; // used by analog output to indicate channels that get data from user space comedi_insn insn; lsampl_t lsample; lsampl_t maxdata; float minvoltage; float maxvoltage; struct converterT converter; float scale; float value; float prevvalue; int trigger; float alevel; }; struct subdeviceT { int subdev; enum subdevTypes type; DECLARE_KFIFO_PTR( fifo, float ); unsigned int frequency; long delay; long duration; // => relative to index of dynclamp-Task int continuous; int startsource; int used; int prepared; // set to 1 by loadSyncCommand int running; // 1 for running, 0 not running, or E_COMEDI, E_NODATA, ... int pending; // set to 1 by startSubdevice(), dynclamp_loop sets it to 0 unsigned int chanN; struct chanT chanlist[MAXCHANLIST]; }; struct triggerT { int enabled; int subdev; unsigned int chan; float alevel; }; // RTAI TASK: struct dynClampTaskT { RT_TASK rtTask; int inuse; unsigned int period; // ns unsigned int frequency; unsigned long duration; int running; unsigned long loopCnt; long aoIndex; }; /////////////////////////////////////////////////////////////////////////////// // *** GLOBAL VARIABLES *** /////////////////////////////////////////////////////////////////////////////// struct cdev *rtcdev; struct mutex mutex; int features = 0; // DAQ-DEVICES: comedi_t *device; char devname[DEV_NAME_MAXLEN+1]; int subdevices[MAXSUBDEV]; int subdevN = 0; struct subdeviceT aisubdev; struct subdeviceT aosubdev; int reqCloseSubdev = -1; struct triggerT trigger; char statusInputNames[MAXCHANLIST][PARAM_NAME_MAXLEN]; char statusInputUnits[MAXCHANLIST][PARAM_NAME_MAXLEN]; float statusInput[MAXCHANLIST]; int statusInputN = 0; int intervalstatusinx = 0; #ifdef ENABLE_AITIME int aitimestatusinx = 0; #endif #ifdef ENABLE_AIACQUISITIONTIME int aiacquisitiontimestatusinx = 0; #endif #ifdef ENABLE_AOTIME int aotimestatusinx = 0; #endif #ifdef ENABLE_MODELTIME int modeltimestatusinx = 0; #endif #ifdef ENABLE_WAITTIME int waittimestatusinx = 0; #endif #ifdef ENABLE_COMPUTATION int outputstatusinx = 0; #endif int traceIndex = 0; #ifdef ENABLE_COMPUTATION int inputChanIndex = 0; int outputChanIndex = 0; #ifdef ENABLE_LOOKUPTABLES int lookupinx = 0; int lookupn[MAXLOOKUPTABLES]; float* lookupx[MAXLOOKUPTABLES]; float* lookupy[MAXLOOKUPTABLES]; #endif #endif // RTAI TASK: struct dynClampTaskT dynClampTask; // synchronization pulses: #ifdef ENABLE_SYNCSEC int syncSECMode = 0; /* 0: fixed period, 1: difftime, n>1: average over n difftime. If < 0 disable generation of pulses. */ float syncSECPulse = 0.0; /*! Length of the inject pulse generated by the SEC in ns. */ unsigned int syncSECMask = 0; #endif // TTL pulse generation: #ifdef ENABLE_TTLPULSE #define ENABLE_DIOINLOOP comedi_insn ttlStartWriteInsn; comedi_insn ttlEndWriteInsn; comedi_insn ttlStartReadInsn; comedi_insn ttlEndReadInsn; comedi_insn ttlStartAOInsn; comedi_insn ttlEndAOInsn; comedi_insn *ttlInsns[MAXTTLPULSETYPES]; lsampl_t ttlStartWriteData[2]; lsampl_t ttlEndWriteData[2]; lsampl_t ttlStartReadData[2]; lsampl_t ttlEndReadData[2]; lsampl_t ttlStartAOData[2]; lsampl_t ttlEndAOData[2]; #endif // Digital IO #ifdef ENABLE_DIOINLOOP comedi_insn dioInsn; lsampl_t dioData[2]; int diorunning; int dioerror; #endif // for debug: char *iocNames[RTMODULE_IOC_MAXNR] = { "dummy", "IOC_OPEN_SUBDEV", "IOC_CHANLIST", "IOC_SYNC_CMD", "IOC_START_SUBDEV", "IOC_CHK_RUNNING", "IOC_REQ_CLOSE", "IOC_STOP_SUBDEV", "IOC_DIO_CMD", "IOC_SET_TRIGGER", "IOC_UNSET_TRIGGER", "IOC_GET_TRACE_INFO", "IOC_SET_TRACE_CHANNEL", "IOC_GETRATE", "IOC_GETLOOPCNT", "IOC_GETAOINDEX", "IOC_SET_LOOKUP_K", "IOC_SET_LOOKUP_N", "IOC_SET_LOOKUP_X", "IOC_SET_LOOKUP_Y", "IOC_CHECK_FEATURES" }; /////////////////////////////////////////////////////////////////////////////// // *** PROTOTYPES *** /////////////////////////////////////////////////////////////////////////////// int dynclampmodule_open( struct inode *devFile, struct file *fModule ); int dynclampmodule_close( struct inode *devFile, struct file *fModule ); ssize_t dynclampmodule_read( struct file *devFile, char *buffer, size_t n, loff_t *pos ); ssize_t dynclampmodule_write( struct file *devFile, const char *buffer, size_t n, loff_t *pos ); #ifdef HAVE_UNLOCKED_IOCTL long dynclampmodule_unlocked_ioctl( struct file *fModule, unsigned int cmd, unsigned long arg ); #else int dynclampmodule_ioctl( struct inode *devFile, struct file *fModule, unsigned int cmd, unsigned long arg ); #endif static struct file_operations fops = { .owner = THIS_MODULE, .read = dynclampmodule_read, .write = dynclampmodule_write, #ifdef HAVE_UNLOCKED_IOCTL .unlocked_ioctl = dynclampmodule_unlocked_ioctl, #else .ioctl = dynclampmodule_ioctl, #endif .open = dynclampmodule_open, .release = dynclampmodule_close, }; int openComediDevice( struct deviceIOCT *deviceIOC ); int openComediSubDevice( const char *devicename, int subdev, char *errorstr ); void releaseSubdevice( int subdev ); void releaseAnalogSubdevice( struct subdeviceT *subdev ); int loadChanList( struct chanlistIOCT *chanlistIOC, struct subdeviceT *subdev ); int loadSyncCmd( struct syncCmdIOCT *syncCmdIOC, struct subdeviceT *subdev ); int startSubdevice( struct subdeviceT *subdev ); int stopSubdevice( struct subdeviceT *subdev ); void dynclamp_loop( long dummy ); int init_dynclamp_loop( void ); void cleanup_dynclamp_loop( void ); int writeDIO( struct dioIOCT *dioIOC ); int setDigitalIO( struct dioIOCT *dioIOC ); int setAnalogTrigger( struct triggerIOCT *triggerIOC ); int unsetAnalogTrigger( struct triggerIOCT *triggerIOC ); /////////////////////////////////////////////////////////////////////////////// // *** MODEL INCLUDE *** /////////////////////////////////////////////////////////////////////////////// #ifdef ENABLE_COMPUTATION #include "model.c" float origParamOutput[PARAMOUTPUT_N]; #endif /////////////////////////////////////////////////////////////////////////////// // *** HELPER FUNCTIONS *** /////////////////////////////////////////////////////////////////////////////// #include "dynclampfeatures.h" void init_globals( void ) { #ifdef ENABLE_COMPUTATION int k; char name[PARAM_NAME_MAXLEN]; #endif #ifdef ENABLE_TTLPULSE int i; int j; #endif device = 0; memset( subdevices, 0, sizeof(subdevices) ); subdevN = 0; memset( &aisubdev, 0, sizeof(aisubdev) ); memset( &aosubdev, 0, sizeof(aosubdev) ); aisubdev.subdev = -1; aosubdev.subdev = -1; reqCloseSubdev = -1; memset( &dynClampTask, 0, sizeof(struct dynClampTaskT ) ); statusInputN = 0; intervalstatusinx = statusInputN; statusInputN++; strcpy( statusInputNames[intervalstatusinx], "Interval" ); strcpy( statusInputUnits[intervalstatusinx], "s" ); statusInput[intervalstatusinx] = 0.0; #ifdef ENABLE_AITIME aitimestatusinx = statusInputN; statusInputN++; strcpy( statusInputNames[aitimestatusinx], "AI-time" ); strcpy( statusInputUnits[aitimestatusinx], "s" ); statusInput[aitimestatusinx] = 0.0; #endif #ifdef ENABLE_AIACQUISITIONTIME aiacquisitiontimestatusinx = statusInputN; statusInputN++; strcpy( statusInputNames[aiacquisitiontimestatusinx], "AI-acquisition-time" ); strcpy( statusInputUnits[aiacquisitiontimestatusinx], "s" ); statusInput[aiacquisitiontimestatusinx] = 0.0; #endif #ifdef ENABLE_AOTIME aotimestatusinx = statusInputN; statusInputN++; strcpy( statusInputNames[aotimestatusinx], "AO-time" ); strcpy( statusInputUnits[aotimestatusinx], "s" ); statusInput[aotimestatusinx] = 0.0; #endif #ifdef ENABLE_MODELTIME modeltimestatusinx = statusInputN; statusInputN++; strcpy( statusInputNames[modeltimestatusinx], "Model-time" ); strcpy( statusInputUnits[modeltimestatusinx], "s" ); statusInput[modeltimestatusinx] = 0.0; #endif #ifdef ENABLE_WAITTIME waittimestatusinx = statusInputN; statusInputN++; strcpy( statusInputNames[waittimestatusinx], "Wait-time" ); strcpy( statusInputUnits[waittimestatusinx], "s" ); statusInput[waittimestatusinx] = 0.0; #endif #ifdef ENABLE_COMPUTATION outputstatusinx = statusInputN; for ( k=0; k<OUTPUT_N; k++ ) { sprintf( name, "Stimulus-%s", outputNames[k] ); strcpy( statusInputNames[statusInputN], name ); strcpy( statusInputUnits[statusInputN], outputUnits[k] ); statusInput[statusInputN] = 0.0; statusInputN++; sprintf( name, "Model-%s", outputNames[k] ); strcpy( statusInputNames[statusInputN], name ); strcpy( statusInputUnits[statusInputN], outputUnits[k] ); statusInput[statusInputN] = 0.0; statusInputN++; sprintf( name, "Total-%s", outputNames[k] ); strcpy( statusInputNames[statusInputN], name ); strcpy( statusInputUnits[statusInputN], outputUnits[k] ); statusInput[statusInputN] = 0.0; statusInputN++; #ifdef ENABLE_SYNCSEC sprintf( name, "Injected-%s", outputNames[k] ); strcpy( statusInputNames[statusInputN], name ); strcpy( statusInputUnits[statusInputN], outputUnits[k] ); statusInput[statusInputN] = 0.0; statusInputN++; #endif } #endif #ifdef ENABLE_COMPUTATION traceIndex = 0; inputChanIndex = 0; outputChanIndex = 0; #ifdef ENABLE_LOOKUPTABLES lookupinx = 0; for ( k=0; k<MAXLOOKUPTABLES; k++ ) { lookupn[k] = 0; if ( lookupx[lookupinx] != NULL ) { vfree( lookupx[lookupinx] ); lookupx[lookupinx] = NULL; } if ( lookupy[lookupinx] != NULL ) { vfree( lookupy[lookupinx] ); lookupy[lookupinx] = NULL; } } #endif #endif #ifdef ENABLE_DIOINLOOP memset( &dioInsn, 0, sizeof(comedi_insn) ); dioInsn.insn = INSN_BITS; dioInsn.n = 2; dioInsn.data = dioData; for ( i=0; i<2; i++ ) dioInsn.data[i] = 0; diorunning = 0; dioerror = 0; #endif #ifdef ENABLE_TTLPULSE ttlInsns[0] = &ttlStartWriteInsn; ttlInsns[1] = &ttlEndWriteInsn; ttlInsns[2] = &ttlStartReadInsn; ttlInsns[3] = &ttlEndReadInsn; ttlInsns[4] = &ttlStartAOInsn; ttlInsns[5] = &ttlEndAOInsn; for ( j=0; j<MAXTTLPULSETYPES; j++ ) { memset( ttlInsns[j], 0, sizeof(comedi_insn) ); ttlInsns[j]->insn = INSN_BITS; ttlInsns[j]->n = 2; } ttlStartWriteInsn.data = ttlStartWriteData; ttlEndWriteInsn.data = ttlEndWriteData; ttlStartReadInsn.data = ttlStartReadData; ttlEndReadInsn.data = ttlEndReadData; ttlStartAOInsn.data = ttlStartAOData; ttlEndAOInsn.data = ttlEndAOData; for ( j=0; j<MAXTTLPULSETYPES; j++ ) { for ( i=0; i<2; i++ ) ttlInsns[j]->data[i] = 0; } #endif #ifdef ENABLE_SYNCSEC syncSECMask = 0; syncSECMode = -1; syncSECPulse = 0.0; #endif } /////////////////////////////////////////////////////////////////////////////// // *** DAQ FUNCTIONS *** /////////////////////////////////////////////////////////////////////////////// int openComediDevice( struct deviceIOCT *deviceIOC ) { struct subdeviceT *subdev; int retval; if ( deviceIOC->subdevType == SUBDEV_IN || deviceIOC->subdevType == SUBDEV_OUT ) { if ( deviceIOC->subdevType == SUBDEV_OUT ) { if ( aisubdev.used == 0 ) { sprintf( deviceIOC->errorstr, "cannot open analog output subdevice on device %s because analog input is not opened yet.", devname ); ERROR_MSG( "openComediDevice ERROR: %s\n", deviceIOC->errorstr ); return -EFAULT; } subdev = &aosubdev; } else subdev = &aisubdev; if ( subdev->used ) { sprintf( deviceIOC->errorstr, "cannot open subdevice %d on device %s because it is already in use.", deviceIOC->subdev, devname ); ERROR_MSG( "openComediDevice ERROR: %s\n", deviceIOC->errorstr ); return -EBUSY; } // initialize subdevice structure: memset( subdev, 0, sizeof(struct subdeviceT) ); subdev->used = 1; subdev->subdev = deviceIOC->subdev; subdev->type = deviceIOC->subdevType; subdev->delay = -1; subdev->duration = -1; subdev->startsource = 0; subdev->running = 0; subdev->chanN = 0; } retval = openComediSubDevice( deviceIOC->devicename, deviceIOC->subdev, deviceIOC->errorstr ); return retval; } int openComediSubDevice( const char *devicename, int subdev, char *errorstr ) { int iS; // add to list (on error releaseSubdevice needs to be able to remove it from list!): if ( subdevN == MAXSUBDEV ) { sprintf( errorstr, "maximum number %d of supported subdevices exceeded for comedi subdevice %i on device %s.", MAXSUBDEV, subdev, devicename ); ERROR_MSG( "openComediSubDevice: %s\n", errorstr ); return -ENOMEM; } subdevices[subdevN] = subdev; subdevN++; if ( device ) { if ( strcmp( devicename, devname ) == 0 ) { DEBUG_MSG( "openComediSubDevice: device %s is opened already\n", devname ); } else { sprintf( errorstr, "a different comedi device, %s, is opened already.", devname ); ERROR_MSG( "openComediSubDevice: %s\n", errorstr ); return -EINVAL; } } if ( ! device ) { // open comedi device: device = comedi_open( devicename ); if ( ! device ) { sprintf( errorstr, "failed to open comedi device %s.", devicename ); ERROR_MSG( "openComediSubDevice: %s\n", errorstr ); comedi_perror( "dynclampmodule: comedi_open" ); return -EFAULT; } strncpy( devname, devicename, DEV_NAME_MAXLEN ); DEBUG_MSG( "openComediSubDevice: opened device %s\n", devname ); } // check subdev index: if ( subdev < 0 || subdev >= comedi_get_n_subdevices( device ) ) { sprintf( errorstr, "invalid comedi subdevice %i on device %s requested.", subdev, devicename ); ERROR_MSG( "openComediSubDevice: %s\n", errorstr ); return -EINVAL; } // subdevice already opened? for ( iS=0; iS<subdevN-1; iS++ ) { if ( subdevices[iS] == subdev ) { sprintf( errorstr, "comedi subdevice %i on device %s already in use.", subdev, devicename ); ERROR_MSG( "openComediSubDevice: %s\n", errorstr ); return -EBUSY; } } // lock requested subdevice: if ( comedi_lock( device, subdev ) != 0 ) { sprintf( errorstr, "failed to lock comedi subdevice %i on device %s.", subdev, devicename ); ERROR_MSG( "openComediSubDevice: %s\n", errorstr ); return -EFAULT; } DEBUG_MSG( "openComediSubDevice: locked subdevice %i on device %s\n", subdev, devname ); return 0; } void releaseSubdevice( int subdev ) { int iS; int subdevinx; #ifdef ENABLE_TTLPULSE int pT; struct dioIOCT dio; #endif if ( subdevN <= 0 ) { ERROR_MSG( "releaseSubdevice ERROR: no subdevices registered!\n" ); return; } subdevinx = -1; for ( iS = 0; iS < subdevN; iS++ ) { if ( subdevices[iS] == subdev ) { subdevinx = iS; break; } } if ( subdevinx < 0 ) { ERROR_MSG( "releaseSubdevice ERROR: subdevice %d not in use!\n", subdev ); return; } DEBUG_MSG( "releaseSubdevice: release subdevice %d on device %s\n", subdev, devname ); // stop analog subdevices: if ( aisubdev.subdev == subdev ) releaseAnalogSubdevice( &aisubdev ); else if ( aosubdev.subdev == subdev ) releaseAnalogSubdevice( &aosubdev ); #ifdef ENABLE_TTLPULSE // remove ttl pulses: for ( pT = 0; pT < MAXTTLPULSETYPES; pT++ ) { if ( ttlInsns[pT]->subdev == subdev && ttlInsns[pT]->data[0] > 0 ) { /* set low: */ dio.subdev = subdev; dio.mask = ttlInsns[pT]->data[0]; dio.bits = 0; writeDIO( &dio ); ttlInsns[pT]->subdev = 0; ttlInsns[pT]->data[0] = 0; ttlInsns[pT]->data[1] = 0; } } #endif // unlock subdevice: if ( device && comedi_unlock( device, subdev ) < 0 ) WARN_MSG( "releaseSubdevice WARNING: failed to unlock subdevice %d on device %s!\n", subdev, devname ); else DEBUG_MSG( "releaseSubdevice: successfully unlocked subdevice %d on device %s!\n", subdev, devname ); // remove from list: for ( iS = subdevinx; iS<subdevN-1; iS++ ) subdevices[iS] = subdevices[iS+1]; subdevN--; DEBUG_MSG( "releaseSubdevice: released subdevice %d, new subdevN=%d\n", subdev, subdevN ); // still some subdevices in use: if ( subdevN > 0 ) return; // otherwise close comedi device: DEBUG_MSG( "releaseSubdevice: release comedi device for last subdev id %d\n", subdev ); if ( device && comedi_close( device ) < 0 ) WARN_MSG( "releaseSubdevice WARNING: failed to close comedi device %s!\n", devname ); else DEBUG_MSG( "releaseSubdevice: successfully closed comedi device %s!\n", devname ); device = 0; devname[0] = '\0'; } void releaseAnalogSubdevice( struct subdeviceT *subdev ) { if ( subdev->running > 0 ) { DEBUG_MSG( "releaseAnalogSubdevice: stop subdevice %d\n", subdev->subdev ); stopSubdevice( subdev ); } // stop periodic task: if ( subdev->type == SUBDEV_IN ) cleanup_dynclamp_loop(); DEBUG_MSG( "releaseAnalogSubdevice: release subdevice %d\n", subdev->subdev ); // free fifo: kfifo_free( &subdev->fifo ); // reset subdevice structure: memset( subdev, 0, sizeof(struct subdeviceT) ); subdev->subdev = -1; } int loadChanList( struct chanlistIOCT *chanlistIOC, struct subdeviceT *subdev ) { int iC, isC; int trig = 0; int notfound = 1; #ifdef ENABLE_COMPUTATION int i; #endif if ( subdev->subdev < 0 || !subdev->used ) { ERROR_MSG( "loadChanList ERROR: First open an appropriate device and subdevice. Chanlist not loaded!\n" ); return -EFAULT; } if ( subdev->running > 0 ) { ERROR_MSG( "loadChanList ERROR: subdevice %i on device %s already running.\n", subdev->subdev, devname ); return -EBUSY; } if ( chanlistIOC->chanlistN > MAXCHANLIST ) { ERROR_MSG( "loadChanList ERROR: Invalid chanlist length for subdevice %i on device %s. Chanlist not loaded!\n", subdev->subdev, devname ); return -ENOMEM; } // check for trigger: if ( trigger.enabled ) { if ( trigger.subdev >= 0 ) { if ( subdev->subdev == trigger.subdev ) trig = 1; } else { if ( subdev->type == SUBDEV_IN ) trig = 1; } DEBUG_MSG( "loadChanList: checked for trigger on subdevice %d: %d\n", subdev->subdev, trig ); } if ( subdev->chanN > 0 ) { // subdev chanlist already exist: for ( iC = 0; iC < chanlistIOC->chanlistN; iC++ ) { notfound = 1; for ( isC = 0; isC < subdev->chanN; isC++ ) { if ( CR_CHAN(chanlistIOC->chanlist[iC]) == subdev->chanlist[isC].chan + PARAM_CHAN_OFFSET*subdev->chanlist[isC].param ) { subdev->chanlist[isC].isUsed = 1; if ( trig && subdev->chanlist[iC].chan == trigger.chan ) { DEBUG_MSG( "loadChanList: set trigger for channel %d id %d on subdevice %d with level %d\n", subdev->chanlist[iC].chan, iC, subdev->subdev, (int)(100.0*trigger.alevel) ); subdev->chanlist[iC].trigger = 1; subdev->chanlist[iC].alevel = trigger.alevel; } else { subdev->chanlist[iC].trigger = 0; subdev->chanlist[iC].alevel = 0.0; } subdev->chanlist[isC].scale = chanlistIOC->scalelist[iC]; if ( subdev->chanlist[isC].param == 0 ) { subdev->chanlist[isC].insn.chanspec = chanlistIOC->chanlist[iC]; memcpy( &subdev->chanlist[iC].converter, &chanlistIOC->conversionlist[iC], sizeof(struct converterT) ); } notfound = 0; break; } } if ( notfound ) { ERROR_MSG( "loadChanList ERROR: Channel %d not found on subdevice %i on device %s.\n", CR_CHAN(chanlistIOC->chanlist[iC]), subdev->subdev, devname ); return -EINVAL; } } } else { // create and initialize chanlist for subdevice: memset( subdev->chanlist, 0, sizeof(subdev->chanlist) ); subdev->chanN = chanlistIOC->chanlistN; for ( iC=0; iC < subdev->chanN; iC++ ) { subdev->chanlist[iC].subdev = subdev->subdev; subdev->chanlist[iC].chan = CR_CHAN( chanlistIOC->chanlist[iC] ); subdev->chanlist[iC].lsample = 0; subdev->chanlist[iC].maxdata = chanlistIOC->maxdata[iC]; subdev->chanlist[iC].minvoltage = chanlistIOC->minvoltage[iC]; subdev->chanlist[iC].maxvoltage = chanlistIOC->maxvoltage[iC]; memset( &subdev->chanlist[iC].insn, 0, sizeof(comedi_insn) ); subdev->chanlist[iC].param = subdev->chanlist[iC].chan/PARAM_CHAN_OFFSET; subdev->chanlist[iC].modelIndex = -1; subdev->chanlist[iC].statusIndex = -1; subdev->chanlist[iC].scale = chanlistIOC->scalelist[iC]; subdev->chanlist[iC].isUsed = chanlistIOC->isused[iC]; subdev->chanlist[iC].value = 0.0; subdev->chanlist[iC].prevvalue = 0.0; if ( trig && subdev->chanlist[iC].chan == trigger.chan ) { DEBUG_MSG( "loadChanList: added trigger to channel %d id %d on subdevice %d with level %d\n", subdev->chanlist[iC].chan, iC, subdev->subdev, (int)(100.0*trigger.alevel) ); subdev->chanlist[iC].trigger = 1; subdev->chanlist[iC].alevel = trigger.alevel; } else { subdev->chanlist[iC].trigger = 0; subdev->chanlist[iC].alevel = 0.0; } if ( subdev->chanlist[iC].param > 0 ) { subdev->chanlist[iC].chan %= PARAM_CHAN_OFFSET; memset( &subdev->chanlist[iC].converter, 0, sizeof( struct converterT ) ); } else { if ( subdev->type == SUBDEV_IN ) { subdev->chanlist[iC].insn.insn = INSN_READ; #ifdef ENABLE_COMPUTATION for ( i = 0; i < INPUT_N; i++ ) { if ( inputChannels[i] == subdev->chanlist[iC].chan ) subdev->chanlist[iC].modelIndex = i; } #endif } else { subdev->chanlist[iC].insn.insn = INSN_WRITE; #ifdef ENABLE_COMPUTATION for ( i = 0; i < OUTPUT_N; i++ ) { if ( outputChannels[i] == subdev->chanlist[iC].chan ) { subdev->chanlist[iC].modelIndex = i; #ifdef ENABLE_SYNCSEC subdev->chanlist[iC].statusIndex = outputstatusinx + 4*i; #else subdev->chanlist[iC].statusIndex = outputstatusinx + 3*i; #endif } } #endif } subdev->chanlist[iC].insn.n = 1; subdev->chanlist[iC].insn.data = &subdev->chanlist[iC].lsample; subdev->chanlist[iC].insn.subdev = subdev->subdev; subdev->chanlist[iC].insn.chanspec = chanlistIOC->chanlist[iC]; memcpy( &subdev->chanlist[iC].converter, &chanlistIOC->conversionlist[iC], sizeof(struct converterT) ); } } } return 0; } int loadSyncCmd( struct syncCmdIOCT *syncCmdIOC, struct subdeviceT *subdev ) { int buffersize; int retval; if ( subdev->subdev < 0 || !subdev->used ) { ERROR_MSG( "loadSyncCmd ERROR: first open an appropriate device and subdevice. Sync-command not loaded!\n" ); return -EFAULT; } if ( subdev->running > 0 ) { ERROR_MSG( "loadSyncCmd ERROR: subdevice %i on device %s already running.\n", subdev->subdev, devname ); return -EBUSY; } if ( subdev->chanN <= 0 || !subdev->chanlist ) { ERROR_MSG( "loadSyncCmd ERROR: first load Chanlist for subdevice %i on device %s. Sync-command not loaded!\n", subdev->subdev, devname ); return -EFAULT; } if ( syncCmdIOC->frequency > MAX_FREQUENCY ) { ERROR_MSG( "loadSyncCmd ERROR: requested frequency is above MAX_FREQUENCY (%dHz). Sync-command not loaded!\n", MAX_FREQUENCY ); return -EINVAL; } if ( subdev->type == SUBDEV_IN ) cleanup_dynclamp_loop(); DEBUG_MSG( "loadSyncCmd: initialize sudevice %i with fifo buffer size %i\n", subdev->subdev, kfifo_size( &subdev->fifo ) ); // initialize sampling parameters for subdevice: subdev->frequency = syncCmdIOC->frequency > 0 ? syncCmdIOC->frequency : dynClampTask.frequency; subdev->delay = syncCmdIOC->delay; subdev->duration = syncCmdIOC->duration; subdev->continuous = syncCmdIOC->continuous; subdev->startsource = syncCmdIOC->startsource; subdev->pending = 0; buffersize = syncCmdIOC->buffersize/sizeof( float ); if ( buffersize < 1024 ) buffersize = 1024; if ( kfifo_size( &subdev->fifo ) < buffersize ) { // init fifo: kfifo_free( &subdev->fifo ); DEBUG_MSG( "loadSyncCmd: allocate fifo buffer for %d elements\n", buffersize ); do { retval = kfifo_alloc( &subdev->fifo, buffersize, GFP_KERNEL ); if ( retval == 0 ) break; buffersize /= 2; if ( buffersize < 1024 ) { ERROR_MSG( "loadSyncCmd: failed to allocate fifo for %d elements\n", buffersize*2 ); return -ENOMEM; } } while ( 1 ); } buffersize = kfifo_size( &subdev->fifo ); syncCmdIOC->buffersize = buffersize*sizeof( float ); kfifo_reset( &subdev->fifo ); // test requested sampling-rate and set frequency for dynamic clamp task: if ( !dynClampTask.running ) { dynClampTask.frequency = subdev->frequency; } else { if ( dynClampTask.frequency != subdev->frequency ) { ERROR_MSG( "loadSyncCmd ERROR: requested frequency %uHz of subdevice %i on device %s is inconsistent to frequency %uHz of other subdevice. Sync-command not loaded!\n", subdev->frequency, subdev->subdev, devname, dynClampTask.frequency ); return -EINVAL; } } DEBUG_MSG( "loadSyncCmd: loaded %ld samples with startsource %d, buffer size %d elements, and frequency %dHz for subdevice %d\n", subdev->duration, subdev->startsource, buffersize, subdev->frequency, subdev->subdev ); subdev->prepared = 1; return 0; } int startSubdevice( struct subdeviceT *subdev ) { int retVal = 0; if ( !subdev->prepared ) { ERROR_MSG( "startSubdevice ERROR: subdevice %i on device %s not prepared.\n", subdev->subdev, devname ); return -EBUSY; } if ( subdev->running > 0 ) { ERROR_MSG( "startSubdevice ERROR: subdevice %i on device %s already running.\n", subdev->subdev, devname ); return -EBUSY; } subdev->pending = 1; if ( !dynClampTask.running && subdev->type == SUBDEV_IN ) { // only ai can start the loop! dynClampTask.aoIndex = 0; dynClampTask.frequency = subdev->frequency; // start dynamic clamp task: retVal = init_dynclamp_loop(); if ( retVal != 0 ) { ERROR_MSG( "startSubdevice: failed to start dynamic clamp loop for subdevice %d. error=%d!\n", subdev->subdev, retVal ); subdev->running = 0; return retVal; } DEBUG_MSG( "startSubdevice: successfully started dynclamp_loop!\n" ); } subdev->running = 1; DEBUG_MSG( "startSubdevice: successfully started subdevice %d type %s!\n", subdev->subdev, subdev->type == SUBDEV_IN ? "AI" : "AO" ); return 0; } int stopSubdevice( struct subdeviceT *subdev ) { int i; int stoppedbyai; // stopping ai stops also ao: if ( subdev->type == SUBDEV_IN ) { stoppedbyai = ( aosubdev.running > 0 ); stopSubdevice( &aosubdev ); if ( stoppedbyai ) aosubdev.running = E_STOPPEDBYAI; } // stop subdevice: DEBUG_MSG( "stopSubdevice: subdevice %d\n", subdev->subdev ); for ( i = 0; i < subdev->chanN; i++ ) subdev->chanlist[i].isUsed = 0; subdev->running = 0; return 0; } int writeDIO( struct dioIOCT *dioIOC ) { unsigned int bit = 0; int retval; #ifdef ENABLE_DIOINLOOP if ( dynClampTask.running ) { if ( diorunning ) { ERROR_MSG( "writeDIO: ERROR digital output is busy" ); return -EBUSY; } dioInsn.subdev = dioIOC->subdev; dioInsn.data[0] = dioIOC->mask; dioInsn.data[1] = dioIOC->bits; diorunning = 1; while ( diorunning && dynClampTask.running ) cpu_relax(); if ( dioerror != 0 || ( ! dynClampTask.running ) ) { dioIOC->bits = 0; retval = dioerror; dioerror = 0; diorunning = 0; if ( retval == 0 ) return -EFAULT; else return retval; } else { dioIOC->bits = dioInsn.data[1]; dioerror = 0; diorunning = 0; } } else { #endif bit = dioIOC->bits; retval = comedi_dio_bitfield( device, dioIOC->subdev, dioIOC->mask, &bit ); if ( retval < 0 ) { dioIOC->bits = 0; comedi_perror( "dynclampmodule: ERROR! writeDIO() -> comedi_dio_bitfield failed" ); return retval; } else dioIOC->bits = bit; #ifdef ENABLE_DIOINLOOP } #endif return 0; } int setDigitalIO( struct dioIOCT *dioIOC ) { int subdevice = dioIOC->subdev; unsigned int bit = 0; int channel = 0; int direction = 0; int retval; #ifdef ENABLE_TTLPULSE int pT = dioIOC->pulseType; int found = 0; #endif if ( dioIOC->op == DIO_CONFIGURE ) { #ifdef ENABLE_DIOINLOOP if ( dynClampTask.running ) { ERROR_MSG( "setDigitalIO: configuring of dio line failed because realtime loop is running!\n" ); return -EBUSY; } #endif bit = 1; for ( channel=0; channel<dioIOC->maxlines; channel++ ) { if ( ( dioIOC->mask & bit ) > 0 ) { direction = COMEDI_INPUT; if ( ( dioIOC->bits & bit ) > 0 ) direction = COMEDI_OUTPUT; retval = comedi_dio_config( device, subdevice, channel, direction ); if ( retval < 0 ) { comedi_perror( "dynclampmodule: ERROR setDigitalIO() -> DIO_CONFIGURE" ); return retval; } } bit <<= 1; } } else if ( dioIOC->op == DIO_WRITE ) { writeDIO( dioIOC ); } else if ( dioIOC->op == DIO_READ ) { #ifdef ENABLE_DIOINLOOP if ( dynClampTask.running ) { dioInsn.subdev = subdevice; dioInsn.data[0] = 0; dioInsn.data[1] = 0; diorunning = 1; while ( dioInsn.data[0] && dynClampTask.running ) cpu_relax(); if ( dioerror != 0 || ( ! dynClampTask.running ) ) { dioIOC->bits = 0; retval = dioerror; dioerror = 0; diorunning = 0; if ( retval == 0 ) return -EFAULT; else return retval; } else { dioIOC->bits = dioInsn.data[1]; dioerror = 0; diorunning = 0; } } else { #endif bit = 0; retval = comedi_dio_bitfield( device, subdevice, 0, &bit ); if ( retval < 0 ) { dioIOC->bits = 0; comedi_perror( "dynclampmodule: ERROR! setDigitalIO() -> DIO_READ failed" ); return retval; } else dioIOC->bits = bit; #ifdef ENABLE_DIOINLOOP } #endif } else if ( dioIOC->op == DIO_ADD_TTLPULSE ) { #ifdef ENABLE_TTLPULSE if ( pT < TTL_START_WRITE || pT >= MAXTTLPULSETYPES ) return -EINVAL; if ( ttlInsns[pT]->data[0] != 0 && ttlInsns[pT]->subdev != subdevice ) { ERROR_MSG( "setDigitalIO: subdevice %d for ttl pulse does not match already used subdevice %d", subdevice, ttlInsns[pT]->subdev ); return -EINVAL; } retval = writeDIO( dioIOC ); if ( retval != 0 ) { ERROR_MSG( "setDigitalIO: failed to write initial TTL pulse" ); return retval; } ttlInsns[pT]->subdev = subdevice; ttlInsns[pT]->data[1] |= dioIOC->bits; ttlInsns[pT]->data[0] |= dioIOC->mask; INFO_MSG( "setDigitalIO: add pulse pT=%d subdev=%u lines=%04x output=%04x\n", pT, ttlInsns[pT]->subdev, ttlInsns[pT]->data[0], ttlInsns[pT]->data[1] ); #else return -ENOTTY; #endif } else if ( dioIOC->op == DIO_CLEAR_TTLPULSE ) { #ifdef ENABLE_TTLPULSE found = 0; for ( pT = 0; pT < MAXTTLPULSETYPES; pT++ ) { if ( ttlInsns[pT]->subdev == subdevice && ( ttlInsns[pT]->data[0] & dioIOC->mask ) > 0 ) { found = 1; ttlInsns[pT]->data[0] &= ~dioIOC->mask; ttlInsns[pT]->data[1] &= ~dioIOC->mask; INFO_MSG( "setDigitalIO: cleared pulse pT=%d subdev=%u mask=%04x\n", pT, ttlInsns[pT]->subdev, dioIOC->mask ); } } if ( found ) { retval = writeDIO( dioIOC ); if ( retval != 0 ) { ERROR_MSG( "setDigitalIO: failed to write clearing TTL pulse" ); return retval; } } #else return -ENOTTY; #endif } else if ( dioIOC->op == DIO_SET_SYNCPULSE ) { #ifdef ENABLE_SYNCSEC if ( dioIOC->pulsewidth <= 0 ) { ERROR_MSG( "setDigitalIO: syncSECPulse %d ns is not positive!\n", dioIOC->pulsewidth ); ERROR_MSG( "setDigitalIO: disabled syncSECPulse.\n" ); return -EINVAL; } if ( dioIOC->intervalmode < 0 ) { ERROR_MSG( "setDigitalIO: invalid intervalmode %d!\n", dioIOC->intervalmode ); ERROR_MSG( "setDigitalIO: disabled syncSECPulse.\n" ); return -EINVAL; } if ( ttlInsns[SYNCSEC_HIGH]->data[0] != 0 && ttlInsns[SYNCSEC_HIGH]->subdev != subdevice ) { ERROR_MSG( "setDigitalIO: subdevice %d for sync pulse high does not match already used subdevice %d", subdevice, ttlInsns[SYNCSEC_HIGH]->subdev ); return -EINVAL; } if ( ttlInsns[SYNCSEC_LOW]->data[0] != 0 && ttlInsns[SYNCSEC_LOW]->subdev != subdevice ) { ERROR_MSG( "setDigitalIO: subdevice %d for sync pulse low does not match already used subdevice %d", subdevice, ttlInsns[SYNCSEC_LOW]->subdev ); return -EINVAL; } /* write sync pulse line: */ syncSECMask = dioIOC->mask; dioIOC->bits = syncSECMask; retval = writeDIO( dioIOC ); if ( retval != 0 ) { ERROR_MSG( "setDigitalIO: failed to write initial sync pulse" ); return retval; } /* turn sync pulses on: */ ttlInsns[SYNCSEC_LOW]->subdev = subdevice; ttlInsns[SYNCSEC_HIGH]->subdev = subdevice; ttlInsns[SYNCSEC_LOW]->data[1] &= ~syncSECMask; /* low */ ttlInsns[SYNCSEC_HIGH]->data[1] |= syncSECMask; /* high */ ttlInsns[SYNCSEC_LOW]->data[0] |= syncSECMask; ttlInsns[SYNCSEC_HIGH]->data[0] |= syncSECMask; /* switch amplifier into synchronized mode: */ if ( dioIOC->modemask > 0 ) { dioIOC->mask = dioIOC->modemask; dioIOC->bits = dioIOC->modebits; retval = writeDIO( dioIOC ); if ( retval != 0 ) { ERROR_MSG( "setDigitalIO: failed to set switching TTL high" ); return retval; } msleep( 3 ); } /* turn sync scaling on: */ syncSECPulse = dioIOC->pulsewidth; syncSECMode = dioIOC->intervalmode; INFO_MSG( "setDigitalIO: initialized sync pulses on device %s subdevice %u. New pulse is %d us with mode %d. High at %d, low at %d with mask %04x\n", devname, subdevice, (int)syncSECPulse, syncSECMode, SYNCSEC_LOW, SYNCSEC_HIGH, syncSECMask ); #else return -ENOTTY; #endif } else if ( dioIOC->op == DIO_CLEAR_SYNCPULSE ) { #ifdef ENABLE_SYNCSEC /* switch amplifier into non-synchronized mode: */ if ( dioIOC->modemask > 0 ) { dioIOC->mask = dioIOC->modemask; dioIOC->bits = dioIOC->modebits; retval = writeDIO( dioIOC ); if ( retval != 0 ) { ERROR_MSG( "setDigitalIO: failed to set switching TTL low" ); return retval; } } /* turn sync scaling off: */ syncSECMode = -1; syncSECPulse = 0.0; if ( syncSECMask > 0 ) { /* turn sync pulses off: */ ttlInsns[SYNCSEC_HIGH]->data[1] &= ~syncSECMask; ttlInsns[SYNCSEC_HIGH]->data[0] &= ~syncSECMask; msleep( 1 ); ttlInsns[SYNCSEC_LOW]->data[1] &= ~syncSECMask; ttlInsns[SYNCSEC_LOW]->data[0] &= ~syncSECMask; /* write sync pulse line low: */ dioIOC->mask = syncSECMask; dioIOC->bits = 0; retval = writeDIO( dioIOC ); if ( retval != 0 ) { ERROR_MSG( "setDigitalIO: failed to write clearing sync pulse" ); return retval; } INFO_MSG( "setDigitalIO: cleared sync pulses on device %s subdevice %u with mask %04x\n", devname, subdevice, syncSECMask ); syncSECMask = 0; } #else return -ENOTTY; #endif } else return -EINVAL; return 0; } int setAnalogTrigger( struct triggerIOCT *triggerIOC ) { #ifdef ENABLE_TRIGGER if ( device ) { if ( strcmp( devname, triggerIOC->devname ) != 0 ) { ERROR_MSG( "setAnalogTrigger: comedi device %s does not match %s of the module\n", triggerIOC->devname, devname ); return -1; } } else { ERROR_MSG( "setAnalogTrigger: no comedi device is opened yet\n" ); return -1; } // disable trigger: trigger.enabled = 0; // setup trigger parameter: trigger.subdev = triggerIOC->subdev; trigger.chan = triggerIOC->channel; trigger.alevel = triggerIOC->alevel; DEBUG_MSG( "setAnalogTrigger: setup trigger for channel %d on subdevice %d\n", trigger.chan, trigger.subdev ); // enable trigger: trigger.enabled = 1; return 0; #else return -EINVAL; #endif } int unsetAnalogTrigger( struct triggerIOCT *triggerIOC ) { #ifdef ENABLE_TRIGGER int iC; // disable trigger: trigger.enabled = 0; for ( iC = 0; iC < aisubdev.chanN; iC++ ) aisubdev.chanlist[iC].trigger = 0; for ( iC = 0; iC < aosubdev.chanN; iC++ ) aosubdev.chanlist[iC].trigger = 0; return 0; #else return -EINVAL; #endif } /////////////////////////////////////////////////////////////////////////////// // *** REAL-TIME TASKS *** /////////////////////////////////////////////////////////////////////////////// static inline void sample_to_value( struct chanT *pChan ) { double term = 1.0; double sample = pChan->lsample - pChan->converter.expansion_origin; unsigned i; pChan->value = 0.0; for ( i=0; i <= pChan->converter.order; ++i ) { pChan->value += pChan->converter.coefficients[i] * term; term *= sample; } pChan->value *= pChan->scale; } static inline float value_to_sample( struct chanT *pChan, float value ) { float outvalue = value; double sample = 0.0; double term = 1.0; unsigned i; value *= pChan->scale; value -= pChan->converter.expansion_origin; for ( i=0; i <= pChan->converter.order; ++i ) { sample += pChan->converter.coefficients[i] * term; term *= value; } if ( sample < 0.0 ) { pChan->lsample = 0; outvalue = pChan->minvoltage / pChan->scale; } else { pChan->lsample = (lsampl_t)sample; // nearbyint(sample) if ( pChan->lsample > pChan->maxdata ) { pChan->lsample = pChan->maxdata; outvalue = pChan->maxvoltage / pChan->scale; } } return outvalue; } /*! Dynamic clamp task */ void dynclamp_loop( long dummy ) { int retVal; int iC; struct chanT *pChan; float outvalue; float aovalues[MAXCHANLIST]; int aonum = 0; int aoinx = 0; float aivalues[MAXCHANLIST]; int failed = 0; RTIME currenttime = 0; RTIME newtime = 0; int difftime = 0; // to avoid __divdi3 issues we use an int here #if defined(ENABLE_AITIME) || defined(ENABLE_AOTIME) || defined(ENABLE_MODELTIME) || defined(ENABLE_WAITTIME) RTIME starttime = 0; RTIME stoptime = 0; int dtime = 0; // to avoid __divdi3 issues we use an int here #endif #ifdef ENABLE_AIACQUISITIONTIME RTIME startsampletime = 0; RTIME stopsampletime = 0; int dsampletime = 0; // to avoid __divdi3 issues we use an int here #endif int triggerevs[5] = { 1, 0, 0, 0, 0 }; int prevtriggerevs[5] = { 0, 0, 0, 0, 0 }; #ifdef ENABLE_SYNCSEC float synctime = dynClampTask.period; float currentfac = 1.0; #endif DEBUG_MSG( "dynclamp_loop: starting dynamic clamp loop at %uHz\n", 1000000000/dynClampTask.period ); dynClampTask.loopCnt = 0; dynClampTask.aoIndex = -1; dynClampTask.running = 1; currenttime = rt_get_cpu_time_ns(); difftime = dynClampTask.period; statusInput[intervalstatusinx] = 1e-9*difftime; #ifdef ENABLE_AITIME statusInput[aitimestatusinx] = 0.0; #endif #ifdef ENABLE_AIACQUISITIONTIME statusInput[aiacquisitiontimestatusinx] = 0.0; #endif #ifdef ENABLE_AOTIME statusInput[aotimestatusinx] = 0.0; #endif #ifdef ENABLE_MODELTIME statusInput[modeltimestatusinx] = 0.0; #endif #ifdef ENABLE_WAITTIME statusInput[waittimestatusinx] = 0.0; #endif #ifdef ENABLE_COMPUTATION // initialize model-specific variables: initModel(); #endif /* Somehow the first time this function waits for nothing... */ rt_task_wait_period(); /**************************************************************************/ /******** LOOP START: *****************************************************/ /**************************************************************************/ while( aisubdev.running > 0 ) { /******** Start read TTL: *******************************************/ #ifdef ENABLE_TTLPULSE if ( ttlStartReadInsn.data[0] > 0 ) { retVal = comedi_do_insn( device, &ttlStartReadInsn ); if ( retVal < 1 ) { if ( retVal < 0 ) comedi_perror( "dynclampmodule: dynclamp_loop ttl pulse at start read: comedi_do_insn" ); ERROR_MSG( "dynclamp_loop: ERROR! failed to write TTL pulses at start read\n" ); } } #endif /******** READ FROM ANALOG INPUT: *******************************************/ /****************************************************************************/ #ifdef ENABLE_AITIME starttime = rt_get_cpu_time_ns(); #endif // ai is always running! if ( aisubdev.pending ) { if ( triggerevs[aisubdev.startsource] && ! prevtriggerevs[aisubdev.startsource] ) { aisubdev.delay = dynClampTask.loopCnt + aisubdev.delay; aisubdev.duration = aisubdev.delay + aisubdev.duration; aisubdev.pending = 0; } } if ( ! aisubdev.pending ) { // check duration: if ( !aisubdev.continuous && aisubdev.chanN > 0 && aisubdev.duration <= dynClampTask.loopCnt ) break; // dynclamp loop // for every ai channel: failed = 0; for ( iC = 0; iC < aisubdev.chanN; iC++ ) { pChan = &aisubdev.chanlist[iC]; // previous sample: pChan->prevvalue = pChan->value; // acquire sample: if ( pChan->param == 0 ) { #ifdef ENABLE_AIACQUISITIONTIME startsampletime = rt_get_cpu_time_ns(); #endif retVal = comedi_do_insn( device, &pChan->insn ); #ifdef ENABLE_AIACQUISITIONTIME stopsampletime = rt_get_cpu_time_ns(); dsampletime = stopsampletime - startsampletime; statusInput[aiacquisitiontimestatusinx] = 1e-9*dsampletime; #endif if ( retVal < 1 ) { stopSubdevice( &aisubdev ); aisubdev.running = E_NODATA; ERROR_MSG( "dynclamp_loop: ERROR! failed to read data from AI subdevice channel %d at loopCnt %lu\n", iC, dynClampTask.loopCnt ); if ( retVal < 0 ) { comedi_perror( "dynclampmodule: dynclamp_loop: comedi_data_read" ); aisubdev.running = E_COMEDI; ERROR_MSG( "dynclamp_loop: ERROR! failed to read from AI subdevice channel %d at loopCnt %lu\n", iC, dynClampTask.loopCnt ); } failed = 1; break; } // convert to value: sample_to_value( pChan ); // sets pChan->value from pChan->lsample #ifdef ENABLE_COMPUTATION if ( pChan->modelIndex >= 0 ) input[pChan->modelIndex] = pChan->value; #endif } else { #ifdef ENABLE_COMPUTATION if ( pChan->param == 1 ) pChan->value = paramInput[pChan->chan]*pChan->scale; else #endif pChan->value = statusInput[pChan->chan]*pChan->scale; } // store channel value: aivalues[iC] = pChan->value; #ifdef ENABLE_TRIGGER // trigger: if ( pChan->trigger ) { prevtriggerevs[1] = triggerevs[1]; if ( pChan->value > pChan->alevel && pChan->prevvalue <= pChan->alevel ) { triggerevs[1] = 1; } else if ( pChan->value < pChan->alevel && pChan->prevvalue >= pChan->alevel ) { triggerevs[1] = 0; } } #endif } // end of channel loop if ( failed ) break; // write analog input values to fifo buffer: if ( kfifo_size( &aisubdev.fifo ) <= 1 ) { // for whatever reason, non-initialized kfifo has size 1! ERROR_MSG( "dynclamp_loop: ERROR! no fifo buffer for AI subdevice at loopCnt %lu\n", dynClampTask.loopCnt ); stopSubdevice( &aisubdev ); aisubdev.running = E_NOMEM; break; // dynclamp loop } retVal = kfifo_in( &aisubdev.fifo, aivalues, aisubdev.chanN ); if ( retVal < aisubdev.chanN ) { ERROR_MSG( "dynclamp_loop: ERROR! fifo overflow for AI subdevice at loopCnt %lu, fifo size=%d, available=%d\n", dynClampTask.loopCnt, kfifo_size( &aisubdev.fifo ), kfifo_avail( &aisubdev.fifo ) ); stopSubdevice( &aisubdev ); aisubdev.running = E_OVERFLOW; break; // dynclamp loop } } // ! pending #ifdef ENABLE_AITIME stoptime = rt_get_cpu_time_ns(); dtime = stoptime - starttime; statusInput[aitimestatusinx] = 1e-9*dtime; #endif /******** End read TTL: *******************************************/ #ifdef ENABLE_TTLPULSE if ( ttlEndReadInsn.data[0] > 0 ) { retVal = comedi_do_insn( device, &ttlEndReadInsn ); if ( retVal < 1 ) { if ( retVal < 0 ) comedi_perror( "dynclampmodule: dynclamp_loop ttl pulse at end read: comedi_do_insn" ); ERROR_MSG( "dynclamp_loop: ERROR! failed to write TTL pulses at end read\n" ); } } #endif /*********** COMPUTE MODEL: *************************************************/ #ifdef ENABLE_MODELTIME starttime = rt_get_cpu_time_ns(); #endif #ifdef ENABLE_COMPUTATION computeModel(); #endif #ifdef ENABLE_MODELTIME stoptime = rt_get_cpu_time_ns(); dtime = stoptime - starttime; statusInput[modeltimestatusinx] = 1e-9*dtime; #endif /******** Start write TTL: *******************************************/ #ifdef ENABLE_TTLPULSE if ( ttlStartWriteInsn.data[0] > 0 ) { retVal = comedi_do_insn( device, &ttlStartWriteInsn ); if ( retVal < 1 ) { if ( retVal < 0 ) comedi_perror( "dynclampmodule: dynclamp_loop ttl pulse at start write: comedi_do_insn" ); ERROR_MSG( "dynclamp_loop: ERROR! failed to write TTL pulses at start write\n" ); } } #endif #ifdef ENABLE_SYNCSEC if ( syncSECMode < 0 ) currentfac = 1.0; else if ( syncSECPulse > 0.0 ) { if ( syncSECMode == 0 ) currentfac = dynClampTask.period / syncSECPulse; else { if ( syncSECMode == 1 ) currentfac = difftime / syncSECPulse; else { // syncSECMode > 1 synctime += (difftime - synctime)/syncSECMode; currentfac = synctime / syncSECPulse; } if ( currentfac < 1.0 ) { DEBUG_MSG( "dynclamp_loop: currentfac=%d < 1! difftime=%d", (int)(100.0*currentfac), difftime ); currentfac = dynClampTask.period / syncSECPulse; } } } else currentfac = 1.0; #endif /******** WRITE TO ANALOG OUTPUT: ******************************************/ /****************************************************************************/ #ifdef ENABLE_AOTIME starttime = rt_get_cpu_time_ns(); #endif if ( aosubdev.running > 0 ) { // check for pending start trigger: if ( aosubdev.pending ) { DEBUG_MSG( "dynclamp_loop: REALTIMELOOP PENDING AO startsrc=%d, prevtriger1=%d, triger1=%d, pv=%d, v=%d\n", aosubdev.startsource, prevtriggerevs[1], triggerevs[1], (int)(100.0*aisubdev.chanlist[0].prevvalue), (int)(100.0*aisubdev.chanlist[0].value) ); if ( triggerevs[aosubdev.startsource] && ! prevtriggerevs[aosubdev.startsource] ) { DEBUG_MSG( "dynclamp_loop: REALTIMELOOP PENDING AO SETUP duration=%lu, delay=%lu, loopCnt=%lu\n", aosubdev.duration, aosubdev.delay, dynClampTask.loopCnt ); aosubdev.delay = dynClampTask.loopCnt + aosubdev.delay; aosubdev.duration = aosubdev.delay + aosubdev.duration; dynClampTask.aoIndex = aosubdev.delay; aosubdev.pending = 0; DEBUG_MSG( "dynclamp_loop: START PENDING AO stop=%lu start=%lu, loopCnt=%lu\n", aosubdev.duration, aosubdev.delay, dynClampTask.loopCnt ); } } if ( ! aosubdev.pending ) { // check end of stimulus: if ( !aosubdev.continuous && aosubdev.duration <= dynClampTask.loopCnt ) { DEBUG_MSG( "dynclamp_loop: finished ao subdevice at loop %lu\n", dynClampTask.loopCnt ); stopSubdevice( &aosubdev ); /******** End AO TTL: *******************************************/ #ifdef ENABLE_TTLPULSE if ( ttlEndAOInsn.data[0] > 0 ) { retVal = comedi_do_insn( device, &ttlEndAOInsn ); if ( retVal < 1 ) { if ( retVal < 0 ) comedi_perror( "dynclampmodule: dynclamp_loop ttl pulse at end ao: comedi_do_insn" ); ERROR_MSG( "dynclamp_loop: ERROR! failed to write TTL pulses at end ao\n" ); } } #endif } else if ( dynClampTask.loopCnt >= aosubdev.delay ) { /******** Start AO TTL: *******************************************/ #ifdef ENABLE_TTLPULSE // start of stimulus: if ( dynClampTask.loopCnt == aosubdev.delay ) { if ( ttlStartAOInsn.data[0] > 0 ) { retVal = comedi_do_insn( device, &ttlStartAOInsn ); if ( retVal < 1 ) { if ( retVal < 0 ) comedi_perror( "dynclampmodule: dynclamp_loop ttl pulse at start ao: comedi_do_insn" ); ERROR_MSG( "dynclamp_loop: ERROR! failed to write TTL pulses at start ao\n" ); } } } #endif // count numbers of channels used: aonum = 0; for ( iC = 0; iC < aosubdev.chanN; iC++ ) { if ( aosubdev.chanlist[iC].isUsed ) aonum++; } // get data from fifo buffer: if ( aonum > 0 ) { if ( kfifo_size( &aosubdev.fifo ) <= 1 ) { // for whatever reason, non-initialized kfifo has size 1! ERROR_MSG( "dynclamp_loop: ERROR! no fifo buffer for AO subdevice at loopCnt %lu\n", dynClampTask.loopCnt ); stopSubdevice( &aosubdev ); aosubdev.running = E_NOMEM; } retVal = kfifo_out( &aosubdev.fifo, aovalues, aonum ); if ( retVal < aonum ) { ERROR_MSG( "dynclamp_loop: ERROR! fifo buffer underrun for AO subdevice at loopCnt %lu\n", dynClampTask.loopCnt ); stopSubdevice( &aosubdev ); aosubdev.running = E_UNDERRUN; } // read output from buffer data: aoinx = 0; for ( iC = 0; iC < aosubdev.chanN; iC++ ) { pChan = &aosubdev.chanlist[iC]; if ( pChan->isUsed ) { pChan->value = aovalues[aoinx++]; #ifdef ENABLE_COMPUTATION if ( pChan->param > 0 ) { paramOutput[pChan->chan] = pChan->value; } #endif } } } // get buffer data } } // ! aosubdev.pending } // aosubdev.running // write output to daq board: for ( iC = 0; iC < aosubdev.chanN; iC++ ) { pChan = &aosubdev.chanlist[iC]; // this is an output to the DAQ board: #ifdef ENABLE_COMPUTATION if ( pChan->param == 0 ) { #endif outvalue = pChan->value; #ifdef ENABLE_COMPUTATION // add model output to sample: if ( pChan->modelIndex >= 0 ) { statusInput[pChan->statusIndex] = outvalue; statusInput[pChan->statusIndex+1] = output[pChan->modelIndex]; outvalue += output[pChan->modelIndex]; } #endif // write out Sample: #ifdef ENABLE_SYNCSEC outvalue *= currentfac; #endif outvalue = value_to_sample( pChan, outvalue ); // sets pChan->lsample retVal = comedi_do_insn( device, &pChan->insn ); if ( retVal < 1 ) { aosubdev.running = E_NODATA; ERROR_MSG( "dynclamp_loop: ERROR! failed to write data to AO subdevice channel %d at loopCnt %lu\n", iC, dynClampTask.loopCnt ); if ( retVal < 0 ) { comedi_perror( "dynclamp_loop: comedi_data_write" ); aosubdev.running = E_COMEDI; ERROR_MSG( "dynclamp_loop: ERROR! failed to write to AO subdevice channel %d at loopCnt %lu\n", iC, dynClampTask.loopCnt ); } break; } #ifdef ENABLE_COMPUTATION if ( pChan->modelIndex >= 0 ) { #ifdef ENABLE_SYNCSEC statusInput[pChan->statusIndex+3] = outvalue; statusInput[pChan->statusIndex+2] = outvalue / currentfac; #else statusInput[pChan->statusIndex+2] = outvalue; #endif } } #endif } // end of ao channel loop #ifdef ENABLE_AOTIME stoptime = rt_get_cpu_time_ns(); dtime = stoptime - starttime; statusInput[aotimestatusinx] = 1e-9*dtime; #endif /******** End write TTL: *******************************************/ #ifdef ENABLE_TTLPULSE if ( ttlEndWriteInsn.data[0] > 0 ) { retVal = comedi_do_insn( device, &ttlEndWriteInsn ); if ( retVal < 1 ) { if ( retVal < 0 ) comedi_perror( "dynclampmodule: dynclamp_loop ttl pulse at end write: comedi_do_insn" ); ERROR_MSG( "dynclamp_loop: ERROR! failed to write TTL pulses at end write\n" ); } } #endif /*********** DIGITAL OUTPUT: ************************************************/ #ifdef ENABLE_DIOINLOOP if ( diorunning ) { retVal = comedi_do_insn( device, &dioInsn ); if ( retVal < 0 ) { dioerror = retVal; comedi_perror( "dynclampmodule: ERROR! dynclamp_loop failed to write DIO lines" ); } else dioerror = 0; diorunning = 0; } #endif /******** WAIT FOR PERIOD: **************************************************/ #ifdef ENABLE_WAITTIME starttime = rt_get_cpu_time_ns(); #endif rt_task_wait_period(); #ifdef ENABLE_WAITTIME stoptime = rt_get_cpu_time_ns(); dtime = stoptime - starttime; statusInput[waittimestatusinx] = 1e-9*dtime; #endif dynClampTask.loopCnt++; newtime = rt_get_cpu_time_ns(); difftime = newtime - currenttime; currenttime = newtime; statusInput[intervalstatusinx] = 1e-9*difftime; } // END OF DYNCLAMP LOOP stopSubdevice( &aisubdev ); dynClampTask.running = 0; dynClampTask.duration = 0; dynClampTask.frequency = 0; DEBUG_MSG( "dynclamp_loop: left dynamic clamp loop after %lu cycles\n", dynClampTask.loopCnt ); } /////////////////////////////////////////////////////////////////////////////// // *** RTAI FUNCTIONS *** /////////////////////////////////////////////////////////////////////////////// int init_dynclamp_loop( void ) { unsigned int reqfreq = 0; int stackSize = 32768; int priority; const int usesFPU = 1; /* we need FPU support in any case! */ void* signal = NULL; int dummy = 23; int retVal; RTIME periodTicks; DEBUG_MSG( "init_dynclamp_loop: Trying to initialize dynamic clamp RTAI task...\n" ); if ( dynClampTask.inuse ) { ERROR_MSG( "init_dynclamp_loop: dynamic clamp RTAI task is still in use!\n" ); return -EBUSY; } #ifndef CONFIG_RTAI_FPU_SUPPORT #error "RTAI FPU support is not enabled. Reconfigure, compile and install RTAI kernel modules." #endif // test if dynamic clamp frequency is valid: if ( dynClampTask.frequency <= 1 || dynClampTask.frequency > MAX_FREQUENCY ) { ERROR_MSG( "init_dynclamp_loop ERROR: %dHz -> invalid dynamic clamp frequency. Valid range is 1 .. %dHz\n", dynClampTask.frequency, MAX_FREQUENCY ); return -EINVAL; } // initializing rt-task for dynamic clamp with high priority: priority = RT_SCHED_HIGHEST_PRIORITY; rt_linux_use_fpu( usesFPU ); /* declare if we use the FPU */ retVal = rt_task_init( &dynClampTask.rtTask, dynclamp_loop, dummy, stackSize, priority, usesFPU, signal ); if ( retVal != 0 ) { ERROR_MSG( "rt_task_init() failed with return value %d.\n", retVal ); if ( retVal == ENOMEM ) ERROR_MSG( "init_dynclamp_loop ERROR: failed to allocate stack! stacksize was set to %d bytes.\n", stackSize ); else if ( retVal == EINVAL ) { ERROR_MSG( "init_dynclamp_loop ERROR: task already in use!\n" ); retVal = EBUSY; } else ERROR_MSG( "init_dynclamp_loop ERROR: failed to initialize real-time task for dynamic clamp!\n" ); return -retVal; } dynClampTask.inuse = 1; DEBUG_MSG( "init_dynclamp_loop: initialized dynamic clamp RTAI task. Trying to make it periodic...\n" ); // compute periods: reqfreq = dynClampTask.frequency; #ifdef ONESHOT_MODE periodTicks = nano2count( 1000000000/dynClampTask.frequency ); #else periodTicks = start_rt_timer( nano2count( 1000000000/dynClampTask.frequency ) ); #endif dynClampTask.period = count2nano( periodTicks ); if ( dynClampTask.period < 1 ) { ERROR_MSG( "init_dynclamp_loop ERROR: period is zero!\n" ); cleanup_dynclamp_loop(); return -3; } dynClampTask.frequency = 1000000000 / dynClampTask.period; #ifdef ENABLE_COMPUTATION loopInterval = 1.0e-9*dynClampTask.period; loopRate = 1.0e9/dynClampTask.period; #endif // START rt-task for dynamic clamp as periodic: if ( rt_task_make_periodic( &dynClampTask.rtTask, rt_get_time() + periodTicks, periodTicks ) != 0 ) { ERROR_MSG( "init_dynclamp_loop ERROR: failed to make real-time task periodic!\n" ); cleanup_dynclamp_loop(); return -3; } INFO_MSG( "init_dynclamp_loop: periodic task successfully started... requested freq: %dHz, accepted freq: ~%uHz (period=%uns)\n", reqfreq, dynClampTask.frequency, dynClampTask.period ); return 0; } void cleanup_dynclamp_loop( void ) { if ( dynClampTask.inuse ) { if ( dynClampTask.running ) { stopSubdevice( &aisubdev ); do { msleep( 1 ); } while ( dynClampTask.running ); } msleep( 1 ); rt_task_delete( &dynClampTask.rtTask ); memset( &dynClampTask, 0, sizeof(struct dynClampTaskT) ); dynClampTask.inuse = 0; INFO_MSG( "cleanup_dynclamp_loop: stopped periodic task\n" ); } } /////////////////////////////////////////////////////////////////////////////// // *** IOCTL *** /////////////////////////////////////////////////////////////////////////////// #ifdef HAVE_UNLOCKED_IOCTL long dynclampmodule_unlocked_ioctl( struct file *fModule, unsigned int cmd, unsigned long arg ) #else int dynclampmodule_ioctl( struct inode *devFile, struct file *fModule, unsigned int cmd, unsigned long arg ) #endif { static struct deviceIOCT deviceIOC; static struct chanlistIOCT chanlistIOC; static struct syncCmdIOCT syncCmdIOC; static struct traceInfoIOCT traceInfo; #ifdef ENABLE_COMPUTATION static struct traceChannelIOCT traceChannel; #endif static struct dioIOCT dioIOC; static struct triggerIOCT triggerIOC; enum subdevTypes subdevtype; int retVal = 0; int rc = 0; int tmp; int running; unsigned long luTmp; if ( _IOC_TYPE(cmd) != RTMODULE_MAJOR || _IOC_NR(cmd) > RTMODULE_IOC_MAXNR) { ERROR_MSG( "ioctl: Major wrong or ioctl %d bigger than max %d\n", _IOC_TYPE(cmd), RTMODULE_IOC_MAXNR ); return -ENOTTY; } /* DEBUG_MSG( "dynclampmodule_ioctl: user triggered ioctl %d %s\n", _IOC_NR( cmd ), iocNames[_IOC_NR( cmd )] ); */ mutex_lock( &mutex ); switch( cmd ) { /******** GIVE INFORMATION TO USER SPACE: ***********************************/ case IOC_GETRATE: tmp = dynClampTask.frequency; retVal = put_user( tmp, (int __user *)arg ); rc = retVal == 0 ? 0 : -EFAULT; break; case IOC_GETAOINDEX: luTmp = dynClampTask.aoIndex; if ( luTmp < 0 ) { rc = -ENOSPC; break; } retVal = put_user( luTmp, (unsigned long __user *)arg ); rc = retVal == 0 ? 0 : -EFAULT; break; case IOC_GETLOOPCNT: luTmp = dynClampTask.loopCnt; if ( luTmp < 0 ) { rc = -ENOSPC; break; } retVal = put_user( luTmp, (unsigned long __user *)arg ); rc = retVal == 0 ? 0 : -EFAULT; break; /******** SET UP COMEDI: ****************************************************/ case IOC_OPEN_SUBDEV: retVal = copy_from_user( &deviceIOC, (void __user *)arg, sizeof(struct deviceIOCT) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to deviceIOCT-struct for reading!\n" ); rc = -EFAULT; break; } rc = openComediDevice( &deviceIOC ); retVal = copy_to_user( (void __user *)arg, &deviceIOC, sizeof(struct deviceIOCT) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to deviceIOCT-struct for writing!\n" ); rc = -EFAULT; break; } break; case IOC_REQ_CLOSE: if ( reqCloseSubdev >= 0 ) { ERROR_MSG( "ioctl IOC_REQ_CLOSE ERROR: Another close-request in progress with id %d!\n", reqCloseSubdev ); rc = -EAGAIN; break; } reqCloseSubdev = (int)arg; rc = 0; break; case IOC_CHANLIST: retVal = copy_from_user( &chanlistIOC, (void __user *)arg, sizeof(struct chanlistIOCT) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to chanlistIOCT-struct!\n" ); rc = -EFAULT; break; } if ( chanlistIOC.type == SUBDEV_IN ) rc = loadChanList( &chanlistIOC, &aisubdev ); else if ( chanlistIOC.type == SUBDEV_OUT ) rc = loadChanList( &chanlistIOC, &aosubdev ); else rc = -EFAULT; break; case IOC_SYNC_CMD: retVal = copy_from_user( &syncCmdIOC, (void __user *)arg, sizeof(struct syncCmdIOCT) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to syncCmdIOCT-struct!\n" ); rc = -EFAULT; break; } if ( syncCmdIOC.type == SUBDEV_IN ) rc = loadSyncCmd( &syncCmdIOC, &aisubdev ); else if ( syncCmdIOC.type == SUBDEV_OUT ) rc = loadSyncCmd( &syncCmdIOC, &aosubdev ); else { rc = -EFAULT; break; } retVal = copy_to_user( (void __user *)arg, &syncCmdIOC, sizeof(struct syncCmdIOCT) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to syncCmdIOCT-struct for writing!\n" ); rc = -EFAULT; break; } break; case IOC_GET_TRACE_INFO: retVal = copy_from_user( &traceInfo, (void __user *)arg, sizeof(struct traceInfoIOCT) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid user pointer for traceInfoIOCT!\n" ); rc = -EFAULT; break; } traceInfo.value = 0.0; rc = 0; switch( traceInfo.traceType ) { #ifdef ENABLE_COMPUTATION case TRACE_IN: if ( traceIndex >= INPUT_N ) { traceIndex = 0; rc = -ERANGE; // signal end of list break; } strncpy( traceInfo.name, inputNames[traceIndex], PARAM_NAME_MAXLEN ); strncpy( traceInfo.unit, inputUnits[traceIndex], PARAM_NAME_MAXLEN ); DEBUG_MSG( "ioctl: input trace %s [%s]\n", traceInfo.name, traceInfo.unit ); break; case TRACE_OUT: if ( traceIndex >= OUTPUT_N ) { traceIndex = 0; rc = -ERANGE; // signal end of list break; } strncpy( traceInfo.name, outputNames[traceIndex], PARAM_NAME_MAXLEN ); strncpy( traceInfo.unit, outputUnits[traceIndex], PARAM_NAME_MAXLEN ); traceInfo.value = output[traceIndex]; DEBUG_MSG( "ioctl: output trace %s [%s]\n", traceInfo.name, traceInfo.unit ); break; case PARAM_IN: if ( traceIndex >= PARAMINPUT_N ) { traceIndex = 0; rc = -ERANGE; // signal end of list break; } strncpy( traceInfo.name, paramInputNames[traceIndex], PARAM_NAME_MAXLEN ); strncpy( traceInfo.unit, paramInputUnits[traceIndex], PARAM_NAME_MAXLEN ); DEBUG_MSG( "ioctl: parameter input trace %s [%s]\n", traceInfo.name, traceInfo.unit ); break; case PARAM_OUT: if ( traceIndex >= PARAMOUTPUT_N ) { traceIndex = 0; rc = -ERANGE; // signal end of list // clear analog output traces: aosubdev.chanN = 0; break; } strncpy( traceInfo.name, paramOutputNames[traceIndex], PARAM_NAME_MAXLEN ); strncpy( traceInfo.unit, paramOutputUnits[traceIndex], PARAM_NAME_MAXLEN ); traceInfo.value = paramOutput[traceIndex]; DEBUG_MSG( "ioctl: parameter output trace %s [%s]\n", traceInfo.name, traceInfo.unit ); break; #endif case STATUS_IN: if ( traceIndex >= statusInputN ) { traceIndex = 0; rc = -ERANGE; // signal end of list break; } strncpy( traceInfo.name, statusInputNames[traceIndex], PARAM_NAME_MAXLEN ); strncpy( traceInfo.unit, statusInputUnits[traceIndex], PARAM_NAME_MAXLEN ); DEBUG_MSG( "ioctl: status input trace %s [%s]\n", traceInfo.name, traceInfo.unit ); break; default: ; rc = -ERANGE; // signal end of list } if ( rc != 0 ) break; retVal = copy_to_user( (void __user *)arg, &traceInfo, sizeof(struct traceInfoIOCT) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid user pointer for traceInfoIOCT!\n" ); rc = -EFAULT; break; } traceIndex++; rc = 0; break; case IOC_SET_TRACE_CHANNEL: #ifdef ENABLE_COMPUTATION rc = 0; retVal = copy_from_user( &traceChannel, (void __user *)arg, sizeof(struct traceChannelIOCT) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid user pointer for traceChannelIOCT!\n" ); rc = -EFAULT; break; } switch( traceChannel.traceType ) { case TRACE_IN: if ( inputChanIndex >= INPUT_N ) { ERROR_MSG( "ioctl ERROR: invalid inputChanIndex=%d!\n", inputChanIndex ); rc = -EFAULT; break; } inputChannels[inputChanIndex] = traceChannel.channel; DEBUG_MSG( "ioctl: input channel set to %d\n", traceChannel.channel ); inputChanIndex++; if ( inputChanIndex >= INPUT_N ) inputChanIndex = 0; break; case TRACE_OUT: if ( outputChanIndex >= OUTPUT_N ) { ERROR_MSG( "ioctl ERROR: invalid outputChanIndex=%d!\n", inputChanIndex ); rc = -EFAULT; break; } outputChannels[outputChanIndex] = traceChannel.channel; outputChanIndex++; if ( outputChanIndex >= OUTPUT_N ) outputChanIndex = 0; DEBUG_MSG( "ioctl: output channel set to %d\n", traceChannel.channel ); break; default: rc = -EINVAL; } break; #else rc = -EFAULT; // Nothing done break; #endif case IOC_START_SUBDEV: subdevtype = (enum subdevTypes)arg; if ( subdevtype == SUBDEV_IN ) rc = startSubdevice( &aisubdev ); else if ( subdevtype == SUBDEV_OUT ) rc = startSubdevice( &aosubdev ); else rc = -EFAULT; break; case IOC_STOP_SUBDEV: subdevtype = (enum subdevTypes)arg; if ( subdevtype == SUBDEV_IN ) { rc = stopSubdevice( &aisubdev ); cleanup_dynclamp_loop(); } else if ( subdevtype == SUBDEV_OUT ) rc = stopSubdevice( &aosubdev ); else rc = -EFAULT; break; case IOC_CHK_RUNNING: retVal = get_user( tmp, (int __user *)arg ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to subdevice ID for running-query!" ); rc = -EFAULT; break; } running = 0; if ( tmp == SUBDEV_IN ) running = aisubdev.running; else if ( tmp == SUBDEV_OUT ) running = aosubdev.running; else { ERROR_MSG( "ioctl ERROR: invalid subdevice type for running-query!\n" ); rc = -EFAULT; break; } if ( running != 1 ) DEBUG_MSG( "ioctl: running = %d for subdev %s\n", running, tmp == SUBDEV_IN ? "AI" : "AO" ); retVal = put_user( running, (int __user *)arg ); rc = retVal == 0 ? 0 : -EFAULT; break; // ******* Digital IO: ******************************************** case IOC_DIO_CMD: retVal = copy_from_user( &dioIOC, (void __user *)arg, sizeof(struct dioIOCT) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to dioIOCT-struct!\n" ); rc = -EFAULT; break; } retVal = setDigitalIO( &dioIOC ); if ( retVal != 0 ) { rc = retVal; break; } retVal = copy_to_user( (void __user *)arg, &dioIOC, sizeof(struct dioIOCT) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to dioIOCT-struct!\n" ); rc = -EFAULT; break; } rc = 0; break; // ******* Trigger: *********************************************** case IOC_SET_TRIGGER: retVal = copy_from_user( &triggerIOC, (void __user *)arg, sizeof(struct triggerIOCT) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to triggerIOCT-struct!\n" ); rc = -EFAULT; break; } rc = setAnalogTrigger( &triggerIOC ); break; case IOC_UNSET_TRIGGER: retVal = copy_from_user( &triggerIOC, (void __user *)arg, sizeof(struct triggerIOCT) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to triggerIOCT-struct!\n" ); rc = -EFAULT; break; } rc = unsetAnalogTrigger( &triggerIOC ); break; #ifdef ENABLE_COMPUTATION #ifdef ENABLE_LOOKUPTABLES // ******* Lookup tables: *********************************************** case IOC_SET_LOOKUP_K: retVal = get_user( lookupinx, (int __user *)arg ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to lookup index!" ); lookupinx = -1; rc = -EFAULT; break; } if ( lookupinx < 0 || lookupinx >= MAXLOOKUPTABLES ) { ERROR_MSG( "ioctl ERROR: invalid lookup index!" ); lookupinx = -1; rc = -EINVAL; break; } lookupn[lookupinx] = 0; if ( lookupx[lookupinx] != NULL ) vfree( lookupx[lookupinx] ); if ( lookupy[lookupinx] != NULL ) vfree( lookupy[lookupinx] ); lookupx[lookupinx] = NULL; lookupy[lookupinx] = NULL; break; case IOC_SET_LOOKUP_N: if ( lookupinx < 0 ) { ERROR_MSG( "ioctl ERROR: invalid lookup index!" ); rc = -EINVAL; break; } retVal = get_user( lookupn[lookupinx], (int __user *)arg ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to size of lookup table!" ); lookupn[lookupinx] = 0; rc = -EFAULT; } break; case IOC_SET_LOOKUP_X: if ( lookupinx < 0 ) { ERROR_MSG( "ioctl ERROR: invalid lookup index!" ); rc = -EINVAL; break; } if ( lookupx[lookupinx] != NULL ) { vfree( lookupx[lookupinx] ); lookupx[lookupinx] = NULL; } if ( lookupn[lookupinx] > 0 ) { lookupx[lookupinx] = vmalloc( lookupn[lookupinx]*sizeof(float) ); if ( lookupx[lookupinx] == NULL ) { ERROR_MSG( "ioctl ERROR: failed to allocate memory for x-array of lookup table!\n" ); rc = -ENOMEM; } else { retVal = copy_from_user( lookupx[lookupinx], (void __user *)arg, lookupn[lookupinx]*sizeof(float) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to x-array of lookup table!\n" ); rc = -EFAULT; } } if ( rc != 0 ) { if ( lookupx[lookupinx] != NULL ) { vfree( lookupx[lookupinx] ); lookupx[lookupinx] = NULL; } if ( lookupy[lookupinx] != NULL ) { vfree( lookupy[lookupinx] ); lookupy[lookupinx] = NULL; } lookupn[lookupinx] = 0; } } break; case IOC_SET_LOOKUP_Y: if ( lookupinx < 0 ) { ERROR_MSG( "ioctl ERROR: invalid lookup index!" ); rc = -EINVAL; break; } if ( lookupy[lookupinx] != NULL ) { vfree( lookupy[lookupinx] ); lookupy[lookupinx] = NULL; } if ( lookupn[lookupinx] > 0 ) { lookupy[lookupinx] = vmalloc( lookupn[lookupinx]*sizeof(float) ); if ( lookupy[lookupinx] == NULL ) { ERROR_MSG( "ioctl ERROR: failed to allocate memory for y-array of lookup table!\n" ); rc = -ENOMEM; } else { retVal = copy_from_user( lookupy[lookupinx], (void __user *)arg, lookupn[lookupinx]*sizeof(float) ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to y-array of lookup table!\n" ); rc = -EFAULT; } } if ( rc != 0 ) { if ( lookupx[lookupinx] != NULL ) { vfree( lookupx[lookupinx] ); lookupx[lookupinx] = NULL; } if ( lookupy[lookupinx] != NULL ) { vfree( lookupy[lookupinx] ); lookupy[lookupinx] = NULL; } lookupn[lookupinx] = 0; } } break; #endif #endif case IOC_CHECK_FEATURES: retVal = get_user( tmp, (int __user *)arg ); if ( retVal ) { ERROR_MSG( "ioctl ERROR: invalid pointer to user space features!" ); rc = -EFAULT; } if ( tmp != features ) { ERROR_MSG( "ioctl ERROR: features of kernel module and user space differ!" ); rc = -EINVAL; } break; default: ERROR_MSG( "ioctl: ERROR - invalid IOCTL!\n" ); rc = -ENOTTY; } mutex_unlock( &mutex ); return rc; } /////////////////////////////////////////////////////////////////////////////// // *** DRIVER FUNCTIONS *** /////////////////////////////////////////////////////////////////////////////// ssize_t dynclampmodule_read( struct file *devFile, char *buffer, size_t n, loff_t *pos ) { int retval; unsigned int ncopied; // for whatever reason, non-initialized kfifo has size 1: if ( kfifo_size( &aisubdev.fifo ) <= 1 ) { ERROR_MSG( "dynclampmodule_read: no fifo buffer\n" ); return -ENOMEM; } retval = kfifo_to_user( &aisubdev.fifo, buffer, n, &ncopied ); if ( retval < 0 ) { ERROR_MSG( "dynclampmodule_read: kfifo_to_user failed\n" ); return retval; } // DEBUG_MSG( "dynclampmodule_read: copied %d from %d elements\n", ncopied/sizeof(float), n/sizeof(float) ); return ncopied; } ssize_t dynclampmodule_write( struct file *devFile, const char *buffer, size_t n, loff_t *pos ) { int retval; unsigned int ncopied; // for whatever reason, non-initialized kfifo has size 1: if ( kfifo_size( &aosubdev.fifo ) <= 1 ) { ERROR_MSG( "dynclampmodule_write: no fifo buffer\n" ); return -ENOMEM; } retval = kfifo_from_user( &aosubdev.fifo, buffer, n, &ncopied ); if ( retval < 0 ) { ERROR_MSG( "dynclampmodule_write: kfifo_from_user failed\n" ); return retval; } // DEBUG_MSG( "dynclampmodule_write: copied %d from %d elements\n", ncopied/sizeof(float), n/sizeof(float) ); return ncopied; } int dynclampmodule_open( struct inode *devFile, struct file *fModule ) { DEBUG_MSG( "================================================\n" ); #ifdef ENABLE_COMPUTATION memcpy( origParamOutput, paramOutput, sizeof(paramOutput) ); #endif return 0; } int dynclampmodule_close( struct inode *devFile, struct file *fModule ) { int iS; // no subdevice specified? => stop & close all subdevices & comedi-devices: if ( reqCloseSubdev < 0 ) { DEBUG_MSG( "dynclampmodule_close: no IOC_REQ_CLOSE request received - closing all subdevices...\n" ); for ( iS = subdevN-1; iS>=0; iS-- ) releaseSubdevice( subdevices[iS] ); cleanup_dynclamp_loop(); init_globals(); return 0; } else { // stop & close specified subdevice (and device): releaseSubdevice( reqCloseSubdev ); DEBUG_MSG( "dynclampmodule_close: closed subdevice %d\n", reqCloseSubdev ); if ( subdevN <= 0 ) { cleanup_dynclamp_loop(); init_globals(); } reqCloseSubdev = -1; return 0; } } static int __init init_dynclampmodule( void ) { dev_t dev = 0; int retVal = 0; #ifdef ENABLE_COMPUTATION #ifdef ENABLE_LOOKUPTABLES int k; #endif #endif char featurestr[256] = ""; // register module device file: dev = MKDEV( RTMODULE_MAJOR, 0 ); retVal = register_chrdev_region( dev, 1, "dynclamp" ); if ( retVal < 0 ) { WARN_MSG( "init_dynclampmodule: can't get major %d\n", RTMODULE_MAJOR ); return retVal; } rtcdev = cdev_alloc(); rtcdev->ops = &fops; rtcdev->owner = THIS_MODULE; retVal = cdev_add( rtcdev, dev, 1 ); if ( retVal ) ERROR_MSG( "init_dynclampmodule: fail to register module with error %d\n", retVal ); INFO_MSG( "init_dynclampmodule: dynamic clamp module loaded\n" ); // get features: features = get_features(); get_feature_str( featurestr ); INFO_MSG( "supported features: %s\n", featurestr ); comedi_loglevel( 3 ); mutex_init( &mutex ); #ifdef ENABLE_COMPUTATION memcpy( paramOutput, origParamOutput, sizeof(origParamOutput) ); #ifdef ENABLE_LOOKUPTABLES for ( k=0; k<MAXLOOKUPTABLES; k++ ) { lookupn[k] = 0; lookupx[lookupinx] = NULL; lookupy[lookupinx] = NULL; } #endif #endif // initialize global variables: init_globals(); #ifdef ONESHOT_MODE rt_set_oneshot_mode(); start_rt_timer(1); #else // rt_set_periodic_mode(); // periodic mode is the default. Calling this function hangs the computer... #endif return retVal; } static void __exit cleanup_dynclampmodule( void ) { int iS; dev_t dev = MKDEV( RTMODULE_MAJOR, 0 ); INFO_MSG( "cleanup_dynclampmodule: dynamic clamp module unloaded\n" ); // stop and release all subdevices & comedi-devices: for ( iS = subdevN-1; iS>=0; iS-- ) releaseSubdevice( subdevices[iS] ); init_globals(); mutex_destroy( &mutex ); // stop rtai timer: stop_rt_timer(); // unregister module device file: cdev_del( rtcdev ); unregister_chrdev_region( dev, 1 ); } module_init( init_dynclampmodule ); module_exit( cleanup_dynclampmodule ); --------------BA32ED73923B6B7D614D483C Content-Type: text/plain; charset="us-ascii" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit Content-Disposition: inline _______________________________________________ Rtai mailing list [email protected] https://mail.rtai.org/cgi-bin/mailman/listinfo/rtai --------------BA32ED73923B6B7D614D483C--