Agronode_feeder/feeder_lib/fm4/fm4_pred_opravou-kazda_jednotka_vlastni_buffer.c

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

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


static unsigned char * int_buffer;
static unsigned int int_buffer_pos = 0, int_buffer_pos_wr = 0;



//static int imei_flag = 0;
//char imei_str[16]; //todo: tohle nejak predavat
unsigned char reply_buffer[32];
unsigned int reply_len;

static double values_out[64];
static uint64_t values_sensor[64];
static uint8_t values_type[64];
static time_t values_time;
//static uint8_t values_type;
static uint8_t values_len;



typedef struct unit_t
{
    int fd;
    char imei[16];
    struct unit_t * next;
} unit_t;


static struct unit_t * units;
static struct unit_t * aunit;

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

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

typedef struct avl_header_t
{
    uint16_t dummy __attribute__ ((packed));
    char imei[15];
} avl_header_t;

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;
}

void arraySwap4(uint8_t * array)
{
    uint8_t temp;

    temp = array[0];
    array[0] = array[3];
    array[3] = temp;
    temp = array[1];
    array[1] = array[2];
    array[2] = temp;
}

enum states {STATE_IDLE = 0, STATE_DATA_LENGTH, STATE_CODEC_ID, STATE_NO_OF_REC, 
    STATE_TIMESTAMP, STATE_PRIORITY, 
    STATE_LON, STATE_LAT, STATE_ALT, STATE_ANGLE, STATE_SATTS, STATE_SPEED, 
    STATE_EVENT_IO_ID, STATE_TOTAL_IO,
    STATE_BYTE1_IO, STATE_BYTE1_ID, STATE_BYTE1_VAL,
    STATE_BYTE2_IO, STATE_BYTE2_ID, STATE_BYTE2_VAL,
    STATE_BYTE4_IO, STATE_BYTE4_ID, STATE_BYTE4_VAL,
    STATE_BYTE8_IO, STATE_BYTE8_ID, STATE_BYTE8_VAL,
    STATE_NO_OF_REC_END, STATE_CRC
    };
static unsigned char state = STATE_IDLE;		//state of status machine, must be global to be accesible by process function when determining if this packet could be imei packet (state is IDLE)
    

unsigned char counts[] =
{
    4, 4, 1, 1, 
    8, 1, 
    4, 4, 2, 2, 1, 2,
    1, 1,
    1, 1, 1,
    1, 1, 2,
    1, 1, 4,
    1, 1, 8,
    1, 4
};

#define PRIO_LOW	0
#define PRIO_HIGHT	1
#define PRIO_PANIC	2
#define PRIO_SECURITY	3

#define VAL_DI1		1
#define VAL_DI2		2


void valuesCopyPos(double lat, double lon, double alt, time_t t, uint8_t type, double * values)
{
    values[0] = lat;
    values[1] = lon;
    values[2] = alt;
    values_time = t;
    values_type[0] = type;
    values_type[1] = type;
    values_type[2] = type;
}

/* {FM4 type, FEEDER sensor type id, sensor id, alertable (if prio is 1(high))} */
uint8_t sensorTypes[][4] =
{
    {1, TYPE_DIGITAL_INPUT, 5, 0},
    {2, TYPE_DIGITAL_INPUT, 6, 1},
    {3, TYPE_DIGITAL_INPUT, 7, 0},
    {4, TYPE_DIGITAL_INPUT, 8, 0},
    {9, TYPE_ANALOG_INPUT, 1, 0},
    {10, TYPE_ANALOG_INPUT, 2, 0},
    {11, TYPE_ANALOG_INPUT, 3, 0},
    {19, TYPE_ANALOG_INPUT, 4, 0},
    {21, TYPE_SIGNAL_STRENGTH, 3, 0},
    {24, TYPE_SPEED, 2, 0},
    {66, TYPE_VOLTAGE, 11, 0},
    {67, TYPE_VOLTAGE, 10, 0},
    {68, TYPE_CURRENT, 2, 0},
    {69, TYPE_SIGNAL_STRENGTH, 4, 0},
    {70, TYPE_THERMOMETER, 11, 0},
    {72, TYPE_THERMOMETER, 12, 0},
    {73, TYPE_THERMOMETER, 13, 0},
    {74, TYPE_THERMOMETER, 14, 0},
    {0, 0, 0, 0}
};

