Added heap-stack corruption protection; decreased stack usage.

notes.md

@@ -2,6 +2,7 @@
 ### TODO
 
 Next:
+ * [ ] Re-check all last-success timestamps - they don't survive the overflow.
  * [ ] Add FTM input
  * [ ] Rework docs.
  * [ ] Add outputs, geo objects and coord conversions to settings

src/debug_node.cpp

@@ -4,6 +4,7 @@
 
 #include "settings.h"
 #include "led_state.h"
+#include "platform.h"
 
 #include <Stream.h>
 #include <core_pins.h>
@@ -81,18 +82,20 @@ extern char _sbss;
 extern char _ebss; // end of static data; bottom of heap
 extern char _estack; // bottom of stack, top of ram: stack grows down towards heap
 
-// Top of heap from mk20dx128.c
+// Dynamic values from mk20dx128.c
 extern char *__brkval; // top of heap (dynamic ram): grows up towards stack
 
 void DebugNode::debug_print(Print &stream) {
  if (print_debug_memory_) {
  uint32_t static_data_size = &_ebss - (char*)((uint32_t)&_sdata & 0xFFFFF000);
  uint32_t allocated_heap = __brkval - &_ebss;
  char c, *top_stack = &c;
- int32_t heap_to_stack_distance = top_stack - __brkval;
- uint32_t stack_size = &_estack - top_stack;
+ int32_t unallocated = (&_estack - stack_size) - __brkval;
+ int32_t stack_max_used = get_stack_high_water_mark();
+ uint32_t stack_used = &_estack - top_stack;
+
  struct mallinfo m = mallinfo();
- stream.printf("RAM: static %d, heap %d (used %d, free %d), unalloc %d, stack %d\n", 
- static_data_size, allocated_heap, m.uordblks, m.fordblks, heap_to_stack_distance, stack_size);
+ stream.printf("RAM: static %d, heap %d (used %d, free %d), unalloc %d, stack %d (used %d, max %d)\n", 
+ static_data_size, allocated_heap, m.uordblks, m.fordblks, unallocated, stack_size, stack_used, stack_max_used);
  }
 }

src/mavlink.cpp

@@ -1,56 +1,23 @@
 #include "outputs.h"
-
-// This strange sequence of includes and definitions is due to how mavlink_helpers.h works :(
-// TODO: This currently includes code to sign packets with SHA256, bloating the binary. Get rid of it.
-#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
-#define MAVLINK_COMM_NUM_BUFFERS 4
-#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
-
-#include <mavlink_types.h>
+#include <assert.h>
+#include <common/mavlink.h>
+#include <Print.h>
+#include <avr_emulation.h>
 
 // Communication parameters of this system.
 mavlink_system_t mavlink_system = {
  .sysid = 155,
  .compid = 1,
 };
 
-void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *chars, unsigned length);
-
-#include <common/mavlink.h>
-
-// Insanity ends here. Back to normal.
-#include <assert.h>
-#include <Print.h>
-#include <avr_emulation.h>
-#include "primitives/vector.h"
-
-// Static list of streams we will output to.
-static Print *output_streams[MAVLINK_COMM_NUM_BUFFERS] = {};
-
-// Send multiple chars (uint8_t) over a comm channel
-void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *chars, unsigned length) {
- output_streams[chan]->write(chars, length);
-}
-
 MavlinkGeometryOutput::MavlinkGeometryOutput(Print &stream)
- : stream_idx_(0)
+ : stream_(stream)
+ , current_tx_seq_(0)
  , last_message_timestamp_()
  , last_pos_{0, 0, 0}
  , debug_print_state_(false)
  , debug_late_messages_(0) {
- bool inserted = false;
- for (int i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++)
- if (!output_streams[i]) {
- stream_idx_ = i;
- output_streams[i] = &stream;
- inserted = true;
- }
- if (!inserted)
- throw_printf("Too many MavlinkGeometryOutputs.");
-}
 
