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; }