hw/net/can/xlnx-zynqmp: Avoid underflow while popping TX FIFOs

[qmiga/qemu.git] / hw / net / can / xlnx-zynqmp-can.c

diff --git a/hw/net/can/xlnx-zynqmp-can.c b/hw/net/can/xlnx-zynqmp-can.c
index affa21a..1f1c686 100644 (file)
--- a/hw/net/can/xlnx-zynqmp-can.c
+++ b/hw/net/can/xlnx-zynqmp-can.c
@@ -37,7 +37,6 @@
 #include "qemu/bitops.h"
 #include "qemu/log.h"
 #include "qemu/cutils.h"
-#include "sysemu/sysemu.h"
 #include "migration/vmstate.h"
 #include "hw/qdev-properties.h"
 #include "net/can_emu.h"
@@ -435,6 +434,52 @@ static bool tx_ready_check(XlnxZynqMPCANState *s)
     return true;
 }
 
+static void read_tx_frame(XlnxZynqMPCANState *s, Fifo32 *fifo, uint32_t *data)
+{
+    unsigned used = fifo32_num_used(fifo);
+    bool is_txhpb = fifo == &s->txhpb_fifo;
+
+    assert(used > 0);
+    used %= CAN_FRAME_SIZE;
+
+    /*
+     * Frame Message Format
+     *
+     * Each frame includes four words (16 bytes). Software must read and write
+     * all four words regardless of the actual number of data bytes and valid
+     * fields in the message.
+     * If software misbehave (not writing all four words), we use the previous
+     * registers content to initialize each missing word.
+     *
+     * If used is 1 then ID, DLC and DATA1 are missing.
+     * if used is 2 then ID and DLC are missing.
+     * if used is 3 then only ID is missing.
+     */
+     if (used > 0) {
+        data[0] = s->regs[is_txhpb ? R_TXHPB_ID : R_TXFIFO_ID];
+    } else {
+        data[0] = fifo32_pop(fifo);
+    }
+    if (used == 1 || used == 2) {
+        data[1] = s->regs[is_txhpb ? R_TXHPB_DLC : R_TXFIFO_DLC];
+    } else {
+        data[1] = fifo32_pop(fifo);
+    }
+    if (used == 1) {
+        data[2] = s->regs[is_txhpb ? R_TXHPB_DATA1 : R_TXFIFO_DATA1];
+    } else {
+        data[2] = fifo32_pop(fifo);
+    }
+    /* DATA2 triggered the transfer thus is always available */
+    data[3] = fifo32_pop(fifo);
+
+    if (used) {
+        qemu_log_mask(LOG_GUEST_ERROR,
+                      "%s: Incomplete CAN frame (only %u/%u slots used)\n",
+                      TYPE_XLNX_ZYNQMP_CAN, used, CAN_FRAME_SIZE);
+    }
+}
+
 static void transfer_fifo(XlnxZynqMPCANState *s, Fifo32 *fifo)
 {
     qemu_can_frame frame;
@@ -452,9 +497,7 @@ static void transfer_fifo(XlnxZynqMPCANState *s, Fifo32 *fifo)
     }
 
     while (!fifo32_is_empty(fifo)) {
-        for (i = 0; i < CAN_FRAME_SIZE; i++) {
-            data[i] = fifo32_pop(fifo);
-        }
+        read_tx_frame(s, fifo, data);
 
         if (ARRAY_FIELD_EX32(s->regs, STATUS_REGISTER, LBACK)) {
             /*
@@ -697,30 +740,30 @@ static void update_rx_fifo(XlnxZynqMPCANState *s, const qemu_can_frame *frame)
                                                timestamp));
 
             /* First 32 bit of the data. */
-            fifo32_push(&s->rx_fifo, deposit32(0, R_TXFIFO_DATA1_DB3_SHIFT,
-                                               R_TXFIFO_DATA1_DB3_LENGTH,
+            fifo32_push(&s->rx_fifo, deposit32(0, R_RXFIFO_DATA1_DB3_SHIFT,
+                                               R_RXFIFO_DATA1_DB3_LENGTH,
                                                frame->data[0]) |
-                                     deposit32(0, R_TXFIFO_DATA1_DB2_SHIFT,
-                                               R_TXFIFO_DATA1_DB2_LENGTH,
+                                     deposit32(0, R_RXFIFO_DATA1_DB2_SHIFT,
+                                               R_RXFIFO_DATA1_DB2_LENGTH,
                                                frame->data[1]) |
-                                     deposit32(0, R_TXFIFO_DATA1_DB1_SHIFT,
-                                               R_TXFIFO_DATA1_DB1_LENGTH,
+                                     deposit32(0, R_RXFIFO_DATA1_DB1_SHIFT,
+                                               R_RXFIFO_DATA1_DB1_LENGTH,
                                                frame->data[2]) |
-                                     deposit32(0, R_TXFIFO_DATA1_DB0_SHIFT,
-                                               R_TXFIFO_DATA1_DB0_LENGTH,
+                                     deposit32(0, R_RXFIFO_DATA1_DB0_SHIFT,
+                                               R_RXFIFO_DATA1_DB0_LENGTH,
                                                frame->data[3]));
             /* Last 32 bit of the data. */
-            fifo32_push(&s->rx_fifo, deposit32(0, R_TXFIFO_DATA2_DB7_SHIFT,
-                                               R_TXFIFO_DATA2_DB7_LENGTH,
+            fifo32_push(&s->rx_fifo, deposit32(0, R_RXFIFO_DATA2_DB7_SHIFT,
+                                               R_RXFIFO_DATA2_DB7_LENGTH,
                                                frame->data[4]) |
-                                     deposit32(0, R_TXFIFO_DATA2_DB6_SHIFT,
-                                               R_TXFIFO_DATA2_DB6_LENGTH,
+                                     deposit32(0, R_RXFIFO_DATA2_DB6_SHIFT,
+                                               R_RXFIFO_DATA2_DB6_LENGTH,
                                                frame->data[5]) |
-                                     deposit32(0, R_TXFIFO_DATA2_DB5_SHIFT,
-                                               R_TXFIFO_DATA2_DB5_LENGTH,
+                                     deposit32(0, R_RXFIFO_DATA2_DB5_SHIFT,
+                                               R_RXFIFO_DATA2_DB5_LENGTH,
                                                frame->data[6]) |
-                                     deposit32(0, R_TXFIFO_DATA2_DB4_SHIFT,
-                                               R_TXFIFO_DATA2_DB4_LENGTH,
+                                     deposit32(0, R_RXFIFO_DATA2_DB4_SHIFT,
+                                               R_RXFIFO_DATA2_DB4_LENGTH,
                                                frame->data[7]));
 
             ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, RXOK, 1);
@@ -1080,7 +1123,7 @@ static void xlnx_zynqmp_can_realize(DeviceState *dev, Error **errp)
 
     /* Allocate a new timer. */
     s->can_timer = ptimer_init(xlnx_zynqmp_can_ptimer_cb, s,
-                               PTIMER_POLICY_DEFAULT);
+                               PTIMER_POLICY_LEGACY);
 
     ptimer_transaction_begin(s->can_timer);