Agronode_feeder/feeder_lib/vhcp/vhcp.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <stdint.h>
#include <syslog.h>
#include <stdarg.h>
#include <signal.h>

#include "uthash/uthash.h"

#include "../../status.h"
#include "../../feeder.h"




typedef struct vhcp_vars_t
{
	uint8_t delimiter;
	uint8_t count;
	uint16_t var[14];
} __attribute__((packed)) vhcp_vars_t;

typedef struct vhcp_dpacket_t
{
	uint32_t no;
	uint16_t id;
	uint32_t timestamp;
	uint32_t lat;
	uint32_t lon;
	uint8_t sgps;
	uint16_t azimuth;
	uint8_t speed;
	uint8_t din;
	uint8_t dout;
	uint32_t distance;
	uint16_t analog1;
	uint16_t analog2;
	uint16_t fuel;
	uint16_t credit;
	uint8_t max_speed;
	uint8_t avg_speed;
	uint16_t overspeed_time;
	union vhcp_text_t
	{
		char text[240];
		struct vhcp_vars_t vars;
	} text;
} __attribute__((packed)) vhcp_dpacket_t;

typedef struct vhcp_data_ack_t
{
    uint32_t ack_val;
    uint16_t id;
} __attribute__((packed)) vhcp_data_ack_t;

typedef struct vhcp_ack_t
{
    uint32_t ack_val;
} __attribute__((packed)) vhcp_ack_t;

typedef struct vhcp_send_older_data_t
{
    uint32_t from_packet_no;
    uint32_t to_packet_no;
} __attribute__((packed)) vhcp_send_older_data_t;

typedef struct vhcp_packet_t
{
	uint8_t delimiter;
	uint8_t type;
	union vhcp_data_t
	{
	    struct vhcp_dpacket_t dpacket;
	    struct vhcp_data_ack_t dack;
	    struct vhcp_send_older_data_t sendolderdata;
	    struct vhcp_ack_t ack;
	} data;
} __attribute__((packed)) vhcp_header_t;





typedef struct vhcp_unit_t
{
    int id;
    uint32_t last_packet_no;
    uint32_t current_packet_no;
    UT_hash_handle hh;
} vhcp_unit_t;

static vhcp_unit_t * units = NULL;



#define VHCP_DELIMITER '#'

#define VHCP_SEND_OLDER_DATA_PACKETS 'R'
#define VHCP_OPEN_CMD_CHANNEL 'C'
#define VHCP_STOP_DATA_XFER 'S'
#define VHCP_START_DATA_XFER 'G'
#define VHCP_TERMINAL_MESSAGE 'M'
#define VHCP_START_FILE_XFER 'F'
#define VHCP_ERASE_FILE 'E'
#define VHCP_UTC_TIME 'U'
#define VHCP_ACK 'A'

//O postfix for online, D postfix for database
#define VHCP_ENGINE_START_O 'B'
#define VHCP_ENGINE_STOP_O 'E'
#define VHCP_DRIVE_O 'D'
#define VHCP_DRIVER_LOGIN_O 'C'
#define VHCP_CARGO_LOAD_O 'F'
#define VHCP_CARGO_UNLOAD_O 'U'
#define VHCP_DRIVER_REST_O 'R'
#define VHCP_SAFETY_PAUSE_O 'P'
#define VHCP_FAST_DRIVE_START_O 'Q'
#define VHCP_FAST_DRIVE_STOP_O 'N'
#define VHCP_TEXT_INFO_O 'I'
#define VHCP_MOVEMENT_START_O 'G'
#define VHCP_MOVEMENT_STOP_O 'S'
#define VHCP_UNAUTHORIZED_MOVEMENT_O 'T'
#define VHCP_ACTIVITY_CODE_O 'O'
#define VHCP_TEXT_MESSAGE_O 'M'

