/* Watch the spin lock for changes */

%{
#include <linux/kdebug.h>
#include <asm/kregs.h>
#include <asm/hw_irq.h>
%}

%{
static void
init_and_panic(const char *msg)
{
	unsigned int cpu, self_cpu;
	self_cpu = smp_processor_id();
	for_each_online_cpu(cpu)
		if (cpu != self_cpu)
			platform_send_ipi(cpu, 0, IA64_IPI_DM_INIT, 0);

	panic(msg);
}

static inline unsigned long
get_siglock(unsigned long addr)
{
	unsigned long ret;

	asm volatile ("ld4.acq %0=[%1]\n" : "=r" (ret) : "r" (addr));
	return ret;
}

struct fault_info {
	unsigned long ip, addr;
	unsigned long oldvalue, newvalue;
	struct task_struct *task;
};

#define DBR_COUNT	4

static DEFINE_SPINLOCK(dbr_lock);
static unsigned long dbr[DBR_COUNT];

#define HISTORY_SIZE	256

static DEFINE_PER_CPU(struct fault_info[HISTORY_SIZE], siglock_history);
static DEFINE_PER_CPU(unsigned int, siglock_history_idx);

static unsigned long
record_oldsiglock(struct pt_regs *regs, unsigned long addr)
{
	unsigned int *idx = &get_cpu_var(siglock_history_idx);
	struct fault_info *info = &get_cpu_var(siglock_history)[*idx];
	unsigned long oldval;

	info->ip = regs->cr_iip;
	info->addr = addr;
	info->oldvalue = oldval = get_siglock(info->addr);
	info->task = current;

	put_cpu_var(siglock_history);
	*idx = (*idx + 1) % HISTORY_SIZE;
	put_cpu_var(siglock_history_idx);

	return oldval;
}

static unsigned long
record_newsiglock(struct pt_regs *regs)
{
	unsigned int idx = (get_cpu_var(siglock_history_idx) - 1) % HISTORY_SIZE;
	struct fault_info *info = &get_cpu_var(siglock_history)[idx];
	unsigned long newval;

	BUG_ON(info->task != current);
	info->newvalue = newval = get_siglock(info->addr);

	put_cpu_var(siglock_history);
	put_cpu_var(siglock_history_idx);

	return newval;
}

#define TICKET_SHIFT    17
#define TICKET_BITS     15
#define TICKET_MASK     ((1 << TICKET_BITS) - 1)

static int
goes_wrong(void)
{
	unsigned int previdx;
	struct fault_info *info;
	long prevhead, curhead;
	long prevtail, curtail;

	previdx = (get_cpu_var(siglock_history_idx) - 1) % HISTORY_SIZE;
	put_cpu_var(siglock_history_idx);

	info = get_cpu_var(siglock_history);
	prevhead = info[previdx].oldvalue & TICKET_MASK;
	curhead = info[previdx].newvalue & TICKET_MASK;
	prevtail = (info[previdx].oldvalue >> TICKET_SHIFT) & TICKET_MASK;
	curtail = (info[previdx].newvalue >> TICKET_SHIFT) & TICKET_MASK;
	put_cpu_var(siglock_history);

	if (curtail == prevtail &&
	    (((curhead - prevhead) & TICKET_MASK) == 0 ||
	     ((curhead - prevhead) & TICKET_MASK) >= (1 << 14)))
		return 1;

	return 0;
}

static unsigned long
get_gr(struct pt_regs *regs, int num)
{
	switch(num) {
	case 1:
		return regs->r1;
	case 2 ... 3:
		return (&regs->r2)[num - 2];
	case 4:
		{
			register unsigned long r4 asm("r4");
			return r4;
		}
	case 5:
		{
			register unsigned long r5 asm("r5");
			return r5;
		}
	case 6:
		{
			register unsigned long r6 asm("r6");
			return r6;
		}
	case 7:
		{
			register unsigned long r7 asm("r7");
			return r7;
		}
	case 8 ... 11:
		return (&regs->r8)[num - 8];
	case 12 ... 13:
	case 15:
		return (&regs->r12)[num - 12];
	case 14:
		return regs->r14;
	case 16 ... 31:
		return (&regs->r16)[num - 16];
	case 32 ... 127:
		/* Too lazy to write code to access the backing store.
		   Just let debug_notify() ignore this access */
		return regs->ar_ccv ^ 1;
	default:
		BUG();
	}
}

static int
should_modify(struct pt_regs *regs)
{
	unsigned long opcode;
	long regnum;

	/* Check if this is a cmpxchg4 instruction */
	/* This is a HACK and only works for our special case */
	opcode = *(unsigned long*)regs->cr_iip;
	if ((opcode & 0x3fd900000003UL) != 0x101100000001UL)
		return 1;

	/* Extract the target register number */
	regnum = (opcode >> 11) & 0x7f;

	/* Check whether the compare was successful (the register
	   value is equal to ar.ccv */
	return get_gr(regs, regnum) == regs->ar_ccv;
}

static int
debug_notify(struct notifier_block *self, unsigned long val, void *data)
{
	struct die_args *args = (struct die_args *)data;
	struct pt_regs *regs = args->regs;
	unsigned long vector = args->err;
	unsigned long lockaddr = regs->ar_ssd & ~0x3UL;

	if (val != DIE_FAULT)
		return NOTIFY_DONE;

	if (vector == 29) {
		unsigned long siglock_value =
			record_oldsiglock(regs, lockaddr);

		if ((siglock_value & ~1ULL) == 0x40000)
			init_and_panic("Spinlock corrupted already!");

		regs->cr_ipsr |= IA64_PSR_DD | IA64_PSR_SS;
		return NOTIFY_STOP;
	} else if (vector == 36) {
		unsigned long siglock_value = record_newsiglock(regs);

		if (should_modify(regs) && goes_wrong())
			init_and_panic("Spinlock goes wrong!");
		if ((siglock_value & ~1ULL) == 0x40000)
			init_and_panic("Caught the corruption!");

		regs->cr_ipsr &= ~IA64_PSR_SS;
		return NOTIFY_STOP;
	}

	return NOTIFY_DONE;
}

static struct notifier_block debug_notifier = {
	.notifier_call = debug_notify,
	.priority = 0x7ffffffe
};

%}

