diff --git a/drivers/spi/spi-tegra114.c b/drivers/spi/spi-tegra114.c index 1435792944c4..876eb2acdef1 100644 --- a/drivers/spi/spi-tegra114.c +++ b/drivers/spi/spi-tegra114.c @@ -307,10 +307,16 @@ static unsigned tegra_spi_fill_tx_fifo_from_client_txbuf( x |= (u32)(*tx_buf++) << (i * 8); tegra_spi_writel(tspi, x, SPI_TX_FIFO); } + + tspi->cur_tx_pos += written_words * tspi->bytes_per_word; } else { + unsigned int write_bytes; max_n_32bit = min(tspi->curr_dma_words, tx_empty_count); written_words = max_n_32bit; nbytes = written_words * tspi->bytes_per_word; + if (nbytes > t->len - tspi->cur_pos) + nbytes = t->len - tspi->cur_pos; + write_bytes = nbytes; for (count = 0; count < max_n_32bit; count++) { u32 x = 0; @@ -319,8 +325,10 @@ static unsigned tegra_spi_fill_tx_fifo_from_client_txbuf( x |= (u32)(*tx_buf++) << (i * 8); tegra_spi_writel(tspi, x, SPI_TX_FIFO); } + + tspi->cur_tx_pos += write_bytes; } - tspi->cur_tx_pos += written_words * tspi->bytes_per_word; + return written_words; } @@ -344,20 +352,27 @@ static unsigned int tegra_spi_read_rx_fifo_to_client_rxbuf( for (i = 0; len && (i < 4); i++, len--) *rx_buf++ = (x >> i*8) & 0xFF; } - tspi->cur_rx_pos += tspi->curr_dma_words * tspi->bytes_per_word; read_words += tspi->curr_dma_words; + tspi->cur_rx_pos += tspi->curr_dma_words * tspi->bytes_per_word; } else { u32 rx_mask = ((u32)1 << t->bits_per_word) - 1; + u8 bytes_per_word = tspi->bytes_per_word; + unsigned int read_bytes; + len = rx_full_count * bytes_per_word; + if (len > t->len - tspi->cur_pos) + len = t->len - tspi->cur_pos; + read_bytes = len; for (count = 0; count < rx_full_count; count++) { u32 x = tegra_spi_readl(tspi, SPI_RX_FIFO) & rx_mask; - for (i = 0; (i < tspi->bytes_per_word); i++) + for (i = 0; len && (i < bytes_per_word); i++, len--) *rx_buf++ = (x >> (i*8)) & 0xFF; } - tspi->cur_rx_pos += rx_full_count * tspi->bytes_per_word; read_words += rx_full_count; + tspi->cur_rx_pos += read_bytes; } + return read_words; } @@ -372,12 +387,17 @@ static void tegra_spi_copy_client_txbuf_to_spi_txbuf( unsigned len = tspi->curr_dma_words * tspi->bytes_per_word; memcpy(tspi->tx_dma_buf, t->tx_buf + tspi->cur_pos, len); + tspi->cur_tx_pos += tspi->curr_dma_words * tspi->bytes_per_word; } else { unsigned int i; unsigned int count; u8 *tx_buf = (u8 *)t->tx_buf + tspi->cur_tx_pos; unsigned consume = tspi->curr_dma_words * tspi->bytes_per_word; + unsigned int write_bytes; + if (consume > t->len - tspi->cur_pos) + consume = t->len - tspi->cur_pos; + write_bytes = consume; for (count = 0; count < tspi->curr_dma_words; count++) { u32 x = 0; @@ -386,8 +406,9 @@ static void tegra_spi_copy_client_txbuf_to_spi_txbuf( x |= (u32)(*tx_buf++) << (i * 8); tspi->tx_dma_buf[count] = x; } + + tspi->cur_tx_pos += write_bytes; } - tspi->cur_tx_pos += tspi->curr_dma_words * tspi->bytes_per_word; /* Make the dma buffer to read by dma */ dma_sync_single_for_device(tspi->dev, tspi->tx_dma_phys, @@ -405,20 +426,28 @@ static void tegra_spi_copy_spi_rxbuf_to_client_rxbuf( unsigned len = tspi->curr_dma_words * tspi->bytes_per_word; memcpy(t->rx_buf + tspi->cur_rx_pos, tspi->rx_dma_buf, len); + tspi->cur_rx_pos += tspi->curr_dma_words * tspi->bytes_per_word; } else { unsigned int i; unsigned int count; unsigned char *rx_buf = t->rx_buf + tspi->cur_rx_pos; u32 rx_mask = ((u32)1 << t->bits_per_word) - 1; + unsigned consume = tspi->curr_dma_words * tspi->bytes_per_word; + unsigned int read_bytes; + if (consume > t->len - tspi->cur_pos) + consume = t->len - tspi->cur_pos; + read_bytes = consume; for (count = 0; count < tspi->curr_dma_words; count++) { u32 x = tspi->rx_dma_buf[count] & rx_mask; - for (i = 0; (i < tspi->bytes_per_word); i++) + for (i = 0; consume && (i < tspi->bytes_per_word); + i++, consume--) *rx_buf++ = (x >> (i*8)) & 0xFF; } + + tspi->cur_rx_pos += read_bytes; } - tspi->cur_rx_pos += tspi->curr_dma_words * tspi->bytes_per_word; /* Make the dma buffer to read by dma */ dma_sync_single_for_device(tspi->dev, tspi->rx_dma_phys,