static int da7280_init()

in misc/da7280.c [962:1141]

• 149 lines of code
• 38 McCabe index (conditional complexity)

static int da7280_init(struct da7280_haptic *haptics)
{
	unsigned int val = 0;
	u32 v2i_factor;
	int error, i;
	u8 mask = 0;

	/*
	 * If device type is DA7280_DEV_MAX then simply use currently
	 * programmed mode.
	 */
	if (haptics->dev_type == DA7280_DEV_MAX) {
		error = regmap_read(haptics->regmap, DA7280_TOP_CFG1, &val);
		if (error)
			goto out_err;

		haptics->dev_type = val & DA7280_ACTUATOR_TYPE_MASK ?
					DA7280_ERM_COIN : DA7280_LRA;
	}

	/* Apply user settings */
	if (haptics->dev_type == DA7280_LRA &&
	    haptics->resonant_freq_l != DA7280_SKIP_INIT) {
		error = regmap_write(haptics->regmap, DA7280_FRQ_LRA_PER_H,
				     haptics->resonant_freq_h);
		if (error)
			goto out_err;
		error = regmap_write(haptics->regmap, DA7280_FRQ_LRA_PER_L,
				     haptics->resonant_freq_l);
		if (error)
			goto out_err;
	} else if (haptics->dev_type == DA7280_ERM_COIN) {
		error = regmap_update_bits(haptics->regmap, DA7280_TOP_INT_CFG1,
					   DA7280_BEMF_FAULT_LIM_MASK, 0);
		if (error)
			goto out_err;

		mask = DA7280_TST_CALIB_IMPEDANCE_DIS_MASK |
			DA7280_V2I_FACTOR_FREEZE_MASK;
		val = DA7280_TST_CALIB_IMPEDANCE_DIS_MASK |
			DA7280_V2I_FACTOR_FREEZE_MASK;
		error = regmap_update_bits(haptics->regmap, DA7280_TOP_CFG4,
					   mask, val);
		if (error)
			goto out_err;

		haptics->acc_en = false;
		haptics->rapid_stop_en = false;
		haptics->amp_pid_en = false;
	}

	mask = DA7280_ACTUATOR_TYPE_MASK |
			DA7280_BEMF_SENSE_EN_MASK |
			DA7280_FREQ_TRACK_EN_MASK |
			DA7280_ACCELERATION_EN_MASK |
			DA7280_RAPID_STOP_EN_MASK |
			DA7280_AMP_PID_EN_MASK;
	val = FIELD_PREP(DA7280_ACTUATOR_TYPE_MASK,
			 (haptics->dev_type ? 1 : 0)) |
		FIELD_PREP(DA7280_BEMF_SENSE_EN_MASK,
			   (haptics->bemf_sense_en ? 1 : 0)) |
		FIELD_PREP(DA7280_FREQ_TRACK_EN_MASK,
			   (haptics->freq_track_en ? 1 : 0)) |
		FIELD_PREP(DA7280_ACCELERATION_EN_MASK,
			   (haptics->acc_en ? 1 : 0)) |
		FIELD_PREP(DA7280_RAPID_STOP_EN_MASK,
			   (haptics->rapid_stop_en ? 1 : 0)) |
		FIELD_PREP(DA7280_AMP_PID_EN_MASK,
			   (haptics->amp_pid_en ? 1 : 0));

	error = regmap_update_bits(haptics->regmap, DA7280_TOP_CFG1, mask, val);
	if (error)
		goto out_err;

	error = regmap_update_bits(haptics->regmap, DA7280_TOP_CFG5,
				   DA7280_V2I_FACTOR_OFFSET_EN_MASK,
				   haptics->acc_en ?
					DA7280_V2I_FACTOR_OFFSET_EN_MASK : 0);
	if (error)
		goto out_err;

	error = regmap_update_bits(haptics->regmap,
				   DA7280_TOP_CFG2,
				   DA7280_MEM_DATA_SIGNED_MASK,
				   haptics->acc_en ?
					0 : DA7280_MEM_DATA_SIGNED_MASK);
	if (error)
		goto out_err;

	if (haptics->nommax != DA7280_SKIP_INIT) {
		error = regmap_write(haptics->regmap, DA7280_ACTUATOR1,
				     haptics->nommax);
		if (error)
			goto out_err;
	}

	if (haptics->absmax != DA7280_SKIP_INIT) {
		error = regmap_write(haptics->regmap, DA7280_ACTUATOR2,
				     haptics->absmax);
		if (error)
			goto out_err;
	}

	error = regmap_update_bits(haptics->regmap, DA7280_ACTUATOR3,
				   DA7280_IMAX_MASK, haptics->imax);
	if (error)
		goto out_err;

	v2i_factor = haptics->impd * (haptics->imax + 4) / 1610400;
	error = regmap_write(haptics->regmap, DA7280_CALIB_V2I_L,
			     v2i_factor & 0xff);
	if (error)
		goto out_err;
	error = regmap_write(haptics->regmap, DA7280_CALIB_V2I_H,
			     v2i_factor >> 8);
	if (error)
		goto out_err;

	error = regmap_update_bits(haptics->regmap,
				   DA7280_TOP_CTL1,
				   DA7280_STANDBY_EN_MASK,
				   DA7280_STANDBY_EN_MASK);
	if (error)
		goto out_err;

	if (haptics->mem_update) {
		error = da7280_haptic_mem_update(haptics);
		if (error)
			goto out_err;
	}

	/* Set  PS_SEQ_ID and PS_SEQ_LOOP */
	val = FIELD_PREP(DA7280_PS_SEQ_ID_MASK, haptics->ps_seq_id) |
		FIELD_PREP(DA7280_PS_SEQ_LOOP_MASK, haptics->ps_seq_loop);
	error = regmap_write(haptics->regmap, DA7280_SEQ_CTL2, val);
	if (error)
		goto out_err;

	/* GPI(N) CTL */
	for (i = 0; i < 3; i++) {
		val = FIELD_PREP(DA7280_GPI0_SEQUENCE_ID_MASK,
				 haptics->gpi_ctl[i].seq_id) |
			FIELD_PREP(DA7280_GPI0_MODE_MASK,
				   haptics->gpi_ctl[i].mode) |
			FIELD_PREP(DA7280_GPI0_POLARITY_MASK,
				   haptics->gpi_ctl[i].polarity);
		error = regmap_write(haptics->regmap,
				     DA7280_GPI_0_CTL + i, val);
		if (error)
			goto out_err;
	}

	/* Mask ADC_SAT_M bit as default */
	error = regmap_update_bits(haptics->regmap,
				   DA7280_IRQ_MASK2,
				   DA7280_ADC_SAT_M_MASK,
				   DA7280_ADC_SAT_M_MASK);
	if (error)
		goto out_err;

	/* Clear Interrupts */
	error = regmap_write(haptics->regmap, DA7280_IRQ_EVENT1, 0xff);
	if (error)
		goto out_err;

	error = regmap_update_bits(haptics->regmap,
				   DA7280_IRQ_MASK1,
				   DA7280_SEQ_FAULT_M_MASK |
					DA7280_SEQ_DONE_M_MASK,
				   0);
	if (error)
		goto out_err;

	haptics->active = false;
	return 0;

out_err:
	dev_err(haptics->dev, "chip initialization error: %d\n", error);
	return error;
}