#define VHCP_ENGINE_START_D 'b'
#define VHCP_ENGINE_STOP_D 'e'
#define VHCP_DRIVE_D 'd'
#define VHCP_DRIVER_LOGIN_D 'c'
#define VHCP_CARGO_LOAD_D 'f'
#define VHCP_CARGO_UNLOAD_D 'u'
#define VHCP_DRIVER_REST_D 'r'
#define VHCP_SAFETY_PAUSE_D 'p'
#define VHCP_FAST_DRIVE_START_D 'q'
#define VHCP_FAST_DRIVE_STOP_D 'n'
#define VHCP_TEXT_INFO_D 'i'
#define VHCP_MOVEMENT_START_D 'g'
#define VHCP_MOVEMENT_STOP_D 's'
#define VHCP_UNAUTHORIZED_MOVEMENT_D 't'
#define VHCP_ACTIVITY_CODE_D 'o'
#define VHCP_TEXT_MESSAGE_D 'm'

#define VHCP_DATA_ACK 'V'


#define VHCP_ACK_OLDER_PACKET 4
#define VHCP_BUFFER_SIZE 512
static uint8_t buffer[VHCP_BUFFER_SIZE];

#define VHCP_STATE_IDLE 0
#define VHCP_STATE_POS 1
#define VHCP_STATE_OBS 2
#define VHCP_STATE_DACK 3
#define VHCP_STATE_PREQ 4

static uint8_t state = VHCP_STATE_IDLE;


static void (* feederLog)(int priority, const char * fmt, ...);

int setLog(void * func)
{
    feederLog = func;
    return 0;
}

uint16_t byteSwap16(uint16_t value)
{
    uint16_t swapped;
    swapped = (((0x00FF) & (value >> 8)) |
              ((0xFF00) & (value << 8)));
    return swapped;
}

uint32_t byteSwap32(uint32_t value)
{
    uint32_t swapped;
    swapped = (((0x000000FF) & (value >> 24)) |
              ((0x0000FF00) & (value >> 8)) |
              ((0x00FF0000) & (value << 8)) |
              ((0xFF000000) & (value << 24)));
    return swapped;
}

uint64_t byteSwap64(uint64_t value)
{
    uint64_t swapped;
    swapped = (((0x00000000000000FFULL) & (value >> 56)) |
              ((0x000000000000FF00ULL) & (value >> 40)) |
              ((0x0000000000FF0000ULL) & (value >> 24)) |
              ((0x00000000FF000000ULL) & (value >> 8)) |
              ((0x000000FF00000000ULL) & (value << 8)) |
              ((0x0000FF0000000000ULL) & (value << 24)) |
              ((0x00FF000000000000ULL) & (value << 40)) |
              ((0xFF00000000000000ULL) & (value << 56)));
    return swapped;
}

int save(void)
{
    FILE *fd;
    struct vhcp_unit_t * unit;
    
    fd = fopen("vhcp.dump", "w");
    if (fd != NULL)
    {
	for (unit = units; unit != NULL; unit = unit->hh.next)
	{
	    feederLog(LOG_DEBUG, "vhcp: Dump unit %d, current packet no %u\n", unit->id, unit->current_packet_no);

	    fwrite(unit, sizeof(struct vhcp_unit_t), 1, fd);
	}
        fclose(fd);
    }
    return 0;
}

int load(void)
{
    FILE *fd;
    struct vhcp_unit_t unit_file, * unit;
    
    fd = fopen("vhcp.dump", "r");
    if (fd != NULL)
    {
	while (fread(&unit_file, sizeof(struct vhcp_unit_t), 1, fd) > 0)
	{
    	    unit = (struct vhcp_unit_t *)malloc(sizeof(struct vhcp_unit_t));
	    if (unit != NULL)
	    {
        	memcpy(unit, &unit_file, sizeof(struct vhcp_unit_t));
	        feederLog(LOG_DEBUG, "vhcp: Dump read unit %d\n", unit->id);
	        
		HASH_ADD_INT(units, id, unit);
	    }
	}
        fclose(fd);
    }
    else
	feederLog(LOG_WARNING, "vhcp: Can not open units hash dump file\n");
    return 0;
}

