New state machine, local UDP route

include/configurations/EthernetConfiguration.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <stdint.h>
+
+struct EthernetConfiguration {
+
+    // The MAC address of the ethernet connection
+    uint8_t macAddress[6];
+
+    // The master-in slave-out pin of the SPI connection for the Ethernet module
+    int8_t spiPinMiso;
+
+    // The master-out slave-in pin of the SPI connection for the Ethernet module
+    int8_t spiPinMosi;
+
+    // The slave clock pin of the SPI connection for the Ethernet module
+    int8_t spiPinSclk;
+
+    // The slave-select pin of the SPI connection for the Ethernet module
+    int8_t spiPinSS;
+
+    unsigned long dhcpLeaseTimeoutMs;
+    unsigned long dhcpLeaseResponseTimeoutMs;
+
+    // The static IP address to assign if DHCP fails
+    uint8_t manualIp[4];
+
+    // The IP address of the DNS server, if DHCP fails
+    uint8_t manualDnsAddress[4];
+
+    // The port for the incoming UDP connection
+    uint16_t udpPort;
+};

include/configurations/KeyConfiguration.h

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <stdint.h>
+
+struct KeyConfiguration {
+
+    const uint8_t* remoteKey;
+
+    const uint8_t* localKey;
+
+    uint32_t challengeExpiryMs;
+};

include/controller.h

@@ -3,62 +3,59 @@
 #include "server.h"
 #include "servo.h"
 #include "message.h"
-#include <ESPAsyncWebServer.h>
+#include "configurations/EthernetConfiguration.h"
+#include "configurations/KeyConfiguration.h"
 
