static int __dpi_config_port(struct mdfld_dsi_config *dsi_config,
			struct panel_funcs *p_funcs, enum enum_ports port)
{
	struct mdfld_dsi_hw_registers *regs;
	struct mdfld_dsi_hw_context *ctx;
	struct drm_device *dev;
	int offset = 0;

	if (!dsi_config)
		return -EINVAL;

	regs = &dsi_config->regs;
	ctx = &dsi_config->dsi_hw_context;
	dev = dsi_config->dev;

	if (port == PORT_C)
		offset = 0x800;

	/*exit ULPS state*/
	__dpi_exit_ulps_locked(dsi_config, offset);

	/*Enable DSI Controller*/
	REG_WRITE(regs->device_ready_reg + offset, ctx->device_ready | BIT0);

	/*set low power output hold*/
	if (port == PORT_C)
		offset = 0x1000;
	REG_WRITE(regs->mipi_reg + offset, (ctx->mipi | BIT16));

	return 0;
}
static int __dpi_enter_ulps_locked(struct mdfld_dsi_config *dsi_config)
{
    struct mdfld_dsi_hw_registers *regs = &dsi_config->regs;
    struct mdfld_dsi_hw_context *ctx = &dsi_config->dsi_hw_context;
    struct drm_device *dev = dsi_config->dev;
    struct mdfld_dsi_pkg_sender *sender
        = mdfld_dsi_get_pkg_sender(dsi_config);
    int retry = 5;

ulps_recovery:

    PSB_DEBUG_ENTRY("\n");
    ctx->device_ready = REG_READ(regs->device_ready_reg);

    if (ctx->device_ready & DSI_POWER_STATE_ULPS_MASK) {
        DRM_ERROR("Broken ULPS states\n");
        return -EINVAL;
    }

    if (!sender) {
        DRM_ERROR("Cannot get sender\n");
        return -EINVAL;
    }

    /* check latch out, expected to be lp11 */
    intel_dc_dsi_check_latch_out(dsi_config);

    /*wait for all FIFOs empty*/
    mdfld_dsi_wait_for_fifos_empty(sender);

    /*inform DSI host is to be put on ULPS*/
    ctx->device_ready |= (DSI_POWER_STATE_ULPS_ENTER | DSI_DEVICE_READY);
    REG_WRITE(regs->device_ready_reg, ctx->device_ready);
    mdelay(1);

    /* check latch out, expected to be lp00 */
    intel_dc_dsi_check_latch_out(dsi_config);

    if (intel_dc_dsi_wait_for_lp00(dsi_config)) {
        DRM_ERROR("faild to wait for lp00 status\n");
        DRM_ERROR("reg[0x%x] = 0x%x\n", regs->mipi_reg, REG_READ(regs->mipi_reg));

        if (retry--) {
            DRM_INFO("%s: retry enter ulps (times=%d)\n", __func__, 5-retry);
            if (__dpi_exit_ulps_locked(dsi_config)) {
                DRM_ERROR("Failed to exit ULPS\n");
            }
            goto ulps_recovery;
        }
    }

    /* set AFE hold value*/
    REG_WRITE(regs->mipi_reg,
              REG_READ(regs->mipi_reg) & (~PASS_FROM_SPHY_TO_AFE));

    DRM_INFO("%s: entered ULPS state\n", __func__);
    return 0;
}
/**
 * Power on sequence for video mode MIPI panel.
 * NOTE: do NOT modify this function
 */
static int __dpi_panel_power_on(struct mdfld_dsi_config *dsi_config,
                                struct panel_funcs *p_funcs)
{
    u32 val = 0;
    struct mdfld_dsi_hw_registers *regs;
    struct mdfld_dsi_hw_context *ctx;
    struct drm_psb_private *dev_priv;
    struct drm_device *dev;
    int retry, reset_count = 10;
    int i;
    int err = 0;

    if (!dsi_config)
        return -EINVAL;

    regs = &dsi_config->regs;
    ctx = &dsi_config->dsi_hw_context;
    dev = dsi_config->dev;
    dev_priv = dev->dev_private;

    if (!ospm_power_using_hw_begin(OSPM_DISPLAY_ISLAND,
                                   OSPM_UHB_FORCE_POWER_ON))
        return -EAGAIN;
reset_recovery:
    --reset_count;
    /*HW-Reset*/
    if (p_funcs && p_funcs->reset)
        p_funcs->reset(dsi_config);

