#include <common.h>
#include <asm/io.h>
#include <asm/gpio.h>
#include "bspchip.h"

#define GPIO_RANGE 30

#define RTS_GETFIELD(val, width, gpio)	\
			(((val) >> (gpio)) & ((1 << (width)) - 1))
#define RTS_SETFIELD(reg, field, width, gpio)	(((reg) & \
			(~(((1 << (width)) - 1) <<	(gpio)))) \
			| (((field) & ((1 << (width)) - 1)) << (gpio)))

static unsigned int rts_gpio_get_field(void __iomem *reg,
				       unsigned int width, unsigned int gpio)
{
	unsigned int val = readl(reg);

	return RTS_GETFIELD(val, width, gpio);
}

static void rts_gpio_set_field(void __iomem *reg,
			       unsigned int field, unsigned int width,
			       unsigned int gpio)
{
	unsigned int val = readl(reg);

	val = RTS_SETFIELD(val, field, width, gpio);
	writel(val, reg);
}

int gpio_request(unsigned gpio, const char *label)
{
	if (gpio > GPIO_RANGE)
		return -EINVAL;
	if (gpio >= 8)
		set_bit(gpio - 8, (void *)XB2_SHARE_GPIO_SELECT);
	return 0;
}

int gpio_free(unsigned gpio)
{
	if (gpio > GPIO_RANGE)
		return -EINVAL;
	if (gpio >= 8)
		clear_bit(gpio - 8, (void *)XB2_SHARE_GPIO_SELECT);
	return 0;
}

int gpio_get_value(unsigned gpio)
{
	if (gpio > GPIO_RANGE)
		return -EINVAL;

	if (gpio < 8)
		return rts_gpio_get_field((void *)XB2_GPIO_O, 1, gpio);
	else
		return rts_gpio_get_field((void *)XB2_SHARE_GPIO_O,
					  1, gpio - 8);
}

int gpio_set_value(unsigned gpio, int value)
{
	if (gpio > GPIO_RANGE)
		return -EINVAL;
	if (value)
		if (gpio < 8)
			set_bit(gpio, (void *)XB2_GPIO_O);
		else
			set_bit(gpio - 8, (void *)XB2_SHARE_GPIO_O);
	else
		if (gpio < 8)
			clear_bit(gpio, (void *)XB2_GPIO_O);
		else
			clear_bit(gpio - 8, (void *)XB2_SHARE_GPIO_O);
	return 0;
}

int gpio_direction_input(unsigned gpio)
{
	if (gpio > GPIO_RANGE)
		return -EINVAL;
	if (gpio < 8)
		rts_gpio_set_field((void *)XB2_GPIO_OE, 0, 1, gpio);
	else
		rts_gpio_set_field((void *)XB2_SHARE_GPIO_OE, 0, 1, (gpio - 8));
	return 0;
}

int gpio_direction_output(unsigned gpio, int value)
{
	if (gpio > GPIO_RANGE)
		return -EINVAL;
	gpio_set_value(gpio, value);
	if (gpio < 8)
		rts_gpio_set_field((void *)XB2_GPIO_OE, 1, 1, gpio);
	else
		rts_gpio_set_field((void *)XB2_SHARE_GPIO_OE, 1, 1, (gpio - 8));
	return 0;
}
