sniffer.c

#include "sdk30.h"
#include "ip_.h"

int logSockData(char* data, int Size, struct sockaddr_in *from);
int isMoreLogSize(int size);
int openLogFile(void);


static struct sockaddr_in server, socaddr_tmp;
static struct sockaddr_in client_cash;
static struct sockaddr_in client_pos;
int my_udp_socket = -1;
S_FS_FILE *file = NULL;


/**
 * Read c string to \n charachter or to num-1
 * @param char* dst : Distination string
 * @param unsigned int num : number of chars to be read
 * @param S_FS_FILE *pxFile : File stream
 * @return int : 0 if end of the file, 1 else
 * @note none
 */
int my_fgets(char* dst, int num, S_FS_FILE *pxFile) {
    int i = 0;
    int iRet = 0;
    char ch = 0;

    if(dst == NULL || num <= 0 || pxFile == NULL)
        return 0;
    else if(num == 1){
        dst[0] = '\0';
        return 1;
    }

    iRet = FS_read(&ch, 1, 1, pxFile);
    if(iRet == 0) return 0;

     do{
        if( ch == '\0' || ch == '\n') {
            break;
        } else if( ch=='\r' ) {
            continue;
        }
        dst[i] = ch;
        i++;
    }while( i < (num - 1) && FS_read(&ch, 1, 1, pxFile) != 0);

    dst[i] = '\0';
    return 1;
}

/**
 * \brief close udp socket
 * \param ctx  channel context
 * \return true if udp socket closed
 */
static int CloseUdp(void) {
    if( my_udp_socket == -1 )
    	return 1;

    if( closesocket(my_udp_socket) == 0)  {
    	my_udp_socket = -1;
        return 1;
    }
    return 0;
}

/**
 * \brief open udp socket
 * \param ctx udp channel context
 * \return
 *  - 0 if create socket success
 *  - -1 if error occur
 */
static int OpenUdp(void) {
    // try to reopen udp channel
    if( my_udp_socket != -1 )
        CloseUdp();

    my_udp_socket = socket(AF_INET, SOCK_DGRAM, 0);
    if(my_udp_socket < 0) {
        return 0;
    }
    return 1;
}

int initUdpParams(void)
{
	int nMode;
	S_FS_FILE *f = NULL;
	int res = FS_mount( "/HOST", &nMode );
	if(FS_OK == res )
	{
	    f = FS_open("/HOST/SOCK_CONFIG.INI", "r" );
		if( NULL != f )
		{
			char buf[64] = {0};
			char *pos = NULL;

			res = my_fgets(buf, sizeof(buf), f);
			if(!res) goto lblFSerror;
			pos = strchr(buf, ':');
			if(pos != NULL )
			{
				server.sin_family = AF_INET;
				server.sin_port = htons(atoi(pos+1));
				server.sin_addr.s_addr = INADDR_ANY;       //__inet_addr(buf);
			}

			memset(&buf, 0, sizeof(buf));

			res = my_fgets(buf, sizeof(buf), f);
			if(!res) goto lblFSerror;
			pos = strchr(buf, ':');
			if(pos != NULL )
			{
				client_cash.sin_family = AF_INET;
				client_cash.sin_port = htons(atoi(pos+1));
				client_cash.sin_addr.s_addr = __inet_addr(buf);
			}

			memset(&buf, 0, sizeof(buf));

			res = my_fgets(buf, sizeof(buf), f);
			if(!res) goto lblFSerror;
			pos = strchr(buf, ':');
			if(pos != NULL )
			{
				client_pos.sin_family = AF_INET;
				client_pos.sin_port = htons(atoi(pos+1));
				client_pos.sin_addr.s_addr = __inet_addr(buf);
			}

			FS_close(f);
		}
	}
	res = OpenUdp();
	if(res != 1) goto lblFSerror;

	res = openLogFile();

	return 1;
lblFSerror:
	FS_unmount( "/HOST" );
	if(f)
	  FS_close(f);
	return 0;
}

/**
 * \brief write data to udp
 *
 * \param ctx channel context
 * \param buffer pointer to buffer with data
 * \param size data size
 * \return
 *  - size of data sended into serial port
 *  - -1 if error occur
 */
static int sendUdpTo(int _socket, const void* buffer, size_t size,  struct sockaddr_in *_to) {

    size = sendto(_socket, buffer, size, 0, (struct sockaddr *)_to, sizeof(struct sockaddr));

    return size;
}

