#include "tee_internal_api.h"
#include <stdio.h>
#include <string.h>
#include "gpio-mst.h"
#include "mstdrv.h"
#include "regs-gpio-mst.h"

#include <core/error.h>
#include <core/access.h>
#include <core/page.h>
#include <driver/mem/phys.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/types.h>
#include <inttypes.h>
#include <fcntl.h>
#include <cache.h>

#define TAG "[SLSI] GPIO-MST : "

#if defined(TA_ARCH64)
#define MST_ARCH_TYPE uint64_t
#else
#define MST_ARCH_TYPE uint32_t
#endif

static uint64_t mst_en_vaddr;
static uint64_t mst_data_vaddr;
static unsigned char* mst_en_mmap_addr;
static unsigned char* mst_data_mmap_addr;
static int phys;

struct regs {
	uint32_t offset;
};

struct gpio_vec {
	MST_ARCH_TYPE base;
	uint8_t pin;
	struct regs con;
	struct regs dat;
	struct regs pud;
	struct regs drv;
};

static struct gpio_vec gpio_vecs[NUM_GPIO_MST] = {
	{ /* MST EN */
		.base = 0,
		.pin = 0,
		.con = {.offset = EN_CON, },
		.dat = {.offset = EN_DAT, },
		.pud = {.offset = EN_PUD, },
		.drv = {.offset = EN_DRV, },
	},
	{ /* MST DATA */
		.base = 0,
		.pin = 0,
		.con = {.offset = DATA_CON, },
		.dat = {.offset = DATA_DAT, },
		.pud = {.offset = DATA_PUD, },
		.drv = {.offset = DATA_CON, },
	},
};

static uint32_t read_sfr_value(uint32_t sfr_addr)
{
	return *((volatile uint32_t *)sfr_addr);
}

static void write_sfr_value(uint32_t sfr_addr, uint32_t in)
{
	*((volatile uint32_t *)sfr_addr) = in;
}

uint32_t gpio_phys_to_vaddr()
{
	uint32_t paddr, offset;

	phys = open(PHYS_DEV_NAME, O_RDWR);
	if(phys == -1){
		printf(TAG "PHYS_DEV : failed to open phys driver. phys: %d\n", phys);
		goto open_failed;
	}

	/* MST EN virtual address mapping */
	paddr = MST_EN_BASE;
	offset = paddr & ~PAGE_MASK; 
	//printf(TAG "PHYS_DEV : paddr : %#lx, offset : %#lx\n", paddr, offset);

	mst_en_mmap_addr = mmap(NULL, PAGE_SIZE, PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_PHYS_NON_SECURE, phys, paddr & PAGE_MASK);
	if (mst_en_mmap_addr == MAP_FAILED) {
		printf(TAG "PHYS_DEV : failed to mmap phys memory for MST_EN addr. errono: %d\n", errno);
		goto mst_en_mmap_failed;
	}
	mst_en_vaddr = (uint64_t)mst_en_mmap_addr + offset;

	/* MST DATA virtual address mapping */
	if (MST_EN_BASE != MST_DATA_BASE) {
		paddr = MST_DATA_BASE;
		offset = paddr & ~PAGE_MASK; 
		//printf(TAG "PHYS_DEV : paddr : %#lx, offset : %#lx\n", paddr, offset);
		mst_data_mmap_addr = mmap(NULL, PAGE_SIZE, PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_PHYS_NON_SECURE, phys, paddr & PAGE_MASK);
		if (mst_data_mmap_addr == MAP_FAILED) {
			printf(TAG "PHYS_DEV : failed to mmap phys memory for MST_DATA addr. errono: %d\n", errno);
			goto mst_data_mmap_failed;
		}
		mst_data_vaddr = (uint64_t)mst_data_mmap_addr + offset;
	} else {
		mst_data_vaddr = mst_en_vaddr;
	}

	/* save in gpio_vec */
	gpio_vecs[GPIO_VECIDX_MST_EN].base = mst_en_vaddr;
	gpio_vecs[GPIO_VECIDX_MST_DATA].base = mst_data_vaddr;

	gpio_vecs[GPIO_VECIDX_MST_EN].pin = GPIO_MST_EN_PIN;
	gpio_vecs[GPIO_VECIDX_MST_DATA].pin = GPIO_MST_DATA_PIN;
#if 1 
	//printf(TAG "gpio_vecs[GPIO_VECIDX_MST_EN].base: %#lx\n", gpio_vecs[GPIO_VECIDX_MST_EN].base);
	//printf(TAG "gpio_vecs[GPIO_VECIDX_MST_DATA].base : %#lx\n", gpio_vecs[GPIO_VECIDX_MST_DATA].base);

	printf(TAG "gpio_vecs[GPIO_VECIDX_MST_EN].pin : %d\n", gpio_vecs[GPIO_VECIDX_MST_EN].pin);
	printf(TAG "gpio_vecs[GPIO_VECIDX_MST_DATA].pin : %d\n", gpio_vecs[GPIO_VECIDX_MST_DATA].pin);
#endif
	return TEE_SUCCESS;

mst_data_mmap_failed:
	if (munmap(mst_en_mmap_addr, PAGE_SIZE)) {
		printf(TAG "DRAM unmap error\n");
	}
mst_en_mmap_failed:
	close(phys);
open_failed:
	return TEE_ERROR_GENERIC;
}

