C++ readl_relaxed函数代码示例

static void __pll_clk_disable_reg(void __iomem *mode_reg)
{
	u32 mode = readl_relaxed(mode_reg);
	mode &= ~PLL_MODE_MASK;
	writel_relaxed(mode, mode_reg);
}

/* Initialize the DLL (Programmable Delay Line) */
static int msm_init_cm_dll(struct sdhci_host *host)
{
	struct mmc_host *mmc = host->mmc;
	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
	struct sdhci_msm_host *msm_host = sdhci_pltfm_priv(pltfm_host);
	int wait_cnt = 50;
	unsigned long flags;
	u32 config;

	spin_lock_irqsave(&host->lock, flags);

	/*
	 * Make sure that clock is always enabled when DLL
	 * tuning is in progress. Keeping PWRSAVE ON may
	 * turn off the clock.
	 */
	config = readl_relaxed(host->ioaddr + CORE_VENDOR_SPEC);
	config &= ~CORE_CLK_PWRSAVE;
	writel_relaxed(config, host->ioaddr + CORE_VENDOR_SPEC);

	if (msm_host->use_14lpp_dll_reset) {
		config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG);
		config &= ~CORE_CK_OUT_EN;
		writel_relaxed(config, host->ioaddr + CORE_DLL_CONFIG);

		config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG_2);
		config |= CORE_DLL_CLOCK_DISABLE;
		writel_relaxed(config, host->ioaddr + CORE_DLL_CONFIG_2);
	}

	config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG);
	config |= CORE_DLL_RST;
	writel_relaxed(config, host->ioaddr + CORE_DLL_CONFIG);

	config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG);
	config |= CORE_DLL_PDN;
	writel_relaxed(config, host->ioaddr + CORE_DLL_CONFIG);
	msm_cm_dll_set_freq(host);

	if (msm_host->use_14lpp_dll_reset &&
	    !IS_ERR_OR_NULL(msm_host->xo_clk)) {
		u32 mclk_freq = 0;

		config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG_2);
		config &= CORE_FLL_CYCLE_CNT;
		if (config)
			mclk_freq = DIV_ROUND_CLOSEST_ULL((host->clock * 8),
					clk_get_rate(msm_host->xo_clk));
		else
			mclk_freq = DIV_ROUND_CLOSEST_ULL((host->clock * 4),
					clk_get_rate(msm_host->xo_clk));

		config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG_2);
		config &= ~(0xFF << 10);
		config |= mclk_freq << 10;

		writel_relaxed(config, host->ioaddr + CORE_DLL_CONFIG_2);
		/* wait for 5us before enabling DLL clock */
		udelay(5);
	}

	config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG);
	config &= ~CORE_DLL_RST;
	writel_relaxed(config, host->ioaddr + CORE_DLL_CONFIG);

	config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG);
	config &= ~CORE_DLL_PDN;
	writel_relaxed(config, host->ioaddr + CORE_DLL_CONFIG);

	if (msm_host->use_14lpp_dll_reset) {
		msm_cm_dll_set_freq(host);
		config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG_2);
		config &= ~CORE_DLL_CLOCK_DISABLE;
		writel_relaxed(config, host->ioaddr + CORE_DLL_CONFIG_2);
	}

	config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG);
	config |= CORE_DLL_EN;
	writel_relaxed(config, host->ioaddr + CORE_DLL_CONFIG);

	config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG);
	config |= CORE_CK_OUT_EN;
	writel_relaxed(config, host->ioaddr + CORE_DLL_CONFIG);

	/* Wait until DLL_LOCK bit of DLL_STATUS register becomes '1' */
	while (!(readl_relaxed(host->ioaddr + CORE_DLL_STATUS) &
		 CORE_DLL_LOCK)) {
		/* max. wait for 50us sec for LOCK bit to be set */
		if (--wait_cnt == 0) {
			dev_err(mmc_dev(mmc), "%s: DLL failed to LOCK\n",
			       mmc_hostname(mmc));
			spin_unlock_irqrestore(&host->lock, flags);
			return -ETIMEDOUT;
		}
		udelay(1);
	}

	spin_unlock_irqrestore(&host->lock, flags);
	return 0;
//.........这里部分代码省略.........

