static int da7280_haptics_upload_effect()

in misc/da7280.c [505:657]

• 122 lines of code
• 27 McCabe index (conditional complexity)

static int da7280_haptics_upload_effect(struct input_dev *dev,
					struct ff_effect *effect,
					struct ff_effect *old)
{
	struct da7280_haptic *haptics = input_get_drvdata(dev);
	s16 data[DA7280_SNP_MEM_SIZE] = { 0 };
	unsigned int val;
	int tmp, i, num;
	int error;

	/* The effect should be uploaded when haptic is not working */
	if (haptics->active)
		return -EBUSY;

	switch (effect->type) {
	/* DRO/PWM modes support this type */
	case FF_CONSTANT:
		haptics->op_mode = haptics->const_op_mode;
		if (haptics->op_mode == DA7280_DRO_MODE) {
			tmp = effect->u.constant.level * 254;
			haptics->level = tmp / 0x7FFF;
			break;
		}

		haptics->gain =	effect->u.constant.level <= 0 ?
					0 : effect->u.constant.level;
		break;

	/* RTWM/ETWM modes support this type */
	case FF_PERIODIC:
		if (effect->u.periodic.waveform != FF_CUSTOM) {
			dev_err(haptics->dev,
				"Device can only accept FF_CUSTOM waveform\n");
			return -EINVAL;
		}

		/*
		 * Load the data and check the length.
		 * the data will be patterns in this case: 4 < X <= 100,
		 * and will be saved into the waveform memory inside DA728x.
		 * If X = 2, the data will be PS_SEQ_ID and PS_SEQ_LOOP.
		 * If X = 3, the 1st data will be GPIX_SEQUENCE_ID .
		 */
		if (effect->u.periodic.custom_len == DA7280_CUSTOM_DATA_LEN)
			goto set_seq_id_loop;

		if (effect->u.periodic.custom_len == DA7280_CUSTOM_GP_DATA_LEN)
			goto set_gpix_seq_id;

		if (effect->u.periodic.custom_len < DA7280_CUSTOM_DATA_LEN ||
		    effect->u.periodic.custom_len > DA7280_SNP_MEM_SIZE) {
			dev_err(haptics->dev, "Invalid waveform data size\n");
			return -EINVAL;
		}

		if (copy_from_user(data, effect->u.periodic.custom_data,
				   sizeof(s16) *
				   effect->u.periodic.custom_len))
			return -EFAULT;

		memset(haptics->snp_mem, 0, DA7280_SNP_MEM_SIZE);

		for (i = 0; i < effect->u.periodic.custom_len; i++) {
			if (data[i] < 0 || data[i] > 0xff) {
				dev_err(haptics->dev,
					"Invalid waveform data %d at offset %d\n",
					data[i], i);
				return -EINVAL;
			}
			haptics->snp_mem[i] = (u8)data[i];
		}

		error = da7280_haptic_mem_update(haptics);
		if (error) {
			dev_err(haptics->dev,
				"Failed to upload waveform: %d\n", error);
			return error;
		}
		break;

set_seq_id_loop:
		if (copy_from_user(data, effect->u.periodic.custom_data,
				   sizeof(s16) * DA7280_CUSTOM_DATA_LEN))
			return -EFAULT;

		if (data[DA7280_CUSTOM_SEQ_ID_IDX] < 0 ||
		    data[DA7280_CUSTOM_SEQ_ID_IDX] > DA7280_SEQ_ID_MAX ||
		    data[DA7280_CUSTOM_SEQ_LOOP_IDX] < 0 ||
		    data[DA7280_CUSTOM_SEQ_LOOP_IDX] > DA7280_SEQ_LOOP_MAX) {
			dev_err(haptics->dev,
				"Invalid custom id (%d) or loop (%d)\n",
				data[DA7280_CUSTOM_SEQ_ID_IDX],
				data[DA7280_CUSTOM_SEQ_LOOP_IDX]);
			return -EINVAL;
		}

		haptics->ps_seq_id = data[DA7280_CUSTOM_SEQ_ID_IDX] & 0x0f;
		haptics->ps_seq_loop = data[DA7280_CUSTOM_SEQ_LOOP_IDX] & 0x0f;
		haptics->op_mode = haptics->periodic_op_mode;

		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) {
			dev_err(haptics->dev,
				"Failed to update PS sequence: %d\n", error);
			return error;
		}
		break;

set_gpix_seq_id:
		if (copy_from_user(data, effect->u.periodic.custom_data,
				   sizeof(s16) * DA7280_CUSTOM_GP_DATA_LEN))
			return -EFAULT;

		if (data[DA7280_CUSTOM_GPI_SEQ_ID_IDX] < 0 ||
		    data[DA7280_CUSTOM_GPI_SEQ_ID_IDX] > DA7280_SEQ_ID_MAX ||
		    data[DA7280_CUSTOM_GPI_NUM_IDX] < 0 ||
		    data[DA7280_CUSTOM_GPI_NUM_IDX] > DA7280_GPI_SEQ_ID_MAX) {
			dev_err(haptics->dev,
				"Invalid custom GPI id (%d) or num (%d)\n",
				data[DA7280_CUSTOM_GPI_SEQ_ID_IDX],
				data[DA7280_CUSTOM_GPI_NUM_IDX]);
			return -EINVAL;
		}

		num = data[DA7280_CUSTOM_GPI_NUM_IDX] & 0x0f;
		haptics->gpi_ctl[num].seq_id =
			data[DA7280_CUSTOM_GPI_SEQ_ID_IDX] & 0x0f;
		haptics->op_mode = haptics->periodic_op_mode;

		val = FIELD_PREP(DA7280_GPI0_SEQUENCE_ID_MASK,
				 haptics->gpi_ctl[num].seq_id);
		error = regmap_update_bits(haptics->regmap,
					   DA7280_GPI_0_CTL + num,
					   DA7280_GPI0_SEQUENCE_ID_MASK,
					   val);
		if (error) {
			dev_err(haptics->dev,
				"Failed to update GPI sequence: %d\n", error);
			return error;
		}
		break;

	default:
		dev_err(haptics->dev, "Unsupported effect type: %d\n",
			effect->type);
		return -EINVAL;
	}

	return 0;
}