-struct EthernetConfiguration {
+enum class SesameDeviceStatus {
 
-    // The MAC address of the ethernet connection
-    uint8_t macAddress[6];
+    /**
+     * @brief The initial state of the device after boot
+     */
+    initial,
 
-    // The master-in slave-out pin of the SPI connection for the Ethernet module
-    int8_t spiPinMiso;
+    /**
+     * @brief The device has configured the individual parts,
+     * but has no the ethernet hardware detected.
+     */
+    configuredButNoEthernetHardware,
 
-    // The master-out slave-in pin of the SPI connection for the Ethernet module
-    int8_t spiPinMosi;
+    /**
+     * @brief The device has ethernet hardware, but no ethernet link
+     */
+    ethernetHardwareButNoLink,
 
-    // The slave clock pin of the SPI connection for the Ethernet module
-    int8_t spiPinSclk;
+    /**
+     * @brief The device has an ethernet link, but no IP address.
+     */
+    ethernetLinkButNoIP,
 
-    // The slave-select pin of the SPI connection for the Ethernet module
-    int8_t spiPinSS;
-
-    unsigned long dhcpLeaseTimeoutMs;
-    unsigned long dhcpLeaseResponseTimeoutMs;
-
-    // The static IP address to assign if DHCP fails
-    uint8_t manualIp[4];
-
-    // The IP address of the DNS server, if DHCP fails
-    uint8_t manualDnsAddress[4];
-
-};
-
-struct KeyConfiguration {
-
-    const uint8_t* remoteKey;
-
-    const uint8_t* localKey;
-
-    uint32_t challengeExpiryMs;
+    /**
+     * @brief The device has an IP address, but no socket connection
+     */
+    ipAddressButNoSocketConnection,
 };
 
 class SesameController: public ServerConnectionCallbacks {
 
 public:
-    SesameController(uint16_t localWebServerPort);
-
-    void configure(ServoConfiguration servoConfig, ServerConfiguration serverConfig, EthernetConfiguration ethernetConfig, KeyConfiguration keyConfig);
+    SesameController(ServoConfiguration servoConfig, ServerConfiguration serverConfig, EthernetConfiguration ethernetConfig, KeyConfiguration keyConfig);
 
     void loop(uint32_t millis);
 
 private:
 
+    SesameDeviceStatus status = SesameDeviceStatus::initial;
+
     uint32_t currentTime = 0;
 
     ServerConnection server;
     ServoController servo;
-    AsyncWebServer localWebServer;
 
+    // UDP
+
+    // An EthernetUDP instance to send and receive packets over UDP
+    EthernetUDP udp;
+    EthernetHardwareStatus ethernetStatus;
     EthernetConfiguration ethernetConfig;
     bool ethernetIsConfigured = false;
 
@@ -75,6 +72,31 @@ private:
 
     SignedMessage outgoingMessage;
 
+    // MARK: Ethernet
+
+    void initializeSpiBusForEthernetModule();
+
+    /**
+     * @brief Checks to ensure that Ethernet hardware is available
+     * 
+     * @return true The hardware is available
+     * @return false The hardware is missing
+     */
+    bool hasAvailableEthernetHardware();
+
+    /**
+     * @brief Check that an active ethernet link is available
+     * 
+     * @return true Link is available
+     * @return false Link is absent
+     */
+    bool hasEthernetLink();
+
+    void configureEthernet();
+
+    void startUDP();
+    void stopUDP();
+    
     bool hasCurrentChallenge() {
         return currentChallengeExpiry > currentTime;
     }
@@ -87,7 +109,7 @@ private:
 
     // MARK: Local client callbacks
 
-    void handleLocalMessage(AsyncWebServerRequest *request);
+    void checkLocalMessage();
     
     // MARK: Socket Callbacks
 
@@ -111,7 +133,7 @@ private:
      * 
      * Note: Prepares the response in the outgoing message buffer.
      */
-    void processMessage(SignedMessage* message);
+    void processMessage(SignedMessage* message, bool shouldPerformUnlock);
 
     /**
      * @brief Checks that the message is valid and prepares a challenge.
@@ -140,7 +162,7 @@ private:
      * 
      * Note: Prepares the response in the outgoing message buffer.
      */
-    void completeUnlockRequest(Message* message);
+    void completeUnlockRequest(Message* message, bool shouldPerformUnlock);
 
     // MARK: Responses
 
@@ -152,12 +174,18 @@ private:
      */
     void prepareResponseBuffer(MessageResult event, Message* message = NULL);
 
+    /**
+     * @brief Read a message from the UDP port
+     * 
+     */
+    bool readLocalMessage();
+
     /**
      * @brief Send the prepared outgoing message to a locally connected client
      * 
      * @param request The original request of the client
      */
-    void sendPreparedLocalResponse(AsyncWebServerRequest *request);
+    void sendPreparedLocalResponse();
 
     /**
      * @brief Send the prepared outgoing message to the server
@@ -166,5 +194,5 @@ private:
 
     // MARK: Helper
 
-     bool convertHexMessageToBinary(const char* str);
+    bool convertHexMessageToBinary(const char* str);
 };

include/interface/ESP32CryptoSource.h

@@ -0,0 +1,62 @@
+#pragma once
+
+#include "relay/interface/CryptoSource.h"
+#include "ESP32NoiseSource.h"
+
+class ESP32CryptoSource: public CryptoSource {
+
+public:
+
+    ESP32CryptoSource(const char* rngInitTag);
+
+    bool isAvailable() override {
+        return true;
+    }
+
+    /**
+     * @brief Create a new random private key
+     * 
+     * @param key The output buffer where the key will be stored
+     * @return true The key was created
+     * @return false The key could not be created
+     */
+    bool createPrivateKey(PrivateKey* key) override;
+
+    /**
+     * @brief Create a the public key corresponding to a private key
+     * 
+     * @param privateKey The private key to use
+     * @param publicKey The output buffer where the public key will be stored
+     * @return true The key was created
+     * @return false The key could not be created
+     */
+    bool createPublicKey(const PrivateKey* privateKey, PublicKey* publicKey) override;
+
+    /**
+     * @brief Sign a message
+     * 
+     * @param message The message payload to include in the message
+     * @param length The length of the payload
+     * @param signature The output buffer where the signature is written
+     * @return true The signature was created
+     * @return false The signature creation failed
+     */
+    bool sign(const uint8_t *message, uint16_t length, Signature* signature, const PrivateKey* privateKey, const PublicKey* publicKey) override;
+
+    /**
+     * @brief Verify a message
+     *
+     * @param signature The message signature
+     * @param publicKey The public key with which the message was signed
+     * @param message The pointer to the message data
+     * @param length The length of the message
+     * @return true The signature is valid
+     * @return false The signature is invalid
+     */
+    bool
+    verify(const Signature* signature, const PublicKey* publicKey, const void *message, uint16_t length) override;
+
+private:
+
+    ESP32NoiseSource noise{};
+};

include/interface/ESP32NoiseSource.h

@@ -0,0 +1,64 @@
+#pragma once
+
+#include <NoiseSource.h>
+
+/// @brief The size of the internal buffer when generating entropy
+constexpr size_t randomNumberBatchSize = 32;
+
+/**
+ * @brief A noise source for crypto operations specifically for the ESP32.
+ * 
+ */
+class ESP32NoiseSource: public NoiseSource {
+
+public:
+    /**
+     * \brief Constructs a new random noise source.
+     */
+    ESP32NoiseSource();
+
+    /**
+     * \brief Destroys this random noise source.
+     */
+    ~ESP32NoiseSource() {}
+
+    /**
+     * @brief Determine if the noise source is still calibrating itself.
+     * 
+     * Noise sources that require calibration start doing so at system startup
+     * and then switch over to random data generation once calibration is complete.
+     * Since no random data is being generated during calibration, the output
+     * from `RNGClass::rand()` and `RNG.rand()` may be predictable.
+     * Use `RNGClass::available()` or `RNG.available()` to determine
+     * when sufficient entropy is available to generate good random values.
+     *
+     * It is possible that the noise source never exits calibration.  This can
+     * happen if the input voltage is insufficient to trigger noise or if the
+     * noise source is not connected.  Noise sources may also periodically
+     * recalibrate themselves.
+     *
+     * @return Returns true if calibration is in progress; false if the noise
+     * source is generating valid random data.
+     * 
+     */
+    bool calibrating() const override;
+
+    /**
+     * @brief Stirs entropy from this noise source into the global random
+     * number pool.
+     *
+     * This function should call `output()` to add the entropy from this noise
+     * source to the global random number pool.
+     *
+     * The noise source should batch up the entropy data, providing between
+     * 16 and 48 bytes of data each time.  If the noise source does not have
+     * sufficient entropy data at the moment, it should return without stiring
+     * the current data in.
+     */
+    void stir() override;
+
+private:
+
+    /// @brief Temporary buffer to hold random bytes before handing them to the crypto module
+    uint8_t data[randomNumberBatchSize];
+};

include/interface/ESP32StorageSource.h

@@ -0,0 +1,17 @@
+
+#include "relay/interface/StorageSource.h"
+
+class ESP32StorageSource: public StorageSource {
+
+    bool writeByteAtIndex(uint8_t byte, uint16_t index) override;
+
+    bool canProvideStorageWithSize(uint16_t size) override;
+
+    bool commitData();
+
+    uint8_t readByteAtIndex(uint16_t index) override;
+
+    uint16_t readBytes(uint16_t startIndex, uint16_t count, uint8_t* output) override;
+
+    uint16_t writeBytes(uint8_t* bytes, uint16_t count, uint16_t startIndex) override;
+};

include/message.h

@@ -14,7 +14,7 @@
 
 #pragma pack(push, 1)
 
-typedef enum {
+enum class MessageType: uint8_t {
 
     /// @brief The initial message from remote to device to request a challenge.
     initial = 0,
@@ -28,7 +28,7 @@ typedef enum {
     /// @brief The final message with the unlock result from the device to the remote
     response = 3,
 
-} MessageType;
+};
 
 enum class MessageResult: uint8_t {
 
@@ -36,31 +36,31 @@ enum class MessageResult: uint8_t {
     MessageAccepted = 0,
     
     /// @brief The web socket received text while waiting for binary data.
-    TextReceived = 1,
+    TextReceivedOverSocket = 1,
 
     /// @brief An unexpected socket event occured while performing the exchange.
     UnexpectedSocketEvent = 2,
 
     /// @brief The received message size is invalid.
-    InvalidMessageSize = 3,
+    InvalidMessageSizeFromRemote = 3,
 
     /// @brief The message signature was incorrect.
-    MessageAuthenticationFailed = 4,
+    InvalidSignatureFromRemote = 4,
 
     /// @brief The server challenge of the message did not match previous messages
-    ServerChallengeMismatch = 5,
+    InvalidServerChallengeFromRemote = 5,
 
     /// @brief The client challenge of the message did not match previous messages
-    ClientChallengeInvalid = 6,
+    InvalidClientChallengeFromRemote = 6,
 
     /// @brief An unexpected or unsupported message type was received
-    InvalidMessageType = 7,
+    InvalidMessageTypeFromRemote = 7,
 
     /// @brief A message is already being processed
     TooManyRequests = 8,
 
     /// @brief The received message result was invalid
-    InvalidMessageResult = 9,
+    InvalidMessageResultFromRemote = 9,
 
     /// @brief An invalid Url parameter was set sending a message to the device over a local connection
     InvalidUrlParameter = 10,

include/relay/CryptoPrimitives.h

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <stdint.h>
+
+#pragma pack(push, 1)
+
+/**
+ * @brief A private key for asymmetric cryptography
+ */
+struct PrivateKey {
+
+    /// @brief The size of a private key
+    static constexpr int size = 32;
+
+    uint8_t bytes[size];
+
+    bool isUnset() {
+        for (uint8_t i = 0; i < size; i += 1) {
+            if (bytes[i] != 0) {
+                return false;
+            }
+        }
+        return true;
+    }
+};
+
+/**
+ * @brief A public key for asymmetric cryptography
+ */
+struct PublicKey {
+
+    /// @brief The size of a public key
+    static constexpr int size = 32;
+
+    uint8_t bytes[size];
+
+    bool isUnset() {
+        for (uint8_t i = 0; i < size; i += 1) {
+            if (bytes[i] != 0) {
+                return false;
+            }
+        }
+        return true;
+    }
+};
+
+/**
+ * @brief A signature of some data using a private key
+ */
+struct Signature {
+
+    /// @brief The size of a message signature
+    static constexpr int size = 64;
+
+    uint8_t bytes[size];
+
+};
+
+#pragma pack(pop)

include/relay/interface/CryptoSource.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include <stdint.h>
+#include "relay/CryptoPrimitives.h"
+
+/**
+ * @brief An abstract definition of an instance capable of crypto operations
+ * 
+ */
+class CryptoSource {
+
+public:
+
+    /**
+     * @brief Indicate that the crypto functions can be used.
+     * 
+     * @return true The crypto functions are available
+     * @return false Some error prevents the use of the crypto functions.
+     */
+    virtual
+    bool isAvailable() = 0;
+
+    /**
+     * @brief Create a new random private key
+     * 
+     * @param key The output buffer where the key will be stored
+     * @return true The key was created
+     * @return false The key could not be created
+     */
+    virtual
+    bool createPrivateKey(PrivateKey* key) = 0;
+
+    /**
+     * @brief Create a the public key corresponding to a private key
+     * 
+     * @param privateKey The private key to use
+     * @param publicKey The output buffer where the public key will be stored
+     * @return true The key was created
+     * @return false The key could not be created
+     */
+    virtual
+    bool createPublicKey(const PrivateKey* privateKey, PublicKey* publicKey) = 0;
+
+    /**
+     * @brief Sign a message
+     * 
+     * @param message The message payload to include in the message
+     * @param length The length of the payload
+     * @param signature The output buffer where the signature is written
+     * @return true The signature was created
+     * @return false The signature creation failed
+     */
+    virtual
+    bool sign(const uint8_t *message, uint16_t length, Signature* signature, const PrivateKey* privateKey, const PublicKey* publicKey) = 0;
+
+    /**
+     * @brief Verify a message
+     * 
+     * @param signature The message signature
+     * @param publicKey The public key with which the message was signed
+     * @param message The pointer to the message data
+     * @param length The length of the message
+     * @return true The signature is valid
+     * @return false The signature is invalid
+     */
+    virtual
+    bool verify(const Signature* signature, const PublicKey* publicKey, const void *message, uint16_t length) = 0;
+};

include/relay/interface/StorageSource.h

@@ -0,0 +1,98 @@
+#pragma once
+
+#include <stdint.h>
+
+/**
+ * @brief An abstract interface for persistent storage
+ * 
+ * @note It is assumed that read operations are cached and therefore quick.
+ */
+class StorageSource {
+
+public:
+
+    /**
+     * @brief Write a byte to disk
+     * 
+     * @note The data is only persisted if the function `commitData()` is called afterwards.
+     * 
+     * @param byte The byte to write
+     * @param index The index where to write the byte
+     * @return true 
+     * @return false 
+     */
+    virtual
+    bool writeByteAtIndex(uint8_t byte, uint16_t index) = 0;
+
+    /**
+     * @brief Ensure that enough storage is available for all data
+     * 
+     * @param size 
+     * @return true The space was initialized an has sufficient size
+     * @return false 
+     */
+    virtual
+    bool canProvideStorageWithSize(uint16_t size) = 0;
+
+    /**
+     * @brief Write the data to persistent storage after a block of bytes was changed.
+     * 
+     * @return true The data was persisted
+     * @return false The data could not be saved
+     */
+    virtual
+    bool commitData() = 0;
+
+    /**
+     * @brief Read a single byte from persistent storage
+     * 
+     * @param index The index of the byte in the data
+     * @return uint8_t The byte at the given index
+     */
+    virtual
+    uint8_t readByteAtIndex(uint16_t index) = 0;
+
+    /**
+     * @brief Read a number of bytes from storage
+     * 
+     * @param startIndex The index of the start byte
+     * @param count The number of bytes to read
+     * @param output The location to write the bytes
+     * @return uint8_t The number of bytes read
+     */
+    virtual
+    uint16_t readBytes(uint16_t startIndex, uint16_t count, uint8_t* output) {
+        uint16_t endIndex = startIndex + count;
+        if (endIndex < startIndex) {
+            return 0; // Overflow
+        }
+        for (uint16_t i = 0; i < endIndex; i += 1) {
+            output[i] = readByteAtIndex(startIndex + i);
+        }
+        return count;
+    }
+
+    /**
+     * @brief Write a number of bytes to storage
+     * 
+     * @note The data is only persisted if the function `commitData()` is called afterwards.
+     * 
+     * @param bytes The memory holding the bytes
+     * @param count The number of bytes to write
+     * @param startIndex The index where the bytes should be written
+     * @return uint16_t The number of bytes written to storage
+     */
+    virtual
+    uint16_t writeBytes(uint8_t* bytes, uint16_t count, uint16_t startIndex) {
+        uint16_t endIndex = startIndex + count;
+        if (endIndex < startIndex) {
+            return 0; // Overflow
+        }
+        for (uint16_t i = 0; i < endIndex; i += 1) {
+            if (!writeByteAtIndex(bytes[i], startIndex + i)) {
+                return i; // Failed to write byte
+            }
+        }
+        return count;
+    }
+};

include/server.h

@@ -46,14 +46,16 @@ class ServerConnection {
 
 public:
 
-    ServerConnection();
+    ServerConnection(ServerConfiguration configuration);
 
     /**
      * @brief Set the configuration and the callback handler
      * 
      * @param callback The handler to handle messages and errors
      */
-    void configure(ServerConfiguration configuration, ServerConnectionCallbacks* callbacks);
+    void setCallbacks(ServerConnectionCallbacks* callbacks);
+
+    void shouldConnect(bool connect);
 
     /**
      * @brief Call this function regularly to handle socket operations.
@@ -73,7 +75,8 @@ public:
 
 private:
 
-    uint32_t currentTime;
+    uint32_t currentTime = 0;
+    bool shouldBeConnected = false;
 
     bool socketIsConnected() {
         return webSocket.isConnected();
@@ -81,15 +84,12 @@ private:
 
     void connect();
 
-    void disconnect();
-
-    bool shouldReconnect = true;
     bool isConnecting = false;
+    bool isDisconnecting = false;
     uint32_t connectionTimeout = 0;
     uint32_t nextReconnectAttemptMs = 0;
 
-    void didDisconnect();
-    void didConnect();
+    void didChangeConnectionState(bool isConnected);
 
 
     ServerConfiguration configuration;

include/servo.h

@@ -52,20 +52,15 @@ public:
 
     /**
      * @brief Construct a new servo controller
-     */
-    ServoController();
-
-    /**
-     * @brief Configure the servo
      * 
      * @param The configuration for the servo
      */
-    void configure(ServoConfiguration configuration);
+    ServoController(ServoConfiguration configuration);
 
     /**
      * @brief Update the servo state periodically
      * 
-     * This function should be periodically called to update the servo state,
+     * This function will be periodically called to update the servo state,
      * specifically to release the button after the opening time has elapsed.
      * 
      * There is no required interval to call this function, but the accuracy of
@@ -78,16 +73,14 @@ public:
      */
     void pressButton();
 
-    /**
-     * Release the door opener button by moving the servo arm.
-     */
-    void releaseButton();
-
 private:
 
     // Indicator that the door button is pushed
     bool buttonIsPressed = false;
 
+    // Indicate that the button should be pressed
+    bool shouldPressButton = false;
+
     uint32_t openDuration = 0;
 
     int pressedValue = 0;
@@ -103,4 +96,12 @@ private:
     // PWM Module needed for the servo
     ESP32PWM pwm;
 
+    // The task on core 1 that resets the servo
+    TaskHandle_t servoResetTask;
+
+    /**
+     * Release the door opener button by moving the servo arm.
+     */
+    void releaseButton();
+
 };

platformio.ini

@@ -13,11 +13,9 @@ platform = espressif32
 board = az-delivery-devkit-v4
 framework = arduino
 lib_deps = 
-	; links2004/WebSockets@^2.4.0
 	madhephaestus/ESP32Servo@^1.1.0
-	ottowinter/ESPAsyncWebServer-esphome@^3.0.0
 	arduino-libraries/Ethernet@^2.0.2
 	https://github.com/christophhagen/arduinoWebSockets#master
-	
+	rweather/Crypto@^0.4.0
 monitor_speed = 115200
 build_flags= -D WEBSOCKETS_NETWORK_TYPE=NETWORK_W5100

src/controller.cpp

@@ -5,88 +5,172 @@
 #include <SPI.h>
 #include <Ethernet.h>
 
-SesameController::SesameController(uint16_t localWebServerPort) : localWebServer(localWebServerPort) {
+SesameController::SesameController(ServoConfiguration servoConfig, ServerConfiguration serverConfig, EthernetConfiguration ethernetConfig, KeyConfiguration keyConfig) 
+    : ethernetConfig(ethernetConfig), keyConfig(keyConfig), servo(servoConfig), server(serverConfig) {
     
 }
 
-void SesameController::configure(ServoConfiguration servoConfig, ServerConfiguration serverConfig, EthernetConfiguration ethernetConfig, KeyConfiguration keyConfig) {
-    this->ethernetConfig = ethernetConfig;
-    this->keyConfig = keyConfig;
-
-    // Ensure source of random numbers without WiFi and Bluetooth
-    enableCrypto();
-
-    // Initialize SPI interface to Ethernet module
+void SesameController::initializeSpiBusForEthernetModule() {
     SPI.begin(ethernetConfig.spiPinSclk, ethernetConfig.spiPinMiso, ethernetConfig.spiPinMosi, ethernetConfig.spiPinSS); //SCLK, MISO, MOSI, SS
     pinMode(ethernetConfig.spiPinSS, OUTPUT);
-
     Ethernet.init(ethernetConfig.spiPinSS);
-
-    if (Ethernet.begin(ethernetConfig.macAddress, ethernetConfig.dhcpLeaseTimeoutMs, ethernetConfig.dhcpLeaseResponseTimeoutMs) == 1) {
-        Serial.print("[INFO] DHCP assigned IP ");
-        Serial.println(Ethernet.localIP());
-        ethernetIsConfigured = true;
-    } else {
-        // Check for Ethernet hardware present
-        if (Ethernet.hardwareStatus() == EthernetNoHardware) {
-            Serial.println("[ERROR] Ethernet shield not found.");
-        } else if (Ethernet.linkStatus() == LinkOFF) {
-            Serial.println("[ERROR] Ethernet cable is not connected.");
-        } else if (Ethernet.linkStatus() == Unknown) {
-            Serial.println("[ERROR] Ethernet cable status unknown.");
-        } else if (Ethernet.linkStatus() == LinkON) {
-            Serial.println("[INFO] Ethernet cable is connected.");
-        
-            // Try to configure using IP address instead of DHCP
-            Ethernet.begin(ethernetConfig.macAddress, ethernetConfig.manualIp, ethernetConfig.manualDnsAddress);
-            Serial.print("[WARNING] DHCP failed, using self-assigned IP ");
-            Serial.println(Ethernet.localIP());
-            ethernetIsConfigured = true;
-        }
-    }
-    
-    servo.configure(servoConfig);
-    Serial.println("[INFO] Servo configured");
-
-    // Direct messages and errors over the websocket to the controller
-    server.configure(serverConfig, this);
-    Serial.println("[INFO] Server connection configured");
-    
-    // Direct messages from the local web server to the controller
-    localWebServer.on("/message", HTTP_POST, [this] (AsyncWebServerRequest *request) { 
-        this->handleLocalMessage(request); 
-        this->sendPreparedLocalResponse(request);
-    });
-
-    Serial.println("[INFO] Local web server configured");
 }
 
 void SesameController::loop(uint32_t millis) {
     currentTime = millis;
-    server.loop(millis);
-    servo.loop(millis);
+
+    switch (status) {
+        case SesameDeviceStatus::initial:
+            // In initial state, first configure SPI and 
+            enableCrypto(); // Ensure source of random numbers without WiFi and Bluetooth
+            initializeSpiBusForEthernetModule();
+            // Direct messages and errors over the websocket to the controller
+            server.setCallbacks(this);
+            configureEthernet();
+            status = SesameDeviceStatus::configuredButNoEthernetHardware;
+            Serial.println("[INFO] State: initial -> noHardware");
+            // Directly check for ethernet hardware
+            // break;
+        case SesameDeviceStatus::configuredButNoEthernetHardware:
+            if (!hasAvailableEthernetHardware()) { 
+                // No ethernet hardware found, wait
+                // TODO: Try rebooting after some time as a potential fix?
+                break; 
+            } 
+            status = SesameDeviceStatus::ethernetHardwareButNoLink;
+            Serial.println("[INFO] State: noHardware -> noLink");
+            // Directly check for Ethernet link
+            // break;
+        case SesameDeviceStatus::ethernetHardwareButNoLink:
+            if (!hasEthernetLink()) { 
+                if (!hasAvailableEthernetHardware()) { 
+                    status = SesameDeviceStatus::configuredButNoEthernetHardware;
+                    Serial.println("[INFO] State: noLink -> noHardware");
+                    break;
+                } 
+                // Wait for ethernet link
+                // TODO: Try rebooting after some time as a potential fix?
+                break; 
+            }
+            status = SesameDeviceStatus::ethernetLinkButNoIP;
+            Serial.println("[INFO] State: noLink -> noIP");
+            // Directly check for socket connection
+            // break;
+
+        case SesameDeviceStatus::ethernetLinkButNoIP:
+            if (!hasEthernetLink()) { 
+                if (hasAvailableEthernetHardware()) {
+                    status = SesameDeviceStatus::ethernetHardwareButNoLink;
+                    Serial.println("[INFO] State: noIP -> noLink");
+                } else {
+                    status = SesameDeviceStatus::configuredButNoEthernetHardware;
+                    Serial.println("[INFO] State: noIP -> noHardware");
+                } 
+                break; 
+            }
+            
+            startUDP();
+            status = SesameDeviceStatus::ipAddressButNoSocketConnection;
+            Serial.println("[INFO] State: noIP -> noSocket");
+            // Directly check for socket connection
+            // break;
+
+        case SesameDeviceStatus::ipAddressButNoSocketConnection:
+            if (!hasEthernetLink()) { 
+                server.shouldConnect(false);
+                stopUDP();
+                if (!hasAvailableEthernetHardware()) { 
+                    status = SesameDeviceStatus::configuredButNoEthernetHardware;
+                    Serial.println("[INFO] State: noSocket -> noHardware");
+                } else {
+                    status = SesameDeviceStatus::ethernetHardwareButNoLink;
+                    Serial.println("[INFO] State: noSocket -> noLink");
+                }
+                break; 
+            }
+            server.shouldConnect(true);
+            server.loop(millis);
+            checkLocalMessage();
+            break;
+    }
+}
+
+bool SesameController::hasAvailableEthernetHardware() {
+    EthernetHardwareStatus ethernetStatus = Ethernet.hardwareStatus();
+
+    static bool didNotify = false;
+
+    if (ethernetStatus != EthernetW5500) {
+        if (!didNotify) {
+            Serial.print("[ERROR] No Ethernet hardware found: ");
+            Serial.println(ethernetStatus);
+            didNotify = true;
+        }
+        return false;
+    }
+    return true;
+}
+
+bool SesameController::hasEthernetLink() {
+    return Ethernet.linkStatus() == EthernetLinkStatus::LinkON;
+}
+
+void SesameController::configureEthernet() {
+    if (Ethernet.begin(ethernetConfig.macAddress, ethernetConfig.dhcpLeaseTimeoutMs, ethernetConfig.dhcpLeaseResponseTimeoutMs) == 1) {
+        Serial.print("[INFO] DHCP assigned IP ");
+        Serial.println(Ethernet.localIP());
+        return;
+    }
+    Ethernet.begin(ethernetConfig.macAddress, ethernetConfig.manualIp, ethernetConfig.manualDnsAddress);
+    Serial.print("[WARNING] DHCP failed, using self-assigned IP ");
+    Serial.println(Ethernet.localIP());
+}
+
+void SesameController::startUDP() {
+    udp.begin(ethernetConfig.udpPort);
+}
+
+void SesameController::stopUDP() {
+    udp.stop();
 }
     
 // MARK: Local
 
-void SesameController::handleLocalMessage(AsyncWebServerRequest *request) {
-    if (!request->hasParam(messageUrlParameter)) {
-        Serial.println("Missing url parameter");
-        prepareResponseBuffer(MessageResult::InvalidUrlParameter);
-        return;
+void SesameController::checkLocalMessage() {
+    if (readLocalMessage()) {
+        sendPreparedLocalResponse();
     }
-    String encoded = request->getParam(messageUrlParameter)->value();
-    if (!convertHexMessageToBinary(encoded.c_str())) {
-        Serial.println("Invalid hex encoding");
-        prepareResponseBuffer(MessageResult::InvalidMessageSize);
-        return;
-    }
-    processMessage(&receivedLocalMessage);
 }
 
-void SesameController::sendPreparedLocalResponse(AsyncWebServerRequest *request) {
-    request->send_P(200, "application/octet-stream", (uint8_t*) &outgoingMessage, SIGNED_MESSAGE_SIZE);
-    Serial.printf("[INFO] Local response %u\n", outgoingMessage.message.messageType);
+bool SesameController::readLocalMessage() {
+    // if there's data available, read a packet
+    int packetSize = udp.parsePacket();
+    if (packetSize == 0) {
+        return false;
+    }
+    if (packetSize != SIGNED_MESSAGE_SIZE) {
+        Serial.print("[WARN] Received UDP packet of invalid size ");
+        Serial.println(packetSize);
+        prepareResponseBuffer(MessageResult::InvalidMessageSizeFromRemote);
+        return true;
+    }
+    int bytesRead = udp.read((uint8_t*) &receivedLocalMessage, SIGNED_MESSAGE_SIZE);
+    if (bytesRead != SIGNED_MESSAGE_SIZE) {
+        Serial.println("[WARN] Failed to read full local message");
+        prepareResponseBuffer(MessageResult::InvalidMessageSizeFromRemote);
+        return true;
+    }
+    Serial.println("[INFO] Received local message");
+    processMessage(&receivedLocalMessage, true);
+    return true;
+}
+
+void SesameController::sendPreparedLocalResponse() {
+    // send a reply to the IP address and port that sent us the packet we received
+    udp.beginPacket(udp.remoteIP(), udp.remotePort());
+    udp.write((uint8_t*) &outgoingMessage, SIGNED_MESSAGE_SIZE);
+    udp.endPacket();
+    Serial.println("[INFO] Sent local response");
 }
 
 // MARK: Server
@@ -99,28 +183,28 @@ void SesameController::sendServerError(MessageResult result) {
 void SesameController::handleServerMessage(uint8_t* payload, size_t length) {
     if (length != SIGNED_MESSAGE_SIZE) {
         // No message saved to discard, don't accidentally delete for other operation
-        sendServerError(MessageResult::InvalidMessageSize);
+        sendServerError(MessageResult::InvalidMessageSizeFromRemote);
         return;
     }
-    processMessage((SignedMessage*) payload);
+    processMessage((SignedMessage*) payload, true);
     sendPreparedResponseToServer();
 }
 
 void SesameController::sendPreparedResponseToServer() {
     server.sendResponse((uint8_t*) &outgoingMessage, SIGNED_MESSAGE_SIZE);
-    Serial.printf("[INFO] Server response %u\n", outgoingMessage.message.messageType);
+    Serial.printf("[INFO] Server response %u,%u\n", outgoingMessage.message.messageType, outgoingMessage.message.result);
 }
 
 // MARK: Message handling
 
-void SesameController::processMessage(SignedMessage* message) {
+void SesameController::processMessage(SignedMessage* message, bool shouldPerformUnlock) {
     // Result must be empty
     if (message->message.result != MessageResult::MessageAccepted) {
-        prepareResponseBuffer(MessageResult::InvalidMessageResult);
+        prepareResponseBuffer(MessageResult::InvalidMessageResultFromRemote);
         return;
     }
     if (!isAuthenticMessage(message, keyConfig.remoteKey)) {
-        prepareResponseBuffer(MessageResult::MessageAuthenticationFailed);
+        prepareResponseBuffer(MessageResult::InvalidSignatureFromRemote);
         return;
     }
     switch (message->message.messageType) {
@@ -128,10 +212,10 @@ void SesameController::processMessage(SignedMessage* message) {
         checkAndPrepareChallenge(&message->message);
         return;
     case MessageType::request:
-        completeUnlockRequest(&message->message);
+        completeUnlockRequest(&message->message, shouldPerformUnlock);
         return;
     default:
-        prepareResponseBuffer(MessageResult::InvalidMessageType);
+        prepareResponseBuffer(MessageResult::InvalidMessageTypeFromRemote);
         return;
     }
 }
@@ -139,7 +223,7 @@ void SesameController::processMessage(SignedMessage* message) {
 void SesameController::checkAndPrepareChallenge(Message* message) {
     // Server challenge must be empty
     if (message->serverChallenge != 0) {
-        prepareResponseBuffer(MessageResult::ClientChallengeInvalid);
+        prepareResponseBuffer(MessageResult::InvalidClientChallengeFromRemote);
         return;
     }
     prepareChallenge(message);
@@ -153,19 +237,20 @@ void SesameController::prepareChallenge(Message* message) {
     // Set challenge and respond
     currentClientChallenge = message->clientChallenge;
     currentServerChallenge = randomChallenge();
+    message->serverChallenge = currentServerChallenge;
     currentChallengeExpiry = currentTime + keyConfig.challengeExpiryMs;
 
     prepareResponseBuffer(MessageResult::MessageAccepted, message);
 }
 
-void SesameController::completeUnlockRequest(Message* message) {
+void SesameController::completeUnlockRequest(Message* message, bool shouldPerformUnlock) {
     // Client and server challenge must match
     if (message->clientChallenge != currentClientChallenge) {
-        prepareResponseBuffer(MessageResult::ClientChallengeInvalid, message);
+        prepareResponseBuffer(MessageResult::InvalidClientChallengeFromRemote, message);
         return;
     }
     if (message->serverChallenge != currentServerChallenge) {
-        prepareResponseBuffer(MessageResult::ServerChallengeMismatch, message);
+        prepareResponseBuffer(MessageResult::InvalidServerChallengeFromRemote, message);
         return;
     }
     if (!hasCurrentChallenge()) {
@@ -180,7 +265,9 @@ void SesameController::completeUnlockRequest(Message* message) {
     clearCurrentChallenge();
     
     // Move servo
-    servo.pressButton();
+    if (shouldPerformUnlock) {
+        servo.pressButton();
+    }
     prepareResponseBuffer(MessageResult::MessageAccepted, message);
     Serial.println("[INFO] Accepted message");
 }
@@ -197,8 +284,8 @@ void SesameController::prepareResponseBuffer(MessageResult result, Message* mess
             outgoingMessage.message.messageType = MessageType::response;
         }
     } else {
-        outgoingMessage.message.clientChallenge = message->clientChallenge;
-        outgoingMessage.message.serverChallenge = message->serverChallenge;
+        outgoingMessage.message.clientChallenge = 0;
+        outgoingMessage.message.serverChallenge = 0;
         outgoingMessage.message.messageType = MessageType::response;
     }
 

src/crypto/ESP32CryptoSource.cpp

@@ -0,0 +1,29 @@
+#include "interface/ESP32CryptoSource.h"
+
+#include <Ed25519.h>
+#include <RNG.h>
+#include <NoiseSource.h>
+
+ESP32CryptoSource::ESP32CryptoSource(const char* rngInitTag) {
+    RNG.begin(rngInitTag);
+    RNG.addNoiseSource(noise);
+}
+
+bool ESP32CryptoSource::createPrivateKey(PrivateKey* key) {
+    Ed25519::generatePrivateKey(key->bytes);
+    return true;
+}
+
+bool ESP32CryptoSource::createPublicKey(const PrivateKey* privateKey, PublicKey* publicKey) {
+    Ed25519::derivePublicKey(publicKey->bytes, privateKey->bytes);
+    return true;
+} 
+
+bool ESP32CryptoSource::sign(const uint8_t *message, uint16_t length, Signature* signature, const PrivateKey* privateKey, const PublicKey* publicKey) {
+    Ed25519::sign(signature->bytes, privateKey->bytes, publicKey->bytes, message, length);
+    return true;
+}
+
+bool ESP32CryptoSource::verify(const Signature* signature, const PublicKey* publicKey, const void *message, uint16_t length) {
+    return Ed25519::verify(signature->bytes, publicKey->bytes, message, length);
+}

src/crypto/ESP32NoiseSource.cpp

@@ -0,0 +1,18 @@
+#include "interface/ESP32NoiseSource.h"
+
+#include <esp_random.h>
+#include <bootloader_random.h>
+
+ESP32NoiseSource::ESP32NoiseSource() {
+    // Ensure that there is randomness even if Bluetooth and WiFi are disabled
+    bootloader_random_enable();
+}
+
+bool ESP32NoiseSource::calibrating() const {
+    return false;
+}
+
+void ESP32NoiseSource::stir() {
+    esp_fill_random(data, randomNumberBatchSize);
+    output(data, randomNumberBatchSize, randomNumberBatchSize * 8);
+}

src/crypto/ESP32StorageSource.cpp

@@ -0,0 +1,28 @@
+#include "interface/ESP32StorageSource.h"
+#include <EEPROM.h>
+
+bool ESP32StorageSource::writeByteAtIndex(uint8_t byte, uint16_t index) {
+    // TODO: What does return value mean?
+    EEPROM.writeByte((int) index, byte);
+    return true;
+}
+
+bool ESP32StorageSource::canProvideStorageWithSize(uint16_t size) {
+    return EEPROM.begin(size);
+}
+
+bool ESP32StorageSource::commitData() {
+    return EEPROM.commit();
+} 
+
+uint8_t ESP32StorageSource::readByteAtIndex(uint16_t index) {
+    return EEPROM.readByte((int) index);
+}
+
+uint16_t ESP32StorageSource::readBytes(uint16_t startIndex, uint16_t count, uint8_t* output) {
+    return EEPROM.readBytes(startIndex, output, count);
+}
+
+uint16_t ESP32StorageSource::writeBytes(uint8_t* bytes, uint16_t count, uint16_t startIndex) {
+    return EEPROM.writeBytes(startIndex, bytes, count);
+}

src/main.cpp

@@ -22,52 +22,51 @@
 #include "controller.h"
 #include "config.h"
 
-SesameController controller(localPort);
+ServoConfiguration servoConfig {
+    .pwmTimer = pwmTimer,
+    .pwmFrequency = servoFrequency,
+    .pin = servoPin,
+    .openDuration = lockOpeningDuration,
+    .pressedValue = servoPressedState,
+    .releasedValue = servoReleasedState,
+};
+
+ServerConfiguration serverConfig {
+    .url = serverUrl,
+    .port = serverPort,
+    .path = serverPath,
+    .key = serverAccessKey,
+    .reconnectTime = 5000,
+    .socketHeartbeatIntervalMs = socketHeartbeatIntervalMs,
+    .socketHeartbeatTimeoutMs = socketHeartbeatTimeoutMs,
+    .socketHeartbeatFailureReconnectCount = socketHeartbeatFailureReconnectCount,
+};
+
+EthernetConfiguration ethernetConfig {
+    .macAddress = ethernetMacAddress,
+    .spiPinMiso = spiPinMiso,
+    .spiPinMosi = spiPinMosi,
+    .spiPinSclk = spiPinSclk,
+    .spiPinSS = spiPinSS,
+    .dhcpLeaseTimeoutMs = dhcpLeaseTimeoutMs,
+    .dhcpLeaseResponseTimeoutMs = dhcpLeaseResponseTimeoutMs,
+    .manualIp = manualIpAddress,
+    .manualDnsAddress = manualDnsServerAddress,
+    .udpPort = localUdpPort,
+};
+
+KeyConfiguration keyConfig {
+    .remoteKey = remoteKey,
+    .localKey = localKey,
+    .challengeExpiryMs = challengeExpiryMs,
+};
+
+SesameController controller(servoConfig, serverConfig, ethernetConfig, keyConfig);
 
 void setup() {
     Serial.begin(serialBaudRate);
     Serial.setDebugOutput(true);
     Serial.println("[INFO] Device started");
-
-    ServoConfiguration servoConfig {
-        .pwmTimer = pwmTimer,
-        .pwmFrequency = servoFrequency,
-        .pin = servoPin,
-        .openDuration = lockOpeningDuration,
-        .pressedValue = servoPressedState,
-        .releasedValue = servoReleasedState,
-    };
-
-    ServerConfiguration serverConfig {
-        .url = serverUrl,
-        .port = serverPort,
-        .path = serverPath,
-        .key = serverAccessKey,
-        .reconnectTime = 5000,
-        .socketHeartbeatIntervalMs = socketHeartbeatIntervalMs,
-        .socketHeartbeatTimeoutMs = socketHeartbeatTimeoutMs,
-        .socketHeartbeatFailureReconnectCount = socketHeartbeatFailureReconnectCount,
-    };
-
-    EthernetConfiguration ethernetConfig {
-        .macAddress = ethernetMacAddress,
-        .spiPinMiso = spiPinMiso,
-        .spiPinMosi = spiPinMosi,
-        .spiPinSclk = spiPinSclk,
-        .spiPinSS = spiPinSS,
-        .dhcpLeaseTimeoutMs = dhcpLeaseTimeoutMs,
-        .dhcpLeaseResponseTimeoutMs = dhcpLeaseResponseTimeoutMs,
-        .manualIp = manualIpAddress,
-        .manualDnsAddress = manualDnsServerAddress,
-    };
-
-    KeyConfiguration keyConfig {
-        .remoteKey = remoteKey,
-        .localKey = localKey,
-        .challengeExpiryMs = challengeExpiryMs,
-    };
-
-    controller.configure(servoConfig, serverConfig, ethernetConfig, keyConfig);
 }
 
 void loop() {

src/server.cpp

@@ -1,26 +1,25 @@
 #include "server.h"
 
-constexpr int32_t pingInterval = 10000;
-constexpr uint32_t pongTimeout = 5000;
-uint8_t disconnectTimeoutCount = 3;
+ServerConnection::ServerConnection(ServerConfiguration configuration) : configuration(configuration) { }
 
-ServerConnection::ServerConnection() { }
-
-void ServerConnection::configure(ServerConfiguration configuration, ServerConnectionCallbacks *callbacks) {
+void ServerConnection::setCallbacks(ServerConnectionCallbacks *callbacks) {
     controller = callbacks;
-    this->configuration = configuration;
 }
 
-void ServerConnection::connect() {
-    if (webSocket.isConnected()) {
+void ServerConnection::shouldConnect(bool connect) {
+    if (connect == shouldBeConnected) {
         return;
     }
+
     if (controller == NULL) {
         Serial.println("[ERROR] No callbacks set for server");
         return;
     }
+    shouldBeConnected = connect;
+    nextReconnectAttemptMs = currentTime;
+}
 
-    isConnecting = true;
+void ServerConnection::connect() {
     Serial.printf("[INFO] Connecting to %s:%d%s\n", configuration.url, configuration.port, configuration.path);
     connectionTimeout = currentTime + configuration.socketHeartbeatIntervalMs;
     webSocket.begin(configuration.url, configuration.port, configuration.path);
@@ -33,50 +32,79 @@ void ServerConnection::connect() {
     webSocket.setReconnectInterval(configuration.reconnectTime);
 }
 
-void ServerConnection::didDisconnect() {
-    if (shouldReconnect || isConnecting) {
-        return; // Disconnect already registered. 
+void ServerConnection::didChangeConnectionState(bool isConnected) {
+    static bool wasConnected = false;
+    if (isConnected) {
+        Serial.println("[INFO] Socket connected, enabling heartbeat");
+        isConnecting = false;
+        webSocket.enableHeartbeat(configuration.socketHeartbeatIntervalMs, configuration.socketHeartbeatTimeoutMs, configuration.socketHeartbeatFailureReconnectCount);
+    } else {
+        isDisconnecting = false;
+        if (!wasConnected && shouldBeConnected && nextReconnectAttemptMs < currentTime) {
+            nextReconnectAttemptMs = currentTime + configuration.socketHeartbeatIntervalMs;
+            Serial.println("[INFO] Socket disconnected, setting reconnect time");
+        } else if (wasConnected) {
+            Serial.println("[INFO] Socket disconnected");
+        }
     }
-    Serial.println("[INFO] Socket disconnected");
-    nextReconnectAttemptMs = currentTime + configuration.socketHeartbeatIntervalMs;
-    shouldReconnect = true;
-}
-
-void ServerConnection::didConnect() {
-    isConnecting = false;
-    Serial.println("[INFO] Socket connected");
-    webSocket.enableHeartbeat(configuration.socketHeartbeatIntervalMs, configuration.socketHeartbeatTimeoutMs, configuration.socketHeartbeatFailureReconnectCount);
-}
-
-void ServerConnection::disconnect() {
-    webSocket.disconnect();
+    wasConnected = isConnected;
 }
 
 void ServerConnection::loop(uint32_t millis) {
     currentTime = millis;
     webSocket.loop();
-    if (shouldReconnect && !isConnecting) {
-        shouldReconnect = false;
+
+    if (shouldBeConnected) {
+        if (isDisconnecting) {
+            return; // Wait for disconnect to finish, then it will be reconnected
+        }
+        if (isConnecting) {
+            if (millis > connectionTimeout) {
+                // Cancel connection attempt
+                Serial.println("[INFO] Canceling socket connection attempt");
+                isDisconnecting = true;
+                isConnecting = false;
+                webSocket.disconnect();
+            }
+            return; // Wait for connect to finish
+        }
+        if (webSocket.isConnected()) {
+            return; // Already connected
+        }
+        if (controller == NULL) {
+            return;
+        }
+        if (millis < nextReconnectAttemptMs) {
+            return; // Wait for next reconnect
+        }
+        isConnecting = true;
         connect();
-    }
-    if (isConnecting && millis > connectionTimeout) {
-        Serial.println("[INFO] Failed to connect");
-        disconnect();
-        shouldReconnect = true;
-        isConnecting = false;
+    } else {
+        if (isDisconnecting) {
+            return; // Wait for disconnect
+        }
+        if (isConnecting) { 
+            return; // Wait until connection is established, then it will be disconnected
+        }
+        if (!webSocket.isConnected()) {
+            return;
+        }
+        isDisconnecting = true;
+        Serial.println("[INFO] Disconnecting socket");
+        webSocket.disconnect();
     }
 }
 
 void ServerConnection::webSocketEventHandler(WStype_t type, uint8_t * payload, size_t length) {
 switch(type) {
     case WStype_DISCONNECTED:
-        didDisconnect();
+        didChangeConnectionState(false);
         break;
     case WStype_CONNECTED:
-        didConnect();
+        didChangeConnectionState(true);
         break;
     case WStype_TEXT:
-        controller->sendServerError(MessageResult::TextReceived);
+        controller->sendServerError(MessageResult::TextReceivedOverSocket);
         break;
     case WStype_BIN:
         controller->handleServerMessage(payload, length);
@@ -99,5 +127,7 @@ switch(type) {
 void ServerConnection::sendResponse(uint8_t* buffer, uint16_t length) {
     if (socketIsConnected()) {
         webSocket.sendBIN(buffer, length);
+    } else {
+        Serial.println("Failed to send response, socket not connected.");
     }
 }

src/servo.cpp

@@ -2,9 +2,28 @@
 
 #include <esp32-hal.h> // For `millis()`
 
-ServoController::ServoController() { }
+void performReset(void * pvParameters) {
+    ServoController* servo = (ServoController *) pvParameters;
+    for(;;){
+        servo->loop(millis());
+        delay(50);
+    }
+}
+
+ServoController::ServoController(ServoConfiguration configuration) {
+
+    // Create a task that runs on a different core,
+    // So that it's always executed
+    xTaskCreatePinnedToCore(
+                    performReset,   /* Task function. */
+                    "Servo",     /* name of task. */
+                    1000,       /* Stack size of task */
+                    this,        /* parameter of the task */
+                    1,           /* priority of the task */
+                    &servoResetTask,
+                    1);          /* pin task to core 1 */
+
 
-void ServoController::configure(ServoConfiguration configuration) {
     openDuration = configuration.openDuration;
     pressedValue = configuration.pressedValue;
     releasedValue = configuration.releasedValue;
@@ -15,9 +34,7 @@ void ServoController::configure(ServoConfiguration configuration) {
 }
 
 void ServoController::pressButton() {
-    servo.write(pressedValue);
-    buttonIsPressed = true;
-    openingEndTime = millis() + openDuration;
+    shouldPressButton = true;
 }
 
 void ServoController::releaseButton() {
@@ -26,7 +43,12 @@ void ServoController::releaseButton() {
 }
 
 void ServoController::loop(uint32_t millis) {
-    if (buttonIsPressed && millis > openingEndTime) {
+    if (shouldPressButton) {
+        servo.write(pressedValue);
+        openingEndTime = millis + openDuration;
+        buttonIsPressed = true;
+        shouldPressButton = false;
+    } else if (buttonIsPressed && millis > openingEndTime) {
         releaseButton();
     }
 }