1 files changed, 31 insertions, 5 deletions
diff --git a/app/steth.c b/app/steth.c
index e861606..9245d17 100644
--- a/app/steth.c
+++ b/app/steth.c
@@ -173,15 +173,42 @@ phy_link_an_done (uint8_t phy)
 }
 
 
+static bool eth_tx_pbuf (struct pbuf *p)
+{
+  uint8_t *buf, *ptr;
+  uint32_t len;
+
+  if (ETH_DES0 (TxBD) & ETH_TDES0_OWN)
+    return false;
+
+  buf = (void *)ETH_DES2 (TxBD);
+
+
+  for (len = 0, ptr = buf; p; p = p->next) {
+    memcpy (ptr, p->payload, p->len);
+    ptr += p->len;
+    len += p->len;
+  }
+
+  ETH_DES1 (TxBD) = len & ETH_TDES1_TBS1;
+  ETH_DES0 (TxBD) |= ETH_TDES0_LS | ETH_TDES0_FS | ETH_TDES0_OWN;
+  TxBD = ETH_DES3 (TxBD);
+
+  if (ETH_DMASR & ETH_DMASR_TBUS) {
+    ETH_DMASR = ETH_DMASR_TBUS;
+    ETH_DMATPDR = 0;
+  }
+
+  return true;
+}
+
+
 static err_t
 steth_tx (struct netif *netif, struct pbuf *p)
 {
   unsigned tries = 3;
 
-  if (p->next) return ERR_IF;
-
-
-  while (!eth_tx (p->payload, p->len)) {
+  while (!eth_tx_pbuf (p)) {
     delay_us (10);
 
     if (! (tries--)) return ERR_IF;
@@ -191,7 +218,6 @@ steth_tx (struct netif *netif, struct pbuf *p)
   return ERR_OK;
 }
 
-
 static bool eth_rx_ptp (uint8_t *ppkt, uint32_t *len, uint32_t maxlen, uint64_t *tstamp)
 {
   bool fs = false;