p54spi: use firmware/DMA bug workaround that work under hight load in p54spi_rx

Under high load first data word, read after available data size
is sometimes lost in p54spi_rx. It seems to depend on frequency
of interrupts and latency of data read request relatively to
'data available' interrupt. The worst consequence of this bug
is loss of packet transmission acknowledgement, which in turn
causes overflow of tx queues and permanent link loss.

Read data size and first data word in one SPI transaction.
No packets from LMAC should have length less than 1 word,
so this shouldn't interfere with the next read transaction.

Also call p54spi_sleep if p54spi_wake succeeded.

Signed-off-by: Max Filippov <jcmvbkbc@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

drivers/net/wireless/p54/p54spi.c

@@ -371,32 +371,44 @@ static int p54spi_rx(struct p54s_priv *priv)
 {
 	struct sk_buff *skb;
 	u16 len;
+	u16 rx_head[2];
+#define READAHEAD_SZ (sizeof(rx_head)-sizeof(u16))
 
 	if (p54spi_wakeup(priv) < 0)
 		return -EBUSY;
 
-	/* dummy read to flush SPI DMA controller bug */
-	p54spi_read16(priv, SPI_ADRS_GEN_PURP_1);
-
-	len = p54spi_read16(priv, SPI_ADRS_DMA_DATA);
+	/* Read data size and first data word in one SPI transaction
+	 * This is workaround for firmware/DMA bug,
+	 * when first data word gets lost under high load.
+	 */
+	p54spi_spi_read(priv, SPI_ADRS_DMA_DATA, rx_head, sizeof(rx_head));
+	len = rx_head[0];
 
 	if (len == 0) {
-		dev_err(&priv->spi->dev, "rx request of zero bytes");
+		p54spi_sleep(priv);
+		dev_err(&priv->spi->dev, "rx request of zero bytes\n");
 		return 0;
 	}
 
-
 	/* Firmware may insert up to 4 padding bytes after the lmac header,
 	 * but it does not amend the size of SPI data transfer.
 	 * Such packets has correct data size in header, thus referencing
 	 * past the end of allocated skb. Reserve extra 4 bytes for this case */
 	skb = dev_alloc_skb(len + 4);
 	if (!skb) {
+		p54spi_sleep(priv);
 		dev_err(&priv->spi->dev, "could not alloc skb");
-		return 0;
+		return -ENOMEM;
 	}
 
-	p54spi_spi_read(priv, SPI_ADRS_DMA_DATA, skb_put(skb, len), len);
+	if (len <= READAHEAD_SZ) {
+		memcpy(skb_put(skb, len), rx_head + 1, len);
+	} else {
+		memcpy(skb_put(skb, READAHEAD_SZ), rx_head + 1, READAHEAD_SZ);
+		p54spi_spi_read(priv, SPI_ADRS_DMA_DATA,
+				skb_put(skb, len - READAHEAD_SZ),
+				len - READAHEAD_SZ);
+	}
 	p54spi_sleep(priv);
 	/* Put additional bytes to compensate for the possible
 	 * alignment-caused truncation */