/*
 * Description:
 *
 *	mprotect test
 */
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/types.h>
#include <rtdm/rtdm.h>
#include <native/task.h>
#include <native/timer.h>
#include <native/mutex.h>
#include <rtdk.h>
#include <signal.h>
#include <execinfo.h>

#define NUM_LOOPS 1000000
#define RTT_PRIO  99

RT_TASK rt_task_desc;
static int finished = 0;

///////////////////////////////////////////////////
// defined from xenomai syscall.h
#define SIGDEBUG_UNDEFINED			0
#define SIGDEBUG_MIGRATE_SIGNAL		1
#define SIGDEBUG_MIGRATE_SYSCALL	2
#define SIGDEBUG_MIGRATE_FAULT		3
#define SIGDEBUG_MIGRATE_PRIOINV	4
#define SIGDEBUG_NOMLOCK			5
#define SIGDEBUG_WATCHDOG			6
///////////////////////////////////////////////////
// own defined
#define SIGDEBUG_STOPTHREAD			17

static const char *GetSIGXCPUsubCode(int subSig) {

	if (subSig == SIGDEBUG_UNDEFINED)			return "SD_UNDEFINED";
	if (subSig == SIGDEBUG_MIGRATE_SIGNAL)		return "SD_MIGRATE_SIGNAL";
	if (subSig == SIGDEBUG_MIGRATE_SYSCALL)		return "SD_MIGRATE_SYSCALL";
	if (subSig == SIGDEBUG_MIGRATE_FAULT)		return "SD_MIGRATE_FAULT";
	if (subSig == SIGDEBUG_MIGRATE_PRIOINV)		return "SD_MIGRATE_PRIOINV";
	if (subSig == SIGDEBUG_NOMLOCK)				return "SD_NOMLOCK";
	if (subSig == SIGDEBUG_WATCHDOG)			return "SD_WATCHDOG";
	if (subSig == SIGDEBUG_STOPTHREAD)			return "SD_STOPTHREAD";
	return "???";
}

static void warn_upon_switch(int sig, siginfo_t *info, void *context)
{
	if (sig == SIGXCPU) {

		printf("received SIGXCPU\n");
		printf("SIGXCPU subcode: %s\n", GetSIGXCPUsubCode(info->si_value.sival_int));
		
		finished = 1;
	}
}

/* This is the realtime task */
static void rt_task_proc(void *arg)
{
	unsigned long long counter = 0;
	
	rt_task_set_mode(0, T_WARNSW, NULL);

	if (rt_task_set_periodic(NULL, TM_NOW, 100000)) {
		rt_printf("failed to set high periodic\n");
		return;
	}
	while (!finished) {
		rt_task_wait_period(NULL);
		if (counter > 1000) {
			counter = 0;
			rt_printf(".");
		}
	}
}

void foo(void)
{
	printf("foo\n");
}

int main(int argc, char *argv[])
{
	int ret;
	int flags = PROT_READ | PROT_EXEC;
	uint8_t awl_orig_instr = 0;
	const uint8_t awl_break_code = 0xcc;
	char * addr = NULL;
	struct sigaction sa_new, sa_old;
	int counter = 1;

	sa_new.sa_sigaction = warn_upon_switch;
	sa_new.sa_flags		=   SA_SIGINFO  /*  Invoke signal-catching function with
											three arguments instead of one. */
						  | SA_NODEFER;	/*  Don't automatically block the signal when
											its handler is being executed. */
	sigemptyset(&sa_new.sa_mask);
	
	if (sigaction(SIGXCPU, &sa_new, &sa_old) < 0) {
		printf("failed to install signal handler for SIGXCPU\n");
		exit(1);
	}

	rt_task_set_mode(0, T_WARNSW, NULL);

	/* No memory-swapping for this programm */
	ret = mlockall(MCL_CURRENT | MCL_FUTURE);
	if (ret) {
		perror("ERROR : mlockall has failled");
		exit(1);
	}
	printf("foo = %p rt_task_proc=%p\n", &foo, &rt_task_proc);
	addr = &rt_task_proc - ((int)(&rt_task_proc) & (0x1000 - 1));
	printf("addr = %p\n", addr);


	if (rt_task_shadow(NULL, "main", 50, 0)) {
		printf("failed to shadow main task\n");
		exit(1);
	}

	rt_print_auto_init(1);

	/*
	 * Turn the current linux task into a xenomai RT-task 
	 * highest priority
	 */
	ret = rt_task_create(&rt_task_desc, "RTT", 0, RTT_PRIO, T_JOINABLE);
	if (ret) {
		fprintf(stderr, "ERROR : rt_task_create: %s\n",
			strerror(-ret));
		exit(1);
	}
	rt_printf("Created task\n");
	ret = rt_task_start(&rt_task_desc, &rt_task_proc, NULL);
	if (ret) {
		fprintf(stderr, "ERROR : rt_task_start: %s\n",
			strerror(-ret));
		exit(1);
	}
	rt_printf("Started task\n");

	while (!finished) {
		flags = PROT_READ | PROT_EXEC | PROT_WRITE;
		mprotect(addr , 0x1000,  flags);

		// set breakpoint code
		if (!awl_orig_instr) {
			memcpy(&awl_orig_instr, &foo, sizeof(awl_orig_instr));
			memcpy(&foo, &awl_break_code, sizeof(awl_break_code));
		}
		// restore original code
		else {
			memcpy(&foo, &awl_orig_instr, sizeof(awl_orig_instr));
			awl_orig_instr = 0;
		}
		
		//rt_printf("- removing memory protection\n");
		flags = PROT_READ | PROT_EXEC;
		mprotect(addr, 0x1000,  flags);

		if (counter++ > 10) {
			counter = 1;
		}

		usleep(counter);
	}

	rt_task_join(&rt_task_desc);
	printf("Task joined\n");

	return 0;
}