1 files changed, 173 insertions, 141 deletions

diff --git a/app/steth.c b/app/steth.c
index 5b94e1d..9d56529 100644
--- a/app/steth.c
+++ b/app/steth.c
@@ -20,87 +20,131 @@ extern uint32_t TxBD;
 extern uint32_t RxBD;
 
 
-#define FOO(a)  \
-  do { static uint32_t w; \
-    uint32_t v=a; \
-    if (v!=w) {\
-      printf (" " #a":   %08x (%08x +%08x -%08x)\r\n", \
-              (unsigned) v,(unsigned) (v^w),(unsigned) ((v^w) &v),(unsigned) ((v^w)&w)); \
-    }\
-    w=v; \
-  } while (0)
 
+static void eth_smi_transact_wtimeo (void)
+{
+  int timeout = 200;
+  /* Begin transaction. */
+  ETH_MACMIIAR |= ETH_MACMIIAR_MB;
 
+  /* Wait for not busy. */
+  while (ETH_MACMIIAR & ETH_MACMIIAR_MB) {
+    if (! (timeout--)) return;
 
-void steth_calculate_mac(void)
+    delay_us (10);
+  }
+}
+
+static void eth_smi_write_wtimeo (uint8_t phy, uint8_t reg, uint16_t data)
 {
-  uint32_t uid[3];
-  uint8_t *ptr;
-  unsigned i;
-  desig_get_unique_id (uid);
+  /* Write operation MW=1*/
+  ETH_MACMIIAR = (ETH_MACMIIAR & ETH_MACMIIAR_CR) | /* save clocks */
+                 (phy << ETH_MACMIIAR_PA_SHIFT) |
+                 (reg << ETH_MACMIIAR_MR_SHIFT) |
+                 ETH_MACMIIAR_MW;
 
-  ptr=(uint8_t *)uid;
+  ETH_MACMIIDR = data & ETH_MACMIIDR_MD;
 
-  for (i=0;i<ETHARP_HWADDR_LEN;++i) 
-	  sa[i]^=*(ptr++);
-  for (i=0;i<ETHARP_HWADDR_LEN;++i) 
-	  sa[i]^=*(ptr++);
+  eth_smi_transact_wtimeo();
+}
 
-  sa[0]&=0xfe; /*Clear I/G */
-  sa[0]|=0x2;  /*Set U/L */
+static uint16_t eth_smi_read_wtimeo (uint8_t phy, uint8_t reg)
+{
+  /* Read operation MW=0*/
+  ETH_MACMIIAR = (ETH_MACMIIAR & ETH_MACMIIAR_CR) | /* save clocks */
+                 (phy << ETH_MACMIIAR_PA_SHIFT) |
+                 (reg << ETH_MACMIIAR_MR_SHIFT);
+
+  eth_smi_transact_wtimeo();
 
-printf("MAC ADDRESS is %02x:%02x:%02x:%02x:%02x:%02x\r\n",sa[0],sa[1],sa[2],sa[3],sa[4],sa[5]);
+  return (uint16_t) (ETH_MACMIIDR & ETH_MACMIIDR_MD);
 }
 
 
 
+static void eth_smi_bit_set_wtimeo (uint8_t phy, uint8_t reg, uint16_t setbits)
+{
+  uint16_t val = eth_smi_read_wtimeo (phy, reg);
+  eth_smi_write_wtimeo (phy, reg, val | setbits);
+}
 
-static void mac_stat (void)
+static bool phy_link_isup_wtimeo (uint8_t phy)
 {
-  //uint32_t d, s;
+  return eth_smi_read (phy, PHY_REG_BSR) & PHY_REG_BSR_UP;
+}
 
-  printf ("Net:\r\n");
+static enum phy_status phy_link_status_wtimeo (uint8_t phy)
+{
+#define PHY_REG_CR1         0x1E
+  return eth_smi_read_wtimeo (phy, PHY_REG_CR1) & 0x07;
+}
 
