int rtdal_itfspscq_recv(r_itf_t obj, void* buffer, int len) {
RTDAL_ASSERT_PARAM(buffer);
RTDAL_ASSERT_PARAM(len>=0);
int n, plen;
void *ptr;
if ((n = rtdal_itfspscq_pop(obj, &ptr, &plen, rtdal_time_slot())) != 1) {
return n;
}
if (plen > len) {
plen = len;
}
hdebug("obj=0x%x, rcv pkt=0x%x\n",obj,ptr);
memcpy(buffer, ptr, (size_t) plen);
if ((n = rtdal_itfspscq_release(obj)) == 1) {
return n;
}
hdebug("release pkt 0x%x\n",ptr);
return plen;
}
int rtdal_itfspscq_push(r_itf_t obj, void *ptr, int len, int tstamp) {
cast(obj,itf);
if (!len) {
return 1;
}
/*
if (spscq_is_full(itf)) {
qdebug("[full] write=%d read=%d\n",itf->write,itf->read);
RTDAL_SETERROR(RTDAL_ERROR_NOSPACE);
return 0;
}
*/
#ifdef USE_SYSTEM_TSTAMP
tstamp=rtdal_time_slot();
#endif
itf->packets[itf->write].tstamp = tstamp+itf->parent.delay;
itf->packets[itf->write].len = len;
itf->packets[itf->write].valid = 1;
qdebug("write=%d/%d, len=%d, tstamp=%d, delay=%d\n",itf->write,itf->max_msg,len,tstamp,itf->parent.delay);
itf->write += (itf->write+1 >= itf->max_msg) ? (1-itf->max_msg) : 1;
if (itf->parent.delay < 0) {
ring_buff_binary_sem_give(itf->sem_r);
}
return 1;
}
inline static void pipeline_run_thread_print_time(pipeline_t *obj) {
#ifdef PRINT_TIME
time_t tdata;
rtdal_time_get(&tdata);
if (!(obj->ts_counter%10000))
printf("Pipeline %d running %d modules at TS=%d\t Start: %d:%d\n",obj->id, obj->nof_processes,
rtdal_time_slot(),
(int) tdata.tv_sec, (int) tdata.tv_usec);
#endif
}
/** sleep the calling thread for the time specified by the time_t structure.
*/
int rtdal_sleep(time_t *t) {
assert(context);
RTDAL_ASSERT_PARAM(t);
hdebug("sleep_for=%d:%d\n",t->tv_sec,t->tv_usec);
struct timespec sleep;
sleep.tv_sec = t->tv_sec;
sleep.tv_nsec = t->tv_usec*1000;
if (clock_nanosleep(CLOCK_REALTIME,0,&sleep,NULL)) {
RTDAL_SYSERROR("clock_nanosleep");
return -1;
}
hdebug("waking up at %d\n",rtdal_time_slot());
return 0;
}
int rtdal_itfspscq_pop(r_itf_t obj, void **ptr, int *len, int tstamp) {
cast(obj,itf);
RTDAL_ASSERT_PARAM(ptr);
RTDAL_ASSERT_PARAM(len);
*ptr = NULL;
*len = 0;
if (spscq_is_empty(itf,tstamp)) {
if (itf->parent.delay==-2) {
usleep(1000);
}
qdebug("[empty] read=%d write=%d\n",itf->read,itf->write);
return 0;
}
if (itf->parent.delay >= 0 ) {
#ifdef USE_SYSTEM_TSTAMP
tstamp=rtdal_time_slot();
#endif
if (itf->packets[itf->read].tstamp > tstamp) {
qdebug("[delay] read=%d, tstamp=%d now=%d\n",itf->read,itf->packets[itf->read].tstamp,tstamp);
return 0;
}
/*
if (itf->packets[itf->read].tstamp < tstamp-1) {
qdebug("[old] read=%d, tstamp=%d now=%d\n",itf->read,itf->packets[itf->read].tstamp,tstamp);
rtdal_itfspscq_release(obj,NULL,0);
return 2;
}
*/
}
qdebug("[ok] read=%d, tstamp=%d (now=%d)\n",itf->read,itf->packets[itf->read].tstamp,tstamp);
*ptr = itf->packets[itf->read].data;
*len = itf->packets[itf->read].len;
return 1;
}
int rtdal_itfspscq_send(r_itf_t obj, void* buffer, int len) {
cast(obj,itf);
RTDAL_ASSERT_PARAM(buffer);
RTDAL_ASSERT_PARAM(len>=0);
int n;
void *ptr;
if (len > itf->max_msg_sz) {
RTDAL_SETERROR(RTDAL_ERROR_LARGE);
return -1;
}
hdebug("requesting pkt for 0x%x\n",obj);
if ((n = rtdal_itfspscq_request(obj, &ptr)) != 1) {
return n;
}
memcpy(ptr, buffer, (size_t) len);
hdebug("put pkt for 0x%x pkt 0x%x\n",obj,ptr);
return rtdal_itfspscq_push(obj,len,rtdal_time_slot());
}
/** Sets up to size bytes of the buffer pointed by ptr to the value of the parameter
* returned by oesr_var_param_get()
*
* \param context OESR context pointer
* \param parameter Handler returned by the oesr_var_param_get() function.
* \param value Pointer to the user memory where the parameter value will be stored
* \param size Size of user memory buffer
*
* \return On success, returns a non-negative integer indicating the number of bytes written to value.
* On error returns -1
*/
int oesr_var_param_get_value(void *context, var_t parameter, void* value, int size) {
int cpy_sz;
cast(ctx,context);
OESR_ASSERT_PARAM(parameter);
OESR_ASSERT_PARAM(value);
OESR_ASSERT_PARAM(size>0);
nod_module_t *module = (nod_module_t*) ctx->module;
variable_t *variable = (variable_t*) parameter;
sdebug("id=0x%x, size=%d, value=0x%x, cur_mode=%d\n",parameter,size,value,module->parent.mode.cur_mode);
cpy_sz = (variable->size > size)?size:variable->size;
if (module->parent.mode.next_tslot &&
module->parent.mode.next_tslot <= rtdal_time_slot()) {
module->parent.mode.cur_mode = module->parent.mode.next_mode;
module->parent.mode.next_tslot = 0;
}
memcpy(value, variable->init_value[module->parent.mode.cur_mode], (size_t) cpy_sz);
sdebug("id=0x%x, copied=%d\n", variable, cpy_sz);
return cpy_sz;
}
/**
* Handler for thread-specific signals (SIGSEGV,SIGILL,SIGFPE,SIGBUS).
* Forwards a signal above SIGRTMIN to myself. Since it is blocked, it will
* be received by sigwait_loop(), which is runs in the main kernel thread.
*
* The thread terminates after exiting the handler.
*/
void thread_signal_handler(int signum, siginfo_t *info, void *ctx) {
union sigval value;
int thread_id;
int i;
#ifdef PRINT_BT_ON_SIGSEGV
if (signum == SIGSEGV) {
signal_segv(signum,info,ctx);
}
#endif
signal_received++;
thread_id = -1;
hdebug("[ts=%d] signal %d received\n",rtdal_time_slot(),signum);
/* try to find the thread that caused the signal */
/** todo: Caution!! is pthread_self() safe in the handler?
* it is not async-signal-safe by the standard,
* but the signals are synchronous.
*/
pthread_t thisthread = pthread_self();
/* if signum is SIGUSR2, its a task termination signal, just exit */
if (signum == TASK_TERMINATION_SIGNAL) {
hdebug("sigusr2 signal. thread=%d\n",thisthread);
goto cancel_and_exit;
}
/* is it a pipeline thread? */
for (i=0;i<rtdal.machine.nof_cores;i++) {
if (thisthread == rtdal.pipelines[i].thread) {
break;
}
}
if (i < rtdal.machine.nof_cores) {
hdebug("pipeline_idx=%d\n",i);
thread_id = i;
/* set the thread to 0 because is terminating */
rtdal.pipelines[thread_id].thread = 0;
} else {
/* it is not, may be it is the kernel timer */
if (thisthread == single_timer_thread) {
hdebug("timer thread=%d\n",thisthread);
thread_id = -1;
} else {
/* @TODO: check if it is a status or init thread of any module */
hdebug("other thread=%d\n",thisthread);
goto cancel_and_exit;
}
}
/* Now send a signal to the kernel */
for (i=0;i<N_THREAD_SPECIFIC_SIGNALS;i++) {
if (thread_specific_signals[i] == signum)
break;
}
hdebug("signal=%d, thread=%d\n",i,thread_id);
value.sival_int = thread_id<<16 | i;
if (sigqueue(kernel_pid,
KERNEL_SIG_THREAD_SPECIFIC,
value)) {
poserror(errno, "sigqueue");
}
cancel_and_exit:
if (signum != SIGABRT || thread_id == -1) {
pthread_exit(NULL);
} else {
rtdal.pipelines[thread_id].waiting=1;
while(rtdal.pipelines[thread_id].waiting) {
hdebug("waiting\n",0);
usleep(1000);
}
}
}
void *_run_main(void *arg) {
int c;
int tslen;
int mode;
rtdal_machine(&machine);
tslen = machine.ts_len_ns/1000;
/* this would be done by the node */
if (nod_anode_initialize(&machine,2)) {
aerror("initializing node\n");
return NULL;
}
/* from here, this is the oesr_man interface only */
if (oesr_man_initialize(NULL,2)) {
aerror("initializing oesr_man\n");
return NULL;
}
memset(&waveform,0,sizeof(waveform_t));
c=0;
printf("\n\nList of commands:\n"
"\t<l>\tLoad waveform\n"
"\t<i>\tSet INIT\n"
"\t<r>\tSet RUN\n"
"\t<p>\tSet PAUSE\n"
"\t<t>\tSet STEP\n"
"\t<s>\tStop waveform\n"
"\t<m>\tSet waveform mode\n"
"\t<e>\tView execution time\n"
"\n<Ctr+C>\tExit\n");
waveform_status_t new_status;
do {
if (c != '\n') {
printf("\n>> ");
fflush(0);
}
c = getchar();
new_status.cur_status = LOADED;
switch((char) c) {
case 'm':
getchar();
print_modes(&waveform);
printf("\nEnter waveform mode (index): ");
if (scanf("%d",&mode) == -1) {
aerror("reading input\n");
break;
}
printf("\nSwitching to '%s'...\n",waveform.modes[mode].desc);
if (waveform_mode_set(&waveform,waveform.modes[mode].name)) {
aerror("setting waveform mode\n");
break;
}
break;
case 'l':
waveform_delete(&waveform);
strcpy(waveform.model_file,arg);
strcpy(waveform.name,arg);
if (waveform_parse(&waveform,1)) {
aerror("parsing waveform\n");
return NULL;
}
if (waveform_load(&waveform)) {
aerror("loading waveform\n");
break;
}
fflush(stdout);
printf("OK!\n");
break;
case 'i':
new_status.cur_status=INIT;
break;
case 'r':
new_status.cur_status=RUN;
break;
case 'p':
new_status.cur_status=PAUSE;
break;
case 't':
new_status.cur_status=STEP;
break;
case 's':
new_status.cur_status=STOP;
break;
case 'e':
if (waveform_update(&waveform)) {
aerror("updating waveform\n");
break;
}
if (print_execinfo(&waveform,tslen)) {
aerror("printing execinfo\n");
break;
}
break;
case '\n':
break;
default:
printf("Unknown command %c\n",(char) c);
break;
}
if (new_status.cur_status != LOADED) {
new_status.next_timeslot = rtdal_time_slot();
if (waveform_status_set(&waveform,&new_status)) {
printf("DID NOT CHANGE!\n");
} else {
printf("OK!\n");
}
}
} while(1);
/* status init */
/* pause o sleep o return */
printf("exit\n");
return NULL;
}
int _run_cycle(void* context) {
int i;
oesr_context_t *ctx = (oesr_context_t*) context;
nod_module_t *module = (nod_module_t*) ctx->module;
nod_waveform_t *waveform = (nod_waveform_t*) module->parent.waveform;
sdebug("context=0x%x, module_id=%d, changing_status=%d, cur_status=%d waveform_status=%d\n",context,
module->parent.id, module->changing_status, module->parent.status, waveform->status.cur_status);
if (waveform->status.cur_status == LOADED && module->parent.status == PARSED) {
/* ack we have successfully been loaded */
module->parent.status = LOADED;
/* register init and stop functions */
module->init = _call_init;
module->stop = _call_stop;
}
/* Change only if finished previous status change */
if (!module->changing_status && module->parent.status != waveform->status.cur_status) {
sdebug("next_tslot=%d, cur_tslot=%d\n",waveform->status.next_timeslot, rtdal_time_slot());
/* is it time to change? */
if (rtdal_time_slot() >= waveform->status.next_timeslot) {
switch(waveform->status.cur_status) {
case INIT:
case STOP:
break;
case STEP:
if (module->parent.status == RUN) {
module->parent.status = waveform->status.cur_status;
} else {
module->parent.status = RUN;
}
break;
case PAUSE:
case RUN:
/* These status does not need confirmation */
module->parent.status = waveform->status.cur_status;
break;
default:
break;
}
}
}
if (!module->changing_status && module->parent.status == RUN) {
#ifdef OESR_API_GETTIME
/* save start time */
rtdal_time_get(&module->parent.execinfo.t_exec[1]);
#endif
/* run aloe cycle */
for (i=0;i<waveform->tslot_multiplicity;i++) {
if (Run(context)) {
sdebug("RUNERROR: module_id=%d\n",module->parent.id);
/* set run-time error code */
if (rtdal_process_seterror(module->process,RUNERROR)) {
aerror("rtdal_process_seterror");
}
}
}
ctx->tstamp++;
/* save end time */
#ifdef OESR_API_GETTIME
rtdal_time_get(&module->parent.execinfo.t_exec[2]);
rtdal_time_interval(module->parent.execinfo.t_exec);
nod_module_execinfo_add_sample(&module->parent.execinfo);
if (DEBUG_TIMEMOD_ID == module->parent.id || DEBUG_TIMEMOD_ID == -1) {
tmdebug("%d,%d\n",module->parent.id,
module->parent.execinfo.t_exec[0].tv_usec);
}
#endif
/* compute execution time, exponential average, max, etc. and save data to mymodule.execinfo */
#ifdef kk
/* stat reports */
for (i=0;i<nof_reporting_vars;i++) {
module.reporting_variables[i].period_cnt++;
/* save the first window samples only */
if (module.reporting_variables[i].period_cnt++<module.reporting_variables[i].window)
module.reporting_variables[i].serialize(module.reporting_variables[i].report_packet);
}
/* send report every period */
if (module.reporting_variables[i].periodCnt++<module.reporting_variables[i].period) {
rtdal.newTask(report_variable,&reporting_variables[i]);
}
}
/* write logs */
for (i=0;i<nof_logs;i++) {
if (logs[i].w_ptr) {
rtdal.new_task(oesr_log._writelog,logs[i]);
}
}
#endif
}
int Run(void *_ctx) {
ctx = _ctx;
int tstamp = oesr_tstamp(ctx);
moddebug("enter ts=%d\n",oesr_tstamp(ctx));
int i;
int n;
if (ctrl_in) {
do {
n = oesr_itf_read(ctrl_in, &ctrl_in_buffer, CTRL_IN_BUFFER);
if (n == -1) {
oesr_perror("oesr_itf_read");
return -1;
} else if (n>0) {
if (process_ctrl_packet()) {
moderror("Error processing control packet\n");
return -1;
}
}
} while(n>0);
}
for (i=0;i<nof_input_itf;i++) {
if (!inputs[i]) {
input_ptr[i] = NULL;
rcv_len[i] = 0;
} else {
n = oesr_itf_ptr_get(inputs[i], &input_ptr[i], &rcv_len[i], tstamp);
if (n == 0) {
itfdebug("[ts=%d] received no input from %d\n",rtdal_time_slot(),i);
} else if (n == -1) {
oesr_perror("oesr_itf_get");
return -1;
} else {
itfdebug("[ts=%d] received %d bytes\n",rtdal_time_slot(),rcv_len[i]);
rcv_len[i] /= input_sample_sz;
}
}
}
for (i=0;i<nof_output_itf;i++) {
if (!outputs[i]) {
output_ptr[i] = NULL;
} else {
n = oesr_itf_ptr_request(outputs[i], &output_ptr[i]);
if (n == 0) {
/* moderror_msg("[ts=%d] no packets available in output interface %d\n",rtdal_time_slot(),i);
*/ } else if (n == -1) {
oesr_perror("oesr_itf_request");
return -1;
}
}
}
memset(snd_len,0,sizeof(int)*nof_output_itf);
#if MOD_DEBUG==1
oesr_counter_start(counter);
#endif
n = work(input_ptr,output_ptr);
#if MOD_DEBUG==1
oesr_counter_stop(counter);
moddebug("work exec time: %d us\n",oesr_counter_usec(counter));
#endif
if (n<0) {
return -1;
}
memset(rcv_len,0,sizeof(int)*nof_input_itf);
for (i=0;i<nof_output_itf;i++) {
if (!snd_len[i] && output_ptr[i]) {
snd_len[i] = n*output_sample_sz;
}
}
for (i=0;i<nof_input_itf;i++) {
if (input_ptr[i]) {
n = oesr_itf_ptr_release(inputs[i]);
if (n == 0) {
itfdebug("[ts=%d] packet from interface %d not released\n",rtdal_time_slot(),i);
} else if (n == -1) {
oesr_perror("oesr_itf_ptr_release\n");
return -1;
}
}
}
for (i=0;i<nof_output_itf;i++) {
if (output_ptr[i] && snd_len[i]) {
n = oesr_itf_ptr_put(outputs[i],snd_len[i],tstamp);
if (n == 0) {
itfdebug("[ts=%d] no space left in output interface %d\n",rtdal_time_slot(),i);
} else if (n == -1) {
oesr_perror("oesr_itf_ptr_put\n");
return -1;
}
}
}
moddebug("exit ts=%d\n",oesr_tstamp(ctx));
return 0;
}