double sensorMulti[] =
{
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    0.001,
    0.001,
    0.001,
    1.0,
    0.1,
    0.1,
    0.1,
    0.1,
};

uint8_t getSensorType(uint8_t sensor)
{
    int i;

    i = 0;
    while (sensorTypes[i][0] != 0)
    {
	if (sensorTypes[i][0] == sensor)
	    return i;
	i++;
    }
    return i;
}

void valuesCopyVal(double value, uint8_t sensor, time_t t, uint8_t type, double * values)
{
    uint8_t sensor_index;

    sensor_index = getSensorType(sensor);
    values[values_len] = value * sensorMulti[sensor_index];
    values_sensor[values_len] = sensorTypes[sensor_index][2] * 10000 + sensorTypes[sensor_index][1] * 10000000;
    values_time = t;
    if ((type == VALUES_TYPE_ALERT) && (sensorTypes[sensor_index][3] == 1))
	values_type[values_len] = VALUES_TYPE_ALERT;
    else
	values_type[values_len] = VALUES_TYPE_OBS;
    feederLog(LOG_DEBUG, "fm4: Observation type %s\n", (values_type[values_len] == VALUES_TYPE_ALERT) ? "alert" : "obs");
	
    values_len++;
}

int stateMachine(unsigned char c)
{
    static uint8_t temp[16];
    static unsigned char count = 0;
    static unsigned char qua = 0, c_qua = 0;
    static unsigned char no_of_recs = 0, current_rec = 0;
    static uint8_t val_byte_id;
    static uint64_t val_byte_val;
    
    static float lat, lon, alt;
    static uint8_t prio;
    static time_t t_t;
    static struct tm t;
    
    int ret;
    
    ret = 0;

/*    if (state != STATE_IDLE)
        printf("0x%x %d|", c, state);*/

    temp[count] = c;
    count++;
    
    if (count < counts[state])
	return 0;
    count = 0;

    switch (state)
    {
	case STATE_IDLE:
	    if (*((uint32_t *)temp) == 0x00000000)
		state = STATE_DATA_LENGTH;
	break;
	case STATE_DATA_LENGTH:
	    feederLog(LOG_DEBUG, "fm4: Data size: %d\n", byteSwap32(*((uint32_t *)temp)));
	    state = STATE_CODEC_ID;
	break;
	case STATE_CODEC_ID:
	    if (temp[0] == 0x08)
		state = STATE_NO_OF_REC;
	    else
	    {
		feederLog(LOG_DEBUG, "fm4: Bad codec, going to idle state\n");
		state = STATE_IDLE;	
	    }
	break;
	case STATE_NO_OF_REC:
	    no_of_recs = temp[0];
	    current_rec = 0;
	    feederLog(LOG_DEBUG, "fm4: Number of records: %d\n", no_of_recs);
	    state = STATE_TIMESTAMP;
	break;
	case STATE_TIMESTAMP:
/*	    if (current_rec > 0)
	    {
		if ((lat != 0.0) && (lon != 0.0))
		{
		}
	    }*/
	    
	    lat = lon = alt = 0.0;
	    prio = PRIO_LOW;
	    t_t = 0;
	    gmtime_r(&t_t, &t);
	
	    t_t = (byteSwap64(*((uint64_t *)temp))) / 1000;	//in ms
	    gmtime_r(&t_t, &t);

	    feederLog(LOG_DEBUG, "fm4: Time of rec: %04d-%02d-%02d %02d:%02d:%02d\n",
		t.tm_year + 1900, t.tm_mon + 1, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec);
	    
	    feederLog(LOG_DEBUG, "fm4: Processing record: %d\n", current_rec);
    	    current_rec++;
	    state = STATE_PRIORITY;
	break;
	case STATE_PRIORITY:
	    prio = temp[0] & 0x03;
	    feederLog(LOG_DEBUG, "fm4: Priority: %d\n", prio); 
	    state = STATE_LON;
	break;
	case STATE_LON:
	    lon = (float)(byteSwap32(*(uint32_t *)temp)) / 10000000.0;
	    feederLog(LOG_DEBUG, "fm4: lon: %f\n", lon);
	    state = STATE_LAT;
	break;
	case STATE_LAT:
	    lat = (float)(byteSwap32(*(uint32_t *)temp)) / 10000000.0;
	    feederLog(LOG_DEBUG, "fm4: lat: %f\n", lat);
	    state = STATE_ALT;
	break;
	case STATE_ALT:
	    alt = (float)(byteSwap16(*(uint16_t *)temp));
	    feederLog(LOG_DEBUG, "fm4: alt: %f\n", alt);
	    state = STATE_ANGLE;
	break;
	case STATE_ANGLE:
	    feederLog(LOG_DEBUG, "fm4: angle: %f\n", (float)(byteSwap16(*(uint16_t *)temp)));
	    state = STATE_SATTS;
	break;
	case STATE_SATTS:
	    feederLog(LOG_DEBUG, "fm4: sattelites: %d\n", temp[0]);
	    state = STATE_SPEED;
	break;
	case STATE_SPEED:
	    feederLog(LOG_DEBUG, "fm4: speed: %f\n", (float)(byteSwap16(*(uint16_t *)temp)));
	    
	    valuesCopyPos(lat, lon, alt, t_t, VALUES_TYPE_POS, values_out);
	    ret = 2;
	    
	    state = STATE_EVENT_IO_ID;
	break;
	case STATE_EVENT_IO_ID:
	    feederLog(LOG_DEBUG, "fm4: event io id: %d\n", temp[0]);
	    state = STATE_TOTAL_IO;
	break;
	case STATE_TOTAL_IO:
	    feederLog(LOG_DEBUG, "fm4: total io: %d\n", temp[0]);
	    values_len = 0;
	    state = STATE_BYTE1_IO;
	break;
	case STATE_BYTE1_IO:
	    qua = temp[0];
	    c_qua = 0;
	    feederLog(LOG_DEBUG, "fm4: byte1 io: %d\n", qua);
	    if (qua == 0)
		state = STATE_BYTE2_IO;
	    else
		state = STATE_BYTE1_ID;
	break;
	case STATE_BYTE1_ID:
	    feederLog(LOG_DEBUG, "fm4: byte1 id: %d\n", temp[0]);
	    val_byte_id = temp[0];
	    state = STATE_BYTE1_VAL;
	break;
	case STATE_BYTE1_VAL:
	    feederLog(LOG_DEBUG, "fm4: byte1 val: %d\n", temp[0]);
	    val_byte_val = temp[0];
	    if (prio == 0)
		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
	    else
		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_ALERT, values_out);
	    c_qua++;
	    if (qua == c_qua)
		state = STATE_BYTE2_IO;
	    else
		state = STATE_BYTE1_ID;
	break;
	case STATE_BYTE2_IO:
	    qua = temp[0];
	    c_qua = 0;
	    feederLog(LOG_DEBUG, "fm4: byte2 io: %d\n", qua);
	    if (qua == 0)
		state = STATE_BYTE4_IO;
	    else
		state = STATE_BYTE2_ID;
	break;
	case STATE_BYTE2_ID:
	    feederLog(LOG_DEBUG, "fm4: byte2 id: %d\n", temp[0]);
	    val_byte_id = temp[0];
	    state = STATE_BYTE2_VAL;
	break;
	case STATE_BYTE2_VAL:
	    feederLog(LOG_DEBUG, "fm4: byte2 val: %d\n", (byteSwap16(*(uint16_t *)temp)));
	    val_byte_val = (byteSwap16(*(uint16_t *)temp));
	    if (prio == 0)
    		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
    	    else
    		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_ALERT, values_out);
	    c_qua++;
	    if (qua == c_qua)
		state = STATE_BYTE4_IO;
	    else
		state = STATE_BYTE2_ID;
	break;
	case STATE_BYTE4_IO:
	    qua = temp[0];
	    c_qua = 0;
	    feederLog(LOG_DEBUG, "fm4: byte4 io: %d\n", qua);
	    if (qua == 0)
			state = STATE_BYTE8_IO;
	    else
			state = STATE_BYTE4_ID;
	break;
	case STATE_BYTE4_ID:
	    feederLog(LOG_DEBUG, "fm4: byte4 id: %d\n", temp[0]);
	    val_byte_id = temp[0];
	    state = STATE_BYTE4_VAL;
	break;
	case STATE_BYTE4_VAL:
	    feederLog(LOG_DEBUG, "fm4: byte4 val: %d\n", (byteSwap32(*(uint32_t *)temp)));
	    val_byte_val = (byteSwap32(*(uint32_t *)temp));
	    if (prio == 0)
		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
	    else
		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_ALERT, values_out);
	    c_qua++;
	    if (qua == c_qua)
			state = STATE_BYTE8_IO;
	    else
			state = STATE_BYTE4_ID;
	break;
	case STATE_BYTE8_IO:
	    qua = temp[0];
	    c_qua = 0;
	    feederLog(LOG_DEBUG, "fm4: byte8 io: %ld\n", qua);
	    if (qua == 0)
	    {
		if (current_rec == no_of_recs)	
		    state = STATE_NO_OF_REC_END;
		else
		    state = STATE_TIMESTAMP;
		
	    }
	    else
			state = STATE_BYTE8_ID;
	break;
	case STATE_BYTE8_ID:
	    feederLog(LOG_DEBUG, "fm4: byte8 id: %d\n", temp[0]);
	    val_byte_id = temp[0];
	    state = STATE_BYTE8_VAL;
	break;
	case STATE_BYTE8_VAL:
	    feederLog(LOG_DEBUG, "fm4: byte8 val: %d\n", (byteSwap64(*(uint64_t *)temp)));
	    val_byte_val = (byteSwap64(*(uint64_t *)temp));
	    valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
	    
	    c_qua++;
	    if (qua == c_qua)
	    {
		if (current_rec == no_of_recs)
		    state = STATE_NO_OF_REC_END;
		else
		    state = STATE_TIMESTAMP;
		
		ret = 2;
	    }
	    else
		state = STATE_BYTE8_ID;
	break;
	case STATE_NO_OF_REC_END:
	    no_of_recs = temp[0];
	    feederLog(LOG_DEBUG, "fm4: Number of records: %d\n", temp[0]);
	    state = STATE_CRC;
	break;
	case STATE_CRC:
	    state = STATE_IDLE;
	    feederLog(LOG_DEBUG, "fm4: Crc\n");

	    reply_buffer[0] = 0x00;
	    reply_buffer[1] = 0x00;
	    reply_buffer[2] = 0x00;
	    reply_buffer[3] = no_of_recs;
	    reply_len = 4;
	    ret = 1;
	break;
    }
    return ret;
}

