Agronode_feeder/feeder_lib/gh120x/gh120x.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[6];
static uint32_t values_sensor[6];
static time_t values_time;
static uint8_t values_type;

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

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


double conv_latlon(char * str)
{
    double res;
    char min[3], dg[4], * point;
    
    point = strchr(str, '.');
    
    memcpy(dg, str, point - str - 2);
    dg[point - str - 2] = 0;
    memcpy(min, point - 2, 2);
    min[2] = 0;

//    printf("str %s: dg %s, min %s, tis %s \n", str, dg, min, point + 1);
//    printf("%p, %p, %d\n", str, point, point - str - 2);
    res = atof(dg);
    res += (atof(min) + atof(point + 1) / 100000) / 60.0;
    
    return res;
}


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_PR_TS, STATE_G_MASK, STATE_MASK,
    STATE_LATLON, STATE_ALT, STATE_ANGLE, STATE_SPEED, STATE_SATTS, STATE_CELL, STATE_QUA, STATE_OP, 
    STATE_IO1_Q, STATE_IO1_ID, STATE_IO1_VAL,
    STATE_IO2_Q, STATE_IO2_ID, STATE_IO2_VAL,
    STATE_IO4_Q, STATE_IO4_ID, STATE_IO4_VAL,
    STATE_NO_OF_REC_END, STATE_CRC
    };

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

#define MASK_LATLON(x)	(x & 0x01)
#define MASK_ALT(x)	((x >> 1) & 0x01)
#define MASK_ANGLE(x)	((x >> 2) & 0x01)
#define MASK_SPEED(x)	((x >> 3) & 0x01)
#define MASK_SATS(x)	((x >> 4) & 0x01)
#define MASK_CELL(x)	((x >> 5) & 0x01)
#define MASK_QUA(x)	((x >> 6) & 0x01)
#define MASK_OP(x)	((x >> 7) & 0x01)
#define MASK_GEN(x, y)	((x >> y) & 0x01)
#define MASK_GPS(x)	(MASK_GEN(x, 0))
#define MASK_IO1(x)	(MASK_GEN(x, 1))
#define MASK_IO2(x)	(MASK_GEN(x, 2))
#define MASK_IO4(x)	(MASK_GEN(x, 3))


#define PRIO_TRACK	0
#define PRIO_PERIODIC	1
#define PRIO_ALARM	2
#define PRIO_UNDEFINED	3

unsigned char maskGlobalNext(unsigned char mask, unsigned char state, unsigned char end_of_data)
{
    unsigned int i, c;
    unsigned char order[] = 
    {STATE_G_MASK, STATE_MASK, STATE_IO1_Q, STATE_IO2_Q, STATE_IO4_Q};

    c = 0;
//    printf("gs%d|%d ", state, mask);
    
    for (i = 0; i < 5; i++)
    {
	if (order[i] == state)
	    c = i;
    }
    for (i = c; i < 5; i++)
    {
//	printf("g%d|%d ", i, mask);
    
	if (MASK_GEN(mask, i))
	{
	    return order[i + 1];
	}
    }
//    printf("End of rec\n");
    if (end_of_data)
	return STATE_NO_OF_REC_END;
    else
	return STATE_PR_TS;
}

unsigned char maskNext(unsigned char mask, unsigned char state)
{
    unsigned char i;

    for (i = state - STATE_MASK; i < 8; i++)
    {
//	printf(" %d|%d ", i, mask);
        if (MASK_GEN(mask, i))
	{
	    return STATE_MASK + 1 + i;
	}
    }
    return STATE_MASK;
}

uint8_t sensorTypes[][2] =
{
    {001, TYPE_CAPACITY},
    {010, TYPE_ALARM},
    {0, 0}
};

