static void omap1_cam_remove_device(struct soc_camera_device *icd)
{
struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
struct omap1_cam_dev *pcdev = ici->priv;
u32 ctrlclock;
BUG_ON(icd != pcdev->icd);
suspend_capture(pcdev);
disable_capture(pcdev);
sensor_reset(pcdev, true);
/* disable and release system clocks */
ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
ctrlclock &= ~(MCLK_EN | DPLL_EN | CAMEXCLK_EN);
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
ctrlclock = (ctrlclock & ~FOSCMOD_MASK) | FOSCMOD_12MHz;
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock | MCLK_EN);
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~MCLK_EN);
clk_disable(pcdev->clk);
pcdev->icd = NULL;
dev_dbg(icd->dev.parent,
"OMAP1 Camera driver detached from camera %d\n", icd->devnum);
}
static mp_obj_t py_sensor_reset() {
sensor_reset();
sensor_set_pixformat(PIXFORMAT_RGB565);
sensor_set_framesize(FRAMESIZE_QQVGA);
sensor_set_framerate(FRAMERATE_30FPS);
sensor_set_gainceiling(GAINCEILING_8X);
sensor_set_contrast(0);
sensor_set_brightness(0);
return mp_const_none;
}
static void sensor_power(bool front, bool on)
{
DBG_INFO("%s sensor power %s", front ? "front" : "rear", on ? "on" : "off");
if (front) {
set_gpio_level(&g_spinfo.gpio_front, !on);
sensor_reset();
} else {
set_gpio_level(&g_spinfo.gpio_rear, !on);
}
mdelay(10);
}
int
PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = OK;
lock();
switch (cmd) {
case GPIO_RESET:
gpio_reset();
break;
case GPIO_SENSOR_RAIL_RESET:
sensor_reset(arg);
break;
case GPIO_PERIPHERAL_RAIL_RESET:
peripheral_reset(arg);
break;
case GPIO_SET_OUTPUT:
case GPIO_SET_INPUT:
case GPIO_SET_ALT_1:
gpio_set_function(arg, cmd);
break;
case GPIO_SET_ALT_2:
case GPIO_SET_ALT_3:
case GPIO_SET_ALT_4:
ret = -EINVAL;
break;
case GPIO_SET:
case GPIO_CLEAR:
gpio_write(arg, cmd);
break;
case GPIO_GET:
*(uint32_t *)arg = gpio_read();
break;
default:
ret = -ENOTTY;
}
unlock();
return ret;
}
/*
* The following two functions absolutely depend on the fact, that
* there can be only one camera on OMAP1 camera sensor interface
*/
static int omap1_cam_add_device(struct soc_camera_device *icd)
{
struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
struct omap1_cam_dev *pcdev = ici->priv;
u32 ctrlclock;
if (pcdev->icd)
return -EBUSY;
clk_enable(pcdev->clk);
/* setup sensor clock */
ctrlclock = CAM_READ(pcdev, CTRLCLOCK);
ctrlclock &= ~(CAMEXCLK_EN | MCLK_EN | DPLL_EN);
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
ctrlclock &= ~FOSCMOD_MASK;
switch (pcdev->camexclk) {
case 6000000:
ctrlclock |= CAMEXCLK_EN | FOSCMOD_6MHz;
break;
case 8000000:
ctrlclock |= CAMEXCLK_EN | FOSCMOD_8MHz | DPLL_EN;
break;
case 9600000:
ctrlclock |= CAMEXCLK_EN | FOSCMOD_9_6MHz | DPLL_EN;
break;
case 12000000:
ctrlclock |= CAMEXCLK_EN | FOSCMOD_12MHz;
break;
case 24000000:
ctrlclock |= CAMEXCLK_EN | FOSCMOD_24MHz | DPLL_EN;
default:
break;
}
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~DPLL_EN);
/* enable internal clock */
ctrlclock |= MCLK_EN;
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
sensor_reset(pcdev, false);
pcdev->icd = icd;
dev_dbg(icd->dev.parent, "OMAP1 Camera driver attached to camera %d\n",
icd->devnum);
return 0;
}
const mp_obj_module_t *py_sensor_init()
{
/* Init sensor */
if (sensor_init() != 0) {
return NULL;
}
/* Reset sensor and registers */
sensor_reset();
/* Use some default settings */
sensor_set_pixformat(PIXFORMAT_RGB565);
sensor_set_framesize(FRAMESIZE_QQVGA);
sensor_set_framerate(FRAMERATE_30FPS);
sensor_set_gainceiling(GAINCEILING_8X);
sensor_set_contrast(0);
sensor_set_brightness(0);
return &sensor_module;
}
/*
* The following two functions absolutely depend on the fact, that
* there can be only one camera on OMAP1 camera sensor interface
*/
static int omap1_cam_clock_start(struct soc_camera_host *ici)
{
struct omap1_cam_dev *pcdev = ici->priv;
u32 ctrlclock;
clk_enable(pcdev->clk);
/* setup sensor clock */
ctrlclock = CAM_READ(pcdev, CTRLCLOCK);
ctrlclock &= ~(CAMEXCLK_EN | MCLK_EN | DPLL_EN);
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
ctrlclock &= ~FOSCMOD_MASK;
switch (pcdev->camexclk) {
case 6000000:
ctrlclock |= CAMEXCLK_EN | FOSCMOD_6MHz;
break;
case 8000000:
ctrlclock |= CAMEXCLK_EN | FOSCMOD_8MHz | DPLL_EN;
break;
case 9600000:
ctrlclock |= CAMEXCLK_EN | FOSCMOD_9_6MHz | DPLL_EN;
break;
case 12000000:
ctrlclock |= CAMEXCLK_EN | FOSCMOD_12MHz;
break;
case 24000000:
ctrlclock |= CAMEXCLK_EN | FOSCMOD_24MHz | DPLL_EN;
default:
break;
}
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~DPLL_EN);
/* enable internal clock */
ctrlclock |= MCLK_EN;
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
sensor_reset(pcdev, false);
return 0;
}
static void omap1_cam_clock_stop(struct soc_camera_host *ici)
{
struct omap1_cam_dev *pcdev = ici->priv;
u32 ctrlclock;
suspend_capture(pcdev);
disable_capture(pcdev);
sensor_reset(pcdev, true);
/* disable and release system clocks */
ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
ctrlclock &= ~(MCLK_EN | DPLL_EN | CAMEXCLK_EN);
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
ctrlclock = (ctrlclock & ~FOSCMOD_MASK) | FOSCMOD_12MHz;
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock | MCLK_EN);
CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~MCLK_EN);
clk_disable(pcdev->clk);
}
static int __init omap1_cam_probe(struct platform_device *pdev)
{
struct omap1_cam_dev *pcdev;
struct resource *res;
struct clk *clk;
void __iomem *base;
unsigned int irq;
int err = 0;
res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
irq = platform_get_irq(pdev, 0);
if (!res || (int)irq <= 0) {
err = -ENODEV;
goto exit;
}
clk = clk_get(&pdev->dev, "armper_ck");
if (IS_ERR(clk)) {
err = PTR_ERR(clk);
goto exit;
}
pcdev = kzalloc(sizeof(*pcdev) + resource_size(res), GFP_KERNEL);
if (!pcdev) {
dev_err(&pdev->dev, "Could not allocate pcdev\n");
err = -ENOMEM;
goto exit_put_clk;
}
pcdev->res = res;
pcdev->clk = clk;
pcdev->pdata = pdev->dev.platform_data;
pcdev->pflags = pcdev->pdata->flags;
if (pcdev->pdata)
pcdev->camexclk = pcdev->pdata->camexclk_khz * 1000;
switch (pcdev->camexclk) {
case 6000000:
case 8000000:
case 9600000:
case 12000000:
case 24000000:
break;
default:
dev_warn(&pdev->dev,
"Incorrect sensor clock frequency %ld kHz, "
"should be one of 0, 6, 8, 9.6, 12 or 24 MHz, "
"please correct your platform data\n",
pcdev->pdata->camexclk_khz);
pcdev->camexclk = 0;
case 0:
dev_info(&pdev->dev,
"Not providing sensor clock\n");
}
INIT_LIST_HEAD(&pcdev->capture);
spin_lock_init(&pcdev->lock);
/*
* Request the region.
*/
if (!request_mem_region(res->start, resource_size(res), DRIVER_NAME)) {
err = -EBUSY;
goto exit_kfree;
}
base = ioremap(res->start, resource_size(res));
if (!base) {
err = -ENOMEM;
goto exit_release;
}
pcdev->irq = irq;
pcdev->base = base;
sensor_reset(pcdev, true);
err = omap_request_dma(OMAP_DMA_CAMERA_IF_RX, DRIVER_NAME,
dma_isr, (void *)pcdev, &pcdev->dma_ch);
if (err < 0) {
dev_err(&pdev->dev, "Can't request DMA for OMAP1 Camera\n");
err = -EBUSY;
goto exit_iounmap;
}
dev_dbg(&pdev->dev, "got DMA channel %d\n", pcdev->dma_ch);
/* preconfigure DMA */
omap_set_dma_src_params(pcdev->dma_ch, OMAP_DMA_PORT_TIPB,
OMAP_DMA_AMODE_CONSTANT, res->start + REG_CAMDATA,
0, 0);
omap_set_dma_dest_burst_mode(pcdev->dma_ch, OMAP_DMA_DATA_BURST_4);
/* setup DMA autoinitialization */
omap_dma_link_lch(pcdev->dma_ch, pcdev->dma_ch);
err = request_irq(pcdev->irq, cam_isr, 0, DRIVER_NAME, pcdev);
if (err) {
dev_err(&pdev->dev, "Camera interrupt register failed\n");
goto exit_free_dma;
}
pcdev->soc_host.drv_name = DRIVER_NAME;
pcdev->soc_host.ops = &omap1_host_ops;
pcdev->soc_host.priv = pcdev;
pcdev->soc_host.v4l2_dev.dev = &pdev->dev;
pcdev->soc_host.nr = pdev->id;
err = soc_camera_host_register(&pcdev->soc_host);
if (err)
goto exit_free_irq;
dev_info(&pdev->dev, "OMAP1 Camera Interface driver loaded\n");
return 0;
exit_free_irq:
free_irq(pcdev->irq, pcdev);
exit_free_dma:
omap_free_dma(pcdev->dma_ch);
exit_iounmap:
iounmap(base);
exit_release:
release_mem_region(res->start, resource_size(res));
exit_kfree:
kfree(pcdev);
exit_put_clk:
clk_put(clk);
exit:
return err;
}
int
fmu_main(int argc, char *argv[])
{
PortMode new_mode = PORT_MODE_UNSET;
const char *verb = argv[1];
if (!strcmp(verb, "stop")) {
fmu_stop();
errx(0, "FMU driver stopped");
}
if (!strcmp(verb, "id")) {
uint8_t id[12];
(void)get_board_serial(id);
errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
(unsigned)id[0], (unsigned)id[1], (unsigned)id[2], (unsigned)id[3], (unsigned)id[4], (unsigned)id[5],
(unsigned)id[6], (unsigned)id[7], (unsigned)id[8], (unsigned)id[9], (unsigned)id[10], (unsigned)id[11]);
}
if (fmu_start() != OK)
errx(1, "failed to start the FMU driver");
/*
* Mode switches.
*/
if (!strcmp(verb, "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
} else if (!strcmp(verb, "mode_pwm")) {
new_mode = PORT_FULL_PWM;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
} else if (!strcmp(verb, "mode_pwm4")) {
new_mode = PORT_PWM4;
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
} else if (!strcmp(verb, "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
} else if (!strcmp(verb, "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
} else if (!strcmp(verb, "mode_pwm_serial")) {
new_mode = PORT_PWM_AND_SERIAL;
} else if (!strcmp(verb, "mode_pwm_gpio")) {
new_mode = PORT_PWM_AND_GPIO;
#endif
}
/* was a new mode set? */
if (new_mode != PORT_MODE_UNSET) {
/* yes but it's the same mode */
if (new_mode == g_port_mode)
return OK;
/* switch modes */
int ret = fmu_new_mode(new_mode);
exit(ret == OK ? 0 : 1);
}
if (!strcmp(verb, "test"))
test();
if (!strcmp(verb, "fake"))
fake(argc - 1, argv + 1);
if (!strcmp(verb, "sensor_reset")) {
if (argc > 2) {
int reset_time = strtol(argv[2], 0, 0);
sensor_reset(reset_time);
} else {
sensor_reset(0);
warnx("resettet default time");
}
exit(0);
}
if (!strcmp(verb, "peripheral_reset")) {
if (argc > 2) {
int reset_time = strtol(argv[2], 0, 0);
peripheral_reset(reset_time);
} else {
peripheral_reset(0);
warnx("resettet default time");
}
exit(0);
}
if (!strcmp(verb, "i2c")) {
if (argc > 3) {
int bus = strtol(argv[2], 0, 0);
int clock_hz = strtol(argv[3], 0, 0);
int ret = fmu_new_i2c_speed(bus, clock_hz);
if (ret) {
errx(ret, "setting I2C clock failed");
}
exit(0);
} else {
warnx("i2c cmd args: <bus id> <clock Hz>");
}
}
fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n");
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds], i2c <bus> <hz>\n");
#endif
exit(1);
}
