File content as of revision 31:2e4b6de6c2f3:

/*
 * Firmware supports NNIO and RPC protocols
 */
#include "mbed.h"
#include "rtos.h"
#include "mbed_rpc.h"
#include "Arguments.h"

#include "eeprom.h"
#include "EthernetInterface.h"
#include "NTPClient.h"

#include "my_eeprom_funcs.h"
#include "Watchdog.h"


/** Debug option
 *
 */
#if 1
//Enable debug
#include <cstdio>
#define DBG(x, ...) std::printf("[main : DBG]"x"\r\n", ##__VA_ARGS__);
#define WARN(x, ...) std::printf("[main : WARN]"x"\r\n", ##__VA_ARGS__);
#define ERR(x, ...) std::printf("[main : ERR]"x"\r\n", ##__VA_ARGS__);
#else
//Disable debug
#define DBG(x, ...)
#define WARN(x, ...)
#define ERR(x, ...)
#endif


/*
* Hardware defines
*/
// Ethernet
SPI spi(PA_7, PA_6, PA_5); // mosi, miso, sclk
EthernetInterface eth(&spi, PA_4, PC_9); // spi, cs, reset

/*
* EEPROM section
*/
// Virtual address defined by the user: 0xFFFF value is prohibited
uint16_t VirtAddVarTab[NumbOfVar] = {0x1212, 0x1313, 0x1414, 0x1515, // IP_Addr
                                     0x2212, 0x2313, 0x2414, 0x2515, // IP_Subnet
                                     0x3212, 0x3313, 0x3414, 0x3515, // IP_Gateway
                                     0x4212, // TCP server port, not used
                                     0x5212, // UDP server port, not used
                                     0x8888, // 1st run? 0xA5A5 = configured
                                     0x6212, 0x6313, 0x6414, // MAC

                                     // this section is for the TCP server that this device connects to in TCP client mode
                                     0x7212, 0x7313, // 0xA5A5 = auto transmit status, time period
                                     0x8212, 0x8313,0x8414, 0x8515, // TCP server IP address
                                     0x9212, // TCP server port

                                     // this section is for selecting protocol, not used
                                     0xA212, // 0xA5A5 = enable TCP server
                                     0xA313, // 0xA5A5 = eanble TCP client
                                     0xA414  // 0xA5A5 = enable UDP server
                                    };


/*
* Network configuration
*/
#define TCP_SERVER
#define TCP_CLIENT
#define UDP_SERVER
//#define UDP_CLIENT
#define NTP

#define TCP_SERVER_WAIT_CLIENT_TIMEOUT     200 // timeout for local tcp server wait for a remote client
#define TCP_SERVER_RECEIVE_TIMEOUT         2000 // timeout for local tcp server wait to receive from remote client
#define TCP_CLIENT_RECEIVE_TIMEOUT         200 // timeout for local tcp client try to connect remote server
#define UDP_SERVER_RECEIVE_TIMEOUT         100 // timeout for checking config command


// TCP server function
TCPSocketServer tcp_server;
TCPSocketConnection tcp_client;
// TCP client function
TCPSocketConnection tcp_sock;
// UDP server
UDPSocket udp_server;
Endpoint ep_udp_client;
// NTP
NTPClient ntp;


/*
* Variables for network configuration, TCP server
*/
uint8_t u8mac[6], u8ip_addr[4];// keep mac and ip address in 8-bits
uint16_t u16mac_addr[3], u16ip_addr[4], u16ip_subnet[4], u16ip_gateway[4]; // 16-bits, directly loaded from eeprom
char str_ip_addr[16], str_ip_subnet[16], str_ip_gateway[16]; // for printf, converted from 16-bits u16ip_xxx
uint16_t configured_ip = 0;  // static ip configured flag

const uint16_t tcp_server_local_port = 10000; // fixed, change to 7000 if internet required
const uint16_t udp_server_local_port = 11000; // fixed

