@@ -462,13 +462,17 @@ static irqreturn_t csid_isr(int irq, void *dev)
static int csid_set_clock_rates(struct csid_device *csid)
{
struct device *dev = csid->camss->dev;
- u32 pixel_clock;
+ const struct csid_format *fmt;
+ s64 link_freq;
int i, j;
int ret;
- ret = camss_get_pixel_clock(&csid->subdev.entity, &pixel_clock);
- if (ret)
- pixel_clock = 0;
+ fmt = csid_get_fmt_entry(csid->formats, csid->nformats,
+ csid->fmt[MSM_CSIPHY_PAD_SINK].code);
+ link_freq = camss_get_link_freq(&csid->subdev.entity, fmt->bpp,
+ csid->phy.lane_cnt);
+ if (link_freq < 0)
+ link_freq = 0;
for (i = 0; i < csid->nclocks; i++) {
struct camss_clock *clock = &csid->clock[i];
@@ -477,13 +481,7 @@ static int csid_set_clock_rates(struct csid_device *csid)
!strcmp(clock->name, "csi1") ||
!strcmp(clock->name, "csi2") ||
!strcmp(clock->name, "csi3")) {
- const struct csid_format *f = csid_get_fmt_entry(
- csid->formats,
- csid->nformats,
- csid->fmt[MSM_CSIPHY_PAD_SINK].code);
- u8 num_lanes = csid->phy.lane_cnt;
- u64 min_rate = pixel_clock * f->bpp /
- (2 * num_lanes * 4);
+ u64 min_rate = link_freq / 4;
long rate;
camss_add_clock_margin(&min_rate);
@@ -51,16 +51,13 @@ static void csiphy_reset(struct csiphy_device *csiphy)
*
* Helper function to calculate settle count value. This is
* based on the CSI2 T_hs_settle parameter which in turn
- * is calculated based on the CSI2 transmitter pixel clock
- * frequency.
+ * is calculated based on the CSI2 transmitter link frequency.
*
- * Return settle count value or 0 if the CSI2 pixel clock
- * frequency is not available
+ * Return settle count value or 0 if the CSI2 link frequency
+ * is not available
*/
-static u8 csiphy_settle_cnt_calc(u32 pixel_clock, u8 bpp, u8 num_lanes,
- u32 timer_clk_rate)
+static u8 csiphy_settle_cnt_calc(s64 link_freq, u32 timer_clk_rate)
{
- u32 mipi_clock; /* Hz */
u32 ui; /* ps */
u32 timer_period; /* ps */
u32 t_hs_prepare_max; /* ps */
@@ -68,8 +65,10 @@ static u8 csiphy_settle_cnt_calc(u32 pixel_clock, u8 bpp, u8 num_lanes,
u32 t_hs_settle; /* ps */
u8 settle_cnt;
- mipi_clock = pixel_clock * bpp / (2 * num_lanes);
- ui = div_u64(1000000000000LL, mipi_clock);
+ if (link_freq <= 0)
+ return 0;
+
+ ui = div_u64(1000000000000LL, link_freq);
ui /= 2;
t_hs_prepare_max = 85000 + 6 * ui;
t_hs_prepare_zero_min = 145000 + 10 * ui;
@@ -83,15 +82,14 @@ static u8 csiphy_settle_cnt_calc(u32 pixel_clock, u8 bpp, u8 num_lanes,
static void csiphy_lanes_enable(struct csiphy_device *csiphy,
struct csiphy_config *cfg,
- u32 pixel_clock, u8 bpp, u8 lane_mask)
+ s64 link_freq, u8 lane_mask)
{
struct csiphy_lanes_cfg *c = &cfg->csi2->lane_cfg;
u8 settle_cnt;
u8 val, l = 0;
int i = 0;
- settle_cnt = csiphy_settle_cnt_calc(pixel_clock, bpp, c->num_data,
- csiphy->timer_clk_rate);
+ settle_cnt = csiphy_settle_cnt_calc(link_freq, csiphy->timer_clk_rate);
writel_relaxed(0x1, csiphy->base +
CAMSS_CSI_PHY_GLBL_T_INIT_CFG0);
@@ -107,24 +107,23 @@ static irqreturn_t csiphy_isr(int irq, void *dev)
*
* Helper function to calculate settle count value. This is
* based on the CSI2 T_hs_settle parameter which in turn
- * is calculated based on the CSI2 transmitter pixel clock
- * frequency.
+ * is calculated based on the CSI2 transmitter link frequency.
*
- * Return settle count value or 0 if the CSI2 pixel clock
- * frequency is not available
+ * Return settle count value or 0 if the CSI2 link frequency
+ * is not available
*/
-static u8 csiphy_settle_cnt_calc(u32 pixel_clock, u8 bpp, u8 num_lanes,
- u32 timer_clk_rate)
+static u8 csiphy_settle_cnt_calc(s64 link_freq, u32 timer_clk_rate)
{
- u32 mipi_clock; /* Hz */
u32 ui; /* ps */
u32 timer_period; /* ps */
u32 t_hs_prepare_max; /* ps */
u32 t_hs_settle; /* ps */
u8 settle_cnt;
- mipi_clock = pixel_clock * bpp / (2 * num_lanes);
- ui = div_u64(1000000000000LL, mipi_clock);
+ if (link_freq <= 0)
+ return 0;
+
+ ui = div_u64(1000000000000LL, link_freq);
ui /= 2;
t_hs_prepare_max = 85000 + 6 * ui;
t_hs_settle = t_hs_prepare_max;
@@ -137,15 +136,14 @@ static u8 csiphy_settle_cnt_calc(u32 pixel_clock, u8 bpp, u8 num_lanes,
static void csiphy_lanes_enable(struct csiphy_device *csiphy,
struct csiphy_config *cfg,
- u32 pixel_clock, u8 bpp, u8 lane_mask)
+ s64 link_freq, u8 lane_mask)
{
struct csiphy_lanes_cfg *c = &cfg->csi2->lane_cfg;
u8 settle_cnt;
u8 val, l = 0;
int i;
- settle_cnt = csiphy_settle_cnt_calc(pixel_clock, bpp, c->num_data,
- csiphy->timer_clk_rate);
+ settle_cnt = csiphy_settle_cnt_calc(link_freq, csiphy->timer_clk_rate);
val = BIT(c->clk.pos);
for (i = 0; i < c->num_data; i++)
@@ -102,23 +102,23 @@ static u8 csiphy_get_bpp(const struct csiphy_format *formats,
static int csiphy_set_clock_rates(struct csiphy_device *csiphy)
{
struct device *dev = csiphy->camss->dev;
- u32 pixel_clock;
+ s64 link_freq;
int i, j;
int ret;
- ret = camss_get_pixel_clock(&csiphy->subdev.entity, &pixel_clock);
- if (ret)
- pixel_clock = 0;
+ u8 bpp = csiphy_get_bpp(csiphy->formats, csiphy->nformats,
+ csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
+ u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
+
+ link_freq = camss_get_link_freq(&csiphy->subdev.entity, bpp, num_lanes);
+ if (link_freq < 0)
+ link_freq = 0;
for (i = 0; i < csiphy->nclocks; i++) {
struct camss_clock *clock = &csiphy->clock[i];
if (csiphy->rate_set[i]) {
- u8 bpp = csiphy_get_bpp(csiphy->formats,
- csiphy->nformats,
- csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
- u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
- u64 min_rate = pixel_clock * bpp / (2 * num_lanes * 4);
+ u64 min_rate = link_freq / 4;
long round_rate;
camss_add_clock_margin(&min_rate);
@@ -238,22 +238,18 @@ static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
static int csiphy_stream_on(struct csiphy_device *csiphy)
{
struct csiphy_config *cfg = &csiphy->cfg;
- u32 pixel_clock;
+ s64 link_freq;
u8 lane_mask = csiphy_get_lane_mask(&cfg->csi2->lane_cfg);
u8 bpp = csiphy_get_bpp(csiphy->formats, csiphy->nformats,
csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
+ u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
u8 val;
- int ret;
- ret = camss_get_pixel_clock(&csiphy->subdev.entity, &pixel_clock);
- if (ret) {
- dev_err(csiphy->camss->dev,
- "Cannot get CSI2 transmitter's pixel clock\n");
- return -EINVAL;
- }
- if (!pixel_clock) {
+ link_freq = camss_get_link_freq(&csiphy->subdev.entity, bpp, num_lanes);
+
+ if (link_freq < 0) {
dev_err(csiphy->camss->dev,
- "Got pixel clock == 0, cannot continue\n");
+ "Cannot get CSI2 transmitter's link frequency\n");
return -EINVAL;
}
@@ -268,7 +264,7 @@ static int csiphy_stream_on(struct csiphy_device *csiphy)
writel_relaxed(val, csiphy->base_clk_mux);
wmb();
- csiphy->ops->lanes_enable(csiphy, cfg, pixel_clock, bpp, lane_mask);
+ csiphy->ops->lanes_enable(csiphy, cfg, link_freq, lane_mask);
return 0;
}
@@ -50,7 +50,7 @@ struct csiphy_hw_ops {
void (*reset)(struct csiphy_device *csiphy);
void (*lanes_enable)(struct csiphy_device *csiphy,
struct csiphy_config *cfg,
- u32 pixel_clock, u8 bpp, u8 lane_mask);
+ s64 link_freq, u8 lane_mask);
void (*lanes_disable)(struct csiphy_device *csiphy,
struct csiphy_config *cfg);
irqreturn_t (*isr)(int irq, void *dev);
@@ -548,6 +548,29 @@ struct media_entity *camss_find_sensor(struct media_entity *entity)
}
}
+/**
+ * camss_get_link_freq - Get link frequency from sensor
+ * @entity: Media entity in the current pipeline
+ * @bpp: Number of bits per pixel for the current format
+ * @lanes: Number of lanes in the link to the sensor
+ *
+ * Return link frequency on success or a negative error code otherwise
+ */
+s64 camss_get_link_freq(struct media_entity *entity, unsigned int bpp,
+ unsigned int lanes)
+{
+ struct media_entity *sensor;
+ struct v4l2_subdev *subdev;
+
+ sensor = camss_find_sensor(entity);
+ if (!sensor)
+ return -ENODEV;
+
+ subdev = media_entity_to_v4l2_subdev(sensor);
+
+ return v4l2_get_link_freq(subdev->ctrl_handler, bpp, 2 * lanes);
+}
+
/*
* camss_get_pixel_clock - Get pixel clock rate from sensor
* @entity: Media entity in the current pipeline
@@ -108,6 +108,8 @@ int camss_enable_clocks(int nclocks, struct camss_clock *clock,
struct device *dev);
void camss_disable_clocks(int nclocks, struct camss_clock *clock);
struct media_entity *camss_find_sensor(struct media_entity *entity);
+s64 camss_get_link_freq(struct media_entity *entity, unsigned int bpp,
+ unsigned int lanes);
int camss_get_pixel_clock(struct media_entity *entity, u32 *pixel_clock);
int camss_pm_domain_on(struct camss *camss, int id);
void camss_pm_domain_off(struct camss *camss, int id);