diff --git a/drivers/gpu/drm/vc4/vc4_hdmi.c b/drivers/gpu/drm/vc4/vc4_hdmi.c index 4d0b44a2ac61..bf537c6d413f 100644 --- a/drivers/gpu/drm/vc4/vc4_hdmi.c +++ b/drivers/gpu/drm/vc4/vc4_hdmi.c @@ -1170,6 +1170,67 @@ static const struct cec_adap_ops vc4_hdmi_cec_adap_ops = { .adap_log_addr = vc4_hdmi_cec_adap_log_addr, .adap_transmit = vc4_hdmi_cec_adap_transmit, }; + +static int vc4_hdmi_cec_init(struct vc4_hdmi *vc4_hdmi) +{ + struct cec_connector_info conn_info; + struct platform_device *pdev = vc4_hdmi->pdev; + u32 value; + int ret; + + vc4_hdmi->cec_adap = cec_allocate_adapter(&vc4_hdmi_cec_adap_ops, + vc4_hdmi, "vc4", + CEC_CAP_DEFAULTS | + CEC_CAP_CONNECTOR_INFO, 1); + ret = PTR_ERR_OR_ZERO(vc4_hdmi->cec_adap); + if (ret < 0) + return ret; + + cec_fill_conn_info_from_drm(&conn_info, &vc4_hdmi->connector); + cec_s_conn_info(vc4_hdmi->cec_adap, &conn_info); + + HDMI_WRITE(HDMI_CEC_CPU_MASK_SET, 0xffffffff); + value = HDMI_READ(HDMI_CEC_CNTRL_1); + value &= ~VC4_HDMI_CEC_DIV_CLK_CNT_MASK; + /* + * Set the logical address to Unregistered and set the clock + * divider: the hsm_clock rate and this divider setting will + * give a 40 kHz CEC clock. + */ + value |= VC4_HDMI_CEC_ADDR_MASK | + (4091 << VC4_HDMI_CEC_DIV_CLK_CNT_SHIFT); + HDMI_WRITE(HDMI_CEC_CNTRL_1, value); + ret = devm_request_threaded_irq(&pdev->dev, platform_get_irq(pdev, 0), + vc4_cec_irq_handler, + vc4_cec_irq_handler_thread, 0, + "vc4 hdmi cec", vc4_hdmi); + if (ret) + goto err_delete_cec_adap; + + ret = cec_register_adapter(vc4_hdmi->cec_adap, &pdev->dev); + if (ret < 0) + goto err_delete_cec_adap; + + return 0; + +err_delete_cec_adap: + cec_delete_adapter(vc4_hdmi->cec_adap); + + return ret; +} + +static void vc4_hdmi_cec_exit(struct vc4_hdmi *vc4_hdmi) +{ + cec_unregister_adapter(vc4_hdmi->cec_adap); +} +#else +static int vc4_hdmi_cec_init(struct vc4_hdmi *vc4_hdmi) +{ + return 0; +} + +static void vc4_hdmi_cec_exit(struct vc4_hdmi *vc4_hdmi) {}; + #endif static int vc4_hdmi_build_regset(struct vc4_hdmi *vc4_hdmi, @@ -1249,9 +1310,6 @@ static int vc4_hdmi_init_resources(struct vc4_hdmi *vc4_hdmi) static int vc4_hdmi_bind(struct device *dev, struct device *master, void *data) { -#ifdef CONFIG_DRM_VC4_HDMI_CEC - struct cec_connector_info conn_info; -#endif const struct vc4_hdmi_variant *variant = of_device_get_match_data(dev); struct platform_device *pdev = to_platform_device(dev); struct drm_device *drm = dev_get_drvdata(master); @@ -1331,43 +1389,13 @@ static int vc4_hdmi_bind(struct device *dev, struct device *master, void *data) if (ret) goto err_destroy_encoder; -#ifdef CONFIG_DRM_VC4_HDMI_CEC - vc4_hdmi->cec_adap = cec_allocate_adapter(&vc4_hdmi_cec_adap_ops, - vc4_hdmi, "vc4", - CEC_CAP_DEFAULTS | - CEC_CAP_CONNECTOR_INFO, 1); - ret = PTR_ERR_OR_ZERO(vc4_hdmi->cec_adap); - if (ret < 0) - goto err_destroy_conn; - - cec_fill_conn_info_from_drm(&conn_info, &vc4_hdmi->connector); - cec_s_conn_info(vc4_hdmi->cec_adap, &conn_info); - - HDMI_WRITE(HDMI_CEC_CPU_MASK_SET, 0xffffffff); - value = HDMI_READ(HDMI_CEC_CNTRL_1); - value &= ~VC4_HDMI_CEC_DIV_CLK_CNT_MASK; - /* - * Set the logical address to Unregistered and set the clock - * divider: the hsm_clock rate and this divider setting will - * give a 40 kHz CEC clock. - */ - value |= VC4_HDMI_CEC_ADDR_MASK | - (4091 << VC4_HDMI_CEC_DIV_CLK_CNT_SHIFT); - HDMI_WRITE(HDMI_CEC_CNTRL_1, value); - ret = devm_request_threaded_irq(dev, platform_get_irq(pdev, 0), - vc4_cec_irq_handler, - vc4_cec_irq_handler_thread, 0, - "vc4 hdmi cec", vc4_hdmi); + ret = vc4_hdmi_cec_init(vc4_hdmi); if (ret) - goto err_delete_cec_adap; - ret = cec_register_adapter(vc4_hdmi->cec_adap, dev); - if (ret < 0) - goto err_delete_cec_adap; -#endif + goto err_destroy_conn; ret = vc4_hdmi_audio_init(vc4_hdmi); if (ret) - goto err_destroy_encoder; + goto err_free_cec; vc4_debugfs_add_file(drm, variant->debugfs_name, vc4_hdmi_debugfs_regs, @@ -1375,12 +1403,10 @@ static int vc4_hdmi_bind(struct device *dev, struct device *master, void *data) return 0; -#ifdef CONFIG_DRM_VC4_HDMI_CEC -err_delete_cec_adap: - cec_delete_adapter(vc4_hdmi->cec_adap); +err_free_cec: + vc4_hdmi_cec_exit(vc4_hdmi); err_destroy_conn: vc4_hdmi_connector_destroy(&vc4_hdmi->connector); -#endif err_destroy_encoder: drm_encoder_cleanup(encoder); err_unprepare_hsm: @@ -1421,7 +1447,7 @@ static void vc4_hdmi_unbind(struct device *dev, struct device *master, kfree(vc4_hdmi->hdmi_regset.regs); kfree(vc4_hdmi->hd_regset.regs); - cec_unregister_adapter(vc4_hdmi->cec_adap); + vc4_hdmi_cec_exit(vc4_hdmi); vc4_hdmi_connector_destroy(&vc4_hdmi->connector); drm_encoder_cleanup(&vc4_hdmi->encoder.base.base);