static void sdhci_msm_set_uhs_signaling(struct sdhci_host *host,
					unsigned int uhs)
{
	struct mmc_host *mmc = host->mmc;
	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
	struct sdhci_msm_host *msm_host = sdhci_pltfm_priv(pltfm_host);
	u16 ctrl_2;
	u32 config;

	ctrl_2 = sdhci_readw(host, SDHCI_HOST_CONTROL2);
	/* Select Bus Speed Mode for host */
	ctrl_2 &= ~SDHCI_CTRL_UHS_MASK;
	switch (uhs) {
	case MMC_TIMING_UHS_SDR12:
		ctrl_2 |= SDHCI_CTRL_UHS_SDR12;
		break;
	case MMC_TIMING_UHS_SDR25:
		ctrl_2 |= SDHCI_CTRL_UHS_SDR25;
		break;
	case MMC_TIMING_UHS_SDR50:
		ctrl_2 |= SDHCI_CTRL_UHS_SDR50;
		break;
	case MMC_TIMING_MMC_HS400:
	case MMC_TIMING_MMC_HS200:
	case MMC_TIMING_UHS_SDR104:
		ctrl_2 |= SDHCI_CTRL_UHS_SDR104;
		break;
	case MMC_TIMING_UHS_DDR50:
	case MMC_TIMING_MMC_DDR52:
		ctrl_2 |= SDHCI_CTRL_UHS_DDR50;
		break;
	}

	/*
	 * When clock frequency is less than 100MHz, the feedback clock must be
	 * provided and DLL must not be used so that tuning can be skipped. To
	 * provide feedback clock, the mode selection can be any value less
	 * than 3'b011 in bits [2:0] of HOST CONTROL2 register.
	 */
	if (host->clock <= CORE_FREQ_100MHZ) {
		if (uhs == MMC_TIMING_MMC_HS400 ||
		    uhs == MMC_TIMING_MMC_HS200 ||
		    uhs == MMC_TIMING_UHS_SDR104)
			ctrl_2 &= ~SDHCI_CTRL_UHS_MASK;
		/*
		 * DLL is not required for clock <= 100MHz
		 * Thus, make sure DLL it is disabled when not required
		 */
		config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG);
		config |= CORE_DLL_RST;
		writel_relaxed(config, host->ioaddr + CORE_DLL_CONFIG);

		config = readl_relaxed(host->ioaddr + CORE_DLL_CONFIG);
		config |= CORE_DLL_PDN;
		writel_relaxed(config, host->ioaddr + CORE_DLL_CONFIG);

		/*
		 * The DLL needs to be restored and CDCLP533 recalibrated
		 * when the clock frequency is set back to 400MHz.
		 */
		msm_host->calibration_done = false;
	}

	dev_dbg(mmc_dev(mmc), "%s: clock=%u uhs=%u ctrl_2=0x%x\n",
		mmc_hostname(host->mmc), host->clock, uhs, ctrl_2);
	sdhci_writew(host, ctrl_2, SDHCI_HOST_CONTROL2);

	if (mmc->ios.timing == MMC_TIMING_MMC_HS400)
		sdhci_msm_hs400(host, &mmc->ios);
}

static int __init gicv2m_init_one(struct device_node *node,
				  struct irq_domain *parent)
{
	int ret;
	struct v2m_data *v2m;
	struct irq_domain *inner_domain, *pci_domain, *plat_domain;

	v2m = kzalloc(sizeof(struct v2m_data), GFP_KERNEL);
	if (!v2m) {
		pr_err("Failed to allocate struct v2m_data.\n");
		return -ENOMEM;
	}

	ret = of_address_to_resource(node, 0, &v2m->res);
	if (ret) {
		pr_err("Failed to allocate v2m resource.\n");
		goto err_free_v2m;
	}

	v2m->base = ioremap(v2m->res.start, resource_size(&v2m->res));
	if (!v2m->base) {
		pr_err("Failed to map GICv2m resource\n");
		ret = -ENOMEM;
		goto err_free_v2m;
	}

	if (!of_property_read_u32(node, "arm,msi-base-spi", &v2m->spi_start) &&
	    !of_property_read_u32(node, "arm,msi-num-spis", &v2m->nr_spis)) {
		pr_info("Overriding V2M MSI_TYPER (base:%u, num:%u)\n",
			v2m->spi_start, v2m->nr_spis);
	} else {
		u32 typer = readl_relaxed(v2m->base + V2M_MSI_TYPER);

		v2m->spi_start = V2M_MSI_TYPER_BASE_SPI(typer);
		v2m->nr_spis = V2M_MSI_TYPER_NUM_SPI(typer);
	}