static void sigHandler(int sig)
{
    printf("signal %d cought\n", sig);
    save();
    exit(EXIT_SUCCESS);
}

int init(void * param)
{
    (void)param;

    struct sigaction s[4];
    

    load();
    
    s[0].sa_handler = sigHandler;
    sigemptyset(&(s[0].sa_mask));
    
    memcpy(&s[1], s, sizeof(struct sigaction));
    memcpy(&s[2], s, sizeof(struct sigaction));
    
    sigaction(SIGTERM, &s[0], &s[3]);
    sigaction(SIGKILL, &s[1], &s[3]);
    sigaction(SIGINT, &s[2], &s[3]);
    return 0;
}


void logHex(char * str, unsigned char * data, unsigned int length)
{
    unsigned int i;

    str[0] = 0;
    for (i = 0; i < length; i++)
    {
	sprintf(str + strlen(str), "%02X ", data[i]);
    }
}

unsigned int
process(void *lib_data, int socket, unsigned char *data,
        unsigned int length, unsigned long long int *id, time_t * tm,
        double *result_array, uint64_t * sensors, unsigned int *type)
{
    unsigned int ret = 0;
    char str[1024];
    unsigned int l;
    int hash_id;

    struct vhcp_packet_t * packet;
    struct vhcp_unit_t * unit;
    
    (void)lib_data;
    (void)socket;
    
    if ((data != NULL) && (length > 0) && (state == VHCP_STATE_IDLE))
    {
        feederLog(LOG_DEBUG, "vhcp: Incoming data: len %d: \n", length);
        l = length;
        while (l > 0)
        {
	    logHex(str, data + length - l, (l > 16) ? 16:l);
	    feederLog(LOG_DEBUG, "%s\n", str);
	    l -= (l > 16) ? 16:l;
	}
        memcpy(buffer, data, length);
        state = VHCP_STATE_POS;
    }
    
    packet = (struct vhcp_packet_t *)buffer;
    
    if (packet->delimiter != VHCP_DELIMITER)
    {
        feederLog(LOG_DEBUG, "vhcp: Bad packet delimiter\n");
        state = VHCP_STATE_IDLE;
        return 0;
    }
    
    feederLog(LOG_DEBUG, "vhcp: Packet type %c\n", packet->type);
    

    switch (packet->type)
    {
	case VHCP_ACK:
	    switch (packet->data.ack.ack_val)
	    {
		case VHCP_ACK_OLDER_PACKET:
		    feederLog(LOG_DEBUG, "vhcp: Request for older packet ack received\n");
		break;
		default:
		break;
	    }
	break;
	case VHCP_ENGINE_START_O:
	case VHCP_ENGINE_STOP_O:
	case VHCP_DRIVE_O:
	case VHCP_DRIVER_LOGIN_O:
	case VHCP_CARGO_LOAD_O:
	case VHCP_CARGO_UNLOAD_O:
	case VHCP_DRIVER_REST_O:
	case VHCP_SAFETY_PAUSE_O:
	case VHCP_FAST_DRIVE_START_O:
	case VHCP_FAST_DRIVE_STOP_O:
	case VHCP_TEXT_INFO_O:
	case VHCP_MOVEMENT_START_O:
	case VHCP_MOVEMENT_STOP_O:
	case VHCP_UNAUTHORIZED_MOVEMENT_O:
	case VHCP_ACTIVITY_CODE_O:
	case VHCP_TEXT_MESSAGE_O:
	case VHCP_ENGINE_START_D:
	case VHCP_ENGINE_STOP_D:
	case VHCP_DRIVE_D:
	case VHCP_DRIVER_LOGIN_D:
	case VHCP_CARGO_LOAD_D:
	case VHCP_CARGO_UNLOAD_D:
	case VHCP_DRIVER_REST_D:
	case VHCP_SAFETY_PAUSE_D:
	case VHCP_FAST_DRIVE_START_D:
	case VHCP_FAST_DRIVE_STOP_D:
	case VHCP_TEXT_INFO_D:
	case VHCP_MOVEMENT_START_D:
	case VHCP_MOVEMENT_STOP_D:
	case VHCP_UNAUTHORIZED_MOVEMENT_D:
	case VHCP_ACTIVITY_CODE_D:
	case VHCP_TEXT_MESSAGE_D:
	
	    hash_id = packet->data.dpacket.id;
	    HASH_FIND_INT(units, &hash_id, unit);
	    if (unit == NULL)
	    {
		unit = (struct vhcp_unit_t *)malloc(sizeof(struct vhcp_unit_t));
		if (unit != NULL)
		{
		    unit->id = packet->data.dpacket.id;
		    HASH_ADD_INT(units, id, unit);
		    feederLog(LOG_DEBUG, "vhcp: Creating new hash unit %u\n", unit->id);

		    unit->last_packet_no = packet->data.dpacket.no;

		}
	    }
	    if (unit != NULL)
	    {
		if (packet->type < 'a')
		{
		    unit->last_packet_no = unit->current_packet_no;
		    unit->current_packet_no = packet->data.dpacket.no;
		}
	    }
	
	    switch (state)
	    {
		case VHCP_STATE_POS:
		{
		    feederLog(LOG_DEBUG, "vhcp: Data packet\n");
		    feederLog(LOG_DEBUG, "vhcp: Car id: %hu, no %u, timestamp %u, lat %u, lon %u\n", packet->data.dpacket.id, packet->data.dpacket.no, packet->data.dpacket.timestamp, packet->data.dpacket.lat, packet->data.dpacket.lon);
		    feederLog(LOG_DEBUG, "vhcp: azimut %hu, speed %hhu, din %hhu, dout %hhu, distance %u, analog1 %hu, analog %hu\n", packet->data.dpacket.azimuth,  packet->data.dpacket.speed, packet->data.dpacket.din,  packet->data.dpacket.dout, packet->data.dpacket.distance, packet->data.dpacket.analog1, packet->data.dpacket.analog2);
		    feederLog(LOG_DEBUG, "vhcp: fuel %hu, kredit %hu, max speed %hhu, avg speed %hhu, overspeed %hu\n", packet->data.dpacket.fuel, packet->data.dpacket.credit, packet->data.dpacket.max_speed, packet->data.dpacket.avg_speed, packet->data.dpacket.overspeed_time);
		    
		    *id = packet->data.dpacket.id;
		    *tm = packet->data.dpacket.timestamp;
	    	    *type = VALUES_TYPE_POS;
	    	    sensors[0] = sensors[1] = sensors[2] = 0x10;
	    	    result_array[0] = ((double)packet->data.dpacket.lat) / 1000.0 / 60.0;
	    	    result_array[1] = ((double)packet->data.dpacket.lon) / 1000.0 / 60.0;
	    	    result_array[2] = 0.0;
	    	    result_array[3] = (double)packet->data.dpacket.sgps;
	    	    result_array[4] = (double)packet->data.dpacket.speed;
	    	    ret = 5;

		    state = VHCP_STATE_OBS;
		    break;
		}
		case VHCP_STATE_OBS:
		{
		    *id = packet->data.dpacket.id;
		    *tm = packet->data.dpacket.timestamp;
	    	    *type = VALUES_TYPE_OBS;
	    	    sensors[0] = 440040000;
	    	    result_array[0] = (double)packet->data.dpacket.speed;
	    	    sensors[1] = 710010000;
	    	    result_array[1] = (double)packet->data.dpacket.azimuth;
	    	    sensors[2] = 330110000;
	    	    result_array[2] = (double)packet->data.dpacket.din;
	    	    sensors[3] = 330120000;
	    	    result_array[3] = (double)packet->data.dpacket.dout;
	    	    sensors[4] = 720010000;
	    	    result_array[4] = (double)packet->data.dpacket.distance / 1000.0;
	    	    sensors[5] = 320050000;
	    	    result_array[5] = (double)packet->data.dpacket.analog1;
	    	    sensors[6] = 320060000;
	    	    result_array[6] = (double)packet->data.dpacket.analog2;
	    	    sensors[7] = 730010000;
	    	    result_array[7] = (double)packet->data.dpacket.fuel;
	    	    sensors[8] = 440050000;
	    	    result_array[8] = (double)packet->data.dpacket.max_speed;
	    	    sensors[9] = 440060000;
	    	    result_array[9] = (double)packet->data.dpacket.avg_speed;
	    	    sensors[10] = 670030000;
	    	    result_array[10] = (double)packet->data.dpacket.overspeed_time;
	    	    
	    	    sensors[11] = 740010000;
	    	    result_array[11] = (double)packet->type;
	    	    
	    	    ret = 12;
	    	    
		    state = VHCP_STATE_DACK;
		    break;
		}
		default:
		break;
		
	    }
	break;
	default:
	break;
	
    }

    return ret;
}