    /*Enable DSI PLL*/
    if (!(REG_READ(regs->dpll_reg) & BIT31)) {
        if(ctx->pll_bypass_mode) {
            uint32_t dpll = 0;

            REG_WRITE(regs->dpll_reg, dpll);
            if (ctx->cck_div) {
                dpll = dpll | BIT11;
                REG_WRITE(regs->dpll_reg, dpll);
                udelay(1);
            }
            dpll = dpll | BIT12;
            REG_WRITE(regs->dpll_reg, dpll);
            mdelay(1);
            dpll = dpll | BIT13;
            REG_WRITE(regs->dpll_reg, dpll);
            mdelay(1);
        } else {
            REG_WRITE(regs->dpll_reg, 0x0);
            REG_WRITE(regs->fp_reg, 0x0);
            REG_WRITE(regs->fp_reg, ctx->fp);
            REG_WRITE(regs->dpll_reg, ((ctx->dpll) & ~BIT30));

            udelay(2);
            val = REG_READ(regs->dpll_reg);
            REG_WRITE(regs->dpll_reg, (val | BIT31));

            /*wait for PLL lock on pipe*/
            retry = 10000;
            while (--retry && !(REG_READ(PIPEACONF) & BIT29))
                udelay(3);
            if (!retry) {
                DRM_ERROR("PLL failed to lock on pipe\n");
                err = -EAGAIN;
                goto power_on_err;
            }
        }
    }

    REG_WRITE(regs->eot_disable_reg,
              (REG_READ(regs->eot_disable_reg) & ~BIT1));
    REG_WRITE(regs->device_ready_reg, ~BIT0);
    REG_WRITE(regs->device_ready_reg, BIT0);
    mdelay(1);

    /*exit ULPS*/
    if (__dpi_exit_ulps_locked(dsi_config)) {
        DRM_ERROR("Failed to exit ULPS\n");
        goto power_on_err;
    }

    /*update MIPI port config*/
    REG_WRITE(regs->mipi_reg, (ctx->mipi | REG_READ(regs->mipi_reg)));

    /*unready dsi adapter for re-programming*/
    REG_WRITE(regs->device_ready_reg,
              REG_READ(regs->device_ready_reg) & ~(DSI_DEVICE_READY));

    /*D-PHY parameter*/
    REG_WRITE(regs->dphy_param_reg, ctx->dphy_param);

    /*Configure DSI controller*/
    REG_WRITE(regs->mipi_control_reg, ctx->mipi_control);
    REG_WRITE(regs->intr_en_reg, ctx->intr_en);
    REG_WRITE(regs->hs_tx_timeout_reg, ctx->hs_tx_timeout);
    REG_WRITE(regs->lp_rx_timeout_reg, ctx->lp_rx_timeout);
    REG_WRITE(regs->turn_around_timeout_reg,
              ctx->turn_around_timeout);
    REG_WRITE(regs->device_reset_timer_reg,
              ctx->device_reset_timer);
    REG_WRITE(regs->high_low_switch_count_reg,
              ctx->high_low_switch_count);
    REG_WRITE(regs->init_count_reg, ctx->init_count);
    REG_WRITE(regs->eot_disable_reg, ctx->eot_disable);
    REG_WRITE(regs->lp_byteclk_reg, ctx->lp_byteclk);
    REG_WRITE(regs->clk_lane_switch_time_cnt_reg,
              ctx->clk_lane_switch_time_cnt);
    if (ctx->pll_bypass_mode) {
        /*Force using non-burst pulse event mode here*/
        REG_WRITE(regs->video_mode_format_reg, 0x1);
    } else {
        REG_WRITE(regs->video_mode_format_reg, ctx->video_mode_format);
    }
    REG_WRITE(regs->dsi_func_prg_reg, ctx->dsi_func_prg);

    /*DSI timing*/
    REG_WRITE(regs->dpi_resolution_reg, ctx->dpi_resolution);
    REG_WRITE(regs->hsync_count_reg, ctx->hsync_count);
    REG_WRITE(regs->hbp_count_reg, ctx->hbp_count);
    REG_WRITE(regs->hfp_count_reg, ctx->hfp_count);
    REG_WRITE(regs->hactive_count_reg, ctx->hactive_count);
    REG_WRITE(regs->vsync_count_reg, ctx->vsync_count);
    REG_WRITE(regs->vbp_count_reg, ctx->vbp_count);
    REG_WRITE(regs->vfp_count_reg, ctx->vfp_count);

    /*Setup pipe timing*/
    REG_WRITE(regs->htotal_reg, ctx->htotal);
    REG_WRITE(regs->hblank_reg, ctx->hblank);
    REG_WRITE(regs->hsync_reg, ctx->hsync);
    REG_WRITE(regs->vtotal_reg, ctx->vtotal);
    REG_WRITE(regs->vblank_reg, ctx->vblank);
    REG_WRITE(regs->vsync_reg, ctx->vsync);
    REG_WRITE(regs->pipesrc_reg, ctx->pipesrc);

    REG_WRITE(regs->dsppos_reg, ctx->dsppos);
    REG_WRITE(regs->dspstride_reg, ctx->dspstride);

    /*Setup plane*/
    REG_WRITE(regs->dspsize_reg, ctx->dspsize);
    REG_WRITE(regs->dspsurf_reg, ctx->dspsurf);
    REG_WRITE(regs->dsplinoff_reg, ctx->dsplinoff);
    REG_WRITE(regs->vgacntr_reg, ctx->vgacntr);

    /*restore color_coef (chrome) */
    for (i = 0; i < 6; i++) {
        REG_WRITE(regs->color_coef_reg + (i<<2), ctx->color_coef[i]);
    }

    /* restore palette (gamma) */
    for (i = 0; i < 256; i++)
        REG_WRITE(regs->palette_reg + (i<<2), ctx->palette[i]);

