EthernetLink.h

/* EthernetLink and Ethernet strategies contributed by Fred Larsen

  This EthernetLink class transfers packets of data over
  TCP/IP Ethernet connections.

  It may accept incoming connections and send ACK back for each incoming
  package, it may connect to another object in another device and deliver a
  packet and get an ACK back, or it may do both -- connect to another device
  for delivering packages and accept incoming connections and packages for
  bidirectional transfer.

  When establishing connections, the target must be registered with the add_node
  function for setting the IP and port along with a unique id that is used for
  referring to the target in all other functions.

  An EthernetLink may work in multiple ways when bidirectional:

  1. One-to-one connection with another EthernetLink. In this mode, using the
     keep_connection is recommended because it reduces network activity and
     speeds things up dramatically. In this mode, there are three options:

    A. When running on a firewall-free network one socket can be created in each
       direction, allowing for bidirectional "full-duplex" (as far as that goes
       for single-threaded code), with data being buffered in each direction. It
       requires each side to know about each other's address, but delivers
       higher throughput than a single_socket approach. An EthernetLink object
       in this mode has one node added by add_node, and both sides calls
       start_listening to accept incoming connections. If there are multiple
       objects on one device, each has to listen to a unique port number
       (specified in the start_listening function call).

    B. Set single_socket to mave a master-slave scenario where one side, the
       initiator, creates a socket connection and sends any packages it may
       have and then receives any package waiting to be sendt in the opposite
       direction. This requires the initiator to know where to connect, but not
       the other way around. This is suitable for connections through the
       Internet because a firewall opening only has to be opened in one place.
       An EthernetLink object in this mode has one node added by add_node
       (the initiator side), or calls the start_listening function to start
       listening (the receiver side). If there are multiple receiving objects
       on one device, each has to listen to a unique port number
       (specified in the start_listening function call).

    C. Set single_initiate_direction to have one device create two sockets to
       the other device, using one for each packet direction. This is a variant
       of B) that is firewall friendly and more efficient, but uses one more
       socket, and cannot be run on the simplest Ethernet cards with only one
       available socket. This mode has 3-4 times the speed of B) and does not
       do polling so it will not generate network traffic when idle.

  2. If running a one-to-many or many-to-many scenario the keep_connection
     should be deactivated because then one incoming socket would block others
     from connecting. Not using keep_connection drops the transfer speed some, though.

    A. When running on a firewall-free network each packet is delivered through
       a dedicated connection created for delivering that package. After
       delivery and ACK the socket is closed. All devices can send to and
       receive from each other.

    B. Using the single_socket approach will use a master-slave like approach
       where only the initiators need to know the address of the receiver(s),
       for easier firewall traversal. After the two-way transfer of packets on
       the same connection, the connection will be closed so that the receiver
       is ready for another connection. This requires only _one firewall
       opening_ for bidirectional transfer with 255 others.

  Limitations for the different modes when using the W5100 based Ethernet shield
  with a 4 socket limit:

  1A. Two EthernetLink objects can be used for bidirectional communication
      with two sites.

  1B. Four EthernetLink objects can be used for bidirectional communication
      with four sites.

  2A. As the sockets are only used temporarily each object can accept
      connections from 255 others, and deliver packets to 255 others. Up to
      two objects can be used in one device, if multiple listening ports are
      needed. Each object can have 1 to 255 nodes added by add_node, and can
      deliver to all. There can be up to three objects in each device if needed.
      One socket must remain free for outgoing connections.
  2B. As 2A, each receiver can receive packets from and send packets to 255
      initiators, using a _single listening port_. There can be four receiver
      objects in on device, or three receiver objects and an unlimited number
      of initiator objects that will be able to send to and receive from to up
      to 255 sites each.

  Limitations for the different modes when using the ENC28J60 based Ethernet
  shield with a 1 socket limit:

  1A. Not available.
  1B. One EthernetLink object can be used for fast bidirectional communication
      with one site.
  1C. Not available.
  2A. Not available.
  2B. One receiver can receive from and deliver packets to unlimited initiators,
      _or_ unlimited initator objects can send to and receive from to up to
      255 sites each

  Note: To use the ENC28J60 shield instead of the WIZ5100 shield, include
  <UIPEthernet.h> before EthernetLink.h.

  PJON and EthernetLink
  The EthernetLink can be used standalone for simple delivery between two
  devices. But the EthernetLink is also used as a tool by the EthernetTCP strategy
  of PJON. A site consisting of one or more buses of devices communicating
  through PJON, wired and/or wirelessly, can be connected seamlessly with
  multiple other similar sites in other places of the world, communicating
  through Internet with minimal firewall configuration.

  NOTE: The W5100 full-size Ethernet shields come in multiple variants. If you
        get a problem where the card is not starting after plugging in the
        power, check if there is small resistor network behind the Ethernet
        outlet, with one resistor having printed "511" on it. If so, try
        another brand. Even startup delays do not fix this, but it can be solved
        with a capacitor+resistor, search for it.

  NOTE: If needing single_socket functionality with ACK (polling mode),
        define ETCP_SINGLE_SOCKET_WITH_ACK. The program size has been reduced by
        only including this when needed.

        The same goes for ETCP_SINGLE_DIRECTION. It is not included by default
        to reduce program size. Define this when needed.
        */

#pragma once

#ifdef ARDUINO
  #include "../../interfaces/ARDUINO/TCPHelper_ARDUINO.h"
#else
  #include "../../interfaces/LINUX/TCPHelper_POSIX.h"
  #ifndef F
  #define F(x) (x)
  #endif
