Reversible primitives (#250)

* Initial work on io state inspection

This allows the debugger to inspect the current io state and load io states from snapshots. Currently only works with chip_digital_write.

* Obtain io state through portOutput/InputRegister

There currently is no support for analog values.

* Remove io map from Module struct

* Use NUM_DIGITAL_PINS because SOC_GPIO_PIN_COUNT is esp specific

* Serialize/deserialize pin mode in snapshot data

* Use digitalPinToBitMask in an attempt to make the code more microcontroller agnostic

* Port WARDuino to the Zephyr RTOS

The main reason for this is to support boards such as Open Bot Brain which uses
an stm32l496zg. By porting to Zephyr we can also add support for some other
boards with relative ease.

* Try correcting for drift in the servo motor driver

* Fixed cmake with branch rebase

* Use vfprintf instead of fprintf to handle variadic arguments

* Loading snapshots should work again now

* Hacky but it's starting to work

* Somewhat hacky way of extracting the current sensor value

* Put uart heartbeat for sensors in a thread + colour_sensor now returns sensor value

* Allow controlling multiple motors

* Added anti-stall system to motor driver

This makes it so if the motor tries to move but it's not actually moving the
voltage applied to the motor is increased until it moves and completes the
operation.

* Make motor driver reversible

* Added some things needed to make uart sensor work

* Added invoke_primitive function that makes it easy to call primitives with arguments

* New API for reversible primitives

Reversible primitives are now defined by using def_prim_reverse and def_prim_serialize.

* Rename PinState to IOStateElement and clean up debugger ioState interrupt

* Clang format

* Only run the reverse operation for the primitive that was just executed and not for all primitives

This particularly important for motor primitives that for example drive two motors at the same time vs a primitive that drives one motor at a time, executing the wrong reverse operation will result in the correct angles but the wrong position.

* Use ms to configure stall timeout for motors

* Adjust speed faster when stalling

Also added helper functions to reduce duplicated code between drive_motor_degrees and drive_motor_degrees_relative.

* Don't wait for the full 10ms if we have already detected movement

* Stripped out open bot brain primitives

* Clang format

* Make chip_digital_write on arduino reversible using the new macros

* Remove io-arduino.cpp and fix usage of PinState

* def_prim_serialize should not return anything for chip_digital_write

* Add extra clause to if so int32_t and int are bot considered I32 in create_stack template function

* Add new macros + restore_external_state + get_io_state to ESP-IDF

* Install primitive reverse for chip_digital_write

* Just use NUM_PRIMITIVES and set it to 5

* Move last remaining bits of io.h into primitives.h

* Add reversible primitive code to emulated.cpp

* Fixed zephyr build

Author: MaartenS11

platforms/Zephyr/boards/stm32l496g_disco.overlay

@@ -107,15 +107,20 @@
 
         pwms = 
             <&pwm8 3 10000 PWM_POLARITY_NORMAL>,
-            <&pwm8 4 10000 PWM_POLARITY_NORMAL>;
+            <&pwm8 4 10000 PWM_POLARITY_NORMAL>,
+            <&pwm8 1 10000 PWM_POLARITY_NORMAL>,
+            <&pwm8 2 10000 PWM_POLARITY_NORMAL>;
+            
+        warduino-uarts =
+            <&usart1>;
      };
 };
 
 &timers8 {
     status = "okay";
     pwm8: pwm {
         status = "okay";
-        pinctrl-0 = <&tim8_ch3_pc8 &tim8_ch4_pc9>;
+        pinctrl-0 = <&tim8_ch3_pc8 &tim8_ch4_pc9 &tim8_ch1_pc6 &tim8_ch2_pc7>;
         pinctrl-names =
             "default";
     };

src/Debug/debugger.cpp

@@ -10,6 +10,7 @@
 #endif
 
 #include "../Memory/mem.h"
+#include "../Primitives/primitives.h"
 #include "../Utils//util.h"
 #include "../Utils/macros.h"
 #include "../WARDuino/CallbackHandler.h"
@@ -714,11 +715,18 @@ bool Debugger::handlePushedEvent(char *bytes) const {
 }
 
 void Debugger::snapshot(Module *m) {
-    uint16_t numberBytes = 10;
-    uint8_t state[] = {
-        pcState,        breakpointsState, callstackState,      globalsState,
-        tableState,     memoryState,      branchingTableState, stackState,
-        callbacksState, eventsState};
+    uint16_t numberBytes = 11;
+    uint8_t state[] = {pcState,
+                       breakpointsState,
+                       callstackState,
+                       globalsState,
+                       tableState,
+                       memoryState,
+                       branchingTableState,
+                       stackState,
+                       callbacksState,
+                       eventsState,
+                       ioState};
     inspect(m, numberBytes, state);
 }
 
@@ -843,6 +851,26 @@ void Debugger::inspect(Module *m, uint16_t sizeStateArray, uint8_t *state) {
                 addComma = true;
                 break;
             }
+            case ioState: {
+                this->channel->write("%s", addComma ? "," : "");
+                this->channel->write("\"io\": [");
+                bool comma = false;
+                std::vector<IOStateElement *> external_state = get_io_state(m);
+                for (auto state_elem : external_state) {
+                    this->channel->write("%s{", comma ? ", " : "");
+                    this->channel->write(
+                        R"("key": "%s", "output": %s, "value": %d)",
+                        state_elem->key.c_str(),
+                        state_elem->output ? "true" : "false",
+                        state_elem->value);
+                    this->channel->write("}");
+                    comma = true;
+                    delete state_elem;
+                }
+                this->channel->write("]");
+                addComma = true;
+                break;
+            }
             default: {
                 debug("dumpExecutionState: Received unknown state request\n");
                 break;
@@ -953,7 +981,8 @@ bool Debugger::saveState(Module *m, uint8_t *interruptData) {
     uint8_t *program_state = nullptr;
     uint8_t *end_state = nullptr;
     program_state = interruptData + 1;  // skip interruptLoadSnapshot
-    end_state = program_state + read_B32(&program_state);
+    uint32_t len = read_B32(&program_state);
+    end_state = program_state + len;
 
     debug("saving program_state\n");
     while (program_state < end_state) {
@@ -1169,6 +1198,29 @@ bool Debugger::saveState(Module *m, uint8_t *interruptData) {
                 }
                 break;
             }
+            case ioState: {
+                debug("receiving ioState\n");
+                uint8_t io_state_count = *program_state++;
+                std::vector<IOStateElement> external_state;
+                external_state.reserve(io_state_count);
+                for (int i = 0; i < io_state_count; i++) {
+                    IOStateElement state_elem;
+                    state_elem.key = "";
+                    char c = (char)*program_state++;
+                    while (c != '\0') {
+                        state_elem.key += c;
+                        c = (char)*program_state++;
+                    }
+                    state_elem.output = *program_state++;
+                    state_elem.value = (int)read_B32(&program_state);
+                    external_state.emplace_back(state_elem);
+                    debug("pin %s(%s) = %d\n", state_elem.key.c_str(),
+                          state_elem.output ? "output" : "input",
+                          state_elem.value);
+                }
+                restore_external_state(m, external_state);
+                break;
+            }
             default: {
                 FATAL("saveState: Received unknown program state\n");
             }

src/Debug/debugger.h

@@ -47,7 +47,8 @@ enum ExecutionState {
     branchingTableState = 0x07,
     stackState = 0x08,
     callbacksState = 0x09,
-    eventsState = 0x0A
+    eventsState = 0x0A,
+    ioState = 0x0B,
 };
 
 enum InterruptTypes {

src/Primitives/arduino.cpp

@@ -150,15 +150,32 @@ int prim_index = 0;
             PrimitiveEntry *p = &primitives[prim_index++];                 \
             p->name = #prim_name;                                          \
             p->f = &(prim_name);                                           \
+            p->f_reverse = nullptr;                                        \
+            p->f_serialize_state = nullptr;                                \
         } else {                                                           \
             FATAL("pim_index out of bounds");                              \
         }                                                                  \
     }
 
+#define install_primitive_reverse(prim_name)             \
+    {                                                    \
+        PrimitiveEntry *p = &primitives[prim_index - 1]; \
+        p->f_reverse = &(prim_name##_reverse);           \
+        p->f_serialize_state = &(prim_name##_serialize); \
+    }
+
 #define def_prim(function_name, type) \
     Type function_name##_type = type; \
     bool function_name(Module *m)
 
+#define def_prim_reverse(function_name)     \
+    void function_name##_reverse(Module *m, \
+                                 std::vector<IOStateElement> external_state)
+
+#define def_prim_serialize(function_name) \
+    void function_name##_serialize(       \
+        std::vector<IOStateElement *> &external_state)
+
 // TODO: use fp
 #define pop_args(n) m->sp -= n
 #define get_arg(m, arg) m->stack[(m)->sp - (arg)].value
@@ -491,6 +508,36 @@ def_prim(chip_digital_write, twoToNoneU32) {
     return true;
 }
 
+def_prim_reverse(chip_digital_write) {
+    for (IOStateElement state : external_state) {
+        if (!state.output) {
+            continue;
+        }
+
+        if (state.key[0] == 'p') {
+            invoke_primitive(m, "chip_digital_write", stoi(state.key.substr(1)),
+                             (uint32_t)state.value);
+        }
+    }
+}
+
+def_prim_serialize(chip_digital_write) {
+    for (int i = 0; i < NUM_DIGITAL_PINS; i++) {
+        uint32_t bit_mask = digitalPinToBitMask(i);
+        auto *state = new IOStateElement();
+        state->key = "p" + std::to_string(i);
+        uint8_t port = digitalPinToPort(i);
+        if (*portModeRegister(port) & bit_mask) {  // DDR
+            state->output = true;
+            state->value = (*portOutputRegister(port) & bit_mask) > 0;
+        } else {
+            state->output = false;
+            state->value = (*portInputRegister(port) & bit_mask) > 0;
+        }
+        external_state.push_back(state);
+    }
+}
+
 def_prim(chip_delay, oneToNoneU32) {
     delay(arg0.uint32);
     pop_args(1);
@@ -954,6 +1001,7 @@ void install_primitives() {
 
     install_primitive(chip_pin_mode);
     install_primitive(chip_digital_write);
+    install_primitive_reverse(chip_digital_write);
     install_primitive(chip_delay);
     install_primitive(chip_digital_read);
     install_primitive(chip_analog_read);
@@ -1026,3 +1074,44 @@ bool resolve_external_memory(char *symbol, Memory **val) {
     FATAL("Could not find memory %s \n", symbol);
     return false;
 }
+
+//------------------------------------------------------
+// Restore external state when restoring a snapshot
+//------------------------------------------------------
+void restore_external_state(Module *m,
+                            std::vector<IOStateElement> external_state) {
+    uint8_t opcode = *m->pc_ptr;
+    // TODO: Maybe primitives can also be called using the other call
+    // instructions such as call_indirect
+    //  maybe there should just be a function that checks if a certain function
+    //  is being called that handles all these cases?
+    if (opcode == 0x10) {  // call opcode
+        uint8_t *pc_copy = m->pc_ptr + 1;
+        uint32_t fidx = read_LEB_32(&pc_copy);
+        if (fidx < m->import_count) {
+            for (auto &primitive : primitives) {
+                if (!strcmp(primitive.name, m->functions[fidx].import_field)) {
+                    if (primitive.f_reverse) {
+                        debug("Reversing action for primitive %s\n",
+                              primitive.name);
+                        primitive.f_reverse(m, external_state);
+                    }
+                    return;
+                }
+            }
+        }
+    }
+}
+
+//------------------------------------------------------
+// Serialize external state into a snapshot
+//------------------------------------------------------
+std::vector<IOStateElement *> get_io_state(Module *m) {
+    std::vector<IOStateElement *> ioState;
+    for (auto &primitive : primitives) {
+        if (primitive.f_serialize_state) {
+            primitive.f_serialize_state(ioState);
+        }
+    }
+    return ioState;
+}

src/Primitives/emulated.cpp

@@ -47,15 +47,32 @@ double sensor_emu = 0;
             PrimitiveEntry *p = &primitives[prim_index++];                 \
             p->name = #prim_name;                                          \
             p->f = &(prim_name);                                           \
+            p->f_reverse = nullptr;                                        \
+            p->f_serialize_state = nullptr;                                \
         } else {                                                           \
             FATAL("pim_index out of bounds");                              \
         }                                                                  \
     }
 
+#define install_primitive_reverse(prim_name)             \
+    {                                                    \
+        PrimitiveEntry *p = &primitives[prim_index - 1]; \
+        p->f_reverse = &(prim_name##_reverse);           \
+        p->f_serialize_state = &(prim_name##_serialize); \
+    }
+
 #define def_prim(function_name, type) \
     Type function_name##_type = type; \
     bool function_name(Module *m)
 
+#define def_prim_reverse(function_name)     \
+    void function_name##_reverse(Module *m, \
+                                 std::vector<IOStateElement> external_state)
+
+#define def_prim_serialize(function_name) \
+    void function_name##_serialize(       \
+        std::vector<IOStateElement *> &external_state)
+
 // TODO: use fp
 #define pop_args(n) m->sp -= n
 #define get_arg(m, arg) m->stack[(m)->sp - (arg)].value
@@ -572,4 +589,42 @@ bool resolve_external_memory(char *symbol, Memory **val) {
     return false;
 }
 
+//------------------------------------------------------
+// Restore external state when restoring a snapshot
+//------------------------------------------------------
+void restore_external_state(Module *m,
+                            std::vector<IOStateElement> external_state) {
+    uint8_t opcode = *m->pc_ptr;
+    // TODO: Maybe primitives can also be called using the other call
+    // instructions such as call_indirect
+    //  maybe there should just be a function that checks if a certain function
+    //  is being called that handles all these cases?
+    if (opcode == 0x10) {  // call opcode
+        uint8_t *pc_copy = m->pc_ptr + 1;
+        uint32_t fidx = read_LEB_32(&pc_copy);
+        if (fidx < m->import_count) {
+            for (auto &primitive : primitives) {
+                if (!strcmp(primitive.name, m->functions[fidx].import_field)) {
+                    if (primitive.f_reverse) {
+                        debug("Reversing action for primitive %s\n",
+                              primitive.name);
+                        primitive.f_reverse(m, external_state);
+                    }
+                    return;
+                }
+            }
+        }
+    }
+}
+
+std::vector<IOStateElement *> get_io_state(Module *m) {
+    std::vector<IOStateElement *> ioState;
+    for (auto &primitive : primitives) {
+        if (primitive.f_serialize_state) {
+            primitive.f_serialize_state(ioState);
+        }
+    }
+    return ioState;
+}
+
 #endif  // ARDUINO