summaryrefslogtreecommitdiff
path: root/drivers/media/platform/st/stm32/stm32-csi.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/media/platform/st/stm32/stm32-csi.c')
-rw-r--r--drivers/media/platform/st/stm32/stm32-csi.c106
1 files changed, 55 insertions, 51 deletions
diff --git a/drivers/media/platform/st/stm32/stm32-csi.c b/drivers/media/platform/st/stm32/stm32-csi.c
index 48941aae8c9b..b69048144cc1 100644
--- a/drivers/media/platform/st/stm32/stm32-csi.c
+++ b/drivers/media/platform/st/stm32/stm32-csi.c
@@ -325,7 +325,6 @@ static const struct stm32_csi_mbps_phy_reg snps_stm32mp25[] = {
{ .mbps = 2400, .hsfreqrange = 0x47, .osc_freq_target = 442 },
{ .mbps = 2450, .hsfreqrange = 0x48, .osc_freq_target = 451 },
{ .mbps = 2500, .hsfreqrange = 0x49, .osc_freq_target = 460 },
- { /* sentinel */ }
};
static const struct v4l2_mbus_framefmt fmt_default = {
@@ -358,7 +357,7 @@ static inline struct stm32_csi_dev *to_csidev(struct v4l2_subdev *sd)
static int stm32_csi_setup_lane_merger(struct stm32_csi_dev *csidev)
{
u32 lmcfgr = 0;
- int i;
+ unsigned int i;
for (i = 0; i < csidev->num_lanes; i++) {
if (!csidev->lanes[i] || csidev->lanes[i] > STM32_CSI_LANES_MAX) {
@@ -444,13 +443,15 @@ static void stm32_csi_phy_reg_write(struct stm32_csi_dev *csidev,
static int stm32_csi_start(struct stm32_csi_dev *csidev,
struct v4l2_subdev_state *state)
{
- const struct stm32_csi_mbps_phy_reg *phy_regs;
+ struct media_pad *src_pad =
+ &csidev->s_subdev->entity.pads[csidev->s_subdev_pad_nb];
+ const struct stm32_csi_mbps_phy_reg *phy_regs = NULL;
struct v4l2_mbus_framefmt *sink_fmt;
const struct stm32_csi_fmts *fmt;
unsigned long phy_clk_frate;
+ u32 lanes_ie, lanes_en;
unsigned int mbps;
- u32 lanes_ie = 0;
- u32 lanes_en = 0;
+ unsigned int i;
s64 link_freq;
int ret;
u32 ccfr;
@@ -465,7 +466,7 @@ static int stm32_csi_start(struct stm32_csi_dev *csidev,
if (!csidev->s_subdev)
return -EIO;
- link_freq = v4l2_get_link_freq(csidev->s_subdev->ctrl_handler,
+ link_freq = v4l2_get_link_freq(src_pad,
fmt->bpp, 2 * csidev->num_lanes);
if (link_freq < 0)
return link_freq;
@@ -474,11 +475,14 @@ static int stm32_csi_start(struct stm32_csi_dev *csidev,
mbps = div_s64(link_freq, 500000);
dev_dbg(csidev->dev, "Computed Mbps: %u\n", mbps);
- for (phy_regs = snps_stm32mp25; phy_regs->mbps != 0; phy_regs++)
- if (phy_regs->mbps >= mbps)
+ for (i = 0; i < ARRAY_SIZE(snps_stm32mp25); i++) {
+ if (snps_stm32mp25[i].mbps >= mbps) {
+ phy_regs = &snps_stm32mp25[i];
break;
+ }
+ }
- if (!phy_regs->mbps) {
+ if (!phy_regs) {
dev_err(csidev->dev, "Unsupported PHY speed (%u Mbps)", mbps);
return -ERANGE;
}
@@ -488,8 +492,8 @@ static int stm32_csi_start(struct stm32_csi_dev *csidev,
phy_regs->osc_freq_target);
/* Prepare lanes related configuration bits */
- lanes_ie |= STM32_CSI_SR1_DL0_ERRORS;
- lanes_en |= STM32_CSI_PCR_DL0EN;
+ lanes_ie = STM32_CSI_SR1_DL0_ERRORS;
+ lanes_en = STM32_CSI_PCR_DL0EN;
if (csidev->num_lanes == 2) {
lanes_ie |= STM32_CSI_SR1_DL1_ERRORS;
lanes_en |= STM32_CSI_PCR_DL1EN;
@@ -497,21 +501,19 @@ static int stm32_csi_start(struct stm32_csi_dev *csidev,
ret = pm_runtime_get_sync(csidev->dev);
if (ret < 0)
- return ret;
+ goto error_put;
/* Retrieve CSI2PHY clock rate to compute CCFR value */
phy_clk_frate = clk_get_rate(csidev->clks[STM32_CSI_CLK_CSI2PHY].clk);
if (!phy_clk_frate) {
- pm_runtime_put(csidev->dev);
dev_err(csidev->dev, "CSI2PHY clock rate invalid (0)\n");
- return ret;
+ ret = -EINVAL;
+ goto error_put;
}
ret = stm32_csi_setup_lane_merger(csidev);
- if (ret) {
- pm_runtime_put(csidev->dev);
- return ret;
- }
+ if (ret)
+ goto error_put;
/* Enable the CSI */
writel_relaxed(STM32_CSI_CR_CSIEN, csidev->base + STM32_CSI_CR);
@@ -567,6 +569,10 @@ static int stm32_csi_start(struct stm32_csi_dev *csidev,
writel_relaxed(0, csidev->base + STM32_CSI_PMCR);
return ret;
+
+error_put:
+ pm_runtime_put(csidev->dev);
+ return ret;
}
static void stm32_csi_stop(struct stm32_csi_dev *csidev)
@@ -591,20 +597,20 @@ static int stm32_csi_start_vc(struct stm32_csi_dev *csidev,
{
struct v4l2_mbus_framefmt *mbus_fmt;
const struct stm32_csi_fmts *fmt;
- u32 cfgr1 = 0;
- int ret = 0;
u32 status;
+ u32 cfgr1;
+ int ret;
mbus_fmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SOURCE);
fmt = stm32_csi_code_to_fmt(mbus_fmt->code);
/* If the mbus code is JPEG, don't enable filtering */
if (mbus_fmt->code == MEDIA_BUS_FMT_JPEG_1X8) {
- cfgr1 |= STM32_CSI_VCXCFGR1_ALLDT;
+ cfgr1 = STM32_CSI_VCXCFGR1_ALLDT;
cfgr1 |= fmt->input_fmt << STM32_CSI_VCXCFGR1_CDTFT_SHIFT;
dev_dbg(csidev->dev, "VC%d: enable AllDT mode\n", vc);
} else {
- cfgr1 |= fmt->datatype << STM32_CSI_VCXCFGR1_DT0_SHIFT;
+ cfgr1 = fmt->datatype << STM32_CSI_VCXCFGR1_DT0_SHIFT;
cfgr1 |= fmt->input_fmt << STM32_CSI_VCXCFGR1_DT0FT_SHIFT;
cfgr1 |= STM32_CSI_VCXCFGR1_DT0EN;
dev_dbg(csidev->dev, "VC%d: enable DT0(0x%x)/DT0FT(0x%x)\n",
@@ -630,8 +636,8 @@ static int stm32_csi_start_vc(struct stm32_csi_dev *csidev,
static int stm32_csi_stop_vc(struct stm32_csi_dev *csidev, u32 vc)
{
- int ret = 0;
u32 status;
+ int ret;
/* Stop the Virtual Channel */
writel_relaxed(STM32_CSI_CR_VCXSTOP(vc) | STM32_CSI_CR_CSIEN,
@@ -690,25 +696,27 @@ static int stm32_csi_enable_streams(struct v4l2_subdev *sd,
ret = stm32_csi_start_vc(csidev, state, 0);
if (ret) {
dev_err(csidev->dev, "Failed to start VC0\n");
- stm32_csi_stop(csidev);
- return ret;
+ goto failed_start_vc;
}
ret = v4l2_subdev_enable_streams(csidev->s_subdev,
csidev->s_subdev_pad_nb, BIT_ULL(0));
- if (ret) {
- stm32_csi_stop_vc(csidev, 0);
- stm32_csi_stop(csidev);
- return ret;
- }
+ if (ret)
+ goto failed_enable_streams;
return 0;
+
+failed_enable_streams:
+ stm32_csi_stop_vc(csidev, 0);
+failed_start_vc:
+ stm32_csi_stop(csidev);
+ return ret;
}
static int stm32_csi_init_state(struct v4l2_subdev *sd,
struct v4l2_subdev_state *state)
{
- int i;
+ unsigned int i;
for (i = 0; i < sd->entity.num_pads; i++)
*v4l2_subdev_state_get_format(state, i) = fmt_default;
@@ -873,7 +881,8 @@ static irqreturn_t stm32_csi_irq_thread(int irq, void *arg)
static int stm32_csi_get_resources(struct stm32_csi_dev *csidev,
struct platform_device *pdev)
{
- int irq, ret, i;
+ unsigned int i;
+ int irq, ret;
csidev->base = devm_platform_get_and_ioremap_resource(pdev, 0, NULL);
if (IS_ERR(csidev->base))
@@ -926,38 +935,32 @@ static int stm32_csi_parse_dt(struct stm32_csi_dev *csidev)
}
ret = v4l2_fwnode_endpoint_parse(ep, &v4l2_ep);
- fwnode_handle_put(ep);
if (ret) {
dev_err(csidev->dev, "Could not parse v4l2 endpoint\n");
- return ret;
+ goto out;
}
csidev->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
if (csidev->num_lanes > STM32_CSI_LANES_MAX) {
dev_err(csidev->dev, "Unsupported number of data-lanes: %d\n",
csidev->num_lanes);
- return -EINVAL;
+ ret = -EINVAL;
+ goto out;
}
memcpy(csidev->lanes, v4l2_ep.bus.mipi_csi2.data_lanes,
sizeof(csidev->lanes));
- ep = fwnode_graph_get_next_endpoint(dev_fwnode(csidev->dev), NULL);
- if (!ep) {
- dev_err(csidev->dev, "Failed to get next endpoint\n");
- return -EINVAL;
- }
-
v4l2_async_subdev_nf_init(&csidev->notifier, &csidev->sd);
asd = v4l2_async_nf_add_fwnode_remote(&csidev->notifier, ep,
struct v4l2_async_connection);
- fwnode_handle_put(ep);
if (IS_ERR(asd)) {
dev_err(csidev->dev, "Failed to add fwnode remote subdev\n");
- return PTR_ERR(asd);
+ ret = PTR_ERR(asd);
+ goto out;
}
csidev->notifier.ops = &stm32_csi_notifier_ops;
@@ -966,9 +969,11 @@ static int stm32_csi_parse_dt(struct stm32_csi_dev *csidev)
if (ret) {
dev_err(csidev->dev, "Failed to register notifier\n");
v4l2_async_nf_cleanup(&csidev->notifier);
- return ret;
+ goto out;
}
+out:
+ fwnode_handle_put(ep);
return ret;
}
@@ -989,11 +994,11 @@ static int stm32_csi_probe(struct platform_device *pdev)
ret = stm32_csi_get_resources(csidev, pdev);
if (ret)
- goto err_free_priv;
+ return ret;
ret = stm32_csi_parse_dt(csidev);
if (ret)
- goto err_free_priv;
+ return ret;
csidev->sd.owner = THIS_MODULE;
csidev->sd.dev = &pdev->dev;
@@ -1018,10 +1023,6 @@ static int stm32_csi_probe(struct platform_device *pdev)
if (ret < 0)
goto err_cleanup;
- ret = v4l2_async_register_subdev(&csidev->sd);
- if (ret < 0)
- goto err_cleanup;
-
/* Reset device */
rstc = devm_reset_control_get_exclusive(&pdev->dev, NULL);
if (IS_ERR(rstc)) {
@@ -1048,6 +1049,10 @@ static int stm32_csi_probe(struct platform_device *pdev)
pm_runtime_enable(&pdev->dev);
+ ret = v4l2_async_register_subdev(&csidev->sd);
+ if (ret < 0)
+ goto err_cleanup;
+
dev_info(&pdev->dev,
"Probed CSI with %u lanes\n", csidev->num_lanes);
@@ -1055,7 +1060,6 @@ static int stm32_csi_probe(struct platform_device *pdev)
err_cleanup:
v4l2_async_nf_cleanup(&csidev->notifier);
-err_free_priv:
return ret;
}