#define Serial DummyPrint
  struct DummyPrint {
    static void print(const char *s) { printf("%s", s); }
    static void print(int n) { printf("%d", n); }
    static void println(const char *s) { printf("%s\n", s); }
    static void println(int n) { printf("%d\n", n); }
  } DummyPrint;
#endif

// Constants
#ifndef PJON_ACK
  #define PJON_ACK                           6
#endif

// Internal constants
#ifndef PJON_FAIL
  #define PJON_FAIL                      0x100
#endif

#ifndef ETCP_MAX_REMOTE_NODES
  #define ETCP_MAX_REMOTE_NODES             10
#endif

#define ETCP_DEFAULT_PORT                 7000

/* The maximum packet size to be transferred, this protects again buffer overflow.
   Set this to a size that is guaranteed to be available in RAM during runtime,
   depending on the hardware and software. */
#ifndef ETCP_MAX_PACKET_SIZE
  #define ETCP_MAX_PACKET_SIZE             300
#endif


/* If an incoming packet has not arrived for some time, disconnect the socket so
   it will be reconnected on demand. The timeout is in ms.
   The reason for this is that in some cases an idle socket may have gotten
   disconnected without it being detected, unless trying to write to it.
   So we could be waiting for data that never arrives. */
#ifndef ETCP_IDLE_TIMEOUT
  #define ETCP_IDLE_TIMEOUT 30000ul
#endif

// Magic number to verify that we are aligned with telegram start and end
#define ETCP_HEADER               0x18ABC427ul
#define ETCP_FOOTER               0x9ABE8873ul
#define ETCP_SINGLE_SOCKET_HEADER 0x4E92AC90ul
#define ETCP_SINGLE_SOCKET_FOOTER 0x7BB1E3F4ul
#define ETCP_CONNECTION_HEADER_A      0xFEDFED67ul  // Primary socket, packets in initiated direction
#define ETCP_CONNECTION_HEADER_A_ACK  0xFEDFED68ul  // Same, but request ACK for all packets
#define ETCP_CONNECTION_HEADER_B      0xFEDFED77ul  // Reverse socket, packets in reverse direction

/* UIPEthernet library used for the ENC28J60 based Ethernet shields has the
   correct return value from the read call, while the standard Ethernet library
   does not follow the standard! */
#if defined(ARDUINO) && !defined(UIPETHERNET_H)
  #define ETCP_ERROR_READ                   0
#else
  #define ETCP_ERROR_READ                  -1
#endif

typedef void (*link_receiver)(
  uint8_t id,
  const uint8_t *payload,
  uint16_t length,
  void *callback_object
);

typedef void (*link_error)(
  uint8_t code,
  uint8_t data
);

class TmpBuffer {
    uint8_t *buf = NULL;
    uint16_t len = 0;
  public:
    TmpBuffer(uint16_t size) { len = size;  buf = new uint8_t[size]; }
    ~TmpBuffer() { if (buf) delete buf;  }
    uint8_t* operator()() { return buf; }
    uint16_t size() { return len; }
};

class EthernetLink {
private:
  // ********* Dynamic members ************

  TCPHelperServer *_server = NULL;

  // Connection for writing outgoing packets
  TCPHelperClient _client_out;

  // Connection for reading incoming packets
  TCPHelperClient _client_in;

  // The id of the remove device/node that we have connected to
  int16_t _current_device = -1;

  // When a socket is received, the connection header specifies if ACKs are wanted
  bool _ack_requested = false;

  // Remember the connection statistics
  uint32_t _connection_time = 0;
  uint32_t _connection_count = 0;
  uint32_t _last_receive_time = 0;

  // ********* Configuration ************

  link_receiver _receiver = NULL;
  link_error    _error = NULL;
  void *_callback_object = NULL;

  // Local node
  uint8_t  _local_id = 0;
  uint8_t  _local_ip[4];
  uint16_t _local_port = ETCP_DEFAULT_PORT;

  // Remote nodes
  uint8_t  _remote_node_count = 0;
  uint8_t  _remote_id[ETCP_MAX_REMOTE_NODES];
  uint8_t  _remote_ip[ETCP_MAX_REMOTE_NODES][4];
  uint16_t _remote_port[ETCP_MAX_REMOTE_NODES];

  // Keep sockets permanently open instead of reconnecting for each transfer
  bool _keep_connection = false;

  // Do bidirectional transfer on a single socket
  bool _single_socket = false;

  // Request an immediate ACK for eack packet delivery to ensure guaranteed delivery
  bool _request_ack = false;

  // To avoid deadlocks while connecting (if receiver tries to connect back simultanously),
  // receive and discard packets while doing non-blocking connect. This should resolve deadlocks,
  // and the missing ACK should make the packet being resent later.
  bool _receive_and_discard = false;

  // With single_socket = false, there is one socket for each packet direction.
  // Normally the sockets are initiated from the side sending the packet.
  // By setting initiate_both_sockets_in_same direction, both sockets can be
  // initiated from one of the devices, to simplify firewall setup, or for letting
  // only one of the devices have a static IP address.
  // This should only be used with _keep_connection = true, and is meant for permanent
  // one-to-one links.
  bool _initiate_both_sockets_in_same_direction = false;
  // Whether to be the side initiating both sockets or not
  bool _initiator = true;

public:

  void init() {
    memset(_local_ip, 0, 4);
  };


  int16_t find_remote_node(uint8_t id) {
    for(uint8_t i = 0; i < _remote_node_count; i++)
      if(_remote_id[i] == id)
        return i;
    return -1;
  };