	if (!is_msi_spi_valid(v2m->spi_start, v2m->nr_spis)) {
		ret = -EINVAL;
		goto err_iounmap;
	}

	v2m->bm = kzalloc(sizeof(long) * BITS_TO_LONGS(v2m->nr_spis),
			  GFP_KERNEL);
	if (!v2m->bm) {
		ret = -ENOMEM;
		goto err_iounmap;
	}

	inner_domain = irq_domain_add_tree(node, &gicv2m_domain_ops, v2m);
	if (!inner_domain) {
		pr_err("Failed to create GICv2m domain\n");
		ret = -ENOMEM;
		goto err_free_bm;
	}

	inner_domain->bus_token = DOMAIN_BUS_NEXUS;
	inner_domain->parent = parent;
	pci_domain = pci_msi_create_irq_domain(node, &gicv2m_msi_domain_info,
					       inner_domain);
	plat_domain = platform_msi_create_irq_domain(node,
						     &gicv2m_pmsi_domain_info,
						     inner_domain);
	if (!pci_domain || !plat_domain) {
		pr_err("Failed to create MSI domains\n");
		ret = -ENOMEM;
		goto err_free_domains;
	}

	spin_lock_init(&v2m->msi_cnt_lock);

	pr_info("Node %s: range[%#lx:%#lx], SPI[%d:%d]\n", node->name,
		(unsigned long)v2m->res.start, (unsigned long)v2m->res.end,
		v2m->spi_start, (v2m->spi_start + v2m->nr_spis));

	return 0;

err_free_domains:
	if (plat_domain)
		irq_domain_remove(plat_domain);
	if (pci_domain)
		irq_domain_remove(pci_domain);
	if (inner_domain)
		irq_domain_remove(inner_domain);
err_free_bm:
	kfree(v2m->bm);
err_iounmap:
	iounmap(v2m->base);
err_free_v2m:
	kfree(v2m);
	return ret;
}

static inline void flush_fifo(struct rockchip_spi *rs)
{
	while (readl_relaxed(rs->regs + ROCKCHIP_SPI_RXFLR))
		readl_relaxed(rs->regs + ROCKCHIP_SPI_RXDR);
}

static void sdhci_msm_handle_pwr_irq(struct sdhci_host *host, int irq)
{
	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
	struct sdhci_msm_host *msm_host = sdhci_pltfm_priv(pltfm_host);
	u32 irq_status, irq_ack = 0;
	int retry = 10;
	int pwr_state = 0, io_level = 0;


	irq_status = readl_relaxed(msm_host->core_mem + CORE_PWRCTL_STATUS);
	irq_status &= INT_MASK;

	writel_relaxed(irq_status, msm_host->core_mem + CORE_PWRCTL_CLEAR);

	/*
	 * There is a rare HW scenario where the first clear pulse could be
	 * lost when actual reset and clear/read of status register is
	 * happening at a time. Hence, retry for at least 10 times to make
	 * sure status register is cleared. Otherwise, this will result in
	 * a spurious power IRQ resulting in system instability.
	 */
	while (irq_status & readl_relaxed(msm_host->core_mem +
				CORE_PWRCTL_STATUS)) {
		if (retry == 0) {
			pr_err("%s: Timedout clearing (0x%x) pwrctl status register\n",
					mmc_hostname(host->mmc), irq_status);
			sdhci_msm_dump_pwr_ctrl_regs(host);
			WARN_ON(1);
			break;
		}
		writel_relaxed(irq_status,
				msm_host->core_mem + CORE_PWRCTL_CLEAR);
		retry--;
		udelay(10);
	}

	/* Handle BUS ON/OFF*/
	if (irq_status & CORE_PWRCTL_BUS_ON) {
		pwr_state = REQ_BUS_ON;
		io_level = REQ_IO_HIGH;
		irq_ack |= CORE_PWRCTL_BUS_SUCCESS;
	}
	if (irq_status & CORE_PWRCTL_BUS_OFF) {
		pwr_state = REQ_BUS_OFF;
		io_level = REQ_IO_LOW;
		irq_ack |= CORE_PWRCTL_BUS_SUCCESS;
	}
	/* Handle IO LOW/HIGH */
	if (irq_status & CORE_PWRCTL_IO_LOW) {
		io_level = REQ_IO_LOW;
		irq_ack |= CORE_PWRCTL_IO_SUCCESS;
	}
	if (irq_status & CORE_PWRCTL_IO_HIGH) {
		io_level = REQ_IO_HIGH;
		irq_ack |= CORE_PWRCTL_IO_SUCCESS;
	}

	/*
	 * The driver has to acknowledge the interrupt, switch voltages and
	 * report back if it succeded or not to this register. The voltage
	 * switches are handled by the sdhci core, so just report success.
	 */
	writel_relaxed(irq_ack, msm_host->core_mem + CORE_PWRCTL_CTL);

	if (pwr_state)
		msm_host->curr_pwr_state = pwr_state;
	if (io_level)
		msm_host->curr_io_level = io_level;

	pr_debug("%s: %s: Handled IRQ(%d), irq_status=0x%x, ack=0x%x\n",
		mmc_hostname(msm_host->mmc), __func__, irq, irq_status,
		irq_ack);
}