-  FOO (ETH_MACCR);
-  FOO (ETH_MACFFR);
-  FOO (ETH_MACFCR);
-  FOO (ETH_MACDBGR);
-  FOO (ETH_MACSR);
-  FOO (ETH_DMAOMR);
-  FOO (ETH_DMASR);
-  FOO (ETH_DMAIER);
-  FOO (ETH_DMACHTDR);
-  FOO (ETH_DMACHRDR);
-  FOO (ETH_DMAOMR);
-  FOO (ETH_DMATDLAR);
-  FOO (ETH_DMARDLAR);
-  FOO (ETH_DMABMR);
+static void phy_autoneg_enable_wtimeo (uint8_t phy)
+{
+  eth_smi_bit_set_wtimeo (phy, PHY_REG_BCR, PHY_REG_BCR_AN | PHY_REG_BCR_ANRST);
+}
 
 
-#if 0
-  s = d = RxBD;
 
+void steth_calculate_mac (void)
+{
+  uint32_t uid[3];
+  uint8_t *ptr;
+  unsigned i;
+  desig_get_unique_id (uid);
+
+  ptr = (uint8_t *)uid;
 
-  if (running)
-    do {
-      printf ("  %08" PRIx32 ": %08" PRIx32 " %08" PRIx32 " %08" PRIx32 " %08"
-              PRIx32 "\r\n", d, ETH_DES0 (d), ETH_DES1 (d), ETH_DES2 (d),
-              ETH_DES3 (d));
+  for (i = 0; i < ETHARP_HWADDR_LEN; ++i)
+    sa[i] ^= * (ptr++);
 
-      d = ETH_DES3 (d);
+  for (i = 0; i < ETHARP_HWADDR_LEN; ++i)
+    sa[i] ^= * (ptr++);
 
-    } while (d != s);
+  sa[0] &= 0xfe; /*Clear I/G */
+  sa[0] |= 0x2; /*Set U/L */
 
-#endif
+  printf ("MAC ADDRESS is %02x:%02x:%02x:%02x:%02x:%02x\r\n", sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
+}
+
+
+
+#define ETH_DEBUG(a)  \
+  do { static uint32_t w; \
+    uint32_t v=a; \
+    if (v!=w) {\
+      printf (" " #a":   %08x (%08x +%08x -%08x)\r\n", \
+              (unsigned) v,(unsigned) (v^w),(unsigned) ((v^w) &v),(unsigned) ((v^w)&w)); \
+    }\
+    w=v; \
+  } while (0)
+
+static void eth_debug (void)
+{
+  printf ("Net:\r\n");
 
+  ETH_DEBUG (ETH_MACCR);
+  ETH_DEBUG (ETH_MACFFR);
+  ETH_DEBUG (ETH_MACFCR);
+  ETH_DEBUG (ETH_MACDBGR);
+  ETH_DEBUG (ETH_MACSR);
+  ETH_DEBUG (ETH_DMAOMR);
+  ETH_DEBUG (ETH_DMASR);
+  ETH_DEBUG (ETH_DMAIER);
+  ETH_DEBUG (ETH_DMACHTDR);
+  ETH_DEBUG (ETH_DMACHRDR);
+  ETH_DEBUG (ETH_DMAOMR);
+  ETH_DEBUG (ETH_DMATDLAR);
+  ETH_DEBUG (ETH_DMARDLAR);
+  ETH_DEBUG (ETH_DMABMR);
 }
 
 
 #define PHY_REGS 0x20
-static void phy_stat_reg (unsigned i)
+static void phy_show_reg (unsigned i)
 {
   static uint16_t last_phy[PHY_REGS];
-  uint16_t cur = eth_smi_read (PHY, i);
+  uint16_t cur = eth_smi_read_wtimeo (PHY, i);
 
   if (cur == last_phy[i]) return;
 
@@ -114,51 +158,99 @@ static void phy_stat_reg (unsigned i)
 }
 
 
-static void phy_stat (void)
+static void phy_debug (void)
 {
   unsigned i;
 
   for (i = 0; i < PHY_REGS; ++i)
-    phy_stat_reg (i);
+    phy_show_reg (i);
 }
 
 static bool
 phy_link_an_done (uint8_t phy)
 {
-  return eth_smi_read (phy, PHY_REG_BSR) & PHY_REG_BSR_ANDONE;
+  return eth_smi_read_wtimeo (phy, PHY_REG_BSR) & PHY_REG_BSR_ANDONE;
 }
 
 
 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 (p->payload, p->len)) {
+    delay_us (10);
+
+    if (! (tries--)) return ERR_IF;
+  }
+
 
   return ERR_OK;
 }
 