// TCP client: this section is used for the TCP server that this device connects to in TCP client mode
// this device will transmit status every transmit_time_period
uint16_t auto_transmit_flag = 0, transmit_time_period = 1000; // auto transmit status, time period = 1s
uint16_t u16server_ip_addr[4]; // directly loaded from eeprom
uint8_t u8server_ip_addr[4]; // server ip address in 8-bits
char str_server_ip_addr[16];// for printf, converted from 16-bits u16server_ip_addr
uint16_t u16tcp_server_port; // directly loaded from eeprom
uint16_t u16enable_tcp_client, u16enable_tcp_server;// flags for enabling TCP client or TCP server

#define NET_BUF_LEN         256
char tcp_receiving_buffer[NET_BUF_LEN];
char udp_receiving_buffer[NET_BUF_LEN];
char network_output_buffer[NET_BUF_LEN]; // output buffer for TCP/UDP control command


/*
 * RPC Protocol
 * Use the RPC enabled wrapped class  - see RpcClasses.h for more info
 */
// DigitalIn
RpcDigitalIn di0(PB_14, "din0");
RpcDigitalIn di1(PB_12, "din1");
RpcDigitalIn di2(PB_10, "din2");
RpcDigitalIn di3(PB_1, "din3");
RpcDigitalIn di4(PB_15, "din4");
RpcDigitalIn di5(PB_13, "din5");
RpcDigitalIn di6(PB_11, "din6");
RpcDigitalIn di7(PB_2, "din7");
DigitalIn din0(PB_14);
DigitalIn din1(PB_12);
DigitalIn din2(PB_10);
DigitalIn din3(PB_1);
DigitalIn din4(PB_15);
DigitalIn din5(PB_13);
DigitalIn din6(PB_11);
DigitalIn din7(PB_2);
// DigitalOut
RpcDigitalOut do0(PB_3, "dout0");
RpcDigitalOut do1(PB_5, "dout1");
RpcDigitalOut do2(PB_7, "dout2");
RpcDigitalOut do3(PB_9, "dout3");
RpcDigitalOut do4(PD_2, "dout4");
RpcDigitalOut do5(PB_4, "dout5");
RpcDigitalOut do6(PB_6, "dout6");
RpcDigitalOut do7(PB_8, "dout7");
DigitalOut dout0(PB_3);
DigitalOut dout1(PB_5);
DigitalOut dout2(PB_7);
DigitalOut dout3(PB_9);
DigitalOut dout4(PD_2);
DigitalOut dout5(PB_4);
DigitalOut dout6(PB_6);
DigitalOut dout7(PB_8);
// AnalogIn
RpcAnalogIn adc10(PC_0, "ain0"); // adc10
RpcAnalogIn adc11(PC_1, "ain1"); // adc11
AnalogIn ain0(PC_0);
AnalogIn ain1(PC_1);
// AnalogOut, PWM
RpcPwmOut pwm11(PA_8, "pwm0"); // pwm11
RpcPwmOut pwm21(PA_15, "pwm1"); // pwm21
// Serial
RpcSerial usart2(USBTX, USBRX, "uart"); // usart2
Serial uart(USBTX,USBRX);
// Timer
RpcTimer timer1("timer1");
// Watchdog
Watchdog wdt;


/*
 * NNIO Protocol
 */
// Commands
#define DEVICE_DESCRIPTION                  "NNIO"
#define DEVICE_CONFIG_CODE                  "NNCF"
#define DEVICE_CONTROL_CODE                 "NNIO"

#define RECEIVING_PROTOCOL_LENGTH           58
#define SENDING_PROTOCOL_LENGTH             39
#define QUERY_NETWORK_CONFIG_CMD_LENGTH     6
#define SET_NETWORK_CONFIG_CMD_LENGTH       19
#define UPDATE_TCP_SERVER_INFO_COMMAND_LENGTH   12