int hdmi_pll_enable(void)
{
	unsigned int val;
	u32 ahb_en_reg, ahb_enabled;
	unsigned int timeout_count;
	int pll_lock_retry = 10;

	ahb_en_reg = readl_relaxed(AHB_EN_REG);
	ahb_enabled = ahb_en_reg & BIT(4);
	if (!ahb_enabled) {
		writel_relaxed(ahb_en_reg | BIT(4), AHB_EN_REG);
		/* Make sure iface clock is enabled before register access */
		mb();
	}

	/* Assert PLL S/W reset */
	writel_relaxed(0x8D, HDMI_PHY_PLL_LOCKDET_CFG2);
	writel_relaxed(0x10, HDMI_PHY_PLL_LOCKDET_CFG0);
	writel_relaxed(0x1A, HDMI_PHY_PLL_LOCKDET_CFG1);
	/* Wait for a short time before de-asserting
	 * to allow the hardware to complete its job.
	 * This much of delay should be fine for hardware
	 * to assert and de-assert.
	 */
	udelay(10);
	/* De-assert PLL S/W reset */
	writel_relaxed(0x0D, HDMI_PHY_PLL_LOCKDET_CFG2);

	val = readl_relaxed(HDMI_PHY_REG_12);
	val |= BIT(5);
	/* Assert PHY S/W reset */
	writel_relaxed(val, HDMI_PHY_REG_12);
	val &= ~BIT(5);
	/* Wait for a short time before de-asserting
	   to allow the hardware to complete its job.
	   This much of delay should be fine for hardware
	   to assert and de-assert. */
	udelay(10);
	/* De-assert PHY S/W reset */
	writel_relaxed(val, HDMI_PHY_REG_12);
	writel_relaxed(0x3f, HDMI_PHY_REG_2);

	val = readl_relaxed(HDMI_PHY_REG_12);
	val |= PWRDN_B;
	writel_relaxed(val, HDMI_PHY_REG_12);
	/* Wait 10 us for enabling global power for PHY */
	mb();
	udelay(10);

	val = readl_relaxed(HDMI_PHY_PLL_PWRDN_B);
	val |= PLL_PWRDN_B;
	val &= ~PD_PLL;
	writel_relaxed(val, HDMI_PHY_PLL_PWRDN_B);
	writel_relaxed(0x80, HDMI_PHY_REG_2);

	timeout_count = 1000;
	while (!(readl_relaxed(HDMI_PHY_PLL_STATUS0) & BIT(0)) &&
			timeout_count && pll_lock_retry) {
		if (--timeout_count == 0) {
			/*
			 * PLL has still not locked.
			 * Do a software reset and try again
			 * Assert PLL S/W reset first
			 */
			writel_relaxed(0x8D, HDMI_PHY_PLL_LOCKDET_CFG2);

			/* Wait for a short time before de-asserting
			 * to allow the hardware to complete its job.
			 * This much of delay should be fine for hardware
			 * to assert and de-assert.
			 */
			udelay(10);
			writel_relaxed(0x0D, HDMI_PHY_PLL_LOCKDET_CFG2);

			/*
			 * Wait for a short duration for the PLL calibration
			 * before checking if the PLL gets locked
			 */
			udelay(350);

			timeout_count = 1000;
			pll_lock_retry--;
		}
		udelay(1);
	}

	if (!ahb_enabled)
		writel_relaxed(ahb_en_reg & ~BIT(4), AHB_EN_REG);

	if (!pll_lock_retry) {
		pr_err("%s: HDMI PLL not locked\n", __func__);
		hdmi_pll_disable();
		return -EAGAIN;
	}

	hdmi_pll_on = 1;
	return 0;
}

