New state machine, local UDP route

.gitignore

@@ -1,3 +1,4 @@
 .pio
 .vscode
 .DS_Store
+include/config.h

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

@@ -0,0 +1,198 @@
+#pragma once
+
+#include "server.h"
+#include "servo.h"
+#include "message.h"
+#include "configurations/EthernetConfiguration.h"
+#include "configurations/KeyConfiguration.h"
+
+enum class SesameDeviceStatus {
+
+    /**
+     * @brief The initial state of the device after boot
+     */
+    initial,
+
+    /**
+     * @brief The device has configured the individual parts,
+     * but has no the ethernet hardware detected.
+     */
+    configuredButNoEthernetHardware,
+
+    /**
+     * @brief The device has ethernet hardware, but no ethernet link
+     */
+    ethernetHardwareButNoLink,
+
+    /**
+     * @brief The device has an ethernet link, but no IP address.
+     */
+    ethernetLinkButNoIP,
+
+    /**
+     * @brief The device has an IP address, but no socket connection
+     */
+    ipAddressButNoSocketConnection,
+};
+
+class SesameController: public ServerConnectionCallbacks {
+
+public:
+    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;
+
+    // UDP
+
+    // An EthernetUDP instance to send and receive packets over UDP
+    EthernetUDP udp;
+    EthernetHardwareStatus ethernetStatus;
+    EthernetConfiguration ethernetConfig;
+    bool ethernetIsConfigured = false;
+
+    KeyConfiguration keyConfig;
+
+    bool isReconnecting = false;
+
+    // Buffer to get local message
+    SignedMessage receivedLocalMessage;
+
+    uint32_t currentClientChallenge;
+    uint32_t currentChallengeExpiry = 0;
+    uint32_t currentServerChallenge;
+
+    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;
+    }
+
+    void clearCurrentChallenge() {
+        currentClientChallenge = 0;
+        currentServerChallenge = 0;
+        currentChallengeExpiry = 0;
+    }
+
+    // MARK: Local client callbacks
+
+    void checkLocalMessage();
+    
+    // MARK: Socket Callbacks
+
+    /**
+     * @brief Callback to send an error back to the server via the web socket.
+     * 
+     * This function is called when the socket get's an error.
+     * 
+     * @param event The error to report back
+     */
+    void sendServerError(MessageResult event);
+
+    void handleServerMessage(uint8_t* payload, size_t length);
+
+    // MARK: Message processing
+
+    /**
+     * @brief Process a received message (local or socket).
+     * 
+     * @param message The message to process.
+     * 
+     * Note: Prepares the response in the outgoing message buffer.
+     */
+    void processMessage(SignedMessage* message, bool shouldPerformUnlock);
+
+    /**
+     * @brief Checks that the message is valid and prepares a challenge.
+     * 
+     * This function is also called when a challenge response arrives too late.
+     * 
+     * @param message The message to respond to
+     * 
+     * Note: Prepares the response in the outgoing message buffer.
+     */
+    void checkAndPrepareChallenge(Message* message);
+
+    /**
+     * @brief Prepare a server challenge for a local or socket message.
+     * 
+     * @param message The message to respond to
+     * 
+     * Note: Prepares the response in the outgoing message buffer.
+     */
+    void prepareChallenge(Message* message);
+
+    /**
+     * @brief Complete an unlock request for a local or socket message.
+     * 
+     * @param message The message to respond to
+     * 
+     * Note: Prepares the response in the outgoing message buffer.
+     */
+    void completeUnlockRequest(Message* message, bool shouldPerformUnlock);
+
+    // MARK: Responses
+
+    /**
+     * @brief Prepare the outgoing message buffer for both socket and local responses.
+     * 
+     * @param event The resulting state to transmit
+     * @param message An optional message to echo
+     */
+    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();
+
+    /**
+     * @brief Send the prepared outgoing message to the server
+     */
+    void sendPreparedResponseToServer();
+
+    // MARK: Helper
+
+    bool convertHexMessageToBinary(const char* str);
+};

include/crypto.h

@@ -3,6 +3,15 @@
 #include "message.h"
 #include <stddef.h>
 
+void enableCrypto();
+
+/**
+ * @brief Create a random server challenge.
+ * 
+ * @return uint32_t 
+ */
+uint32_t randomChallenge();
+
 /**
  * @brief Create a message authentication code (MAC) for some data.
  * 
@@ -10,11 +19,10 @@
  * @param dataLength The number of bytes to authenticate
  * @param mac The output to store the MAC (must be at least 32 bytes)
  * @param key The secret key used for authentication
- * @param keyLength The length of the secret key
  * @return true The MAC was successfully written
  * @return false The MAC could not be created
  */
-bool authenticateData(const uint8_t* data, size_t dataLength, uint8_t* mac, const uint8_t* key, size_t keyLength);
+bool authenticateData(const uint8_t* data, size_t dataLength, uint8_t* mac, const uint8_t* key);
 
 /**
  * @brief Calculate a MAC for message content.
@@ -22,22 +30,20 @@ bool authenticateData(const uint8_t* data, size_t dataLength, uint8_t* mac, cons
  * @param message The message for which to calculate the MAC.
  * @param mac The output where the computed MAC is stored
  * @param key The secret key used for authentication
- * @param keyLength The length of the secret key
  * @return true The MAC was successfully computed
  * @return false The MAC could not be created
  */
-bool authenticateMessage(Message* message, uint8_t* mac, const uint8_t* key, size_t keyLength);
+bool authenticateMessage(Message* message, uint8_t* mac, const uint8_t* key);
 
 /**
  * @brief Create a message authentication code (MAC) for a message.
  * 
  * @param message The message to authenticate
  * @param key The secret key used for authentication
- * @param keyLength The length of the secret key
  * @return true The MAC was successfully added to the message
  * @return false The MAC could not be created
  */
