#include "chip.h"

static int get_rst_reg_and_bitoff(enum soft_rst_n reset_dev, uint32_t *reg, uint8_t *bitoff)
{
	switch (reset_dev/32) {
		case 0:
			*reg = REGS_SYSCTL_BASE + SYS_SOFTRESET_CTL;
			break;
		case 1:
			*reg = REGS_SYSCTL_BASE + SYS_SOFTRESET_CTL1;
			break;
		case 2:
			*reg = REGS_SYSCTL_BASE + SYS_SOFTRESET_CTL2;
			break;
		default:
			printf ("illegal reset device (%d)\n", reset_dev);
			return -1;
	}
	*bitoff = reset_dev%32;

	return 0;
}

void vSysctlConfigure(uint32_t regoffset, uint32_t bitoffset, uint32_t mask, uint32_t val)
{
	uint32_t tmp = readl(REGS_SYSCTL_BASE + regoffset);

	tmp &= ~(mask << bitoffset);
	tmp |= val << bitoffset;
	writel(tmp, REGS_SYSCTL_BASE + regoffset);
}

void sys_soft_reset (enum soft_rst_n reset_dev)
{
	uint8_t mask;
	volatile uint32_t *reg;

	if (get_rst_reg_and_bitoff(reset_dev, (uint32_t *)&reg, &mask) != 0)
		return;

	portENTER_CRITICAL();
	*reg &= ~(1 << mask);
	udelay (10);
	*reg |=  (1 << mask);
	udelay (10);
	portEXIT_CRITICAL();
}

void sys_soft_reset_from_isr (enum soft_rst_n reset_dev)
{
	uint8_t mask;
	volatile uint32_t *reg;

	if (get_rst_reg_and_bitoff(reset_dev, (uint32_t *)&reg, &mask) != 0)
		return;

	*reg &= ~(1 << mask);
	udelay (10);
	*reg |=  (1 << mask);
	udelay (10);
}