static void hci_pio_push_to_next_rx()

in master/mipi-i3c-hci/pio.c [394:471]

• 60 lines of code
• 9 McCabe index (conditional complexity)

static void hci_pio_push_to_next_rx(struct i3c_hci *hci, struct hci_xfer *xfer,
				    unsigned int words_to_keep)
{
	u32 *from = xfer->data;
	u32 from_last;
	unsigned int received, count;

	received = (xfer->data_len - xfer->data_left) / 4;
	if ((xfer->data_len - xfer->data_left) & 3) {
		from_last = xfer->data_word_before_partial;
		received += 1;
	} else {
		from_last = from[received];
	}
	from += words_to_keep;
	count = received - words_to_keep;

	while (count) {
		unsigned int room, left, chunk, bytes_to_move;
		u32 last_word;

		xfer = xfer->next_data;
		if (!xfer) {
			dev_err(&hci->master.dev, "pushing RX data to unexistent xfer\n");
			return;
		}

		room = DIV_ROUND_UP(xfer->data_len, 4);
		left = DIV_ROUND_UP(xfer->data_left, 4);
		chunk = min(count, room);
		if (chunk > left) {
			hci_pio_push_to_next_rx(hci, xfer, chunk - left);
			left = chunk;
			xfer->data_left = left * 4;
		}

		bytes_to_move = xfer->data_len - xfer->data_left;
		if (bytes_to_move & 3) {
			/* preserve word  to become partial */
			u32 *p = xfer->data;

			xfer->data_word_before_partial = p[bytes_to_move / 4];
		}
		memmove(xfer->data + chunk, xfer->data, bytes_to_move);

		/* treat last word specially because of partial word issues */
		chunk -= 1;

		memcpy(xfer->data, from, chunk * 4);
		xfer->data_left -= chunk * 4;
		from += chunk;
		count -= chunk;

		last_word = (count == 1) ? from_last : *from++;
		if (xfer->data_left < 4) {
			/*
			 * Like in hci_pio_do_trailing_rx(), preserve original
			 * word to be stored partially then store bytes it
			 * in an endian independent way.
			 */
			u8 *p_byte = xfer->data;

			p_byte += chunk * 4;
			xfer->data_word_before_partial = last_word;
			last_word = (__force u32) cpu_to_le32(last_word);
			while (xfer->data_left--) {
				*p_byte++ = last_word;
				last_word >>= 8;
			}
		} else {
			u32 *p = xfer->data;

			p[chunk] = last_word;
			xfer->data_left -= 4;
		}
		count--;
	}
}