  int16_t read_bytes(
    TCPHelperClient &client,
    uint8_t *contents,
    uint16_t length,
    uint16_t timeout_ms = 2000
  ) {
    int32_t total_bytes_read = 0, bytes_read = ETCP_ERROR_READ;
    uint32_t start_ms = PJON_MILLIS();
    /* NOTE: The Arduino standard recv/read functions returns
       -1 if no data waiting
        0 if socket closed
        This is the opposite of POSIX. */
    do {
      #ifdef HAS_ETHERNETUDP // Avoid using blocking read until there is data present
      int16_t avail = 0;
      while(
        client.connected() &&
        (avail = client.available()) <= 0 &&
        (uint32_t)(PJON_MILLIS() - start_ms) < min(1000, timeout_ms)
      ) PJON_DELAY_MICROSECONDS(250);
      if (avail <= 0) continue;
      #endif

      bytes_read = client.read(
        &contents[total_bytes_read],
        length - total_bytes_read
      );

      if(bytes_read > 0) total_bytes_read += bytes_read;
    } while(
      bytes_read != ETCP_ERROR_READ &&
      total_bytes_read < length &&
      (uint32_t)(PJON_MILLIS() - start_ms) < timeout_ms
    );

    if(bytes_read == ETCP_ERROR_READ) {
      stop(client); // Lost connection
      #ifdef ETCP_ERROR_PRINT
        Serial.println(F("Read failed, closing cl"));
      #endif
    }
    return total_bytes_read;
  };


  // Read a package from a connected client (incoming or outgoing socket) and send ACK
  uint16_t receive(TCPHelperClient &client, bool wait) {
    uint16_t return_value = PJON_FAIL;
    uint32_t start_ms = PJON_MILLIS(), avail;
    if (wait) {
      while(
        client.connected() &&
        (avail = client.available()) <= 0 &&
        (uint32_t)(PJON_MILLIS() - start_ms) < 1000
      ) PJON_DELAY_MICROSECONDS(250);
    } else avail = client.connected() ? client.available() : 0;

    if(avail > 0) {
      #ifdef ETCP_DEBUG_PRINT
        Serial.println(F("Recv from cl"));
      #endif

      // Locate and read encapsulation header (4 bytes magic number)
      bool ok = read_until_header(client, ETCP_HEADER);
      #ifdef ETCP_DEBUG_PRINT
        Serial.print(F("Read header, stat "));
        Serial.println(ok);
      #endif

      // Read sender device id (1 byte) and length of contents (4 bytes)
      int16_t bytes_read = 0;
      uint8_t sender_id = 0;
      uint32_t content_length = 0;
      if(ok) {
        uint8_t buf[5];
        bytes_read = read_bytes(client, buf, 5);
        if(bytes_read != 5) ok = false;
        else {
          memcpy(&sender_id, buf, 1);
          memcpy(&content_length, &buf[1], 4);
          content_length = ntohl(content_length);
          if(content_length == 0) ok = 0;
        }
      }

      // Protect against too large packets
      if (content_length > ETCP_MAX_PACKET_SIZE) return PJON_FAIL;

      // Read contents and footer
      TmpBuffer buf(content_length);
      if(ok) {
        bytes_read = read_bytes(client, (uint8_t*)buf(), content_length);
        if((uint32_t)bytes_read != content_length) ok = false;
      }

      // Read footer (4 bytes magic number)
      if(ok) {
        uint32_t foot = 0;
        bytes_read = read_bytes(client, (uint8_t*) &foot, 4);
        if(bytes_read != 4 || foot != htonl(ETCP_FOOTER)) ok = false;
      }

      #ifdef ETCP_DEBUG_PRINT
        Serial.print(F("Stat rec: "));
        Serial.print(ok);
        Serial.print(" len: ");
        Serial.println(content_length);
      #else
        #if defined(ETCP_ERROR_PRINT)
        if (!ok) {
          Serial.print(F("FAIL rec: "));
          Serial.println(bytes_read);
        }
        #endif
      #endif

      return_value = ok ? PJON_ACK : PJON_FAIL;
      if (ok) _last_receive_time = PJON_MILLIS();
      if (_ack_requested && !_receive_and_discard) {
        // Write PJON_ACK
        int8_t acklen = 0;
        if(ok) {
          uint16_t r = htons(return_value);
          acklen = client.write((uint8_t*) &r, 2);
          client.flush();
        }

        #ifdef ETCP_DEBUG_PRINT
          Serial.print("Sent ");
          Serial.print(ok ? "PJON_ACK: " : "PJON_FAIL: ");
          Serial.println(acklen);
        #else
          (void)acklen; // Avoid "set but not used" warning
        #if defined(ETCP_ERROR_PRINT)
          if (!ok) Serial.println("FAILURE sending ACK");
        #endif
        #endif
      }

      // Call receiver callback function
      if(ok && !_receive_and_discard && content_length > 0)
        _receiver(sender_id, (uint8_t*)buf(), content_length, _callback_object);

      if (!ok) disconnect_in();
    }
    return return_value;
  };