-MavlinkGeometryOutput::~MavlinkGeometryOutput() {
- output_streams[stream_idx_] = nullptr;
 }
 
 bool MavlinkGeometryOutput::position_valid(const ObjectGeometry& g) {
@@ -81,12 +48,64 @@ void MavlinkGeometryOutput::consume(const ObjectGeometry& g) {
  if (!position_valid(g))
  return;
 
+ mavlink_att_pos_mocap_t packet;
+
  // TODO: Use our time here. Someday.
  // Zero will be converted to the time of receive.
- uint64_t time_usec = 0;
- mavlink_channel_t chan = static_cast<mavlink_channel_t>(stream_idx_);
+ packet.time_usec = 0;
+ packet.x = g.xyz[0];
+ packet.y = g.xyz[1];
+ packet.z = g.xyz[2];
+ mav_array_memcpy(packet.q, g.q, sizeof(float)*4);
+ send_message(MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)&packet, 
+ MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, 
+ MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, 
+ MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC);
+}
 
- mavlink_msg_att_pos_mocap_send(chan, time_usec, g.q, g.xyz[0], g.xyz[1], g.xyz[2]);
+// Derived from _mav_finalize_message_chan_send
+// We reimplement it here to avoid depending on channel machinery, SHA256 signatures and too large stack usage.
+void MavlinkGeometryOutput::send_message(uint32_t msgid, const char *packet, uint8_t min_length, uint8_t length, uint8_t crc_extra)
+{
+ uint16_t checksum;
+ uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
+ uint8_t ck[2];
+ uint8_t header_len = MAVLINK_CORE_HEADER_LEN;
+ bool mavlink1 = false;
+
+ if (mavlink1) {
+ length = min_length;
+ assert(msgid <= 255); // can't send 16 bit messages
+ header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
+ buf[0] = MAVLINK_STX_MAVLINK1;
+ buf[1] = length;
+ buf[2] = current_tx_seq_++;
+ buf[3] = mavlink_system.sysid;
+ buf[4] = mavlink_system.compid;
+ buf[5] = msgid & 0xFF;
+ } else {
+ uint8_t incompat_flags = 0;
+ length = _mav_trim_payload(packet, length);
+ buf[0] = MAVLINK_STX;
+ buf[1] = length;
+ buf[2] = incompat_flags;
+ buf[3] = 0; // compat_flags
+ buf[4] = current_tx_seq_++;
+ buf[5] = mavlink_system.sysid;
+ buf[6] = mavlink_system.compid;
+ buf[7] = msgid & 0xFF;
+ buf[8] = (msgid >> 8) & 0xFF;
+ buf[9] = (msgid >> 16) & 0xFF;
+ }
+ checksum = crc_calculate((const uint8_t*)&buf[1], header_len);
+ crc_accumulate_buffer(&checksum, packet, length);
+ crc_accumulate(crc_extra, &checksum);
+ ck[0] = (uint8_t)(checksum & 0xFF);
+ ck[1] = (uint8_t)(checksum >> 8);
+
+ stream_.write((const char *)buf, header_len+1);
+ stream_.write(packet, length);
+ stream_.write((const char *)ck, 2);
 }
 
 bool MavlinkGeometryOutput::debug_cmd(HashedWord *input_words) {

src/message_logging.h

@@ -29,7 +29,7 @@ inline void print_value<SensorAnglesFrame>(Print &stream, const SensorAnglesFram
  if (sens.updated_cycles[phase] == val.cycle_idx + phase_delta)
  stream.printf("%c%.4f ", (phase == val.phase_id) ? '*' : ' ', sens.angles[phase]);
  else
- stream.printf("---- ");
+ stream.printf(" ------ ");
  }
  }
 }