int parse(unsigned char * data)
{
    int ret, res;
    
    ret = 0;

    while (res = stateMachine(int_buffer[int_buffer_pos]), res == 0)
    {
	if (int_buffer_pos + 1 >= int_buffer_pos_wr)
	    break;
	int_buffer_pos++;

    }
    int_buffer_pos++;
    
    if (res == 1)
    {
		feederLog(LOG_DEBUG, "fm4: End of data packet\n");

		int_buffer_pos_wr = int_buffer_pos = 0;
//		aunit->imei[0] = 0;		//todo: predpokladame ze dalsi paket bude opet uvozen 17byty s IMEI
    }
    if (res == 2)
    {
	feederLog(LOG_DEBUG, "fm4: Valid data decoded\n");
    
	ret = 1;
    }
    return ret;
}

//----------------------------------------------------------------------------------------------

struct unit_t * unitFind(int fd)
{
    struct unit_t * unit;
    
    unit = units;
    while (unit != NULL)
    {
	if (unit->fd == fd)
	{
	    return unit;
	}
	unit = unit->next;
    }
    return NULL;
}

struct unit_t * unitCreate(int fd)
{
    struct unit_t * unit, * unit_new;
    
    unit_new = malloc(sizeof(struct unit_t));

    if (unit_new == NULL)
	return NULL;
    unit_new->fd = fd;
    unit_new->imei[0] = 0;
    unit_new->next = NULL;