    /* restore gamma correction max (RGB) */
    REG_WRITE(regs->gamma_red_max_reg, ctx->gamma_red_max);
    REG_WRITE(regs->gamma_green_max_reg, ctx->gamma_green_max);
    REG_WRITE(regs->gamma_blue_max_reg, ctx->gamma_blue_max);

#ifdef CONFIG_CTP_DPST
    /* restore dpst setting */
    if (dev_priv->psb_dpst_state) {
        dpstmgr_reg_restore_locked(dsi_config);
        psb_enable_pipestat(dev_priv, 0, PIPE_DPST_EVENT_ENABLE);
    }
#endif

    /*Enable DSI Controller*/
    REG_WRITE(regs->device_ready_reg, (REG_READ(regs->device_ready_reg) | BIT0));

    /**
     * Different panel may have different ways to have
     * drvIC initialized. Support it!
     */
    if (p_funcs && p_funcs->drv_ic_init) {
        if (p_funcs->drv_ic_init(dsi_config)) {
            if (!reset_count) {
                goto reset_err_bypass;
            }
            pmu_nc_set_power_state(OSPM_MIPI_ISLAND,
                                   OSPM_ISLAND_DOWN, OSPM_REG_TYPE);

            pmu_nc_set_power_state(OSPM_MIPI_ISLAND,
                                   OSPM_ISLAND_UP, OSPM_REG_TYPE);

            DRM_ERROR("Failed to init dsi controller, reset it!\n");
            goto reset_recovery;
        }
    }

reset_err_bypass:

    if (ctx->pll_bypass_mode) {
        /*Reprogram to use pll clock*/
        REG_WRITE(regs->dpll_reg, 0x0);
        REG_WRITE(regs->fp_reg, 0x0);
        REG_WRITE(regs->fp_reg, ctx->fp);
        REG_WRITE(regs->dpll_reg, ((ctx->dpll) & ~BIT30));

        udelay(2);
        REG_WRITE(regs->dpll_reg, 0x2 << 17);
        mdelay(1);
        REG_WRITE(regs->dpll_reg, (0x2 << 17 | BIT31));

        /*wait for PLL lock on pipe*/
        retry = 10000;
        while (--retry && !(REG_READ(PIPEACONF) & BIT29))
            udelay(3);
        if (!retry) {
            DRM_ERROR("PLL failed to lock on pipe\n");
            err = -EAGAIN;
            goto power_on_err;
        }

        /*Switch back to burst mode*/
        REG_WRITE(regs->video_mode_format_reg, ctx->video_mode_format);

        /*Clear device ready reg*/
        REG_WRITE(regs->device_ready_reg, REG_READ(regs->device_ready_reg) & ~DSI_DEVICE_READY);
        msleep(1);
        /*Enable DSI Controller*/
        REG_WRITE(regs->device_ready_reg, REG_READ(regs->device_ready_reg) | DSI_DEVICE_READY);
        msleep(1);
    }

    /**
     * Different panel may have different ways to have
     * panel turned on. Support it!
     */
    if (p_funcs && p_funcs->power_on)
        if (p_funcs->power_on(dsi_config)) {
            DRM_ERROR("Failed to power on panel\n");
            err = -EAGAIN;
            goto power_on_err;
        }

    /*Enable MIPI Port*/
    REG_WRITE(regs->mipi_reg, (ctx->mipi | BIT31));

    /*Enable pipe*/
    val = ctx->pipeconf;
    val &= ~0x000c0000;
    val |= BIT31;

    /* disable gamma if needed */
    if (drm_psb_enable_color_conversion == 0)
        val &= ~(PIPEACONF_COLOR_MATRIX_ENABLE);


    REG_WRITE(regs->pipeconf_reg, val);
    REG_WRITE(regs->pipestat_reg, ctx->pipestat |
              PIPE_VBLANK_INTERRUPT_ENABLE);

    /*Wait for pipe enabling,when timing generator
    is wroking */
    if (REG_READ(regs->mipi_reg) & BIT31) {
        retry = 10000;
        while (--retry && !(REG_READ(regs->pipeconf_reg) & BIT30))
            udelay(3);

        if (!retry) {
            DRM_ERROR("Failed to enable pipe\n");
            err = -EAGAIN;
            goto power_on_err;
        }
    }
    /*enable plane*/
    val = ctx->dspcntr | BIT31;

    /* disable gamma if needed */
    if (drm_psb_enable_gamma == 0)
        val &= ~(PIPEACONF_GAMMA);

    REG_WRITE(regs->dspcntr_reg, val);

    /*Notify PVR module that screen is on*/
    if (dev_priv->pvr_screen_event_handler)
        dev_priv->pvr_screen_event_handler(dev, 1);

    if (p_funcs && p_funcs->set_brightness)
        if (p_funcs->set_brightness(dsi_config, ctx->lastbrightnesslevel))
            DRM_ERROR("Failed to set panel brightness\n");

power_on_err:
    ospm_power_using_hw_end(OSPM_DISPLAY_ISLAND);
    return err;
}