Commit f856e7ba authored by Al Semjonovs's avatar Al Semjonovs Committed by Maureen Helm
Browse files

icm42688: Fix channels read BIT shift error



channel_pos_read bit was being cleared with a sensor channel enum value
instead of the encoded bit position.

Signed-off-by: default avatarAl Semjonovs <asemjonovs@google.com>
parent 3038a3c4
Loading
Loading
Loading
Loading
+11 −11
Original line number Diff line number Diff line
@@ -223,39 +223,39 @@ static int icm42688_one_shot_decode(const uint8_t *buffer, sensor_frame_iterator
				    q31_t *values, uint8_t max_count)
{
	const struct icm42688_encoded_data *edata = (const struct icm42688_encoded_data *)buffer;
	uint8_t channels_read = edata->channels;
	uint8_t channel_pos_read = edata->channels;
	struct icm42688_cfg cfg = {
		.accel_fs = edata->header.accel_fs,
		.gyro_fs = edata->header.gyro_fs,
	};
	int chan;
	enum sensor_channel chan;
	int pos;
	int count = 0;
	int num_samples = __builtin_popcount(channels_read);
	int num_samples = __builtin_popcount(channel_pos_read);

	channels_read = edata->channels;
	channel_pos_read = edata->channels;

	if (*fit != 0) {
		return 0;
	}

	/* Skip channels already decoded */
	for (int i = 0; i < *cit && channels_read; i++) {
		chan = __builtin_ctz(channels_read);
		channels_read &= ~BIT(chan);
	for (int i = 0; i < *cit && channel_pos_read; i++) {
		pos = __builtin_ctz(channel_pos_read);
		channel_pos_read &= ~BIT(pos);
	}

	/* Decode remaining channels */
	while (channels_read && *cit < num_samples && count < max_count) {
		chan = icm42688_get_channel_from_position(__builtin_ctz(channels_read));
	while (channel_pos_read && *cit < num_samples && count < max_count) {
		pos = __builtin_ctz(channel_pos_read);
		chan = icm42688_get_channel_from_position(pos);

		channels[count] = chan;
		pos = icm42688_get_channel_position(chan);

		icm42688_convert_raw_to_q31(&cfg, chan, edata->readings[pos], &values[count]);

		count++;
		channels_read &= ~BIT(chan);
		channel_pos_read &= ~BIT(pos);
		*cit += 1;
	}