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");