Hi Javier,
On Wednesday 04 May 2011 09:33:49 javier Martin wrote:
> Hi,
> for those interested on mt9p031 working on the Beagleboard xM. I
> attach 2 patches here that must be applied to kernel-2.6.39-rc commit
> e8dad69408a9812d6bb42d03e74d2c314534a4fa
> These patches include a fix for the USB ethernet.
>
> What currently works:
> - Test suggested by Guennadi
> (http://download.open-technology.de/BeagleBoard_xM-MT9P031/).
>
> Known problems:
> 1. You might be required to create device node for the sensor manually:
>
> mknod /dev/v4l-subdev8 c 81 15
> chown root:video /dev/v4l-subdev8
>
> 2. Images captured seem to be too dull and dark. Values of pixels seem
> always to low, it seems to me like MSB of each pixel were stuck at 0.
> I hope someone can help here.
I've inline the patches for easier review, please send the inline next time.
First 0001-mt9p031.patch. As a general rule, please try to split your patches
properly. This one contains several unrelated changes. For instance the
drivers/media/video/Makefile modification should go in the patch that adds the
mt9p031 driver code.
> diff --git a/arch/arm/mach-omap2/board-omap3beagle.c b/arch/arm/mach-
omap2/board-omap3beagle.c
> index 33007fd..eba2235 100644
> --- a/arch/arm/mach-omap2/board-omap3beagle.c
> +++ b/arch/arm/mach-omap2/board-omap3beagle.c
[snip]
> @@ -273,6 +274,44 @@ static struct regulator_consumer_supply
> beagle_vsim_supply = {
>
> static struct gpio_led gpio_leds[];
>
> +static struct regulator_consumer_supply beagle_vaux3_supply = {
> + .supply = "cam_1v8",
> +};
> +
> +static struct regulator_consumer_supply beagle_vaux4_supply = {
> + .supply = "cam_2v8",
That's a weird name for a supply that is connected to a 1.8V regulator output.
What about renaming the supplies cam_dig and cam_io ?
> +};
[snip]
> @@ -309,6 +348,15 @@ static int beagle_twl_gpio_setup(struct device *dev,
> pr_err("%s: unable to configure EHCI_nOC\n", __func__);
> }
>
> + if (omap3_beagle_get_rev() == OMAP3BEAGLE_BOARD_XM) {
> + /*
> + * Power on camera interface - only on pre-production, not
> + * needed on production boards
> + */
> + gpio_request(gpio + 2, "CAM_EN");
> + gpio_direction_output(gpio + 2, 1);
There's already similar code in 2.6.39-rc6 (but the GPIO is called
DVI_LDO_EN).
> + }
> +
> /*
> * TWL4030_GPIO_MAX + 0 == ledA, EHCI nEN_USB_PWR (out, XM active
> * high / others active low)
[snip]
> @@ -472,6 +522,7 @@ static int __init omap3_beagle_i2c_init(void)
> {
> omap_register_i2c_bus(1, 2600, beagle_i2c_boardinfo,
> ARRAY_SIZE(beagle_i2c_boardinfo));
> +// omap_register_i2c_bus(2, 100, NULL, 0);
Please don't add commented out code.
> /* Bus 3 is attached to the DVI port where devices like the pico DLP
> * projector don't work reliably with 400kHz */
> omap_register_i2c_bus(3, 100, beagle_i2c_eeprom,
> ARRAY_SIZE(beagle_i2c_eeprom));
> @@ -598,6 +649,34 @@ static const struct usbhs_omap_board_data usbhs_bdata
__initconst = {
>
> #ifdef CONFIG_OMAP_MUX
> static struct omap_board_mux board_mux[] __initdata = {
> +// #if 0
Same here.
Does the boot loader leave the camera interface unconfigured ?
> + /* Camera - Parallel Data */
> + OMAP3_MUX(CAM_D0, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_D1, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_D2, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_D3, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_D4, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_D5, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_D6, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_D7, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_D8, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_D9, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_D10, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_D11, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_PCLK, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> +
> + /* Camera - HS/VS signals */
> + OMAP3_MUX(CAM_HS, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
> + OMAP3_MUX(CAM_VS, OMAP_MUX_MODE0 | OMAP_PIN_INPUT),
What about pull-ups ont the HS/VS and PCLK signals ?
> +
> + /* Camera - Reset GPIO 98 */
> + OMAP3_MUX(CAM_FLD, OMAP_MUX_MODE4 | OMAP_PIN_OUTPUT),
> +
> + /* Camera - XCLK */
> + OMAP3_MUX(CAM_XCLKA, OMAP_MUX_MODE0 | OMAP_PIN_OUTPUT),
> +// #endif
> +// OMAP3_MUX(I2C2_SCL, OMAP_MUX_MODE0 | OMAP_PIN_OUTPUT),
> +// OMAP3_MUX(I2C2_SDA, OMAP_MUX_MODE0 | OMAP_PIN_OFF_INPUT_PULLUP |
> OMAP_PIN_INPUT_PULLUP),
> { .reg_offset = OMAP_MUX_TERMINATOR },
> };
> #endif
> @@ -656,6 +735,8 @@ static void __init beagle_opp_init(void)
>
> static void __init omap3_beagle_init(void)
> {
> +// u32 ctrl;
> +
No commented code.
> omap3_mux_init(board_mux, OMAP_PACKAGE_CBB);
> omap3_beagle_init_rev();
> omap3_beagle_i2c_init();
> @@ -679,6 +760,8 @@ static void __init omap3_beagle_init(void)
>
> beagle_display_init();
> beagle_opp_init();
> +// ctrl = omap_ctrl_readl(OMAP343X_CONTROL_PROG_IO1);
> +// omap_ctrl_writel(ctrl & 0xFFFFFFFE, OMAP343X_CONTROL_PROG_IO1);
Same here.
> }
>
> MACHINE_START(OMAP3_BEAGLE, "OMAP3 Beagle Board")
> diff --git a/arch/arm/mach-omap2/devices.c b/arch/arm/mach-omap2/devices.c
> index 7b85585..ae5ba25 100644
> --- a/arch/arm/mach-omap2/devices.c
> +++ b/arch/arm/mach-omap2/devices.c
> @@ -200,7 +200,7 @@ static struct resource omap3isp_resources[] = {
> }
> };
>
> -static struct platform_device omap3isp_device = {
> +struct platform_device omap3isp_device = {
> .name = "omap3isp",
> .id = -1,
> .num_resources = ARRAY_SIZE(omap3isp_resources),
> diff --git a/arch/arm/plat-omap/iommu.c b/arch/arm/plat-omap/iommu.c
> index 8a51fd5..cdc161e 100644
> --- a/arch/arm/plat-omap/iommu.c
> +++ b/arch/arm/plat-omap/iommu.c
> @@ -793,7 +793,9 @@ static irqreturn_t iommu_fault_handler(int irq, void
*data)
> clk_enable(obj->clk);
> errs = iommu_report_fault(obj, &da);
> clk_disable(obj->clk);
> -
> + if (errs == 0)
> + return IRQ_HANDLED;
> +
That's a separate patch.
> /* Fault callback or TLB/PTE Dynamic loading */
> if (obj->isr && !obj->isr(obj, da, errs, obj->isr_priv))
> return IRQ_HANDLED;
> diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
> index 00f51dd..32df8bd 100644
> --- a/drivers/media/video/Kconfig
> +++ b/drivers/media/video/Kconfig
> @@ -329,6 +329,14 @@ config VIDEO_OV7670
> OV7670 VGA camera. It currently only works with the M88ALP01
> controller.
>
> +config VIDEO_MT9P031
> + tristate "Micron MT9P031 support"
> + depends on I2C && VIDEO_V4L2
and VIDEO_V4L2_SUBDEV_API
> + ---help---
> + This driver supports MT9P031 cameras from Micron
> + This is a Video4Linux2 sensor-level driver for the Micron
> + mt0p031 5 Mpixel camera.
> +
We should s/Micron/Aptina/ at some point.
> config VIDEO_MT9V011
> tristate "Micron mt9v011 sensor support"
> depends on I2C && VIDEO_V4L2
[snip]
> diff --git a/drivers/media/video/omap3isp/isp.c
> b/drivers/media/video/omap3isp/isp.c
> index 472a693..2637193 100644
> --- a/drivers/media/video/omap3isp/isp.c
> +++ b/drivers/media/video/omap3isp/isp.c
> @@ -177,6 +177,8 @@ static void isp_disable_interrupts(struct isp_device
*isp)
> isp_reg_writel(isp, 0, OMAP3_ISP_IOMEM_MAIN, ISP_IRQ0ENABLE);
> }
>
> +static int isp_enable_clocks(struct isp_device *isp);
> +
> /**
> * isp_set_xclk - Configures the specified cam_xclk to the desired
frequency.
> * @isp: OMAP3 ISP device
> @@ -239,7 +241,7 @@ static u32 isp_set_xclk(struct isp_device *isp, u32
xclk, u8 xclksel)
>
> /* Do we go from stable whatever to clock? */
> if (divisor >= 2 && isp->xclk_divisor[xclksel - 1] < 2)
> - omap3isp_get(isp);
> + isp_enable_clocks(isp);
> /* Stopping the clock. */
> else if (divisor < 2 && isp->xclk_divisor[xclksel - 1] >= 2)
> omap3isp_put(isp);
That's a separate patch (and under discussion).
> diff --git a/drivers/media/video/omap3isp/isp.h
b/drivers/media/video/omap3isp/isp.h
> index 2620c40..f455d80 100644
> --- a/drivers/media/video/omap3isp/isp.h
> +++ b/drivers/media/video/omap3isp/isp.h
> @@ -148,6 +148,7 @@ struct isp_parallel_platform_data {
> unsigned int data_lane_shift:2;
> unsigned int clk_pol:1;
> unsigned int bridge:4;
> + unsigned int data_bus_width;
> };
>
> /**
> diff --git a/drivers/media/video/omap3isp/ispccdc.c
b/drivers/media/video/omap3isp/ispccdc.c
> index 39d501b..81bd9aa 100644
> --- a/drivers/media/video/omap3isp/ispccdc.c
> +++ b/drivers/media/video/omap3isp/ispccdc.c
> @@ -2253,6 +2253,9 @@ int omap3isp_ccdc_init(struct isp_device *isp)
> ccdc->syncif.ccdc_mastermode = 0;
> ccdc->syncif.datapol = 0;
> ccdc->syncif.datsz = 0;
> + if (isp->pdata->subdevs->interface == ISP_INTERFACE_PARALLEL
> + && isp->pdata->subdevs->bus.parallel.data_bus_width != 0)
> + ccdc->syncif.datsz = isp->pdata->subdevs->bus.parallel.data_bus_width;
This should be handled automatically based on the format at the sensor output
pad if you're using the latest ISP driver. Have you tried it ?
> ccdc->syncif.fldmode = 0;
> ccdc->syncif.fldout = 0;
> ccdc->syncif.fldpol = 0;
> @@ -2261,7 +2264,7 @@ int omap3isp_ccdc_init(struct isp_device *isp)
> ccdc->syncif.vdpol = 0;
>
> ccdc->clamp.oblen = 0;
> - ccdc->clamp.dcsubval = 0;
> + ccdc->clamp.dcsubval = (ccdc->syncif.datsz == 8) ? 0 : 64;
This should be set by userspace.
> ccdc->vpcfg.pixelclk = 0;
>
> diff --git a/drivers/mfd/omap-usb-host.c b/drivers/mfd/omap-usb-host.c
> index 53450f4..491cac5 100644
> --- a/drivers/mfd/omap-usb-host.c
> +++ b/drivers/mfd/omap-usb-host.c
> @@ -719,14 +719,14 @@ static int usbhs_enable(struct device *dev)
> gpio_request(pdata->ehci_data->reset_gpio_port[0],
> "USB1 PHY reset");
> gpio_direction_output
> - (pdata->ehci_data->reset_gpio_port[0], 1);
> + (pdata->ehci_data->reset_gpio_port[0], 0);
> }
>
> if (gpio_is_valid(pdata->ehci_data->reset_gpio_port[1])) {
> gpio_request(pdata->ehci_data->reset_gpio_port[1],
> "USB2 PHY reset");
> gpio_direction_output
> - (pdata->ehci_data->reset_gpio_port[1], 1);
> + (pdata->ehci_data->reset_gpio_port[1], 0);
> }
>
> /* Hold the PHY in RESET for enough time till DIR is high */
> @@ -906,11 +906,11 @@ static int usbhs_enable(struct device *dev)
>
> if (gpio_is_valid(pdata->ehci_data->reset_gpio_port[0]))
> gpio_set_value
> - (pdata->ehci_data->reset_gpio_port[0], 0);
> + (pdata->ehci_data->reset_gpio_port[0], 1);
>
> if (gpio_is_valid(pdata->ehci_data->reset_gpio_port[1]))
> gpio_set_value
> - (pdata->ehci_data->reset_gpio_port[1], 0);
> + (pdata->ehci_data->reset_gpio_port[1], 1);
> }
>
> end_count:
This is a separate patch.
> diff --git a/include/media/v4l2-chip-ident.h b/include/media/v4l2-chip-
ident.h
> index b3edb67..97919f2 100644
> --- a/include/media/v4l2-chip-ident.h
> +++ b/include/media/v4l2-chip-ident.h
> @@ -297,6 +297,7 @@ enum {
> V4L2_IDENT_MT9T112 = 45022,
> V4L2_IDENT_MT9V111 = 45031,
> V4L2_IDENT_MT9V112 = 45032,
> + V4L2_IDENT_MT9P031 = 45033,
>
> /* HV7131R CMOS sensor: just ident 46000 */
> V4L2_IDENT_HV7131R = 46000,
There's no need to a V4L2_IDENT when using the media controller and subdev
device nodes.
Review of 0002-mt9p031.patch will follow in another e-mail.
new file mode 100644
@@ -0,0 +1,158 @@
+/*
+ * arch/arm/mach-omap2/board-rx51-camera.c
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ *
+ * Contact: Sakari Ailus <sakari.ailus@nokia.com>
+ * Tuukka Toivonen <tuukka.o.toivonen@nokia.com>
+ *
+ * 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., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/i2c.h>
+#include <linux/i2c/twl.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+
+#include <media/mt9p031.h>
+
+#include <plat/i2c.h>
+
+#include <asm/gpio.h>
+//#include <plat/control.h>
+
+#include "../../../drivers/media/video/omap3isp/isp.h"
+#include "../../../drivers/media/video/omap3isp/ispreg.h"
+
+#include "devices.h"
+
+#define MT9P031_XCLK ISP_XCLK_A
+#if 0
+static int beagle_configure_interface(struct v4l2_subdev *subdev,
+ int width, int height)
+{
+ struct isp_device *isp = v4l2_dev_to_isp_device(subdev->v4l2_dev);
+
+ isp_set_pixel_clock(isp, 0);
+
+ return 0;
+}
+
+static int beagle_set_power(struct v4l2_subdev *subdev, int on)
+{
+ return 0;
+}
+#endif
+
+extern struct platform_device omap3isp_device;
+
+static int beagle_set_xclk(struct v4l2_subdev *subdev, int hz)
+{
+ struct isp_device *isp = platform_get_drvdata(&omap3isp_device);
+// struct isp_device *isp = v4l2_dev_to_isp_device(subdev->v4l2_dev);
+ int ret;
+// static int current_hz;
+
+// if (hz != current_hz) {
+// current_hz = hz;
+ ret = isp->platform_cb.set_xclk(isp, hz, MT9P031_XCLK);
+ pr_info("%s(): set_xclk() = %d\n", __func__, ret);
+// }
+
+ return 0;
+}
+
+#define MT9P031_RESET_GPIO 98
+#define MT9P031_DATA_SHIFT 2 /* 0: 12-bit, 1: 10-bit, 2: 8-bit */
+
+static int beagle_reset(struct v4l2_subdev *subdev, int active)
+{
+ /* Set RESET_BAR to !active */
+
+ pr_info("%s(%d)\n", __func__, active);
+ gpio_set_value(MT9P031_RESET_GPIO, !active);
+
+ return 0;
+}
+
+static struct mt9p031_platform_data beagle_mt9p031_platform_data = {
+// .configure_interface = beagle_configure_interface,
+ .set_xclk = beagle_set_xclk,
+ .reset = beagle_reset,
+ .data_shift = MT9P031_DATA_SHIFT,
+// .set_power = beagle_set_power,
+};
+
+static struct i2c_board_info mt9p031_camera_i2c_device = {
+ I2C_BOARD_INFO("mt9p031", 0x48),
+// I2C_BOARD_INFO("mt9p031", 0x5d),
+ .platform_data = &beagle_mt9p031_platform_data,
+};
+
+static struct isp_subdev_i2c_board_info mt9p031_camera_subdevs[] = {
+ {
+ .board_info = &mt9p031_camera_i2c_device,
+ .i2c_adapter_id = 2,
+ },
+ { NULL, 0, },
+};
+
+static struct isp_v4l2_subdevs_group beagle_camera_subdevs[] = {
+ {
+ .subdevs = mt9p031_camera_subdevs,
+ .interface = ISP_INTERFACE_PARALLEL,
+ .bus = {
+ .parallel = {
+ .data_lane_shift = MT9P031_DATA_SHIFT,
+ .clk_pol = 0,
+ .bridge = ISPCTRL_PAR_BRIDGE_DISABLE,
+ .data_bus_width = 8,
+ }
+ },
+ },
+ { },
+};
+
+static struct isp_platform_data beagle_isp_platform_data = {
+ .subdevs = beagle_camera_subdevs,
+};
+
+static int __init beagle_camera_init(void)
+{
+ int ret = gpio_request(MT9P031_RESET_GPIO, "cam_rst");
+ printk("%s: 1\n", __func__);
+ if (ret < 0)
+ return ret;
+ printk("%s: 2\n", __func__);
+ gpio_direction_output(MT9P031_RESET_GPIO, 0);
+
+ omap3isp_device.dev.platform_data = &beagle_isp_platform_data;
+ omap_register_i2c_bus(2, 100, NULL, 0);
+ printk("%s: 3\n", __func__);
+ return platform_device_register(&omap3isp_device);
+}
+
+static void __exit beagle_camera_exit(void)
+{
+ platform_device_unregister(&omap3isp_device);
+}
+
+module_init(beagle_camera_init);
+module_exit(beagle_camera_exit);
+
+MODULE_LICENSE("GPL v2");
new file mode 100644
@@ -0,0 +1,884 @@
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+#include <linux/pm.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <media/v4l2-subdev.h>
+#include <linux/videodev2.h>
+
+#include <media/mt9p031.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+
+/*
+ * mt9p031 i2c address 0x5d (0xBA read, 0xBB write) same as mt9t031
+ * The platform has to define i2c_board_info and link to it from
+ * struct soc_camera_link
+ */
+
+/* mt9p031 selected register addresses */
+#define MT9P031_CHIP_VERSION 0x00
+#define MT9P031_ROW_START 0x01
+#define MT9P031_COLUMN_START 0x02
+#define MT9P031_WINDOW_HEIGHT 0x03
+#define MT9P031_WINDOW_WIDTH 0x04
+#define MT9P031_HORIZONTAL_BLANKING 0x05
+#define MT9P031_VERTICAL_BLANKING 0x06
+#define MT9P031_OUTPUT_CONTROL 0x07
+#define MT9P031_SHUTTER_WIDTH_UPPER 0x08
+#define MT9P031_SHUTTER_WIDTH 0x09
+#define MT9P031_PIXEL_CLOCK_CONTROL 0x0a
+#define MT9P031_FRAME_RESTART 0x0b
+#define MT9P031_SHUTTER_DELAY 0x0c
+#define MT9P031_RESET 0x0d
+#define MT9P031_READ_MODE_1 0x1e
+#define MT9P031_READ_MODE_2 0x20
+//#define MT9T031_READ_MODE_3 0x21 // NA readmode_2 is 2 bytes
+#define MT9P031_ROW_ADDRESS_MODE 0x22
+#define MT9P031_COLUMN_ADDRESS_MODE 0x23
+#define MT9P031_GLOBAL_GAIN 0x35
+//#define MT9T031_CHIP_ENABLE 0xF8 // PDN is pin signal. no i2c register
+
+#define MT9P031_MAX_HEIGHT 1944
+#define MT9P031_MAX_WIDTH 2592
+#define MT9P031_MIN_HEIGHT 2
+#define MT9P031_MIN_WIDTH 18
+#define MT9P031_HORIZONTAL_BLANK 0
+#define MT9P031_VERTICAL_BLANK 25
+#define MT9P031_COLUMN_SKIP 16
+#define MT9P031_ROW_SKIP 54
+
+#define MT9P031_CHIP_VERSION_VALUE 0x1801
+
+/*
+#define MT9T031_BUS_PARAM (SOCAM_PCLK_SAMPLE_RISING | \
+ SOCAM_PCLK_SAMPLE_FALLING | SOCAM_HSYNC_ACTIVE_HIGH | \
+ SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_DATA_ACTIVE_HIGH | \
+ SOCAM_MASTER | SOCAM_DATAWIDTH_10)
+*/
+struct mt9p031 {
+ struct v4l2_subdev subdev;
+ struct media_pad pad;
+
+ struct v4l2_rect rect; /* Sensor window */
+ struct v4l2_mbus_framefmt format;
+ int model; /* V4L2_IDENT_MT9P031* codes from v4l2-chip-ident.h */
+ u16 xskip;
+ u16 yskip;
+ struct regulator *reg_1v8, *reg_2v8;
+};
+
+static struct mt9p031 *to_mt9p031(const struct i2c_client *client)
+{
+ return container_of(i2c_get_clientdata(client), struct mt9p031, subdev);
+}
+
+static int reg_read(struct i2c_client *client, const u8 reg)
+{
+ s32 data = i2c_smbus_read_word_data(client, reg);
+ return data < 0 ? data : swab16(data);
+}
+
+static int reg_write(struct i2c_client *client, const u8 reg,
+ const u16 data)
+{
+ return i2c_smbus_write_word_data(client, reg, swab16(data));
+}
+
+static int reg_set(struct i2c_client *client, const u8 reg,
+ const u16 data)
+{
+ int ret;
+
+ ret = reg_read(client, reg);
+ if (ret < 0)
+ return ret;
+ return reg_write(client, reg, ret | data);
+}
+
+static int reg_clear(struct i2c_client *client, const u8 reg,
+ const u16 data)
+{
+ int ret;
+
+ ret = reg_read(client, reg);
+ if (ret < 0)
+ return ret;
+ return reg_write(client, reg, ret & ~data);
+}
+
+static int set_shutter(struct i2c_client *client, const u32 data)
+{
+ int ret;
+
+ ret = reg_write(client, MT9P031_SHUTTER_WIDTH_UPPER, data >> 16);
+
+ if (ret >= 0)
+ ret = reg_write(client, MT9P031_SHUTTER_WIDTH, data & 0xffff);
+
+ return ret;
+}
+
+static int get_shutter(struct i2c_client *client, u32 *data)
+{
+ int ret;
+
+ ret = reg_read(client, MT9P031_SHUTTER_WIDTH_UPPER);
+ *data = ret << 16;
+
+ if (ret >= 0)
+ ret = reg_read(client, MT9P031_SHUTTER_WIDTH);
+ *data |= ret & 0xffff;
+
+ return ret < 0 ? ret : 0;
+}
+
+static int mt9p031_idle(struct i2c_client *client)
+{
+ int ret;
+
+ /* Disable chip output, synchronous option update */
+ ret = reg_write(client, MT9P031_RESET, 1);
+ if (!ret)
+ ret = reg_write(client, MT9P031_RESET, 0);
+ if (!ret)
+ ret = reg_clear(client, MT9P031_OUTPUT_CONTROL, 2);
+
+ return ret >= 0 ? 0 : -EIO;
+}
+
+static int mt9p031_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
+
+ if (code->pad || code->index)
+ return -EINVAL;
+
+ code->code = mt9p031->format.code;
+
+ return 0;
+}
+#if 0
+static int mt9p031_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ pr_info("%s()\n", __func__);
+ return 0;
+}
+#endif
+static struct v4l2_mbus_framefmt *mt9p031_get_pad_format(struct mt9p031 *mt9p031,
+ struct v4l2_subdev_fh *fh, unsigned int pad, u32 which)
+{
+ switch (which) {
+ case V4L2_SUBDEV_FORMAT_TRY:
+ return v4l2_subdev_get_try_format(fh, pad);
+ case V4L2_SUBDEV_FORMAT_ACTIVE:
+ return &mt9p031->format;
+ default:
+ return NULL;
+ }
+}
+
+static struct v4l2_rect *mt9p031_get_pad_crop(struct mt9p031 *mt9p031,
+ struct v4l2_subdev_fh *fh, unsigned int pad, u32 which)
+{
+ switch (which) {
+ case V4L2_SUBDEV_FORMAT_TRY:
+ return v4l2_subdev_get_try_crop(fh, pad);
+ case V4L2_SUBDEV_FORMAT_ACTIVE:
+ return &mt9p031->rect;
+ default:
+ return NULL;
+ }
+}
+
+static int mt9p031_get_crop(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh,
+ struct v4l2_subdev_crop *crop)
+{
+ struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
+ struct v4l2_rect *rect = mt9p031_get_pad_crop(mt9p031, fh, crop->pad,
+ crop->which);
+ pr_info("%s()\n", __func__);
+
+ if (!rect)
+ return -EINVAL;
+
+ crop->rect = *rect;
+
+ return 0;
+}
+
+static u16 mt9p031_skip_for_crop(s32 source, s32 *target, s32 max_skip)
+{
+ unsigned int skip;
+
+ if (source - source / 4 < *target) {
+ *target = source;
+ return 1;
+ }
+
+ skip = (source + *target / 2) / *target;
+ if (skip > max_skip)
+ skip = max_skip;
+
+ *target = 2 * ((source + 2 * skip - 1) / (2 * skip));
+
+ return skip;
+}
+
+static int mt9p031_set_params(struct i2c_client *client,
+ struct v4l2_rect *rect, u16 xskip, u16 yskip)
+{
+ struct mt9p031 *mt9p031 = to_mt9p031(client);
+ int ret;
+ u16 xbin, ybin;
+ const u16 hblank = MT9P031_HORIZONTAL_BLANK,
+ vblank = MT9P031_VERTICAL_BLANK;
+
+ /*
+ * TODO: Attention! When implementing horizontal flipping, adjust
+ * alignment according to R2 "Column Start" description in the datasheet
+ */
+ if (xskip & 1) {
+ xbin = 1;
+ rect->left &= ~3;
+ } else if (xskip & 2) {
+ xbin = 2;
+ rect->left &= ~7;
+ } else {
+ xbin = 4;
+ rect->left &= ~15;
+ }
+
+ ybin = min(yskip, (u16)4);
+
+ rect->top &= ~1;
+
+ /* Disable register update, reconfigure atomically */
+ ret = reg_set(client, MT9P031_OUTPUT_CONTROL, 1);
+ if (ret < 0)
+ return ret;
+
+ dev_dbg(&client->dev, "skip %u:%u, rect %ux%u@%u:%u\n",
+ xskip, yskip, rect->width, rect->height, rect->left, rect->top);
+
+ /* Blanking and start values - default... */
+ ret = reg_write(client, MT9P031_HORIZONTAL_BLANKING, hblank);
+ if (!ret)
+ ret = reg_write(client, MT9P031_VERTICAL_BLANKING, vblank);
+
+ if (yskip != mt9p031->yskip || xskip != mt9p031->xskip) {
+ /* Binning, skipping */
+ if (!ret)
+ ret = reg_write(client, MT9P031_COLUMN_ADDRESS_MODE,
+ ((xbin - 1) << 4) | (xskip - 1));
+ if (!ret)
+ ret = reg_write(client, MT9P031_ROW_ADDRESS_MODE,
+ ((ybin - 1) << 4) | (yskip - 1));
+ }
+ dev_dbg(&client->dev, "new physical left %u, top %u\n",
+ rect->left, rect->top);
+
+ if (!ret)
+ ret = reg_write(client, MT9P031_COLUMN_START,
+ rect->left + MT9P031_COLUMN_SKIP);
+ if (!ret)
+ ret = reg_write(client, MT9P031_ROW_START,
+ rect->top + MT9P031_ROW_SKIP);
+ if (!ret)
+ ret = reg_write(client, MT9P031_WINDOW_WIDTH,
+ rect->width - 1);
+ if (!ret)
+ ret = reg_write(client, MT9P031_WINDOW_HEIGHT,
+ rect->height - 1);
+
+ /* Re-enable register update, commit all changes */
+ if (!ret)
+ ret = reg_clear(client, MT9P031_OUTPUT_CONTROL, 1);
+
+ if (!ret) {
+ mt9p031->xskip = xskip;
+ mt9p031->yskip = yskip;
+ }
+
+ return ret;
+}
+
+static int mt9p031_set_crop(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh,
+ struct v4l2_subdev_crop *crop)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(sd);
+ struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
+ struct v4l2_mbus_framefmt *f;
+ struct v4l2_rect *c;
+ struct v4l2_rect rect;
+ u16 xskip, yskip;
+ s32 width, height;
+ int ret;
+
+ pr_info("%s(%ux%u@%u:%u : %u)\n", __func__,
+ crop->rect.width, crop->rect.height, crop->rect.left, crop->rect.top, crop->which);
+
+ /*
+ * Clamp the crop rectangle boundaries and align them to a multiple of 2
+ * pixels.
+ */
+ rect.width = ALIGN(clamp(crop->rect.width,
+ MT9P031_MIN_WIDTH, MT9P031_MAX_WIDTH), 2);
+ rect.height = ALIGN(clamp(crop->rect.height,
+ MT9P031_MIN_HEIGHT, MT9P031_MAX_HEIGHT), 2);
+ rect.left = ALIGN(clamp(crop->rect.left,
+ 0, MT9P031_MAX_WIDTH - rect.width), 2);
+ rect.top = ALIGN(clamp(crop->rect.top,
+ 0, MT9P031_MAX_HEIGHT - rect.height), 2);
+
+ c = mt9p031_get_pad_crop(mt9p031, fh, crop->pad, crop->which);
+
+#if 1
+ if (rect.width != c->width || rect.height != c->height) {
+ /*
+ * Reset the output image size if the crop rectangle size has
+ * been modified.
+ */
+ f = mt9p031_get_pad_format(mt9p031, fh, crop->pad,
+ crop->which);
+ width = f->width;
+ height = f->height;
+
+ xskip = mt9p031_skip_for_crop(rect.width, &width, 7);
+ yskip = mt9p031_skip_for_crop(rect.height, &height, 8);
+ } else {
+ xskip = mt9p031->xskip;
+ yskip = mt9p031->yskip;
+ f = NULL;
+ }
+
+ if (crop->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+ ret = mt9p031_set_params(client, &rect, xskip, yskip);
+ if (ret < 0)
+ return ret;
+ }
+
+ if (f) {
+ f->width = width;
+ f->height = height;
+ }
+#else
+ f = mt9p031_get_pad_format(mt9p031, fh, crop->pad,
+ crop->which);
+ f->width = rect.width;
+ f->height = rect.height;
+#endif
+
+ *c = rect;
+ crop->rect = rect;
+
+ return 0;
+}
+
+static int mt9p031_get_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh,
+ struct v4l2_subdev_format *fmt)
+{
+ struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
+// fmt->format = mt9p031->format;
+ pr_info("%s()\n", __func__);
+
+ fmt->format = *mt9p031_get_pad_format(mt9p031, fh, fmt->pad, fmt->which);
+ return 0;
+}
+
+static u16 mt9p031_skip_for_scale(s32 *source, s32 target, s32 max_skip, s32 max)
+{
+ unsigned int skip;
+
+ if (*source - *source / 4 < target) {
+ *source = target;
+ return 1;
+ }
+
+ skip = min(max, *source + target / 2) / target;
+ if (skip > max_skip)
+ skip = max_skip;
+ *source = target * skip;
+
+ return skip;
+}
+
+static int mt9p031_fmt_validate(struct v4l2_subdev *sd, struct v4l2_subdev_format *fmt)
+{
+ struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
+ struct v4l2_mbus_framefmt *format = &fmt->format;
+
+ if (format->code != mt9p031->format.code || fmt->pad)
+ return -EINVAL;
+
+ format->colorspace = V4L2_COLORSPACE_SRGB;
+ format->width = clamp_t(int, ALIGN(format->width, 2), 2, MT9P031_MAX_WIDTH);
+ format->height = clamp_t(int, ALIGN(format->height, 2), 2, MT9P031_MAX_HEIGHT);
+ format->field = V4L2_FIELD_NONE;
+
+ return 0;
+}
+
+static int mt9p031_set_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh,
+ struct v4l2_subdev_format *fmt)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(sd);
+ struct v4l2_subdev_format sdf = *fmt;
+ struct v4l2_mbus_framefmt *f, *format = &sdf.format;
+ struct v4l2_rect *c, rect;
+ struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
+ u16 xskip, yskip;
+ int ret;
+
+ ret = mt9p031_fmt_validate(sd, &sdf);
+ if (ret < 0)
+ return ret;
+
+ f = mt9p031_get_pad_format(mt9p031, fh, fmt->pad, fmt->which);
+
+ if (f->width != format->width || f->height != format->height) {
+ c = mt9p031_get_pad_crop(mt9p031, fh, fmt->pad, fmt->which);
+
+ rect.width = c->width;
+ rect.height = c->height;
+
+ xskip = mt9p031_skip_for_scale(&rect.width, format->width, 7, MT9P031_MAX_WIDTH);
+ if (rect.width + c->left > MT9P031_MAX_WIDTH)
+ rect.left = (MT9P031_MAX_WIDTH - rect.width) / 2;
+ else
+ rect.left = c->left;
+ yskip = mt9p031_skip_for_scale(&rect.height, format->height, 8, MT9P031_MAX_HEIGHT);
+ if (rect.height + c->top > MT9P031_MAX_HEIGHT)
+ rect.top = (MT9P031_MAX_HEIGHT - rect.height) / 2;
+ else
+ rect.top = c->top;
+ } else {
+ xskip = mt9p031->xskip;
+ yskip = mt9p031->yskip;
+ c = NULL;
+ }
+
+ pr_info("%s(%ux%u : %u)\n", __func__, format->width, format->height, fmt->which);
+
+ if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+ /* mt9p031_set_params() doesn't change width and height */
+ ret = mt9p031_set_params(client, &rect, xskip, yskip);
+ if (ret < 0)
+ return ret;
+ }
+
+ if (c)
+ *c = rect;
+
+ *f = *format;
+ fmt->format = *format;
+
+ return 0;
+#if 0
+// fmt->format = mt9p031->format;
+ ret = mt9p031_set_crop(sd, fh, &crop);
+ if (!ret)
+ fmt->format = *mt9p031_get_pad_format(mt9p031, fh, fmt->pad, fmt->which);
+
+ return ret;
+#endif
+}
+
+static int mt9p031_s_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(sd);
+ int ret;
+
+ if (enable)
+ /* Switch to master "normal" mode */
+ ret = reg_set(client, MT9P031_OUTPUT_CONTROL, 2);
+ else
+ /* Stop sensor readout */
+ ret = reg_clear(client, MT9P031_OUTPUT_CONTROL, 2);
+
+ if (ret < 0)
+ return -EIO;
+
+ return 0;
+}
+
+enum {
+ MT9P031_CTRL_VFLIP,
+ MT9P031_CTRL_HFLIP,
+ MT9P031_CTRL_GAIN,
+ MT9P031_CTRL_EXPOSURE,
+ MT9P031_CTRL_EXPOSURE_AUTO,
+};
+
+static const struct v4l2_queryctrl mt9p031_controls[] = {
+ [MT9P031_CTRL_VFLIP] = {
+ .id = V4L2_CID_VFLIP,
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .name = "Flip Vertically",
+ .minimum = 0,
+ .maximum = 1,
+ .step = 1,
+ .default_value = 0,
+ },
+ [MT9P031_CTRL_HFLIP] = {
+ .id = V4L2_CID_HFLIP,
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .name = "Flip Horizontally",
+ .minimum = 0,
+ .maximum = 1,
+ .step = 1,
+ .default_value = 0,
+ },
+};
+
+static int mt9p031_g_chip_ident(struct v4l2_subdev *sd,
+ struct v4l2_dbg_chip_ident *id)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(sd);
+ struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
+
+ if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+ return -EINVAL;
+
+ if (id->match.addr != client->addr)
+ return -ENODEV;
+
+ id->ident = mt9p031->model;
+ id->revision = 0;
+
+ return 0;
+}
+
+static int mt9p031_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(sd);
+ int data;
+
+ switch (ctrl->id) {
+ case V4L2_CID_VFLIP:
+ data = reg_read(client, MT9P031_READ_MODE_2);
+ if (data < 0)
+ return -EIO;
+ ctrl->value = !!(data & 0x8000);
+ break;
+ case V4L2_CID_HFLIP:
+ data = reg_read(client, MT9P031_READ_MODE_2);
+ if (data < 0)
+ return -EIO;
+ ctrl->value = !!(data & 0x4000);
+ break;
+ }
+ return 0;
+}
+
+static int mt9p031_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(sd);
+ int data;
+
+ pr_info("%s()\n", __func__);
+
+ switch (ctrl->id) {
+ case V4L2_CID_VFLIP:
+ if (ctrl->value)
+ data = reg_set(client, MT9P031_READ_MODE_2, 0x8000);
+ else
+ data = reg_clear(client, MT9P031_READ_MODE_2, 0x8000);
+ if (data < 0)
+ return -EIO;
+ break;
+ case V4L2_CID_HFLIP:
+ if (ctrl->value)
+ data = reg_set(client, MT9P031_READ_MODE_2, 0x4000);
+ else
+ data = reg_clear(client, MT9P031_READ_MODE_2, 0x4000);
+ if (data < 0)
+ return -EIO;
+ break;
+ default:
+ return -EINVAL;
+ }
+ return 0;
+}
+
+/*
+static struct dev_pm_ops mt9p031_dev_pm_ops = {
+ .runtime_suspend = mt9p031_runtime_suspend,
+ .runtime_resume = mt9p031_runtime_resume,
+};
+
+static struct device_type mt9p031_dev_type = {
+ .name = "MT9P031",
+ .pm = &mt9p031_dev_pm_ops,
+};
+*/
+
+/*
+ * Interface active, can use i2c. If it fails, it can indeed mean, that
+ * this wasn't our capture interface, so, we wait for the right one
+ */
+static int mt9p031_video_probe(struct i2c_client *client)
+{
+ struct mt9p031 *mt9p031 = to_mt9p031(client);
+ s32 data;
+ int ret;
+
+ /* Enable the chip */
+ //data = reg_write(client, MT9P031_CHIP_ENABLE, 1);
+ //dev_dbg(&client->dev, "write: %d\n", data);
+
+ /* Read out the chip version register */
+ data = reg_read(client, MT9P031_CHIP_VERSION);
+
+ switch (data) {
+ case MT9P031_CHIP_VERSION_VALUE:
+ mt9p031->model = V4L2_IDENT_MT9P031;
+ break;
+ default:
+ dev_err(&client->dev,
+ "No MT9P031 chip detected, register read %x\n", data);
+ return -ENODEV;
+ }
+
+ dev_info(&client->dev, "Detected a MT9P031 chip ID %x\n", data);
+
+ ret = mt9p031_idle(client);
+ if (ret < 0)
+ dev_err(&client->dev, "Failed to initialise the camera\n");
+
+ return ret;
+}
+
+static int mt9p031_open(struct v4l2_subdev *sd, u32 i)
+{
+ pr_info("%s()\n", __func__);
+ return 0;
+}
+static int mt9p031_query_ctrl(struct v4l2_subdev *sd,
+ struct v4l2_queryctrl *qc)
+{
+ return 0;
+}
+
+static struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
+ .g_ctrl = mt9p031_g_ctrl,
+ .s_ctrl = mt9p031_s_ctrl,
+ .g_chip_ident = mt9p031_g_chip_ident,
+ .init = mt9p031_open,
+ .queryctrl = mt9p031_query_ctrl,
+};
+
+static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
+ .s_stream = mt9p031_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
+ .enum_mbus_code = mt9p031_enum_mbus_code,
+// .enum_frame_size = mt9p031_enum_frame_size,
+ .get_fmt = mt9p031_get_format,
+ .set_fmt = mt9p031_set_format,
+ .get_crop = mt9p031_get_crop,
+ .set_crop = mt9p031_set_crop,
+};
+
+static struct v4l2_subdev_ops mt9p031_subdev_ops = {
+ .core = &mt9p031_subdev_core_ops,
+ .video = &mt9p031_subdev_video_ops,
+ .pad = &mt9p031_subdev_pad_ops,
+};
+
+static int mt9p031_probe(struct i2c_client *client,
+ const struct i2c_device_id *did)
+{
+ struct mt9p031 *mt9p031;
+ struct v4l2_subdev *sd = i2c_get_clientdata(client);
+ struct mt9p031_platform_data *pdata = client->dev.platform_data;
+ struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+ int ret;
+
+ if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
+ dev_warn(&adapter->dev,
+ "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
+ return -EIO;
+ }
+
+ mt9p031 = kzalloc(sizeof(struct mt9p031), GFP_KERNEL);
+ if (!mt9p031)
+ return -ENOMEM;
+
+ v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
+
+// struct isp_device *isp = v4l2_dev_to_isp_device(subdev->v4l2_dev);
+// isp_set_xclk(isp, 16*1000*1000, ISP_XCLK_A);
+
+ mt9p031->rect.left = 0/*MT9P031_COLUMN_SKIP*/;
+ mt9p031->rect.top = 0/*MT9P031_ROW_SKIP*/;
+ mt9p031->rect.width = MT9P031_MAX_WIDTH;
+ mt9p031->rect.height = MT9P031_MAX_HEIGHT;
+
+ switch (pdata->data_shift) {
+ case 2:
+ mt9p031->format.code = V4L2_MBUS_FMT_SGRBG8_1X8;
+ break;
+ case 1:
+ mt9p031->format.code = V4L2_MBUS_FMT_SGRBG10_1X10;
+ break;
+ case 0:
+ mt9p031->format.code = V4L2_MBUS_FMT_SBGGR12_1X12;
+ }
+ mt9p031->format.width = MT9P031_MAX_WIDTH;
+ mt9p031->format.height = MT9P031_MAX_HEIGHT;
+ mt9p031->format.field = V4L2_FIELD_NONE;
+ mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
+
+ /* mt9p031_idle() will reset the chip to default. */
+
+ mt9p031->xskip = 1;
+ mt9p031->yskip = 1;
+
+//#error FIXME: this is needed for i2c chip detection, but then has to be desabled and reloaded for capture!
+#if 1
+ if (pdata->set_xclk)
+ pdata->set_xclk(sd, 54000000);
+#endif
+
+ mt9p031->reg_1v8 = regulator_get(NULL, "cam_1v8");
+ if (IS_ERR(mt9p031->reg_1v8)) {
+ ret = PTR_ERR(mt9p031->reg_1v8);
+ pr_err("Failed 1.8v regulator: %d\n", ret);
+ goto e1v8;
+ }
+
+ mt9p031->reg_2v8 = regulator_get(NULL, "cam_2v8");
+ if (IS_ERR(mt9p031->reg_2v8)) {
+ ret = PTR_ERR(mt9p031->reg_2v8);
+ pr_err("Failed 2.8v regulator: %d\n", ret);
+ goto e2v8;
+ }
+
+ if (pdata->reset)
+ pdata->reset(sd, 1);
+
+ /* turn on VDD */
+ ret = regulator_enable(mt9p031->reg_1v8);
+ if (ret) {
+ pr_err("Failed to enable 1.8v regulator: %d\n", ret);
+ goto e1v8en;
+ }
+
+// msleep(1);
+
+ /* turn on VDD_IO */
+ ret = regulator_enable(mt9p031->reg_2v8);
+ if (ret) {
+ pr_err("Failed to enable 2.8v regulator: %d\n", ret);
+ goto e2v8en;
+ }
+
+ msleep(50);
+
+ if (pdata->reset)
+ pdata->reset(sd, 0);
+
+// udelay(500);
+ msleep(50);
+
+// mt9p031_idle(client);
+
+ ret = mt9p031_video_probe(client);
+
+ //mt9p031_disable(client);
+
+ if (ret)
+ goto evprobe;
+
+ mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
+ ret = media_entity_init(&mt9p031->subdev.entity, 1, &mt9p031->pad, 0);
+ if (ret)
+ goto evprobe;
+
+ mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+#if 0
+ if (pdata->set_xclk)
+ pdata->set_xclk(sd, 0);
+
+ msleep(1);
+
+ if (pdata->set_xclk)
+ pdata->set_xclk(sd, 54000000);
+
+ msleep(1);
+#endif
+ return ret;
+
+evprobe:
+ regulator_disable(mt9p031->reg_2v8);
+e2v8en:
+ regulator_disable(mt9p031->reg_1v8);
+e1v8en:
+ regulator_put(mt9p031->reg_2v8);
+e2v8:
+ regulator_put(mt9p031->reg_1v8);
+e1v8:
+ kfree(mt9p031);
+ return ret;
+}
+
+static int mt9p031_remove(struct i2c_client *client)
+{
+ struct v4l2_subdev *sd = i2c_get_clientdata(client);
+ struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
+
+ v4l2_device_unregister_subdev(sd);
+ media_entity_cleanup(&sd->entity);
+ regulator_disable(mt9p031->reg_2v8);
+ regulator_disable(mt9p031->reg_1v8);
+ regulator_put(mt9p031->reg_2v8);
+ regulator_put(mt9p031->reg_1v8);
+ kfree(mt9p031);
+
+ return 0;
+}
+
+static const struct i2c_device_id mt9p031_id[] = {
+ { "mt9p031", 0 },
+ { }
+};
+MODULE_DEVICE_TABLE(i2c, mt9p031_id);
+
+static struct i2c_driver mt9p031_i2c_driver = {
+ .driver = {
+ .name = "mt9p031",
+ },
+ .probe = mt9p031_probe,
+ .remove = mt9p031_remove,
+ .id_table = mt9p031_id,
+};
+
+static int __init mt9p031_mod_init(void)
+{
+ return i2c_add_driver(&mt9p031_i2c_driver);
+}
+
+static void __exit mt9p031_mod_exit(void)
+{
+ i2c_del_driver(&mt9p031_i2c_driver);
+}
+
+module_init(mt9p031_mod_init);
+module_exit(mt9p031_mod_exit);
+
+MODULE_DESCRIPTION("Micron MT9P031 Camera driver");
+MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
+MODULE_LICENSE("GPL v2");
new file mode 100644
@@ -0,0 +1,12 @@
+#ifndef MT9P031_H
+#define MT9P031_H
+
+struct v4l2_subdev;
+
+struct mt9p031_platform_data {
+ int (*set_xclk)(struct v4l2_subdev *subdev, int hz);
+ int (*reset)(struct v4l2_subdev *subdev, int active);
+ unsigned int data_shift;
+};
+
+#endif