int rtos_sem_init(rt_sem_t* m, int value )
{
CHK_LXRT_CALL();
// store the pointer in m->opaque...
m->sem = rt_sem_init( rt_get_name(0) , value);
return m->sem == 0 ? -1 : 0;
}
int rtos_mutex_rec_init(rt_mutex_t* m)
{
CHK_LXRT_CALL();
// RES_SEM is PRIO_Q anyhow.
m->sem = rt_typed_sem_init( rt_get_name(0), 1, RES_SEM);
return m->sem == 0 ? -1 : 0;
}
void rt_named_free(void *adr)
{
unsigned long name;
name = rt_get_name(adr);
if (!rt_drg_on_name_cnt(name)) {
_rt_hfree(adr, &rt_smp_linux_task->heap[GLOBAL]);
}
}
static inline void rt_named_hfree_typed(void *adr, int htype)
{
RT_TASK *task;
unsigned long name;
RTAI_TASK(return);
name = rt_get_name(task->heap[htype].kadr + (adr - task->heap[htype].uadr));
if (!rt_drg_on_name_cnt(name)) {
_rt_hfree(adr, &task->heap[htype]);
}
}
int main(void)
{
RT_TASK *mytask;
int smbx, rmbx[NTASKS];
int i, bthread, mthread[NTASKS];
char msg[] = "let's end the game";
mytask = rt_thread_init(nam2num("MAIN"), 2, 0, SCHED_FIFO, 0xF);
barrier = rt_sem_init(rt_get_name(0), NTASKS + 1);
smbx = rt_msgget(nam2num("SMSG"), 0x666 | IPC_CREAT);
for (i = 0; i < NTASKS; i++) {
char mname[6] = "RMBX";
mname[4] = i + '0';
mname[5] = 0;
rmbx[i] = rt_msgget(nam2num(mname), 0x666 | IPC_CREAT);
}
rt_set_oneshot_mode();
start_rt_timer(0);
bthread = rt_thread_create(bfun, 0, 0x8000);
for (i = 0; i < NTASKS; i++) {
mthread[i] = rt_thread_create(mfun, (void *)i, 0x8000);
}
printf("IF NOTHING HAPPENS IS OK, TYPE ENTER TO FINISH.\n");
getchar();
for (i = 0; i < NTASKS; i++) {
end = i;
rt_msgsnd_nu(rmbx[i], 1, msg, sizeof(msg), 0);
rt_thread_join(mthread[i]);
}
end = NTASKS;
rt_msgsnd_nu(smbx, 1, msg, sizeof(msg), 0);
rt_thread_join(bthread);
for (i = 0; i < NTASKS; i++) {
rt_msgctl(rmbx[i], IPC_RMID, NULL);
printf("TASK %d, LOOPS: %d.\n", i, cnt[i]);
}
rt_msgctl(smbx, IPC_RMID, NULL);
stop_rt_timer();
rt_sem_delete(barrier);
rt_task_delete(mytask);
printf("MAIN TASK ENDS.\n");
return 0;
}
static inline long long handle_lxrt_request (unsigned int lxsrq, long *arg, RT_TASK *task)
{
#define larg ((struct arg *)arg)
union {unsigned long name; RT_TASK *rt_task; SEM *sem; MBX *mbx; RWL *rwl; SPL *spl; int i; void *p; long long ll; } arg0;
int srq;
if (likely((srq = SRQ(lxsrq)) < MAX_LXRT_FUN)) {
unsigned long type;
struct rt_fun_entry *funcm;
/*
* The next two lines of code do a lot. It makes possible to extend the use of
* USP to any other real time module service in user space, both for soft and
* hard real time. Concept contributed and copyrighted by: Giuseppe Renoldi
* ([email protected]).
*/
if (unlikely(!(funcm = rt_fun_ext[INDX(lxsrq)]))) {
rt_printk("BAD: null rt_fun_ext, no module for extension %d?\n", INDX(lxsrq));
return -ENOSYS;
}
if (!(type = funcm[srq].type)) {
return ((RTAI_SYSCALL_MODE long long (*)(unsigned long, ...))funcm[srq].fun)(RTAI_FUN_ARGS);
}
if (unlikely(NEED_TO_RW(type))) {
lxrt_fun_call_wbuf(task, funcm[srq].fun, NARG(lxsrq), arg, type);
} else {
lxrt_fun_call(task, funcm[srq].fun, NARG(lxsrq), arg);
}
return task->retval;
}
arg0.name = arg[0];
switch (srq) {
case LXRT_GET_ADR: {
arg0.p = rt_get_adr(arg0.name);
return arg0.ll;
}
case LXRT_GET_NAME: {
arg0.name = rt_get_name(arg0.p);
return arg0.ll;
}
case LXRT_TASK_INIT: {
struct arg { unsigned long name; long prio, stack_size, max_msg_size, cpus_allowed; };
arg0.rt_task = __task_init(arg0.name, larg->prio, larg->stack_size, larg->max_msg_size, larg->cpus_allowed);
return arg0.ll;
}
case LXRT_TASK_DELETE: {
arg0.i = __task_delete(arg0.rt_task ? arg0.rt_task : task);
return arg0.ll;
}
case LXRT_SEM_INIT: {
if (rt_get_adr(arg0.name)) {
return 0;
}
if ((arg0.sem = rt_malloc(sizeof(SEM)))) {
struct arg { unsigned long name; long cnt; long typ; };
lxrt_typed_sem_init(arg0.sem, larg->cnt, larg->typ);
if (rt_register(larg->name, arg0.sem, IS_SEM, current)) {
return arg0.ll;
} else {
rt_free(arg0.sem);
}
}
return 0;
}
case LXRT_SEM_DELETE: {
if (lxrt_sem_delete(arg0.sem)) {
arg0.i = -EFAULT;
return arg0.ll;
}
rt_free(arg0.sem);
arg0.i = rt_drg_on_adr(arg0.sem);
return arg0.ll;
}
case LXRT_MBX_INIT: {
if (rt_get_adr(arg0.name)) {
return 0;
}
if ((arg0.mbx = rt_malloc(sizeof(MBX)))) {
struct arg { unsigned long name; long size; int qtype; };
if (lxrt_typed_mbx_init(arg0.mbx, larg->size, larg->qtype) < 0) {
rt_free(arg0.mbx);
return 0;
}
if (rt_register(larg->name, arg0.mbx, IS_MBX, current)) {
return arg0.ll;
} else {
rt_free(arg0.mbx);
}
}
return 0;
}
case LXRT_MBX_DELETE: {
if (lxrt_mbx_delete(arg0.mbx)) {
arg0.i = -EFAULT;
return arg0.ll;
}
rt_free(arg0.mbx);
arg0.i = rt_drg_on_adr(arg0.mbx);
return arg0.ll;
}
case LXRT_RWL_INIT: {
if (rt_get_adr(arg0.name)) {
return 0;
}
if ((arg0.rwl = rt_malloc(sizeof(RWL)))) {
struct arg { unsigned long name; long type; };
lxrt_typed_rwl_init(arg0.rwl, larg->type);
if (rt_register(larg->name, arg0.rwl, IS_SEM, current)) {
return arg0.ll;
} else {
rt_free(arg0.rwl);
}
}
return 0;
}
case LXRT_RWL_DELETE: {
if (lxrt_rwl_delete(arg0.rwl)) {
arg0.i = -EFAULT;
return arg0.ll;
}
rt_free(arg0.rwl);
arg0.i = rt_drg_on_adr(arg0.rwl);
return arg0.ll;
}
case LXRT_SPL_INIT: {
if (rt_get_adr(arg0.name)) {
return 0;
}
if ((arg0.spl = rt_malloc(sizeof(SPL)))) {
struct arg { unsigned long name; };
lxrt_spl_init(arg0.spl);
if (rt_register(larg->name, arg0.spl, IS_SEM, current)) {
return arg0.ll;
} else {
rt_free(arg0.spl);
}
}
return 0;
}
case LXRT_SPL_DELETE: {
if (lxrt_spl_delete(arg0.spl)) {
arg0.i = -EFAULT;
return arg0.ll;
}
rt_free(arg0.spl);
arg0.i = rt_drg_on_adr(arg0.spl);
return arg0.ll;
}
case MAKE_HARD_RT: {
rt_make_hard_real_time(task);
return 0;
if (!task || task->is_hard) {
return 0;
}
steal_from_linux(task);
return 0;
}
case MAKE_SOFT_RT: {
rt_make_soft_real_time(task);
return 0;
if (!task || !task->is_hard) {
return 0;
}
if (task->is_hard < 0) {
task->is_hard = 0;
} else {
give_back_to_linux(task, 0);
}
return 0;
}
case PRINT_TO_SCREEN: {
struct arg { char *display; long nch; };
arg0.i = rtai_print_to_screen("%s", larg->display);
return arg0.ll;
}
case PRINTK: {
struct arg { char *display; long nch; };
arg0.i = rt_printk("%s", larg->display);
return arg0.ll;
}
case NONROOT_HRT: {
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
current->cap_effective |= ((1 << CAP_IPC_LOCK) |
(1 << CAP_SYS_RAWIO) |
(1 << CAP_SYS_NICE));
#else
set_lxrt_perm(CAP_IPC_LOCK);
set_lxrt_perm(CAP_SYS_RAWIO);
set_lxrt_perm(CAP_SYS_NICE);
#endif
return 0;
}
case RT_BUDDY: {
arg0.rt_task = task && current->rtai_tskext(TSKEXT1) == current ? task : NULL;
return arg0.ll;
}
case HRT_USE_FPU: {
struct arg { RT_TASK *task; long use_fpu; };
if(!larg->use_fpu) {
clear_lnxtsk_uses_fpu((larg->task)->lnxtsk);
} else {
init_fpu((larg->task)->lnxtsk);
}
return 0;
}
case GET_USP_FLAGS: {
arg0.name = arg0.rt_task->usp_flags;
return arg0.ll;
}
case SET_USP_FLAGS: {
struct arg { RT_TASK *task; unsigned long flags; };
arg0.rt_task->usp_flags = larg->flags;
arg0.rt_task->force_soft = (arg0.rt_task->is_hard > 0) && (larg->flags & arg0.rt_task->usp_flags_mask & FORCE_SOFT);
return 0;
}
case GET_USP_FLG_MSK: {
arg0.name = arg0.rt_task->usp_flags_mask;
return arg0.ll;
}
case SET_USP_FLG_MSK: {
task->usp_flags_mask = arg0.name;
task->force_soft = (task->is_hard > 0) && (task->usp_flags & arg0.name & FORCE_SOFT);
return 0;
}
case FORCE_TASK_SOFT: {
extern void rt_do_force_soft(RT_TASK *rt_task);
struct task_struct *ltsk;
if ((ltsk = find_task_by_pid(arg0.name))) {
if ((arg0.rt_task = ltsk->rtai_tskext(TSKEXT0))) {
if ((arg0.rt_task->force_soft = (arg0.rt_task->is_hard != 0) && FORCE_SOFT)) {
rt_do_force_soft(arg0.rt_task);
}
return arg0.ll;
}
}
return 0;
}
case IS_HARD: {
arg0.i = arg0.rt_task || (arg0.rt_task = current->rtai_tskext(TSKEXT0)) ? arg0.rt_task->is_hard : 0;
return arg0.ll;
}
case GET_EXECTIME: {
struct arg { RT_TASK *task; RTIME *exectime; };
if ((larg->task)->exectime[0] && (larg->task)->exectime[1]) {
larg->exectime[0] = (larg->task)->exectime[0];
larg->exectime[1] = (larg->task)->exectime[1];
larg->exectime[2] = rtai_rdtsc();
}
return 0;
}
case GET_TIMEORIG: {
struct arg { RTIME *time_orig; };
if (larg->time_orig) {
RTIME time_orig[2];
rt_gettimeorig(time_orig);
rt_copy_to_user(larg->time_orig, time_orig, sizeof(time_orig));
} else {
rt_gettimeorig(NULL);
}
return 0;
}
case LINUX_SERVER: {
struct arg { struct linux_syscalls_list syscalls; };
if (larg->syscalls.nr) {
if (larg->syscalls.task->linux_syscall_server) {
RT_TASK *serv;
rt_get_user(serv, &larg->syscalls.serv);
rt_task_masked_unblock(serv, ~RT_SCHED_READY);
}
larg->syscalls.task->linux_syscall_server = larg->syscalls.serv;
rtai_set_linux_task_priority(current, (larg->syscalls.task)->lnxtsk->policy, (larg->syscalls.task)->lnxtsk->rt_priority);
arg0.rt_task = __task_init((unsigned long)larg->syscalls.task, larg->syscalls.task->base_priority >= BASE_SOFT_PRIORITY ? larg->syscalls.task->base_priority - BASE_SOFT_PRIORITY : larg->syscalls.task->base_priority, 0, 0, 1 << larg->syscalls.task->runnable_on_cpus);
larg->syscalls.task->linux_syscall_server = arg0.rt_task;
arg0.rt_task->linux_syscall_server = larg->syscalls.serv;
return arg0.ll;
} else {
if (!larg->syscalls.task) {
larg->syscalls.task = RT_CURRENT;
}
if ((arg0.rt_task = larg->syscalls.task->linux_syscall_server)) {
larg->syscalls.task->linux_syscall_server = NULL;
arg0.rt_task->suspdepth = -RTE_HIGERR;
rt_task_masked_unblock(arg0.rt_task, ~RT_SCHED_READY);
}
}
return 0;
}
default: {
rt_printk("RTAI/LXRT: Unknown srq #%d\n", srq);
arg0.i = -ENOSYS;
return arg0.ll;
}
}
return 0;
}
int rtos_mutex_init(rt_mutex_t* m)
{
CHK_LXRT_CALL();
m->sem = rt_typed_sem_init( rt_get_name(0),1, BIN_SEM | PRIO_Q);
return m->sem == 0 ? -1 : 0;
}
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;
}