@@ -80,24 +80,27 @@ class CountingProducerLogger : public PrintableProduceLogger<T> {
 
 template<typename T>
 class PrintingProducerLogger : public PrintableProduceLogger<T> {
+ constexpr static int kValuesToKeep = 16;
 public:
  PrintingProducerLogger(const char *name, uint32_t idx): name_(name), idx_(idx), counter_(0) {}
  virtual void log_produce(const T& val) {
+ if (log_.full()) 
+ log_.pop_front(); // Ensure we have space to write, we're less interested in old values.
  log_.enqueue(val);
  counter_++;
  }
  virtual void print_logs(Print &stream) {
  bool first = true;
  uint32_t has_idx = idx_ != (uint32_t)-1;
  stream.printf("%s%.*u: ", name_, has_idx, has_idx && idx_);
- T val;
- while (log_.dequeue(&val)) {
+ while (!log_.empty()) {
  if (first) {
  first = false;
  } else {
  stream.printf("| ");
  }
- print_value(stream, val);
+ print_value(stream, log_.front());
+ log_.pop_front();
  }
  if (!first) {
  stream.printf("(%d total)\n", counter_);
@@ -110,7 +113,7 @@ class PrintingProducerLogger : public PrintableProduceLogger<T> {
  const char *name_;
  uint32_t idx_;
  uint32_t counter_;
- CircularBuffer<T, 16> log_;
+ CircularBuffer<T, kValuesToKeep> log_;
 };
 
 

src/outputs.h

@@ -49,11 +49,12 @@ class MavlinkGeometryOutput
  virtual bool debug_cmd(HashedWord *input_words);
  virtual void debug_print(Print& stream);
 
- virtual ~MavlinkGeometryOutput();
 private:
  bool position_valid(const ObjectGeometry& g);
+ void send_message(uint32_t msgid, const char *packet, uint8_t min_length, uint8_t length, uint8_t crc_extra);
 
- uint32_t stream_idx_;
+ Print &stream_;
+ uint32_t current_tx_seq_;
  Timestamp last_message_timestamp_;
  float last_pos_[3];
  bool debug_print_state_;

src/platform.cpp

@@ -1,3 +1,4 @@
+#include "platform.h"
 #include <exception>
 #include <assert.h>
 #include <stdlib.h>
@@ -71,18 +72,63 @@ namespace __cxxabiv1 {
 }
 
 // TODO: We might want to re-define _exit() and __cxa_pure_virtual() as well to provide meaningful actions.
-// TODO: Define correct actions for hard failures like division by zero or invalid memory access.
+// TODO: Define correct actions for hard failures like division by zero or invalid memory access (see -fnon-call-exceptions)
 
-// ==== 2. Memory ================================
-
-// Low-leve _sbrk() syscall to allocate memory is defined in teensy3/mk20dx128.c and very simple - just moves 
-// __brkval, current top of the heap.
-// malloc() uses _sbrk().
-// Teensy defines basic "operator new()", but it's too simple for our case, so we use standard one instead.
+// ==== 2. Memory & Stack ================================
+// Low-level _sbrk() syscall used to allocate memory is defined in teensy3/mk20dx128.c and very simple - just moves 
+// __brkval, current top of the heap. We're rewriting it to add stack clashing check.
+// malloc() uses _sbrk(). operator new() uses malloc().
+// Teensy defines a basic "operator new()", but it's too simple for our case, so we use standard one instead.
 
 // Nice lib to get memory stats: https://github.com/michaeljball/RamMonitor
 // Or, use standard interface mallinfo(); (uordblks = bytes in use, fordblks = bytes free.)
 
+// Stack starts at the end of ram and grows down, towards the heap.
+// It's a bit hard to know how much stack is actually used. There are some static methods like the one in 
+// http:https://www.dlbeer.co.nz/oss/avstack.html, some compile-time warnings help (-Wstack-usage=256), plus we can
+// use dynamic methods like filling stack with a pattern.
+// NOTE: Stack overflow is not actually checked, but we're guaranteed that heap don't grow into stack of given size.
+// A proper solution would require setting up MMU correctly, which we don't want to do now.
+
+extern char *__brkval; // top of heap (dynamic ram): grows up towards stack
+extern char _estack; // bottom of stack, top of ram: stack grows down towards heap
+
+extern "C" void * _sbrk(int incr)
+{
+ // Check we're not overwriting the stack at the end of ram.
+ if (__brkval + incr > &_estack - stack_size) {
+ errno = ENOMEM;
+ return (void *)-1;
+ }
+ char *prev = __brkval;
+ __brkval += incr;
+ return prev;
+}
+
+// Check the high water mark of stack by filling the memory with a pattern and then checking
+// how much memory still has it.
+struct StackFillChecker {
+ const uint32_t pattern = 0xFAAFBABA;
+ StackFillChecker() {
+ char *frame_address = (char*)__builtin_frame_address(0);
+ for (uint32_t *p = (uint32_t*)(&_estack - stack_size); p < (uint32_t*)(frame_address - 64); p++)
+ *p = pattern;
+ }
+
+ // Returns max used stack in bytes.
+ int get_high_water_mark() {
+ char *frame_address = (char*)__builtin_frame_address(0);
+ for (uint32_t *p = (uint32_t*)(&_estack - stack_size); p < (uint32_t*)frame_address; p++)
+ if (*p != pattern)
+ return &_estack - (char *)p;
+ return &_estack - frame_address;
+ }
+};
+static StackFillChecker static_stack_fill_checker;
+
+int get_stack_high_water_mark() {
+ return static_stack_fill_checker.get_high_water_mark();
+}
 
 // ==== 3. Assertions ================================
 

src/platform.h

@@ -0,0 +1,10 @@
+#pragma once
+
+// Platform-specific constants
+
+// Max stack size.
+// NOTE: Stack overflow is not checked for now. We do check heap from growing into stack, though.
+constexpr int stack_size = 4096;
+
+// Returns max actually used stack.
+int get_stack_high_water_mark();

src/primitives/circular_buffer.h

@@ -1,6 +1,7 @@
 #pragma once
 
 // Circular buffer/queue of elements of type T, with capacity C.
+// NOTE: Both read and write indexes will freely overflow uint32_t and that's fine.
 template<typename T, unsigned int C>
 class CircularBuffer {
  static_assert(!(C & (C-1)), "Only power-of-two sizes of circular buffer are supported.");
@@ -24,6 +25,17 @@ class CircularBuffer {
  return C;
  }
 
+ inline const T& front() const {
+ return elems_[read_idx_ & (C-1)];
+ }
+
+ // Increment read counter.
+ inline void pop_front() {
+ if (!empty()) {
+ read_idx_++;
+ }
+ }
+
  // Example usage:
  // T cur_elem;
  // while (c.dequeue(&cur_elem)) {
@@ -33,7 +45,7 @@ class CircularBuffer {
  if (!empty()) {
  *result_elem = elems_[read_idx_ & (C-1)];
  asm volatile ("dmb 0xF":::"memory");
- read_idx_++; // NOTE: Index will freely overflow uint32_t and that's fine.
+ read_idx_++; 
  return true;
  } else {
  return false;
@@ -46,7 +58,7 @@ class CircularBuffer {
  if (!full()) {
  elems_[write_idx_ & (C-1)] = elem;
  asm volatile ("dmb 0xF":::"memory");
- write_idx_++; // NOTE: Index will freely overflow uint32_t and that's fine.
+ write_idx_++;
  return true;
  } else {
  // TODO: We might want to increment an overflow counter here.
@@ -55,6 +67,6 @@ class CircularBuffer {
  }
 
 private:
- unsigned long read_idx_, write_idx_;
+ volatile unsigned long read_idx_, write_idx_;
  T elems_[C];
 };

src/pulse_processor.cpp

@@ -134,8 +134,8 @@ void PulseProcessor::process_cycle_fix(Timestamp cur_time) {
  int cycle_phase = phase_classifier_.get_phase(cycle_idx_);
  if (cycle_phase >= 0) {
  // From (potentially several) short pulses for the same input, we choose the longest one.
- Pulse *short_pulses[num_inputs_] = {};
- TimeDelta short_pulse_timings[num_inputs_] = {};
+ Pulse *short_pulses[max_num_inputs] = {};
+ TimeDelta short_pulse_timings[max_num_inputs] = {};
  for (uint32_t i = 0; i < cycle_short_pulses_.size(); i++) {
  Pulse *p = &cycle_short_pulses_[i];
  uint32_t input_idx = p->input_idx;