#define QUERY_DISCOVERY_CMD                 "NNCFDS"
#define QUERY_IP_CMD                        "NNCFIP"
#define QUERY_SUBNET_CMD                    "NNCFSN"
#define QUERY_GATEWAY_CMD                   "NNCFGW"
#define QUERY_MAC_CMD                       "NNCFMC"
#define QUERY_UDP_PORT_CMD                  "NNCFUP"
#define QUERY_TCP_PORT_CMD                  "NNCFTP"
#define QUERY_UPDATE_TIME_CMD               "NNCFTM"
#define RECEIVING_PROTOCOL_ENABLE_OUTPUT    'O'
#define QUERY_STATUS_COMMAND                'Q'
#define DIGITAL_HIGH                        'H'
#define DIGITAL_LOW                         'L'


// Positions
#define RECEIVING_PROTOCOL_ID_POS           0
#define RECEIVING_PROTOCOL_OP_POS           4
#define RECEIVING_PROTOCOL_EN_DO_POS        RECEIVING_PROTOCOL_OP_POS + 0
#define RECEIVING_PROTOCOL_EN_A0O_POS       RECEIVING_PROTOCOL_OP_POS + 1
#define RECEIVING_PROTOCOL_EN_A1O_POS       RECEIVING_PROTOCOL_OP_POS + 2
#define RECEIVING_PROTOCOL_EN_UART_POS      RECEIVING_PROTOCOL_OP_POS + 3
#define RECEIVING_PROTOCOL_COMMAND_POS      RECEIVING_PROTOCOL_OP_POS + 4

#define RECEIVING_PROTOCOL_IP_POS           9
#define RECEIVING_PROTOCOL_DO_POS           13
#define RECEIVING_PROTOCOL_A0O_POS          21
#define RECEIVING_PROTOCOL_A01_POS          23
#define RECEIVING_PROTOCOL_UART_POS         25


#define SENDING_PROTOCOL_ID_POS             0
#define SENDING_PROTOCOL_MAC_POS            4
#define SENDING_PROTOCOL_IP_POS             10
#define SENDING_PROTOCOL_DI_POS             14
#define SENDING_PROTOCOL_DO_POS             22
#define SENDING_PROTOCOL_AI0_POS            30
#define SENDING_PROTOCOL_AI1_POS            32
#define SENDING_PROTOCOL_AO0_POS            34
#define SENDING_PROTOCOL_AO1_POS            36
#define SENDING_PROTOCOL_CR_POS             38


/*
* RTOS
*/
struct message_t {
    int len;
    char *msg;
};
Queue<message_t, 16> uart_queue;
Queue<bool, 1> auto_update_queue;



// Prototypes
int ethernet_init(void);
int process_control_command(char* received_buffer, int len);
void process_config_command(char* received_buffer, int len);
void update_digital_outputs(char* buf);
void update_sending_frame(char* buf);


/*
* Threads
*/
// Timer thread for auto update in TCP client function
void auto_update_timer_thread(void const* args)
{
    bool update_flag = true;

    Thread::wait(500);
    while(true) {
        auto_update_queue.put(&update_flag);
        Thread::wait(1000*transmit_time_period); // Thread::wait() in ms
    }
}


// WDT reset
void wdt_reset_thread(void const* args)
{
    while (true)
        wdt.Service();
}