static int msm_ehci_resume(struct msm_hcd *mhcd)
{
	struct msm_usb_host_platform_data *pdata;
	struct usb_hcd *hcd = mhcd_to_hcd(mhcd);
	unsigned long timeout;
	unsigned temp;
	int ret;

	pdata = mhcd->dev->platform_data;

	if (!atomic_read(&mhcd->in_lpm)) {
		dev_dbg(mhcd->dev, "%s called in !in_lpm\n", __func__);
		return 0;
	}

	if (mhcd->pmic_gpio_dp_irq_enabled) {
		disable_irq_wake(mhcd->pmic_gpio_dp_irq);
		disable_irq_nosync(mhcd->pmic_gpio_dp_irq);
		mhcd->pmic_gpio_dp_irq_enabled = 0;
	}
	wake_lock(&mhcd->wlock);

	/* Vote for TCXO when waking up the phy */
	ret = msm_xo_mode_vote(mhcd->xo_handle, MSM_XO_MODE_ON);
	if (ret)
		dev_err(mhcd->dev, "%s failed to vote for "
			"TCXO D0 buffer%d\n", __func__, ret);

	clk_prepare_enable(mhcd->core_clk);
	clk_prepare_enable(mhcd->iface_clk);

	if (!pdata->mpm_xo_wakeup_int)
		msm_ehci_config_vddcx(mhcd, 1);

	temp = readl_relaxed(USB_USBCMD);
	temp &= ~ASYNC_INTR_CTRL;
	temp &= ~ULPI_STP_CTRL;
	writel_relaxed(temp, USB_USBCMD);

	if (!(readl_relaxed(USB_PORTSC) & PORTSC_PHCD))
		goto skip_phy_resume;

	temp = readl_relaxed(USB_PORTSC) & ~PORTSC_PHCD;
	writel_relaxed(temp, USB_PORTSC);

	timeout = jiffies + usecs_to_jiffies(PHY_RESUME_TIMEOUT_USEC);
	while ((readl_relaxed(USB_PORTSC) & PORTSC_PHCD) ||
			!(readl_relaxed(USB_ULPI_VIEWPORT) & ULPI_SYNC_STATE)) {
		if (time_after(jiffies, timeout)) {
			/*This is a fatal error. Reset the link and PHY*/
			dev_err(mhcd->dev, "Unable to resume USB. Resetting the h/w\n");
			msm_hsusb_reset(mhcd);
			break;
		}
		udelay(1);
	}

skip_phy_resume:

	usb_hcd_resume_root_hub(hcd);
	atomic_set(&mhcd->in_lpm, 0);

	if (mhcd->async_int) {
		mhcd->async_int = false;
		pm_runtime_put_noidle(mhcd->dev);
		enable_irq(hcd->irq);
	}

	if (atomic_read(&mhcd->pm_usage_cnt)) {
		atomic_set(&mhcd->pm_usage_cnt, 0);
		pm_runtime_put_noidle(mhcd->dev);
	}

	dev_info(mhcd->dev, "EHCI USB exited from low power mode\n");

	return 0;
}