    if (units == NULL)
	units = unit_new;
    else
    {
	unit = units;
	while (unit->next != NULL)
	{
	    unit = unit->next;
	}
	unit->next = unit_new;
    }

    return unit_new;
}


int imei(struct unit_t * unit, unsigned char * data)
{
    struct avl_header_t * avl_header;
    char imei_str[16];

    avl_header = (struct avl_header_t *)data;
    memcpy(imei_str, avl_header->imei, 15);
    imei_str[15] = 0;
    feederLog(LOG_DEBUG, "fm4: Imei is %s\n", imei_str);

    strcpy(unit->imei, imei_str);

    return 1;
}

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

    bzero(reply_buffer, 32);
    reply_len = 0;
    int_buffer = malloc(32768);
    units = aunit = NULL;
    return 0;
}


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

    buffer[0] = 0;
    for (i = 0; i < length; i++)
    {
	sprintf(buffer + strlen(buffer), "%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;
    struct unit_t * unit;
    uint8_t i;
    uint8_t buffer[32768];
    unsigned int l;

    if (data != NULL)
    {
        feederLog(LOG_DEBUG, "fm4: Incoming data: len %d: \n", length);
        l = length;
        while (l > 0)
        {
	    logHex(buffer, data + length - l, (l > 16) ? 16:l);
	    feederLog(LOG_DEBUG, "%s\n", buffer);
	    l -= (l > 16) ? 16:l;
	}
    }
    unit = unitFind(socket);
    
    if (unit == NULL)
    {
	feederLog(LOG_DEBUG, "fm4: new unit for socket %d\n", socket);
    
        unit = unitCreate(socket);
    }
    else
    {
	feederLog(LOG_DEBUG, "fm4: data from known unit on socket %d\n", socket);
    }

    aunit = unit;

//    if ((length != 0) && (strlen(aunit->imei) == 0))
    if ((length != 0) && ((memcmp(data, "\0\0\0\0", 4) != 0) && (state == STATE_IDLE)))
    {
	imei(aunit, data);
	feederLog(LOG_DEBUG, "fm4: imei for socket %d is %s\n", socket, unit->imei);//todo: snulovat buffer
	
	reply_buffer[0] = 0x01;
	reply_len = 1;

	ret = 0;
    }
    else
    {
	if (length != 0)
	{
	    feederLog(LOG_DEBUG, "fm4: Copying data for unit imei %s\n", unit->imei);
	
	    memcpy(int_buffer + int_buffer_pos_wr, data, length);
	    int_buffer_pos_wr += length;
	    
	}
	if ((parse(int_buffer)) || (values_len != 0))
	{
	    *id = atol(aunit->imei);
	    *tm = values_time;
//	    printf("val type %d\n", values_type[0]);
	    if (values_type[0] == VALUES_TYPE_POS)
	    {
		memcpy(result_array, values_out, sizeof(double) * 3);
	    
	        *type = VALUES_TYPE_POS;
	        sensors[0] = sensors[1] = sensors[2] = 0x10;
	        ret = 3;
	        values_len -= 3;
	    }
	    else if (values_type[values_len - 1] == VALUES_TYPE_OBS)
	    {
		i = 0;
		while ((values_len > 0) && (values_type[values_len - 1] == VALUES_TYPE_OBS))
		{
		    result_array[i] = values_out[values_len - 1];
		    sensors[i] = values_sensor[values_len - 1];
		    i++;
		    values_len--;
		}
		*type = VALUES_TYPE_OBS;
		ret = i;
		
		/*
		memcpy(result_array, values_out, sizeof(double) * values_len);
		*type = VALUES_TYPE_OBS;
		memcpy(sensors, values_sensor, sizeof(uint64_t) * values_len);
		ret = values_len;
		*/
	    }
	    else if (values_type[values_len - 1] == VALUES_TYPE_ALERT)
	    {
		i = 0;
		while ((values_len > 0) && (values_type[values_len - 1] == VALUES_TYPE_ALERT))
		{
		    result_array[i] = values_out[values_len - 1];
		    sensors[i] = values_sensor[values_len - 1];
		    i++;
		    values_len--;
		}
		*type = VALUES_TYPE_ALERT;
		ret = i;
	    
/*		memcpy(result_array, values_out, sizeof(double) * values_len);
		*type = VALUES_TYPE_ALERT;
		memcpy(sensors, values_sensor, sizeof(uint64_t) * values_len);
		ret = values_len;*/
	    }
	}
    }

    return ret;
}

int reply(void *lib_data, int socket, unsigned char *data)
{
    unsigned int temp_reply_len;

    feederLog(LOG_DEBUG, "fm4: replying\n");

    memcpy(data, reply_buffer, reply_len);
    temp_reply_len = reply_len;
    reply_len = 0;

    return temp_reply_len;
}

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

    return 0;
}

int close(void *lib_data, int socket)
{
    struct unit_t * unit;

    feederLog(LOG_DEBUG, "fm4: socket %d closed\n", socket);

    unit = unitFind(socket);
    if (unit != NULL)
    {
		unit->imei[0] = 0;		//todo: nebo mozna lepe, zrusit celou jednotku
    }

    int_buffer_pos_wr = int_buffer_pos = 0;
    return 0;
}