/*	$OpenBSD: fault.c,v 1.2 2014/03/29 18:09:29 guenther Exp $	*/

/*
 * Copyright (c) 2013 Miodrag Vallat.
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

/*
 * Standalone code to recover from faults. Allows for hardware detection.
 */

#include <sys/param.h>

#include <machine/asm.h>
#include <machine/asm_macro.h>
#include <machine/psl.h>

typedef struct label_t {
        long val[19];
} label_t;

extern int setjmp(label_t *);
extern void longjmp(label_t *);

static label_t badaddr_jmpbuf;
static uint32_t badaddr_psr;

static uint32_t prom_vbr;
static uint32_t vector_page[512 * 2] __attribute__ ((__aligned__(0x1000)));

static __inline__ uint32_t
get_vbr()
{
	uint32_t vbr;
	__asm__ volatile ("ldcr %0, %%cr7" : "=r"(vbr));
	return vbr;
}

static __inline__ void
set_vbr(uint32_t vbr)
{
	__asm__ volatile ("stcr %0, %%cr7" :: "r"(vbr));
}

/*
 * This is an horribly crude logic to recover from data access exceptions
 * by longjmp'ing back to badaddr(). We should theoretically at least use
 * an `rte' somewhere to unfreeze the scoreboard. But since we restore a
 * PSR with interrupts disabled, this turns out to be safe.
 */
static void
libsa_fault_handler(void)
{
	set_psr(badaddr_psr | PSR_IND);	/* undo SFRZ */
	flush_pipeline();

	longjmp(&badaddr_jmpbuf);
	/* NOTREACHED */
}

static __inline__ uint32_t
br(uint32_t delta)
{
	return 0xc0000000 | (((int32_t)delta >> 2) & 0x03ffffff);
}

static void
libsa_fault_init()
{
	int vec;
	uint32_t *insn;
	uint32_t br_insn;

	prom_vbr = get_vbr();

	insn = vector_page;
	br_insn = br(prom_vbr - (uint32_t)&vector_page - 4);
	for (vec = 512; vec != 0; vec--) {
		*insn++ = 0xf4005800;	/* nop */
		*insn++ = br_insn;	/* br into prom vbr page */
	}

	/* override data access exception */
	vector_page[3 * 2 + 1] =
	    br((uint32_t)&libsa_fault_handler -
	       (uint32_t)&vector_page[3 * 2 + 1]);
}

int
badaddr(void *addr, int len)
{
	int rc;

	if (vector_page[0] == 0)
		libsa_fault_init();

	badaddr_psr = get_psr();
	set_psr(badaddr_psr | PSR_IND);

	set_vbr((uint32_t)&vector_page);

	if (setjmp(&badaddr_jmpbuf) == 0) {
		switch (len) {
		case 1:
			(void)*(volatile uint8_t *)addr;
			rc = 0;
			break;
		case 2:
			if ((uint32_t)addr & 1)
				rc = 1;
			else {
				(void)*(volatile uint16_t *)addr;
				rc = 0;
			}
			break;
		case 4:
			if ((uint32_t)addr & 3)
				rc = 1;
			else {
				(void)*(volatile uint32_t *)addr;
				rc = 0;
			}
			break;
		default:
			rc = 1;
			break;
		}
	} else {
		rc = 1;
	}

	set_vbr(prom_vbr);
	flush_pipeline();
	set_psr(badaddr_psr);

	return rc;
}