  bool connect(uint8_t id) {
    // Locate the node's IP address and port number
    int16_t pos = find_remote_node(id);

    // Determine if to disconnect
    bool disconnect = !_keep_connection,
         reverse = _initiate_both_sockets_in_same_direction && _initiator;

     // Break existing connection if not connected to the wanted server
     if(!disconnect && _client_out && _current_device != id) {
       // Connected, but to the wrong device
       #ifdef ETCP_DEBUG_PRINT
         Serial.println(F("Switch conn to another srv"));
       #endif
       disconnect = true;
       _current_device = -1;
     }

    // Check if established sockets have been disconnected
    if (!disconnect && _client_out && !_client_out.connected()) disconnect = true;
    #ifdef ETCP_SINGLE_DIRECTION
    if (!disconnect && reverse && _client_in && !_client_in.connected()) disconnect = true;
    if (!disconnect && reverse && (!_client_in || !_client_out)) disconnect = true;
    if (!disconnect && reverse && _client_in && got_receive_timeout()) {
      #ifdef ETCP_ERROR_PRINT
        Serial.println(F("Receive timeout, disconn."));
      #endif
      disconnect = true;
    }
    #endif
    if (disconnect) disconnect_out();
    // NOTE: From this point we can avoid using the expensive connected() call

    bool connected = _client_out;
    #ifdef ETCP_DEBUG_PRINT
    if (!connected) {
      Serial.print(F("Conn to srv pos="));
      Serial.println(pos);
    }
    #endif

    if(pos < 0) return false;

    // Try to connect to server if not already connected
    bool did_connect = false;
    if(!connected) {
      #ifdef ETCP_DEBUG_PRINT
        Serial.println("Conn..");
      #endif
      #ifdef HAS_ETHERNETUDP // Arduino, otherwise POSIX
        connected = _client_out.connect(_remote_ip[pos], _remote_port[pos]);
      #else
        // Use non-blocking calls, and receive and discard incoming packets
        if (_client_out.prepare_connect(_remote_ip[pos], _remote_port[pos])) {
          int8_t status = 0;
          uint32_t start = PJON_MILLIS();
          do {
            if (!_initiate_both_sockets_in_same_direction && !_single_socket && _server) {
              TCPHelperClient client = _server->available();
              if (client) client.stop();
            }
            status = _client_out.try_connect();
            if (status == 0) PJON_DELAY_MICROSECONDS(PJON_RANDOM(250)); // Avoid misusing CPU while waiting
          } while (status == 0 && (uint32_t)(PJON_MILLIS()-start)<4000);
          connected = (status == 1);
        }
      #endif

      #ifdef ETCP_DEBUG_PRINT
        Serial.println(connected ? "Conn to srv" : "Failed conn to srv");
      #endif
      if(connected) {
        // Adjust connection header A to include ACK request flag
        uint32_t conn_header = htonl(_request_ack ? ETCP_CONNECTION_HEADER_A_ACK : ETCP_CONNECTION_HEADER_A);
        if (_client_out.write((uint8_t*) &conn_header, 4) != 4) {
          connected = false;
          #ifdef ETCP_ERROR_PRINT
            Serial.println(F("Disconn. failed write connhead"));
          #endif
        }
      }
      if (!connected) stop(_client_out); else did_connect = true;
    }

    bool connected_rev = false;
    #ifdef ETCP_SINGLE_DIRECTION
    if (connected && reverse) {
      connected_rev = _client_in;
      if (!connected_rev) {
        #ifdef ETCP_DEBUG_PRINT
          Serial.println("Conn rev..");
        #endif
        uint32_t start = PJON_MILLIS();
        do {
          connected_rev = _client_in.connect(_remote_ip[pos], _remote_port[pos]);
        } while (!connected_rev && (uint32_t)(PJON_MILLIS()-start) < 2000);
        #ifdef ETCP_DEBUG_PRINT
          Serial.println(connected_rev ? F("Conn rev to srv") : F("Failed rev conn to srv"));
        #endif
        if(connected_rev) {
           uint32_t conn_header = htonl(ETCP_CONNECTION_HEADER_B);
           if (_client_in.write((uint8_t*) &conn_header, 4) != 4) {
             connected_rev = false;
             #ifdef ETCP_ERROR_PRINT
               Serial.println(F("Disconn rev. failed write connhead"));
             #endif
           }
        }
        if (connected_rev) {
          // ACK active on both sockets or none in this mode
          _ack_requested = _request_ack;
          did_connect = true;
          _last_receive_time = PJON_MILLIS(); // Count the connection as a receive action
        }
      }
    }
    #endif

    if (did_connect) { // Gather a litte connection information
      _connection_time = PJON_MILLIS();
      _connection_count++;
      if (_single_socket) _ack_requested = _request_ack; // Same mode in both directions
    }

    if (!connected || (reverse && !connected_rev)) {
      #ifdef ETCP_ERROR_PRINT
        Serial.print(F("Fail conn, closing "));
        Serial.print(connected);
        Serial.println(connected_rev);
      #endif
      disconnect_out();
      _current_device = -1;
      PJON_DELAY(10);  // Slow down if failure
      return false; // Server is unreachable or busy
    }
    else _current_device = id; // Remember who we are connected to

    return true;
  };


  void stop(TCPHelperClient &client) {
    client.stop();
  };

  void disconnect_in() {
    #ifdef ETCP_DEBUG_PRINT
      if (_client_in || (_initiate_both_sockets_in_same_direction && _client_out))
        Serial.println(F("Disconn in&rev"));
    #endif
    #ifdef ETCP_SINGLE_DIRECTION
    if (_initiate_both_sockets_in_same_direction && _client_out) stop(_client_out);
    #endif
    if (_client_in) stop(_client_in);
  }


  void disconnect_out() {
    #ifdef ETCP_DEBUG_PRINT
      if (_client_out || (_initiate_both_sockets_in_same_direction && _client_in))
        Serial.println(F("Disconn out&rev"));
    #endif
    if (_client_out) stop(_client_out);
    #ifdef ETCP_SINGLE_DIRECTION
    if (_initiate_both_sockets_in_same_direction && _client_in) stop(_client_in);
    #endif
  }


