File content as of revision 2:990c985a69ae:

// ----------------------------------------------------------------------------
// Copyright 2019 PFP Cybersecurity.
//
// SPDX-License-Identifier: Apache-2.0
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ----------------------------------------------------------------------------
#ifndef MBED_TEST_MODE

#include "mbed.h"

// Default network interface object. Don't forget to change the WiFi SSID/password in mbed_app.json if you're using WiFi.
NetworkInterface *net = NetworkInterface::get_default_instance();

AnalogIn ain(A0);

///////////////////////////////////////////////////////////////////////////////////////
///
///                        PFP Main code start
///
//////////////////////////////////////////////////////////////////////////////////////
uint8_t PFP_REGISTER_DIGITIZER = 87;
uint8_t PFP_CLK_FREQ = 11;
uint8_t PFP_ADC_GET_RAW = 3;
uint8_t PFP_ADC_INIT = 0;
uint8_t PFP_TRIG_CONFIG = 26;
uint8_t PFP_TRIG_RECEIVED = 88;

struct DeviceData {
    int channel;

    int numberOfTraces;
    int traceLength;

    int sampleRate;
    int digitizer;

    int trigMode;
    int trigSource;

    float trigLevel;
    float trigHyst;

    int trigPercentage;
    int gain;
};

struct DeviceData deviceData;

int bytesToInt(int8_t b0, int8_t b1, int8_t b2, int8_t b3)
{
    int value = (b3 << 24) & 0xff000000
                | (b2 << 16) & 0x00ff0000
                | (b1 << 8) & 0x0000ff00
                | (b0 << 0) & 0x000000ff;
    return value;
}

float bytesToFloat(int8_t b0, int8_t b1, int8_t b2, int8_t b3)
{

    float val = 0;
    unsigned long result = 0;
    result |= ((unsigned long) (b0) << 0x18);
    result |= ((unsigned long) (b1) << 0x10);
    result |= ((unsigned long) (b2) << 0x08);
    result |= ((unsigned long) (b3));
    memcpy(&val, &result, 4);

    return val;
}

int8_t * intToBytes(int value)
{
    int8_t *b = (int8_t*) calloc(4, sizeof(int8_t));

    b[0] = ((int8_t) (value >> 0));
    b[1] = ((int8_t) (value >> 8));
    b[2] = ((int8_t) (value >> 16));
    b[3] = ((int8_t) (value >> 24));
    return b;
}

int8_t * longToBytes(long value)
{
    static int8_t b[8];
    b[0] = (int8_t) ((value >> 0) & 0xFF);
    b[1] = (int8_t) ((value >> 8) & 0xFF);
    b[2] = (int8_t) ((value >> 16) & 0xFF);
    b[3] = (int8_t) ((value >> 24) & 0xFF);
    b[4] = (int8_t) ((value >> 32) & 0xFF);
    b[5] = (int8_t) ((value >> 40) & 0xFF);
    b[6] = (int8_t) ((value >> 48) & 0xFF);
    b[7] = (int8_t) ((value >> 56) & 0xFF);
    return b;
}

int8_t * pfp_emon_create_ack_for_client(int commandType, int numberOfBytes)
{
    static int8_t b[64];

    int8_t *totalBytes = intToBytes(numberOfBytes);
    int8_t *successBytes = intToBytes(3);
    int8_t *returnBytes = intToBytes(commandType);
    int8_t *totaTrace = intToBytes(numberOfBytes / 2);

    // EMON HEADER
    b[0] = 69;
    b[1] = 77;
    b[2] = 79;
    b[3] = 78;

    // NUMBER OF BYTES
    b[4] = totalBytes[0];
    b[5] = totalBytes[1];
    b[6] = totalBytes[2];
    b[7] = totalBytes[3];

    // ERROR
    b[8] = 0;
    b[9] = 0;
    b[10] = 0;
    b[11] = 0;

    // SKIP BYTES
    b[12] = 0;
    b[13] = 0;
    b[14] = 0;
    b[15] = 0;

    // SUCCESS COMMAND
    b[16] = successBytes[0];
    b[17] = successBytes[1];
    b[18] = successBytes[2];
    b[19] = successBytes[3];

    // RETURN COMMAND
    b[20] = returnBytes[0];
    b[21] = returnBytes[1];
    b[22] = returnBytes[2];
    b[23] = returnBytes[3];

    // SKIP BYTES
    b[24] = 0;
    b[25] = 0;
    b[26] = 0;
    b[27] = 0;

    // TOTAL TRACE
    b[28] = totaTrace[0];
    b[29] = totaTrace[1];
    b[30] = totaTrace[2];
    b[31] = totaTrace[3];


    free(totalBytes);
    free(successBytes);
    free(returnBytes);
    free(totaTrace);


    return b;
}


