1 files changed, 48 insertions, 0 deletions

diff --git a/firmware/target/arm/as3525/dbop-as3525.c b/firmware/target/arm/as3525/dbop-as3525.c
index 938eee2fb1..b54ad17a6c 100644
--- a/firmware/target/arm/as3525/dbop-as3525.c
+++ b/firmware/target/arm/as3525/dbop-as3525.c
@@ -20,6 +20,7 @@
  ****************************************************************************/
 
 #include "config.h"
+#include <inttypes.h>
 #include "as3525.h"
 #include "dbop-as3525.h"
 
@@ -75,3 +76,50 @@ unsigned short dbop_debug(void)
 {
     return dbop_input_value;
 }
+
+static inline void dbop_set_mode(int mode)
+{
+    int delay = 10;
+    if (mode == 32 && (!(DBOP_CTRL & (1<<13|1<<14))))
+        DBOP_CTRL |= (1<<13|1<<14);
+    else if (mode == 16 && (DBOP_CTRL & (1<<13|1<<14)))
+        DBOP_CTRL &= ~(1<<14|1<<13);
+    else
+        return;
+    while(delay--) asm volatile("nop");
+}
+
+void dbop_write_data(const int16_t* p_bytes, int count)
+{
+    
+    const int32_t *data;
+    if ((intptr_t)p_bytes & 0x3 || count == 1)
+    {   /* need to do a single 16bit write beforehand if the address is
+         * not word aligned or count is 1, switch to 16bit mode if needed */
+        dbop_set_mode(16);
+        DBOP_DOUT16 = *p_bytes++;
+        if (!(--count))
+            return;
+    }
+    /* from here, 32bit transfers are save
+     * set it to transfer 4*(outputwidth) units at a time,
+     * if bit 12 is set it only does 2 halfwords though (we never set it)
+     * switch to 32bit output if needed */
+    dbop_set_mode(32);
+    data = (int32_t*)p_bytes;
+    while (count > 1)
+    {
+        DBOP_DOUT32 = *data++;
+        count -= 2;
+
+        /* Wait if push fifo is full */
+        while ((DBOP_STAT & (1<<6)) != 0);
+    }
+    /* While push fifo is not empty */
+    while ((DBOP_STAT & (1<<10)) == 0);
+
+    /* due to the 32bit alignment requirement or uneven count,
+     * we possibly need to do a 16bit transfer at the end also */
+    if (count > 0)
+        dbop_write_data((int16_t*)data, 1);
+}