  bool accept() {
    // Determine if to disconnect
    bool disconnect = !_keep_connection;
    #ifdef ETCP_SINGLE_DIRECTION
    bool reverse = _initiate_both_sockets_in_same_direction && !_initiator;
    #endif
    if (!disconnect && _client_in && !_client_in.connected()) disconnect = true;
    #ifdef ETCP_SINGLE_DIRECTION
    if (!disconnect && reverse && _client_out && !_client_out.connected()) disconnect = true;
    if (!disconnect && reverse && (!_client_in || !_client_out)) disconnect = true;
    #endif
    if (!disconnect && _client_in && got_receive_timeout()) {
      #ifdef ETCP_ERROR_PRINT
        Serial.println(F("Receive timeout, disconn."));
      #endif
      disconnect = true;
    }
    if (disconnect) disconnect_in();
    // NOTE: From this point we can avoid using the expensive connected() call

    // Accept new incoming connection
    bool did_connect = false, connected = _client_in;
    if (!connected) {
      _client_in = _server->available();
      connected = _client_in;
      #ifdef ETCP_DEBUG_PRINT
        if(connected) Serial.println(F("Accepted"));
      #endif
      if (connected) {
        uint32_t connection_header = 0;
        bool header_ok = false;
        if (read_bytes(_client_in, (uint8_t*) &connection_header, 4) == 4) {
          if (connection_header == htonl(ETCP_CONNECTION_HEADER_A)) {
            header_ok = true;
            _ack_requested = false;
          } else if (connection_header == htonl(ETCP_CONNECTION_HEADER_A_ACK)) {
            header_ok = true;
            _ack_requested = true;
          }
        }
        if (header_ok) did_connect = true;
        else {
          disconnect_in();
          connected = false;
          #ifdef ETCP_ERROR_PRINT
            Serial.println(F("Disconn. no connhead"));
          #endif
        }
      }
    }

    // Accept reverse connection if relevant
    #ifdef ETCP_SINGLE_DIRECTION
    if (connected && reverse && !_client_out) {
      #ifdef ETCP_DEBUG_PRINT
        Serial.println(F("Lst rev"));
      #endif
      // ACK active on both sockets or none in this mode
      _request_ack = _ack_requested;

      bool connected_reverse = false;
      uint32_t start = PJON_MILLIS();
      do {
        _client_out = _server->available();
      } while (!_client_out && (uint32_t)(PJON_MILLIS()-start) < 2000);

      if(_client_out) {
        #ifdef ETCP_DEBUG_PRINT
          Serial.println("Accept rev OK");
        #endif
        uint32_t connection_header = 0;
        if (read_bytes(_client_out, (uint8_t*) &connection_header, 4) == 4 &&
            connection_header == htonl(ETCP_CONNECTION_HEADER_B))
          connected_reverse = true;
        else {
          #ifdef ETCP_ERROR_PRINT
            Serial.println(F("Disconn. rev no connhead"));
          #endif
        }
      } else {
        #ifdef ETCP_ERROR_PRINT
          Serial.println(F("Accept rev timed out"));
        #endif
      }

      if (connected_reverse) did_connect = true;
      else {
        #ifdef ETCP_ERROR_PRINT
          Serial.print(F("Fail accept, closing, "));
          Serial.print(connected);
          Serial.println(connected_reverse);
        #endif
        connected = false;
        disconnect_in();
      }
    }
    #endif

    if (did_connect) {
      _connection_time = PJON_MILLIS();
      _connection_count++;
      _last_receive_time = PJON_MILLIS(); // Count the connection as a receive action
    }
    return connected;
  };


  void disconnect_out_if_needed(uint16_t result) {
    //bool connected = _client_out.connected();
    if(_client_out && (result == PJON_FAIL || !_keep_connection)) {
      stop(_client_out);
      #ifdef ETCP_DEBUG_PRINT
        Serial.print("Disconn outcl. OK=");
        Serial.println(result == PJON_ACK);
      #endif
    }
  };

  bool got_receive_timeout() { return (uint32_t)(PJON_MILLIS() - _last_receive_time) > ETCP_IDLE_TIMEOUT; }

  bool disconnect_in_if_needed() {
    if(_client_in && !_keep_connection) {
      #ifdef ETCP_DEBUG_PRINT
        Serial.println("Disc. inclient.");
      #endif
      stop(_client_in);
    }
    return true;
  };


  uint16_t send(
    TCPHelperClient &client,
    uint8_t id,
    const uint8_t *packet,
    uint16_t length
  ) {
    // Assume we are connected. Try to deliver the package
    uint32_t head = htonl(ETCP_HEADER), foot = htonl(ETCP_FOOTER), len = htonl(length);
    uint8_t buf[9];
    memcpy(buf, &head, 4);
    memcpy(&buf[4], &id, 1);
    memcpy(&buf[5], &len, 4);

    if (!_single_socket && _client_in) if (_client_in.available() > 0) return PJON_BUSY;

    #ifdef HAS_ETHERNETUDP
    bool ok = client.write((uint8_t*) buf, 9) == 9;
    if(ok) ok = client.write((uint8_t*) packet, length) == length;
    if(ok) ok = client.write((uint8_t*) &foot, 4) == 4;
    #else
    // On a POSIX capable device we expect to have enough memory to collect all into one buffer
    // so that it will not be sent as 3 separate packets when TCP_NODELAY is active.
    TmpBuffer totalbuf(9+length+4);
    memcpy(totalbuf(), buf, 9);
    memcpy(&(totalbuf()[9]), packet, length);
    memcpy(&(totalbuf()[9+length]), &foot, 4);
    bool ok = client.write((uint8_t*)totalbuf(), 9+length+4) == (9+length+4);
    #endif
    if(ok) client.flush();
    #ifdef ETCP_DEBUG_PRINT
      Serial.print("Write stat: ");
      Serial.println(ok);
    #endif

    uint16_t result = PJON_FAIL;
    if (_request_ack) {
      // Read ACK
      if(ok) {
        uint16_t code = 0;
        int16_t status = read_bytes(client, (uint8_t*) &code, 2);
        code = ntohs(code);
        if(status == 2 && code == PJON_ACK) result = code; else ok = false;
      }
      #ifdef ETCP_DEBUG_PRINT
        Serial.print("PJON_ACK stat: ");
        Serial.print(result == PJON_ACK);
        Serial.println(ok ? " OK" : " FAIL");
      #endif
    } else {
      result = ok ? PJON_ACK : PJON_FAIL;
    }
    return result;  // PJON_FAIL, PJON_ACK
  };