int reply(void *lib_data, int socket, unsigned char *data)
{
    struct vhcp_packet_t * packet;
    struct vhcp_unit_t * unit;
    int hash_id;
    int ret;

    (void)lib_data;
    (void)socket;
    feederLog(LOG_DEBUG, "vhcp: replying\n");

    switch (state)
    {
	case VHCP_STATE_DACK:
	    packet = (struct vhcp_packet_t *)data;
    	    packet->delimiter = VHCP_DELIMITER;
	    packet->type = VHCP_DATA_ACK;
	    packet->data.dack.ack_val = 0;//todo: hodnota a zeptat se
	    packet->data.dack.id = ((struct vhcp_packet_t *)buffer)->data.dpacket.id;
	    ret = 8;
	    
	    state = VHCP_STATE_PREQ;
	break;
	case VHCP_STATE_PREQ:
	    ret = 0;
	    hash_id = (((struct vhcp_packet_t *)buffer)->data.dpacket.id);
	    HASH_FIND_INT(units, &hash_id, unit);
	    if (unit != NULL)
	    {
		if ((unit->last_packet_no + 1 != unit->current_packet_no) && (((struct vhcp_packet_t *)buffer)->type < 'a'))		//ovsem pokud je soucasne prijimanej paket typu z databaze tak na to hod bobek
		{
		    feederLog(LOG_DEBUG, "vhcp: requesting lost packets from %u to %u\n", unit->last_packet_no + 1, unit->current_packet_no - 1);
		    packet = (struct vhcp_packet_t *)data;
    		    packet->delimiter = VHCP_DELIMITER;
		    packet->type = VHCP_SEND_OLDER_DATA_PACKETS;
		    packet->data.sendolderdata.from_packet_no = unit->last_packet_no + 1;
		    packet->data.sendolderdata.to_packet_no = unit->current_packet_no - 1;
		    ret = 10;
		}
	    }
	    
	    state = VHCP_STATE_IDLE;
	break;
	default:
	    state = VHCP_STATE_IDLE;
	ret = 0;
	break;
    }

    return ret;
}

int open(void *lib_data, int socket)
{
    (void)lib_data;
    (void)socket;
    feederLog(LOG_DEBUG, "vhcp: socket %d opened\n", socket);

    return 0;
}

int close(void *lib_data, int socket)
{
    (void)lib_data;
    (void)socket;
    feederLog(LOG_DEBUG, "vhcp: socket %d closed\n", socket);
    
    return 0;
}