-bool authenticateMessage(AuthenticatedMessage* message, const uint8_t* key, size_t keyLength);
+bool authenticateMessage(SignedMessage* message, const uint8_t* key);
 
 /**
  * @brief Check if a received unlock message is authentic
@@ -48,8 +54,7 @@ bool authenticateMessage(AuthenticatedMessage* message, const uint8_t* key, size
  * 
  * @param message The message to authenticate
  * @param key The secret key used for authentication
- * @param keyLength The length of the key in bytes
  * @return true The message is authentic
  * @return false The message is invalid, or the MAC could not be calculated
  */
-bool isAuthenticMessage(AuthenticatedMessage* message, const uint8_t* key, size_t keyLength);
+bool isAuthenticMessage(SignedMessage* message, const uint8_t* key);

include/example_config.h

@@ -12,6 +12,9 @@ constexpr uint32_t serialBaudRate = 115200;
 
 /* Keys */
 
+// The number of remote devices
+constexpr int remoteDeviceCount = 1;
+
 // The size of the symmetric keys used for signing and verifying messages
 constexpr size_t keySize = 32;
 
@@ -40,9 +43,19 @@ constexpr const char* wifiSSID = "MyWiFi";
 // The WiFi password to connect to the above network
 constexpr const char* wifiPassword = "00000000";
 
+// The name of the device on the network
+constexpr const char* networkName = "Sesame-Device";
+
 // The interval to reconnect to WiFi if the connection is broken
 constexpr uint32_t wifiReconnectInterval = 10000;
 
+// The interval to reconnect to WiFi if the connection is broken
+constexpr uint32_t wifiPeriodicReconnectInterval = 86400;
+
+/* Local server */
+
+// The port for the local server to directly receive messages over WiFi
+constexpr uint16_t localPort = 80;
 
 /* Server */ 
 

include/fresh.h

@@ -1,114 +0,0 @@
-#pragma once
-
-#include <stdint.h>
-
-/**
- * @brief The size of the message counter in bytes (uint32_t)
- */
-#define MESSAGE_COUNTER_SIZE sizeof(uint32_t)
-
-/**
- * @brief Configure an NTP server to get the current time
- * 
- * @param offsetToGMT The timezone offset in seconds
- * @param offsetDaylightSavings The daylight savings offset in seconds
- * @param serverUrl The url of the NTP server
- */
-void configureNTP(int32_t offsetToGMT, int32_t offsetDaylightSavings, const char* serverUrl);
-
-/**
- * @brief Print the current time to the serial output
- * 
- * The time must be initialized by calling `configureNTP()` before use.
- */
-void printLocalTime();
-
-/** 
- * Gets the current epoch time
- */
-uint32_t getEpochTime();
-
-/**
- * @brief The allowed time discrepancy (in seconds)
- * 
- * Specifies the allowed discrepancy between the time of a received message
- * and the device time (in seconds).
- * 
- * A stricter (lower) value better prevents against replay attacks,
- * but may lead to issues when dealing with slow networks and other
- * routing delays.
- * 
- * @param offset The offset in both directions (seconds)
- */
-void setMessageTimeAllowedOffset(uint32_t offset);
-
-/**
- * @brief Check wether the time of a message is within the allowed bounds regarding freshness.
- * 
- * The timestamp is used to ensure 'freshness' of the messages,
- * i.e. that they are not unreasonably delayed or captured and
- * later replayed by an attacker.
- * 
- * @param messageTime The timestamp of the message (seconds since epoch) 
- * @return true The time is within the acceptable offset of the local time
- * @return false The message time is invalid
- */
-bool isMessageTimeAcceptable(uint32_t messageTime);
-
-/**
- * @brief Initialize the use of the message counter API
- * 
- * The message counter is stored in EEPROM, which must be initialized before use.
- * 
- * @note The ESP32 does not have a true EEPROM, 
- * which is emulated using a section of the flash memory.
- */
-void prepareMessageCounterUsage();
-
-/**
- * @brief Get the expected count for the next message.
- * 
- * The counter is stored in EEPROM to persist across restarts
- * 
- * @return The next counter to use by the remote 
- */
-uint32_t getNextMessageCounter();
-
-/**
- * @brief Print info about the current message counter to the serial output
- * 
- */
-void printMessageCounter();
-
-/**
- * @brief Check if a received counter is valid
- * 
- * The counter is valid if it is larger than the previous counter
- * (larger or equal to the next expected counter).
- * 
- * @param counter The counter to check
- * @return true The counter is valid
- * @return false The counter belongs to an old message
- */
-bool isMessageCounterValid(uint32_t counter);
-
-/**
- * @brief Mark a counter of a message as used.
- * 
- * The counter value is stored in EEPROM to persist across restarts.
- * 
- * All messages with counters lower than the given one will become invalid.
- * 
- * @param counter The counter used in the last message.
- */
-void didUseMessageCounter(uint32_t counter);
-
-/**
- * @brief Reset the message counter.
- * 
- * @warning The counter should never be reset in production environments,
- * and only together with a new secret key. Otherwise old messages may be
- * used for replay attacks.
- * 
- */
-void resetMessageCounter();

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

@@ -1,6 +1,7 @@
 #pragma once
 
