static int sdw_slave_set_frequency()

in bus.c [1166:1267]

• 72 lines of code
• 14 McCabe index (conditional complexity)

static int sdw_slave_set_frequency(struct sdw_slave *slave)
{
	u32 mclk_freq = slave->bus->prop.mclk_freq;
	u32 curr_freq = slave->bus->params.curr_dr_freq >> 1;
	unsigned int scale;
	u8 scale_index;
	u8 base;
	int ret;

	/*
	 * frequency base and scale registers are required for SDCA
	 * devices. They may also be used for 1.2+/non-SDCA devices,
	 * but we will need a DisCo property to cover this case
	 */
	if (!slave->id.class_id)
		return 0;

	if (!mclk_freq) {
		dev_err(&slave->dev,
			"no bus MCLK, cannot set SDW_SCP_BUS_CLOCK_BASE\n");
		return -EINVAL;
	}

	/*
	 * map base frequency using Table 89 of SoundWire 1.2 spec.
	 * The order of the tests just follows the specification, this
	 * is not a selection between possible values or a search for
	 * the best value but just a mapping.  Only one case per platform
	 * is relevant.
	 * Some BIOS have inconsistent values for mclk_freq but a
	 * correct root so we force the mclk_freq to avoid variations.
	 */
	if (!(19200000 % mclk_freq)) {
		mclk_freq = 19200000;
		base = SDW_SCP_BASE_CLOCK_19200000_HZ;
	} else if (!(24000000 % mclk_freq)) {
		mclk_freq = 24000000;
		base = SDW_SCP_BASE_CLOCK_24000000_HZ;
	} else if (!(24576000 % mclk_freq)) {
		mclk_freq = 24576000;
		base = SDW_SCP_BASE_CLOCK_24576000_HZ;
	} else if (!(22579200 % mclk_freq)) {
		mclk_freq = 22579200;
		base = SDW_SCP_BASE_CLOCK_22579200_HZ;
	} else if (!(32000000 % mclk_freq)) {
		mclk_freq = 32000000;
		base = SDW_SCP_BASE_CLOCK_32000000_HZ;
	} else {
		dev_err(&slave->dev,
			"Unsupported clock base, mclk %d\n",
			mclk_freq);
		return -EINVAL;
	}

	if (mclk_freq % curr_freq) {
		dev_err(&slave->dev,
			"mclk %d is not multiple of bus curr_freq %d\n",
			mclk_freq, curr_freq);
		return -EINVAL;
	}

	scale = mclk_freq / curr_freq;

	/*
	 * map scale to Table 90 of SoundWire 1.2 spec - and check
	 * that the scale is a power of two and maximum 64
	 */
	scale_index = ilog2(scale);

	if (BIT(scale_index) != scale || scale_index > 6) {
		dev_err(&slave->dev,
			"No match found for scale %d, bus mclk %d curr_freq %d\n",
			scale, mclk_freq, curr_freq);
		return -EINVAL;
	}
	scale_index++;

	ret = sdw_write_no_pm(slave, SDW_SCP_BUS_CLOCK_BASE, base);
	if (ret < 0) {
		dev_err(&slave->dev,
			"SDW_SCP_BUS_CLOCK_BASE write failed:%d\n", ret);
		return ret;
	}

	/* initialize scale for both banks */
	ret = sdw_write_no_pm(slave, SDW_SCP_BUSCLOCK_SCALE_B0, scale_index);
	if (ret < 0) {
		dev_err(&slave->dev,
			"SDW_SCP_BUSCLOCK_SCALE_B0 write failed:%d\n", ret);
		return ret;
	}
	ret = sdw_write_no_pm(slave, SDW_SCP_BUSCLOCK_SCALE_B1, scale_index);
	if (ret < 0)
		dev_err(&slave->dev,
			"SDW_SCP_BUSCLOCK_SCALE_B1 write failed:%d\n", ret);

	dev_dbg(&slave->dev,
		"Configured bus base %d, scale %d, mclk %d, curr_freq %d\n",
		base, scale_index, mclk_freq, curr_freq);

	return ret;
}