#include <rtems.h>
#include <stdio.h>
#include <stdlib.h>

#include <bsp.h> /* set_vector() */
#include "mmu_setup.h"

/* SRMMU has alignment requirements on tables that must be
 * fullfilled:
 *   - CTX Table:        align 0x2000, size 0x400
 *   - Level 1 Table:    align 0x0400, size 0x400
 *   - Level 2 Table:    align 0x0100, size 0x100
 *   - Level 3 Table:    align 0x0100, size 0x100
 */
struct srmmu_tables {
	unsigned int ctx_table[256];

	/* Context 0 - Level 1 tables (16MB/entry) */
	unsigned int ctx0_lvl1[256]; /* Level1 0x00000000-0xFFFFFFFF */
};

struct srmmu_tables __attribute__((aligned(0x2000))) mmu_table;

/* SRMMU table setup:
 *
 * 0x00000000-0x00ffffff: CPU RAM : R/W/X cacheable
 * 0x01000000-0x07ffffff: DMA MEM : R/W non-cacheable
 * 0x08000000-0x7fffffff: N/A     : INVALID
 * 0x80000000-0xffffffff: IO etc. : R/W
 */
void srmmu_table_setup(void)
{
	struct srmmu_tables *tabs = &mmu_table;
	unsigned int i;
#define INVALID 0
#define PTD 1
#define PTE 2
#define CACHABLE 0x80
#define US_RWX (3<<2) /* Access permission User and Supervisor: R/W/X */
#define US_RW (1<<2) /* Access permission User and Supervisor: R/W */

	/* Setup Context Table */
	tabs->ctx_table[0] = ((unsigned int)&tabs->ctx0_lvl1[0] >> 4) | PTD;
	for (i=1; i<256; i++)
		tabs->ctx_table[i] = INVALID; /* Context not used */

	/*** Setup Context 0 Access ***/
	tabs->ctx0_lvl1[0] = (0x00000000UL >> 4) | CACHABLE | US_RWX | PTE; /* MAP 1:1, Allow All */
	/* R/W non-cacheable 112MB */
	tabs->ctx0_lvl1[1] = (0x01000000UL >> 4) | US_RW | PTE; /* MAP 1:1, Allow All */
	tabs->ctx0_lvl1[2] = (0x02000000UL >> 4) | US_RW | PTE; /* MAP 1:1, Allow All */
	tabs->ctx0_lvl1[3] = (0x03000000UL >> 4) | US_RW | PTE; /* MAP 1:1, Allow All */
	tabs->ctx0_lvl1[4] = (0x04000000UL >> 4) | US_RW | PTE; /* MAP 1:1, Allow All */
	tabs->ctx0_lvl1[5] = (0x05000000UL >> 4) | US_RW | PTE; /* MAP 1:1, Allow All */
	tabs->ctx0_lvl1[6] = (0x06000000UL >> 4) | US_RW | PTE; /* MAP 1:1, Allow All */
	tabs->ctx0_lvl1[7] = (0x07000000UL >> 4) | US_RW | PTE; /* MAP 1:1, Allow All */
	/* 0x08000000-0x7fffffff: INVALID */
	for (i=8; i<128; i++) {
		/* 16MByte blocks */
		tabs->ctx0_lvl1[i] = INVALID;
	}
	/* 0x80000000-0xffffffff: R/W */
	for (i=128; i<256; i++) {
		tabs->ctx0_lvl1[i] = ((i * 0x1000000UL) >> 4) | US_RW | PTE;
	}
}

/* Physical page extraction from PTP's and PTE's. */
#define SRMMU_CTX_PMASK    0xfffffff0
#define SRMMU_PTD_PMASK    0xfffffff0
#define SRMMU_PTE_PMASK    0xffffff00

/* SRMMU Register addresses in ASI ASI_M_MMUREGS */
#define SRMMU_CTRL_REG           0x00000000
#define SRMMU_CTXTBL_PTR         0x00000100
#define SRMMU_CTX_REG            0x00000200
#define SRMMU_FAULT_STATUS       0x00000300
#define SRMMU_FAULT_ADDR         0x00000400

#define ASI_M_MMUREGS 0x19	/* READ/Write MMU Registers */
#define ASI_MMUFLUSH 0x18    /* FLUSH TLB */
#define ASI_DFLUSH 0x11    /* Flush D-Cache */

static __inline__ void srmmu_set_ctable_ptr(unsigned long paddr)
{
	paddr = ((paddr >> 4) & SRMMU_CTX_PMASK);
	__asm__ __volatile__("sta %0, [%1] %2\n\t"::"r"(paddr),
			     "r"(SRMMU_CTXTBL_PTR),
			     "i"(ASI_M_MMUREGS):"memory");
}