  /* Do ACKed bidirectional transfer of packets over a single socket connection by
     using a master-slave mode where the master connects and delivers packets
     or a placeholder, then reads packets or placeholder back before closing
     the connection (unless letting it stay open). */
  #ifdef ETCP_SINGLE_SOCKET_WITH_ACK
  uint16_t single_socket_transfer(
    TCPHelperClient &client,
    int16_t id,
    bool master,
    const uint8_t *contents,
    uint16_t length
  ) {
    if(master) { // Creating outgoing connections
      // Connect or check that we are already connected to the correct server
      bool connected = connect(id);
      #ifdef ETCP_DEBUG_PRINT
        //Serial.println(connected ? "Out conn" : "No out conn");
      #endif
      if(!connected) return PJON_FAIL;

      // Send singlesocket header and number of outgoing packets
      bool ok = true;
      uint32_t head = htonl(ETCP_SINGLE_SOCKET_HEADER);
      uint8_t numpackets_out = length > 0 ? 1 : 0;
      uint8_t buf[5];
      memcpy(buf, &head, 4);
      memcpy(&buf[4], &numpackets_out, 1);
      if(ok) ok = client.write((uint8_t*) &buf, 5) == 5;
      if(ok) client.flush();

      // Send the packet and read PJON_ACK
      if(ok && numpackets_out > 0) {
         ok = send(client, id, contents, length) == PJON_ACK;
         #ifdef ETCP_DEBUG_PRINT
           Serial.print(F("++++Sent p, ok="));
           Serial.println(ok);
         #endif
      }

      // Read number of incoming messages
      uint8_t numpackets_in = 0;
      if(ok) ok = read_bytes(client, &numpackets_in, 1) == 1;
      #ifdef ETCP_DEBUG_PRINT
        if (!ok || numpackets_in > 0) {
          Serial.print("Read np_in: ");
          Serial.print(numpackets_in);
          Serial.println(ok ? " OK" : " FAIL");
      }
      #endif

      // Read incoming packages if any
      for(uint8_t i = 0; ok && i < numpackets_in; i++) {
        while(client.available() < 1 && client.connected()) ;
        ok = receive(client, true) == PJON_ACK;
        #ifdef ETCP_DEBUG_PRINT
          Serial.print(F("------->Read p ")); Serial.print(i);
          Serial.print(F(", ok=")); Serial.println(ok);
        #endif
      }

      // Write singlesocket footer for the whole thing
      uint32_t foot = htonl(ETCP_SINGLE_SOCKET_FOOTER);
      if(ok) ok = client.write((uint8_t*) &foot, 4) == 4;
      if(ok) client.flush();
      #ifdef ETCP_DEBUG_PRINT
//        Serial.print("Sent ss foot, ok=");
//        Serial.println(ok);
      #endif

      // Disconnect
      uint16_t result = ok ? PJON_ACK : PJON_FAIL;
      disconnect_out_if_needed(result);
      #ifdef ETCP_DEBUG_PRINT
      if (numpackets_in > 0 || numpackets_out > 0) {
        Serial.print("INOUTm "); Serial.print(numpackets_in); Serial.print(numpackets_out);
        Serial.println(ok ? " OK" : " FAIL");
      }
      #endif

      // Return PJON_ACK if successfully sent or received a packet
      return contents == NULL ? (numpackets_in > 0 ? result : PJON_FAIL) : result;

    } else { // Receiving incoming connections and packets and request
      if (client && got_receive_timeout()) {
        #ifdef ETCP_ERROR_PRINT
          Serial.println(F("Receive timeout, disconn."));
        #endif
        stop(client);
      }

      // Wait for and accept connection
      bool connected = accept();
      #ifdef ETCP_DEBUG_PRINT
        //Serial.println(connected ? "In conn" : "No in conn");
      #endif
      if(!connected) return PJON_FAIL;

      // Read singlesocket header
      bool ok = read_until_header(client, ETCP_SINGLE_SOCKET_HEADER);
      if (ok) _last_receive_time = PJON_MILLIS();
      #ifdef ETCP_DEBUG_PRINT
        //Serial.print("Read ss head, ok=");
        //Serial.println(ok);
      #endif

      // Read number of incoming packets
      uint8_t numpackets_in = 0;
      if(ok) ok = read_bytes(client, (uint8_t*) &numpackets_in, 1) == 1;
      #ifdef ETCP_DEBUG_PRINT
        if (!ok || numpackets_in > 0) {
          Serial.print("Read np_in: ");
          Serial.print(numpackets_in);
          Serial.println(ok ? " OK" : " FAIL");
        }
      #endif

      // Read incoming packets if any, send ACK for each
      for(uint8_t i = 0; ok && i < numpackets_in; i++) {
        while(client.available() < 1 && client.connected()) ;
        ok = receive(client, true) == PJON_ACK;
        #ifdef ETCP_DEBUG_PRINT
          Serial.print(F("Read p ")); Serial.print(i); Serial.print(F(", ok="));
          Serial.println(ok);
        #endif
      }

      // Write number of outgoing packets
      uint8_t numpackets_out = length > 0 ? 1 : 0;
      if(ok) ok = client.write((uint8_t*) &numpackets_out, 1) == 1;
      if(ok) client.flush();

      // Write outgoing packets if any
      if(ok && numpackets_out > 0) {
        ok = send(client, id, contents, length) == PJON_ACK;
        #ifdef ETCP_DEBUG_PRINT
           Serial.print(F("Sent p, ok="));
           Serial.println(ok);
        #endif
      }

      // Read singlesocket footer
      if(ok) {
        uint32_t foot = 0;
        ok = read_bytes(client, (uint8_t*) &foot, 4) == 4;
        if(foot != htonl(ETCP_SINGLE_SOCKET_FOOTER)) ok = 0;
        #ifdef ETCP_DEBUG_PRINT
          //Serial.print(F("Read ss foot, ok="));
          //Serial.println(ok);
        #endif
      }

      // Disconnect
      disconnect_in_if_needed();

      #ifdef ETCP_DEBUG_PRINT
      if (numpackets_in > 0 || numpackets_out > 0) {
        Serial.print("INOUT "); Serial.print(numpackets_in); Serial.print(numpackets_out);
        Serial.println(ok ? " OK" : " FAIL");
      }
      #endif

      if (!ok) {
        stop(client);
        #ifdef ETCP_ERROR_PRINT
        Serial.print(F("Failure, disconnecting."));
        #endif
      }

      // Return PJON_ACK if successfully sent or received a packet
      uint16_t result = ok ? PJON_ACK : PJON_FAIL;
      return contents == NULL ? (numpackets_in > 0 ? result : PJON_FAIL) : result;
    }
    return PJON_FAIL;
  };
  #endif