static uint32_t set_gpio_con(uint32_t reg_value, uint8_t pin, uint32_t con)
{
	switch (pin) {
		case 0:
			reg_value &= ~CON0_SET(0xF);
			reg_value |= CON0_SET(con);
			break;
		case 1:
			reg_value &= ~CON1_SET(0xF);
			reg_value |= CON1_SET(con);
			break;
		case 2:
			reg_value &= ~CON2_SET(0xF);
			reg_value |= CON2_SET(con);
			break;
		case 3:
			reg_value &= ~CON3_SET(0xF);
			reg_value |= CON3_SET(con);
			break;
		case 4:
			reg_value &= ~CON4_SET(0xF);
			reg_value |= CON4_SET(con);
			break;
		case 5:
			reg_value &= ~CON5_SET(0xF);
			reg_value |= CON5_SET(con);
			break;
		case 6:
			reg_value &= ~CON6_SET(0xF);
			reg_value |= CON6_SET(con);
			break;
		case 7:
			reg_value &= ~CON7_SET(0xF);
			reg_value |= CON7_SET(con);
			break;
		default:
			return -1;  // wrong cmd_id
	}
	return reg_value;
}


static uint32_t set_gpio_pud(uint32_t reg_value, uint8_t pin, uint32_t pud)
{
	switch (pin) {
		case 0:
			reg_value &= ~PUD0_SET(0xF);
			reg_value |= PUD0_SET(pud);
			break;
		case 1:
			reg_value &= ~PUD1_SET(0xF);
			reg_value |= PUD1_SET(pud);
			break;
		case 2:
			reg_value &= ~PUD2_SET(0xF);
			reg_value |= PUD2_SET(pud);
			break;
		case 3:
			reg_value &= ~PUD3_SET(0xF);
			reg_value |= PUD3_SET(pud);
			break;
		case 4:
			reg_value &= ~PUD4_SET(0xF);
			reg_value |= PUD4_SET(pud);
			break;
		case 5:
			reg_value &= ~PUD5_SET(0xF);
			reg_value |= PUD5_SET(pud);
			break;
		case 6:
			reg_value &= ~PUD6_SET(0xF);
			reg_value |= PUD6_SET(pud);
			break;
		case 7:
			reg_value &= ~PUD7_SET(0xF);
			reg_value |= PUD7_SET(pud);
			break;
		default:
			return -1;  // wrong cmd_id
	}
	return reg_value;
}

static uint32_t set_gpio_dat(uint32_t reg_value, uint8_t pin, uint32_t dat)
{
	switch (pin) {
		case 0:
			reg_value &= ~DAT0_SET(1);
			reg_value |= DAT0_SET(dat);
			break;
		case 1:
			reg_value &= ~DAT1_SET(1);
			reg_value |= DAT1_SET(dat);
			break;
		case 2:
			reg_value &= ~DAT2_SET(1);
			reg_value |= DAT2_SET(dat);
			break;
		case 3:
			reg_value &= ~DAT3_SET(1);
			reg_value |= DAT3_SET(dat);
			break;
		case 4:
			reg_value &= ~DAT4_SET(1);
			reg_value |= DAT4_SET(dat);
			break;
		case 5:
			reg_value &= ~DAT5_SET(1);
			reg_value |= DAT5_SET(dat);
			break;
		case 6:
			reg_value &= ~DAT6_SET(1);
			reg_value |= DAT6_SET(dat);
			break;
		case 7:
			reg_value &= ~DAT7_SET(1);
			reg_value |= DAT7_SET(dat);
			break;
		default:
			return -1;  // wrong cmd_id
	}
	return reg_value;
}

uint32_t gpio_set_mst_init()
{
	uint32_t reg_value;

	for (uint8_t idx = 0; idx < NUM_GPIO_MST; idx++) {
		/* con */
		reg_value = *(volatile uint32_t *)(gpio_vecs[idx].base + gpio_vecs[idx].con.offset);
		reg_value = set_gpio_con(reg_value, gpio_vecs[idx].pin, (uint32_t)CON_OUTPUT);
		*(volatile uint32_t *)(gpio_vecs[idx].base + gpio_vecs[idx].con.offset) = reg_value;

		/* pud */
		reg_value = *(volatile uint32_t *)(gpio_vecs[idx].base + gpio_vecs[idx].pud.offset);
		reg_value = set_gpio_pud(reg_value, gpio_vecs[idx].pin, (uint32_t)PULL_DOWN);
		*(volatile uint32_t *)(gpio_vecs[idx].base + gpio_vecs[idx].pud.offset) = reg_value;

		/* dat */
		reg_value = *(volatile uint32_t *)(gpio_vecs[idx].base + gpio_vecs[idx].dat.offset);
		reg_value = set_gpio_dat(reg_value, gpio_vecs[idx].pin, 0);
		*(volatile uint32_t *)(gpio_vecs[idx].base + gpio_vecs[idx].dat.offset) = reg_value;
	}
}

uint32_t gpio_set_mst_exit()
{
	uint32_t ret = TEE_SUCCESS;

	if (munmap(mst_en_mmap_addr, PAGE_SIZE)) {
		printf(TAG "DRAM unmap error\n");
		ret = TEE_ERROR_GENERIC;
	}
	if (MST_EN_BASE != MST_DATA_BASE) {
		if (munmap(mst_data_mmap_addr, PAGE_SIZE)) {
			printf(TAG "DRAM unmap error\n");
			ret = TEE_ERROR_GENERIC;
		}
	}
	close(phys);
	return ret;
}

uint32_t gpio_set_mst_data(uint8_t data, const uint8_t idx, uint8_t pin)
{
	uint32_t reg_value;

	reg_value = *(volatile uint32_t *)(gpio_vecs[idx].base + gpio_vecs[idx].dat.offset);

	switch (data) {
		case 0:
			reg_value = set_gpio_dat(reg_value, pin, 0);
			break;
		case 1:
			reg_value = set_gpio_dat(reg_value, pin, 1);
			break;
		default: 
			return -1;
	}

	*(volatile uint32_t *)(gpio_vecs[idx].base + gpio_vecs[idx].dat.offset) = reg_value;

	return 0;
}