-#include "stdint.h"
+#include <stdint.h>
+#include <stddef.h>
 
 /**
  * @brief The size of a message authentication code
@@ -13,51 +14,96 @@
 
 #pragma pack(push, 1)
 
+enum class MessageType: uint8_t {
+
+    /// @brief The initial message from remote to device to request a challenge.
+    initial = 0,
+
+    /// @brief The second message in an unlock with the challenge from the device to the remote
+    challenge = 1,
+
+    /// @brief The third message with the signed challenge from the remote to the device
+    request = 2,
+
+    /// @brief The final message with the unlock result from the device to the remote
+    response = 3,
+
+};
+
+enum class MessageResult: uint8_t {
+
+    /// @brief The message was accepted.
+    MessageAccepted = 0,
+    
+    /// @brief The web socket received text while waiting for binary data.
+    TextReceivedOverSocket = 1,
+
+    /// @brief An unexpected socket event occured while performing the exchange.
+    UnexpectedSocketEvent = 2,
+
+    /// @brief The received message size is invalid.
+    InvalidMessageSizeFromRemote = 3,
+
+    /// @brief The message signature was incorrect.
+    InvalidSignatureFromRemote = 4,
+
+    /// @brief The server challenge of the message did not match previous messages
+    InvalidServerChallengeFromRemote = 5,
+
+    /// @brief The client challenge of the message did not match previous messages
+    InvalidClientChallengeFromRemote = 6,
+
+    /// @brief An unexpected or unsupported message type was received
+    InvalidMessageTypeFromRemote = 7,
+
+    /// @brief A message is already being processed
+    TooManyRequests = 8,
+
+    /// @brief The received message result was invalid
+    InvalidMessageResultFromRemote = 9,
+
+    /// @brief An invalid Url parameter was set sending a message to the device over a local connection
+    InvalidUrlParameter = 10,
+
+};
+
 /**
- * @brief The content of an unlock message.
+ * @brief A generic message to exchange during challenge-response authentication.
- * 
- * The content is necessary to ensure freshness of the message
- * by requiring a recent time and a monotonously increasing counter.
- * This prevents messages from being delayed or being blocked and
- * replayed later.
  */
 typedef struct {
 
-    /** 
+    /// @brief The type of message being sent.
-     * The timestamp of message creation
+    MessageType messageType;
-     * 
-     * The timestamp is encoded as the epoch time, i.e. seconds since 1970 (GMT).
-     * 
-     * The timestamp is used to ensure 'freshness' of the messages,
-     * i.e. that they are not unreasonably delayed or captured and
-     * later replayed by an attacker.
-     */
-    uint32_t time;
 
     /**
-     * The counter of unlock messages
+     * @brief The random nonce created by the remote
      * 
-     * This counter must always increase with each message from the remote
+     * This nonce is a random number created by the remote, different for each unlock request.
-     * in order for the messages to be deemed valid. Transfering the counters
+     * It is set for all message types.
-     * back and forth also gives information about lost messages and potential
-     * attacks. Both the remote and the device keep a record of at least the
-     * last used counter.
      */
-    uint32_t id;
+    uint32_t clientChallenge;
+
+    /**
+     * @brief A random number to sign by the remote
+     * 
+     * This nonce is set by the server after receiving an initial message.
+     * It is set for the message types `challenge`, `request`, and `response`.
+     */
+    uint32_t serverChallenge;
+
+    /**
+     * @brief The response status for the previous message.
+     * 
+     * It is set only for messages from the server, e.g. the `challenge` and `response` message types.
+     * Must be set to `MessageAccepted` for other messages.
+     */
+    MessageResult result;
 
 } Message;
 
 /**
- * @brief An authenticated message by the mobile device to command unlocking.
+ * @brief The signed version of a message.
  * 
- * The message is protected by a message authentication code (MAC) based on
- * a symmetric key shared by the device and the remote. This code ensures
- * that the contents of the request were not altered. The message further
- * contains a timestamp to ensure that the message is recent, and not replayed
- * by an attacker. An additional counter is also included for this purpose,
- * which must continously increase for a message to be valid. This increases
- * security a bit, since the timestamp validation must be tolerant to some 
- * inaccuracy due to mismatching clocks.
  */
 typedef struct {
 
@@ -69,19 +115,29 @@ typedef struct {
      */
     uint8_t mac[SHA256_MAC_SIZE];
 
-    /**
+    /// @brief The message
-     * @brief The message content.
-     * 
-     * The content is necessary to ensure freshness of the message
-     * by requiring a recent time and a monotonously increasing counter.
-     * This prevents messages from being delayed or being blocked and
-     * replayed later.
-     */
     Message message;
 
-} AuthenticatedMessage;
+} SignedMessage;
+
+constexpr size_t messageCounterSize = sizeof(uint32_t);
+
 #pragma pack(pop)
 
-#define MESSAGE_CONTENT_SIZE sizeof(Message)
+constexpr int MESSAGE_CONTENT_SIZE = sizeof(Message);
+
+constexpr int SIGNED_MESSAGE_SIZE = sizeof(SignedMessage);
+
+/**
+ * @brief A callback for messages received over the socket
+ * 
+ * The first parameter is a pointer to the byte buffer.
+ * The second parameter indicates the number of received bytes.
+ */
+typedef void (*MessageCallback)(uint8_t* payload, size_t length);
+
+/**
+ * @brief A callback for socket errors
+ */
+typedef void (*ErrorCallback)(MessageResult event);
 
-#define AUTHENTICATED_MESSAGE_SIZE sizeof(AuthenticatedMessage)

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

@@ -2,68 +2,103 @@
 
 #include "message.h"
 #include "crypto.h"
-#include <WiFiMulti.h>
-#include <WiFiClientSecure.h>
 #include <WebSocketsClient.h>
 
-/**
+struct ServerConfiguration {
- * An event signaled from the device
+
+    /**
+     * @brief The url of the remote server to connect to
      */
-enum class SesameEvent {
+    const char* url; 
-    TextReceived = 1,
+    
-    UnexpectedSocketEvent = 2,
+    /**
-    InvalidMessageData = 3,
+     * @brief The server port
-    MessageAuthenticationFailed = 4,
+     */
-    MessageTimeMismatch = 5,
+    int port;
-    MessageCounterInvalid = 6,
+
-    MessageAccepted = 7,
+    /**
+     * @brief The path on the server
+     */
+    const char* path;
+
+    /**
+     * @brief The authentication key for the server
+     */
+    const char* key;
+    
+    uint32_t reconnectTime;
+
+    uint32_t socketHeartbeatIntervalMs;
+    uint32_t socketHeartbeatTimeoutMs;
+    uint8_t socketHeartbeatFailureReconnectCount;
+
 };
 
