diff --git a/src/HTTPConnection.cpp b/src/HTTPConnection.cpp
index efad8bc..e449593 100644
--- a/src/HTTPConnection.cpp
+++ b/src/HTTPConnection.cpp
@@ -26,23 +26,28 @@ HTTPConnection::~HTTPConnection() {
 }
 
 /**
- * Initializes the connection from a server socket.
+ * Initializes the connection
+ */
+void HTTPConnection::initialize(int serverSocketID, HTTPHeaders *defaultHeaders) {
+  _defaultHeaders = defaultHeaders;
+  _serverSocket = serverSocketID;
+}
+
+/**
+ * Accepts the connection from a server socket.
  *
  * The call WILL BLOCK if accept(serverSocketID) blocks. So use select() to check for that in advance.
  */
-int HTTPConnection::initialize(int serverSocketID, HTTPHeaders *defaultHeaders) {
+int HTTPConnection::initialAccept() {
   if (_connectionState == STATE_UNDEFINED) {
-    _defaultHeaders = defaultHeaders;
-    _socket = accept(serverSocketID, (struct sockaddr * )&_sockAddr, &_addrLen);
+    _socket = accept(_serverSocket, (struct sockaddr * )&_sockAddr, &_addrLen);
 
-    // Build up SSL Connection context if the socket has been created successfully
     if (_socket >= 0) {
       HTTPS_LOGI("New connection. Socket FID=%d", _socket);
-      _connectionState = STATE_INITIAL;
+      _connectionState = STATE_ACCEPTED;
       _httpHeaders = new HTTPHeaders();
       refreshTimeout();
       return _socket;
-
     }
      
     HTTPS_LOGE("Could not accept() new connection");
@@ -58,6 +63,23 @@ int HTTPConnection::initialize(int serverSocketID, HTTPHeaders *defaultHeaders)
   return -1;
 }
 
+int HTTPConnection::fullyAccept() {
+  if (_connectionState == STATE_UNDEFINED) {
+    initialAccept();
+  }
+  if (_connectionState == STATE_ACCEPTED) {
+    _connectionState = STATE_INITIAL;
+    return _socket;
+  }
+  return -1;
+}
+
+/**
+ *  Get connection socket
+ */
+int HTTPConnection::getSocket() {
+  return _socket;
+}
 
 /**
  * True if the connection is timed out.
@@ -68,6 +90,17 @@ bool HTTPConnection::isTimeoutExceeded() {
   return _lastTransmissionTS + HTTPS_CONNECTION_TIMEOUT < millis();
 }
 
+/**
+ * Return remaining milliseconds until timeout
+ * 
+ * (Should return 0 or negative value if connection is timed-out or closed)
+ */
+long int HTTPConnection::remainingMsUntilTimeout() {
+  if (isClosed()) return -1;
+  unsigned long remain = _lastTransmissionTS + HTTPS_CONNECTION_TIMEOUT - millis();
+  return (long int)remain;
+}
+
 /**
  * Resets the timeout to allow again the full HTTPS_CONNECTION_TIMEOUT milliseconds
  */
@@ -89,6 +122,14 @@ bool HTTPConnection::isError() {
   return (_connectionState == STATE_ERROR);
 }
 
+bool HTTPConnection::isIdle() {
+  if (_connectionState == STATE_INITIAL) {
+   uint32_t delta = millis() - _lastTransmissionTS;
+   return (int32_t)delta > HTTPS_CONNECTION_IDLE_TIMEOUT;
+  }
+  return false;
+}
+
 bool HTTPConnection::isSecure() {
   return false;
 }
@@ -129,6 +170,7 @@ void HTTPConnection::closeConnection() {
   if (_wsHandler != nullptr) {
     HTTPS_LOGD("Free WS Handler");
     delete _wsHandler;
+    _wsHandler = NULL;
   }
 }
 
@@ -258,20 +300,19 @@ size_t HTTPConnection::readBytesToBuffer(byte* buffer, size_t length) {
   return recv(_socket, buffer, length, MSG_WAITALL | MSG_DONTWAIT);
 }
 
-void HTTPConnection::serverError() {
+void HTTPConnection::raiseError(uint16_t code, std::string reason) {
   _connectionState = STATE_ERROR;
-
-  char staticResponse[] = "HTTP/1.1 500 Internal Server Error\r\nServer: esp32https\r\nConnection:close\r\nContent-Type: text/html\r\nContent-Length:34\r\n\r\n<h1>500 Internal Server Error</h1>";
-  writeBuffer((byte*)staticResponse, strlen(staticResponse));
-  closeConnection();
-}
-
-
-void HTTPConnection::clientError() {
-  _connectionState = STATE_ERROR;
-
-  char staticResponse[] = "HTTP/1.1 400 Bad Request\r\nServer: esp32https\r\nConnection:close\r\nContent-Type: text/html\r\nContent-Length:26\r\n\r\n<h1>400 Bad Request</h1>";
-  writeBuffer((byte*)staticResponse, strlen(staticResponse));
+  std::string sCode = intToString(code);
+
+  char headers[] = "\r\nConnection: close\r\nContent-Type: text/plain;charset=utf8\r\n\r\n";
+  writeBuffer((byte*)"HTTP/1.1 ", 9);
+  writeBuffer((byte*)sCode.c_str(), sCode.length());
+  writeBuffer((byte*)" ", 1);
+  writeBuffer((byte*)(reason.c_str()), reason.length());
+  writeBuffer((byte*)headers, strlen(headers));
+  writeBuffer((byte*)sCode.c_str(), sCode.length());
+  writeBuffer((byte*)" ", 1);
+  writeBuffer((byte*)(reason.c_str()), reason.length());
   closeConnection();
 }
 
@@ -289,7 +330,7 @@ void HTTPConnection::readLine(int lengthLimit) {
         } else {
           // Line has not been terminated by \r\n
           HTTPS_LOGW("Line without \\r\\n (got only \\r). FID=%d", _socket);
-          clientError();
+          raiseError(400, "Bad Request");
           return;
         }
       }
@@ -301,7 +342,7 @@ void HTTPConnection::readLine(int lengthLimit) {
     // Check that the max request string size is not exceeded
     if (_parserLine.text.length() > lengthLimit) {
       HTTPS_LOGW("Header length exceeded. FID=%d", _socket);
-      serverError();
+      raiseError(431, "Request Header Fields Too Large");
       return;
     }
   }
@@ -319,7 +360,7 @@ void HTTPConnection::signalClientClose() {
  */
 void HTTPConnection::signalRequestError() {
   // TODO: Check that no response has been transmitted yet
-  serverError();
+  raiseError(400, "Bad Request");
 }
 
 /**
@@ -365,7 +406,7 @@ bool HTTPConnection::loop() {
         size_t spaceAfterMethodIdx = _parserLine.text.find(' ');
         if (spaceAfterMethodIdx == std::string::npos) {
           HTTPS_LOGW("Missing space after method");
-          clientError();
+          raiseError(400, "Bad Request");
           break;
         }
         _httpMethod = _parserLine.text.substr(0, spaceAfterMethodIdx);
@@ -374,14 +415,14 @@ bool HTTPConnection::loop() {
         size_t spaceAfterResourceIdx = _parserLine.text.find(' ', spaceAfterMethodIdx + 1);
         if (spaceAfterResourceIdx == std::string::npos) {
           HTTPS_LOGW("Missing space after resource");
-          clientError();
+          raiseError(400, "Bad Request");
           break;
         }
         _httpResource = _parserLine.text.substr(spaceAfterMethodIdx + 1, spaceAfterResourceIdx - _httpMethod.length() - 1);
 
         _parserLine.parsingFinished = false;
         _parserLine.text = "";
-        HTTPS_LOGI("Request: %s %s (FID=%d)", _httpMethod.c_str(), _httpResource.c_str(), _socket);
+        HTTPS_LOGI("Request: %s %s (FID=%d, T=%p)", _httpMethod.c_str(), _httpResource.c_str(), _socket, xTaskGetCurrentTaskHandle());
         _connectionState = STATE_REQUEST_FINISHED;
       }
 
@@ -411,7 +452,7 @@ bool HTTPConnection::loop() {
               HTTPS_LOGD("Header: %s = %s (FID=%d)", _parserLine.text.substr(0, idxColon).c_str(), _parserLine.text.substr(idxColon+2).c_str(), _socket);
             } else {
               HTTPS_LOGW("Malformed request header: %s", _parserLine.text.c_str());
-              clientError();
+              raiseError(400, "Bad Request");
               break;
             }
           }
@@ -558,7 +599,7 @@ bool HTTPConnection::loop() {
         } else {
           // No match (no default route configured, nothing does match)
           HTTPS_LOGW("Could not find a matching resource");
-          serverError();
+          raiseError(404, "Not Found");
         }
 
       }
@@ -594,7 +635,6 @@ bool HTTPConnection::loop() {
   return (!isClosed() && ((_bufferProcessed < _bufferUnusedIdx) || canReadData()));
 }
 
-
 bool HTTPConnection::checkWebsocket() {
   if(_httpMethod == "GET" &&
      !_httpHeaders->getValue("Host").empty() &&
diff --git a/src/HTTPConnection.hpp b/src/HTTPConnection.hpp
index 3c33e8c..7de2f42 100644
--- a/src/HTTPConnection.hpp
+++ b/src/HTTPConnection.hpp
@@ -39,13 +39,18 @@ class HTTPConnection : private ConnectionContext {
   HTTPConnection(ResourceResolver * resResolver);
   virtual ~HTTPConnection();
 
-  virtual int initialize(int serverSocketID, HTTPHeaders *defaultHeaders);
+  virtual void initialize(int serverSocketID, HTTPHeaders *defaultHeaders);
+  virtual int initialAccept();
+  virtual int fullyAccept();
   virtual void closeConnection();
   virtual bool isSecure();
 
   bool loop();
   bool isClosed();
   bool isError();
+  bool isIdle();
+  long int remainingMsUntilTimeout();
+  int getSocket();
 
 protected:
   friend class HTTPRequest;
@@ -57,6 +62,9 @@ class HTTPConnection : private ConnectionContext {
   virtual bool canReadData();
   virtual size_t pendingByteCount();
 
+  // Connection socket (LWIP)
+  int _socket;
+
   // Timestamp of the last transmission action
   unsigned long _lastTransmissionTS;
 
@@ -82,6 +90,8 @@ class HTTPConnection : private ConnectionContext {
 
     // The connection has not been established yet
     STATE_UNDEFINED,
+    // The connection fully established (i.e. TLS)
+    STATE_ACCEPTED,
     // The connection has just been created
     STATE_INITIAL,
     // The request line has been parsed
@@ -107,8 +117,7 @@ class HTTPConnection : private ConnectionContext {
   } _clientState;
 
 private:
-  void serverError();
-  void clientError();
+  void raiseError(uint16_t code, std::string reason);
   void readLine(int lengthLimit);
 
   bool isTimeoutExceeded();
@@ -134,8 +143,8 @@ class HTTPConnection : private ConnectionContext {
   // Socket address, length etc for the connection
   struct sockaddr _sockAddr;
   socklen_t _addrLen;
-  int _socket;
-
+  int _serverSocket;
+  
   // Resource resolver used to resolve resources
   ResourceResolver * _resResolver;
 
@@ -158,7 +167,6 @@ class HTTPConnection : private ConnectionContext {
 
   //Websocket connection
   WebsocketHandler * _wsHandler;
-
 };
 
 void handleWebsocketHandshake(HTTPRequest * req, HTTPResponse * res);
diff --git a/src/HTTPSConnection.cpp b/src/HTTPSConnection.cpp
index 3385f79..7ca92ce 100644
--- a/src/HTTPSConnection.cpp
+++ b/src/HTTPSConnection.cpp
@@ -5,6 +5,7 @@ namespace httpsserver {
 
 HTTPSConnection::HTTPSConnection(ResourceResolver * resResolver):
   HTTPConnection(resResolver) {
+  _sslCtx = NULL;
   _ssl = NULL;
   _TLSTickets = NULL;
 }
@@ -19,19 +20,33 @@ bool HTTPSConnection::isSecure() {
 }
 
 /**
- * Initializes the connection from a server socket.
+ * Initializes the connection with SSL context
+ */
+void HTTPSConnection::initialize(int serverSocketID, HTTPHeaders *defaultHeaders, SSL_CTX * sslCtx, TLSTickets * tickets) {
+  HTTPConnection::initialize(serverSocketID, defaultHeaders);
+  _sslCtx = sslCtx;
+  _TLSTickets = tickets;
+}
+
+/**
+ * Accepts the connection from a server socket.
  *
  * The call WILL BLOCK if accept(serverSocketID) blocks. So use select() to check for that in advance.
  */
-int HTTPSConnection::initialize(int serverSocketID, SSL_CTX * sslCtx, HTTPHeaders *defaultHeaders) {
+int HTTPSConnection::fullyAccept() {
+
   if (_connectionState == STATE_UNDEFINED) {
-    // Let the base class connect the plain tcp socket
-    int resSocket = HTTPConnection::initialize(serverSocketID, defaultHeaders);
+    initialAccept();
+  }
+  
+  if (_connectionState == STATE_ACCEPTED) {
+    int resSocket = _socket;
 
     // Build up SSL Connection context if the socket has been created successfully
     if (resSocket >= 0) {
+      HTTPS_LOGV("Before SSL accept free:%u, lfb:%u\n", heap_caps_get_free_size(MALLOC_CAP_8BIT), heap_caps_get_largest_free_block(MALLOC_CAP_8BIT));
+      _ssl = SSL_new(_sslCtx);
 
-      _ssl = SSL_new(sslCtx);
       if (_TLSTickets != NULL) _TLSTickets->enable(_ssl);
 
       if (_ssl) {
@@ -42,9 +57,13 @@ int HTTPSConnection::initialize(int serverSocketID, SSL_CTX * sslCtx, HTTPHeader
           // Perform the handshake
           success = SSL_accept(_ssl);
           if (success) {
+            HTTPS_LOGD("SSL accepted (FID=%d)", resSocket);
+            HTTPS_LOGV("After SSL accept free:%u, lfb:%u", heap_caps_get_free_size(MALLOC_CAP_8BIT), heap_caps_get_largest_free_block(MALLOC_CAP_8BIT));
+            _connectionState = STATE_INITIAL;
             return resSocket;
           } else {
             HTTPS_LOGE("SSL_accept failed. Aborting handshake. FID=%d", resSocket);
+            HTTPS_LOGV("After fail free:%u, lfb:%u", heap_caps_get_free_size(MALLOC_CAP_8BIT), heap_caps_get_largest_free_block(MALLOC_CAP_8BIT));
           }
         } else {
           HTTPS_LOGE("SSL_set_fd failed. Aborting handshake. FID=%d", resSocket);
diff --git a/src/HTTPSConnection.hpp b/src/HTTPSConnection.hpp
index 7289c13..fcc471d 100644
--- a/src/HTTPSConnection.hpp
+++ b/src/HTTPSConnection.hpp
@@ -35,7 +35,8 @@ class HTTPSConnection : public HTTPConnection {
   HTTPSConnection(ResourceResolver * resResolver);
   virtual ~HTTPSConnection();
 
-  virtual int initialize(int serverSocketID, SSL_CTX * sslCtx, HTTPHeaders *defaultHeaders);
+  virtual void initialize(int serverSocketID, HTTPHeaders *defaultHeaders, SSL_CTX * sslCtx, TLSTickets * tickets);
+  virtual int fullyAccept() override; 
   virtual void closeConnection();
   virtual bool isSecure();
 
@@ -50,6 +51,7 @@ class HTTPSConnection : public HTTPConnection {
 
 private:
   // SSL context for this connection
+  SSL_CTX * _sslCtx;
   SSL * _ssl;
 	TLSTickets * _TLSTickets;
 
diff --git a/src/HTTPSServer.cpp b/src/HTTPSServer.cpp
index 85cf6c2..5881ae2 100644
--- a/src/HTTPSServer.cpp
+++ b/src/HTTPSServer.cpp
@@ -59,10 +59,10 @@ void HTTPSServer::teardownSocket() {
   _sslctx = NULL;
 }
 
-int HTTPSServer::createConnection(int idx) {
+HTTPSConnection * HTTPSServer::createConnection() {
   HTTPSConnection * newConnection = new HTTPSConnection(this);
-  _connections[idx] = newConnection;
-  return newConnection->initialize(_socket, _sslctx, &_defaultHeaders);
+  newConnection->initialize(_socket, &_defaultHeaders, _sslctx, _TLSTickets);
+  return newConnection;
 }
 
 /**
diff --git a/src/HTTPSServer.hpp b/src/HTTPSServer.hpp
index 6b99864..29459c0 100644
--- a/src/HTTPSServer.hpp
+++ b/src/HTTPSServer.hpp
@@ -36,6 +36,8 @@ class HTTPSServer : public HTTPServer {
   // RFC 5077 TLS session tickets
   void enableTLSTickets(uint32_t liftimeSeconds = 86400, bool useHardwareRNG = false);
 
+  virtual HTTPSConnection * createConnection() override;
+
 private:
   // Static configuration. Port, keys, etc. ====================
   // Certificate that should be used (includes private key)
@@ -51,9 +53,6 @@ class HTTPSServer : public HTTPServer {
   virtual void teardownSocket();
   uint8_t setupSSLCTX();
   uint8_t setupCert();
-
-  // Helper functions
-  virtual int createConnection(int idx);
 };
 
 } /* namespace httpsserver */
diff --git a/src/HTTPSServerConstants.hpp b/src/HTTPSServerConstants.hpp
index 3e82429..ca56853 100644
--- a/src/HTTPSServerConstants.hpp
+++ b/src/HTTPSServerConstants.hpp
@@ -7,6 +7,7 @@
 // 2: Error + Warn
 // 3: Error + Warn + Info
 // 4: Error + Warn + Info + Debug
+// 5: Error + Warn + Info + Debug + Verbose
 
 #ifndef HTTPS_LOGLEVEL
   #define HTTPS_LOGLEVEL 3
@@ -82,6 +83,12 @@
 #define HTTPS_CONNECTION_TIMEOUT               20000
 #endif
 
+// Timeout for connection in STATE_INITIAL to be considered idle.
+// When connection is idle, server may close it and switch to pending connection
+#ifndef HTTPS_CONNECTION_IDLE_TIMEOUT          
+#define HTTPS_CONNECTION_IDLE_TIMEOUT          500
+#endif 
+
 // Timeout used to wait for shutdown of SSL connection (ms)
 // (time for the client to return notify close flag) - without it, truncation attacks might be possible
 #ifndef HTTPS_SHUTDOWN_TIMEOUT
@@ -93,4 +100,13 @@
 #define HTTPS_SHA1_LENGTH                      20
 #endif
 
+// Default values for workers. 
+// Stack size should not be less than 4096 for TLS connections
+#ifndef HTTPS_CONN_TASK_STACK_SIZE
+#define HTTPS_CONN_TASK_STACK_SIZE             4096
+#endif
+#ifndef HTTPS_CONN_TASK_PRIORITY
+#define HTTPS_CONN_TASK_PRIORITY               (tskIDLE_PRIORITY + 1)
+#endif
+
 #endif /* SRC_HTTPSSERVERCONSTANTS_HPP_ */
diff --git a/src/HTTPServer.cpp b/src/HTTPServer.cpp
index d11bf6b..d74c2b2 100644
--- a/src/HTTPServer.cpp
+++ b/src/HTTPServer.cpp
@@ -8,12 +8,15 @@ HTTPServer::HTTPServer(const uint16_t port, const uint8_t maxConnections, const
   _maxConnections(maxConnections),
   _bindAddress(bindAddress) {
 
-  // Create space for the connections
-  _connections = new HTTPConnection*[maxConnections];
-  for(uint8_t i = 0; i < maxConnections; i++) _connections[i] = NULL;
+  _connections = NULL;
+  _workers = NULL;
 
   // Configure runtime data
   _socket = -1;
+  _selectMutex = xSemaphoreCreateMutex();
+  _numWorkers = 0;
+  _workQueue = NULL;
+  _pendingConnection = NULL;
   _running = false;
 }
 
@@ -25,8 +28,28 @@ HTTPServer::~HTTPServer() {
     stop();
   }
 
-  // Delete connection pointers
-  delete[] _connections;
+  // Delete allocated memory
+  if (_workers) delete[] _workers;
+  if (_connections) delete[] _connections;
+  if (_connectionMutex) delete[] _connectionMutex;
+  if (_selectMutex) vSemaphoreDelete(_selectMutex);
+  if (_workQueue) vQueueDelete(_workQueue);
+}
+
+/**
+ *  Enables workers, each running in separate FreeRTOS task
+ */
+void HTTPServer::enableWorkers(uint8_t numWorkers, size_t stackSize, int priority) {
+  if (!_running && (numWorkers > 0)) {
+    _numWorkers = numWorkers;
+    if (_workers == NULL) {
+      _workers = new HTTPWorker * [numWorkers];
+      HTTPS_LOGD("Creating %d worker(s) (%u,%d)", numWorkers, stackSize, priority);
+      for(uint8_t i = 0; i < numWorkers; i++) {
+        _workers[i] = new HTTPWorker(this, stackSize, priority);
+      }
+    }
+  }
 }
 
 /**
@@ -35,7 +58,23 @@ HTTPServer::~HTTPServer() {
 uint8_t HTTPServer::start() {
   if (!_running) {
     if (setupSocket()) {
+      // Create space for the connections if not using worker tasks
+      if (!_workQueue) {
+        _workQueue = xQueueCreate(2 * _maxConnections, sizeof(int8_t));
+      }
+      if (!_connections) {
+        _connections = new HTTPConnection*[_maxConnections];
+        for(uint8_t i = 0; i < _maxConnections; i++) _connections[i] = NULL;
+      }
+      if (!_connectionMutex) {
+        _connectionMutex = new SemaphoreHandle_t[_maxConnections];
+        for(uint8_t i = 0; i < _maxConnections; i++) _connectionMutex[i] = xSemaphoreCreateMutex();
+      }
       _running = true;
+      // start the workers
+      if (_numWorkers > 0) {
+        for(uint8_t i = 0; i < _numWorkers; i++) _workers[i]->start();
+      }
       return 1;
     }
     return 0;
@@ -56,29 +95,59 @@ void HTTPServer::stop() {
   if (_running) {
     // Set the flag that the server is stopped
     _running = false;
-
-    // Clean up the connections
-    bool hasOpenConnections = true;
-    while(hasOpenConnections) {
-      hasOpenConnections = false;
-      for(int i = 0; i < _maxConnections; i++) {
-        if (_connections[i] != NULL) {
-          _connections[i]->closeConnection();
-
-          // Check if closing succeeded. If not, we need to call the close function multiple times
-          // and wait for the client
-          if (_connections[i]->isClosed()) {
-            delete _connections[i];
-          } else {
-            hasOpenConnections = true;
+    xSemaphoreTake(_selectMutex, portMAX_DELAY); // We won't be releasing this
+
+    if (_connections) {
+      // Clean up the connections    
+      bool hasOpenConnections = true;
+      while(hasOpenConnections) {
+        hasOpenConnections = false;
+        for(int i = 0; i < _maxConnections; i++) {
+          xSemaphoreTake(_connectionMutex[i], portMAX_DELAY);
+          if (_connections[i] != NULL) {
+            _connections[i]->closeConnection();
+
+            // Check if closing succeeded. If not, we need to call the close function multiple times
+            // and wait for the client
+            if (_connections[i]->isClosed()) {
+              delete _connections[i];
+              _connections[i] = NULL;
+            } else {
+              hasOpenConnections = true;
+            }
           }
+          xSemaphoreGive(_connectionMutex[i]);
+          vSemaphoreDelete(_connectionMutex[i]);
         }
+        delay(1);
       }
-      delay(1);
-    }
+    } // if (_connections)
 
     teardownSocket();
 
+    // Server _running is false, workers should terminate themselves...
+    if (_workers) {
+      // Just give them invalid connection number if they are blocked on the work queue
+      int8_t noWork = -1;
+      for(int i = 0; i < _numWorkers; i++) xQueueSend(_workQueue, &noWork, 0);
+      bool workerStillActive = false;
+      do {
+        for(int i = 0; i < _maxConnections; i++) {          
+          if (_workers[i] != NULL) {
+            if (_workers[i]->isRunning()) {
+              workerStillActive = true;
+            } else {
+              delete _workers[i];
+              _workers[i] = NULL;
+            }
+          }
+        }
+        if (workerStillActive) vTaskDelay(1);
+      } while (workerStillActive);
+      delete _workers;
+      _workers = NULL;
+    } // if (_workers)
+
   }
 }
 
@@ -92,75 +161,235 @@ void HTTPServer::setDefaultHeader(std::string name, std::string value) {
 }
 
 /**
- * The loop method can either be called by periodical interrupt or in the main loop and handles processing
- * of data
+ * Manages server connections 
+ *  - Cleans up closed connections
+ *  - Queues work if there is new data on existing connection
+ *  - Checks for new connections
+ *  - Accepts pending connections when there is space in connection pool
+ *  - Closes idle connections when there are pending ones
+ *  
+ *  Returns after all needed work is done or maxTimeoutMs expires
  */
-void HTTPServer::loop() {
-
-  // Only handle requests if the server is still running
-  if(!_running) return;
+void HTTPServer::manageConnections(int maxTimeoutMs) {
+  fd_set readFDs, exceptFDs, timeoutFDs;
+  FD_ZERO(&readFDs);
+  FD_ZERO(&exceptFDs);
+  FD_ZERO(&timeoutFDs);
+  int maxSocket = -1;
+
+  // The idea here is to block on something (up to maxTimeoutMs) until 
+  // there is new data or new connection, so work can be queue up
+
+  // Add only the server socket or the pending connection socket
+  // as trying to select on the server socket while we know that
+  // there is pending connection will return imediatelly
+  if (_pendingConnection) {
+    // If there is pending connection and we have not yet received data
+    if (!_lookForIdleConnection) {
+      int pendingSocket = _pendingConnection->getSocket();
+      FD_SET(pendingSocket, &readFDs);
+      FD_SET(pendingSocket, &exceptFDs);
+      maxSocket = pendingSocket;
+    }
+  } else {
+    // No pending connections (that we know of), monitor server socket
+    FD_SET(_socket, &readFDs);
+    FD_SET(_socket, &exceptFDs);
+    maxSocket = _socket;
+  }
 
-  // Step 1: Process existing connections
-  // Process open connections and store the index of a free connection
-  // (we might use that later on)
-  int freeConnectionIdx = -1;
+  // Cleanup closed connections and find minimal select timeout
+  // Add active connections to select sets
+  int minRemain = maxTimeoutMs;
   for (int i = 0; i < _maxConnections; i++) {
-    // Fetch a free index in the pointer array
-    if (_connections[i] == NULL) {
-      freeConnectionIdx = i;
+    if (_connections[i] != NULL) {
+      // Try to lock connection.
+      if (xSemaphoreTake(_connectionMutex[i], 0)) {
+        // If we suceeded, connection is currently not beening worked by other task
+        int fd = _connections[i]->getSocket();
+        if (_connections[i]->isClosed()) {
+          // if it's closed clean up:
+          HTTPS_LOGV("Deleted connection[%d], FID=%d", i, fd);
+          delete _connections[i];
+          _connections[i] = NULL;
+           // We released one connection slot, don't look for idle connection
+          _lookForIdleConnection = false;
+          fd = -1;
+        }
+        if (fd > 0) {
+          int remain = _connections[i]->remainingMsUntilTimeout();
+          if (_lookForIdleConnection) {
+            // There is partially accepted pending connection, check for idle connections
+            if ((remain < 1) || _connections[i]->isIdle()) {
+              HTTPS_LOGI("Closing IDLE connection[%d] FID=%d to accept FID=%d", i, fd, _pendingConnection->getSocket());
+              _connections[i]->closeConnection();
+              // We closed one connection, don't look for more idle connections
+              _lookForIdleConnection = false;              
+              fd = _connections[i]->getSocket();
+            } else { 
+              remain = min(remain, (int)HTTPS_CONNECTION_IDLE_TIMEOUT);
+            }
+          }
+          if (fd > 0) {
+            // Add the connection to select sets
+            if (remain < 1) FD_SET(fd, &timeoutFDs);
+            FD_SET(fd, &readFDs);
+            FD_SET(fd, &exceptFDs);
+            if (fd > maxSocket) maxSocket = fd;      
+          } else {
+            remain = 0; // Force imediate rescan
+          }
+          if (remain < minRemain) minRemain = remain;
+        }
+        xSemaphoreGive(_connectionMutex[i]);        
+      }
+    }
+  }
 
-    } else {
-      // if there is a connection (_connections[i]!=NULL), check if its open or closed:
-      if (_connections[i]->isClosed()) {
-        // if it's closed, clean up:
-        delete _connections[i];
-        _connections[i] = NULL;
-        freeConnectionIdx = i;
-      } else {
-        // if not, process it:
-        _connections[i]->loop();
+  // Select on socket sets with minRemain (ms) timeout
+  if (minRemain < 0) minRemain = 0;
+  timeval _timeout;
+  _timeout.tv_sec  = minRemain / 1000;
+  _timeout.tv_usec = (minRemain - _timeout.tv_sec * 1000) * 1000;
+  select(maxSocket + 1, &readFDs, NULL, &exceptFDs, &_timeout);
+
+  // if FD_ISSET(serverSocket, &except_fds) {} // server is stopping ?
+
+  // Assign work for connections that have data, error or timeout
+  // and find empty connection slot 
+  int8_t freeIndex = -1;
+  for (int8_t i = 0; i < _maxConnections; i++) {
+    if (_connections[i] != NULL) {
+      int fd = _connections[i]->getSocket();
+      if ((fd < 1) || (FD_ISSET(fd, &readFDs)) || (FD_ISSET(fd, &exceptFDs)) || (FD_ISSET(fd, &timeoutFDs))) {
+        xQueueSend(_workQueue, &i, 0);
+        HTTPS_LOGV("Queued work for connection[%d], FID=%d", i, fd);
       }
+    } else {
+      freeIndex = i;
     }
   }
- 
-  // Step 2: Check for new connections
-  // This makes only sense if there is space to store the connection
-  if (freeConnectionIdx > -1) {
-
-    // We create a file descriptor set to be able to use the select function
-    fd_set sockfds;
-    // Out socket is the only socket in this set
-    FD_ZERO(&sockfds);
-    FD_SET(_socket, &sockfds);
-
-    // We define a "immediate" timeout
-    timeval timeout;
-    timeout.tv_sec  = 0;
-    timeout.tv_usec = 0; // Return immediately, if possible
-
-    // Wait for input
-    // As by 2017-12-14, it seems that FD_SETSIZE is defined as 0x40, but socket IDs now
-    // start at 0x1000, so we need to use _socket+1 here
-    select(_socket + 1, &sockfds, NULL, NULL, &timeout);
-
-    // There is input
-    if (FD_ISSET(_socket, &sockfds)) {
-      int socketIdentifier = createConnection(freeConnectionIdx);
-
-      // If initializing did not work, discard the new socket immediately
-      if (socketIdentifier < 0) {
-        delete _connections[freeConnectionIdx];
-        _connections[freeConnectionIdx] = NULL;
+
+  // If we have known pending connection ...
+  if (_pendingConnection) {
+    int pendingSocket = _pendingConnection->getSocket();
+    // ... and if it is talking to us (client speaks first for both HTTP and TLS) ...
+    if (_lookForIdleConnection || (FD_ISSET(pendingSocket, &readFDs))) {
+      // ... and if we have space to fully accept ...
+      if (freeIndex >= 0) {
+        // ... try to fully accept the connection.
+        if (_pendingConnection->fullyAccept() > 0) {
+          // Fully accepted, add to active connections
+          HTTPS_LOGV("Accepted connection[%d], FID=%d", freeIndex, pendingSocket);
+          _connections[freeIndex] = _pendingConnection;
+          xQueueSend(_workQueue, &freeIndex, 0);
+        } else {
+          HTTPS_LOGD("Discarded connection FID=%d", pendingSocket);
+          delete _pendingConnection;
+        }
+        _pendingConnection = NULL;
+        _lookForIdleConnection = false;
+      } else {
+        // Pending connection has data to read but we currently 
+        // have no space in connection pool... set flag to try 
+        // to close one of the idle connections...
+        _lookForIdleConnection = true;
+      }    
+    }
+  } else {
+    // No pending connection, see if we have new one on the server socket
+    if (FD_ISSET(_socket, &readFDs)) {
+      // Try to initially accept the new connection
+      HTTPConnection * connection = createConnection();
+      int newSocket = (connection) ? connection->initialAccept() : -1;
+      if (newSocket > 0) {
+        // Initial accept succeded, do we have space in the pool
+        if (freeIndex >= 0) {
+          if (connection->fullyAccept() > 0) {
+            _connections[freeIndex] = connection;
+            xQueueSend(_workQueue, &freeIndex, 0);
+            HTTPS_LOGV("Accepted pending connection[%d], FID=%d", freeIndex, newSocket);
+          } else {
+            HTTPS_LOGD("Discarded pending connection, FID=%d", newSocket);
+            delete connection;
+          }
+        } else {
+          // No space in the connection pool, keep it as pending connection
+          // until it actually sends data (HTTP request or TLS 'hello')
+          HTTPS_LOGV("Connection is pending, FID=%d", newSocket);
+          _pendingConnection = connection;
+        }
+      } else {
+        // Discard new connection imediatelly
+        delete connection;
       }
     }
+  } // if/else (_pendingConnection)
+
+} // manageConnections()
 
+/**
+ * Pick up item (connection index) from work queue and 
+ * call connection's loop method to consume the data on the socket
+ * 
+ * Returns false if there was no work in the queue and timeout expired
+ */
+bool HTTPServer::doQueuedWork(TickType_t waitDelay) {
+  int8_t connIndex = -1;
+  if (xQueueReceive(_workQueue, &connIndex, waitDelay)) {
+    if ((connIndex >= 0) && xSemaphoreTake(_connectionMutex[connIndex], portMAX_DELAY)) {
+      HTTPConnection * connection = _connections[connIndex];
+      // Work the connection until it runs out of data
+      while (connection && connection->loop()); 
+      xSemaphoreGive(_connectionMutex[connIndex]);
+    } 
+    return true;
+  } 
+  return false;
+}
+
+/**
+ * The loop method handles processing of data and should be called by periodicaly 
+ * from the main loop when there are no workers enabled
+ * 
+ * If timeout (in millisecods) is supplied, it will wait for event 
+ * on the 'server soceket' for new connection and/or on established 
+ * connection sockets for closing/error.
+ * 
+ * Return value is remaining milliseconds if funtion returned early
+ * 
+ * NOTE: When workers are enabled, calling this method periodically
+ *       is not needed and has no effect.
+ */
+int HTTPServer::loop(int timeoutMs) {
+
+  // Only handle requests if the server is still running
+  // and we are handling connections in async mode
+  if (!_running || (_numWorkers > 0)) {
+    delay(timeoutMs);
+    return 0;
   }
+  uint32_t startMs = millis();
+
+  // Step 1: Process existing connections
+  manageConnections(timeoutMs);
+
+  // Step 2: Complete any remaining work (without waiting)
+  while (doQueuedWork(0));
+
+  // Return the remaining time from the timeoutMs requested
+  uint32_t deltaMs = (startMs + timeoutMs - millis());
+  if (deltaMs > 0x7FFFFFFF) deltaMs = 0;
+  return deltaMs;
 }
 
-int HTTPServer::createConnection(int idx) {
+/**
+ * Create new connection, initialize headers and return the pointer
+ */
+HTTPConnection * HTTPServer::createConnection() {
   HTTPConnection * newConnection = new HTTPConnection(this);
-  _connections[idx] = newConnection;
-  return newConnection->initialize(_socket, &_defaultHeaders);
+  newConnection->initialize(_socket, &_defaultHeaders);  
+  return newConnection;  
 }
 
 /**
@@ -207,4 +436,8 @@ void HTTPServer::teardownSocket() {
   _socket = -1;
 }
 
+int HTTPServer::serverSocket() {
+  return _socket;
+}
+
 } /* namespace httpsserver */
diff --git a/src/HTTPServer.hpp b/src/HTTPServer.hpp
index 47746c1..73724cf 100644
--- a/src/HTTPServer.hpp
+++ b/src/HTTPServer.hpp
@@ -13,6 +13,10 @@
 #include "lwip/sockets.h"
 #include "lwip/inet.h"
 
+// FreeRTOS 
+#include "freertos/FreeRTOS.h"
+#include "freertos/semphr.h"
+
 // Internal includes
 #include "HTTPSServerConstants.hpp"
 #include "HTTPHeaders.hpp"
@@ -21,6 +25,7 @@
 #include "ResourceResolver.hpp"
 #include "ResolvedResource.hpp"
 #include "HTTPConnection.hpp"
+#include "HTTPWorker.hpp"
 
 namespace httpsserver {
 
@@ -36,11 +41,24 @@ class HTTPServer : public ResourceResolver {
   void stop();
   bool isRunning();
 
-  void loop();
+  // Return value is remaining miliseconds if function returned early
+  int loop(int timeoutMs = 0);
 
   void setDefaultHeader(std::string name, std::string value);
 
+  // Enable separate FreeRTOS tasks handling for connections. 
+  // Must be called before start()
+  void enableWorkers(
+    uint8_t numWorkers = 2, 
+    size_t taskStackSize = HTTPS_CONN_TASK_STACK_SIZE, 
+    int taskPriority = HTTPS_CONN_TASK_PRIORITY
+  );
+
+  HTTPHeaders * getDefaultHeaders();
+  int serverSocket();
+
 protected:
+  friend class HTTPWorker;
   // Static configuration. Port, keys, etc. ====================
   // Certificate that should be used (includes private key)
   const uint16_t _port;
@@ -57,6 +75,18 @@ class HTTPServer : public ResourceResolver {
   boolean _running;
   // The server socket
   int _socket;
+  
+  // Keep state if we have pendig connections
+  HTTPConnection * _pendingConnection = NULL;
+  bool _pendingData = false;
+  bool _lookForIdleConnection = false;
+  
+  // HTTPWorker(s) and syncronization
+  uint8_t _numWorkers = 0;
+  SemaphoreHandle_t _selectMutex = NULL;
+  SemaphoreHandle_t * _connectionMutex = NULL;
+  QueueHandle_t _workQueue = NULL;
+  HTTPWorker ** _workers;
 
   // The server socket address, that our service is bound to
   sockaddr_in _sock_addr;
@@ -67,8 +97,14 @@ class HTTPServer : public ResourceResolver {
   virtual uint8_t setupSocket();
   virtual void teardownSocket();
 
+  // Internal functions
+  void manageConnections(int maxTimeoutMs);
+  bool doQueuedWork(TickType_t waitDelay);
+
   // Helper functions
-  virtual int createConnection(int idx);
+  virtual HTTPConnection * createConnection();
+  //int createConnection(int idx);
+
 };
 
 }
diff --git a/src/HTTPWorker.cpp b/src/HTTPWorker.cpp
new file mode 100644
index 0000000..ccbb70f
--- /dev/null
+++ b/src/HTTPWorker.cpp
@@ -0,0 +1,67 @@
+#include "HTTPWorker.hpp"
+#include "HTTPServer.hpp"
+
+#include "freertos/semphr.h"
+
+namespace httpsserver {
+
+HTTPWorker::HTTPWorker(HTTPServer * server, size_t stackSize, int priority) {
+  _server = server;
+  BaseType_t taskRes = xTaskCreate(static_task, "HTTPSWorker", stackSize, this, priority, &_handle);
+  if (taskRes == pdTRUE) {
+    HTTPS_LOGI("Started connection task %p", _handle);
+  } else {
+    HTTPS_LOGE("Error starting connection task");
+    _running = false;
+  }
+}
+
+bool HTTPWorker::isRunning() {
+  return _running;
+}
+
+void HTTPWorker::run() {
+  // Run while server is running
+  while (_server->isRunning()) {
+
+    if (xSemaphoreTake(_server->_selectMutex, 0) == pdTRUE) {
+      // One worker task will manage the connections 
+      // i.e block on select call
+      HTTPS_LOGV("Task %p managing connections", _handle);
+      _server->manageConnections(HTTPS_CONNECTION_TIMEOUT);
+      xSemaphoreGive(_server->_selectMutex);
+    } else { 
+      // While others should wait for work from the queue
+       HTTPS_LOGV("Task %p waiting for work", _handle);
+      _server->doQueuedWork(portMAX_DELAY);
+    }
+
+    // Then all tasks complete any remaining work (without waiting)
+    while (_server->doQueuedWork(0));
+
+  } // while server->isRunning()
+
+} // HTTPConnectionTask::run;
+
+void HTTPWorker::start() {
+  vTaskResume(_handle);
+}
+
+void HTTPWorker::static_task(void* param) {
+  HTTPWorker * _this = static_cast<HTTPWorker *>(param);
+
+  // Start suspended wait for server to call start() method
+  vTaskSuspend(NULL);  
+
+  // Run the worker 
+  _this->run();
+
+  HTTPS_LOGI("Shutting down worker task %p", _this->_handle);
+  _this->_running = false;
+  // Mark the task for deltetion.
+  // The FreeRTOS idle task has reposnibilty to cleanup structures and memory
+  vTaskDelete(NULL);
+} 
+
+
+} // namespace httpsserver 
diff --git a/src/HTTPWorker.hpp b/src/HTTPWorker.hpp
new file mode 100644
index 0000000..63bc8df
--- /dev/null
+++ b/src/HTTPWorker.hpp
@@ -0,0 +1,45 @@
+#ifndef SRC_HTTPWORKER_HPP_
+#define SRC_HTTPWORKER_HPP_
+
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+#include "HTTPConnection.hpp"
+
+namespace httpsserver {
+
+class HTTPServer; // forward declaration
+
+class HTTPWorker {
+
+public:
+  HTTPWorker(HTTPServer * server, size_t stackSize, int priority);
+  void start();
+  bool isRunning();
+  
+protected:
+  // Create the instance flagged as running
+  // If constructor fails to start the actual task, 
+  // or task has ended due to server shutdown, this will be set to false.
+  bool _running = true;
+
+  // FreeRTOS task handle 
+  TaskHandle_t _handle = NULL;
+
+  // HTTP(S)Server to which this task is attached
+  HTTPServer * _server = NULL;
+
+  /**
+   * Worker (FreeRTOS task) main loop
+   */
+  void run();
+
+  /**
+   * Static method to start the worker in separate FreeRTOS task
+   */
+  static void static_task(void * param);
+};
+
+} // end namespace httpsserver
+
+#endif // SRC_HTTPWORKER_HPP_
\ No newline at end of file
diff --git a/src/ResourceResolver.cpp b/src/ResourceResolver.cpp
index f6f86b2..588b6ad 100644
--- a/src/ResourceResolver.cpp
+++ b/src/ResourceResolver.cpp
@@ -58,8 +58,7 @@ void ResourceResolver::resolveNode(const std::string &method, const std::string
       std::string name  = param.substr(0, nvSplitIdx);
       std::string value = "";
       if (nvSplitIdx != std::string::npos) {
-        // TODO: There may be url encoding in here.
-        value = param.substr(nvSplitIdx+1);
+        value = urlDecode(param.substr(nvSplitIdx+1));
       }
 
       // Now we finally have name and value.
@@ -116,7 +115,7 @@ void ResourceResolver::resolveNode(const std::string &method, const std::string
               // Second step: Grab the parameter value
               if (nodeIdx == nodepath.length()) {
                 // Easy case: parse until end of string
-                params->setUrlParameter(pIdx, resourceName.substr(urlIdx));
+                params->setUrlParameter(pIdx, urlDecode(resourceName.substr(urlIdx)));
               } else {
                 // parse until first char after the placeholder
                 char terminatorChar = nodepath[nodeIdx];
@@ -124,7 +123,7 @@ void ResourceResolver::resolveNode(const std::string &method, const std::string
                 if (terminatorPosition != std::string::npos) {
                   // We actually found the terminator
                   size_t dynamicLength = terminatorPosition-urlIdx;
-                  params->setUrlParameter(pIdx, resourceName.substr(urlIdx, dynamicLength));
+                  params->setUrlParameter(pIdx, urlDecode(resourceName.substr(urlIdx, dynamicLength)));
                   urlIdx = urlIdx + dynamicLength;
                 } else {
                   // We did not find the terminator
diff --git a/src/util.cpp b/src/util.cpp
index 4554f34..275671c 100644
--- a/src/util.cpp
+++ b/src/util.cpp
@@ -44,7 +44,7 @@ std::string intToString(int i) {
     return "0";
   }
   // We need this much digits
-  int digits = ceil(log10(i));
+  int digits = ceil(log10(i+1));
   char c[digits+1];
   c[digits] = '\0';
 
@@ -58,3 +58,34 @@ std::string intToString(int i) {
 }
 
 }
+
+std::string urlDecode(std::string input) {
+  std::size_t idxReplaced = 0;
+  std::size_t idxFound = input.find('%');
+  while (idxFound != std::string::npos) {
+    if (idxFound <= input.length() + 3) {
+      char hex[2] = { input[idxFound+1], input[idxFound+2] };
+      byte val = 0;
+      for(int n = 0; n < sizeof(hex); n++) {
+        val <<= 4;
+        if ('0' <= hex[n] && hex[n] <= '9') {
+          val += hex[n]-'0';
+        }
+        else if ('A' <= hex[n] && hex[n] <= 'F') {
+          val += hex[n]-'A'+10;
+        }
+        else if ('a' <= hex[n] && hex[n] <= 'f') {
+          val += hex[n]-'a'+10;
+        }
+        else {
+          goto skipChar;
+        }
+      }
+      input.replace(idxFound, 3, 1, (char)val);
+    }
+    skipChar:
+    idxReplaced = idxFound + 1;
+    idxFound = input.find('%', idxReplaced);
+  }
+  return input;
+}
diff --git a/src/util.hpp b/src/util.hpp
index 51241e5..07b859d 100644
--- a/src/util.hpp
+++ b/src/util.hpp
@@ -27,4 +27,9 @@ std::string intToString(int i);
 
 }
 
+/**
+ * \brief **Utility function**: Removes URL encoding from the string (e.g. %20 -> space)
+ */
+std::string urlDecode(std::string input);
+
 #endif /* SRC_UTIL_HPP_ */