int main()
{
    int n, ret;

    Thread::wait(500); // turn on delay

    /*
    * Configure
    */
    uart.baud(115200);
    DBG("\r\nStarting...");

    // check watchdog
    if (wdt.WatchdogCausedReset())
        DBG("Watchdog caused reset.");
    wdt.Configure(4);

    /*
    * FLASH
    */
    load_eeprom_network();
    load_eeprom_tcpserver();

    /*
    * UI threads
    */
    Thread t2(auto_update_timer_thread);
    Thread t3(wdt_reset_thread);

    /*
    * Ethernet
    */
    ret = ethernet_init();
    if (ret) {
        ERR("Ethernet initialisation failed. App halted.");
        while (true) {};
    }

    Thread::wait(2000); // TCP/UDP stack delay

    /*
    * UDP server
    * TCP server/client
    */
#ifdef UDP_SERVER
    ret = udp_server.bind(udp_server_local_port);
    DBG("UDP server started (sock.bind = %d)...", ret);
    udp_server.set_blocking(false, UDP_SERVER_RECEIVE_TIMEOUT);
#endif

#ifdef TCP_SERVER
    tcp_server.bind(tcp_server_local_port);
    tcp_server.listen();
    DBG("TCP server started...");
    tcp_server.set_blocking(false, TCP_SERVER_WAIT_CLIENT_TIMEOUT);
#endif

#ifdef TCP_CLIENT

#endif

    /*
    * Network loop processor
    */
    while (true) {
#ifdef TCP_CLIENT // auto update device status to a remote TCP server
        if (auto_transmit_flag == 0xA5A5) {
            // connect to TCP server if required
            if (!tcp_sock.is_connected()) {
                ret = tcp_sock.connect(str_server_ip_addr, u16tcp_server_port); // timeout is default in connect() in W5500.h
                if (ret > -1) {
                    DBG("Successfully connected to %s on port %d", str_server_ip_addr, u16tcp_server_port);
                } else {
                    ERR("Unable to connect to %s on port %d", str_server_ip_addr, u16tcp_server_port);
                }
            }

            // transmit data if connected
            if (tcp_sock.is_connected()) {
                osEvent evt = auto_update_queue.get(1); // timeout after 1ms
                if (evt.status == osEventMessage) {
                    DBG("Updating...");
                    update_sending_frame(network_output_buffer);
                    tcp_sock.send_all(network_output_buffer, SENDING_PROTOCOL_LENGTH);
                }

                // check to receive or timeout
                tcp_sock.set_blocking(false, TCP_CLIENT_RECEIVE_TIMEOUT);
                n = tcp_sock.receive(tcp_receiving_buffer, sizeof(tcp_receiving_buffer));
                if (n > 0) {
                    // got some data, test it
                    DBG("TCP client received %d bytes: %s", n, tcp_receiving_buffer);
                    process_control_command(tcp_receiving_buffer, n);
                }
            }
        } // if tcp client enabled && auto transmit
#endif


#ifdef TCP_SERVER // control and monitor from a remote TCP client, both NNIO and RPC-style
        // no tcp client connected{
        if (1) {
            // wait for client within timeout
            ret = tcp_server.accept(tcp_client);

            // tcp client connected
            if (ret > -1) {
                DBG("Connection from: %s", tcp_client.get_address());

                // loop waiting and receiving data within timeout
                tcp_client.set_blocking(false, TCP_SERVER_RECEIVE_TIMEOUT); // Timeout after x seconds
                while (tcp_client.is_connected()) {
                    n = tcp_client.receive(tcp_receiving_buffer, sizeof(tcp_receiving_buffer));
                    if (n <= 0) break;

                    // got some data, process it
                    tcp_receiving_buffer[n] = '\0'; // for debugging purpose
                    DBG("TCP server received: %s", tcp_receiving_buffer);
                    n = process_control_command(tcp_receiving_buffer, n);
                    // send reply back to client, NNIO protocol always returns 0
                    // RPC-style protocol
                    if (n > 0) {
                        network_output_buffer[n] = '\0';
                        tcp_client.send_all(network_output_buffer, strlen(network_output_buffer));
                    } // RPC-style protocol
                    else if (n == 0) {
                        // then, check query status command and sending protocol if required
                        if (tcp_receiving_buffer[RECEIVING_PROTOCOL_COMMAND_POS] == QUERY_STATUS_COMMAND) {
                            DBG("Requested to send device status through TCP");
                            // sending protocol
                            update_sending_frame(network_output_buffer);
                            tcp_client.send_all(network_output_buffer, SENDING_PROTOCOL_LENGTH);
                            DBG("Sent");
                        }
                    } // NNIO protocol
                } // end loop if no data received within timeout
            } // if client connected
            tcp_client.close();
        } // if tcp server enabled && no client connected
#endif

#ifdef UDP_SERVER // configuration and control, both NNIO and RPC-style
        bool discovery_mode_flag, config_mode_flag;

        n = udp_server.receiveFrom(ep_udp_client, udp_receiving_buffer, sizeof(udp_receiving_buffer));

        // check to see if it is a query command
        // if yes, is it a discovery command or an ip query to enter config mode
        discovery_mode_flag = false;
        config_mode_flag = false;
        if ((n == QUERY_NETWORK_CONFIG_CMD_LENGTH) && (strstr(udp_receiving_buffer, QUERY_DISCOVERY_CMD) != NULL)) {
            discovery_mode_flag = true;
            DBG("Received discovery command");
            char str[50];
            sprintf(str, "%s%s%s", DEVICE_DESCRIPTION, eth.getMACAddress(), eth.getIPAddress());
            udp_server.sendTo(ep_udp_client, str, strlen(str));
        } // NNCFDS
        else if ((n == QUERY_NETWORK_CONFIG_CMD_LENGTH) && (strstr(udp_receiving_buffer, QUERY_IP_CMD) != NULL)) {
            config_mode_flag = true;
            DBG("Entered configuration mode...");
            DBG("!!! RESET when finished");
        } // NNCFIP

        // if received NNCFIP, enter config mode
        if (config_mode_flag) {
            while (n > 0) {
                // got some data, test it
                DBG("UDP received (%s) from (%s:%d)", udp_receiving_buffer, ep_udp_client.get_address(), ep_udp_client.get_port());
                process_config_command(udp_receiving_buffer, n);
                // wait to receive new config command
                udp_server.set_blocking(true);
                n = udp_server.receiveFrom(ep_udp_client, udp_receiving_buffer, sizeof(udp_receiving_buffer));
            } // while (n > 0), config loop
        } // if (config_mode_flag)
        // process control packages sent using UDP
        else if ((n > 0) && (!discovery_mode_flag)) {
            n = process_control_command(udp_receiving_buffer, n);
            // send rpc reply back to client, NNIO protocol always returns 0
            // RPC-style protocol
            if (n > 0) {
                network_output_buffer[n] = '\0';
                udp_server.sendTo(ep_udp_client, network_output_buffer, strlen(network_output_buffer));
            } // RPC-style protocol
            else if (n == 0) {
                // then, check query status command and sending protocol if required
                if (udp_receiving_buffer[RECEIVING_PROTOCOL_COMMAND_POS] == QUERY_STATUS_COMMAND) {
                    DBG("Requested to send device status through UDP");
                    // sending protocol
                    update_sending_frame(network_output_buffer);
                    udp_server.sendTo(ep_udp_client, network_output_buffer, SENDING_PROTOCOL_LENGTH);
                    DBG("Sent");
                }
            }
        }
#endif
    } // network processor
}