static __inline__ void srmmu_set_context(int context)
{
	__asm__ __volatile__("sta %0, [%1] %2\n\t"::"r"(context),
			     "r"(SRMMU_CTX_REG), "i"(ASI_M_MMUREGS):"memory");
}

static __inline__ void srmmu_set_mmureg(unsigned long regval)
{
	__asm__ __volatile__("sta %0, [%%g0] %1\n\t"::"r"(regval),
			     "i"(ASI_M_MMUREGS):"memory");
}

void __inline__ leon_flush_cache_all(void)
{
	__asm__ __volatile__(" flush ");
      __asm__ __volatile__("sta %%g0, [%%g0] %0\n\t"::"i"(ASI_DFLUSH):"memory");
}

void __inline__ leon_flush_tlb_all(void)
{
	leon_flush_cache_all();
	__asm__ __volatile__("sta %%g0, [%0] %1\n\t"::"r"(0x400),
			     "i"(ASI_MMUFLUSH):"memory");
}

rtems_isr srmmu_bad_trap(rtems_vector_number trap, CPU_Interrupt_frame *isf)
{
	uint32_t real_trap = SPARC_REAL_TRAP_NUMBER(trap);

	printk("!!!BAD TRAP: 0x%x\n", real_trap);
}

#define MMU_TRAPS 6
int traps[MMU_TRAPS] = {0x09, 0x29, 0x2c, 0x20, 0x21, 0x3c};

/* Setup and enable */
void srmmu_setup(int options)
{
	struct srmmu_tables *tabs = &mmu_table;
	rtems_interrupt_level level;
	int i;

	if ((options & MMU_ENABLE) == 0) {
		/* NO MMU */
		printf(" MMU: not used - never enabled\n");
		return;
	}

	rtems_interrupt_disable(level);

	/* Register TRAP Handler for MMU Traps */
	for (i=0; i<MMU_TRAPS; i++) {
		set_vector(
			(rtems_isr_entry) srmmu_bad_trap,
			SPARC_SYNCHRONOUS_TRAP(traps[i]),
			1);
	}

	/* Set Context Pointer of MMU */
	srmmu_set_ctable_ptr((unsigned long)&tabs->ctx_table[0]);

	/* Set Context Number */
	srmmu_set_context(0);

	/* Invalidate all Cache */
	__asm__ __volatile__("flush\n\t");

	/* flush TLB cache */
	leon_flush_tlb_all();

	srmmu_set_mmureg(0x00000001);

	/* flush TLB cache */
	leon_flush_tlb_all();

	/* Flush cache */
	leon_flush_cache_all();

	rtems_interrupt_enable(level);

	printf(" MMU: enabled with DMA regions marked not cacheable\n");
}

#include <amba.h>
#include <ambapp.h>

#define L2C_MTRR_ADR_BIT 18
#define L2C_MTRR_ACC_BIT 16
#define L2C_MTRR_MASK_BIT 2
#define L2C_MTRR_CTRL_BIT 0

#define LC2_MTRR_NC 0
#define LC2_MTRR_C (1 << L2C_MTRR_ACC_BIT)
#define LC2_MTRR_EN (1 << L2C_MTRR_CTRL_BIT)

struct l2c_regs {
	volatile unsigned int ctrl;
	volatile unsigned int status;
	volatile unsigned int flush_mem;
	volatile unsigned int flush_set;
	volatile unsigned int resv1[4];
	volatile unsigned int err_cs;	/* 0x20 */
	volatile unsigned int err_adr;
	volatile unsigned int tag;
	volatile unsigned int data;
	volatile unsigned int scrub_cs; /* 0x30 */
	volatile unsigned int scrub_delay;
	volatile unsigned int ei;
	volatile unsigned int resv[17];	/* 0x3c-0x7f */
	volatile unsigned int mtrr[32];	/* 0x80 */
};

volatile struct l2c_regs *l2c = NULL;


/* Setup Level 2 cache:
 *
 * 0x01000000 - 0x01ffffff : non-cacheable (16MB)
 * 0x02000000 - 0x03ffffff : non-cacheable (32MB)
 * 0x04000000 - 0x07ffffff : non-cacheable (64MB)
 */
