From 5b7dcab412af9bdfc53a243df49adc0670e82784 Mon Sep 17 00:00:00 2001
From: Ian Gilmour <ian@sdrsharp.com>
Date: Wed, 1 Jul 2015 23:17:12 +0100
Subject: [PATCH] Added inline asm packing routine. (Pending review) Go back to
 stock clock for packing builds.

---
 airspy_m4/airspy_m4.c      | 44 ++++++++++++++++++++++++++++++++++----
 airspy_m4/airspy_m4_conf.c |  4 ----
 common/airspy_conf.h       |  6 ------
 3 files changed, 40 insertions(+), 14 deletions(-)

diff --git a/airspy_m4/airspy_m4.c b/airspy_m4/airspy_m4.c
index d1247be..e9d90ba 100644
--- a/airspy_m4/airspy_m4.c
+++ b/airspy_m4/airspy_m4.c
@@ -122,6 +122,7 @@ uint32_t data_counter = 0;
 #endif
 
 #ifdef USE_PACKING
+/*
 __attribute__ ((always_inline)) static void pack(uint16_t* input, uint32_t* output, uint32_t length)
 {
   uint32_t i;
@@ -139,7 +140,42 @@ __attribute__ ((always_inline)) static void pack(uint16_t* input, uint32_t* outp
 
     output += 3;
   }
+}*/
+
+__attribute__ ((always_inline)) static void pack(uint32_t* input, uint32_t* output, uint32_t length)
+{
+	register uint32_t *a0 asm("r0") = input;
+	register uint32_t *a1 asm("r1") = output;
+	register uint32_t a2 asm("r2") = length;
+	
+	asm volatile("1:\n\t"
+				 "ldm.w %0!, {r4, r5, r6, r7}\n\t"
+				 
+				 "lsr	r8, r4, #16\n\t"
+				 "ubfx	r3, r5, #4, #12\n\t"
+				 "orr	r8, r3, r8, lsl #8\n\t"
+				 "orr	r8, r8, r4, lsl #20\n\t"
+				 "lsrs	r3, r5, #16\n\t"
+				 "lsls	r5, r5, #28\n\t"
+				 "orr	r5, r5, r3, lsl #16\n\t"
+				 "orr	r5, r5, r6, lsr #24\n\t"
+				 "uxth	r9, r6\n\t"
+				 "orr	r9, r5, r9, lsl #4\n\t"
+				 "lsrs	r6, r6, #16\n\t"
+				 "uxth	r10, r7\n\t"
+				 "lsl	r10, r10, #12\n\t"
+				 "orr	r10, r10, r6, lsl #24\n\t"	
+				 "orr	r10, r10, r7, lsr #16\n\t"
+		
+				 "stm.w %1!, {r8, r9, r10}\n\t"
+
+				 "subs	%2, %2, #8\n\t"
+				 "bne 1b\n\t"
+				: "+r"(a0), "+r"(a1), "+r"(a2)
+				:: "memory", "r3", "r4", "r5", "r6", "r7", "r8", "r9", "r10");	
+				
 }
+
 #endif
 
 static __inline__ void clr_usb_buffer_offset(void)
@@ -443,7 +479,7 @@ int main(void)
   case 0:
     if(phase == 0)
     {
-      pack((uint16_t*) &usb_bulk_buffer[0x0000], (uint32_t*) &usb_bulk_buffer[0x0000], 0x1000);
+      pack((uint32_t*) &usb_bulk_buffer[0x0000], (uint32_t*) &usb_bulk_buffer[0x0000], 0x1000);
     
       set_usb_buffer_offset( inc_mask_usb_buffer_offset(get_usb_buffer_offset(), 1) );    
       signal_sev();
@@ -453,7 +489,7 @@ int main(void)
   case 1:
     if(phase == 1)
     {
-      pack((uint16_t*) &usb_bulk_buffer[0x2000], (uint32_t*) &usb_bulk_buffer[0x2000], 0x1000);
+      pack((uint32_t*) &usb_bulk_buffer[0x2000], (uint32_t*) &usb_bulk_buffer[0x2000], 0x1000);
     
       set_usb_buffer_offset( inc_mask_usb_buffer_offset(get_usb_buffer_offset(), 1) );    
       signal_sev();
@@ -463,7 +499,7 @@ int main(void)
   case 2:
     if(phase == 2)
     {
-      pack((uint16_t*) &usb_bulk_buffer[0x4000], (uint32_t*) &usb_bulk_buffer[0x4000], 0x1000);
+      pack((uint32_t*) &usb_bulk_buffer[0x4000], (uint32_t*) &usb_bulk_buffer[0x4000], 0x1000);
     
       set_usb_buffer_offset( inc_mask_usb_buffer_offset(get_usb_buffer_offset(), 1) );    
       signal_sev();
@@ -473,7 +509,7 @@ int main(void)
   case 3:
     if(phase == 3)
     {
-      pack((uint16_t*) &usb_bulk_buffer[0x6000], (uint32_t*) &usb_bulk_buffer[0x6000], 0x1000);
+      pack((uint32_t*) &usb_bulk_buffer[0x6000], (uint32_t*) &usb_bulk_buffer[0x6000], 0x1000);
     
       set_usb_buffer_offset( inc_mask_usb_buffer_offset(get_usb_buffer_offset(), 1) );    
       signal_sev();
diff --git a/airspy_m4/airspy_m4_conf.c b/airspy_m4/airspy_m4_conf.c
index e11dd35..b2a76db 100644
--- a/airspy_m4/airspy_m4_conf.c
+++ b/airspy_m4/airspy_m4_conf.c
@@ -42,11 +42,7 @@ const airspy_sys_clock_t airspy_m4_init_conf =
   {
     0, // uint32_t pll1_hs_psel;
     0, // uint32_t pll1_hs_nsel;
-#ifdef USE_PACKING	
-    8, // uint32_t pll1_hs_msel;
-#else
 	6, // uint32_t pll1_hs_msel;
-#endif
   },
   /* PLL1 Low Speed Mode => Set PLL1 to 20MHz * (1+1) = 40MHz */
   {
diff --git a/common/airspy_conf.h b/common/airspy_conf.h
index daaa7ab..4b0110a 100644
--- a/common/airspy_conf.h
+++ b/common/airspy_conf.h
@@ -33,15 +33,9 @@ extern "C"
 #define AIRSPY_SI5351C_CONFIG_XTAL (0)
 #define AIRSPY_SI5351C_CONFIG_CLKIN (1)
 
-#ifdef USE_PACKING
-/* Configure I2C0 & IC21 to about 400kHz (180MHz/(2*280)=0.32MHz) */
-#define AIRSPY_I2C0_PLL1_LS_HS_CONF_VAL (280)
-#define AIRSPY_I2C1_PLL1_HS_CONF_VAL (280)
-#else
 /* Configure I2C0 & IC21 to 400kHz (140MHz/(2*175)=0.4MHz) */
 #define AIRSPY_I2C0_PLL1_LS_HS_CONF_VAL (175)
 #define AIRSPY_I2C1_PLL1_HS_CONF_VAL (175)
-#endif
 
 /* Configure I2C1 to 400kHz (40MHz/(2*50)=0.4MHz) */
 #define AIRSPY_I2C1_PLL1_LS_CONF_VAL (50)