3 files changed, 1026 insertions, 308 deletions

diff --git a/target/linux/ar71xx/files/drivers/mtd/nand/rb4xx_nand.c b/target/linux/ar71xx/files/drivers/mtd/nand/rb4xx_nand.c
index a107708a85..59ea17d896 100644
--- a/target/linux/ar71xx/files/drivers/mtd/nand/rb4xx_nand.c
+++ b/target/linux/ar71xx/files/drivers/mtd/nand/rb4xx_nand.c
@@ -1,7 +1,7 @@
 /*
  *  NAND flash driver for the MikroTik RouterBoard 4xx series
  *
- *  Copyright (C) 2008 Gabor Juhos <juhosg@openwrt.org>
+ *  Copyright (C) 2008-2010 Gabor Juhos <juhosg@openwrt.org>
  *  Copyright (C) 2008 Imre Kaloz <kaloz@openwrt.org>
  *
  *  This file was based on the driver for Linux 2.6.22 published by
@@ -23,49 +23,16 @@
 #include <linux/slab.h>
 
 #include <asm/mach-ar71xx/ar71xx.h>
+#include <asm/mach-ar71xx/rb4xx_cpld.h>
 
 #define DRV_NAME        "rb4xx-nand"
-#define DRV_VERSION     "0.1.10"
+#define DRV_VERSION     "0.2.0"
 #define DRV_DESC        "NAND flash driver for RouterBoard 4xx series"
 
-#define USE_FAST_READ	1
-#define USE_FAST_WRITE	1
-#undef RB4XX_NAND_DEBUG
-
-#ifdef RB4XX_NAND_DEBUG
-#define DBG(fmt, arg...)	printk(KERN_DEBUG DRV_NAME ": " fmt, ## arg)
-#else
-#define DBG(fmt, arg...)	do {} while (0)
-#endif
-
-#define RB4XX_NAND_GPIO_RDY	5
-#define RB4XX_FLASH_HZ		33333334
-#define RB4XX_NAND_HZ		33333334
-
-#define SPI_CTRL_FASTEST	0x40
-#define SPI_CTRL_SAFE		0x43	/* 25 MHz for AHB 200 MHz */
-#define SBIT_IOC_BASE		SPI_IOC_CS1
-#define SBIT_IOC_DO_SHIFT	0
-#define SBIT_IOC_DO		(1u << SBIT_IOC_DO_SHIFT)
-#define SBIT_IOC_DO2_SHIFT	18
-#define SBIT_IOC_DO2		(1u << SBIT_IOC_DO2_SHIFT)
-
-#define CPLD_CMD_WRITE_MULT	0x08	/* send cmd, n x send data, read data */
-#define CPLD_CMD_WRITE_CFG	0x09	/* send cmd, n x send cfg */
-#define CPLD_CMD_READ_MULT	0x0a	/* send cmd, send idle, n x read data */
-#define CPLD_CMD_READ_FAST	0x0b	/* send cmd, 4 x idle, n x read data */
-
-#define CFG_BIT_nCE	0x80
-#define CFG_BIT_CLE	0x40
-#define CFG_BIT_ALE	0x20
-#define CFG_BIT_FAN	0x10
-#define CFG_BIT_nLED4	0x08
-#define CFG_BIT_nLED3	0x04
-#define CFG_BIT_nLED2	0x02
-#define CFG_BIT_nLED1	0x01
-
-#define CFG_BIT_nLEDS \
-	(CFG_BIT_nLED1 | CFG_BIT_nLED2 | CFG_BIT_nLED3 | CFG_BIT_nLED4)
+#define RB4XX_NAND_GPIO_READY	5
+#define RB4XX_NAND_GPIO_ALE	37
+#define RB4XX_NAND_GPIO_CLE	38
+#define RB4XX_NAND_GPIO_NCE	39
 
 struct rb4xx_nand_info {
 	struct nand_chip	chip;
@@ -102,318 +69,138 @@ static struct mtd_partition rb4xx_nand_partitions[] = {
 	},
 };
 
-#if USE_FAST_READ
-#define SPI_NDATA_BASE	0x00800000
-static unsigned spi_ctrl_fread = SPI_CTRL_SAFE;
-static unsigned spi_ctrl_flash = SPI_CTRL_SAFE;
-extern unsigned mips_hpt_frequency;
-#endif
-
-static inline unsigned rb4xx_spi_rreg(unsigned r)
-{
-	return __raw_readl((void * __iomem)(KSEG1ADDR(AR71XX_SPI_BASE) + r));
-}
-
-static inline void rb4xx_spi_wreg(unsigned r, unsigned v)
-{
-	__raw_writel(v, (void * __iomem)(KSEG1ADDR(AR71XX_SPI_BASE) + r));
-}
-
-static inline void do_spi_clk(int bit)
-{
-	unsigned bval = SBIT_IOC_BASE | (bit & 1);
-
-	rb4xx_spi_wreg(SPI_REG_IOC, bval);
-	rb4xx_spi_wreg(SPI_REG_IOC, bval | SPI_IOC_CLK);
-}
-
-static void do_spi_byte(uint8_t byte)
+static int rb4xx_nand_dev_ready(struct mtd_info *mtd)
 {
-	do_spi_clk(byte >> 7);
-	do_spi_clk(byte >> 6);
-	do_spi_clk(byte >> 5);
-	do_spi_clk(byte >> 4);
-	do_spi_clk(byte >> 3);
-	do_spi_clk(byte >> 2);
-	do_spi_clk(byte >> 1);
-	do_spi_clk(byte);
-
-	DBG("spi_byte sent 0x%02x got 0x%x\n",
-					byte, rb4xx_spi_rreg(SPI_REG_RDS));
+	return gpio_get_value(RB4XX_NAND_GPIO_READY);
 }
 
-#if USE_FAST_WRITE
-static inline void do_spi_clk_fast(int bit1, int bit2)
+static void rb4xx_nand_write_cmd(unsigned char cmd)
 {
-	unsigned bval = (SBIT_IOC_BASE |
-			((bit1 << SBIT_IOC_DO_SHIFT) & SBIT_IOC_DO) |
-			((bit2 << SBIT_IOC_DO2_SHIFT) & SBIT_IOC_DO2));
+	unsigned char data = cmd;
+	int err;
 
-	rb4xx_spi_wreg(SPI_REG_IOC, bval);
-	rb4xx_spi_wreg(SPI_REG_IOC, bval | SPI_IOC_CLK);
+	err = rb4xx_cpld_write(&data, 1);
+	if (err)
+		pr_err("rb4xx_nand: write cmd failed, err=%d\n", err);
 }
 
