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.
Thank you.
--
Javier Martin
Vista Silicon S.L.
CDTUC - FASE C - Oficina S-345
Avda de los Castros s/n
39005- Santander. Cantabria. Spain
+34 942 25 32 60
www.vista-silicon.com
diff --git a/arch/arm/mach-omap2/Makefile b/arch/arm/mach-omap2/Makefile
index a45cd64..c437391 100644
--- a/arch/arm/mach-omap2/Makefile
+++ b/arch/arm/mach-omap2/Makefile
@@ -178,7 +178,7 @@ obj-$(CONFIG_MACH_OMAP_H4) += board-h4.o
obj-$(CONFIG_MACH_OMAP_2430SDP) += board-2430sdp.o \
hsmmc.o
obj-$(CONFIG_MACH_OMAP_APOLLON) += board-apollon.o
-obj-$(CONFIG_MACH_OMAP3_BEAGLE) += board-omap3beagle.o \
+obj-$(CONFIG_MACH_OMAP3_BEAGLE) += board-omap3beagle.o board-omap3beagle-camera.o \
hsmmc.o
obj-$(CONFIG_MACH_DEVKIT8000) += board-devkit8000.o \
hsmmc.o
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
@@ -52,6 +52,7 @@
#include "hsmmc.h"
#include "timer-gp.h"
#include "pm.h"
+#include "control.h"
#define NAND_BLOCK_SIZE SZ_128K
@@ -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",
+};
+
+/* VAUX3 for CAM_1V8 */
+static struct regulator_init_data beagle_vaux3 = {
+ .constraints = {
+ .min_uV = 1800000,
+ .max_uV = 1800000,
+ .apply_uV = true,
+ .valid_modes_mask = REGULATOR_MODE_NORMAL
+ | REGULATOR_MODE_STANDBY,
+ .valid_ops_mask = REGULATOR_CHANGE_MODE
+ | REGULATOR_CHANGE_STATUS,
+ },
+ .num_consumer_supplies = 1,
+ .consumer_supplies = &beagle_vaux3_supply,
+};
+
+/* VAUX4 for CAM_2V8 */
+static struct regulator_init_data beagle_vaux4 = {
+ .constraints = {
+ .min_uV = 1800000,
+ .max_uV = 1800000,
+ .apply_uV = true,
+ .valid_modes_mask = REGULATOR_MODE_NORMAL
+ | REGULATOR_MODE_STANDBY,
+ .valid_ops_mask = REGULATOR_CHANGE_MODE
+ | REGULATOR_CHANGE_STATUS,
+ },
+ .num_consumer_supplies = 1,
+ .consumer_supplies = &beagle_vaux4_supply,
+};
+
static int beagle_twl_gpio_setup(struct device *dev,
unsigned gpio, unsigned ngpio)
{
@@ -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);
+ }
+
/*
* TWL4030_GPIO_MAX + 0 == ledA, EHCI nEN_USB_PWR (out, XM active
* high / others active low)
@@ -451,6 +499,8 @@ static struct twl4030_platform_data beagle_twldata = {
.vsim = &beagle_vsim,
.vdac = &beagle_vdac,
.vpll2 = &beagle_vpll2,
+ .vaux3 = &beagle_vaux3,
+ .vaux4 = &beagle_vaux4,
};
static struct i2c_board_info __initdata beagle_i2c_boardinfo[] = {
@@ -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);
/* 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
+ /* 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),
+
+ /* 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;
+
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);
}
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;
+
/* 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
+ ---help---
+ This driver supports MT9P031 cameras from Micron
+ This is a Video4Linux2 sensor-level driver for the Micron
+ mt0p031 5 Mpixel camera.
+
config VIDEO_MT9V011
tristate "Micron mt9v011 sensor support"
depends on I2C && VIDEO_V4L2
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index ace5d8b..912b29b 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -65,6 +65,7 @@ obj-$(CONFIG_VIDEO_UPD64083) += upd64083.o
obj-$(CONFIG_VIDEO_OV7670) += ov7670.o
obj-$(CONFIG_VIDEO_TCM825X) += tcm825x.o
obj-$(CONFIG_VIDEO_TVEEPROM) += tveeprom.o
+obj-$(CONFIG_VIDEO_MT9P031) += mt9p031.o
obj-$(CONFIG_VIDEO_MT9V011) += mt9v011.o
obj-$(CONFIG_VIDEO_SR030PC30) += sr030pc30.o
obj-$(CONFIG_VIDEO_NOON010PC30) += noon010pc30.o
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);
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;
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;
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:
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,
diff --git a/arch/arm/mach-omap2/board-omap3beagle-camera.c b/arch/arm/mach-omap2/board-omap3beagle-camera.c
new file mode 100644
index 0000000..92389dd
--- /dev/null
+++ b/arch/arm/mach-omap2/board-omap3beagle-camera.c
@@ -0,0 +1,158 @@
+/*
+ * arch/arm/mach-omap2/board-rx51-camera.c
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ *
+ * Contact: Sakari Ailus <sakari.ailus@xxxxxxxxx>
+ * Tuukka Toivonen <tuukka.o.toivonen@xxxxxxxxx>
+ *
+ * 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");
diff --git a/drivers/media/video/mt9p031.c b/drivers/media/video/mt9p031.c
new file mode 100644
index 0000000..d42d783
--- /dev/null
+++ b/drivers/media/video/mt9p031.c
@@ -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@xxxxxxxxx>");
+MODULE_LICENSE("GPL v2");
diff --git a/include/media/mt9p031.h b/include/media/mt9p031.h
new file mode 100644
index 0000000..ee2d2ba
--- /dev/null
+++ b/include/media/mt9p031.h
@@ -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