/*
 * Process NNCF commands
 */
void process_config_command(char* received_buffer, int len)
{
    DBG("Processing configuration command");

    // a configuration command always starts with NN
    if ((received_buffer[0] == 'N') && (received_buffer[1] == 'N') &&
            (received_buffer[2] == 'C') && (received_buffer[3] == 'F')) {
        switch (len) {
                // length = 6, a QUERY command (discovery command, TCP port, or UDP port)
                // Format: NNCFDS, NNCFTP, NNCFUP, NNCFTM
            case QUERY_NETWORK_CONFIG_CMD_LENGTH: {
                if (strstr(received_buffer, QUERY_IP_CMD) != NULL) {
                    udp_server.sendTo(ep_udp_client, eth.getIPAddress(), strlen(eth.getIPAddress()));
                } // NNCFIP
                else if (strstr(received_buffer, QUERY_SUBNET_CMD) != NULL) {
                    udp_server.sendTo(ep_udp_client, eth.getNetworkMask(), strlen(eth.getNetworkMask()));
                } // NNCFSN
                else if (strstr(received_buffer, QUERY_GATEWAY_CMD) != NULL) {
                    udp_server.sendTo(ep_udp_client, eth.getGateway(), strlen(eth.getGateway()));
                } // NNCFGW
                else if (strstr(received_buffer, QUERY_MAC_CMD) != NULL) {
                    udp_server.sendTo(ep_udp_client, eth.getMACAddress(), strlen(eth.getMACAddress()));
                } // NNCFMC
                // ask for TCP server port
                else if (strstr(received_buffer, QUERY_TCP_PORT_CMD) != NULL) {
                    char port[5];
                    sprintf(port, "%5d", tcp_server_local_port);
                    udp_server.sendTo(ep_udp_client, port, strlen(port));
                } // NNCFTP
                // ask for UDP server port
                else if (strstr(received_buffer, QUERY_UDP_PORT_CMD) != NULL) {
                    char port[5];
                    sprintf(port, "%5d", udp_server_local_port);
                    udp_server.sendTo(ep_udp_client, port, strlen(port));
                } // NNCFUP
                else if (strstr(received_buffer, QUERY_UPDATE_TIME_CMD) != NULL) {
#ifdef NTP
                    char str_time[50];

                    DBG("Trying to update time...");
                    if (ntp.setTime("0.pool.ntp.org") == 0) {
                        DBG("Set time successfully");
                        time_t ctTime;
                        ctTime = time(NULL);

                        DBG("Time is set to (UTC): %s", ctime(&ctTime));
                        sprintf(str_time, "%s", ctime(&ctTime));
                        udp_server.sendTo(ep_udp_client, str_time, strlen(str_time));
                    } else {
                        WARN("Error");
                        sprintf(str_time, "ERR");
                        udp_server.sendTo(ep_udp_client, str_time, strlen(str_time));
                    }
#elif
                    WARN("NTP disabled");
                    sprintf(str_time, "DIS");
                    udp_server.sendTo(ep_udp_client, str_time, strlen(str_time));
#endif
                } // NNCFTM
                break;
            }
            // length = 19, SET NETWORK CONFIGURATION
            // Format: 4E 4E 43 46      C0 A8 00 78        FF FF FF 00            C0 A8 00 01      00 00 01
            //        (NNCF;            IP: 192.168.0.120; Subnet: 255.255.255.0; GW: 192.168.0.1; MAC: 0 0 1)
            case SET_NETWORK_CONFIG_CMD_LENGTH: {
                // check device id
                char* id = strstr(received_buffer, DEVICE_CONFIG_CODE);
                if (id == NULL)
                    break;
                else if ((id - received_buffer) > 0)
                    break;

                DBG("Received user configuration");
                write_eeprom_network(&received_buffer[strlen(DEVICE_CONFIG_CODE)]); // parameters from 5th char, 15-bytes
                break;
            }
            // length = 12, SET TCP SERVER CONFIGURATION
            // auto update & its time period, TCP server configuration (IP & port)
            // Format: 4E 4E 43 46   'Y'     01   C0 A8 00 09   E0 2E (LSB MSB)
            //         NNCF          Auto    1s   192.168.0.9   12000
            case UPDATE_TCP_SERVER_INFO_COMMAND_LENGTH: {
                char* id = strstr(received_buffer, DEVICE_CONFIG_CODE);
                if (id == NULL)
                    break;
                else if ((id - received_buffer) > 0)
                    break;

                DBG("Received TCP server configuration");
                write_eeprom_tcpserver(&received_buffer[strlen(DEVICE_CONFIG_CODE)]); // parameters from 5th char
                break;
            }
            default:
                break;
        } // switch (n), check configuration command length
    } // if starts with NNCF, a config command
    else { // if not a configuration command
    }
}