-static inline void do_spi_byte_fast(uint8_t byte)
+static void rb4xx_nand_cmd_ctrl(struct mtd_info *mtd, int cmd,
+				unsigned int ctrl)
 {
-	do_spi_clk_fast(byte >> 7, byte >> 6);
-	do_spi_clk_fast(byte >> 5, byte >> 4);
-	do_spi_clk_fast(byte >> 3, byte >> 2);
-	do_spi_clk_fast(byte >> 1, byte >> 0);
+	if (ctrl & NAND_CTRL_CHANGE) {
+		gpio_set_value(RB4XX_NAND_GPIO_CLE, (ctrl & NAND_CLE) ? 1 : 0);
+		gpio_set_value(RB4XX_NAND_GPIO_ALE, (ctrl & NAND_ALE) ? 1 : 0);
+		gpio_set_value(RB4XX_NAND_GPIO_NCE, (ctrl & NAND_NCE) ? 0 : 1);
+	}
 
-	DBG("spi_byte_fast sent 0x%02x got 0x%x\n",
-					byte, rb4xx_spi_rreg(SPI_REG_RDS));
-}
-#else
-static inline void do_spi_byte_fast(uint8_t byte)
-{
-	do_spi_byte(byte);
+	if (cmd != NAND_CMD_NONE)
+		rb4xx_nand_write_cmd(cmd);
 }
-#endif /* USE_FAST_WRITE */
 
