From 684d263e75a6a7ede638afa60e35a238e24c12ba Mon Sep 17 00:00:00 2001 From: Richard Purdie Date: Tue, 26 Jan 2010 15:59:18 +0000 Subject: linux-moblin: Add 2.6.31.5 Signed-off-by: Richard Purdie --- .../linux-2.6.31-1-2-timberdale.patch | 12910 +++++++++++++++++++ 1 file changed, 12910 insertions(+) create mode 100644 meta-moblin/packages/linux/linux-moblin-2.6.31.5/linux-2.6.31-1-2-timberdale.patch (limited to 'meta-moblin/packages/linux/linux-moblin-2.6.31.5/linux-2.6.31-1-2-timberdale.patch') diff --git a/meta-moblin/packages/linux/linux-moblin-2.6.31.5/linux-2.6.31-1-2-timberdale.patch b/meta-moblin/packages/linux/linux-moblin-2.6.31.5/linux-2.6.31-1-2-timberdale.patch new file mode 100644 index 000000000..9db5b4ac7 --- /dev/null +++ b/meta-moblin/packages/linux/linux-moblin-2.6.31.5/linux-2.6.31-1-2-timberdale.patch @@ -0,0 +1,12910 @@ +diff -uNr linux-2.6.31/drivers/gpio/Kconfig linux-2.6.31.new/drivers/gpio/Kconfig +--- linux-2.6.31/drivers/gpio/Kconfig 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/gpio/Kconfig 2009-10-23 11:17:19.000000000 -0700 +@@ -173,6 +173,12 @@ + + If unsure, say N. + ++config GPIO_TIMBERDALE ++ bool "Support for timberdale GPIO IP" ++ depends on MFD_TIMBERDALE && GPIOLIB && HAS_IOMEM ++ ---help--- ++ Add support for the GPIO IP in the timberdale FPGA. ++ + comment "SPI GPIO expanders:" + + config GPIO_MAX7301 +@@ -188,4 +194,11 @@ + SPI driver for Microchip MCP23S08 I/O expander. This provides + a GPIO interface supporting inputs and outputs. + ++config GPIO_MC33880 ++ tristate "Freescale MC33880 high-side/low-side switch" ++ depends on SPI_MASTER ++ help ++ SPI driver for Freescale MC33880 high-side/low-side switch. ++ This provides GPIO interface supporting inputs and outputs. ++ + endif +diff -uNr linux-2.6.31/drivers/gpio/Makefile linux-2.6.31.new/drivers/gpio/Makefile +--- linux-2.6.31/drivers/gpio/Makefile 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/gpio/Makefile 2009-10-23 11:17:19.000000000 -0700 +@@ -14,3 +14,6 @@ + obj-$(CONFIG_GPIO_XILINX) += xilinx_gpio.o + obj-$(CONFIG_GPIO_BT8XX) += bt8xxgpio.o + obj-$(CONFIG_GPIO_VR41XX) += vr41xx_giu.o ++obj-$(CONFIG_GPIO_TIMBERDALE) += timbgpio.o ++obj-$(CONFIG_GPIO_MC33880) += mc33880.o ++ +diff -uNr linux-2.6.31/drivers/gpio/mc33880.c linux-2.6.31.new/drivers/gpio/mc33880.c +--- linux-2.6.31/drivers/gpio/mc33880.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/gpio/mc33880.c 2009-10-23 11:17:19.000000000 -0700 +@@ -0,0 +1,196 @@ ++/* ++ * mc33880.c MC33880 high-side/low-side switch 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: ++ * Freescale MC33880 high-side/low-side switch ++ */ ++ ++#include ++#include ++#include ++#include ++#include ++ ++#define DRIVER_NAME "mc33880" ++ ++/* ++ * Pin configurations, see MAX7301 datasheet page 6 ++ */ ++#define PIN_CONFIG_MASK 0x03 ++#define PIN_CONFIG_IN_PULLUP 0x03 ++#define PIN_CONFIG_IN_WO_PULLUP 0x02 ++#define PIN_CONFIG_OUT 0x01 ++ ++#define PIN_NUMBER 8 ++ ++ ++/* ++ * Some registers must be read back to modify. ++ * To save time we cache them here in memory ++ */ ++struct mc33880 { ++ struct mutex lock; /* protect from simultanous accesses */ ++ u8 port_config; ++ struct gpio_chip chip; ++ struct spi_device *spi; ++}; ++ ++static int mc33880_write_config(struct mc33880 *mc) ++{ ++ return spi_write(mc->spi, &mc->port_config, sizeof(mc->port_config)); ++} ++ ++ ++static int __mc33880_set(struct mc33880 *mc, unsigned offset, int value) ++{ ++ if (value) ++ mc->port_config |= 1 << offset; ++ else ++ mc->port_config &= ~(1 << offset); ++ ++ return mc33880_write_config(mc); ++} ++ ++ ++static void mc33880_set(struct gpio_chip *chip, unsigned offset, int value) ++{ ++ struct mc33880 *mc = container_of(chip, struct mc33880, chip); ++ ++ mutex_lock(&mc->lock); ++ ++ __mc33880_set(mc, offset, value); ++ ++ mutex_unlock(&mc->lock); ++} ++ ++static int __devinit mc33880_probe(struct spi_device *spi) ++{ ++ struct mc33880 *mc; ++ struct mc33880_platform_data *pdata; ++ int ret; ++ ++ pdata = spi->dev.platform_data; ++ if (!pdata || !pdata->base) { ++ dev_dbg(&spi->dev, "incorrect or missing platform data\n"); ++ return -EINVAL; ++ } ++ ++ /* ++ * bits_per_word cannot be configured in platform data ++ */ ++ spi->bits_per_word = 8; ++ ++ ret = spi_setup(spi); ++ if (ret < 0) ++ return ret; ++ ++ mc = kzalloc(sizeof(struct mc33880), GFP_KERNEL); ++ if (!mc) ++ return -ENOMEM; ++ ++ mutex_init(&mc->lock); ++ ++ dev_set_drvdata(&spi->dev, mc); ++ ++ mc->spi = spi; ++ ++ mc->chip.label = DRIVER_NAME, ++ mc->chip.set = mc33880_set; ++ mc->chip.base = pdata->base; ++ mc->chip.ngpio = PIN_NUMBER; ++ mc->chip.can_sleep = 1; ++ mc->chip.dev = &spi->dev; ++ mc->chip.owner = THIS_MODULE; ++ ++ mc->port_config = 0x00; ++ /* write twice, because during initialisation the first setting ++ * is just for testing SPI communication, and the second is the ++ * "real" configuration ++ */ ++ ret = mc33880_write_config(mc); ++ mc->port_config = 0x00; ++ if (!ret) ++ ret = mc33880_write_config(mc); ++ ++ if (ret) { ++ printk(KERN_ERR "Failed writing to " DRIVER_NAME ": %d\n", ret); ++ goto exit_destroy; ++ } ++ ++ ret = gpiochip_add(&mc->chip); ++ if (ret) ++ goto exit_destroy; ++ ++ return ret; ++ ++exit_destroy: ++ dev_set_drvdata(&spi->dev, NULL); ++ mutex_destroy(&mc->lock); ++ kfree(mc); ++ return ret; ++} ++ ++static int mc33880_remove(struct spi_device *spi) ++{ ++ struct mc33880 *mc; ++ int ret; ++ ++ mc = dev_get_drvdata(&spi->dev); ++ if (mc == NULL) ++ return -ENODEV; ++ ++ dev_set_drvdata(&spi->dev, NULL); ++ ++ ret = gpiochip_remove(&mc->chip); ++ if (!ret) { ++ mutex_destroy(&mc->lock); ++ kfree(mc); ++ } else ++ dev_err(&spi->dev, "Failed to remove the GPIO controller: %d\n", ++ ret); ++ ++ return ret; ++} ++ ++static struct spi_driver mc33880_driver = { ++ .driver = { ++ .name = DRIVER_NAME, ++ .owner = THIS_MODULE, ++ }, ++ .probe = mc33880_probe, ++ .remove = __devexit_p(mc33880_remove), ++}; ++ ++static int __init mc33880_init(void) ++{ ++ return spi_register_driver(&mc33880_driver); ++} ++/* register after spi postcore initcall and before ++ * subsys initcalls that may rely on these GPIOs ++ */ ++subsys_initcall(mc33880_init); ++ ++static void __exit mc33880_exit(void) ++{ ++ spi_unregister_driver(&mc33880_driver); ++} ++module_exit(mc33880_exit); ++ ++MODULE_AUTHOR("Mocean Laboratories "); ++MODULE_LICENSE("GPL v2"); ++ +diff -uNr linux-2.6.31/drivers/gpio/timbgpio.c linux-2.6.31.new/drivers/gpio/timbgpio.c +--- linux-2.6.31/drivers/gpio/timbgpio.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/gpio/timbgpio.c 2009-10-23 11:17:19.000000000 -0700 +@@ -0,0 +1,342 @@ ++/* ++ * 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 ++#include ++#include ++#include ++#include ++#include ++ ++#define DRIVER_NAME "timb-gpio" ++ ++#define TGPIOVAL 0x00 ++#define TGPIODIR 0x04 ++#define TGPIO_IER 0x08 ++#define TGPIO_ISR 0x0c ++#define TGPIO_IPR 0x10 ++#define TGPIO_ICR 0x14 ++#define TGPIO_FLR 0x18 ++#define TGPIO_LVR 0x1c ++ ++struct timbgpio { ++ void __iomem *membase; ++ spinlock_t lock; /* mutual exclusion */ ++ struct gpio_chip gpio; ++ int irq_base; ++}; ++ ++static int timbgpio_update_bit(struct gpio_chip *gpio, unsigned index, ++ unsigned offset, bool enabled) ++{ ++ struct timbgpio *tgpio = container_of(gpio, struct timbgpio, gpio); ++ u32 reg; ++ ++ spin_lock(&tgpio->lock); ++ reg = ioread32(tgpio->membase + offset); ++ ++ if (enabled) ++ reg |= (1 << index); ++ else ++ reg &= ~(1 << index); ++ ++ iowrite32(reg, tgpio->membase + offset); ++ spin_unlock(&tgpio->lock); ++ ++ return 0; ++} ++ ++static int timbgpio_gpio_direction_input(struct gpio_chip *gpio, unsigned nr) ++{ ++ return timbgpio_update_bit(gpio, nr, TGPIODIR, true); ++} ++ ++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) ++{ ++ return timbgpio_update_bit(gpio, nr, TGPIODIR, false); ++} ++ ++static void timbgpio_gpio_set(struct gpio_chip *gpio, ++ unsigned nr, int val) ++{ ++ timbgpio_update_bit(gpio, nr, TGPIOVAL, val != 0); ++} ++ ++static int timbgpio_to_irq(struct gpio_chip *gpio, unsigned offset) ++{ ++ struct timbgpio *tgpio = container_of(gpio, struct timbgpio, gpio); ++ ++ if (tgpio->irq_base <= 0) ++ return -EINVAL; ++ ++ return tgpio->irq_base + offset; ++} ++ ++/* ++ * GPIO IRQ ++ */ ++static void timbgpio_irq_disable(unsigned irq) ++{ ++ struct timbgpio *tgpio = get_irq_chip_data(irq); ++ int offset = irq - tgpio->irq_base; ++ ++ timbgpio_update_bit(&tgpio->gpio, offset, TGPIO_IER, 0); ++} ++ ++static void timbgpio_irq_enable(unsigned irq) ++{ ++ struct timbgpio *tgpio = get_irq_chip_data(irq); ++ int offset = irq - tgpio->irq_base; ++ ++ timbgpio_update_bit(&tgpio->gpio, offset, TGPIO_IER, 1); ++} ++ ++static int timbgpio_irq_type(unsigned irq, unsigned trigger) ++{ ++ struct timbgpio *tgpio = get_irq_chip_data(irq); ++ int offset = irq - tgpio->irq_base; ++ unsigned long flags; ++ u32 lvr, flr; ++ ++ if (offset < 0 || offset > tgpio->gpio.ngpio) ++ return -EINVAL; ++ ++ spin_lock_irqsave(&tgpio->lock, flags); ++ ++ lvr = ioread32(tgpio->membase + TGPIO_LVR); ++ flr = ioread32(tgpio->membase + TGPIO_FLR); ++ ++ if (trigger & (IRQ_TYPE_LEVEL_HIGH | IRQ_TYPE_LEVEL_LOW)) { ++ flr &= ~(1 << offset); ++ if (trigger & IRQ_TYPE_LEVEL_HIGH) ++ lvr |= 1 << offset; ++ else ++ lvr &= ~(1 << offset); ++ } ++ ++ if ((trigger & IRQ_TYPE_EDGE_BOTH) == IRQ_TYPE_EDGE_BOTH) ++ return -EINVAL; ++ else { ++ flr |= 1 << offset; ++ /* opposite compared to the datasheet, but it mirrors the ++ * reality ++ */ ++ if (trigger & IRQ_TYPE_EDGE_FALLING) ++ lvr |= 1 << offset; ++ else ++ lvr &= ~(1 << offset); ++ } ++ ++ iowrite32(lvr, tgpio->membase + TGPIO_LVR); ++ iowrite32(flr, tgpio->membase + TGPIO_FLR); ++ iowrite32(1 << offset, tgpio->membase + TGPIO_ICR); ++ spin_unlock_irqrestore(&tgpio->lock, flags); ++ ++ return 0; ++} ++ ++static void timbgpio_irq(unsigned int irq, struct irq_desc *desc) ++{ ++ struct timbgpio *tgpio = get_irq_data(irq); ++ unsigned long ipr; ++ int offset; ++ ++ desc->chip->ack(irq); ++ ipr = ioread32(tgpio->membase + TGPIO_IPR); ++ iowrite32(ipr, tgpio->membase + TGPIO_ICR); ++ ++ for_each_bit(offset, &ipr, tgpio->gpio.ngpio) ++ generic_handle_irq(timbgpio_to_irq(&tgpio->gpio, offset)); ++} ++ ++static struct irq_chip timbgpio_irqchip = { ++ .name = "GPIO", ++ .enable = timbgpio_irq_enable, ++ .disable = timbgpio_irq_disable, ++ .set_type = timbgpio_irq_type, ++}; ++ ++static int __devinit timbgpio_probe(struct platform_device *pdev) ++{ ++ int err, i; ++ struct gpio_chip *gc; ++ struct timbgpio *tgpio; ++ struct resource *iomem; ++ struct timbgpio_platform_data *pdata = pdev->dev.platform_data; ++ int irq = platform_get_irq(pdev, 0); ++ ++ if (!pdata || pdata->nr_pins > 32) { ++ err = -EINVAL; ++ goto err_mem; ++ } ++ ++ iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0); ++ if (!iomem) { ++ err = -EINVAL; ++ goto err_mem; ++ } ++ ++ tgpio = kzalloc(sizeof(*tgpio), GFP_KERNEL); ++ if (!tgpio) { ++ err = -EINVAL; ++ goto err_mem; ++ } ++ tgpio->irq_base = pdata->irq_base; ++ ++ spin_lock_init(&tgpio->lock); ++ ++ if (!request_mem_region(iomem->start, resource_size(iomem), ++ DRIVER_NAME)) { ++ err = -EBUSY; ++ goto err_request; ++ } ++ ++ tgpio->membase = ioremap(iomem->start, resource_size(iomem)); ++ if (!tgpio->membase) { ++ err = -ENOMEM; ++ goto err_ioremap; ++ } ++ ++ gc = &tgpio->gpio; ++ ++ gc->label = dev_name(&pdev->dev); ++ gc->owner = THIS_MODULE; ++ gc->dev = &pdev->dev; ++ 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->to_irq = (irq >= 0 && tgpio->irq_base > 0) ? timbgpio_to_irq : NULL; ++ gc->dbg_show = NULL; ++ gc->base = pdata->gpio_base; ++ gc->ngpio = pdata->nr_pins; ++ gc->can_sleep = 0; ++ ++ err = gpiochip_add(gc); ++ if (err) ++ goto err_chipadd; ++ ++ platform_set_drvdata(pdev, tgpio); ++ ++ /* make sure to disable interrupts */ ++ iowrite32(0x0, tgpio->membase + TGPIO_IER); ++ ++ if (irq < 0 || tgpio->irq_base <= 0) ++ return 0; ++ ++ for (i = 0; i < pdata->nr_pins; i++) { ++ set_irq_chip_and_handler_name(tgpio->irq_base + i, ++ &timbgpio_irqchip, handle_simple_irq, "mux"); ++ set_irq_chip_data(tgpio->irq_base + i, tgpio); ++#ifdef CONFIG_ARM ++ set_irq_flags(tgpio->irq_base + i, IRQF_VALID | IRQF_PROBE); ++#endif ++ } ++ ++ set_irq_data(irq, tgpio); ++ set_irq_chained_handler(irq, timbgpio_irq); ++ ++ return 0; ++ ++err_chipadd: ++ iounmap(tgpio->membase); ++err_ioremap: ++ release_mem_region(iomem->start, resource_size(iomem)); ++err_request: ++ kfree(tgpio); ++err_mem: ++ printk(KERN_ERR DRIVER_NAME": Failed to register GPIOs: %d\n", err); ++ ++ return err; ++} ++ ++static int __devexit timbgpio_remove(struct platform_device *pdev) ++{ ++ int err; ++ struct timbgpio_platform_data *pdata = pdev->dev.platform_data; ++ struct timbgpio *tgpio = platform_get_drvdata(pdev); ++ struct resource *iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0); ++ int irq = platform_get_irq(pdev, 0); ++ ++ if (irq >= 0 && tgpio->irq_base > 0) { ++ int i; ++ for (i = 0; i < pdata->nr_pins; i++) { ++ set_irq_chip(tgpio->irq_base + i, NULL); ++ set_irq_chip_data(tgpio->irq_base + i, NULL); ++ } ++ ++ set_irq_handler(irq, NULL); ++ set_irq_data(irq, NULL); ++ } ++ ++ err = gpiochip_remove(&tgpio->gpio); ++ if (err) ++ printk(KERN_ERR DRIVER_NAME": failed to remove gpio_chip\n"); ++ ++ iounmap(tgpio->membase); ++ release_mem_region(iomem->start, resource_size(iomem)); ++ kfree(tgpio); ++ ++ platform_set_drvdata(pdev, NULL); ++ ++ return 0; ++} ++ ++static struct platform_driver timbgpio_platform_driver = { ++ .driver = { ++ .name = DRIVER_NAME, ++ .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:"DRIVER_NAME); ++ +diff -uNr linux-2.6.31/drivers/i2c/busses/i2c-xiic.c linux-2.6.31.new/drivers/i2c/busses/i2c-xiic.c +--- linux-2.6.31/drivers/i2c/busses/i2c-xiic.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/i2c/busses/i2c-xiic.c 2009-10-23 11:17:29.000000000 -0700 +@@ -0,0 +1,1132 @@ ++/* ++ * i2c-xiic.c ++ * 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 IIC ++ */ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#define DRIVER_NAME "xiic-i2c" ++ ++struct xiic_i2c { ++ void __iomem *base; ++ wait_queue_head_t wait; ++ struct i2c_adapter adap; ++ struct i2c_msg *tx_msg; ++ spinlock_t lock; /* mutual exclusion */ ++ unsigned int tx_pos; ++ unsigned int nmsgs; ++ int state; /* see STATE_ */ ++ ++ struct i2c_msg *rx_msg; /* current RX message */ ++ int rx_pos; ++}; ++ ++static inline void xiic_setreg8(struct xiic_i2c *i2c, int reg, u8 value); ++ ++static inline u8 xiic_getreg8(struct xiic_i2c *i2c, int reg); ++ ++static inline void xiic_setreg16(struct xiic_i2c *i2c, int reg, u16 value); ++ ++static inline void xiic_setreg32(struct xiic_i2c *i2c, int reg, int value); ++ ++static inline int xiic_getreg32(struct xiic_i2c *i2c, int reg); ++ ++static void xiic_start_xfer(struct xiic_i2c *i2c); ++static void __xiic_start_xfer(struct xiic_i2c *i2c); ++ ++/************************** Constant Definitions ****************************/ ++ ++#define STATE_DONE 0x00 ++#define STATE_ERROR 0x01 ++#define STATE_START 0x02 ++ ++#define XIIC_MSB_OFFSET 0 ++#define XIIC_REG_OFFSET (0x100+XIIC_MSB_OFFSET) ++ ++/* ++ * Register offsets in bytes from RegisterBase. Three is added to the ++ * base offset to access LSB (IBM style) of the word ++ */ ++#define XIIC_CR_REG_OFFSET (0x00+XIIC_REG_OFFSET) /* Control Register */ ++#define XIIC_SR_REG_OFFSET (0x04+XIIC_REG_OFFSET) /* Status Register */ ++#define XIIC_DTR_REG_OFFSET (0x08+XIIC_REG_OFFSET) /* Data Tx Register */ ++#define XIIC_DRR_REG_OFFSET (0x0C+XIIC_REG_OFFSET) /* Data Rx Register */ ++#define XIIC_ADR_REG_OFFSET (0x10+XIIC_REG_OFFSET) /* Address Register */ ++#define XIIC_TFO_REG_OFFSET (0x14+XIIC_REG_OFFSET) /* Tx FIFO Occupancy */ ++#define XIIC_RFO_REG_OFFSET (0x18+XIIC_REG_OFFSET) /* Rx FIFO Occupancy */ ++#define XIIC_TBA_REG_OFFSET (0x1C+XIIC_REG_OFFSET) /* 10 Bit Address reg */ ++#define XIIC_RFD_REG_OFFSET (0x20+XIIC_REG_OFFSET) /* Rx FIFO Depth reg */ ++#define XIIC_GPO_REG_OFFSET (0x24+XIIC_REG_OFFSET) /* Output Register */ ++ ++/* Control Register masks */ ++#define XIIC_CR_ENABLE_DEVICE_MASK 0x01 /* Device enable = 1 */ ++#define XIIC_CR_TX_FIFO_RESET_MASK 0x02 /* Transmit FIFO reset=1 */ ++#define XIIC_CR_MSMS_MASK 0x04 /* Master starts Txing=1 */ ++#define XIIC_CR_DIR_IS_TX_MASK 0x08 /* Dir of tx. Txing=1 */ ++#define XIIC_CR_NO_ACK_MASK 0x10 /* Tx Ack. NO ack = 1 */ ++#define XIIC_CR_REPEATED_START_MASK 0x20 /* Repeated start = 1 */ ++#define XIIC_CR_GENERAL_CALL_MASK 0x40 /* Gen Call enabled = 1 */ ++ ++/* Status Register masks */ ++#define XIIC_SR_GEN_CALL_MASK 0x01 /* 1=a mstr issued a GC */ ++#define XIIC_SR_ADDR_AS_SLAVE_MASK 0x02 /* 1=when addr as slave */ ++#define XIIC_SR_BUS_BUSY_MASK 0x04 /* 1 = bus is busy */ ++#define XIIC_SR_MSTR_RDING_SLAVE_MASK 0x08 /* 1=Dir: mstr <-- slave */ ++#define XIIC_SR_TX_FIFO_FULL_MASK 0x10 /* 1 = Tx FIFO full */ ++#define XIIC_SR_RX_FIFO_FULL_MASK 0x20 /* 1 = Rx FIFO full */ ++#define XIIC_SR_RX_FIFO_EMPTY_MASK 0x40 /* 1 = Rx FIFO empty */ ++#define XIIC_SR_TX_FIFO_EMPTY_MASK 0x80 /* 1 = Tx FIFO empty */ ++ ++/* Interrupt Status Register masks Interrupt occurs when... */ ++#define XIIC_INTR_ARB_LOST_MASK 0x01 /* 1 = arbitration lost */ ++#define XIIC_INTR_TX_ERROR_MASK 0x02 /* 1=Tx error/msg complete */ ++#define XIIC_INTR_TX_EMPTY_MASK 0x04 /* 1 = Tx FIFO/reg empty */ ++#define XIIC_INTR_RX_FULL_MASK 0x08 /* 1=Rx FIFO/reg=OCY level */ ++#define XIIC_INTR_BNB_MASK 0x10 /* 1 = Bus not busy */ ++#define XIIC_INTR_AAS_MASK 0x20 /* 1 = when addr as slave */ ++#define XIIC_INTR_NAAS_MASK 0x40 /* 1 = not addr as slave */ ++#define XIIC_INTR_TX_HALF_MASK 0x80 /* 1 = TX FIFO half empty */ ++ ++/* The following constants specify the depth of the FIFOs */ ++#define IIC_RX_FIFO_DEPTH 16 /* Rx fifo capacity */ ++#define IIC_TX_FIFO_DEPTH 16 /* Tx fifo capacity */ ++ ++/* The following constants specify groups of interrupts that are typically ++ * enabled or disables at the same time ++ */ ++#define XIIC_TX_INTERRUPTS \ ++(XIIC_INTR_TX_ERROR_MASK | XIIC_INTR_TX_EMPTY_MASK | XIIC_INTR_TX_HALF_MASK) ++ ++#define XIIC_TX_RX_INTERRUPTS (XIIC_INTR_RX_FULL_MASK | XIIC_TX_INTERRUPTS) ++ ++/* The following constants are used with the following macros to specify the ++ * operation, a read or write operation. ++ */ ++#define XIIC_READ_OPERATION 1 ++#define XIIC_WRITE_OPERATION 0 ++ ++/* ++ * Tx Fifo upper bit masks. ++ */ ++#define XIIC_TX_DYN_START_MASK 0x0100 /* 1 = Set dynamic start */ ++#define XIIC_TX_DYN_STOP_MASK 0x0200 /* 1 = Set dynamic stop */ ++ ++/* ++ * The following constants define the register offsets for the Interrupt ++ * registers. There are some holes in the memory map for reserved addresses ++ * to allow other registers to be added and still match the memory map of the ++ * interrupt controller registers ++ */ ++#define XIIC_DGIER_OFFSET 0x1C /* Device Global Interrupt Enable Register */ ++#define XIIC_IISR_OFFSET 0x20 /* Interrupt Status Register */ ++#define XIIC_IIER_OFFSET 0x28 /* Interrupt Enable Register */ ++#define XIIC_RESETR_OFFSET 0x40 /* Reset Register */ ++ ++#define XIIC_RESET_MASK 0xAUL ++ ++/* ++ * The following constant is used for the device global interrupt enable ++ * register, to enable all interrupts for the device, this is the only bit ++ * in the register ++ */ ++#define XIIC_GINTR_ENABLE_MASK 0x80000000UL ++ ++/***************** Macros (Inline Functions) Definitions *********************/ ++ ++ ++/****************************************************************************** ++* ++* This macro disables all interrupts for the device by writing to the Global ++* interrupt enable register. This register provides the ability to disable ++* interrupts without any modifications to the interrupt enable register such ++* that it is minimal effort to restore the interrupts to the previous enabled ++* state. The corresponding function, XIIC_GINTR_ENABLE, is provided to ++* restore the interrupts to the previous enabled state. This function is ++* designed to be used in critical sections of device drivers such that it is ++* not necessary to disable other device interrupts. ++* ++* @param Instance local i2c instance ++* ++* @return None. ++* ++* @note C-Style signature: ++* void XIIC_GINTR_DISABLE(i2c); ++* ++******************************************************************************/ ++#define XIIC_GINTR_DISABLE(Instance) \ ++ xiic_setreg32(Instance, XIIC_DGIER_OFFSET, 0) ++ ++/****************************************************************************** ++* ++* This macro writes to the global interrupt enable register to enable ++* interrupts from the device. This register provides the ability to enable ++* interrupts without any modifications to the interrupt enable register such ++* that it is minimal effort to restore the interrupts to the previous enabled ++* state. This function does not enable individual interrupts as the interrupt ++* enable register must be set appropriately. This function is designed to be ++* used in critical sections of device drivers such that it is not necessary to ++* disable other device interrupts. ++* ++* @param Instance local I2C instance ++* ++* @return None. ++* ++* @note C-Style signature: ++* void XIIC_GINTR_ENABLE(i2c); ++* ++******************************************************************************/ ++#define XIIC_GINTR_ENABLE(Instance) \ ++ xiic_setreg32(Instance, XIIC_DGIER_OFFSET, XIIC_GINTR_ENABLE_MASK) ++ ++/****************************************************************************** ++* ++* ++* This function sets the Interrupt status register to the specified value. ++* This register indicates the status of interrupt sources for the device. ++* The status is independent of whether interrupts are enabled such that ++* the status register may also be polled when interrupts are not enabled. ++* ++* Each bit of the register correlates to a specific interrupt source within the ++* IIC device. All bits of this register are latched. Setting a bit which is 0 ++* within this register causes an interrupt to be generated. The device global ++* interrupt enable register and the device interrupt enable register must be set ++* appropriately to allow an interrupt to be passed out of the device. The ++* interrupt is cleared by writing to this register with the bits to be ++* cleared set to a one and all others to zero. This register implements a ++* toggle on write functionality meaning any bits which are set in the value ++* written cause the bits in the register to change to the opposite state. ++* ++* This function writes only the specified value to the register such that ++* some status bits may be set and others cleared. It is the caller's ++* responsibility to get the value of the register prior to setting the value ++* to prevent an destructive behavior. ++* ++* @param Instance local I2C instance ++* @param Status contains the value to be written to the Interrupt ++* status register. ++* ++* @return None. ++* ++* @note C-Style signature: ++* void XIIC_WRITE_IISR(i2c, u32 Status); ++* ++******************************************************************************/ ++#define XIIC_WRITE_IISR(Instance, Status) \ ++ xiic_setreg32(Instance, XIIC_IISR_OFFSET, (Status)) ++ ++/****************************************************************************** ++* ++* ++* This function gets the contents of the Interrupt Status Register. ++* This register indicates the status of interrupt sources for the device. ++* The status is independent of whether interrupts are enabled such ++* that the status register may also be polled when interrupts are not enabled. ++* ++* Each bit of the register correlates to a specific interrupt source within the ++* device. All bits of this register are latched. Writing a 1 to a bit within ++* this register causes an interrupt to be generated if enabled in the interrupt ++* enable register and the global interrupt enable is set. Since the status is ++* latched, each status bit must be acknowledged in order for the bit in the ++* status register to be updated. Each bit can be acknowledged by writing a ++* 0 to the bit in the status register. ++ ++* @param Instance local I2C instance ++* ++* @return A status which contains the value read from the Interrupt ++* Status Register. ++* ++* @note C-Style signature: ++* u32 XIIC_READ_IISR(i2c); ++* ++******************************************************************************/ ++#define XIIC_READ_IISR(Instance) \ ++ xiic_getreg32(Instance, XIIC_IISR_OFFSET) ++ ++/****************************************************************************** ++* ++* This function sets the contents of the Interrupt Enable Register . This ++* register controls which interrupt sources of the IIC device are allowed to ++* generate an interrupt. The global interrupt enable register and the device ++* interrupt enable register must also be set appropriately for an interrupt ++* to be passed out of the device. ++* ++* Each bit of the register correlates to a specific interrupt source within the ++* device. Setting a bit in this register enables the interrupt source to gen ++* an interrupt. Clearing a bit in this register disables interrupt generation ++* for that interrupt source. ++* ++* This function writes only the specified value to the register such that ++* some interrupt sources may be enabled and others disabled. It is the ++* caller's responsibility to get the value of the interrupt enable register ++* prior to setting the value to prevent a destructive behavior. ++* ++* @param Instance local I2C instance ++* @param Enable contains the value to be written to the Interrupt Enable ++* Register. ++* ++* @return None ++* ++* @note C-Style signature: ++* void XIIC_WRITE_IIER(i2c, u32 Enable); ++* ++******************************************************************************/ ++#define XIIC_WRITE_IIER(Instance, Enable) \ ++ xiic_setreg32(Instance, XIIC_IIER_OFFSET, (Enable)) ++ ++/****************************************************************************** ++* ++* ++* This function gets the Interrupt enable register contents. This register ++* controls which interrupt sources of the device are allowed to generate an ++* interrupt. The global interrupt enable register and the device interrupt ++* enable register must also be set appropriately for an interrupt to be ++* passed out of the IIC device. ++* ++* Each bit of the register correlates to a specific interrupt source within the ++* IIC device. Setting a bit in this register enables the interrupt source to ++* generate an interrupt. Clearing a bit in this register disables interrupt ++* generation for that interrupt source. ++* ++* @param Instance local I2C instance ++* ++* @return The contents read from the Interrupt Enable Register. ++* ++* @note C-Style signature: ++* u32 XIIC_READ_IIER(i2c) ++* ++******************************************************************************/ ++#define XIIC_READ_IIER(Instance) \ ++ xiic_getreg32(Instance, XIIC_IIER_OFFSET) ++ ++/************************** Function Prototypes ******************************/ ++ ++/****************************************************************************** ++* ++* This macro disables the specified interrupts in the Interrupt enable ++* register. It is non-destructive in that the register is read and only the ++* interrupts specified is changed. ++* ++* @param BaseAddress is the base address of the IIC device. ++* @param InterruptMask contains the interrupts to be disabled ++* ++* @return None. ++* ++* @note Signature: ++* void XIic_mDisableIntr(u32 BaseAddress, u32 InterruptMask); ++* ++******************************************************************************/ ++#define XIic_mDisableIntr(Instance, InterruptMask) \ ++ XIIC_WRITE_IIER((Instance), XIIC_READ_IIER(Instance) & ~(InterruptMask)) ++ ++/****************************************************************************** ++* ++* This macro enables the specified interrupts in the Interrupt enable ++* register. It is non-destructive in that the register is read and only the ++* interrupts specified is changed. ++* ++* @param BaseAddress is the base address of the IIC device. ++* @param InterruptMask contains the interrupts to be disabled ++* ++* @return None. ++* ++* @note Signature: ++* void XIic_mEnableIntr(u32 BaseAddress, u32 InterruptMask); ++* ++******************************************************************************/ ++#define XIic_mEnableIntr(Instance, InterruptMask) \ ++ XIIC_WRITE_IIER((Instance), XIIC_READ_IIER(Instance) | (InterruptMask)) ++ ++/****************************************************************************** ++* ++* This macro clears the specified interrupt in the Interrupt status ++* register. It is non-destructive in that the register is read and only the ++* interrupt specified is cleared. Clearing an interrupt acknowledges it. ++* ++* @param BaseAddress is the base address of the IIC device. ++* @param InterruptMask contains the interrupts to be disabled ++* ++* @return None. ++* ++* @note Signature: ++* void XIic_mClearIntr(u32 BaseAddress, u32 InterruptMask); ++* ++******************************************************************************/ ++#define XIic_mClearIntr(Instance, InterruptMask) \ ++ XIIC_WRITE_IISR((Instance), XIIC_READ_IISR(Instance) & (InterruptMask)) ++ ++/****************************************************************************** ++* ++* This macro clears and enables the specified interrupt in the Interrupt ++* status and enable registers. It is non-destructive in that the registers are ++* read and only the interrupt specified is modified. ++* Clearing an interrupt acknowledges it. ++* ++* @param BaseAddress is the base address of the IIC device. ++* @param InterruptMask contains the interrupts to be cleared and enabled ++* ++* @return None. ++* ++* @note Signature: ++* void XIic_mClearEnableIntr(u32 BaseAddress, u32 InterruptMask); ++* ++******************************************************************************/ ++#define XIic_mClearEnableIntr(Instance, InterruptMask) { \ ++ XIIC_WRITE_IISR(Instance, \ ++ (XIIC_READ_IISR(Instance) & (InterruptMask))); \ ++ XIIC_WRITE_IIER(Instance, \ ++ (XIIC_READ_IIER(Instance) | (InterruptMask))); \ ++} ++ ++ ++#define xiic_tx_space(i2c) ((i2c)->tx_msg->len - (i2c)->tx_pos) ++#define xiic_rx_space(i2c) ((i2c)->rx_msg->len - (i2c)->rx_pos) ++ ++static void xiic_clear_rx_fifo(struct xiic_i2c *i2c) ++{ ++ u8 sr; ++ for (sr = xiic_getreg8(i2c, XIIC_SR_REG_OFFSET); ++ !(sr & XIIC_SR_RX_FIFO_EMPTY_MASK); ++ sr = xiic_getreg8(i2c, XIIC_SR_REG_OFFSET)) ++ xiic_getreg8(i2c, XIIC_DRR_REG_OFFSET); ++} ++ ++/****************************************************************************** ++ * ++ * Initialize the IIC core for Dynamic Functionality. ++ * ++ * @param i2c local I2C instance ++ * ++ * @return None. ++ * ++ * @note None. ++ * ++ ******************************************************************************/ ++static void xiic_reinit(struct xiic_i2c *i2c) ++{ ++ xiic_setreg32(i2c, XIIC_RESETR_OFFSET, XIIC_RESET_MASK); ++ ++ /* Set receive Fifo depth to maximum (zero based). */ ++ xiic_setreg8(i2c, XIIC_RFD_REG_OFFSET, IIC_RX_FIFO_DEPTH - 1); ++ ++ /* Reset Tx Fifo. */ ++ xiic_setreg8(i2c, XIIC_CR_REG_OFFSET, XIIC_CR_TX_FIFO_RESET_MASK); ++ ++ /* Enable IIC Device, remove Tx Fifo reset & disable general call. */ ++ xiic_setreg8(i2c, XIIC_CR_REG_OFFSET, XIIC_CR_ENABLE_DEVICE_MASK); ++ ++ /* make sure RX fifo is empty */ ++ xiic_clear_rx_fifo(i2c); ++ ++ /* Enable interrupts */ ++ xiic_setreg32(i2c, XIIC_DGIER_OFFSET, XIIC_GINTR_ENABLE_MASK); ++ ++ XIic_mClearEnableIntr(i2c, XIIC_INTR_AAS_MASK | ++ XIIC_INTR_ARB_LOST_MASK); ++} ++ ++/****************************************************************************** ++ * ++ * De-Initialize the IIC core. ++ * ++ * @param i2c local I2C instance ++ * ++ * @return None. ++ * ++ * @note None. ++ * ++ ******************************************************************************/ ++static void xiic_deinit(struct xiic_i2c *i2c) ++{ ++ u8 cr; ++ ++ xiic_setreg32(i2c, XIIC_RESETR_OFFSET, XIIC_RESET_MASK); ++ ++ /* Disable IIC Device. */ ++ cr = xiic_getreg8(i2c, XIIC_CR_REG_OFFSET); ++ xiic_setreg8(i2c, XIIC_CR_REG_OFFSET, cr & ~XIIC_CR_ENABLE_DEVICE_MASK); ++} ++ ++ ++ ++/***************************************************************************** ++ * ++ * ++ * This function is called when the receive register is full. The number ++ * of bytes received to cause the interrupt is adjustable using the Receive FIFO ++ * Depth register. The number of bytes in the register is read in the Receive ++ * FIFO occupancy register. Both these registers are zero based values (0-15) ++ * such that a value of zero indicates 1 byte. ++ * ++ * For a Master Receiver to properly signal the end of a message, the data must ++ * be read in up to the message length - 1, where control register bits will be ++ * set for bus controls to occur on reading of the last byte. ++ * ++ * @param InstancePtr is a pointer to the XIic instance to be worked on. ++ * ++ * @return None. ++ * ++ * @note None. ++ * ++ ******************************************************************************/ ++static void xiic_read_rx(struct xiic_i2c *i2c) ++{ ++ u8 bytes_in_fifo; ++ int i; ++ ++ bytes_in_fifo = xiic_getreg8(i2c, XIIC_RFO_REG_OFFSET) + 1; ++ ++ dev_dbg(i2c->adap.dev.parent, "%s entry, bytes in fifo: %d, msg: %d" ++ ", SR: 0x%x, CR: 0x%x\n", ++ __func__, bytes_in_fifo, xiic_rx_space(i2c), ++ xiic_getreg8(i2c, XIIC_SR_REG_OFFSET), ++ xiic_getreg8(i2c, XIIC_CR_REG_OFFSET)); ++ ++ if (bytes_in_fifo > xiic_rx_space(i2c)) ++ bytes_in_fifo = xiic_rx_space(i2c); ++ ++ for (i = 0; i < bytes_in_fifo; i++) ++ i2c->rx_msg->buf[i2c->rx_pos++] = ++ xiic_getreg8(i2c, XIIC_DRR_REG_OFFSET); ++ ++ xiic_setreg8(i2c, XIIC_RFD_REG_OFFSET, ++ (xiic_rx_space(i2c) > IIC_RX_FIFO_DEPTH) ? ++ IIC_RX_FIFO_DEPTH - 1 : xiic_rx_space(i2c) - 1); ++} ++ ++/****************************************************************************** ++ * ++ * This function fills the FIFO using the occupancy register to determine the ++ * available space to be filled. When the repeated start option is on, the last ++ * byte is withheld to allow the control register to be properly set on the last ++ * byte. ++ * ++ * @param InstancePtr is a pointer to the XIic instance to be worked on. ++ * ++ * @param Role indicates the role of this IIC device, a slave or a master, on ++ * the IIC bus (XIIC_SLAVE_ROLE or XIIC_MASTER_ROLE) ++ * ++ * @return ++ * ++ * None. ++ * ++ * @note ++ * ++ * None. ++ * ++ ******************************************************************************/ ++static int xiic_tx_fifo_space(struct xiic_i2c *i2c) ++{ ++ return IIC_TX_FIFO_DEPTH - xiic_getreg8(i2c, XIIC_TFO_REG_OFFSET) - 1; ++} ++ ++static void xiic_fill_tx_fifo(struct xiic_i2c *i2c) ++{ ++ u8 fifo_space = xiic_tx_fifo_space(i2c); ++ int len = xiic_tx_space(i2c); ++ ++ len = (len > fifo_space) ? fifo_space : len; ++ ++ dev_dbg(i2c->adap.dev.parent, "%s entry, len: %d, fifo space: %d\n", ++ __func__, len, fifo_space); ++ ++ while (len--) { ++ u16 data = i2c->tx_msg->buf[i2c->tx_pos++]; ++ if ((xiic_tx_space(i2c) == 0) && (i2c->nmsgs == 1)) { ++ /* last message in transfer -> STOP */ ++ data |= XIIC_TX_DYN_STOP_MASK; ++ dev_dbg(i2c->adap.dev.parent, "%s TX STOP\n", __func__); ++ ++ xiic_setreg16(i2c, XIIC_DTR_REG_OFFSET, data); ++ } else ++ xiic_setreg8(i2c, XIIC_DTR_REG_OFFSET, data); ++ } ++} ++ ++static void xiic_wakeup(struct xiic_i2c *i2c, int code) ++{ ++ i2c->tx_msg = NULL; ++ i2c->rx_msg = NULL; ++ i2c->nmsgs = 0; ++ i2c->state = code; ++ wake_up(&i2c->wait); ++} ++ ++static void xiic_process(struct xiic_i2c *i2c) ++{ ++ u32 pend, isr, ier; ++ u32 Clear = 0; ++ ++ /* Get the interrupt Status from the IPIF. There is no clearing of ++ * interrupts in the IPIF. Interrupts must be cleared at the source. ++ * To find which interrupts are pending; AND interrupts pending with ++ * interrupts masked. ++ */ ++ isr = XIIC_READ_IISR(i2c); ++ ier = XIIC_READ_IIER(i2c); ++ pend = isr & ier; ++ ++ dev_dbg(i2c->adap.dev.parent, "%s entry, IER: 0x%x, ISR: 0x%x, " ++ "pend: 0x%x, SR: 0x%x, msg: %p, nmsgs: %d\n", ++ __func__, ier, isr, pend, xiic_getreg8(i2c, XIIC_SR_REG_OFFSET), ++ i2c->tx_msg, i2c->nmsgs); ++ ++ /* Do not processes a devices interrupts if the device has no ++ * interrupts pending ++ */ ++ if (!pend) ++ return; ++ ++ /* Service requesting interrupt */ ++ if ((pend & XIIC_INTR_ARB_LOST_MASK) || ++ ((pend & XIIC_INTR_TX_ERROR_MASK) && ++ !(pend & XIIC_INTR_RX_FULL_MASK))) { ++ /* bus arbritration lost, or... ++ * Transmit error _OR_ RX completed ++ * if this happens when RX_FULL is not set ++ * this is probably a TX error ++ */ ++ ++ dev_dbg(i2c->adap.dev.parent, ++ "%s error\n", __func__); ++ ++ /* dynamic mode seem to suffer from problems if we just flushes ++ * fifos and the next message is a TX with len 0 (only addr) ++ * reset the IP instead of just flush fifos ++ */ ++ xiic_reinit(i2c); ++ ++ if (i2c->tx_msg) ++ xiic_wakeup(i2c, STATE_ERROR); ++ ++ } else if (pend & XIIC_INTR_RX_FULL_MASK) { ++ /* Receive register/FIFO is full */ ++ ++ Clear = XIIC_INTR_RX_FULL_MASK; ++ if (!i2c->rx_msg) { ++ dev_dbg(i2c->adap.dev.parent, ++ "%s unexpexted RX IRQ\n", __func__); ++ xiic_clear_rx_fifo(i2c); ++ goto out; ++ } ++ ++ xiic_read_rx(i2c); ++ if (xiic_rx_space(i2c) == 0) { ++ /* this is the last part of the message */ ++ i2c->rx_msg = NULL; ++ ++ /* also clear TX error if there (RX complete) */ ++ Clear |= (isr & XIIC_INTR_TX_ERROR_MASK); ++ ++ dev_dbg(i2c->adap.dev.parent, ++ "%s end of message, nmsgs: %d\n", ++ __func__, i2c->nmsgs); ++ ++ /* send next message if this wasn't the last, ++ * otherwise the transfer will be finialise when ++ * receiving the bus not busy interrupt ++ */ ++ if (i2c->nmsgs > 1) { ++ i2c->nmsgs--; ++ i2c->tx_msg++; ++ dev_dbg(i2c->adap.dev.parent, ++ "%s will start next...\n", __func__); ++ ++ __xiic_start_xfer(i2c); ++ } ++ } ++ } else if (pend & XIIC_INTR_BNB_MASK) { ++ /* IIC bus has transitioned to not busy */ ++ Clear = XIIC_INTR_BNB_MASK; ++ ++ /* The bus is not busy, disable BusNotBusy interrupt */ ++ XIic_mDisableIntr(i2c, XIIC_INTR_BNB_MASK); ++ ++ if (!i2c->tx_msg) ++ goto out; ++ ++ if ((i2c->nmsgs == 1) && !i2c->rx_msg && ++ xiic_tx_space(i2c) == 0) ++ xiic_wakeup(i2c, STATE_DONE); ++ else ++ xiic_wakeup(i2c, STATE_ERROR); ++ ++ } else if (pend & (XIIC_INTR_TX_EMPTY_MASK | XIIC_INTR_TX_HALF_MASK)) { ++ /* Transmit register/FIFO is empty or ½ empty */ ++ ++ Clear = pend & ++ (XIIC_INTR_TX_EMPTY_MASK | XIIC_INTR_TX_HALF_MASK); ++ ++ if (!i2c->tx_msg) { ++ dev_dbg(i2c->adap.dev.parent, ++ "%s unexpexted TX IRQ\n", __func__); ++ goto out; ++ } ++ ++ xiic_fill_tx_fifo(i2c); ++ ++ /* current message sent and there is space in the fifo */ ++ if (!xiic_tx_space(i2c) && xiic_tx_fifo_space(i2c) >= 2) { ++ dev_dbg(i2c->adap.dev.parent, ++ "%s end of message sent, nmsgs: %d\n", ++ __func__, i2c->nmsgs); ++ if (i2c->nmsgs > 1) { ++ i2c->nmsgs--; ++ i2c->tx_msg++; ++ __xiic_start_xfer(i2c); ++ } else { ++ XIic_mDisableIntr(i2c, XIIC_INTR_TX_HALF_MASK); ++ ++ dev_err(i2c->adap.dev.parent, ++ "%s Got TX IRQ but no more to do...\n", ++ __func__); ++ } ++ } else if (!xiic_tx_space(i2c) && (i2c->nmsgs == 1)) ++ /* current frame is sent and is last, ++ * make sure to disable tx half ++ */ ++ XIic_mDisableIntr(i2c, XIIC_INTR_TX_HALF_MASK); ++ } else { ++ /* got IRQ which is not acked */ ++ dev_err(i2c->adap.dev.parent, "%s Got unexpected IRQ\n", ++ __func__); ++ Clear = pend; ++ } ++out: ++ dev_dbg(i2c->adap.dev.parent, "%s Clear: 0x%x\n", __func__, Clear); ++ ++ XIIC_WRITE_IISR(i2c, Clear); ++} ++ ++/****************************************************************************** ++ * ++ * This function checks to see if the IIC bus is busy. If so, it will enable ++ * the bus not busy interrupt such that the driver is notified when the bus ++ * is no longer busy. ++ * ++ * @param InstancePtr points to the Iic instance to be worked on. ++ * ++ * @return FALSE if the IIC bus is not busy else TRUE. ++ * ++ * @note The BusNotBusy interrupt is enabled which will update the ++ * EventStatus when the bus is no longer busy. ++ * ++ ******************************************************************************/ ++static int xiic_bus_busy(struct xiic_i2c *i2c) ++{ ++ u8 sr = xiic_getreg8(i2c, XIIC_SR_REG_OFFSET); ++ ++ return (sr & XIIC_SR_BUS_BUSY_MASK) ? -EBUSY : 0; ++} ++ ++static int xiic_busy(struct xiic_i2c *i2c) ++{ ++ int tries = 3; ++ int err; ++ if (i2c->tx_msg) ++ return -EBUSY; ++ ++ /* for instance if previous transfer was terminated due to TX error ++ * it might be that the bus is on it's way to become available ++ * give it at most 3 ms to wake ++ */ ++ err = xiic_bus_busy(i2c); ++ while (err && tries--) { ++ mdelay(1); ++ err = xiic_bus_busy(i2c); ++ } ++ ++ return err; ++} ++ ++static void xiic_dump_regs(struct xiic_i2c *i2c, const char *caller) ++{ ++ dev_dbg(i2c->adap.dev.parent, "%s msg: %p, nmsgs: %d, " ++ "ISR: 0x%x, CR: 0x%x, SR: 0x%x\n", ++ caller, i2c->tx_msg, i2c->nmsgs, XIIC_READ_IISR(i2c), ++ xiic_getreg8(i2c, XIIC_CR_REG_OFFSET), ++ xiic_getreg8(i2c, XIIC_SR_REG_OFFSET)); ++} ++ ++static void xiic_start_recv(struct xiic_i2c *i2c) ++{ ++ u8 rx_watermark; ++ struct i2c_msg *msg = i2c->rx_msg = i2c->tx_msg; ++ ++ xiic_dump_regs(i2c, __func__); ++ ++ /* Clear and enable Rx full interrupt. */ ++ XIic_mClearEnableIntr(i2c, XIIC_INTR_RX_FULL_MASK | ++ XIIC_INTR_TX_ERROR_MASK); ++ ++ /* we want to get all but last byte, because the TX_ERROR IRQ is used ++ * to inidicate error ACK on the address, and negative ack on the last ++ * received byte, so to not mix them receive all but last. ++ * In the case where there is only one byte to receive ++ * we can check if ERROR and RX full is set at the same time ++ */ ++ rx_watermark = msg->len; ++ if (rx_watermark > IIC_RX_FIFO_DEPTH) ++ rx_watermark = IIC_RX_FIFO_DEPTH; ++ xiic_setreg8(i2c, XIIC_RFD_REG_OFFSET, rx_watermark - 1); ++ ++ if (!(msg->flags & I2C_M_NOSTART)) ++ /* write the address */ ++ xiic_setreg16(i2c, XIIC_DTR_REG_OFFSET, ++ (msg->addr << 1) | XIIC_READ_OPERATION | ++ XIIC_TX_DYN_START_MASK); ++ ++ XIic_mClearEnableIntr(i2c, ++ XIIC_INTR_BNB_MASK); ++ ++ xiic_dump_regs(i2c, "after address"); ++ ++ xiic_setreg16(i2c, XIIC_DTR_REG_OFFSET, ++ msg->len | ((i2c->nmsgs == 1) ? XIIC_TX_DYN_STOP_MASK : 0)); ++ if (i2c->nmsgs == 1) { ++ /* very last, enable bus busy as well */ ++ XIic_mClearEnableIntr(i2c, XIIC_INTR_BNB_MASK); ++ } ++ ++ xiic_dump_regs(i2c, "xiic_start_recv exit"); ++ ++ /* the message is tx:ed */ ++ i2c->tx_pos = msg->len; ++} ++ ++static void xiic_start_send(struct xiic_i2c *i2c) ++{ ++ struct i2c_msg *msg = i2c->tx_msg; ++ ++ XIic_mClearIntr(i2c, XIIC_INTR_TX_ERROR_MASK); ++ ++ dev_dbg(i2c->adap.dev.parent, "%s entry, msg: %p, len: %d, " ++ "ISR: 0x%x, CR: 0x%x\n", ++ __func__, msg, msg->len, XIIC_READ_IISR(i2c), ++ xiic_getreg8(i2c, XIIC_CR_REG_OFFSET)); ++ ++ if (!(msg->flags & I2C_M_NOSTART)) { ++ /* write the address */ ++ u16 data = ((msg->addr << 1) & 0xfe) | XIIC_WRITE_OPERATION | ++ XIIC_TX_DYN_START_MASK; ++ if ((i2c->nmsgs == 1) && msg->len == 0) ++ /* no data and last message -> add STOP */ ++ data |= XIIC_TX_DYN_STOP_MASK; ++ ++ xiic_setreg16(i2c, XIIC_DTR_REG_OFFSET, data); ++ } ++ ++ xiic_fill_tx_fifo(i2c); ++ ++ /* Clear any pending Tx empty, Tx Error and then enable them. */ ++ XIic_mClearEnableIntr(i2c, ++ XIIC_INTR_TX_EMPTY_MASK | XIIC_INTR_TX_ERROR_MASK | ++ XIIC_INTR_BNB_MASK); ++} ++ ++static inline void xiic_setreg8(struct xiic_i2c *i2c, int reg, u8 value) ++{ ++ iowrite8(value, i2c->base + reg); ++} ++ ++static inline u8 xiic_getreg8(struct xiic_i2c *i2c, int reg) ++{ ++ return ioread8(i2c->base + reg); ++} ++ ++static inline void xiic_setreg16(struct xiic_i2c *i2c, int reg, u16 value) ++{ ++ iowrite16(value, i2c->base + reg); ++} ++ ++static inline void xiic_setreg32(struct xiic_i2c *i2c, int reg, int value) ++{ ++ iowrite32(value, i2c->base + reg); ++} ++ ++static inline int xiic_getreg32(struct xiic_i2c *i2c, int reg) ++{ ++ return ioread32(i2c->base + reg); ++} ++ ++static irqreturn_t xiic_isr(int irq, void *dev_id) ++{ ++ struct xiic_i2c *i2c = dev_id; ++ spin_lock(&i2c->lock); ++ XIIC_GINTR_DISABLE(i2c); ++ ++ dev_dbg(i2c->adap.dev.parent, "%s entry\n", __func__); ++ ++ xiic_process(i2c); ++ ++ XIIC_GINTR_ENABLE(i2c); ++ ++ spin_unlock(&i2c->lock); ++ ++ return IRQ_HANDLED; ++} ++ ++static void __xiic_start_xfer(struct xiic_i2c *i2c) ++{ ++ int first = 1; ++ int fifo_space = xiic_tx_fifo_space(i2c); ++ dev_dbg(i2c->adap.dev.parent, "%s entry, msg: %p, fifos space: %d\n", ++ __func__, i2c->tx_msg, fifo_space); ++ ++ if (!i2c->tx_msg) ++ return; ++ ++ i2c->rx_pos = 0; ++ i2c->tx_pos = 0; ++ i2c->state = STATE_START; ++ while ((fifo_space >= 2) && (first || (i2c->nmsgs > 1))) { ++ if (!first) { ++ i2c->nmsgs--; ++ i2c->tx_msg++; ++ i2c->tx_pos = 0; ++ } else ++ first = 0; ++ ++ if (i2c->tx_msg->flags & I2C_M_RD) { ++ /* we dont date putting several reads in the FIFO */ ++ xiic_start_recv(i2c); ++ return; ++ } else { ++ xiic_start_send(i2c); ++ if (xiic_tx_space(i2c) != 0) { ++ /* the message could not be completely sent */ ++ break; ++ } ++ } ++ ++ fifo_space = xiic_tx_fifo_space(i2c); ++ } ++ ++ /* there are more messages or the current one could not be completely ++ * put into the FIFO, also enable the half empty interrupt ++ */ ++ if (i2c->nmsgs > 1 || xiic_tx_space(i2c)) ++ XIic_mClearEnableIntr(i2c, XIIC_INTR_TX_HALF_MASK); ++ ++} ++ ++static void xiic_start_xfer(struct xiic_i2c *i2c) ++{ ++ unsigned long flags; ++ ++ spin_lock_irqsave(&i2c->lock, flags); ++ xiic_reinit(i2c); ++ XIIC_GINTR_DISABLE(i2c); ++ spin_unlock_irqrestore(&i2c->lock, flags); ++ ++ __xiic_start_xfer(i2c); ++ ++ XIIC_GINTR_ENABLE(i2c); ++} ++ ++static int xiic_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num) ++{ ++ struct xiic_i2c *i2c = i2c_get_adapdata(adap); ++ int err; ++ ++ dev_dbg(adap->dev.parent, "%s entry SR: 0x%x\n", __func__, ++ xiic_getreg8(i2c, XIIC_SR_REG_OFFSET)); ++ ++ err = xiic_busy(i2c); ++ if (err) { ++ xiic_dump_regs(i2c, "bus busy"); ++ return err; ++ } ++ ++ i2c->tx_msg = msgs; ++ i2c->nmsgs = num; ++ ++ xiic_start_xfer(i2c); ++ ++ if (wait_event_timeout(i2c->wait, (i2c->state == STATE_ERROR) || ++ (i2c->state == STATE_DONE), HZ)) ++ return (i2c->state == STATE_DONE) ? num : -EIO; ++ else { ++ xiic_dump_regs(i2c, __func__); ++ i2c->tx_msg = NULL; ++ i2c->rx_msg = NULL; ++ i2c->nmsgs = 0; ++ return -ETIMEDOUT; ++ } ++} ++ ++static u32 xiic_func(struct i2c_adapter *adap) ++{ ++ return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL; ++} ++ ++static const struct i2c_algorithm xiic_algorithm = { ++ .master_xfer = xiic_xfer, ++ .functionality = xiic_func, ++}; ++ ++static struct i2c_adapter xiic_adapter = { ++ .owner = THIS_MODULE, ++ .name = DRIVER_NAME, ++ .class = I2C_CLASS_HWMON | I2C_CLASS_SPD, ++ .algo = &xiic_algorithm, ++}; ++ ++ ++static int __devinit xiic_i2c_probe(struct platform_device *pdev) ++{ ++ struct xiic_i2c *i2c; ++ struct xiic_i2c_platform_data *pdata; ++ struct resource *res; ++ int ret, irq; ++ u8 i; ++ ++ res = platform_get_resource(pdev, IORESOURCE_MEM, 0); ++ if (!res) ++ return -ENODEV; ++ ++ irq = platform_get_irq(pdev, 0); ++ if (irq < 0) ++ return -ENODEV; ++ ++ pdata = (struct xiic_i2c_platform_data *) pdev->dev.platform_data; ++ if (!pdata) ++ return -ENODEV; ++ ++ i2c = kzalloc(sizeof(*i2c), GFP_KERNEL); ++ if (!i2c) ++ return -ENOMEM; ++ ++ if (!request_mem_region(res->start, resource_size(res), pdev->name)) { ++ dev_err(&pdev->dev, "Memory region busy\n"); ++ ret = -EBUSY; ++ goto request_mem_failed; ++ } ++ ++ i2c->base = ioremap(res->start, resource_size(res)); ++ if (!i2c->base) { ++ dev_err(&pdev->dev, "Unable to map registers\n"); ++ ret = -EIO; ++ goto map_failed; ++ } ++ ++ /* hook up driver to tree */ ++ platform_set_drvdata(pdev, i2c); ++ i2c->adap = xiic_adapter; ++ i2c_set_adapdata(&i2c->adap, i2c); ++ i2c->adap.dev.parent = &pdev->dev; ++ ++ xiic_reinit(i2c); ++ ++ spin_lock_init(&i2c->lock); ++ init_waitqueue_head(&i2c->wait); ++ ret = request_irq(irq, xiic_isr, 0, pdev->name, i2c); ++ if (ret) { ++ dev_err(&pdev->dev, "Cannot claim IRQ\n"); ++ goto request_irq_failed; ++ } ++ ++ /* add i2c adapter to i2c tree */ ++ ret = i2c_add_adapter(&i2c->adap); ++ if (ret) { ++ dev_err(&pdev->dev, "Failed to add adapter\n"); ++ 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: ++ free_irq(irq, i2c); ++request_irq_failed: ++ xiic_deinit(i2c); ++ iounmap(i2c->base); ++map_failed: ++ release_mem_region(res->start, resource_size(res)); ++request_mem_failed: ++ kfree(i2c); ++ ++ return ret; ++} ++ ++static int __devexit xiic_i2c_remove(struct platform_device* pdev) ++{ ++ struct xiic_i2c *i2c = platform_get_drvdata(pdev); ++ struct resource *res; ++ ++ /* remove adapter & data */ ++ i2c_del_adapter(&i2c->adap); ++ ++ xiic_deinit(i2c); ++ ++ platform_set_drvdata(pdev, NULL); ++ ++ free_irq(platform_get_irq(pdev, 0), i2c); ++ ++ iounmap(i2c->base); ++ ++ res = platform_get_resource(pdev, IORESOURCE_MEM, 0); ++ if (res) ++ release_mem_region(res->start, resource_size(res)); ++ ++ kfree(i2c); ++ ++ return 0; ++} ++ ++ ++/* work with hotplug and coldplug */ ++MODULE_ALIAS("platform:"DRIVER_NAME); ++ ++static struct platform_driver xiic_i2c_driver = { ++ .probe = xiic_i2c_probe, ++ .remove = __devexit_p(xiic_i2c_remove), ++ .driver = { ++ .owner = THIS_MODULE, ++ .name = DRIVER_NAME, ++ }, ++}; ++ ++static int __init xiic_i2c_init(void) ++{ ++ return platform_driver_register(&xiic_i2c_driver); ++} ++ ++static void __exit xiic_i2c_exit(void) ++{ ++ platform_driver_unregister(&xiic_i2c_driver); ++} ++ ++module_init(xiic_i2c_init); ++module_exit(xiic_i2c_exit); ++ ++MODULE_AUTHOR("info@mocean-labs.com"); ++MODULE_DESCRIPTION("Xilinx I2C bus driver"); ++MODULE_LICENSE("GPL v2"); +diff -uNr linux-2.6.31/drivers/i2c/busses/Kconfig linux-2.6.31.new/drivers/i2c/busses/Kconfig +--- linux-2.6.31/drivers/i2c/busses/Kconfig 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/i2c/busses/Kconfig 2009-10-23 11:17:29.000000000 -0700 +@@ -433,6 +433,16 @@ + This driver can also be built as a module. If so, the module + will be called i2c-ocores. + ++config I2C_XILINX ++ tristate "Xilinx I2C Controller" ++ depends on EXPERIMENTAL && HAS_IOMEM ++ help ++ If you say yes to this option, support will be included for the ++ Xilinx I2C controller. ++ ++ This driver can also be built as a module. If so, the module ++ will be called xilinx_i2c. ++ + config I2C_OMAP + tristate "OMAP I2C adapter" + depends on ARCH_OMAP +diff -uNr linux-2.6.31/drivers/i2c/busses/Makefile linux-2.6.31.new/drivers/i2c/busses/Makefile +--- linux-2.6.31/drivers/i2c/busses/Makefile 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/i2c/busses/Makefile 2009-10-23 11:17:29.000000000 -0700 +@@ -40,6 +40,7 @@ + obj-$(CONFIG_I2C_MPC) += i2c-mpc.o + obj-$(CONFIG_I2C_MV64XXX) += i2c-mv64xxx.o + obj-$(CONFIG_I2C_OCORES) += i2c-ocores.o ++obj-$(CONFIG_I2C_XILINX) += i2c-xiic.o + obj-$(CONFIG_I2C_OMAP) += i2c-omap.o + obj-$(CONFIG_I2C_PASEMI) += i2c-pasemi.o + obj-$(CONFIG_I2C_PNX) += i2c-pnx.o +diff -uNr linux-2.6.31/drivers/input/touchscreen/tsc2007.c linux-2.6.31.new/drivers/input/touchscreen/tsc2007.c +--- linux-2.6.31/drivers/input/touchscreen/tsc2007.c 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/input/touchscreen/tsc2007.c 2009-10-23 11:17:19.000000000 -0700 +@@ -21,15 +21,14 @@ + */ + + #include +-#include + #include + #include + #include + #include + #include + +-#define TS_POLL_DELAY (10 * 1000) /* ns delay before the first sample */ +-#define TS_POLL_PERIOD (5 * 1000) /* ns delay between samples */ ++#define TS_POLL_DELAY 1 /* ms delay between samples */ ++#define TS_POLL_PERIOD 1 /* ms delay between samples */ + + #define TSC2007_MEASURE_TEMP0 (0x0 << 4) + #define TSC2007_MEASURE_AUX (0x2 << 4) +@@ -70,17 +69,15 @@ + struct tsc2007 { + struct input_dev *input; + char phys[32]; +- struct hrtimer timer; +- struct ts_event tc; ++ struct delayed_work work; + + struct i2c_client *client; + +- spinlock_t lock; +- + u16 model; + u16 x_plate_ohms; + +- unsigned pendown; ++ bool pendown; ++ bool ignore_first_irq; + int irq; + + int (*get_pendown_state)(void); +@@ -109,52 +106,96 @@ + return val; + } + +-static void tsc2007_send_event(void *tsc) ++static void tsc2007_read_values(struct tsc2007 *tsc, struct ts_event *tc) ++{ ++ /* y- still on; turn on only y+ (and ADC) */ ++ tc->y = tsc2007_xfer(tsc, READ_Y); ++ ++ /* turn y- off, x+ on, then leave in lowpower */ ++ tc->x = tsc2007_xfer(tsc, READ_X); ++ ++ /* turn y+ off, x- on; we'll use formula #1 */ ++ tc->z1 = tsc2007_xfer(tsc, READ_Z1); ++ tc->z2 = tsc2007_xfer(tsc, READ_Z2); ++ ++ /* Prepare for next touch reading - power down ADC, enable PENIRQ */ ++ tsc2007_xfer(tsc, PWRDOWN); ++} ++ ++static u32 tsc2007_calculate_pressure(struct tsc2007 *tsc, struct ts_event *tc) + { +- struct tsc2007 *ts = tsc; +- u32 rt; +- u16 x, y, z1, z2; +- +- x = ts->tc.x; +- y = ts->tc.y; +- z1 = ts->tc.z1; +- z2 = ts->tc.z2; ++ u32 rt = 0; + + /* range filtering */ +- if (x == MAX_12BIT) +- x = 0; ++ if (tc->x == MAX_12BIT) ++ tc->x = 0; + +- if (likely(x && z1)) { ++ if (likely(tc->x && tc->z1)) { + /* compute touch pressure resistance using equation #1 */ +- rt = z2; +- rt -= z1; +- rt *= x; +- rt *= ts->x_plate_ohms; +- rt /= z1; ++ rt = tc->z2 - tc->z1; ++ rt *= tc->x; ++ rt *= tsc->x_plate_ohms; ++ rt /= tc->z1; + rt = (rt + 2047) >> 12; +- } else +- rt = 0; ++ } ++ ++ return rt; ++} ++ ++static void tsc2007_send_up_event(struct tsc2007 *tsc) ++{ ++ struct input_dev *input = tsc->input; ++ ++ dev_dbg(&tsc->client->dev, "UP\n"); + +- /* 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 ++ input_report_key(input, BTN_TOUCH, 0); ++ input_report_abs(input, ABS_PRESSURE, 0); ++ input_sync(input); ++} ++ ++static void tsc2007_work(struct work_struct *work) ++{ ++ struct tsc2007 *ts = ++ container_of(to_delayed_work(work), struct tsc2007, work); ++ struct ts_event tc; ++ u32 rt; ++ ++ /* ++ * NOTE: We can't rely on the pressure to determine the pen down ++ * state, even though 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 ++ * work function by reading the pen signal state (it's a GPIO ++ * and IRQ). Unfortunately such callback is not always available, ++ * in that case we have rely on the pressure anyway. + */ ++ if (ts->get_pendown_state) { ++ if (unlikely(!ts->get_pendown_state())) { ++ tsc2007_send_up_event(ts); ++ ts->pendown = false; ++ goto out; ++ } ++ ++ dev_dbg(&ts->client->dev, "pen is still down\n"); ++ } ++ ++ tsc2007_read_values(ts, &tc); ++ ++ rt = tsc2007_calculate_pressure(ts, &tc); + if (rt > MAX_12BIT) { ++ /* ++ * 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. ++ */ + dev_dbg(&ts->client->dev, "ignored pressure %d\n", rt); ++ goto out; + +- hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_PERIOD), +- HRTIMER_MODE_REL); +- 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) { + struct input_dev *input = ts->input; + +@@ -162,102 +203,82 @@ + dev_dbg(&ts->client->dev, "DOWN\n"); + + input_report_key(input, BTN_TOUCH, 1); +- ts->pendown = 1; ++ ts->pendown = true; + } + +- input_report_abs(input, ABS_X, x); +- input_report_abs(input, ABS_Y, y); ++ input_report_abs(input, ABS_X, tc.x); ++ input_report_abs(input, ABS_Y, tc.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); +- } +- +- hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_PERIOD), +- HRTIMER_MODE_REL); +-} +- +-static int tsc2007_read_values(struct tsc2007 *tsc) +-{ +- /* y- still on; turn on only y+ (and ADC) */ +- tsc->tc.y = tsc2007_xfer(tsc, READ_Y); +- +- /* turn y- off, x+ on, then leave in lowpower */ +- tsc->tc.x = tsc2007_xfer(tsc, READ_X); +- +- /* turn y+ off, x- on; we'll use formula #1 */ +- tsc->tc.z1 = tsc2007_xfer(tsc, READ_Z1); +- tsc->tc.z2 = tsc2007_xfer(tsc, READ_Z2); +- +- /* power down */ +- tsc2007_xfer(tsc, PWRDOWN); +- +- return 0; +-} +- +-static enum hrtimer_restart tsc2007_timer(struct hrtimer *handle) +-{ +- struct tsc2007 *ts = container_of(handle, struct tsc2007, timer); +- unsigned long flags; +- +- spin_lock_irqsave(&ts->lock, flags); +- +- if (unlikely(!ts->get_pendown_state() && ts->pendown)) { +- struct input_dev *input = ts->input; +- +- dev_dbg(&ts->client->dev, "UP\n"); ++ tc.x, tc.y, rt); + +- input_report_key(input, BTN_TOUCH, 0); +- input_report_abs(input, ABS_PRESSURE, 0); +- input_sync(input); ++ } else if (!ts->get_pendown_state && ts->pendown) { ++ /* ++ * We don't have callback to check pendown state, so we ++ * have to assume that since pressure reported is 0 the ++ * pen was lifted up. ++ */ ++ tsc2007_send_up_event(ts); ++ ts->pendown = false; ++ } + +- ts->pendown = 0; ++ out: ++ if (ts->pendown) ++ schedule_delayed_work(&ts->work, ++ msecs_to_jiffies(TS_POLL_PERIOD)); ++ else { ++ if (!ts->get_pendown_state) ++ ts->ignore_first_irq = 1; + enable_irq(ts->irq); +- } else { +- /* pen is still down, continue with the measurement */ +- dev_dbg(&ts->client->dev, "pen is still down\n"); +- +- tsc2007_read_values(ts); +- tsc2007_send_event(ts); + } +- +- spin_unlock_irqrestore(&ts->lock, flags); +- +- return HRTIMER_NORESTART; + } + + static irqreturn_t tsc2007_irq(int irq, void *handle) + { + struct tsc2007 *ts = handle; +- unsigned long flags; + +- spin_lock_irqsave(&ts->lock, flags); ++ if (ts->ignore_first_irq) { ++ ts->ignore_first_irq = 0; ++ return IRQ_HANDLED; ++ } + +- if (likely(ts->get_pendown_state())) { ++ if (!ts->get_pendown_state || likely(ts->get_pendown_state())) { + disable_irq_nosync(ts->irq); +- hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_DELAY), +- HRTIMER_MODE_REL); ++ schedule_delayed_work(&ts->work, ++ msecs_to_jiffies(TS_POLL_DELAY)); + } + + if (ts->clear_penirq) + ts->clear_penirq(); + +- spin_unlock_irqrestore(&ts->lock, flags); +- + return IRQ_HANDLED; + } + +-static int tsc2007_probe(struct i2c_client *client, +- const struct i2c_device_id *id) ++static void tsc2007_free_irq(struct tsc2007 *ts) ++{ ++ free_irq(ts->irq, ts); ++ if (cancel_delayed_work_sync(&ts->work)) { ++ /* ++ * Work was pending, therefore we need to enable ++ * IRQ here to balance the disable_irq() done in the ++ * interrupt handler. ++ */ ++ enable_irq(ts->irq); ++ } ++} ++ ++static int __devinit tsc2007_probe(struct i2c_client *client, ++ const struct i2c_device_id *id) + { + struct tsc2007 *ts; + struct tsc2007_platform_data *pdata = pdata = client->dev.platform_data; + struct input_dev *input_dev; + int err; + +- if (!pdata || !pdata->get_pendown_state) { ++ if (!pdata) { + dev_err(&client->dev, "platform data is required!\n"); + return -EINVAL; + } +@@ -274,22 +295,15 @@ + } + + ts->client = client; +- i2c_set_clientdata(client, ts); +- ++ ts->irq = client->irq; + ts->input = input_dev; +- +- hrtimer_init(&ts->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); +- ts->timer.function = tsc2007_timer; +- +- spin_lock_init(&ts->lock); ++ INIT_DELAYED_WORK(&ts->work, tsc2007_work); + + ts->model = pdata->model; + ts->x_plate_ohms = pdata->x_plate_ohms; + ts->get_pendown_state = pdata->get_pendown_state; + ts->clear_penirq = pdata->clear_penirq; + +- pdata->init_platform_hw(); +- + snprintf(ts->phys, sizeof(ts->phys), + "%s/input0", dev_name(&client->dev)); + +@@ -304,9 +318,8 @@ + 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); + +- tsc2007_read_values(ts); +- +- ts->irq = client->irq; ++ if (pdata->init_platform_hw) ++ pdata->init_platform_hw(); + + err = request_irq(ts->irq, tsc2007_irq, 0, + client->dev.driver->name, ts); +@@ -319,29 +332,37 @@ + if (err) + goto err_free_irq; + +- dev_info(&client->dev, "registered with irq (%d)\n", ts->irq); ++ i2c_set_clientdata(client, ts); ++ ++ /* Prepare for touch readings - power down ADC and enable PENIRQ */ ++ err = tsc2007_xfer(ts, PWRDOWN); ++ if (err < 0) ++ goto err_unreg_dev; + + return 0; + ++ err_unreg_dev: ++ input_unregister_device(ts->input); + err_free_irq: +- free_irq(ts->irq, ts); +- hrtimer_cancel(&ts->timer); ++ tsc2007_free_irq(ts); ++ if (pdata->exit_platform_hw) ++ pdata->exit_platform_hw(); + err_free_mem: + input_free_device(input_dev); + kfree(ts); + return err; + } + +-static int tsc2007_remove(struct i2c_client *client) ++static int __devexit tsc2007_remove(struct i2c_client *client) + { + struct tsc2007 *ts = i2c_get_clientdata(client); +- struct tsc2007_platform_data *pdata; ++ struct tsc2007_platform_data *pdata = client->dev.platform_data; + +- pdata = client->dev.platform_data; +- pdata->exit_platform_hw(); ++ tsc2007_free_irq(ts); ++ ++ if (pdata->exit_platform_hw) ++ pdata->exit_platform_hw(); + +- free_irq(ts->irq, ts); +- hrtimer_cancel(&ts->timer); + input_unregister_device(ts->input); + kfree(ts); + +@@ -362,7 +383,7 @@ + }, + .id_table = tsc2007_idtable, + .probe = tsc2007_probe, +- .remove = tsc2007_remove, ++ .remove = __devexit_p(tsc2007_remove), + }; + + static int __init tsc2007_init(void) +diff -uNr linux-2.6.31/drivers/media/radio/Kconfig linux-2.6.31.new/drivers/media/radio/Kconfig +--- linux-2.6.31/drivers/media/radio/Kconfig 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/media/radio/Kconfig 2009-10-23 11:17:28.000000000 -0700 +@@ -406,4 +406,38 @@ + Say Y here if TEA5764 have a 32768 Hz crystal in circuit, say N + here if TEA5764 reference frequency is connected in FREQIN. + ++config RADIO_SAA7706H ++ tristate "SAA7706H Car Radio DSP" ++ depends on I2C && VIDEO_V4L2 ++ ---help--- ++ Say Y here if you want to use the SAA7706H Car radio Digital ++ Signal Processor, found for instance on the Russellville development ++ board. On the russellville the device is connected to internal ++ timberdale I2C bus. ++ ++ To compile this driver as a module, choose M here: the ++ module will be called SAA7706H. ++ ++config RADIO_TEF6862 ++ tristate "TEF6862 Car Radio Enhanced Selectivity Tuner" ++ depends on I2C && VIDEO_V4L2 ++ ---help--- ++ Say Y here if you want to use the TEF6862 Car Radio Enhanced ++ Selectivity Tuner, found for instance on the Russellville development ++ board. On the russellville the device is connected to internal ++ timberdale I2C bus. ++ ++ To compile this driver as a module, choose M here: the ++ module will be called TEF6862. ++ ++config RADIO_TIMBERDALE ++ tristate "Enable the Timberdale radio driver" ++ depends on MFD_TIMBERDALE && VIDEO_V4L2 && HAS_IOMEM ++ select RADIO_TEF6862 ++ select RADIO_SAA7706H ++ ---help--- ++ This is a kind of umbrella driver for the Radio Tuner and DSP ++ found behind the Timberdale FPGA on the Russellville board. ++ Enable this driver will automatically select the DSP and tuner. ++ + endif # RADIO_ADAPTERS +diff -uNr linux-2.6.31/drivers/media/radio/Makefile linux-2.6.31.new/drivers/media/radio/Makefile +--- linux-2.6.31/drivers/media/radio/Makefile 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/media/radio/Makefile 2009-10-23 11:17:28.000000000 -0700 +@@ -20,5 +20,8 @@ + obj-$(CONFIG_USB_SI470X) += radio-si470x.o + obj-$(CONFIG_USB_MR800) += radio-mr800.o + obj-$(CONFIG_RADIO_TEA5764) += radio-tea5764.o ++obj-$(CONFIG_RADIO_SAA7706H) += saa7706h.o ++obj-$(CONFIG_RADIO_TEF6862) += tef6862.o ++obj-$(CONFIG_RADIO_TIMBERDALE) += radio-timb.o + + EXTRA_CFLAGS += -Isound +diff -uNr linux-2.6.31/drivers/media/radio/radio-timb.c linux-2.6.31.new/drivers/media/radio/radio-timb.c +--- linux-2.6.31/drivers/media/radio/radio-timb.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/media/radio/radio-timb.c 2009-10-23 11:17:28.000000000 -0700 +@@ -0,0 +1,545 @@ ++/* ++ * radio-timb.c Timberdale FPGA Radio 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 ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#define DRIVER_NAME "timb-radio" ++ ++#define RDS_BLOCK_SIZE 4 ++#define RDS_BUFFER_SIZE (RDS_BLOCK_SIZE * 100) ++ ++struct timbradio { ++ struct mutex lock; /* for mutual exclusion */ ++ void __iomem *membase; ++ struct timb_radio_platform_data pdata; ++ struct v4l2_subdev *sd_tuner; ++ struct module *tuner_owner; ++ struct v4l2_subdev *sd_dsp; ++ struct module *dsp_owner; ++ struct video_device *video_dev; ++ /* RDS related */ ++ int open_count; ++ int rds_irq; ++ wait_queue_head_t read_queue; ++ unsigned char buffer[RDS_BUFFER_SIZE]; ++ unsigned int rd_index; ++ unsigned int wr_index; ++}; ++ ++ ++static int timbradio_vidioc_querycap(struct file *file, void *priv, ++ struct v4l2_capability *v) ++{ ++ strlcpy(v->driver, DRIVER_NAME, sizeof(v->driver)); ++ strlcpy(v->card, "Timberdale Radio", sizeof(v->card)); ++ snprintf(v->bus_info, sizeof(v->bus_info), "platform:"DRIVER_NAME); ++ v->version = KERNEL_VERSION(0, 0, 1); ++ v->capabilities = V4L2_CAP_TUNER | V4L2_CAP_RADIO; ++ return 0; ++} ++ ++static int timbradio_vidioc_g_tuner(struct file *file, void *priv, ++ struct v4l2_tuner *v) ++{ ++ struct timbradio *tr = video_drvdata(file); ++ int ret; ++ ++ mutex_lock(&tr->lock); ++ ret = v4l2_subdev_call(tr->sd_tuner, tuner, g_tuner, v); ++ mutex_unlock(&tr->lock); ++ ++ return ret; ++} ++ ++static int timbradio_vidioc_s_tuner(struct file *file, void *priv, ++ struct v4l2_tuner *v) ++{ ++ struct timbradio *tr = video_drvdata(file); ++ int ret; ++ ++ mutex_lock(&tr->lock); ++ ret = v4l2_subdev_call(tr->sd_tuner, tuner, s_tuner, v); ++ mutex_unlock(&tr->lock); ++ ++ return ret; ++} ++ ++static int timbradio_vidioc_g_input(struct file *filp, void *priv, ++ unsigned int *i) ++{ ++ *i = 0; ++ return 0; ++} ++ ++static int timbradio_vidioc_s_input(struct file *filp, void *priv, ++ unsigned int i) ++{ ++ return i ? -EINVAL : 0; ++} ++ ++static int timbradio_vidioc_g_audio(struct file *file, void *priv, ++ struct v4l2_audio *a) ++{ ++ a->index = 0; ++ strlcpy(a->name, "Radio", sizeof(a->name)); ++ a->capability = V4L2_AUDCAP_STEREO; ++ return 0; ++} ++ ++ ++static int timbradio_vidioc_s_audio(struct file *file, void *priv, ++ struct v4l2_audio *a) ++{ ++ return a->index ? -EINVAL : 0; ++} ++ ++static int timbradio_vidioc_s_frequency(struct file *file, void *priv, ++ struct v4l2_frequency *f) ++{ ++ struct timbradio *tr = video_drvdata(file); ++ int ret; ++ ++ mutex_lock(&tr->lock); ++ ret = v4l2_subdev_call(tr->sd_tuner, tuner, s_frequency, f); ++ mutex_unlock(&tr->lock); ++ ++ return ret; ++} ++ ++static int timbradio_vidioc_g_frequency(struct file *file, void *priv, ++ struct v4l2_frequency *f) ++{ ++ struct timbradio *tr = video_drvdata(file); ++ int ret; ++ ++ mutex_lock(&tr->lock); ++ ret = v4l2_subdev_call(tr->sd_tuner, tuner, g_frequency, f); ++ mutex_unlock(&tr->lock); ++ ++ return ret; ++} ++ ++static int timbradio_vidioc_queryctrl(struct file *file, void *priv, ++ struct v4l2_queryctrl *qc) ++{ ++ struct timbradio *tr = video_drvdata(file); ++ int ret; ++ ++ mutex_lock(&tr->lock); ++ ret = v4l2_subdev_call(tr->sd_dsp, core, queryctrl, qc); ++ mutex_unlock(&tr->lock); ++ ++ return ret; ++} ++ ++static int timbradio_vidioc_g_ctrl(struct file *file, void *priv, ++ struct v4l2_control *ctrl) ++{ ++ struct timbradio *tr = video_drvdata(file); ++ int ret; ++ ++ mutex_lock(&tr->lock); ++ ret = v4l2_subdev_call(tr->sd_dsp, core, g_ctrl, ctrl); ++ mutex_unlock(&tr->lock); ++ ++ return ret; ++} ++ ++static int timbradio_vidioc_s_ctrl(struct file *file, void *priv, ++ struct v4l2_control *ctrl) ++{ ++ struct timbradio *tr = video_drvdata(file); ++ int ret; ++ ++ mutex_lock(&tr->lock); ++ ret = v4l2_subdev_call(tr->sd_dsp, core, s_ctrl, ctrl); ++ mutex_unlock(&tr->lock); ++ ++ return ret; ++} ++ ++static const struct v4l2_ioctl_ops timbradio_ioctl_ops = { ++ .vidioc_querycap = timbradio_vidioc_querycap, ++ .vidioc_g_tuner = timbradio_vidioc_g_tuner, ++ .vidioc_s_tuner = timbradio_vidioc_s_tuner, ++ .vidioc_g_frequency = timbradio_vidioc_g_frequency, ++ .vidioc_s_frequency = timbradio_vidioc_s_frequency, ++ .vidioc_g_input = timbradio_vidioc_g_input, ++ .vidioc_s_input = timbradio_vidioc_s_input, ++ .vidioc_g_audio = timbradio_vidioc_g_audio, ++ .vidioc_s_audio = timbradio_vidioc_s_audio, ++ .vidioc_queryctrl = timbradio_vidioc_queryctrl, ++ .vidioc_g_ctrl = timbradio_vidioc_g_ctrl, ++ .vidioc_s_ctrl = timbradio_vidioc_s_ctrl ++}; ++ ++static irqreturn_t timbradio_irq(int irq, void *devid) ++{ ++ struct timbradio *tr = devid; ++ u32 data = ioread32(tr->membase); ++ ++ tr->buffer[tr->wr_index++] = data >> 24; ++ tr->buffer[tr->wr_index++] = data >> 16; ++ tr->buffer[tr->wr_index++] = data >> 8; ++ tr->buffer[tr->wr_index++] = data; ++ tr->wr_index %= RDS_BUFFER_SIZE; ++ ++ wake_up(&tr->read_queue); ++ ++ /* new RDS data received, read it */ ++ return IRQ_HANDLED; ++} ++ ++/************************************************************************** ++ * File Operations Interface ++ **************************************************************************/ ++ ++static ssize_t timbradio_rds_fops_read(struct file *file, char __user *buf, ++ size_t count, loff_t *ppos) ++{ ++ struct timbradio *tr = video_drvdata(file); ++ int outblocks = 0; ++ ++ /* block if no new data available */ ++ while (tr->wr_index == tr->rd_index) { ++ if (file->f_flags & O_NONBLOCK) ++ return -EWOULDBLOCK; ++ ++ if (wait_event_interruptible(tr->read_queue, ++ tr->wr_index != tr->rd_index)) ++ return -EINTR; ++ } ++ ++ count /= RDS_BLOCK_SIZE; ++ /* copy RDS block out of internal buffer and to user buffer */ ++ mutex_lock(&tr->lock); ++ while (outblocks < count) { ++ if (tr->rd_index == tr->wr_index) ++ break; ++ ++ if (copy_to_user(buf, tr->buffer + tr->rd_index, ++ RDS_BLOCK_SIZE)) ++ break; ++ tr->rd_index += RDS_BLOCK_SIZE; ++ tr->rd_index %= RDS_BUFFER_SIZE; ++ outblocks++; ++ } ++ mutex_unlock(&tr->lock); ++ ++ return outblocks *RDS_BLOCK_SIZE; ++} ++ ++static unsigned int timbradio_rds_fops_poll(struct file *file, ++ struct poll_table_struct *pts) ++{ ++ struct timbradio *tr = video_drvdata(file); ++ ++ poll_wait(file, &tr->read_queue, pts); ++ ++ if (tr->rd_index != tr->wr_index) ++ return POLLIN | POLLRDNORM; ++ ++ return 0; ++} ++ ++struct find_addr_arg { ++ char const *name; ++ struct i2c_client *client; ++}; ++ ++static int find_name(struct device *dev, void *argp) ++{ ++ struct find_addr_arg *arg = (struct find_addr_arg *)argp; ++ struct i2c_client *client = i2c_verify_client(dev); ++ ++ if (client && !strcmp(arg->name, client->name) && client->driver) ++ arg->client = client; ++ ++ return 0; ++} ++ ++static struct i2c_client *find_client(struct i2c_adapter *adapt, ++ const char *name) ++{ ++ struct find_addr_arg find_arg; ++ /* now find the client */ ++#ifdef MODULE ++ request_module(name); ++#endif ++ /* code for finding the I2C child */ ++ find_arg.name = name; ++ find_arg.client = NULL; ++ device_for_each_child(&adapt->dev, &find_arg, find_name); ++ return find_arg.client; ++} ++ ++static int timbradio_rds_fops_open(struct file *file) ++{ ++ struct timbradio *tr = video_drvdata(file); ++ int err = 0; ++ ++ mutex_lock(&tr->lock); ++ if (tr->open_count == 0) { ++ /* device currently not open, check if the DSP and tuner is not ++ * yet found, in that case find them ++ */ ++ if (!tr->sd_tuner) { ++ struct i2c_adapter *adapt; ++ struct i2c_client *tuner; ++ struct i2c_client *dsp; ++ ++ /* find the I2C bus */ ++ adapt = i2c_get_adapter(tr->pdata.i2c_adapter); ++ if (!adapt) { ++ printk(KERN_ERR DRIVER_NAME": No I2C bus\n"); ++ err = -ENODEV; ++ goto out; ++ } ++ ++ /* now find the tuner and dsp */ ++ tuner = find_client(adapt, tr->pdata.tuner); ++ dsp = find_client(adapt, tr->pdata.dsp); ++ ++ i2c_put_adapter(adapt); ++ ++ if (!tuner || !dsp) { ++ printk(KERN_ERR DRIVER_NAME ++ ": Failed to get tuner or DSP\n"); ++ err = -ENODEV; ++ goto out; ++ } ++ ++ tr->sd_tuner = i2c_get_clientdata(tuner); ++ tr->sd_dsp = i2c_get_clientdata(dsp); ++ ++ tr->tuner_owner = tr->sd_tuner->owner; ++ tr->dsp_owner = tr->sd_dsp->owner; ++ /* Lock the modules */ ++ if (!try_module_get(tr->tuner_owner)) { ++ err = -ENODEV; ++ goto err_get_tuner; ++ } ++ ++ if (!try_module_get(tr->dsp_owner)) { ++ err = -ENODEV; ++ goto err_get_dsp; ++ } ++ } ++ ++ /* enable the IRQ for receiving RDS data */ ++ err = request_irq(tr->rds_irq, timbradio_irq, 0, DRIVER_NAME, ++ tr); ++ } ++ goto out; ++ ++err_get_dsp: ++ module_put(tr->tuner_owner); ++err_get_tuner: ++ tr->sd_tuner = NULL; ++ tr->sd_dsp = NULL; ++out: ++ if (!err) ++ tr->open_count++; ++ mutex_unlock(&tr->lock); ++ return err; ++} ++ ++static int timbradio_rds_fops_release(struct file *file) ++{ ++ struct timbradio *tr = video_drvdata(file); ++ ++ mutex_lock(&tr->lock); ++ tr->open_count--; ++ if (!tr->open_count) { ++ free_irq(tr->rds_irq, tr); ++ ++ tr->wr_index = 0; ++ tr->rd_index = 0; ++ ++ /* cancel read processes */ ++ wake_up_interruptible(&tr->read_queue); ++ } ++ mutex_unlock(&tr->lock); ++ ++ return 0; ++} ++ ++ ++static const struct v4l2_file_operations timbradio_fops = { ++ .owner = THIS_MODULE, ++ .ioctl = video_ioctl2, ++ .read = timbradio_rds_fops_read, ++ .poll = timbradio_rds_fops_poll, ++ .open = timbradio_rds_fops_open, ++ .release = timbradio_rds_fops_release, ++}; ++ ++static const struct video_device timbradio_template = { ++ .name = "Timberdale Radio", ++ .fops = &timbradio_fops, ++ .ioctl_ops = &timbradio_ioctl_ops, ++ .release = video_device_release_empty, ++ .minor = -1 ++}; ++ ++ ++ ++static int timbradio_probe(struct platform_device *pdev) ++{ ++ struct timb_radio_platform_data *pdata = pdev->dev.platform_data; ++ struct timbradio *tr; ++ struct resource *iomem; ++ int irq; ++ int err; ++ ++ if (!pdata) { ++ printk(KERN_ERR DRIVER_NAME": Platform data missing\n"); ++ err = -EINVAL; ++ goto err; ++ } ++ ++ iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0); ++ if (!iomem) { ++ err = -ENODEV; ++ goto err; ++ } ++ ++ irq = platform_get_irq(pdev, 0); ++ if (irq < 0) { ++ err = -ENODEV; ++ goto err; ++ } ++ ++ if (!request_mem_region(iomem->start, resource_size(iomem), ++ DRIVER_NAME)) { ++ err = -EBUSY; ++ goto err; ++ } ++ ++ tr = kzalloc(sizeof(*tr), GFP_KERNEL); ++ if (!tr) { ++ err = -ENOMEM; ++ goto err_alloc; ++ } ++ mutex_init(&tr->lock); ++ ++ tr->membase = ioremap(iomem->start, resource_size(iomem)); ++ if (!tr->membase) { ++ err = -ENOMEM; ++ goto err_ioremap; ++ } ++ ++ memcpy(&tr->pdata, pdata, sizeof(tr->pdata)); ++ ++ tr->video_dev = video_device_alloc(); ++ if (!tr->video_dev) { ++ err = -ENOMEM; ++ goto err_video_req; ++ } ++ *tr->video_dev = timbradio_template; ++ tr->rds_irq = irq; ++ init_waitqueue_head(&tr->read_queue); ++ ++ err = video_register_device(tr->video_dev, VFL_TYPE_RADIO, -1); ++ if (err) { ++ printk(KERN_ALERT DRIVER_NAME": Error reg video\n"); ++ goto err_video_req; ++ } ++ ++ video_set_drvdata(tr->video_dev, tr); ++ ++ platform_set_drvdata(pdev, tr); ++ return 0; ++ ++err_video_req: ++ if (tr->video_dev->minor != -1) ++ video_unregister_device(tr->video_dev); ++ else ++ video_device_release(tr->video_dev); ++ iounmap(tr->membase); ++err_ioremap: ++ kfree(tr); ++err_alloc: ++ release_mem_region(iomem->start, resource_size(iomem)); ++err: ++ printk(KERN_ERR DRIVER_NAME ": Failed to register: %d\n", err); ++ ++ return err; ++} ++ ++static int timbradio_remove(struct platform_device *pdev) ++{ ++ struct resource *iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0); ++ struct timbradio *tr = platform_get_drvdata(pdev); ++ ++ if (tr->video_dev->minor != -1) ++ video_unregister_device(tr->video_dev); ++ else ++ video_device_release(tr->video_dev); ++ ++ if (tr->sd_tuner) { ++ module_put(tr->tuner_owner); ++ module_put(tr->dsp_owner); ++ } ++ ++ iounmap(tr->membase); ++ release_mem_region(iomem->start, resource_size(iomem)); ++ kfree(tr); ++ ++ return 0; ++} ++ ++static struct platform_driver timbradio_platform_driver = { ++ .driver = { ++ .name = DRIVER_NAME, ++ .owner = THIS_MODULE, ++ }, ++ .probe = timbradio_probe, ++ .remove = timbradio_remove, ++}; ++ ++/*--------------------------------------------------------------------------*/ ++ ++static int __init timbradio_init(void) ++{ ++ return platform_driver_register(&timbradio_platform_driver); ++} ++ ++static void __exit timbradio_exit(void) ++{ ++ platform_driver_unregister(&timbradio_platform_driver); ++} ++ ++module_init(timbradio_init); ++module_exit(timbradio_exit); ++ ++MODULE_DESCRIPTION("Timberdale Radio driver"); ++MODULE_AUTHOR("Mocean Laboratories "); ++MODULE_LICENSE("GPL v2"); ++MODULE_ALIAS("platform:"DRIVER_NAME); ++ +diff -uNr linux-2.6.31/drivers/media/radio/saa7706h.c linux-2.6.31.new/drivers/media/radio/saa7706h.c +--- linux-2.6.31/drivers/media/radio/saa7706h.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/media/radio/saa7706h.c 2009-10-23 11:17:28.000000000 -0700 +@@ -0,0 +1,496 @@ ++/* ++ * saa7706.c Philips SAA7706H Car Radio DSP 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 ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#define DRIVER_NAME "saa7706h" ++ ++/* the I2C memory map looks like this ++ ++ $1C00 - $FFFF Not Used ++ $2200 - $3FFF Reserved YRAM (DSP2) space ++ $2000 - $21FF YRAM (DSP2) ++ $1FF0 - $1FFF Hardware Registers ++ $1280 - $1FEF Reserved XRAM (DSP2) space ++ $1000 - $127F XRAM (DSP2) ++ $0FFF DSP CONTROL ++ $0A00 - $0FFE Reserved ++ $0980 - $09FF Reserved YRAM (DSP1) space ++ $0800 - $097F YRAM (DSP1) ++ $0200 - $07FF Not Used ++ $0180 - $01FF Reserved XRAM (DSP1) space ++ $0000 - $017F XRAM (DSP1) ++*/ ++ ++#define SAA7706H_REG_CTRL 0x0fff ++#define SAA7706H_CTRL_BYP_PLL 0x0001 ++#define SAA7706H_CTRL_PLL_DIV_MASK 0x003e ++#define SAA7706H_CTRL_PLL3_62975MHZ 0x003e ++#define SAA7706H_CTRL_DSP_TURBO 0x0040 ++#define SAA7706H_CTRL_PC_RESET_DSP1 0x0080 ++#define SAA7706H_CTRL_PC_RESET_DSP2 0x0100 ++#define SAA7706H_CTRL_DSP1_ROM_EN_MASK 0x0600 ++#define SAA7706H_CTRL_DSP1_FUNC_PROM 0x0000 ++#define SAA7706H_CTRL_DSP2_ROM_EN_MASK 0x1800 ++#define SAA7706H_CTRL_DSP2_FUNC_PROM 0x0000 ++#define SAA7706H_CTRL_DIG_SIL_INTERPOL 0x8000 ++ ++#define SAA7706H_REG_EVALUATION 0x1ff0 ++#define SAA7706H_EVAL_DISABLE_CHARGE_PUMP 0x000001 ++#define SAA7706H_EVAL_DCS_CLOCK 0x000002 ++#define SAA7706H_EVAL_GNDRC1_ENABLE 0x000004 ++#define SAA7706H_EVAL_GNDRC2_ENABLE 0x000008 ++ ++#define SAA7706H_REG_CL_GEN1 0x1ff3 ++#define SAA7706H_CL_GEN1_MIN_LOOPGAIN_MASK 0x00000f ++#define SAA7706H_CL_GEN1_LOOPGAIN_MASK 0x0000f0 ++#define SAA7706H_CL_GEN1_COARSE_RATION 0xffff00 ++ ++#define SAA7706H_REG_CL_GEN2 0x1ff4 ++#define SAA7706H_CL_GEN2_WSEDGE_FALLING 0x000001 ++#define SAA7706H_CL_GEN2_STOP_VCO 0x000002 ++#define SAA7706H_CL_GEN2_FRERUN 0x000004 ++#define SAA7706H_CL_GEN2_ADAPTIVE 0x000008 ++#define SAA7706H_CL_GEN2_FINE_RATIO_MASK 0x0ffff0 ++ ++#define SAA7706H_REG_CL_GEN4 0x1ff6 ++#define SAA7706H_CL_GEN4_BYPASS_PLL1 0x001000 ++#define SAA7706H_CL_GEN4_PLL1_DIV_MASK 0x03e000 ++#define SAA7706H_CL_GEN4_DSP1_TURBO 0x040000 ++ ++#define SAA7706H_REG_SEL 0x1ff7 ++#define SAA7706H_SEL_DSP2_SRCA_MASK 0x000007 ++#define SAA7706H_SEL_DSP2_FMTA_MASK 0x000031 ++#define SAA7706H_SEL_DSP2_SRCB_MASK 0x0001c0 ++#define SAA7706H_SEL_DSP2_FMTB_MASK 0x000e00 ++#define SAA7706H_SEL_DSP1_SRC_MASK 0x003000 ++#define SAA7706H_SEL_DSP1_FMT_MASK 0x01c003 ++#define SAA7706H_SEL_SPDIF2 0x020000 ++#define SAA7706H_SEL_HOST_IO_FMT_MASK 0x1c0000 ++#define SAA7706H_SEL_EN_HOST_IO 0x200000 ++ ++#define SAA7706H_REG_IAC 0x1ff8 ++#define SAA7706H_REG_CLK_SET 0x1ff9 ++#define SAA7706H_REG_CLK_COEFF 0x1ffa ++#define SAA7706H_REG_INPUT_SENS 0x1ffb ++#define SAA7706H_INPUT_SENS_RDS_VOL_MASK 0x0003f ++#define SAA7706H_INPUT_SENS_FM_VOL_MASK 0x00fc0 ++#define SAA7706H_INPUT_SENS_FM_MPX 0x01000 ++#define SAA7706H_INPUT_SENS_OFF_FILTER_A_EN 0x02000 ++#define SAA7706H_INPUT_SENS_OFF_FILTER_B_EN 0x04000 ++#define SAA7706H_REG_PHONE_NAV_AUDIO 0x1ffc ++#define SAA7706H_REG_IO_CONF_DSP2 0x1ffd ++#define SAA7706H_REG_STATUS_DSP2 0x1ffe ++#define SAA7706H_REG_PC_DSP2 0x1fff ++ ++#define SAA7706H_DSP1_MOD0 0x0800 ++#define SAA7706H_DSP1_ROM_VER 0x097f ++#define SAA7706H_DSP2_MPTR0 0x1000 ++ ++#define SAA7706H_DSP1_MODPNTR 0x0000 ++ ++#define SAA7706H_DSP2_XMEM_CONTLLCW 0x113e ++#define SAA7706H_DSP2_XMEM_BUSAMP 0x114a ++#define SAA7706H_DSP2_XMEM_FDACPNTR 0x11f9 ++#define SAA7706H_DSP2_XMEM_IIS1PNTR 0x11fb ++ ++#define SAA7706H_DSP2_YMEM_PVGA 0x212a ++#define SAA7706H_DSP2_YMEM_PVAT1 0x212b ++#define SAA7706H_DSP2_YMEM_PVAT 0x212c ++#define SAA7706H_DSP2_YMEM_ROM_VER 0x21ff ++ ++#define SUPPORTED_DSP1_ROM_VER 0x667 ++ ++struct saa7706h_state { ++ struct v4l2_subdev sd; ++ unsigned muted; ++}; ++ ++static inline struct saa7706h_state *to_state(struct v4l2_subdev *sd) ++{ ++ return container_of(sd, struct saa7706h_state, sd); ++} ++ ++static int saa7706h_i2c_send(struct i2c_client *client, const u8 *data, int len) ++{ ++ int err = i2c_master_send(client, data, len); ++ if (err == len) ++ return 0; ++ else if (err > 0) ++ return -EIO; ++ return err; ++} ++ ++static int saa7706h_i2c_transfer(struct i2c_client *client, ++ struct i2c_msg *msgs, int num) ++{ ++ int err = i2c_transfer(client->adapter, msgs, num); ++ if (err == num) ++ return 0; ++ else if (err > 0) ++ return -EIO; ++ else ++ return err; ++} ++ ++static int saa7706h_set_reg24(struct i2c_client *client, u16 reg, u32 val) ++{ ++ u8 buf[5]; ++ int pos = 0; ++ ++ buf[pos++] = reg >> 8; ++ buf[pos++] = reg; ++ buf[pos++] = val >> 16; ++ buf[pos++] = val >> 8; ++ buf[pos++] = val; ++ ++ return saa7706h_i2c_send(client, buf, pos); ++} ++ ++static int saa7706h_set_reg16(struct i2c_client *client, u16 reg, u16 val) ++{ ++ u8 buf[4]; ++ int pos = 0; ++ ++ buf[pos++] = reg >> 8; ++ buf[pos++] = reg; ++ buf[pos++] = val >> 8; ++ buf[pos++] = val; ++ ++ return saa7706h_i2c_send(client, buf, pos); ++} ++ ++static int saa7706h_get_reg16(struct i2c_client *client, u16 reg) ++{ ++ u8 buf[2]; ++ int err; ++ u8 regaddr[] = {reg >> 8, reg}; ++ struct i2c_msg msg[] = { {client->addr, 0, sizeof(regaddr), regaddr}, ++ {client->addr, I2C_M_RD, sizeof(buf), buf} }; ++ ++ err = saa7706h_i2c_transfer(client, msg, ARRAY_SIZE(msg)); ++ if (err) ++ return err; ++ ++ return buf[0] << 8 | buf[1]; ++} ++ ++static int saa7706h_unmute(struct v4l2_subdev *sd) ++{ ++ struct i2c_client *client = v4l2_get_subdevdata(sd); ++ struct saa7706h_state *state = to_state(sd); ++ int err; ++ ++ err = saa7706h_set_reg16(client, SAA7706H_REG_CTRL, ++ SAA7706H_CTRL_PLL3_62975MHZ | SAA7706H_CTRL_PC_RESET_DSP1 | ++ SAA7706H_CTRL_PC_RESET_DSP2); ++ if (err) ++ goto out; ++ ++ /* newer versions of the chip requires a small sleep after reset */ ++ msleep(1); ++ ++ err = saa7706h_set_reg16(client, SAA7706H_REG_CTRL, ++ SAA7706H_CTRL_PLL3_62975MHZ); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_EVALUATION, 0); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_CL_GEN1, 0x040022); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_CL_GEN2, ++ SAA7706H_CL_GEN2_WSEDGE_FALLING); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_CL_GEN4, 0x024080); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_SEL, 0x200080); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_IAC, 0xf4caed); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_CLK_SET, 0x124334); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_CLK_COEFF, 0x004a1a); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_INPUT_SENS, 0x0071c7); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_PHONE_NAV_AUDIO, ++ 0x0e22ff); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_IO_CONF_DSP2, 0x001ff8); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_STATUS_DSP2, 0x080003); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_REG_PC_DSP2, 0x000004); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg16(client, SAA7706H_DSP1_MOD0, 0x0c6c); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_DSP2_MPTR0, 0x000b4b); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_DSP1_MODPNTR, 0x000600); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_DSP1_MODPNTR, 0x0000c0); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_DSP2_XMEM_CONTLLCW, 0x000819); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_DSP2_XMEM_CONTLLCW, 0x00085a); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_DSP2_XMEM_BUSAMP, 0x7fffff); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_DSP2_XMEM_FDACPNTR, 0x2000cb); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_DSP2_XMEM_IIS1PNTR, 0x2000cb); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg16(client, SAA7706H_DSP2_YMEM_PVGA, 0x0f80); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg16(client, SAA7706H_DSP2_YMEM_PVAT1, 0x0800); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg16(client, SAA7706H_DSP2_YMEM_PVAT, 0x0800); ++ if (err) ++ goto out; ++ ++ err = saa7706h_set_reg24(client, SAA7706H_DSP2_XMEM_CONTLLCW, 0x000905); ++ if (err) ++ goto out; ++ ++ state->muted = 0; ++out: ++ return err; ++} ++ ++static int saa7706h_mute(struct v4l2_subdev *sd) ++{ ++ struct i2c_client *client = v4l2_get_subdevdata(sd); ++ struct saa7706h_state *state = to_state(sd); ++ int err; ++ ++ err = saa7706h_set_reg16(client, SAA7706H_REG_CTRL, ++ SAA7706H_CTRL_PLL3_62975MHZ | SAA7706H_CTRL_PC_RESET_DSP1 | ++ SAA7706H_CTRL_PC_RESET_DSP2); ++ if (err) ++ goto out; ++ ++ state->muted = 1; ++out: ++ return err; ++} ++ ++static int saa7706h_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc) ++{ ++ switch (qc->id) { ++ case V4L2_CID_AUDIO_MUTE: ++ return v4l2_ctrl_query_fill(qc, 0, 1, 1, 1); ++ } ++ return -EINVAL; ++} ++ ++static int saa7706h_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl) ++{ ++ struct saa7706h_state *state = to_state(sd); ++ ++ switch (ctrl->id) { ++ case V4L2_CID_AUDIO_MUTE: ++ ctrl->value = state->muted; ++ return 0; ++ } ++ return -EINVAL; ++} ++ ++static int saa7706h_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl) ++{ ++ switch (ctrl->id) { ++ case V4L2_CID_AUDIO_MUTE: ++ if (ctrl->value) ++ return saa7706h_mute(sd); ++ else ++ return saa7706h_unmute(sd); ++ } ++ return -EINVAL; ++} ++ ++static int saa7706h_g_chip_ident(struct v4l2_subdev *sd, ++ struct v4l2_dbg_chip_ident *chip) ++{ ++ struct i2c_client *client = v4l2_get_subdevdata(sd); ++ ++ return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_SAA7706H, 0); ++} ++ ++static const struct v4l2_subdev_core_ops saa7706h_core_ops = { ++ .g_chip_ident = saa7706h_g_chip_ident, ++ .queryctrl = saa7706h_queryctrl, ++ .g_ctrl = saa7706h_g_ctrl, ++ .s_ctrl = saa7706h_s_ctrl, ++}; ++ ++static const struct v4l2_subdev_ops saa7706h_ops = { ++ .core = &saa7706h_core_ops, ++}; ++ ++/* ++ * Generic i2c probe ++ * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1' ++ */ ++ ++static int __devinit saa7706h_probe(struct i2c_client *client, ++ const struct i2c_device_id *id) ++{ ++ struct saa7706h_state *state; ++ struct v4l2_subdev *sd; ++ int err; ++ ++ /* Check if the adapter supports the needed features */ ++ if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA)) ++ return -EIO; ++ ++ v4l_info(client, "chip found @ 0x%02x (%s)\n", ++ client->addr << 1, client->adapter->name); ++ ++ state = kmalloc(sizeof(struct saa7706h_state), GFP_KERNEL); ++ if (state == NULL) ++ return -ENOMEM; ++ sd = &state->sd; ++ v4l2_i2c_subdev_init(sd, client, &saa7706h_ops); ++ ++ /* check the rom versions */ ++ err = saa7706h_get_reg16(client, SAA7706H_DSP1_ROM_VER); ++ if (err < 0) ++ goto err; ++ if (err != SUPPORTED_DSP1_ROM_VER) ++ printk(KERN_WARNING DRIVER_NAME ++ ": Unknown DSP1 ROM code version: 0x%x\n", err); ++ ++ state->muted = 1; ++ ++ /* startup in a muted state */ ++ err = saa7706h_mute(sd); ++ if (err) ++ goto err; ++ ++ return 0; ++ ++err: ++ v4l2_device_unregister_subdev(sd); ++ kfree(to_state(sd)); ++ ++ printk(KERN_ERR DRIVER_NAME ": Failed to probe: %d\n", err); ++ ++ return err; ++} ++ ++static int __devexit saa7706h_remove(struct i2c_client *client) ++{ ++ struct v4l2_subdev *sd = i2c_get_clientdata(client); ++ ++ saa7706h_mute(sd); ++ v4l2_device_unregister_subdev(sd); ++ kfree(to_state(sd)); ++ return 0; ++} ++ ++static const struct i2c_device_id saa7706h_id[] = { ++ {DRIVER_NAME, 0}, ++ {}, ++}; ++ ++MODULE_DEVICE_TABLE(i2c, saa7706h_id); ++ ++static struct i2c_driver saa7706h_driver = { ++ .driver = { ++ .owner = THIS_MODULE, ++ .name = DRIVER_NAME, ++ }, ++ .probe = saa7706h_probe, ++ .remove = saa7706h_remove, ++ .id_table = saa7706h_id, ++}; ++ ++static __init int saa7706h_init(void) ++{ ++ return i2c_add_driver(&saa7706h_driver); ++} ++ ++static __exit void saa7706h_exit(void) ++{ ++ i2c_del_driver(&saa7706h_driver); ++} ++ ++module_init(saa7706h_init); ++module_exit(saa7706h_exit); ++ ++MODULE_DESCRIPTION("SAA7706H Car Radio DSP driver"); ++MODULE_AUTHOR("Mocean Laboratories"); ++MODULE_LICENSE("GPL v2"); ++ +diff -uNr linux-2.6.31/drivers/media/radio/tef6862.c linux-2.6.31.new/drivers/media/radio/tef6862.c +--- linux-2.6.31/drivers/media/radio/tef6862.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/media/radio/tef6862.c 2009-10-23 11:17:28.000000000 -0700 +@@ -0,0 +1,232 @@ ++/* ++ * tef6862.c Philips TEF6862 Car Radio Enhanced Selectivity Tuner ++ * 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 ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#define DRIVER_NAME "tef6862" ++ ++#define FREQ_MUL 16000 ++ ++#define TEF6862_LO_FREQ (875 * FREQ_MUL / 10) ++#define TEF6862_HI_FREQ (108 * FREQ_MUL) ++ ++/* Write mode sub addresses */ ++#define WM_SUB_BANDWIDTH 0x0 ++#define WM_SUB_PLLM 0x1 ++#define WM_SUB_PLLL 0x2 ++#define WM_SUB_DAA 0x3 ++#define WM_SUB_AGC 0x4 ++#define WM_SUB_BAND 0x5 ++#define WM_SUB_CONTROL 0x6 ++#define WM_SUB_LEVEL 0x7 ++#define WM_SUB_IFCF 0x8 ++#define WM_SUB_IFCAP 0x9 ++#define WM_SUB_ACD 0xA ++#define WM_SUB_TEST 0xF ++ ++/* Different modes of the MSA register */ ++#define MODE_BUFFER 0x0 ++#define MODE_PRESET 0x1 ++#define MODE_SEARCH 0x2 ++#define MODE_AF_UPDATE 0x3 ++#define MODE_JUMP 0x4 ++#define MODE_CHECK 0x5 ++#define MODE_LOAD 0x6 ++#define MODE_END 0x7 ++#define MODE_SHIFT 5 ++ ++struct tef6862_state { ++ struct v4l2_subdev sd; ++ unsigned long freq; ++}; ++ ++static inline struct tef6862_state *to_state(struct v4l2_subdev *sd) ++{ ++ return container_of(sd, struct tef6862_state, sd); ++} ++ ++static u16 tef6862_sigstr(struct i2c_client *client) ++{ ++ u8 buf[4]; ++ int err = i2c_master_recv(client, buf, sizeof(buf)); ++ if (err == sizeof(buf)) ++ return buf[3] << 8; ++ return 0; ++} ++ ++static int tef6862_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *v) ++{ ++ if (v->index > 0) ++ return -EINVAL; ++ ++ /* only support FM for now */ ++ strlcpy(v->name, "FM", sizeof(v->name)); ++ v->type = V4L2_TUNER_RADIO; ++ v->rangelow = TEF6862_LO_FREQ; ++ v->rangehigh = TEF6862_HI_FREQ; ++ v->rxsubchans = V4L2_TUNER_SUB_MONO; ++ v->capability = V4L2_TUNER_CAP_LOW; ++ v->audmode = V4L2_TUNER_MODE_STEREO; ++ v->signal = tef6862_sigstr(v4l2_get_subdevdata(sd)); ++ ++ return 0; ++} ++ ++static int tef6862_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *v) ++{ ++ return v->index ? -EINVAL : 0; ++} ++ ++static int tef6862_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f) ++{ ++ struct tef6862_state *state = to_state(sd); ++ struct i2c_client *client = v4l2_get_subdevdata(sd); ++ u16 pll; ++ u8 i2cmsg[3]; ++ int err; ++ ++ if (f->tuner != 0) ++ return -EINVAL; ++ ++ pll = 1964 + ((f->frequency - TEF6862_LO_FREQ) * 20) / FREQ_MUL; ++ i2cmsg[0] = (MODE_PRESET << MODE_SHIFT) | WM_SUB_PLLM; ++ i2cmsg[1] = (pll >> 8) & 0xff; ++ i2cmsg[2] = pll & 0xff; ++ ++ err = i2c_master_send(client, i2cmsg, sizeof(i2cmsg)); ++ if (!err) ++ state->freq = f->frequency; ++ return err; ++} ++ ++static int tef6862_g_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f) ++{ ++ struct tef6862_state *state = to_state(sd); ++ ++ if (f->tuner != 0) ++ return -EINVAL; ++ f->type = V4L2_TUNER_RADIO; ++ f->frequency = state->freq; ++ return 0; ++} ++ ++static int tef6862_g_chip_ident(struct v4l2_subdev *sd, ++ struct v4l2_dbg_chip_ident *chip) ++{ ++ struct i2c_client *client = v4l2_get_subdevdata(sd); ++ ++ return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_TEF6862, 0); ++} ++ ++static const struct v4l2_subdev_tuner_ops tef6862_tuner_ops = { ++ .g_tuner = tef6862_g_tuner, ++ .s_tuner = tef6862_s_tuner, ++ .s_frequency = tef6862_s_frequency, ++ .g_frequency = tef6862_g_frequency, ++}; ++ ++static const struct v4l2_subdev_core_ops tef6862_core_ops = { ++ .g_chip_ident = tef6862_g_chip_ident, ++}; ++ ++static const struct v4l2_subdev_ops tef6862_ops = { ++ .core = &tef6862_core_ops, ++ .tuner = &tef6862_tuner_ops, ++}; ++ ++/* ++ * Generic i2c probe ++ * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1' ++ */ ++ ++static int __devinit tef6862_probe(struct i2c_client *client, ++ const struct i2c_device_id *id) ++{ ++ struct tef6862_state *state; ++ struct v4l2_subdev *sd; ++ ++ /* Check if the adapter supports the needed features */ ++ if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA)) ++ return -EIO; ++ ++ v4l_info(client, "chip found @ 0x%02x (%s)\n", ++ client->addr << 1, client->adapter->name); ++ ++ state = kmalloc(sizeof(struct tef6862_state), GFP_KERNEL); ++ if (state == NULL) ++ return -ENOMEM; ++ state->freq = TEF6862_LO_FREQ; ++ ++ sd = &state->sd; ++ v4l2_i2c_subdev_init(sd, client, &tef6862_ops); ++ ++ return 0; ++} ++ ++static int __devexit tef6862_remove(struct i2c_client *client) ++{ ++ struct v4l2_subdev *sd = i2c_get_clientdata(client); ++ ++ v4l2_device_unregister_subdev(sd); ++ kfree(to_state(sd)); ++ return 0; ++} ++ ++static const struct i2c_device_id tef6862_id[] = { ++ {DRIVER_NAME, 0}, ++ {}, ++}; ++ ++MODULE_DEVICE_TABLE(i2c, tef6862_id); ++ ++static struct i2c_driver tef6862_driver = { ++ .driver = { ++ .owner = THIS_MODULE, ++ .name = DRIVER_NAME, ++ }, ++ .probe = tef6862_probe, ++ .remove = tef6862_remove, ++ .id_table = tef6862_id, ++}; ++ ++static __init int tef6862_init(void) ++{ ++ return i2c_add_driver(&tef6862_driver); ++} ++ ++static __exit void tef6862_exit(void) ++{ ++ i2c_del_driver(&tef6862_driver); ++} ++ ++module_init(tef6862_init); ++module_exit(tef6862_exit); ++ ++MODULE_DESCRIPTION("TEF6862 Car Radio Enhanced Selectivity Tuner"); ++MODULE_AUTHOR("Mocean Laboratories"); ++MODULE_LICENSE("GPL v2"); ++ +diff -uNr linux-2.6.31/drivers/media/video/adv7180.c linux-2.6.31.new/drivers/media/video/adv7180.c +--- linux-2.6.31/drivers/media/video/adv7180.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/media/video/adv7180.c 2009-10-23 11:17:28.000000000 -0700 +@@ -0,0 +1,475 @@ ++/* ++ * 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 ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#define DRIVER_NAME "adv7180" ++ ++#define ADV7180_INPUT_CONTROL_REG 0x00 ++#define ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM 0x00 ++#define ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM_PED 0x10 ++#define ADV7180_INPUT_CONTROL_AD_PAL_N_NTSC_J_SECAM 0x20 ++#define ADV7180_INPUT_CONTROL_AD_PAL_N_NTSC_M_SECAM 0x30 ++#define ADV7180_INPUT_CONTROL_NTSC_J 0x40 ++#define ADV7180_INPUT_CONTROL_NTSC_M 0x50 ++#define ADV7180_INPUT_CONTROL_PAL60 0x60 ++#define ADV7180_INPUT_CONTROL_NTSC_443 0x70 ++#define ADV7180_INPUT_CONTROL_PAL_BG 0x80 ++#define ADV7180_INPUT_CONTROL_PAL_N 0x90 ++#define ADV7180_INPUT_CONTROL_PAL_M 0xa0 ++#define ADV7180_INPUT_CONTROL_PAL_M_PED 0xb0 ++#define ADV7180_INPUT_CONTROL_PAL_COMB_N 0xc0 ++#define ADV7180_INPUT_CONTROL_PAL_COMB_N_PED 0xd0 ++#define ADV7180_INPUT_CONTROL_PAL_SECAM 0xe0 ++#define ADV7180_INPUT_CONTROL_PAL_SECAM_PED 0xf0 ++ ++#define ADV7180_EXTENDED_OUTPUT_CONTROL_REG 0x04 ++#define ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS 0xC5 ++ ++#define ADV7180_AUTODETECT_ENABLE_REG 0x07 ++#define ADV7180_AUTODETECT_DEFAULT 0x7f ++ ++#define ADV7180_ADI_CTRL_REG 0x0e ++#define ADV7180_ADI_CTRL_IRQ_SPACE 0x20 ++ ++#define ADV7180_STATUS1_REG 0x10 ++#define ADV7180_STATUS1_IN_LOCK 0x01 ++#define ADV7180_STATUS1_AUTOD_MASK 0x70 ++#define ADV7180_STATUS1_AUTOD_NTSM_M_J 0x00 ++#define ADV7180_STATUS1_AUTOD_NTSC_4_43 0x10 ++#define ADV7180_STATUS1_AUTOD_PAL_M 0x20 ++#define ADV7180_STATUS1_AUTOD_PAL_60 0x30 ++#define ADV7180_STATUS1_AUTOD_PAL_B_G 0x40 ++#define ADV7180_STATUS1_AUTOD_SECAM 0x50 ++#define ADV7180_STATUS1_AUTOD_PAL_COMB 0x60 ++#define ADV7180_STATUS1_AUTOD_SECAM_525 0x70 ++ ++#define ADV7180_IDENT_REG 0x11 ++#define ADV7180_ID_7180 0x18 ++ ++#define ADV7180_ICONF1_ADI 0x40 ++#define ADV7180_ICONF1_ACTIVE_LOW 0x01 ++#define ADV7180_ICONF1_PSYNC_ONLY 0x10 ++#define ADV7180_ICONF1_ACTIVE_TO_CLR 0xC0 ++ ++#define ADV7180_IRQ1_LOCK 0x01 ++#define ADV7180_IRQ1_UNLOCK 0x02 ++#define ADV7180_ISR1_ADI 0x42 ++#define ADV7180_ICR1_ADI 0x43 ++#define ADV7180_IMR1_ADI 0x44 ++#define ADV7180_IMR2_ADI 0x48 ++#define ADV7180_IRQ3_AD_CHANGE 0x08 ++#define ADV7180_ISR3_ADI 0x4A ++#define ADV7180_ICR3_ADI 0x4B ++#define ADV7180_IMR3_ADI 0x4C ++#define ADV7180_IMR4_ADI 0x50 ++ ++#define ADV7180_NTSC_V_BIT_END_REG 0xE6 ++#define ADV7180_NTSC_V_BIT_END_MANUAL_NVEND 0x4F ++ ++struct adv7180_state { ++ struct v4l2_subdev sd; ++ struct work_struct work; ++ struct mutex mutex; /* mutual excl. when accessing chip */ ++ int irq; ++ v4l2_std_id curr_norm; ++ bool autodetect; ++}; ++ ++static v4l2_std_id adv7180_std_to_v4l2(u8 status1) ++{ ++ switch (status1 & ADV7180_STATUS1_AUTOD_MASK) { ++ case ADV7180_STATUS1_AUTOD_NTSM_M_J: ++ return V4L2_STD_NTSC; ++ case ADV7180_STATUS1_AUTOD_NTSC_4_43: ++ return V4L2_STD_NTSC_443; ++ case ADV7180_STATUS1_AUTOD_PAL_M: ++ return V4L2_STD_PAL_M; ++ case ADV7180_STATUS1_AUTOD_PAL_60: ++ return V4L2_STD_PAL_60; ++ case ADV7180_STATUS1_AUTOD_PAL_B_G: ++ return V4L2_STD_PAL; ++ case ADV7180_STATUS1_AUTOD_SECAM: ++ return V4L2_STD_SECAM; ++ case ADV7180_STATUS1_AUTOD_PAL_COMB: ++ return V4L2_STD_PAL_Nc | V4L2_STD_PAL_N; ++ case ADV7180_STATUS1_AUTOD_SECAM_525: ++ return V4L2_STD_SECAM; ++ default: ++ return V4L2_STD_UNKNOWN; ++ } ++} ++ ++static int v4l2_std_to_adv7180(v4l2_std_id std) ++{ ++ if (std == V4L2_STD_PAL_60) ++ return ADV7180_INPUT_CONTROL_PAL60; ++ if (std == V4L2_STD_NTSC_443) ++ return ADV7180_INPUT_CONTROL_NTSC_443; ++ if (std == V4L2_STD_PAL_N) ++ return ADV7180_INPUT_CONTROL_PAL_N; ++ if (std == V4L2_STD_PAL_M) ++ return ADV7180_INPUT_CONTROL_PAL_M; ++ if (std == V4L2_STD_PAL_Nc) ++ return ADV7180_INPUT_CONTROL_PAL_COMB_N; ++ ++ /* pal is a combination of several variants */ ++ if (std & V4L2_STD_PAL) ++ return ADV7180_INPUT_CONTROL_PAL_BG; ++ if (std & V4L2_STD_NTSC) ++ return ADV7180_INPUT_CONTROL_NTSC_M; ++ if (std & V4L2_STD_SECAM) ++ return ADV7180_INPUT_CONTROL_PAL_SECAM; ++ ++ return -EINVAL; ++} ++ ++static u32 adv7180_status_to_v4l2(u8 status1) ++{ ++ if (!(status1 & ADV7180_STATUS1_IN_LOCK)) ++ return V4L2_IN_ST_NO_SIGNAL; ++ ++ return 0; ++} ++ ++static int __adv7180_status(struct i2c_client *client, u32 *status, ++ v4l2_std_id *std) ++{ ++ int status1 = i2c_smbus_read_byte_data(client, ADV7180_STATUS1_REG); ++ ++ if (status1 < 0) ++ return status1; ++ ++ if (status) ++ *status = adv7180_status_to_v4l2(status1); ++ if (std) ++ *std = adv7180_std_to_v4l2(status1); ++ ++ return 0; ++} ++ ++static inline struct adv7180_state *to_state(struct v4l2_subdev *sd) ++{ ++ return container_of(sd, struct adv7180_state, sd); ++} ++ ++static int adv7180_querystd(struct v4l2_subdev *sd, v4l2_std_id *std) ++{ ++ struct adv7180_state *state = to_state(sd); ++ int err = mutex_lock_interruptible(&state->mutex); ++ if (err) ++ return err; ++ ++ /* when we are interrupt driven we know the state */ ++ if (!state->autodetect || state->irq > 0) ++ *std = state->curr_norm; ++ else ++ err = __adv7180_status(v4l2_get_subdevdata(sd), NULL, std); ++ ++ mutex_unlock(&state->mutex); ++ return err; ++} ++ ++static int adv7180_s_std(struct v4l2_subdev *sd, v4l2_std_id std) ++{ ++ struct adv7180_state *state = to_state(sd); ++ struct i2c_client *client = v4l2_get_subdevdata(sd); ++ int ret = mutex_lock_interruptible(&state->mutex); ++ if (ret) ++ return ret; ++ ++ /* all standards -> autodetect */ ++ if (std == V4L2_STD_ALL) { ++ ret = i2c_smbus_write_byte_data(client, ++ ADV7180_INPUT_CONTROL_REG, ++ ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM); ++ if (ret < 0) ++ goto out; ++ ++ __adv7180_status(client, NULL, &state->curr_norm); ++ state->autodetect = true; ++ } else { ++ ret = v4l2_std_to_adv7180(std); ++ if (ret < 0) ++ goto out; ++ ++ ret = i2c_smbus_write_byte_data(client, ++ ADV7180_INPUT_CONTROL_REG, ret); ++ if (ret < 0) ++ goto out; ++ ++ state->curr_norm = std; ++ state->autodetect = false; ++ } ++ ret = 0; ++out: ++ mutex_unlock(&state->mutex); ++ return ret; ++} ++ ++static int adv7180_g_input_status(struct v4l2_subdev *sd, u32 *status) ++{ ++ struct adv7180_state *state = to_state(sd); ++ int ret = mutex_lock_interruptible(&state->mutex); ++ if (ret) ++ return ret; ++ ++ ret = __adv7180_status(v4l2_get_subdevdata(sd), status, NULL); ++ mutex_unlock(&state->mutex); ++ return ret; ++} ++ ++static int adv7180_g_chip_ident(struct v4l2_subdev *sd, ++ struct v4l2_dbg_chip_ident *chip) ++{ ++ struct i2c_client *client = v4l2_get_subdevdata(sd); ++ ++ return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_ADV7180, 0); ++} ++ ++static const struct v4l2_subdev_video_ops adv7180_video_ops = { ++ .querystd = adv7180_querystd, ++ .g_input_status = adv7180_g_input_status, ++}; ++ ++static const struct v4l2_subdev_core_ops adv7180_core_ops = { ++ .g_chip_ident = adv7180_g_chip_ident, ++ .s_std = adv7180_s_std, ++}; ++ ++static const struct v4l2_subdev_ops adv7180_ops = { ++ .core = &adv7180_core_ops, ++ .video = &adv7180_video_ops, ++}; ++ ++static void adv7180_work(struct work_struct *work) ++{ ++ struct adv7180_state *state = container_of(work, struct adv7180_state, ++ work); ++ struct i2c_client *client = v4l2_get_subdevdata(&state->sd); ++ u8 isr3; ++ ++ mutex_lock(&state->mutex); ++ i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG, ++ ADV7180_ADI_CTRL_IRQ_SPACE); ++ isr3 = i2c_smbus_read_byte_data(client, ADV7180_ISR3_ADI); ++ /* clear */ ++ i2c_smbus_write_byte_data(client, ADV7180_ICR3_ADI, isr3); ++ i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG, 0); ++ ++ if (isr3 & ADV7180_IRQ3_AD_CHANGE && state->autodetect) ++ __adv7180_status(client, NULL, &state->curr_norm); ++ mutex_unlock(&state->mutex); ++ ++ enable_irq(state->irq); ++} ++ ++static irqreturn_t adv7180_irq(int irq, void *devid) ++{ ++ struct adv7180_state *state = devid; ++ ++ schedule_work(&state->work); ++ ++ disable_irq_nosync(state->irq); ++ ++ return IRQ_HANDLED; ++} ++ ++/* ++ * Generic i2c probe ++ * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1' ++ */ ++ ++static int __devinit adv7180_probe(struct i2c_client *client, ++ const struct i2c_device_id *id) ++{ ++ struct adv7180_state *state; ++ struct v4l2_subdev *sd; ++ int ret; ++ ++ /* Check if the adapter supports the needed features */ ++ if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA)) ++ return -EIO; ++ ++ v4l_info(client, "chip found @ 0x%02x (%s)\n", ++ client->addr << 1, client->adapter->name); ++ ++ state = kzalloc(sizeof(struct adv7180_state), GFP_KERNEL); ++ if (state == NULL) { ++ ret = -ENOMEM; ++ goto err; ++ } ++ ++ state->irq = client->irq; ++ INIT_WORK(&state->work, adv7180_work); ++ mutex_init(&state->mutex); ++ state->autodetect = true; ++ sd = &state->sd; ++ v4l2_i2c_subdev_init(sd, client, &adv7180_ops); ++ ++ /* Initialize adv7180 */ ++ /* Enable autodetection */ ++ ret = i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG, ++ ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM); ++ if (ret < 0) ++ goto err_unreg_subdev; ++ ++ ret = i2c_smbus_write_byte_data(client, ADV7180_AUTODETECT_ENABLE_REG, ++ ADV7180_AUTODETECT_DEFAULT); ++ if (ret < 0) ++ goto err_unreg_subdev; ++ ++ /* ITU-R BT.656-4 compatible */ ++ ret = i2c_smbus_write_byte_data(client, ++ ADV7180_EXTENDED_OUTPUT_CONTROL_REG, ++ ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS); ++ if (ret < 0) ++ goto err_unreg_subdev; ++ ++ ++ /* Manually set V bit end position in NTSC mode */ ++ ret = i2c_smbus_write_byte_data(client, ++ ADV7180_NTSC_V_BIT_END_REG, ++ ADV7180_NTSC_V_BIT_END_MANUAL_NVEND); ++ if (ret < 0) ++ goto err_unreg_subdev; ++ ++ /* read current norm */ ++ __adv7180_status(client, NULL, &state->curr_norm); ++ ++ /* register for interrupts */ ++ if (state->irq > 0) { ++ ret = request_irq(state->irq, adv7180_irq, 0, DRIVER_NAME, ++ state); ++ if (ret) ++ goto err_unreg_subdev; ++ ++ ret = i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG, ++ ADV7180_ADI_CTRL_IRQ_SPACE); ++ if (ret < 0) ++ goto err_unreg_subdev; ++ ++ /* config the Interrupt pin to be active low */ ++ ret = i2c_smbus_write_byte_data(client, ADV7180_ICONF1_ADI, ++ ADV7180_ICONF1_ACTIVE_LOW | ADV7180_ICONF1_PSYNC_ONLY); ++ if (ret < 0) ++ goto err_unreg_subdev; ++ ++ ret = i2c_smbus_write_byte_data(client, ADV7180_IMR1_ADI, 0); ++ if (ret < 0) ++ goto err_unreg_subdev; ++ ++ ret = i2c_smbus_write_byte_data(client, ADV7180_IMR2_ADI, 0); ++ if (ret < 0) ++ goto err_unreg_subdev; ++ ++ /* enable AD change interrupts interrupts */ ++ ret = i2c_smbus_write_byte_data(client, ADV7180_IMR3_ADI, ++ ADV7180_IRQ3_AD_CHANGE); ++ if (ret < 0) ++ goto err_unreg_subdev; ++ ++ ret = i2c_smbus_write_byte_data(client, ADV7180_IMR4_ADI, 0); ++ if (ret < 0) ++ goto err_unreg_subdev; ++ ++ ret = i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG, ++ 0); ++ if (ret < 0) ++ goto err_unreg_subdev; ++ } ++ ++ return 0; ++ ++err_unreg_subdev: ++ mutex_destroy(&state->mutex); ++ v4l2_device_unregister_subdev(sd); ++ kfree(state); ++err: ++ printk(KERN_ERR DRIVER_NAME ": Failed to probe: %d\n", ret); ++ return ret; ++} ++ ++static int __devexit adv7180_remove(struct i2c_client *client) ++{ ++ struct v4l2_subdev *sd = i2c_get_clientdata(client); ++ struct adv7180_state *state = to_state(sd); ++ ++ if (state->irq > 0) { ++ free_irq(client->irq, state); ++ if (cancel_work_sync(&state->work)) { ++ /* ++ * Work was pending, therefore we need to enable ++ * IRQ here to balance the disable_irq() done in the ++ * interrupt handler. ++ */ ++ enable_irq(state->irq); ++ } ++ } ++ ++ mutex_destroy(&state->mutex); ++ v4l2_device_unregister_subdev(sd); ++ kfree(to_state(sd)); ++ return 0; ++} ++ ++static const struct i2c_device_id adv7180_id[] = { ++ {DRIVER_NAME, 0}, ++ {}, ++}; ++ ++MODULE_DEVICE_TABLE(i2c, adv7180_id); ++ ++static struct i2c_driver adv7180_driver = { ++ .driver = { ++ .owner = THIS_MODULE, ++ .name = DRIVER_NAME, ++ }, ++ .probe = adv7180_probe, ++ .remove = adv7180_remove, ++ .id_table = adv7180_id, ++}; ++ ++static __init int adv7180_init(void) ++{ ++ return i2c_add_driver(&adv7180_driver); ++} ++ ++static __exit void adv7180_exit(void) ++{ ++ i2c_del_driver(&adv7180_driver); ++} ++ ++module_init(adv7180_init); ++module_exit(adv7180_exit); ++ ++MODULE_DESCRIPTION("Analog Devices ADV7180 video decoder driver"); ++MODULE_AUTHOR("Mocean Laboratories"); ++MODULE_LICENSE("GPL v2"); ++ +diff -uNr linux-2.6.31/drivers/media/video/Kconfig linux-2.6.31.new/drivers/media/video/Kconfig +--- linux-2.6.31/drivers/media/video/Kconfig 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/media/video/Kconfig 2009-10-23 11:17:28.000000000 -0700 +@@ -265,6 +265,15 @@ + + comment "Video decoders" + ++config VIDEO_ADV7180 ++ tristate "Analog Devices ADV7180 decoder" ++ depends on VIDEO_V4L2 && 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_V4L2 && I2C +@@ -816,6 +825,13 @@ + ---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 ++ select VIDEO_ADV7180 ++ ---help--- ++ Add support for the Video In peripherial of the timberdale FPGA. ++ + # + # USB Multimedia device configuration + # +diff -uNr linux-2.6.31/drivers/media/video/Makefile linux-2.6.31.new/drivers/media/video/Makefile +--- linux-2.6.31/drivers/media/video/Makefile 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/media/video/Makefile 2009-10-23 11:17:27.000000000 -0700 +@@ -45,6 +45,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_ADV7343) += adv7343.o + obj-$(CONFIG_VIDEO_VPX3220) += vpx3220.o + obj-$(CONFIG_VIDEO_BT819) += bt819.o +@@ -156,6 +157,8 @@ + + obj-$(CONFIG_VIDEO_AU0828) += au0828/ + ++obj-$(CONFIG_VIDEO_TIMBERDALE) += timblogiw.o ++ + obj-$(CONFIG_USB_VIDEO_CLASS) += uvc/ + + obj-$(CONFIG_VIDEO_IR_I2C) += ir-kbd-i2c.o +diff -uNr linux-2.6.31/drivers/media/video/timblogiw.c linux-2.6.31.new/drivers/media/video/timblogiw.c +--- linux-2.6.31/drivers/media/video/timblogiw.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/media/video/timblogiw.c 2009-10-23 11:17:28.000000000 -0700 +@@ -0,0 +1,1058 @@ ++/* ++ * 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 ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include "timblogiw.h" ++#include ++#include ++ ++#define DRIVER_NAME "timb-video" ++ ++#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_BUF 0x04 ++#define TIMBLOGIW_TBI 0x2c ++#define TIMBLOGIW_BPL 0x30 ++ ++#define dbg(...) ++ ++#define BYTES_PER_LINE (720 * 2) ++ ++#define DMA_BUFFER_SIZE (BYTES_PER_LINE * 576) ++ ++#define TIMBLOGIW_VIDEO_FORMAT V4L2_PIX_FMT_UYVY ++ ++static void timblogiw_release_buffers(struct timblogiw *lw); ++ ++const struct timblogiw_tvnorm timblogiw_tvnorms[] = { ++ { ++ .std = V4L2_STD_PAL, ++ .width = 720, ++ .height = 576 ++ }, ++ { ++ .std = V4L2_STD_NTSC, ++ .width = 720, ++ .height = 480 ++ } ++}; ++ ++static int timblogiw_bytes_per_line(const struct timblogiw_tvnorm *norm) ++{ ++ return norm->width * 2; ++} ++ ++ ++static int timblogiw_frame_size(const struct timblogiw_tvnorm *norm) ++{ ++ return norm->height * timblogiw_bytes_per_line(norm); ++} ++ ++static const struct timblogiw_tvnorm *timblogiw_get_norm(const v4l2_std_id std) ++{ ++ int i; ++ for (i = 0; i < ARRAY_SIZE(timblogiw_tvnorms); i++) ++ if (timblogiw_tvnorms[i].std & std) ++ return timblogiw_tvnorms + i; ++ ++ /* default to first element */ ++ return timblogiw_tvnorms; ++} ++ ++static void timblogiw_handleframe(unsigned long arg) ++{ ++ struct timblogiw_frame *f; ++ struct timblogiw *lw = (struct timblogiw *)arg; ++ ++ spin_lock_bh(&lw->queue_lock); ++ if (lw->dma.filled && !list_empty(&lw->inqueue)) { ++ /* put the entry in the outqueue */ ++ f = list_entry(lw->inqueue.next, struct timblogiw_frame, frame); ++ ++ /* sync memory and unmap */ ++ dma_sync_single_for_cpu(lw->dev, lw->dma.filled->handle, ++ timblogiw_frame_size(lw->cur_norm), DMA_FROM_DEVICE); ++ ++ /* 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 = f->buf.length; ++ list_move_tail(&f->frame, &lw->outqueue); ++ /* wake up any waiter */ ++ wake_up(&lw->wait_frame); ++ } else { ++ /* No user buffer available, consume buffer anyway ++ * who wants an old video frame? ++ */ ++ lw->dma.filled = NULL; ++ } ++ spin_unlock_bh(&lw->queue_lock); ++} ++ ++static int __timblogiw_start_dma(struct timblogiw *lw) ++{ ++ int size = timblogiw_frame_size(lw->cur_norm); ++ int ret; ++ struct timbdma_transfer *transfer = lw->dma.transfer + lw->dma.curr; ++ int bytes_per_line = timblogiw_bytes_per_line(lw->cur_norm); ++ ++ ret = timbdma_prep_desc(transfer->desc, transfer->handle, size); ++ if (ret) ++ goto err; ++ ++ ret = timbdma_start(DMA_IRQ_VIDEO_RX, transfer->desc, bytes_per_line); ++ if (ret) ++ goto err; ++ return ret; ++err: ++ return ret; ++} ++ ++static int timblogiw_isr(u32 flag, void *pdev) ++{ ++ struct timblogiw *lw = (struct timblogiw *)pdev; ++ ++ if (lw->stream == STREAM_OFF) { ++ timbdma_stop(DMA_IRQ_VIDEO_RX); ++ /* stream is stopped, signal that the current transfer is ++ * finished */ ++ complete(&lw->irq_done); ++ } else { ++ struct timeval timestamp; ++ ++ do_gettimeofday(×tamp); ++ ++ if (!lw->dma.filled && (flag & DMA_IRQ_VIDEO_RX)) { ++ /* Got a frame, store it, and flip to next DMA buffer */ ++ lw->dma.filled = lw->dma.transfer + lw->dma.curr; ++ lw->dma.curr = !lw->dma.curr; ++ } else if (lw->dma.filled && (flag & DMA_IRQ_VIDEO_RX)) ++ printk("No free frame\n"); ++ ++ __timblogiw_start_dma(lw); ++ ++ 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(timblogiw_frame_size(lw->cur_norm)); ++ void *buff = NULL; ++ int ret; ++ u32 i; ++ ++ dbg("%s - request of %i buffers of size %zi\n", ++ __func__, count, imagesize); ++ ++ lw->dma.transfer[0].buf = kzalloc(DMA_BUFFER_SIZE, GFP_KERNEL); ++ if (!lw->dma.transfer[0].buf) ++ goto err; ++ ++ lw->dma.transfer[1].buf = kzalloc(DMA_BUFFER_SIZE, GFP_KERNEL); ++ if (!lw->dma.transfer[1].buf) ++ goto err; ++ ++ lw->dma.transfer[0].desc = ++ timbdma_alloc_desc(DMA_BUFFER_SIZE, BYTES_PER_LINE * 2); ++ if (!lw->dma.transfer[0].desc) ++ goto err; ++ ++ lw->dma.transfer[1].desc = ++ timbdma_alloc_desc(DMA_BUFFER_SIZE, BYTES_PER_LINE * 2); ++ if (!lw->dma.transfer[1].desc) ++ goto err; ++ ++ /* map up the DMA buffers */ ++ lw->dma.transfer[0].handle = dma_map_single(lw->dev, ++ lw->dma.transfer[0].buf, DMA_BUFFER_SIZE, DMA_FROM_DEVICE); ++ ret = dma_mapping_error(lw->dev, lw->dma.transfer[0].handle); ++ if (ret) { ++ lw->dma.transfer[0].handle = 0; ++ goto err; ++ } ++ ++ lw->dma.transfer[1].handle = dma_map_single(lw->dev, ++ lw->dma.transfer[1].buf, DMA_BUFFER_SIZE, DMA_FROM_DEVICE); ++ ret = dma_mapping_error(lw->dev, lw->dma.transfer[1].handle); ++ if (ret) { ++ lw->dma.transfer[1].handle = 0; ++ goto err; ++ } ++ ++ 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 = timblogiw_frame_size(lw->cur_norm); ++ 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; ++err: ++ timblogiw_release_buffers(lw); ++ ++ return 0; ++} ++ ++static 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; ++ } ++ ++ if (lw->dma.transfer[0].handle) ++ dma_unmap_single(lw->dev, lw->dma.transfer[0].handle, ++ DMA_BUFFER_SIZE, DMA_FROM_DEVICE); ++ ++ if (lw->dma.transfer[1].handle) ++ dma_unmap_single(lw->dev, lw->dma.transfer[1].handle, ++ DMA_BUFFER_SIZE, DMA_FROM_DEVICE); ++ ++ if (lw->dma.transfer[0].buf != NULL) ++ kfree(lw->dma.transfer[0].buf); ++ lw->dma.transfer[0].buf = NULL; ++ ++ if (lw->dma.transfer[1].buf != NULL) ++ kfree(lw->dma.transfer[1].buf); ++ lw->dma.transfer[1].buf = NULL; ++ ++ if (lw->dma.transfer[0].desc != NULL) ++ timbdma_free_desc(lw->dma.transfer[0].desc); ++ lw->dma.transfer[0].desc = NULL; ++ ++ if (lw->dma.transfer[1].desc != NULL) ++ timbdma_free_desc(lw->dma.transfer[1].desc); ++ lw->dma.transfer[1].desc = NULL; ++ ++ ++ lw->num_frames = TIMBLOGIW_NUM_FRAMES; ++} ++ ++/* IOCTL functions */ ++ ++static int timblogiw_g_fmt(struct file *file, void *priv, ++ struct v4l2_format *format) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ ++ dbg("%s\n", __func__); ++ ++ if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ++ return -EINVAL; ++ ++ format->fmt.pix.width = lw->cur_norm->width; ++ format->fmt.pix.height = lw->cur_norm->height; ++ format->fmt.pix.pixelformat = TIMBLOGIW_VIDEO_FORMAT; ++ format->fmt.pix.bytesperline = timblogiw_bytes_per_line(lw->cur_norm); ++ format->fmt.pix.sizeimage = timblogiw_frame_size(lw->cur_norm); ++ format->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; ++ format->fmt.pix.field = V4L2_FIELD_NONE; ++ return 0; ++} ++ ++static int timblogiw_try_fmt(struct file *file, void *priv, ++ struct v4l2_format *format) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ struct v4l2_pix_format *pix = &format->fmt.pix; ++ ++ 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); ++ ++ if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ++ return -EINVAL; ++ ++ if (pix->field != V4L2_FIELD_NONE) ++ return -EINVAL; ++ ++ if (pix->pixelformat != TIMBLOGIW_VIDEO_FORMAT) ++ return -EINVAL; ++ ++ if ((lw->cur_norm->height != pix->height) || ++ (lw->cur_norm->width != pix->width)) { ++ pix->width = lw->cur_norm->width; ++ pix->height = lw->cur_norm->height; ++ } ++ ++ return 0; ++} ++ ++static int timblogiw_querycap(struct file *file, void *priv, ++ struct v4l2_capability *cap) ++{ ++ dbg("%s\n", __func__); ++ 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 file *file, void *priv, ++ struct v4l2_fmtdesc *fmt) ++{ ++ dbg("%s, index: %d\n", __func__, fmt->index); ++ ++ 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 = TIMBLOGIW_VIDEO_FORMAT; ++ memset(fmt->reserved, 0, sizeof(fmt->reserved)); ++ ++ return 0; ++} ++ ++static int timblogiw_reqbufs(struct file *file, void *priv, ++ struct v4l2_requestbuffers *rb) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ ++ dbg("%s\n", __func__); ++ ++ 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 file *file, void *priv, ++ struct v4l2_buffer *b) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ ++ dbg("%s\n", __func__); ++ ++ 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 file *file, void *priv, struct v4l2_buffer *b) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ 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 (!lw->frame[b->index].bufmem) ++ return -EINVAL; ++ ++ 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 file *file, void *priv, ++ struct v4l2_buffer *b) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ 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_g_std(struct file *file, void *priv, v4l2_std_id *std) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ ++ dbg("%s\n", __func__); ++ ++ *std = lw->cur_norm->std; ++ return 0; ++} ++ ++static int timblogiw_s_std(struct file *file, void *priv, v4l2_std_id *std) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ int err; ++ ++ dbg("%s\n", __func__); ++ ++ err = v4l2_subdev_call(lw->sd_enc, core, s_std, *std); ++ if (!err) ++ lw->cur_norm = timblogiw_get_norm(*std); ++ ++ return err; ++} ++ ++static int timblogiw_enuminput(struct file *file, void *priv, ++ struct v4l2_input *inp) ++{ ++ dbg("%s\n", __func__); ++ ++ 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 file *file, void *priv, ++ unsigned int *input) ++{ ++ dbg("%s\n", __func__); ++ ++ *input = 0; ++ ++ return 0; ++} ++ ++static int timblogiw_s_input(struct file *file, void *priv, unsigned int input) ++{ ++ dbg("%s\n", __func__); ++ ++ if (input != 0) ++ return -EINVAL; ++ return 0; ++} ++ ++static int timblogiw_streamon(struct file *file, void *priv, unsigned int type) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ struct timblogiw_frame *f; ++ ++ dbg("%s\n", __func__); ++ ++ 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__, ++ timblogiw_frame_size(lw->cur_norm), ++ timblogiw_bytes_per_line(lw->cur_norm), ++ (unsigned int)lw->dma.transfer[lw->dma.curr].handle); ++ ++ __timblogiw_start_dma(lw); ++ ++ return 0; ++} ++ ++static void timblogiw_stopstream(struct timblogiw *lw) ++{ ++ if (lw->stream == STREAM_ON) { ++ /* The FPGA might be busy copying the current frame, we have ++ * to wait for the frame to finish ++ */ ++ unsigned long lock_flags; ++ ++ init_completion(&lw->irq_done); ++ ++ spin_lock_irqsave(&lw->queue_lock, lock_flags); ++ lw->stream = STREAM_OFF; ++ spin_unlock_irqrestore(&lw->queue_lock, lock_flags); ++ ++ wait_for_completion_timeout(&lw->irq_done, ++ msecs_to_jiffies(100)); ++ } ++} ++ ++static int timblogiw_streamoff(struct file *file, void *priv, ++ unsigned int type) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ ++ dbg("%s\n", __func__); ++ ++ if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ++ return -EINVAL; ++ ++ timblogiw_stopstream(lw); ++ ++ timblogiw_empty_framequeues(lw); ++ ++ return 0; ++} ++ ++static int timblogiw_querystd(struct file *file, void *priv, v4l2_std_id *std) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ ++ dbg("%s\n", __func__); ++ ++ return v4l2_subdev_call(lw->sd_enc, video, querystd, std); ++} ++ ++static int timblogiw_enum_framesizes(struct file *file, void *priv, ++ struct v4l2_frmsizeenum *fsize) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ ++ dbg("%s - index: %d, format: %d\n", __func__, ++ fsize->index, fsize->pixel_format); ++ ++ if ((fsize->index != 0) || ++ (fsize->pixel_format != TIMBLOGIW_VIDEO_FORMAT)) ++ return -EINVAL; ++ ++ fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE; ++ fsize->discrete.width = lw->cur_norm->width; ++ fsize->discrete.height = lw->cur_norm->height; ++ ++ return 0; ++} ++ ++struct find_addr_arg { ++ char const *name; ++ struct i2c_client *client; ++}; ++ ++static int find_name(struct device *dev, void *argp) ++{ ++ struct find_addr_arg *arg = (struct find_addr_arg *)argp; ++ struct i2c_client *client = i2c_verify_client(dev); ++ ++ if (client && !strcmp(arg->name, client->name) && client->driver) ++ arg->client = client; ++ ++ return 0; ++} ++ ++static struct i2c_client *find_client(struct i2c_adapter *adapt, ++ const char *name) ++{ ++ struct find_addr_arg find_arg; ++ /* now find the client */ ++#ifdef MODULE ++ request_module(name); ++#endif ++ /* code for finding the I2C child */ ++ find_arg.name = name; ++ find_arg.client = NULL; ++ device_for_each_child(&adapt->dev, &find_arg, find_name); ++ return find_arg.client; ++} ++ ++/******************************* ++ * Device Operations functions * ++ *******************************/ ++ ++static int timblogiw_open(struct file *file) ++{ ++ struct video_device *vdev = video_devdata(file); ++ struct timblogiw *lw = video_get_drvdata(vdev); ++ v4l2_std_id std = V4L2_STD_UNKNOWN; ++ int err = 0; ++ ++ 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); ++ ++ if (!lw->sd_enc) { ++ struct i2c_adapter *adapt; ++ struct i2c_client *encoder; ++ ++ /* find the video decoder */ ++ adapt = i2c_get_adapter(lw->pdata.i2c_adapter); ++ if (!adapt) { ++ printk(KERN_ERR DRIVER_NAME": No I2C bus\n"); ++ err = -ENODEV; ++ goto out; ++ } ++ ++ /* now find the encoder */ ++ encoder = find_client(adapt, lw->pdata.encoder); ++ ++ i2c_put_adapter(adapt); ++ ++ if (!encoder) { ++ printk(KERN_ERR DRIVER_NAME": Failed to get encoder\n"); ++ err = -ENODEV; ++ goto out; ++ } ++ ++ lw->sd_enc = i2c_get_clientdata(encoder); ++ lw->enc_owner = lw->sd_enc->owner; ++ /* Lock the module */ ++ if (!try_module_get(lw->enc_owner)) { ++ lw->sd_enc = NULL; ++ err = -ENODEV; ++ goto out; ++ } ++ } ++ ++ timblogiw_querystd(file, NULL, &std); ++ lw->cur_norm = timblogiw_get_norm(std); ++ ++ file->private_data = lw; ++ lw->stream = STREAM_OFF; ++ lw->num_frames = TIMBLOGIW_NUM_FRAMES; ++ ++ timblogiw_empty_framequeues(lw); ++ timbdma_set_interruptcb(DMA_IRQ_VIDEO_RX | DMA_IRQ_VIDEO_DROP, ++ timblogiw_isr, (void *)lw); ++ ++out: ++ mutex_unlock(&lw->lock); ++ ++ return err; ++} ++ ++static int timblogiw_close(struct file *file) ++{ ++ struct timblogiw *lw = file->private_data; ++ ++ dbg("%s - entry\n", __func__); ++ ++ mutex_lock(&lw->lock); ++ ++ timblogiw_stopstream(lw); ++ ++ timbdma_set_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; ++} ++ ++ ++void timblogiw_vdev_release(struct video_device *vdev) ++{ ++ kfree(vdev); ++} ++ ++static const struct v4l2_ioctl_ops timblogiw_ioctl_ops = { ++ .vidioc_querycap = timblogiw_querycap, ++ .vidioc_enum_fmt_vid_cap = timblogiw_enum_fmt, ++ .vidioc_g_fmt_vid_cap = timblogiw_g_fmt, ++ .vidioc_try_fmt_vid_cap = timblogiw_try_fmt, ++ .vidioc_s_fmt_vid_cap = timblogiw_try_fmt, ++ .vidioc_reqbufs = timblogiw_reqbufs, ++ .vidioc_querybuf = timblogiw_querybuf, ++ .vidioc_qbuf = timblogiw_qbuf, ++ .vidioc_dqbuf = timblogiw_dqbuf, ++ .vidioc_g_std = timblogiw_g_std, ++ .vidioc_s_std = timblogiw_s_std, ++ .vidioc_enum_input = timblogiw_enuminput, ++ .vidioc_g_input = timblogiw_g_input, ++ .vidioc_s_input = timblogiw_s_input, ++ .vidioc_streamon = timblogiw_streamon, ++ .vidioc_streamoff = timblogiw_streamoff, ++ .vidioc_querystd = timblogiw_querystd, ++ .vidioc_enum_framesizes = timblogiw_enum_framesizes, ++}; ++ ++static const struct v4l2_file_operations timblogiw_fops = { ++ .owner = THIS_MODULE, ++ .open = timblogiw_open, ++ .release = timblogiw_close, ++ .ioctl = video_ioctl2, /* V4L2 ioctl handler */ ++ .mmap = timblogiw_mmap, ++ .read = timblogiw_read, ++}; ++ ++static const struct video_device timblogiw_template = { ++ .name = TIMBLOGIWIN_NAME, ++ .fops = &timblogiw_fops, ++ .ioctl_ops = &timblogiw_ioctl_ops, ++ .release = &timblogiw_vdev_release, ++ .minor = -1, ++ .tvnorms = V4L2_STD_PAL | V4L2_STD_NTSC ++}; ++ ++static int timblogiw_probe(struct platform_device *dev) ++{ ++ int err; ++ struct timblogiw *lw = NULL; ++ struct resource *iomem; ++ struct timb_video_platform_data *pdata = dev->dev.platform_data; ++ ++ if (!pdata) { ++ printk(KERN_ERR DRIVER_NAME": Platform data missing\n"); ++ err = -EINVAL; ++ goto err_mem; ++ } ++ ++ iomem = platform_get_resource(dev, IORESOURCE_MEM, 0); ++ if (!iomem) { ++ err = -EINVAL; ++ goto err_mem; ++ } ++ ++ lw = kzalloc(sizeof(*lw), GFP_KERNEL); ++ if (!lw) { ++ err = -ENOMEM; ++ goto err_mem; ++ } ++ ++ if (dev->dev.parent) ++ lw->dev = dev->dev.parent; ++ else ++ lw->dev = &dev->dev; ++ ++ memcpy(&lw->pdata, pdata, sizeof(lw->pdata)); ++ ++ mutex_init(&lw->lock); ++ ++ lw->video_dev = video_device_alloc(); ++ if (!lw->video_dev) { ++ err = -ENOMEM; ++ goto err_mem; ++ } ++ *lw->video_dev = timblogiw_template; ++ ++ err = video_register_device(lw->video_dev, VFL_TYPE_GRABBER, 0); ++ if (err) { ++ printk(KERN_ALERT DRIVER_NAME": Error reg video\n"); ++ goto err_request; ++ } ++ ++ tasklet_init(&lw->tasklet, timblogiw_handleframe, (unsigned long)lw); ++ ++ if (!request_mem_region(iomem->start, resource_size(iomem), ++ DRIVER_NAME)) { ++ 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_mem: ++ kfree(lw); ++ printk(KERN_ERR DRIVER_NAME ": Failed to register: %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); ++ ++ if (lw->sd_enc) ++ module_put(lw->enc_owner); ++ 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 = DRIVER_NAME, ++ .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_AUTHOR("Mocean Laboratories "); ++MODULE_LICENSE("GPL v2"); ++MODULE_ALIAS("platform:"DRIVER_NAME); ++ +diff -uNr linux-2.6.31/drivers/media/video/timblogiw.h linux-2.6.31.new/drivers/media/video/timblogiw.h +--- linux-2.6.31/drivers/media/video/timblogiw.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/media/video/timblogiw.h 2009-10-23 11:17:28.000000000 -0700 +@@ -0,0 +1,96 @@ ++/* ++ * 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 ++#include ++#include ++ ++#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_DONE, ++}; ++ ++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 { ++ v4l2_std_id std; ++ u16 width; ++ u16 height; ++}; ++ ++ ++ ++struct timbdma_transfer { ++ dma_addr_t handle; ++ void *buf; ++ void *desc; ++}; ++ ++struct timblogiw_dma_control { ++ struct timbdma_transfer transfer[2]; ++ struct timbdma_transfer *filled; ++ int curr; ++}; ++ ++struct timblogiw { ++ 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; ++ struct completion irq_done; ++ struct timblogiw_tvnorm const *cur_norm; ++ struct device *dev; ++ struct timblogiw_dma_control dma; ++ void __iomem *membase; ++ struct tasklet_struct tasklet; ++ struct timb_video_platform_data pdata; ++ struct v4l2_subdev *sd_enc; /* encoder */ ++ struct module *enc_owner; ++}; ++ ++#endif /* _TIMBLOGIW_H */ ++ +diff -uNr linux-2.6.31/drivers/mfd/Kconfig linux-2.6.31.new/drivers/mfd/Kconfig +--- linux-2.6.31/drivers/mfd/Kconfig 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/mfd/Kconfig 2009-10-23 11:17:29.000000000 -0700 +@@ -263,6 +263,25 @@ + This enables the PCAP ASIC present on EZX Phones. This is + needed for MMC, TouchScreen, Sound, USB, etc.. + ++config MFD_TIMBERDALE ++ tristate "Support for the Timberdale FPGA" ++ select MFD_CORE ++ depends on PCI ++ ---help--- ++ This is the core driver for the timberdale FPGA. This device is a ++ multifunctioanl device which may provide numerous interfaces. ++ ++ The timberdale FPGA can be found on the Intel Atom development board ++ for automotive in-vehicle infontainment board called Russellville. ++ ++config MFD_TIMBERDALE_DMA ++ tristate "Support for timberdale DMA" ++ depends on MFD_TIMBERDALE ++ depends on HAS_IOMEM ++ ---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 ++ + endmenu + + menu "Multimedia Capabilities Port drivers" +diff -uNr linux-2.6.31/drivers/mfd/Makefile linux-2.6.31.new/drivers/mfd/Makefile +--- linux-2.6.31/drivers/mfd/Makefile 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/mfd/Makefile 2009-10-23 11:17:29.000000000 -0700 +@@ -44,3 +44,7 @@ + obj-$(CONFIG_PCF50633_ADC) += pcf50633-adc.o + obj-$(CONFIG_PCF50633_GPIO) += pcf50633-gpio.o + obj-$(CONFIG_AB3100_CORE) += ab3100-core.o ++ ++obj-$(CONFIG_MFD_TIMBERDALE) += timberdale.o ++obj-$(CONFIG_MFD_TIMBERDALE_DMA) += timbdma.o ++ +diff -uNr linux-2.6.31/drivers/mfd/timbdma.c linux-2.6.31.new/drivers/mfd/timbdma.c +--- linux-2.6.31/drivers/mfd/timbdma.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/mfd/timbdma.c 2009-10-23 11:17:29.000000000 -0700 +@@ -0,0 +1,542 @@ ++/* ++ * 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 ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#define DRIVER_NAME "timb-dma" ++ ++#define TIMBDMA_ACR 0x34 ++#define TIMBDMA_32BIT_ADDR 0x01 ++ ++#define TIMBDMA_ISR 0x080000 ++#define TIMBDMA_IPR 0x080004 ++#define TIMBDMA_IER 0x080008 ++ ++/* DMA configuration registers */ ++/* RX registers */ ++#define TIMBDMA_OFFS_RX_DHAR 0x00 ++#define TIMBDMA_OFFS_RX_DLAR 0x04 ++#define TIMBDMA_OFFS_RX_LR 0x0C ++#define TIMBDMA_OFFS_RX_BLR 0x10 ++#define TIMBDMA_OFFS_RX_ER 0x14 ++#define TIMBDMA_RX_EN 0x01 ++/* bytes per Row, video specific register */ ++#define TIMBDMA_OFFS_RX_BPRR 0x30 ++ ++/* TX registers */ ++#define TIMBDMA_OFFS_TX_DHAR 0x18 ++#define TIMBDMA_OFFS_TX_DLAR 0x1C ++#define TIMBDMA_OFFS_TX_BLR 0x24 ++#define TIMBDMA_OFFS_TX_LR 0x28 ++ ++#define DMA_DESC_SIZE 8 ++ ++struct dma_desc { ++ u32 len; ++ u32 chunk_size; ++ u8 buf[0]; ++}; ++ ++struct timbdma_control { ++ timbdma_interruptcb callback; ++ void *callback_data; ++ dma_addr_t desc; ++ int desc_len; ++ /* the following are used to store a desc while the hw has not been ++ * probed yet ++ */ ++ struct dma_desc *stored_desc; ++ int stored_bytes_per_row; ++}; ++ ++struct timbdma_dev { ++ void __iomem *membase; ++ struct device *dev; ++ struct timbdma_control control[DMA_IRQS]; ++ spinlock_t lock; /* mutual exclusion */ ++}; ++ ++static struct timbdma_dev *self_g; ++ ++ ++void *timbdma_alloc_desc(u32 size, u16 alignment) ++{ ++ /* calculate the number of chunks needed */ ++ int chunk_size = USHORT_MAX - (USHORT_MAX % alignment); ++ int chunks = size / chunk_size; ++ int len; ++ struct dma_desc *dma_desc; ++ ++ if (size % chunk_size) ++ chunks++; ++ ++ len = sizeof(struct dma_desc) + DMA_DESC_SIZE * chunks; ++ ++ dma_desc = kzalloc(len, GFP_KERNEL); ++ if (dma_desc) { ++ dma_desc->len = DMA_DESC_SIZE * chunks; ++ dma_desc->chunk_size = chunk_size; ++ } ++ return dma_desc; ++} ++EXPORT_SYMBOL(timbdma_alloc_desc); ++ ++void timbdma_free_desc(void *desc) ++{ ++ kfree(desc); ++} ++EXPORT_SYMBOL(timbdma_free_desc); ++ ++int timbdma_prep_desc(void *desc, dma_addr_t addr, u32 size) ++{ ++ struct dma_desc *dma_desc = desc; ++ u8 *buf = dma_desc->buf; ++ dma_addr_t cur_addr = addr; ++ int chunks = size / dma_desc->chunk_size; ++ if (size % dma_desc->chunk_size) ++ chunks++; ++ ++ if (dma_desc->len < chunks * DMA_DESC_SIZE) ++ return -EINVAL; ++ ++ while (size > 0) { ++ int chunk_size = dma_desc->chunk_size; ++ if (chunk_size > size) ++ chunk_size = size; ++ buf[7] = (cur_addr >> 24) & 0xff; ++ buf[6] = (cur_addr >> 16) & 0xff; ++ buf[5] = (cur_addr >> 8) & 0xff; ++ buf[4] = (cur_addr >> 0) & 0xff; ++ ++ buf[3] = (chunk_size >> 8) & 0xff; ++ buf[2] = (chunk_size >> 0) & 0xff; ++ ++ buf[1] = 0x00; ++ buf[0] = 0x21; /* tran, valid */ ++ ++ buf += DMA_DESC_SIZE; ++ cur_addr += chunk_size; ++ size -= chunk_size; ++ } ++ ++ /* make sure to mark the last one as end */ ++ (buf-DMA_DESC_SIZE)[0] |= 0x2; ++ ++ return 0; ++} ++EXPORT_SYMBOL(timbdma_prep_desc); ++ ++static irqreturn_t timbdma_handleinterrupt(int irq, void *devid) ++{ ++ struct timbdma_dev *dev = (struct timbdma_dev *)devid; ++ u32 ipr, ier; ++ int i; ++ ++ ipr = ioread32(dev->membase + TIMBDMA_IPR); ++ /* the MSI-X controller is level triggered, help it a bit, ++ * by disabling interrupts and re-enable them in the end. ++ */ ++ ier = ioread32(dev->membase + TIMBDMA_IER); ++ iowrite32(0, dev->membase + TIMBDMA_IER); ++ ++ if (ipr) { ++ /* ack */ ++ iowrite32(ipr, dev->membase + TIMBDMA_ISR); ++ ++ /* call the callbacks */ ++ for (i = 0; i < DMA_IRQS; i++) { ++ int mask = 1 << i; ++ if (ipr & mask) { ++ struct timbdma_control *ctrl = dev->control + i; ++ struct timbdma_control *unmap_ctrl = ctrl; ++ ++ /* special case for video frame drop */ ++ if (mask == DMA_IRQ_VIDEO_DROP) ++ unmap_ctrl = dev->control + i - 1; ++ ++ /* unmap memory */ ++ dma_unmap_single(dev->dev, unmap_ctrl->desc, ++ unmap_ctrl->desc_len, DMA_TO_DEVICE); ++ unmap_ctrl->desc = 0; ++ ++ if (ctrl->callback) ++ ctrl->callback(mask, ++ ctrl->callback_data); ++ } ++ } ++ ++ iowrite32(ier, dev->membase + TIMBDMA_IER); ++ return IRQ_HANDLED; ++ } else { ++ iowrite32(ier, dev->membase + TIMBDMA_IER); ++ return IRQ_NONE; ++ } ++} ++ ++static int __timbdma_start(struct timbdma_dev *dev, int index, ++ struct dma_desc *dma_desc, int bytes_per_row) ++{ ++ u32 offset; ++ unsigned long flags; ++ struct timbdma_control *ctrl; ++ int err; ++ ++ ctrl = dev->control + index; ++ ++ BUG_ON(ctrl->desc); ++ ++ /* check if we already have a descriptor */ ++ if (ctrl->desc) ++ return -EALREADY; ++ ++ /* map up the descriptor */ ++ ctrl->desc = dma_map_single(dev->dev, dma_desc->buf, dma_desc->len, ++ DMA_TO_DEVICE); ++ err = dma_mapping_error(dev->dev, ctrl->desc); ++ if (err) { ++ ctrl->desc = 0; ++ return err; ++ } ++ ctrl->desc_len = dma_desc->len; ++ ++ /* now enable the DMA transfer */ ++ offset = index / 2 * 0x40; ++ ++ spin_lock_irqsave(&dev->lock, flags); ++ if (!(index % 2)) { ++ /* RX */ ++ /* descriptor address */ ++ iowrite32(0, dev->membase + offset + TIMBDMA_OFFS_RX_DHAR); ++ iowrite32(ctrl->desc, dev->membase + offset + ++ TIMBDMA_OFFS_RX_DLAR); ++ /* Bytes per line */ ++ iowrite32(bytes_per_row, dev->membase + offset + ++ TIMBDMA_OFFS_RX_BPRR); ++ /* enable RX */ ++ iowrite32(TIMBDMA_RX_EN, dev->membase + offset + ++ TIMBDMA_OFFS_RX_ER); ++ } else { ++ /* TX */ ++ /* address high */ ++ iowrite32(0, dev->membase + offset + TIMBDMA_OFFS_TX_DHAR); ++ iowrite32(ctrl->desc, dev->membase + offset + ++ TIMBDMA_OFFS_TX_DLAR); ++ } ++ spin_unlock_irqrestore(&dev->lock, flags); ++ ++ return 0; ++} ++int timbdma_start(u32 flag, void *desc, int bytes_per_row) ++{ ++ int i; ++ struct timbdma_dev *dev = self_g; ++ struct dma_desc *dma_desc = desc; ++ int ret = 0; ++ ++ /* only allow 1 flag bit to be set */ ++ for (i = 0; i < DMA_IRQS && !(flag & (1 << i)); i++) ++ ; ++ if (i == DMA_IRQS || (flag & ~(1 << i))) ++ return -EINVAL; ++ ++ if (!dev->membase) { ++ /* the physical DMA device has not showed up yet */ ++ unsigned long flags; ++ struct timbdma_control *ctrl = dev->control + i; ++ BUG_ON(ctrl->stored_desc); ++ if (ctrl->stored_desc) ++ ret = -EALREADY; ++ else { ++ spin_lock_irqsave(&dev->lock, flags); ++ ctrl->stored_desc = desc; ++ ctrl->stored_bytes_per_row = bytes_per_row; ++ spin_unlock_irqrestore(&dev->lock, flags); ++ } ++ } else ++ ret = __timbdma_start(dev, i, dma_desc, bytes_per_row); ++ ++ if (ret) ++ printk(KERN_ERR DRIVER_NAME": Failed to start DMA: %d\n", ret); ++ return ret; ++} ++EXPORT_SYMBOL(timbdma_start); ++ ++int timbdma_stop(u32 flags) ++{ ++ int i; ++ unsigned long irqflags; ++ struct timbdma_dev *dev = self_g; ++ int ret = 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 DESC ADDR LOW registers are located at: ++ 0x1C ++ 0x5C ++ */ ++ struct timbdma_control *ctrl = dev->control + i; ++ if (ctrl->desc) { ++ u32 offset = i / 2 * 0x40; ++ ++ if (!(i % 2)) { ++ /* even -> RX enable */ ++ offset += TIMBDMA_OFFS_RX_ER; ++ /** TODO: FIX received length */ ++ } else { ++ /* odd -> TX desc addr low */ ++ offset += TIMBDMA_OFFS_TX_DLAR; ++ /** TODO: FIX written lenth */ ++ } ++ ++ if (dev->membase) ++ iowrite32(0, dev->membase + offset); ++ ++ dma_unmap_single(dev->dev, ctrl->desc, ++ ctrl->desc_len, DMA_TO_DEVICE); ++ ctrl->desc = 0; ++ } else if (ctrl->stored_desc) ++ ctrl->stored_desc = NULL; ++ } ++ ++ if (dev->membase) ++ /* ack any pending IRQs */ ++ iowrite32(flags, dev->membase + TIMBDMA_ISR); ++ ++ spin_unlock_irqrestore(&dev->lock, irqflags); ++ ++ return ret; ++} ++EXPORT_SYMBOL(timbdma_stop); ++ ++void timbdma_set_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)) { ++ struct timbdma_control *ctrl = dev->control + i; ++ ctrl->callback = icb; ++ ctrl->callback_data = data; ++ } ++ ++ /* the DMA device might not have showed up yet */ ++ if (dev->membase) { ++ /* Ack any pending IRQ */ ++ iowrite32(flags, dev->membase + TIMBDMA_ISR); ++ ++ /* if a null callback is given -> clear interrupt, ++ * else -> enable ++ */ ++ ier = ioread32(dev->membase + TIMBDMA_IER); ++ if (icb != NULL) ++ ier |= flags; ++ else ++ ier &= ~flags; ++ iowrite32(ier, dev->membase + TIMBDMA_IER); ++ } ++ ++ spin_unlock_irqrestore(&dev->lock, irqflags); ++} ++EXPORT_SYMBOL(timbdma_set_interruptcb); ++ ++static void timbdma_start_operations(struct timbdma_dev *self) ++{ ++ int i; ++ u32 ier; ++ unsigned long flags; ++ ++ spin_lock_irqsave(&self->lock, flags); ++ ier = ioread32(self->membase + TIMBDMA_IER); ++ for (i = 0; i < DMA_IRQS; i++) ++ if (self->control[i].callback) ++ ier |= 1 << i; ++ iowrite32(ier, self->membase + TIMBDMA_IER); ++ spin_unlock_irqrestore(&self->lock, flags); ++ ++ /* look for any transfers that were started before the HW was ++ * available, and start them ++ */ ++ for (i = 0; i < DMA_IRQS; i++) { ++ struct timbdma_control *ctrl = self->control + i; ++ if (ctrl->stored_desc) { ++ struct dma_desc *dma_desc = ctrl->stored_desc; ++ ctrl->stored_desc = NULL; ++ if (__timbdma_start(self, i, dma_desc, ++ ctrl->stored_bytes_per_row)) ++ printk(KERN_ERR DRIVER_NAME ++ ": Failed to start DMA\n"); ++ } ++ } ++} ++ ++ ++static int timbdma_probe(struct platform_device *dev) ++{ ++ int err, irq; ++ struct resource *iomem; ++ struct timbdma_dev *self = self_g; ++ ++ iomem = platform_get_resource(dev, IORESOURCE_MEM, 0); ++ if (!iomem) { ++ err = -EINVAL; ++ goto err_request; ++ } ++ ++ if (dev->dev.parent) ++ self->dev = dev->dev.parent; ++ else ++ self->dev = &dev->dev; ++ ++ if (!request_mem_region(iomem->start, ++ resource_size(iomem), DRIVER_NAME)) { ++ err = -EBUSY; ++ goto err_request; ++ } ++ ++ self->membase = ioremap(iomem->start, resource_size(iomem)); ++ if (!self->membase) { ++ printk(KERN_ERR DRIVER_NAME ": Failed to remap I/O memory\n"); ++ err = -ENOMEM; ++ goto err_ioremap; ++ } ++ ++ /* 32bit addressing */ ++ iowrite32(TIMBDMA_32BIT_ADDR, self->membase + TIMBDMA_ACR); ++ ++ /* disable and clear any interrupts */ ++ iowrite32(0x0, self->membase + TIMBDMA_IER); ++ iowrite32(0x0, self->membase + TIMBDMA_ISR); ++ ++ /* 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, ++ DRIVER_NAME, self); ++ if (err) { ++ printk(KERN_ERR DRIVER_NAME ": Failed to request IRQ\n"); ++ goto err_get_irq; ++ } ++ ++ platform_set_drvdata(dev, self); ++ ++ /* assign the global pointer */ ++ self_g = self; ++ ++ timbdma_start_operations(self); ++ ++ return 0; ++ ++err_get_irq: ++ iounmap(self->membase); ++err_ioremap: ++ release_mem_region(iomem->start, resource_size(iomem)); ++err_request: ++ printk(KERN_ERR DRIVER_NAME ": Failed to register Timberdale DMA: %d\n", ++ err); ++ self->membase = NULL; ++ self->dev = NULL; ++ ++ 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)); ++ self->membase = NULL; ++ self->dev = NULL; ++ return 0; ++} ++ ++static struct platform_driver timbdma_platform_driver = { ++ .driver = { ++ .name = DRIVER_NAME, ++ .owner = THIS_MODULE, ++ }, ++ .probe = timbdma_probe, ++ .remove = timbdma_remove, ++}; ++ ++/*--------------------------------------------------------------------------*/ ++ ++static int __init timbdma_init(void) ++{ ++ struct timbdma_dev *self; ++ int err; ++ ++ self = kzalloc(sizeof(*self), GFP_KERNEL); ++ if (!self) ++ return -ENOMEM; ++ ++ spin_lock_init(&self->lock); ++ ++ self_g = self; ++ err = platform_driver_register(&timbdma_platform_driver); ++ if (err) ++ kfree(self); ++ ++ return err; ++} ++ ++static void __exit timbdma_exit(void) ++{ ++ platform_driver_unregister(&timbdma_platform_driver); ++ kfree(self_g); ++} ++ ++module_init(timbdma_init); ++module_exit(timbdma_exit); ++ ++MODULE_DESCRIPTION("Timberdale DMA driver"); ++MODULE_AUTHOR("Mocean Laboratories "); ++MODULE_LICENSE("GPL v2"); ++MODULE_ALIAS("platform:"DRIVER_NAME); ++ +diff -uNr linux-2.6.31/drivers/mfd/timberdale.c linux-2.6.31.new/drivers/mfd/timberdale.c +--- linux-2.6.31/drivers/mfd/timberdale.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/mfd/timberdale.c 2009-10-23 11:17:29.000000000 -0700 +@@ -0,0 +1,914 @@ ++/* ++ * 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 ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#include ++ ++#include ++#include ++#include ++#include ++#include ++ ++#include ++#include ++#include ++#include ++ ++#include ++#include ++#include ++ ++#include ++ ++#include "timberdale.h" ++ ++#define DRIVER_NAME "timberdale" ++ ++struct timberdale_device { ++ resource_size_t intc_mapbase; ++ resource_size_t ctl_mapbase; ++ unsigned char __iomem *ctl_membase; ++ /* locking from interrupts while modifiying registers */ ++ spinlock_t lock; ++ struct { ++ u32 major; ++ u32 minor; ++ u32 config; ++ } fw; ++}; ++ ++/*--------------------------------------------------------------------------*/ ++ ++static struct tsc2007_platform_data timberdale_tsc2007_platform_data = { ++ .model = 2003, ++ .x_plate_ohms = 100 ++}; ++ ++static struct ascb_platform_data timberdale_ascb_platform_data = { ++ .gpio_pin = GPIO_PIN_ASCB ++}; ++ ++static struct i2c_board_info timberdale_i2c_board_info[] = { ++ { ++ I2C_BOARD_INFO("tsc2007", 0x48), ++ .platform_data = &timberdale_tsc2007_platform_data, ++ .irq = IRQ_TIMBERDALE_TSC_INT ++ }, ++ { ++ /* Requires jumper JP9 to be off */ ++ I2C_BOARD_INFO("adv7180", 0x42 >> 1), ++ .irq = IRQ_TIMBERDALE_ADV7180 ++ }, ++ { ++ I2C_BOARD_INFO("tef6862", 0x60) ++ }, ++ { ++ I2C_BOARD_INFO("saa7706h", 0x1C) ++ }, ++ { ++ I2C_BOARD_INFO("ascb-can", 0x18), ++ .platform_data = &timberdale_ascb_platform_data, ++ } ++}; ++ ++static __devinitdata struct xiic_i2c_platform_data ++timberdale_xiic_platform_data = { ++ .devices = timberdale_i2c_board_info, ++ .num_devices = ARRAY_SIZE(timberdale_i2c_board_info) ++}; ++ ++static __devinitdata struct ocores_i2c_platform_data ++timberdale_ocores_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_xiic_resources[] = { ++ { ++ .start = XIICOFFSET, ++ .end = XIICEND, ++ .flags = IORESOURCE_MEM, ++ }, ++ { ++ .start = IRQ_TIMBERDALE_I2C, ++ .end = IRQ_TIMBERDALE_I2C, ++ .flags = IORESOURCE_IRQ, ++ }, ++}; ++ ++const static __devinitconst struct resource timberdale_ocores_resources[] = { ++ { ++ .start = OCORESOFFSET, ++ .end = OCORESEND, ++ .flags = IORESOURCE_MEM, ++ }, ++ { ++ .start = IRQ_TIMBERDALE_I2C, ++ .end = IRQ_TIMBERDALE_I2C, ++ .flags = IORESOURCE_IRQ, ++ }, ++}; ++ ++const struct max7301_platform_data timberdale_max7301_platform_data = { ++ .base = 200 ++}; ++ ++const struct mc33880_platform_data timberdale_mc33880_platform_data = { ++ .base = 100 ++}; ++ ++static struct spi_board_info timberdale_spi_16bit_board_info[] = { ++ { ++ .modalias = "max7301", ++ .max_speed_hz = 26000, ++ .chip_select = 2, ++ .mode = SPI_MODE_0, ++ .platform_data = &timberdale_max7301_platform_data ++ }, ++}; ++ ++static struct spi_board_info timberdale_spi_8bit_board_info[] = { ++ { ++ .modalias = "mc33880", ++ .max_speed_hz = 4000, ++ .chip_select = 1, ++ .mode = SPI_MODE_1, ++ .platform_data = &timberdale_mc33880_platform_data ++ }, ++}; ++ ++static __devinitdata struct xspi_platform_data timberdale_xspi_platform_data = { ++ /* Current(2009-03-06) revision of ++ * Timberdale we can handle 3 chip selects ++ */ ++ .num_chipselect = 3, ++ /* bits per word and devices will be filled in runtime depending ++ * on the HW config ++ */ ++}; ++ ++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, ++ }, ++}; ++ ++static __devinitdata struct timbgpio_platform_data ++ timberdale_gpio_platform_data = { ++ .gpio_base = 0, ++ .nr_pins = GPIO_NR_PINS, ++ .irq_base = 200, ++}; ++ ++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, ++ }, ++}; ++ ++static __devinitdata struct timbmlb_platform_data ++ timberdale_mlb_platform_data = { ++ .reset_pin = GPIO_PIN_INIC_RST ++}; ++ ++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_mlogicore_resources[] = { ++ { ++ .start = MLCOREOFFSET, ++ .end = MLCOREEND, ++ .flags = IORESOURCE_MEM, ++ }, ++ { ++ .start = IRQ_TIMBERDALE_MLCORE, ++ .end = IRQ_TIMBERDALE_MLCORE, ++ .flags = IORESOURCE_IRQ, ++ }, ++ { ++ .start = IRQ_TIMBERDALE_MLCORE_BUF, ++ .end = IRQ_TIMBERDALE_MLCORE_BUF, ++ .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_uartlite_resources[] = { ++ { ++ .start = UARTLITEOFFSET, ++ .end = UARTLITEEND, ++ .flags = IORESOURCE_MEM, ++ }, ++ { ++ .start = IRQ_TIMBERDALE_UARTLITE, ++ .end = IRQ_TIMBERDALE_UARTLITE, ++ .flags = IORESOURCE_IRQ, ++ }, ++}; ++ ++static __devinitdata struct timbi2s_bus_data timbi2s_bus_data[] = { ++ { ++ .rx = 0, ++ .sample_rate = 8000, ++ }, ++ { ++ .rx = 1, ++ .sample_rate = 8000, ++ }, ++ { ++ .rx = 1, ++ .sample_rate = 44100, ++ }, ++}; ++ ++static __devinitdata struct timbi2s_platform_data timbi2s_platform_data = { ++ .busses = timbi2s_bus_data, ++ .num_busses = ARRAY_SIZE(timbi2s_bus_data), ++ .main_clk = 62500000, ++}; ++ ++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, ++ }, ++}; ++ ++static __devinitdata struct timb_video_platform_data ++ timberdale_video_platform_data = { ++ .i2c_adapter = 0, ++ .encoder = "adv7180" ++}; ++ ++const static __devinitconst struct resource timberdale_radio_resources[] = { ++ { ++ .start = RDSOFFSET, ++ .end = RDSEND, ++ .flags = IORESOURCE_MEM, ++ }, ++ { ++ .start = IRQ_TIMBERDALE_RDS, ++ .end = IRQ_TIMBERDALE_RDS, ++ .flags = IORESOURCE_IRQ, ++ }, ++}; ++ ++static __devinitdata struct timb_radio_platform_data ++ timberdale_radio_platform_data = { ++ .i2c_adapter = 0, ++ .tuner = "tef6862", ++ .dsp = "saa7706h" ++}; ++ ++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_cfg0[] = { ++ { ++ .name = "timb-uart", ++ .num_resources = ARRAY_SIZE(timberdale_uart_resources), ++ .resources = timberdale_uart_resources, ++ }, ++ { ++ .name = "xiic-i2c", ++ .num_resources = ARRAY_SIZE(timberdale_xiic_resources), ++ .resources = timberdale_xiic_resources, ++ .platform_data = &timberdale_xiic_platform_data, ++ .data_size = sizeof(timberdale_xiic_platform_data), ++ }, ++ { ++ .name = "timb-gpio", ++ .num_resources = ARRAY_SIZE(timberdale_gpio_resources), ++ .resources = timberdale_gpio_resources, ++ .platform_data = &timberdale_gpio_platform_data, ++ .data_size = sizeof(timberdale_gpio_platform_data), ++ }, ++ { ++ .name = "timb-i2s", ++ .num_resources = ARRAY_SIZE(timberdale_i2s_resources), ++ .resources = timberdale_i2s_resources, ++ .platform_data = &timbi2s_platform_data, ++ .data_size = sizeof(timbi2s_platform_data), ++ }, ++ { ++ .name = "timb-most", ++ .num_resources = ARRAY_SIZE(timberdale_most_resources), ++ .resources = timberdale_most_resources, ++ .platform_data = &timberdale_mlb_platform_data, ++ .data_size = sizeof(timberdale_mlb_platform_data), ++ }, ++ { ++ .name = "timb-video", ++ .num_resources = ARRAY_SIZE(timberdale_video_resources), ++ .resources = timberdale_video_resources, ++ .platform_data = &timberdale_video_platform_data, ++ .data_size = sizeof(timberdale_video_platform_data), ++ }, ++ { ++ .name = "timb-radio", ++ .num_resources = ARRAY_SIZE(timberdale_radio_resources), ++ .resources = timberdale_radio_resources, ++ .platform_data = &timberdale_radio_platform_data, ++ .data_size = sizeof(timberdale_radio_platform_data), ++ }, ++ { ++ .name = "xilinx_spi", ++ .num_resources = ARRAY_SIZE(timberdale_spi_resources), ++ .resources = timberdale_spi_resources, ++ .platform_data = &timberdale_xspi_platform_data, ++ .data_size = sizeof(timberdale_xspi_platform_data), ++ }, ++ { ++ .name = "ks8842", ++ .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 __devinitdata struct mfd_cell timberdale_cells_bar0_cfg1[] = { ++ { ++ .name = "timb-uart", ++ .num_resources = ARRAY_SIZE(timberdale_uart_resources), ++ .resources = timberdale_uart_resources, ++ }, ++ { ++ .name = "uartlite", ++ .num_resources = ARRAY_SIZE(timberdale_uartlite_resources), ++ .resources = timberdale_uartlite_resources, ++ }, ++ { ++ .name = "xiic-i2c", ++ .num_resources = ARRAY_SIZE(timberdale_xiic_resources), ++ .resources = timberdale_xiic_resources, ++ .platform_data = &timberdale_xiic_platform_data, ++ .data_size = sizeof(timberdale_xiic_platform_data), ++ }, ++ { ++ .name = "timb-gpio", ++ .num_resources = ARRAY_SIZE(timberdale_gpio_resources), ++ .resources = timberdale_gpio_resources, ++ .platform_data = &timberdale_gpio_platform_data, ++ .data_size = sizeof(timberdale_gpio_platform_data), ++ }, ++ { ++ .name = "timb-mlogicore", ++ .num_resources = ARRAY_SIZE(timberdale_mlogicore_resources), ++ .resources = timberdale_mlogicore_resources, ++ }, ++ { ++ .name = "timb-video", ++ .num_resources = ARRAY_SIZE(timberdale_video_resources), ++ .resources = timberdale_video_resources, ++ .platform_data = &timberdale_video_platform_data, ++ .data_size = sizeof(timberdale_video_platform_data), ++ }, ++ { ++ .name = "timb-radio", ++ .num_resources = ARRAY_SIZE(timberdale_radio_resources), ++ .resources = timberdale_radio_resources, ++ .platform_data = &timberdale_radio_platform_data, ++ .data_size = sizeof(timberdale_radio_platform_data), ++ }, ++ { ++ .name = "xilinx_spi", ++ .num_resources = ARRAY_SIZE(timberdale_spi_resources), ++ .resources = timberdale_spi_resources, ++ .platform_data = &timberdale_xspi_platform_data, ++ .data_size = sizeof(timberdale_xspi_platform_data), ++ }, ++ { ++ .name = "ks8842", ++ .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 __devinitdata struct mfd_cell timberdale_cells_bar0_cfg2[] = { ++ { ++ .name = "timb-uart", ++ .num_resources = ARRAY_SIZE(timberdale_uart_resources), ++ .resources = timberdale_uart_resources, ++ }, ++ { ++ .name = "xiic-i2c", ++ .num_resources = ARRAY_SIZE(timberdale_xiic_resources), ++ .resources = timberdale_xiic_resources, ++ .platform_data = &timberdale_xiic_platform_data, ++ .data_size = sizeof(timberdale_xiic_platform_data), ++ }, ++ { ++ .name = "timb-gpio", ++ .num_resources = ARRAY_SIZE(timberdale_gpio_resources), ++ .resources = timberdale_gpio_resources, ++ .platform_data = &timberdale_gpio_platform_data, ++ .data_size = sizeof(timberdale_gpio_platform_data), ++ }, ++ { ++ .name = "timb-video", ++ .num_resources = ARRAY_SIZE(timberdale_video_resources), ++ .resources = timberdale_video_resources, ++ .platform_data = &timberdale_video_platform_data, ++ .data_size = sizeof(timberdale_video_platform_data), ++ }, ++ { ++ .name = "timb-radio", ++ .num_resources = ARRAY_SIZE(timberdale_radio_resources), ++ .resources = timberdale_radio_resources, ++ .platform_data = &timberdale_radio_platform_data, ++ .data_size = sizeof(timberdale_radio_platform_data), ++ }, ++ { ++ .name = "xilinx_spi", ++ .num_resources = ARRAY_SIZE(timberdale_spi_resources), ++ .resources = timberdale_spi_resources, ++ .platform_data = &timberdale_xspi_platform_data, ++ .data_size = sizeof(timberdale_xspi_platform_data), ++ }, ++ { ++ .name = "timb-dma", ++ .num_resources = ARRAY_SIZE(timberdale_dma_resources), ++ .resources = timberdale_dma_resources, ++ }, ++}; ++ ++static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg3[] = { ++ { ++ .name = "timb-uart", ++ .num_resources = ARRAY_SIZE(timberdale_uart_resources), ++ .resources = timberdale_uart_resources, ++ }, ++ { ++ .name = "ocores-i2c", ++ .num_resources = ARRAY_SIZE(timberdale_ocores_resources), ++ .resources = timberdale_ocores_resources, ++ .platform_data = &timberdale_ocores_platform_data, ++ .data_size = sizeof(timberdale_ocores_platform_data), ++ }, ++ { ++ .name = "timb-gpio", ++ .num_resources = ARRAY_SIZE(timberdale_gpio_resources), ++ .resources = timberdale_gpio_resources, ++ .platform_data = &timberdale_gpio_platform_data, ++ .data_size = sizeof(timberdale_gpio_platform_data), ++ }, ++ { ++ .name = "timb-i2s", ++ .num_resources = ARRAY_SIZE(timberdale_i2s_resources), ++ .resources = timberdale_i2s_resources, ++ .platform_data = &timbi2s_platform_data, ++ .data_size = sizeof(timbi2s_platform_data), ++ }, ++ { ++ .name = "timb-most", ++ .num_resources = ARRAY_SIZE(timberdale_most_resources), ++ .resources = timberdale_most_resources, ++ .platform_data = &timberdale_mlb_platform_data, ++ .data_size = sizeof(timberdale_mlb_platform_data), ++ }, ++ { ++ .name = "timb-video", ++ .num_resources = ARRAY_SIZE(timberdale_video_resources), ++ .resources = timberdale_video_resources, ++ .platform_data = &timberdale_video_platform_data, ++ .data_size = sizeof(timberdale_video_platform_data), ++ }, ++ { ++ .name = "timb-radio", ++ .num_resources = ARRAY_SIZE(timberdale_radio_resources), ++ .resources = timberdale_radio_resources, ++ .platform_data = &timberdale_radio_platform_data, ++ .data_size = sizeof(timberdale_radio_platform_data), ++ }, ++ { ++ .name = "xilinx_spi", ++ .num_resources = ARRAY_SIZE(timberdale_spi_resources), ++ .resources = timberdale_spi_resources, ++ .platform_data = &timberdale_xspi_platform_data, ++ .data_size = sizeof(timberdale_xspi_platform_data), ++ }, ++ { ++ .name = "ks8842", ++ .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[] = { ++ /* located in bar 1 and bar 2 */ ++ { ++ .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), ++ .resources = timberdale_sdhc_resources, ++ }, ++}; ++ ++static __devinitdata struct mfd_cell timberdale_cells_bar2[] = { ++ { ++ .name = "sdhci", ++ .num_resources = ARRAY_SIZE(timberdale_sdhc_resources), ++ .resources = timberdale_sdhc_resources, ++ }, ++}; ++ ++static ssize_t show_fw_ver(struct device *dev, struct device_attribute *attr, ++ char *buf) ++{ ++ struct pci_dev *pdev = to_pci_dev(dev); ++ struct timberdale_device *priv = pci_get_drvdata(pdev); ++ ++ return sprintf(buf, "%d.%d.%d\n", priv->fw.major, priv->fw.minor, ++ priv->fw.config); ++} ++ ++static DEVICE_ATTR(fw_ver, S_IRUGO, show_fw_ver, NULL); ++ ++/*--------------------------------------------------------------------------*/ ++ ++static int __devinit timb_probe(struct pci_dev *dev, ++ const struct pci_device_id *id) ++{ ++ struct timberdale_device *priv; ++ int err, i; ++ resource_size_t mapbase; ++ struct msix_entry *msix_entries = NULL; ++ u8 ip_setup; ++ ++ 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 DRIVER_NAME ": No resource\n"); ++ goto err_start; ++ } ++ ++ /* create a resource for the PCI master register */ ++ priv->ctl_mapbase = mapbase + CHIPCTLOFFSET; ++ if (!request_mem_region(priv->ctl_mapbase, CHIPCTLSIZE, "timb-ctl")) { ++ printk(KERN_ERR DRIVER_NAME ": Failed to request ctl mem\n"); ++ goto err_request; ++ } ++ ++ priv->ctl_membase = ioremap(priv->ctl_mapbase, CHIPCTLSIZE); ++ if (!priv->ctl_membase) { ++ printk(KERN_ALERT DRIVER_NAME": Map error, ctl\n"); ++ goto err_ioremap; ++ } ++ ++ /* read the HW config */ ++ priv->fw.major = ioread32(priv->ctl_membase + TIMB_REV_MAJOR); ++ priv->fw.minor = ioread32(priv->ctl_membase + TIMB_REV_MINOR); ++ priv->fw.config = ioread32(priv->ctl_membase + TIMB_HW_CONFIG); ++ ++ if (priv->fw.major > TIMB_SUPPORTED_MAJOR) { ++ printk(KERN_ERR DRIVER_NAME": The driver supports an older " ++ "version of the FPGA, please update the driver to " ++ "support %d.%d\n", priv->fw.major, priv->fw.minor); ++ goto err_ioremap; ++ } ++ if (priv->fw.major < TIMB_SUPPORTED_MAJOR || ++ priv->fw.minor < TIMB_REQUIRED_MINOR) { ++ printk(KERN_ERR DRIVER_NAME ++ ": The FPGA image is too old (%d.%d), " ++ "please upgrade the FPGA to at least: %d.%d\n", ++ priv->fw.major, priv->fw.minor, ++ TIMB_SUPPORTED_MAJOR, TIMB_REQUIRED_MINOR); ++ goto err_ioremap; ++ } ++ ++ msix_entries = kzalloc(TIMBERDALE_NR_IRQS * sizeof(*msix_entries), ++ GFP_KERNEL); ++ if (!msix_entries) ++ goto err_ioremap; ++ ++ for (i = 0; i < TIMBERDALE_NR_IRQS; i++) ++ msix_entries[i].entry = i; ++ ++ err = pci_enable_msix(dev, msix_entries, TIMBERDALE_NR_IRQS); ++ if (err) { ++ printk(KERN_WARNING DRIVER_NAME ++ ": MSI-X init failed: %d, expected entries: %d\n", ++ err, TIMBERDALE_NR_IRQS); ++ goto err_msix; ++ } ++ ++ err = device_create_file(&dev->dev, &dev_attr_fw_ver); ++ if (err) ++ goto err_create_file; ++ ++ /* Reset all FPGA PLB peripherals */ ++ iowrite32(0x1, priv->ctl_membase + TIMB_SW_RST); ++ ++ /* update IRQ offsets in I2C board info */ ++ for (i = 0; i < ARRAY_SIZE(timberdale_i2c_board_info); i++) ++ timberdale_i2c_board_info[i].irq = ++ msix_entries[timberdale_i2c_board_info[i].irq].vector; ++ ++ /* Update the SPI configuration depending on the HW (8 or 16 bit) */ ++ if (priv->fw.config & TIMB_HW_CONFIG_SPI_8BIT) { ++ timberdale_xspi_platform_data.bits_per_word = 8; ++ timberdale_xspi_platform_data.devices = ++ timberdale_spi_8bit_board_info; ++ timberdale_xspi_platform_data.num_devices = ++ ARRAY_SIZE(timberdale_spi_8bit_board_info); ++ } else { ++ timberdale_xspi_platform_data.bits_per_word = 16; ++ timberdale_xspi_platform_data.devices = ++ timberdale_spi_16bit_board_info; ++ timberdale_xspi_platform_data.num_devices = ++ ARRAY_SIZE(timberdale_spi_16bit_board_info); ++ } ++ ++ ip_setup = priv->fw.config & TIMB_HW_VER_MASK; ++ if (ip_setup == TIMB_HW_VER0) ++ err = mfd_add_devices(&dev->dev, -1, ++ timberdale_cells_bar0_cfg0, ++ ARRAY_SIZE(timberdale_cells_bar0_cfg0), ++ &dev->resource[0], msix_entries[0].vector); ++ else if (ip_setup == TIMB_HW_VER1) ++ err = mfd_add_devices(&dev->dev, -1, ++ timberdale_cells_bar0_cfg1, ++ ARRAY_SIZE(timberdale_cells_bar0_cfg1), ++ &dev->resource[0], msix_entries[0].vector); ++ else if (ip_setup == TIMB_HW_VER2) ++ err = mfd_add_devices(&dev->dev, -1, ++ timberdale_cells_bar0_cfg2, ++ ARRAY_SIZE(timberdale_cells_bar0_cfg2), ++ &dev->resource[0], msix_entries[0].vector); ++ else if (ip_setup == TIMB_HW_VER3) ++ err = mfd_add_devices(&dev->dev, -1, ++ timberdale_cells_bar0_cfg3, ++ ARRAY_SIZE(timberdale_cells_bar0_cfg3), ++ &dev->resource[0], msix_entries[0].vector); ++ else { ++ /* unknown version */ ++ printk(KERN_ERR"Uknown IP setup: %d.%d.%d\n", ++ priv->fw.major, priv->fw.minor, ip_setup); ++ err = -ENODEV; ++ goto err_mfd; ++ } ++ ++ if (err) { ++ printk(KERN_WARNING DRIVER_NAME ++ ": mfd_add_devices failed: %d\n", err); ++ goto err_mfd; ++ } ++ ++ err = mfd_add_devices(&dev->dev, 0, ++ timberdale_cells_bar1, ARRAY_SIZE(timberdale_cells_bar1), ++ &dev->resource[1], msix_entries[0].vector); ++ if (err) { ++ printk(KERN_WARNING DRIVER_NAME ++ "mfd_add_devices failed: %d\n", err); ++ goto err_mfd2; ++ } ++ ++ /* only version 0 and 3 have the iNand routed to SDHCI */ ++ if (((priv->fw.config & TIMB_HW_VER_MASK) == TIMB_HW_VER0) || ++ ((priv->fw.config & TIMB_HW_VER_MASK) == TIMB_HW_VER3)) { ++ err = mfd_add_devices(&dev->dev, 1, timberdale_cells_bar2, ++ ARRAY_SIZE(timberdale_cells_bar2), ++ &dev->resource[2], msix_entries[0].vector); ++ if (err) { ++ printk(KERN_WARNING DRIVER_NAME ++ ": mfd_add_devices failed: %d\n", err); ++ goto err_mfd2; ++ } ++ } ++ ++ kfree(msix_entries); ++ ++ printk(KERN_INFO ++ "Found Timberdale Card. Rev: %d.%d, HW config: 0x%02x\n", ++ priv->fw.major, priv->fw.minor, priv->fw.config); ++ ++ return 0; ++ ++err_mfd2: ++ mfd_remove_devices(&dev->dev); ++err_mfd: ++ device_remove_file(&dev->dev, &dev_attr_fw_ver); ++err_create_file: ++ pci_disable_msix(dev); ++err_msix: ++ iounmap(priv->ctl_membase); ++err_ioremap: ++ release_mem_region(priv->ctl_mapbase, CHIPCTLSIZE); ++err_request: ++ pci_set_drvdata(dev, NULL); ++err_start: ++ pci_disable_device(dev); ++err_enable: ++ kfree(msix_entries); ++ kfree(priv); ++ pci_set_drvdata(dev, NULL); ++ return -ENODEV; ++} ++ ++static void __devexit timb_remove(struct pci_dev *dev) ++{ ++ struct timberdale_device *priv = pci_get_drvdata(dev); ++ ++ mfd_remove_devices(&dev->dev); ++ ++ device_remove_file(&dev->dev, &dev_attr_fw_ver); ++ ++ iounmap(priv->ctl_membase); ++ release_mem_region(priv->ctl_mapbase, CHIPCTLSIZE); ++ ++ pci_disable_msix(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 = DRIVER_NAME, ++ .id_table = timberdale_pci_tbl, ++ .probe = timb_probe, ++ .remove = __devexit_p(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_init(timberdale_init); ++module_exit(timberdale_exit); ++ ++MODULE_AUTHOR("Mocean Laboratories "); ++MODULE_VERSION(DRV_VERSION); ++MODULE_LICENSE("GPL v2"); ++ +diff -uNr linux-2.6.31/drivers/mfd/timberdale.h linux-2.6.31.new/drivers/mfd/timberdale.h +--- linux-2.6.31/drivers/mfd/timberdale.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/mfd/timberdale.h 2009-10-23 11:17:29.000000000 -0700 +@@ -0,0 +1,152 @@ ++/* ++ * 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 ++ ++#define DRV_VERSION "1.0" ++ ++/* This driver only support versions >= 3.8 and < 4.0 */ ++#define TIMB_SUPPORTED_MAJOR 3 ++ ++/* This driver only support minor >= 8 */ ++#define TIMB_REQUIRED_MINOR 8 ++ ++/* 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_MAJOR 0x00 ++#define TIMB_REV_MINOR 0x04 ++#define TIMB_HW_CONFIG 0x08 ++#define TIMB_SW_RST 0x40 ++ ++/* bits in the TIMB_HW_CONFIG register */ ++#define TIMB_HW_CONFIG_SPI_8BIT 0x80 ++ ++#define TIMB_HW_VER_MASK 0x0f ++#define TIMB_HW_VER0 0x00 ++#define TIMB_HW_VER1 0x01 ++#define TIMB_HW_VER2 0x02 ++#define TIMB_HW_VER3 0x03 ++ ++#define OCORESOFFSET 0x0 ++#define OCORESEND 0x1f ++ ++#define SPIOFFSET 0x80 ++#define SPIEND 0xff ++ ++#define UARTLITEOFFSET 0x100 ++#define UARTLITEEND 0x10f ++ ++#define RDSOFFSET 0x180 ++#define RDSEND 0x183 ++ ++#define ETHOFFSET 0x300 ++#define ETHEND 0x3ff ++ ++#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 XIICOFFSET 0x1800 ++#define XIICEND 0x19ff ++ ++#define I2SOFFSET 0x1C00 ++#define I2SEND 0x1fff ++ ++#define LOGIWOFFSET 0x30000 ++#define LOGIWEND 0x37fff ++ ++#define MLCOREOFFSET 0x40000 ++#define MLCOREEND 0x43fff ++ ++#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 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 IRQ_TIMBERDALE_UARTLITE 12 ++#define IRQ_TIMBERDALE_MLCORE 13 ++#define IRQ_TIMBERDALE_MLCORE_BUF 14 ++#define IRQ_TIMBERDALE_RDS 15 ++ ++#define TIMBERDALE_NR_IRQS 16 ++ ++/* 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_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) | \ ++ (1 << IRQ_TIMBERDALE_DMA)) ++ ++#define GPIO_PIN_ASCB 8 ++#define GPIO_PIN_INIC_RST 14 ++#define GPIO_PIN_BT_RST 15 ++#define GPIO_NR_PINS 16 ++ ++#endif ++ +diff -uNr linux-2.6.31/drivers/mmc/host/sdhci.c linux-2.6.31.new/drivers/mmc/host/sdhci.c +--- linux-2.6.31/drivers/mmc/host/sdhci.c 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/mmc/host/sdhci.c 2009-10-23 11:17:25.000000000 -0700 +@@ -652,7 +652,7 @@ + count = sdhci_calc_timeout(host, data); + sdhci_writeb(host, count, SDHCI_TIMEOUT_CONTROL); + +- if (host->flags & SDHCI_USE_DMA) ++ if (host->flags & (SDHCI_USE_SDMA | SDHCI_USE_ADMA)) + host->flags |= SDHCI_REQ_USE_DMA; + + /* +@@ -1597,7 +1597,7 @@ + { + int ret; + +- if (host->flags & SDHCI_USE_DMA) { ++ if (host->flags & (SDHCI_USE_SDMA | SDHCI_USE_ADMA)) { + if (host->ops->enable_dma) + host->ops->enable_dma(host); + } +@@ -1678,23 +1678,20 @@ + caps = sdhci_readl(host, SDHCI_CAPABILITIES); + + if (host->quirks & SDHCI_QUIRK_FORCE_DMA) +- host->flags |= SDHCI_USE_DMA; +- else if (!(caps & SDHCI_CAN_DO_DMA)) +- DBG("Controller doesn't have DMA capability\n"); ++ host->flags |= SDHCI_USE_SDMA; ++ else if (!(caps & SDHCI_CAN_DO_SDMA)) ++ DBG("Controller doesn't have SDMA capability\n"); + else +- host->flags |= SDHCI_USE_DMA; ++ host->flags |= SDHCI_USE_SDMA; + + if ((host->quirks & SDHCI_QUIRK_BROKEN_DMA) && +- (host->flags & SDHCI_USE_DMA)) { ++ (host->flags & SDHCI_USE_SDMA)) { + DBG("Disabling DMA as it is marked broken\n"); +- host->flags &= ~SDHCI_USE_DMA; ++ host->flags &= ~SDHCI_USE_SDMA; + } + +- if (host->flags & SDHCI_USE_DMA) { +- if ((host->version >= SDHCI_SPEC_200) && +- (caps & SDHCI_CAN_DO_ADMA2)) +- host->flags |= SDHCI_USE_ADMA; +- } ++ if ((host->version >= SDHCI_SPEC_200) && (caps & SDHCI_CAN_DO_ADMA2)) ++ host->flags |= SDHCI_USE_ADMA; + + if ((host->quirks & SDHCI_QUIRK_BROKEN_ADMA) && + (host->flags & SDHCI_USE_ADMA)) { +@@ -1702,13 +1699,14 @@ + host->flags &= ~SDHCI_USE_ADMA; + } + +- if (host->flags & SDHCI_USE_DMA) { ++ if (host->flags & (SDHCI_USE_SDMA | SDHCI_USE_ADMA)) { + if (host->ops->enable_dma) { + if (host->ops->enable_dma(host)) { + printk(KERN_WARNING "%s: No suitable DMA " + "available. Falling back to PIO.\n", + mmc_hostname(mmc)); +- host->flags &= ~(SDHCI_USE_DMA | SDHCI_USE_ADMA); ++ host->flags &= ++ ~(SDHCI_USE_SDMA | SDHCI_USE_ADMA); + } + } + } +@@ -1736,7 +1734,7 @@ + * mask, but PIO does not need the hw shim so we set a new + * mask here in that case. + */ +- if (!(host->flags & SDHCI_USE_DMA)) { ++ if (!(host->flags & (SDHCI_USE_SDMA | SDHCI_USE_ADMA))) { + host->dma_mask = DMA_BIT_MASK(64); + mmc_dev(host->mmc)->dma_mask = &host->dma_mask; + } +@@ -1810,7 +1808,7 @@ + */ + if (host->flags & SDHCI_USE_ADMA) + mmc->max_hw_segs = 128; +- else if (host->flags & SDHCI_USE_DMA) ++ else if (host->flags & SDHCI_USE_SDMA) + mmc->max_hw_segs = 1; + else /* PIO */ + mmc->max_hw_segs = 128; +@@ -1893,10 +1891,10 @@ + + mmc_add_host(mmc); + +- printk(KERN_INFO "%s: SDHCI controller on %s [%s] using %s%s\n", ++ printk(KERN_INFO "%s: SDHCI controller on %s [%s] using %s\n", + mmc_hostname(mmc), host->hw_name, dev_name(mmc_dev(mmc)), +- (host->flags & SDHCI_USE_ADMA)?"A":"", +- (host->flags & SDHCI_USE_DMA)?"DMA":"PIO"); ++ (host->flags & SDHCI_USE_ADMA) ? "ADMA" : ++ (host->flags & SDHCI_USE_SDMA) ? "DMA" : "PIO"); + + sdhci_enable_card_detection(host); + +diff -uNr linux-2.6.31/drivers/mmc/host/sdhci.h linux-2.6.31.new/drivers/mmc/host/sdhci.h +--- linux-2.6.31/drivers/mmc/host/sdhci.h 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/mmc/host/sdhci.h 2009-10-23 11:17:25.000000000 -0700 +@@ -143,7 +143,7 @@ + #define SDHCI_CAN_DO_ADMA2 0x00080000 + #define SDHCI_CAN_DO_ADMA1 0x00100000 + #define SDHCI_CAN_DO_HISPD 0x00200000 +-#define SDHCI_CAN_DO_DMA 0x00400000 ++#define SDHCI_CAN_DO_SDMA 0x00400000 + #define SDHCI_CAN_VDD_330 0x01000000 + #define SDHCI_CAN_VDD_300 0x02000000 + #define SDHCI_CAN_VDD_180 0x04000000 +@@ -250,7 +250,7 @@ + spinlock_t lock; /* Mutex */ + + int flags; /* Host attributes */ +-#define SDHCI_USE_DMA (1<<0) /* Host is DMA capable */ ++#define SDHCI_USE_SDMA (1<<0) /* Host is SDMA capable */ + #define SDHCI_USE_ADMA (1<<1) /* Host is ADMA capable */ + #define SDHCI_REQ_USE_DMA (1<<2) /* Use DMA for this req. */ + #define SDHCI_DEVICE_DEAD (1<<3) /* Device unresponsive */ +diff -uNr linux-2.6.31/drivers/mmc/host/sdhci-pci.c linux-2.6.31.new/drivers/mmc/host/sdhci-pci.c +--- linux-2.6.31/drivers/mmc/host/sdhci-pci.c 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/mmc/host/sdhci-pci.c 2009-10-23 11:17:25.000000000 -0700 +@@ -395,7 +395,7 @@ + + if (((pdev->class & 0xFFFF00) == (PCI_CLASS_SYSTEM_SDHCI << 8)) && + ((pdev->class & 0x0000FF) != PCI_SDHCI_IFDMA) && +- (host->flags & SDHCI_USE_DMA)) { ++ (host->flags & SDHCI_USE_SDMA)) { + dev_warn(&pdev->dev, "Will use DMA mode even though HW " + "doesn't fully claim to support it.\n"); + } +diff -uNr linux-2.6.31/drivers/net/Kconfig linux-2.6.31.new/drivers/net/Kconfig +--- linux-2.6.31/drivers/net/Kconfig 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/net/Kconfig 2009-10-23 11:17:23.000000000 -0700 +@@ -1730,6 +1730,16 @@ + This platform driver is for Micrel KSZ8842 / KS8842 + 2-port ethernet switch chip (managed, VLAN, QoS). + ++config KS8842_TIMB_DMA ++ bool "Use Timberdale specific DMA engine" ++ depends on KS8842 && MFD_TIMBERDALE ++ select MFD_TIMBERDALE_DMA ++ help ++ This option enables usage of the timberdale specific DMA engine ++ for the KS8842 driver. Rather than using PIO which results in ++ single accesses over PCIe, the DMA block of the timberdale FPGA ++ will burst data to and from the KS8842. ++ + config KS8851 + tristate "Micrel KS8851 SPI" + depends on SPI +diff -uNr linux-2.6.31/drivers/net/ks8842.c linux-2.6.31.new/drivers/net/ks8842.c +--- linux-2.6.31/drivers/net/ks8842.c 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/net/ks8842.c 2009-10-23 11:17:22.000000000 -0700 +@@ -26,11 +26,17 @@ + #include + #include + #include ++#include + + #define DRV_NAME "ks8842" + + /* Timberdale specific Registers */ +-#define REG_TIMB_RST 0x1c ++#define REG_TIMB_RST 0x1c ++#define REG_TIMB_FIFO 0x20 ++#define REG_TIMB_ISR 0x24 ++#define REG_TIMB_IER 0x28 ++#define REG_TIMB_IAR 0x2C ++#define REQ_TIMB_DMA_RESUME 0x30 + + /* KS8842 registers */ + +@@ -73,6 +79,11 @@ + #define IRQ_RX_ERROR 0x0080 + #define ENABLED_IRQS (IRQ_LINK_CHANGE | IRQ_TX | IRQ_RX | IRQ_RX_STOPPED | \ + IRQ_TX_STOPPED | IRQ_RX_OVERRUN | IRQ_RX_ERROR) ++#ifdef CONFIG_KS8842_TIMB_DMA ++ #define ENABLED_IRQS_IP (IRQ_LINK_CHANGE | IRQ_RX_STOPPED | \ ++ IRQ_TX_STOPPED | IRQ_RX_OVERRUN | IRQ_RX_ERROR) ++ #define ENABLED_IRQS_DMA (ENABLED_IRQS_IP | IRQ_RX) ++#endif + #define REG_ISR 0x02 + #define REG_RXSR 0x04 + #define RXSR_VALID 0x8000 +@@ -111,14 +122,50 @@ + #define REG_P1CR4 0x02 + #define REG_P1SR 0x04 + ++#ifdef CONFIG_KS8842_TIMB_DMA ++#define DMA_BUFFER_SIZE 2048 ++ ++#define DMA_DEV(a) ((a->dev->parent) ? a->dev->parent : a->dev) ++ ++#define DMA_ONGOING(a) (a->dma_tx.ongoing | a->dma_rx.ongoing) ++ ++struct ks8842_dma_ctl { ++ void *desc; ++ void *buf; ++ dma_addr_t addr; ++ unsigned ongoing; ++}; ++ ++struct ks8842_rx_dma_ctl { ++ void *desc; ++ struct sk_buff *skb; ++ dma_addr_t addr; ++}; ++ ++#endif ++ + struct ks8842_adapter { + void __iomem *hw_addr; + int irq; + struct tasklet_struct tasklet; + spinlock_t lock; /* spinlock to be interrupt safe */ +- struct platform_device *pdev; ++ struct device *dev; ++ struct work_struct timeout_work; ++ struct net_device *netdev; ++#ifdef CONFIG_KS8842_TIMB_DMA ++ unsigned use_dma; ++ struct ks8842_dma_ctl dma_tx; ++ struct ks8842_rx_dma_ctl dma_rx; ++#endif + }; + ++#ifdef CONFIG_KS8842_TIMB_DMA ++static inline void ks8842_resume_dma(struct ks8842_adapter *adapter) ++{ ++ iowrite32(1, adapter->hw_addr + REQ_TIMB_DMA_RESUME); ++} ++#endif ++ + static inline void ks8842_select_bank(struct ks8842_adapter *adapter, u16 bank) + { + iowrite16(bank, adapter->hw_addr + REG_SELECT_BANK); +@@ -195,7 +242,6 @@ + msleep(10); + iowrite16(0, adapter->hw_addr + REG_GRR); + */ +- iowrite16(32, adapter->hw_addr + REG_SELECT_BANK); + iowrite32(0x1, adapter->hw_addr + REG_TIMB_RST); + msleep(20); + } +@@ -203,8 +249,10 @@ + static void ks8842_update_link_status(struct net_device *netdev, + struct ks8842_adapter *adapter) + { ++ u16 p1mbsr = ks8842_read16(adapter, 45, REG_P1MBSR); ++ + /* check the status of the link */ +- if (ks8842_read16(adapter, 45, REG_P1MBSR) & 0x4) { ++ if (p1mbsr & 0x4) { + netif_carrier_on(netdev); + netif_wake_queue(netdev); + } else { +@@ -241,10 +289,8 @@ + /* Enable QMU Transmit flow control / transmit padding / Transmit CRC */ + ks8842_write16(adapter, 16, 0x000E, REG_TXCR); + +- /* enable the receiver, uni + multi + broadcast + flow ctrl +- + crc strip */ +- ks8842_write16(adapter, 16, 0x8 | 0x20 | 0x40 | 0x80 | 0x400, +- REG_RXCR); ++ /* enable the receiver, uni + multi + broadcast + crc strip */ ++ ks8842_write16(adapter, 16, 0x8 | 0x20 | 0x40 | 0x80, REG_RXCR); + + /* TX frame pointer autoincrement */ + ks8842_write16(adapter, 17, 0x4000, REG_TXFDPR); +@@ -261,13 +307,11 @@ + /* enable no excessive collison drop */ + ks8842_enable_bits(adapter, 32, 1 << 3, REG_SGCR2); + +- /* Enable port 1 force flow control / back pressure / transmit / recv */ +- ks8842_write16(adapter, 48, 0x1E07, REG_P1CR2); ++ /* Enable port 1 / back pressure / transmit / recv */ ++ ks8842_write16(adapter, 48, 0xE07, REG_P1CR2); + + /* restart port auto-negotiation */ + ks8842_enable_bits(adapter, 49, 1 << 13, REG_P1CR4); +- /* only advertise 10Mbps */ +- ks8842_clear_bits(adapter, 49, 3 << 2, REG_P1CR4); + + /* Enable the transmitter */ + ks8842_enable_tx(adapter); +@@ -279,7 +323,17 @@ + ks8842_write16(adapter, 18, 0xffff, REG_ISR); + + /* enable interrupts */ +- ks8842_write16(adapter, 18, ENABLED_IRQS, REG_IER); ++#ifdef CONFIG_KS8842_TIMB_DMA ++ if (adapter->use_dma) { ++ iowrite16(ENABLED_IRQS_IP, adapter->hw_addr + REG_TIMB_IER); ++ ks8842_write16(adapter, 18, ENABLED_IRQS_DMA, REG_IER); ++ } else { ++#endif ++ ks8842_write16(adapter, 18, ENABLED_IRQS, REG_IER); ++ iowrite16(ENABLED_IRQS, adapter->hw_addr + REG_TIMB_IER); ++#ifdef CONFIG_KS8842_TIMB_DMA ++ } ++#endif + + /* enable the switch */ + ks8842_write16(adapter, 32, 0x1, REG_SW_ID_AND_ENABLE); +@@ -302,11 +356,73 @@ + ks8842_write16(adapter, 39, mac, REG_MACAR3); + } + ++static void ks8842_write_mac_addr(struct ks8842_adapter *adapter, u8 *mac) ++{ ++ unsigned long flags; ++ unsigned i; ++ ++ spin_lock_irqsave(&adapter->lock, flags); ++ for (i = 0; i < ETH_ALEN; i++) { ++ ks8842_write8(adapter, 2, mac[ETH_ALEN - i - 1], REG_MARL + i); ++ ks8842_write8(adapter, 39, mac[ETH_ALEN - i - 1], ++ REG_MACAR1 + i); ++ } ++ spin_unlock_irqrestore(&adapter->lock, flags); ++} ++ + static inline u16 ks8842_tx_fifo_space(struct ks8842_adapter *adapter) + { + return ks8842_read16(adapter, 16, REG_TXMIR) & 0x1fff; + } + ++#ifdef CONFIG_KS8842_TIMB_DMA ++static int ks8842_tx_frame_dma(struct sk_buff *skb, struct net_device *netdev) ++{ ++ struct ks8842_adapter *adapter = netdev_priv(netdev); ++ struct ks8842_dma_ctl *ctl = &adapter->dma_tx; ++ int err; ++ int len = skb->len + sizeof(u32); ++ u8 *buf = ctl->buf; ++ ++ if (ctl->ongoing) { ++ dev_dbg(adapter->dev, "%s: TX ongoing\n", __func__); ++ /* transfer ongoing */ ++ return NETDEV_TX_BUSY; ++ } ++ ++ /* copy data to the TX buffer */ ++ /* the control word, enable IRQ, port 1 and the length */ ++ *buf++ = 0x00; ++ *buf++ = 0x01; /* Port 1 */ ++ *buf++ = skb->len & 0xff; ++ *buf++ = (skb->len >> 8) & 0xff; ++ skb_copy_from_linear_data(skb, buf, skb->len); ++ ++ dma_sync_single_range_for_device(DMA_DEV(adapter), ctl->addr, 0, len, ++ DMA_TO_DEVICE); ++ ++ /* make sure the length is a multiple of 4 */ ++ if (len % 4) ++ len += 4 - len % 4; ++ ++ err = timbdma_prep_desc(ctl->desc, ctl->addr, len); ++ if (err) ++ return NETDEV_TX_BUSY; ++ ++ ctl->ongoing = 1; ++ err = timbdma_start(DMA_IRQ_ETH_TX, ctl->desc, 0); ++ if (err) { ++ ctl->ongoing = 0; ++ return NETDEV_TX_BUSY; ++ } ++ netdev->stats.tx_bytes += skb->len; ++ ++ dev_kfree_skb(skb); ++ ++ return NETDEV_TX_OK; ++} ++#endif ++ + static int ks8842_tx_frame(struct sk_buff *skb, struct net_device *netdev) + { + struct ks8842_adapter *adapter = netdev_priv(netdev); +@@ -314,7 +430,7 @@ + u32 *ptr = (u32 *)skb->data; + u32 ctrl; + +- dev_dbg(&adapter->pdev->dev, ++ dev_dbg(adapter->dev, + "%s: len %u head %p data %p tail %p end %p\n", + __func__, skb->len, skb->head, skb->data, + skb_tail_pointer(skb), skb_end_pointer(skb)); +@@ -344,6 +460,104 @@ + return NETDEV_TX_OK; + } + ++#ifdef CONFIG_KS8842_TIMB_DMA ++static int __ks8842_start_new_rx_dma(struct net_device *netdev, ++ struct ks8842_adapter *adapter) ++{ ++ struct ks8842_rx_dma_ctl *ctl = &adapter->dma_rx; ++ int err; ++ ++ ctl->skb = netdev_alloc_skb(netdev, DMA_BUFFER_SIZE); ++ if (ctl->skb) { ++ ctl->addr = dma_map_single(DMA_DEV(adapter), ctl->skb->data, ++ DMA_BUFFER_SIZE, DMA_FROM_DEVICE); ++ err = dma_mapping_error(DMA_DEV(adapter), ctl->addr); ++ if (unlikely(err)) { ++ ctl->addr = 0; ++ goto out; ++ } ++ err = timbdma_prep_desc(ctl->desc, ctl->addr, DMA_BUFFER_SIZE); ++ if (unlikely(err)) ++ goto out; ++ err = timbdma_start(DMA_IRQ_ETH_RX, ctl->desc, 0); ++ if (unlikely(err)) ++ goto out; ++ } else { ++ err = -ENOMEM; ++ ctl->addr = 0; ++ goto out; ++ } ++ ++ return err; ++out: ++ if (ctl->addr) ++ dma_unmap_single(DMA_DEV(adapter), ctl->addr, DMA_BUFFER_SIZE, ++ DMA_FROM_DEVICE); ++ ctl->addr = 0; ++ if (ctl->skb) ++ dev_kfree_skb(ctl->skb); ++ ++ ctl->skb = NULL; ++ ++ printk(KERN_ERR DRV_NAME": Failed to start RX DMA: %d\n", err); ++ return err; ++} ++ ++static void ks8842_rx_frame_dma(struct net_device *netdev, ++ struct ks8842_adapter *adapter) ++{ ++ struct ks8842_rx_dma_ctl *ctl = &adapter->dma_rx; ++ struct sk_buff *skb = ctl->skb; ++ dma_addr_t addr = ctl->addr; ++ u32 status; ++ ++ /* kick next transfer going */ ++ __ks8842_start_new_rx_dma(netdev, adapter); ++ ++ /* now handle the data we got */ ++ dma_unmap_single(DMA_DEV(adapter), addr, DMA_BUFFER_SIZE, ++ DMA_FROM_DEVICE); ++ ++ status = *((u32 *)skb->data); ++ ++ dev_dbg(adapter->dev, "%s - rx_data: status: %x\n", ++ __func__, status & 0xffff); ++ ++ /* check the status */ ++ if ((status & RXSR_VALID) && !(status & RXSR_ERROR)) { ++ int len = (status >> 16) & 0x7ff; ++ ++ dev_dbg(adapter->dev, "%s, got package, len: %d, skb: %p\n", ++ __func__, len, skb); ++ ++ netdev->stats.rx_packets++; ++ netdev->stats.rx_bytes += len; ++ if (status & RXSR_MULTICAST) ++ netdev->stats.multicast++; ++ ++ /* we are not nice to the stack, we want to be nice ++ * to our DMA engine instead, reserve 4 bytes ++ * which is the status word ++ */ ++ skb_reserve(skb, 4); ++ skb_put(skb, len); ++ ++ skb->protocol = eth_type_trans(skb, netdev); ++ netif_rx(skb); ++ } else { ++ dev_dbg(adapter->dev, "RX error, status: %x\n", status); ++ netdev->stats.rx_errors++; ++ if (status & RXSR_TOO_LONG) ++ netdev->stats.rx_length_errors++; ++ if (status & RXSR_CRC_ERROR) ++ netdev->stats.rx_crc_errors++; ++ if (status & RXSR_RUNT) ++ netdev->stats.rx_frame_errors++; ++ dev_kfree_skb(skb); ++ } ++} ++#endif ++ + static void ks8842_rx_frame(struct net_device *netdev, + struct ks8842_adapter *adapter) + { +@@ -352,14 +566,14 @@ + + status &= 0xffff; + +- dev_dbg(&adapter->pdev->dev, "%s - rx_data: status: %x\n", ++ dev_dbg(adapter->dev, "%s - rx_data: status: %x\n", + __func__, status); + + /* check the status */ + if ((status & RXSR_VALID) && !(status & RXSR_ERROR)) { + struct sk_buff *skb = netdev_alloc_skb(netdev, len + 2); + +- dev_dbg(&adapter->pdev->dev, "%s, got package, len: %d\n", ++ dev_dbg(adapter->dev, "%s, got package, len: %d\n", + __func__, len); + if (skb) { + u32 *data; +@@ -386,7 +600,7 @@ + } else + netdev->stats.rx_dropped++; + } else { +- dev_dbg(&adapter->pdev->dev, "RX error, status: %x\n", status); ++ dev_dbg(adapter->dev, "RX error, status: %x\n", status); + netdev->stats.rx_errors++; + if (status & RXSR_TOO_LONG) + netdev->stats.rx_length_errors++; +@@ -409,7 +623,7 @@ + void ks8842_handle_rx(struct net_device *netdev, struct ks8842_adapter *adapter) + { + u16 rx_data = ks8842_read16(adapter, 16, REG_RXMIR) & 0x1fff; +- dev_dbg(&adapter->pdev->dev, "%s Entry - rx_data: %d\n", ++ dev_dbg(adapter->dev, "%s Entry - rx_data: %d\n", + __func__, rx_data); + while (rx_data) { + ks8842_rx_frame(netdev, adapter); +@@ -420,7 +634,7 @@ + void ks8842_handle_tx(struct net_device *netdev, struct ks8842_adapter *adapter) + { + u16 sr = ks8842_read16(adapter, 16, REG_TXSR); +- dev_dbg(&adapter->pdev->dev, "%s - entry, sr: %x\n", __func__, sr); ++ dev_dbg(adapter->dev, "%s - entry, sr: %x\n", __func__, sr); + netdev->stats.tx_packets++; + if (netif_queue_stopped(netdev)) + netif_wake_queue(netdev); +@@ -429,7 +643,7 @@ + void ks8842_handle_rx_overrun(struct net_device *netdev, + struct ks8842_adapter *adapter) + { +- dev_dbg(&adapter->pdev->dev, "%s: entry\n", __func__); ++ dev_dbg(adapter->dev, "%s: entry\n", __func__); + netdev->stats.rx_errors++; + netdev->stats.rx_fifo_errors++; + } +@@ -448,20 +662,33 @@ + spin_unlock_irqrestore(&adapter->lock, flags); + + isr = ks8842_read16(adapter, 18, REG_ISR); +- dev_dbg(&adapter->pdev->dev, "%s - ISR: 0x%x\n", __func__, isr); ++ dev_dbg(adapter->dev, "%s - ISR: 0x%x\n", __func__, isr); ++ ++#ifdef CONFIG_KS8842_TIMB_DMA ++ if (adapter->use_dma) ++ isr &= ~IRQ_RX; ++#endif + + /* Ack */ + ks8842_write16(adapter, 18, isr, REG_ISR); + ++ /* Ack in the timberdale IP as well */ ++ iowrite32(0x1, adapter->hw_addr + REG_TIMB_IAR); ++ + if (!netif_running(netdev)) + return; + + if (isr & IRQ_LINK_CHANGE) + ks8842_update_link_status(netdev, adapter); + ++ /* should not get IRQ_RX when in DMA mode */ + if (isr & (IRQ_RX | IRQ_RX_ERROR)) +- ks8842_handle_rx(netdev, adapter); ++#ifdef CONFIG_KS8842_TIMB_DMA ++ if (!adapter->use_dma) ++#endif ++ ks8842_handle_rx(netdev, adapter); + ++ /* should only happen when not doing DMA */ + if (isr & IRQ_TX) + ks8842_handle_tx(netdev, adapter); + +@@ -480,8 +707,18 @@ + + /* re-enable interrupts, put back the bank selection register */ + spin_lock_irqsave(&adapter->lock, flags); +- ks8842_write16(adapter, 18, ENABLED_IRQS, REG_IER); ++#ifdef CONFIG_KS8842_TIMB_DMA ++ if (adapter->use_dma) ++ ks8842_write16(adapter, 18, ENABLED_IRQS_DMA, REG_IER); ++ else ++#endif ++ ks8842_write16(adapter, 18, ENABLED_IRQS, REG_IER); ++ + iowrite16(entry_bank, adapter->hw_addr + REG_SELECT_BANK); ++#ifdef CONFIG_KS8842_TIMB_DMA ++ /* resume DMA operations */ ++ ks8842_resume_dma(adapter); ++#endif + spin_unlock_irqrestore(&adapter->lock, flags); + } + +@@ -493,11 +730,17 @@ + irqreturn_t ret = IRQ_NONE; + + isr = ks8842_read16(adapter, 18, REG_ISR); +- dev_dbg(&adapter->pdev->dev, "%s - ISR: 0x%x\n", __func__, isr); ++ dev_dbg(adapter->dev, "%s - ISR: 0x%x\n", __func__, isr); + + if (isr) { +- /* disable IRQ */ +- ks8842_write16(adapter, 18, 0x00, REG_IER); ++#ifdef CONFIG_KS8842_TIMB_DMA ++ if (adapter->use_dma) ++ /* disable all but RX IRQ, since the FPGA relies on it*/ ++ ks8842_write16(adapter, 18, IRQ_RX, REG_IER); ++ else ++#endif ++ /* disable IRQ */ ++ ks8842_write16(adapter, 18, 0x00, REG_IER); + + /* schedule tasklet */ + tasklet_schedule(&adapter->tasklet); +@@ -506,23 +749,129 @@ + } + + iowrite16(entry_bank, adapter->hw_addr + REG_SELECT_BANK); +- ++#ifdef CONFIG_KS8842_TIMB_DMA ++ ks8842_resume_dma(adapter); ++#endif + return ret; + } + ++#ifdef CONFIG_KS8842_TIMB_DMA ++static int ks8842_dma_irq(u32 flag, void *data) ++{ ++ struct net_device *netdev = data; ++ struct ks8842_adapter *adapter = netdev_priv(netdev); ++ ++ if (flag & DMA_IRQ_ETH_RX) { ++ dev_dbg(adapter->dev, "RX DMA finished\n"); ++ ks8842_rx_frame_dma(netdev, adapter); ++ } ++ if (flag & DMA_IRQ_ETH_TX) { ++ struct ks8842_dma_ctl *ctl = &adapter->dma_tx; ++ dev_dbg(adapter->dev, "TX DMA finished\n"); ++ ++ netdev->stats.tx_packets++; ++ ctl->ongoing = 0; ++ ++ if (netif_queue_stopped(netdev)) ++ netif_wake_queue(netdev); ++ } ++ ++ return 0; ++} ++ ++static void ks8842_dealloc_dma_bufs(struct ks8842_adapter *adapter) ++{ ++ struct ks8842_dma_ctl *tx_ctl = &adapter->dma_tx; ++ struct ks8842_rx_dma_ctl *rx_ctl = &adapter->dma_rx; ++ ++ if (tx_ctl->ongoing) ++ timbdma_stop(DMA_IRQ_ETH_TX); ++ tx_ctl->ongoing = 0; ++ if (rx_ctl->skb) ++ timbdma_stop(DMA_IRQ_ETH_RX); ++ ++ timbdma_set_interruptcb(DMA_IRQ_ETH_RX | DMA_IRQ_ETH_TX, NULL, NULL); ++ ++ if (rx_ctl->desc) ++ timbdma_free_desc(rx_ctl->desc); ++ rx_ctl->desc = NULL; ++ if (tx_ctl->desc) ++ timbdma_free_desc(tx_ctl->desc); ++ tx_ctl->desc = NULL; ++ if (rx_ctl->addr) ++ dma_unmap_single(DMA_DEV(adapter), rx_ctl->addr, ++ DMA_BUFFER_SIZE, DMA_FROM_DEVICE); ++ rx_ctl->addr = 0; ++ if (tx_ctl->addr) ++ dma_unmap_single(DMA_DEV(adapter), tx_ctl->addr, ++ DMA_BUFFER_SIZE, DMA_TO_DEVICE); ++ tx_ctl->addr = 0; ++ dev_kfree_skb(rx_ctl->skb); ++ rx_ctl->skb = NULL; ++ kfree(tx_ctl->buf); ++ tx_ctl->buf = NULL; ++} ++#endif + + /* Netdevice operations */ + + static int ks8842_open(struct net_device *netdev) + { + struct ks8842_adapter *adapter = netdev_priv(netdev); ++#ifdef CONFIG_KS8842_TIMB_DMA ++ struct ks8842_dma_ctl *tx_ctl = &adapter->dma_tx; ++ struct ks8842_rx_dma_ctl *rx_ctl = &adapter->dma_rx; ++ int use_dma = 0; ++#endif + int err; + +- dev_dbg(&adapter->pdev->dev, "%s - entry\n", __func__); ++ dev_dbg(adapter->dev, "%s - entry\n", __func__); ++ ++#ifdef CONFIG_KS8842_TIMB_DMA ++ if (adapter->use_dma) { ++ /* allocate SG descriptor */ ++ tx_ctl->buf = kmalloc(DMA_BUFFER_SIZE, GFP_KERNEL); ++ if (!tx_ctl->buf) ++ goto no_dma; ++ tx_ctl->addr = dma_map_single(DMA_DEV(adapter), tx_ctl->buf, ++ DMA_BUFFER_SIZE, DMA_TO_DEVICE); ++ err = dma_mapping_error(DMA_DEV(adapter), tx_ctl->addr); ++ if (err) { ++ tx_ctl->addr = 0; ++ goto no_dma; ++ } ++ tx_ctl->desc = timbdma_alloc_desc(DMA_BUFFER_SIZE, 1); ++ if (!tx_ctl->desc) ++ goto no_dma; ++ ++ rx_ctl->desc = timbdma_alloc_desc(DMA_BUFFER_SIZE, 1); ++ if (!rx_ctl->desc) ++ goto no_dma; ++ ++ timbdma_set_interruptcb(DMA_IRQ_ETH_RX | DMA_IRQ_ETH_TX, ++ ks8842_dma_irq, (void *)netdev); ++ ++ /* start RX dma */ ++ err = __ks8842_start_new_rx_dma(netdev, adapter); ++ if (err) ++ goto no_dma; ++ ++ use_dma = 1; ++ } ++no_dma: ++ if (!use_dma) { ++ printk(KERN_WARNING DRV_NAME ++ ": Failed to initiate DMA, falling back to PIO\n"); ++ ks8842_dealloc_dma_bufs(adapter); ++ adapter->use_dma = 0; ++ } ++#endif + + /* reset the HW */ + ks8842_reset_hw(adapter); + ++ ks8842_write_mac_addr(adapter, netdev->dev_addr); ++ + ks8842_update_link_status(netdev, adapter); + + err = request_irq(adapter->irq, ks8842_irq, IRQF_SHARED, DRV_NAME, +@@ -536,11 +885,19 @@ + return 0; + } + ++ + static int ks8842_close(struct net_device *netdev) + { + struct ks8842_adapter *adapter = netdev_priv(netdev); + +- dev_dbg(&adapter->pdev->dev, "%s - entry\n", __func__); ++ dev_dbg(adapter->dev, "%s - entry\n", __func__); ++ ++ cancel_work_sync(&adapter->timeout_work); ++ ++#ifdef CONFIG_KS8842_TIMB_DMA ++ if (adapter->use_dma) ++ ks8842_dealloc_dma_bufs(adapter); ++#endif + + /* free the irq */ + free_irq(adapter->irq, adapter); +@@ -556,8 +913,20 @@ + int ret; + struct ks8842_adapter *adapter = netdev_priv(netdev); + +- dev_dbg(&adapter->pdev->dev, "%s: entry\n", __func__); ++ dev_dbg(adapter->dev, "%s: entry\n", __func__); + ++#ifdef CONFIG_KS8842_TIMB_DMA ++ if (adapter->use_dma) { ++ unsigned long flags; ++ ret = ks8842_tx_frame_dma(skb, netdev); ++ /* for now only allow one transfer at the time */ ++ spin_lock_irqsave(&adapter->lock, flags); ++ if (adapter->dma_tx.ongoing) ++ netif_stop_queue(netdev); ++ spin_unlock_irqrestore(&adapter->lock, flags); ++ return ret; ++ } ++#endif + ret = ks8842_tx_frame(skb, netdev); + + if (ks8842_tx_fifo_space(adapter) < netdev->mtu + 8) +@@ -569,44 +938,77 @@ + static int ks8842_set_mac(struct net_device *netdev, void *p) + { + struct ks8842_adapter *adapter = netdev_priv(netdev); +- unsigned long flags; + struct sockaddr *addr = p; + char *mac = (u8 *)addr->sa_data; +- int i; + +- dev_dbg(&adapter->pdev->dev, "%s: entry\n", __func__); ++ dev_dbg(adapter->dev, "%s: entry\n", __func__); + + if (!is_valid_ether_addr(addr->sa_data)) + return -EADDRNOTAVAIL; + + memcpy(netdev->dev_addr, mac, netdev->addr_len); + +- spin_lock_irqsave(&adapter->lock, flags); +- for (i = 0; i < ETH_ALEN; i++) { +- ks8842_write8(adapter, 2, mac[ETH_ALEN - i - 1], REG_MARL + i); +- ks8842_write8(adapter, 39, mac[ETH_ALEN - i - 1], +- REG_MACAR1 + i); +- } +- spin_unlock_irqrestore(&adapter->lock, flags); ++ ks8842_write_mac_addr(adapter, mac); + return 0; + } + +-static void ks8842_tx_timeout(struct net_device *netdev) ++static void ks8842_tx_timeout_work(struct work_struct *work) + { +- struct ks8842_adapter *adapter = netdev_priv(netdev); ++ struct ks8842_adapter *adapter = ++ container_of(work, struct ks8842_adapter, timeout_work); ++ struct net_device *netdev = adapter->netdev; + unsigned long flags; + +- dev_dbg(&adapter->pdev->dev, "%s: entry\n", __func__); ++ dev_dbg(adapter->dev, "%s: entry\n", __func__); + + spin_lock_irqsave(&adapter->lock, flags); ++#ifdef CONFIG_KS8842_TIMB_DMA ++ if (adapter->use_dma) { ++ struct ks8842_dma_ctl *tx_ctl = &adapter->dma_tx; ++ struct ks8842_rx_dma_ctl *rx_ctl = &adapter->dma_rx; ++ ++ if (tx_ctl->ongoing) ++ timbdma_stop(DMA_IRQ_ETH_TX); ++ tx_ctl->ongoing = 0; ++ ++ timbdma_stop(DMA_IRQ_ETH_RX); ++ ++ dma_unmap_single(DMA_DEV(adapter), rx_ctl->addr, ++ DMA_BUFFER_SIZE, DMA_FROM_DEVICE); ++ rx_ctl->addr = 0; ++ ++ dev_kfree_skb(rx_ctl->skb); ++ rx_ctl->skb = NULL; ++ } ++#endif ++ + /* disable interrupts */ + ks8842_write16(adapter, 18, 0, REG_IER); + ks8842_write16(adapter, 18, 0xFFFF, REG_ISR); ++ ++ netif_stop_queue(netdev); ++ + spin_unlock_irqrestore(&adapter->lock, flags); + + ks8842_reset_hw(adapter); + ++ ks8842_write_mac_addr(adapter, netdev->dev_addr); ++ + ks8842_update_link_status(netdev, adapter); ++ ++#ifdef CONFIG_KS8842_TIMB_DMA ++ if (adapter->use_dma) ++ __ks8842_start_new_rx_dma(netdev, adapter); ++#endif ++} ++ ++static void ks8842_tx_timeout(struct net_device *netdev) ++{ ++ struct ks8842_adapter *adapter = netdev_priv(netdev); ++ ++ dev_dbg(adapter->dev, "%s: entry\n", __func__); ++ ++ schedule_work(&adapter->timeout_work); + } + + static const struct net_device_ops ks8842_netdev_ops = { +@@ -641,6 +1043,8 @@ + SET_NETDEV_DEV(netdev, &pdev->dev); + + adapter = netdev_priv(netdev); ++ adapter->netdev = netdev; ++ INIT_WORK(&adapter->timeout_work, ks8842_tx_timeout_work); + adapter->hw_addr = ioremap(iomem->start, resource_size(iomem)); + if (!adapter->hw_addr) + goto err_ioremap; +@@ -651,8 +1055,10 @@ + goto err_get_irq; + } + +- adapter->pdev = pdev; +- ++ adapter->dev = &pdev->dev; ++#ifdef CONFIG_KS8842_TIMB_DMA ++ adapter->use_dma = 1; ++#endif + tasklet_init(&adapter->tasklet, ks8842_tasklet, (unsigned long)netdev); + spin_lock_init(&adapter->lock); + +@@ -660,6 +1066,8 @@ + netdev->ethtool_ops = &ks8842_ethtool_ops; + + ks8842_read_mac_addr(adapter, netdev->dev_addr); ++ if (!is_valid_ether_addr(netdev->dev_addr)) ++ random_ether_addr(netdev->dev_addr); + + id = ks8842_read16(adapter, 32, REG_SW_ID_AND_ENABLE); + +diff -uNr linux-2.6.31/drivers/net/Makefile linux-2.6.31.new/drivers/net/Makefile +--- linux-2.6.31/drivers/net/Makefile 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/net/Makefile 2009-10-23 11:17:22.000000000 -0700 +@@ -16,6 +16,7 @@ + obj-$(CONFIG_CHELSIO_T3) += cxgb3/ + obj-$(CONFIG_EHEA) += ehea/ + obj-$(CONFIG_CAN) += can/ ++obj-$(CONFIG_MOST) += most/ + obj-$(CONFIG_BONDING) += bonding/ + obj-$(CONFIG_ATL1) += atlx/ + obj-$(CONFIG_ATL2) += atlx/ +diff -uNr linux-2.6.31/drivers/net/most/Kconfig linux-2.6.31.new/drivers/net/most/Kconfig +--- linux-2.6.31/drivers/net/most/Kconfig 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/net/most/Kconfig 2009-10-23 11:17:22.000000000 -0700 +@@ -0,0 +1,14 @@ ++menu "MOST Device Drivers" ++ depends on MOST ++ ++config MOST_TIMB_MLB ++ tristate "The timberdale MOST block" ++ depends on MOST ++ depends on MFD_TIMBERDALE_DMA ++ depends on GENERIC_GPIO ++ depends on HAS_IOMEM ++ default N ++ ---help--- ++ Adds support for MOST on the timberdale FPGA. ++ ++endmenu +diff -uNr linux-2.6.31/drivers/net/most/Makefile linux-2.6.31.new/drivers/net/most/Makefile +--- linux-2.6.31/drivers/net/most/Makefile 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/net/most/Makefile 2009-10-23 11:17:22.000000000 -0700 +@@ -0,0 +1,6 @@ ++# ++# Makefile for the Linux Media Oriented Systems Transport drivers. ++# ++ ++obj-$(CONFIG_MOST_TIMB_MLB) += timbmlb.o ++ +diff -uNr linux-2.6.31/drivers/net/most/timbmlb.c linux-2.6.31.new/drivers/net/most/timbmlb.c +--- linux-2.6.31/drivers/net/most/timbmlb.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/net/most/timbmlb.c 2009-10-23 11:17:22.000000000 -0700 +@@ -0,0 +1,1087 @@ ++/* ++ * timbmlb.c Driver for the timberdale MLB block ++ * 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 ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#define DRIVER_NAME "timb-most" ++ ++#define MLB_REG_CFG 0x00 ++#define MLB_REG_CH_CTRL 0x04 ++#define MLB_REG_ISR 0x08 ++#define MLB_REG_IMR 0x0C ++#define MLB_REG_CH_CFG_1 0x10 ++#define MLB_REG_CH_CFG_2 0x14 ++#define MLB_REG_CH_CFG_3 0x18 ++#define MLB_REG_CH_CFG_4 0x1C ++#define MLB_REG_CH_CFG_5 0x20 ++#define MLB_REG_CH_CFG_6 0x24 ++#define MLB_REG_CH_CFG_7 0x28 ++#define MLB_REG_CTRL_RX 0x2C /* 8 bits */ ++#define MLB_REG_CTRL_TX MLB_REG_CTRL_RX ++#define MLB_REG_ASYNC_RX 0x30 /* 32 bits */ ++#define MLB_REG_ASYNC_TX MLB_REG_ASYNC_RX ++#define MLB_REG_SYNC_RX 0x34 /* 32 bits */ ++#define MLB_REG_SYNC_TX MLB_REG_SYNC_RX ++#define MLB_REG_FIFO_RST 0x38 ++ ++#define MLB_WR_CFG_CTRL_RX_EMPTY 0x20000 ++#define MLB_WR_CFG_ASYNC_RX_EMPTY 0x10000 ++#define MLB_CFG_SYNC_TX_EN 0x00200 ++#define MLB_CFG_SYNC_RX_EN 0x00100 ++#define MLB_CFG_ASYNC_RX_EN 0x00080 ++#define MLB_CFG_CTRL_RX_EN 0x00040 ++ ++#define MLB_CH_CTRL_ASYNC_TX_START 0x8000 ++#define MLB_CH_CTRL_ASYNC_RX_BREAK 0x4000 ++#define MLB_CH_CTRL_CTRL_TX_START 0x0800 ++#define MLB_CH_CTRL_CTRL_RX_BREAK 0x0400 ++ ++#define MLB_WR_I_SYNC_RX_EMPTY 0x80000 ++#define MLB_WR_I_SYNC_RX_ALMOST_FULL 0x40000 ++#define MLB_WR_I_SYNC_TX_FULL 0x20000 ++#define MLB_WR_I_SYNC_TX_ALMOST_EMPTY 0x10000 ++#define MLB_I_ASYNC_TX_READY 0x08000 ++#define MLB_I_ASYNC_TX_PROT_ERR 0x04000 ++#define MLB_I_ASYNC_TX_RX_BREAK 0x02000 ++#define MLB_I_ASYNC_TX_BUSY_BREAK 0x01000 ++#define MLB_I_ASYNC_RX_READY 0x00800 ++#define MLB_I_ASYNC_RX_PROT_ERR 0x00400 ++#define MLB_I_ASYNC_RX_CMD_BREAK 0x00200 ++#define MLB_I_SYNC_LOCK 0x00100 ++#define MLB_I_CTRL_TX_READY 0x00080 ++#define MLB_I_CTRL_TX_PROT_ERR 0x00040 ++#define MLB_I_CTRL_TX_RX_BREAK 0x00020 ++#define MLB_I_CTRL_TX_BUSY_BREAK 0x00010 ++#define MLB_I_CTRL_RX_READY 0x00008 ++#define MLB_I_CTRL_RX_PROT_ERR 0x00004 ++#define MLB_I_CTRL_RX_CMD_BREAK 0x00002 ++#define MLB_I_SYNC_RX_PROT_ERR 0x00001 ++ ++#define MLB_CH_CFG_NOT_ALLOCATED 0x0000 ++#define MLB_CH_CFG_SYNC_TX 0x0001 ++#define MLB_CH_CFG_SYNC_RX 0x0002 ++#define MLB_CH_CFG_ASYNC_TX 0x0003 ++#define MLB_CH_CFG_ASYNC_RX 0x0004 ++#define MLB_CH_CFG_CTRL_TX 0x0005 ++#define MLB_CH_CFG_CTRL_RX 0x0006 ++ ++#define MLB_FIFO_RST_CTRL_TX 0x010000 ++#define MLB_FIFO_RST_CTRL_RX 0x020000 ++#define MLB_FIFO_RST_ASYNC_TX 0x040000 ++#define MLB_FIFO_RST_ASYNC_RX 0x080000 ++#define MLB_FIFO_RST_SYNC_TX 0x100000 ++#define MLB_FIFO_RST_SYNC_RX 0x200000 ++#define MLB_FIFO_RST_MLB 0x400000 ++#define MLB_FIFO_RST_ALL (MLB_FIFO_RST_CTRL_TX | \ ++ MLB_FIFO_RST_CTRL_RX | \ ++ MLB_FIFO_RST_ASYNC_TX | \ ++ MLB_FIFO_RST_ASYNC_RX | \ ++ MLB_FIFO_RST_SYNC_TX | \ ++ MLB_FIFO_RST_SYNC_RX | \ ++ MLB_FIFO_RST_MLB) ++ ++#define ASYNC_SKB_SIZE 1024 ++#define SYNC_SKB_SIZE 32 ++ ++#define SYNC_MAX_DMA_SIZE 4096 ++ ++#define RX_CHAN 0 ++#define TX_CHAN 1 ++#define CHANNELS 2 ++ ++#define DMA_DEV(s) ((s->mdev->parent->parent) ? \ ++ s->mdev->parent->parent : s->mdev->parent) ++ ++struct timbmost { ++ void __iomem *membase; ++ struct most_dev *mdev; ++ int irq; ++ int reset_pin; ++ spinlock_t lock; /* mutual exclusion */ ++ ++ /* one queue per channel (type) */ ++ struct sk_buff_head ctl_q; ++ struct sk_buff_head async_q; ++ struct sk_buff_head sync_q; ++ ++ /* The SKB currently written/read into by the DMA engine ++ * only used for the synchronous channel ++ */ ++ struct sk_buff *sync_read_skb; ++ dma_addr_t sync_read_handle; ++ void *sync_read_desc; ++ struct sk_buff *sync_write_skb; ++ void *sync_write_desc; ++ int sync_write_next_map; ++ ++ /* channel numbers */ ++ u8 ctl_channels[CHANNELS]; ++ u8 sync_channels[CHANNELS]; ++ u8 async_channels[CHANNELS]; ++}; ++ ++static void timbmost_ctl_write_wake(struct timbmost *self); ++static void timbmost_async_write_wake(struct timbmost *self); ++ ++static void __timbmost_dump_regs(struct timbmost *self, const char *caption) ++{ ++ dev_dbg(self->mdev->parent, "%s\nMLB_CFG:\t%x\tCH_CTRL:\t%x\n", ++ caption, ++ ioread32(self->membase + MLB_REG_CFG), ++ ioread32(self->membase + MLB_REG_CH_CTRL)); ++ ++ dev_dbg(self->mdev->parent, "ISTAT:\t%x\tIMASK:\t%x\n", ++ ioread32(self->membase + MLB_REG_ISR), ++ ioread32(self->membase + MLB_REG_IMR)); ++ ++ dev_dbg(self->mdev->parent, "CH_CFG1:\t%x\tCH_CFG2:\t%x\n", ++ ioread32(self->membase + MLB_REG_CH_CFG_1), ++ ioread32(self->membase + MLB_REG_CH_CFG_2)); ++ ++ dev_dbg(self->mdev->parent, "CH_CFG3:\t%x\tCH_CFG4:\t%x\n", ++ ioread32(self->membase + MLB_REG_CH_CFG_3), ++ ioread32(self->membase + MLB_REG_CH_CFG_4)); ++ ++ dev_dbg(self->mdev->parent, "CH_CFG5:\t%x\tCH_CFG6:\t%x\n", ++ ioread32(self->membase + MLB_REG_CH_CFG_5), ++ ioread32(self->membase + MLB_REG_CH_CFG_6)); ++ ++ dev_dbg(self->mdev->parent, "CH_CFG7:\t%x\n", ++ ioread32(self->membase + MLB_REG_CH_CFG_7)); ++} ++ ++static void __timbmost_hw_reset(struct timbmost *self) ++{ ++ /* disable all interrupts */ ++ iowrite32(0, self->membase + MLB_REG_IMR); ++ iowrite32(0, self->membase + MLB_REG_ISR); ++ ++ /* disable RX and TX */ ++ iowrite32(0, self->membase + MLB_REG_CFG); ++ iowrite32(0, self->membase + MLB_REG_CH_CTRL); ++ ++ /* make sure the channels are not allocated */ ++ iowrite32(MLB_CH_CFG_NOT_ALLOCATED, self->membase + MLB_REG_CH_CFG_1); ++ iowrite32(MLB_CH_CFG_NOT_ALLOCATED, self->membase + MLB_REG_CH_CFG_2); ++ iowrite32(MLB_CH_CFG_NOT_ALLOCATED, self->membase + MLB_REG_CH_CFG_3); ++ iowrite32(MLB_CH_CFG_NOT_ALLOCATED, self->membase + MLB_REG_CH_CFG_4); ++ iowrite32(MLB_CH_CFG_NOT_ALLOCATED, self->membase + MLB_REG_CH_CFG_5); ++ iowrite32(MLB_CH_CFG_NOT_ALLOCATED, self->membase + MLB_REG_CH_CFG_6); ++ ++ /* reset */ ++ iowrite32(MLB_FIFO_RST_ALL, self->membase + MLB_REG_FIFO_RST); ++ ++ /* reset the INIC */ ++ gpio_direction_output(self->reset_pin, 0); ++ msleep(10); ++ gpio_set_value(self->reset_pin, 1); ++} ++ ++static void __timbmost_ctl_rx(struct timbmost *self) ++{ ++ u32 cfg; ++ do { ++ struct sk_buff *skb = ++ most_skb_alloc(CTL_FRAME_SIZE, GFP_ATOMIC); ++ if (!skb) ++ return; ++ ++ do { ++ u32 word = ioread32(self->membase + MLB_REG_CTRL_RX); ++ int i; ++ ++ for (i = 0; i < 4; i++) ++ *skb_put(skb, 1) = (word >> (i * 8)) & 0xff; ++ ++ cfg = ioread32(self->membase + MLB_REG_CFG); ++ } while ((skb->len < CTL_FRAME_SIZE) && ++ !(cfg & MLB_WR_CFG_CTRL_RX_EMPTY)); ++ ++ /* deliver SKB upstreams */ ++ skb->dev = (void *)self->mdev; ++ most_cb(skb)->channel_type = CHAN_CTL; ++ /* only one channel is supported... */ ++ most_cb(skb)->channel = self->ctl_channels[RX_CHAN]; ++ ++ most_recv_frame(skb); ++ } while (!(cfg & MLB_WR_CFG_CTRL_RX_EMPTY)); ++} ++ ++static void __timbmost_async_rx(struct timbmost *self) ++{ ++ /* TODO: The FIFO is 32bit not 8bit */ ++ u32 cfg; ++ ++ __timbmost_dump_regs(self, "Before read"); ++ ++ do { ++ struct sk_buff *skb = ++ most_skb_alloc(ASYNC_SKB_SIZE, GFP_ATOMIC); ++ if (!skb) ++ return; ++ ++ do { ++ *skb_put(skb, 1) = ++ ioread32(self->membase + MLB_REG_ASYNC_RX); ++ cfg = ioread32(self->membase + MLB_REG_CFG); ++ } while ((skb->len < ASYNC_SKB_SIZE) && ++ !(cfg & MLB_WR_CFG_ASYNC_RX_EMPTY)); ++ ++ /* deliver SKB upstreams */ ++ skb->dev = (void *)self->mdev; ++ most_cb(skb)->channel_type = CHAN_ASYNC; ++ /* only one channel is supported... */ ++ most_cb(skb)->channel = self->async_channels[RX_CHAN]; ++ ++ most_recv_frame(skb); ++ } while (!(cfg & MLB_WR_CFG_ASYNC_RX_EMPTY)); ++} ++ ++static void __timbmost_sync_read_wake(struct timbmost *self) ++{ ++ struct sk_buff *skb = self->sync_read_skb; ++ dma_addr_t map; ++ int err; ++ ++ if (skb) ++ return; ++ ++ skb = most_skb_alloc(SYNC_SKB_SIZE, GFP_ATOMIC); ++ if (!skb) ++ return; ++ ++ map = dma_map_single(DMA_DEV(self), skb->data, SYNC_SKB_SIZE, ++ DMA_FROM_DEVICE); ++ if (dma_mapping_error(DMA_DEV(self), map)) ++ goto map_failed; ++ ++ err = timbdma_prep_desc(self->sync_read_desc, map, SYNC_SKB_SIZE); ++ if (err) ++ goto prep_failed; ++ ++ dev_dbg(self->mdev->parent, "%s: will start RX: to: %x, size: %d\n", ++ __func__, (u32)map, SYNC_SKB_SIZE); ++ ++ err = timbdma_start(DMA_IRQ_MLB_RX, self->sync_read_desc, 0); ++ if (err) ++ goto start_failed; ++ ++ self->sync_read_skb = skb; ++ self->sync_read_handle = map; ++ return; ++start_failed: ++prep_failed: ++ dma_unmap_single(DMA_DEV(self), map, SYNC_SKB_SIZE, DMA_FROM_DEVICE); ++map_failed: ++ dev_kfree_skb(skb); ++} ++ ++static void __timbmost_sync_rx_done(struct timbmost *self) ++{ ++ struct sk_buff *skb = self->sync_read_skb; ++ int len; ++ ++ BUG_ON(!skb); ++ ++ /* unmap DMA */ ++ dma_unmap_single(DMA_DEV(self), self->sync_read_handle, SYNC_SKB_SIZE, ++ DMA_FROM_DEVICE); ++ ++ /* set the length */ ++ len = timbdma_stop(DMA_IRQ_MLB_RX); ++ skb_put(skb, len); ++ /* send the SKB upwards */ ++ skb->dev = (void *)self->mdev; ++ most_cb(skb)->channel_type = CHAN_SYNC; ++ /* only one channel is supported... */ ++ most_cb(skb)->channel = self->sync_channels[RX_CHAN]; ++ most_recv_frame(skb); ++ self->sync_read_skb = NULL; ++ ++ __timbmost_sync_read_wake(self); ++} ++ ++static void __timbmost_sync_write_wake(struct timbmost *self) ++{ ++ unsigned long flags; ++ int len; ++ dma_addr_t map; ++ struct sk_buff *skb = self->sync_write_skb; ++ u32 isr; ++ ++ dev_dbg(self->mdev->parent, "%s entry\n", __func__); ++ ++ if (!skb) { ++ /* check for next SKB */ ++ skb = skb_dequeue(&self->sync_q); ++ if (!skb) ++ return; ++ ++ if (skb_dma_map(DMA_DEV(self), skb, DMA_TO_DEVICE)) { ++ /* failed to dma map? */ ++ dev_kfree_skb(skb); ++ return; ++ } ++ /* next dma map to write is the first ... */ ++ self->sync_write_next_map = -1; ++ self->sync_write_skb = skb; ++ dev_dbg(self->mdev->parent, "%s: New skb: fragments: %d\n", ++ __func__, skb_shinfo(skb)->nr_frags); ++ } ++ ++ /* check if there is space in the FIFO */ ++ spin_lock_irqsave(&self->lock, flags); ++ isr = ioread32(self->membase + MLB_REG_ISR); ++ if (isr & MLB_WR_I_SYNC_TX_FULL) { ++ /* FIFO full enable, almost empty interrupt */ ++ u32 imr = ioread32(self->membase + MLB_REG_IMR); ++ imr |= MLB_WR_I_SYNC_TX_ALMOST_EMPTY; ++ iowrite32(imr, self->membase + MLB_REG_IMR); ++ } ++ spin_unlock_irqrestore(&self->lock, flags); ++ ++ /* exit if the FIFO is full, we will continue when the almost empty ++ * interrupt occurs ++ */ ++ if (isr & MLB_WR_I_SYNC_TX_FULL) ++ return; ++ ++ /* send next fragment */ ++ if (self->sync_write_next_map < 0) { ++ len = skb_headlen(skb); ++ map = skb_shinfo(skb)->dma_head; ++ } else { ++ len = skb_shinfo(skb)->frags[self->sync_write_next_map].size; ++ map = skb_shinfo(skb)->dma_maps[self->sync_write_next_map]; ++ } ++ self->sync_write_next_map++; ++ dev_dbg(self->mdev->parent, "%s: Will send %x, len: %d\n", ++ __func__, (uint32_t)map, len); ++ timbdma_prep_desc(self->sync_write_desc, map, len); ++ timbdma_start(DMA_IRQ_MLB_TX, self->sync_write_desc, 0); ++} ++ ++static void __timbmost_sync_tx_done(struct timbmost *self) ++{ ++ struct sk_buff *skb = self->sync_write_skb; ++ ++ /* TX done, free current SKB, and check for next */ ++ BUG_ON(!skb); ++ ++ /* check if this was the last DMA map */ ++ if (self->sync_write_next_map >= skb_shinfo(skb)->nr_frags) { ++ ++ /* it was the last... */ ++ skb_dma_unmap(DMA_DEV(self), skb, DMA_TO_DEVICE); ++ dev_kfree_skb(skb); ++ self->sync_write_skb = NULL; ++ } ++ ++ __timbmost_sync_write_wake(self); ++} ++ ++static void timbmost_sync_start_write(struct timbmost *self) ++{ ++ unsigned long flags; ++ struct sk_buff *skb; ++ ++ spin_lock_irqsave(&self->lock, flags); ++ skb = self->sync_write_skb; ++ spin_unlock_irqrestore(&self->lock, flags); ++ ++ /* transfer is ongoing */ ++ if (skb) ++ return; ++ ++ __timbmost_sync_write_wake(self); ++} ++ ++/* function called in interrupt context by the timberdale DMA engine ++ * when a transfer is finished ++ */ ++static int timbmost_dma_irq(u32 flag, void *devid) ++{ ++ struct timbmost *self = (struct timbmost *)devid; ++ ++ if (flag & DMA_IRQ_MLB_RX) ++ __timbmost_sync_rx_done(self); ++ ++ if (flag & DMA_IRQ_MLB_TX) ++ __timbmost_sync_tx_done(self); ++ ++ return 0; ++} ++ ++static irqreturn_t timbmost_irq(int irq, void *devid) ++{ ++ struct timbmost *self = (struct timbmost *)devid; ++ u32 isr, imr; ++ ++ isr = ioread32(self->membase + MLB_REG_ISR); ++ imr = ioread32(self->membase + MLB_REG_IMR); ++ ++ dev_dbg(self->mdev->parent, "%s: entry, isr: %x, imr: %x\n", __func__, ++ isr, imr); ++ ++ /* mask out only enabled interrupts */ ++ isr &= imr; ++ ++ /* ack */ ++ iowrite32(isr, self->membase + MLB_REG_ISR); ++ ++ if (isr & MLB_WR_I_SYNC_TX_ALMOST_EMPTY) { ++ /* disable the interrupt */ ++ imr &= ~MLB_WR_I_SYNC_TX_ALMOST_EMPTY; ++ iowrite32(imr, self->membase + MLB_REG_IMR); ++ __timbmost_sync_write_wake(self); ++ } ++ ++ if (isr & MLB_I_ASYNC_TX_READY) { ++ /* disable TX interrupts */ ++ imr &= ~(MLB_I_ASYNC_TX_READY | MLB_I_ASYNC_TX_PROT_ERR); ++ iowrite32(imr, self->membase + MLB_REG_IMR); ++ /* schedule to send next package */ ++ timbmost_async_write_wake(self); ++ } ++ ++ if (isr & MLB_I_ASYNC_RX_READY) ++ /* pass data upstreams */ ++ __timbmost_async_rx(self); ++ ++ if (isr & MLB_I_CTRL_TX_READY) { ++ /* disable TX interrupts */ ++ imr &= ~(MLB_I_CTRL_TX_READY | MLB_I_CTRL_TX_PROT_ERR); ++ iowrite32(imr, self->membase + MLB_REG_IMR); ++ /* schedule to send next package */ ++ timbmost_ctl_write_wake(self); ++ } ++ ++ if (isr & MLB_I_CTRL_RX_READY) ++ /* pass data upstreams */ ++ __timbmost_ctl_rx(self); ++ ++ if (isr) ++ return IRQ_HANDLED; ++ else ++ return IRQ_NONE; ++} ++ ++static int timbmost_open(struct most_dev *mdev) ++{ ++ struct timbmost *self = (struct timbmost *)mdev->driver_data; ++ int err; ++ ++ dev_dbg(mdev->parent, "%s\n", __func__); ++ ++ skb_queue_head_init(&self->ctl_q); ++ skb_queue_head_init(&self->sync_q); ++ skb_queue_head_init(&self->async_q); ++ ++ spin_lock_init(&self->lock); ++ ++ /* request the GPIO reset pin */ ++ err = gpio_request(self->reset_pin, DRIVER_NAME); ++ if (err) { ++ printk(KERN_ERR DRIVER_NAME ++ " Failed to request reset pin: %d, err: %d\n", ++ self->reset_pin, err); ++ return err; ++ } ++ ++ __timbmost_hw_reset(self); ++ ++ /* set DMA callback */ ++ timbdma_set_interruptcb(DMA_IRQ_MLB_RX | DMA_IRQ_MLB_TX, ++ timbmost_dma_irq, (void *)self); ++ ++ self->sync_read_desc = timbdma_alloc_desc(SYNC_MAX_DMA_SIZE, 1); ++ if (!self->sync_read_desc) { ++ err = -ENOMEM; ++ goto err_alloc_r_desc; ++ } ++ ++ self->sync_write_desc = timbdma_alloc_desc(SYNC_MAX_DMA_SIZE, 1); ++ if (!self->sync_write_desc) { ++ err = -ENOMEM; ++ goto err_alloc_w_desc; ++ } ++ ++ /* request IRQ */ ++ err = request_irq(self->irq, timbmost_irq, IRQF_SHARED, "timb-most", ++ self); ++ if (err) ++ goto err_req_irq; ++ ++ return 0; ++ ++err_req_irq: ++ timbdma_free_desc(self->sync_write_desc); ++err_alloc_w_desc: ++ timbdma_free_desc(self->sync_read_desc); ++err_alloc_r_desc: ++ gpio_free(self->reset_pin); ++ return err; ++} ++ ++static void timbmost_stop_sync_dma(struct timbmost *self) ++{ ++ if (self->sync_read_skb) { ++ timbdma_stop(DMA_IRQ_MLB_RX); ++ dma_unmap_single(DMA_DEV(self), self->sync_read_handle, ++ SYNC_SKB_SIZE, DMA_FROM_DEVICE); ++ kfree_skb(self->sync_read_skb); ++ self->sync_read_skb = NULL; ++ } ++ ++ if (self->sync_write_skb) { ++ timbdma_stop(DMA_IRQ_MLB_TX); ++ skb_dma_unmap(DMA_DEV(self), self->sync_write_skb, ++ DMA_TO_DEVICE); ++ kfree_skb(self->sync_write_skb); ++ self->sync_write_skb = NULL; ++ } ++} ++ ++static int timbmost_close(struct most_dev *mdev) ++{ ++ struct timbmost *self = (struct timbmost *)mdev->driver_data; ++ ++ dev_dbg(mdev->parent, "%s\n", __func__); ++ ++ /* free IRQ */ ++ free_irq(self->irq, self); ++ ++ __timbmost_hw_reset(self); ++ ++ /* free GPIO */ ++ gpio_free(self->reset_pin); ++ ++ /* empty all queues */ ++ skb_queue_purge(&self->ctl_q); ++ skb_queue_purge(&self->sync_q); ++ skb_queue_purge(&self->async_q); ++ ++ /* clear DMA callback */ ++ timbdma_set_interruptcb(DMA_IRQ_MLB_RX | DMA_IRQ_MLB_TX, NULL, NULL); ++ ++ return 0; ++} ++ ++static int __timbmost_conf_channel(struct timbmost *self, u8 channel, ++ u8 channel_mask) ++{ ++ int register_offset; ++ int shift; ++ u32 ch_cfg; ++ ++ /* only even channel numbers are allowed */ ++ if (channel % 2 || channel > 0x3e || channel == 0) { ++ printk(KERN_WARNING DRIVER_NAME": Invalid channel: %d\n", ++ channel); ++ return -EINVAL; ++ } ++ ++ channel = (channel / 2) - 1; ++ /* the channel conf is spread out over the 7 channel config registers ++ * each register configures 5 channels, each reg is 32bit ++ */ ++ register_offset = MLB_REG_CH_CFG_1 + (channel / 5) * 4; ++ ++ /* each register configures 5 channels, 3 bit per channel ++ * lowest bits configures highest channel ++ */ ++ shift = (4 - (channel % 5)) * 3; ++ ++ ch_cfg = ioread32(self->membase + register_offset); ++ ch_cfg &= ~(0x7 << shift); ++ ch_cfg |= (channel_mask & 0x7) << shift; ++ iowrite32(ch_cfg, self->membase + register_offset); ++ return 0; ++} ++ ++static int timbmost_conf_channel(struct most_dev *mdev, ++ enum most_chan_type type, u8 channel, u8 flags) ++{ ++ struct timbmost *self = (struct timbmost *)mdev->driver_data; ++ unsigned long irq_flags; ++ u32 imr, cfg; ++ int err = -EINVAL; ++ int chan_idx = (flags & MOST_CONF_FLAG_TX) ? TX_CHAN : RX_CHAN; ++ ++ dev_dbg(mdev->parent, "%s: channel: %d, flags: %x\n", ++ __func__, channel, flags); ++ ++ if (flags & MOST_CONF_FLAG_UP) { ++ switch (type) { ++ case CHAN_CTL: ++ spin_lock_irqsave(&self->lock, irq_flags); ++ /* we only support one channel at the time */ ++ if (self->ctl_channels[chan_idx]) ++ goto error; ++ ++ /* reset the FIFO */ ++ iowrite32((chan_idx == TX_CHAN) ? MLB_FIFO_RST_CTRL_TX : ++ MLB_FIFO_RST_CTRL_RX, ++ self->membase + MLB_REG_FIFO_RST); ++ ++ err = __timbmost_conf_channel(self, channel, ++ (chan_idx == TX_CHAN) ? MLB_CH_CFG_CTRL_TX : ++ MLB_CH_CFG_CTRL_RX); ++ if (err) ++ goto error; ++ ++ if (chan_idx == RX_CHAN) { ++ /* enable the receiver */ ++ cfg = ioread32(self->membase + MLB_REG_CFG); ++ cfg |= MLB_CFG_CTRL_RX_EN; ++ iowrite32(cfg, self->membase + MLB_REG_CFG); ++ ++ /* enable RX interrupts */ ++ imr = ioread32(self->membase + MLB_REG_IMR); ++ imr |= (MLB_I_CTRL_RX_READY | ++ MLB_I_CTRL_RX_PROT_ERR | ++ MLB_I_CTRL_RX_CMD_BREAK); ++ iowrite32(imr, self->membase + MLB_REG_IMR); ++ } ++ self->ctl_channels[chan_idx] = channel; ++ spin_unlock_irqrestore(&self->lock, irq_flags); ++ break; ++ case CHAN_SYNC: ++ spin_lock_irqsave(&self->lock, irq_flags); ++ /* we only support one channel at the time */ ++ if (self->sync_channels[chan_idx]) ++ goto error; ++ ++ /* reset the FIFO */ ++ iowrite32((chan_idx == TX_CHAN) ? MLB_FIFO_RST_SYNC_TX : ++ MLB_FIFO_RST_SYNC_RX, ++ self->membase + MLB_REG_FIFO_RST); ++ ++ err = __timbmost_conf_channel(self, channel, ++ (chan_idx == TX_CHAN) ? MLB_CH_CFG_SYNC_TX : ++ MLB_CH_CFG_SYNC_RX); ++ if (err) ++ goto error; ++ ++ if (chan_idx == RX_CHAN) { ++ /* enable the receiver */ ++ cfg = ioread32(self->membase + MLB_REG_CFG); ++ cfg |= MLB_CFG_SYNC_RX_EN; ++ iowrite32(cfg, self->membase + MLB_REG_CFG); ++ ++ /* enable prot error interrupts */ ++ imr = ioread32(self->membase + MLB_REG_IMR); ++ imr |= MLB_I_SYNC_RX_PROT_ERR; ++ iowrite32(imr, self->membase + MLB_REG_IMR); ++ /* start RX DMA */ ++ __timbmost_sync_read_wake(self); ++ } ++ self->sync_channels[chan_idx] = channel; ++ spin_unlock_irqrestore(&self->lock, irq_flags); ++ ++ break; ++ case CHAN_ASYNC: ++ spin_lock_irqsave(&self->lock, irq_flags); ++ /* we only support one channel at the time */ ++ if (self->async_channels[chan_idx]) ++ goto error; ++ /* reset the FIFO */ ++ iowrite32((chan_idx == TX_CHAN) ? ++ MLB_FIFO_RST_ASYNC_TX : MLB_FIFO_RST_ASYNC_RX, ++ self->membase + MLB_REG_FIFO_RST); ++ ++ err = __timbmost_conf_channel(self, channel, ++ (chan_idx == TX_CHAN) ? MLB_CH_CFG_ASYNC_TX : ++ MLB_CH_CFG_ASYNC_RX); ++ if (err) ++ goto error; ++ ++ if (chan_idx == RX_CHAN) { ++ /* enable the receiver */ ++ cfg = ioread32(self->membase + MLB_REG_CFG); ++ cfg |= MLB_CFG_ASYNC_RX_EN; ++ iowrite32(cfg, self->membase + MLB_REG_CFG); ++ ++ /* enable RX interrupts */ ++ imr = ioread32(self->membase + MLB_REG_IMR); ++ imr |= (MLB_I_ASYNC_RX_READY | ++ MLB_I_ASYNC_RX_PROT_ERR | ++ MLB_I_ASYNC_RX_CMD_BREAK); ++ iowrite32(imr, self->membase + MLB_REG_IMR); ++ } ++ self->async_channels[chan_idx] = channel; ++ spin_unlock_irqrestore(&self->lock, irq_flags); ++ break; ++ default: ++ printk(KERN_WARNING "timbmlb: Uknown channel type\n"); ++ return -EINVAL; ++ } ++ } else { ++ switch (type) { ++ case CHAN_CTL: ++ /* stop any ongoing transfer */ ++ spin_lock_irqsave(&self->lock, irq_flags); ++ if (self->ctl_channels[chan_idx] != channel) ++ goto error; ++ ++ imr = ioread32(self->membase + MLB_REG_IMR); ++ imr &= ~(MLB_I_CTRL_TX_READY | ++ MLB_I_CTRL_TX_PROT_ERR | ++ MLB_I_CTRL_TX_RX_BREAK | ++ MLB_I_CTRL_TX_BUSY_BREAK | ++ MLB_I_CTRL_RX_READY | ++ MLB_I_CTRL_RX_PROT_ERR | ++ MLB_I_CTRL_RX_CMD_BREAK); ++ iowrite32(imr, self->membase + MLB_REG_IMR); ++ ++ /* disable CTL RX */ ++ cfg = ioread32(self->membase + MLB_REG_CFG); ++ cfg &= ~MLB_CFG_CTRL_RX_EN; ++ iowrite32(cfg, self->membase + MLB_REG_CFG); ++ ++ err = __timbmost_conf_channel(self, channel, ++ MLB_CH_CFG_NOT_ALLOCATED); ++ spin_unlock_irqrestore(&self->lock, irq_flags); ++ skb_queue_purge(&self->ctl_q); ++ self->ctl_channels[chan_idx] = 0; ++ return err; ++ case CHAN_SYNC: ++ ++ /* stop any ongoing transfer */ ++ spin_lock_irqsave(&self->lock, irq_flags); ++ if (self->sync_channels[chan_idx] != channel) ++ goto error; ++ ++ /* stop DMA */ ++ timbmost_stop_sync_dma(self); ++ imr = ioread32(self->membase + MLB_REG_IMR); ++ imr &= ~MLB_I_SYNC_RX_PROT_ERR; ++ iowrite32(imr, self->membase + MLB_REG_IMR); ++ ++ /* disable SYNC TX/RX */ ++ cfg = ioread32(self->membase + MLB_REG_CFG); ++ cfg &= ~(MLB_CFG_SYNC_TX_EN | ++ MLB_CFG_SYNC_RX_EN); ++ iowrite32(cfg, self->membase + MLB_REG_CFG); ++ ++ err = __timbmost_conf_channel(self, channel, ++ MLB_CH_CFG_NOT_ALLOCATED); ++ spin_unlock_irqrestore(&self->lock, irq_flags); ++ skb_queue_purge(&self->sync_q); ++ self->sync_channels[chan_idx] = 0; ++ return err; ++ case CHAN_ASYNC: ++ /* stop any ongoing transfer */ ++ spin_lock_irqsave(&self->lock, irq_flags); ++ if (self->async_channels[chan_idx] != channel) ++ goto error; ++ imr = ioread32(self->membase + MLB_REG_IMR); ++ imr &= ~(MLB_I_ASYNC_TX_READY | ++ MLB_I_ASYNC_TX_PROT_ERR | ++ MLB_I_ASYNC_TX_RX_BREAK | ++ MLB_I_ASYNC_TX_BUSY_BREAK | ++ MLB_I_ASYNC_RX_READY | ++ MLB_I_ASYNC_RX_PROT_ERR | ++ MLB_I_ASYNC_RX_CMD_BREAK); ++ iowrite32(imr, self->membase + MLB_REG_IMR); ++ ++ /* disable CTL RX */ ++ cfg = ioread32(self->membase + MLB_REG_CFG); ++ cfg &= ~MLB_CFG_ASYNC_RX_EN; ++ iowrite32(cfg, self->membase + MLB_REG_CFG); ++ ++ err = __timbmost_conf_channel(self, channel, ++ MLB_CH_CFG_NOT_ALLOCATED); ++ spin_unlock_irqrestore(&self->lock, irq_flags); ++ skb_queue_purge(&self->async_q); ++ self->async_channels[chan_idx] = 0; ++ return err; ++ default: ++ return -EINVAL; ++ } ++ } ++ return 0; ++ ++error: ++ spin_unlock_irqrestore(&self->lock, irq_flags); ++ return err; ++} ++ ++static void timbmost_ctl_write_wake(struct timbmost *self) ++{ ++ unsigned long flags; ++ u32 imr; ++ u32 isr; ++ struct sk_buff *skb; ++ int i; ++ ++ dev_dbg(self->mdev->parent, "%s entry\n", __func__); ++ __timbmost_dump_regs(self, "Before write"); ++ ++ spin_lock_irqsave(&self->lock, flags); ++ imr = ioread32(self->membase + MLB_REG_IMR); ++ isr = ioread32(self->membase + MLB_REG_ISR); ++ spin_unlock_irqrestore(&self->lock, flags); ++ ++ /* check if the hardware is currently writing */ ++ if (imr & MLB_I_CTRL_TX_READY) ++ return; ++ ++ /* check if we have sync */ ++ if (!(isr & MLB_I_SYNC_LOCK)) ++ return; ++ ++ skb = skb_dequeue(&self->ctl_q); ++ if (!skb) ++ return; ++ ++ /* now write to the FIFO */ ++ for (i = 0; i < skb->len;) { ++ u32 word = 0; ++ int j; ++ ++ for (j = 0; j < 4 && i < skb->len; j++, i++) ++ word |= ((u8 *)skb->data)[i] << j * 8; ++ ++ iowrite32(word, self->membase + MLB_REG_CTRL_TX); ++ } ++ ++ /* data is in the FIFO, enable proper interrupts */ ++ spin_lock_irqsave(&self->lock, flags); ++ imr = ioread32(self->membase + MLB_REG_IMR) | MLB_I_CTRL_TX_READY | ++ MLB_I_CTRL_TX_PROT_ERR; ++ iowrite32(imr, self->membase + MLB_REG_IMR); ++ /* start TX */ ++ iowrite32(MLB_CH_CTRL_CTRL_TX_START, self->membase + MLB_REG_CH_CTRL); ++ spin_unlock_irqrestore(&self->lock, flags); ++ ++ kfree_skb(skb); ++} ++ ++static void timbmost_async_write_wake(struct timbmost *self) ++{ ++ unsigned long flags; ++ u32 imr; ++ u32 isr; ++ struct sk_buff *skb; ++ int i; ++ ++ spin_lock_irqsave(&self->lock, flags); ++ imr = ioread32(self->membase + MLB_REG_IMR); ++ isr = ioread32(self->membase + MLB_REG_ISR); ++ spin_unlock_irqrestore(&self->lock, flags); ++ ++ /* check if the hardware is currently writing */ ++ if (imr & MLB_I_ASYNC_TX_READY) ++ return; ++ ++ /* check if we have sync */ ++ if (!(isr & MLB_I_SYNC_LOCK)) ++ return; ++ ++ skb = skb_dequeue(&self->async_q); ++ if (!skb) ++ return; ++ ++ /* TODO: The FIFO is 32bit not 8bit */ ++ /* now write to the FIFO */ ++ for (i = 0; i < skb->len; i++) ++ iowrite32(skb->data[i], self->membase + MLB_REG_ASYNC_TX); ++ ++ /* data is in the FIFO, enable proper interrupts */ ++ spin_lock_irqsave(&self->lock, flags); ++ imr = ioread32(self->membase + MLB_REG_IMR) | MLB_I_ASYNC_TX_READY | ++ MLB_I_ASYNC_TX_PROT_ERR; ++ iowrite32(imr, self->membase + MLB_REG_IMR); ++ /* start TX */ ++ iowrite32(MLB_CH_CTRL_ASYNC_TX_START, self->membase + MLB_REG_CH_CTRL); ++ spin_unlock_irqrestore(&self->lock, flags); ++ ++ kfree_skb(skb); ++} ++ ++static int timbmost_send(struct sk_buff *skb) ++{ ++ struct most_dev *mdev = (struct most_dev *)skb->dev; ++ struct timbmost *self = (struct timbmost *)mdev->driver_data; ++ ++ dev_dbg(mdev->parent, "%s, type: %d\n", ++ __func__, most_cb(skb)->channel_type); ++ ++ switch (most_cb(skb)->channel_type) { ++ case CHAN_CTL: ++ skb_queue_tail(&self->ctl_q, skb); ++ timbmost_ctl_write_wake(self); ++ break; ++ case CHAN_ASYNC: ++ skb_queue_tail(&self->async_q, skb); ++ timbmost_async_write_wake(self); ++ break; ++ case CHAN_SYNC: ++ skb_queue_tail(&self->sync_q, skb); ++ timbmost_sync_start_write(self); ++ break; ++ default: ++ printk(KERN_WARNING "%s: Got unsupported channel type: %d\n", ++ __func__, most_cb(skb)->channel_type); ++ kfree_skb(skb); ++ break; ++ } ++ ++ return 0; ++} ++ ++static int timbmost_probe(struct platform_device *dev) ++{ ++ int err; ++ struct timbmost *self = NULL; ++ struct resource *iomem; ++ struct timbmlb_platform_data *pdata = dev->dev.platform_data; ++ ++ if (!pdata) { ++ printk(KERN_ERR DRIVER_NAME "No platform data supplied\n"); ++ err = -EINVAL; ++ goto err_mem; ++ } ++ ++ iomem = platform_get_resource(dev, IORESOURCE_MEM, 0); ++ if (!iomem) { ++ err = -EINVAL; ++ goto err_mem; ++ } ++ ++ self = kzalloc(sizeof(*self), GFP_KERNEL); ++ if (!self) { ++ err = -ENOMEM; ++ goto err_mem; ++ } ++ ++ self->mdev = most_alloc_dev(); ++ if (!self->mdev) { ++ err = -ENOMEM; ++ goto err_mem; ++ } ++ ++ self->mdev->owner = THIS_MODULE; ++ self->mdev->driver_data = self; ++ self->mdev->parent = &dev->dev; ++ self->mdev->open = timbmost_open; ++ self->mdev->close = timbmost_close; ++ self->mdev->send = timbmost_send; ++ self->mdev->conf_channel = timbmost_conf_channel; ++ ++ if (!request_mem_region(iomem->start, ++ resource_size(iomem), "timb-most")) { ++ err = -EBUSY; ++ goto err_mem; ++ } ++ ++ self->membase = ioremap(iomem->start, resource_size(iomem)); ++ if (!self->membase) { ++ printk(KERN_ERR "timbmost: Failed to remap I/O memory\n"); ++ err = -ENOMEM; ++ goto err_ioremap; ++ } ++ ++ self->reset_pin = pdata->reset_pin; ++ ++ /* find interrupt */ ++ self->irq = platform_get_irq(dev, 0); ++ if (self->irq < 0) { ++ err = self->irq; ++ goto err_get_irq; ++ } ++ ++ /* register to the MOST layer */ ++ err = most_register_dev(self->mdev); ++ if (err) ++ goto err_register; ++ ++ ++ platform_set_drvdata(dev, self); ++ ++ return 0; ++ ++err_get_irq: ++err_register: ++ iounmap(self->membase); ++err_ioremap: ++ release_mem_region(iomem->start, resource_size(iomem)); ++err_mem: ++ if (self) { ++ if (self->mdev) ++ most_free_dev(self->mdev); ++ ++ timbdma_free_desc(self->sync_read_desc); ++ timbdma_free_desc(self->sync_write_desc); ++ ++ kfree(self); ++ } ++ printk(KERN_ERR "timb-most: Failed to register: %d\n", err); ++ ++ return err; ++} ++ ++static int timbmost_remove(struct platform_device *dev) ++{ ++ struct timbmost *self = platform_get_drvdata(dev); ++ struct resource *iomem = platform_get_resource(dev, IORESOURCE_MEM, 0); ++ ++ most_unregister_dev(self->mdev); ++ iounmap(self->membase); ++ release_mem_region(iomem->start, resource_size(iomem)); ++ most_free_dev(self->mdev); ++ kfree(self); ++ return 0; ++} ++ ++static struct platform_driver timbmost_platform_driver = { ++ .driver = { ++ .name = DRIVER_NAME, ++ .owner = THIS_MODULE, ++ }, ++ .probe = timbmost_probe, ++ .remove = timbmost_remove, ++}; ++ ++/*--------------------------------------------------------------------------*/ ++ ++static int __init timbmost_init(void) ++{ ++ return platform_driver_register(&timbmost_platform_driver); ++} ++ ++static void __exit timbmost_exit(void) ++{ ++ platform_driver_unregister(&timbmost_platform_driver); ++} ++ ++module_init(timbmost_init); ++module_exit(timbmost_exit); ++ ++MODULE_DESCRIPTION("Timberdale MLB driver"); ++MODULE_AUTHOR("Mocean Laboratories "); ++MODULE_LICENSE("GPL v2"); ++MODULE_ALIAS("platform:timb-most"); ++ +diff -uNr linux-2.6.31/drivers/serial/Kconfig linux-2.6.31.new/drivers/serial/Kconfig +--- linux-2.6.31/drivers/serial/Kconfig 2009-10-23 11:18:08.000000000 -0700 ++++ linux-2.6.31.new/drivers/serial/Kconfig 2009-10-23 11:17:29.000000000 -0700 +@@ -855,7 +855,7 @@ + + config SERIAL_UARTLITE + tristate "Xilinx uartlite serial port support" +- depends on PPC32 || MICROBLAZE ++ depends on PPC32 || MICROBLAZE || MFD_TIMBERDALE + select SERIAL_CORE + help + Say Y here if you want to use the Xilinx uartlite serial controller. +diff -uNr linux-2.6.31/drivers/serial/timbuart.c linux-2.6.31.new/drivers/serial/timbuart.c +--- linux-2.6.31/drivers/serial/timbuart.c 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/serial/timbuart.c 2009-10-23 11:17:29.000000000 -0700 +@@ -31,6 +31,7 @@ + + struct timbuart_port { + struct uart_port port; ++ struct uart_driver uart_driver; + struct tasklet_struct tasklet; + int usedma; + u32 last_ier; +@@ -410,7 +411,7 @@ + .verify_port = timbuart_verify_port + }; + +-static struct uart_driver timbuart_driver = { ++static const __devinitconst struct uart_driver timbuart_driver_template = { + .owner = THIS_MODULE, + .driver_name = "timberdale_uart", + .dev_name = "ttyTU", +@@ -419,7 +420,7 @@ + .nr = 1 + }; + +-static int timbuart_probe(struct platform_device *dev) ++static int __devinit timbuart_probe(struct platform_device *dev) + { + int err; + struct timbuart_port *uart; +@@ -433,6 +434,8 @@ + goto err_mem; + } + ++ uart->uart_driver = timbuart_driver_template; ++ + uart->usedma = 0; + + uart->port.uartclk = 3250000 * 16; +@@ -461,11 +464,11 @@ + + tasklet_init(&uart->tasklet, timbuart_tasklet, (unsigned long)uart); + +- err = uart_register_driver(&timbuart_driver); ++ err = uart_register_driver(&uart->uart_driver); + if (err) + goto err_register; + +- err = uart_add_one_port(&timbuart_driver, &uart->port); ++ err = uart_add_one_port(&uart->uart_driver, &uart->port); + if (err) + goto err_add_port; + +@@ -474,7 +477,7 @@ + return 0; + + err_add_port: +- uart_unregister_driver(&timbuart_driver); ++ uart_unregister_driver(&uart->uart_driver); + err_register: + kfree(uart); + err_mem: +@@ -484,13 +487,13 @@ + return err; + } + +-static int timbuart_remove(struct platform_device *dev) ++static int __devexit 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); ++ uart_remove_one_port(&uart->uart_driver, &uart->port); ++ uart_unregister_driver(&uart->uart_driver); + kfree(uart); + + return 0; +diff -uNr linux-2.6.31/drivers/spi/Kconfig linux-2.6.31.new/drivers/spi/Kconfig +--- linux-2.6.31/drivers/spi/Kconfig 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/spi/Kconfig 2009-10-23 11:17:32.000000000 -0700 +@@ -218,8 +218,8 @@ + SPI driver for Toshiba TXx9 MIPS SoCs + + config SPI_XILINX +- tristate "Xilinx SPI controller" +- depends on (XILINX_VIRTEX || MICROBLAZE) && EXPERIMENTAL ++ tristate "Xilinx SPI controller common module" ++ depends on EXPERIMENTAL + select SPI_BITBANG + help + This exposes the SPI controller IP from the Xilinx EDK. +@@ -227,6 +227,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.31/drivers/spi/Makefile linux-2.6.31.new/drivers/spi/Makefile +--- linux-2.6.31/drivers/spi/Makefile 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/spi/Makefile 2009-10-23 11:17:32.000000000 -0700 +@@ -30,6 +30,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.31/drivers/spi/xilinx_spi.c linux-2.6.31.new/drivers/spi/xilinx_spi.c +--- linux-2.6.31/drivers/spi/xilinx_spi.c 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/drivers/spi/xilinx_spi.c 2009-10-23 11:17:32.000000000 -0700 +@@ -14,22 +14,35 @@ + #include + #include + #include +-#include +- +-#include +-#include +-#include + + #include + #include + #include + +-#define XILINX_SPI_NAME "xilinx_spi" ++#include "xilinx_spi.h" ++ ++struct xilinx_spi { ++ /* bitbang has to be first */ ++ struct spi_bitbang bitbang; ++ struct completion done; ++ struct resource mem; /* phys mem */ ++ void __iomem *regs; /* virt. address of the control registers */ ++ u32 irq; ++ 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 bits_per_word; ++ bool big_endian; /* The device could be accessed big or little ++ * endian ++ */ ++}; ++ + + /* 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 0x60 /* Control Register */ + + #define XSPI_CR_ENABLE 0x02 + #define XSPI_CR_MASTER_MODE 0x04 +@@ -40,8 +53,9 @@ + #define XSPI_CR_RXFIFO_RESET 0x40 + #define XSPI_CR_MANUAL_SSELECT 0x80 + #define XSPI_CR_TRANS_INHIBIT 0x100 ++#define XSPI_CR_LSB_FIRST 0x200 + +-#define XSPI_SR_OFFSET 0x67 /* 8-bit Status Register */ ++#define XSPI_SR_OFFSET 0x64 /* 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,8 +63,8 @@ + #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 0x68 /* Data Transmit Register */ ++#define XSPI_RXD_OFFSET 0x6C /* Data Receive Register */ + + #define XSPI_SSR_OFFSET 0x70 /* 32-bit Slave Select Register */ + +@@ -70,43 +84,72 @@ + #define XSPI_INTR_TX_UNDERRUN 0x08 /* TxFIFO was underrun */ + #define XSPI_INTR_RX_FULL 0x10 /* RxFIFO is full */ + #define XSPI_INTR_RX_OVERRUN 0x20 /* RxFIFO was overrun */ ++#define XSPI_INTR_TX_HALF_EMPTY 0x40 /* TxFIFO is half empty */ + + #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; ++/* to follow are some functions that does little of big endian read and ++ * write depending on the config of the device. ++ */ ++static inline void xspi_write8(struct xilinx_spi *xspi, u32 offs, u8 val) ++{ ++ iowrite8(val, xspi->regs + offs + ((xspi->big_endian) ? 3 : 0)); ++} + +- void __iomem *regs; /* virt. address of the control registers */ ++static inline void xspi_write16(struct xilinx_spi *xspi, u32 offs, u16 val) ++{ ++ if (xspi->big_endian) ++ iowrite16be(val, xspi->regs + offs + 2); ++ else ++ iowrite16(val, xspi->regs + offs); ++} + +- u32 irq; ++static inline void xspi_write32(struct xilinx_spi *xspi, u32 offs, u32 val) ++{ ++ if (xspi->big_endian) ++ iowrite32be(val, xspi->regs + offs); ++ else ++ iowrite32(val, xspi->regs + offs); ++} + +- u32 speed_hz; /* SCK has a fixed frequency of speed_hz Hz */ ++static inline u8 xspi_read8(struct xilinx_spi *xspi, u32 offs) ++{ ++ return ioread8(xspi->regs + offs + ((xspi->big_endian) ? 3 : 0)); ++} + +- 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 inline u16 xspi_read16(struct xilinx_spi *xspi, u32 offs) ++{ ++ if (xspi->big_endian) ++ return ioread16be(xspi->regs + offs + 2); ++ else ++ return ioread16(xspi->regs + offs); ++} ++ ++static inline u32 xspi_read32(struct xilinx_spi *xspi, u32 offs) ++{ ++ if (xspi->big_endian) ++ return ioread32be(xspi->regs + offs); ++ else ++ return ioread32(xspi->regs + offs); ++} + +-static void xspi_init_hw(void __iomem *regs_base) ++static void xspi_init_hw(struct xilinx_spi *xspi) + { + /* Reset the SPI device */ +- out_be32(regs_base + XIPIF_V123B_RESETR_OFFSET, +- XIPIF_V123B_RESET_MASK); ++ xspi_write32(xspi, XIPIF_V123B_RESETR_OFFSET, XIPIF_V123B_RESET_MASK); + /* Disable all the interrupts just in case */ +- out_be32(regs_base + XIPIF_V123B_IIER_OFFSET, 0); ++ xspi_write32(xspi, XIPIF_V123B_IIER_OFFSET, 0); + /* Enable the global IPIF interrupt */ +- out_be32(regs_base + XIPIF_V123B_DGIER_OFFSET, +- XIPIF_V123B_GINTR_ENABLE); ++ xspi_write32(xspi, XIPIF_V123B_DGIER_OFFSET, XIPIF_V123B_GINTR_ENABLE); + /* Deselect the slave on the SPI bus */ +- out_be32(regs_base + XSPI_SSR_OFFSET, 0xffff); ++ xspi_write32(xspi, 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, +- XSPI_CR_TRANS_INHIBIT | XSPI_CR_MANUAL_SSELECT +- | XSPI_CR_MASTER_MODE | XSPI_CR_ENABLE); ++ xspi_write16(xspi, XSPI_CR_OFFSET, ++ XSPI_CR_TRANS_INHIBIT | XSPI_CR_MANUAL_SSELECT | ++ XSPI_CR_MASTER_MODE | XSPI_CR_ENABLE | XSPI_CR_TXFIFO_RESET | ++ XSPI_CR_RXFIFO_RESET); + } + + static void xilinx_spi_chipselect(struct spi_device *spi, int is_on) +@@ -115,16 +158,16 @@ + + if (is_on == BITBANG_CS_INACTIVE) { + /* Deselect the slave on the SPI bus */ +- out_be32(xspi->regs + XSPI_SSR_OFFSET, 0xffff); ++ xspi_write32(xspi, 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 = xspi_read16(xspi, 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); ++ xspi_write16(xspi, 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,24 +175,27 @@ + */ + + /* Activate the chip select */ +- out_be32(xspi->regs + XSPI_SSR_OFFSET, ++ xspi_write32(xspi, XSPI_SSR_OFFSET, + ~(0x0001 << spi->chip_select)); + } + } + + /* 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 +- * supports just 8 bits per word, and SPI clock can't be changed in software. +- * Check for 8 bits per word. Chip select delay calculations could be ++ * supports 8 or 16 bits per word, which can not be changed in software. ++ * SPI clock can't be changed in software. ++ * Check for correct 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) ++ struct spi_transfer *t) + { + u8 bits_per_word; ++ struct xilinx_spi *xspi = spi_master_get_devdata(spi->master); + +- bits_per_word = (t) ? t->bits_per_word : spi->bits_per_word; +- if (bits_per_word != 8) { ++ bits_per_word = (t->bits_per_word) ? t->bits_per_word : ++ spi->bits_per_word; ++ if (bits_per_word != xspi->bits_per_word) { + dev_err(&spi->dev, "%s, unsupported bits_per_word=%d\n", + __func__, bits_per_word); + return -EINVAL; +@@ -160,34 +206,50 @@ + + static int xilinx_spi_setup(struct spi_device *spi) + { +- struct spi_bitbang *bitbang; +- struct xilinx_spi *xspi; +- int retval; +- +- xspi = spi_master_get_devdata(spi->master); +- bitbang = &xspi->bitbang; +- +- retval = xilinx_spi_setup_transfer(spi, NULL); +- if (retval < 0) +- return retval; +- ++ /* always return 0, we can not check the number of bits. ++ * There are cases when SPI setup is called before any driver is ++ * there, in that case the SPI core defaults to 8 bits, which we ++ * do not support in some cases. But if we return an error, the ++ * SPI device would not be registered and no driver can get hold of it ++ * When the driver is there, it will call SPI setup again with the ++ * correct number of bits per transfer. ++ * If a driver setups with the wrong bit number, it will fail when ++ * it tries to do a transfer ++ */ + return 0; + } + + static void xilinx_spi_fill_tx_fifo(struct xilinx_spi *xspi) + { + u8 sr; ++ u8 wsize; ++ if (xspi->bits_per_word == 8) ++ wsize = 1; ++ else if (xspi->bits_per_word == 16) ++ wsize = 2; ++ else ++ wsize = 4; + + /* Fill the Tx FIFO with as many bytes as possible */ +- sr = in_8(xspi->regs + XSPI_SR_OFFSET); +- while ((sr & XSPI_SR_TX_FULL_MASK) == 0 && xspi->remaining_bytes > 0) { ++ sr = xspi_read8(xspi, 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); +- } +- xspi->remaining_bytes--; +- sr = in_8(xspi->regs + XSPI_SR_OFFSET); ++ if (wsize == 1) ++ xspi_write8(xspi, XSPI_TXD_OFFSET, ++ *xspi->tx_ptr); ++ else if (wsize == 2) ++ xspi_write16(xspi, XSPI_TXD_OFFSET, ++ *(u16 *)(xspi->tx_ptr)); ++ else if (wsize == 4) ++ xspi_write32(xspi, XSPI_TXD_OFFSET, ++ *(u32 *)(xspi->tx_ptr)); ++ ++ xspi->tx_ptr += wsize; ++ } else ++ xspi_write8(xspi, XSPI_TXD_OFFSET, 0); ++ xspi->remaining_bytes -= wsize; ++ sr = xspi_read8(xspi, XSPI_SR_OFFSET); + } + } + +@@ -209,23 +271,22 @@ + /* Enable the transmit empty interrupt, which we use to determine + * progress on the transmission. + */ +- ipif_ier = in_be32(xspi->regs + XIPIF_V123B_IIER_OFFSET); +- out_be32(xspi->regs + XIPIF_V123B_IIER_OFFSET, ++ ipif_ier = xspi_read32(xspi, XIPIF_V123B_IIER_OFFSET); ++ xspi_write32(xspi, XIPIF_V123B_IIER_OFFSET, + 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 = xspi_read16(xspi, XSPI_CR_OFFSET) & ~XSPI_CR_TRANS_INHIBIT; ++ xspi_write16(xspi, XSPI_CR_OFFSET, cr); + + wait_for_completion(&xspi->done); + + /* Disable the transmit empty interrupt */ +- out_be32(xspi->regs + XIPIF_V123B_IIER_OFFSET, ipif_ier); ++ xspi_write32(xspi, XIPIF_V123B_IIER_OFFSET, ipif_ier); + + return t->len - xspi->remaining_bytes; + } + +- + /* 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 +@@ -237,32 +298,50 @@ + u32 ipif_isr; + + /* Get the IPIF interrupts, and clear them immediately */ +- ipif_isr = in_be32(xspi->regs + XIPIF_V123B_IISR_OFFSET); +- out_be32(xspi->regs + XIPIF_V123B_IISR_OFFSET, ipif_isr); ++ ipif_isr = xspi_read32(xspi, XIPIF_V123B_IISR_OFFSET); ++ xspi_write32(xspi, XIPIF_V123B_IISR_OFFSET, ipif_isr); + + if (ipif_isr & XSPI_INTR_TX_EMPTY) { /* Transmission completed */ + u16 cr; + u8 sr; ++ u8 rsize; ++ if (xspi->bits_per_word == 8) ++ rsize = 1; ++ else if (xspi->bits_per_word == 16) ++ rsize = 2; ++ else ++ rsize = 4; + + /* A transmit has just completed. Process received data and + * check for more data to transmit. Always inhibit the + * 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 | XSPI_CR_TRANS_INHIBIT); ++ cr = xspi_read16(xspi, XSPI_CR_OFFSET); ++ xspi_write16(xspi, 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 = xspi_read8(xspi, XSPI_SR_OFFSET); + while ((sr & XSPI_SR_RX_EMPTY_MASK) == 0) { +- u8 data; ++ u32 data; ++ if (rsize == 1) ++ data = xspi_read8(xspi, XSPI_RXD_OFFSET); ++ else if (rsize == 2) ++ data = xspi_read16(xspi, XSPI_RXD_OFFSET); ++ else ++ data = xspi_read32(xspi, XSPI_RXD_OFFSET); + +- data = in_8(xspi->regs + XSPI_RXD_OFFSET); + if (xspi->rx_ptr) { +- *xspi->rx_ptr++ = data; ++ if (rsize == 1) ++ *xspi->rx_ptr = data & 0xff; ++ else if (rsize == 2) ++ *(u16 *)(xspi->rx_ptr) = data & 0xffff; ++ else ++ *((u32 *)(xspi->rx_ptr)) = data; ++ xspi->rx_ptr += rsize; + } +- sr = in_8(xspi->regs + XSPI_SR_OFFSET); ++ ++ sr = xspi_read8(xspi, XSPI_SR_OFFSET); + } + + /* See if there is more data to send */ +@@ -271,7 +350,7 @@ + /* Start the transfer by not inhibiting the + * transmitter any longer + */ +- out_be16(xspi->regs + XSPI_CR_OFFSET, cr); ++ xspi_write16(xspi, XSPI_CR_OFFSET, cr); + } else { + /* No more data to send. + * Indicate the transfer is completed. +@@ -279,44 +358,21 @@ + complete(&xspi->done); + } + } +- + return IRQ_HANDLED; + } + +-static int __init xilinx_spi_of_probe(struct of_device *ofdev, +- const struct of_device_id *match) ++struct spi_master *xilinx_spi_init(struct device *dev, struct resource *mem, ++ u32 irq, s16 bus_num, u16 num_chipselect, u8 bits_per_word, ++ bool big_endian) + { + struct spi_master *master; + struct xilinx_spi *xspi; +- struct resource r_irq_struct; +- struct resource r_mem_struct; ++ int ret = 0; + +- struct resource *r_irq = &r_irq_struct; +- struct resource *r_mem = &r_mem_struct; +- int rc = 0; +- const u32 *prop; +- int len; ++ master = spi_alloc_master(dev, sizeof(struct xilinx_spi)); + +- /* 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; +- } ++ if (master == NULL) ++ return ERR_PTR(-ENOMEM); + + /* the spi->mode bits understood by this driver: */ + master->mode_bits = SPI_CPOL | SPI_CPHA; +@@ -329,128 +385,73 @@ + 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"); ++ if (!request_mem_region(mem->start, resource_size(mem), ++ XILINX_SPI_NAME)) { ++ ret = -ENXIO; + goto put_master; + } + +- xspi->regs = ioremap(r_mem->start, r_mem->end - r_mem->start + 1); ++ xspi->regs = ioremap(mem->start, resource_size(mem)); + if (xspi->regs == NULL) { +- rc = -ENOMEM; +- dev_warn(&ofdev->dev, "ioremap failure\n"); +- goto release_mem; ++ ret = -ENOMEM; ++ dev_warn(dev, "ioremap failure\n"); ++ goto map_failed; + } +- 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 unmap_io; +- } +- master->num_chipselect = *prop; ++ master->bus_num = bus_num; ++ master->num_chipselect = num_chipselect; ++ ++ xspi->mem = *mem; ++ xspi->irq = irq; ++ xspi->bits_per_word = bits_per_word; ++ xspi->big_endian = big_endian; + + /* SPI controller initializations */ +- xspi_init_hw(xspi->regs); ++ xspi_init_hw(xspi); + + /* 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); ++ ret = request_irq(xspi->irq, xilinx_spi_irq, 0, XILINX_SPI_NAME, xspi); ++ if (ret != 0) + goto unmap_io; +- } + +- rc = spi_bitbang_start(&xspi->bitbang); +- if (rc != 0) { +- dev_err(&ofdev->dev, "spi_bitbang_start FAILED\n"); ++ ret = spi_bitbang_start(&xspi->bitbang); ++ if (ret != 0) { ++ dev_err(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; ++ dev_info(dev, "at 0x%08X mapped to 0x%08X, irq=%d\n", ++ (u32)mem->start, (u32)xspi->regs, xspi->irq); ++ return master; + + free_irq: + free_irq(xspi->irq, xspi); + unmap_io: + iounmap(xspi->regs); +-release_mem: +- release_mem_region(r_mem->start, resource_size(r_mem)); ++map_failed: ++ release_mem_region(mem->start, resource_size(mem)); + put_master: + spi_master_put(master); +- return rc; ++ return ERR_PTR(ret); + } ++EXPORT_SYMBOL(xilinx_spi_init); + +-static int __devexit xilinx_spi_remove(struct of_device *ofdev) ++void xilinx_spi_deinit(struct spi_master *master) + { + struct xilinx_spi *xspi; +- struct spi_master *master; +- struct resource r_mem; + +- master = platform_get_drvdata(ofdev); + xspi = spi_master_get_devdata(master); + + spi_bitbang_stop(&xspi->bitbang); + free_irq(xspi->irq, xspi); + iounmap(xspi->regs); +- if (!of_address_to_resource(ofdev->node, 0, &r_mem)) +- release_mem_region(r_mem.start, resource_size(&r_mem)); +- 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); ++ release_mem_region(xspi->mem.start, resource_size(&xspi->mem)); ++ spi_master_put(xspi->bitbang.master); + } +-module_init(xilinx_spi_init); ++EXPORT_SYMBOL(xilinx_spi_deinit); + +-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. "); + MODULE_DESCRIPTION("Xilinx SPI driver"); + MODULE_LICENSE("GPL"); ++ +diff -uNr linux-2.6.31/drivers/spi/xilinx_spi.h linux-2.6.31.new/drivers/spi/xilinx_spi.h +--- linux-2.6.31/drivers/spi/xilinx_spi.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/spi/xilinx_spi.h 2009-10-23 11:17:32.000000000 -0700 +@@ -0,0 +1,33 @@ ++/* ++ * xilinx_spi.h ++ * 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. ++ */ ++ ++#ifndef _XILINX_SPI_H_ ++#define _XILINX_SPI_H_ 1 ++ ++#include ++#include ++#include ++ ++#define XILINX_SPI_NAME "xilinx_spi" ++ ++struct spi_master *xilinx_spi_init(struct device *dev, struct resource *mem, ++ u32 irq, s16 bus_num, u16 num_chipselect, u8 bits_per_word, ++ bool big_endian); ++ ++void xilinx_spi_deinit(struct spi_master *master); ++#endif +diff -uNr linux-2.6.31/drivers/spi/xilinx_spi_of.c linux-2.6.31.new/drivers/spi/xilinx_spi_of.c +--- linux-2.6.31/drivers/spi/xilinx_spi_of.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/spi/xilinx_spi_of.c 2009-10-23 11:17:32.000000000 -0700 +@@ -0,0 +1,120 @@ ++/* ++ * xilinx_spi_of.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 ++#include ++#include ++#include ++#include ++ ++#include ++#include ++#include ++ ++#include ++#include ++ ++#include "xilinx_spi.h" ++ ++ ++static int __init xilinx_spi_of_probe(struct of_device *ofdev, ++ const struct of_device_id *match) ++{ ++ struct resource r_irq_struct; ++ struct resource r_mem_struct; ++ struct spi_master *master; ++ ++ struct resource *r_irq = &r_irq_struct; ++ struct resource *r_mem = &r_mem_struct; ++ int rc = 0; ++ const u32 *prop; ++ int len; ++ ++ rc = of_address_to_resource(ofdev->node, 0, r_mem); ++ if (rc) { ++ dev_warn(&ofdev->dev, "invalid address\n"); ++ return rc; ++ } ++ ++ rc = of_irq_to_resource(ofdev->node, 0, r_irq); ++ if (rc == NO_IRQ) { ++ dev_warn(&ofdev->dev, "no IRQ found\n"); ++ return -ENODEV; ++ } ++ ++ /* 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"); ++ return -EINVAL; ++ } ++ master = xilinx_spi_init(&ofdev->dev, r_mem, r_irq->start, -1, *prop, 8, ++ true); ++ if (IS_ERR(master)) ++ return PTR_ERR(master); ++ ++ dev_set_drvdata(&ofdev->dev, master); ++ ++ /* Add any subnodes on the SPI bus */ ++ of_register_spi_devices(master, ofdev->node); ++ ++ return 0; ++} ++ ++static int __devexit xilinx_spi_remove(struct of_device *ofdev) ++{ ++ xilinx_spi_deinit(dev_get_drvdata(&ofdev->dev)); ++ dev_set_drvdata(&ofdev->dev, 0); ++ return 0; ++} ++ ++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_of_init(void) ++{ ++ return of_register_platform_driver(&xilinx_spi_of_driver); ++} ++module_init(xilinx_spi_of_init); ++ ++static void __exit xilinx_spi_of_exit(void) ++{ ++ of_unregister_platform_driver(&xilinx_spi_of_driver); ++} ++module_exit(xilinx_spi_of_exit); ++MODULE_AUTHOR("MontaVista Software, Inc. "); ++MODULE_DESCRIPTION("Xilinx SPI driver"); ++MODULE_LICENSE("GPL"); ++ +diff -uNr linux-2.6.31/drivers/spi/xilinx_spi_pltfm.c linux-2.6.31.new/drivers/spi/xilinx_spi_pltfm.c +--- linux-2.6.31/drivers/spi/xilinx_spi_pltfm.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/drivers/spi/xilinx_spi_pltfm.c 2009-10-23 11:17:32.000000000 -0700 +@@ -0,0 +1,104 @@ ++/* ++ * 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 ++#include ++#include ++#include ++#include ++ ++#include ++#include ++#include ++ ++#include "xilinx_spi.h" ++ ++static int __devinit xilinx_spi_probe(struct platform_device *dev) ++{ ++ struct xspi_platform_data *pdata; ++ struct resource *r; ++ int irq; ++ struct spi_master *master; ++ u8 i; ++ ++ pdata = dev->dev.platform_data; ++ if (pdata == NULL) ++ return -ENODEV; ++ ++ r = platform_get_resource(dev, IORESOURCE_MEM, 0); ++ if (r == NULL) ++ return -ENODEV; ++ ++ irq = platform_get_irq(dev, 0); ++ if (irq < 0) ++ return -ENXIO; ++ ++ master = xilinx_spi_init(&dev->dev, r, irq, dev->id, ++ pdata->num_chipselect, pdata->bits_per_word, false); ++ if (IS_ERR(master)) ++ return PTR_ERR(master); ++ ++ for (i = 0; i < pdata->num_devices; i++) ++ spi_new_device(master, pdata->devices + i); ++ ++ platform_set_drvdata(dev, master); ++ return 0; ++} ++ ++static int __devexit xilinx_spi_remove(struct platform_device *dev) ++{ ++ xilinx_spi_deinit(platform_get_drvdata(dev)); ++ platform_set_drvdata(dev, 0); ++ ++ 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_pltfm_init(void) ++{ ++ return platform_driver_register(&xilinx_spi_driver); ++} ++module_init(xilinx_spi_pltfm_init); ++ ++static void __exit xilinx_spi_pltfm_exit(void) ++{ ++ platform_driver_unregister(&xilinx_spi_driver); ++} ++module_exit(xilinx_spi_pltfm_exit); ++ ++MODULE_AUTHOR("Mocean Laboratories "); ++MODULE_DESCRIPTION("Xilinx SPI platform driver"); ++MODULE_LICENSE("GPL v2"); ++ +diff -uNr linux-2.6.31/include/linux/can/platform/ascb.h linux-2.6.31.new/include/linux/can/platform/ascb.h +--- linux-2.6.31/include/linux/can/platform/ascb.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/linux/can/platform/ascb.h 2009-10-23 11:16:56.000000000 -0700 +@@ -0,0 +1,8 @@ ++#ifndef _CAN_PLATFORM_ASCB_H_ ++#define _CAN_PLATFORM_ASCB_H_ ++ ++struct ascb_platform_data { ++ int gpio_pin; ++}; ++ ++#endif +diff -uNr linux-2.6.31/include/linux/i2c-xiic.h linux-2.6.31.new/include/linux/i2c-xiic.h +--- linux-2.6.31/include/linux/i2c-xiic.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/linux/i2c-xiic.h 2009-10-23 11:16:56.000000000 -0700 +@@ -0,0 +1,31 @@ ++/* ++ * i2c-xiic.h ++ * 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 IIC ++ */ ++ ++#ifndef _LINUX_I2C_XIIC_H ++#define _LINUX_I2C_XIIC_H ++ ++struct xiic_i2c_platform_data { ++ u8 num_devices; /* number of devices in the devices list */ ++ struct i2c_board_info const *devices; /* devices connected to the bus */ ++}; ++ ++#endif /* _LINUX_I2C_XIIC_H */ +diff -uNr linux-2.6.31/include/linux/mfd/timbdma.h linux-2.6.31.new/include/linux/mfd/timbdma.h +--- linux-2.6.31/include/linux/mfd/timbdma.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/linux/mfd/timbdma.h 2009-10-23 11:16:56.000000000 -0700 +@@ -0,0 +1,58 @@ ++/* ++ * 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 ++ ++ ++#define DMA_IRQ_UART_RX 0x001 ++#define DMA_IRQ_UART_TX 0x002 ++#define DMA_IRQ_MLB_RX 0x004 ++#define DMA_IRQ_MLB_TX 0x008 ++#define DMA_IRQ_VIDEO_RX 0x010 ++#define DMA_IRQ_VIDEO_DROP 0x020 ++#define DMA_IRQ_SDHCI_RX 0x040 ++#define DMA_IRQ_SDHCI_TX 0x080 ++#define DMA_IRQ_ETH_RX 0x100 ++#define DMA_IRQ_ETH_TX 0x200 ++#define DMA_IRQS 10 ++ ++ ++typedef int (*timbdma_interruptcb)(u32 flag, void *data); ++ ++ ++int timbdma_start(u32 flag, void *desc, int bytes_per_row); ++ ++int timbdma_stop(u32 flags); ++ ++void timbdma_set_interruptcb(u32 flags, timbdma_interruptcb icb, void *data); ++ ++void *timbdma_alloc_desc(u32 size, u16 alignment); ++ ++void timbdma_free_desc(void *desc); ++ ++int timbdma_prep_desc(void *desc, dma_addr_t buf, u32 size); ++ ++#endif /* _TIMBDMA_H */ ++ +diff -uNr linux-2.6.31/include/linux/most/timbmlb.h linux-2.6.31.new/include/linux/most/timbmlb.h +--- linux-2.6.31/include/linux/most/timbmlb.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/linux/most/timbmlb.h 2009-10-23 11:16:56.000000000 -0700 +@@ -0,0 +1,9 @@ ++#ifndef __LINUX_MOST_TIMBMLB_H ++#define __LINUX_MOST_TIMBMLB_H ++ ++/* Timberdale MLB IP */ ++struct timbmlb_platform_data { ++ int reset_pin; /* pin used for reset of the INIC */ ++}; ++ ++#endif +diff -uNr linux-2.6.31/include/linux/socket.h linux-2.6.31.new/include/linux/socket.h +--- linux-2.6.31/include/linux/socket.h 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/include/linux/socket.h 2009-10-23 11:16:56.000000000 -0700 +@@ -195,7 +195,8 @@ + #define AF_ISDN 34 /* mISDN sockets */ + #define AF_PHONET 35 /* Phonet sockets */ + #define AF_IEEE802154 36 /* IEEE802154 sockets */ +-#define AF_MAX 37 /* For now.. */ ++#define AF_MOST 37 /* Media Oriented Systems Transport */ ++#define AF_MAX 38 /* For now.. */ + + /* Protocol families, same as address families. */ + #define PF_UNSPEC AF_UNSPEC +@@ -235,6 +236,7 @@ + #define PF_ISDN AF_ISDN + #define PF_PHONET AF_PHONET + #define PF_IEEE802154 AF_IEEE802154 ++#define PF_MOST AF_MOST + #define PF_MAX AF_MAX + + /* Maximum queue length specifiable by listen. */ +diff -uNr linux-2.6.31/include/linux/spi/mc33880.h linux-2.6.31.new/include/linux/spi/mc33880.h +--- linux-2.6.31/include/linux/spi/mc33880.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/linux/spi/mc33880.h 2009-10-23 11:16:56.000000000 -0700 +@@ -0,0 +1,10 @@ ++#ifndef LINUX_SPI_MC33880_H ++#define LINUX_SPI_MC33880_H ++ ++struct mc33880_platform_data { ++ /* number assigned to the first GPIO */ ++ unsigned base; ++}; ++ ++#endif ++ +diff -uNr linux-2.6.31/include/linux/spi/xilinx_spi.h linux-2.6.31.new/include/linux/spi/xilinx_spi.h +--- linux-2.6.31/include/linux/spi/xilinx_spi.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/linux/spi/xilinx_spi.h 2009-10-23 11:16:56.000000000 -0700 +@@ -0,0 +1,19 @@ ++#ifndef __LINUX_SPI_XILINX_SPI_H ++#define __LINUX_SPI_XILINX_SPI_H ++ ++/** ++ * struct xspi_platform_data - Platform data of the Xilinx SPI driver ++ * @num_chipselect: Number of chip select by the IP ++ * @bits_per_word: Number of bits per word. 8/16/32, Note that the DS464 ++ * only support 8bit SPI. ++ * @devices: Devices to add when the driver is probed. ++ * @num_devices: Number of devices in the devices array. ++ */ ++struct xspi_platform_data { ++ u16 num_chipselect; ++ u8 bits_per_word; ++ struct spi_board_info *devices; ++ u8 num_devices; ++}; ++ ++#endif /* __LINUX_SPI_XILINX_SPI_H */ +diff -uNr linux-2.6.31/include/linux/timb_gpio.h linux-2.6.31.new/include/linux/timb_gpio.h +--- linux-2.6.31/include/linux/timb_gpio.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/linux/timb_gpio.h 2009-10-23 11:16:56.000000000 -0700 +@@ -0,0 +1,28 @@ ++/* ++ * timb_gpio.h timberdale FPGA GPIO driver, platform data definition ++ * 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. ++ */ ++ ++#ifndef _LINUX_TIMB_GPIO_H ++#define _LINUX_TIMB_GPIO_H ++ ++struct timbgpio_platform_data { ++ int gpio_base; ++ int nr_pins; ++ int irq_base; ++}; ++ ++#endif +diff -uNr linux-2.6.31/include/media/timb_radio.h linux-2.6.31.new/include/media/timb_radio.h +--- linux-2.6.31/include/media/timb_radio.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/media/timb_radio.h 2009-10-23 11:16:55.000000000 -0700 +@@ -0,0 +1,31 @@ ++/* ++ * timb_radio.h Platform struct for the Timberdale radio 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. ++ */ ++ ++#ifndef _TIMB_RADIO_ ++#define _TIMB_RADIO_ 1 ++ ++#include ++ ++struct timb_radio_platform_data { ++ int i2c_adapter; /* I2C adapter where the tuner and dsp are attached */ ++ char tuner[32]; ++ char dsp[32]; ++}; ++ ++#endif ++ +diff -uNr linux-2.6.31/include/media/timb_video.h linux-2.6.31.new/include/media/timb_video.h +--- linux-2.6.31/include/media/timb_video.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/media/timb_video.h 2009-10-23 11:16:55.000000000 -0700 +@@ -0,0 +1,30 @@ ++/* ++ * timb_video.h Platform struct for the Timberdale video 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. ++ */ ++ ++#ifndef _TIMB_VIDEO_ ++#define _TIMB_VIDEO_ 1 ++ ++#include ++ ++struct timb_video_platform_data { ++ int i2c_adapter; /* The I2C adapter where the encoder is attached */ ++ char encoder[32]; ++}; ++ ++#endif ++ +diff -uNr linux-2.6.31/include/media/v4l2-chip-ident.h linux-2.6.31.new/include/media/v4l2-chip-ident.h +--- linux-2.6.31/include/media/v4l2-chip-ident.h 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/include/media/v4l2-chip-ident.h 2009-10-23 11:16:55.000000000 -0700 +@@ -129,12 +129,18 @@ + V4L2_IDENT_SAA6752HS = 6752, + V4L2_IDENT_SAA6752HS_AC3 = 6753, + ++ /* modules tef6862: just ident 6862 */ ++ V4L2_IDENT_TEF6862 = 6862, ++ + /* module adv7170: just ident 7170 */ + V4L2_IDENT_ADV7170 = 7170, + + /* module adv7175: just ident 7175 */ + V4L2_IDENT_ADV7175 = 7175, + ++ /* module adv7180: just ident 7180 */ ++ V4L2_IDENT_ADV7180 = 7180, ++ + /* module saa7185: just ident 7185 */ + V4L2_IDENT_SAA7185 = 7185, + +@@ -147,6 +153,9 @@ + /* module adv7343: just ident 7343 */ + V4L2_IDENT_ADV7343 = 7343, + ++ /* module saa7706h: just ident 7706 */ ++ V4L2_IDENT_SAA7706H = 7706, ++ + /* module wm8739: just ident 8739 */ + V4L2_IDENT_WM8739 = 8739, + +diff -uNr linux-2.6.31/include/net/most/async.h linux-2.6.31.new/include/net/most/async.h +--- linux-2.6.31/include/net/most/async.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/net/most/async.h 2009-10-23 11:16:55.000000000 -0700 +@@ -0,0 +1,12 @@ ++#ifndef __ASYNC_H ++#define __ASYNC_H ++ ++struct sockaddr_mostasync { ++ sa_family_t most_family; ++ unsigned short most_dev; ++ unsigned char rx_channel; ++ unsigned char tx_channel; ++}; ++ ++#endif ++ +diff -uNr linux-2.6.31/include/net/most/ctl.h linux-2.6.31.new/include/net/most/ctl.h +--- linux-2.6.31/include/net/most/ctl.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/net/most/ctl.h 2009-10-23 11:16:55.000000000 -0700 +@@ -0,0 +1,12 @@ ++#ifndef __CTL_H ++#define __CTL_H ++ ++struct sockaddr_mostctl { ++ sa_family_t most_family; ++ unsigned short most_dev; ++ unsigned char rx_channel; ++ unsigned char tx_channel; ++}; ++ ++#endif ++ +diff -uNr linux-2.6.31/include/net/most/dev.h linux-2.6.31.new/include/net/most/dev.h +--- linux-2.6.31/include/net/most/dev.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/net/most/dev.h 2009-10-23 11:16:55.000000000 -0700 +@@ -0,0 +1,27 @@ ++#ifndef __DEV_H ++#define __DEV_H ++ ++struct sockaddr_mostdev { ++ sa_family_t most_family; ++ unsigned short most_dev; ++}; ++ ++ ++/* MOST Dev ioctl defines */ ++#define MOSTDEVUP _IOW('M', 201, int) ++#define MOSTDEVDOWN _IOW('M', 202, int) ++ ++#define MOSTGETDEVLIST _IOR('M', 210, int) ++ ++struct most_dev_req { ++ uint16_t dev_id; ++}; ++ ++struct most_dev_list_req { ++ uint16_t dev_num; ++ struct most_dev_req dev_req[0]; ++}; ++ ++ ++#endif ++ +diff -uNr linux-2.6.31/include/net/most/most_core.h linux-2.6.31.new/include/net/most/most_core.h +--- linux-2.6.31/include/net/most/most_core.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/net/most/most_core.h 2009-10-23 11:16:55.000000000 -0700 +@@ -0,0 +1,133 @@ ++#ifndef __MOST_CORE_H ++#define __MOST_CORE_H ++ ++#include ++ ++enum most_chan_type { ++ CHAN_CTL = 0, ++ CHAN_SYNC, ++ CHAN_ASYNC, ++ CHAN_DEV ++}; ++ ++#define MOST_CONF_FLAG_UP 0x01 ++#define MOST_CONF_FLAG_TX 0x02 ++ ++enum most_dev_state { ++ MOST_DEV_DOWN = 0, ++ MOST_DEV_UP ++}; ++ ++struct most_dev { ++ ++ struct list_head list; ++ atomic_t refcnt; ++ ++ char name[8]; ++ ++ __u16 id; ++ enum most_dev_state state; ++ ++ struct module *owner; ++ ++ struct tasklet_struct rx_task; ++ struct tasklet_struct tx_task; ++ ++ struct sk_buff_head rx_q; ++ struct sk_buff_head ctl_q; ++ struct sk_buff_head async_q; ++ struct sk_buff_head sync_q; ++ ++ /* set by the driver */ ++ ++ void *driver_data; ++ struct device *parent; ++ ++ int (*open)(struct most_dev *mdev); ++ int (*close)(struct most_dev *mdev); ++ int (*conf_channel)(struct most_dev *mdev, enum most_chan_type type, ++ u8 channel, u8 flags); ++ int (*send)(struct sk_buff *skb); ++ int (*can_send)(struct sk_buff *skb); ++}; ++ ++#define most_dbg(...) printk(__VA_ARGS__) ++ ++static inline struct most_dev *most_dev_hold(struct most_dev *d) ++{ ++ if (try_module_get(d->owner)) ++ return d; ++ return NULL; ++} ++ ++static inline void most_dev_put(struct most_dev *d) ++{ ++ module_put(d->owner); ++} ++ ++static inline void most_sched_tx(struct most_dev *mdev) ++{ ++ tasklet_schedule(&mdev->tx_task); ++} ++ ++static inline void most_sched_rx(struct most_dev *mdev) ++{ ++ tasklet_schedule(&mdev->rx_task); ++} ++ ++static inline int most_recv_frame(struct sk_buff *skb) ++{ ++ struct most_dev *mdev = (struct most_dev *) skb->dev; ++ ++ /* Time stamp */ ++ __net_timestamp(skb); ++ ++ /* Queue frame for rx task */ ++ skb_queue_tail(&mdev->rx_q, skb); ++ most_sched_rx(mdev); ++ return 0; ++} ++ ++static inline int __most_configure_channel(struct most_dev *mdev, ++ u8 channel_type, u8 channel, u8 up) ++{ ++ if (mdev->state != MOST_DEV_UP) ++ return -ENETDOWN; ++ ++ if (mdev->conf_channel) ++ if (channel != MOST_NO_CHANNEL) ++ return mdev->conf_channel(mdev, channel_type, channel, ++ up); ++ return 0; ++} ++ ++static inline int most_configure_channels(struct most_dev *mdev, ++ struct most_sock *sk, u8 up) ++{ ++ int err; ++ u8 flags = (up) ? MOST_CONF_FLAG_UP : 0; ++ ++ err = __most_configure_channel(mdev, sk->channel_type, sk->rx_channel, ++ flags); ++ if (err) ++ return err; ++ ++ err = __most_configure_channel(mdev, sk->channel_type, sk->tx_channel, ++ flags | MOST_CONF_FLAG_TX); ++ if (err) ++ __most_configure_channel(mdev, sk->channel_type, sk->rx_channel, ++ (up) ? 0 : MOST_CONF_FLAG_UP); ++ return err; ++} ++ ++struct most_dev *most_alloc_dev(void); ++void most_free_dev(struct most_dev *mdev); ++int most_register_dev(struct most_dev *mdev); ++int most_unregister_dev(struct most_dev *mdev); ++ ++int most_get_dev_list(void __user *arg); ++int most_open_dev(u16 dev_id); ++int most_close_dev(u16 dev_id); ++ ++#endif ++ +diff -uNr linux-2.6.31/include/net/most/most.h linux-2.6.31.new/include/net/most/most.h +--- linux-2.6.31/include/net/most/most.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/net/most/most.h 2009-10-23 11:16:55.000000000 -0700 +@@ -0,0 +1,110 @@ ++#ifndef __MOST_H ++#define __MOST_H ++ ++#include ++ ++#ifndef AF_MOST ++#define AF_MOST 37 ++#define PF_MOST AF_MOST ++#endif ++ ++/* Reserve for core and drivers use */ ++#define MOST_SKB_RESERVE 8 ++ ++#define CTL_FRAME_SIZE 32 ++ ++#define MOSTPROTO_DEV 0 ++#define MOSTPROTO_CTL 1 ++#define MOSTPROTO_SYNC 2 ++#define MOSTPROTO_ASYNC 3 ++ ++#define MOST_NO_CHANNEL 0xFE ++ ++enum { ++ MOST_CONNECTED = 1, /* Equal to TCP_ESTABLISHED makes net code happy */ ++ MOST_OPEN, ++ MOST_BOUND, ++}; ++ ++ ++struct most_skb_cb { ++ __u8 channel_type; ++ __u8 channel; ++}; ++#define most_cb(skb) ((struct most_skb_cb *)(skb->cb)) ++ ++struct most_sock { ++ struct sock sk; ++ u8 channel_type; ++ u8 rx_channel; ++ u8 tx_channel; ++ int dev_id; ++ struct most_dev *mdev; ++}; ++#define most_sk(sk) ((struct most_sock *)sk) ++ ++static inline struct sock *most_sk_alloc(struct net *net, ++ struct proto *pops, u8 channel_type) ++{ ++ struct sock *sk = sk_alloc(net, PF_MOST, GFP_ATOMIC, pops); ++ if (sk) { ++ most_sk(sk)->channel_type = channel_type; ++ most_sk(sk)->dev_id = -1; ++ } ++ ++ return sk; ++} ++static inline struct sk_buff *most_skb_alloc(unsigned int len, gfp_t how) ++{ ++ struct sk_buff *skb = alloc_skb(len + MOST_SKB_RESERVE, how); ++ ++ if (skb) ++ skb_reserve(skb, MOST_SKB_RESERVE); ++ ++ return skb; ++} ++ ++static inline struct sk_buff *most_skb_send_alloc(struct sock *sk, ++ unsigned long len, int nb, int *err) ++{ ++ struct sk_buff *skb = ++ sock_alloc_send_skb(sk, len + MOST_SKB_RESERVE, nb, err); ++ ++ if (skb) ++ skb_reserve(skb, MOST_SKB_RESERVE); ++ ++ return skb; ++} ++ ++struct most_sock_list { ++ struct hlist_head head; ++ rwlock_t lock; ++}; ++ ++struct most_dev *most_dev_get(int index); ++ ++int most_sock_register(int proto, struct net_proto_family *ops); ++int most_sock_unregister(int proto); ++void most_sock_link(struct sock *s); ++void most_sock_unlink(struct sock *sk); ++ ++int most_send_to_sock(int dev_id, struct sk_buff *skb); ++ ++/* default implementation of socket operations */ ++int most_sock_release(struct socket *sock); ++int most_sock_bind(struct socket *sock, int dev_id, u8 rx_chan, u8 tx_chan); ++int most_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg); ++int most_sock_recvmsg(struct kiocb *iocb, struct socket *sock, ++ struct msghdr *msg, size_t len, int flags); ++int most_sock_sendmsg(struct kiocb *iocb, struct socket *sock, ++ struct msghdr *msg, size_t len); ++int most_sock_setsockopt(struct socket *sock, int level, int optname, ++ char __user *optval, int len); ++int most_sock_getsockopt(struct socket *sock, int level, int optname, ++ char __user *optval, int __user *optlen); ++ ++extern int dev_sock_init(void); ++extern void dev_sock_cleanup(void); ++ ++#endif /* __MOST_H */ ++ +diff -uNr linux-2.6.31/include/net/most/sync.h linux-2.6.31.new/include/net/most/sync.h +--- linux-2.6.31/include/net/most/sync.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/net/most/sync.h 2009-10-23 11:16:55.000000000 -0700 +@@ -0,0 +1,12 @@ ++#ifndef __SYNC_H ++#define __SYNC_H ++ ++struct sockaddr_mostsync { ++ sa_family_t most_family; ++ unsigned short most_dev; ++ unsigned char rx_channel; ++ unsigned char tx_channel; ++}; ++ ++#endif ++ +diff -uNr linux-2.6.31/include/sound/timbi2s.h linux-2.6.31.new/include/sound/timbi2s.h +--- linux-2.6.31/include/sound/timbi2s.h 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/include/sound/timbi2s.h 2009-10-23 11:16:55.000000000 -0700 +@@ -0,0 +1,32 @@ ++/* ++ * timbi2s.h timberdale FPGA I2S platform data ++ * 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. ++ */ ++#ifndef __INCLUDE_SOUND_TIMBI2S_H ++#define __INCLUDE_SOUND_TIMBI2S_H ++ ++struct timbi2s_bus_data { ++ u8 rx; ++ u16 sample_rate; ++}; ++ ++struct timbi2s_platform_data { ++ const struct timbi2s_bus_data *busses; ++ int num_busses; ++ u32 main_clk; ++}; ++ ++#endif +diff -uNr linux-2.6.31/net/Kconfig linux-2.6.31.new/net/Kconfig +--- linux-2.6.31/net/Kconfig 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/net/Kconfig 2009-10-23 11:17:37.000000000 -0700 +@@ -235,6 +235,7 @@ + source "net/irda/Kconfig" + source "net/bluetooth/Kconfig" + source "net/rxrpc/Kconfig" ++source "net/most/Kconfig" + + config FIB_RULES + bool +diff -uNr linux-2.6.31/net/Makefile linux-2.6.31.new/net/Makefile +--- linux-2.6.31/net/Makefile 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/net/Makefile 2009-10-23 11:17:36.000000000 -0700 +@@ -44,6 +44,7 @@ + obj-$(CONFIG_DECNET) += decnet/ + obj-$(CONFIG_ECONET) += econet/ + obj-$(CONFIG_PHONET) += phonet/ ++obj-$(CONFIG_MOST) += most/ + ifneq ($(CONFIG_VLAN_8021Q),) + obj-y += 8021q/ + endif +diff -uNr linux-2.6.31/net/most/af_most.c linux-2.6.31.new/net/most/af_most.c +--- linux-2.6.31/net/most/af_most.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/net/most/af_most.c 2009-10-23 11:17:37.000000000 -0700 +@@ -0,0 +1,169 @@ ++/* ++ * af_most.c Support for the MOST address family ++ * 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 ++#include ++ ++#define MOST_MAX_PROTO 4 ++static struct net_proto_family *most_proto[MOST_MAX_PROTO]; ++static DEFINE_RWLOCK(most_proto_lock); ++ ++#ifdef CONFIG_DEBUG_LOCK_ALLOC ++static struct lock_class_key most_lock_key[MOST_MAX_PROTO]; ++static const char *most_key_strings[MOST_MAX_PROTO] = { ++ "sk_lock-AF_MOST-MOSTPROTO_DEV", ++ "sk_lock-AF_MOST-MOSTPROTO_CTL", ++ "sk_lock-AF_MOST-MOSTPROTO_SYNC", ++ "sk_lock-AF_MOST-MOSTPROTO_ASYNC", ++}; ++ ++static struct lock_class_key most_slock_key[MOST_MAX_PROTO]; ++static const char *most_slock_key_strings[MOST_MAX_PROTO] = { ++ "slock-AF_MOST-MOSTPROTO_DEV", ++ "slock-AF_MOST-MOSTPROTO_CTL", ++ "slock-AF_MOST-MOSTPROTO_SYNC", ++ "slock-AF_MOST-MOSTPROTO_ASYNC", ++}; ++ ++static inline void most_sock_reclassify_lock(struct socket *sock, int proto) ++{ ++ struct sock *sk = sock->sk; ++ ++ if (!sk) ++ return; ++ ++ BUG_ON(sock_owned_by_user(sk)); ++ ++ sock_lock_init_class_and_name(sk, ++ most_slock_key_strings[proto], &most_slock_key[proto], ++ most_key_strings[proto], &most_lock_key[proto]); ++} ++#else ++static inline void most_sock_reclassify_lock(struct socket *sock, int proto) ++{ ++} ++#endif ++ ++ ++int most_sock_register(int proto, struct net_proto_family *ops) ++{ ++ int err = 0; ++ ++ if (proto < 0 || proto >= MOST_MAX_PROTO) ++ return -EINVAL; ++ ++ write_lock(&most_proto_lock); ++ ++ if (most_proto[proto]) ++ err = -EEXIST; ++ else ++ most_proto[proto] = ops; ++ ++ write_unlock(&most_proto_lock); ++ ++ return err; ++} ++EXPORT_SYMBOL(most_sock_register); ++ ++int most_sock_unregister(int proto) ++{ ++ int err = 0; ++ ++ if (proto < 0 || proto >= MOST_MAX_PROTO) ++ return -EINVAL; ++ ++ write_lock(&most_proto_lock); ++ ++ if (!most_proto[proto]) ++ err = -ENOENT; ++ else ++ most_proto[proto] = NULL; ++ ++ write_unlock(&most_proto_lock); ++ ++ return err; ++} ++EXPORT_SYMBOL(most_sock_unregister); ++ ++static int most_sock_create(struct net *net, struct socket *sock, int proto) ++{ ++ int err; ++ ++ if (net != &init_net) ++ return -EAFNOSUPPORT; ++ ++ if (proto < 0 || proto >= MOST_MAX_PROTO) ++ return -EINVAL; ++ ++ if (!most_proto[proto]) ++ request_module("most-proto-%d", proto); ++ ++ err = -EPROTONOSUPPORT; ++ ++ read_lock(&most_proto_lock); ++ ++ if (most_proto[proto] && try_module_get(most_proto[proto]->owner)) { ++ err = most_proto[proto]->create(net, sock, proto); ++ most_sock_reclassify_lock(sock, proto); ++ module_put(most_proto[proto]->owner); ++ } ++ ++ read_unlock(&most_proto_lock); ++ ++ return err; ++} ++ ++static struct net_proto_family most_sock_family_ops = { ++ .owner = THIS_MODULE, ++ .family = PF_MOST, ++ .create = most_sock_create, ++}; ++ ++static int __init most_init(void) ++{ ++ int err; ++ ++ err = sock_register(&most_sock_family_ops); ++ if (err < 0) ++ return err; ++ ++ err = dev_sock_init(); ++ if (err < 0) { ++ sock_unregister(PF_MOST); ++ return err; ++ } ++ ++ printk(KERN_INFO "MOST is initialized\n"); ++ ++ return 0; ++} ++ ++static void __exit most_exit(void) ++{ ++ dev_sock_cleanup(); ++ ++ sock_unregister(PF_MOST); ++} ++ ++subsys_initcall(most_init); ++module_exit(most_exit); ++ ++MODULE_DESCRIPTION("MOST Core"); ++MODULE_LICENSE("GPL v2"); ++MODULE_ALIAS_NETPROTO(PF_MOST); ++ +diff -uNr linux-2.6.31/net/most/async_sock.c linux-2.6.31.new/net/most/async_sock.c +--- linux-2.6.31/net/most/async_sock.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/net/most/async_sock.c 2009-10-23 11:17:37.000000000 -0700 +@@ -0,0 +1,154 @@ ++/* ++ * async_sock.c MOST asyncronous socket support ++ * 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: ++ * Support for MOST asynchronous sockets ++ */ ++ ++#include ++#include ++#include ++#include ++ ++static int async_sock_bind(struct socket *sock, struct sockaddr *addr, ++ int addr_len) ++{ ++ struct sockaddr_mostasync *aaddr = (struct sockaddr_mostasync *)addr; ++ ++ if (!aaddr || aaddr->most_family != AF_MOST) ++ return -EINVAL; ++ ++ return most_sock_bind(sock, aaddr->most_dev, aaddr->rx_channel, ++ aaddr->tx_channel); ++} ++ ++static int async_sock_getname(struct socket *sock, struct sockaddr *addr, ++ int *addr_len, int peer) ++{ ++ struct sockaddr_mostasync *aaddr = (struct sockaddr_mostasync *)addr; ++ struct sock *sk = sock->sk; ++ struct most_dev *mdev = most_sk(sk)->mdev; ++ ++ if (!mdev) ++ return -EBADFD; ++ ++ lock_sock(sk); ++ ++ *addr_len = sizeof(*aaddr); ++ aaddr->most_family = AF_MOST; ++ aaddr->most_dev = mdev->id; ++ aaddr->rx_channel = most_sk(sk)->rx_channel; ++ aaddr->tx_channel = most_sk(sk)->tx_channel; ++ ++ release_sock(sk); ++ return 0; ++} ++ ++ ++static const struct proto_ops async_sock_ops = { ++ .family = PF_MOST, ++ .owner = THIS_MODULE, ++ .release = most_sock_release, ++ .bind = async_sock_bind, ++ .getname = async_sock_getname, ++ .sendmsg = most_sock_sendmsg, ++ .recvmsg = most_sock_recvmsg, ++ .ioctl = most_sock_ioctl, ++ .poll = datagram_poll, ++ .listen = sock_no_listen, ++ .shutdown = sock_no_shutdown, ++ .setsockopt = most_sock_setsockopt, ++ .getsockopt = most_sock_getsockopt, ++ .connect = sock_no_connect, ++ .socketpair = sock_no_socketpair, ++ .accept = sock_no_accept, ++ .mmap = sock_no_mmap ++}; ++static struct proto async_sk_proto = { ++ .name = "ASYNC", ++ .owner = THIS_MODULE, ++ .obj_size = sizeof(struct most_sock) ++}; ++ ++static int async_sock_create(struct net *net, struct socket *sock, int protocol) ++{ ++ struct sock *sk; ++ ++ if (sock->type != SOCK_DGRAM) ++ return -ESOCKTNOSUPPORT; ++ ++ sock->ops = &async_sock_ops; ++ ++ sk = most_sk_alloc(net, &async_sk_proto, CHAN_ASYNC); ++ if (!sk) ++ return -ENOMEM; ++ ++ sock_init_data(sock, sk); ++ ++ sock_reset_flag(sk, SOCK_ZAPPED); ++ ++ sk->sk_protocol = protocol; ++ ++ sock->state = SS_UNCONNECTED; ++ sk->sk_state = MOST_OPEN; ++ ++ most_sock_link(sk); ++ return 0; ++} ++ ++static struct net_proto_family async_sock_family_ops = { ++ .family = PF_MOST, ++ .owner = THIS_MODULE, ++ .create = async_sock_create, ++}; ++ ++ ++static int __init async_init(void) ++{ ++ int err; ++ ++ err = proto_register(&async_sk_proto, 0); ++ if (err < 0) ++ return err; ++ ++ err = most_sock_register(MOSTPROTO_ASYNC, &async_sock_family_ops); ++ if (err < 0) { ++ printk(KERN_ERR "MOST socket registration failed\n"); ++ return err; ++ } ++ ++ printk(KERN_INFO "MOST asynchronous socket layer initialized\n"); ++ ++ return 0; ++} ++ ++static void __exit async_exit(void) ++{ ++ if (most_sock_unregister(MOSTPROTO_ASYNC) < 0) ++ printk(KERN_ERR "ASYNC socket unregistration failed\n"); ++ ++ proto_unregister(&async_sk_proto); ++} ++ ++module_init(async_init); ++module_exit(async_exit); ++ ++MODULE_DESCRIPTION("Most Asyncronous"); ++MODULE_LICENSE("GPL v2"); ++MODULE_ALIAS("most-proto-3"); ++ +diff -uNr linux-2.6.31/net/most/ctl_sock.c linux-2.6.31.new/net/most/ctl_sock.c +--- linux-2.6.31/net/most/ctl_sock.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/net/most/ctl_sock.c 2009-10-23 11:17:37.000000000 -0700 +@@ -0,0 +1,159 @@ ++/* ++ * ctl_sock.c Support for MOST control sockets ++ * 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 ++#include ++#include ++#include ++ ++ ++static int ctl_sock_bind(struct socket *sock, struct sockaddr *addr, ++ int addr_len) ++{ ++ struct sockaddr_mostctl *caddr = (struct sockaddr_mostctl *) addr; ++ ++ if (!caddr || caddr->most_family != AF_MOST) ++ return -EINVAL; ++ ++ return most_sock_bind(sock, caddr->most_dev, caddr->rx_channel, ++ caddr->tx_channel); ++} ++ ++static int ctl_sock_getname(struct socket *sock, struct sockaddr *addr, ++ int *addr_len, int peer) ++{ ++ struct sockaddr_mostctl *caddr = (struct sockaddr_mostctl *) addr; ++ struct sock *sk = sock->sk; ++ struct most_dev *mdev = most_sk(sk)->mdev; ++ ++ if (!mdev) ++ return -EBADFD; ++ ++ lock_sock(sk); ++ ++ *addr_len = sizeof(*caddr); ++ caddr->most_family = AF_MOST; ++ caddr->most_dev = mdev->id; ++ caddr->rx_channel = most_sk(sk)->rx_channel; ++ caddr->tx_channel = most_sk(sk)->tx_channel; ++ ++ release_sock(sk); ++ return 0; ++} ++ ++int ctl_sock_sendmsg(struct kiocb *iocb, struct socket *sock, ++ struct msghdr *msg, size_t len) ++{ ++ if (len != CTL_FRAME_SIZE) ++ return -EINVAL; ++ ++ return most_sock_sendmsg(iocb, sock, msg, len); ++} ++ ++static const struct proto_ops ctl_sock_ops = { ++ .family = PF_MOST, ++ .owner = THIS_MODULE, ++ .release = most_sock_release, ++ .bind = ctl_sock_bind, ++ .getname = ctl_sock_getname, ++ .sendmsg = most_sock_sendmsg, ++ .recvmsg = most_sock_recvmsg, ++ .ioctl = most_sock_ioctl, ++ .poll = datagram_poll, ++ .listen = sock_no_listen, ++ .shutdown = sock_no_shutdown, ++ .setsockopt = most_sock_setsockopt, ++ .getsockopt = most_sock_getsockopt, ++ .connect = sock_no_connect, ++ .socketpair = sock_no_socketpair, ++ .accept = sock_no_accept, ++ .mmap = sock_no_mmap ++}; ++static struct proto ctl_sk_proto = { ++ .name = "CTL", ++ .owner = THIS_MODULE, ++ .obj_size = sizeof(struct most_sock) ++}; ++ ++static int ctl_sock_create(struct net *net, struct socket *sock, int protocol) ++{ ++ struct sock *sk; ++ ++ if (sock->type != SOCK_RAW) ++ return -ESOCKTNOSUPPORT; ++ ++ sock->ops = &ctl_sock_ops; ++ ++ sk = most_sk_alloc(net, &ctl_sk_proto, CHAN_CTL); ++ if (!sk) ++ return -ENOMEM; ++ ++ sock_init_data(sock, sk); ++ ++ sock_reset_flag(sk, SOCK_ZAPPED); ++ ++ sk->sk_protocol = protocol; ++ ++ sock->state = SS_UNCONNECTED; ++ sk->sk_state = MOST_OPEN; ++ ++ most_sock_link(sk); ++ return 0; ++} ++ ++static struct net_proto_family ctl_sock_family_ops = { ++ .family = PF_MOST, ++ .owner = THIS_MODULE, ++ .create = ctl_sock_create, ++}; ++ ++ ++static int __init ctl_init(void) ++{ ++ int err; ++ ++ err = proto_register(&ctl_sk_proto, 0); ++ if (err < 0) ++ return err; ++ ++ err = most_sock_register(MOSTPROTO_CTL, &ctl_sock_family_ops); ++ if (err < 0) { ++ printk(KERN_ERR "MOST socket registration failed\n"); ++ return err; ++ } ++ ++ printk(KERN_INFO "MOST control socket layer initialized\n"); ++ ++ return 0; ++} ++ ++static void __exit ctl_exit(void) ++{ ++ if (most_sock_unregister(MOSTPROTO_CTL) < 0) ++ printk(KERN_ERR "Control socket unregistration failed\n"); ++ ++ proto_unregister(&ctl_sk_proto); ++} ++ ++module_init(ctl_init); ++module_exit(ctl_exit); ++ ++MODULE_DESCRIPTION("Most Control"); ++MODULE_LICENSE("GPL v2"); ++MODULE_ALIAS("most-proto-1"); ++ +diff -uNr linux-2.6.31/net/most/dev_sock.c linux-2.6.31.new/net/most/dev_sock.c +--- linux-2.6.31/net/most/dev_sock.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/net/most/dev_sock.c 2009-10-23 11:17:37.000000000 -0700 +@@ -0,0 +1,170 @@ ++/* ++ * dev_sock.c Device MOST sockets, to control the underlaying 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. ++ */ ++ ++#include ++#include ++#include ++#include ++ ++/* Ioctls that require bound socket */ ++static inline int dev_sock_bound_ioctl(struct sock *sk, unsigned int cmd, ++ unsigned long arg) ++{ ++ return -ENOSYS; ++} ++ ++static int dev_sock_ioctl(struct socket *sock, unsigned int cmd, ++ unsigned long arg) ++{ ++ void __user *argp = (void __user *) arg; ++ ++ switch (cmd) { ++ case MOSTDEVUP: ++ return most_open_dev(arg & 0xffff); ++ case MOSTDEVDOWN: ++ return most_close_dev(arg & 0xffff); ++ case MOSTGETDEVLIST: ++ return most_get_dev_list(argp); ++ default: ++ return -EINVAL; ++ } ++} ++ ++static int dev_sock_bind(struct socket *sock, struct sockaddr *addr, ++ int addr_len) ++{ ++ return -ENOSYS; ++} ++ ++static int dev_sock_getname(struct socket *sock, struct sockaddr *addr, ++ int *addr_len, int peer) ++{ ++ struct sockaddr_mostdev *daddr = (struct sockaddr_mostdev *) addr; ++ struct sock *sk = sock->sk; ++ struct most_dev *mdev = most_sk(sk)->mdev; ++ ++ if (!mdev) ++ return -EBADFD; ++ ++ lock_sock(sk); ++ ++ *addr_len = sizeof(*daddr); ++ daddr->most_family = AF_MOST; ++ daddr->most_dev = mdev->id; ++ ++ release_sock(sk); ++ return 0; ++} ++ ++static int dev_sock_setsockopt(struct socket *sock, int level, int optname, ++ char __user *optval, int len) ++{ ++ return -ENOSYS; ++} ++ ++static int dev_sock_getsockopt(struct socket *sock, int level, int optname, ++ char __user *optval, int __user *optlen) ++{ ++ return -ENOSYS; ++} ++ ++static const struct proto_ops dev_sock_ops = { ++ .family = PF_MOST, ++ .owner = THIS_MODULE, ++ .release = most_sock_release, ++ .bind = dev_sock_bind, ++ .getname = dev_sock_getname, ++ .sendmsg = sock_no_sendmsg, ++ .recvmsg = sock_no_recvmsg, ++ .ioctl = dev_sock_ioctl, ++ .poll = sock_no_poll, ++ .listen = sock_no_listen, ++ .shutdown = sock_no_shutdown, ++ .setsockopt = dev_sock_setsockopt, ++ .getsockopt = dev_sock_getsockopt, ++ .connect = sock_no_connect, ++ .socketpair = sock_no_socketpair, ++ .accept = sock_no_accept, ++ .mmap = sock_no_mmap ++}; ++static struct proto dev_sk_proto = { ++ .name = "DEV", ++ .owner = THIS_MODULE, ++ .obj_size = sizeof(struct most_sock) ++}; ++ ++static int dev_sock_create(struct net *net, struct socket *sock, int protocol) ++{ ++ struct sock *sk; ++ ++ if (sock->type != SOCK_RAW) ++ return -ESOCKTNOSUPPORT; ++ ++ sock->ops = &dev_sock_ops; ++ ++ sk = most_sk_alloc(net, &dev_sk_proto, CHAN_DEV); ++ if (!sk) ++ return -ENOMEM; ++ ++ sock_init_data(sock, sk); ++ ++ sock_reset_flag(sk, SOCK_ZAPPED); ++ ++ sk->sk_protocol = protocol; ++ ++ sock->state = SS_UNCONNECTED; ++ sk->sk_state = MOST_OPEN; ++ ++ most_sock_link(sk); ++ return 0; ++} ++ ++static struct net_proto_family dev_sock_family_ops = { ++ .family = PF_MOST, ++ .owner = THIS_MODULE, ++ .create = dev_sock_create, ++}; ++ ++ ++int __init dev_sock_init(void) ++{ ++ int err; ++ ++ err = proto_register(&dev_sk_proto, 0); ++ if (err < 0) ++ return err; ++ ++ err = most_sock_register(MOSTPROTO_DEV, &dev_sock_family_ops); ++ if (err < 0) { ++ printk(KERN_ERR "MOST socket registration failed\n"); ++ return err; ++ } ++ ++ printk(KERN_INFO "MOST device socket layer initialized\n"); ++ ++ return 0; ++} ++ ++void __exit dev_sock_cleanup(void) ++{ ++ if (most_sock_unregister(MOSTPROTO_DEV) < 0) ++ printk(KERN_ERR "Device socket unregistration failed\n"); ++ ++ proto_unregister(&dev_sk_proto); ++} ++ +diff -uNr linux-2.6.31/net/most/Kconfig linux-2.6.31.new/net/most/Kconfig +--- linux-2.6.31/net/most/Kconfig 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/net/most/Kconfig 2009-10-23 11:17:37.000000000 -0700 +@@ -0,0 +1,38 @@ ++# ++# Media Oriented Systems Transport (MOST) network layer core configuration ++# ++ ++menuconfig MOST ++ depends on NET ++ tristate "MOST bus subsystem support" ++ ---help--- ++ Media Oriented Systems Transport (MOST) is a multimedia ++ communications protocol in the automotive industry. ++ ++ If you want MOST support you should say Y here. ++ ++config MOST_CTL ++ tristate "Support for Control data over MOST" ++ depends on MOST ++ default N ++ ---help--- ++ Support for the control channel of the MOST bus. ++ ++config MOST_ASYNC ++ tristate "Support for Asynchronous data over MOST" ++ depends on MOST ++ default N ++ ---help--- ++ Support for the asyncronous channel of the MOST bus. Normally ++ used for software download od file transfers. ++ ++config MOST_SYNC ++ tristate "Support for Synchronous data over MOST" ++ depends on MOST ++ default N ++ ---help--- ++ Support for synchronous channles of the MOST bus. Normally used ++ for streaming media such as audio and video. ++ ++ ++source "drivers/net/most/Kconfig" +diff -uNr linux-2.6.31/net/most/Makefile linux-2.6.31.new/net/most/Makefile +--- linux-2.6.31/net/most/Makefile 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/net/most/Makefile 2009-10-23 11:17:37.000000000 -0700 +@@ -0,0 +1,15 @@ ++# ++# Makefile for the Linux Media Oriented Systems Transport core. ++# ++ ++obj-$(CONFIG_MOST) += most.o ++most-objs := af_most.o most_core.o most_sock.o dev_sock.o ++ ++obj-$(CONFIG_MOST_CTL) += ctl.o ++ctl-objs := ctl_sock.o ++ ++obj-$(CONFIG_MOST_SYNC) += sync.o ++sync-objs := sync_sock.o ++ ++obj-$(CONFIG_MOST_ASYNC) += async.o ++async-objs := async_sock.o +diff -uNr linux-2.6.31/net/most/most_core.c linux-2.6.31.new/net/most/most_core.c +--- linux-2.6.31/net/most/most_core.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/net/most/most_core.c 2009-10-23 11:17:37.000000000 -0700 +@@ -0,0 +1,287 @@ ++/* ++ * most_core.c The MOST core functions ++ * 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 ++#include ++#include ++ ++#include ++#include ++ ++/* MOST device list */ ++LIST_HEAD(most_dev_list); ++DEFINE_RWLOCK(most_dev_list_lock); ++ ++ ++int most_open_dev(u16 dev_id) ++{ ++ struct most_dev *mdev = most_dev_get(dev_id); ++ int err = 0; ++ ++ if (!mdev) ++ return -ENODEV; ++ ++ most_dbg("%s: %s, state: %d\n", __func__, mdev->name, mdev->state); ++ ++ if (mdev->state == MOST_DEV_UP) ++ err = -EALREADY; ++ ++ if (!err) ++ err = mdev->open(mdev); ++ if (!err) ++ mdev->state = MOST_DEV_UP; ++ ++ most_dev_put(mdev); ++ most_dbg("%s: %s, state: %d, err: %d\n", __func__, ++ mdev->name, mdev->state, err); ++ return err; ++} ++ ++static int __most_close_dev(struct most_dev *mdev) ++{ ++ int err = 0; ++ ++ most_dbg("%s: %s, state: %d\n", __func__, mdev ? mdev->name : "nil", ++ mdev ? mdev->state : -1); ++ ++ if (!mdev) ++ return -ENODEV; ++ ++ if (mdev->state == MOST_DEV_DOWN) ++ err = -EALREADY; ++ ++ if (!err) ++ err = mdev->close(mdev); ++ if (!err) ++ mdev->state = MOST_DEV_DOWN; ++ ++ most_dev_put(mdev); ++ most_dbg("%s: %s, state: %d, err: %d\n", __func__, ++ mdev->name, mdev->state, err); ++ return err; ++} ++ ++int most_close_dev(u16 dev_id) ++{ ++ return __most_close_dev(most_dev_get(dev_id)); ++} ++ ++int most_get_dev_list(void __user *arg) ++{ ++ struct most_dev_list_req *dl; ++ struct most_dev_req *dr; ++ struct list_head *p; ++ int n = 0, size, err; ++ u16 dev_num; ++ ++ if (get_user(dev_num, (u16 __user *) arg)) ++ return -EFAULT; ++ ++ if (!dev_num || dev_num > (PAGE_SIZE * 2) / sizeof(*dr)) ++ return -EINVAL; ++ ++ size = sizeof(*dl) + dev_num * sizeof(*dr); ++ ++ dl = kzalloc(size, GFP_KERNEL); ++ if (!dl) ++ return -ENOMEM; ++ ++ dr = dl->dev_req; ++ ++ read_lock_bh(&most_dev_list_lock); ++ list_for_each(p, &most_dev_list) { ++ struct most_dev *mdev; ++ mdev = list_entry(p, struct most_dev, list); ++ (dr + n)->dev_id = mdev->id; ++ if (++n >= dev_num) ++ break; ++ } ++ read_unlock_bh(&most_dev_list_lock); ++ ++ dl->dev_num = n; ++ size = sizeof(*dl) + n * sizeof(*dr); ++ ++ err = copy_to_user(arg, dl, size); ++ kfree(dl); ++ ++ return err ? -EFAULT : 0; ++} ++ ++static int most_send_frame(struct sk_buff *skb) ++{ ++ struct most_dev *mdev = (struct most_dev *) skb->dev; ++ ++ if (!mdev) { ++ kfree_skb(skb); ++ return -ENODEV; ++ } ++ ++ most_dbg("%s: %s type %d len %d\n", __func__, mdev->name, ++ most_cb(skb)->channel_type, skb->len); ++ ++ /* Get rid of skb owner, prior to sending to the driver. */ ++ skb_orphan(skb); ++ ++ return mdev->send(skb); ++} ++ ++static void most_send_queue(struct sk_buff_head *q) ++{ ++ struct sk_buff *skb; ++ ++ while ((skb = skb_dequeue(q))) { ++ struct most_dev *mdev = (struct most_dev *)skb->dev; ++ ++ most_dbg("%s: skb %p len %d\n", __func__, skb, skb->len); ++ ++ if (!mdev->can_send || mdev->can_send(skb)) ++ most_send_frame(skb); ++ else { ++ most_dbg("%s, could not send frame, requeueing\n", ++ __func__); ++ skb_queue_tail(q, skb); ++ break; ++ } ++ } ++} ++ ++static void most_tx_task(unsigned long arg) ++{ ++ struct most_dev *mdev = (struct most_dev *) arg; ++ ++ most_dbg("%s: %s\n", __func__, mdev->name); ++ ++ most_send_queue(&mdev->ctl_q); ++ most_send_queue(&mdev->sync_q); ++ most_send_queue(&mdev->async_q); ++} ++ ++static void most_rx_task(unsigned long arg) ++{ ++ struct most_dev *mdev = (struct most_dev *) arg; ++ struct sk_buff *skb = skb_dequeue(&mdev->rx_q); ++ ++ most_dbg("%s: %s\n", __func__, mdev->name); ++ ++ while (skb) { ++ /* Send to the sockets */ ++ most_send_to_sock(mdev->id, skb); ++ kfree_skb(skb); ++ skb = skb_dequeue(&mdev->rx_q); ++ } ++} ++ ++ ++/* Get MOST device by index. ++ * Device is held on return. */ ++struct most_dev *most_dev_get(int index) ++{ ++ struct most_dev *mdev = NULL; ++ struct list_head *p; ++ ++ if (index < 0) ++ return NULL; ++ ++ read_lock(&most_dev_list_lock); ++ list_for_each(p, &most_dev_list) { ++ struct most_dev *d = list_entry(p, struct most_dev, list); ++ if (d->id == index) { ++ mdev = most_dev_hold(d); ++ break; ++ } ++ } ++ read_unlock(&most_dev_list_lock); ++ return mdev; ++} ++EXPORT_SYMBOL(most_dev_get); ++ ++ ++/* Alloc MOST device */ ++struct most_dev *most_alloc_dev(void) ++{ ++ struct most_dev *mdev; ++ ++ mdev = kzalloc(sizeof(struct most_dev), GFP_KERNEL); ++ if (!mdev) ++ return NULL; ++ ++ mdev->state = MOST_DEV_DOWN; ++ ++ return mdev; ++} ++EXPORT_SYMBOL(most_alloc_dev); ++ ++ ++void most_free_dev(struct most_dev *mdev) ++{ ++ kfree(mdev); ++} ++EXPORT_SYMBOL(most_free_dev); ++ ++ ++/* Register MOST device */ ++int most_register_dev(struct most_dev *mdev) ++{ ++ struct list_head *head = &most_dev_list, *p; ++ int id = 0; ++ ++ if (!mdev->open || !mdev->close || !mdev->send || !mdev->owner) ++ return -EINVAL; ++ ++ write_lock_bh(&most_dev_list_lock); ++ ++ /* Find first available device id */ ++ list_for_each(p, &most_dev_list) { ++ if (list_entry(p, struct most_dev, list)->id != id) ++ break; ++ head = p; id++; ++ } ++ ++ sprintf(mdev->name, "most%d", id); ++ mdev->id = id; ++ list_add(&mdev->list, head); ++ ++ tasklet_init(&mdev->rx_task, most_rx_task, (unsigned long) mdev); ++ tasklet_init(&mdev->tx_task, most_tx_task, (unsigned long) mdev); ++ ++ skb_queue_head_init(&mdev->rx_q); ++ skb_queue_head_init(&mdev->ctl_q); ++ skb_queue_head_init(&mdev->sync_q); ++ skb_queue_head_init(&mdev->async_q); ++ ++ write_unlock_bh(&most_dev_list_lock); ++ return 0; ++} ++EXPORT_SYMBOL(most_register_dev); ++ ++int most_unregister_dev(struct most_dev *mdev) ++{ ++ int ret = 0; ++ most_dbg("%s: %s: state: %d\n", __func__, mdev->name, mdev->state); ++ ++ if (mdev->state != MOST_DEV_DOWN) ++ ret = __most_close_dev(mdev); ++ ++ write_lock_bh(&most_dev_list_lock); ++ list_del(&mdev->list); ++ write_unlock_bh(&most_dev_list_lock); ++ ++ return ret; ++} ++EXPORT_SYMBOL(most_unregister_dev); ++ +diff -uNr linux-2.6.31/net/most/most_sock.c linux-2.6.31.new/net/most/most_sock.c +--- linux-2.6.31/net/most/most_sock.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/net/most/most_sock.c 2009-10-23 11:17:37.000000000 -0700 +@@ -0,0 +1,315 @@ ++/* ++ * most_sock.c Generic functions for MOST sockets ++ * 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 ++#include ++ ++static struct most_sock_list most_sk_list = { ++ .lock = __RW_LOCK_UNLOCKED(ctl_sk_list.lock) ++}; ++ ++void most_sock_link(struct sock *sk) ++{ ++ write_lock_bh(&most_sk_list.lock); ++ sk_add_node(sk, &most_sk_list.head); ++ write_unlock_bh(&most_sk_list.lock); ++} ++EXPORT_SYMBOL(most_sock_link); ++ ++void most_sock_unlink(struct sock *sk) ++{ ++ write_lock_bh(&most_sk_list.lock); ++ sk_del_node_init(sk); ++ write_unlock_bh(&most_sk_list.lock); ++} ++EXPORT_SYMBOL(most_sock_unlink); ++ ++static int channel_in_use(int dev_id, u8 channel) ++{ ++ struct sock *sk; ++ struct hlist_node *node; ++ ++ read_lock_bh(&most_sk_list.lock); ++ ++ sk_for_each(sk, node, &most_sk_list.head) ++ if (most_sk(sk)->dev_id == dev_id && ++ sk->sk_state == MOST_BOUND && ++ (most_sk(sk)->rx_channel == channel || ++ most_sk(sk)->tx_channel == channel)) ++ goto found; ++ ++ sk = NULL; ++found: ++ read_unlock_bh(&most_sk_list.lock); ++ ++ return sk != NULL; ++} ++ ++int most_send_to_sock(int dev_id, struct sk_buff *skb) ++{ ++ struct sock *sk; ++ struct hlist_node *node; ++ ++ read_lock(&most_sk_list.lock); ++ sk_for_each(sk, node, &most_sk_list.head) { ++ if (most_sk(sk)->dev_id == dev_id && ++ most_sk(sk)->channel_type == most_cb(skb)->channel_type ++ && most_sk(sk)->rx_channel == most_cb(skb)->channel && ++ sk->sk_state == MOST_BOUND) { ++ ++ struct sk_buff *nskb = skb_clone(skb, GFP_ATOMIC); ++ if (nskb) ++ if (sock_queue_rcv_skb(sk, nskb)) ++ kfree_skb(nskb); ++ } ++ ++ } ++ read_unlock(&most_sk_list.lock); ++ ++ return 0; ++} ++EXPORT_SYMBOL(most_send_to_sock); ++ ++int most_sock_release(struct socket *sock) ++{ ++ struct sock *sk = sock->sk; ++ struct most_dev *mdev; ++ ++ most_dbg("%s: sock %p sk %p\n", __func__, sock, sk); ++ ++ if (!sk) ++ return 0; ++ ++ mdev = most_sk(sk)->mdev; ++ ++ most_sock_unlink(sk); ++ ++ if (mdev) { ++ if (sk->sk_state == MOST_BOUND) ++ most_configure_channels(mdev, most_sk(sk), 0); ++ ++ most_dev_put(mdev); ++ } ++ ++ sock_orphan(sk); ++ sock_put(sk); ++ return 0; ++} ++EXPORT_SYMBOL(most_sock_release); ++ ++int most_sock_bind(struct socket *sock, int dev_id, u8 rx_chan, u8 tx_chan) ++{ ++ struct sock *sk = sock->sk; ++ struct most_dev *mdev = NULL; ++ int err = 0; ++ ++ most_dbg("%s: sock %p sk %p, rx: %d, tx: %d\n", ++ __func__, sock, sk, rx_chan, tx_chan); ++ ++ lock_sock(sk); ++ ++ if (sk->sk_state != MOST_OPEN) { ++ err = -EBADFD; ++ goto done; ++ } ++ ++ if (most_sk(sk)->mdev) { ++ err = -EALREADY; ++ goto done; ++ } ++ ++ if (channel_in_use(dev_id, rx_chan) || ++ channel_in_use(dev_id, tx_chan)) { ++ err = -EADDRINUSE; ++ goto done; ++ } else { ++ most_sk(sk)->rx_channel = rx_chan; ++ most_sk(sk)->tx_channel = tx_chan; ++ } ++ ++ mdev = most_dev_get(dev_id); ++ if (!mdev) { ++ err = -ENODEV; ++ goto done; ++ } ++ ++ err = most_configure_channels(mdev, most_sk(sk), 1); ++ if (err) { ++ most_dev_put(mdev); ++ goto done; ++ } ++ ++ most_sk(sk)->mdev = mdev; ++ most_sk(sk)->dev_id = mdev->id; ++ ++ sk->sk_state = MOST_BOUND; ++ ++done: ++ release_sock(sk); ++ return err; ++} ++EXPORT_SYMBOL(most_sock_bind); ++ ++ ++int most_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) ++{ ++ most_dbg("%s\n", __func__); ++ return -EINVAL; ++} ++EXPORT_SYMBOL(most_sock_ioctl); ++ ++int most_sock_recvmsg(struct kiocb *iocb, struct socket *sock, ++ struct msghdr *msg, size_t len, int flags) ++{ ++ int noblock = flags & MSG_DONTWAIT; ++ struct sock *sk = sock->sk; ++ struct sk_buff *skb; ++ int copied, err; ++ ++ most_dbg("%s\n", __func__); ++ ++ if (most_sk(sk)->rx_channel == MOST_NO_CHANNEL) ++ return -EOPNOTSUPP; ++ ++ if (flags & (MSG_OOB)) ++ return -EOPNOTSUPP; ++ ++ if (sk->sk_state != MOST_BOUND) ++ return 0; ++ ++ skb = skb_recv_datagram(sk, flags, noblock, &err); ++ if (!skb) ++ return err; ++ ++ msg->msg_namelen = 0; ++ ++ copied = skb->len; ++ if (len < copied) { ++ msg->msg_flags |= MSG_TRUNC; ++ copied = len; ++ } ++ ++ skb_reset_transport_header(skb); ++ err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied); ++ ++ skb_free_datagram(sk, skb); ++ ++ return err ? : copied; ++} ++EXPORT_SYMBOL(most_sock_recvmsg); ++ ++int most_sock_sendmsg(struct kiocb *iocb, struct socket *sock, ++ struct msghdr *msg, size_t len) ++{ ++ struct sock *sk = sock->sk; ++ struct most_dev *mdev; ++ struct sk_buff *skb; ++ int err; ++ ++ most_dbg("%s: sock %p sk %p, channeltype: %d\n", ++ __func__, sock, sk, most_sk(sk)->channel_type); ++ ++ if (most_sk(sk)->tx_channel == MOST_NO_CHANNEL) ++ return -EOPNOTSUPP; ++ ++ if (msg->msg_flags & MSG_OOB) ++ return -EOPNOTSUPP; ++ ++ if (msg->msg_flags & ~(MSG_DONTWAIT|MSG_NOSIGNAL|MSG_ERRQUEUE)) ++ return -EINVAL; ++ ++ lock_sock(sk); ++ ++ mdev = most_sk(sk)->mdev; ++ if (!mdev) { ++ err = -EBADFD; ++ goto done; ++ } ++ ++ skb = sock_alloc_send_skb(sk, len, msg->msg_flags & MSG_DONTWAIT, &err); ++ if (!skb) ++ goto done; ++ ++ most_cb(skb)->channel = most_sk(sk)->tx_channel; ++ most_cb(skb)->channel_type = most_sk(sk)->channel_type; ++ ++ if (memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len)) { ++ err = -EFAULT; ++ goto drop; ++ } ++ ++ skb->dev = (void *) mdev; ++ ++ skb_queue_tail(&mdev->ctl_q, skb); ++ most_sched_tx(mdev); ++ ++ err = len; ++ ++done: ++ release_sock(sk); ++ return err; ++ ++drop: ++ kfree_skb(skb); ++ goto done; ++} ++EXPORT_SYMBOL(most_sock_sendmsg); ++ ++int most_sock_setsockopt(struct socket *sock, int level, int optname, ++ char __user *optval, int len) ++{ ++ struct sock *sk = sock->sk; ++ int err = 0; ++ ++ most_dbg("%s: sk %p", __func__, sk); ++ ++ lock_sock(sk); ++ ++ switch (optname) { ++ default: ++ err = -ENOPROTOOPT; ++ break; ++ } ++ ++ release_sock(sk); ++ return err; ++} ++EXPORT_SYMBOL(most_sock_setsockopt); ++ ++ ++int most_sock_getsockopt(struct socket *sock, int level, int optname, ++ char __user *optval, int __user *optlen) ++{ ++ struct sock *sk = sock->sk; ++ int err = 0; ++ ++ most_dbg("%s: sk %p", __func__, sk); ++ ++ lock_sock(sk); ++ ++ switch (optname) { ++ default: ++ err = -ENOPROTOOPT; ++ break; ++ } ++ ++ release_sock(sk); ++ return err; ++} ++EXPORT_SYMBOL(most_sock_getsockopt); ++ +diff -uNr linux-2.6.31/net/most/sync_sock.c linux-2.6.31.new/net/most/sync_sock.c +--- linux-2.6.31/net/most/sync_sock.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/net/most/sync_sock.c 2009-10-23 11:17:37.000000000 -0700 +@@ -0,0 +1,150 @@ ++/* ++ * sync_sock.c Support for MOST synchronous sockets ++ * 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 ++#include ++#include ++#include ++ ++static int sync_sock_bind(struct socket *sock, struct sockaddr *addr, ++ int addr_len) ++{ ++ struct sockaddr_mostsync *saddr = (struct sockaddr_mostsync *)addr; ++ ++ if (!saddr || saddr->most_family != AF_MOST) ++ return -EINVAL; ++ ++ return most_sock_bind(sock, saddr->most_dev, saddr->rx_channel, ++ saddr->tx_channel); ++} ++ ++static int sync_sock_getname(struct socket *sock, struct sockaddr *addr, ++ int *addr_len, int peer) ++{ ++ struct sockaddr_mostsync *saddr = (struct sockaddr_mostsync *)addr; ++ struct sock *sk = sock->sk; ++ struct most_dev *mdev = most_sk(sk)->mdev; ++ ++ if (!mdev) ++ return -EBADFD; ++ ++ lock_sock(sk); ++ ++ *addr_len = sizeof(*saddr); ++ saddr->most_family = AF_MOST; ++ saddr->most_dev = mdev->id; ++ saddr->rx_channel = most_sk(sk)->rx_channel; ++ saddr->tx_channel = most_sk(sk)->tx_channel; ++ ++ release_sock(sk); ++ return 0; ++} ++ ++ ++static const struct proto_ops sync_sock_ops = { ++ .family = PF_MOST, ++ .owner = THIS_MODULE, ++ .release = most_sock_release, ++ .bind = sync_sock_bind, ++ .getname = sync_sock_getname, ++ .sendmsg = most_sock_sendmsg, ++ .recvmsg = most_sock_recvmsg, ++ .ioctl = most_sock_ioctl, ++ .poll = datagram_poll, ++ .listen = sock_no_listen, ++ .shutdown = sock_no_shutdown, ++ .setsockopt = most_sock_setsockopt, ++ .getsockopt = most_sock_getsockopt, ++ .connect = sock_no_connect, ++ .socketpair = sock_no_socketpair, ++ .accept = sock_no_accept, ++ .mmap = sock_no_mmap ++}; ++static struct proto sync_sk_proto = { ++ .name = "SYNC", ++ .owner = THIS_MODULE, ++ .obj_size = sizeof(struct most_sock) ++}; ++ ++static int sync_sock_create(struct net *net, struct socket *sock, int protocol) ++{ ++ struct sock *sk; ++ ++ if (sock->type != SOCK_STREAM) ++ return -ESOCKTNOSUPPORT; ++ ++ sock->ops = &sync_sock_ops; ++ ++ sk = most_sk_alloc(net, &sync_sk_proto, CHAN_SYNC); ++ if (!sk) ++ return -ENOMEM; ++ ++ sock_init_data(sock, sk); ++ ++ sock_reset_flag(sk, SOCK_ZAPPED); ++ ++ sk->sk_protocol = protocol; ++ ++ sock->state = SS_UNCONNECTED; ++ sk->sk_state = MOST_OPEN; ++ ++ most_sock_link(sk); ++ return 0; ++} ++ ++static struct net_proto_family sync_sock_family_ops = { ++ .family = PF_MOST, ++ .owner = THIS_MODULE, ++ .create = sync_sock_create, ++}; ++ ++ ++static int __init sync_init(void) ++{ ++ int err; ++ ++ err = proto_register(&sync_sk_proto, 0); ++ if (err < 0) ++ return err; ++ ++ err = most_sock_register(MOSTPROTO_SYNC, &sync_sock_family_ops); ++ if (err < 0) { ++ printk(KERN_ERR "MOST socket registration failed\n"); ++ return err; ++ } ++ ++ printk(KERN_INFO "MOST synchronous socket layer initialized\n"); ++ ++ return 0; ++} ++ ++static void __exit sync_exit(void) ++{ ++ if (most_sock_unregister(MOSTPROTO_SYNC) < 0) ++ printk(KERN_ERR "SYNC socket unregistration failed\n"); ++ ++ proto_unregister(&sync_sk_proto); ++} ++ ++module_init(sync_init); ++module_exit(sync_exit); ++ ++MODULE_DESCRIPTION("Most Syncronous"); ++MODULE_LICENSE("GPL v2"); ++MODULE_ALIAS("most-proto-2"); ++ +diff -uNr linux-2.6.31/sound/drivers/Kconfig linux-2.6.31.new/sound/drivers/Kconfig +--- linux-2.6.31/sound/drivers/Kconfig 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/sound/drivers/Kconfig 2009-10-23 11:16:57.000000000 -0700 +@@ -182,4 +182,17 @@ + The default time-out value in seconds for AC97 automatic + power-save mode. 0 means to disable the power-save mode. + ++config SND_TIMBERDALE_I2S ++ tristate "The timberdale FPGA I2S driver" ++ depends on MFD_TIMBERDALE && HAS_IOMEM ++ default y ++ help ++ Say Y here to enable driver for the I2S block found within the ++ Timberdale FPGA. ++ There is support for up to 8 I2S channels, in either transmitter ++ or receiver mode. ++ ++ To compile this driver as a module, choose M here: the module ++ will be called snd-timbi2s. ++ + endif # SND_DRIVERS +diff -uNr linux-2.6.31/sound/drivers/Makefile linux-2.6.31.new/sound/drivers/Makefile +--- linux-2.6.31/sound/drivers/Makefile 2009-10-23 11:18:30.000000000 -0700 ++++ linux-2.6.31.new/sound/drivers/Makefile 2009-10-23 11:16:57.000000000 -0700 +@@ -10,6 +10,7 @@ + snd-serial-u16550-objs := serial-u16550.o + snd-virmidi-objs := virmidi.o + snd-ml403-ac97cr-objs := ml403-ac97cr.o pcm-indirect2.o ++snd-timbi2s-objs := timbi2s.o + + # Toplevel Module Dependency + obj-$(CONFIG_SND_DUMMY) += snd-dummy.o +@@ -19,5 +20,6 @@ + obj-$(CONFIG_SND_MTS64) += snd-mts64.o + obj-$(CONFIG_SND_PORTMAN2X4) += snd-portman2x4.o + obj-$(CONFIG_SND_ML403_AC97CR) += snd-ml403-ac97cr.o ++obj-$(CONFIG_SND_TIMBERDALE_I2S) += snd-timbi2s.o + + obj-$(CONFIG_SND) += opl3/ opl4/ mpu401/ vx/ pcsp/ +diff -uNr linux-2.6.31/sound/drivers/timbi2s.c linux-2.6.31.new/sound/drivers/timbi2s.c +--- linux-2.6.31/sound/drivers/timbi2s.c 1969-12-31 16:00:00.000000000 -0800 ++++ linux-2.6.31.new/sound/drivers/timbi2s.c 2009-10-23 11:16:57.000000000 -0700 +@@ -0,0 +1,755 @@ ++/* ++ * 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 ++ * ++ */ ++ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#define DRIVER_NAME "timb-i2s" ++ ++#define MAX_BUSSES 8 ++ ++#define TIMBI2S_REG_VER 0x00 ++#define TIMBI2S_REG_UIR 0x04 ++ ++#define TIMBI2S_BUS_PRESCALE 0x00 ++#define TIMBI2S_BUS_ICLR 0x04 ++#define TIMBI2S_BUS_IPR 0x08 ++#define TIMBI2S_BUS_ISR 0x0c ++#define TIMBI2S_BUS_IER 0x10 ++ ++ ++#define TIMBI2S_IRQ_TX_FULL 0x01 ++#define TIMBI2S_IRQ_TX_ALMOST_FULL 0x02 ++#define TIMBI2S_IRQ_TX_ALMOST_EMPTY 0x04 ++#define TIMBI2S_IRQ_TX_EMPTY 0x08 ++ ++#define TIMBI2S_IRQ_RX_FULL 0x10 ++#define TIMBI2S_IRQ_RX_ALMOST_FULL 0x20 ++#define TIMBI2S_IRQ_RX_ALMOST_EMPTY 0x40 ++#define TIMBI2S_IRQ_RX_NOT_EMPTY 0x80 ++ ++#define TIMBI2S_BUS_ICOR 0x14 ++#define TIMBI2S_ICOR_TX_ENABLE 0x00000001 ++#define TIMBI2S_ICOR_RX_ENABLE 0x00000002 ++#define TIMBI2S_ICOR_LFIFO_RST 0x00000004 ++#define TIMBI2S_ICOR_RFIFO_RST 0x00000008 ++#define TIMBI2S_ICOR_FIFO_RST (TIMBI2S_ICOR_LFIFO_RST | TIMBI2S_ICOR_RFIFO_RST) ++#define TIMBI2S_ICOR_SOFT_RST 0x00000010 ++#define TIMBI2S_ICOR_WORD_SEL_LEFT_SHIFT 8 ++#define TIMBI2S_ICOR_WORD_SEL_LEFT_MASK (0xff << 8) ++#define TIMBI2S_ICOR_WORD_SEL_RIGHT_SHIFT 16 ++#define TIMBI2S_ICOR_WORD_SEL_RIGHT_MASK (0xff << 16) ++#define TIMBI2S_ICOR_CLK_MASTER 0x10000000 ++#define TIMBI2S_ICOR_RX_ID 0x20000000 ++#define TIMBI2S_ICOR_TX_ID 0x40000000 ++#define TIMBI2S_ICOR_WORD_SEL 0x80000000 ++#define TIMBI2S_BUS_FIFO 0x18 ++ ++#define TIMBI2S_BUS_REG_AREA_SIZE (TIMBI2S_BUS_FIFO - \ ++ TIMBI2S_BUS_PRESCALE + 4) ++#define TIMBI2S_FIRST_BUS_AREA_OFS 0x08 ++ ++struct timbi2s_bus { ++ u32 flags; ++ u32 prescale; ++ struct snd_pcm *pcm; ++ struct snd_card *card; ++ struct snd_pcm_substream *substream; ++ unsigned buf_pos; ++ spinlock_t lock; /* mutual exclusion */ ++ u16 sample_rate; ++}; ++ ++#define BUS_RX 0x200 ++#define BUS_MASTER 0x100 ++#define BUS_INDEX_MASK 0xff ++#define BUS_INDEX(b) ((b)->flags & BUS_INDEX_MASK) ++#define BUS_IS_MASTER(b) ((b)->flags & BUS_MASTER) ++#define BUS_IS_RX(b) ((b)->flags & BUS_RX) ++ ++#define SET_BUS_INDEX(b, id) ((b)->flags = ((b)->flags & ~BUS_INDEX_MASK) | id) ++#define SET_BUS_MASTER(b) ((b)->flags |= BUS_MASTER) ++#define SET_BUS_RX(b) ((b)->flags |= BUS_RX) ++ ++#define TIMBI2S_BUS_OFFSET(bus) (TIMBI2S_FIRST_BUS_AREA_OFS + \ ++ TIMBI2S_BUS_REG_AREA_SIZE * BUS_INDEX(bus)) ++ ++struct timbi2s { ++ void __iomem *membase; ++ int irq; ++ struct tasklet_struct tasklet; ++ u32 main_clk; ++ unsigned num_busses; ++ struct timbi2s_bus busses[0]; ++}; ++ ++#define BITS_PER_CHANNEL 16 ++#define NUM_CHANNELS 2 ++ ++#define SAMPLE_SIZE ((NUM_CHANNELS * BITS_PER_CHANNEL) / 8) ++#define NUM_PERIODS 32 ++#define NUM_SAMPLES 256 ++ ++static struct snd_pcm_hardware timbi2s_rx_hw = { ++ .info = (SNDRV_PCM_INFO_MMAP ++ | SNDRV_PCM_INFO_MMAP_VALID ++ | SNDRV_PCM_INFO_INTERLEAVED), ++ .formats = SNDRV_PCM_FMTBIT_S16_LE, ++ .rates = SNDRV_PCM_RATE_44100, ++ .rate_min = 44100, ++ .rate_max = 44100, ++ .channels_min = 2, /* only stereo */ ++ .channels_max = 2, ++ .buffer_bytes_max = NUM_PERIODS * SAMPLE_SIZE * NUM_SAMPLES, ++ .period_bytes_min = SAMPLE_SIZE * NUM_SAMPLES, ++ .period_bytes_max = SAMPLE_SIZE * NUM_SAMPLES, ++ .periods_min = NUM_PERIODS, ++ .periods_max = NUM_PERIODS, ++}; ++ ++static struct snd_pcm_hardware timbi2s_tx_hw = { ++ .info = (SNDRV_PCM_INFO_MMAP ++ | SNDRV_PCM_INFO_MMAP_VALID ++ | SNDRV_PCM_INFO_INTERLEAVED), ++ .formats = SNDRV_PCM_FMTBIT_S16_LE, ++ .rates = SNDRV_PCM_RATE_8000, ++ .rate_min = 8000, ++ .rate_max = 8000, ++ .channels_min = 2, /* only stereo */ ++ .channels_max = 2, ++ .buffer_bytes_max = NUM_PERIODS * SAMPLE_SIZE * NUM_SAMPLES, ++ .period_bytes_min = SAMPLE_SIZE * NUM_SAMPLES, ++ .period_bytes_max = SAMPLE_SIZE * NUM_SAMPLES, ++ .periods_min = NUM_PERIODS, ++ .periods_max = NUM_PERIODS, ++}; ++ ++static inline void timbi2s_bus_write(struct timbi2s_bus *bus, u32 val, u32 reg) ++{ ++ struct timbi2s *i2s = snd_pcm_chip(bus->card); ++ ++ iowrite32(val, i2s->membase + TIMBI2S_BUS_OFFSET(bus) + reg); ++} ++ ++static inline u32 timbi2s_bus_read(struct timbi2s_bus *bus, u32 reg) ++{ ++ struct timbi2s *i2s = snd_pcm_chip(bus->card); ++ ++ return ioread32(i2s->membase + TIMBI2S_BUS_OFFSET(bus) + reg); ++} ++ ++static u32 timbi2s_calc_prescale(u32 main_clk, u32 sample_rate) ++{ ++ u32 halfbit_rate = sample_rate * BITS_PER_CHANNEL * NUM_CHANNELS * 2; ++ return main_clk / halfbit_rate; ++} ++ ++static int timbi2s_open(struct snd_pcm_substream *substream) ++{ ++ struct timbi2s_bus *bus = snd_pcm_substream_chip(substream); ++ struct snd_card *card = bus->card; ++ struct snd_pcm_runtime *runtime = substream->runtime; ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Entry, substream: %p, bus: %d\n", __func__, substream, ++ BUS_INDEX(bus)); ++ ++ if (BUS_IS_RX(bus)) { ++ runtime->hw = timbi2s_rx_hw; ++ if (bus->sample_rate == 8000) { ++ runtime->hw.rates = SNDRV_PCM_RATE_8000; ++ runtime->hw.rate_min = 8000; ++ runtime->hw.rate_max = 8000; ++ } ++ } else ++ runtime->hw = timbi2s_tx_hw; ++ ++ bus->substream = substream; ++ ++ return 0; ++} ++ ++static int timbi2s_close(struct snd_pcm_substream *substream) ++{ ++ struct timbi2s_bus *bus = snd_pcm_substream_chip(substream); ++ struct snd_card *card = bus->card; ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Entry, substream: %p, bus: %d\n", __func__, substream, ++ BUS_INDEX(bus)); ++ ++ bus->substream = NULL; ++ ++ return 0; ++} ++ ++static int timbi2s_hw_params(struct snd_pcm_substream *substream, ++ struct snd_pcm_hw_params *hw_params) ++{ ++ struct timbi2s_bus *bus = snd_pcm_substream_chip(substream); ++ struct snd_card *card = bus->card; ++ struct timbi2s *i2s = snd_pcm_chip(card); ++ int err; ++ ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Entry, substream: %p, bus: %d\n", __func__, ++ substream, BUS_INDEX(bus)); ++ ++ bus->prescale = timbi2s_calc_prescale(i2s->main_clk, ++ params_rate(hw_params)); ++ ++ err = snd_pcm_lib_malloc_pages(substream, ++ params_buffer_bytes(hw_params)); ++ if (err < 0) ++ return err; ++ ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Rate: %d, format: %d\n", __func__, params_rate(hw_params), ++ params_format(hw_params)); ++ ++ return 0; ++} ++ ++static int timbi2s_hw_free(struct snd_pcm_substream *substream) ++{ ++ struct timbi2s_bus *bus = snd_pcm_substream_chip(substream); ++ struct snd_card *card = bus->card; ++ unsigned long flags; ++ ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Entry, substream: %p\n", __func__, substream); ++ ++ spin_lock_irqsave(&bus->lock, flags); ++ /* disable interrupts */ ++ timbi2s_bus_write(bus, 0, TIMBI2S_BUS_IER); ++ spin_unlock_irqrestore(&bus->lock, flags); ++ ++ /* disable TX and RX */ ++ timbi2s_bus_write(bus, TIMBI2S_ICOR_FIFO_RST | TIMBI2S_ICOR_SOFT_RST, ++ TIMBI2S_BUS_ICOR); ++ ++ return snd_pcm_lib_free_pages(substream); ++} ++ ++static int timbi2s_prepare(struct snd_pcm_substream *substream) ++{ ++ struct timbi2s_bus *bus = snd_pcm_substream_chip(substream); ++ struct snd_card *card = bus->card; ++ struct snd_pcm_runtime *runtime = substream->runtime; ++ u32 data; ++ ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Entry, substream: %p, bus: %d, buffer: %d, period: %d\n", ++ __func__, substream, ++ BUS_INDEX(bus), (int)snd_pcm_lib_buffer_bytes(substream), ++ (int)snd_pcm_lib_period_bytes(substream)); ++ ++ if (runtime->dma_addr & 3 || runtime->buffer_size & 3) { ++ dev_err(snd_card_get_device_link(card), ++ "%s: Only word aligned data allowed\n", __func__); ++ return -EINVAL; ++ } ++ ++ if (runtime->channels != NUM_CHANNELS) { ++ dev_err(snd_card_get_device_link(card), ++ "%s: Number of channels unsupported %d\n", __func__, ++ runtime->channels); ++ return -EINVAL; ++ } ++ ++ /* reset */ ++ timbi2s_bus_write(bus, TIMBI2S_ICOR_FIFO_RST | TIMBI2S_ICOR_SOFT_RST, ++ TIMBI2S_BUS_ICOR); ++ ++ /* only masters have prescaling, don't write if not needed */ ++ if (BUS_IS_MASTER(bus)) ++ timbi2s_bus_write(bus, bus->prescale, TIMBI2S_BUS_PRESCALE); ++ ++ /* write word select */ ++ data = ((BITS_PER_CHANNEL << TIMBI2S_ICOR_WORD_SEL_LEFT_SHIFT) & ++ TIMBI2S_ICOR_WORD_SEL_LEFT_MASK) | ++ ((BITS_PER_CHANNEL << TIMBI2S_ICOR_WORD_SEL_RIGHT_SHIFT) & ++ TIMBI2S_ICOR_WORD_SEL_RIGHT_MASK); ++ timbi2s_bus_write(bus, data, TIMBI2S_BUS_ICOR); ++ ++ bus->buf_pos = 0; ++ ++ return 0; ++} ++ ++static int ++timbi2s_playback_trigger(struct snd_pcm_substream *substream, int cmd) ++{ ++ struct timbi2s_bus *bus = snd_pcm_substream_chip(substream); ++ struct snd_card *card = bus->card; ++ unsigned long flags; ++ u32 data; ++ ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Entry, substream: %p, bus: %d, cmd: %d\n", __func__, ++ substream, BUS_INDEX(bus), cmd); ++ ++ switch (cmd) { ++ case SNDRV_PCM_TRIGGER_START: ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Got TRIGGER_START command\n", __func__); ++ ++ /* start */ ++ data = timbi2s_bus_read(bus, TIMBI2S_BUS_ICOR); ++ data |= TIMBI2S_ICOR_TX_ENABLE; ++ timbi2s_bus_write(bus, data, TIMBI2S_BUS_ICOR); ++ ++ /* enable interrupts */ ++ timbi2s_bus_write(bus, TIMBI2S_IRQ_TX_ALMOST_EMPTY, ++ TIMBI2S_BUS_IER); ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: ISR: %x, ICOR: %x\n", __func__, ++ timbi2s_bus_read(bus, TIMBI2S_BUS_ISR), ++ timbi2s_bus_read(bus, TIMBI2S_BUS_ICOR)); ++ break; ++ case SNDRV_PCM_TRIGGER_STOP: ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Got TRIGGER_STOP command\n", __func__); ++ ++ spin_lock_irqsave(&bus->lock, flags); ++ /* disable interrupts */ ++ timbi2s_bus_write(bus, 0, TIMBI2S_BUS_IER); ++ spin_unlock_irqrestore(&bus->lock, flags); ++ ++ /* reset */ ++ data = timbi2s_bus_read(bus, TIMBI2S_BUS_ICOR); ++ data &= ~TIMBI2S_ICOR_TX_ENABLE; ++ ++ timbi2s_bus_write(bus, data, TIMBI2S_BUS_ICOR); ++ break; ++ default: ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Got unsupported command\n", __func__); ++ ++ return -EINVAL; ++ } ++ ++ return 0; ++} ++ ++static int ++timbi2s_capture_trigger(struct snd_pcm_substream *substream, int cmd) ++{ ++ struct timbi2s_bus *bus = snd_pcm_substream_chip(substream); ++ struct snd_card *card = bus->card; ++ unsigned long flags; ++ ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Entry, substream: %p, bus: %d, cmd: %d\n", __func__, ++ substream, BUS_INDEX(bus), cmd); ++ ++ switch (cmd) { ++ case SNDRV_PCM_TRIGGER_START: ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Got TRIGGER_START command\n", __func__); ++ ++ timbi2s_bus_write(bus, TIMBI2S_ICOR_RX_ENABLE | ++ TIMBI2S_ICOR_FIFO_RST, TIMBI2S_BUS_ICOR); ++ ++ timbi2s_bus_write(bus, TIMBI2S_IRQ_RX_ALMOST_FULL, ++ TIMBI2S_BUS_IER); ++ break; ++ case SNDRV_PCM_TRIGGER_STOP: ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Got TRIGGER_STOP command\n", __func__); ++ /* disable interrupts */ ++ spin_lock_irqsave(&bus->lock, flags); ++ timbi2s_bus_write(bus, 0, TIMBI2S_BUS_IER); ++ spin_unlock_irqrestore(&bus->lock, flags); ++ /* Stop RX */ ++ timbi2s_bus_write(bus, 0, TIMBI2S_BUS_ICOR); ++ break; ++ default: ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Got unsupported command\n", __func__); ++ ++ return -EINVAL; ++ } ++ ++ return 0; ++} ++ ++static snd_pcm_uframes_t ++timbi2s_pointer(struct snd_pcm_substream *substream) ++{ ++ struct timbi2s_bus *bus = snd_pcm_substream_chip(substream); ++ struct snd_card *card = bus->card; ++ snd_pcm_uframes_t ret; ++ ++ dev_dbg(snd_card_get_device_link(card), ++ "%s: Entry, substream: %p\n", __func__, substream); ++ ++ ret = bytes_to_frames(substream->runtime, bus->buf_pos); ++ if (ret >= substream->runtime->buffer_size) ++ ret -= substream->runtime->buffer_size; ++ ++ return ret; ++} ++ ++static struct snd_pcm_ops timbi2s_playback_ops = { ++ .open = timbi2s_open, ++ .close = timbi2s_close, ++ .ioctl = snd_pcm_lib_ioctl, ++ .hw_params = timbi2s_hw_params, ++ .hw_free = timbi2s_hw_free, ++ .prepare = timbi2s_prepare, ++ .trigger = timbi2s_playback_trigger, ++ .pointer = timbi2s_pointer, ++}; ++ ++static struct snd_pcm_ops timbi2s_capture_ops = { ++ .open = timbi2s_open, ++ .close = timbi2s_close, ++ .ioctl = snd_pcm_lib_ioctl, ++ .hw_params = timbi2s_hw_params, ++ .hw_free = timbi2s_hw_free, ++ .prepare = timbi2s_prepare, ++ .trigger = timbi2s_capture_trigger, ++ .pointer = timbi2s_pointer, ++}; ++ ++static void timbi2s_irq_process_rx(struct timbi2s_bus *bus) ++{ ++ struct snd_pcm_runtime *runtime = bus->substream->runtime; ++ u32 buffer_size = snd_pcm_lib_buffer_bytes(bus->substream); ++ u32 ipr = timbi2s_bus_read(bus, TIMBI2S_BUS_IPR); ++ int i; ++ ++ dev_dbg(snd_card_get_device_link(bus->card), ++ "%s: Entry, bus: %d, IPR %x\n", __func__, BUS_INDEX(bus), ipr); ++ ++ for (i = 0; i < NUM_SAMPLES; i++) { ++ *(u32 *)(runtime->dma_area + bus->buf_pos) = ++ timbi2s_bus_read(bus, TIMBI2S_BUS_FIFO); ++ bus->buf_pos += SAMPLE_SIZE; ++ bus->buf_pos %= buffer_size; ++ } ++ ++ timbi2s_bus_write(bus, ipr, TIMBI2S_BUS_ICLR); ++ ++ /* inform ALSA that a period was received */ ++ snd_pcm_period_elapsed(bus->substream); ++} ++ ++static void timbi2s_irq_process_tx(struct timbi2s_bus *bus) ++{ ++ struct snd_pcm_runtime *runtime = bus->substream->runtime; ++ u32 buffer_size = snd_pcm_lib_buffer_bytes(bus->substream); ++ u32 ipr = timbi2s_bus_read(bus, TIMBI2S_BUS_IPR); ++ int i; ++ ++ dev_dbg(snd_card_get_device_link(bus->card), ++ "%s: Entry, bus: %d, IPR %x\n", __func__, BUS_INDEX(bus), ipr); ++ ++ for (i = 0; i < NUM_SAMPLES; i++) { ++ timbi2s_bus_write(bus, ++ *(u32 *)(runtime->dma_area + bus->buf_pos), ++ TIMBI2S_BUS_FIFO); ++ bus->buf_pos += SAMPLE_SIZE; ++ bus->buf_pos %= buffer_size; ++ } ++ ++ dev_dbg(snd_card_get_device_link(bus->card), "%s: ISR: %x, ICOR: %x\n", ++ __func__, timbi2s_bus_read(bus, TIMBI2S_BUS_ISR), ++ timbi2s_bus_read(bus, TIMBI2S_BUS_ICOR)); ++ ++ timbi2s_bus_write(bus, ipr, TIMBI2S_BUS_ICLR); ++ ++ /* inform ALSA that a period was received */ ++ snd_pcm_period_elapsed(bus->substream); ++} ++ ++static void timbi2s_tasklet(unsigned long arg) ++{ ++ struct snd_card *card = (struct snd_card *)arg; ++ struct timbi2s *i2s = snd_pcm_chip(card); ++ u32 uir = ioread32(i2s->membase + TIMBI2S_REG_UIR); ++ unsigned i; ++ ++ dev_dbg(snd_card_get_device_link(card), "%s: Entry, UIR %x\n", ++ __func__, uir); ++ ++ for (i = 0; i < i2s->num_busses; i++) ++ if (uir & (1 << i)) { ++ struct timbi2s_bus *bus = i2s->busses + i; ++ if (BUS_IS_RX(bus)) ++ timbi2s_irq_process_rx(bus); ++ else ++ timbi2s_irq_process_tx(bus); ++ } ++ ++ enable_irq(i2s->irq); ++} ++ ++static irqreturn_t timbi2s_irq(int irq, void *devid) ++{ ++ struct timbi2s *i2s = devid; ++ ++ tasklet_schedule(&i2s->tasklet); ++ disable_irq_nosync(i2s->irq); ++ ++ return IRQ_HANDLED; ++} ++ ++static int timbi2s_setup_busses(struct snd_card *card, ++ struct platform_device *pdev) ++{ ++ const struct timbi2s_platform_data *pdata = pdev->dev.platform_data; ++ unsigned i; ++ ++ dev_dbg(&pdev->dev, "%s: Entry, no busses: %d, busses: %p\n", __func__, ++ pdata->num_busses, pdata->busses); ++ ++ for (i = 0; i < pdata->num_busses; i++) { ++ int capture = pdata->busses[i].rx; ++ int err; ++ u32 ctl; ++ struct timbi2s *i2s = snd_pcm_chip(card); ++ struct timbi2s_bus *bus = i2s->busses + i; ++ ++ dev_dbg(&pdev->dev, "%s: Setting up bus: %d\n", __func__, i); ++ ++ SET_BUS_INDEX(bus, i); ++ bus->sample_rate = pdata->busses[i].sample_rate; ++ bus->card = card; ++ /* prescaling only applies to master busses, we use the ++ * knowledge of that to identify the direction later ++ * eg, bus->prescale != 0 -> master bus ++ */ ++ if (capture) ++ SET_BUS_RX(bus); ++ ++ spin_lock_init(&bus->lock); ++ ++ if (bus->sample_rate != 44100 && bus->sample_rate != 8000) { ++ dev_err(&pdev->dev, ++ "Unsupported bitrate: %d\n", bus->sample_rate); ++ return -EINVAL; ++ } ++ ++ dev_dbg(&pdev->dev, "%s: Will check HW direction on bus: %d\n", ++ __func__, BUS_INDEX(bus)); ++ ++ /* check that the HW agrees with the direction */ ++ ctl = timbi2s_bus_read(bus, TIMBI2S_BUS_ICOR); ++ if ((capture && !(ctl & TIMBI2S_ICOR_RX_ID)) || ++ (!capture && !(ctl & TIMBI2S_ICOR_TX_ID))) { ++ dev_dbg(&pdev->dev, ++ "HW and platform data disagree on direction\n"); ++ return -EINVAL; ++ } ++ ++ dev_dbg(&pdev->dev, "%s: Will create PCM channel for bus: %d\n", ++ __func__, BUS_INDEX(bus)); ++ err = snd_pcm_new(card, card->shortname, i, !capture, ++ capture, &bus->pcm); ++ if (err) { ++ dev_dbg(&pdev->dev, "%s, Failed to create pcm: %d\n", ++ __func__, err); ++ return err; ++ } ++ ++ if (capture) ++ snd_pcm_set_ops(bus->pcm, SNDRV_PCM_STREAM_CAPTURE, ++ &timbi2s_capture_ops); ++ if (!capture) ++ snd_pcm_set_ops(bus->pcm, SNDRV_PCM_STREAM_PLAYBACK, ++ &timbi2s_playback_ops); ++ ++ dev_dbg(&pdev->dev, "%s: Will preallocate buffers to bus: %d\n", ++ __func__, BUS_INDEX(bus)); ++ ++ err = snd_pcm_lib_preallocate_pages_for_all(bus->pcm, ++ SNDRV_DMA_TYPE_CONTINUOUS, ++ snd_dma_continuous_data(GFP_KERNEL), ++ NUM_SAMPLES * NUM_PERIODS * SAMPLE_SIZE * 2, ++ NUM_SAMPLES * NUM_PERIODS * SAMPLE_SIZE * 2); ++ if (err) { ++ dev_dbg(&pdev->dev, "%s, Failed to create pcm: %d\n", ++ __func__, err); ++ ++ return err; ++ } ++ ++ bus->pcm->private_data = bus; ++ bus->pcm->info_flags = 0; ++ strcpy(bus->pcm->name, card->shortname); ++ i2s->num_busses++; ++ } ++ ++ return 0; ++} ++ ++static int __devinit timbi2s_probe(struct platform_device *pdev) ++{ ++ int err; ++ int irq; ++ struct timbi2s *i2s; ++ struct resource *iomem; ++ const struct timbi2s_platform_data *pdata = pdev->dev.platform_data; ++ struct snd_card *card; ++ u32 ver; ++ ++ if (!pdata) { ++ err = -ENODEV; ++ goto out; ++ } ++ ++ if (pdata->num_busses > MAX_BUSSES) { ++ err = -EINVAL; ++ goto out; ++ } ++ ++ iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0); ++ if (!iomem) { ++ err = -ENODEV; ++ goto out; ++ } ++ ++ irq = platform_get_irq(pdev, 0); ++ if (irq < 0) { ++ err = -ENODEV; ++ goto out; ++ } ++ ++ err = snd_card_create(SNDRV_DEFAULT_IDX1, SNDRV_DEFAULT_STR1, ++ THIS_MODULE, sizeof(struct timbi2s) + ++ sizeof(struct timbi2s_bus) * pdata->num_busses, &card); ++ if (err) ++ goto out; ++ ++ strcpy(card->driver, "Timberdale I2S"); ++ strcpy(card->shortname, "Timberdale I2S"); ++ sprintf(card->longname, "Timberdale I2S Driver"); ++ ++ snd_card_set_dev(card, &pdev->dev); ++ ++ i2s = snd_pcm_chip(card); ++ ++ if (!request_mem_region(iomem->start, resource_size(iomem), ++ DRIVER_NAME)) { ++ err = -EBUSY; ++ goto err_region; ++ } ++ ++ i2s->membase = ioremap(iomem->start, resource_size(iomem)); ++ if (!i2s->membase) { ++ err = -ENOMEM; ++ goto err_ioremap; ++ } ++ ++ err = timbi2s_setup_busses(card, pdev); ++ if (err) ++ goto err_setup; ++ ++ tasklet_init(&i2s->tasklet, timbi2s_tasklet, (unsigned long)card); ++ i2s->irq = irq; ++ i2s->main_clk = pdata->main_clk; ++ ++ err = request_irq(irq, timbi2s_irq, 0, DRIVER_NAME, i2s); ++ if (err) ++ goto err_request_irq; ++ ++ err = snd_card_register(card); ++ if (err) ++ goto err_register; ++ ++ platform_set_drvdata(pdev, card); ++ ++ ver = ioread32(i2s->membase + TIMBI2S_REG_VER); ++ ++ printk(KERN_INFO ++ "Driver for Timberdale I2S (ver: %d.%d) successfully probed.\n", ++ ver >> 16 , ver & 0xffff); ++ ++ return 0; ++ ++err_register: ++ free_irq(irq, card); ++err_request_irq: ++err_setup: ++ iounmap(i2s->membase); ++err_ioremap: ++ release_mem_region(iomem->start, resource_size(iomem)); ++err_region: ++ snd_card_free(card); ++out: ++ printk(KERN_ERR DRIVER_NAME": Failed to register: %d\n", err); ++ ++ return err; ++} ++ ++static int __devexit timbi2s_remove(struct platform_device *pdev) ++{ ++ struct snd_card *card = platform_get_drvdata(pdev); ++ struct timbi2s *i2s = snd_pcm_chip(card); ++ struct resource *iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0); ++ ++ tasklet_kill(&i2s->tasklet); ++ free_irq(i2s->irq, i2s); ++ ++ iounmap(i2s->membase); ++ release_mem_region(iomem->start, resource_size(iomem)); ++ snd_card_free(card); ++ ++ platform_set_drvdata(pdev, 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_ALIAS("platform:"DRIVER_NAME); ++MODULE_DESCRIPTION("Timberdale I2S bus driver"); ++MODULE_AUTHOR("Mocean Laboratories "); ++MODULE_LICENSE("GPL v2"); -- cgit v1.2.3