-/**
+class ServerConnectionCallbacks {
- * @brief A callback for messages received over the socket
+
- * 
+public:
- * The first parameter is the received message.
+
- * The second parameter is the response to the remote.
+    virtual void sendServerError(MessageResult event) = 0;
- * The return value is the type of event to respond with.
+    virtual void handleServerMessage(uint8_t* payload, size_t length) = 0;
- */
+};
-typedef SesameEvent (*MessageCallback)(AuthenticatedMessage*, AuthenticatedMessage*);
 
 class ServerConnection {
 
 public:
 
-    ServerConnection(const char* url, int port, const char* path);
+    ServerConnection(ServerConfiguration configuration);
 
-    void connect(const char* key, uint32_t reconnectTime = 5000);
+    /**
+     * @brief Set the configuration and the callback handler
+     * 
+     * @param callback The handler to handle messages and errors
+     */
+    void setCallbacks(ServerConnectionCallbacks* callbacks);
 
-    void connectSSL(const char* key, uint32_t reconnectTime = 5000);
+    void shouldConnect(bool connect);
 
-    void loop();
+    /**
+     * @brief Call this function regularly to handle socket operations.
+     * 
+     * Connecting and disconnecting is done automatically.
+     * 
+     */
+    void loop(uint32_t millis);
 
-    void onMessage(MessageCallback callback);
+    /**
-
+     * @brief Send a response message over the socket
-    // Indicator that the socket is connected.
+     * 
-    bool socketIsConnected = false;
+     * @param buffer The data buffer
+     * @param length The number of bytes to send
+     */
+    void sendResponse(uint8_t* buffer, uint16_t length);
 
 private:
 
-    const char* url;
+    uint32_t currentTime = 0;
+    bool shouldBeConnected = false;
 
-    int port;
+    bool socketIsConnected() {
+        return webSocket.isConnected();
+    }
 
-    const char* path;
+    void connect();
 
-    const char* key = NULL;
+    bool isConnecting = false;
+    bool isDisconnecting = false;
+    uint32_t connectionTimeout = 0;
+    uint32_t nextReconnectAttemptMs = 0;
 
-    MessageCallback messageCallback = NULL;
+    void didChangeConnectionState(bool isConnected);
+
+
+    ServerConfiguration configuration;
+
+    ServerConnectionCallbacks* controller = NULL;
 
     // WebSocket to connect to the control server
     WebSocketsClient webSocket;
 
-    void reconnectAfter(uint32_t reconnectTime);
-
-    void registerEventCallback();
-
     /**
      * Callback for WebSocket events.
      *
@@ -73,35 +108,4 @@ private:
      * @param length The number of bytes received
      */
     void webSocketEventHandler(WStype_t type, uint8_t * payload, size_t length);
-
-    /**
-     * Process received binary data.
-     *
-     * Checks whether the received data is a valid and unused key,
-     * and then signals that the motor should move.
-     * Sends the event id to the server as a response to the request.
-     *
-     * If the key is valid, then `shouldStartOpening` is set to true.
-     *
-     * @param payload The pointer to the received data.
-     * @param length The number of bytes received.
-     */
-    void processReceivedBytes(uint8_t* payload, size_t length);
-
-    /**
-     * Send a response event to the server and include the next key index.
-     *
-     * Sends the event type as three byte.
-     * @param event The event type
-     */
-    void sendFailureResponse(SesameEvent event);
-
-    /**
-     * Send a response event to the server and include the next key index.
-     *
-     * Sends the event type as three byte.
-     * @param event The event type
-     */
-    void sendResponse(SesameEvent event, AuthenticatedMessage* message);
-
 };

include/servo.h

@@ -3,6 +3,40 @@
 #include <stdint.h>
 #include <ESP32Servo.h> // To control the servo
 
+struct ServoConfiguration {
+    /**
+     * @brief The timer to use for the servo control
+     * number 0-3 indicating which timer to allocate in this library
+     */
+    int pwmTimer;
+
+    /**
+     * @brief The servo frequency (depending on the model used)
+     */
+    int pwmFrequency;
+
+    /**
+     * @brief The pin where the servo PWM line is connected
+     */
+    int pin;
+
+    /**
+     * @brief The duration (in ms) for which the button should remain pressed
+     */
+    uint32_t openDuration;
+
+    /**
+     * @brief The servo value (in µs) that specifies the 'pressed' state
+     */
+    int pressedValue;
+
+    /**
+     * @brief The servo value (in µs) that specifies the 'released' state
+     */
+    int releasedValue;
+
+};
+
 /**
  * @brief A controller for the button control servo
  * 
@@ -17,54 +51,35 @@ class ServoController {
 public:
 
     /**
-     * @brief Construct a new Servo Controller object
+     * @brief Construct a new servo controller
      * 
-     * @param timer The timer to use for the servo control
+     * @param The configuration for the servo
-     * @param frequency The servo frequency (depending on the model used)
-     * @param pin The pin where the servo PWM line is connected
      */
-    ServoController(int timer, int frequency, int pin);
+    ServoController(ServoConfiguration configuration);
-
-    /**
-     * @brief Configure the button values
-     * 
-     * @param openDuration The duration (in ms) for which the button should remain pressed
-     * @param pressedValue The servo value (in µs) that specifies the 'pressed' state
-     * @param releasedValue The servo value (in µs) that specifies the 'released' state
-     */
-    void configure(uint32_t openDuration, int pressedValue, int releasedValue);
 
     /**
      * @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
      * the opening interval is dependent on the calling frequency.
      */
-    void loop();
+    void loop(uint32_t millis);
 
     /**
      * Push the door opener button down by moving the servo arm.
      */
     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;
 
-    int timer;
+    // Indicate that the button should be pressed
-    
+    bool shouldPressButton = false;
-    int frequency;
-    
-    int pin;
 
     uint32_t openDuration = 0;
 
@@ -81,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,6 +13,9 @@ platform = espressif32
 board = az-delivery-devkit-v4
 framework = arduino
 lib_deps = 
-	links2004/WebSockets@^2.3.7
+	madhephaestus/ESP32Servo@^1.1.0
-	madhephaestus/ESP32Servo@^0.11.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