void startEmonThread(void const *args)
{

    printf("The target IP address is '%s'\r\n", net->get_ip_address());

    TCPServer srv;
    TCPSocket clt_sock;
    SocketAddress clt_addr;

    /* Open the server on ethernet stack */
    srv.open(net);

    /* Bind the HTTP port (TCP 80) to the server */
    srv.bind(net->get_ip_address(), 7001);

    long traceTrack = 0;
    float sleepCore = 0.001;


    //srv.set_blocking(false);
    while (true) {
        /* Can handle 5 simultaneous connections */
        int err= srv.listen(1);
        printf("server listening error : %d\r\n",err);

        while(1) {

            printf("waiting for client connection\r\n");
            err = srv.accept(&clt_sock, &clt_addr);
            if(err == 0) {
                printf("client connected :%s:%d\r\n", clt_addr.get_ip_address(), clt_addr.get_port());
                int MAX_LEN = 80;
                int8_t bytes[MAX_LEN];
                int32_t rawDataLen = 2048;
                int16_t *rawData = (int16_t*)calloc(rawDataLen, sizeof(int16_t));
                int16_t MAX_TRANSMIT_DATA = 500;
                int16_t data[MAX_TRANSMIT_DATA];
                traceTrack = 0;

                bool debug = false;

                while(1) {

                    ////////////////
                    int len = clt_sock.recv(bytes, MAX_LEN);
                    if(debug) {
                        printf("====================> len = %i <======================\n",len);
                    }

                    if (len < 1) {
                        printf("Connection is closed....\n");
                        break;
                    }

                    int commandType = 0;
                    int commandLength = 0;
                    int indexOffset = 0;

                    int8_t *sendBytes = NULL;

                    if (len < 0) {
                        return;
                    }
                    if (len == 8) {
                        commandType = bytesToInt(bytes[0], bytes[1], bytes[2], bytes[3]);
                    } else if (len == 12) {
                        commandType = bytesToInt(bytes[0], bytes[1], bytes[2], bytes[3]);
                        commandLength = bytesToInt(bytes[4], bytes[5], bytes[6], bytes[7]);
                        if (commandType == PFP_REGISTER_DIGITIZER) {

                            deviceData.digitizer = bytesToInt(bytes[8], bytes[9], bytes[10],
                                                              bytes[11]);

                            //  send_PFP_REGISTER_DIGITIZER(clt_sock);

                        } else if (commandType == PFP_CLK_FREQ) {
                            deviceData.sampleRate = bytesToInt(bytes[8], bytes[9],
                                                               bytes[10], bytes[11]);
                            //  send_PFP_CLK_FREQ(clt_sock);
                        } else if (commandType == PFP_ADC_GET_RAW) {
                            deviceData.channel = bytesToInt(bytes[8], bytes[9], bytes[10],
                                                            bytes[11]);
                        }
                        if(debug) {
                            printf("====================> len 12 commandType= %i <======================\n",commandType);
                            printf("====================> len 12 commandLength= %i <======================\n",commandLength);
                        }

                    } else {
                        commandType = bytesToInt(bytes[0], bytes[1], bytes[2], bytes[3]);
                        commandLength = bytesToInt(bytes[4], bytes[5], bytes[6], bytes[7]);
                        if (commandLength == 8) {
                            if ((len == 60) || (len == 72)) {
                                commandType =bytesToInt(bytes[8], bytes[9], bytes[10], bytes[11]);
                                commandLength = bytesToInt(bytes[12], bytes[13], bytes[14], bytes[15]);
                                indexOffset = 8;
                                if(debug) {
                                    printf("====================> inside commandType= %i <======================\n",commandType);
                                    printf("====================> inside commandLength= %i <======================\n",commandLength);
                                }
                            }
                        } else {
                            if(debug) {
                                printf("====================> outside commandType= %i <======================\n",commandType);
                                printf("====================> outside commandLength= %i <======================\n",commandLength);
                            }
                        }
                    }

                    // Got command form client. Send back header
                    if (commandType == PFP_ADC_INIT) {
                        sendBytes = pfp_emon_create_ack_for_client(commandType, 0);

                        if (sendBytes != NULL) {
                            clt_sock.send(sendBytes,64);
                            wait(sleepCore);
                        }
                    }

                    if (commandType == PFP_REGISTER_DIGITIZER) {
                        sendBytes = pfp_emon_create_ack_for_client(commandType, 0);

                        if (sendBytes != NULL) {
                            clt_sock.send(sendBytes,64);
                            wait(sleepCore);
                        }
                    }
                    if (commandType == PFP_TRIG_CONFIG) {

                        if ((commandLength > 12) && (commandLength < 1000)) {

                            deviceData.channel = bytesToInt(bytes[indexOffset+8], bytes[indexOffset+9], bytes[indexOffset+10],
                                                            bytes[indexOffset+11]);
                            deviceData.traceLength = bytesToInt(bytes[indexOffset+12], bytes[indexOffset+13],
                                                                bytes[indexOffset+14], bytes[indexOffset+15]);

                            deviceData.trigMode = bytesToInt(bytes[indexOffset+16], bytes[indexOffset+17],
                                                             bytes[indexOffset+18], bytes[indexOffset+19]);
                            deviceData.trigSource = bytesToInt(bytes[indexOffset+20], bytes[indexOffset+21],
                                                               bytes[indexOffset+22], bytes[indexOffset+23]);

                            deviceData.trigLevel = bytesToFloat(bytes[indexOffset+24], bytes[indexOffset+25],
                                                                bytes[indexOffset+26], bytes[indexOffset+27]);
                            deviceData.trigHyst = bytesToFloat(bytes[indexOffset+28], bytes[indexOffset+29],
                                                               bytes[indexOffset+30], bytes[indexOffset+31]);

                            deviceData.trigPercentage = bytesToInt(bytes[indexOffset+32], bytes[indexOffset+33],
                                                                   bytes[indexOffset+34], bytes[indexOffset+35]);
                            deviceData.gain = bytesToInt(bytes[indexOffset+36], bytes[indexOffset+37], bytes[indexOffset+38],
                                                         bytes[indexOffset+39]);
                            if(debug) {
                                printf("channel is %i\n", deviceData.channel);
                                printf("traceLength is %i\n", deviceData.traceLength);
                                printf("trigMode is %f\n", deviceData.trigLevel);
                                printf("trigSource is %i\n", deviceData.trigSource);
                                printf("trigLevel is %f\n", deviceData.trigLevel);
                                printf("trigHyst is %f\n", deviceData.trigHyst);
                                printf("trigPercentage is %i\n", deviceData.trigPercentage);
                                printf("gain is %i\n", deviceData.gain);
                                printf("sample rate is %i\n", deviceData.sampleRate);
                                printf("gain is %i\n", deviceData.gain);
                            }

                            commandType = PFP_TRIG_RECEIVED;
                            sendBytes = pfp_emon_create_ack_for_client(commandType,
                                        deviceData.traceLength * 2);

                            if (sendBytes != NULL) {
                                clt_sock.send(sendBytes,64);
                            }

                            int numOfInteration = (len - (indexOffset+40))/4;

                            for(int i=0; i<numOfInteration; i++) {
                                int index2 = i*4;
                                int b0 = indexOffset+40+index2;
                                int b1 = indexOffset+41+index2;
                                int b2 = indexOffset+42+index2;
                                int b3 = indexOffset+43+index2;
                                int v = bytesToInt(bytes[b0], bytes[b1], bytes[b2],bytes[b3]);
                                if(debug) {
                                    printf("Trailing value %i\n", v);
                                }
                            }

                            if(numOfInteration==6) {

                                commandType = bytesToInt(bytes[indexOffset+40], bytes[indexOffset+41], bytes[indexOffset+42],bytes[indexOffset+43]);
                                commandLength = bytesToInt(bytes[indexOffset+44], bytes[indexOffset+45], bytes[indexOffset+46],bytes[indexOffset+47]);
                                int sampleRate = bytesToInt(bytes[indexOffset+48], bytes[indexOffset+49], bytes[indexOffset+50],bytes[indexOffset+51]);
                                deviceData.sampleRate = sampleRate;

                                if (commandType == PFP_CLK_FREQ) {
                                    sendBytes = pfp_emon_create_ack_for_client(commandType, 0);
                                    if (sendBytes != NULL) {
                                        clt_sock.send(sendBytes,64);
                                    }
                                }
                                wait(sleepCore);

                                commandType = bytesToInt(bytes[indexOffset+52], bytes[indexOffset+53], bytes[indexOffset+54],bytes[indexOffset+55]);
                                commandLength = bytesToInt(bytes[indexOffset+56], bytes[indexOffset+57], bytes[indexOffset+58],bytes[indexOffset+59]);
                                //  int sampleRate = bytesToInt(bytes[indexOffset+40], bytes[indexOffset+41], bytes[indexOffset+42],bytes[indexOffset+43]);
                            }


                        }
                    }
                    if (commandType == PFP_CLK_FREQ) {
                        sendBytes = pfp_emon_create_ack_for_client(commandType, 0);
                        if (sendBytes != NULL) {
                            clt_sock.send(sendBytes,64);
                            wait(sleepCore);
                        }

                        if(len==24) {
                            commandType = PFP_ADC_GET_RAW;
                        }

                    }


                    if (commandType == PFP_ADC_GET_RAW) {
                        sendBytes = pfp_emon_create_ack_for_client(PFP_TRIG_RECEIVED,
                                    deviceData.traceLength * 2);

                        if (sendBytes != NULL) {
                            clt_sock.send(sendBytes,64);
                        }

                        sendBytes = pfp_emon_create_ack_for_client(commandType,
                                    deviceData.traceLength * 2);
                        if (sendBytes != NULL) {
                            clt_sock.send(sendBytes,64);
                        }

                        if (deviceData.traceLength != rawDataLen) {
                            printf("Data size change to %i===============>\n",rawDataLen);
                            free(rawData);
                            rawDataLen = deviceData.traceLength;
                            rawData = (int16_t*)calloc(rawDataLen, sizeof(int16_t));
                        }

                        // Store raw data

                        int val=0;

                        for (int i = 0; i < rawDataLen; i++) {
                            val = ain.read_u16() - 32768;
                            if(val>32762) {
                                val = 32762;
                            } else if(val<-32762) {
                                val = -32762;
                            }

                            rawData[i] =val;
                        }

                        int num = rawDataLen/MAX_TRANSMIT_DATA;
                        int startIndex = 0;
                        for(int k =0; k<num; k++) {
                            startIndex = k*MAX_TRANSMIT_DATA;
                            for(int i=0; i<MAX_TRANSMIT_DATA; i++) {
                                data[i] = rawData[i + startIndex];
                            }
                            clt_sock.send(data,MAX_TRANSMIT_DATA*sizeof(int16_t));
                            wait(sleepCore);
                        }

                        int leftOver = rawDataLen - num*MAX_TRANSMIT_DATA;

                        if(leftOver>0) {
                            startIndex = num*MAX_TRANSMIT_DATA;

                            for(int j=startIndex; j<rawDataLen; j++) {
                                int i = j-startIndex;
                                data[i] = rawData[j];
                            }
                            clt_sock.send(data,leftOver * sizeof(int16_t));
                        }
                        traceTrack++;
                        if(traceTrack>100000000) {
                            traceTrack = 0;
                        }
                        if(debug) {
                            printf("<================== Trace Count is %ld ===============>\n",traceTrack);
                        }
                    }
                }

                free(rawData);
            }
        }
    }
}

///////////////////////////////////////////////////////////////////////////////////////
///
///                        PFP Main code end
///
//////////////////////////////////////////////////////////////////////////////////////

int main(void)
{
    printf("Connecting to the network using the default network interface...\n");
    net = NetworkInterface::get_default_instance();

    nsapi_error_t net_status = NSAPI_ERROR_NO_CONNECTION;
    while ((net_status = net->connect()) != NSAPI_ERROR_OK) {
        printf("Unable to connect to network (%d). Retrying...\n", net_status);
    }

    Thread thread(startEmonThread);

    while (true) {
        wait(0.001);
    }
}

#endif /* MBED_TEST_MODE */