+
+static bool eth_rx_ptp (uint8_t *ppkt, uint32_t *len, uint32_t maxlen, uint64_t *tstamp)
+{
+  bool fs = false;
+  bool ls = false;
+  bool overrun = false;
+  uint32_t l = 0;
+
+  while (! (ETH_DES0 (RxBD) & ETH_RDES0_OWN) && !ls) {
+    l = (ETH_DES0 (RxBD) & ETH_RDES0_FL) >> ETH_RDES0_FL_SHIFT;
+
+    fs |= ETH_DES0 (RxBD) & ETH_RDES0_FS;
+    ls |= ETH_DES0 (RxBD) & ETH_RDES0_LS;
+    /* frame buffer overrun ?*/
+    overrun |= fs && (maxlen < l);
+
+    if (fs && !overrun) {
+      memcpy (ppkt, (void *)ETH_DES2 (RxBD), l);
+      ppkt += l;
+      *len += l;
+      maxlen -= l;
+    }
+
+    *tstamp = ptp_to_u64 (ETH_DES6 (RxBD), ETH_DES7 (RxBD));
+
+    ETH_DES0 (RxBD) = ETH_RDES0_OWN;
+    RxBD = ETH_DES3 (RxBD);
+  }
+
+  if (ETH_DMASR & ETH_DMASR_RBUS) {
+    ETH_DMASR = ETH_DMASR_RBUS;
+    ETH_DMARPDR = 0;
+  }
+
+  return fs && ls && !overrun;
+}
+
+
 static err_t
 steth_rx (void)
 {
   struct pbuf *p;
   uint32_t len;
 
-
   p = pbuf_alloc (PBUF_RAW, MTU, PBUF_POOL);
 
   if (!p) return ERR_MEM;
 
   len = 0;
 
-  if (!eth_rx (p->payload, &len, MTU)) {
+  p->ptp_timestamp = 0; //get_ptp();
+
+  if (!eth_rx_ptp (p->payload, &len, MTU, &p->ptp_timestamp)) {
     pbuf_free (p);
     return ERR_IF;
   }
 
   pbuf_realloc (p, len);
 
+
   return if0.input (p, &if0);
 }
 
@@ -239,36 +331,6 @@ static void my_eth_init (uint8_t phy, enum eth_clk clock)
 }
 
 
-
-#if 0
-static void phy_set_ignore_address (void)
-{
-  unsigned i;
-
-  i = 0;
-
-  while (eth_smi_read (PHY0, 0) == 0xffff) {
-    delay_us (1000);
-    i++;
-
-    if (i > 10) break;
-  }
-
-  eth_smi_write (PHY, 0x11, 0x8);
-
-  i = 0;
-
-  while (eth_smi_read (PHY1, 0) == 0xffff) {
-    delay_us (1000);
-    i++;
-
-    if (i > 10) break;
-  }
-
-  eth_smi_write (PHY1, 0x11, 0x8);
-}
-#endif
-
 static void eth_reset (void)
 {
   unsigned i;
@@ -286,18 +348,6 @@ static void eth_reset (void)
   rcc_periph_reset_release (RST_ETHMAC);
 
 
-#ifdef NRST
-#if 0
-  delay_us (1000);
-  CLEAR (NRST);
-  delay_us (1);
-  SET (NRST);
-  delay_us (1000);
-#endif
-#endif
-
-
-  TRACE;
   ETH_DMABMR |= ETH_DMABMR_SR;
 
   i = 0;
@@ -306,20 +356,18 @@ static void eth_reset (void)
     delay_us (1000);
     i++;
 
-    if (i > 1000) {
+    if (i > 10) {
       printf ("No 50MHz clock to ethernet MAC\n");
       return;
     }
   }
 
   /*MDC = HCLK / 102 (0b100) => 1.6MHz */
-  TRACE;
-
   my_eth_init (PHY, ETH_CLK_150_168MHZ);
+
   //phy_set_ignore_address();
 
-  TRACE;
-  phy_stat();
+  phy_debug();
 
   eth_set_mac (sa);
   eth_desc_init (eth_buf, TX_BUFS, RX_BUFS, FRAME_SZ, FRAME_SZ, 1);
@@ -328,19 +376,15 @@ static void eth_reset (void)
   eth_irq_enable (ETH_DMAIER_NISE);
   eth_irq_enable (ETH_DMAIER_RIE);
   eth_irq_enable (ETH_DMASR_TS);
-
-
 }
 
 static void eth_start_an (void)
 {
   printf ("starting autonegociation\r\n");
-  eth_smi_write (PHY, PHY_REG_ANTX, 0x1e1);
-  phy_autoneg_enable (PHY);
+  eth_smi_write_wtimeo (PHY, PHY_REG_ANTX, 0x1e1);
+  phy_autoneg_enable_wtimeo (PHY);
 }
 