-static int do_spi_cmd(unsigned cmd, unsigned sendCnt, const uint8_t *sendData,
-		unsigned recvCnt, uint8_t *recvData,
-		const uint8_t *verifyData, int fastWrite)
+static unsigned char rb4xx_nand_read_byte(struct mtd_info *mtd)
 {
-	unsigned i;
-
-	DBG("SPI cmd 0x%x send %u recv %u\n", cmd, sendCnt, recvCnt);
-
-	rb4xx_spi_wreg(SPI_REG_FS, SPI_FS_GPIO);
-	rb4xx_spi_wreg(SPI_REG_CTRL, SPI_CTRL_FASTEST);
+	unsigned char data = 0;
+	int err;
 
-	do_spi_byte(cmd);
-#if 0
-	if (cmd == CPLD_CMD_READ_FAST) {
-		do_spi_byte(0x80);
-		do_spi_byte(0);
-		do_spi_byte(0);
-	}
-#endif
-	for (i = 0; i < sendCnt; ++i) {
-		if (fastWrite)
-			do_spi_byte_fast(sendData[i]);
-		else
-			do_spi_byte(sendData[i]);
-	}
-
-	for (i = 0; i < recvCnt; ++i) {
-		if (fastWrite)
-			do_spi_byte_fast(0);
-		else
-			do_spi_byte(0);
-
-		if (recvData) {
-			recvData[i] = rb4xx_spi_rreg(SPI_REG_RDS) & 0xff;
-		} else if (verifyData) {
-			if (verifyData[i] != (rb4xx_spi_rreg(SPI_REG_RDS)
-							 & 0xff))
-				break;
-		}
+	err = rb4xx_cpld_read(&data, NULL, 1);
+	if (err) {
+		pr_err("rb4xx_nand: read data failed, err=%d\n", err);
+		data = 0xff;
 	}
 
-	rb4xx_spi_wreg(SPI_REG_IOC, SBIT_IOC_BASE | SPI_IOC_CS0);
-	rb4xx_spi_wreg(SPI_REG_CTRL, spi_ctrl_flash);
-	rb4xx_spi_wreg(SPI_REG_FS, 0);
-
-	return i == recvCnt;
+	return data;
 }
 
-static int got_write = 1;
-
-static void rb4xx_nand_write_data(const uint8_t *byte, unsigned cnt)
+static void rb4xx_nand_write_buf(struct mtd_info *mtd, const unsigned char *buf,
+				 int len)
 {
-	do_spi_cmd(CPLD_CMD_WRITE_MULT, cnt, byte, 1, NULL, NULL, 1);
-	got_write = 1;
-}
+	int err;
 
-static void rb4xx_nand_write_byte(uint8_t byte)
-{
-	rb4xx_nand_write_data(&byte, 1);
+	err = rb4xx_cpld_write(buf, len);
+	if (err)
+		pr_err("rb4xx_nand: write buf failed, err=%d\n", err);
 }
 
-#if USE_FAST_READ
-static uint8_t *rb4xx_nand_read_getaddr(unsigned cnt)
+static void rb4xx_nand_read_buf(struct mtd_info *mtd, unsigned char *buf,
+				int len)
 {
-	static unsigned nboffset = 0x100000;
-	unsigned addr;
-
-	if (got_write) {
-		nboffset = (nboffset + 31) & ~31;
-		if (nboffset >= 0x100000)	/* 1MB */
-			nboffset = 0;
-
-		got_write = 0;
-		rb4xx_spi_wreg(SPI_REG_FS, SPI_FS_GPIO);
-		rb4xx_spi_wreg(SPI_REG_CTRL, spi_ctrl_fread);
-		rb4xx_spi_wreg(SPI_REG_FS, 0);
-	}
+	int err;
 
-	addr = KSEG1ADDR(AR71XX_SPI_BASE + SPI_NDATA_BASE) + nboffset;
-	DBG("rb4xx_nand_read_getaddr 0x%x cnt 0x%x\n", addr, cnt);
-
-	nboffset += cnt;
-	return (uint8_t *)addr;
+	err = rb4xx_cpld_read(buf, NULL, len);
+	if (err)
+		pr_err("rb4xx_nand: read buf failed, err=%d\n", err);
 }
 
-static void rb4xx_nand_read_data(uint8_t *buf, unsigned cnt)
+static int __init rb4xx_nand_probe(struct platform_device *pdev)
 {
-	unsigned size32 = cnt & ~31;
-	unsigned remain = cnt & 31;
+	struct rb4xx_nand_info	*info;
+	int ret;
 
-	if (size32) {
-		uint8_t *addr = rb4xx_nand_read_getaddr(size32);
-		memcpy(buf, (void *)addr, size32);
-	}
+	printk(KERN_INFO DRV_DESC " version " DRV_VERSION "\n");
 
-	if (remain) {
-		do_spi_cmd(CPLD_CMD_READ_MULT, 1, buf, remain,
-			   buf + size32, NULL, 0);
+	ret = gpio_request(RB4XX_NAND_GPIO_READY, "NAND RDY");
+	if (ret) {
+		dev_err(&pdev->dev, "unable to request gpio %d\n",
+			RB4XX_NAND_GPIO_READY);
+		goto err;
 	}
-}
-
-static int rb4xx_nand_verify_data(const uint8_t *buf, unsigned cnt)
-{
-	unsigned size32 = cnt & ~31;
-	unsigned remain = cnt & 31;
 
-	if (size32) {
-		uint8_t *addr = rb4xx_nand_read_getaddr(size32);
-		if (memcmp(buf, (void *)addr, size32) != 0)
-			return 0;
+	ret = gpio_direction_input(RB4XX_NAND_GPIO_READY);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to set input mode on gpio %d\n",
+			RB4XX_NAND_GPIO_READY);
+		goto err_free_gpio_ready;
 	}
 
-	if (remain) {
-		return do_spi_cmd(CPLD_CMD_READ_MULT, 1, buf, remain,
-				  NULL, buf + size32, 0);
+	ret = gpio_request(RB4XX_NAND_GPIO_ALE, "NAND ALE");
+	if (ret) {
+		dev_err(&pdev->dev, "unable to request gpio %d\n",
+			RB4XX_NAND_GPIO_ALE);
+		goto err_free_gpio_ready;
 	}
-	return 1;
-}
-#else /* USE_FAST_READ */
-static void rb4xx_nand_read_data(uint8_t *buf, unsigned cnt)
-{
-	do_spi_cmd(CPLD_CMD_READ_MULT, 1, buf, cnt, buf, NULL, 0);
-}
 
-static int rb4xx_nand_verify_data(const uint8_t *buf, unsigned cnt)
-{
-	return do_spi_cmd(CPLD_CMD_READ_MULT, 1, buf, cnt, NULL, buf, 0);
-}
-#endif /* USE_FAST_READ */
-
-static void rb4xx_nand_write_cfg(uint8_t byte)
-{
-	do_spi_cmd(CPLD_CMD_WRITE_CFG, 1, &byte, 0, NULL, NULL, 0);
-	got_write = 1;
-}
-
-static int rb4xx_nand_dev_ready(struct mtd_info *mtd)
-{
-	return gpio_get_value(RB4XX_NAND_GPIO_RDY);
-}
-
-static void rb4xx_nand_cmd_ctrl(struct mtd_info *mtd, int cmd,
-				unsigned int ctrl)
-{
-	if (ctrl & NAND_CTRL_CHANGE) {
-		uint8_t cfg = CFG_BIT_nLEDS;
-
-		cfg |= (ctrl & NAND_CLE) ? CFG_BIT_CLE : 0;
-		cfg |= (ctrl & NAND_ALE) ? CFG_BIT_ALE : 0;
-		cfg |= (ctrl & NAND_NCE) ? 0 : CFG_BIT_nCE;
-
-		rb4xx_nand_write_cfg(cfg);
+	ret = gpio_direction_output(RB4XX_NAND_GPIO_ALE, 0);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to set output mode on gpio %d\n",
+			RB4XX_NAND_GPIO_ALE);
+		goto err_free_gpio_ale;
 	}
 
-	if (cmd != NAND_CMD_NONE)
-		rb4xx_nand_write_byte(cmd);
-}
-
-static uint8_t rb4xx_nand_read_byte(struct mtd_info *mtd)
-{
-	uint8_t byte = 0;
-
-	rb4xx_nand_read_data(&byte, 1);
-	return byte;
-}
-
-static void rb4xx_nand_write_buf(struct mtd_info *mtd, const uint8_t *buf,
-				int len)
-{
-	rb4xx_nand_write_data(buf, len);
-}
-
-static void rb4xx_nand_read_buf(struct mtd_info *mtd, uint8_t *buf,
-				int len)
-{
-	rb4xx_nand_read_data(buf, len);
-}
-
-static int rb4xx_nand_verify_buf(struct mtd_info *mtd, const uint8_t *buf,
-				int len)
-{
-	if (!rb4xx_nand_verify_data(buf, len))
-		return -EFAULT;
-
-	return 0;
-}
-
-static unsigned get_spi_ctrl(unsigned hz_max, const char *name)
-{
-	unsigned div;
-
-	div = (ar71xx_ahb_freq - 1) / (2 * hz_max);
-	/*
-	 * CPU has a bug at (div == 0) - first bit read is random
-	 */
-	if (div == 0)
-		++div;
-
-	if (name) {
-		unsigned ahb_khz = (ar71xx_ahb_freq + 500) / 1000;
-		unsigned div_real = 2 * (div + 1);
-		printk(KERN_INFO "%s SPI clock %u kHz (AHB %u kHz / %u)\n",
-		       name,
-		       ahb_khz / div_real,
-		       ahb_khz, div_real);
+	ret = gpio_request(RB4XX_NAND_GPIO_CLE, "NAND CLE");
+	if (ret) {
+		dev_err(&pdev->dev, "unable to request gpio %d\n",
+			RB4XX_NAND_GPIO_CLE);
+		goto err_free_gpio_ale;
 	}
 
-	return SPI_CTRL_FASTEST + div;
-}
-
-static int __init rb4xx_nand_probe(struct platform_device *pdev)
-{
-	struct rb4xx_nand_info	*info;
-	int ret;
-
-	printk(KERN_INFO DRV_DESC " version " DRV_VERSION "\n");
+	ret = gpio_direction_output(RB4XX_NAND_GPIO_CLE, 0);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to set output mode on gpio %d\n",
+			RB4XX_NAND_GPIO_CLE);
+		goto err_free_gpio_cle;
+	}
 
-	ret = gpio_request(RB4XX_NAND_GPIO_RDY, "NAND RDY");
+	ret = gpio_request(RB4XX_NAND_GPIO_NCE, "NAND NCE");
 	if (ret) {
-		printk(KERN_ERR "rb4xx-nand: gpio request failed\n");
-		return ret;
+		dev_err(&pdev->dev, "unable to request gpio %d\n",
+			RB4XX_NAND_GPIO_NCE);
+		goto err_free_gpio_cle;
 	}
 
-	ret = gpio_direction_input(RB4XX_NAND_GPIO_RDY);
+	ret = gpio_direction_output(RB4XX_NAND_GPIO_NCE, 1);
 	if (ret) {
-		printk(KERN_ERR "rb4xx-nand: unable to set input mode "
-					"on gpio%d\n", RB4XX_NAND_GPIO_RDY);
-		goto err_free_gpio;
+		dev_err(&pdev->dev, "unable to set output mode on gpio %d\n",
+			RB4XX_NAND_GPIO_ALE);
+		goto err_free_gpio_nce;
 	}
 
 	info = kzalloc(sizeof(*info), GFP_KERNEL);
 	if (!info) {
-		printk(KERN_ERR "rb4xx-nand: no memory for private data\n");
+		dev_err(&pdev->dev, "rb4xx-nand: no memory for private data\n");
 		ret = -ENOMEM;
-		goto err_free_gpio;
+		goto err_free_gpio_nce;
 	}
 
-#if USE_FAST_READ
-	spi_ctrl_fread = get_spi_ctrl(RB4XX_NAND_HZ, "NAND");
-#endif
-	spi_ctrl_flash = get_spi_ctrl(RB4XX_FLASH_HZ, "FLASH");
-
-	rb4xx_nand_write_cfg(CFG_BIT_nLEDS | CFG_BIT_nCE);
-
 	info->chip.priv	= &info;
 	info->mtd.priv	= &info->chip;
 	info->mtd.owner	= THIS_MODULE;
@@ -423,7 +210,7 @@ static int __init rb4xx_nand_probe(struct platform_device *pdev)
 	info->chip.read_byte	= rb4xx_nand_read_byte;
 	info->chip.write_buf	= rb4xx_nand_write_buf;
 	info->chip.read_buf	= rb4xx_nand_read_buf;
-	info->chip.verify_buf	= rb4xx_nand_verify_buf;
+//	info->chip.verify_buf	= rb4xx_nand_verify_buf;
 
 	info->chip.chip_delay	= 25;
 	info->chip.ecc.mode	= NAND_ECC_SOFT;
@@ -457,14 +244,21 @@ static int __init rb4xx_nand_probe(struct platform_device *pdev)
 
 	return 0;
 
-err_release_nand:
+ err_release_nand:
 	nand_release(&info->mtd);
-err_set_drvdata:
+ err_set_drvdata:
 	platform_set_drvdata(pdev, NULL);
-err_free_info:
+ err_free_info:
 	kfree(info);
-err_free_gpio:
-	gpio_free(RB4XX_NAND_GPIO_RDY);
+ err_free_gpio_nce:
+	gpio_free(RB4XX_NAND_GPIO_NCE);
+ err_free_gpio_cle:
+	gpio_free(RB4XX_NAND_GPIO_CLE);
+ err_free_gpio_ale:
+	gpio_free(RB4XX_NAND_GPIO_ALE);
+ err_free_gpio_ready:
+	gpio_free(RB4XX_NAND_GPIO_READY);
+ err:
 	return ret;
 }
 
