Line data    Source code

       1             : #include "AP_HAL.h"
       2             : #include "Util.h"
       3             : #include "utility/print_vprintf.h"
       4             : #if defined(__APPLE__) && defined(__MACH__)
       5             : #include <sys/time.h>
       6             : #elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
       7             : #include "ch.h"
       8             : #include "hal.h"
       9             : #else
      10             : #include <time.h>
      11             : #endif
      12             : #include <AP_InternalError/AP_InternalError.h>
      13             : #include <AP_Math/AP_Math.h>
      14             : 
      15             : /* Helper class implements AP_HAL::Print so we can use utility/vprintf */
      16             : class BufferPrinter : public AP_HAL::BetterStream {
      17             : public:
      18     2073353 :     BufferPrinter(char* str, size_t size)  :
      19     2073353 :         _offs(0), _str(str), _size(size)  {}
      20             : 
      21    50519452 :     size_t write(uint8_t c) override {
      22    50519452 :         if (_offs < _size) {
      23    50365570 :             _str[_offs] = c;
      24             :         }
      25    50519452 :         _offs++;
      26    50519452 :         return 1;
      27             :     }
      28           0 :     size_t write(const uint8_t *buffer, size_t size) override {
      29           0 :         size_t n = 0;
      30           0 :         while (size--) {
      31           0 :             n += write(*buffer++);
      32             :         }
      33           0 :         return n;
      34             :     }
      35             : 
      36             :     size_t _offs;
      37             :     char* const  _str;
      38             :     const size_t _size;
      39             : 
      40           0 :     uint32_t available() override { return 0; }
      41           0 :     bool read(uint8_t &b) override { return false; }
      42           0 :     uint32_t txspace() override { return 0; }
      43           0 :     bool discard_input() override { return false; }
      44             : };
      45             : 
      46      741178 : int AP_HAL::Util::snprintf(char* str, size_t size, const char *format, ...)
      47             : {
      48             :     va_list ap;
      49      741178 :     va_start(ap, format);
      50      741178 :     int res = vsnprintf(str, size, format, ap);
      51      741178 :     va_end(ap);
      52      741178 :     return res;
      53             : }
      54             : 
      55     2073353 : int AP_HAL::Util::vsnprintf(char* str, size_t size, const char *format, va_list ap)
      56             : {
      57             :     // note that size==0 must be handled as functions like vasprintf() rely on the return
      58             :     // value being the number of bytes that would be printed if there was enough space.
      59     2073353 :     BufferPrinter buf(str, size?size-1:0);
      60     2073352 :     print_vprintf(&buf, format, ap);
      61             :     // null terminate
      62     2073353 :     size_t ret = buf._offs;
      63     2073353 :     if (ret < size) {
      64             :         // if the string did fit then nul terminate
      65     2069505 :         str[ret] = '\0';
      66        3848 :     } else if (size > 0) {
      67             :         // if it didn't fit then terminate using passed in size
      68        1966 :         str[size-1] = 0;
      69             :     }
      70     2073353 :     return int(ret);
      71             : }
      72             : 
      73     9008914 : void AP_HAL::Util::set_soft_armed(const bool b)
      74             : {
      75     9008914 :     if (b != soft_armed) {
      76        2308 :         soft_armed = b;
      77        2308 :         last_armed_change_ms = AP_HAL::millis();
      78        2308 :         if (!was_watchdog_reset()) {
      79        2308 :             persistent_data.armed = b;
      80             :         }
      81             :     }
      82     9008914 : }
      83             : 
      84             : extern const AP_HAL::HAL& hal;
      85             : #include <stdio.h>
      86             : // stack overflow hook for low level RTOS code, C binding
      87           0 : void AP_stack_overflow(const char *thread_name)
      88             : {
      89             : #if HAL_REBOOT_ON_MEMORY_ERRORS
      90             :     (void)thread_name;
      91             :     hal.scheduler->reboot(false);
      92             : #else
      93             :     static bool done_stack_overflow;
      94           0 :     INTERNAL_ERROR(AP_InternalError::error_t::stack_overflow);
      95           0 :     if (!done_stack_overflow) {
      96             :         // we don't want to record the thread name more than once, as
      97             :         // first overflow can trigger a 2nd
      98           0 :         strncpy_noterm(hal.util->persistent_data.thread_name4, thread_name, 4);
      99           0 :         done_stack_overflow = true;
     100             :     }
     101           0 :     hal.util->persistent_data.fault_type = 42; // magic value
     102           0 :     if (!hal.util->get_soft_armed()) {
     103           0 :         AP_HAL::panic("stack overflow %s", thread_name);
     104             :     }
     105             : #endif
     106           0 : }
     107             : 
     108             : // hook for memory guard errors with --enable-memory-guard
     109           0 : void AP_memory_guard_error(uint32_t size)
     110             : {
     111             : #if HAL_REBOOT_ON_MEMORY_ERRORS
     112             :     (void)size;
     113             :     hal.scheduler->reboot(false);
     114             : #else
     115           0 :     INTERNAL_ERROR(AP_InternalError::error_t::mem_guard);
     116           0 :     if (!hal.util->get_soft_armed()) {
     117           0 :         ::printf("memory guard error size=%u\n", unsigned(size));
     118           0 :         AP_HAL::panic("memory guard size=%u", unsigned(size));
     119             :     }
     120             : #endif
     121           0 : }