From 7d48ef263a0b26ff3e63c02874e1175edb587cfc Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Tue, 14 Oct 2014 12:47:04 +0800
Subject: [PATCH] soc/runtime: fix RTIO sequence error detection on FUD

---
 soc/runtime/dds.c  | 57 ++++------------------------------------------
 soc/runtime/rtio.c | 38 +++++++++++++++++++++++++++++++
 soc/runtime/rtio.h |  3 +++
 3 files changed, 46 insertions(+), 52 deletions(-)

diff --git a/soc/runtime/dds.c b/soc/runtime/dds.c
index 0b85e30d1..56442eeb9 100644
--- a/soc/runtime/dds.c
+++ b/soc/runtime/dds.c
@@ -2,7 +2,7 @@
 #include <hw/common.h>
 #include <stdio.h>
 
-#include "exceptions.h"
+#include "rtio.h"
 #include "dds.h"
 
 #define DDS_FTW0  0x0a
@@ -17,53 +17,6 @@
 #define DDS_WRITE(addr, data) \
     MMPTR(0xb0000000 + (addr)*4) = data
 
-#define RTIO_FUD_CHANNEL 4
-
-static void fud_sync(void)
-{
-    rtio_chan_sel_write(RTIO_FUD_CHANNEL);
-    while(rtio_o_level_read() != 0);
-}
-
-static void fud(long long int fud_time)
-{
-    int r;
-    long long int fud_end_time;
-    static long long int previous_fud_end_time;
-
-    r = rtio_reset_counter_read();
-    if(r)
-        previous_fud_end_time = 0;
-    rtio_reset_counter_write(0);
-
-    rtio_chan_sel_write(RTIO_FUD_CHANNEL);
-    if(fud_time < 0) {
-        rtio_counter_update_write(1);
-        fud_time = rtio_counter_read() + 4000;
-    }
-    fud_end_time = fud_time + 3*8;
-    if(fud_time < previous_fud_end_time)
-        exception_raise(EID_RTIO_SEQUENCE_ERROR);
-    previous_fud_end_time = fud_end_time;
-
-    rtio_o_timestamp_write(fud_time);
-    rtio_o_value_write(1);
-    rtio_o_we_write(1);
-    rtio_o_timestamp_write(fud_end_time);
-    rtio_o_value_write(0);
-    rtio_o_we_write(1);
-    if(rtio_o_error_read()) {
-        rtio_reset_logic_write(1);
-        rtio_reset_logic_write(0);
-        exception_raise(EID_RTIO_UNDERFLOW);
-    }
-
-    if(r) {
-        fud_sync();
-        rtio_reset_counter_write(1);
-    }
-}
-
 void dds_init(void)
 {
     int i;
@@ -75,18 +28,18 @@ void dds_init(void)
         DDS_WRITE(0x01, 0x00);
         DDS_WRITE(0x02, 0x00);
         DDS_WRITE(0x03, 0x00);
-        fud(-1);
-        fud_sync();
+        rtio_fud(-1);
+        rtio_fud_sync();
     }
 }
 
 void dds_program(int channel, int ftw, long long int fud_time)
 {
-    fud_sync();
+    rtio_fud_sync();
     DDS_WRITE(DDS_GPIO, channel);
     DDS_WRITE(DDS_FTW0, ftw & 0xff);
     DDS_WRITE(DDS_FTW1, (ftw >> 8) & 0xff);
     DDS_WRITE(DDS_FTW2, (ftw >> 16) & 0xff);
     DDS_WRITE(DDS_FTW3, (ftw >> 24) & 0xff);
-    fud(fud_time);
+    rtio_fud(fud_time);
 }
diff --git a/soc/runtime/rtio.c b/soc/runtime/rtio.c
index 14153e6d0..2b172d890 100644
--- a/soc/runtime/rtio.c
+++ b/soc/runtime/rtio.c
@@ -3,8 +3,11 @@
 #include "exceptions.h"
 #include "rtio.h"
 
+long long int previous_fud_end_time;
+
 void rtio_init(void)
 {
+    previous_fud_end_time = 0;
     rtio_reset_counter_write(1);
     rtio_reset_logic_write(1);
     rtio_reset_logic_write(0);
@@ -64,3 +67,38 @@ long long int rtio_get(int channel)
     }
     return -1;
 }
+
+#define RTIO_FUD_CHANNEL 4
+
+void rtio_fud_sync(void)
+{
+    rtio_sync(RTIO_FUD_CHANNEL);
+}
+
+void rtio_fud(long long int fud_time)
+{
+    long long int fud_end_time;
+
+    rtio_reset_counter_write(0);
+    rtio_chan_sel_write(RTIO_FUD_CHANNEL);
+    if(fud_time < 0) {
+        rtio_counter_update_write(1);
+        fud_time = rtio_counter_read() + 4000;
+    }
+    fud_end_time = fud_time + 3*8;
+    if(fud_time < previous_fud_end_time)
+        exception_raise(EID_RTIO_SEQUENCE_ERROR);
+    previous_fud_end_time = fud_end_time;
+
+    rtio_o_timestamp_write(fud_time);
+    rtio_o_value_write(1);
+    rtio_o_we_write(1);
+    rtio_o_timestamp_write(fud_end_time);
+    rtio_o_value_write(0);
+    rtio_o_we_write(1);
+    if(rtio_o_error_read()) {
+        rtio_reset_logic_write(1);
+        rtio_reset_logic_write(0);
+        exception_raise(EID_RTIO_UNDERFLOW);
+    }
+}
diff --git a/soc/runtime/rtio.h b/soc/runtime/rtio.h
index bce2602ea..92a54674a 100644
--- a/soc/runtime/rtio.h
+++ b/soc/runtime/rtio.h
@@ -8,4 +8,7 @@ void rtio_replace(long long int timestamp, int channel, int value);
 void rtio_sync(int channel);
 long long int rtio_get(int channel);
 
+void rtio_fud_sync(void);
+void rtio_fud(long long int fud_time);
+
 #endif /* __RTIO_H */