@@ -475,6 +269,10 @@ static int __devexit rb4xx_nand_remove(struct platform_device *pdev)
 	nand_release(&info->mtd);
 	platform_set_drvdata(pdev, NULL);
 	kfree(info);
+	gpio_free(RB4XX_NAND_GPIO_NCE);
+	gpio_free(RB4XX_NAND_GPIO_CLE);
+	gpio_free(RB4XX_NAND_GPIO_ALE);
+	gpio_free(RB4XX_NAND_GPIO_READY);
 
 	return 0;
 }
diff --git a/target/linux/ar71xx/files/drivers/spi/rb4xx_spi.c b/target/linux/ar71xx/files/drivers/spi/rb4xx_spi.c
new file mode 100644
index 0000000000..731f501547
--- /dev/null
+++ b/target/linux/ar71xx/files/drivers/spi/rb4xx_spi.c
@@ -0,0 +1,480 @@
+/*
+ * SPI controller driver for the Mikrotik RB4xx boards
+ *
+ * Copyright (C) 2010 Gabor Juhos <juhosg@openwrt.org>
+ *
+ * This file was based on the patches for Linux 2.6.27.39 published by
+ * MikroTik for their RouterBoard 4xx series devices.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+
+#include <asm/mach-ar71xx/ar71xx.h>
+
+#define DRV_NAME	"rb4xx-spi"
+#define DRV_DESC	"Mikrotik RB4xx SPI controller driver"
+#define DRV_VERSION	"0.1.0"
+
+#define SPI_CTRL_FASTEST	0x40
+#define SPI_FLASH_HZ		33333334
+#define SPI_CPLD_HZ		33333334
+
+#define CPLD_CMD_READ_FAST	0x0b
+
+#undef RB4XX_SPI_DEBUG
+
+struct rb4xx_spi {
+	void __iomem		*base;
+	struct spi_master	*master;
+
+	unsigned		spi_ctrl_flash;
+	unsigned		spi_ctrl_fread;
+
+	spinlock_t		lock;
+	struct list_head	queue;
+	int			busy:1;
+	int			cs_wait;
+};
+
+static unsigned spi_clk_low = SPI_IOC_CS1;
+
+#ifdef RB4XX_SPI_DEBUG
+static inline void do_spi_delay(void)
+{
+	ndelay(20000);
+}
+#else
+static inline void do_spi_delay(void) { }
+#endif
+
+static inline void do_spi_init(struct spi_device *spi)
+{
+	unsigned cs = SPI_IOC_CS0 | SPI_IOC_CS1;
+
+	if (!(spi->mode & SPI_CS_HIGH))
+		cs ^= (spi->chip_select == 2) ? SPI_IOC_CS1 : SPI_IOC_CS0;
+
+	spi_clk_low = cs;
+}
+
+static inline void do_spi_finish(void __iomem *base)
+{
+	do_spi_delay();
+	__raw_writel(SPI_IOC_CS0 | SPI_IOC_CS1, base + SPI_REG_IOC);
+}
+
+static inline void do_spi_clk(void __iomem *base, int bit)
+{
+	unsigned bval = spi_clk_low | ((bit & 1) ? SPI_IOC_DO : 0);
+
+	do_spi_delay();
+	__raw_writel(bval, base + SPI_REG_IOC);
+	do_spi_delay();
+	__raw_writel(bval | SPI_IOC_CLK, base + SPI_REG_IOC);
+}
+
+static void do_spi_byte(void __iomem *base, unsigned char byte)
+{
+	do_spi_clk(base, byte >> 7);
+	do_spi_clk(base, byte >> 6);
+	do_spi_clk(base, byte >> 5);
+	do_spi_clk(base, byte >> 4);
+	do_spi_clk(base, byte >> 3);
+	do_spi_clk(base, byte >> 2);
+	do_spi_clk(base, byte >> 1);
+	do_spi_clk(base, byte);
+
+#ifdef RB4XX_SPI_DEBUG
+	printk("spi_byte sent 0x%02x got 0x%02x\n",
+	       (unsigned)byte,
+	       (unsigned char)__raw_readl(base + SPI_REG_RDS));
+#endif
+}
+
+static inline void do_spi_clk_fast(void __iomem *base, unsigned bit1,
+				   unsigned bit2)
+{
+	unsigned bval = (spi_clk_low |
+			 ((bit1 & 1) ? SPI_IOC_DO : 0) |
+			 ((bit2 & 1) ? SPI_IOC_CS2 : 0));
+	do_spi_delay();
+	__raw_writel(bval, base + SPI_REG_IOC);
+	do_spi_delay();
+	__raw_writel(bval | SPI_IOC_CLK, base + SPI_REG_IOC);
+}
+
+static void do_spi_byte_fast(void __iomem *base, unsigned char byte)
+{
+	do_spi_clk_fast(base, byte >> 7, byte >> 6);
+	do_spi_clk_fast(base, byte >> 5, byte >> 4);
+	do_spi_clk_fast(base, byte >> 3, byte >> 2);
+	do_spi_clk_fast(base, byte >> 1, byte >> 0);
+
+#ifdef RB4XX_SPI_DEBUG
+	printk("spi_byte_fast sent 0x%02x got 0x%02x\n",
+	       (unsigned)byte,
+	       (unsigned char) __raw_readl(base + SPI_REG_RDS));
+#endif
+}
+
+static int rb4xx_spi_txrx(void __iomem *base, struct spi_transfer *t)
+{
+	const unsigned char *rxv_ptr = NULL;
+	const unsigned char *tx_ptr = t->tx_buf;
+	unsigned char *rx_ptr = t->rx_buf;
+	unsigned i;
+
+#ifdef RB4XX_SPI_DEBUG
+	printk("spi_txrx len %u tx %u rx %u\n",
+	       t->len,
+	       (t->tx_buf ? 1 : 0),
+	       (t->rx_buf ? 1 : 0));
+#endif
+
+	if (t->verify) {
+		rxv_ptr = tx_ptr;
+		tx_ptr = NULL;
+	}
+
+	for (i = 0; i < t->len; ++i) {
+		unsigned char sdata = tx_ptr ? tx_ptr[i] : 0;
+
+		if (t->fast_write)
+			do_spi_byte_fast(base, sdata);
+		else
+			do_spi_byte(base, sdata);
+
+		if (rx_ptr) {
+			rx_ptr[i] = __raw_readl(base + SPI_REG_RDS) & 0xff;
+		} else if (rxv_ptr) {
+			unsigned char c = __raw_readl(base + SPI_REG_RDS);
+			if (rxv_ptr[i] != c)
+				return i;
+		}
+	}
+
+	return i;
+}
+
+static int rb4xx_spi_read_fast(struct rb4xx_spi *rbspi,
+			       struct spi_message *m)
+{
+	struct spi_transfer *t;
+	const unsigned char *tx_ptr;
+	unsigned addr;
+	void __iomem *base = rbspi->base;
+
+	/* check for exactly two transfers */
+	if (list_empty(&m->transfers) ||
+	    list_is_last(m->transfers.next, &m->transfers) ||
+	    !list_is_last(m->transfers.next->next, &m->transfers)) {
+		return -1;
+	}
+
+	/* first transfer contains command and address  */
+	t = list_entry(m->transfers.next,
+		       struct spi_transfer, transfer_list);
+
+	if (t->len != 5 || t->tx_buf == NULL)
+		return -1;
+
+	tx_ptr = t->tx_buf;
+	if (tx_ptr[0] != CPLD_CMD_READ_FAST)
+		return -1;
+
+	addr = tx_ptr[1];
+	addr = tx_ptr[2] | (addr << 8);
+	addr = tx_ptr[3] | (addr << 8);
+	addr += (unsigned) base;
+
+	m->actual_length += t->len;
+
+	/* second transfer contains data itself */
+	t = list_entry(m->transfers.next->next,
+		       struct spi_transfer, transfer_list);
+
+	if (t->tx_buf && !t->verify)
+		return -1;
+
+	__raw_writel(SPI_FS_GPIO, base + SPI_REG_FS);
+	__raw_writel(rbspi->spi_ctrl_fread, base + SPI_REG_CTRL);
+	__raw_writel(0, base + SPI_REG_FS);
+
+	if (t->rx_buf) {
+		memcpy(t->rx_buf, (const void *)addr, t->len);
+	} else if (t->tx_buf) {
+		unsigned char buf[t->len];
+		memcpy(buf, (const void *)addr, t->len);
+		if (memcmp(t->tx_buf, buf, t->len) != 0)
+			m->status = -EMSGSIZE;
+	}
+	m->actual_length += t->len;
+
+	if (rbspi->spi_ctrl_flash != rbspi->spi_ctrl_fread) {
+		__raw_writel(SPI_FS_GPIO, base + SPI_REG_FS);
+		__raw_writel(rbspi->spi_ctrl_flash, base + SPI_REG_CTRL);
+		__raw_writel(0, base + SPI_REG_FS);
+	}
+
+	return 0;
+}
+
+static int rb4xx_spi_msg(struct rb4xx_spi *rbspi, struct spi_message *m)
+{
+	struct spi_transfer *t = NULL;
+	void __iomem *base = rbspi->base;
+
+	m->status = 0;
+	if (list_empty(&m->transfers))
+		return -1;
+
+	if (m->fast_read)
+		if (rb4xx_spi_read_fast(rbspi, m) == 0)
+			return -1;
+
+	__raw_writel(SPI_FS_GPIO, base + SPI_REG_FS);
+	__raw_writel(SPI_CTRL_FASTEST, base + SPI_REG_CTRL);
+	do_spi_init(m->spi);
+
+	list_for_each_entry (t, &m->transfers, transfer_list) {
+		int len;
+
+		len = rb4xx_spi_txrx(base, t);
+		if (len != t->len) {
+			m->status = -EMSGSIZE;
+			break;
+		}
+		m->actual_length += len;
+
+		if (t->cs_change) {
+			if (list_is_last(&t->transfer_list, &m->transfers)) {
+				/* wait for continuation */
+				return m->spi->chip_select;
+			}
+			do_spi_finish(base);
+			ndelay(100);
+		}
+	}
+
+	do_spi_finish(base);
+	__raw_writel(rbspi->spi_ctrl_flash, base + SPI_REG_CTRL);
+	__raw_writel(0, base + SPI_REG_FS);
+	return -1;
+}
+
+static void rb4xx_spi_process_queue_locked(struct rb4xx_spi *rbspi,
+					   unsigned long *flags)
+{
+	int cs = rbspi->cs_wait;
+
+	rbspi->busy = 1;
+	while (!list_empty(&rbspi->queue)) {
+		struct spi_message *m;
+
+		list_for_each_entry(m, &rbspi->queue, queue)
+			if (cs < 0 || cs == m->spi->chip_select)
+				break;
+
+		if (&m->queue == &rbspi->queue)
+			break;
+
+		list_del_init(&m->queue);
+		spin_unlock_irqrestore(&rbspi->lock, *flags);
+
+		cs = rb4xx_spi_msg(rbspi, m);
+		m->complete(m->context);
+
+		spin_lock_irqsave(&rbspi->lock, *flags);
+	}
+
+	rbspi->cs_wait = cs;
+	rbspi->busy = 0;
+
+	if (cs >= 0) {
+		/* TODO: add timer to unlock cs after 1s inactivity */
+	}
+}
+
+static int rb4xx_spi_transfer(struct spi_device *spi,
+			      struct spi_message *m)
+{
+	struct rb4xx_spi *rbspi = spi_master_get_devdata(spi->master);
+	unsigned long flags;
+
+	m->actual_length = 0;
+	m->status = -EINPROGRESS;
+
+	spin_lock_irqsave(&rbspi->lock, flags);
+	list_add_tail(&m->queue, &rbspi->queue);
+	if (rbspi->busy ||
+	    (rbspi->cs_wait >= 0 && rbspi->cs_wait != m->spi->chip_select)) {
+		/* job will be done later */
+		spin_unlock_irqrestore(&rbspi->lock, flags);
+		return 0;
+	}
+
+	/* process job in current context */
+	rb4xx_spi_process_queue_locked(rbspi, &flags);
+	spin_unlock_irqrestore(&rbspi->lock, flags);
+
+	return 0;
+}
+
+static int rb4xx_spi_setup(struct spi_device *spi)
+{
+	struct rb4xx_spi *rbspi = spi_master_get_devdata(spi->master);
+	unsigned long flags;
+
+	if (spi->mode & ~(SPI_CS_HIGH)) {
+		dev_err(&spi->dev, "mode %x not supported\n",
+		        (unsigned) spi->mode);
+		return -EINVAL;
+	}
+
+	if (spi->bits_per_word != 8 && spi->bits_per_word != 0) {
+		dev_err(&spi->dev, "bits_per_word %u not supported\n",
+		        (unsigned) spi->bits_per_word);
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&rbspi->lock, flags);
+	if (rbspi->cs_wait == spi->chip_select && !rbspi->busy) {
+		rbspi->cs_wait = -1;
+		rb4xx_spi_process_queue_locked(rbspi, &flags);
+	}
+	spin_unlock_irqrestore(&rbspi->lock, flags);
+
+	return 0;
+}
+
+static unsigned get_spi_ctrl(unsigned hz_max, const char *name)
+{
+	unsigned div;
+
+	div = (ar71xx_ahb_freq - 1) / (2 * hz_max);
+
+	/*
+	 * CPU has a bug at (div == 0) - first bit read is random
+	 */
+	if (div == 0)
+		++div;
+
+	if (name) {
+		unsigned ahb_khz = (ar71xx_ahb_freq + 500) / 1000;
+		unsigned div_real = 2 * (div + 1);
+		pr_debug("rb4xx: %s SPI clock %u kHz (AHB %u kHz / %u)\n",
+		       name,
+		       ahb_khz / div_real,
+		       ahb_khz, div_real);
+	}
+
+	return SPI_CTRL_FASTEST + div;
+}
+
+static int rb4xx_spi_probe(struct platform_device *pdev)
+{
+	struct spi_master *master;
+	struct rb4xx_spi *rbspi;
+	struct resource *r;
+	int err = 0;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(*rbspi));
+	if (master == NULL) {
+		dev_err(&pdev->dev, "no memory for spi_master\n");
+		err = -ENOMEM;
+		goto err_out;
+	}
+
+	master->bus_num = 0;
+	master->num_chipselect = 3;
+	master->setup = rb4xx_spi_setup;
+	master->transfer = rb4xx_spi_transfer;
+
+	rbspi = spi_master_get_devdata(master);
+	platform_set_drvdata(pdev, rbspi);
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (r == NULL) {
+		err = -ENOENT;
+		goto err_put_master;
+	}
+
+	rbspi->base = ioremap(r->start, r->end - r->start + 1);
+	if (!rbspi->base) {
+		err = -ENXIO;
+		goto err_put_master;
+	}
+
+	rbspi->master = master;
+	rbspi->spi_ctrl_flash = get_spi_ctrl(SPI_FLASH_HZ, "FLASH");
+	rbspi->spi_ctrl_fread = get_spi_ctrl(SPI_CPLD_HZ, "CPLD");
+	rbspi->cs_wait = -1;
+
+	spin_lock_init(&rbspi->lock);
+	INIT_LIST_HEAD(&rbspi->queue);
+
+	err = spi_register_master(master);
+	if (err) {
+		dev_err(&pdev->dev, "failed to register SPI master\n");
+		goto err_iounmap;
+	}
+
+	return 0;
+
+ err_iounmap:
+	iounmap(rbspi->base);
+ err_put_master:
+	platform_set_drvdata(pdev, NULL);
+	spi_master_put(master);
+ err_out:
+	return err;
+}
+
+static int rb4xx_spi_remove(struct platform_device *pdev)
+{
+	struct rb4xx_spi *rbspi = platform_get_drvdata(pdev);
+
+	iounmap(rbspi->base);
+	platform_set_drvdata(pdev, NULL);
+	spi_master_put(rbspi->master);
+
+	return 0;
+}
+
+static struct platform_driver rb4xx_spi_drv = {
+	.probe		= rb4xx_spi_probe,
+	.remove		= rb4xx_spi_remove,
+        .driver		= {
+		.name	= DRV_NAME,
+		.owner	= THIS_MODULE,
+        },
+};
+
+static int __init rb4xx_spi_init(void)
+{
+	return platform_driver_register(&rb4xx_spi_drv);
+}
+subsys_initcall(rb4xx_spi_init);
+
+static void __exit rb4xx_spi_exit(void)
+{
+	platform_driver_unregister(&rb4xx_spi_drv);
+}
+
+module_exit(rb4xx_spi_exit);
+
+MODULE_DESCRIPTION(DRV_DESC);
+MODULE_VERSION(DRV_VERSION);
+MODULE_AUTHOR("Gabor Juhos <juhosg@openwrt.org>");
+MODULE_LICENSE("GPL v2");
diff --git a/target/linux/ar71xx/files/drivers/spi/spi_rb4xx_cpld.c b/target/linux/ar71xx/files/drivers/spi/spi_rb4xx_cpld.c
new file mode 100644
index 0000000000..8b34c1b23d
--- /dev/null
+++ b/target/linux/ar71xx/files/drivers/spi/spi_rb4xx_cpld.c
@@ -0,0 +1,440 @@
+/*
+ * SPI driver for the CPLD chip on the Mikrotik RB4xx boards
+ *
+ * Copyright (C) 2010 Gabor Juhos <juhosg@openwrt.org>
+ *
+ * This file was based on the patches for Linux 2.6.27.39 published by
+ * MikroTik for their RouterBoard 4xx series devices.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ */
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/bitops.h>
+#include <linux/spi/spi.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+
+#include <asm/mach-ar71xx/rb4xx_cpld.h>
+
+#define DRV_NAME	"spi-rb4xx-cpld"
+#define DRV_DESC	"RB4xx CPLD driver"
+#define DRV_VERSION	"0.1.0"
+
+#define CPLD_CMD_WRITE_NAND	0x08 /* send cmd, n x send data, send indle */
+#define CPLD_CMD_WRITE_CFG	0x09 /* send cmd, n x send cfg */
+#define CPLD_CMD_READ_NAND	0x0a /* send cmd, send idle, n x read data */
+#define CPLD_CMD_READ_FAST	0x0b /* send cmd, 4 x idle, n x read data */
+#define CPLD_CMD_LED5_ON	0x0c /* send cmd */
+#define CPLD_CMD_LED5_OFF	0x0d /* send cmd */
+
+struct rb4xx_cpld {
+	struct spi_device	*spi;
+	struct mutex		lock;
+	struct gpio_chip	chip;
+	unsigned int		config;
+};
+
+static struct rb4xx_cpld *rb4xx_cpld;
+
+static inline struct rb4xx_cpld *gpio_to_cpld(struct gpio_chip *chip)
+{
+	return container_of(chip, struct rb4xx_cpld, chip);
+}
+
+static int rb4xx_cpld_write_cmd(struct rb4xx_cpld *cpld, unsigned char cmd)
+{
+	struct spi_transfer t[1];
+	struct spi_message m;
+	unsigned char tx_buf[1];
+	int err;
+
+	spi_message_init(&m);
+	memset(&t, 0, sizeof(t));
+
+	t[0].tx_buf = tx_buf;
+	t[0].len = sizeof(tx_buf);
+	spi_message_add_tail(&t[0], &m);
+
+	tx_buf[0] = cmd;
+
+	err = spi_sync(cpld->spi, &m);
+	return err;
+}
+
+static int rb4xx_cpld_write_cfg(struct rb4xx_cpld *cpld, unsigned char config)
+{
+	struct spi_transfer t[1];
+	struct spi_message m;
+	unsigned char cmd[2];
+	int err;
+
+	spi_message_init(&m);
+	memset(&t, 0, sizeof(t));
+
+	t[0].tx_buf = cmd;
+	t[0].len = sizeof(cmd);
+	spi_message_add_tail(&t[0], &m);
+
+	cmd[0] = CPLD_CMD_WRITE_CFG;
+	cmd[1] = config;
+
+	err = spi_sync(cpld->spi, &m);
+	return err;
+}
+
+static int __rb4xx_cpld_change_cfg(struct rb4xx_cpld *cpld, unsigned mask,
+				   unsigned value)
+{
+	unsigned int config;
+	int err;
+
+	config = cpld->config & ~mask;
+	config |= value;
+
+	if ((cpld->config ^ config) & 0xff) {
+		err = rb4xx_cpld_write_cfg(cpld, config);
+		if (err)
+			return err;
+	}
+
+	if ((cpld->config ^ config) & CPLD_CFG_nLED5) {
+		err = rb4xx_cpld_write_cmd(cpld, (value) ? CPLD_CMD_LED5_ON :
+							   CPLD_CMD_LED5_OFF);
+		if (err)
+			return err;
+	}
+
+	cpld->config = config;
+	return 0;
+}
+
+int rb4xx_cpld_change_cfg(unsigned mask, unsigned value)
+{
+	int ret;
+
+	if (rb4xx_cpld == NULL)
+		return -ENODEV;
+
+	mutex_lock(&rb4xx_cpld->lock);
+	ret = __rb4xx_cpld_change_cfg(rb4xx_cpld, mask, value);
+	mutex_unlock(&rb4xx_cpld->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(rb4xx_cpld_change_cfg);
+
+int rb4xx_cpld_read_from(unsigned addr, unsigned char *rx_buf,
+			 const unsigned char *verify_buf, unsigned count)
+{
+	const unsigned char cmd[5] = {
+		CPLD_CMD_READ_FAST,
+		(addr >> 16) & 0xff,
+		(addr >> 8) & 0xff,
+		 addr & 0xff,
+		 0
+	};
+	struct spi_transfer t[2] = {
+		{
+			.tx_buf = &cmd,
+			.len = 5,
+		},
+		{
+			.tx_buf = verify_buf,
+			.rx_buf = rx_buf,
+			.len = count,
+			.verify = (verify_buf != NULL),
+		},
+	};
+	struct spi_message m;
+
+	if (rb4xx_cpld == NULL)
+		return -ENODEV;
+
+	spi_message_init(&m);
+	m.fast_read = 1;
+	spi_message_add_tail(&t[0], &m);
+	spi_message_add_tail(&t[1], &m);
+	return spi_sync(rb4xx_cpld->spi, &m);
+}
+EXPORT_SYMBOL_GPL(rb4xx_cpld_read_from);
+
+#if 0
+int rb4xx_cpld_read(unsigned char *buf, unsigned char *verify_buf,
+		    unsigned count)
+{
+	struct spi_transfer t[2];
+	struct spi_message m;
+	unsigned char cmd[2];
+
+	if (rb4xx_cpld == NULL)
+		return -ENODEV;
+
+	spi_message_init(&m);
+	memset(&t, 0, sizeof(t));
+
+	/* send command */
+	t[0].tx_buf = cmd;
+	t[0].len = sizeof(cmd);
+	spi_message_add_tail(&t[0], &m);
+
+	cmd[0] = CPLD_CMD_READ_NAND;
+	cmd[1] = 0;
+
+	/* read data */
+	t[1].rx_buf = buf;
+	t[1].len = count;
+	spi_message_add_tail(&t[1], &m);
+
+	return spi_sync(rb4xx_cpld->spi, &m);
+}
+#else
+int rb4xx_cpld_read(unsigned char *rx_buf, const unsigned char *verify_buf,
+		    unsigned count)
+{
+	static const unsigned char cmd[2] = { CPLD_CMD_READ_NAND, 0 };
+	struct spi_transfer t[2] = {
+		{
+			.tx_buf = &cmd,
+			.len = 2,
+		}, {
+			.tx_buf = verify_buf,
+			.rx_buf = rx_buf,
+			.len = count,
+			.verify = (verify_buf != NULL),
+		},
+	};
+	struct spi_message m;
+
+	if (rb4xx_cpld == NULL)
+		return -ENODEV;
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t[0], &m);
+	spi_message_add_tail(&t[1], &m);
+	return spi_sync(rb4xx_cpld->spi, &m);
+}
+#endif
+EXPORT_SYMBOL_GPL(rb4xx_cpld_read);
+
+int rb4xx_cpld_write(const unsigned char *buf, unsigned count)
+{
+#if 0
+	struct spi_transfer t[3];
+	struct spi_message m;
+	unsigned char cmd[1];
+
+	if (rb4xx_cpld == NULL)
+		return -ENODEV;
+
+	memset(&t, 0, sizeof(t));
+	spi_message_init(&m);
+
+	/* send command */
+	t[0].tx_buf = cmd;
+	t[0].len = sizeof(cmd);
+	spi_message_add_tail(&t[0], &m);
+
+	cmd[0] = CPLD_CMD_WRITE_NAND;
+
+	/* write data */
+	t[1].tx_buf = buf;
+	t[1].len = count;
+	spi_message_add_tail(&t[1], &m);
+
+	/* send idle */
+	t[2].len = 1;
+	spi_message_add_tail(&t[2], &m);
+
+	return spi_sync(rb4xx_cpld->spi, &m);
+#else
+	static const unsigned char cmd = CPLD_CMD_WRITE_NAND;
+	struct spi_transfer t[3] = {
+		{
+			.tx_buf = &cmd,
+			.len = 1,
+		}, {
+			.tx_buf = buf,
+			.len = count,
+			.fast_write = 1,
+		}, {
+			.len = 1,
+			.fast_write = 1,
+		},
+	};
+	struct spi_message m;
+
+	if (rb4xx_cpld == NULL)
+		return -ENODEV;
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t[0], &m);
+	spi_message_add_tail(&t[1], &m);
+	spi_message_add_tail(&t[2], &m);
+	return spi_sync(rb4xx_cpld->spi, &m);
+#endif
+}
+EXPORT_SYMBOL_GPL(rb4xx_cpld_write);
+
+static int rb4xx_cpld_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	struct rb4xx_cpld *cpld = gpio_to_cpld(chip);
+	int ret;
+
+	mutex_lock(&cpld->lock);
+	ret = (cpld->config >> offset) & 1;
+	mutex_unlock(&cpld->lock);
+
+	return ret;
+}
+
+static void rb4xx_cpld_gpio_set(struct gpio_chip *chip, unsigned offset,
+				int value)
+{
+	struct rb4xx_cpld *cpld = gpio_to_cpld(chip);
+
+	mutex_lock(&cpld->lock);
+	__rb4xx_cpld_change_cfg(cpld, (1 << offset), !!value << offset);
+	mutex_unlock(&cpld->lock);
+}
+
+static int rb4xx_cpld_gpio_direction_input(struct gpio_chip *chip,
+					   unsigned offset)
+{
+	return -EOPNOTSUPP;
+}
+
+static int rb4xx_cpld_gpio_direction_output(struct gpio_chip *chip,
+					    unsigned offset,
+					    int value)
+{
+	struct rb4xx_cpld *cpld = gpio_to_cpld(chip);
+	int ret;
+
+	mutex_lock(&cpld->lock);
+	ret = __rb4xx_cpld_change_cfg(cpld, (1 << offset), !!value << offset);
+	mutex_unlock(&cpld->lock);
+
+	return ret;
+}
+
+static int rb4xx_cpld_gpio_init(struct rb4xx_cpld *cpld, unsigned int base)
+{
+	int err;
+
+	/* init config */
+	cpld->config = CPLD_CFG_nLED1 | CPLD_CFG_nLED2 | CPLD_CFG_nLED3 |
+		       CPLD_CFG_nLED4 | CPLD_CFG_nCE;
+	rb4xx_cpld_write_cfg(cpld, cpld->config);
+
+	/* setup GPIO chip */
+	cpld->chip.label = DRV_NAME;
+
+	cpld->chip.get = rb4xx_cpld_gpio_get;
+	cpld->chip.set = rb4xx_cpld_gpio_set;
+	cpld->chip.direction_input = rb4xx_cpld_gpio_direction_input;
+	cpld->chip.direction_output = rb4xx_cpld_gpio_direction_output;
+
+	cpld->chip.base = base;
+	cpld->chip.ngpio = CPLD_NUM_GPIOS;
+	cpld->chip.can_sleep = 1;
+	cpld->chip.dev = &cpld->spi->dev;
+	cpld->chip.owner = THIS_MODULE;
+
+	err = gpiochip_add(&cpld->chip);
+	if (err)
+		dev_err(&cpld->spi->dev, "adding GPIO chip failed, err=%d\n",
+			err);
+
+	return err;
+}
+
+static int __devinit rb4xx_cpld_probe(struct spi_device *spi)
+{
+	struct rb4xx_cpld *cpld;
+	struct rb4xx_cpld_platform_data *pdata;
+	int err;
+
+	pdata = spi->dev.platform_data;
+	if (!pdata) {
+		dev_dbg(&spi->dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	cpld = kzalloc(sizeof(*cpld), GFP_KERNEL);
+	if (!cpld) {
+		dev_err(&spi->dev, "no memory for private data\n");
+		return -ENOMEM;
+	}
+
+	mutex_init(&cpld->lock);
+	cpld->spi = spi_dev_get(spi);
+	dev_set_drvdata(&spi->dev, cpld);
+
+	spi->mode = SPI_MODE_0;
+	spi->bits_per_word = 8;
+	err = spi_setup(spi);
+	if (err) {
+		dev_err(&spi->dev, "spi_setup failed, err=%d\n", err);
+		goto err_drvdata;
+	}
+
+	err = rb4xx_cpld_gpio_init(cpld, pdata->gpio_base);
+	if (err)
+		goto err_drvdata;
+
+	rb4xx_cpld = cpld;
+
+	return 0;
+
+ err_drvdata:
+	dev_set_drvdata(&spi->dev, NULL);
+	kfree(cpld);
+
+	return err;
+}
+
+static int __devexit rb4xx_cpld_remove(struct spi_device *spi)
+{
+	struct rb4xx_cpld *cpld;
+
+	rb4xx_cpld = NULL;
+	cpld = dev_get_drvdata(&spi->dev);
+	dev_set_drvdata(&spi->dev, NULL);
+	kfree(cpld);
+
+	return 0;
+}
+
+static struct spi_driver rb4xx_cpld_driver = {
+	.driver = {
+		.name		= DRV_NAME,
+		.bus		= &spi_bus_type,
+		.owner		= THIS_MODULE,
+	},
+	.probe		= rb4xx_cpld_probe,
+	.remove		= __devexit_p(rb4xx_cpld_remove),
+};
+
+static int __init rb4xx_cpld_init(void)
+{
+	return spi_register_driver(&rb4xx_cpld_driver);
+}
+module_init(rb4xx_cpld_init);
+
+static void __exit rb4xx_cpld_exit(void)
+{
+	spi_unregister_driver(&rb4xx_cpld_driver);
+}
+module_exit(rb4xx_cpld_exit);
+
+MODULE_DESCRIPTION(DRV_DESC);
+MODULE_VERSION(DRV_VERSION);
+MODULE_AUTHOR("Gabor Juhos <juhosg@openwrt.org>");
+MODULE_LICENSE("GPL v2");