#include <linux/kernel.h>
#include <linux/bug.h>
#include <asm/avm_enh/avm_hw_breakpoint.h>

struct wp_info {
        unsigned long address;
        avm_wp_callback callback;
};

#define MAX_WP 4

struct wp_info wps[MAX_WP];

static void avm_configure_wp(int n, u32 addr, u32 ctrl)
{
	switch (n) {
		case 0:
			WCP14_DBGWVR0(addr);
			isb();
			WCP14_DBGWCR0(ctrl);
			isb();
			break;
		case 1:
			WCP14_DBGWVR1(addr);
			isb();
			WCP14_DBGWCR1(ctrl);
			isb();
			break;
		case 2:
			WCP14_DBGWVR2(addr);
			isb();
			WCP14_DBGWCR2(ctrl);
			isb();
			break;
		case 3:
			WCP14_DBGWVR3(addr);
			isb();
			WCP14_DBGWCR3(ctrl);
			isb();
			break;
        }
}

static int wp_find(unsigned long addr)
{
	int n;

	for (n = 0; n < MAX_WP; n++) {
		if (wps[n].address == addr) break;
	}
	if (n == MAX_WP) return -1;
	return n;
}

void avm_wp_disable(unsigned long addr)
{
	int n = wp_find(addr);
	if (n < 0) return;

	avm_configure_wp(n, 0, 0);
	wps[n].address = 0;
}
EXPORT_SYMBOL(avm_wp_disable);

int avm_wp_handler(unsigned long addr, unsigned int fsr, struct pt_regs *regs)
{
	int n = wp_find(addr);

	pr_err("wp #%d handler for address %08lx\n", n, addr);
	if (n < 0) return n;

	if (wps[n].callback == NULL) {
		avm_wp_disable(addr);
		BUG();
	} else {
		wps[n].callback(addr, fsr, regs);
	}
	return 0;
}

int avm_wp_enable(unsigned long addr, avm_wp_callback callback)
{
	int n = wp_find(0);
	u32 ctrl_enc;
	struct arch_hw_breakpoint_ctrl ctrl = {
		.len = ARM_BREAKPOINT_LEN_4,
		.type = ARM_BREAKPOINT_STORE,
		.enabled = 1,
		.privilege = ARM_BREAKPOINT_USER | ARM_BREAKPOINT_PRIV,
	};

	pr_err("enable wp #%d\n", n);
	if (n < 0) return n;

	ctrl_enc = encode_ctrl_reg(ctrl);

	avm_configure_wp(n, addr & ~0x3, ctrl_enc);

	wps[n].address = addr;
	wps[n].callback = callback;

	return n;
}
EXPORT_SYMBOL(avm_wp_enable);