/*
 * Procedure to process receiving protocol, which includes command to control outputs.
 * Support both NNIO and RPC-style commands.
 * RPC always starts with '/'.
 * Return:
 *      0 if NNIO protocol;
 *      length of rpc output buffer if rpc;
 *      -1 if rpc failed
 */
int process_control_command(char* received_buffer, int len)
{
    char* received_frame;
    bool rpc_style;
    int pos;
    char inbuf[NET_BUF_LEN];

    DBG("Processing control command");

    /*
     * This section is for RPC-style command
     */
    // check if it is a RPC-style command
    rpc_style = false;
    strncpy(inbuf, received_buffer, len);
    inbuf[len] = '\r'; // use inbuf for RPC protocol
    inbuf[len+1] = '\n';
    inbuf[len+2] = '\0'; // add CR-LF
    if ((len > 0) && (inbuf[0] == '/')) {
        char obj_name[32];
        bool result;

        rpc_style = true;
        // find RPC object name
        for (int i = 1; i < strlen(inbuf); i++) {
            if (inbuf[i] != '/') {
                obj_name[i-1] = inbuf[i];
            } else {
                obj_name[i-1] = '\0';
                break;
            }
        }
        DBG("Rpc command = %s", inbuf);
        /*
         * execute RPC command, return reply length and reply in rpc_outbuf
         */
        result = RPC::call(inbuf, network_output_buffer);
        if (result) {
            // re-arrange output buffer as following: object_name:output_value
            strcpy(inbuf, network_output_buffer); // use inbuf as temp
            strcpy(network_output_buffer, obj_name); // rpc object name
            strcat(network_output_buffer, ":");
            strcat(network_output_buffer, inbuf); // concat rpc reply
            strcat(network_output_buffer, "\r\n"); // CR-LF
            int j = strlen(network_output_buffer);
            DBG("Reply of rpc command on \"%s\" (%d bytes): %s", obj_name, j, network_output_buffer);
            return j; // return length of rpc_outbuf
        } else {
            ERR("Failed: %s", inbuf);
            return -1;
        }
    }

    /*
     * This section below is  for NNIO protocol
     */
    while ((!rpc_style) && (len >= RECEIVING_PROTOCOL_LENGTH)) {
        // find device ID
        DBG("Checking device ID...");
        char* id = strstr(received_buffer, DEVICE_CONTROL_CODE);
        if (id == NULL) {
            DBG("No device ID found");
            break;
        }
        pos = id - received_buffer;
        DBG("Found a frame at %d", pos);

        // extract this frame
        received_frame = &received_buffer[pos];
        // calculate the rest
        received_buffer = &received_buffer[pos + RECEIVING_PROTOCOL_LENGTH];
        len -= RECEIVING_PROTOCOL_LENGTH;

        // process this received frame
        // firstly, update outputs if required
        // digital outputs
        if (received_frame[RECEIVING_PROTOCOL_EN_DO_POS] == RECEIVING_PROTOCOL_ENABLE_OUTPUT) {
            DBG("Update digital outputs");
            char str_dout[9];
            memcpy(str_dout, &received_frame[RECEIVING_PROTOCOL_DO_POS], 8);
            str_dout[8] = '\0';
            update_digital_outputs(str_dout);
        }
        // analog output 0
        if (received_frame[RECEIVING_PROTOCOL_EN_A0O_POS] == RECEIVING_PROTOCOL_ENABLE_OUTPUT) {
            DBG("Update analog output 0");
            //TODO Update analog output
        }
        // analog output 1
        if (received_buffer[RECEIVING_PROTOCOL_EN_A1O_POS] == RECEIVING_PROTOCOL_ENABLE_OUTPUT) {
            DBG("Update analog output 1");
            //TODO Update analog output
        }
        // UART
        if (received_frame[RECEIVING_PROTOCOL_EN_UART_POS] == RECEIVING_PROTOCOL_ENABLE_OUTPUT) {
            DBG("UART data: ");
            char str_uart[33];
            memcpy(str_uart, &received_frame[RECEIVING_PROTOCOL_UART_POS], 32);
            str_uart[32] = '\0';
            uart.printf("%s\r\n", str_uart);
        }
    }

    DBG("Successfully processed.");
    return 0;
}