%{
static void
load_dbr(void *dummy)
{
	unsigned long defmask = (((1UL << 56) - 1) & ~0x3ULL) +
		(0xfUL << 56) +	/* enable in all privilege levels */
		(1UL << 62);	/* trigger on write */
	int i;

	for (i = 0; i < DBR_COUNT; ++i) {
		unsigned long mask = dbr[i] ? defmask : 0UL;

		asm volatile (
			"mov dbr[%0]=%1;;\n"
			"mov dbr[%2]=%3;;\n"
			: : "r" (2*i), "r" (dbr[i]), "r" (2*i+1), "r" (mask));
	}
	asm volatile("srlz.d;;");
}
%}

function enable_debug ()
%{
	load_dbr(NULL);
	CONTEXT->regs->cr_ipsr |= IA64_PSR_DB;
%}

function disable_debug ()
%{
	unsigned long flags;
	int i;

	for (i = 0; i < DBR_COUNT; ++i) {
		asm volatile (
			"mov dbr[%0]=%1;;\n"
			"mov dbr[%2]=%3;;\n"
			: : "r" (2*i), "r" (0UL), "r" (2*i+1), "r" (0UL));
	}
	asm volatile("srlz.d;;");

	CONTEXT->regs->cr_ipsr &= ~IA64_PSR_DB;
%}

function try_add_debug (sighand:long)
%{
	struct sighand_struct *sighand =
		(struct sighand_struct*)THIS->sighand;
	unsigned long lockaddr = (unsigned long) &sighand->siglock;
	unsigned long flags;
	int i, freeslot;

	spin_lock_irqsave(&dbr_lock, flags);

	freeslot = -1;
	for (i = 0; i < DBR_COUNT; ++i) {
		if (dbr[i] == lockaddr)
			break;
		if (!dbr[i] && freeslot < 0)
			freeslot = i;
	}
	if (i == DBR_COUNT && freeslot >= 0) {
		dbr[freeslot] = lockaddr;
		smp_call_function(load_dbr, NULL, 0);
	}

	spin_unlock_irqrestore(&dbr_lock, flags);
%}

function try_remove_debug (sighand:long)
%{
	struct sighand_struct *sighand =
		(struct sighand_struct*)THIS->sighand;
	unsigned long lockaddr = (unsigned long) &sighand->siglock;
	unsigned long flags;
	int i;

	/* Racy, but best we can get :( */
	if (atomic_read(&sighand->count) > 1)
		return;

	spin_lock_irqsave(&dbr_lock, flags);

	for (i = 0; i < DBR_COUNT; ++i) {
		if (dbr[i] == lockaddr)
			dbr[i] = 0;
	}

	spin_unlock_irqrestore(&dbr_lock, flags);
%}

probe syscall.*
{
	enable_debug();
}

#if 0
probe syscall.*.return
{
	disable_debug();
}
#endif

probe kernel.function("schedule").return
{
	enable_debug ();
}

function is_our_task (task:long)
%{
	struct task_struct *tsk = (struct task_struct*)THIS->task;
        THIS->__retvalue = !IS_ERR(tsk) && !strcmp(tsk->comm, "count");
%}

probe kernel.function("copy_process").return
{
	if (is_our_task($return))
		try_add_debug($return->sighand);
}

probe kernel.function("__cleanup_sighand")
{
	try_remove_debug($sighand);
}

function setup_watch ()
%{
	register_die_notifier(&debug_notifier);
%}

function remove_watch ()
%{
	memset(dbr, sizeof dbr, 0);
	smp_call_function(load_dbr, NULL, 0);
	unregister_die_notifier(&debug_notifier);
%}

probe begin
{
	setup_watch();
}

probe end, error
{
	remove_watch();
}