#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>

#include <unistd.h>
#include <sys/mman.h>
#include <inttypes.h>
#include <fcntl.h>

#include <libwr/pio.h>
#include <libwr/wrs-msg.h>
#include <libwr/shw_io.h>
#include <libwr/util.h>

volatile uint8_t *_pio_base[4][NUM_PIO_BANKS + 1];
volatile uint8_t *_sys_base;

int shw_pio_mmap_init()
{
	int i;
	int fd = open("/dev/mem", O_RDWR);

	if (fd < 0) {
		pr_error("Can't open /dev/mem: %s\n", strerror(errno));
		exit(-1);
	}

	_sys_base =
	    mmap(NULL, 0x4000, PROT_READ | PROT_WRITE, MAP_SHARED, fd,
		 FPGA_GPIO_BASE);

	if (_sys_base == NULL) {
		pr_error("Can't mmap wb_gpio_port registers\n");
		close(fd);
		exit(-1);
	}

	pr_debug("wb_gpio_port virtual base = %p\n", _sys_base);

	for(i = 0; i < NUM_PIO_BANKS; i++)
	{
		_pio_base[IDX_REG_CODR][i] = _sys_base + i * FPGA_GPIO_BANK_SIZE + FPGA_GPIO_CODR;
		_pio_base[IDX_REG_SODR][i] = _sys_base + i * FPGA_GPIO_BANK_SIZE + FPGA_GPIO_SODR;
		_pio_base[IDX_REG_DDR][i] = _sys_base + i * FPGA_GPIO_BANK_SIZE + FPGA_GPIO_DDR;
		_pio_base[IDX_REG_PSR][i] = _sys_base + i * FPGA_GPIO_BANK_SIZE + FPGA_GPIO_PSR;
	}

	close(fd);

	return 0;
}

void shw_pio_configure(const pio_pin_t * pin)
{
	uint32_t mask;
	uint32_t ddr;

	mask = (1 << pin->pin);
	ddr = _readl(_pio_base[IDX_REG_DDR][pin->port]);

	if (pin->dir == PIO_OUT_1) {
		_writel(_pio_base[IDX_REG_SODR][pin->port], mask);
		ddr |= mask;
	} else if (pin->dir == PIO_OUT_0) {
		_writel(_pio_base[IDX_REG_CODR][pin->port], mask);
		ddr |= mask;
	} else
		ddr &= ~mask;

	_writel(_pio_base[IDX_REG_DDR][pin->port], ddr);
}

void shw_pio_toggle_pin(pio_pin_t * pin, uint32_t udelay)
{
#if 0
	while (1) {
		shw_pio_set(pin, 0);
		shw_udelay(udelay);
		shw_pio_set(pin, 1);
		shw_udelay(udelay);
	}
#endif	
}