/*
 ** This work is part of the White Rabbit project
 *
 * Copyright (C) 2019 CERN (www.cern.ch)
 * Author: Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or (at your
 * option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdint.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>*/

#include "gpio.h"

#define GPIO_SIZE		0x10	/* Address space used by one bank of GPIO */

#define GPIO_REG_CODR 	0x00	/* Clear Output Data Register */
#define GPIO_REG_SODR 	0x04	/* Set Output Data Register */
#define GPIO_REG_DDR 	0x08	/* Data Direction Register */
#define GPIO_REG_PSR 	0x0C	/* Pin State Register */

#define GPIO_BANK_SIZE 	32

#define pr_error 		printf

static void wb_gpio_set_dir(const struct gpio_pin *pin, int dir)
{
	void *base;
	volatile uint32_t *ddr;
	uint32_t mask;

	base = (void*)pin->device->priv;
	ddr = base + (GPIO_BANK_SIZE * (pin->pin >> 5)) + GPIO_REG_DDR;

	mask = 1 << (pin->pin & 0x1f);

	if (dir)
		*ddr |= mask;
	else
		*ddr &= ~mask;
}

static void wb_gpio_out(const struct gpio_pin *pin, int value)
{
	void *base;
	volatile void *regs;
	uint32_t mask;

	base = (void*)pin->device->priv;
	regs = base + (GPIO_BANK_SIZE * (pin->pin >> 5));
	mask = 1 << (pin->pin & 0x1f);

	if(value)
		*(volatile uint32_t *)(regs + GPIO_REG_SODR) = mask;
	else
		*(volatile uint32_t *)(regs + GPIO_REG_CODR) = mask;
}

static int wb_gpio_in(const struct gpio_pin *pin)
{
	void *base;
	volatile uint32_t *psr;
	uint32_t mask;
	
	base = (void*)pin->device->priv;
	psr = base + (GPIO_BANK_SIZE * (pin->pin >> 5)) + GPIO_REG_PSR;
	mask = 1 << (pin->pin & 0x1f);

	return (*psr & mask) ? 1 : 0;
}

int wb_gpio_create(struct gpio_device *device, uint64_t base_addr)
{
	int fd;
	uint64_t pagesize;
	uint64_t base_addr_pg;
	uint64_t base_addr_off;
    uint64_t len, len_pg;

	pagesize = getpagesize();
	len = GPIO_SIZE;
	fd = open("/dev/mem", O_RDWR);

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

    /* page-align our numbers, as we depend on the MMU */
	/* Get the starting page address for our base address
	   ie: pagesize=4096, base_addr=8200, base_addr_pg=8192 */
    base_addr_pg  = base_addr & ~(pagesize-1);
	/* Get the offset from the starting page address
	   ie: pagesize=4096, base_addr=8200, base_addr_off=8 */
    base_addr_off = base_addr &  (pagesize-1);
	/* Get the new length from starting page, aligned on page
	   ie: base_addr_pg=8192 --> len_pg=4096*/
    len_pg  = (len + base_addr_off + pagesize-1) & ~(pagesize-1);

	printf("base_addr = 0x%lX, len = %lu\n", base_addr, len);
	printf("page_size = %lu, base_addr_pg = 0x%lX, base_addr_off = %lu, len_pg = %lu\n", pagesize, base_addr_pg, base_addr_off, len_pg);

	/* Start our mapping on page boundary: base_addr_pg 
	   Mapping length multiple of pagesize */
    device->priv = mmap(NULL, len_pg,
         PROT_READ | PROT_WRITE, MAP_SHARED, fd, base_addr_pg);
	/* Add the offset from base_addr_pg (aligned on pagesize)
	   to base_addr: base_addr_off */
	device->priv += base_addr_off;

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

	close(fd);

	device->set_dir = wb_gpio_set_dir;
	device->set_out = wb_gpio_out;
	device->read_pin = wb_gpio_in;

	return 1;
}

void gen_gpio_set_dir(const struct gpio_pin *pin, int dir)
{
	if (!pin)
		return;
	if (!pin->device->set_dir)
		return;

	pin->device->set_dir(pin, dir);
}

void gen_gpio_out(const struct gpio_pin *pin, int value)
{
	if (!pin)
		return;
	if (!pin->device->set_out)
		return;

	pin->device->set_out(pin, value);
}

int gen_gpio_in(const struct gpio_pin *pin)
{
	if (!pin)
		return 0;
	if (!pin->device->read_pin)
		return 0;

	return pin->device->read_pin(pin);
}

void gen_gpio_bang(const struct gpio_pin *pin, int count)
{
	int i = 0;

	gen_gpio_out(pin, 0);

	for(i=0; i<count; i++)
	{
		gen_gpio_out(pin, 1);
		gen_gpio_out(pin, 0);
	}
}


