void __rtai_shm_exit (void)
{
extern int max_slots;
int slot;
struct rt_registry_entry entry;
#ifdef CONFIG_RTAI_MALLOC_VMALLOC
rt_drg_on_name_cnt(GLOBAL_HEAP_ID);
#endif
for (slot = 1; slot <= max_slots; slot++) {
if (rt_get_registry_slot(slot, &entry)) {
if (abs(entry.type) >= PAGE_SIZE) {
char name[8];
while (_rt_shm_free(entry.name, entry.type));
num2nam(entry.name, name);
rt_printk("\nSHM_CLEANUP_MODULE releases: '%s':0x%lx:%lu (%d).\n", name, entry.name, entry.name, entry.type);
}
}
}
reset_rt_fun_entries(rt_shm_entries);
misc_deregister(&rtai_shm_dev);
#if USE_UDEV_CLASS
class_device_destroy(shm_class, MKDEV(MISC_MAJOR, RTAI_SHM_MISC_MINOR));
class_destroy(shm_class);
#endif
return;
}
/*
* _shm_alloc allocs chunk from Fusion kheap or alloc a new heap
*/
void *_shm_alloc(unsigned long name, int size, int suprt, int in_kheap,
unsigned long *opaque)
{
void *ret = NULL;
xnholder_t *holder;
xnshm_a_t *p;
spl_t s;
xnlock_get_irqsave(&nklock, s);
holder = getheadq(&xnshm_allocq);
while (holder != NULL) {
p = link2shma(holder);
if (p->name == name) {
/* assert(size==p->size); */
p->ref++;
ret = p->chunk;
*opaque = (unsigned long)p->heap;
goto unlock_and_exit;
}
holder = nextq(&xnshm_allocq, holder);
}
if (in_kheap) {
p = kalloc_new_shm(name, size);
} else {
/* create new heap can suspend */
xnlock_put_irqrestore(&nklock, s);
p = create_new_heap(name, size, suprt);
xnlock_get_irqsave(&nklock, s);
}
if (!p)
goto unlock_and_exit;
*opaque = (unsigned long)p->heap;
appendq(&xnshm_allocq, &p->link);
#ifdef CONFIG_XENO_OPT_REGISTRY
{
p->handle = 0;
num2nam(p->name, p->szName);
xnregistry_enter(p->szName, p, &p->handle, &__shm_pnode);
}
#endif /* CONFIG_XENO_OPT_REGISTRY */
ret = p->chunk;
unlock_and_exit:
xnlock_put_irqrestore(&nklock, s);
return ret;
}
INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task,
int priority,
const char* name,
int sched_type,
size_t stack_size,
void * (*start_routine)(void *),
ThreadInterface* obj)
{
char taskName[7];
if ( strlen(name) == 0 )
name = "Thread";
strncpy(taskName, name, 7);
unsigned long task_num = nam2num( taskName );
if ( rt_get_adr(nam2num( taskName )) != 0 ) {
unsigned long nname = nam2num( taskName );
while ( rt_get_adr( nname ) != 0 ) // check for existing 'NAME'
++nname;
num2nam( nname, taskName); // set taskName to new name
taskName[6] = 0;
task_num = nname;
}
// Set and truncate name
task->name = strcpy( (char*)malloc( (strlen(name)+1)*sizeof(char) ), name);
// name, priority, stack_size, msg_size, policy, cpus_allowed ( 1111 = 4 first cpus)
// Set priority
task->priority = priority;
// Set rtai task struct to zero
task->rtaitask = 0;
RTAI_Thread* rt = (RTAI_Thread*)malloc( sizeof(RTAI_Thread) );
rt->priority = priority;
rt->data = obj;
rt->wrapper = start_routine;
rt->task = task;
rt->tnum = task_num;
int retv = pthread_create(&(task->thread), 0,
rtai_thread_wrapper, rt);
// poll for thread creation to be done.
int timeout = 0;
while ( task->rtaitask == 0 && ++timeout < 20)
usleep(100000);
return timeout < 20 ? retv : -1;
}
void __rtai_shm_pkg_cleanup(void)
{
#if 0
xnholder_t *holder;
xnshm_a_t *p;
char szName[6];
// Garbage collector : to be added : lock problem
holder = getheadq(&xnshm_allocq);
while (holder != NULL) {
p = link2shma(holder);
if (p) {
num2nam(p->name, szName);
printk
("[RTAI -SHM] Cleanup of unfreed memory %s( %d ref.)\n",
szName, p->ref);
if (p->heap == &kheap)
xnheap_free(&kheap, p->chunk);
else {
/* FIXME: MUST release lock here.
*/
#ifdef CONFIG_XENO_OPT_PERVASIVE
xnheap_destroy_mapped(p->heap, NULL, NULL);
#else /* !CONFIG_XENO_OPT_PERVASIVE */
xnheap_destroy(p->heap, &__heap_flush_private,
NULL);
#endif /* !CONFIG_XENO_OPT_PERVASIVE */
xnheap_free(&kheap, p->heap);
}
removeq(&xnshm_allocq, &p->link);
xnheap_free(&kheap, p);
}
holder = nextq(&xnshm_allocq, holder);
}
#endif
}
static void *rt_HostInterface(void *args)
{
RT_TASK *task;
unsigned int Request;
int Reply, len;
char nome[8];
if (!(rt_HostInterfaceTask = rt_task_init_schmod(nam2num(HostInterfaceTaskName), rt_HostInterfaceTaskPriority, 0, 0, SCHED_RR, 0xFF))) {
fprintf(stderr,"Cannot init rt_HostInterfaceTask.\n");
return (void *)1;
}
sem_post(&err_sem);
while (!endInterface) {
task = rt_receive(0, &Request);
num2nam(rt_get_name(task),nome);
if (endInterface) break;
switch (Request & 0xFF) {
case 'c': {
int i ,Idx, idx[2];
rtTargetParamInfo rtParam;
float samplingTime;
int NPAR,i1,i2;
mxp_t pardata;
double value;
strncpyz(rtParam.modelName, modelname, MAX_NAME_SIZE);
rtParam.dataType = 0;
NPAR = mxp_getnvars();
rt_return(task, (isRunning << 16) | ( NPAR & 0xFFFF ));
rt_receivex(task, &Request, 1, &len);
rt_returnx(task, &rtParam, sizeof(rtParam));
for (i = 0; i < NPAR; i++) {
sprintf(rtParam.blockName,"%s/%s",rtParam.modelName,"Tunable Parameters");
mxp_getvarbyidx(i, &pardata);
if ( pardata.ndim == 0 ){
rt_receivex(task,&Request,1,&len);
sprintf(rtParam.paramName, pardata.name);
mxp_getparam(i,idx, &value);
rtParam.dataValue[0] = value;
rtParam.dataClass = rt_SCALAR;
rtParam.nRows = 1;
rtParam.nCols = 1;
rt_returnx(task, &rtParam, sizeof(rtParam));
}
if ( pardata.ndim == 1 ){
rt_receivex(task,&Request,1,&len);
sprintf(rtParam.paramName, pardata.name);
rtParam.dataClass = rt_VECTOR;
rtParam.nRows = 1;
rtParam.nCols = pardata.dim[0];
for (i1 = 0; i1 < pardata.dim[0] ; i1++){
idx[0] = i1;
mxp_getparam(i,idx, &value);
rtParam.dataValue[i1] = value;
}
rt_returnx(task, &rtParam, sizeof(rtParam));
}
if ( pardata.ndim == 2 ){
rt_receivex(task,&Request,1,&len);
sprintf(rtParam.paramName, pardata.name);
rtParam.dataClass = rt_MATRIX_ROW_MAJOR;
rtParam.nRows = pardata.dim[0];
rtParam.nCols = pardata.dim[1];
for (i1 = 0; i1 < pardata.dim[0] ; i1++){
for (i2 = 0; i2 < pardata.dim[1] ; i2++){
idx[0] = i1;
idx[1] = i2;
mxp_getparam(i,idx, &value);
rtParam.dataValue[i1*rtParam.nCols+i2] = value;
}
}
rt_returnx(task, &rtParam, sizeof(rtParam));
}
if (pardata.ndim > 2){
fprintf(stderr,"MAX PARAMETER DIMESION = 2........\n");
}
}
while (1) {
rt_receivex(task, &Idx, sizeof(int), &len);
if (Idx < 0) {
rt_returnx(task, &Idx, sizeof(int));
break;
} else {
rt_returnx(task, &rtaiScope[Idx].ntraces, sizeof(int));
rt_receivex(task, &Idx, sizeof(int), &len);
rt_returnx(task, rtaiScope[Idx].name, MAX_NAME_SIZE);
rt_receivex(task, &Idx, sizeof(int), &len);
samplingTime = get_tsamp();
rt_returnx(task, &samplingTime, sizeof(float));
}
}
while (1) {
rt_receivex(task, &Idx, sizeof(int), &len);
if (Idx < 0) {
rt_returnx(task, &Idx, sizeof(int));
break;
} else {
rt_returnx(task, &rtaiLogData[Idx].nrow, sizeof(int));
rt_receivex(task, &Idx, sizeof(int), &len);
rt_returnx(task, &rtaiLogData[Idx].ncol, sizeof(int));
rt_receivex(task, &Idx, sizeof(int), &len);
rt_returnx(task, rtaiLogData[Idx].name, MAX_NAME_SIZE);
rt_receivex(task, &Idx, sizeof(int), &len);
samplingTime = get_tsamp();
rt_returnx(task, &samplingTime, sizeof(float));
}
}
while (1) {
rt_receivex(task, &Idx, sizeof(int), &len);
if (Idx < 0) {
rt_returnx(task, &Idx, sizeof(int));
break;
} else {
rt_returnx(task, &rtaiALogData[Idx].nrow, sizeof(int));
rt_receivex(task, &Idx, sizeof(int), &len);
rt_returnx(task, &rtaiALogData[Idx].ncol, sizeof(int));
rt_receivex(task, &Idx, sizeof(int), &len);
rt_returnx(task, rtaiALogData[Idx].name, MAX_NAME_SIZE);
rt_receivex(task, &Idx, sizeof(int), &len);
samplingTime = get_tsamp();
rt_returnx(task, &samplingTime, sizeof(float));
}
}
while (1) {
rt_receivex(task, &Idx, sizeof(int), &len);
if (Idx < 0) {
rt_returnx(task, &Idx, sizeof(int));
break;
} else {
rt_returnx(task, &rtaiLed[Idx].nleds, sizeof(int));
rt_receivex(task, &Idx, sizeof(int), &len);
rt_returnx(task, rtaiLed[Idx].name, MAX_NAME_SIZE);
rt_receivex(task, &Idx, sizeof(int), &len);
samplingTime = get_tsamp();
rt_returnx(task, &samplingTime, sizeof(float));
}
}
while (1) {
rt_receivex(task, &Idx, sizeof(int), &len);
if (Idx < 0) {
rt_returnx(task, &Idx, sizeof(int));
break;
} else {
rt_returnx(task, rtaiMeter[Idx].name, MAX_NAME_SIZE);
rt_receivex(task, &Idx, sizeof(int), &len);
samplingTime = get_tsamp();
rt_returnx(task, &samplingTime, sizeof(float));
}
}
while (1) {
rt_receivex(task, &Idx, sizeof(int), &len);
if (Idx < 0) {
rt_returnx(task, &Idx, sizeof(int));
break;
} else {
rt_returnx(task, "", MAX_NAME_SIZE);
rt_receivex(task, &Idx, sizeof(int), &len);
samplingTime = get_tsamp();
rt_returnx(task, &samplingTime, sizeof(float));
}
}
break;
}
case 's': {
rt_task_resume(rt_MainTask);
rt_return(task, 1);
break;
}
case 't': {
endex = 1;
rt_return(task, 0);
break;
}
case 'p': {
int index;
double param;
int mat_ind,Idx[2];
mxp_t pardata;
rt_return(task, isRunning);
rt_receivex(task, &index, sizeof(int), &len);
Reply = 0;
rt_returnx(task, &Reply, sizeof(int));
rt_receivex(task, ¶m, sizeof(double), &len);
Reply = 1;
rt_returnx(task, &Reply, sizeof(int));
rt_receivex(task, &mat_ind, sizeof(int), &len);
mxp_getvarbyidx(index, &pardata);
if ( pardata.ndim == 1 ) Idx[0] = mat_ind;
if ( pardata.ndim == 2 ){
Idx[0] = mat_ind/pardata.dim[1];
Idx[1] = mat_ind - Idx[0]*pardata.dim[1];
}
mxp_setparam(index, Idx, param);
rt_returnx(task, &Reply, sizeof(int));
break;
}
case 'g': {
int i, idx[2], i1,i2;
rtTargetParamInfo rtParam;
int NPAR;
mxp_t pardata;
double value;
strncpyz(rtParam.modelName, modelname, MAX_NAME_SIZE);
rtParam.dataType = 0;
NPAR = mxp_getnvars();
rt_return(task, isRunning);
for (i = 0; i < NPAR; i++) {
sprintf(rtParam.blockName,"%s/%s",rtParam.modelName,"Tunable Parameters");
mxp_getvarbyidx(i, &pardata);
if ( pardata.ndim == 0 ){
rt_receivex(task,&Request,1,&len);
sprintf(rtParam.paramName, pardata.name);
mxp_getparam(i,idx, &value);
rtParam.dataValue[0] = value;
rtParam.dataClass = rt_SCALAR;
rtParam.nRows = 1;
rtParam.nCols = 1;
rt_returnx(task, &rtParam, sizeof(rtParam));
}
if ( pardata.ndim == 1 ){
rt_receivex(task,&Request,1,&len);
sprintf(rtParam.paramName, pardata.name);
rtParam.dataClass = rt_VECTOR;
rtParam.nRows = 1;
rtParam.nCols = pardata.dim[0];
for (i1 = 0; i1 < pardata.dim[0] ; i1++){
idx[0] = i1;
mxp_getparam(i,idx, &value);
rtParam.dataValue[i1] = value;
}
rt_returnx(task, &rtParam, sizeof(rtParam));
}
if ( pardata.ndim == 2 ){
rt_receivex(task,&Request,1,&len);
sprintf(rtParam.paramName, pardata.name);
rtParam.dataClass = rt_MATRIX_ROW_MAJOR;
rtParam.nRows = pardata.dim[0];
rtParam.nCols = pardata.dim[1];
for (i1 = 0; i1 < pardata.dim[0] ; i1++){
for (i2 = 0; i2 < pardata.dim[1] ; i2++){
idx[0] = i1;
idx[1] = i2;
mxp_getparam(i,idx, &value);
rtParam.dataValue[i1*rtParam.nCols+i2] = value;
}
}
rt_returnx(task, &rtParam, sizeof(rtParam));
}
}
break;
}
case 'd': {
int ParamCnt;
int Idx[2];
mxp_t pardata;
rt_return(task, isRunning);
rt_receivex(task, &ParamCnt, sizeof(int), &len);
Reply = 0;
rt_returnx(task, &Reply, sizeof(int));
{
struct {
int index;
int mat_ind;
double value;
} Params[ParamCnt];
int i;
rt_receivex(task, &Params, sizeof(Params), &len);
for (i = 0; i < ParamCnt; i++) {
mxp_getvarbyidx(Params[i].index, &pardata);
if ( pardata.ndim == 1 ) Idx[0] = Params[i].mat_ind;
if ( pardata.ndim == 2 ){
Idx[0] = Params[i].mat_ind/pardata.dim[1];
Idx[1] = Params[i].mat_ind - Idx[0]*pardata.dim[1];
}
mxp_setparam(Params[i].index, Idx, Params[i].value);
}
}
Reply = 1;
rt_returnx(task, &Reply, sizeof(int));
break;
}
case 'm': {
float time = SIM_TIME;
rt_return(task, isRunning);
rt_receivex(task, &Reply, sizeof(int), &len);
rt_returnx(task, &time, sizeof(float));
break;
}
case 'b': {
rt_return(task, (unsigned int)rt_BaseRateTask);
break;
}
default : {
break;
}
}
}
rt_task_delete(rt_HostInterfaceTask);
return 0;
}