void l2cache_setup(int options)
{
	struct ambapp_dev *adev;
	int i;

	/* Find L2C APB registers */
	adev = (struct ambapp_dev *)ambapp_for_each(&ambapp_plb,
				(OPTIONS_ALL|OPTIONS_AHB_SLVS),
				VENDOR_GAISLER, GAISLER_L2CACHE,
				ambapp_find_by_idx, NULL);
	if (adev == NULL) {
		printf(" ### L2CACHE controller not found, aborting.\n");
		exit(0);
	}
	l2c = (volatile struct l2c_regs *)DEV_TO_AHB(adev)->start[1];
	printf(" L2CACHE regs: %p\n", l2c);

	/* Check that L2C is enabled */
	if ((l2c->ctrl & 0x80000000) == 0) {
		printf(" ### L2CACHE is not enabled, MTRR setup is skipped\n");
		return;
	}

	if ((options & L2C_MTTR) == 0) {
		printf(" L2CACHE MTRR are disabled\n");
		for (i = 0; i < 32; i++)
			l2c->mtrr[i] = 0;
		return;
	}

	/* Flush & invalidate all cache */
	l2c->flush_mem = 0x7;

	/* Set uncached regions */
	l2c->mtrr[0] = 0x01000000 | LC2_MTRR_NC | (0xff000000>>16) | LC2_MTRR_EN;
	l2c->mtrr[1] = 0x02000000 | LC2_MTRR_NC | (0xfe000000>>16) | LC2_MTRR_EN;
	l2c->mtrr[2] = 0x04000000 | LC2_MTRR_NC | (0xfc000000>>16) | LC2_MTRR_EN;
	for (i = 3; i < 32; i++)
		l2c->mtrr[i] = 0;

	/* check to see that MTRR[2] has successfully written */
	if (l2c->mtrr[2] != (0x04000000 | LC2_MTRR_NC | (0xfc000000>>16) | LC2_MTRR_EN)) {
		printf(" ### Not enough MTR registers, 3 is required\n");
		exit(0);
	}
	printf(" L2CACHE: defined MTRR regions as uncacheable\n");
}

struct griommu_regs {
        volatile unsigned int cap0;
        volatile unsigned int cap1;
        volatile unsigned int cap2;
        volatile unsigned int resv1;
        volatile unsigned int ctrl;
        volatile unsigned int flush;
        volatile unsigned int status;
        volatile unsigned int imask;
        volatile unsigned int ahbstat;
        volatile unsigned int resv2[7];
        volatile unsigned int grp_assign[16];
        volatile unsigned int grp_ctrl[16];
        volatile unsigned int diag_ca;
        volatile unsigned int diag_cad[8];
        volatile unsigned int diag_cat;
        volatile unsigned int ei_data;
        volatile unsigned int ei_tag;
        volatile unsigned int resv3[4];
};

volatile struct griommu_regs *iommu = NULL;

/* Setup IOMMU master bridge:
 * 
 * prefetch  - 0=disable prefetching, 1=enable prefetching
 * bus       - 0=Route traffic over CPU-BUS, 1=Directly to MEM-BUS
 */
void iommu_setup(int options)
{
	struct ambapp_dev *adev;
	int i;
	int bus = (options >> 0) & 1;
	int prefetch = (options >> 1) & 1;

	/* Find L2C APB registers */
	adev = (struct ambapp_dev *)ambapp_for_each(&ambapp_plb,
				(OPTIONS_ALL|OPTIONS_AHB_SLVS),
				VENDOR_GAISLER, GAISLER_GRIOMMU,
				ambapp_find_by_idx, NULL);
	if (adev == NULL) {
		printf(" ### IOMMU controller not found, aborting.\n");
		exit(0);
	}
	iommu = (volatile struct griommu_regs *)DEV_TO_AHB(adev)->start[0];
	printf(" IOMMU regs: %p\n", iommu);
	printf("       prefetching %s, traffic routed %s\n",
		prefetch ? "enabled" : "disabled",
		bus ? "directly to MEM-BUS" : "over CPU-BUS");

	if (prefetch)
		iommu->ctrl &= ~(1 << 11);	/* Enable prefetching */
	else
		iommu->ctrl |= 1 << 11;		/* Disable prefetching */

	/* Select memory bus for all Masters */
	for (i=0; i<16; i++)
		iommu->grp_assign[i] = (iommu->grp_assign[i]&~0x10)|(bus&1)<<4;
}

void address_region_setup(int mmu_opts, int l2c_opts, int iommu_opts)
{
	srmmu_table_setup();
	srmmu_setup(mmu_opts);
	l2cache_setup(l2c_opts);
	iommu_setup(iommu_opts);
	printf("\n\n");
}