void tegra_init_cache(bool init)
{
#ifdef CONFIG_CACHE_L2X0
	void __iomem *p = IO_ADDRESS(TEGRA_ARM_PERIF_BASE) + 0x3000;
	u32 aux_ctrl;
	u32 speedo;
	u32 tmp;

#ifdef CONFIG_TRUSTED_FOUNDATIONS
	/* issue the SMC to enable the L2 */
	aux_ctrl = readl_relaxed(p + L2X0_AUX_CTRL);
	tegra_cache_smc(true, aux_ctrl);

	/* after init, reread aux_ctrl and register handlers */
	aux_ctrl = readl_relaxed(p + L2X0_AUX_CTRL);
	l2x0_init(p, aux_ctrl, 0xFFFFFFFF);

	/* override outer_disable() with our disable */
	outer_cache.disable = tegra_l2x0_disable;
#else
#if defined(CONFIG_ARCH_TEGRA_2x_SOC)
	writel_relaxed(0x331, p + L2X0_TAG_LATENCY_CTRL);
	writel_relaxed(0x441, p + L2X0_DATA_LATENCY_CTRL);

#elif defined(CONFIG_ARCH_TEGRA_3x_SOC)
#ifdef CONFIG_TEGRA_SILICON_PLATFORM
	/* PL310 RAM latency is CPU dependent. NOTE: Changes here
	   must also be reflected in __cortex_a9_l2x0_restart */

	if (is_lp_cluster()) {
		writel(0x221, p + L2X0_TAG_LATENCY_CTRL);
		writel(0x221, p + L2X0_DATA_LATENCY_CTRL);
	} else {
		/* relax l2-cache latency for speedos 4,5,6 (T33's chips) */
		speedo = tegra_cpu_speedo_id();
		if (speedo == 4 || speedo == 5 || speedo == 6) {
			writel(0x442, p + L2X0_TAG_LATENCY_CTRL);
			writel(0x552, p + L2X0_DATA_LATENCY_CTRL);
		} else {
			writel(0x441, p + L2X0_TAG_LATENCY_CTRL);
			writel(0x551, p + L2X0_DATA_LATENCY_CTRL);
		}
	}
#else
	writel(0x770, p + L2X0_TAG_LATENCY_CTRL);
	writel(0x770, p + L2X0_DATA_LATENCY_CTRL);
#endif
#endif
	aux_ctrl = readl(p + L2X0_CACHE_TYPE);
	aux_ctrl = (aux_ctrl & 0x700) << (17-8);
	aux_ctrl |= 0x7C000001;
	if (init) {
		l2x0_init(p, aux_ctrl, 0x8200c3fe);
	} else {
		tmp = aux_ctrl;
		aux_ctrl = readl(p + L2X0_AUX_CTRL);
		aux_ctrl &= 0x8200c3fe;
		aux_ctrl |= tmp;
		writel(aux_ctrl, p + L2X0_AUX_CTRL);
	}
	l2x0_enable();
#endif
#endif
}

static unsigned int u2o_get(void __iomem *base, unsigned int offset)
{
	return readl_relaxed(base + offset);
}

static void u2o_write(void __iomem *base, unsigned int offset,
		unsigned int value)
{
	writel_relaxed(value, base + offset);
	readl_relaxed(base + offset);
}

static struct clk *_rcg_clk_get_parent(struct rcg_clk *rcg, int has_mnd)
{
	u32 n_regval = 0, m_regval = 0, d_regval = 0;
	u32 cfg_regval;
	struct clk_freq_tbl *freq;
	u32 cmd_rcgr_regval;

	/* Is there a pending configuration? */
	cmd_rcgr_regval = readl_relaxed(CMD_RCGR_REG(rcg));
	if (cmd_rcgr_regval & CMD_RCGR_CONFIG_DIRTY_MASK)
		return NULL;

	/* Get values of m, n, d, div and src_sel registers. */
	if (has_mnd) {
		m_regval = readl_relaxed(M_REG(rcg));
		n_regval = readl_relaxed(N_REG(rcg));
		d_regval = readl_relaxed(D_REG(rcg));

		/*
		 * The n and d values stored in the frequency tables are sign
		 * extended to 32 bits. The n and d values in the registers are
		 * sign extended to 8 or 16 bits. Sign extend the values read
		 * from the registers so that they can be compared to the
		 * values in the frequency tables.
		 */
		n_regval |= (n_regval >> 8) ? BM(31, 16) : BM(31, 8);
		d_regval |= (d_regval >> 8) ? BM(31, 16) : BM(31, 8);
	}

	cfg_regval = readl_relaxed(CFG_RCGR_REG(rcg));
	cfg_regval &= CFG_RCGR_SRC_SEL_MASK | CFG_RCGR_DIV_MASK
				| MND_MODE_MASK;

	/* If mnd counter is present, check if it's in use. */
	has_mnd = (has_mnd) &&
		((cfg_regval & MND_MODE_MASK) == MND_DUAL_EDGE_MODE_BVAL);

	/*
	 * Clear out the mn counter mode bits since we now want to compare only
	 * the source mux selection and pre-divider values in the registers.
	 */
	cfg_regval &= ~MND_MODE_MASK;

	/* Figure out what rate the rcg is running at */
	for (freq = rcg->freq_tbl; freq->freq_hz != FREQ_END; freq++) {
		if (freq->div_src_val != cfg_regval)
			continue;
		if (has_mnd) {
			if (freq->m_val != m_regval)
				continue;
			if (freq->n_val != n_regval)
				continue;
			if (freq->d_val != d_regval)
				continue;
		}
		break;
	}