-
-
 void
 steth_init (void)
 {
@@ -393,37 +437,23 @@ steth_init (void)
   MAP_AF_100 (RXD1, GPIO_AF11);
 
 
-
   /* The switch to RMII has be done with steth under reset, with no clock */
 
-
   eth_reset();
-
-  phy_stat();
-  mac_stat();
-
-
-
-
-#if 0
-  eth_smi_write (PHY, PHY_REG_BCR, 0x2100);
-  printf ("Waiting for link\r\n");
-
-  while (!phy_link_isup (phy)) {
-    phy_stat();
-    delay_ms (1000);
-  }
-
-#endif
+  phy_debug();
+  eth_debug();
 
   eth_start_an();
 
   nvic_enable_irq (NVIC_ETH_IRQ);
 
+  /* Fire up timestamping hardware */
 
-  ready++;
-
+  ETH_PTPTSCR |= ETH_PTPTSCR_TSE | ETH_PTPTSCR_TSSARFE;
+  ETH_PTPTSCR &= ~ETH_PTPTSCR_TSFCU;
+  ETH_PTPSSIR = 1;
 
+  ready++;
 }
 
 
@@ -438,27 +468,28 @@ static void eth_stop (void)
 
 void steth_slow_tick (void)
 {
-  phy_stat();
+  //printf("eth slow tick an_clock=%d\r\n",an_clock);
+
+  phy_debug();
+
 
 #if 0
-  mac_stat();
+  eth_debug();
 #endif
 
   if (!ready) return;
 
   an_happy = phy_link_an_done (PHY);
 
-  if ((!phy_link_isup (PHY) || !an_happy) && running) {
+  if ((!phy_link_isup_wtimeo (PHY) || !an_happy) && running) {
 
     printf ("stopping nic\r\n");
     //eth_reset();
     ETH_MACCR |= ETH_MACCR_RD;
-
     running = 0;
-
   }
 
-  if (!phy_link_isup (PHY) && an_happy) {
+  if (!phy_link_isup_wtimeo (PHY) && an_happy) {
     eth_start_an();
     an_clock = 0;
   }
@@ -475,11 +506,11 @@ void steth_slow_tick (void)
     an_clock = 0;
 
 
-  if (phy_link_isup (PHY) && an_happy && !running) {
+  if (phy_link_isup_wtimeo (PHY) && an_happy && !running) {
     printf ("autonegociation done\r\n");
     printf ("phy link status %x\r\n", phy_link_status (PHY));
 
-    switch (phy_link_status (PHY)) {
+    switch (phy_link_status_wtimeo (PHY)) {
     case LINK_HD_10M:
       TRACE;
       break;
@@ -496,11 +527,12 @@ void steth_slow_tick (void)
 
     ETH_MACCR &= ~ETH_MACCR_RD;
     eth_start();
+
+
     printf ("starting nic\r\n");
     running++;
   }
 
-
-
+  //  printf("end slow tick\r\n");
 }