summaryrefslogtreecommitdiff
path: root/meta-moblin/packages/linux/linux-moblin-2.6.29.1/linux-2.6.29-timberdale.patch
diff options
context:
space:
mode:
Diffstat (limited to 'meta-moblin/packages/linux/linux-moblin-2.6.29.1/linux-2.6.29-timberdale.patch')
-rw-r--r--meta-moblin/packages/linux/linux-moblin-2.6.29.1/linux-2.6.29-timberdale.patch6095
1 files changed, 6095 insertions, 0 deletions
diff --git a/meta-moblin/packages/linux/linux-moblin-2.6.29.1/linux-2.6.29-timberdale.patch b/meta-moblin/packages/linux/linux-moblin-2.6.29.1/linux-2.6.29-timberdale.patch
new file mode 100644
index 000000000..c36e5ba4a
--- /dev/null
+++ b/meta-moblin/packages/linux/linux-moblin-2.6.29.1/linux-2.6.29-timberdale.patch
@@ -0,0 +1,6095 @@
+Patch provided by Mocean in order to enable the timberdale subsystem of the Russelville board.
+
+Signed-off-by: Joel Clark <joel.clark@intel.com>
+Acked-by: Arjan van de Ven <arjan@infradead.org>
+Signed-off-by: Todd Brandt todd.e.brandt@intel.com
+
+
+diff -uNr linux-2.6.29-clean/drivers/gpio/Kconfig linux-2.6.29/drivers/gpio/Kconfig
+--- linux-2.6.29-clean/drivers/gpio/Kconfig 2009-04-01 09:20:23.000000000 -0700
++++ linux-2.6.29/drivers/gpio/Kconfig 2009-04-06 13:51:47.000000000 -0700
+@@ -161,6 +161,12 @@
+
+ If unsure, say N.
+
++config GPIO_TIMBERDALE
++ tristate "Support for timberdale GPIO"
++ depends on MFD_TIMBERDALE && GPIOLIB
++ ---help---
++ Add support for GPIO usage of some pins of the timberdale FPGA.
++
+ comment "SPI GPIO expanders:"
+
+ config GPIO_MAX7301
+diff -uNr linux-2.6.29-clean/drivers/gpio/Makefile linux-2.6.29/drivers/gpio/Makefile
+--- linux-2.6.29-clean/drivers/gpio/Makefile 2009-04-01 09:20:23.000000000 -0700
++++ linux-2.6.29/drivers/gpio/Makefile 2009-04-06 13:51:47.000000000 -0700
+@@ -12,3 +12,4 @@
+ obj-$(CONFIG_GPIO_TWL4030) += twl4030-gpio.o
+ obj-$(CONFIG_GPIO_XILINX) += xilinx_gpio.o
+ obj-$(CONFIG_GPIO_BT8XX) += bt8xxgpio.o
++obj-$(CONFIG_GPIO_TIMBERDALE) += timbgpio.o
+diff -uNr linux-2.6.29-clean/drivers/gpio/timbgpio.c linux-2.6.29/drivers/gpio/timbgpio.c
+--- linux-2.6.29-clean/drivers/gpio/timbgpio.c 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/gpio/timbgpio.c 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,275 @@
++/*
++ * timbgpio.c timberdale FPGA GPIO driver
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Timberdale FPGA GPIO
++ */
++
++#include <linux/module.h>
++#include <linux/gpio.h>
++#include <linux/pci.h>
++#include <linux/platform_device.h>
++#include <linux/interrupt.h>
++
++#include "timbgpio.h"
++
++static u32 timbgpio_configure(struct gpio_chip *gpio, unsigned nr,
++ unsigned off, unsigned val)
++{
++ struct timbgpio *tgpio = container_of(gpio, struct timbgpio, gpio);
++
++ u32 config, oldconfig, wconfig;
++
++ mutex_lock(&tgpio->lock);
++ config = ioread32(tgpio->membase + off);
++ oldconfig = config;
++
++ if (val)
++ config |= (1 << nr);
++ else
++ config &= ~(1 << nr);
++
++ iowrite32(config, tgpio->membase + off);
++ wconfig = ioread32(tgpio->membase + off);
++ mutex_unlock(&tgpio->lock);
++
++ return oldconfig;
++}
++
++static int timbgpio_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
++{
++ timbgpio_configure(gpio, nr, TGPIODIR, 1);
++ return 0;
++}
++
++static int timbgpio_gpio_get(struct gpio_chip *gpio, unsigned nr)
++{
++ struct timbgpio *tgpio = container_of(gpio, struct timbgpio, gpio);
++ u32 value;
++
++ value = ioread32(tgpio->membase + TGPIOVAL);
++ return (value & (1 << nr)) ? 1 : 0;
++}
++
++static int timbgpio_gpio_direction_output(struct gpio_chip *gpio,
++ unsigned nr, int val)
++{
++ timbgpio_configure(gpio, nr, TGPIODIR, 0);
++ return 0;
++}
++
++
++
++static void timbgpio_gpio_set(struct gpio_chip *gpio,
++ unsigned nr, int val)
++{
++ timbgpio_configure(gpio, nr, TGPIOVAL, val);
++}
++
++/*
++ * Function to control flank or level triggered GPIO pin
++ * @nr - pin
++ * @ val - 1: flank, 0: level
++ *
++ */
++static void timbgpio_gpio_flnk_lvl_ctrl(struct gpio_chip *gpio,
++ unsigned nr, int val)
++{
++ timbgpio_configure(gpio, nr, TGPIOFLK, val);
++}
++EXPORT_SYMBOL(timbgpio_gpio_flnk_lvl_ctrl);
++
++/*
++ * Enable or disable interrupt
++ *
++ */
++static void timbgpio_gpio_int_ctrl(struct gpio_chip *gpio,
++ unsigned nr, int val)
++{
++ timbgpio_configure(gpio, nr, TGPIOINT, val);
++}
++EXPORT_SYMBOL(timbgpio_gpio_int_ctrl);
++
++/*
++ * @val - 1: Asserted high or on positive flank, 0: Asserted low or on negative flank
++ *
++ */
++static void timbgpio_gpio_lvl_ctrl(struct gpio_chip *gpio,
++ unsigned nr, int val)
++{
++ timbgpio_configure(gpio, nr, TGPIOLVL, val);
++}
++EXPORT_SYMBOL(timbgpio_gpio_lvl_ctrl);
++
++static void timbgpio_gpio_int_clr(struct gpio_chip *gpio,
++ unsigned nr, int val)
++{
++ timbgpio_configure(gpio, nr, TGPIOINT_CLR, val);
++}
++EXPORT_SYMBOL(timbgpio_gpio_int_clr);
++
++
++static irqreturn_t timbgpio_handleinterrupt(int irq, void *devid)
++{
++ struct timbgpio *tgpio = (struct timbgpio *)devid;
++
++ iowrite32(0xffffffff, tgpio->membase + TGPIOINT_CLR);
++
++ return IRQ_HANDLED;
++}
++
++static int timbgpio_probe(struct platform_device *dev)
++{
++ int err, irq;
++ struct gpio_chip *gc;
++ struct timbgpio *tgpio;
++ struct resource *iomem, *rscr;
++
++ iomem = platform_get_resource(dev, IORESOURCE_MEM, 0);
++ if (!iomem) {
++ err = -EINVAL;
++ goto err_mem;
++ }
++
++ tgpio = kzalloc(sizeof(*tgpio), GFP_KERNEL);
++ if (!tgpio) {
++ err = -EINVAL;
++ goto err_mem;
++ }
++
++ mutex_init(&tgpio->lock);
++
++ rscr = &tgpio->rscr;
++ rscr->name = "timb-gpio";
++ rscr->start = iomem->start;
++ rscr->end = iomem->end;
++ rscr->flags = IORESOURCE_MEM;
++
++ err = request_resource(iomem, rscr);
++ if (err)
++ goto err_request;
++
++ tgpio->membase = ioremap(rscr->start, resource_size(rscr));
++ if (!tgpio->membase) {
++ err = -ENOMEM;
++ goto err_ioremap;
++ }
++
++ gc = &tgpio->gpio;
++
++ gc->label = "timbgpio";
++ gc->owner = THIS_MODULE;
++ gc->direction_input = timbgpio_gpio_direction_input;
++ gc->get = timbgpio_gpio_get;
++ gc->direction_output = timbgpio_gpio_direction_output;
++ gc->set = timbgpio_gpio_set;
++ gc->dbg_show = NULL;
++ gc->base = 0;
++ gc->ngpio = TIMB_NR_GPIOS;
++ gc->can_sleep = 0;
++
++ err = gpiochip_add(gc);
++ if (err)
++ goto err_chipadd;
++
++ platform_set_drvdata(dev, tgpio);
++
++ /* register interrupt */
++ irq = platform_get_irq(dev, 0);
++ if (irq < 0)
++ goto err_get_irq;
++
++ /* clear pending interrupts */
++ iowrite32(0xffffffff, tgpio->membase + TGPIOINT_CLR);
++ iowrite32(0x0, tgpio->membase + TGPIOINT);
++
++ /* request IRQ */
++ err = request_irq(irq, timbgpio_handleinterrupt, IRQF_SHARED,
++ "timb-gpio", tgpio);
++ if (err) {
++ printk(KERN_ERR "timbgpio: Failed to request IRQ\n");
++ goto err_get_irq;
++ }
++
++ return err;
++
++err_get_irq:
++ err = gpiochip_remove(&tgpio->gpio);
++ if (err)
++ printk(KERN_ERR "timbgpio: failed to remove gpio_chip\n");
++err_chipadd:
++ iounmap(tgpio->membase);
++err_ioremap:
++ release_resource(&tgpio->rscr);
++err_request:
++ kfree(tgpio);
++err_mem:
++ printk(KERN_ERR "timberdale: Failed to register GPIOs: %d\n", err);
++
++ return err;
++}
++
++static int timbgpio_remove(struct platform_device *dev)
++{
++ int err;
++ struct timbgpio *tgpio = platform_get_drvdata(dev);
++
++ /* disable interrupts */
++ iowrite32(0x0, tgpio->membase + TGPIOINT);
++
++ free_irq(platform_get_irq(dev, 0), tgpio);
++ err = gpiochip_remove(&tgpio->gpio);
++ if (err)
++ printk(KERN_ERR "timbgpio: failed to remove gpio_chip\n");
++
++ iounmap(tgpio->membase);
++ release_resource(&tgpio->rscr);
++ kfree(tgpio);
++
++ return 0;
++}
++
++static struct platform_driver timbgpio_platform_driver = {
++ .driver = {
++ .name = "timb-gpio",
++ .owner = THIS_MODULE,
++ },
++ .probe = timbgpio_probe,
++ .remove = timbgpio_remove,
++};
++
++/*--------------------------------------------------------------------------*/
++
++static int __init timbgpio_init(void)
++{
++ return platform_driver_register(&timbgpio_platform_driver);
++}
++
++static void __exit timbgpio_exit(void)
++{
++ platform_driver_unregister(&timbgpio_platform_driver);
++}
++
++module_init(timbgpio_init);
++module_exit(timbgpio_exit);
++
++MODULE_DESCRIPTION("Timberdale GPIO driver");
++MODULE_LICENSE("GPL v2");
++MODULE_AUTHOR("Mocean Laboratories");
++MODULE_ALIAS("platform:timb-gpio");
++
+diff -uNr linux-2.6.29-clean/drivers/gpio/timbgpio.h linux-2.6.29/drivers/gpio/timbgpio.h
+--- linux-2.6.29-clean/drivers/gpio/timbgpio.h 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/gpio/timbgpio.h 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,48 @@
++/*
++ * timbgpio.h timberdale FPGA GPIO driver defines
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Timberdale FPGA GPIO
++ */
++
++#ifndef _TIMBGPIO_H_
++#define _TIMBGPIO_H_
++
++#include <linux/mutex.h>
++#include <linux/gpio.h>
++
++#define TIMB_NR_GPIOS 16
++
++#define TGPIOVAL 0
++#define TGPIODIR 0x04
++#define TGPIOINT 0x08
++#define TGPIOINT_STATUS 0x0c
++#define TGPIOINT_PENDING 0x10
++#define TGPIOINT_CLR 0x14
++#define TGPIOFLK 0x18
++#define TGPIOLVL 0x1c
++
++struct timbgpio {
++ void __iomem *membase;
++ struct resource rscr;
++ struct mutex lock; /* mutual exclusion */
++ struct pci_dev *pdev;
++ struct gpio_chip gpio;
++};
++
++#endif
+diff -uNr linux-2.6.29-clean/drivers/i2c/busses/i2c-ocores.c linux-2.6.29/drivers/i2c/busses/i2c-ocores.c
+--- linux-2.6.29-clean/drivers/i2c/busses/i2c-ocores.c 2009-04-01 09:20:24.000000000 -0700
++++ linux-2.6.29/drivers/i2c/busses/i2c-ocores.c 2009-04-06 13:51:47.000000000 -0700
+@@ -216,6 +216,7 @@
+ struct ocores_i2c_platform_data *pdata;
+ struct resource *res, *res2;
+ int ret;
++ u8 i;
+
+ res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+ if (!res)
+@@ -271,6 +272,10 @@
+ goto add_adapter_failed;
+ }
+
++ /* add in known devices to the bus */
++ for (i = 0; i < pdata->num_devices; i++)
++ i2c_new_device(&i2c->adap, pdata->devices + i);
++
+ return 0;
+
+ add_adapter_failed:
+diff -uNr linux-2.6.29-clean/drivers/input/touchscreen/Kconfig linux-2.6.29/drivers/input/touchscreen/Kconfig
+--- linux-2.6.29-clean/drivers/input/touchscreen/Kconfig 2009-04-01 09:20:23.000000000 -0700
++++ linux-2.6.29/drivers/input/touchscreen/Kconfig 2009-04-06 13:51:47.000000000 -0700
+@@ -397,6 +397,17 @@
+ To compile this driver as a module, choose M here: the
+ module will be called touchit213.
+
++config TOUCHSCREEN_TSC2003
++ tristate "TSC2003 based touchscreens"
++ depends on I2C
++ help
++ Say Y here if you have a TSC2003 based touchscreen.
++
++ If unsure, say N.
++
++ To compile this driver as a module, choose M here: the
++ module will be called tsc2003.
++
+ config TOUCHSCREEN_TSC2007
+ tristate "TSC2007 based touchscreens"
+ depends on I2C
+diff -uNr linux-2.6.29-clean/drivers/input/touchscreen/Makefile linux-2.6.29/drivers/input/touchscreen/Makefile
+--- linux-2.6.29-clean/drivers/input/touchscreen/Makefile 2009-04-01 09:20:23.000000000 -0700
++++ linux-2.6.29/drivers/input/touchscreen/Makefile 2009-04-06 13:51:47.000000000 -0700
+@@ -25,6 +25,7 @@
+ obj-$(CONFIG_TOUCHSCREEN_TOUCHIT213) += touchit213.o
+ obj-$(CONFIG_TOUCHSCREEN_TOUCHRIGHT) += touchright.o
+ obj-$(CONFIG_TOUCHSCREEN_TOUCHWIN) += touchwin.o
++obj-$(CONFIG_TOUCHSCREEN_TSC2003) += tsc2003.o
+ obj-$(CONFIG_TOUCHSCREEN_TSC2007) += tsc2007.o
+ obj-$(CONFIG_TOUCHSCREEN_UCB1400) += ucb1400_ts.o
+ obj-$(CONFIG_TOUCHSCREEN_WACOM_W8001) += wacom_w8001.o
+diff -uNr linux-2.6.29-clean/drivers/input/touchscreen/tsc2003.c linux-2.6.29/drivers/input/touchscreen/tsc2003.c
+--- linux-2.6.29-clean/drivers/input/touchscreen/tsc2003.c 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/input/touchscreen/tsc2003.c 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,387 @@
++/*
++ * tsc2003.c Driver for TI TSC2003 touch screen controller
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * TI TSC2003
++ *
++ * Inspired by tsc2007, Copyright (c) 2008 MtekVision Co., Ltd.
++ */
++#include <linux/module.h>
++#include <linux/input.h>
++#include <linux/interrupt.h>
++#include <linux/i2c.h>
++#include <linux/i2c/tsc2007.h>
++#include <linux/kthread.h>
++#include <linux/semaphore.h>
++
++#define TSC2003_DRIVER_NAME "tsc2003"
++
++#define TS_POLL_PERIOD 20 /* ms delay between samples */
++
++#define TSC2003_MEASURE_TEMP0 (0x0 << 4)
++#define TSC2003_MEASURE_AUX (0x2 << 4)
++#define TSC2003_MEASURE_TEMP1 (0x4 << 4)
++#define TSC2003_ACTIVATE_XN (0x8 << 4)
++#define TSC2003_ACTIVATE_YN (0x9 << 4)
++#define TSC2003_ACTIVATE_YP_XN (0xa << 4)
++#define TSC2003_SETUP (0xb << 4)
++#define TSC2003_MEASURE_X (0xc << 4)
++#define TSC2003_MEASURE_Y (0xd << 4)
++#define TSC2003_MEASURE_Z1 (0xe << 4)
++#define TSC2003_MEASURE_Z2 (0xf << 4)
++
++#define TSC2003_POWER_OFF_IRQ_EN (0x0 << 2)
++#define TSC2003_ADC_ON_IRQ_DIS0 (0x1 << 2)
++#define TSC2003_ADC_OFF_IRQ_EN (0x2 << 2)
++#define TSC2003_ADC_ON_IRQ_DIS1 (0x3 << 2)
++
++#define TSC2003_12BIT (0x0 << 1)
++#define TSC2003_8BIT (0x1 << 1)
++
++#define MAX_12BIT ((1 << 12) - 1)
++
++#define ADC_ON_12BIT (TSC2003_12BIT | TSC2003_ADC_ON_IRQ_DIS0)
++
++#define READ_Y (ADC_ON_12BIT | TSC2003_MEASURE_Y)
++#define READ_Z1 (ADC_ON_12BIT | TSC2003_MEASURE_Z1)
++#define READ_Z2 (ADC_ON_12BIT | TSC2003_MEASURE_Z2)
++#define READ_X (ADC_ON_12BIT | TSC2003_MEASURE_X)
++#define PWRDOWN (TSC2003_12BIT | TSC2003_POWER_OFF_IRQ_EN)
++
++struct ts_event {
++ int x;
++ int y;
++ int z1, z2;
++};
++
++struct tsc2003 {
++ struct input_dev *input;
++ char phys[32];
++ struct task_struct *task;
++ struct ts_event tc;
++ struct completion penirq_completion;
++
++ struct i2c_client *client;
++
++ u16 model;
++ u16 x_plate_ohms;
++
++ unsigned pendown;
++};
++
++static inline int tsc2003_xfer(struct tsc2003 *tsc, u8 cmd)
++{
++ s32 data;
++ u16 val;
++
++ data = i2c_smbus_read_word_data(tsc->client, cmd);
++ if (data < 0) {
++ dev_err(&tsc->client->dev, "i2c io error: %d\n", data);
++ return data;
++ }
++
++ /* The protocol and raw data format from i2c interface:
++ * S Addr Wr [A] Comm [A] S Addr Rd [A] [DataLow] A [DataHigh] NA P
++ * Where DataLow has [D11-D4], DataHigh has [D3-D0 << 4 | Dummy 4bit].
++ */
++ val = swab16(data) >> 4;
++
++ dev_dbg(&tsc->client->dev, "data: 0x%x, val: 0x%x\n", data, val);
++
++ return val;
++}
++
++static void tsc2003_send_event(void *tsc)
++{
++ struct tsc2003 *ts = tsc;
++ struct input_dev *input = ts->input;
++ u32 rt = 0;
++ u16 x, y, z1, z2;
++
++ x = ts->tc.x;
++ y = ts->tc.y;
++ z1 = ts->tc.z1;
++ z2 = ts->tc.z2;
++
++ /* range filtering */
++ if (x == MAX_12BIT)
++ x = 0;
++
++ if (likely(x && z1)) {
++ /* compute touch pressure resistance using equation #1 */
++ rt = z2;
++ rt -= z1;
++ rt *= x;
++ rt *= ts->x_plate_ohms;
++ rt /= z1;
++ rt = (rt + 2047) >> 12;
++ }
++
++ /* Sample found inconsistent by debouncing or pressure is beyond
++ * the maximum. Don't report it to user space, repeat at least
++ * once more the measurement
++ */
++ if (rt > MAX_12BIT)
++ return;
++
++ /* NOTE: We can't rely on the pressure to determine the pen down
++ * state, even this controller has a pressure sensor. The pressure
++ * value can fluctuate for quite a while after lifting the pen and
++ * in some cases may not even settle at the expected value.
++ *
++ * The only safe way to check for the pen up condition is in the
++ * timer by reading the pen signal state (it's a GPIO _and_ IRQ).
++ */
++ if (rt) {
++ if (!ts->pendown) {
++ dev_dbg(&ts->client->dev, "DOWN\n");
++
++ input_report_key(input, BTN_TOUCH, 1);
++ ts->pendown = 1;
++ }
++
++ input_report_abs(input, ABS_X, x);
++ input_report_abs(input, ABS_Y, y);
++ input_report_abs(input, ABS_PRESSURE, rt);
++
++ input_sync(input);
++
++ dev_dbg(&ts->client->dev, "point(%4d,%4d), pressure (%4u)\n",
++ x, y, rt);
++ } else if (ts->pendown) {
++ /* pen up */
++ dev_dbg(&ts->client->dev, "UP\n");
++ input_report_key(input, BTN_TOUCH, 0);
++ input_report_abs(input, ABS_PRESSURE, 0);
++ input_sync(input);
++
++ ts->pendown = 0;
++ }
++}
++
++static int tsc2003_power_off_irq_en(struct tsc2003 *tsc)
++{
++ /* power down */
++ return tsc2003_xfer(tsc, PWRDOWN);
++}
++
++static int tsc2003_read_values(struct tsc2003 *tsc)
++{
++ /* y- still on; turn on only y+ (and ADC) */
++ tsc->tc.y = tsc2003_xfer(tsc, READ_Y);
++ if (tsc->tc.y < 0)
++ return tsc->tc.y;
++
++ /* turn y- off, x+ on, then leave in lowpower */
++ tsc->tc.x = tsc2003_xfer(tsc, READ_X);
++ if (tsc->tc.x < 0)
++ return tsc->tc.x;
++
++ /* turn y+ off, x- on; we'll use formula #1 */
++ tsc->tc.z1 = tsc2003_xfer(tsc, READ_Z1);
++ if (tsc->tc.z1 < 0)
++ return tsc->tc.z1;
++
++ tsc->tc.z2 = tsc2003_xfer(tsc, READ_Z2);
++ if (tsc->tc.z2 < 0)
++ return tsc->tc.z2;
++
++ return 0;
++}
++
++
++static irqreturn_t tsc2003_irq(int irq, void *handle)
++{
++ struct tsc2003 *ts = handle;
++
++ /* do not call the synced version -> deadlock */
++ disable_irq_nosync(irq);
++ /* signal the thread to continue */
++ complete(&ts->penirq_completion);
++
++ return IRQ_HANDLED;
++}
++
++static int tsc2003_thread(void *d)
++{
++ struct tsc2003 *ts = (struct tsc2003 *)d;
++ int ret;
++
++ allow_signal(SIGKILL);
++
++ while (!signal_pending(current)) {
++ /* power down and wait for interrupt */
++ do {
++ /* loop because the I2C bus might be busy */
++ ret = msleep_interruptible(TS_POLL_PERIOD);
++ if (!ret)
++ ret = tsc2003_power_off_irq_en(ts);
++ } while (ret == -EAGAIN && !signal_pending(current));
++
++ if (signal_pending(current))
++ break;
++
++ ret = wait_for_completion_interruptible(&ts->penirq_completion);
++ if (!ret) {
++ int first = 1;
++ /* got IRQ, start poll, until pen is up */
++ while (!ret && !signal_pending(current)
++ && (first || ts->pendown)) {
++ ret = tsc2003_read_values(ts);
++ if (!ret)
++ tsc2003_send_event(ts);
++ ret = msleep_interruptible(TS_POLL_PERIOD);
++ first = 0;
++ }
++
++ /* we re enable the interrupt */
++ if (!signal_pending(current))
++ enable_irq(ts->client->irq);
++ }
++ }
++
++ return 0;
++}
++
++static int tsc2003_probe(struct i2c_client *client,
++ const struct i2c_device_id *id)
++{
++ struct tsc2003 *ts;
++ struct tsc2007_platform_data *pdata = client->dev.platform_data;
++ struct input_dev *input_dev;
++ int err;
++
++ if (!pdata) {
++ dev_err(&client->dev, "platform data is required!\n");
++ return -EINVAL;
++ }
++
++ if (!i2c_check_functionality(client->adapter,
++ I2C_FUNC_SMBUS_READ_WORD_DATA))
++ return -EIO;
++
++ ts = kzalloc(sizeof(struct tsc2003), GFP_KERNEL);
++ input_dev = input_allocate_device();
++ if (!ts || !input_dev) {
++ err = -ENOMEM;
++ goto err_free_mem;
++ }
++
++ ts->client = client;
++ i2c_set_clientdata(client, ts);
++
++ ts->input = input_dev;
++
++ ts->model = pdata->model;
++ ts->x_plate_ohms = pdata->x_plate_ohms;
++
++ snprintf(ts->phys, sizeof(ts->phys),
++ "%s/input0", dev_name(&client->dev));
++
++ input_dev->name = TSC2003_DRIVER_NAME" Touchscreen";
++ input_dev->phys = ts->phys;
++ input_dev->id.bustype = BUS_I2C;
++
++ input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
++ input_dev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
++
++ input_set_abs_params(input_dev, ABS_X, 0, MAX_12BIT, 0, 0);
++ input_set_abs_params(input_dev, ABS_Y, 0, MAX_12BIT, 0, 0);
++ input_set_abs_params(input_dev, ABS_PRESSURE, 0, MAX_12BIT, 0, 0);
++
++ init_completion(&ts->penirq_completion);
++
++ ts->task = kthread_run(tsc2003_thread, ts, TSC2003_DRIVER_NAME);
++ if (IS_ERR(ts->task)) {
++ err = PTR_ERR(ts->task);
++ goto err_free_mem;
++ }
++
++ err = request_irq(client->irq, tsc2003_irq, 0,
++ client->dev.driver->name, ts);
++ if (err < 0) {
++ dev_err(&client->dev, "irq %d busy?\n", client->irq);
++ goto err_free_thread;
++ }
++
++ err = input_register_device(input_dev);
++ if (err)
++ goto err_free_irq;
++
++ dev_info(&client->dev, "registered with irq (%d)\n", client->irq);
++
++ return 0;
++
++ err_free_irq:
++ free_irq(client->irq, ts);
++ err_free_thread:
++ kthread_stop(ts->task);
++ err_free_mem:
++ input_free_device(input_dev);
++ kfree(ts);
++ return err;
++}
++
++static int tsc2003_remove(struct i2c_client *client)
++{
++ struct tsc2003 *ts = i2c_get_clientdata(client);
++
++ free_irq(client->irq, ts);
++ send_sig(SIGKILL, ts->task, 1);
++ kthread_stop(ts->task);
++ input_unregister_device(ts->input);
++ kfree(ts);
++
++ return 0;
++}
++
++static struct i2c_device_id tsc2003_idtable[] = {
++ { TSC2003_DRIVER_NAME, 0 },
++ { }
++};
++
++MODULE_DEVICE_TABLE(i2c, tsc2003_idtable);
++
++static struct i2c_driver tsc2003_driver = {
++ .driver = {
++ .owner = THIS_MODULE,
++ .name = TSC2003_DRIVER_NAME,
++ .bus = &i2c_bus_type,
++ },
++ .id_table = tsc2003_idtable,
++ .probe = tsc2003_probe,
++ .remove = tsc2003_remove,
++};
++
++static int __init tsc2003_init(void)
++{
++ return i2c_add_driver(&tsc2003_driver);
++}
++
++static void __exit tsc2003_exit(void)
++{
++ i2c_del_driver(&tsc2003_driver);
++}
++
++module_init(tsc2003_init);
++module_exit(tsc2003_exit);
++
++MODULE_AUTHOR("Mocean Laboratories <info@mocean-labs.com>");
++MODULE_DESCRIPTION("TSC2003 TouchScreen Driver");
++MODULE_LICENSE("GPL v2");
++
+diff -uNr linux-2.6.29-clean/drivers/media/video/adv7180.c linux-2.6.29/drivers/media/video/adv7180.c
+--- linux-2.6.29-clean/drivers/media/video/adv7180.c 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/media/video/adv7180.c 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,361 @@
++/*
++ * adv7180.c Analog Devices ADV7180 video decoder driver
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++#include <linux/module.h>
++#include <linux/init.h>
++#include <linux/interrupt.h>
++#include <linux/delay.h>
++#include <linux/errno.h>
++#include <linux/fs.h>
++#include <linux/kernel.h>
++#include <linux/major.h>
++#include <linux/slab.h>
++#include <linux/mm.h>
++#include <linux/signal.h>
++#include <linux/types.h>
++#include <linux/io.h>
++#include <asm/pgtable.h>
++#include <asm/page.h>
++#include <linux/uaccess.h>
++
++#include <linux/i2c-ocores.h>
++#include <linux/platform_device.h>
++#include <linux/i2c.h>
++#include <linux/i2c-id.h>
++#include <linux/videodev.h>
++#include <linux/video_decoder.h>
++#include <media/v4l2-ioctl.h>
++#include <media/adv7180.h>
++
++
++MODULE_DESCRIPTION("Analog Devices ADV7180 video decoder driver");
++MODULE_AUTHOR("Mocean Laboratories");
++MODULE_LICENSE("GPL v2");
++
++static inline int adv7180_write(struct i2c_client *client,
++ u8 reg, u8 value)
++{
++ struct adv7180 *decoder = i2c_get_clientdata(client);
++
++ decoder->reg[reg] = value;
++ return i2c_smbus_write_byte_data(client, reg, value);
++}
++
++static inline int adv7180_read(struct i2c_client *client, u8 reg)
++{
++ return i2c_smbus_read_byte_data(client, reg);
++}
++
++static int adv7180_write_block(struct i2c_client *client,
++ const u8 *data, unsigned int len)
++{
++ int ret = -1;
++ u8 reg;
++
++ /* the adv7180 has an autoincrement function, use it if
++ * the adapter understands raw I2C */
++ if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
++ /* do raw I2C, not smbus compatible */
++ struct adv7180 *decoder = i2c_get_clientdata(client);
++ u8 block_data[32];
++ int block_len;
++
++ while (len >= 2) {
++ block_len = 0;
++ reg = data[0];
++ block_data[block_len++] = reg;
++ do {
++ block_data[block_len++] =
++ decoder->reg[reg++] = data[1];
++ len -= 2;
++ data += 2;
++ } while (len >= 2 && data[0] == reg &&
++ block_len < 32);
++
++ ret = i2c_master_send(client, block_data, block_len);
++ if (ret < 0)
++ break;
++ }
++ } else {
++ /* do some slow I2C emulation kind of thing */
++ while (len >= 2) {
++ reg = *data++;
++ ret = adv7180_write(client, reg, *data++);
++ if (ret < 0)
++ break;
++
++ len -= 2;
++ }
++ }
++
++ return ret;
++}
++#ifdef CONFIG_MFD_TIMBERDALE
++static irqreturn_t adv7180_irq(int irq, void *dev_id)
++{
++ struct adv7180 *decoder = (struct adv7180 *) dev_id;
++
++ /* Activate access to sub-regs */
++ adv7180_write(decoder->client, ADV7180_ADI_CTRL, ADI_ENABLE);
++
++ /* TODO: implement a real interrupt handler
++ * for now just
++ * clear all four regs
++ */
++ adv7180_write_block(decoder->client, reset_icr, sizeof(reset_icr));
++
++ return IRQ_HANDLED;
++}
++#endif
++static int adv7180_command(struct i2c_client *client,
++ unsigned int cmd, void *arg)
++{
++ struct adv7180 *decoder = i2c_get_clientdata(client);
++ int *iarg = (int *)arg;
++ int status;
++
++ switch (cmd) {
++
++ case DECODER_INIT:
++ adv7180_write(client, 0x0f, 0x80); /* Reset */
++ break;
++
++ case DECODER_GET_CAPABILITIES:
++ {
++ struct video_decoder_capability *cap = arg;
++ cap->flags = VIDEO_DECODER_PAL |
++ VIDEO_DECODER_NTSC |
++ VIDEO_DECODER_SECAM |
++ VIDEO_DECODER_AUTO;
++ cap->inputs = 3;
++ cap->outputs = 1;
++ }
++ break;
++
++ case DECODER_GET_STATUS:
++ {
++ *iarg = 0;
++ status = adv7180_read(client, ADV7180_SR);
++ if ((status & ADV7180_STATUS_PAL))
++ *iarg = (*iarg | DECODER_STATUS_PAL);
++
++ if ((status & ADV7180_STATUS_NTSC))
++ *iarg = (*iarg | DECODER_STATUS_NTSC);
++
++ if ((status & ADV7180_STATUS_SECAM))
++ *iarg = (*iarg | DECODER_STATUS_SECAM);
++ }
++ break;
++
++ case DECODER_SET_NORM:
++ {
++ int v = *(int *) arg;
++ if (decoder->norm != v) {
++ decoder->norm = v;
++ switch (v) {
++ case VIDEO_MODE_NTSC:
++ adv7180_write(client, ADV7180_IN_CTRL, 0x40);
++ break;
++ case VIDEO_MODE_PAL:
++ adv7180_write(client, ADV7180_IN_CTRL, 0x70);
++ break;
++ case VIDEO_MODE_SECAM:
++ adv7180_write(client, ADV7180_IN_CTRL, 0x90);
++ break;
++ case VIDEO_MODE_AUTO:
++ adv7180_write(client, ADV7180_IN_CTRL, 0x00);
++ break;
++ default:
++ return -EPERM;
++ }
++ }
++ }
++ break;
++
++ case DECODER_SET_INPUT:
++ {
++ int v = *(int *) arg;
++ if (decoder->input != v) {
++ decoder->input = v;
++
++ switch (v) {
++ case CVBS:
++ adv7180_write_block(client, init_cvbs_64,
++ sizeof(init_cvbs_64));
++ break;
++ case SVIDEO:
++ adv7180_write_block(client, init_svideo_64,
++ sizeof(init_svideo_64));
++ break;
++ case YPbPr:
++ adv7180_write_block(client, init_ypbpr_64,
++ sizeof(init_ypbpr_64));
++ break;
++ default:
++ return -EINVAL;
++ }
++ }
++ }
++ break;
++
++ case DECODER_SET_OUTPUT:
++ {
++ }
++ break;
++
++ case DECODER_ENABLE_OUTPUT:
++ {
++ }
++ break;
++
++ case DECODER_SET_PICTURE:
++ {
++ }
++ break;
++
++ case DECODER_DUMP:
++ {
++ adv7180_write(client, 1, 0x88);
++ }
++ break;
++
++ default:
++ return -EINVAL;
++ }
++ return 0;
++}
++
++/* ----------------------------------------------------------------------- */
++
++/*
++ * Generic i2c probe
++ * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1'
++ */
++static unsigned short normal_i2c[] = {
++ 0x40 >> 1, 0x41 >> 1,
++ I2C_ADV7180 >> 1, 0x43 >> 1,
++ I2C_CLIENT_END
++};
++
++I2C_CLIENT_INSMOD;
++
++static int adv7180_detect(struct i2c_client *client, int kind,
++ struct i2c_board_info *info)
++{
++ struct i2c_adapter *adapter = client->adapter;
++
++ if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE
++ | I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
++ return -ENODEV;
++
++ /* Is chip alive ? */
++ if (adv7180_read(client, 0x11) != 0x1b)
++ return -ENODEV;
++
++ strlcpy(info->type, DRIVER_NAME, I2C_NAME_SIZE);
++
++ return 0;
++}
++
++static int adv7180_probe(struct i2c_client *client,
++ const struct i2c_device_id *id)
++{
++ int err = 0;
++ struct adv7180 *decoder;
++
++ printk(KERN_INFO DRIVER_NAME" chip found @ 0x%x (%s)\n",
++ client->addr << 1, client->adapter->name);
++
++ decoder = kzalloc(sizeof(struct adv7180), GFP_KERNEL);
++ if (decoder == NULL)
++ return -ENOMEM;
++
++ decoder->norm = VIDEO_MODE_PAL | VIDEO_MODE_NTSC |
++ VIDEO_MODE_SECAM |
++ VIDEO_MODE_AUTO;
++ decoder->input = CVBS;
++ decoder->enable = 1;
++ decoder->client = client;
++ i2c_set_clientdata(client, decoder);
++#ifdef CONFIG_MFD_TIMBERDALE
++ err = request_irq(client->irq, adv7180_irq, 0,
++ client->dev.driver->name, decoder);
++ if (err < 0) {
++ dev_err(&client->dev, "irq %d busy?\n", client->irq);
++ goto err_free_dec;
++ }
++ dev_info(&client->dev, "registered with irq (%d)\n", client->irq);
++#endif
++ adv7180_command(client, DECODER_INIT, NULL); /* Reset */
++
++ return 0;
++#ifdef CONFIG_MFD_TIMBERDALE
++err_free_dec:
++ kfree(decoder);
++
++ return err;
++#endif
++}
++
++static int adv7180_remove(struct i2c_client *client)
++{
++ struct adv7180 *decoder = i2c_get_clientdata(client);
++#ifdef CONFIG_MFD_TIMBERDALE
++ free_irq(client->irq, decoder);
++#endif
++ kfree(decoder);
++ return 0;
++}
++
++/* ----------------------------------------------------------------------- */
++static const struct i2c_device_id adv7180_id[] = {
++ { DRIVER_NAME, 0 },
++ { }
++};
++MODULE_DEVICE_TABLE(i2c, adv7180_id);
++
++static struct i2c_driver i2c_driver_adv7180 = {
++ .driver = {
++ .owner = THIS_MODULE,
++ .name = DRIVER_NAME,
++ .bus = &i2c_bus_type,
++ },
++
++ .id_table = adv7180_id,
++ .probe = adv7180_probe,
++ .remove = adv7180_remove,
++
++ .class = 0xffffffff,
++ .detect = adv7180_detect,
++ .address_data = &addr_data,
++
++ .command = adv7180_command,
++};
++
++static int __init adv7180_init(void)
++{
++ return i2c_add_driver(&i2c_driver_adv7180);
++}
++
++static void __exit adv7180_exit(void)
++{
++ i2c_del_driver(&i2c_driver_adv7180);
++}
++
++module_init(adv7180_init);
++module_exit(adv7180_exit);
+diff -uNr linux-2.6.29-clean/drivers/media/video/Kconfig linux-2.6.29/drivers/media/video/Kconfig
+--- linux-2.6.29-clean/drivers/media/video/Kconfig 2009-04-01 09:20:24.000000000 -0700
++++ linux-2.6.29/drivers/media/video/Kconfig 2009-04-06 13:51:47.000000000 -0700
+@@ -251,6 +251,15 @@
+
+ comment "Video decoders"
+
++config VIDEO_ADV7180
++ tristate "Analog Devices ADV7180 decoder"
++ depends on VIDEO_V4L1 && I2C
++ ---help---
++ Support for the Analog Devices ADV7180 video decoder.
++
++ To compile this driver as a module, choose M here: the
++ module will be called adv7180.
++
+ config VIDEO_BT819
+ tristate "BT819A VideoStream decoder"
+ depends on VIDEO_V4L1 && I2C
+@@ -800,6 +809,12 @@
+ ---help---
+ This is a v4l2 driver for the TI OMAP2 camera capture interface
+
++config VIDEO_TIMBERDALE
++ tristate "Support for timberdale Video In/LogiWIN"
++ depends on VIDEO_V4L2 && MFD_TIMBERDALE_DMA
++ ---help---
++ Add support for the Video In peripherial of the timberdale FPGA.
++
+ #
+ # USB Multimedia device configuration
+ #
+diff -uNr linux-2.6.29-clean/drivers/media/video/Makefile linux-2.6.29/drivers/media/video/Makefile
+--- linux-2.6.29-clean/drivers/media/video/Makefile 2009-04-01 09:20:24.000000000 -0700
++++ linux-2.6.29/drivers/media/video/Makefile 2009-04-06 13:51:47.000000000 -0700
+@@ -52,6 +52,7 @@
+ obj-$(CONFIG_VIDEO_SAA7191) += saa7191.o
+ obj-$(CONFIG_VIDEO_ADV7170) += adv7170.o
+ obj-$(CONFIG_VIDEO_ADV7175) += adv7175.o
++obj-$(CONFIG_VIDEO_ADV7180) += adv7180.o
+ obj-$(CONFIG_VIDEO_VPX3220) += vpx3220.o
+ obj-$(CONFIG_VIDEO_BT819) += bt819.o
+ obj-$(CONFIG_VIDEO_BT856) += bt856.o
+@@ -148,6 +149,8 @@
+
+ obj-$(CONFIG_VIDEO_AU0828) += au0828/
+
++obj-$(CONFIG_VIDEO_TIMBERDALE) += timblogiw.o
++
+ obj-$(CONFIG_USB_VIDEO_CLASS) += uvc/
+
+ EXTRA_CFLAGS += -Idrivers/media/dvb/dvb-core
+diff -uNr linux-2.6.29-clean/drivers/media/video/timblogiw.c linux-2.6.29/drivers/media/video/timblogiw.c
+--- linux-2.6.29-clean/drivers/media/video/timblogiw.c 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/media/video/timblogiw.c 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,930 @@
++/*
++ * timblogiw.c timberdale FPGA LogiWin Video In driver
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Timberdale FPGA LogiWin Video In
++ */
++
++#include <linux/list.h>
++#include <linux/version.h>
++#include <linux/module.h>
++#include <linux/pci.h>
++#include <linux/dma-mapping.h>
++#include <media/v4l2-common.h>
++#include <media/v4l2-ioctl.h>
++#include <linux/platform_device.h>
++#include <linux/interrupt.h>
++#include "timblogiw.h"
++#include <linux/mfd/timbdma.h>
++
++
++#define TIMBLOGIW_CTRL 0x40
++
++#define TIMBLOGIW_H_SCALE 0x20
++#define TIMBLOGIW_V_SCALE 0x28
++
++#define TIMBLOGIW_X_CROP 0x58
++#define TIMBLOGIW_Y_CROP 0x60
++
++#define TIMBLOGIW_W_CROP 0x00
++#define TIMBLOGIW_H_CROP 0x08
++
++#define TIMBLOGIW_VERSION_CODE 0x02
++
++#define TIMBLOGIW_FRAME 0x10
++#define TIMBLOGIW_DROP 0x20
++
++#define TIMBLOGIW_BUF 0x04
++#define TIMBLOGIW_TBI 0x2c
++#define TIMBLOGIW_BPL 0x30
++
++#define dbg(...)
++
++const struct timblogiw_tvnorm timblogiw_tvnorms[] = {
++ {
++ .v4l2_id = V4L2_STD_PAL,
++ .name = "PAL",
++ .swidth = 720,
++ .sheight = 576
++ },
++ {
++ .v4l2_id = V4L2_STD_NTSC_M,
++ .name = "NTSC",
++ .swidth = 720,
++ .sheight = 480
++ }
++};
++
++static void timblogiw_handleframe(unsigned long arg)
++{
++ struct timblogiw_frame *f;
++ struct timblogiw *lw = (struct timblogiw *)arg;
++
++ spin_lock_bh(&lw->queue_lock);
++ if (!list_empty(&lw->inqueue)) {
++ /* put the entry in the outqueue */
++ f = list_entry(lw->inqueue.next, struct timblogiw_frame, frame);
++
++ /* copy data from the DMA buffer */
++ memcpy(f->bufmem, lw->dma.filled->buf, f->buf.length);
++ /* buffer consumed */
++ lw->dma.filled = NULL;
++
++ do_gettimeofday(&f->buf.timestamp);
++ f->buf.sequence = ++lw->frame_count;
++ f->buf.field = V4L2_FIELD_NONE;
++ f->state = F_DONE;
++ f->buf.bytesused = lw->frame_size;
++ list_move_tail(&f->frame, &lw->outqueue);
++ /* wake up any waiter */
++ wake_up(&lw->wait_frame);
++ }
++ spin_unlock_bh(&lw->queue_lock);
++}
++
++static int timblogiw_isr(u32 flag, void *pdev)
++{
++ struct timblogiw *lw = (struct timblogiw *)pdev;
++
++ if (!lw->dma.filled) {
++ /* no stored transfer so far, store this, and flip to next */
++ lw->dma.filled = lw->dma.transfer + lw->dma.curr;
++ lw->dma.curr = !lw->dma.curr;
++ }
++
++ if (lw->stream == STREAM_ON)
++ timb_start_dma(DMA_IRQ_VIDEO_RX,
++ lw->dma.transfer[lw->dma.curr].handle, lw->frame_size,
++ lw->bytesperline);
++
++ if (flag & DMA_IRQ_VIDEO_DROP)
++ dbg("%s: frame dropped\n", __func__);
++ if (flag & DMA_IRQ_VIDEO_RX) {
++ dbg("%s: frame RX\n", __func__);
++ tasklet_schedule(&lw->tasklet);
++ }
++ return 0;
++}
++
++static void timblogiw_empty_framequeues(struct timblogiw *lw)
++{
++ u32 i;
++
++ dbg("%s\n", __func__);
++
++ INIT_LIST_HEAD(&lw->inqueue);
++ INIT_LIST_HEAD(&lw->outqueue);
++
++ for (i = 0; i < lw->num_frames; i++) {
++ lw->frame[i].state = F_UNUSED;
++ lw->frame[i].buf.bytesused = 0;
++ }
++}
++
++u32 timblogiw_request_buffers(struct timblogiw *lw, u32 count)
++{
++ /* needs to be page aligned cause the */
++ /* buffers can be mapped individually! */
++ const size_t imagesize = PAGE_ALIGN(lw->frame_size);
++ void *buff = NULL;
++ u32 i;
++
++ dbg("%s - request of %i buffers of size %zi\n",
++ __func__, count, lw->frame_size);
++
++ lw->dma.transfer[0].buf = pci_alloc_consistent(lw->dev, imagesize,
++ &lw->dma.transfer[0].handle);
++ lw->dma.transfer[1].buf = pci_alloc_consistent(lw->dev, imagesize,
++ &lw->dma.transfer[1].handle);
++ if ((lw->dma.transfer[0].buf == NULL) ||
++ (lw->dma.transfer[1].buf == NULL)) {
++ printk(KERN_ALERT "alloc failed\n");
++ if (lw->dma.transfer[0].buf != NULL)
++ pci_free_consistent(lw->dev, imagesize,
++ lw->dma.transfer[0].buf,
++ lw->dma.transfer[0].handle);
++ if (lw->dma.transfer[1].buf != NULL)
++ pci_free_consistent(lw->dev, imagesize,
++ lw->dma.transfer[1].buf,
++ lw->dma.transfer[1].handle);
++ return 0;
++ }
++
++ if (count > TIMBLOGIW_NUM_FRAMES)
++ count = TIMBLOGIW_NUM_FRAMES;
++
++ lw->num_frames = count;
++ while (lw->num_frames > 0) {
++ buff = vmalloc_32(lw->num_frames * imagesize);
++ if (buff) {
++ memset(buff, 0, lw->num_frames * imagesize);
++ break;
++ }
++ lw->num_frames--;
++ }
++
++ for (i = 0; i < lw->num_frames; i++) {
++ lw->frame[i].bufmem = buff + i * imagesize;
++ lw->frame[i].buf.index = i;
++ lw->frame[i].buf.m.offset = i * imagesize;
++ lw->frame[i].buf.length = lw->frame_size;
++ lw->frame[i].buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
++ lw->frame[i].buf.sequence = 0;
++ lw->frame[i].buf.field = V4L2_FIELD_NONE;
++ lw->frame[i].buf.memory = V4L2_MEMORY_MMAP;
++ lw->frame[i].buf.flags = 0;
++ }
++
++ lw->dma.curr = 0;
++ lw->dma.filled = NULL;
++ return lw->num_frames;
++}
++
++void timblogiw_release_buffers(struct timblogiw *lw)
++{
++ dbg("%s\n", __func__);
++
++ if (lw->frame[0].bufmem != NULL) {
++ vfree(lw->frame[0].bufmem);
++ lw->frame[0].bufmem = NULL;
++ lw->num_frames = TIMBLOGIW_NUM_FRAMES;
++ pci_free_consistent(lw->dev, lw->frame_size,
++ lw->dma.transfer[0].buf, lw->dma.transfer[0].handle);
++ pci_free_consistent(lw->dev, lw->frame_size,
++ lw->dma.transfer[1].buf, lw->dma.transfer[1].handle);
++ }
++}
++
++/* IOCTL functions */
++
++static int timblogiw_g_fmt(struct timblogiw *lw, struct v4l2_format *format)
++{
++ dbg("%s -\n", __func__);
++
++ if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
++ return -EINVAL;
++
++ format->fmt.pix.width = lw->width;
++ format->fmt.pix.height = lw->height;
++ format->fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
++ format->fmt.pix.bytesperline = lw->bytesperline;
++ format->fmt.pix.sizeimage = lw->frame_size;
++ format->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
++ format->fmt.pix.field = V4L2_FIELD_NONE;
++ return 0;
++}
++
++static int timblogiw_s_fmt(struct timblogiw *lw, struct v4l2_format *format)
++{
++ struct v4l2_pix_format *pix = &format->fmt.pix;
++ dbg("%s - type: %d\n", __func__, format->type);
++
++ if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
++ return -EINVAL;
++
++ if ((lw->height != pix->height) || (lw->width != lw->width))
++ return -EINVAL;
++
++ if (format->fmt.pix.field != V4L2_FIELD_NONE)
++ return -EINVAL;
++
++ dbg("%s - width=%d, height=%d, pixelformat=%d, field=%d\n"
++ "bytes per line %d, size image: %d, colorspace: %d\n",
++ __func__,
++ pix->width, pix->height, pix->pixelformat, pix->field,
++ pix->bytesperline, pix->sizeimage, pix->colorspace);
++
++ return 0;
++}
++
++static int timblogiw_querycap(struct timblogiw *lw,
++ struct v4l2_capability *cap)
++{
++ memset(cap, 0, sizeof(*cap));
++ strncpy(cap->card, "Timberdale Video", sizeof(cap->card)-1);
++ strncpy(cap->driver, "Timblogiw", sizeof(cap->card)-1);
++ cap->version = TIMBLOGIW_VERSION_CODE;
++ cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
++ V4L2_CAP_STREAMING;
++
++ return 0;
++}
++
++static int timblogiw_enum_fmt(struct timblogiw *lw, struct v4l2_fmtdesc *fmt)
++{
++ dbg("%s - VIDIOC_ENUM_FMT\n", __func__);
++
++ if (fmt->index != 0)
++ return -EINVAL;
++ memset(fmt, 0, sizeof(*fmt));
++ fmt->index = 0;
++ fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
++ strncpy(fmt->description, "4:2:2, packed, YUYV",
++ sizeof(fmt->description)-1);
++ fmt->pixelformat = V4L2_PIX_FMT_YUYV;
++ memset(fmt->reserved, 0, sizeof(fmt->reserved));
++
++ return 0;
++}
++
++static int timblogiw_reqbufs(struct timblogiw *lw,
++ struct v4l2_requestbuffers *rb)
++{
++ if (rb->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
++ rb->memory != V4L2_MEMORY_MMAP)
++ return -EINVAL;
++
++ timblogiw_empty_framequeues(lw);
++
++ timblogiw_release_buffers(lw);
++ if (rb->count)
++ rb->count = timblogiw_request_buffers(lw, rb->count);
++
++ dbg("%s - VIDIOC_REQBUFS: io method is mmap. num bufs %i\n",
++ __func__, rb->count);
++
++ return 0;
++}
++
++static int timblogiw_querybuf(struct timblogiw *lw, struct v4l2_buffer *b)
++{
++ if (b->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
++ b->index >= lw->num_frames)
++ return -EINVAL;
++
++ memcpy(b, &lw->frame[b->index].buf, sizeof(*b));
++
++ if (lw->frame[b->index].vma_use_count)
++ b->flags |= V4L2_BUF_FLAG_MAPPED;
++
++ if (lw->frame[b->index].state == F_DONE)
++ b->flags |= V4L2_BUF_FLAG_DONE;
++ else if (lw->frame[b->index].state != F_UNUSED)
++ b->flags |= V4L2_BUF_FLAG_QUEUED;
++
++ return 0;
++}
++
++static int timblogiw_qbuf(struct timblogiw *lw, struct v4l2_buffer *b)
++{
++ unsigned long lock_flags;
++
++ if (b->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
++ b->index >= lw->num_frames)
++ return -EINVAL;
++
++ if (lw->frame[b->index].state != F_UNUSED)
++ return -EAGAIN;
++
++ if (b->memory != V4L2_MEMORY_MMAP)
++ return -EINVAL;
++
++ lw->frame[b->index].state = F_QUEUED;
++
++ spin_lock_irqsave(&lw->queue_lock, lock_flags);
++ list_add_tail(&lw->frame[b->index].frame, &lw->inqueue);
++ spin_unlock_irqrestore(&lw->queue_lock, lock_flags);
++
++ return 0;
++}
++
++static int timblogiw_dqbuf(struct timblogiw *lw, struct file *file,
++ struct v4l2_buffer *b)
++{
++ struct timblogiw_frame *f;
++ unsigned long lock_flags;
++ int ret = 0;
++
++ if (b->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
++ dbg("%s - VIDIOC_DQBUF, illegal buf type!\n",
++ __func__);
++ return -EINVAL;
++ }
++
++ if (list_empty(&lw->outqueue)) {
++ if (file->f_flags & O_NONBLOCK)
++ return -EAGAIN;
++
++ ret = wait_event_interruptible(lw->wait_frame,
++ !list_empty(&lw->outqueue));
++ if (ret)
++ return ret;
++ }
++
++ spin_lock_irqsave(&lw->queue_lock, lock_flags);
++ f = list_entry(lw->outqueue.next,
++ struct timblogiw_frame, frame);
++ list_del(lw->outqueue.next);
++ spin_unlock_irqrestore(&lw->queue_lock, lock_flags);
++
++ f->state = F_UNUSED;
++ memcpy(b, &f->buf, sizeof(*b));
++
++ if (f->vma_use_count)
++ b->flags |= V4L2_BUF_FLAG_MAPPED;
++
++ return 0;
++}
++
++static int timblogiw_enumstd(struct timblogiw *lw, struct v4l2_standard *std)
++{
++ if (std->index != 0)
++ return -EINVAL;
++
++ memset(std, 0, sizeof(*std));
++ std->index = 0;
++
++ std->id = V4L2_STD_PAL;
++ strncpy(std->name, "PAL", sizeof(std->name)-1);
++
++ return 0;
++}
++
++static int timblogiw_g_std(struct timblogiw *lw, v4l2_std_id *std)
++{
++ *std = V4L2_STD_PAL;
++ return 0;
++}
++
++static int timblogiw_s_std(struct timblogiw *lw, v4l2_std_id *std)
++{
++ if (!(*std & V4L2_STD_PAL))
++ return -EINVAL;
++ return 0;
++}
++
++static int timblogiw_enuminput(struct timblogiw *lw, struct v4l2_input *inp)
++{
++ if (inp->index != 0)
++ return -EINVAL;
++
++ memset(inp, 0, sizeof(*inp));
++ inp->index = 0;
++
++ strncpy(inp->name, "Timb input 1", sizeof(inp->name) - 1);
++ inp->type = V4L2_INPUT_TYPE_CAMERA;
++ inp->std = V4L2_STD_ALL;
++
++ return 0;
++}
++
++static int timblogiw_g_input(struct timblogiw *lw, int *input)
++{
++ *input = 0;
++
++ return 0;
++}
++
++static int timblogiw_s_input(struct timblogiw *lw, int *input)
++{
++ if (*input != 0)
++ return -EINVAL;
++ return 0;
++}
++
++static int timblogiw_streamon(struct timblogiw *lw, int *type)
++{
++ struct timblogiw_frame *f;
++
++ if (*type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
++ dbg("%s - No capture device\n", __func__);
++ return -EINVAL;
++ }
++
++ if (list_empty(&lw->inqueue)) {
++ dbg("%s - inqueue is empty\n", __func__);
++ return -EINVAL;
++ }
++
++ if (lw->stream == STREAM_ON)
++ return 0;
++
++ lw->stream = STREAM_ON;
++
++ f = list_entry(lw->inqueue.next,
++ struct timblogiw_frame, frame);
++
++ dbg("%s - f size: %d, bpr: %d, dma addr: %x\n", __func__,
++ lw->frame_size, lw->bytesperline,
++ (unsigned int)lw->dma.transfer[lw->dma.curr].handle);
++ timb_start_dma(DMA_IRQ_VIDEO_RX,
++ lw->dma.transfer[lw->dma.curr].handle,
++ lw->frame_size, lw->bytesperline);
++
++ return 0;
++}
++
++static int timblogiw_streamoff(struct timblogiw *lw, int *type)
++{
++ if (*type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
++ return -EINVAL;
++
++ if (lw->stream == STREAM_ON) {
++ unsigned long lock_flags;
++ spin_lock_irqsave(&lw->queue_lock, lock_flags);
++ timb_stop_dma(DMA_IRQ_VIDEO_RX);
++ lw->stream = STREAM_OFF;
++ spin_unlock_irqrestore(&lw->queue_lock, lock_flags);
++ }
++ timblogiw_empty_framequeues(lw);
++
++ return 0;
++}
++
++static int timblogiw_querystd(struct timblogiw *lw, v4l2_std_id *std)
++{
++ /* TODO: Ask encoder */
++ *std = V4L2_STD_PAL;
++ return 0;
++}
++
++static int timblogiw_enum_framsizes(struct timblogiw *lw,
++ struct v4l2_frmsizeenum *fsize)
++{
++ if ((fsize->index != 0) ||
++ (fsize->pixel_format != V4L2_PIX_FMT_YUYV))
++ return -EINVAL;
++
++ fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
++ fsize->discrete.width = lw->width;
++ fsize->discrete.height = lw->height;
++
++ return 0;
++}
++
++static int timblogiw_g_parm(struct timblogiw *lw, struct v4l2_streamparm *sp)
++{
++ if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
++ return -EINVAL;
++
++ sp->parm.capture.extendedmode = 0;
++ sp->parm.capture.readbuffers = lw->num_frames;
++ return 0;
++}
++
++/*******************************
++ * Device Operations functions *
++ *******************************/
++
++static int timblogiw_open(struct file *file)
++{
++ struct video_device *vdev = video_devdata(file);
++ struct timblogiw *lw = video_get_drvdata(vdev);
++
++ dbg("%s -\n", __func__);
++
++ mutex_init(&lw->fileop_lock);
++ spin_lock_init(&lw->queue_lock);
++ init_waitqueue_head(&lw->wait_frame);
++
++ mutex_lock(&lw->lock);
++
++ lw->width = 720; /* TODO: Should depend on tv norm */
++ lw->height = 576;
++ lw->frame_size = lw->width * lw->height * 2;
++ lw->bytesperline = lw->width * 2;
++
++ file->private_data = lw;
++ lw->stream = STREAM_OFF;
++ lw->num_frames = TIMBLOGIW_NUM_FRAMES;
++
++ timblogiw_empty_framequeues(lw);
++
++ timb_set_dma_interruptcb(DMA_IRQ_VIDEO_RX | DMA_IRQ_VIDEO_DROP,
++ timblogiw_isr, (void *)lw);
++
++ mutex_unlock(&lw->lock);
++
++ return 0;
++}
++
++static int timblogiw_close(struct file *file)
++{
++ struct timblogiw *lw = file->private_data;
++
++ dbg("%s - entry\n", __func__);
++
++ mutex_lock(&lw->lock);
++
++ timb_stop_dma(DMA_IRQ_VIDEO_RX);
++ timb_set_dma_interruptcb(DMA_IRQ_VIDEO_RX | DMA_IRQ_VIDEO_DROP, NULL,
++ NULL);
++ timblogiw_release_buffers(lw);
++
++ mutex_unlock(&lw->lock);
++ return 0;
++}
++
++static ssize_t timblogiw_read(struct file *file, char __user *data,
++ size_t count, loff_t *ppos)
++{
++ dbg("%s - read request\n", __func__);
++ return -EINVAL;
++}
++
++static void timblogiw_vm_open(struct vm_area_struct *vma)
++{
++ struct timblogiw_frame *f = vma->vm_private_data;
++ f->vma_use_count++;
++}
++
++static void timblogiw_vm_close(struct vm_area_struct *vma)
++{
++ struct timblogiw_frame *f = vma->vm_private_data;
++ f->vma_use_count--;
++}
++
++static struct vm_operations_struct timblogiw_vm_ops = {
++ .open = timblogiw_vm_open,
++ .close = timblogiw_vm_close,
++};
++
++static int timblogiw_mmap(struct file *filp, struct vm_area_struct *vma)
++{
++ unsigned long size = vma->vm_end - vma->vm_start, start = vma->vm_start;
++ void *pos;
++ u32 i;
++ int ret = -EINVAL;
++
++ struct timblogiw *lw = filp->private_data;
++ dbg("%s\n", __func__);
++
++ if (mutex_lock_interruptible(&lw->fileop_lock))
++ return -ERESTARTSYS;
++
++ if (!(vma->vm_flags & VM_WRITE) ||
++ size != PAGE_ALIGN(lw->frame[0].buf.length))
++ goto error_unlock;
++
++ for (i = 0; i < lw->num_frames; i++)
++ if ((lw->frame[i].buf.m.offset >> PAGE_SHIFT) == vma->vm_pgoff)
++ break;
++
++ if (i == lw->num_frames) {
++ dbg("%s - user supplied mapping address is out of range\n",
++ __func__);
++ goto error_unlock;
++ }
++
++ vma->vm_flags |= VM_IO;
++ vma->vm_flags |= VM_RESERVED; /* Do not swap out this VMA */
++
++ pos = lw->frame[i].bufmem;
++ while (size > 0) { /* size is page-aligned */
++ if (vm_insert_page(vma, start, vmalloc_to_page(pos))) {
++ dbg("%s - vm_insert_page failed\n", __func__);
++ ret = -EAGAIN;
++ goto error_unlock;
++ }
++ start += PAGE_SIZE;
++ pos += PAGE_SIZE;
++ size -= PAGE_SIZE;
++ }
++
++ vma->vm_ops = &timblogiw_vm_ops;
++ vma->vm_private_data = &lw->frame[i];
++ timblogiw_vm_open(vma);
++ ret = 0;
++
++error_unlock:
++ mutex_unlock(&lw->fileop_lock);
++ return ret;
++}
++
++static long
++timblogiw_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
++{
++ struct timblogiw *lw = file->private_data;
++
++ switch (cmd) {
++
++ case VIDIOC_QUERYCAP:
++ {
++ dbg("%s - VIDIOC_QUERYCAP\n", __func__);
++ return timblogiw_querycap(lw, (struct v4l2_capability *)arg);
++ }
++
++ case VIDIOC_ENUM_FMT:
++ {
++ dbg("%s - VIDIOC_ENUM_FMT\n", __func__);
++ return timblogiw_enum_fmt(lw, (struct v4l2_fmtdesc *)arg);
++ }
++
++ case VIDIOC_G_FMT:
++ {
++ dbg("%s - VIDIOC_G_FMT\n", __func__);
++ return timblogiw_g_fmt(lw, (struct v4l2_format *) arg);
++ }
++
++ case VIDIOC_TRY_FMT:
++ case VIDIOC_S_FMT:
++ {
++ dbg("%s - VIDIOC_S_FMT\n", __func__);
++ return timblogiw_s_fmt(lw, (struct v4l2_format *)arg);
++ }
++
++ case VIDIOC_REQBUFS:
++ {
++ dbg("%s - VIDIOC_REQBUFS\n", __func__);
++ return timblogiw_reqbufs(lw, (struct v4l2_requestbuffers *)arg);
++ }
++
++ case VIDIOC_QUERYBUF:
++ {
++ dbg("%s - VIDIOC_QUERYBUF\n", __func__);
++ return timblogiw_querybuf(lw, (struct v4l2_buffer *)arg);
++ }
++
++ case VIDIOC_QBUF:
++ {
++ return timblogiw_qbuf(lw, (struct v4l2_buffer *)arg);
++ }
++
++ case VIDIOC_DQBUF:
++ {
++ return timblogiw_dqbuf(lw, file, (struct v4l2_buffer *)arg);
++ }
++
++ case VIDIOC_ENUMSTD:
++ {
++ dbg("%s - VIDIOC_ENUMSTD\n", __func__);
++ return timblogiw_enumstd(lw, (struct v4l2_standard *)arg);
++ }
++
++ case VIDIOC_G_STD:
++ {
++ dbg("%s - VIDIOC_G_STD\n", __func__);
++ return timblogiw_g_std(lw, (v4l2_std_id *)arg);
++ }
++
++ case VIDIOC_S_STD:
++ {
++ dbg("%s - VIDIOC_S_STD\n", __func__);
++ return timblogiw_s_std(lw, (v4l2_std_id *)arg);
++ }
++
++ case VIDIOC_ENUMINPUT:
++ {
++ dbg("%s - VIDIOC_ENUMINPUT\n", __func__);
++ return timblogiw_enuminput(lw, (struct v4l2_input *)arg);
++ }
++
++ case VIDIOC_G_INPUT:
++ {
++ dbg("%s - VIDIOC_G_INPUT\n", __func__);
++ return timblogiw_g_input(lw, (int *)arg);
++ }
++
++ case VIDIOC_S_INPUT:
++ {
++ dbg("%s - VIDIOC_S_INPUT\n", __func__);
++ return timblogiw_s_input(lw, (int *)arg);
++ }
++
++ case VIDIOC_STREAMON:
++ {
++ dbg("%s - VIDIOC_STREAMON\n", __func__);
++ return timblogiw_streamon(lw, (int *)arg);
++ }
++
++ case VIDIOC_STREAMOFF:
++ {
++ dbg("%s - VIDIOC_STREAMOFF\n", __func__);
++ return timblogiw_streamoff(lw, (int *)arg);
++ }
++
++ case VIDIOC_QUERYSTD:
++ {
++ dbg("%s - VIDIOC_QUERYSTD\n", __func__);
++ return timblogiw_querystd(lw, (v4l2_std_id *)arg);
++ }
++
++ case VIDIOC_ENUM_FRAMESIZES:
++ {
++ dbg("%s - VIDIOC_ENUM_FRAMESIZES\n", __func__);
++ return timblogiw_enum_framsizes(lw,
++ (struct v4l2_frmsizeenum *)arg);
++ }
++
++ case VIDIOC_G_PARM:
++ {
++ dbg("%s - VIDIOC_G_PARM\n", __func__);
++ return timblogiw_g_parm(lw, (struct v4l2_streamparm *)arg);
++ }
++
++ default:
++ {
++ dbg("%s Unknown command, dir: %x, type: %x, nr: %x, size: %x\n",
++ __func__,
++ _IOC_DIR(cmd),
++ _IOC_TYPE(cmd),
++ _IOC_NR(cmd),
++ _IOC_SIZE(cmd));
++ break;
++ }
++ }
++
++ return -EINVAL;
++}
++
++void timblogiw_vdev_release(struct video_device *vdev)
++{
++ kfree(vdev);
++}
++
++static const struct v4l2_file_operations timblogiw_fops = {
++ .owner = THIS_MODULE,
++ .open = timblogiw_open,
++ .release = timblogiw_close,
++ .ioctl = timblogiw_ioctl,
++ .mmap = timblogiw_mmap,
++ .read = timblogiw_read,
++};
++
++static const struct video_device timblogiw_template = {
++ .name = TIMBLOGIWIN_NAME,
++ .fops = &timblogiw_fops,
++ .release = &timblogiw_vdev_release,
++ .minor = -1
++};
++
++static int timblogiw_probe(struct platform_device *dev)
++{
++ int err;
++ struct timblogiw *lw;
++ struct resource *iomem;
++
++ iomem = platform_get_resource(dev, IORESOURCE_MEM, 0);
++ if (!iomem) {
++ err = -EINVAL;
++ goto err_mem;
++ }
++
++ lw = kzalloc(sizeof(*lw), GFP_KERNEL);
++ if (!lw) {
++ err = -EINVAL;
++ goto err_mem;
++ }
++
++ /* find the PCI device from the parent... */
++ if (!dev->dev.parent) {
++ printk(KERN_ERR "timblogwi: No parent device found??\n");
++ err = -ENODEV;
++ goto err_mem;
++ }
++
++ lw->dev = container_of(dev->dev.parent, struct pci_dev, dev);
++
++ mutex_init(&lw->lock);
++
++ lw->video_dev = video_device_alloc();
++ if (!lw->video_dev) {
++ err = -ENOMEM;
++ goto err_video_req;
++ }
++ *lw->video_dev = timblogiw_template;
++
++ err = video_register_device(lw->video_dev, VFL_TYPE_GRABBER, 0);
++ if (err) {
++ video_device_release(lw->video_dev);
++ printk(KERN_ALERT "Error reg video\n");
++ goto err_video_req;
++ }
++
++ tasklet_init(&lw->tasklet, timblogiw_handleframe, (unsigned long)lw);
++
++ if (!request_mem_region(iomem->start, resource_size(iomem),
++ "timb-video")) {
++ err = -EBUSY;
++ goto err_request;
++ }
++
++ lw->membase = ioremap(iomem->start, resource_size(iomem));
++ if (!lw->membase) {
++ err = -ENOMEM;
++ goto err_ioremap;
++ }
++
++ platform_set_drvdata(dev, lw);
++ video_set_drvdata(lw->video_dev, lw);
++
++ return 0;
++
++err_ioremap:
++ release_mem_region(iomem->start, resource_size(iomem));
++err_request:
++ if (-1 != lw->video_dev->minor)
++ video_unregister_device(lw->video_dev);
++ else
++ video_device_release(lw->video_dev);
++err_video_req:
++ kfree(lw);
++err_mem:
++ printk(KERN_ERR
++ "timberdale: Failed to register Timberdale Video In: %d\n",
++ err);
++
++ return err;
++}
++
++static int timblogiw_remove(struct platform_device *dev)
++{
++ struct timblogiw *lw = platform_get_drvdata(dev);
++ struct resource *iomem = platform_get_resource(dev, IORESOURCE_MEM, 0);
++
++ if (-1 != lw->video_dev->minor)
++ video_unregister_device(lw->video_dev);
++ else
++ video_device_release(lw->video_dev);
++
++ tasklet_kill(&lw->tasklet);
++ iounmap(lw->membase);
++ release_mem_region(iomem->start, resource_size(iomem));
++ kfree(lw);
++
++ return 0;
++}
++
++static struct platform_driver timblogiw_platform_driver = {
++ .driver = {
++ .name = "timb-video",
++ .owner = THIS_MODULE,
++ },
++ .probe = timblogiw_probe,
++ .remove = timblogiw_remove,
++};
++
++/*--------------------------------------------------------------------------*/
++
++static int __init timblogiw_init(void)
++{
++ return platform_driver_register(&timblogiw_platform_driver);
++}
++
++static void __exit timblogiw_exit(void)
++{
++ platform_driver_unregister(&timblogiw_platform_driver);
++}
++
++module_init(timblogiw_init);
++module_exit(timblogiw_exit);
++
++MODULE_DESCRIPTION("Timberdale Video In driver");
++MODULE_LICENSE("GPL v2");
++MODULE_ALIAS("platform:timb-video");
++
+diff -uNr linux-2.6.29-clean/drivers/media/video/timblogiw.h linux-2.6.29/drivers/media/video/timblogiw.h
+--- linux-2.6.29-clean/drivers/media/video/timblogiw.h 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/media/video/timblogiw.h 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,95 @@
++/*
++ * timblogiw.h timberdale FPGA LogiWin Video In driver defines
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Timberdale FPGA LogiWin Video In
++ */
++
++#ifndef _TIMBLOGIW_H
++#define _TIMBLOGIW_H
++
++#include <linux/interrupt.h>
++
++#define TIMBLOGIWIN_NAME "Timberdale Video-In"
++
++#define TIMBLOGIW_NUM_FRAMES 10
++
++
++enum timblogiw_stream_state {
++ STREAM_OFF,
++ STREAM_ON,
++};
++
++enum timblogiw_frame_state {
++ F_UNUSED = 0,
++ F_QUEUED,
++ F_GRABBING,
++ F_DONE,
++ F_ERROR,
++};
++
++struct timblogiw_frame {
++ void *bufmem;
++ struct v4l2_buffer buf;
++ enum timblogiw_frame_state state;
++ struct list_head frame;
++ unsigned long vma_use_count;
++};
++
++struct timblogiw_tvnorm {
++ int v4l2_id;
++ char *name;
++ u16 swidth;
++ u16 sheight;
++};
++
++
++struct timbdma_transfer {
++ dma_addr_t handle;
++ void *buf;
++};
++
++struct timbdma_control {
++ struct timbdma_transfer transfer[2];
++ struct timbdma_transfer *filled;
++ int curr;
++};
++
++struct timblogiw {
++ struct i2c_client *decoder;
++ struct timblogiw_frame frame[TIMBLOGIW_NUM_FRAMES];
++ int num_frames;
++ unsigned int frame_count;
++ struct list_head inqueue, outqueue;
++ spinlock_t queue_lock; /* mutual exclusion */
++ enum timblogiw_stream_state stream;
++ struct video_device *video_dev;
++ struct mutex lock, fileop_lock;
++ wait_queue_head_t wait_frame;
++ int width;
++ int height;
++ u32 frame_size;
++ int bytesperline;
++ struct pci_dev *dev;
++ struct timbdma_control dma;
++ void __iomem *membase;
++ struct tasklet_struct tasklet;
++};
++
++#endif /* _TIMBLOGIW_H */
++
+diff -uNr linux-2.6.29-clean/drivers/mfd/Kconfig linux-2.6.29/drivers/mfd/Kconfig
+--- linux-2.6.29-clean/drivers/mfd/Kconfig 2009-04-01 09:20:24.000000000 -0700
++++ linux-2.6.29/drivers/mfd/Kconfig 2009-04-06 13:51:47.000000000 -0700
+@@ -240,6 +240,27 @@
+ Say yes here if you want to include support GPIO for pins on
+ the PCF50633 chip.
+
++config MFD_TIMBERDALE
++ bool "Support for Timberdale"
++ select MFD_CORE
++ ---help---
++ This is the core driver for the timberdale FPGA. This device is a
++ multifunctioanl device which may provide numerous interfaces.
++
++config MFD_TIMBERDALE_DMA
++ tristate "Support for timberdale DMA"
++ depends on MFD_TIMBERDALE
++ ---help---
++ Add support the DMA block inside the timberdale FPGA. This to be able
++ to do DMA transfers directly to some of the blocks inside the FPGA
++
++config MFD_TIMBERDALE_I2S
++ tristate "Support for timberdale I2S bus"
++ depends on MFD_TIMBERDALE
++ ---help---
++ Add support for the I2S bus handled by timberdale FPGA.
++ I2S RX and TX instances are then available for other devices to make use of.
++
+ endmenu
+
+ menu "Multimedia Capabilities Port drivers"
+diff -uNr linux-2.6.29-clean/drivers/mfd/Makefile linux-2.6.29/drivers/mfd/Makefile
+--- linux-2.6.29-clean/drivers/mfd/Makefile 2009-04-01 09:20:24.000000000 -0700
++++ linux-2.6.29/drivers/mfd/Makefile 2009-04-06 13:51:47.000000000 -0700
+@@ -40,4 +40,8 @@
+
+ obj-$(CONFIG_MFD_PCF50633) += pcf50633-core.o
+ obj-$(CONFIG_PCF50633_ADC) += pcf50633-adc.o
+-obj-$(CONFIG_PCF50633_GPIO) += pcf50633-gpio.o
+\ No newline at end of file
++obj-$(CONFIG_PCF50633_GPIO) += pcf50633-gpio.o
++
++obj-$(CONFIG_MFD_TIMBERDALE) += timberdale.o
++obj-$(CONFIG_MFD_TIMBERDALE_DMA) += timbdma.o
++obj-$(CONFIG_MFD_TIMBERDALE_I2S) += timbi2s.o
+diff -uNr linux-2.6.29-clean/drivers/mfd/timbdma.c linux-2.6.29/drivers/mfd/timbdma.c
+--- linux-2.6.29-clean/drivers/mfd/timbdma.c 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/mfd/timbdma.c 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,301 @@
++/*
++ * timbdma.c timberdale FPGA DMA driver
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Timberdale FPGA DMA engine
++ */
++
++#include <linux/version.h>
++#include <linux/module.h>
++#include <linux/pci.h>
++#include <linux/interrupt.h>
++#include <linux/platform_device.h>
++
++#include <linux/mfd/timbdma.h>
++
++static struct timbdma_dev *self_g;
++
++static irqreturn_t timbdma_handleinterrupt(int irq, void *devid)
++{
++ struct timbdma_dev *dev = (struct timbdma_dev *)devid;
++ int ipr;
++ int i;
++
++ ipr = ioread32(dev->membase + timbdma_ctrlmap_TIMBPEND);
++
++ /* ack */
++ iowrite32(ipr, dev->membase + timbdma_ctrlmap_TIMBSTATUS);
++
++ /* call the callbacks */
++ for (i = 0; i < DMA_IRQS; i++) {
++ int mask = 1 << i;
++ if ((ipr & mask) && dev->callbacks[i])
++ dev->callbacks[i](mask, dev->callback_data[i]);
++ }
++
++ if (ipr)
++ return IRQ_HANDLED;
++ else
++ return IRQ_NONE;
++}
++
++
++void timb_start_dma(u32 flag, unsigned long buf, int len, int bytes_per_row)
++{
++ int i;
++ unsigned long irqflags;
++ struct timbdma_dev *dev = self_g;
++
++ spin_lock_irqsave(&dev->lock, irqflags);
++
++ /* now enable the DMA transfer */
++ for (i = 0; i < DMA_IRQS; i++)
++ if (flag & (1 << i)) {
++ u32 offset = i / 2 * 0x40;
++
++ if (!(i % 2)) {
++ /* RX */
++ /* bytes per row */
++ iowrite32(bytes_per_row, dev->membase + offset +
++ timbdma_dmacfg_BPERROW);
++ /* address high */
++ iowrite32(0, dev->membase + offset +
++ timbdma_dmacfg_RXSTARTH);
++ /* address low */
++ iowrite32(buf, dev->membase + offset +
++ timbdma_dmacfg_RXSTARTL);
++ /* Length */
++ iowrite32(len, dev->membase + offset +
++ timbdma_dmacfg_RXLENGTH);
++ /* Clear rx sw read pointer */
++ iowrite32(0, dev->membase + offset +
++ timbdma_dmacfg_RXSWRP);
++ /* enable the transfer */
++ iowrite32(1, dev->membase + offset +
++ timbdma_dmacfg_RXENABLE);
++ } else {
++ /* TX */
++ /* address high */
++ iowrite32(0, dev->membase + offset +
++ timbdma_dmacfg_TXSTARTH);
++ /* address low */
++ iowrite32(buf, dev->membase + offset +
++ timbdma_dmacfg_TXSTARTL);
++ /* Length */
++ iowrite32(len, dev->membase + offset +
++ timbdma_dmacfg_TXLENGTH);
++ /* Set tx sw write pointer */
++ iowrite32(len, dev->membase + offset +
++ timbdma_dmacfg_TXSWWP);
++ }
++
++ /* only allow one bit in the flag field */
++ break;
++ }
++ spin_unlock_irqrestore(&dev->lock, irqflags);
++}
++EXPORT_SYMBOL(timb_start_dma);
++
++void *timb_stop_dma(u32 flags)
++{
++ int i;
++ unsigned long irqflags;
++ struct timbdma_dev *dev = self_g;
++ void *result = 0;
++
++ spin_lock_irqsave(&dev->lock, irqflags);
++
++ /* now disable the DMA transfers */
++ for (i = 0; i < DMA_IRQS; i++)
++ if (flags & (1 << i)) {
++ /*
++ RX enable registers are located at:
++ 0x14
++ 0x54
++ 0x94
++
++ TX SW pointer registers are located at:
++ 0x24
++ 0x64
++ */
++ u32 offset = i / 2 * 0x40;
++ u32 result_offset = offset;
++ if (!(i % 2)) {
++ /* even -> RX enable */
++ offset += timbdma_dmacfg_RXENABLE;
++ result_offset += timbdma_dmacfg_RXFPGAWP;
++ } else {
++ /* odd -> TX SW pointer reg */
++ offset += timbdma_dmacfg_TXSWWP;
++ result_offset = timbdma_dmacfg_TXFPGARP;
++ }
++
++ iowrite32(0, dev->membase + offset);
++ /* check how far the FPGA has written/read */
++ result = (void *)ioread32(dev->membase + result_offset);
++ }
++
++ /* ack any pending IRQs */
++ iowrite32(flags, dev->membase + timbdma_ctrlmap_TIMBSTATUS);
++
++ spin_unlock_irqrestore(&dev->lock, irqflags);
++
++ return result;
++}
++EXPORT_SYMBOL(timb_stop_dma);
++
++void timb_set_dma_interruptcb(u32 flags, timbdma_interruptcb icb, void *data)
++{
++ int i;
++ unsigned long irqflags;
++ struct timbdma_dev *dev = self_g;
++ u32 ier;
++
++ spin_lock_irqsave(&dev->lock, irqflags);
++
++ for (i = 0; i < DMA_IRQS; i++)
++ if (flags & (1 << i)) {
++ dev->callbacks[i] = icb;
++ dev->callback_data[i] = data;
++ }
++
++ /* Ack any pending IRQ */
++ iowrite32(flags, dev->membase + timbdma_ctrlmap_TIMBSTATUS);
++
++ /* if a null callback is given -> clear interrupt, else -> enable */
++ ier = ioread32(dev->membase + timbdma_ctrlmap_TIMBENABLE);
++ if (icb != NULL)
++ ier |= flags;
++ else
++ ier &= ~flags;
++ iowrite32(ier, dev->membase + timbdma_ctrlmap_TIMBENABLE);
++
++ spin_unlock_irqrestore(&dev->lock, irqflags);
++}
++EXPORT_SYMBOL(timb_set_dma_interruptcb);
++
++static int timbdma_probe(struct platform_device *dev)
++{
++ int err, irq;
++ struct timbdma_dev *self;
++ struct resource *iomem;
++
++ iomem = platform_get_resource(dev, IORESOURCE_MEM, 0);
++ if (!iomem) {
++ err = -EINVAL;
++ goto err_mem;
++ }
++
++ self = kzalloc(sizeof(*self), GFP_KERNEL);
++ if (!self) {
++ err = -EINVAL;
++ goto err_mem;
++ }
++
++ spin_lock_init(&self->lock);
++
++ if (!request_mem_region(iomem->start,
++ resource_size(iomem), "timb-dma")) {
++ err = -EBUSY;
++ goto err_request;
++ }
++
++ self->membase = ioremap(iomem->start, resource_size(iomem));
++ if (!self->membase) {
++ printk(KERN_ERR "timbdma: Failed to remap I/O memory\n");
++ err = -ENOMEM;
++ goto err_ioremap;
++ }
++
++ /* register interrupt */
++ irq = platform_get_irq(dev, 0);
++ if (irq < 0) {
++ err = irq;
++ goto err_get_irq;
++ }
++
++ /* request IRQ */
++ err = request_irq(irq, timbdma_handleinterrupt, IRQF_SHARED,
++ "timb-dma", self);
++ if (err) {
++ printk(KERN_ERR "timbdma: Failed to request IRQ\n");
++ goto err_get_irq;
++ }
++
++ platform_set_drvdata(dev, self);
++
++ /* assign the global pointer */
++ self_g = self;
++
++ return 0;
++
++err_get_irq:
++ iounmap(self->membase);
++err_ioremap:
++ release_mem_region(iomem->start, resource_size(iomem));
++err_request:
++ kfree(self);
++err_mem:
++ printk(KERN_ERR "timberdale: Failed to register Timberdale DMA: %d\n",
++ err);
++
++ return err;
++}
++
++static int timbdma_remove(struct platform_device *dev)
++{
++ struct timbdma_dev *self = platform_get_drvdata(dev);
++ struct resource *iomem = platform_get_resource(dev, IORESOURCE_MEM, 0);
++
++ free_irq(platform_get_irq(dev, 0), self);
++ iounmap(self->membase);
++ release_mem_region(iomem->start, resource_size(iomem));
++ kfree(self);
++ self_g = NULL;
++ return 0;
++}
++
++static struct platform_driver timbdma_platform_driver = {
++ .driver = {
++ .name = "timb-dma",
++ .owner = THIS_MODULE,
++ },
++ .probe = timbdma_probe,
++ .remove = timbdma_remove,
++};
++
++/*--------------------------------------------------------------------------*/
++
++static int __init timbdma_init(void)
++{
++ self_g = NULL;
++ return platform_driver_register(&timbdma_platform_driver);
++}
++
++static void __exit timbdma_exit(void)
++{
++ platform_driver_unregister(&timbdma_platform_driver);
++}
++
++module_init(timbdma_init);
++module_exit(timbdma_exit);
++
++MODULE_DESCRIPTION("Timberdale DMA driver");
++MODULE_LICENSE("GPL v2");
++MODULE_ALIAS("platform:timb-dma");
++
+diff -uNr linux-2.6.29-clean/drivers/mfd/timberdale.c linux-2.6.29/drivers/mfd/timberdale.c
+--- linux-2.6.29-clean/drivers/mfd/timberdale.c 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/mfd/timberdale.c 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,599 @@
++/*
++ * timberdale.c timberdale FPGA mfd shim driver
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Timberdale FPGA
++ */
++
++#include <linux/kernel.h>
++#include <linux/module.h>
++#include <linux/pci.h>
++#include <linux/msi.h>
++#include <linux/init.h>
++#include <linux/interrupt.h>
++#include <linux/platform_device.h>
++#include <linux/mfd/core.h>
++#include <linux/irq.h>
++
++#include <linux/i2c.h>
++#include <linux/i2c-ocores.h>
++#include <linux/i2c/tsc2007.h>
++#include <linux/spi/xilinx_spi.h>
++#include "timberdale.h"
++
++struct timberdale_device {
++ resource_size_t intc_mapbase;
++ resource_size_t ctl_mapbase;
++ unsigned char __iomem *intc_membase;
++ unsigned char __iomem *ctl_membase;
++ int irq_base;
++ u32 irq_ack_mask;
++ /* locking from interrupts while modifiying registers */
++ spinlock_t lock;
++};
++
++/*--------------------------------------------------------------------------*/
++
++struct tsc2007_platform_data timberdale_tsc2007_platform_data = {
++ .model = 2003,
++ .x_plate_ohms = 100
++};
++
++struct i2c_board_info timberdale_i2c_board_info[] = {
++ {
++ I2C_BOARD_INFO("tsc2003", 0x48),
++ .platform_data = &timberdale_tsc2007_platform_data,
++ .irq = IRQ_TIMBERDALE_TSC_INT
++ },
++ {
++ I2C_BOARD_INFO("adv7180", 0x42 >> 1),
++ .irq = IRQ_TIMBERDALE_ADV7180
++ }
++};
++
++static __devinitdata struct ocores_i2c_platform_data
++timberdale_i2c_platform_data = {
++ .regstep = 4,
++ .clock_khz = 62500,
++ .devices = timberdale_i2c_board_info,
++ .num_devices = ARRAY_SIZE(timberdale_i2c_board_info)
++};
++
++const static __devinitconst struct resource timberdale_i2c_resources[] = {
++ {
++ .start = I2COFFSET,
++ .end = I2CEND,
++ .flags = IORESOURCE_MEM,
++ },
++ {
++ .start = IRQ_TIMBERDALE_I2C,
++ .end = IRQ_TIMBERDALE_I2C,
++ .flags = IORESOURCE_IRQ,
++ },
++};
++
++static __devinitdata struct xspi_platform_data timberdale_xspi_platorm_data = {
++ .bus_num = -1,
++ /* according to spec. we can have up to 32 slaves however,
++ * as of current(2009-03-06) revision of
++ * Timberdale we can only handle 3 right now
++ */
++ .num_chipselect = 3,
++ .speed_hz = 1953125, /* hardcoded value in IP, for now */
++ .cr_offset = 0x60,
++ .sr_offset = 0x64,
++ .txd_offset = 0x68,
++ .rxd_offset = 0x6c,
++ .ssr_offset = 0x70
++};
++
++const static __devinitconst struct resource timberdale_spi_resources[] = {
++ {
++ .start = SPIOFFSET,
++ .end = SPIEND,
++ .flags = IORESOURCE_MEM,
++ },
++ {
++ .start = IRQ_TIMBERDALE_SPI,
++ .end = IRQ_TIMBERDALE_SPI,
++ .flags = IORESOURCE_IRQ,
++ },
++};
++
++const static __devinitconst struct resource timberdale_eth_resources[] = {
++ {
++ .start = ETHOFFSET,
++ .end = ETHEND,
++ .flags = IORESOURCE_MEM,
++ },
++ {
++ .start = IRQ_TIMBERDALE_ETHSW_IF,
++ .end = IRQ_TIMBERDALE_ETHSW_IF,
++ .flags = IORESOURCE_IRQ,
++ },
++};
++
++const static __devinitconst struct resource timberdale_gpio_resources[] = {
++ {
++ .start = GPIOOFFSET,
++ .end = GPIOEND,
++ .flags = IORESOURCE_MEM,
++ },
++ {
++ .start = IRQ_TIMBERDALE_GPIO,
++ .end = IRQ_TIMBERDALE_GPIO,
++ .flags = IORESOURCE_IRQ,
++ },
++};
++
++
++const static __devinitconst struct resource timberdale_most_resources[] = {
++ {
++ .start = MOSTOFFSET,
++ .end = MOSTEND,
++ .flags = IORESOURCE_MEM,
++ },
++ {
++ .start = IRQ_TIMBERDALE_MLB,
++ .end = IRQ_TIMBERDALE_MLB,
++ .flags = IORESOURCE_IRQ,
++ },
++};
++
++const static __devinitconst struct resource timberdale_uart_resources[] = {
++ {
++ .start = UARTOFFSET,
++ .end = UARTEND,
++ .flags = IORESOURCE_MEM,
++ },
++ {
++ .start = IRQ_TIMBERDALE_UART,
++ .end = IRQ_TIMBERDALE_UART,
++ .flags = IORESOURCE_IRQ,
++ },
++};
++
++const static __devinitconst struct resource timberdale_i2s_resources[] = {
++ {
++ .start = I2SOFFSET,
++ .end = I2SEND,
++ .flags = IORESOURCE_MEM,
++ },
++ {
++ .start = IRQ_TIMBERDALE_I2S,
++ .end = IRQ_TIMBERDALE_I2S,
++ .flags = IORESOURCE_IRQ,
++ },
++};
++
++const static __devinitconst struct resource timberdale_video_resources[] = {
++ {
++ .start = LOGIWOFFSET,
++ .end = LOGIWEND,
++ .flags = IORESOURCE_MEM,
++ },
++ /*
++ note that the "frame buffer" is located in DMA area
++ starting at 0x1200000
++ */
++};
++
++const static __devinitconst struct resource timberdale_dma_resources[] = {
++ {
++ .start = DMAOFFSET,
++ .end = DMAEND,
++ .flags = IORESOURCE_MEM,
++ },
++ {
++ .start = IRQ_TIMBERDALE_DMA,
++ .end = IRQ_TIMBERDALE_DMA,
++ .flags = IORESOURCE_IRQ,
++ },
++};
++
++static __devinitdata struct mfd_cell timberdale_cells_bar0[] = {
++ {
++ .name = "timb-uart",
++ .num_resources = ARRAY_SIZE(timberdale_uart_resources),
++ .resources = timberdale_uart_resources,
++ },
++ {
++ .name = "ocores-i2c",
++ .num_resources = ARRAY_SIZE(timberdale_i2c_resources),
++ .resources = timberdale_i2c_resources,
++ .platform_data = &timberdale_i2c_platform_data,
++ .data_size = sizeof(timberdale_i2c_platform_data),
++ },
++ {
++ .name = "timb-gpio",
++ .num_resources = ARRAY_SIZE(timberdale_gpio_resources),
++ .resources = timberdale_gpio_resources,
++ },
++ {
++ .name = "timb-i2s",
++ .num_resources = ARRAY_SIZE(timberdale_i2s_resources),
++ .resources = timberdale_i2s_resources,
++ },
++ {
++ .name = "timb-most",
++ .num_resources = ARRAY_SIZE(timberdale_most_resources),
++ .resources = timberdale_most_resources,
++ },
++ {
++ .name = "timb-video",
++ .num_resources = ARRAY_SIZE(timberdale_video_resources),
++ .resources = timberdale_video_resources,
++ },
++ {
++ .name = "xilinx_spi",
++ .num_resources = ARRAY_SIZE(timberdale_spi_resources),
++ .resources = timberdale_spi_resources,
++ .platform_data = &timberdale_xspi_platorm_data,
++ .data_size = sizeof(timberdale_xspi_platorm_data),
++ },
++ {
++ .name = "ks884x",
++ .num_resources = ARRAY_SIZE(timberdale_eth_resources),
++ .resources = timberdale_eth_resources,
++ },
++ {
++ .name = "timb-dma",
++ .num_resources = ARRAY_SIZE(timberdale_dma_resources),
++ .resources = timberdale_dma_resources,
++ },
++};
++
++static const __devinitconst struct resource timberdale_sdhc_resources_bar1[] = {
++ {
++ .start = SDHC0OFFSET,
++ .end = SDHC0END,
++ .flags = IORESOURCE_MEM,
++ },
++ {
++ .start = IRQ_TIMBERDALE_SDHC,
++ .end = IRQ_TIMBERDALE_SDHC,
++ .flags = IORESOURCE_IRQ,
++ },
++};
++
++static __devinitdata struct mfd_cell timberdale_cells_bar1[] = {
++ {
++ .name = "sdhci",
++ .num_resources = ARRAY_SIZE(timberdale_sdhc_resources_bar1),
++ .resources = timberdale_sdhc_resources_bar1,
++ },
++};
++
++/*--------------------------------------------------------------------------*/
++
++
++/* Handle the timberdale interrupt mux */
++static void timberdale_irq(unsigned int irq, struct irq_desc *desc)
++{
++ struct timberdale_device *priv = get_irq_data(irq);
++ unsigned int i, ipr;
++
++ desc->chip->ack(irq);
++
++ while ((ipr = ioread32(priv->intc_membase + IPR))) {
++ priv->irq_ack_mask = 0;
++ for (i = 0; i < TIMBERDALE_NR_IRQS; i++)
++ if (ipr & (1 << i))
++ generic_handle_irq(priv->irq_base + i);
++ if (priv->irq_ack_mask)
++ iowrite32(priv->irq_ack_mask, priv->intc_membase + IAR);
++ }
++}
++
++static void timberdale_irq_mask(unsigned int irq)
++{
++ struct timberdale_device *priv = get_irq_chip_data(irq);
++ unsigned long flags;
++
++ spin_lock_irqsave(&priv->lock, flags);
++ iowrite32(1 << (irq - priv->irq_base), priv->intc_membase + CIE);
++ spin_unlock_irqrestore(&priv->lock, flags);
++}
++
++static void timberdale_irq_unmask(unsigned int irq)
++{
++ struct timberdale_device *priv = get_irq_chip_data(irq);
++ unsigned long flags;
++
++ spin_lock_irqsave(&priv->lock, flags);
++ iowrite32(1 << (irq - priv->irq_base), priv->intc_membase + SIE);
++ spin_unlock_irqrestore(&priv->lock, flags);
++}
++
++static void timberdale_irq_ack(unsigned int irq)
++{
++ struct timberdale_device *priv = get_irq_chip_data(irq);
++ unsigned long flags;
++ u32 ack_mask = 1 << (irq - priv->irq_base);
++
++ spin_lock_irqsave(&priv->lock, flags);
++ /* if edge triggered, ack directly. Otherwhise ack in the end of
++ * irq handler
++ */
++ if (ack_mask & IRQ_TIMBERDALE_EDGE_MASK)
++ iowrite32(ack_mask, priv->intc_membase + IAR);
++ else
++ priv->irq_ack_mask |= ack_mask;
++ spin_unlock_irqrestore(&priv->lock, flags);
++}
++
++static struct irq_chip timberdale_chip = {
++ .name = "timberdale",
++ .ack = timberdale_irq_ack,
++ .mask = timberdale_irq_mask,
++ .unmask = timberdale_irq_unmask,
++ .disable = timberdale_irq_mask,
++ .enable = timberdale_irq_unmask,
++};
++
++/*--------------------------------------------------------------------------*/
++
++/* Install the IRQ handler */
++static void timberdale_attach_irq(struct pci_dev *dev)
++{
++ struct timberdale_device *priv = pci_get_drvdata(dev);
++ unsigned int irq, irq_base;
++
++ irq_base = priv->irq_base;
++ for (irq = irq_base; irq < irq_base + TIMBERDALE_NR_IRQS; irq++) {
++ set_irq_chip_and_handler_name(irq, &timberdale_chip,
++ handle_edge_irq, "mux");
++
++ set_irq_chip_data(irq, priv);
++
++#ifdef CONFIG_ARM
++ set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
++#endif
++ }
++
++ set_irq_data(dev->irq, priv);
++ set_irq_chained_handler(dev->irq, timberdale_irq);
++}
++
++static void timberdale_detach_irq(struct pci_dev *dev)
++{
++ struct timberdale_device *priv = pci_get_drvdata(dev);
++ unsigned int irq, irq_base;
++
++ irq_base = priv->irq_base;
++
++ set_irq_chained_handler(dev->irq, NULL);
++ set_irq_data(dev->irq, NULL);
++
++ for (irq = irq_base; irq < irq_base + TIMBERDALE_NR_IRQS; irq++) {
++#ifdef CONFIG_ARM
++ set_irq_flags(irq, 0);
++#endif
++ set_irq_chip(irq, NULL);
++ set_irq_chip_data(irq, NULL);
++ }
++}
++
++static int __devinit timb_probe(struct pci_dev *dev,
++ const struct pci_device_id *id)
++{
++ struct timberdale_device *priv;
++ int err, i;
++ u16 ver;
++ resource_size_t mapbase;
++
++ priv = kzalloc(sizeof(*priv), GFP_KERNEL);
++ if (!priv)
++ return -ENOMEM;
++
++ spin_lock_init(&priv->lock);
++ pci_set_drvdata(dev, priv);
++
++ err = pci_enable_device(dev);
++ if (err)
++ goto err_enable;
++
++ mapbase = pci_resource_start(dev, 0);
++ if (!mapbase) {
++ printk(KERN_ERR "timberdale: No resource\n");
++ goto err_start;
++ }
++
++ /* create a resource for the Interrupt controller registers */
++ priv->intc_mapbase = mapbase + INTCOFFSET;
++ if (!request_mem_region(priv->intc_mapbase, INTCSIZE, "timb-intc")) {
++ printk(KERN_ERR "timberdale: Failed to request intc mem\n");
++ goto err_request;
++ }
++
++ /* create a resource for the PCI master register */
++ priv->ctl_mapbase = mapbase + CHIPCTLOFFSET;
++ if (!request_mem_region(priv->ctl_mapbase, CHIPCTLSIZE, "timb-intc")) {
++ printk(KERN_ERR "timberdale: Failed to request ctl mem\n");
++ goto err_request_ctl;
++ }
++
++ priv->intc_membase = ioremap(priv->intc_mapbase, INTCSIZE);
++ if (!priv->intc_membase) {
++ printk(KERN_ALERT "timberdale: Map error, intc\n");
++ goto err_ioremap;
++ }
++
++ priv->ctl_membase = ioremap(priv->ctl_mapbase, CHIPCTLSIZE);
++ if (!priv->ctl_membase) {
++ printk(KERN_ALERT "timberdale: Map error, ctl\n");
++ goto err_ioremap_ctl;
++ }
++
++ err = pci_enable_msi(dev);
++ if (err) {
++ printk(KERN_WARNING "timberdale: MSI init failed: %d\n", err);
++ goto err_msi;
++ }
++
++ /* Reset all FPGA PLB peripherals */
++ iowrite32(0x1, priv->ctl_membase + MAYSVILLERST);
++
++ /* at this stage the FPGA does not generate a
++ * unique interrupt per function, to emulate real interrupts
++ * we assign them a faked interrupt which we issue in the
++ * interrupt handler. For now just hard code a base number
++ */
++ priv->irq_base = NR_IRQS - TIMBERDALE_NR_IRQS - 1;
++ if (priv->irq_base < dev->irq)
++ /* ops the device itself got the IRQ in the end... */
++ priv->irq_base = 400;
++
++ timberdale_attach_irq(dev);
++
++ /* update IRQ offsets in I2C board info */
++ for (i = 0; i < ARRAY_SIZE(timberdale_i2c_board_info); i++)
++ timberdale_i2c_board_info[i].irq += priv->irq_base;
++
++ /* don't leave platform_data empty on any device */
++ for (i = 0; i < ARRAY_SIZE(timberdale_cells_bar0); i++)
++ if (timberdale_cells_bar0[i].platform_data == NULL) {
++ timberdale_cells_bar0[i].platform_data =
++ timberdale_cells_bar0 + i;
++ timberdale_cells_bar0[i].data_size =
++ sizeof(timberdale_cells_bar0[i]);
++ }
++
++ err = mfd_add_devices(&dev->dev, -1,
++ timberdale_cells_bar0, ARRAY_SIZE(timberdale_cells_bar0),
++ &dev->resource[0], priv->irq_base);
++ if (err)
++ printk(KERN_WARNING
++ "timberdale: mfd_add_devices failed: %d\n", err);
++ else {
++ err = mfd_add_devices(&dev->dev, -1,
++ timberdale_cells_bar1,
++ ARRAY_SIZE(timberdale_cells_bar1),
++ &dev->resource[1], priv->irq_base);
++
++ if (err)
++ printk(KERN_WARNING
++ "timberdale: timb_add_sdhci failed: %d\n", err);
++ }
++
++ if (err)
++ goto err_mfd;
++
++ ver = ioread16(priv->ctl_membase + TIMB_REV);
++
++ printk(KERN_INFO "Found Maysville Card. Rev: %d\n", ver);
++
++ /* Enable interrupts and wire the hardware interrupts */
++ iowrite32(0x3, priv->intc_membase + MER);
++
++ return 0;
++err_mfd:
++ timberdale_detach_irq(dev);
++ pci_disable_msi(dev);
++err_msi:
++ iounmap(priv->ctl_membase);
++err_ioremap_ctl:
++ iounmap(priv->intc_membase);
++err_ioremap:
++ release_mem_region(priv->ctl_mapbase, CHIPCTLSIZE);
++err_request_ctl:
++ release_mem_region(priv->intc_mapbase, INTCSIZE);
++err_request:
++ pci_set_drvdata(dev, NULL);
++err_start:
++ pci_disable_device(dev);
++err_enable:
++ kfree(priv);
++ pci_set_drvdata(dev, NULL);
++ return -ENODEV;
++}
++
++static void __devexit timb_remove(struct pci_dev *dev)
++{
++ /* clean up any allocated resources and stuff here.
++ * like call release_region();
++ */
++ struct timberdale_device *priv;
++
++ priv = pci_get_drvdata(dev);
++
++ mfd_remove_devices(&dev->dev);
++
++ timberdale_detach_irq(dev);
++
++ iowrite32(0xffffffff, priv->intc_membase + IAR);
++ iowrite32(0, priv->intc_membase + MER);
++ iowrite32(0, priv->intc_membase + IER);
++
++ iounmap(priv->ctl_membase);
++ iounmap(priv->intc_membase);
++ release_mem_region(priv->ctl_mapbase, CHIPCTLSIZE);
++ release_mem_region(priv->intc_mapbase, INTCSIZE);
++
++ pci_disable_msi(dev);
++ pci_disable_device(dev);
++ pci_set_drvdata(dev, NULL);
++ kfree(priv);
++}
++
++static struct pci_device_id timberdale_pci_tbl[] = {
++ { PCI_DEVICE(PCI_VENDOR_ID_TIMB, PCI_DEVICE_ID_TIMB) },
++ { 0 }
++};
++MODULE_DEVICE_TABLE(pci, timberdale_pci_tbl);
++
++static struct pci_driver timberdale_pci_driver = {
++ .name = "timberdale",
++ .id_table = timberdale_pci_tbl,
++ .probe = timb_probe,
++ .remove = timb_remove,
++};
++
++static int __init timberdale_init(void)
++{
++ int err;
++
++ err = pci_register_driver(&timberdale_pci_driver);
++ if (err < 0) {
++ printk(KERN_ERR
++ "Failed to register PCI driver for %s device.\n",
++ timberdale_pci_driver.name);
++ return -ENODEV;
++ }
++
++ printk(KERN_INFO "Driver for %s has been successfully registered.\n",
++ timberdale_pci_driver.name);
++
++ return 0;
++}
++
++static void __exit timberdale_exit(void)
++{
++ pci_unregister_driver(&timberdale_pci_driver);
++
++ printk(KERN_INFO "Driver for %s has been successfully unregistered.\n",
++ timberdale_pci_driver.name);
++}
++
++MODULE_LICENSE("GPL v2");
++MODULE_VERSION(DRV_VERSION);
++MODULE_AUTHOR("Richard Rojfors");
++
++module_init(timberdale_init);
++module_exit(timberdale_exit);
++
+diff -uNr linux-2.6.29-clean/drivers/mfd/timberdale.h linux-2.6.29/drivers/mfd/timberdale.h
+--- linux-2.6.29-clean/drivers/mfd/timberdale.h 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/mfd/timberdale.h 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,114 @@
++/*
++ * timberdale.h timberdale FPGA mfd shim driver defines
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Timberdale FPGA
++ */
++
++#ifndef MFD_TIMBERDALE_H
++#define MFD_TIMBERDALE_H
++
++/* Registers of the interrupt controller */
++#define ISR 0x00
++#define IPR 0x04
++#define IER 0x08
++#define IAR 0x0c
++#define SIE 0x10
++#define CIE 0x14
++#define MER 0x1c
++
++/* Registers of the control area */
++#define TIMB_REV 0x00
++#define MAYSVILLERST 0x40
++
++
++#define I2COFFSET 0x0
++#define I2CEND 0x1f
++
++#define SPIOFFSET 0x80
++#define SPIEND 0xff
++
++#define ETHOFFSET 0x300
++#define ETHEND 0x30f
++
++#define GPIOOFFSET 0x400
++#define GPIOEND 0x7ff
++
++#define CHIPCTLOFFSET 0x800
++#define CHIPCTLEND 0x8ff
++#define CHIPCTLSIZE (CHIPCTLEND - CHIPCTLOFFSET)
++
++#define INTCOFFSET 0xc00
++#define INTCEND 0xfff
++#define INTCSIZE (INTCEND - INTCOFFSET)
++
++#define MOSTOFFSET 0x1000
++#define MOSTEND 0x13ff
++
++#define UARTOFFSET 0x1400
++#define UARTEND 0x17ff
++
++#define I2SOFFSET 0x1C00
++#define I2SEND 0x1fff
++
++#define LOGIWOFFSET 0x30000
++#define LOGIWEND 0x37fff
++
++#define DMAOFFSET 0x01000000
++#define DMAEND 0x013fffff
++
++/* SDHC0 is placed in PCI bar 1 */
++#define SDHC0OFFSET 0x00
++#define SDHC0END 0xff
++
++/* SDHC1 is placed in PCI bar 2 */
++#define SDHC1OFFSET 0x00
++#define SDHC1END 0xff
++
++#define PCI_VENDOR_ID_TIMB 0x10ee
++#define PCI_DEVICE_ID_TIMB 0xa123
++#define DRV_VERSION "0.1"
++
++
++#define IRQ_TIMBERDALE_INIC 0
++#define IRQ_TIMBERDALE_MLB 1
++#define IRQ_TIMBERDALE_GPIO 2
++#define IRQ_TIMBERDALE_I2C 3
++#define IRQ_TIMBERDALE_UART 4
++#define IRQ_TIMBERDALE_DMA 5
++#define IRQ_TIMBERDALE_I2S 6
++#define IRQ_TIMBERDALE_TSC_INT 7
++#define IRQ_TIMBERDALE_SDHC 8
++#define IRQ_TIMBERDALE_ADV7180 9
++#define IRQ_TIMBERDALE_ETHSW_IF 10
++#define IRQ_TIMBERDALE_SPI 11
++
++#define TIMBERDALE_NR_IRQS 12
++
++/* Some of the interrupts are level triggered, some are edge triggered */
++#define IRQ_TIMBERDALE_EDGE_MASK ((1 << IRQ_TIMBERDALE_ADV7180) | \
++ (1 << IRQ_TIMBERDALE_TSC_INT) | (1 << IRQ_TIMBERDALE_DMA) | \
++ (1 << IRQ_TIMBERDALE_MLB) | (1 << IRQ_TIMBERDALE_INIC))
++
++#define IRQ_TIMBERDALE_LEVEL_MASK ((1 << IRQ_TIMBERDALE_SPI) | \
++ (1 << IRQ_TIMBERDALE_ETHSW_IF) | (1 << IRQ_TIMBERDALE_SDHC) | \
++ (1 << IRQ_TIMBERDALE_I2S) | (1 << IRQ_TIMBERDALE_UART) | \
++ (1 << IRQ_TIMBERDALE_I2C) | (1 << IRQ_TIMBERDALE_GPIO))
++
++#endif
++
+diff -uNr linux-2.6.29-clean/drivers/mfd/timbi2s.c linux-2.6.29/drivers/mfd/timbi2s.c
+--- linux-2.6.29-clean/drivers/mfd/timbi2s.c 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/mfd/timbi2s.c 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,597 @@
++/*
++ * timbi2s.c timberdale FPGA I2S driver
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Timberdale FPGA I2S
++ *
++ * As of 2009-03-23 I2S instances
++ * are not configured as masters
++ *
++ * TODO: implement switching between master and slave
++ */
++
++#include <linux/io.h>
++#include <linux/fs.h>
++#include <linux/module.h>
++#include <linux/circ_buf.h>
++#include <linux/spinlock.h>
++#include <linux/workqueue.h>
++#include <linux/interrupt.h>
++#include <linux/platform_device.h>
++
++#include <linux/mfd/timbi2s.h>
++
++#define DRIVER_NAME "timb-i2s"
++
++#define I2S_CLOCK_SPEED 62500000 /* 62,5MHz */
++
++#define FIFO_FILL_SIZE 127
++#define I2S_BUFFER_SIZE PAGE_SIZE
++
++#define ALMOST_FULL 170
++#define ALMOST_EMPTY 85
++
++/* As of 2009-03-16, IP can instanciate max. 4 RX and 4 TX */
++#define MAX_TX_NR 4
++#define MAX_RX_NR 4
++/* and actually up and running only 4.
++ * 1 TX and 3 RX
++ */
++#define IP_I2S_NR 4
++#define REGSTEP 0x04
++
++#define VERSION 0x00
++#define I2S_UIR 0x04 /* Unit Interrupt Register */
++
++/* Registers for all possible I2S IP instances
++ * are the same as for first one (from 0x08 to 0x20)
++ */
++#define I2S_PRESCALE 0x08 /* Holds prescale value, if clock master */
++#define I2S_ICR 0x0c /* Interrupt Clear Register */
++# define ICR_F 0x01 /* Full */
++# define ICR_AF 0x02 /* Almost full */
++# define ICR_AE 0x04 /* Almost empty */
++# define ICR_RX_D 0x08 /* Data present, RX only */
++# define ICR_TX_E 0x08 /* Epmty, TX only */
++
++#define I2S_IPR 0x10 /* Interrupt Pending Register */
++#define I2S_ISR 0x14 /* Interrupt Status Register */
++
++#define I2S_IER 0x18 /* Interrupt Enable Register */
++# define IER_FF 0x01 /* RX/TX FIFO Full */
++# define IER_FAF 0x02 /* RX/TX FIFO Almost Full */
++# define IER_FAE 0x04 /* RX/TX FIFO Almost Empty */
++# define IER_RX_DATA 0x08 /* RX. Data Present */
++# define IER_TX_FE 0x08 /* TX. FIFO Empty */
++
++#define I2S_CTRL 0x1c /* Control Register */
++# define CTRL_TX_ENABLE 0x01 /* Enable TX */
++# define CTRL_RX_ENABLE 0x02 /* Enable RX */
++# define CTRL_NONE 0x04 /* Not used */
++# define CTRL_FIFO_CLR 0x08 /* FIFO Clear */
++# define CTRL_SWR 0x10 /* Soft reset */
++# define CTRL_CLKMASTER 0x1000 /* IP I2S instance is master */
++# define CTRL_IS_TX 0x40000000 /* IP I2S is an TX-instance */
++# define CTRL_IS_RX 0x20000000 /* IP I2S is an RX-instance */
++
++#define I2S_FIFO 0x20 /* read/write FIFO */
++
++#define INC_HEAD(buf, size) \
++ (buf->head = (buf->head + 1) & (size-1))
++
++#define INC_TAIL(buf, size) \
++ (buf->tail = (buf->tail + 1) & (size-1))
++
++
++/* circular buffer */
++static struct circ_buf *timbi2s_buf_alloc(void);
++static void timbi2s_buf_free(struct circ_buf *cb);
++static void timbi2s_buf_clear(struct circ_buf *cb);
++
++static int timbi2s_fifo_read(struct circ_buf *cb, ssize_t count, long add);
++static int timbi2s_fifo_write(struct circ_buf *cb, ssize_t count, long add);
++
++static int timbi2s_ioctrl(struct timbi2s_dev *);
++
++static struct timbi2s_bus *bus_p;
++
++static int timbi2s_is_tx(struct timbi2s_dev *i2sdev)
++{
++ return (ioread32(i2sdev->membase + i2sdev->ctrl_offset)
++ & CTRL_IS_TX) ? 1 : 0;
++}
++
++static int timbi2s_is_rx(struct timbi2s_dev *i2sdev)
++{
++ return (ioread32(i2sdev->membase + i2sdev->ctrl_offset)
++ & CTRL_IS_RX) ? 1 : 0;
++}
++
++/* Return unused TX-instance */
++static struct timbi2s_dev *timbi2s_get_tx(void)
++{
++ struct timbi2s_dev *tdev, *tmp;
++
++ if (bus_p == NULL)
++ return NULL;
++
++ list_for_each_entry_safe(tdev, tmp, &bus_p->control->list, item) {
++ if (!tdev->in_use && timbi2s_is_tx(tdev)) {
++ tdev->in_use = 1;
++ return tdev;
++ }
++
++ }
++ return NULL;
++}
++EXPORT_SYMBOL_GPL(timbi2s_get_tx);
++
++/* Return unused RX-instance */
++static struct timbi2s_dev *timbi2s_get_rx(void)
++{
++ struct timbi2s_dev *tdev, *tmp;
++
++ if (bus_p == NULL)
++ return NULL;
++
++ list_for_each_entry_safe(tdev, tmp, &bus_p->control->list, item) {
++ if (!tdev->in_use && timbi2s_is_rx(tdev)) {
++ tdev->in_use = 1;
++ return tdev;
++ }
++
++ }
++ return NULL;
++}
++EXPORT_SYMBOL_GPL(timbi2s_get_rx);
++
++/* Flag TX/RX as unused and reset it */
++static void timbi2s_put(struct timbi2s_dev *tdev)
++{
++ if (tdev->in_use) {
++ tdev->in_use = 0;
++ timbi2s_ioctrl(tdev);
++ }
++}
++EXPORT_SYMBOL_GPL(timbi2s_put);
++
++/*
++ * Write data to the FIFO
++ */
++static void timbi2s_tx_handler(struct timbi2s_dev *i2sdev)
++{
++ u32 pend;
++
++ pend = ioread32(i2sdev->membase + i2sdev->ipr_offset);
++
++ if (pend & IER_FAE) {
++ timbi2s_fifo_write(i2sdev->buffer,
++ ALMOST_FULL - ALMOST_EMPTY,
++ (unsigned long)i2sdev->membase +
++ i2sdev->fifo);
++ /* clear interrupt */
++ iowrite32(ICR_AE, i2sdev->membase + i2sdev->icr_offset);
++ }
++}
++
++/*
++ * Read data from the FIFO
++ */
++static void timbi2s_rx_handler(struct timbi2s_dev *i2sdev)
++{
++ u32 pend;
++ pend = ioread32(i2sdev->membase + i2sdev->ipr_offset);
++
++ if (pend & IER_FAE) {
++ timbi2s_fifo_read(i2sdev->buffer,
++ ALMOST_EMPTY,
++ (unsigned long)i2sdev->membase +
++ i2sdev->fifo);
++
++ /* clear interrupt */
++ iowrite32(ICR_AE | ICR_AF,
++ i2sdev->membase + i2sdev->icr_offset);
++ }
++}
++
++void timbi2s_int_handler(struct work_struct *workp)
++{
++ u32 pend, stat, i2stype;
++ unsigned long flags;
++ struct timbi2s_dev *i2sdev = container_of(workp,
++ struct timbi2s_dev,
++ work);
++
++ pend = ioread32(i2sdev->membase + i2sdev->ipr_offset);
++ stat = ioread32(i2sdev->membase + i2sdev->isr_offset);
++ i2stype = ioread32(i2sdev->membase + i2sdev->ctrl_offset);
++
++ spin_lock_irqsave(&i2sdev->lock, flags);
++
++ if (i2stype & CTRL_IS_RX) {
++ /* Enable Almost Empty Almost Full interrupt */
++ iowrite32(IER_FAE | IER_FAF,
++ i2sdev->membase + i2sdev->ier_offset);
++ /* Enable RX */
++ iowrite32(CTRL_RX_ENABLE,
++ i2sdev->membase + i2sdev->ctrl_offset);
++ timbi2s_rx_handler(i2sdev);
++ } else if (i2stype & CTRL_IS_TX) {
++ /* Enable Almost Empty interrupt */
++ iowrite32(IER_FAE, i2sdev->membase + i2sdev->ier_offset);
++ /* Enable TX */
++ iowrite32(CTRL_TX_ENABLE,
++ i2sdev->membase + i2sdev->ctrl_offset);
++ timbi2s_tx_handler(i2sdev);
++ }
++
++ spin_unlock_irqrestore(&i2sdev->lock, flags);
++}
++
++static int timbi2s_ioctrl(struct timbi2s_dev *i2sdev)
++{
++ u32 i2stype;
++
++ /* Reset */
++ iowrite8(CTRL_SWR, i2sdev->membase + i2sdev->ctrl_offset);
++ /* Clear IER */
++ iowrite32(0x00000000, i2sdev->membase + i2sdev->ier_offset);
++ /* Clear ICR */
++ iowrite32(0xffffffff, i2sdev->membase + i2sdev->icr_offset);
++
++ i2stype = ioread32(i2sdev->membase + i2sdev->ctrl_offset);
++
++ if (i2stype & CTRL_IS_TX)
++ printk(KERN_INFO DRIVER_NAME": found active I2S Transmitter\n");
++ else if (i2stype & CTRL_IS_RX)
++ printk(KERN_INFO DRIVER_NAME": found active I2S Receiver\n");
++
++ return 1;
++}
++EXPORT_SYMBOL_GPL(timbi2s_ioctrl);
++
++static struct circ_buf *timbi2s_buf_alloc(void)
++{
++ struct circ_buf *cb;
++
++ cb = kzalloc(sizeof(*cb), GFP_KERNEL);
++ if (cb == NULL)
++ return NULL;
++
++ cb->buf = kzalloc(I2S_BUFFER_SIZE, GFP_KERNEL);
++ if (cb->buf == NULL) {
++ kfree(cb);
++ return NULL;
++ }
++
++ timbi2s_buf_clear(cb);
++
++ return cb;
++}
++
++static void timbi2s_buf_free(struct circ_buf *cb)
++{
++ kfree(cb->buf);
++ kfree(cb);
++}
++
++static void timbi2s_buf_clear(struct circ_buf *cb)
++{
++ cb->head = 0;
++ cb->tail = cb->head;
++}
++
++/*
++ * Read data from the FIFO and write it to the given circular buffer
++ */
++static int timbi2s_fifo_read(struct circ_buf *cb, ssize_t count, long add)
++{
++ int c, ret = 0;
++
++ unsigned char *hi = (unsigned char *)ioread32((void *)(add >> 16));
++ unsigned char *lo = (unsigned char *)ioread32((void *)(add & 0xFFFF));
++
++ c = CIRC_SPACE_TO_END(cb->head, cb->tail, I2S_BUFFER_SIZE);
++ if (count < c)
++ c = count;
++
++ if (c <= 0)
++ return 1;
++
++ while (c >= 0) {
++ memcpy(cb->buf + cb->head, hi, 2);
++ INC_HEAD(cb, I2S_BUFFER_SIZE);
++
++ memcpy(cb->buf + cb->head, lo, 2);
++ INC_HEAD(cb, I2S_BUFFER_SIZE);
++ count -= 4;
++ }
++ return ret;
++}
++
++/*
++ * Get data from the circular buffer and write it to the given FIFO address
++ */
++static int timbi2s_fifo_write(struct circ_buf *cb, ssize_t count, long add)
++{
++ int c, ret = 0;
++
++ c = CIRC_CNT_TO_END(cb->head, cb->tail, I2S_BUFFER_SIZE);
++ if (count < c)
++ c = count;
++
++ if (c <= 0)
++ return 1;
++
++ while (c >= 0) {
++ iowrite32(*(s16 *)(cb->buf + cb->tail), (void *)(add >> 16));
++ INC_TAIL(cb, I2S_BUFFER_SIZE);
++
++ iowrite32(*(s16 *)(cb->buf + cb->tail), (void *)(add & 0xFFFF));
++ INC_TAIL(cb, I2S_BUFFER_SIZE);
++ count -= 4;
++ }
++
++ return ret;
++}
++
++static void timbi2s_control_destroy(struct timbi2s_bus_control *control)
++{
++ kfree(control);
++ control = NULL;
++}
++
++static void timbi2s_control_add_dev(struct timbi2s_dev *i2sdev)
++{
++ list_add(&i2sdev->item, &i2sdev->bus->control->list);
++}
++
++static void timbi2s_control_del_dev(struct timbi2s_dev *i2sdev)
++{
++ list_del(&i2sdev->item);
++ if (list_empty(&i2sdev->bus->control->list))
++ timbi2s_control_destroy(i2sdev->bus->control);
++}
++
++static irqreturn_t timbi2s_irq(int irq, void *dev_id)
++{
++ u8 pend;
++ u32 iunit;
++ int i;
++
++ struct timbi2s_bus *tbus = dev_id;
++ queue_work(tbus->workqueue, &tbus->work);
++
++ iunit = ioread32(tbus->membase + I2S_UIR);
++ /* Find out which I2S instance is interrupting */
++ for (i = 0; i < 32; i++) {
++ if ((1 << i) & iunit) {
++ pend = ioread8(tbus->membase +
++ (I2S_IPR + (i * REGSTEP * 7)));
++ iowrite8(pend, tbus->membase +
++ (I2S_ICR + (i * REGSTEP * 7)));
++ }
++ }
++
++ return IRQ_HANDLED;
++}
++
++static int __init timbi2s_probe(struct platform_device *dev)
++{
++ int err = 0;
++ struct timbi2s_dev *tdev, *tmp;
++ struct timbi2s_bus *tbus;
++ struct resource *iomem;
++ int i;
++
++ iomem = platform_get_resource(dev, IORESOURCE_MEM, 0);
++ if (!iomem) {
++ err = -EINVAL;
++ goto err_mem;
++ }
++
++ tbus = kzalloc(sizeof(*tbus), GFP_KERNEL);
++ if (!tbus) {
++ err = -EINVAL;
++ goto err_mem;
++ }
++
++ /* Init bus_control */
++ tbus->control = kzalloc(sizeof(struct timbi2s_bus_control), GFP_KERNEL);
++ if (!tbus->control) {
++ printk(KERN_ERR DRIVER_NAME
++ ": Failed to allocate timbi2s_bus_control.\n");
++ err = -ENOMEM;
++ goto err_free;
++ }
++ INIT_LIST_HEAD(&tbus->control->list);
++
++ /* Init workqueue */
++ tbus->workqueue = create_singlethread_workqueue("timbi2s");
++ if (tbus->workqueue == NULL) {
++ printk(KERN_ERR DRIVER_NAME
++ ": unable to create workqueue\n");
++ err = -ENOMEM;
++ goto err_control;
++ }
++ INIT_WORK(&tbus->work, timbi2s_int_handler);
++
++ if (!request_mem_region(iomem->start,
++ resource_size(iomem), DRIVER_NAME)) {
++ printk(KERN_EMERG DRIVER_NAME
++ ": Mem region is already in use\n");
++ err = -ENXIO;
++ goto err_control;
++ }
++
++ tbus->membase = ioremap(iomem->start, resource_size(iomem));
++ if (tbus->membase == NULL) {
++ err = -ENOMEM;
++ goto err_request;
++ }
++
++ bus_p = tbus;
++
++
++
++ /* For now we have only 4 I2S instances in IP : 3 RX and 1 TX */
++ /* Note: TX'es are always on top */
++ /* TODO: auto-check how many are alive and bring them into control */
++ for (i = 0; i < IP_I2S_NR; i++) {
++ tdev = kzalloc(sizeof(*tdev), GFP_KERNEL);
++ if (!tdev) {
++ err = -EINVAL;
++ goto clean_list;
++ }
++
++ /* Allocate circ_buf */
++ tdev->buffer = timbi2s_buf_alloc();
++ if (tdev->buffer == NULL) {
++ printk(KERN_ERR "timbi2s: unable to allocate buffer\n");
++ goto clean_list;
++ }
++
++ INIT_LIST_HEAD(&tdev->item);
++ spin_lock_init(&tdev->lock);
++
++ /* set up offsets for each instance of I2S */
++ tdev->bus = tbus; /* ptr to our bus */
++ tdev->membase = tbus->membase;
++ tdev->in_use = 0;
++ tdev->pscale_offset = I2S_PRESCALE + (i * REGSTEP * 7);
++ tdev->icr_offset = I2S_ICR + (i * REGSTEP * 7);
++ tdev->isr_offset = I2S_ISR + (i * REGSTEP * 7);
++ tdev->ipr_offset = I2S_IPR + (i * REGSTEP * 7);
++ tdev->ier_offset = I2S_IER + (i * REGSTEP * 7);
++ tdev->ctrl_offset = I2S_CTRL + (i * REGSTEP * 7);
++ tdev->fifo = I2S_FIFO + (i * REGSTEP * 7);
++
++ /* Try to check and reset hardware */
++ if (timbi2s_ioctrl(tdev))
++ timbi2s_control_add_dev(tdev);
++
++ tdev = NULL;
++ }
++
++ tbus->irq = platform_get_irq(dev, 0);
++ if (tbus->irq < 0) {
++ err = -EINVAL;
++ goto clean_list;
++ }
++
++ err = request_irq(tbus->irq, timbi2s_irq, 0, DRIVER_NAME, tbus);
++ if (err != 0)
++ goto clean_list;
++
++ platform_set_drvdata(dev, tbus);
++
++ dev_info(&dev->dev, "Driver for Timberdale I2S (ver: %d)"
++ " has been successfully registered.\n",
++ ioread32(tbus->membase + 0x00));
++ return 0;
++
++clean_list:
++ list_for_each_entry_safe(tdev, tmp, &tbus->control->list, item) {
++ if (tdev->workqueue != NULL) {
++ flush_workqueue(tdev->workqueue);
++ destroy_workqueue(tdev->workqueue);
++ }
++
++ if (tdev->buffer != NULL)
++ timbi2s_buf_free(tdev->buffer);
++
++ timbi2s_control_del_dev(tdev);
++ kfree(tdev);
++ }
++ free_irq(tbus->irq, tbus);
++ iounmap(tbus->membase);
++err_request:
++ release_mem_region(iomem->start, resource_size(iomem));
++err_control:
++ if (tbus->control != NULL)
++ timbi2s_control_destroy(tbus->control);
++err_free:
++ kfree(tbus);
++err_mem:
++ printk(KERN_ERR
++ DRIVER_NAME": Failed to register Timberdale I2S: %d\n", err);
++
++ return err;
++}
++
++static int __devexit timbi2s_remove(struct platform_device *dev)
++{
++ struct timbi2s_bus *tbus;
++ struct timbi2s_dev *tdev, *tmp;
++ struct resource *r;
++
++ tbus = platform_get_drvdata(dev);
++ free_irq(tbus->irq, tbus);
++
++ r = platform_get_resource(dev, IORESOURCE_MEM, 0);
++
++ list_for_each_entry_safe(tdev, tmp, &tbus->control->list, item) {
++ if (tdev->workqueue != NULL) {
++ flush_workqueue(tdev->workqueue);
++ destroy_workqueue(tdev->workqueue);
++ }
++
++ if (tdev->buffer != NULL)
++ timbi2s_buf_free(tdev->buffer);
++
++ kfree(tdev);
++ }
++
++ iounmap(tdev->membase);
++ if (r)
++ release_mem_region(r->start, resource_size(r));
++
++ dev_info(&dev->dev, "Driver for Timberdale I2S has been"
++ " successfully unregistered.\n");
++
++ platform_set_drvdata(dev, 0);
++ return 0;
++}
++
++static struct platform_driver timbi2s_platform_driver = {
++ .driver = {
++ .name = DRIVER_NAME,
++ .owner = THIS_MODULE,
++ },
++ .probe = timbi2s_probe,
++ .remove = __devexit_p(timbi2s_remove),
++};
++
++/*--------------------------------------------------------------------------*/
++
++static int __init timbi2s_init(void)
++{
++ return platform_driver_register(&timbi2s_platform_driver);
++}
++
++static void __exit timbi2s_exit(void)
++{
++ platform_driver_unregister(&timbi2s_platform_driver);
++}
++
++module_init(timbi2s_init);
++module_exit(timbi2s_exit);
++
++MODULE_AUTHOR("Mocean Laboratories");
++MODULE_DESCRIPTION("Timberdale I2S bus driver");
++MODULE_LICENSE("GPL v2");
+diff -uNr linux-2.6.29-clean/drivers/mmc/host/Kconfig linux-2.6.29/drivers/mmc/host/Kconfig
+--- linux-2.6.29-clean/drivers/mmc/host/Kconfig 2009-04-01 09:20:24.000000000 -0700
++++ linux-2.6.29/drivers/mmc/host/Kconfig 2009-04-06 13:51:47.000000000 -0700
+@@ -65,6 +65,16 @@
+
+ If unsure, say Y.
+
++config MMC_SDHCI_PLTFM
++ tristate "SDHCI support on platform devices"
++ depends on MMC_SDHCI
++ help
++ This selects the Secure Digital Host Controller Interface.
++
++ If you have a controller with this interface, say Y or M here.
++
++ If unsure, say N.
++
+ config MMC_OMAP
+ tristate "TI OMAP Multimedia Card Interface support"
+ depends on ARCH_OMAP
+diff -uNr linux-2.6.29-clean/drivers/mmc/host/Makefile linux-2.6.29/drivers/mmc/host/Makefile
+--- linux-2.6.29-clean/drivers/mmc/host/Makefile 2009-04-01 09:20:24.000000000 -0700
++++ linux-2.6.29/drivers/mmc/host/Makefile 2009-04-06 13:51:47.000000000 -0700
+@@ -13,6 +13,7 @@
+ obj-$(CONFIG_MMC_SDHCI) += sdhci.o
+ obj-$(CONFIG_MMC_SDHCI_PCI) += sdhci-pci.o
+ obj-$(CONFIG_MMC_RICOH_MMC) += ricoh_mmc.o
++obj-$(CONFIG_MMC_SDHCI_PLTFM) += sdhci-pltfm.o
+ obj-$(CONFIG_MMC_WBSD) += wbsd.o
+ obj-$(CONFIG_MMC_AU1X) += au1xmmc.o
+ obj-$(CONFIG_MMC_OMAP) += omap.o
+diff -uNr linux-2.6.29-clean/drivers/mmc/host/sdhci-pltfm.c linux-2.6.29/drivers/mmc/host/sdhci-pltfm.c
+--- linux-2.6.29-clean/drivers/mmc/host/sdhci-pltfm.c 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/mmc/host/sdhci-pltfm.c 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,262 @@
++/*
++ * sdhci-pltfm.c Support for SDHCI platform devices
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * SDHCI platform devices
++ *
++ * Inspired by sdhci-pci.c, by Pierre Ossman
++ */
++
++#include <linux/delay.h>
++#include <linux/highmem.h>
++#include <linux/platform_device.h>
++
++#include <linux/mmc/host.h>
++
++#include <linux/io.h>
++
++#include "sdhci.h"
++
++
++#define MAX_SLOTS 8
++
++struct sdhci_pltfm_chip;
++
++struct sdhci_pltfm_slot {
++ struct sdhci_pltfm_chip *chip;
++ struct sdhci_host *host;
++
++ int pltfm_resource;
++};
++
++struct sdhci_pltfm_chip {
++ struct platform_device *pdev;
++
++ unsigned int quirks;
++
++ int num_slots; /* Slots on controller */
++ struct sdhci_pltfm_slot *slots[MAX_SLOTS]; /* Pointers to host slots */
++};
++
++
++/*****************************************************************************\
++ * *
++ * SDHCI core callbacks *
++ * *
++\*****************************************************************************/
++
++static struct sdhci_ops sdhci_pltfm_ops = {
++};
++
++/*****************************************************************************\
++ * *
++ * Device probing/removal *
++ * *
++\*****************************************************************************/
++
++
++static struct sdhci_pltfm_slot * __devinit sdhci_pltfm_probe_slot(
++ struct platform_device *pdev, struct sdhci_pltfm_chip *chip,
++ int resource)
++{
++ struct sdhci_pltfm_slot *slot;
++ struct sdhci_host *host;
++ struct resource *iomem;
++ int ret;
++
++ iomem = platform_get_resource(pdev, IORESOURCE_MEM, resource);
++ if (!iomem)
++ return ERR_PTR(-ENODEV);
++
++ if (resource_size(iomem) != 0x100) {
++ dev_err(&pdev->dev, "Invalid iomem size. You may "
++ "experience problems.\n");
++ }
++
++ if (!pdev->dev.parent) {
++ dev_err(&pdev->dev, "The parent device be a PCI device\n");
++ return ERR_PTR(-ENODEV);
++ }
++
++ host = sdhci_alloc_host(pdev->dev.parent,
++ sizeof(struct sdhci_pltfm_slot));
++ if (IS_ERR(host))
++ return ERR_PTR(PTR_ERR(host));
++
++ slot = sdhci_priv(host);
++
++ slot->chip = chip;
++ slot->host = host;
++ slot->pltfm_resource = resource;
++
++ host->hw_name = "PLTFM";
++ host->ops = &sdhci_pltfm_ops;
++ host->quirks = chip->quirks;
++
++ host->irq = platform_get_irq(pdev, 0);
++
++ if (!request_mem_region(iomem->start, resource_size(iomem),
++ mmc_hostname(host->mmc))) {
++ dev_err(&pdev->dev, "cannot request region\n");
++ ret = -EBUSY;
++ goto free;
++ }
++
++ host->ioaddr = ioremap(iomem->start, resource_size(iomem));
++ if (!host->ioaddr) {
++ dev_err(&pdev->dev, "failed to remap registers\n");
++ goto release;
++ }
++
++ ret = sdhci_add_host(host);
++ if (ret)
++ goto unmap;
++
++ return slot;
++
++unmap:
++ iounmap(host->ioaddr);
++release:
++ release_mem_region(iomem->start, resource_size(iomem));
++free:
++ sdhci_free_host(host);
++
++ return ERR_PTR(ret);
++}
++
++static void sdhci_pltfm_remove_slot(struct sdhci_pltfm_slot *slot)
++{
++ int dead;
++ u32 scratch;
++ struct resource *iomem;
++
++ dead = 0;
++ scratch = readl(slot->host->ioaddr + SDHCI_INT_STATUS);
++ if (scratch == (u32)-1)
++ dead = 1;
++
++ sdhci_remove_host(slot->host, dead);
++
++ iounmap(slot->host->ioaddr);
++
++ iomem = platform_get_resource(slot->chip->pdev, IORESOURCE_MEM,
++ slot->pltfm_resource);
++ release_mem_region(iomem->start, resource_size(iomem));
++
++ sdhci_free_host(slot->host);
++}
++
++static int __devinit sdhci_pltfm_probe(struct platform_device *pdev)
++{
++ struct sdhci_pltfm_chip *chip;
++ struct sdhci_pltfm_slot *slot;
++ u8 slots;
++ int ret, i;
++
++ BUG_ON(pdev == NULL);
++
++ for (slots = 0; slots <= MAX_SLOTS; slots++)
++ if (!platform_get_resource(pdev, IORESOURCE_MEM, slots))
++ break;
++
++ BUG_ON(slots > MAX_SLOTS || slots == 0);
++
++ chip = kzalloc(sizeof(struct sdhci_pltfm_chip), GFP_KERNEL);
++ if (!chip) {
++ ret = -ENOMEM;
++ goto err;
++ }
++
++ chip->pdev = pdev;
++ chip->num_slots = slots;
++ platform_set_drvdata(pdev, chip);
++
++ for (i = 0; i < slots; i++) {
++ slot = sdhci_pltfm_probe_slot(pdev, chip, i);
++ if (IS_ERR(slot)) {
++ for (i--; i >= 0; i--)
++ sdhci_pltfm_remove_slot(chip->slots[i]);
++ ret = PTR_ERR(slot);
++ goto free;
++ }
++
++ chip->slots[i] = slot;
++ }
++
++ return 0;
++
++free:
++ platform_set_drvdata(pdev, NULL);
++ kfree(chip);
++
++err:
++ printk(KERN_ERR"Probing of sdhci-pltfm failed: %d\n", ret);
++ return ret;
++}
++
++static int __devexit sdhci_pltfm_remove(struct platform_device *pdev)
++{
++ int i;
++ struct sdhci_pltfm_chip *chip;
++
++ chip = platform_get_drvdata(pdev);
++
++ if (chip) {
++ for (i = 0; i < chip->num_slots; i++)
++ sdhci_pltfm_remove_slot(chip->slots[i]);
++
++ platform_set_drvdata(pdev, NULL);
++ kfree(chip);
++ }
++
++ return 0;
++}
++
++static struct platform_driver sdhci_pltfm_driver = {
++ .driver = {
++ .name = "sdhci",
++ .owner = THIS_MODULE,
++ },
++ .probe = sdhci_pltfm_probe,
++ .remove = __devexit_p(sdhci_pltfm_remove),
++};
++
++/*****************************************************************************\
++ * *
++ * Driver init/exit *
++ * *
++\*****************************************************************************/
++
++static int __init sdhci_drv_init(void)
++{
++ return platform_driver_register(&sdhci_pltfm_driver);
++}
++
++static void __exit sdhci_drv_exit(void)
++{
++ platform_driver_unregister(&sdhci_pltfm_driver);
++}
++
++module_init(sdhci_drv_init);
++module_exit(sdhci_drv_exit);
++
++MODULE_DESCRIPTION("Secure Digital Host Controller Interface platform driver");
++MODULE_AUTHOR("Mocean Laboratories <info@mocean-labs.com>");
++MODULE_LICENSE("GPL v2");
++MODULE_ALIAS("platform:sdhci");
++
+diff -uNr linux-2.6.29-clean/drivers/serial/Kconfig linux-2.6.29/drivers/serial/Kconfig
+--- linux-2.6.29-clean/drivers/serial/Kconfig 2009-04-01 09:20:24.000000000 -0700
++++ linux-2.6.29/drivers/serial/Kconfig 2009-04-06 13:51:47.000000000 -0700
+@@ -1412,4 +1412,11 @@
+ default 19200 if (SERIAL_SPORT_BAUD_RATE_19200)
+ default 9600 if (SERIAL_SPORT_BAUD_RATE_9600)
+
++config SERIAL_TIMBERDALE
++ tristate "Support for timberdale UART"
++ depends on MFD_TIMBERDALE
++ select SERIAL_CORE
++ ---help---
++ Add support for UART controller on timberdale.
++
+ endmenu
+diff -uNr linux-2.6.29-clean/drivers/serial/Makefile linux-2.6.29/drivers/serial/Makefile
+--- linux-2.6.29-clean/drivers/serial/Makefile 2009-04-01 09:20:24.000000000 -0700
++++ linux-2.6.29/drivers/serial/Makefile 2009-04-06 13:51:47.000000000 -0700
+@@ -76,3 +76,4 @@
+ obj-$(CONFIG_SERIAL_KS8695) += serial_ks8695.o
+ obj-$(CONFIG_KGDB_SERIAL_CONSOLE) += kgdboc.o
+ obj-$(CONFIG_SERIAL_QE) += ucc_uart.o
++obj-$(CONFIG_SERIAL_TIMBERDALE) += timbuart.o
+diff -uNr linux-2.6.29-clean/drivers/serial/timbuart.c linux-2.6.29/drivers/serial/timbuart.c
+--- linux-2.6.29-clean/drivers/serial/timbuart.c 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/serial/timbuart.c 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,519 @@
++/*
++ * timbuart.c timberdale FPGA UART driver
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Timberdale FPGA UART
++ */
++
++#include <linux/pci.h>
++#include <linux/interrupt.h>
++#include <linux/serial_core.h>
++#include <linux/kernel.h>
++#include <linux/platform_device.h>
++#include <linux/ioport.h>
++
++#include "timbuart.h"
++
++struct timbuart_port {
++ struct uart_port port;
++ struct tasklet_struct tasklet;
++ int usedma;
++ u8 last_ier;
++ struct platform_device *dev;
++};
++
++static int baudrates[] = {9600, 19200, 38400, 57600, 115200, 230400, 460800,
++ 921600, 1843200, 3250000};
++
++static void timbuart_mctrl_check(struct uart_port *port, u8 isr, u8 *ier);
++
++static irqreturn_t timbuart_handleinterrupt(int irq, void *devid);
++
++static void timbuart_stop_rx(struct uart_port *port)
++{
++ /* spin lock held by upper layer, disable all RX interrupts */
++ u8 ier = ioread8(port->membase + TIMBUART_IER) & ~RXFLAGS;
++ iowrite8(ier, port->membase + TIMBUART_IER);
++}
++
++static void timbuart_stop_tx(struct uart_port *port)
++{
++ /* spinlock held by upper layer, disable TX interrupt */
++ u8 ier = ioread8(port->membase + TIMBUART_IER) & ~TXBAE;
++ iowrite8(ier, port->membase + TIMBUART_IER);
++}
++
++static void timbuart_start_tx(struct uart_port *port)
++{
++ struct timbuart_port *uart =
++ container_of(port, struct timbuart_port, port);
++
++ /* do not transfer anything here -> fire off the tasklet */
++ tasklet_schedule(&uart->tasklet);
++}
++
++static void timbuart_flush_buffer(struct uart_port *port)
++{
++ u8 ctl = ioread8(port->membase + TIMBUART_CTRL) | TIMBUART_CTRL_FLSHTX;
++
++ iowrite8(ctl, port->membase + TIMBUART_CTRL);
++ iowrite8(TXBF, port->membase + TIMBUART_ISR);
++}
++
++static void timbuart_rx_chars(struct uart_port *port)
++{
++ struct tty_struct *tty = port->info->port.tty;
++
++ while (ioread8(port->membase + TIMBUART_ISR) & RXDP) {
++ u8 ch = ioread8(port->membase + TIMBUART_RXFIFO);
++ /* ack */
++ iowrite8(RXDP, port->membase + TIMBUART_ISR);
++ port->icount.rx++;
++ tty_insert_flip_char(tty, ch, TTY_NORMAL);
++ }
++
++ spin_unlock(&port->lock);
++ tty_flip_buffer_push(port->info->port.tty);
++ spin_lock(&port->lock);
++
++ dev_dbg(port->dev, "%s - total read %d bytes\n",
++ __func__, port->icount.rx);
++}
++
++static void timbuart_tx_chars(struct uart_port *port)
++{
++ struct circ_buf *xmit = &port->info->xmit;
++
++ while (!(ioread8(port->membase + TIMBUART_ISR) & TXBF) &&
++ !uart_circ_empty(xmit)) {
++ iowrite8(xmit->buf[xmit->tail],
++ port->membase + TIMBUART_TXFIFO);
++ xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
++ port->icount.tx++;
++ }
++
++ dev_dbg(port->dev,
++ "%s - total written %d bytes, CTL: %x, RTS: %x, baud: %x\n",
++ __func__,
++ port->icount.tx,
++ ioread8(port->membase + TIMBUART_CTRL),
++ port->mctrl & TIOCM_RTS,
++ ioread8(port->membase + TIMBUART_BAUDRATE));
++}
++
++static void timbuart_handle_tx_port(struct uart_port *port, u8 isr, u8 *ier)
++{
++ struct timbuart_port *uart =
++ container_of(port, struct timbuart_port, port);
++ struct circ_buf *xmit = &port->info->xmit;
++
++ if (uart_circ_empty(xmit) || uart_tx_stopped(port))
++ return;
++
++ if (port->x_char)
++ return;
++
++ if (isr & TXFLAGS) {
++ timbuart_tx_chars(port);
++ /* clear all TX interrupts */
++ iowrite8(TXFLAGS, port->membase + TIMBUART_ISR);
++
++ if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
++ uart_write_wakeup(port);
++ } else
++ /* Re-enable any tx interrupt */
++ *ier |= uart->last_ier & TXFLAGS;
++
++ /* enable interrupts if there are chars in the transmit buffer,
++ * Or if we delivered some bytes and want the almost empty interrupt
++ * we wake up the upper layer later when we got the interrupt
++ * to give it some time to go out...
++ */
++ if (!uart_circ_empty(xmit))
++ *ier |= TXBAE;
++
++ dev_dbg(port->dev, "%s - leaving\n", __func__);
++}
++
++void timbuart_handle_rx_port(struct uart_port *port, u8 isr, u8 *ier)
++{
++ if (isr & RXFLAGS) {
++ /* Some RX status is set */
++ if (isr & RXBF) {
++ u8 ctl = ioread8(port->membase + TIMBUART_CTRL) |
++ TIMBUART_CTRL_FLSHRX;
++ iowrite8(ctl, port->membase + TIMBUART_CTRL);
++ port->icount.overrun++;
++ } else if (isr & (RXDP))
++ timbuart_rx_chars(port);
++
++ /* ack all RX interrupts */
++ iowrite8(RXFLAGS, port->membase + TIMBUART_ISR);
++ }
++
++ /* always have the RX interrupts enabled */
++ *ier |= RXBAF | RXBF | RXTT;
++
++ dev_dbg(port->dev, "%s - leaving\n", __func__);
++}
++
++void timbuart_tasklet(unsigned long arg)
++{
++ struct timbuart_port *uart = (struct timbuart_port *)arg;
++ u8 isr, ier = 0;
++
++ spin_lock(&uart->port.lock);
++
++ isr = ioread8(uart->port.membase + TIMBUART_ISR);
++ dev_dbg(uart->port.dev, "%s ISR: %x\n", __func__, isr);
++
++ if (!uart->usedma)
++ timbuart_handle_tx_port(&uart->port, isr, &ier);
++
++ timbuart_mctrl_check(&uart->port, isr, &ier);
++
++ if (!uart->usedma)
++ timbuart_handle_rx_port(&uart->port, isr, &ier);
++
++ iowrite8(ier, uart->port.membase + TIMBUART_IER);
++
++ spin_unlock(&uart->port.lock);
++ dev_dbg(uart->port.dev, "%s leaving\n", __func__);
++}
++
++static unsigned int timbuart_tx_empty(struct uart_port *port)
++{
++ u8 isr = ioread8(port->membase + TIMBUART_ISR);
++
++ return (isr & TXBAE) ? TIOCSER_TEMT : 0;
++}
++
++static unsigned int timbuart_get_mctrl(struct uart_port *port)
++{
++ u8 cts = ioread8(port->membase + TIMBUART_CTRL);
++ dev_dbg(port->dev, "%s - cts %x\n", __func__, cts);
++
++ if (cts & TIMBUART_CTRL_CTS)
++ return TIOCM_CTS | TIOCM_DSR | TIOCM_CAR;
++ else
++ return TIOCM_DSR | TIOCM_CAR;
++}
++
++static void timbuart_set_mctrl(struct uart_port *port, unsigned int mctrl)
++{
++ dev_dbg(port->dev, "%s - %x\n", __func__, mctrl);
++
++ if (mctrl & TIOCM_RTS)
++ iowrite8(TIMBUART_CTRL_RTS, port->membase + TIMBUART_CTRL);
++ else
++ iowrite8(TIMBUART_CTRL_RTS, port->membase + TIMBUART_CTRL);
++}
++
++static void timbuart_mctrl_check(struct uart_port *port, u8 isr, u8 *ier)
++{
++ unsigned int cts;
++
++ if (isr & CTS_DELTA) {
++ /* ack */
++ iowrite8(CTS_DELTA, port->membase + TIMBUART_ISR);
++ cts = timbuart_get_mctrl(port);
++ uart_handle_cts_change(port, cts & TIOCM_CTS);
++ wake_up_interruptible(&port->info->delta_msr_wait);
++ }
++
++ *ier |= CTS_DELTA;
++}
++
++static void timbuart_enable_ms(struct uart_port *port)
++{
++ /* N/A */
++}
++
++static void timbuart_break_ctl(struct uart_port *port, int ctl)
++{
++ /* N/A */
++}
++
++static int timbuart_startup(struct uart_port *port)
++{
++ struct timbuart_port *uart =
++ container_of(port, struct timbuart_port, port);
++
++ dev_dbg(port->dev, "%s\n", __func__);
++
++ iowrite8(TIMBUART_CTRL_FLSHRX, port->membase + TIMBUART_CTRL);
++ iowrite8(0xff, port->membase + TIMBUART_ISR);
++ /* Enable all but TX interrupts */
++ iowrite8(RXBAF | RXBF | RXTT | CTS_DELTA,
++ port->membase + TIMBUART_IER);
++
++ return request_irq(port->irq, timbuart_handleinterrupt, IRQF_SHARED,
++ "timb-uart", uart);
++}
++
++static void timbuart_shutdown(struct uart_port *port)
++{
++ struct timbuart_port *uart =
++ container_of(port, struct timbuart_port, port);
++ dev_dbg(port->dev, "%s\n", __func__);
++ free_irq(port->irq, uart);
++ iowrite8(0, port->membase + TIMBUART_IER);
++}
++
++static int get_bindex(int baud)
++{
++ int i;
++
++ for (i = 0; i < ARRAY_SIZE(baudrates); i++)
++ if (baud == baudrates[i])
++ return i;
++
++ return -1;
++}
++
++static void timbuart_set_termios(struct uart_port *port,
++ struct ktermios *termios,
++ struct ktermios *old)
++{
++ unsigned int baud;
++ short bindex;
++ unsigned long flags;
++
++ baud = uart_get_baud_rate(port, termios, old, 0, port->uartclk / 16);
++ bindex = get_bindex(baud);
++ dev_dbg(port->dev, "%s - bindex %d\n", __func__, bindex);
++
++ if (bindex < 0) {
++ printk(KERN_ALERT "timbuart: Unsupported baud rate\n");
++ } else {
++ spin_lock_irqsave(&port->lock, flags);
++ iowrite8((u8)bindex, port->membase + TIMBUART_BAUDRATE);
++ uart_update_timeout(port, termios->c_cflag, baud);
++ spin_unlock_irqrestore(&port->lock, flags);
++ }
++}
++
++static const char *timbuart_type(struct uart_port *port)
++{
++ return port->type == PORT_UNKNOWN ? "timbuart" : NULL;
++}
++
++/* We do not request/release mappings of the registers here,
++ * currently it's done in the proble function.
++ */
++static void timbuart_release_port(struct uart_port *port)
++{
++ struct platform_device *pdev = to_platform_device(port->dev);
++ int size =
++ resource_size(platform_get_resource(pdev, IORESOURCE_MEM, 0));
++
++ if (port->flags & UPF_IOREMAP) {
++ iounmap(port->membase);
++ port->membase = NULL;
++ }
++
++ release_mem_region(port->mapbase, size);
++}
++
++static int timbuart_request_port(struct uart_port *port)
++{
++ struct platform_device *pdev = to_platform_device(port->dev);
++ int size =
++ resource_size(platform_get_resource(pdev, IORESOURCE_MEM, 0));
++
++ if (!request_mem_region(port->mapbase, size, "timb-uart"))
++ return -EBUSY;
++
++ if (port->flags & UPF_IOREMAP) {
++ port->membase = ioremap(port->mapbase, size);
++ if (port->membase == NULL) {
++ release_mem_region(port->mapbase, size);
++ return -ENOMEM;
++ }
++ }
++
++ return 0;
++}
++
++static irqreturn_t timbuart_handleinterrupt(int irq, void *devid)
++{
++ struct timbuart_port *uart = (struct timbuart_port *)devid;
++
++ uart->last_ier = ioread8(uart->port.membase + TIMBUART_IER);
++
++ /* disable interrupts, let the tasklet enable them again if needed */
++ iowrite8(0, uart->port.membase + TIMBUART_IER);
++
++ /* fire off bottom half */
++ tasklet_schedule(&uart->tasklet);
++
++ return IRQ_HANDLED;
++}
++
++/*
++ * Configure/autoconfigure the port.
++ */
++static void timbuart_config_port(struct uart_port *port, int flags)
++{
++ if (flags & UART_CONFIG_TYPE) {
++ port->type = PORT_TIMBUART;
++ timbuart_request_port(port);
++ }
++}
++
++static int timbuart_verify_port(struct uart_port *port,
++ struct serial_struct *ser)
++{
++ /* we don't want the core code to modify any port params */
++ return -EINVAL;
++}
++
++static struct uart_ops timbuart_ops = {
++ .tx_empty = timbuart_tx_empty,
++ .set_mctrl = timbuart_set_mctrl,
++ .get_mctrl = timbuart_get_mctrl,
++ .stop_tx = timbuart_stop_tx,
++ .start_tx = timbuart_start_tx,
++ .flush_buffer = timbuart_flush_buffer,
++ .stop_rx = timbuart_stop_rx,
++ .enable_ms = timbuart_enable_ms,
++ .break_ctl = timbuart_break_ctl,
++ .startup = timbuart_startup,
++ .shutdown = timbuart_shutdown,
++ .set_termios = timbuart_set_termios,
++ .type = timbuart_type,
++ .release_port = timbuart_release_port,
++ .request_port = timbuart_request_port,
++ .config_port = timbuart_config_port,
++ .verify_port = timbuart_verify_port
++};
++
++static struct uart_driver timbuart_driver = {
++ .owner = THIS_MODULE,
++ .driver_name = "timberdale_uart",
++ .dev_name = "ttyTU",
++ .major = TIMBUART_MAJOR,
++ .minor = TIMBUART_MINOR,
++ .nr = 1
++};
++
++static int timbuart_probe(struct platform_device *dev)
++{
++ int err;
++ struct timbuart_port *uart;
++ struct resource *iomem;
++
++ dev_dbg(&dev->dev, "%s\n", __func__);
++
++ uart = kzalloc(sizeof(*uart), GFP_KERNEL);
++ if (!uart) {
++ err = -EINVAL;
++ goto err_mem;
++ }
++
++ uart->usedma = 0;
++
++ uart->port.uartclk = 3250000 * 16;
++ uart->port.fifosize = TIMBUART_FIFO_SIZE;
++ uart->port.regshift = 2;
++ uart->port.iotype = UPIO_MEM;
++ uart->port.ops = &timbuart_ops;
++ uart->port.irq = 0;
++ uart->port.flags = UPF_BOOT_AUTOCONF | UPF_IOREMAP;
++ uart->port.line = 0;
++ uart->port.dev = &dev->dev;
++
++ iomem = platform_get_resource(dev, IORESOURCE_MEM, 0);
++ if (!iomem) {
++ err = -ENOMEM;
++ goto err_register;
++ }
++ uart->port.mapbase = iomem->start;
++ uart->port.membase = NULL;
++
++ uart->port.irq = platform_get_irq(dev, 0);
++ if (uart->port.irq < 0) {
++ err = -EINVAL;
++ goto err_register;
++ }
++
++ tasklet_init(&uart->tasklet, timbuart_tasklet, (unsigned long)uart);
++
++ err = uart_register_driver(&timbuart_driver);
++ if (err)
++ goto err_register;
++
++ err = uart_add_one_port(&timbuart_driver, &uart->port);
++ if (err)
++ goto err_add_port;
++
++ platform_set_drvdata(dev, uart);
++
++ return 0;
++
++err_add_port:
++ uart_unregister_driver(&timbuart_driver);
++err_register:
++ kfree(uart);
++err_mem:
++ printk(KERN_ERR "timberdale: Failed to register Timberdale UART: %d\n",
++ err);
++
++ return err;
++}
++
++static int timbuart_remove(struct platform_device *dev)
++{
++ struct timbuart_port *uart = platform_get_drvdata(dev);
++
++ tasklet_kill(&uart->tasklet);
++ uart_remove_one_port(&timbuart_driver, &uart->port);
++ uart_unregister_driver(&timbuart_driver);
++ kfree(uart);
++
++ return 0;
++}
++
++static struct platform_driver timbuart_platform_driver = {
++ .driver = {
++ .name = "timb-uart",
++ .owner = THIS_MODULE,
++ },
++ .probe = timbuart_probe,
++ .remove = timbuart_remove,
++};
++
++/*--------------------------------------------------------------------------*/
++
++static int __init timbuart_init(void)
++{
++ return platform_driver_register(&timbuart_platform_driver);
++}
++
++static void __exit timbuart_exit(void)
++{
++ platform_driver_unregister(&timbuart_platform_driver);
++}
++
++module_init(timbuart_init);
++module_exit(timbuart_exit);
++
++MODULE_DESCRIPTION("Timberdale UART driver");
++MODULE_LICENSE("GPL v2");
++MODULE_ALIAS("platform:timb-uart");
++
+diff -uNr linux-2.6.29-clean/drivers/serial/timbuart.h linux-2.6.29/drivers/serial/timbuart.h
+--- linux-2.6.29-clean/drivers/serial/timbuart.h 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/serial/timbuart.h 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,57 @@
++/*
++ * timbuart.c timberdale FPGA GPIO driver
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Timberdale FPGA UART
++ */
++
++#ifndef _TIMBUART_H
++#define _TIMBUART_H
++
++#define TIMBUART_FIFO_SIZE 2048
++
++#define TIMBUART_RXFIFO 0x08
++#define TIMBUART_TXFIFO 0x0c
++#define TIMBUART_IER 0x10
++#define TIMBUART_IPR 0x14
++#define TIMBUART_ISR 0x18
++#define TIMBUART_CTRL 0x1c
++#define TIMBUART_BAUDRATE 0x20
++
++#define TIMBUART_CTRL_RTS 0x01
++#define TIMBUART_CTRL_CTS 0x02
++#define TIMBUART_CTRL_FLSHTX 0x40
++#define TIMBUART_CTRL_FLSHRX 0x80
++
++#define TXBF 0x01
++#define TXBAE 0x02
++#define CTS_DELTA 0x04
++#define RXDP 0x08
++#define RXBAF 0x10
++#define RXBF 0x20
++#define RXTT 0x40
++#define RXBNAE 0x80
++
++#define RXFLAGS (RXDP | RXBAF | RXBF | RXTT | RXBNAE)
++#define TXFLAGS (TXBF | TXBAE)
++
++#define TIMBUART_MAJOR 204
++#define TIMBUART_MINOR 192
++
++#endif /* _TIMBUART_H */
++
+diff -uNr linux-2.6.29-clean/drivers/spi/Kconfig linux-2.6.29/drivers/spi/Kconfig
+--- linux-2.6.29-clean/drivers/spi/Kconfig 2009-04-01 09:20:25.000000000 -0700
++++ linux-2.6.29/drivers/spi/Kconfig 2009-04-06 13:51:47.000000000 -0700
+@@ -211,8 +211,8 @@
+ SPI driver for Toshiba TXx9 MIPS SoCs
+
+ config SPI_XILINX
+- tristate "Xilinx SPI controller"
+- depends on XILINX_VIRTEX && EXPERIMENTAL
++ tristate "Xilinx SPI controller common module"
++ depends on EXPERIMENTAL
+ select SPI_BITBANG
+ help
+ This exposes the SPI controller IP from the Xilinx EDK.
+@@ -220,6 +220,25 @@
+ See the "OPB Serial Peripheral Interface (SPI) (v1.00e)"
+ Product Specification document (DS464) for hardware details.
+
++config SPI_XILINX_OF
++ tristate "Xilinx SPI controller OF device"
++ depends on SPI_XILINX && XILINX_VIRTEX
++ help
++ This exposes the SPI controller IP from the Xilinx EDK.
++
++ See the "OPB Serial Peripheral Interface (SPI) (v1.00e)"
++ Product Specification document (DS464) for hardware details.
++
++config SPI_XILINX_PLTFM
++ tristate "Xilinx SPI controller platform device"
++ depends on SPI_XILINX
++ help
++ This exposes the SPI controller IP from the Xilinx EDK.
++
++ See the "OPB Serial Peripheral Interface (SPI) (v1.00e)"
++ Product Specification document (DS464) for hardware details.
++
++
+ #
+ # Add new SPI master controllers in alphabetical order above this line
+ #
+diff -uNr linux-2.6.29-clean/drivers/spi/Makefile linux-2.6.29/drivers/spi/Makefile
+--- linux-2.6.29-clean/drivers/spi/Makefile 2009-04-01 09:20:25.000000000 -0700
++++ linux-2.6.29/drivers/spi/Makefile 2009-04-06 13:51:47.000000000 -0700
+@@ -29,6 +29,8 @@
+ obj-$(CONFIG_SPI_S3C24XX) += spi_s3c24xx.o
+ obj-$(CONFIG_SPI_TXX9) += spi_txx9.o
+ obj-$(CONFIG_SPI_XILINX) += xilinx_spi.o
++obj-$(CONFIG_SPI_XILINX_OF) += xilinx_spi_of.o
++obj-$(CONFIG_SPI_XILINX_PLTFM) += xilinx_spi_pltfm.o
+ obj-$(CONFIG_SPI_SH_SCI) += spi_sh_sci.o
+ # ... add above this line ...
+
+diff -uNr linux-2.6.29-clean/drivers/spi/xilinx_spi.c linux-2.6.29/drivers/spi/xilinx_spi.c
+--- linux-2.6.29-clean/drivers/spi/xilinx_spi.c 2009-04-01 09:20:25.000000000 -0700
++++ linux-2.6.29/drivers/spi/xilinx_spi.c 2009-04-06 13:51:47.000000000 -0700
+@@ -14,22 +14,28 @@
+ #include <linux/module.h>
+ #include <linux/init.h>
+ #include <linux/interrupt.h>
+-#include <linux/platform_device.h>
+-
+-#include <linux/of_platform.h>
+-#include <linux/of_device.h>
+-#include <linux/of_spi.h>
+
+ #include <linux/spi/spi.h>
+ #include <linux/spi/spi_bitbang.h>
+ #include <linux/io.h>
+
+-#define XILINX_SPI_NAME "xilinx_spi"
++#include "xilinx_spi.h"
++
++#ifndef CONFIG_PPC
++#define in_8(addr) ioread8(addr)
++#define in_be16(addr) ioread16(addr)
++#define in_be32(addr) ioread32(addr)
++
++#define out_8(addr, b) iowrite8(b, addr)
++#define out_be16(addr, w) iowrite16(w, addr)
++#define out_be32(addr, l) iowrite32(l, addr)
++#endif
++
+
+ /* Register definitions as per "OPB Serial Peripheral Interface (SPI) (v1.00e)
+ * Product Specification", DS464
+ */
+-#define XSPI_CR_OFFSET 0x62 /* 16-bit Control Register */
++#define XSPI_CR_OFFSET_DEF 0x62 /* 16-bit Control Register */
+
+ #define XSPI_CR_ENABLE 0x02
+ #define XSPI_CR_MASTER_MODE 0x04
+@@ -41,7 +47,7 @@
+ #define XSPI_CR_MANUAL_SSELECT 0x80
+ #define XSPI_CR_TRANS_INHIBIT 0x100
+
+-#define XSPI_SR_OFFSET 0x67 /* 8-bit Status Register */
++#define XSPI_SR_OFFSET_DEF 0x67 /* 8-bit Status Register */
+
+ #define XSPI_SR_RX_EMPTY_MASK 0x01 /* Receive FIFO is empty */
+ #define XSPI_SR_RX_FULL_MASK 0x02 /* Receive FIFO is full */
+@@ -49,10 +55,10 @@
+ #define XSPI_SR_TX_FULL_MASK 0x08 /* Transmit FIFO is full */
+ #define XSPI_SR_MODE_FAULT_MASK 0x10 /* Mode fault error */
+
+-#define XSPI_TXD_OFFSET 0x6b /* 8-bit Data Transmit Register */
+-#define XSPI_RXD_OFFSET 0x6f /* 8-bit Data Receive Register */
++#define XSPI_TXD_OFFSET_DEF 0x6b /* 8-bit Data Transmit Register */
++#define XSPI_RXD_OFFSET_DEF 0x6f /* 8-bit Data Receive Register */
+
+-#define XSPI_SSR_OFFSET 0x70 /* 32-bit Slave Select Register */
++#define XSPI_SSR_OFFSET_DEF 0x70 /* 32-bit Slave Select Register */
+
+ /* Register definitions as per "OPB IPIF (v3.01c) Product Specification", DS414
+ * IPIF registers are 32 bit
+@@ -74,24 +80,10 @@
+ #define XIPIF_V123B_RESETR_OFFSET 0x40 /* IPIF reset register */
+ #define XIPIF_V123B_RESET_MASK 0x0a /* the value to write */
+
+-struct xilinx_spi {
+- /* bitbang has to be first */
+- struct spi_bitbang bitbang;
+- struct completion done;
+-
+- void __iomem *regs; /* virt. address of the control registers */
+-
+- u32 irq;
+-
+- u32 speed_hz; /* SCK has a fixed frequency of speed_hz Hz */
+-
+- u8 *rx_ptr; /* pointer in the Tx buffer */
+- const u8 *tx_ptr; /* pointer in the Rx buffer */
+- int remaining_bytes; /* the number of bytes left to transfer */
+-};
+
+-static void xspi_init_hw(void __iomem *regs_base)
++void xspi_init_hw(struct xilinx_spi *xspi)
+ {
++ void __iomem *regs_base = xspi->regs;
+ /* Reset the SPI device */
+ out_be32(regs_base + XIPIF_V123B_RESETR_OFFSET,
+ XIPIF_V123B_RESET_MASK);
+@@ -101,30 +93,31 @@
+ out_be32(regs_base + XIPIF_V123B_DGIER_OFFSET,
+ XIPIF_V123B_GINTR_ENABLE);
+ /* Deselect the slave on the SPI bus */
+- out_be32(regs_base + XSPI_SSR_OFFSET, 0xffff);
++ out_be32(regs_base + xspi->ssr_offset, 0xffff);
+ /* Disable the transmitter, enable Manual Slave Select Assertion,
+ * put SPI controller into master mode, and enable it */
+- out_be16(regs_base + XSPI_CR_OFFSET,
++ out_be16(regs_base + xspi->cr_offset,
+ XSPI_CR_TRANS_INHIBIT | XSPI_CR_MANUAL_SSELECT
+ | XSPI_CR_MASTER_MODE | XSPI_CR_ENABLE);
+ }
++EXPORT_SYMBOL(xspi_init_hw);
+
+-static void xilinx_spi_chipselect(struct spi_device *spi, int is_on)
++void xilinx_spi_chipselect(struct spi_device *spi, int is_on)
+ {
+ struct xilinx_spi *xspi = spi_master_get_devdata(spi->master);
+
+ if (is_on == BITBANG_CS_INACTIVE) {
+ /* Deselect the slave on the SPI bus */
+- out_be32(xspi->regs + XSPI_SSR_OFFSET, 0xffff);
++ out_be32(xspi->regs + xspi->ssr_offset, 0xffff);
+ } else if (is_on == BITBANG_CS_ACTIVE) {
+ /* Set the SPI clock phase and polarity */
+- u16 cr = in_be16(xspi->regs + XSPI_CR_OFFSET)
++ u16 cr = in_be16(xspi->regs + xspi->cr_offset)
+ & ~XSPI_CR_MODE_MASK;
+ if (spi->mode & SPI_CPHA)
+ cr |= XSPI_CR_CPHA;
+ if (spi->mode & SPI_CPOL)
+ cr |= XSPI_CR_CPOL;
+- out_be16(xspi->regs + XSPI_CR_OFFSET, cr);
++ out_be16(xspi->regs + xspi->cr_offset, cr);
+
+ /* We do not check spi->max_speed_hz here as the SPI clock
+ * frequency is not software programmable (the IP block design
+@@ -132,10 +125,11 @@
+ */
+
+ /* Activate the chip select */
+- out_be32(xspi->regs + XSPI_SSR_OFFSET,
++ out_be32(xspi->regs + xspi->ssr_offset,
+ ~(0x0001 << spi->chip_select));
+ }
+ }
++EXPORT_SYMBOL(xilinx_spi_chipselect);
+
+ /* spi_bitbang requires custom setup_transfer() to be defined if there is a
+ * custom txrx_bufs(). We have nothing to setup here as the SPI IP block
+@@ -143,8 +137,7 @@
+ * Check for 8 bits per word. Chip select delay calculations could be
+ * added here as soon as bitbang_work() can be made aware of the delay value.
+ */
+-static int xilinx_spi_setup_transfer(struct spi_device *spi,
+- struct spi_transfer *t)
++int xilinx_spi_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
+ {
+ u8 bits_per_word;
+
+@@ -157,11 +150,12 @@
+
+ return 0;
+ }
++EXPORT_SYMBOL(xilinx_spi_setup_transfer);
+
+ /* the spi->mode bits understood by this driver: */
+ #define MODEBITS (SPI_CPOL | SPI_CPHA)
+
+-static int xilinx_spi_setup(struct spi_device *spi)
++int xilinx_spi_setup(struct spi_device *spi)
+ {
+ struct spi_bitbang *bitbang;
+ struct xilinx_spi *xspi;
+@@ -188,25 +182,25 @@
+
+ return 0;
+ }
++EXPORT_SYMBOL(xilinx_spi_setup);
+
+ static void xilinx_spi_fill_tx_fifo(struct xilinx_spi *xspi)
+ {
+ u8 sr;
+
+ /* Fill the Tx FIFO with as many bytes as possible */
+- sr = in_8(xspi->regs + XSPI_SR_OFFSET);
++ sr = in_8(xspi->regs + xspi->sr_offset);
+ while ((sr & XSPI_SR_TX_FULL_MASK) == 0 && xspi->remaining_bytes > 0) {
+- if (xspi->tx_ptr) {
+- out_8(xspi->regs + XSPI_TXD_OFFSET, *xspi->tx_ptr++);
+- } else {
+- out_8(xspi->regs + XSPI_TXD_OFFSET, 0);
+- }
++ if (xspi->tx_ptr)
++ out_8(xspi->regs + xspi->txd_offset, *xspi->tx_ptr++);
++ else
++ out_8(xspi->regs + xspi->txd_offset, 0);
+ xspi->remaining_bytes--;
+- sr = in_8(xspi->regs + XSPI_SR_OFFSET);
++ sr = in_8(xspi->regs + xspi->sr_offset);
+ }
+ }
+
+-static int xilinx_spi_txrx_bufs(struct spi_device *spi, struct spi_transfer *t)
++int xilinx_spi_txrx_bufs(struct spi_device *spi, struct spi_transfer *t)
+ {
+ struct xilinx_spi *xspi = spi_master_get_devdata(spi->master);
+ u32 ipif_ier;
+@@ -229,8 +223,8 @@
+ ipif_ier | XSPI_INTR_TX_EMPTY);
+
+ /* Start the transfer by not inhibiting the transmitter any longer */
+- cr = in_be16(xspi->regs + XSPI_CR_OFFSET) & ~XSPI_CR_TRANS_INHIBIT;
+- out_be16(xspi->regs + XSPI_CR_OFFSET, cr);
++ cr = in_be16(xspi->regs + xspi->cr_offset) & ~XSPI_CR_TRANS_INHIBIT;
++ out_be16(xspi->regs + xspi->cr_offset, cr);
+
+ wait_for_completion(&xspi->done);
+
+@@ -239,14 +233,14 @@
+
+ return t->len - xspi->remaining_bytes;
+ }
+-
++EXPORT_SYMBOL(xilinx_spi_txrx_bufs);
+
+ /* This driver supports single master mode only. Hence Tx FIFO Empty
+ * is the only interrupt we care about.
+ * Receive FIFO Overrun, Transmit FIFO Underrun, Mode Fault, and Slave Mode
+ * Fault are not to happen.
+ */
+-static irqreturn_t xilinx_spi_irq(int irq, void *dev_id)
++irqreturn_t xilinx_spi_irq(int irq, void *dev_id)
+ {
+ struct xilinx_spi *xspi = dev_id;
+ u32 ipif_isr;
+@@ -264,20 +258,19 @@
+ * transmitter while the Isr refills the transmit register/FIFO,
+ * or make sure it is stopped if we're done.
+ */
+- cr = in_be16(xspi->regs + XSPI_CR_OFFSET);
+- out_be16(xspi->regs + XSPI_CR_OFFSET,
++ cr = in_be16(xspi->regs + xspi->cr_offset);
++ out_be16(xspi->regs + xspi->cr_offset,
+ cr | XSPI_CR_TRANS_INHIBIT);
+
+ /* Read out all the data from the Rx FIFO */
+- sr = in_8(xspi->regs + XSPI_SR_OFFSET);
++ sr = in_8(xspi->regs + xspi->sr_offset);
+ while ((sr & XSPI_SR_RX_EMPTY_MASK) == 0) {
+ u8 data;
+
+- data = in_8(xspi->regs + XSPI_RXD_OFFSET);
+- if (xspi->rx_ptr) {
++ data = in_8(xspi->regs + xspi->rxd_offset);
++ if (xspi->rx_ptr)
+ *xspi->rx_ptr++ = data;
+- }
+- sr = in_8(xspi->regs + XSPI_SR_OFFSET);
++ sr = in_8(xspi->regs + xspi->sr_offset);
+ }
+
+ /* See if there is more data to send */
+@@ -286,7 +279,7 @@
+ /* Start the transfer by not inhibiting the
+ * transmitter any longer
+ */
+- out_be16(xspi->regs + XSPI_CR_OFFSET, cr);
++ out_be16(xspi->regs + xspi->cr_offset, cr);
+ } else {
+ /* No more data to send.
+ * Indicate the transfer is completed.
+@@ -297,167 +290,18 @@
+
+ return IRQ_HANDLED;
+ }
++EXPORT_SYMBOL(xilinx_spi_irq);
+
+-static int __init xilinx_spi_of_probe(struct of_device *ofdev,
+- const struct of_device_id *match)
+-{
+- struct spi_master *master;
+- struct xilinx_spi *xspi;
+- struct resource r_irq_struct;
+- struct resource r_mem_struct;
+-
+- struct resource *r_irq = &r_irq_struct;
+- struct resource *r_mem = &r_mem_struct;
+- int rc = 0;
+- const u32 *prop;
+- int len;
+-
+- /* Get resources(memory, IRQ) associated with the device */
+- master = spi_alloc_master(&ofdev->dev, sizeof(struct xilinx_spi));
+-
+- if (master == NULL) {
+- return -ENOMEM;
+- }
+-
+- dev_set_drvdata(&ofdev->dev, master);
+-
+- rc = of_address_to_resource(ofdev->node, 0, r_mem);
+- if (rc) {
+- dev_warn(&ofdev->dev, "invalid address\n");
+- goto put_master;
+- }
+-
+- rc = of_irq_to_resource(ofdev->node, 0, r_irq);
+- if (rc == NO_IRQ) {
+- dev_warn(&ofdev->dev, "no IRQ found\n");
+- goto put_master;
+- }
+-
+- xspi = spi_master_get_devdata(master);
+- xspi->bitbang.master = spi_master_get(master);
+- xspi->bitbang.chipselect = xilinx_spi_chipselect;
+- xspi->bitbang.setup_transfer = xilinx_spi_setup_transfer;
+- xspi->bitbang.txrx_bufs = xilinx_spi_txrx_bufs;
+- xspi->bitbang.master->setup = xilinx_spi_setup;
+- init_completion(&xspi->done);
+-
+- xspi->irq = r_irq->start;
+-
+- if (!request_mem_region(r_mem->start,
+- r_mem->end - r_mem->start + 1, XILINX_SPI_NAME)) {
+- rc = -ENXIO;
+- dev_warn(&ofdev->dev, "memory request failure\n");
+- goto put_master;
+- }
+-
+- xspi->regs = ioremap(r_mem->start, r_mem->end - r_mem->start + 1);
+- if (xspi->regs == NULL) {
+- rc = -ENOMEM;
+- dev_warn(&ofdev->dev, "ioremap failure\n");
+- goto put_master;
+- }
+- xspi->irq = r_irq->start;
+-
+- /* dynamic bus assignment */
+- master->bus_num = -1;
+-
+- /* number of slave select bits is required */
+- prop = of_get_property(ofdev->node, "xlnx,num-ss-bits", &len);
+- if (!prop || len < sizeof(*prop)) {
+- dev_warn(&ofdev->dev, "no 'xlnx,num-ss-bits' property\n");
+- goto put_master;
+- }
+- master->num_chipselect = *prop;
+-
+- /* SPI controller initializations */
+- xspi_init_hw(xspi->regs);
+-
+- /* Register for SPI Interrupt */
+- rc = request_irq(xspi->irq, xilinx_spi_irq, 0, XILINX_SPI_NAME, xspi);
+- if (rc != 0) {
+- dev_warn(&ofdev->dev, "irq request failure: %d\n", xspi->irq);
+- goto unmap_io;
+- }
+-
+- rc = spi_bitbang_start(&xspi->bitbang);
+- if (rc != 0) {
+- dev_err(&ofdev->dev, "spi_bitbang_start FAILED\n");
+- goto free_irq;
+- }
+-
+- dev_info(&ofdev->dev, "at 0x%08X mapped to 0x%08X, irq=%d\n",
+- (unsigned int)r_mem->start, (u32)xspi->regs, xspi->irq);
+-
+- /* Add any subnodes on the SPI bus */
+- of_register_spi_devices(master, ofdev->node);
+-
+- return rc;
+-
+-free_irq:
+- free_irq(xspi->irq, xspi);
+-unmap_io:
+- iounmap(xspi->regs);
+-put_master:
+- spi_master_put(master);
+- return rc;
+-}
+-
+-static int __devexit xilinx_spi_remove(struct of_device *ofdev)
++void xilinx_spi_set_default_reg_offsets(struct xilinx_spi *xspi)
+ {
+- struct xilinx_spi *xspi;
+- struct spi_master *master;
+-
+- master = platform_get_drvdata(ofdev);
+- xspi = spi_master_get_devdata(master);
+-
+- spi_bitbang_stop(&xspi->bitbang);
+- free_irq(xspi->irq, xspi);
+- iounmap(xspi->regs);
+- dev_set_drvdata(&ofdev->dev, 0);
+- spi_master_put(xspi->bitbang.master);
+-
+- return 0;
+-}
+-
+-/* work with hotplug and coldplug */
+-MODULE_ALIAS("platform:" XILINX_SPI_NAME);
+-
+-static int __exit xilinx_spi_of_remove(struct of_device *op)
+-{
+- return xilinx_spi_remove(op);
++ xspi->cr_offset = XSPI_CR_OFFSET_DEF;
++ xspi->sr_offset = XSPI_SR_OFFSET_DEF;
++ xspi->txd_offset = XSPI_TXD_OFFSET_DEF;
++ xspi->rxd_offset = XSPI_RXD_OFFSET_DEF;
++ xspi->ssr_offset = XSPI_SSR_OFFSET_DEF;
+ }
++EXPORT_SYMBOL(xilinx_spi_set_default_reg_offsets);
+
+-static struct of_device_id xilinx_spi_of_match[] = {
+- { .compatible = "xlnx,xps-spi-2.00.a", },
+- { .compatible = "xlnx,xps-spi-2.00.b", },
+- {}
+-};
+-
+-MODULE_DEVICE_TABLE(of, xilinx_spi_of_match);
+-
+-static struct of_platform_driver xilinx_spi_of_driver = {
+- .owner = THIS_MODULE,
+- .name = "xilinx-xps-spi",
+- .match_table = xilinx_spi_of_match,
+- .probe = xilinx_spi_of_probe,
+- .remove = __exit_p(xilinx_spi_of_remove),
+- .driver = {
+- .name = "xilinx-xps-spi",
+- .owner = THIS_MODULE,
+- },
+-};
+-
+-static int __init xilinx_spi_init(void)
+-{
+- return of_register_platform_driver(&xilinx_spi_of_driver);
+-}
+-module_init(xilinx_spi_init);
+-
+-static void __exit xilinx_spi_exit(void)
+-{
+- of_unregister_platform_driver(&xilinx_spi_of_driver);
+-}
+-module_exit(xilinx_spi_exit);
+ MODULE_AUTHOR("MontaVista Software, Inc. <source@mvista.com>");
+ MODULE_DESCRIPTION("Xilinx SPI driver");
+ MODULE_LICENSE("GPL");
+diff -uNr linux-2.6.29-clean/drivers/spi/xilinx_spi.h linux-2.6.29/drivers/spi/xilinx_spi.h
+--- linux-2.6.29-clean/drivers/spi/xilinx_spi.h 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/spi/xilinx_spi.h 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,52 @@
++/*
++ * xilinx_spi.c
++ *
++ * Xilinx SPI controller driver (master mode only)
++ *
++ * Author: MontaVista Software, Inc.
++ * source@mvista.com
++ *
++ * 2002-2007 (c) MontaVista Software, Inc. This file is licensed under the
++ * terms of the GNU General Public License version 2. This program is licensed
++ * "as is" without any warranty of any kind, whether express or implied.
++ */
++
++#ifndef _XILINX_SPI_H_
++#define _XILINX_SPI_H_ 1
++
++#include <linux/spi/spi.h>
++#include <linux/spi/spi_bitbang.h>
++
++#define XILINX_SPI_NAME "xilinx_spi"
++
++
++struct xilinx_spi {
++ /* bitbang has to be first */
++ struct spi_bitbang bitbang;
++ struct completion done;
++
++ void __iomem *regs; /* virt. address of the control registers */
++
++ u32 irq;
++
++ u32 speed_hz; /* SCK has a fixed frequency of speed_hz Hz */
++
++ u8 *rx_ptr; /* pointer in the Tx buffer */
++ const u8 *tx_ptr; /* pointer in the Rx buffer */
++ int remaining_bytes; /* the number of bytes left to transfer */
++ /* offset to the XSPI regs, these might vary... */
++ u8 cr_offset;
++ u8 sr_offset;
++ u8 txd_offset;
++ u8 rxd_offset;
++ u8 ssr_offset;
++};
++
++void xspi_init_hw(struct xilinx_spi *xspi);
++void xilinx_spi_set_default_reg_offsets(struct xilinx_spi *xspi);
++void xilinx_spi_chipselect(struct spi_device *spi, int is_on);
++int xilinx_spi_setup_transfer(struct spi_device *spi, struct spi_transfer *t);
++int xilinx_spi_setup(struct spi_device *spi);
++int xilinx_spi_txrx_bufs(struct spi_device *spi, struct spi_transfer *t);
++irqreturn_t xilinx_spi_irq(int irq, void *dev_id);
++#endif
+diff -uNr linux-2.6.29-clean/drivers/spi/xilinx_spi_of.c linux-2.6.29/drivers/spi/xilinx_spi_of.c
+--- linux-2.6.29-clean/drivers/spi/xilinx_spi_of.c 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/spi/xilinx_spi_of.c 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,193 @@
++/*
++ * xilinx_spi.c
++ *
++ * Xilinx SPI controller driver (master mode only)
++ *
++ * Author: MontaVista Software, Inc.
++ * source@mvista.com
++ *
++ * 2002-2007 (c) MontaVista Software, Inc. This file is licensed under the
++ * terms of the GNU General Public License version 2. This program is licensed
++ * "as is" without any warranty of any kind, whether express or implied.
++ */
++
++#include <linux/module.h>
++#include <linux/init.h>
++#include <linux/interrupt.h>
++#include <linux/io.h>
++#include <linux/platform_device.h>
++
++#include <linux/of_platform.h>
++#include <linux/of_device.h>
++#include <linux/of_spi.h>
++
++#include <linux/spi/spi.h>
++#include <linux/spi/spi_bitbang.h>
++
++#include "xilinx_spi.h"
++
++
++static int __init xilinx_spi_of_probe(struct of_device *ofdev,
++ const struct of_device_id *match)
++{
++ struct spi_master *master;
++ struct xilinx_spi *xspi;
++ struct resource r_irq_struct;
++ struct resource r_mem_struct;
++
++ struct resource *r_irq = &r_irq_struct;
++ struct resource *r_mem = &r_mem_struct;
++ int rc = 0;
++ const u32 *prop;
++ int len;
++
++ /* Get resources(memory, IRQ) associated with the device */
++ master = spi_alloc_master(&ofdev->dev, sizeof(struct xilinx_spi));
++
++ if (master == NULL)
++ return -ENOMEM;
++
++ dev_set_drvdata(&ofdev->dev, master);
++
++ rc = of_address_to_resource(ofdev->node, 0, r_mem);
++ if (rc) {
++ dev_warn(&ofdev->dev, "invalid address\n");
++ goto put_master;
++ }
++
++ rc = of_irq_to_resource(ofdev->node, 0, r_irq);
++ if (rc == NO_IRQ) {
++ dev_warn(&ofdev->dev, "no IRQ found\n");
++ goto put_master;
++ }
++
++ xspi = spi_master_get_devdata(master);
++ xspi->bitbang.master = spi_master_get(master);
++ xspi->bitbang.chipselect = xilinx_spi_chipselect;
++ xspi->bitbang.setup_transfer = xilinx_spi_setup_transfer;
++ xspi->bitbang.txrx_bufs = xilinx_spi_txrx_bufs;
++ xspi->bitbang.master->setup = xilinx_spi_setup;
++ init_completion(&xspi->done);
++
++ xspi->irq = r_irq->start;
++
++ if (!request_mem_region(r_mem->start,
++ r_mem->end - r_mem->start + 1, XILINX_SPI_NAME)) {
++ rc = -ENXIO;
++ dev_warn(&ofdev->dev, "memory request failure\n");
++ goto put_master;
++ }
++
++ xspi->regs = ioremap(r_mem->start, r_mem->end - r_mem->start + 1);
++ if (xspi->regs == NULL) {
++ rc = -ENOMEM;
++ dev_warn(&ofdev->dev, "ioremap failure\n");
++ goto put_master;
++ }
++ xspi->irq = r_irq->start;
++
++ /* dynamic bus assignment */
++ master->bus_num = -1;
++
++ /* number of slave select bits is required */
++ prop = of_get_property(ofdev->node, "xlnx,num-ss-bits", &len);
++ if (!prop || len < sizeof(*prop)) {
++ dev_warn(&ofdev->dev, "no 'xlnx,num-ss-bits' property\n");
++ goto put_master;
++ }
++ master->num_chipselect = *prop;
++
++ xilinx_spi_set_default_reg_offsets(xspi);
++
++ /* SPI controller initializations */
++ xspi_init_hw(xspi->regs);
++
++ /* Register for SPI Interrupt */
++ rc = request_irq(xspi->irq, xilinx_spi_irq, 0, XILINX_SPI_NAME, xspi);
++ if (rc != 0) {
++ dev_warn(&ofdev->dev, "irq request failure: %d\n", xspi->irq);
++ goto unmap_io;
++ }
++
++ rc = spi_bitbang_start(&xspi->bitbang);
++ if (rc != 0) {
++ dev_err(&ofdev->dev, "spi_bitbang_start FAILED\n");
++ goto free_irq;
++ }
++
++ dev_info(&ofdev->dev, "at 0x%08X mapped to 0x%08X, irq=%d\n",
++ (unsigned int)r_mem->start, (u32)xspi->regs, xspi->irq);
++
++ /* Add any subnodes on the SPI bus */
++ of_register_spi_devices(master, ofdev->node);
++
++ return rc;
++
++free_irq:
++ free_irq(xspi->irq, xspi);
++unmap_io:
++ iounmap(xspi->regs);
++put_master:
++ spi_master_put(master);
++ return rc;
++}
++
++static int __devexit xilinx_spi_remove(struct of_device *ofdev)
++{
++ struct xilinx_spi *xspi;
++ struct spi_master *master;
++
++ master = platform_get_drvdata(ofdev);
++ xspi = spi_master_get_devdata(master);
++
++ spi_bitbang_stop(&xspi->bitbang);
++ free_irq(xspi->irq, xspi);
++ iounmap(xspi->regs);
++ dev_set_drvdata(&ofdev->dev, 0);
++ spi_master_put(xspi->bitbang.master);
++
++ return 0;
++}
++
++/* work with hotplug and coldplug */
++MODULE_ALIAS("platform:" XILINX_SPI_NAME);
++
++static int __exit xilinx_spi_of_remove(struct of_device *op)
++{
++ return xilinx_spi_remove(op);
++}
++
++static struct of_device_id xilinx_spi_of_match[] = {
++ { .compatible = "xlnx,xps-spi-2.00.a", },
++ { .compatible = "xlnx,xps-spi-2.00.b", },
++ {}
++};
++
++MODULE_DEVICE_TABLE(of, xilinx_spi_of_match);
++
++static struct of_platform_driver xilinx_spi_of_driver = {
++ .owner = THIS_MODULE,
++ .name = "xilinx-xps-spi",
++ .match_table = xilinx_spi_of_match,
++ .probe = xilinx_spi_of_probe,
++ .remove = __exit_p(xilinx_spi_of_remove),
++ .driver = {
++ .name = "xilinx-xps-spi",
++ .owner = THIS_MODULE,
++ },
++};
++
++static int __init xilinx_spi_init(void)
++{
++ return of_register_platform_driver(&xilinx_spi_of_driver);
++}
++module_init(xilinx_spi_init);
++
++static void __exit xilinx_spi_exit(void)
++{
++ of_unregister_platform_driver(&xilinx_spi_of_driver);
++}
++module_exit(xilinx_spi_exit);
++MODULE_AUTHOR("MontaVista Software, Inc. <source@mvista.com>");
++MODULE_DESCRIPTION("Xilinx SPI driver");
++MODULE_LICENSE("GPL");
+diff -uNr linux-2.6.29-clean/drivers/spi/xilinx_spi_pltfm.c linux-2.6.29/drivers/spi/xilinx_spi_pltfm.c
+--- linux-2.6.29-clean/drivers/spi/xilinx_spi_pltfm.c 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/drivers/spi/xilinx_spi_pltfm.c 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,184 @@
++/*
++ * xilinx_spi_pltfm.c Support for Xilinx SPI platform devices
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Xilinx SPI devices as platform devices
++ *
++ * Inspired by xilinx_spi.c, 2002-2007 (c) MontaVista Software, Inc.
++ */
++
++#include <linux/module.h>
++#include <linux/init.h>
++#include <linux/interrupt.h>
++#include <linux/io.h>
++#include <linux/platform_device.h>
++
++#include <linux/spi/spi.h>
++#include <linux/spi/spi_bitbang.h>
++#include <linux/spi/xilinx_spi.h>
++
++#include "xilinx_spi.h"
++
++static int __init xilinx_spi_probe(struct platform_device *dev)
++{
++ int ret = 0;
++ struct spi_master *master;
++ struct xilinx_spi *xspi;
++ struct xspi_platform_data *pdata;
++ struct resource *r;
++
++ master = spi_alloc_master(&dev->dev, sizeof(struct xilinx_spi));
++
++ if (master == NULL)
++ return -ENOMEM;
++
++
++ platform_set_drvdata(dev, master);
++ pdata = dev->dev.platform_data;
++ if (pdata == NULL) {
++ ret = -ENODEV;
++ goto put_master;
++ }
++
++ r = platform_get_resource(dev, IORESOURCE_MEM, 0);
++ if (r == NULL) {
++ ret = -ENODEV;
++ goto put_master;
++ }
++
++ xspi = spi_master_get_devdata(master);
++ xspi->bitbang.master = spi_master_get(master);
++ xspi->bitbang.chipselect = xilinx_spi_chipselect;
++ xspi->bitbang.setup_transfer = xilinx_spi_setup_transfer;
++ xspi->bitbang.txrx_bufs = xilinx_spi_txrx_bufs;
++ xspi->bitbang.master->setup = xilinx_spi_setup;
++ init_completion(&xspi->done);
++
++ if (!request_mem_region(r->start, resource_size(r), XILINX_SPI_NAME)) {
++ ret = -ENXIO;
++ goto put_master;
++ }
++
++ xspi->regs = ioremap(r->start, resource_size(r));
++ if (xspi->regs == NULL) {
++ ret = -ENOMEM;
++ goto map_failed;
++ }
++
++ ret = platform_get_irq(dev, 0);
++ if (ret < 0) {
++ ret = -ENXIO;
++ goto unmap_io;
++ }
++ xspi->irq = ret;
++
++ master->bus_num = pdata->bus_num;
++ master->num_chipselect = pdata->num_chipselect;
++ xspi->speed_hz = pdata->speed_hz;
++ xilinx_spi_set_default_reg_offsets(xspi);
++ if (pdata->cr_offset)
++ xspi->cr_offset = pdata->cr_offset;
++ if (pdata->sr_offset)
++ xspi->sr_offset = pdata->sr_offset;
++ if (pdata->txd_offset)
++ xspi->txd_offset = pdata->txd_offset;
++ if (pdata->rxd_offset)
++ xspi->rxd_offset = pdata->rxd_offset;
++ if (pdata->ssr_offset)
++ xspi->ssr_offset = pdata->ssr_offset;
++
++ /* SPI controller initializations */
++ xspi_init_hw(xspi);
++
++ /* Register for SPI Interrupt */
++ ret = request_irq(xspi->irq, xilinx_spi_irq, 0, XILINX_SPI_NAME, xspi);
++ if (ret != 0)
++ goto unmap_io;
++
++ ret = spi_bitbang_start(&xspi->bitbang);
++ if (ret != 0) {
++ dev_err(&dev->dev, "spi_bitbang_start FAILED\n");
++ goto free_irq;
++ }
++
++ dev_info(&dev->dev, "at 0x%08X mapped to 0x%08X, irq=%d\n",
++ (u32)r->start, (u32)xspi->regs, xspi->irq);
++ return ret;
++
++free_irq:
++ free_irq(xspi->irq, xspi);
++unmap_io:
++ iounmap(xspi->regs);
++map_failed:
++ release_mem_region(r->start, resource_size(r));
++put_master:
++ spi_master_put(master);
++ return ret;
++}
++
++static int __devexit xilinx_spi_remove(struct platform_device *dev)
++{
++ struct xilinx_spi *xspi;
++ struct spi_master *master;
++ struct resource *r;
++
++ master = platform_get_drvdata(dev);
++ xspi = spi_master_get_devdata(master);
++ r = platform_get_resource(dev, IORESOURCE_MEM, 0);
++
++ spi_bitbang_stop(&xspi->bitbang);
++ free_irq(xspi->irq, xspi);
++ iounmap(xspi->regs);
++
++ if (r)
++ release_mem_region(r->start, resource_size(r));
++
++ platform_set_drvdata(dev, 0);
++ spi_master_put(xspi->bitbang.master);
++
++ return 0;
++}
++
++/* work with hotplug and coldplug */
++MODULE_ALIAS("platform:" XILINX_SPI_NAME);
++
++static struct platform_driver xilinx_spi_driver = {
++ .probe = xilinx_spi_probe,
++ .remove = __devexit_p(xilinx_spi_remove),
++ .driver = {
++ .name = XILINX_SPI_NAME,
++ .owner = THIS_MODULE,
++ },
++};
++
++static int __init xilinx_spi_init(void)
++{
++ return platform_driver_register(&xilinx_spi_driver);
++}
++module_init(xilinx_spi_init);
++
++static void __exit xilinx_spi_exit(void)
++{
++ platform_driver_unregister(&xilinx_spi_driver);
++}
++module_exit(xilinx_spi_exit);
++
++MODULE_AUTHOR("Mocean Laboratories <info@mocean-labs.com>");
++MODULE_DESCRIPTION("Xilinx SPI platform driver");
++MODULE_LICENSE("GPL v2");
++
+diff -uNr linux-2.6.29-clean/include/linux/i2c-ocores.h linux-2.6.29/include/linux/i2c-ocores.h
+--- linux-2.6.29-clean/include/linux/i2c-ocores.h 2009-04-01 09:20:20.000000000 -0700
++++ linux-2.6.29/include/linux/i2c-ocores.h 2009-04-06 13:51:47.000000000 -0700
+@@ -14,6 +14,8 @@
+ struct ocores_i2c_platform_data {
+ u32 regstep; /* distance between registers */
+ u32 clock_khz; /* input clock in kHz */
++ u8 num_devices; /* number of devices in the devices list */
++ struct i2c_board_info const *devices; /* devices connected to the bus */
+ };
+
+ #endif /* _LINUX_I2C_OCORES_H */
+diff -uNr linux-2.6.29-clean/include/linux/mfd/timbdma.h linux-2.6.29/include/linux/mfd/timbdma.h
+--- linux-2.6.29-clean/include/linux/mfd/timbdma.h 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/include/linux/mfd/timbdma.h 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,80 @@
++/*
++ * timbdma.h timberdale FPGA DMA driver defines
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Timberdale FPGA DMA engine
++ */
++
++#ifndef _TIMBDMA_H
++#define _TIMBDMA_H
++
++#include <linux/spinlock.h>
++
++
++#define DMA_IRQ_UART_RX 0x01
++#define DMA_IRQ_UART_TX 0x02
++#define DMA_IRQ_MLB_RX 0x04
++#define DMA_IRQ_MLB_TX 0x08
++#define DMA_IRQ_VIDEO_RX 0x10
++#define DMA_IRQ_VIDEO_DROP 0x20
++#define DMA_IRQS 6
++
++
++typedef int (*timbdma_interruptcb)(u32 flag, void *data);
++
++enum timbdma_ctrlmap {
++ timbdma_ctrlmap_DMACFGBTUART = 0x000000,
++ timbdma_ctrlmap_DMACFGMLBSY = 0x000040,
++ timbdma_ctrlmap_DMACFGVIDEO = 0x000080,
++ timbdma_ctrlmap_TIMBSTATUS = 0x080000,
++ timbdma_ctrlmap_TIMBPEND = 0x080004,
++ timbdma_ctrlmap_TIMBENABLE = 0x080008,
++ timbdma_ctrlmap_VIDEOBUFFER = 0x200000
++};
++
++enum timbdma_dmacfg {
++ timbdma_dmacfg_RXSTARTH = 0x00,
++ timbdma_dmacfg_RXSTARTL = 0x04,
++ timbdma_dmacfg_RXLENGTH = 0x08,
++ timbdma_dmacfg_RXFPGAWP = 0x0C,
++ timbdma_dmacfg_RXSWRP = 0x10,
++ timbdma_dmacfg_RXENABLE = 0x14,
++ timbdma_dmacfg_TXSTARTH = 0x18,
++ timbdma_dmacfg_TXSTARTL = 0x1C,
++ timbdma_dmacfg_TXLENGTH = 0x20,
++ timbdma_dmacfg_TXSWWP = 0x24,
++ timbdma_dmacfg_TXFPGARP = 0x28,
++ timbdma_dmacfg_TXBEFINT = 0x2C,
++ timbdma_dmacfg_BPERROW = 0x30
++};
++
++struct timbdma_dev {
++ void __iomem *membase;
++ timbdma_interruptcb callbacks[DMA_IRQS];
++ void *callback_data[DMA_IRQS];
++ spinlock_t lock; /* mutual exclusion */
++};
++
++void timb_start_dma(u32 flag, unsigned long buf, int len, int bytes_per_row);
++
++void *timb_stop_dma(u32 flags);
++
++void timb_set_dma_interruptcb(u32 flags, timbdma_interruptcb icb, void *data);
++
++#endif /* _TIMBDMA_H */
++
+diff -uNr linux-2.6.29-clean/include/linux/mfd/timbi2s.h linux-2.6.29/include/linux/mfd/timbi2s.h
+--- linux-2.6.29-clean/include/linux/mfd/timbi2s.h 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/include/linux/mfd/timbi2s.h 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,66 @@
++/*
++ * timbi2s.h timberdale FPGA I2S driver
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++/* Supports:
++ * Timberdale FPGA I2S
++ */
++
++struct timbi2s_bus_control {
++ struct list_head list;
++};
++
++struct timbi2s_bus {
++ void __iomem *membase;
++ u32 irq;
++ struct timbi2s_bus_control *control;
++ struct workqueue_struct *workqueue;
++ struct work_struct work;
++};
++
++struct timbi2s_dev {
++ void __iomem *membase;
++ u32 irq;
++ struct timbi2s_bus *bus;
++ struct workqueue_struct *workqueue;
++ struct work_struct work;
++ u32 ioctrl;
++ u32 devid;
++ u8 timbi2s_rx;
++ u8 timbi2s_tx;
++ struct circ_buf *buffer;
++ /* Register access */
++ spinlock_t lock;
++
++ int in_use;
++ u8 pscale_offset; /* Prescale */
++ u8 icr_offset; /* Clear register */
++ u8 isr_offset; /* Status */
++ u8 ipr_offset; /* Pending register */
++ u8 ier_offset; /* Interrupt Enable register */
++ u8 ctrl_offset;
++ u8 fifo;
++
++ struct list_head item;
++};
++
++static struct timbi2s_dev *timbi2s_get_tx(void);
++static struct timbi2s_dev *timbi2s_get_rx(void);
++static void timbi2s_put(struct timbi2s_dev *tdev);
++
++static int timbi2s_ioctrl(struct timbi2s_dev *i2sdev);
++
+diff -uNr linux-2.6.29-clean/include/linux/serial_core.h linux-2.6.29/include/linux/serial_core.h
+--- linux-2.6.29-clean/include/linux/serial_core.h 2009-04-01 09:20:20.000000000 -0700
++++ linux-2.6.29/include/linux/serial_core.h 2009-04-06 13:51:47.000000000 -0700
+@@ -164,6 +164,9 @@
+ /* NWPSERIAL */
+ #define PORT_NWPSERIAL 85
+
++/* Timberdale UART */
++#define PORT_TIMBUART 86
++
+ #ifdef __KERNEL__
+
+ #include <linux/compiler.h>
+diff -uNr linux-2.6.29-clean/include/linux/spi/xilinx_spi.h linux-2.6.29/include/linux/spi/xilinx_spi.h
+--- linux-2.6.29-clean/include/linux/spi/xilinx_spi.h 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/include/linux/spi/xilinx_spi.h 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,17 @@
++#ifndef __LINUX_SPI_XILINX_SPI_H
++#define __LINUX_SPI_XILINX_SPI_H
++
++/* SPI Controller IP */
++struct xspi_platform_data {
++ s16 bus_num;
++ u16 num_chipselect;
++ u32 speed_hz;
++ u8 cr_offset;
++ u8 sr_offset;
++ u8 txd_offset;
++ u8 rxd_offset;
++ u8 ssr_offset;
++};
++
++#endif /* __LINUX_SPI_XILINX_SPI_H */
++
+diff -uNr linux-2.6.29-clean/include/media/adv7180.h linux-2.6.29/include/media/adv7180.h
+--- linux-2.6.29-clean/include/media/adv7180.h 1969-12-31 16:00:00.000000000 -0800
++++ linux-2.6.29/include/media/adv7180.h 2009-04-06 13:51:47.000000000 -0700
+@@ -0,0 +1,127 @@
++/*
++ * adv7180.h Analog Devices ADV7180 video decoder driver defines
++ * Copyright (c) 2009 Intel Corporation
++ *
++ * 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.
++ *
++ * 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, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++#define DRIVER_NAME "adv7180"
++
++#define I2C_ADV7180 0x42
++#define ADV7180_NR_REG 0xfc
++
++#define ADV7180_IN_CTRL 0x00 /* Input CR */
++#define ADV7180_OUT_CTRL 0x03 /* Output CR */
++#define ADV7180_EXT_OUT_CTRL 0x04 /* Extended Output CR */
++
++#define ADV7180_ADI_CTRL 0x0e /* ADI CR */
++# define ADI_ENABLE 0x20 /* Enable access to sub-regs */
++
++#define ADV7180_SR_1 0x10 /* Status Register 1 */
++#define ADV7180_SR_2 0x12
++#define ADV7180_SR_3 0x13
++
++/* Interrupt and VDP sub-registers */
++#define ADV7180_ISR_1 0x42 /* Interrupt Status Register 1 */
++#define ADV7180_ICR_1 0x43 /* Interrupt Clear Register 1 */
++
++#define ADV7180_ISR_2 0x46
++#define ADV7180_ICR_2 0x47
++
++#define ADV7180_ISR_3 0x4a
++#define ADV7180_ICR_3 0x4b
++
++#define ADV7180_ISR_4 0x4e
++#define ADV7180_ICR_4 0x4f
++/* */
++
++#define ADV7180_SR 0x10
++#define ADV7180_STATUS_NTSM 0x00 /* NTSM M/J */
++#define ADV7180_STATUS_NTSC 0x10 /* NTSC 4.43 */
++#define ADV7180_STATUS_PAL_M 0x20 /* PAL M */
++#define ADV7180_STATUS_PAL_60 0x30 /* PAL 60 */
++#define ADV7180_STATUS_PAL 0x40 /* PAL B/G/H/I/D */
++#define ADV7180_STATUS_SECAM 0x50 /* SECAM */
++#define ADV7180_STATUS_PAL_N 0x60 /* PAL Combination N */
++#define ADV7180_STATUS_SECAM_525 0x70 /* SECAM 525 */
++
++enum input_mode {
++ CVBS, /* Composite */
++ SVIDEO, /* S-video */
++ YPbPr, /* Component */
++};
++
++struct adv7180 {
++ unsigned char reg[ADV7180_NR_REG];
++ int norm;
++ enum input_mode input;
++ int enable;
++ struct i2c_client *client;
++};
++
++static const unsigned char reset_icr[] = {
++ ADV7180_ICR_1, 0x00,
++ ADV7180_ICR_2, 0x00,
++ ADV7180_ICR_3, 0x00,
++ ADV7180_ICR_4, 0x00,
++};
++
++/* ADV7180 LQFP-64. ADV7180.pdf, page 104 */
++static const unsigned char init_cvbs_64[] = {
++ 0x00, 0x01, /* INSEL = CVBS in on Ain2 */
++ 0x04, 0x57, /* Enable SFL */
++ 0x17, 0x41, /* Select SH1 */
++
++ 0x31, 0x02, /* Clear NEWAV_MODE, SAV/EAV to
++ * suit ADV video encoders
++ */
++ 0x3d, 0xa2, /* MWE enable manual window,
++ * color kill threshold to 2
++ */
++ 0x3e, 0x6a, /* BLM optimization */
++ 0x3f, 0xa0, /* BGB optimization */
++ 0x0e, 0x80, /* Hidden space */
++ 0x55, 0x81, /* ADC configuration */
++ 0x0e, 0x00, /* User space */
++};
++
++static const unsigned char init_svideo_64[] = {
++ 0x00, 0x08, /* Insel = Y/C, Y = AIN3, C = AIN6 */
++ 0x04, 0x57, /* Enable SFL */
++ 0x31, 0x02, /* Clear NEWAV_MODE, SAV/EAV to
++ * suit ADV video encoders
++ */
++ 0x3d, 0xa2, /* MWE enable manual window,
++ * color kill threshold to 2
++ */
++ 0x3e, 0x6a, /* BLM optimization */
++ 0x3f, 0xa0, /* BGB optimization */
++ 0x58, 0x04, /* Mandatory write. This must be
++ * performed for correct operation.
++ */
++ 0x0e, 0x80, /* Hidden space */
++ 0x55, 0x81, /* ADC configuration */
++ 0x0e, 0x00, /* User space */
++};
++
++static const unsigned char init_ypbpr_64[] = {
++ 0x00, 0x09, /* INSEL = YPrPb, Y = AIN1, Pr = AIN4, Pb = AIN5 */
++ 0x31, 0x02, /* Clear NEWAV_MODE, SAV/EAV to suit ADV video encoders */
++ 0x3d, 0xa2, /* MWE enable manual window */
++ 0x3e, 0x6a, /* BLM optimization */
++ 0x3f, 0xa0, /* ADI recommended */
++ 0x0e, 0x80, /* Hidden space */
++ 0x55, 0x81, /* ADC configuration */
++ 0x0e, 0x00, /* User space */
++};