static void bfun(int t)
{
unsigned long long msg;
unsigned long long name = 0xccccccccccccccccLL;
while (1) {
cpu_used[hard_cpu_id()]++;
msg = 0LL;
bstat = 'r';
rt_mbx_receive(&smbx, &msg, sizeof(msg));
if (msg == 0x1122334455667788LL) {
t = 0;
} else {
if (msg == 0x99aabbccddeeff00LL) {
t = 1;
} else {
rtai_print_to_screen("SERVER RECEIVED AN UNKNOWN MSG: %x%x, STAT: %c %c %c.\n", ((int *)&msg)[1], ((int *)&msg)[0], mstat[0], mstat[1], bstat);
t = 0;
goto prem;
}
}
bstat = '0' + t;
rt_mbx_send(&rmbx[t], &name, sizeof(name));
}
prem:
premature = 1;
rtai_print_to_screen("SERVER TASK ENDS PREMATURELY.\n");
}
int rtos_printf(const char *fmt, ...)
{
va_list list;
char printkbuf [2000];
printkbuf[0] = '\0';
va_start (list, fmt);
vsprintf(printkbuf, fmt, list);
va_end (list);
// XXX revert to print to screen when debugging is over
return rtai_print_to_screen(printkbuf);
//return printf(printkbuf);
}
static void mfun(int t)
{
RTIME time;
unsigned long long msg;
int size, maxt = 0, itime;
while (1) {
time = rt_get_cpu_time_ns();
cpu_used[hard_cpu_id()]++;
mstat[t] = 's';
if ((size = rt_mbx_send_timed(&smbx, &name[t], sizeof(long long), nano2count(TIMEOUT)))) {
rtai_print_to_screen("SEND TIMEDOUT TASK: %d, UNSENT: %d, STAT: %c %c %c, OVERTIME: %d.\n", t, size, mstat[0], mstat[1], bstat, (int)(rt_get_cpu_time_ns() - time));
goto prem;
}
msg = 0;
mstat[t] = 'r';
if ((size = rt_mbx_receive_timed(&rmbx[t], &msg, sizeof(msg), nano2count(TIMEOUT)))) {
rtai_print_to_screen("RECEIVE TIMEDOUT TIME: %d, NOTRECEIVED: %d, MSG: %x%x, STAT: %c %c %c, OVERTIME: %d.\n", t, size, ((int *)&msg)[1], ((int *)&msg)[0], mstat[0], mstat[1], bstat, (int)(rt_get_cpu_time_ns() - time));
goto prem;
}
if (msg != 0xccccccccccccccccLL) {
rtai_print_to_screen("WRONG REPLY TO TASK: %d, MSG: %x %x.\n", t, ((int *)&msg)[0], ((int *)&msg)[1]);
goto prem;
}
mstat[t] = 'd';
if ((itime = rt_get_cpu_time_ns() - time) > maxt) {
rtai_print_to_screen("TASK: %d, MAXWAITIME: %d.\n", t, maxt = itime);
}
meant[t] = (itime + meant[t]) >> 1;
rt_sleep(nano2count(SLEEP_TIME));
}
prem:
premature = 1;
rtai_print_to_screen("TASK # %d ENDS PREMATURELY\n", t);
}
static void YetAnotherOne( void *var1, int var2 )
{
int i, *zi;
zi = (int *)0;
rtai_print_to_screen( "Stomping from qBlk\n");
if (crashkern) {
rt_stomp();
} else {
//__asm__("cli; hlt"); // Privilige check!
i = -1;
*zi = i; // Bad pointer
}
}
void dump_malloc_stats(void)
{
struct mallinfo mi;
extern int rtai_print_to_screen(const char *fmt, ...);
// memset(&mi, 0, sizeof(mi));
mi = mallinfo();
rtai_print_to_screen("\ntotal space allocated from system %d\n", mi.arena);
rtai_print_to_screen("number of non-inuse chunks %d\n", mi.ordblks);
rtai_print_to_screen("number of mmapped regions %d\n", mi.hblks);
rtai_print_to_screen("total space in mmapped regions %d\n", mi.hblkhd);
rtai_print_to_screen("total allocated space %d\n", mi.uordblks);
rtai_print_to_screen("total non-inuse space %d\n", mi.fordblks);
rtai_print_to_screen("top-most, releasable space %d\n", mi.keepcost);
}
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 main(int argc, char *argv[])
{
unsigned long srv_name = nam2num("SRV");
RT_TASK *srv;
pid_t pid, my_pid ;
int err, i, count, msglen, *zpt ;
RTIME period, delay;
char *opt;
QBLK *q;
hrt = 1;
crashkern = 0;
for( i=1 ; i < argc ; i++ ) {
opt = argv[i];
switch((*opt<<8) + *(opt+1)) {
case OPT_MINUS('k'):
crashkern++;
break;
case OPT_MINUS('f'):
crashfunc++;
break;
case OPT_MINUS('q'):
query++;
break;
case OPT_MINUS('s'):
hrt--;
break;
}
}
if(query) {
printf("STOMP OVER KERNEL MEMORY FROM KERNEL (!=0) OR USER SPACE CODE (==0):");
scanf("%d", &crashkern);
printf("STOMP OVER KERNEL MEMORY FROM QBLK FUNCTION (!=0) OR NORMAL USER SPACE (==0):");
scanf("%d", &crashfunc);
printf("STOMP OVER KERNEL MEMORY FROM HARD REALTIME (!=0) OR SOFT REALTIME (==0):");
scanf("%d", &hrt);
}
init_linux_scheduler(SCHED_FIFO, 98);
err = lock_all(0,0);
if(err) {
printf("lock_all() failed\n");
exit(2);
}
printf("SRV in %s mode to write over kernel memory %sfrom %s space\n", hrt ? "HRT" : "POSIX", crashfunc ? "in qBlk ":"", crashkern ? "kernel" : "user");
if (!(srv = rt_task_init(srv_name, 0, 0, 0))) {
printf("CANNOT INIT SRV TASK\n");
exit(3);
}
rt_set_oneshot_mode();
period = nano2count(10000000);
start_rt_timer(period);
// printf("SRV starts\n");
if (hrt) rt_make_hard_real_time();
rt_task_make_periodic(srv, rt_get_time() + period, period);
my_pid = rt_Alias_attach("");
if(!my_pid) {
rtai_print_to_screen("Cannot attach name\n");
goto Exit;
}
rt_InitTickQueue();
rt_qDynAlloc(32);
rt_sleep(nano2count(1000000000));
i = count = 0 ;
zpt = (int *)0xc43d0128; // kernel's address space
while(++count) {
memset( msg, 0, sizeof(msg));
pid = rt_Creceive( 0, msg, sizeof(msg), &msglen, period);
if(pid) {
rtai_print_to_screen("SRV received msg [%s] %d bytes from pid %04X\n", msg, msglen, pid);
memcpy( rep, msg, sizeof(rep));
if(rt_Reply(pid, rep, sizeof(rep)))
rtai_print_to_screen("SRV rt_Reply() failed\n");
}
rtai_print_to_screen("SRV Loop %d\n", count);
if(count == 10 && !crashfunc) {
if (crashkern) {
rt_stomp();
} else {
// __asm__("cli; hlt"); // Privilige check!
*zpt = -1; // Bad pointer
}
} else if(count == 10) {
q = rt_qDynInit( 0, YetAnotherOne, 0, 0);
delay = nano2count(100000000);
rt_qBlkWait(q, delay);
rt_qLoop();
}
if(count == 20) break;
}
if(rt_Name_detach(my_pid))
rtai_print_to_screen("SRV cannot detach name\n");
rt_sleep(nano2count(1000000000));
Exit:
if (hrt) rt_make_soft_real_time();
if(rt_task_delete(srv))
rtai_print_to_screen("SRV cannot delete task\n");
stop_rt_timer();
exit(0);
}
int main(int argc, char* argv[])
{
unsigned long srv_name = nam2num("SRV");
RT_TASK *srv;
pid_t pid, my_pid, proxy ;
int err,/* i,*/ count, msglen ;
RTIME period;
char *pt;
init_linux_scheduler(SCHED_FIFO, 98);
err = lock_all(0,0);
if(err) {
printf("lock_all() fails\n");
exit(-1);
}
if (!(srv = rt_task_init(srv_name, 0, 0, 0))) {
printf("CANNOT INIT SRV TASK\n");
exit(-1);
}
my_pid = rt_Alias_attach("");
if(!my_pid) {
rtai_print_to_screen("Cannot attach name SRV\n");
exit(-1);
}
period = nano2count(1000000);
rt_set_oneshot_mode();
start_rt_timer(period);
printf("SRV starts (%p)\n", srv);
rt_make_hard_real_time();
rt_task_make_periodic(srv, rt_get_time(), period);
proxy = rt_Proxy_attach( 0, "More beer please", 17, -1);
if(proxy <= 0 ) {
rtai_print_to_screen("Failed to attach proxy\n");
exit(-1);
}
pid = rt_Receive( 0, 0, 0, &msglen);
if(pid) {
// handshake to give the proxy to CLT
rtai_print_to_screen("rt_Reply the proxy %04X msglen = %d\n", proxy, msglen);
rt_Reply( pid, &proxy, sizeof(proxy));
}
rt_sleep(nano2count(1000000000));
count = 8 ;
while(count--) {
memset( msg, 0, sizeof(msg));
pid = rt_Receive( 0, msg, sizeof(msg), &msglen);
if(pid == proxy) {
rtai_print_to_screen("SRV receives the PROXY event [%s]\n", msg);
continue;
} else if( pid <= 0) {
rtai_print_to_screen("SRV rt_Receive() failed\n");
// goto Exit;
continue;
}
rtai_print_to_screen("SRV received msg [%s] %d bytes from pid %04X\n", msg, msglen, pid);
memcpy( rep, msg, sizeof(rep));
pt = (char *) rep;
while(*pt) *pt++ = toupper(*pt);
if(rt_Reply(pid, rep, sizeof(rep)))
rtai_print_to_screen("SRV rt_Reply() failed\n");
}
if(rt_Proxy_detach(proxy))
rtai_print_to_screen("SRV cannot detach proxy\n");
if(rt_Name_detach(my_pid))
rtai_print_to_screen("SRV cannot detach name\n");
//Exit:
rt_make_soft_real_time();
rt_sleep(nano2count(1000000000));
if(rt_task_delete(srv))
rtai_print_to_screen("SRV cannot delete task\n");
return 0;
}