  /* Read until a specific 4 byte value is found.
     This will resync if stream position is lost. */

  bool read_until_header(TCPHelperClient &client, uint32_t header) {
    header = htonl(header); // Network byte order
    uint32_t head = 0;
    int8_t bytes_read = 0;
    bytes_read = (uint8_t)read_bytes(client, (uint8_t*) &head, 4);
    if(bytes_read != 4 || head != header) {
      // Did not get header. Lost position in stream?
      do { /* Try to resync if we lost position in the stream
              (throw avay all until ETCP_HEADER found) */
        head = head >> 8;
        // Make space for 8 bits to be read into the most significant byte
        bytes_read = (uint8_t)read_bytes(client, &((uint8_t*) &head)[3], 1);
        if(bytes_read != 1) break;
      } while(head != header);
    }
    return head == header;
  };

public:

  EthernetLink() {
    init();
  };


  EthernetLink(uint8_t id) {
    init();
    set_id(id);
  };

  #ifndef ARDUINO
  // Destructor not needed on Arduino, uses a lot of program space
  ~EthernetLink() {
    if (_server) delete _server;
  }
  #endif

  int16_t add_node(
    uint8_t remote_id,
    const uint8_t remote_ip[],
    uint16_t port_number = ETCP_DEFAULT_PORT
  ) {
    if (_remote_node_count == ETCP_MAX_REMOTE_NODES) return -1;
    _remote_id[_remote_node_count] = remote_id;
    memcpy(_remote_ip[_remote_node_count], remote_ip, 4);
    _remote_port[_remote_node_count] = port_number;
    _remote_node_count++;
    return _remote_node_count - 1;
  };


  /* This function must be called after configuration before running,
     for ALL configurations EXCEPT when initiator in single_socket
     and single_initiate_direction modes.
     In single_socket and single_initiate_direction modes, calling this
     function sets it as receiver, NOT initiator. */

  void start_listening(uint16_t port_number = ETCP_DEFAULT_PORT) {
    _local_port = port_number;
    _initiator = false; // If we are listening, we are not the initiator
    if(_server == NULL && (!_initiate_both_sockets_in_same_direction || !_initiator)) {
      #ifdef ETCP_DEBUG_PRINT
        Serial.print("Lst on port ");
        Serial.println(port_number);
      #endif
      _server = new TCPHelperServer(port_number);
      _server->begin();
    }
  };


  /* Whether to keep outgoing connection live until we need connect to
     another EthernetLink node */

  void keep_connection(bool keep) {
    _keep_connection = keep;
  };


  /* Whether to do bidirectional data transfer on a single socket or use one
     socket for each direction. Single socket transfer is slower but more
     firewall-friendly. Client connects to server, and packets are exchanged in
     both directions on the same socket. If using this, _only one_ of the sides
     should call start_listening, and that side will be receiver with the other
     initiator (establishing connections) */
  #ifdef ETCP_SINGLE_SOCKET_WITH_ACK
  void single_socket(bool single_socket) {
    _single_socket = single_socket;
    keep_connection(true);
    request_ack(true);
  };
  #endif

