new cascade math function by Frank Mori Hess

diff --git a/comedi/drivers/8253.h b/comedi/drivers/8253.h
index 85073bf29437f85d5a1671458a8e1d670441c2f9..84a3bfe8ae938f4e2b585c2490965244391f2f8c 100644 (file)
--- a/comedi/drivers/8253.h
+++ b/comedi/drivers/8253.h
@@ -26,7 +26,7 @@
 
 #define i8253_cascade_ns_to_timer i8253_cascade_ns_to_timer_power
 
-static inline void i8253_cascade_ns_to_timer_2div(int i8253_osc_base, unsigned int *d1, unsigned int *d2, unsigned int *nanosec, int round_mode)
+static inline void i8253_cascade_ns_to_timer_2div_old(int i8253_osc_base, unsigned int *d1, unsigned int *d2, unsigned int *nanosec, int round_mode)
 {
        int divider;
        int div1, div2;
@@ -68,8 +68,8 @@ static inline void i8253_cascade_ns_to_timer_2div(int i8253_osc_base, unsigned i
        }
 
        *nanosec = div1_lub * div2_lub * i8253_osc_base;
-       *d1 = div1_lub;
-       *d2 = div2_lub;
+       *d1 = div1_lub & 0xffff;
+       *d2 = div2_lub & 0xffff;
        return;
 }
 
@@ -110,5 +110,79 @@ static inline void i8253_cascade_ns_to_timer_power(int i8253_osc_base, unsigned
 }
 
 
+/* this function is meant to replace i8253_cascade_ns_to_timer_2div
+ * (which is completely broken at the moment)
+ */
+static inline void i8253_cascade_ns_to_timer_2div(int i8253_osc_base,
+       unsigned int *d1, unsigned int *d2, unsigned int *nanosec, int round_mode)
+{
+       unsigned int divider;
+       unsigned int div1, div2;
+       unsigned int div1_glb, div2_glb, ns_glb;
+       unsigned int div1_lub, div2_lub, ns_lub;
+       unsigned int ns;
+       unsigned int start;
+       unsigned int ns_low, ns_high;
+
+       divider = *nanosec / i8253_osc_base;
+
+       div1_lub = div2_lub = 0;
+       div1_glb = div2_glb = 0;
+
+       ns_glb = 0;
+       ns_lub = 0xffffffff;
+
+       div2 = 0x10000;
+       start = divider / div2;
+       if(start < 2) start = 2;
+       for (div1 = start; div1 <= divider / div1 + 1; div1++) {
+               for(div2 = divider / div1; div1 * div2 <= divider + div1 + 1; div2++) {
+                       ns = i8253_osc_base * div1 * div2;
+                       if (ns <= *nanosec && ns > ns_glb) {
+                               ns_glb = ns;
+                               div1_glb = div1;
+                               div2_glb = div2;
+                       }
+                       if (div2 <= 0x10000) {
+                               ns = i8253_osc_base * div1 * div2;
+                               if (ns >= *nanosec && ns < ns_lub) {
+                                       ns_lub = ns;
+                                       div1_lub = div1;
+                                       div2_lub = div2;
+                               }
+                       }
+               }
+       }
+
+       switch (round_mode) {
+       case TRIG_ROUND_NEAREST:
+       default:
+               ns_high = div1_lub * div2_lub * i8253_osc_base;
+               ns_low = div1_glb * div2_glb * i8253_osc_base;
+               if( ns_high - *nanosec < *nanosec - ns_low) {
+                       div1 = div1_lub;
+                       div2 = div2_lub;
+               } else {
+                       div1 = div1_glb;
+                       div2 = div2_glb;
+               }
+               break;
+       case TRIG_ROUND_UP:
+               div1 = div1_lub;
+               div2 = div2_lub;
+               break;
+       case TRIG_ROUND_DOWN:
+               div1 = div1_glb;
+               div2 = div2_glb;
+               break;
+       }
+
+       *nanosec = div1 * div2 * i8253_osc_base;
+       *d1 = div1 & 0xffff;
+       *d2 = div2 & 0xffff;
+       return;
+}
+
+
 #endif