/*
* W5500 Ethernet init
*/
int ethernet_init(void)
{
    int dhcp_ret, ret;

    DBG("Initialising ethernet...");

    // if not configured, try dhcp
    dhcp_ret = -1;
    if (configured_ip != DEFAULT_ENABLE_FLAG_VALUE) {
        DBG("Connecting to DHCP server...");
        dhcp_ret = eth.init(u8mac);
        if (dhcp_ret == 0)
            dhcp_ret = eth.connect();
    }

    if (dhcp_ret != 0) {
        DBG("No DHCP, load static IP configuration");
        ret = eth.init(u8mac, str_ip_addr, str_ip_subnet, str_ip_gateway); // static
    } else {
        snprintf(str_ip_addr, 16, "%s", eth.getIPAddress());
        snprintf(str_ip_subnet, 16, "%s", eth.getNetworkMask());
        snprintf(str_ip_gateway, 16, "%s", eth.getGateway());
        ret = 0;
    }

    if (ret == 0) {
        DBG("Initialized, MAC: %s", eth.getMACAddress());
    } else {
        ERR("Error eth.init() - ret = %d", ret);
        return -1;
    }

    ret = eth.connect();
    if (!ret) {
        DBG("IP: %s, MASK: %s, GW: %s", eth.getIPAddress(), eth.getNetworkMask(), eth.getGateway());
    } else {
        ERR("Error eth.connect() - ret = %d", ret);
        return -1;
    }

    return 0;
}


