/*
 * Copyright (C) 2014-2018, Samsung Electronics Co., Ltd.
 *
 * SFR Read/Write APIs for Custom Handler
 */

#include <stdbool.h>
#include <stdint.h>
#include "handler.h"
#include "kernel_api.h"

static void *local_phys_to_virt_dev(unsigned long phys_addr)
{
	unsigned long vaddr;
	unsigned long pfn[1];

	pfn[0] = (phys_addr / PAGE_SIZE); /* calc page number from phys.addr */
	vaddr = (unsigned long)vm_map_phys_buffer_dev(pfn, 1, VMA_NON_CACHED);
	if (vaddr) {
			/* add offset in the page to virtual address */
			vaddr += (phys_addr % PAGE_SIZE);
	}
	return (void *)vaddr;
}

static void local_phys_unmap(void *vaddr)
{
	if (vaddr) {
		vm_unmap_phys_buffer((unsigned long)vaddr & ~(PAGE_SIZE - 1), 1);
	}
}

uint32_t read_sfr(unsigned long phys_addr, size_t size)
{
	void *vaddr = local_phys_to_virt_dev(phys_addr);
	uint32_t data = 0;

	if (!vaddr) {
		return 0;
	}
	switch(size) {
	case 1:
		data = *(volatile unsigned char *)vaddr;
		break;
	case 2:
		data = *(volatile unsigned short *)vaddr;
		break;
	case 4:
		data = *(volatile unsigned int *)vaddr;
		break;
	}
	local_phys_unmap(vaddr);
	return data;
}


void write_sfr(unsigned long phys_addr, unsigned long data, size_t size)
{
	void *vaddr = local_phys_to_virt_dev(phys_addr);

	if (!vaddr) {
	}
	switch(size) {
	case 1:
		*(volatile unsigned char *)vaddr = data;
		break;
	case 2:
		*(volatile unsigned short *)vaddr = data;
		break;
	case 4:
		*(volatile unsigned int *)vaddr = data;
		break;
	}
	local_phys_unmap(vaddr);
}