timing: reworked timestamp calculation, added test program

[eurobot/public.git] / src / timing / timing.c

diff --git a/src/timing/timing.c b/src/timing/timing.c
index eae74a1b005cfbeb917a0569c71a4cddee25a5d9..78a5b7e28cce21536c5c1746a55cfd24342a2a63 100644 (file)
--- a/src/timing/timing.c
+++ b/src/timing/timing.c
@@ -52,39 +52,42 @@ void timing_output_trace_screen(void);
 void timing_output_trace_file(void);
 void timing_statistics_clock_gettime(void);
 
+uint32_t do_div(uint64_t *n, uint32_t base)
+{
+       uint32_t remainder = *n % base;
+       *n = *n / base;
+       return remainder;
+}
+
+long int ns2usec(uint64_t nsec)
+{
+       nsec += 500;
+       do_div(&nsec, 1000);
+
+       return nsec;
+}
+
 uint64_t mulhdu(uint64_t a, uint64_t b)
 {
-       long double res;
-       res = ((long double)a*(long double)b)/pow(2.0,64.0);
-       return (uint64_t)res;
-//     asm volatile (
-//             "        cmpwi   6,0\n"
-//             "        cmpwi   cr1,3,0\n"
-//             "        mr      10,4\n"
-//             "        mulhwu  4,4,5\n"
-//             "        beq     1f\n"
-//             "        mulhwu  0,10,6\n"
-//             "        mullw   7,10,5\n"
-//             "        addc    7,0,7\n"
-//             "        addze   4,4\n"
-//             "1:      beqlr   cr1\n"             /* all done if high part of A is 0 */
-//             "        mr      10,3\n"
-//             "        mullw   9,3,5\n"
-//             "        mulhwu  3,3,5\n"
-//             "        beq     2f\n"
-//             "        mullw   0,10,6\n"
-//             "        mulhwu  8,10,6\n"
-//             "        addc    7,0,7\n"
-//             "        adde    4,4,8\n"
-//             "        addze   3,3\n"
-//             "2:      addc    4,4,9\n"
-//             "        addze   3,3\n"
-//             "        blr\n"
-//             :
-//             :
-//             :"cr1"
-//     
-//     );
+       uint64_t ret, ah, al, bh, bl, carry, c1, c2, c3;
+       ah = a>>32;
+       al = a & 0xffffffff;
+       bh = b>>32;
+       bl = b & 0xffffffff;
+
+       c1 = al*bl;
+       c2 = (ah*bl)<<32;
+       c3 = (al*bh)<<32;
+
+       carry = 0;
+       if (~c1 < c2) carry++;
+       c1 += c2;
+       if (~c1 < c3) carry++;
+
+
+       ret = ah*bh + (ah*bl>>32) + (al*bh>>32) + carry;
+
+       return ret;
 }
 
 #ifdef CONFIG_TIMING_IPOINT_ASM
@@ -114,20 +117,16 @@ uint64_t get_boot_tb(void)
 void print_tstamp_to_schedclock(timestamp_t *ts)
 {
        uint64_t tb;
-       uint64_t sched_clock;
+       uint64_t sched_clock, ns;
        unsigned long int sec, usec;
 
        tb = restore_tb(ts->tbu, ts->tbl);
-//     printf("tb=%llu\n",tb);
-//     printf("mulhdu=%llu\n",mulhdu(tb - get_boot_tb(), tb_to_ns_scale));
-       sched_clock = (uint64_t)((long double)(mulhdu(tb - get_boot_tb(), tb_to_ns_scale))*pow(2.0,5.0));
-//     printf("sched_clock=%llu\n",sched_clock);
+       ns = (uint64_t)((mulhdu(tb - get_boot_tb(), tb_to_ns_scale))<<5);
        
-       sched_clock+=500;
-       sched_clock=sched_clock / 1000;
+       sched_clock = ns2usec(ns);
 
-       sec = (uint32_t) (sched_clock/1000000);
-       usec = (sched_clock % 1000000ULL);
+       usec = do_div(&sched_clock, 1000000ULL);
+       sec = (uint32_t) (sched_clock);
 
 #ifdef CONFIG_TIMING_OUTPUT_SCREEN
        printf("%d %5lu.%06lu\n", ts->id, sec, usec);
@@ -233,6 +232,7 @@ void timing_init(void)
        timing_statistics = timing_statistics_clock_gettime;
 #endif
        system("mount -t debugfs trace /debug");
+       system("echo nop > /debug/tracing/current_tracer");     //clean the buffer
        system("echo sched_switch > /debug/tracing/current_tracer");
 
 #ifdef CONFIG_CLOCK_RT
@@ -271,12 +271,11 @@ void timing_finish()
 {
        system("echo 0 > /debug/tracing/tracing_enabled");
 #ifdef CONFIG_TIMING_OUTPUT_FILE
-       system("cat /debug/tracing/trace | tee taskswitch.log");
+       system("cat /debug/tracing/trace > taskswitch.log");
 #endif
 #ifdef CONFIG_TIMING_OUTPUT_SCREEN
        system("cat /debug/tracing/trace");
 #endif
-       system("echo nop > /debug/tracing/current_tracer");
 }
 
 #ifdef CONFIG_TIMING_IPOINT_ASM
@@ -328,7 +327,7 @@ void timing_output_trace_file(void)
        fprintf(file, "Timing analysis application trace\n");
        ts_iter = ts;
        if(timestamp_enabled)
-       while(ts_iter != (timestamp_ptr+1))
+       while(ts_iter != (timestamp_ptr))
        {
 #ifdef CONFIG_TIMING_IPOINT_ASM
                print_tstamp_to_schedclock(ts_iter);