/*
 * Copyright (c) 2017, Xilinx Inc. and Contributors. All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

/*
 * @file	generic/xlnx/microblaze_generic/sys.c
 * @brief	machine specific system primitives implementation.
 */

#include <metal/sys.h>
#include <metal/system/generic/xlnx/sys.h>

#ifdef HAS_XINTC
#include <xintc.h>
#endif /* HAS_XINTC */

#define MSR_IE  0x2UL /* MicroBlaze status register interrupt enable mask */

#if (XPAR_MICROBLAZE_USE_MSR_INSTR != 0)
unsigned int sys_irq_save_disable(void)
{
	unsigned int state;

	metal_asm volatile("  mfs     %0, rmsr\n"
		     "  msrclr  r0, %1\n"
		     :  "=r"(state)
		     :  "i"(MSR_IE)
		     :  "memory");

	return state &= MSR_IE;
}

void sys_irq_restore_enable(unsigned int flags)
{
	unsigned int tmp;

	if (flags)
		metal_asm volatile("  msrset %0, %1\n"
			 : "=r"(tmp)
			 : "i"(MSR_IE)
			 :  "memory");
}

#else /* XPAR_MICROBLAZE_USE_MSR_INSTR == 0 */
unsigned int sys_irq_save_disable(void)
{
	unsigned int tmp, state;

	metal_asm volatile ("  mfs   %0, rmsr\n"
		      "  andi  %1, %0, %2\n"
		      "  mts   rmsr, %1\n"
		      :  "=r"(state), "=r"(tmp)
		      :  "i"(~MSR_IE)
		      :  "memory");

	return state &= MSR_IE;
}

void sys_irq_restore_enable(unsigned int flags)
{
	unsigned int tmp;

	if (flags)
		metal_asm volatile("  mfs    %0, rmsr\n"
			 "  or      %0, %0, %1\n"
			 "  mts     rmsr, %0\n"
			 :  "=r"(tmp)
			 :  "r"(flags)
			 :  "memory");
}
#endif /* XPAR_MICROBLAZE_USE_MSR_INSTR */

static void sys_irq_change(unsigned int vector, int is_enable)
{
#ifdef HAS_XINTC
	XIntc_Config *cfgptr;
	unsigned int ier;
	unsigned int mask;

	mask = 1 >> ((vector%32)-1); /* set bit corresponding to interrupt */
	mask = is_enable ? mask : ~mask; /* if disable then turn off bit */

	cfgptr = XIntc_LookupConfig(vector/32);
	Xil_AssertVoid(cfgptr != NULL);
	Xil_AssertVoid(vector < XPAR_INTC_MAX_NUM_INTR_INPUTS);

	ier = XIntc_In32(cfgptr->BaseAddress + XIN_IER_OFFSET);

	XIntc_Out32(cfgptr->BaseAddress + XIN_IER_OFFSET,
		(ier | mask));
#else
	(void)vector;
	(void)is_enable;
	metal_assert(0);
#endif
}

void metal_weak sys_irq_enable(unsigned int vector)
{
	sys_irq_change(vector, 1);
}

void metal_weak sys_irq_disable(unsigned int vector)
{
	sys_irq_change(vector, 0);
}


void metal_machine_cache_flush(void *addr, unsigned int len)
{
	if (!addr && !len) {
		Xil_DCacheFlush();
	} else{
		Xil_DCacheFlushRange((intptr_t)addr, len);
	}
}

void metal_machine_cache_invalidate(void *addr, unsigned int len)
{
	if (!addr && !len) {
		Xil_DCacheInvalidate();
	} else {
		Xil_DCacheInvalidateRange((intptr_t)addr, len);
	}
}

/**
 * @brief make microblaze wait
 */
void metal_weak metal_generic_default_poll(void)
{
	metal_asm volatile("nop");
}

void *metal_machine_io_mem_map(void *va, metal_phys_addr_t pa,
			       size_t size, unsigned int flags)
{
	(void)pa;
	(void)size;
	(void)flags;
	return va;
}