Tools/CPUInfo/CPUInfo.cpp

   1 /*
   2   test CPU speed
   3   Andrew Tridgell September 2011
   4 */
   5
   6 #define ALLOW_DOUBLE_MATH_FUNCTIONS
   7
   8 #include <cmath>
   9
  10 #include <AP_HAL/AP_HAL.h>
  11 #include <AP_Common/AP_Common.h>
  12 #include <AP_Math/AP_Math.h>
  13 #include <AP_Math/div1000.h>
  14 #include <AP_ESC_Telem/AP_ESC_Telem.h>
  15 #include "EKF_Maths.h"
  16
  17 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
  18 #if HAL_WITH_DSP
  19 #include <arm_math.h>
  20 #endif
  21 #include <hrt.h>
  22 #include <ch.h>
  23 #endif // HAL_BOARD_CHIBIOS
  24
  25 void setup();
  26 void loop();
  27
  28 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
  29
  30 // On H750 we want to measure external flash to ram performance
  31 #if defined(EXT_FLASH_SIZE_MB) && EXT_FLASH_SIZE_MB>0 && defined(STM32H7)
  32 #include "ch.h"
  33 #define DISABLE_CACHES
  34 #endif
  35
  36 #ifdef STM32_SYS_CK
  37 static uint32_t sysclk = STM32_SYS_CK;
  38 #elif defined(STM32_SYSCLK)
  39 static uint32_t sysclk = STM32_SYSCLK;
  40 #else
  41 static uint32_t sysclk = 0;
  42 #endif
  43
  44 static EKF_Maths ekf;
  45
  46 HAL_Semaphore sem;
  47 #if HAL_WITH_ESC_TELEM
  48 AP_ESC_Telem telem;
  49 #endif
  50
  51 void setup() {
  52 #ifdef DISABLE_CACHES
  53 #if !HAL_XIP_ENABLED // can't disable DCache in memory-mapped mode
  54     SCB_DisableDCache();
  55 #endif
  56     SCB_DisableICache();
  57 #endif
  58     ekf.init();
  59 }
  60
  61 static void show_sizes(void)
  62 {
  63     hal.console->printf("SYSCLK %uMHz\n", unsigned(sysclk/1000000U));
  64
  65     hal.console->printf("Type sizes:\n");
  66     hal.console->printf("char      : %lu\n", (unsigned long)sizeof(char));
  67     hal.console->printf("short     : %lu\n", (unsigned long)sizeof(short));
  68     hal.console->printf("int       : %lu\n", (unsigned long)sizeof(int));
  69     hal.console->printf("long      : %lu\n", (unsigned long)sizeof(long));
  70     hal.console->printf("long long : %lu\n", (unsigned long)sizeof(long long));
  71     hal.console->printf("bool      : %lu\n", (unsigned long)sizeof(bool));
  72     hal.console->printf("void*     : %lu\n", (unsigned long)sizeof(void *));
  73
  74     hal.console->printf("printing NaN: %f\n", (double)sqrtf(-1.0f));
  75     hal.console->printf("printing +Inf: %f\n", (double)(1.0f/0.0f));
  76     hal.console->printf("printing -Inf: %f\n", (double)(-1.0f/0.0f));
  77 }
  78
  79 #define TENTIMES(x) do { x; x; x; x; x; x; x; x; x; x; } while (0)
  80 #define FIFTYTIMES(x) do { TENTIMES(x); TENTIMES(x); TENTIMES(x); TENTIMES(x); TENTIMES(x); } while (0)
  81
  82 #define TIMEIT(name, op, count) do { \
  83     uint16_t us_end, us_start; \
  84     us_start = AP_HAL::micros16(); \
  85     for (uint8_t i = 0; i < count; i++) { \
  86         FIFTYTIMES(op); \
  87     } \
  88     us_end = AP_HAL::micros16(); \
  89     uint16_t dt_us = us_end - us_start; \
  90     hal.console->printf("%-10s %7.4f usec/call\n", name, double(dt_us) / double(count * 50.0)); \
  91     hal.scheduler->delay(10); \
  92 } while (0)
  93
  94 volatile float v_f = 1.0;
  95 volatile float v_out;
  96 volatile double v_d = 1.0;
  97 volatile double v_out_d;
  98 volatile uint32_t v_32 = 1;
  99 volatile uint32_t v_out_32 = 1;
 100 volatile uint16_t v_16 = 1;
 101 volatile uint16_t v_out_16 = 1;
 102 volatile uint8_t v_8 = 1;
 103 volatile uint8_t v_out_8 = 1;
 104 volatile uint8_t mbuf1[128], mbuf2[128];
 105 volatile uint64_t v_64 = 1;
 106 volatile uint64_t v_out_64 = 1;
 107
 108 #pragma GCC diagnostic error "-Wframe-larger-than=2000"
 109 static void show_timings(void)
 110 {
 111
 112     v_f = 1+(AP_HAL::micros() % 5);
 113     v_out = 1+(AP_HAL::micros() % 3);
 114
 115     v_32 = AP_HAL::millis();
 116     v_32 = 1+(AP_HAL::micros() % 5);
 117     v_out_32 = 1+(AP_HAL::micros() % 3);
 118
 119     v_16 = 1+(AP_HAL::micros() % 5);
 120     v_out_16 = 1+(AP_HAL::micros() % 3);
 121
 122     v_8 = 1+(AP_HAL::micros() % 5);
 123     v_out_8 = 1+(AP_HAL::micros() % 3);
 124
 125
 126     hal.console->printf("Operation timings:\n");
 127     hal.console->printf("Note: timings for some operations are very data dependent\n");
 128
 129     TIMEIT("nop", asm volatile("nop"::), 255);
 130
 131     TIMEIT("micros()", AP_HAL::micros(), 200);
 132     TIMEIT("micros16()", AP_HAL::micros16(), 200);
 133     TIMEIT("millis()", AP_HAL::millis(), 200);
 134     TIMEIT("millis16()", AP_HAL::millis16(), 200);
 135     TIMEIT("micros64()", AP_HAL::micros64(), 200);
 136
 137 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 138     TIMEIT("hrt_micros32()", hrt_micros32(), 200);
 139     TIMEIT("hrt_micros64()", hrt_micros64(), 200);
 140     TIMEIT("hrt_millis32()", hrt_millis32(), 200);
 141     TIMEIT("hrt_millis64()", hrt_millis64(), 200);
 142 #endif
 143
 144     TIMEIT("fadd", v_out += v_f, 100);
 145     TIMEIT("fsub", v_out -= v_f, 100);
 146     TIMEIT("fmul", v_out *= v_f, 100);
 147     TIMEIT("fdiv /=", v_out /= v_f, 100);
 148     TIMEIT("fdiv 2/x", v_out = 2.0f/v_f, 100);
 149
 150     TIMEIT("dadd", v_out_d += v_d, 100);
 151     TIMEIT("dsub", v_out_d -= v_d, 100);
 152     TIMEIT("dmul", v_out_d *= v_d, 100);
 153     TIMEIT("ddiv", v_out_d /= v_d, 100);
 154
 155     TIMEIT("sinf()", v_out = sinf(v_f), 100);
 156     TIMEIT("cosf()", v_out = cosf(v_f), 100);
 157     #if HAL_WITH_DSP && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 158     TIMEIT("arm_sin_f32()", v_out = arm_sin_f32(v_f), 100);
 159     TIMEIT("arm_cos_f32()", v_out = arm_cos_f32(v_f), 100);
 160     #endif
 161     TIMEIT("tanf()", v_out = tanf(v_f), 100);
 162     TIMEIT("acosf()", v_out = acosf(v_f * 0.2), 100);
 163     TIMEIT("asinf()", v_out = asinf(v_f * 0.2), 100);
 164     TIMEIT("atan2f()", v_out = atan2f(v_f * 0.2, v_f * 0.3), 100);
 165     TIMEIT("sqrtf()",v_out = sqrtf(v_f), 100);
 166
 167     TIMEIT("sin()", v_out = sin(v_f), 100);
 168     TIMEIT("cos()", v_out = cos(v_f), 100);
 169     TIMEIT("tan()", v_out = tan(v_f), 100);
 170     TIMEIT("acos()", v_out = acos(v_f * 0.2), 100);
 171     TIMEIT("asin()", v_out = asin(v_f * 0.2), 100);
 172     TIMEIT("atan2()", v_out = atan2(v_f * 0.2, v_f * 0.3), 100);
 173     TIMEIT("sqrt()",v_out = sqrt(v_f), 100);
 174     #if HAL_WITH_DSP && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 175         TIMEIT("arm_sqrt_f32()", arm_sqrt_f32(v_f, (float32_t*)&v_out), 100);
 176     #endif
 177     TIMEIT("sq()",v_out = sq(v_f), 100);
 178     TIMEIT("powf(v,2)",v_out = powf(v_f, 2), 100);
 179     TIMEIT("powf(v,3.1)",v_out = powf(v_f, 3.1), 100);
 180     TIMEIT("EKF",v_out = ekf.test(), 5);
 181
 182     TIMEIT("iadd8", v_out_8 += v_8, 100);
 183     TIMEIT("isub8", v_out_8 -= v_8, 100);
 184     TIMEIT("imul8", v_out_8 *= v_8, 100);
 185     TIMEIT("idiv8", v_out_8 /= v_8, 100);
 186
 187     TIMEIT("iadd16", v_out_16 += v_16, 100);
 188     TIMEIT("isub16", v_out_16 -= v_16, 100);
 189     TIMEIT("imul16", v_out_16 *= v_16, 100);
 190     TIMEIT("idiv16", v_out_16 /= v_16, 100);
 191
 192     TIMEIT("iadd32", v_out_32 += v_32, 100);
 193     TIMEIT("isub32", v_out_32 -= v_32, 100);
 194     TIMEIT("imul32", v_out_32 *= v_32, 100);
 195     TIMEIT("idiv32", v_out_32 /= v_32, 100);
 196
 197     TIMEIT("iadd64", v_out_64 += v_64, 100);
 198     TIMEIT("isub64", v_out_64 -= v_64, 100);
 199     TIMEIT("imul64", v_out_64 *= v_64, 100);
 200     TIMEIT("idiv64", v_out_64 /= v_64, 100);
 201
 202     TIMEIT("memcpy128", memcpy((void*)mbuf1, (const void *)mbuf2, sizeof(mbuf1)); v_out_8 += mbuf1[0], 200);
 203     TIMEIT("memset128", memset((void*)mbuf1, 1, sizeof(mbuf1)); v_out_8 += mbuf1[0], 200);
 204     TIMEIT("delay(1)", hal.scheduler->delay(1), 5);
 205
 206     TIMEIT("SEM", { WITH_SEMAPHORE(sem); v_out_32 += v_32;}, 100);
 207 }
 208
 209 static void test_div1000(void)
 210 {
 211     hal.console->printf("Testing div1000\n");
 212     for (uint32_t i=0; i<2000000; i++) {
 213         uint64_t v = 0;
 214         if (!hal.util->get_random_vals((uint8_t*)&v, sizeof(v))) {
 215             AP_HAL::panic("ERROR: div1000 no random");
 216             break;
 217         }
 218         uint64_t v1 = v / 1000ULL;
 219         uint64_t v2 = uint64_div1000(v);
 220         if (v1 != v2) {
 221             AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx",
 222                           (unsigned long long)v, (unsigned long long)v1, (unsigned long long)v2);
 223             return;
 224         }
 225     }
 226 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 227     // test from locked context
 228     for (uint32_t i=0; i<2000000; i++) {
 229         uint64_t v = 0;
 230         if (!hal.util->get_random_vals((uint8_t*)&v, sizeof(v))) {
 231             AP_HAL::panic("ERROR: div1000 no random");
 232             break;
 233         }
 234         chSysLock();
 235         uint64_t v1 = v / 1000ULL;
 236         uint64_t v2 = uint64_div1000(v);
 237         chSysUnlock();
 238         if (v1 != v2) {
 239             AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx",
 240                           (unsigned long long)v, (unsigned long long)v1, (unsigned long long)v2);
 241             return;
 242         }
 243     }
 244 #endif
 245     hal.console->printf("div1000 OK\n");
 246 }
 247
 248 void loop()
 249 {
 250     show_sizes();
 251     hal.console->printf("\n");
 252     show_timings();
 253     test_div1000();
 254     hal.console->printf("\n");
 255     hal.scheduler->delay(3000);
 256 }
 257
 258 AP_HAL_MAIN();