ec_master_t *ecrt_open_master(unsigned int master_index)
{
char path[MAX_PATH_LEN];
ec_master_t *master = NULL;
ec_ioctl_module_t module_data;
int ret;
master = malloc(sizeof(ec_master_t));
if (!master) {
fprintf(stderr, "Failed to allocate memory.\n");
return 0;
}
master->process_data = NULL;
master->process_data_size = 0;
master->first_domain = NULL;
master->first_config = NULL;
snprintf(path, MAX_PATH_LEN - 1,
#ifdef USE_RTDM
"EtherCAT%u",
#else
"/dev/EtherCAT%u",
#endif
master_index);
#ifdef USE_RTDM
master->fd = rt_dev_open(path, O_RDWR);
#else
master->fd = open(path, O_RDWR);
#endif
if (EC_IOCTL_IS_ERROR(master->fd)) {
fprintf(stderr, "Failed to open %s: %s\n", path,
strerror(EC_IOCTL_ERRNO(master->fd)));
goto out_clear;
}
ret = ioctl(master->fd, EC_IOCTL_MODULE, &module_data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get module information from %s: %s\n",
path, strerror(EC_IOCTL_ERRNO(ret)));
goto out_clear;
}
if (module_data.ioctl_version_magic != EC_IOCTL_VERSION_MAGIC) {
fprintf(stderr, "ioctl() version magic is differing:"
" %s: %u, libethercat: %u.\n",
path, module_data.ioctl_version_magic,
EC_IOCTL_VERSION_MAGIC);
goto out_clear;
}
return master;
out_clear:
ec_master_clear(master);
free(master);
return 0;
}
///================================================= FUNCTION HEADER==================================================
/// Name : Q8TimerStart
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory
/// Output : None
/// Return : None
/// Description : Deacctivates the timer to stop counting
///===================================================================================================================
void Q8TimerStop(int cardNum) {
struct_Q8_Config q8Str;
q8Str.uiFuncIndex=41;
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
}
///============================================FUNCTION HEADER=========================================================
/// Name : Q8WatchdogTimer
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory
/// fInterval - Timeout interval needed analog output channnels to be set
/// Output : None
/// Return : None
/// Description : Configures and enables Watchdog timer
///
///=====================================================================================================================
void Q8WatchdogTimer(int iCardNum,float fInterval) {
struct_Q8_Config q8Str;
q8Str.uiFuncIndex=28;
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
q8Str.udwPeriod=(uint32_t)(fInterval/ 60e-9)-1;
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
}
void Q8EncoderReset(int iCardNum,int iChannelNum) {
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
struct_Q8_Config q8Str;
q8Str.iCardNum=iCardNum;
q8Str.uiFuncIndex=iChannelNum+31;
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
}
///============================================== FUNCTION HEADER=======================================================
/// Name : Q8EncoderInput
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory
/// iChannelNum - Encoder channel number
/// Output : None
/// Return : an int32_t value that is read
/// Description : Reads the specified channel of encoder.Actually the value read is the counter value of the specified
/// channel
///
///=====================================================================================================================
int32_t Q8EncoderInput(int iCardNum,int iChannelNum) {
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
struct_Q8_Config q8Str;
q8Str.uiFuncIndex=iChannelNum+16;
rt_dev_read(fd,&q8Str,sizeof(struct_Q8_Config));
return q8Str.dwNvalue;
}
///================================================= FUNCTION HEADER====================================================
/// Name : Q8TimerSet
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory
/// fPeriod - Timer set period in seconds
/// Output : None
/// Return : None
/// Description : Configures 32 bit counter as timer interrupt source
///
///=====================================================================================================================
void Q8TimerSet(int cardNum,float fPeriod) {
struct_Q8_Config q8Str;
q8Str.udwPeriod=(uint32_t)(fPeriod / 60e-9)-1;
q8Str.uiFuncIndex=39;
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
}
///================================================= FUNCTION HEADER==================================================
/// Name : Q8GetIrq
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory
/// Output : None
/// Return : Q8 IRQ Line number
/// Description : This function fetches irq line number assigned to Q8 card.
///===================================================================================================================
uint8_t Q8GetIrq(int cardNum) {
struct_Q8_Config q8Str;
q8Str.uiFuncIndex=42;
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
rt_dev_read(fd,&q8Str,sizeof(struct_Q8_Config));
return q8Str.irq;
}
///==========================================FUNCTION HEADER============================================================
/// Name : Q8AnalogInput
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory
/// iCahnnelNum - D/A input channel number
/// Output : None
/// Return : None
/// Description : Reads the voltage value from the specified channel.
///
///=====================================================================================================================
int16_t Q8AnalogInput(int iCardNum,int iChannelNum) {
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
struct_Q8_Config q8Str;
q8Str.iCardNum=iCardNum;
q8Str.uiFuncIndex=iChannelNum+8;
rt_dev_read(fd,&q8Str,sizeof(struct_Q8_Config));
return q8Str.wValueSigned;
}
///=============================================== FUNCTION HEADER =====================================================
/// Name : Q8CounterOutput
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory
/// iClockSource - ClockSource selection,WATCHDOG or COUNTER
/// fPeriod - Square wave period
/// iCntrlEn - Not used
/// Output : None
/// Return : None
/// Description : Configures counter output and generates periodic square wave
///
///=====================================================================================================================
void Q8CounterOutput(int iCardNum,int iClockSource, float fPeriod,int iCntrlEn )
{
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
struct_Q8_Config q8Str;
q8Str.uiFuncIndex=26;
q8Str.iClockSource=iClockSource;
q8Str.udwPeriod=(uint32_t)(fPeriod / 60e-9)-1;
q8Str.iCntrlEn=iCntrlEn;
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
}
///=============================================== FUNCTION HEADER =====================================================
/// Name : Q8PwmOutput
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory
/// iClockSource - ClockSource selection,WATCHDOG or COUNTER
/// fPeriod - PWM period
/// iCntrlEn - Not used
/// fPulseWidth - PWM pulse width,0...100
/// Output : None
/// Return : None
/// Description : Configures counter output and generates PWM based periodic wave.
///
///=====================================================================================================================
void Q8PwmOutput(int iCardNum,int iClockSource, float fPeriod,float fPulseWidth,int iCntrlEn) {
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
struct_Q8_Config q8Str;
q8Str.uiFuncIndex=27;
q8Str.iClockSource=iClockSource;
q8Str.udwLow=(uint32_t)( ((100-fPulseWidth)*fPeriod/100) / 30e-9) -1;
q8Str.udwHigh=(uint32_t)( ((fPulseWidth)*fPeriod/100) / 30e-9)-1;
q8Str.iCntrlEn=iCntrlEn;
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
}
/*!*****************************************************************************
*******************************************************************************
\note open_serial
\date Oct 2000
\remarks
opens a specific serial port
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] fname : name of serial port
\param[in] baud : baudrate (choose from termios.h, e.g., B38400)
\param[in] mode : O_RDONLY or O_RDWR
return serial_fd, i.e., the file descriptor, or FALSE
******************************************************************************/
int
open_serial(char *fname, int baud, int mode)
{
int serial_fd;
struct rtser_config get_config;
int rc;
serial_fd = rt_dev_open( fname, mode | O_NOCTTY | O_NDELAY );
if (serial_fd < 0) {
printf("Can't open serial port %s for mode %d (err=%d)\n",fname,mode,serial_fd);
return FALSE;
}
// get settings of the serial port
rc = rt_dev_ioctl(serial_fd, RTSER_RTIOC_GET_CONFIG, &get_config);
if (rc) {
printf("ERROR: cannot get RTSER_RTIOC_GET_CONFIG, status = %d, errno = %d, strerror = %s \n",
rc,errno,strerror(errno));
return FALSE;
}
// set baud rate
get_config.baud_rate = baud;
// set parity and bits
get_config.parity = RTSER_NO_PARITY;
get_config.data_bits = RTSER_8_BITS;
get_config.stop_bits = RTSER_DEF_STOPB;
get_config.rx_timeout = 50*1000; // in ns
get_config.tx_timeout = 50*1000; // in ns
get_config.event_timeout = 50*1000; // in ns
get_config.event_mask = RTSER_EVENT_RXPEND;
get_config.config_mask =
RTSER_SET_BAUD | RTSER_SET_PARITY | RTSER_SET_DATA_BITS | RTSER_SET_STOP_BITS |
RTSER_SET_TIMEOUT_RX | RTSER_SET_TIMEOUT_TX | RTSER_SET_TIMEOUT_EVENT | RTSER_SET_EVENT_MASK;
// and update the serial line
rc = rt_dev_ioctl(serial_fd, RTSER_RTIOC_SET_CONFIG, &get_config);
if (rc) {
printf("ERROR: cannot get RTSER_RTIOC_SET_CONFIG, status = %d, errno = %d, strerror = %s \n",
rc,errno,strerror(errno));
return FALSE;
}
// clear the buffer
clear_serial(serial_fd);
return serial_fd;
}
///================================================= FUNCTION HEADER====================================================
/// Name : Q8AnalogConfig
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory
/// iChannelNum - D/A Output channel number
/// iMode - Output voltage range selection
/// Output : None
/// Return : None
/// Description : Configures the specified analog output channel
///
///=====================================================================================================================
void Q8AnalogConfig(int iCardNum,int iChannelNum,int iMode) {
//update process of configuration of analogoutput mode
dacCon[iChannelNum]=iMode;
struct_Q8_Config q8Str;
q8Str.iCardNum=iCardNum;
q8Str.uiFuncIndex=iChannelNum;
q8Str.ubDacMode=iMode;
q8Str.iChannelNum=iChannelNum;
q8Str.uiFuncIndex=30;
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
}
///============================================== FUNCTION HEADER=======================================================
/// Name : Q8DigitalOutput
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory.
/// iPtrChannelNums - Array address.In this array numbers of channels written are defined and stored.
/// iArraySİze - Array size.
/// Output : None
/// Return : None
/// Description : Configures the specified channel(s) as output(s) and writes the specified channel(s)
///=====================================================================================================================
void Q8DigitalOutput(int iCardNum,uint32_t channel,uint32_t data,int writeOnly) {
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
struct_Q8_Config q8Str;
q8Str.uiFuncIndex=24;
q8Str.onlyDigitalData=writeOnly;
q8Str.udwDataDirection=0;
q8Str.udwDataDirection|=channel;
q8Str.udwDataRegister=data;
q8Str.iCardNum=iCardNum;
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
}
int main(int argc, char *argv[])
{
char buf[1024];
ssize_t size;
int device;
int ret;
/* open the device */
device = rt_dev_open(DEVICE_NAME, 0);
if (device < 0) {
printf("ERROR : can't open device %s (%s)\n",
DEVICE_NAME, strerror(-device));
fflush(stdout);
exit(1);
}
/* first read */
size = rt_dev_read (device, (void *)buf, 1024);
printf("Read in device %s\t: %d bytes\n", DEVICE_NAME, size);
/* first write */
sprintf(buf, "HelloWorld!");
size = rt_dev_write (device, (const void *)buf, strlen(buf) + 1);
printf("Write from device %s\t: %d bytes\n", DEVICE_NAME, size);
/* second read */
size = rt_dev_read (device, (void *)buf, 1024);
printf("Read in device %s\t: %s\n", DEVICE_NAME, buf);
/* third read */
size = rt_dev_read (device, (void *)buf, 1024);
printf("Read in device %s\t: %d bytes\n", DEVICE_NAME, size);
/* close the device */
ret = rt_dev_close(device);
if (ret < 0) {
printf("ERROR : can't close device %s (%s)\n",
DEVICE_NAME, strerror(-ret));
fflush(stdout);
exit(1);
}
return 0;
}
///================================================FUNCTION HEADER=====================================================
/// Name : Q8EncoderConfig
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory
/// iChannelNum - Encoder channel number
/// iCountMode - Count mode to use
/// iBcd - Bcd or binary counting
/// iQuadrature - Quadrature type selection
/// iIndexEn - Enable or disable index feature
/// iIndexPol - Select index polarity
/// Output : None
/// Return : None
/// Description : Configures the encoder channel
///
///=====================================================================================================================
int Q8EncoderConfig(int iCardNum, int iChannelNum,int iCountMode,
int iBcd, int iQuadrature, int iIndexEn, int iIndexPol)
{
struct_Q8_Config q8Str;
q8Str.uiFuncIndex=29;
q8Str.iCardNum=iCardNum;
q8Str.iChannelNum=iChannelNum;
q8Str.ubEcountMode=iCountMode;
q8Str.ubEbcdMode=iBcd;
q8Str.ubEquadratureMode=iQuadrature;
q8Str.ubIndexEnable=iIndexEn;
q8Str.ubIndexPolarity=iIndexPol;
if(fd < 0) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
return fd;
}
///================================================ FUNCTION HEADER======================================================
/// Name : Q8DigitalInput
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory.
/// iPtrChannelNums - Array address.In this array numbers of channels read are defined and stored.
/// iArraySİze - Array size.
/// Output : None
/// Return : an unint32_t value
/// Description : Configures the specified channel(s) as input(s) and reads the specified channel(s)
///
///=====================================================================================================================
uint32_t Q8DigitalInput(int iCardNum,uint32_t channel,int readonly) {
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
uint32_t udwDigitalRead=0;
struct_Q8_Config q8Str;
q8Str.uiFuncIndex=25;
q8Str.onlyDigitalData=readonly;
q8Str.udwDataDirection=0xFFFFFFFF;
q8Str.udwDataDirection&=channel;
q8Str.iCardNum=iCardNum;
int i;
uint32_t udwDataBuffer;
uint32_t udwDataReg;
rt_dev_read(fd,&q8Str,sizeof(struct_Q8_Config));
return q8Str.udwDataRegister;
}
///============================================ FUNCTION HEADER=========================================================
/// Name : Q8AnalogOutput
/// Input : iCardNum - PCI memory region that is mapped to Q8 card memory
/// iChannelNum - D/A Output channel number
/// fValue - User defined voltage which is wanted to see at output of the specified channel
/// Output : None
/// Return : None
/// Description : Writes user defined voltage value to the specified channel.
///
///=====================================================================================================================
void Q8AnalogOutput(int iCardNum,int iChannelNum,real_T dacValue) {
if(fd==-1) {
fd = rt_dev_open(DEV_NAME, O_RDWR);
}
struct_Q8_Config q8Str;
q8Str.iCardNum=iCardNum;
q8Str.uiFuncIndex=iChannelNum;
if (dacValue <=0) {
if(dacCon[iChannelNum]==BIPOLAR_10V) {
q8Str.uwValueUnsigned=(DAC_BIPOLAR_ZERO-(uint16_t)((-dacValue)*4096/20));
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
return;
}
else if(dacCon[iChannelNum]==BIPOLAR_5V) {
q8Str.uwValueUnsigned=(DAC_BIPOLAR_ZERO-(uint16_t)((-dacValue)*4096/10));
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
return;
}
else
return;
}//if
if(dacCon[iChannelNum]==BIPOLAR_10V) {
q8Str.uwValueUnsigned=(DAC_BIPOLAR_ZERO+(uint16_t)(dacValue*4096/20));
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
return;
}
else if(dacCon[iChannelNum]==BIPOLAR_5V) {
q8Str.uwValueUnsigned=(DAC_BIPOLAR_ZERO+(uint16_t)(dacValue*4096/10));
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
return;
}
else if(dacCon[iChannelNum]==UNIPOLAR_10V) {
q8Str.uwValueUnsigned=(uint16_t)(dacValue*4096/10);
rt_dev_write(fd,&q8Str,sizeof(struct_Q8_Config));
return;
}
} // end of function
int main (int argc, char * argv[])
{
int fd;
int i;
if (argc < 2) {
fprintf(stderr, "usage: %s rtdm_device\n", argv[0]);
exit(EXIT_FAILURE);
}
fd = rt_dev_open(argv[1], O_WRONLY);
if (fd < 0) {
fprintf(stderr, "%s: %s\n", argv[1], strerror(-fd));
exit(EXIT_FAILURE);
}
for (i = 2; i < argc; i ++)
rt_dev_write(fd, argv[i], strlen(argv[i]));
rt_dev_close(fd);
exit(EXIT_SUCCESS);
}
/**
* This function is called when the control program is loaded to zenom.
* Use this function to register control parameters, to register log variables
* and to initialize control parameters.
*
* @return Return non-zero to indicate an error.
*/
int ZenomMatlab::initialize()
{
int paramIdx, sigIdx;
int nBlockParams, nSignals;
const char* status;
// İsmi gözükmeyen parametrelerin sayısını tutar.
unknown_param_counter = 0;
/* Here is where Q8 dirver is loaded to kernel space */
//system(STR(sudo insmod DQ8));
fd = rt_dev_open(DEV_NAME, O_RDWR);
if(fd < 0)
fprintf(stderr, "target:Q8 device open error!\n");
init_xenomai();
rtM = MODEL();
if (rtmGetErrorStatus(rtM) != NULL) {
(void)fprintf(stderr,"Error during model registration: %s\n",
rtmGetErrorStatus(rtM));
exit(EXIT_FAILURE);
}
MdlInitializeSizes();
MdlInitializeSampleTimes();
status = rt_SimInitTimingEngine(rtmGetNumSampleTimes(rtM),
rtmGetStepSize(rtM),
rtmGetSampleTimePtr(rtM),
rtmGetOffsetTimePtr(rtM),
rtmGetSampleHitPtr(rtM),
rtmGetSampleTimeTaskIDPtr(rtM),
rtmGetTStart(rtM),
&rtmGetSimTimeStep(rtM),
&rtmGetTimingData(rtM));
if (status != NULL) {
(void)fprintf(stderr, "Failed to initialize sample time engine: %s\n", status);
exit(EXIT_FAILURE);
}
rt_CreateIntegrationData(rtM);
mmi = &(rtmGetDataMapInfo(rtM).mmi);
if (mmi!=NULL){
//exception here
}
bp = rtwCAPI_GetBlockParameters(mmi);
sig = rtwCAPI_GetSignals(mmi);
nBlockParams = rtwCAPI_GetNumBlockParameters(mmi);
nSignals = rtwCAPI_GetNumSignals(mmi);
xrtpi = new XrtTargetParamInfo[nBlockParams];
xtsi = new XrtTargetSignalInfo[nSignals];
/** Get parameter and register */
Xrt_GetParameterInfo(mmi);
Xrt_GetSignalInfo(mmi);
/**************** ZENOM PART ***************/
for(paramIdx = 0; paramIdx < rtwCAPI_GetNumBlockParameters(mmi); paramIdx++){
std::string paramName(xrtpi[paramIdx].paramName);
std::string blockName(xrtpi[paramIdx].blockName);
if(paramName.empty()){
paramName = std::string("unknownBlock");
}
for (std::size_t found = paramName.find_first_of(" ");
found != std::string::npos ;
found = paramName.find_first_of(" "))
{
paramName.erase (found);
}
for (std::size_t found = blockName.find_first_of(" ");
found != std::string::npos ;
found = blockName.find_first_of(" "))
{
blockName.erase (found);
}
paramName = blockName + "-" + paramName;
registerControlVariable( xrtpi[paramIdx].dataValue,
paramName,
xrtpi[paramIdx].numRows,
xrtpi[paramIdx].numColumns );
}
for (sigIdx = 0; sigIdx < rtwCAPI_GetNumSignals(mmi); ++sigIdx){
std::string sigName(xtsi[sigIdx].signalName);
if(sigName.empty()){
sigName = std::string("unknownSignal");
sigName += unknown_param_counter;
}
for (std::size_t found = sigName.find_first_of(" ");
found != std::string::npos ;
found = sigName.find_first_of(" "))
{
sigName.erase (found);
}
registerLogVariable(xtsi[sigIdx].dataValue,
sigName,
xtsi[sigIdx].numRows,
xtsi[sigIdx].numColumns);
}
setFrequency( (double)rtmGetStepSize(rtM) );
setDuration ( (double)rtmGetTFinal(rtM) );
fprintf(stderr, "init done!");
return 0;
}
static int __init klat_mod_init(void)
{
char devname[RTDM_MAX_DEVNAME_LEN + 1];
unsigned dev_nr;
int err;
err = rt_pipe_create(&klat_pipe, "klat_pipe", pipe, 4096);
if (err) {
printk("rt_pipe_create(klat_pipe): %d\n", err);
return err;
}
err = rt_task_create(&klat_srvr, "klat_srvr", 0, 0, 0);
if (err) {
printk("rt_task_create(klat_srvr): %d\n", err);
goto err_close_pipe;
}
pkt.config.mode = mode;
pkt.config.priority = priority;
pkt.config.period = period * 1000;
pkt.config.warmup_loops = 1;
pkt.config.histogram_size = 0;
pkt.config.freeze_max = freeze_max;
for (dev_nr = 0; dev_nr < DEV_NR_MAX; dev_nr++) {
snprintf(devname, sizeof(devname),
"rttest-timerbench%d",
dev_nr);
fd = rt_dev_open(devname, O_RDONLY);
if (fd < 0)
continue;
err = rt_dev_ioctl(fd, RTTST_RTIOC_TMBENCH_START, &pkt.config);
if (err == -ENOTTY) {
rt_dev_close(fd);
continue;
}
if (err < 0) {
printk("rt_dev_ioctl(RTTST_RTIOC_TMBENCH_START): %d\n",
err);
goto err_destroy_task;
}
break;
}
if (fd < 0) {
printk("rt_dev_open: could not find rttest device\n"
"(modprobe timerbench?)");
err = fd;
goto err_destroy_task;
}
err = rt_task_start(&klat_srvr, &klat_server, NULL);
if (err) {
printk("rt_task_start: %d\n", err);
goto err_close_dev;
}
return 0;
err_close_dev:
rt_dev_close(fd);
err_destroy_task:
rt_task_delete(&klat_srvr);
err_close_pipe:
rt_pipe_delete(&klat_pipe);
return err;
}
TPCANStatus __stdcall CAN_Initialize(TPCANHandle Channel,
TPCANBaudrate Btr0Btr1, TPCANType HwType, DWORD IOPort, WORD Interrupt) {
PCAN_DESCRIPTOR *desc = NULL;
errno = 0;
TPCANStatus Result = PCAN_ERROR_OK;
try {
// logging
char szLog[MAX_LOG];
LOG_EnteringTo("CAN_Initialize");
sprintf(
szLog,
"Channel: 0x%02X, Btr0Btr1: %d, HwType: 0x%08X, IOPort: 0x%08X, Interrupt: 0x%08X",
Channel, Btr0Btr1, IOPort, HwType, Interrupt);
LOG_Parameters("CAN_Initialize", szLog);
// device name and falgs
char *szDeviceName = CAN_Get_Device_Name(Channel, HwType, IOPort,
IOPort);
int nFlag = O_RDWR | O_NONBLOCK;
// allocate for descriptors
if (ppDescriptors == NULL) {
if ((ppDescriptors = (PCAN_DESCRIPTOR **) malloc(
sizeof(PCAN_DESCRIPTOR *) * 0x100)) == NULL) {
Result = PCAN_ERROR_RESOURCE;
goto fail;
}
memset(ppDescriptors, 0, sizeof(PCAN_DESCRIPTOR *) * 0x100);
}
// check if descriptor allready existing
desc = ppDescriptors[Channel];
if (ppDescriptors[Channel] != NULL) {
Result = PCAN_ERROR_INITIALIZE;
goto fail;
}
// create desciptor
if ((ppDescriptors[Channel] = (PCAN_DESCRIPTOR *) malloc(
sizeof(PCAN_DESCRIPTOR))) == NULL) {
Result = PCAN_ERROR_RESOURCE;
goto fail;
}
// fill descriptor
desc = ppDescriptors[Channel];
desc->szDevicePath[0] = 0;
desc->nFileNo = -1;
desc->nFilter = PCAN_FILTER_OPEN;
desc->nListenOnly = 0;
desc->Btr0Btr1 = Btr0Btr1;
strncpy(desc->szDevicePath, szDeviceName, LOCAL_STRING_LEN);
#ifndef NO_RT
char DeviceName[256];
sscanf(szDeviceName, "/dev/%s", DeviceName);
if ((desc->nFileNo = rt_dev_open(DeviceName, nFlag)) == -1)
#else
if ((desc->nFileNo = open(szDeviceName, nFlag)) == -1)
#endif
{
Result = PCAN_ERROR_NODRIVER;
goto fail;
}
// initialize
TPCANInit init;
init.wBTR0BTR1 = desc->Btr0Btr1; // combined BTR0 and BTR1 register of the SJA100
init.ucCANMsgType = MSGTYPE_EXTENDED; // 11 or 29 bits
init.ucListenOnly = desc->nListenOnly; // listen only mode when != 0
if (__ioctl(desc->nFileNo, PCAN_INIT, &init) < 0) {
Result = PCAN_ERROR_NODRIVER;
goto fail;
}
goto leave;
// error handling
fail: if (desc) {
if (desc->nFileNo > -1)
__close(desc->nFileNo);
free(desc);
ppDescriptors[Channel] = NULL;
}
leave: LOG_LeavingFrom("CAN_Initialize", Result);
return Result;
} catch (...) {
Result = PCAN_ERROR_UNKNOWN;
LOG_Exception("CAN_Initialize");
}
return Result;
}
int main (int ac, char **av)
{
int err;
char *cp, *progname = (char*)basename(av[0]), *rtdm_driver;
struct sched_param param_square = {.sched_priority = 99 };
struct sched_param param_irq = {.sched_priority = 98 };
pthread_attr_t thattr_square, thattr_irq;
period_ns = PERIOD; /* ns */
signal(SIGINT, cleanup_upon_sig);
signal(SIGTERM, cleanup_upon_sig);
signal(SIGHUP, cleanup_upon_sig);
signal(SIGALRM, cleanup_upon_sig);
while (--ac) {
if ((cp = *++av) == NULL)
break;
if (*cp == '-' && *++cp) {
switch(*cp) {
case 'p' :
period_ns = (unsigned long)atoi(*++av);
break;
case 'r' :
rtdm_driver = *++av;
break;
default:
usage(progname);
break;
}
}
else
break;
}
// Display every 2 sec
loop_prt = 2000000000 / period_ns;
printf ("Using driver \"%s\" and period %d ns\n", rtdm_driver, period_ns);
// Avoid paging: MANDATORY for RT !!
mlockall(MCL_CURRENT|MCL_FUTURE);
// Init rt_printf() system
rt_print_auto_init(1);
// Open RTDM driver
if ((fd = rt_dev_open(rtdm_driver, 0)) < 0) {
perror("rt_open");
exit(EXIT_FAILURE);
}
// Thread attributes
pthread_attr_init(&thattr_square);
// Joinable
pthread_attr_setdetachstate(&thattr_square, PTHREAD_CREATE_JOINABLE);
// Priority, set priority to 99
pthread_attr_setinheritsched(&thattr_square, PTHREAD_EXPLICIT_SCHED);
pthread_attr_setschedpolicy(&thattr_square, SCHED_FIFO);
pthread_attr_setschedparam(&thattr_square, ¶m_square);
// Create thread(s)
err = pthread_create(&thid_square, &thattr_square, &thread_square, NULL);
if (err)
{
fprintf(stderr,"square: failed to create square thread, code %d\n",err);
return 0;
}
// Thread attributes
pthread_attr_init(&thattr_irq);
// Priority, set priority to 98
pthread_attr_setinheritsched(&thattr_irq, PTHREAD_EXPLICIT_SCHED);
pthread_attr_setschedpolicy(&thattr_irq, SCHED_FIFO);
pthread_attr_setschedparam(&thattr_irq, ¶m_irq);
err = pthread_create(&thid_irq, &thattr_irq, &thread_irq, NULL);
if (err)
{
fprintf(stderr,"square: failed to create IRQ thread, code %d\n",err);
return 0;
}
pause();
return 0;
}
int main(int argc, char **argv)
{
int cpu = 0, c, err, sig;
struct sigaction sa;
char task_name[16];
sigset_t mask;
while ((c = getopt(argc, argv, "g:hp:l:T:qH:B:sD:t:fc:P:b")) != EOF)
switch (c) {
case 'g':
do_gnuplot = strdup(optarg);
break;
case 'h':
do_histogram = 1;
break;
case 's':
do_stats = 1;
break;
case 'H':
histogram_size = atoi(optarg);
break;
case 'B':
bucketsize = atoi(optarg);
break;
case 'p':
period_ns = atoi(optarg) * 1000LL;
break;
case 'l':
data_lines = atoi(optarg);
break;
case 'T':
test_duration = atoi(optarg);
alarm(test_duration + WARMUP_TIME);
break;
case 'q':
quiet = 1;
break;
case 'D':
benchdev_no = atoi(optarg);
break;
case 't':
test_mode = atoi(optarg);
break;
case 'f':
freeze_max = 1;
break;
case 'c':
cpu = T_CPU(atoi(optarg));
break;
case 'P':
priority = atoi(optarg);
break;
case 'b':
stop_upon_switch = 1;
break;
default:
fprintf(stderr,
"usage: latency [options]\n"
" [-h] # print histograms of min, avg, max latencies\n"
" [-g <file>] # dump histogram to <file> in gnuplot format\n"
" [-s] # print statistics of min, avg, max latencies\n"
" [-H <histogram-size>] # default = 200, increase if your last bucket is full\n"
" [-B <bucket-size>] # default = 1000ns, decrease for more resolution\n"
" [-p <period_us>] # sampling period\n"
" [-l <data-lines per header>] # default=21, 0 to supress headers\n"
" [-T <test_duration_seconds>] # default=0, so ^C to end\n"
" [-q] # supresses RTD, RTH lines if -T is used\n"
" [-D <testing_device_no>] # number of testing device, default=0\n"
" [-t <test_mode>] # 0=user task (default), 1=kernel task, 2=timer IRQ\n"
" [-f] # freeze trace for each new max latency\n"
" [-c <cpu>] # pin measuring task down to given CPU\n"
" [-P <priority>] # task priority (test mode 0 and 1 only)\n"
" [-b] # break upon mode switch\n"
);
exit(2);
}
if (!test_duration && quiet) {
fprintf(stderr,
"latency: -q only works if -T has been given.\n");
quiet = 0;
}
if ((test_mode < USER_TASK) || (test_mode > TIMER_HANDLER)) {
fprintf(stderr, "latency: invalid test mode.\n");
exit(2);
}
time(&test_start);
histogram_avg = calloc(histogram_size, sizeof(long));
histogram_max = calloc(histogram_size, sizeof(long));
histogram_min = calloc(histogram_size, sizeof(long));
if (!(histogram_avg && histogram_max && histogram_min))
cleanup();
if (period_ns == 0)
period_ns = CONFIG_XENO_DEFAULT_PERIOD; /* ns */
if (priority <= T_LOPRIO)
priority = T_LOPRIO + 1;
else if (priority > T_HIPRIO)
priority = T_HIPRIO;
sigemptyset(&mask);
sigaddset(&mask, SIGINT);
sigaddset(&mask, SIGTERM);
sigaddset(&mask, SIGHUP);
sigaddset(&mask, SIGALRM);
pthread_sigmask(SIG_BLOCK, &mask, NULL);
sigemptyset(&sa.sa_mask);
sa.sa_sigaction = sigdebug;
sa.sa_flags = SA_SIGINFO;
sigaction(SIGDEBUG, &sa, NULL);
if (freeze_max) {
/* If something goes wrong, we want to freeze the current
trace path to help debugging. */
signal(SIGSEGV, faulthand);
signal(SIGBUS, faulthand);
}
setlinebuf(stdout);
printf("== Sampling period: %Ld us\n"
"== Test mode: %s\n"
"== All results in microseconds\n",
period_ns / 1000, test_mode_names[test_mode]);
mlockall(MCL_CURRENT | MCL_FUTURE);
if (test_mode != USER_TASK) {
char devname[RTDM_MAX_DEVNAME_LEN];
snprintf(devname, RTDM_MAX_DEVNAME_LEN, "rttest-timerbench%d",
benchdev_no);
benchdev = rt_dev_open(devname, O_RDWR);
if (benchdev < 0) {
fprintf(stderr,
"latency: failed to open benchmark device, code %d\n"
"(modprobe RTAI_timerbench?)\n", benchdev);
return 0;
}
}
rt_timer_set_mode(TM_ONESHOT); /* Force aperiodic timing. */
snprintf(task_name, sizeof(task_name), "display-%d", getpid());
err = rt_task_create(&display_task, task_name, 0, 0, T_FPU);
if (err) {
fprintf(stderr,
"latency: failed to create display task, code %d\n",
err);
return 0;
}
err = rt_task_start(&display_task, &display, NULL);
if (err) {
fprintf(stderr,
"latency: failed to start display task, code %d\n",
err);
return 0;
}
if (test_mode == USER_TASK) {
snprintf(task_name, sizeof(task_name), "sampling-%d", getpid());
err =
rt_task_create(&latency_task, task_name, 0, priority,
T_FPU | cpu | T_WARNSW);
if (err) {
fprintf(stderr,
"latency: failed to create latency task, code %d\n",
err);
return 0;
}
err = rt_task_start(&latency_task, &latency, NULL);
if (err) {
fprintf(stderr,
"latency: failed to start latency task, code %d\n",
err);
return 0;
}
}
sigwait(&mask, &sig);
finished = 1;
cleanup();
return 0;
}
static void sync_task_func(void *arg)
{
int ret;
rtdm_lockctx_t lock_ctx;
nanosecs_abs_t timestamp;
nanosecs_abs_t timestamp_master;
rtser_event_t ser_rx_event;
can_frame_t can_frame = {
.can_id = clock_sync_can_id,
.can_dlc = sizeof(timestamp),
};
struct iovec iov = {
.iov_base = &can_frame,
.iov_len = sizeof(can_frame_t),
};
struct msghdr msg = {
.msg_name = NULL,
.msg_namelen = 0,
.msg_iov = &iov,
.msg_iovlen = 1,
.msg_control = NULL,
.msg_controllen = 0,
};
if (clock_sync_mode == SYNC_CAN_SLAVE) {
msg.msg_control = ×tamp;
msg.msg_controllen = sizeof(timestamp);
}
while (1) {
switch (clock_sync_mode) {
case SYNC_SER_MASTER:
timestamp = cpu_to_be64(rtdm_clock_read());
ret = sync_dev_ctx->ops->write_rt(sync_dev_ctx, NULL,
×tamp, sizeof(timestamp));
if (ret != sizeof(timestamp)) {
tims_error("[CLOCK SYNC]: can't write serial time stamp, "
"code = %d\n", ret);
goto exit_task;
}
rtdm_task_wait_period();
break;
case SYNC_SER_SLAVE:
ret = sync_dev_ctx->ops->ioctl_rt(sync_dev_ctx, NULL,
RTSER_RTIOC_WAIT_EVENT, &ser_rx_event);
if (ret < 0) {
tims_error("[CLOCK SYNC]: can't read serial time stamp, "
"code = %d\n", ret);
goto exit_task;
}
ret = sync_dev_ctx->ops->read_rt(sync_dev_ctx, NULL,
×tamp_master, sizeof(timestamp_master));
if (ret != sizeof(timestamp_master)) {
tims_error("[CLOCK SYNC]: can't read serial time stamp, "
"code = %d\n", ret);
goto exit_task;
}
timestamp_master = be64_to_cpu(timestamp_master);
rtdm_lock_get_irqsave(&sync_lock, lock_ctx);
clock_offset =
timestamp_master - ser_rx_event.rxpend_timestamp;
rtdm_lock_put_irqrestore(&sync_lock, lock_ctx);
break;
case SYNC_CAN_MASTER:
// workaround for kernel working on user data
iov.iov_len = sizeof(can_frame_t);
iov.iov_base = &can_frame;
// workaround end
*(nanosecs_abs_t *)can_frame.data =
cpu_to_be64(rtdm_clock_read());
ret = sync_dev_ctx->ops->sendmsg_rt(sync_dev_ctx, NULL,
&msg, 0);
if (ret < 0) {
tims_error("[CLOCK SYNC]: can't send CAN time stamp, "
"code = %d\n", ret);
goto exit_task;
}
rtdm_task_wait_period();
break;
case SYNC_CAN_SLAVE:
// workaround for kernel working on user data
iov.iov_len = sizeof(can_frame_t);
iov.iov_base = &can_frame;
// workaround end
ret = sync_dev_ctx->ops->recvmsg_rt(sync_dev_ctx, NULL,
&msg, 0);
if (ret < 0) {
tims_error("[CLOCK SYNC]: can't receive CAN time stamp, "
"code = %d\n", ret);
return;
}
timestamp_master =
be64_to_cpu(*(nanosecs_abs_t *)can_frame.data);
rtdm_lock_get_irqsave(&sync_lock, lock_ctx);
clock_offset = timestamp_master - timestamp;
rtdm_lock_put_irqrestore(&sync_lock, lock_ctx);
break;
}
}
exit_task:
rtdm_context_unlock(sync_dev_ctx);
}
static __initdata char *mode_str[] = {
"Local Clock", "RTnet", "CAN Master", "CAN Slave",
"Serial Master", "Serial Slave"
};
static __initdata struct rtser_config sync_serial_config = {
.config_mask = RTSER_SET_BAUD | RTSER_SET_FIFO_DEPTH |
RTSER_SET_TIMESTAMP_HISTORY | RTSER_SET_EVENT_MASK,
.baud_rate = 115200,
.fifo_depth = RTSER_FIFO_DEPTH_8,
.timestamp_history = RTSER_RX_TIMESTAMP_HISTORY,
.event_mask = RTSER_EVENT_RXPEND,
};
int __init tims_clock_init(void)
{
struct can_filter filter;
int nr_filters = 1;
struct ifreq can_ifr;
struct sockaddr_can can_addr;
int ret;
if (clock_sync_mode < SYNC_NONE || clock_sync_mode > SYNC_SER_SLAVE) {
tims_error("invalid clock_sync_mode %d", clock_sync_mode);
return -EINVAL;
}
printk("TIMS: clock sync mode is %s\n", mode_str[clock_sync_mode]);
printk("TIMS: clock sync dev is %s\n", clock_sync_dev);
rtdm_lock_init(&sync_lock);
switch(clock_sync_mode) {
case SYNC_NONE:
return 0;
case SYNC_RTNET:
sync_dev_fd = rt_dev_open(clock_sync_dev, O_RDONLY);
if (sync_dev_fd < 0)
goto sync_dev_error;
set_bit(TIMS_INIT_BIT_SYNC_DEV, &init_flags);
break;
case SYNC_CAN_MASTER:
case SYNC_CAN_SLAVE:
sync_dev_fd = rt_dev_socket(PF_CAN, SOCK_RAW, 0);
if (sync_dev_fd < 0) {
tims_error("[CLOCK SYNC]: error opening CAN socket: %d\n",
sync_dev_fd);
return sync_dev_fd;
}
set_bit(TIMS_INIT_BIT_SYNC_DEV, &init_flags);
strcpy(can_ifr.ifr_name, clock_sync_dev);
ret = rt_dev_ioctl(sync_dev_fd, SIOCGIFINDEX, &can_ifr);
if (ret) {
tims_info("[CLOCK SYNC]: error resolving CAN interface: %d\n",
ret);
return ret;
}
if (clock_sync_mode == SYNC_CAN_MASTER)
nr_filters = 0;
else {
filter.can_id = clock_sync_can_id;
filter.can_mask = 0xFFFFFFFF;
}
ret = rt_dev_setsockopt(sync_dev_fd, SOL_CAN_RAW, CAN_RAW_FILTER,
&filter, nr_filters*sizeof(can_filter_t));
if (ret < 0)
goto config_error;
/* Bind socket to default CAN ID */
can_addr.can_family = AF_CAN;
can_addr.can_ifindex = can_ifr.ifr_ifindex;
ret = rt_dev_bind(sync_dev_fd, (struct sockaddr *)&can_addr,
sizeof(can_addr));
if (ret < 0)
goto config_error;
/* Enable timestamps for incoming packets */
ret = rt_dev_ioctl(sync_dev_fd, RTCAN_RTIOC_TAKE_TIMESTAMP,
RTCAN_TAKE_TIMESTAMPS);
if (ret < 0)
goto config_error;
/* Calculate transmission delay */
ret = rt_dev_ioctl(sync_dev_fd, SIOCGCANBAUDRATE, &can_ifr);
if (ret < 0)
goto config_error;
/* (47+64 bit) * 1.000.000.000 (ns/sec) / baudrate (bit/s) */
sync_delay = 1000 * (111000000 / can_ifr.ifr_ifru.ifru_ivalue);
break;
case SYNC_SER_MASTER:
case SYNC_SER_SLAVE:
sync_dev_fd = rt_dev_open(clock_sync_dev, O_RDWR);
if (sync_dev_fd < 0)
goto sync_dev_error;
set_bit(TIMS_INIT_BIT_SYNC_DEV, &init_flags);
ret = rt_dev_ioctl(sync_dev_fd, RTSER_RTIOC_SET_CONFIG,
&sync_serial_config);
if (ret < 0)
goto config_error;
/* (80 bit) * 1.000.000.000 (ns/sec) / baudrate (bit/s) */
sync_delay = 1000 * (80000000 / sync_serial_config.baud_rate);
break;
}
sync_dev_ctx = rtdm_context_get(sync_dev_fd);
if (clock_sync_mode != SYNC_RTNET) {
ret = rtdm_task_init(&sync_task, "TIMSClockSync", sync_task_func,
NULL, CLOCK_SYNC_PRIORITY, CLOCK_SYNC_PERIOD);
if (ret < 0)
return ret;
set_bit(TIMS_INIT_BIT_SYNC_TASK, &init_flags);
}
return 0;
sync_dev_error:
tims_error("[CLOCK SYNC]: cannot open %s\n", clock_sync_dev);
return sync_dev_fd;
config_error:
tims_info("[CLOCK SYNC]: error configuring sync device: %d\n", ret);
return ret;
}
void tims_clock_cleanup(void)
{
if (test_and_clear_bit(TIMS_INIT_BIT_SYNC_DEV, &init_flags))
rt_dev_close(sync_dev_fd);
if (test_and_clear_bit(TIMS_INIT_BIT_SYNC_TASK, &init_flags))
rtdm_task_join_nrt(&sync_task, 100);
}
