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;
}
/**
* Creates a new low-priority periodic function. If it succeeds, the function callback
* will be called every period timeslots with low priority.
*
* @param callback Pointer to the periodic function
* @param period Positive integer, in time slots
* @return zero on success, -1 on error
*/
int rtdal_periodic_add(void (*callback)(void), int period) {
assert(context);
RTDAL_ASSERT_PARAM(callback);
RTDAL_ASSERT_PARAM(period>0);
pthread_mutex_lock(&context->mutex);
int i;
for (i=0;i<MAX(rtdal_periodic);i++) {
if (!context->periodic[i].callback)
break;
}
if (i == MAX(rtdal_periodic)) {
RTDAL_SETERROR(RTDAL_ERROR_NOSPACE);
pthread_mutex_unlock(&context->mutex);
return -1;
}
context->periodic[i].counter = 0;
context->periodic[i].period = period;
context->periodic[i].callback = callback;
pthread_mutex_unlock(&context->mutex);
hdebug("i=%d, period=%d, callback=0x%x\n",i,period,callback);
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)) {
qdebug("[empty] read=%d, tstamp=%d\n",itf->read,itf->packets[itf->read].tstamp);
return 0;
}
if (itf->packets[itf->read].tstamp > tstamp) {
qdebug("[delay] read=%d, tstamp=%d\n",itf->read,itf->packets[itf->read].tstamp);
return 0;
}
if (itf->packets[itf->read].tstamp < tstamp && itf->parent.delay) {
rtdal_itfspscq_release(obj);
}
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;
}
/**
* Removes the process proc from the pipeline pointed by obj.
* @param obj Pointer to the pipeline object
* @param proc Pointer to the process to remove
* @return Zero on success, -1 on error
*/
int pipeline_remove(pipeline_t *obj, rtdal_process_t *proc) {
hdebug("pipeid=%d, nof_process=%d, pid=%d, pid_pos=%d\n",obj->id,obj->nof_processes,
proc->pid,proc->attributes.exec_position);
RTDAL_ASSERT_PARAM(obj);
RTDAL_ASSERT_PARAM(proc);
rtdal_process_t *cur, *prev;
prev = NULL;
cur = obj->first_process;
while(cur != proc && cur) {
hdebug("pipeid=%d, prev=0x%x, cur=0x%x\n", obj->id,
prev,cur);
prev = cur;
cur = cur->next;
}
if (!cur) {
RTDAL_SETERROR(RTDAL_ERROR_NOTFOUND);
return -1;
}
if (prev) {
hdebug("pipeid=%d remove middle/end\n",obj->id);
prev->next = cur->next;
} else {
hdebug("pipeid=%d remove first\n",obj->id);
obj->first_process = cur->next;
}
obj->nof_processes--;
proc->next = NULL;
return 0;
}
/**
* Disables the execution of the process identified by the first argument.
* The process must have been previously loaded using rtdal_process_new().
* @param process Process handler given by rtdal_process_new()
* @returns zero on success, -1 on error
*/
int rtdal_process_stop(r_proc_t process) {
RTDAL_ASSERT_PARAM(process);
rtdal_process_t *obj = (rtdal_process_t*) process;
hdebug("pid=%d\n",obj->pid);
obj->runnable = 0;
return 0;
}
/**
* Loads a process binary into memory. The process must have been created using
* rtdal_process_new(). This function loads the library defined in the process
* attributes during the call to rtdal_process_new().
* @param obj Pointer to the rtdal_process_t object.
* @return Zero on success, -1 on error.
*/
int rtdal_process_launch(rtdal_process_t *obj) {
hdebug("path=%s\n",obj->attributes.binary_path);
RTDAL_ASSERT_PARAM(obj);
char *error;
snprintf(tmp,LSTR_LEN,"/tmp/am_%d.so",obj->pid);
snprintf(tmp2,LSTR_LEN,"cp %s/%s %s",libs_path,obj->attributes.binary_path,tmp);
if (system(tmp2) == -1) {
aerror("Error removing file\n");
return -1;
}
obj->dl_handle = dlopen(tmp,RTLD_NOW);
if (!obj->dl_handle) {
RTDAL_DLERROR(dlerror());
return -1;
}
dlerror();
*(void**) (&obj->run_point) = dlsym(obj->dl_handle, "_run_cycle");
if ((error = dlerror()) != NULL) {
RTDAL_DLERROR(error);
return -1;
}
return 0;
}
/**
* Sets an error code for a process.
* \param proc Process handler given by rtdal_process_new()
* \returns zero on success, -1 on error
*/
int rtdal_process_seterror(r_proc_t proc, rtdal_processerrors_t code) {
RTDAL_ASSERT_PARAM(proc);
rtdal_process_t *obj = (rtdal_process_t*) proc;
hdebug("pid=%d, code=%d\n",obj->pid,(int)code);
obj->finish_code = code;
return 0;
}
/**
* Adds a process to the pipeline. It is inserted in the position
* min(n,exec_position) where n is the number of
* elements in the spscq and exec_position is defined in the process attributes
* used in the call to rtdal_process_new().
* Returns the position it has finally been inserted.
*
* @param obj Pointer to the pipeline_t object where the process is inserted
* @param process pointer to the rtdal_process_t object to insert.
* @returns non-negative integer number indicating the position it has been
* inserted, or -1 on error.
*/
int pipeline_add(pipeline_t *obj, rtdal_process_t *process) {
hdebug("pipeid=%d, nof_process=%d, pid=%d, exec_pos=%d\n",obj->id,obj->nof_processes,
process->pid,process->attributes.exec_position);
RTDAL_ASSERT_PARAM(obj);
RTDAL_ASSERT_PARAM(process);
int exec_pos, i;
rtdal_process_t *p = NULL;
exec_pos = process->attributes.exec_position;
/* head because empty list */
if (!obj->first_process) {
hdebug("pipeid=%d add pid=%d to head\n", obj->id, process->pid);
obj->first_process = process;
process->next = NULL;
goto end;
}
/* head because first exec position */
if (exec_pos < obj->first_process->attributes.exec_position) {
hdebug("pipeid=%d add pid=%d to head\n", obj->id, process->pid);
process->next = obj->first_process;
obj->first_process = process;
goto end;
}
/* middle */
i=0;
p = obj->first_process;
while(p->next && exec_pos < p->next->attributes.exec_position) {
p=p->next;
i++;
}
process->next = p->next;
p->next = process;
hdebug("pipeid=%d, add pid=%d to pos=%d\n", obj->id, process->pid,i);
end:
obj->nof_processes++;
/* assign pipeline to object */
process->pipeline = obj;
return i;
}
/**
* Acknowledges that the process group error notification has been processed, enabling another
* future call to the finish_callback function.
* \returns 0 on success, -1 on error
*/
int rtdal_process_group_notified(r_proc_t proc) {
RTDAL_ASSERT_PARAM(proc);
rtdal_process_t *obj = (rtdal_process_t*) proc;
hdebug("pid=%d, running=%d\n",obj->pid,obj->runnable);
if (obj->attributes.process_group_id < 0 || obj->attributes.process_group_id > MAX_PROCESS_GROUP_ID) {
RTDAL_SETERROR(RTDAL_ERROR_INVAL);
return -1;
}
pgroup_notified_failure[obj->attributes.process_group_id] = 0;
return 0;
}
int rtdal_itfspscq_send(r_itf_t obj, void* buffer, int len, int tstamp) {
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;
}
if ((n = rtdal_itfspscq_request(obj, &ptr)) != 1) {
return n;
}
memcpy(ptr, buffer, (size_t) len);
return rtdal_itfspscq_push(obj,ptr,len,tstamp);
}
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());
}
int rtdal_itfspscq_recv(r_itf_t obj, void* buffer, int len, int tstamp) {
RTDAL_ASSERT_PARAM(buffer);
RTDAL_ASSERT_PARAM(len>=0);
int n, plen;
void *ptr=NULL;
do {
if ((n = rtdal_itfspscq_pop(obj, &ptr, &plen, tstamp)) < 1) {
return n;
}
} while (n == 2);
if (plen > len) {
plen = len;
}
memcpy(buffer, ptr, (size_t) plen);
if ((n = rtdal_itfspscq_release(obj,NULL,0)) != 1) {
printf("Caution packet could not be released (%d)\n",n);
}
return plen;
}
/** 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_request(r_itf_t obj, void **ptr) {
cast(obj,itf);
RTDAL_ASSERT_PARAM(ptr);
*ptr = NULL;
qdebug("write=%d, tstamp=%d\n",itf->write,itf->packets[itf->write].tstamp);
if (spscq_is_full(itf)) {
RTDAL_SETERROR(RTDAL_ERROR_NOSPACE);
return 0;
}
qdebug("[ok] write=%d/%d, addr=0x%x\n",itf->write,itf->max_msg,
itf->packets[itf->write].data);
*ptr = itf->packets[itf->write].data;
return 1;
}
int rtdal_itfspscq_init(rtdal_itfspscq_t *itf) {
RTDAL_ASSERT_PARAM(itf);
int i;
itf->parent.is_external = 0;
itf->read = 0;
itf->write = 0;
itf->data = malloc(itf->max_msg*itf->max_msg_sz);
itf->packets = calloc(itf->max_msg,sizeof(r_pkt_t));
if (!itf->data || !itf->packets) {
return -1;
}
for (i=0;i<itf->max_msg;i++) {
itf->packets[i].data = &itf->data[i*itf->max_msg_sz];
itf->packets[i].valid = 0;
}
return 0;
}
int rtdal_itfspscq_request(r_itf_t obj, void **ptr) {
cast(obj,itf);
RTDAL_ASSERT_PARAM(ptr);
*ptr = NULL;
qdebug("write=%d read=%d\n",itf->write,itf->read);
if (spscq_is_full(itf)) {
qdebug("[full] id=%d write=%d, valid=%d\n",itf->parent.id,itf->write,itf->packets[itf->write].valid);
RTDAL_SETERROR(RTDAL_ERROR_NOSPACE);
return 0;
}
qdebug("[ok] write=%d/%d\n",itf->write,itf->max_msg);
*ptr = itf->packets[itf->write].data;
return 1;
}
/**
* Removes a process from the system, previously loaded using rtdal_process_new().
* rtdal_process_remove() removes the process from the allocated pipeline and
* then unloads the program code from memory. After, if a finish_callback function
* was provided in the process attributes during rtdal_process_new(), it is now
* called.
* @param obj Pointer to the rtdal_process_t object.
* @return Zero on success, -1 on error.
*/
int rtdal_process_remove(r_proc_t process) {
RTDAL_ASSERT_PARAM(process);
rtdal_process_t *obj = (rtdal_process_t*) process;
hdebug("pid=%d\n",obj->pid)
if (pipeline_remove((pipeline_t*) obj->pipeline, obj)) {
return -1;
}
dlclose(obj->dl_handle);
snprintf(tmp,LSTR_LEN,"/tmp/am_%d.so",obj->pid);
snprintf(tmp2,LSTR_LEN,"rm %s",tmp);
if (system(tmp2) == -1) {
aerror("Error removing file\n");
return -1;
}
obj->pid = 0;
return 0;
}
/**
* Removes the function pointed by callback from the kernel periodic callback functions,
* previously added with rtdal_periodic_add().
*
* @param callback Pointer to the periodic function
* @returns zero on success, -1 on error
*/
int rtdal_periodic_remove(void (*callback)(void)) {
assert(context);
RTDAL_ASSERT_PARAM(callback);
int i;
pthread_mutex_lock(&context->mutex);
for (i=0;i<MAX(rtdal_periodic);i++) {
if (context->periodic[i].callback == callback)
break;
}
if (i == MAX(rtdal_periodic)) {
RTDAL_SETERROR(RTDAL_ERROR_NOTFOUND);
return -1;
}
context->periodic[i].counter = 0;
context->periodic[i].period = 0;
context->periodic[i].callback = NULL;
pthread_mutex_unlock(&context->mutex);
hdebug("i=%d\n",i);
return 0;
}
/**
* Returns the id of the process identified by the first argument.
* The process must have been previously loaded using rtdal_process_new().
* @param process Handler to the process
* @returns non-negative id on success, -1 on error
*/
int rtdal_process_pid(r_proc_t process) {
RTDAL_ASSERT_PARAM(process);
rtdal_process_t *obj = (rtdal_process_t*) process;
return obj->pid;
}
/**
* Returns 1 if the process is running or zero otherwise
*/
int rtdal_process_isrunning(r_proc_t proc) {
RTDAL_ASSERT_PARAM(proc);
rtdal_process_t *obj = (rtdal_process_t*) proc;
hdebug("pid=%d, running=%d\n",obj->pid,obj->runnable);
return obj->is_running;
}