static int recvUdpFrom(int _socket, void* buffer, size_t size, struct sockaddr_in *_from, int *_fromlen) {

    size = recvfrom(_socket, buffer, size, 0, (struct sockaddr *)_from, _fromlen);

    return size;
}


static int routeSockData(void){
	char message[512] = {0};
	int c = 0;
    int read_size = 0;


    if (bind(my_udp_socket, (struct sockaddr *) &server, sizeof (server)) < 0) {
        return 0;
    }

    while ((read_size = recvUdpFrom(my_udp_socket, message, sizeof(message), &socaddr_tmp, &c)) > 0) {

    	if(read_size > sizeof(message)){
    		continue;
    	}
    	if(isMoreLogSize(read_size*2)){
    		logSockData(message, read_size, &socaddr_tmp);
    	}
        if(!memcmp(&socaddr_tmp.sin_port, &client_cash.sin_port, sizeof(unsigned short))
        		&& memcmp(&socaddr_tmp.sin_addr, &client_cash.sin_addr, sizeof(unsigned int))){
        	sendUdpTo(my_udp_socket, message, read_size, &client_pos);
        }
        else if(!memcmp(&socaddr_tmp.sin_port, &client_pos.sin_port, sizeof(unsigned short))
        		&& memcmp(&socaddr_tmp.sin_addr, &client_pos.sin_addr, sizeof(unsigned int))){
        	sendUdpTo(my_udp_socket, message, read_size, &client_cash);
        }
        else{
        	// Unknown message received
        }

        memset(&message, 0, sizeof (message));
        memset(&socaddr_tmp, 0, sizeof (socaddr_tmp));
    }

    return 1;
}

int openLogFile(void){
    file = FS_open("/HOST/SOCK_LOG.TXT", "a+");
    if(!file){
        return 0;
    }
    return 1;
}

int isMoreLogSize(int size){

	static int more_size = 1;
	long ret = 0;

	if(!more_size){
		return 0;
	}

	ret = FS_dskfree("/HOST");

	if(ret == FS_ERROR){
		return 0;
	}
	else if(ret <= size){
		more_size = 0;
		FS_close(file);
		file = NULL;
	}

	return more_size;
}


int logSockData(char* data, int Size, struct sockaddr_in *from) {
    char a, c;
    char line[17] = {0};
    char raw[12] = {0};
    char buf[128] = {0};
    int j, i;
    unsigned short port = 0;
    char ip_addr[16] = {0};
    char *ip_addr_pnt = NULL;

    if (data == NULL || from == NULL || file == NULL ) {
        return 0;
    }

    ip_addr_pnt = __inet_ntoa(from->sin_addr);
    if (ip_addr_pnt != NULL)
        strcpy(ip_addr, ip_addr_pnt);
    port = ntohs(from->sin_port);

    snprintf(buf, sizeof (buf), "--------Received from ip %s:%u--------\n\r", ip_addr, port);
    FS_write(buf, sizeof(char), strlen(buf), file);

    for (i = 0; i < Size; i++) {
        c = data[i];
        snprintf(raw, sizeof (raw), "%.2x \0", (unsigned char) c);
        FS_write(raw, sizeof(char), strlen(raw), file);
        //memset(raw, 0, sizeof(raw));
        a = (c >= 32 && c <= 128) ? (unsigned char) c : '.';
        line[i % 16] = a;

        if ((i != 0 && (i + 1) % 16 == 0) || i == Size - 1) {
            line[i % 16 + 1] = '\0';
            snprintf(raw, sizeof (raw), "          \0");
            FS_write(raw, sizeof(char), strlen(raw), file);
            //memset(raw, 0, sizeof(raw));
            for (j = strlen(line); j < 16; j++) {
                snprintf(raw, sizeof (raw), "   \0");
                FS_write(raw, sizeof(char), strlen(raw), file);
                //memset(raw, 0, sizeof(raw));
            }

            FS_write(line, sizeof(char), strlen(line), file);
            snprintf(raw, sizeof (raw), " \n\r\0");
            FS_write(raw, sizeof(char), strlen(raw), file);
            //memset(raw, 0, sizeof(raw));
        }
    }
    snprintf(raw, sizeof (raw), "\n\r\0");
    FS_write(raw, sizeof(char), strlen(raw), file);
    //memset(raw, 0, sizeof(raw));

    return 1;
}