@@ -0,0 +1,333 @@
+#include "controller.h"
+#include "crypto.h"
+#include "config.h"
+
+#include <SPI.h>
+#include <Ethernet.h>
+
+SesameController::SesameController(ServoConfiguration servoConfig, ServerConfiguration serverConfig, EthernetConfiguration ethernetConfig, KeyConfiguration keyConfig) 
+    : ethernetConfig(ethernetConfig), keyConfig(keyConfig), servo(servoConfig), server(serverConfig) {
+    
+}
+
+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);
+}
+
+void SesameController::loop(uint32_t millis) {
+    currentTime = 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::checkLocalMessage() {
+    if (readLocalMessage()) {
+        sendPreparedLocalResponse();
+    }
+}
+
+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
+
+void SesameController::sendServerError(MessageResult result) {
+    prepareResponseBuffer(result); // No message to echo
+    sendPreparedResponseToServer();
+}
+
+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::InvalidMessageSizeFromRemote);
+        return;
+    }
+    processMessage((SignedMessage*) payload, true);
+    sendPreparedResponseToServer();
+}
+
+void SesameController::sendPreparedResponseToServer() {
+    server.sendResponse((uint8_t*) &outgoingMessage, SIGNED_MESSAGE_SIZE);
+    Serial.printf("[INFO] Server response %u,%u\n", outgoingMessage.message.messageType, outgoingMessage.message.result);
+}
+
+// MARK: Message handling
+
+void SesameController::processMessage(SignedMessage* message, bool shouldPerformUnlock) {
+    // Result must be empty
+    if (message->message.result != MessageResult::MessageAccepted) {
+        prepareResponseBuffer(MessageResult::InvalidMessageResultFromRemote);
+        return;
+    }
+    if (!isAuthenticMessage(message, keyConfig.remoteKey)) {
+        prepareResponseBuffer(MessageResult::InvalidSignatureFromRemote);
+        return;
+    }
+    switch (message->message.messageType) {
+    case MessageType::initial:
+        checkAndPrepareChallenge(&message->message);
+        return;
+    case MessageType::request:
+        completeUnlockRequest(&message->message, shouldPerformUnlock);
+        return;
+    default:
+        prepareResponseBuffer(MessageResult::InvalidMessageTypeFromRemote);
+        return;
+    }
+}
+
+void SesameController::checkAndPrepareChallenge(Message* message) {
+    // Server challenge must be empty
+    if (message->serverChallenge != 0) {
+        prepareResponseBuffer(MessageResult::InvalidClientChallengeFromRemote);
+        return;
+    }
+    prepareChallenge(message);
+}
+
+void SesameController::prepareChallenge(Message* message) {
+    if (hasCurrentChallenge()) {
+        Serial.println("[INFO] Overwriting old challenge");
+    }
+
+    // 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, bool shouldPerformUnlock) {
+    // Client and server challenge must match
+    if (message->clientChallenge != currentClientChallenge) {
+        prepareResponseBuffer(MessageResult::InvalidClientChallengeFromRemote, message);
+        return;
+    }
+    if (message->serverChallenge != currentServerChallenge) {
+        prepareResponseBuffer(MessageResult::InvalidServerChallengeFromRemote, message);
+        return;
+    }
+    if (!hasCurrentChallenge()) {
+        // Directly send new challenge on expiry, since rest of message is valid
+        // This allows the remote to directly try again without requesting a new challenge.
+        // Security note: The client nonce is reused in this case, but an attacker would still 
+        // not be able to create a valid unlock request due to the new server nonce.
+        prepareChallenge(message);
+        return;
+    }
+
+    clearCurrentChallenge();
+    
+    // Move servo
+    if (shouldPerformUnlock) {
+        servo.pressButton();
+    }
+    prepareResponseBuffer(MessageResult::MessageAccepted, message);
+    Serial.println("[INFO] Accepted message");
+}
+
+void SesameController::prepareResponseBuffer(MessageResult result, Message* message) {
+    outgoingMessage.message.result = result;
+    if (message != NULL) {
+        outgoingMessage.message.clientChallenge = message->clientChallenge;
+        outgoingMessage.message.serverChallenge = message->serverChallenge;
+        // All outgoing messages are responses, except if an initial message is accepted
+        if (message->messageType == MessageType::initial && result == MessageResult::MessageAccepted) {
+            outgoingMessage.message.messageType = MessageType::challenge;
+        } else {
+            outgoingMessage.message.messageType = MessageType::response;
+        }
+    } else {
+        outgoingMessage.message.clientChallenge = 0;
+        outgoingMessage.message.serverChallenge = 0;
+        outgoingMessage.message.messageType = MessageType::response;
+    }
+
+    if (!authenticateMessage(&outgoingMessage, keyConfig.localKey)) {
+        Serial.println("[ERROR] Failed to sign message");
+    }
+}
+
+// MARK: Helper
+
+/**
+ * @brief 
+ * 
+ * Based on https://stackoverflow.com/a/23898449/266720
+ * 
+ * @param str 
+ * @return true 
+ * @return false 
+ */
+bool SesameController::convertHexMessageToBinary(const char* str) {
+    uint8_t* buffer = (uint8_t*) &receivedLocalMessage;
+    // TODO: Fail if invalid hex values are used
+    uint8_t  idx0, idx1;
+
+    // mapping of ASCII characters to hex values
+    const uint8_t hashmap[] = {
+        0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, // 01234567
+        0x08, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 89:;<=>?
+        0x00, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, 0x00, // @ABCDEFG
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // HIJKLMNO
+    };
+
+    size_t len = strlen(str);
+    if (len != SIGNED_MESSAGE_SIZE * 2) {
+        // Require exact message size
+        return false;
+    }
+
+    for (size_t pos = 0; pos < len; pos += 2) {
+        idx0 = ((uint8_t)str[pos+0] & 0x1F) ^ 0x10;
+        idx1 = ((uint8_t)str[pos+1] & 0x1F) ^ 0x10;
+        buffer[pos/2] = (uint8_t)(hashmap[idx0] << 4) | hashmap[idx1];
+    };
+    return true;
+}

src/crypto.cpp

@@ -1,8 +1,19 @@
 #include "crypto.h"
+#include "config.h"
 #include <string.h>
 #include <mbedtls/md.h>
+#include <esp_random.h>
+#include <bootloader_random.h>
 