/*
* Update digital outputs according to receiving protocol
*/
void update_digital_outputs(char* buf)
{
    DBG("Digital outputs: %s", buf);

    dout0 = (buf[0] == DIGITAL_HIGH)? 1 : 0;
    dout1 = (buf[1] == DIGITAL_HIGH)? 1 : 0;
    dout2 = (buf[2] == DIGITAL_HIGH)? 1 : 0;
    dout3 = (buf[3] == DIGITAL_HIGH)? 1 : 0;
    dout4 = (buf[4] == DIGITAL_HIGH)? 1 : 0;
    dout5 = (buf[5] == DIGITAL_HIGH)? 1 : 0;
    dout6 = (buf[6] == DIGITAL_HIGH)? 1 : 0;
    dout7 = (buf[7] == DIGITAL_HIGH)? 1 : 0;
}

/*
* Prepare a frame for sending protocol, which includes status of I/Os
*/
void update_sending_frame(char* buf)
{
    memcpy(&buf[SENDING_PROTOCOL_ID_POS], DEVICE_CONTROL_CODE, 4); // device id
    memcpy(&buf[SENDING_PROTOCOL_MAC_POS], &u8mac, 6);
    memcpy(&buf[SENDING_PROTOCOL_IP_POS], &u8ip_addr, 4);

    buf[SENDING_PROTOCOL_DI_POS+0] = (din0 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DI_POS+1] = (din1 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DI_POS+2] = (din2 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DI_POS+3] = (din3 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DI_POS+4] = (din4 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DI_POS+5] = (din5 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DI_POS+6] = (din6 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DI_POS+7] = (din7 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;

    buf[SENDING_PROTOCOL_DO_POS+0] = (dout0 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DO_POS+1] = (dout1 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DO_POS+2] = (dout2 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DO_POS+3] = (dout3 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DO_POS+4] = (dout4 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DO_POS+5] = (dout5 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DO_POS+6] = (dout6 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;
    buf[SENDING_PROTOCOL_DO_POS+7] = (dout7 == 1) ? DIGITAL_HIGH : DIGITAL_LOW;

    uint16_t val = ain0.read_u16(); // 16-bits normalised
    memcpy(&buf[SENDING_PROTOCOL_AI0_POS], &val, 2); // LSB MSB
    val = ain1.read_u16(); // 16-bits normalised
    memcpy(&buf[SENDING_PROTOCOL_AI1_POS], &val, 2); // LSB MSB
    val = 0x1234;
    memcpy(&buf[SENDING_PROTOCOL_AO0_POS], &val, 2); // LSB MSB
    val = 0x5678;
    memcpy(&buf[SENDING_PROTOCOL_AO1_POS], &val, 2); // LSB MSB
    buf[SENDING_PROTOCOL_CR_POS] = 0x0D;
    buf[SENDING_PROTOCOL_CR_POS+1] = '\0';
}