uint8_t getSensorType(uint8_t sensor)
{
    int i;

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

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

void valuesCopySingle(double val, uint8_t sensor, time_t t, double * values)
{
    values[0] = val;
    values_sensor[0] = sensor * 10000 + getSensorType(sensor) * 10000000;
    values_time = t;
    if (sensor == 010)
	values_type = VALUES_TYPE_ALARM;
    else
	values_type = VALUES_TYPE_OBS;
}

int stateMachine(unsigned char c)
{
    static unsigned char state = STATE_IDLE;
    static uint8_t temp[16];
    static unsigned char count = 0;
    static unsigned char mask = 0, mask_global = 0;
    static unsigned char state_type = STATE_IDLE;
    static unsigned char qua = 0, c_qua = 0;
    static unsigned char no_of_recs = 0, current_rec = 0;

    static float lat, lon, alt;
    static uint8_t prio, io_type;
    static struct tm rec_time;

    static time_t t_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, "Data size: %d\n", byteSwap32(*((uint32_t *)temp)));
	    state = STATE_CODEC_ID;
	break;
	case STATE_CODEC_ID:
	    if (temp[0] == 0x07)
		state = STATE_NO_OF_REC;
	    else
	    {
		feederLog(LOG_DEBUG, "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, "Number of records: %d\n", no_of_recs);
	    state = STATE_PR_TS;
	break;
	case STATE_PR_TS:
	    gmtime_r(&t_t, &rec_time);
	
	    t_t = (byteSwap32(*((uint32_t *)temp)) & 0x3fffffff) + 1166832000 + 777600;
	
/*	    if (current_rec > 0)
	    {
		if ((lat != 0.0) && (lon != 0.0))
		{
		}
	    }*/
	    
	    lat = lon = alt = 0.0;
	    prio = PRIO_UNDEFINED;
	    t_t = 0;
	    gmtime_r(&t_t, &rec_time);
	
	    t_t = (byteSwap32(*((uint32_t *)temp)) & 0x3fffffff) + 1166832000 + 777600;
	    localtime_r(&t_t, &rec_time);

	    feederLog(LOG_DEBUG, "Time of rec: %04d-%02d-%02d %02d:%02d:%02d\n",
		rec_time.tm_year + 1900, rec_time.tm_mon + 1, rec_time.tm_mday, rec_time.tm_hour, rec_time.tm_min, rec_time.tm_sec);

	    prio = temp[0] >> 6;
	    feederLog(LOG_DEBUG, "Priority: %d\n", prio); 

	    if (prio == 2)
	    {
		valuesCopySingle(1.0, 010, t_t, values_out);
		ret = 2;
	    }

	    feederLog(LOG_DEBUG, "Next processed record will be: %d\n", current_rec);
    	    current_rec++;
	    state = STATE_G_MASK;
	break;
	case STATE_G_MASK:
	    feederLog(LOG_DEBUG, "gps:%d, io_1:%d, io_2:%d, io_4:%d\n", 
		MASK_GPS(temp[0]), MASK_IO1(temp[0]), MASK_IO2(temp[0]), MASK_IO4(temp[0]));
	    state_type = STATE_G_MASK;
	    mask_global = temp[0];
	    state = maskGlobalNext(mask_global, state_type, current_rec == no_of_recs);
	    state_type = state;
	break;
	case STATE_MASK:
	    feederLog(LOG_DEBUG, "latlon:%d, alt:%d, angle:%d, speed:%d, sats:%d, cell:%d, qua:%d, op:%d\n", 
		MASK_LATLON(temp[0]), MASK_ALT(temp[0]), MASK_ANGLE(temp[0]), MASK_SPEED(temp[0]),
		MASK_SATS(temp[0]), MASK_CELL(temp[0]), MASK_QUA(temp[0]), MASK_OP(temp[0]));
	    mask = temp[0];
	    state = maskNext(mask, state);
	break;
	case STATE_LATLON:
	    arraySwap4(temp);
	    arraySwap4(&temp[4]);
	    lon = *((float *)(&temp[4]));
	    lat = *((float *)(&temp[0]));
	    feederLog(LOG_DEBUG, "latlon: %f, %f\n", lat, lon);
	    state = maskNext(mask, state);
	break;
	case STATE_ALT:
	    alt = (float)byteSwap16(*(uint16_t *)temp);
	    feederLog(LOG_DEBUG, "alt: %f\n", alt);

	    if ((lat != 0.0) && (lon != 0.0))
	    {
		valuesCopyPos(lat, lon, alt, t_t, values_out);
		ret = 2;
	    }

	    state = maskNext(mask, state);
	break;
	case STATE_ANGLE:
	    feederLog(LOG_DEBUG, "angle: %f\n", temp[0] * 360.0 / 256.0);
	    state = maskNext(mask, state);
	break;
	case STATE_SPEED:
	    feederLog(LOG_DEBUG, "speed: %d\n", *(uint8_t *)temp);
	    state = maskNext(mask, state);
	break;
	case STATE_SATTS:
	    feederLog(LOG_DEBUG, "sats: %d\n", *(uint8_t *)temp);
	    state = maskNext(mask, state);
	    if (state == STATE_MASK)
	    {
		feederLog(LOG_DEBUG, "End of gps\n");
		state = maskGlobalNext(mask_global, STATE_MASK, current_rec == no_of_recs);
		state_type = state;
	    }
	break;
	case STATE_CELL:
	    feederLog(LOG_DEBUG, "cell:\n");
	    state = maskNext(mask, state);
	break;
	case STATE_QUA:
	    feederLog(LOG_DEBUG, "quality: %d\n", temp[0]);
	    state = maskNext(mask, state);
	break;
	case STATE_OP:
	    feederLog(LOG_DEBUG, "operator: %d\n", byteSwap32(*(uint32_t *)temp));
	    state = maskGlobalNext(mask_global, state_type, current_rec == no_of_recs);	//todo: ted predpokladam ze jsou vsechny v gps (0xff)(rep minimalne tahle posledni)
	    state_type = state;
	break;
	case STATE_IO1_Q:
	    qua = temp[0];
	    feederLog(LOG_DEBUG, "io1 qua: %d\n", qua);
	    c_qua = 0;
	    if (c_qua == qua)
	    {
		state = maskGlobalNext(mask_global, state_type, current_rec == no_of_recs);
		state_type = state;
	    }
	    else
		state = STATE_IO1_ID;
	break;
	case STATE_IO1_ID:
	    feederLog(LOG_DEBUG, "io1 id: %d\n", temp[0]);
	    io_type = temp[0];
	    state = STATE_IO1_VAL;
	break;
	case STATE_IO1_VAL:
	    feederLog(LOG_DEBUG, "io1 val: %d\n", temp[0]);
	    if (io_type == 1)
	    {
		valuesCopySingle(temp[0], io_type, t_t, values_out);
		ret = 2;
	    }
	    c_qua++;
	    if (c_qua == qua)
	    {
		state = maskGlobalNext(mask_global, state_type, current_rec == no_of_recs);
		state_type = state;
	    }
	    else
		state = STATE_IO1_ID;
	break;
	case STATE_IO2_Q:
	    qua = temp[0];
	    feederLog(LOG_DEBUG, "io2 qua: %d\n", qua);
	    c_qua = 0;
	    if (c_qua == qua)
	    {
		state = maskGlobalNext(mask_global, state_type, current_rec == no_of_recs);
		state_type = state;
	    }
	    else
		state = STATE_IO2_ID;
	break;
	case STATE_IO2_ID:
//	    feederLog(LOG_DEBUG, "io2 id:\n");
	    state = STATE_IO2_VAL;
	break;
	case STATE_IO2_VAL:
//	    feederLog(LOG_DEBUG, "io2 val:\n");
	    c_qua++;
	    if (c_qua == qua)
	    {
		state = maskGlobalNext(mask_global, state_type, current_rec == no_of_recs);
		state_type = state;
	    }
	    else
		state = STATE_IO2_ID;
	break;
	case STATE_IO4_Q:
	    qua = temp[0];
	    feederLog(LOG_DEBUG, "io4 qua: %d\n", qua);
	    c_qua = 0;
	    if (c_qua == qua)
	    {
		state = maskGlobalNext(mask_global, state_type, current_rec == no_of_recs);
		state_type = state;
	    }
	    else
		state = STATE_IO4_ID;
	break;
	case STATE_IO4_ID:
//	    feederLog(LOG_DEBUG, "io4 id:\n");
	    state = STATE_IO4_VAL;
	break;
	case STATE_IO4_VAL:
//	    feederLog(LOG_DEBUG, "io4 val:\n");
	    c_qua++;
	    if (c_qua == qua)
	    {
		state = maskGlobalNext(mask_global, state_type, current_rec == no_of_recs);
		state_type = state;
	    }
	    else
		state = STATE_IO4_ID;
	break;
	case STATE_NO_OF_REC_END:
	    no_of_recs = temp[0];
	    feederLog(LOG_DEBUG, "Number of records: %d\n", temp[0]);
	    state = STATE_CRC;
	break;
	case STATE_CRC:
	    state = STATE_IDLE;
	    feederLog(LOG_DEBUG, "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 translate(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, "End of data packet\n");

	int_buffer_pos_wr = int_buffer_pos = 0;
    }
    if (res == 2)
	ret = 1;
    return ret;
}

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

int imei(unsigned char * data)
{
    struct avl_header_t * avl_header;

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

    reply_buffer[0] = 0x01;
    reply_len = 1;

    return 1;
}

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

    imei_flag = 0;
    bzero(imei_str, 16);
    bzero(reply_buffer, 32);
    reply_len = 0;
    int_buffer = malloc(32768);
    return 0;
}

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;

    if ((!imei_flag) && (length != 0))
    {
	if (!imei_flag)
	{
	    if (imei(data))
		imei_flag = 1;
	}
	ret = 0;
    }
    else
    {
	if (length != 0)
	{
	    feederLog(LOG_DEBUG, "Copying data\n", int_buffer_pos);
	
	    memcpy(int_buffer + int_buffer_pos_wr, data, length);
	    int_buffer_pos_wr += length;
	    
	}
	if (translate(int_buffer))
	{
	    *id = atol(imei_str);
	    *tm = values_time;
	    if (values_type == VALUES_TYPE_POS)
	    {
		memcpy(result_array, values_out, sizeof(double) * 3);

	        sensors[0] = sensors[1] = sensors[2] = 0x10;
	        *type = VALUES_TYPE_POS;
	        ret = 3;
	    }
	    if (values_type == VALUES_TYPE_ALARM)
	    {
		result_array[0] = 1.0;
		*type = VALUES_TYPE_ALARM;
		sensors[0] = TYPE_ALARM * 10000000 + 10 * 10000;
		
		ret = 1;
	    }
	    if (values_type == VALUES_TYPE_OBS)
	    {
		result_array[0] = values_out[0];
		*type = VALUES_TYPE_OBS;
		sensors[0] = values_sensor[0];
		
		ret = 1;
	    }
	}
    }

    return ret;
}

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

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

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

    return temp_reply_len;
}

int close(void *lib_data, int socket)
{
    feederLog(LOG_DEBUG, "gh120x: socket closed\n");

    int_buffer_pos_wr = int_buffer_pos = 0;
    imei_flag = 0;
    imei_str[0] = 0;
    return 0;
}

int open(void *lib_data, int socket)
{
    feederLog(LOG_DEBUG, "gh120x: socket open\n");
    return 0;
}