  /* With single_socket = false, there is one socket for each packet direction.
    Normally the sockets are initiated from the side sending the packet.
    By setting initiate_both_sockets_in_same direction, both sockets can be
    initiated from one of the devices, to simplify firewall setup, or for letting
    only one of the devices have a static IP address.
    This should only be used with _keep_connection = true, and is meant for permanent
    one-to-one links. */
  #ifdef ETCP_SINGLE_DIRECTION
  void single_initiate_direction(bool single_initiate_direction) {
    _initiate_both_sockets_in_same_direction = single_initiate_direction;
    keep_connection(true);
    request_ack(true);
  };
  #endif

  /* Request an explicit immediate ACK for each packet being sent.
     This make it possible with guearanteed delivery.
     Without ACK, a packet may be lost before a dead socket is discovered,
     but for some shemes this is acceptable. */

  void request_ack(bool request_ack) {
    _request_ack = request_ack;
  }

  /* Some connection statistics */
  uint32_t get_connection_time() const { return _connection_time; }
  uint32_t get_connection_count() const { return _connection_count; }


  // Keep trying to send for a maximum duration

  uint16_t send_with_duration(
    uint8_t id,
    const uint8_t *packet,
    uint16_t length,
    uint32_t duration_us
  ) {
    uint32_t start = PJON_MICROS();
    uint16_t result = PJON_FAIL;
    do {
      result = send(id, packet, length);
    } while(
      result != PJON_ACK &&
      (uint32_t)(PJON_MICROS() - start) <= duration_us
    );
    return result;
  };


  /* In single-socket mode and acting as initiator, connect and check for
     incoming packets from a specific device */

  uint16_t poll_receive(uint8_t remote_id) {
    /* Create connection if needed but only poll for incoming packet
       without delivering any */
    if(_single_socket) {
      if(!_server) {
      #ifdef ETCP_SINGLE_SOCKET_WITH_ACK
        return single_socket_transfer(_client_out, remote_id, true, NULL, 0);
      #else
        (void)remote_id; // Avoid "unused parameter" warning
        return PJON_FAIL;
      #endif
      }
    } else { // Just do an ordinary receive without using the id
      return receive();
    }
    return PJON_FAIL;
  };


  uint8_t get_id() const {
    return _local_id;
  };


  // Overridden functions below -----------------------------------------------

  // Connect to a server if needed, then read incoming package and send ACK

  uint16_t receive() {
    if(_server == NULL) { // Not listening for incoming connections
      #if defined(ETCP_SINGLE_SOCKET_WITH_ACK) || defined(ETCP_SINGLE_DIRECTION)
      int16_t remote_id = _remote_node_count == 1 ? _remote_id[0] : -1;
      #endif
      if(_single_socket) { // Single-socket mode.
        /* Only read from already established outgoing socket, or create
        connection if there is only one remote node configured (no doubt about
        which node to connect to). */
        #ifdef ETCP_SINGLE_SOCKET_WITH_ACK
        return single_socket_transfer(_client_out, remote_id, true, NULL, 0);
        #else
        return PJON_FAIL;
        #endif
      }
      #ifdef ETCP_SINGLE_DIRECTION
      else if (_initiate_both_sockets_in_same_direction && _initiator) {
        connect(remote_id);
        uint16_t result = receive(_client_in, false);
        disconnect_out_if_needed(PJON_ACK); // Do not disconnect if nothing to read
        disconnect_in_if_needed();
        return result;
      }
      #endif
    } else {
      // Accept new incoming connection if connection has been lost
      if(_single_socket) {
        #ifdef ETCP_SINGLE_SOCKET_WITH_ACK
        return single_socket_transfer(_client_in, -1, false, NULL, 0);
        #else
        return PJON_FAIL;
        #endif
      }else {
        // Accept incoming connection(s)
        if(!accept()) return PJON_FAIL;

        uint16_t result = receive(_client_in, !_keep_connection);

        disconnect_in_if_needed();
        return result;
      }
    }
    return PJON_FAIL;
  };


  uint16_t receive(uint32_t duration_us) {
    uint32_t start = PJON_MICROS();
    uint16_t result = PJON_FAIL;
    do {
      result = receive();
      if (result != PJON_ACK) PJON_DELAY_MICROSECONDS(250);
    } while(
      result != PJON_ACK &&
      (uint32_t)(PJON_MICROS() - start) <= duration_us
    );
    return result;
  };


  uint16_t send(
    uint8_t id,
    const uint8_t *packet,
    uint16_t length,
    uint32_t = 0 // timing_us
  ) {
    // Special algorithm for single-socket transfers
    if(_single_socket)
      #ifdef ETCP_SINGLE_SOCKET_WITH_ACK
      return single_socket_transfer(
        _server ? _client_in : _client_out,
        id,
        _server ? false : true,
        packet,
        length
      );
      #else
      return PJON_FAIL;
      #endif

    // Connect or check that we are already connected to the correct server
    bool connected = false;
    #ifdef ETCP_SINGLE_DIRECTION
    if (_initiate_both_sockets_in_same_direction && !_initiator) connected = accept();
    else
    #endif
    connected = connect(id);

    // Send the packet and read PJON_ACK
    uint16_t result = PJON_FAIL;
    if(connected) result = send(_client_out, id, packet, length);

    // Disconnect
    disconnect_out_if_needed(result);

    return result;
  };


  uint8_t device_id() {
    return _local_id;
  };


  uint8_t acquire_id() { // Not supported yet
    return 0;
  };


  void set_error(link_error e) {
    _error = e;
  };


  void set_id(uint8_t id) {
    _local_id = id;
  };


  void set_receiver(link_receiver r, void *callback_object) {
    _receiver = r;
    _callback_object = callback_object;
  };


  void update() { };
};

#undef Serial