	/* No known frequency found */
	if (freq->freq_hz == FREQ_END)
		return NULL;

	rcg->current_freq = freq;
	return freq->src_clk;
}

static int modem_reset(struct pil_desc *pil)
{
	u32 reg;
	const struct modem_data *drv = dev_get_drvdata(pil->dev);
	phys_addr_t start_addr = pil_get_entry_addr(pil);

	/* Put modem AHB0,1,2 clocks into reset */
	writel_relaxed(BIT(0) | BIT(1), drv->cbase + MAHB0_SFAB_PORT_RESET);
	writel_relaxed(BIT(7), drv->cbase + MAHB1_CLK_CTL);
	writel_relaxed(BIT(7), drv->cbase + MAHB2_CLK_CTL);

	/* Vote for pll8 on behalf of the modem */
	reg = readl_relaxed(drv->cbase + PLL_ENA_MARM);
	reg |= BIT(8);
	writel_relaxed(reg, drv->cbase + PLL_ENA_MARM);

	/* Wait for PLL8 to enable */
	while (!(readl_relaxed(drv->cbase + PLL8_STATUS) & BIT(16)))
		cpu_relax();

	/* Set MAHB1 divider to Div-5 to run MAHB1,2 and sfab at 79.8 Mhz*/
	writel_relaxed(0x4, drv->cbase + MAHB1_NS);

	/* Vote for modem AHB1 and 2 clocks to be on on behalf of the modem */
	reg = readl_relaxed(drv->cbase + MARM_CLK_BRANCH_ENA_VOTE);
	reg |= BIT(0) | BIT(1);
	writel_relaxed(reg, drv->cbase + MARM_CLK_BRANCH_ENA_VOTE);

	/* Source marm_clk off of PLL8 */
	reg = readl_relaxed(drv->cbase + MARM_CLK_SRC_CTL);
	if ((reg & 0x1) == 0) {
		writel_relaxed(0x3, drv->cbase + MARM_CLK_SRC1_NS);
		reg |= 0x1;
	} else {
		writel_relaxed(0x3, drv->cbase + MARM_CLK_SRC0_NS);
		reg &= ~0x1;
	}
	writel_relaxed(reg | 0x2, drv->cbase + MARM_CLK_SRC_CTL);

	/*
	 * Force core on and periph on signals to remain active during halt
	 * for marm_clk and mahb2_clk
	 */
	writel_relaxed(0x6F, drv->cbase + MARM_CLK_FS);
	writel_relaxed(0x6F, drv->cbase + MAHB2_CLK_FS);

	/*
	 * Enable all of the marm_clk branches, cxo sourced marm branches,
	 * and sleep clock branches
	 */
	writel_relaxed(0x10, drv->cbase + MARM_CLK_CTL);
	writel_relaxed(0x10, drv->cbase + MAHB0_CLK_CTL);
	writel_relaxed(0x10, drv->cbase + SFAB_MSS_S_HCLK_CTL);
	writel_relaxed(0x10, drv->cbase + MSS_MODEM_CXO_CLK_CTL);
	writel_relaxed(0x10, drv->cbase + MSS_SLP_CLK_CTL);
	writel_relaxed(0x10, drv->cbase + MSS_MARM_SYS_REF_CLK_CTL);

	/* Wait for above clocks to be turned on */
	while (readl_relaxed(drv->cbase + CLK_HALT_MSS_SMPSS_MISC_STATE) &
			(BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(4) | BIT(6)))
		cpu_relax();

	/* Take MAHB0,1,2 clocks out of reset */
	writel_relaxed(0x0, drv->cbase + MAHB2_CLK_CTL);
	writel_relaxed(0x0, drv->cbase + MAHB1_CLK_CTL);
	writel_relaxed(0x0, drv->cbase + MAHB0_SFAB_PORT_RESET);
	mb();

	/* Setup exception vector table base address */
	writel_relaxed(start_addr | 0x1, drv->base + MARM_BOOT_CONTROL);

	/* Wait for vector table to be setup */
	mb();

	/* Bring modem out of reset */
	writel_relaxed(0x0, drv->cbase + MARM_RESET);

	return 0;
}

static int pll_clk_is_enabled(struct clk *c)
{
	return readl_relaxed(PLL_MODE_REG(to_pll_shared_clk(c))) & BIT(0);
}

static inline u32 xgene_clk_read(void __iomem *csr)
{
	return readl_relaxed(csr);
}