-bool authenticateData(const uint8_t* data, size_t dataLength, uint8_t* mac, const uint8_t* key, size_t keyLength) {
+void enableCrypto() {
+    bootloader_random_enable();
+}
+
+uint32_t randomChallenge() {
+    return esp_random();
+}
+
+bool authenticateData(const uint8_t* data, size_t dataLength, uint8_t* mac, const uint8_t* key) {
     mbedtls_md_context_t ctx;
     mbedtls_md_type_t md_type = MBEDTLS_MD_SHA256;
     int result;
@@ -12,7 +23,7 @@ bool authenticateData(const uint8_t* data, size_t dataLength, uint8_t* mac, cons
     if (result) {
         return false;
     }
-    result = mbedtls_md_hmac_starts(&ctx, key, keyLength);
+    result = mbedtls_md_hmac_starts(&ctx, key, keySize);
     if (result) {
         return false;
     }
@@ -28,17 +39,17 @@ bool authenticateData(const uint8_t* data, size_t dataLength, uint8_t* mac, cons
     return true;
 }
 
-bool authenticateMessage(Message* message, uint8_t* mac, const uint8_t* key, size_t keyLength) {
+bool authenticateMessage(Message* message, uint8_t* mac, const uint8_t* key) {
-    return authenticateData((const uint8_t*) message, MESSAGE_CONTENT_SIZE, mac, key, keyLength);
+    return authenticateData((const uint8_t*) message, MESSAGE_CONTENT_SIZE, mac, key);
 }
 
-bool authenticateMessage(AuthenticatedMessage* message, const uint8_t* key, size_t keyLength) {
+bool authenticateMessage(SignedMessage* message, const uint8_t* key) {
-    return authenticateMessage(&message->message, message->mac, key, keyLength);
+    return authenticateMessage(&message->message, message->mac, key);
 }
 
-bool isAuthenticMessage(AuthenticatedMessage* message, const uint8_t* key, size_t keyLength) {
+bool isAuthenticMessage(SignedMessage* message, const uint8_t* key) {
     uint8_t mac[SHA256_MAC_SIZE];
-    if (!authenticateMessage(&message->message, mac, key, keyLength)) {
+    if (!authenticateMessage(&message->message, mac, key)) {
         return false;
     }
     return memcmp(mac, message->mac, SHA256_MAC_SIZE) == 0;

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/fresh.cpp

@@ -1,102 +0,0 @@
-#include "fresh.h"
-
-#include <Arduino.h>
-#include <time.h>
-#include <EEPROM.h>
-
-/**
- * @brief The size of the message counter in bytes (uint32_t)
- */
-#define MESSAGE_COUNTER_SIZE sizeof(uint32_t)
-
-/**
- * @brief The allowed discrepancy between the time of a received message
- * and the device time (in seconds)
- * 
- * A stricter (lower) value better prevents against replay attacks,
- * but may lead to issues when dealing with slow networks and other
- * routing delays.
- */
-uint32_t allowedOffset = 60;
-
-void setMessageTimeAllowedOffset(uint32_t offset) {
-    allowedOffset = offset;
-}
-
-void configureNTP(int32_t offsetToGMT, int32_t offsetDaylightSavings, const char* serverUrl) {
-    configTime(offsetToGMT, offsetDaylightSavings, serverUrl);
-}
-
-void printLocalTime() {
-    struct tm timeinfo;
-    if (getLocalTime(&timeinfo)) {
-        Serial.println(&timeinfo, "[INFO] Time is %A, %d. %B %Y %H:%M:%S");
-    } else {
-        Serial.println("[WARN] No local time available");
-    }
-}
-
-uint32_t getEpochTime() {
-    time_t now;
-    struct tm timeinfo;
-    if (!getLocalTime(&timeinfo)) {
-        Serial.println("[WARN] Failed to obtain local time");
-        return(0);
-    }
-    time(&now);
-    return now;
-}
-
-bool isMessageTimeAcceptable(uint32_t t) {
-    uint32_t localTime = getEpochTime();
-    if (localTime == 0) {
-        Serial.println("No epoch time available");
-        return false;
-    }
-    if (t > localTime + allowedOffset) {
-        return false;
-    }
-    if (t < localTime - allowedOffset) {
-        return false;
-    }
-    return true;
-}
-
-void prepareMessageCounterUsage() {
-    EEPROM.begin(MESSAGE_COUNTER_SIZE);
-}
-
-uint32_t getNextMessageCounter() {
-    uint32_t counter = (uint32_t) EEPROM.read(0) << 24;
-    counter         += (uint32_t) EEPROM.read(1) << 16;
-    counter         += (uint32_t) EEPROM.read(2) << 8;
-    counter         += (uint32_t) EEPROM.read(3);
-    return counter;
-}
-
-void printMessageCounter() {
-    Serial.printf("[INFO] Next message number: %u\n", getNextMessageCounter());
-}
-
-bool isMessageCounterValid(uint32_t counter) {
-    return counter >= getNextMessageCounter();
-}
-
-void setMessageCounter(uint32_t counter) {
-    EEPROM.write(0, (counter >> 24) & 0xFF);
-    EEPROM.write(1, (counter >> 16) & 0xFF);
-    EEPROM.write(2, (counter >>  8) & 0xFF);
-    EEPROM.write(3,  counter        & 0xFF);
-
-    EEPROM.commit();
-}
-
-void didUseMessageCounter(uint32_t counter) {
-    // Store the next counter, so that resetting starts at 0
-    setMessageCounter(counter+1);
-}
-
-void resetMessageCounter() {
-    setMessageCounter(0);
-    Serial.println("[WARN] Message counter reset");
-}

src/main.cpp

@@ -4,108 +4,73 @@
  *
  * The code for a simple door unlock mechanism where a servo pushes on an existing
  * physical button.
+ * 
+ * On compile error:
+ * 
+ * In <Server.h>
+ * 
+ * change:
+ *      virtual void begin(uint16_t port=0) =0;
+ * to:
+ *      virtual void begin() =0;
  */
 #include <Arduino.h>
-#include <WiFi.h>
 
-#include "crypto.h"
-#include "fresh.h"
 #include "message.h"
 #include "server.h"
 #include "servo.h"
+#include "controller.h"
 #include "config.h"
 
-/* Global variables */
+ServoConfiguration servoConfig {
+    .pwmTimer = pwmTimer,
+    .pwmFrequency = servoFrequency,
+    .pin = servoPin,
+    .openDuration = lockOpeningDuration,
+    .pressedValue = servoPressedState,
+    .releasedValue = servoReleasedState,
+};
 
-ServerConnection server(serverUrl, serverPort, serverPath);
+ServerConfiguration serverConfig {
+    .url = serverUrl,
+    .port = serverPort,
+    .path = serverPath,
+    .key = serverAccessKey,
+    .reconnectTime = 5000,
+    .socketHeartbeatIntervalMs = socketHeartbeatIntervalMs,
+    .socketHeartbeatTimeoutMs = socketHeartbeatTimeoutMs,
+    .socketHeartbeatFailureReconnectCount = socketHeartbeatFailureReconnectCount,
+};
 
-ServoController servo(pwmTimer, servoFrequency, servoPin);
+EthernetConfiguration ethernetConfig {
+    .macAddress = ethernetMacAddress,
+    .spiPinMiso = spiPinMiso,
+    .spiPinMosi = spiPinMosi,
+    .spiPinSclk = spiPinSclk,
+    .spiPinSS = spiPinSS,
+    .dhcpLeaseTimeoutMs = dhcpLeaseTimeoutMs,
+    .dhcpLeaseResponseTimeoutMs = dhcpLeaseResponseTimeoutMs,
+    .manualIp = manualIpAddress,
+    .manualDnsAddress = manualDnsServerAddress,
+    .udpPort = localUdpPort,
+};
 
-/* Event callbacks */
+KeyConfiguration keyConfig {
+    .remoteKey = remoteKey,
+    .localKey = localKey,
+    .challengeExpiryMs = challengeExpiryMs,
+};
 
-SesameEvent handleReceivedMessage(AuthenticatedMessage* payload, AuthenticatedMessage* response);
+SesameController controller(servoConfig, serverConfig, ethernetConfig, keyConfig);
-
-/* Logic */
 
 void setup() {
     Serial.begin(serialBaudRate);
     Serial.setDebugOutput(true);
     Serial.println("[INFO] Device started");
-
-    servo.configure(lockOpeningDuration, servoPressedState, servoReleasedState);
-    Serial.println("[INFO] Servo configured");
-
-    prepareMessageCounterUsage();
-    //resetMessageCounter();
-    printMessageCounter();
-
-    server.onMessage(handleReceivedMessage);
 }
 
-uint32_t nextWifiReconnect = 0;
-bool isReconnecting = false;
-
 void loop() {
     uint32_t time = millis();
 
-    server.loop();
+    controller.loop(time);
-    servo.loop();
-    
-    // Reconnect to WiFi
-    if(time > nextWifiReconnect && WiFi.status() != WL_CONNECTED) {
-        Serial.println("[INFO] Reconnecting WiFi...");
-        WiFi.begin(wifiSSID, wifiPassword);
-        isReconnecting = true;
-        nextWifiReconnect = time + wifiReconnectInterval;
-    }
-    if (isReconnecting && WiFi.status() == WL_CONNECTED) {
-        isReconnecting = false;
-        Serial.println("[INFO] WiFi connected, opening socket");
-        server.connectSSL(serverAccessKey);
-        configureNTP(timeOffsetToGMT, timeOffsetDaylightSavings, ntpServerUrl);
-        printLocalTime();
-    }
-}
-
-SesameEvent processMessage(AuthenticatedMessage* message) {
-    if (!isMessageCounterValid(message->message.id)) {
-        return SesameEvent::MessageCounterInvalid;
-    }
-    if (!isMessageTimeAcceptable(message->message.time)) {
-        return SesameEvent::MessageTimeMismatch;
-    }
-    if (!isAuthenticMessage(message, remoteKey, keySize)) {
-        return SesameEvent::MessageAuthenticationFailed;
-    }
-    return SesameEvent::MessageAccepted;
-}
-
-/**
- * Process received binary data.
- *
- * Checks whether the received data is a valid and unused key,
- * and then signals that the motor should move.
- *
- * @param payload The pointer to the received data.
- * @param length The number of bytes received.
- * @return The event to signal to the server.
- */
-SesameEvent handleReceivedMessage(AuthenticatedMessage* message, AuthenticatedMessage* response) {
-    SesameEvent event = processMessage(message);
-    
-    // Only open when message is valid
-    if (event == SesameEvent::MessageAccepted) {
-        didUseMessageCounter(message->message.id);
-        // Move servo
-        servo.pressButton();
-        Serial.printf("[Info] Accepted message %d\n", message->message.id);
-    }
-
-    // Create response for all cases
-    response->message.time = getEpochTime();
-    response->message.id = getNextMessageCounter();
-    if (!authenticateMessage(response, localKey, keySize)) {
-        return SesameEvent::MessageAuthenticationFailed;
-    }
-    return event;
 }

src/server.cpp

@@ -1,105 +1,133 @@
 #include "server.h"
 
-constexpr int32_t pingInterval = 10000;
+ServerConnection::ServerConnection(ServerConfiguration configuration) : configuration(configuration) { }
-constexpr uint32_t pongTimeout = 5000;
-uint8_t disconnectTimeoutCount = 3;
-
-ServerConnection::ServerConnection(const char* url, int port, const char* path) : 
-    url(url), port(port), path(path) {
 
+void ServerConnection::setCallbacks(ServerConnectionCallbacks *callbacks) {
+    controller = callbacks;
 }
 
-void ServerConnection::connect(const char* key, uint32_t reconnectTime) {
+void ServerConnection::shouldConnect(bool connect) {
-    webSocket.begin(url, port, path);
+    if (connect == shouldBeConnected) {
-    registerEventCallback();
-    reconnectAfter(reconnectTime);
-}
-
-void ServerConnection::connectSSL(const char* key, uint32_t reconnectTime) {
-    if (socketIsConnected) {
         return;
     }
-    this->key = key;
+
-    webSocket.beginSSL(url, port, path);
+    if (controller == NULL) {
-    registerEventCallback();
+        Serial.println("[ERROR] No callbacks set for server");
-    reconnectAfter(reconnectTime);
+        return;
+    }
+    shouldBeConnected = connect;
+    nextReconnectAttemptMs = currentTime;
 }
 
-void ServerConnection::loop() {
+void ServerConnection::connect() {
-    webSocket.loop();
+    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);
+    webSocket.setAuthorization(configuration.key);
 
-void ServerConnection::onMessage(MessageCallback callback) {
-    messageCallback = callback;
-}
-
-void ServerConnection::reconnectAfter(uint32_t reconnectTime) {
-    webSocket.setReconnectInterval(reconnectTime);
-}
-
-void ServerConnection::registerEventCallback() {
     std::function<void(WStype_t, uint8_t *, size_t)> f = [this](WStype_t type, uint8_t *payload, size_t length) {
         this->webSocketEventHandler(type, payload, length);
     };
     webSocket.onEvent(f);
+    webSocket.setReconnectInterval(configuration.reconnectTime);
+}
+
+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");
+        }
+    }
+    wasConnected = isConnected;
+}
+
+void ServerConnection::loop(uint32_t millis) {
+    currentTime = millis;
+    webSocket.loop();
+
+    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();
+    } 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:
-        socketIsConnected = false;
+        didChangeConnectionState(false);
-        Serial.println("[INFO] Socket disconnected.");
         break;
     case WStype_CONNECTED:
-        socketIsConnected = true;
+        didChangeConnectionState(true);
-        webSocket.sendTXT(key);
-        Serial.printf("[INFO] Socket connected to url: %s\n", payload);
-        webSocket.enableHeartbeat(pingInterval, pongTimeout, disconnectTimeoutCount);
         break;
     case WStype_TEXT:
-        sendFailureResponse(SesameEvent::TextReceived);
+        controller->sendServerError(MessageResult::TextReceivedOverSocket);
         break;
     case WStype_BIN:
-        processReceivedBytes(payload, length);
+        controller->handleServerMessage(payload, length);
         break;
     case WStype_PONG:
         break;
     case WStype_PING:
+        break;
     case WStype_ERROR:
     case WStype_FRAGMENT_TEXT_START:
     case WStype_FRAGMENT_BIN_START:
     case WStype_FRAGMENT:
     case WStype_FRAGMENT_FIN:
-        sendFailureResponse(SesameEvent::UnexpectedSocketEvent);
+    Serial.printf("[WARN] Unexpected socket event %d\n", type);
+        controller->sendServerError(MessageResult::UnexpectedSocketEvent);
         break;
     }
 }
 
-void ServerConnection::processReceivedBytes(uint8_t* payload, size_t length) {
+void ServerConnection::sendResponse(uint8_t* buffer, uint16_t length) {
-    if (length != AUTHENTICATED_MESSAGE_SIZE) {
+    if (socketIsConnected()) {
-        sendFailureResponse(SesameEvent::InvalidMessageData);
+        webSocket.sendBIN(buffer, length);
-        return;
+    } else {
+        Serial.println("Failed to send response, socket not connected.");
     }
-    AuthenticatedMessage* message = (AuthenticatedMessage*) payload;
-    if (messageCallback == NULL) {
-        sendFailureResponse(SesameEvent::MessageAuthenticationFailed);
-        return;
-    }
-    AuthenticatedMessage responseMessage;
-    SesameEvent event = messageCallback(message, &responseMessage);
-    sendResponse(event, &responseMessage);
-}
-
-void ServerConnection::sendFailureResponse(SesameEvent event) {
-    uint8_t response = static_cast<uint8_t>(event);
-    webSocket.sendBIN(&response, 1);
-    Serial.printf("[INFO] Socket failure %d\n", response);
-}
-
-void ServerConnection::sendResponse(SesameEvent event, AuthenticatedMessage* message) {
-    uint8_t response[AUTHENTICATED_MESSAGE_SIZE+1];
-    response[0] = static_cast<uint8_t>(event);
-    memcpy(response+1, (uint8_t*) message, AUTHENTICATED_MESSAGE_SIZE);
-    webSocket.sendBIN(response, AUTHENTICATED_MESSAGE_SIZE+1);
-    Serial.printf("[INFO] Socket response %d\n", response[0]);
 }

src/servo.cpp

@@ -2,25 +2,39 @@
 
 #include <esp32-hal.h> // For `millis()`
 
-ServoController::ServoController(int timer, int frequency, int pin) 
+void performReset(void * pvParameters) {
-    : timer(timer), frequency(frequency), pin(pin) {
+    ServoController* servo = (ServoController *) pvParameters;
-    
+    for(;;){
+        servo->loop(millis());
+        delay(50);
+    }
 }
 
-void ServoController::configure(uint32_t openDuration, int pressedValue, int releasedValue) {
+ServoController::ServoController(ServoConfiguration configuration) {
-    this->openDuration = openDuration;
+
-    this->pressedValue = pressedValue;
+    // Create a task that runs on a different core,
-    this->releasedValue = releasedValue;
+    // So that it's always executed
-    ESP32PWM::allocateTimer(timer);
+    xTaskCreatePinnedToCore(
-    servo.setPeriodHertz(frequency);
+                    performReset,   /* Task function. */
-    servo.attach(pin);
+                    "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 */
+
+
+    openDuration = configuration.openDuration;
+    pressedValue = configuration.pressedValue;
+    releasedValue = configuration.releasedValue;
+    ESP32PWM::allocateTimer(configuration.pwmTimer);
+    servo.setPeriodHertz(configuration.pwmFrequency);
+    servo.attach(configuration.pin);
     releaseButton();
 }
 
 void ServoController::pressButton() {
-    servo.write(pressedValue);
+    shouldPressButton = true;
-    buttonIsPressed = true;
-    openingEndTime = millis() + openDuration;
 }
 
 void ServoController::releaseButton() {
@@ -28,8 +42,13 @@ void ServoController::releaseButton() {
     buttonIsPressed = false;
 }
 
-void ServoController::loop() {
+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();
     }
 }