int rts5261_pci_switch_clock()

in cardreader/rts5261.c [637:766]

• 113 lines of code
• 24 McCabe index (conditional complexity)

int rts5261_pci_switch_clock(struct rtsx_pcr *pcr, unsigned int card_clock,
		u8 ssc_depth, bool initial_mode, bool double_clk, bool vpclk)
{
	int err, clk;
	u16 n;
	u8 clk_divider, mcu_cnt, div;
	static const u8 depth[] = {
		[RTSX_SSC_DEPTH_4M] = RTS5261_SSC_DEPTH_4M,
		[RTSX_SSC_DEPTH_2M] = RTS5261_SSC_DEPTH_2M,
		[RTSX_SSC_DEPTH_1M] = RTS5261_SSC_DEPTH_1M,
		[RTSX_SSC_DEPTH_500K] = RTS5261_SSC_DEPTH_512K,
	};

	if (initial_mode) {
		/* We use 250k(around) here, in initial stage */
		if (is_version_higher_than(pcr, PID_5261, IC_VER_C)) {
			clk_divider = SD_CLK_DIVIDE_256;
			card_clock = 60000000;
		} else {
			clk_divider = SD_CLK_DIVIDE_128;
			card_clock = 30000000;
		}
	} else {
		clk_divider = SD_CLK_DIVIDE_0;
	}
	err = rtsx_pci_write_register(pcr, SD_CFG1,
			SD_CLK_DIVIDE_MASK, clk_divider);
	if (err < 0)
		return err;

	card_clock /= 1000000;
	pcr_dbg(pcr, "Switch card clock to %dMHz\n", card_clock);

	clk = card_clock;
	if (!initial_mode && double_clk)
		clk = card_clock * 2;
	pcr_dbg(pcr, "Internal SSC clock: %dMHz (cur_clock = %d)\n",
		clk, pcr->cur_clock);

	if (clk == pcr->cur_clock)
		return 0;

	if (pcr->ops->conv_clk_and_div_n)
		n = pcr->ops->conv_clk_and_div_n(clk, CLK_TO_DIV_N);
	else
		n = clk - 4;
	if ((clk <= 4) || (n > 396))
		return -EINVAL;

	mcu_cnt = 125/clk + 3;
	if (mcu_cnt > 15)
		mcu_cnt = 15;

	div = CLK_DIV_1;
	while ((n < MIN_DIV_N_PCR - 4) && (div < CLK_DIV_8)) {
		if (pcr->ops->conv_clk_and_div_n) {
			int dbl_clk = pcr->ops->conv_clk_and_div_n(n,
					DIV_N_TO_CLK) * 2;
			n = pcr->ops->conv_clk_and_div_n(dbl_clk,
					CLK_TO_DIV_N);
		} else {
			n = (n + 4) * 2 - 4;
		}
		div++;
	}

	n = (n / 2) - 1;
	pcr_dbg(pcr, "n = %d, div = %d\n", n, div);

	ssc_depth = depth[ssc_depth];
	if (double_clk)
		ssc_depth = double_ssc_depth(ssc_depth);

	if (ssc_depth) {
		if (div == CLK_DIV_2) {
			if (ssc_depth > 1)
				ssc_depth -= 1;
			else
				ssc_depth = RTS5261_SSC_DEPTH_8M;
		} else if (div == CLK_DIV_4) {
			if (ssc_depth > 2)
				ssc_depth -= 2;
			else
				ssc_depth = RTS5261_SSC_DEPTH_8M;
		} else if (div == CLK_DIV_8) {
			if (ssc_depth > 3)
				ssc_depth -= 3;
			else
				ssc_depth = RTS5261_SSC_DEPTH_8M;
		}
	} else {
		ssc_depth = 0;
	}
	pcr_dbg(pcr, "ssc_depth = %d\n", ssc_depth);

	rtsx_pci_init_cmd(pcr);
	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CLK_CTL,
				CLK_LOW_FREQ, CLK_LOW_FREQ);
	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CLK_DIV,
			0xFF, (div << 4) | mcu_cnt);
	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, SSC_CTL1, SSC_RSTB, 0);
	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, SSC_CTL2,
			SSC_DEPTH_MASK, ssc_depth);
	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, SSC_DIV_N_0, 0xFF, n);
	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, SSC_CTL1, SSC_RSTB, SSC_RSTB);
	if (vpclk) {
		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, SD_VPCLK0_CTL,
				PHASE_NOT_RESET, 0);
		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, SD_VPCLK1_CTL,
				PHASE_NOT_RESET, 0);
		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, SD_VPCLK0_CTL,
				PHASE_NOT_RESET, PHASE_NOT_RESET);
		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, SD_VPCLK1_CTL,
				PHASE_NOT_RESET, PHASE_NOT_RESET);
	}

	err = rtsx_pci_send_cmd(pcr, 2000);
	if (err < 0)
		return err;

	/* Wait SSC clock stable */
	udelay(SSC_CLOCK_STABLE_WAIT);
	err = rtsx_pci_write_register(pcr, CLK_CTL, CLK_LOW_FREQ, 0);
	if (err < 0)
		return err;

	pcr->cur_clock = clk;
	return 0;

}