Agronode_feeder/feeder_lib/sensob_iridium/sensob_iridium_.c

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

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


typedef struct sensob_item_t
{
	uint8_t flag;
	uint32_t sensor;
	uint32_t timestamp;
	uint8_t type;
} __attribute__((packed)) sensob_item_t;

typedef struct sensob_header_t
{
	uint8_t delimiter;
	uint64_t id;
	uint16_t seq;
	uint16_t size;
} __attribute__((packed)) sensob_header_t;

typedef struct sensob_footer_t
{
	uint16_t crc;
} __attribute__((packed)) sensob_footer_t;

typedef struct sensob_ack_t
{
        uint8_t delimiter;
	uint16_t seq;
} __attribute__((packed)) sensob_ack_t;


typedef struct sensob_item_position_data_t
{
        double lat;
        double lon;
        double alt;
} __attribute__((packed)) sensob_item_position_data_t;

uint8_t sensob_types_len[] =
{
	sizeof(uint8_t), 
	sizeof(int8_t), 
	sizeof(uint16_t), 
	sizeof(int16_t), 
	sizeof(uint32_t), 
	sizeof(int32_t), 
	sizeof(uint64_t), 
	sizeof(int64_t), 
	sizeof(float), 
	sizeof(double), 
	sizeof(time_t),
	sizeof(uint8_t), 
	sizeof(sensob_item_position_data_t),
	sizeof(uint32_t)
};


typedef struct sensobn_item_t
{
	uint8_t sensor;
	float value;
} __attribute__((packed)) sensobn_item_t;

typedef struct sensobn_header_t
{
	uint8_t delimiter;
	uint32_t id;
	uint32_t timestamp;
	uint8_t size;
} __attribute__((packed)) sensobn_header_t;

typedef struct directip_overall_t
{
    uint8_t protocol_revision_number;
    uint16_t overall_message_length;
    uint8_t elements[512];
} __attribute__((packed)) directip_overall_t;

typedef struct directip_iei_t
{
    uint8_t iei;
    uint16_t length;
    uint8_t d[512];
} __attribute__((packed)) directip_iei_t;

typedef struct directip_mo_header_t
{
    uint8_t mo_header_iei;
    uint16_t mo_header_length;
    uint32_t cdr_reference;
    char imei[15];
    uint8_t session_status;
    uint16_t momsn;
    uint16_t mtmsn;
    uint32_t time_of_session;
} __attribute__((packed)) directip_mo_header_t;

typedef struct directip_mo_payload_t
{
    uint8_t mo_payload_iei;
    uint16_t mo_payload_length;
    uint8_t mo_payload[1960];
} __attribute__((packed)) directip_mo_payload;

typedef struct directip_mo_location_information_t
{
    uint8_t mo_location_information_iei;
    uint16_t mo_location_info_length;
    uint8_t lat_lon[7];
    uint32_t cep_radius;
} __attribute__((packed)) directip_mo_location_information;

typedef struct directip_mo_confirmation_t
{
    uint8_t mo_confirmation_iei;
    uint16_t mo_confirmation_length;
    uint8_t confirmation_status;
} __attribute__((packed)) directip_mo_confirmation_t;


#define DIRECTIP_MO_HEADER_IEI			0x01
#define DIRECTIP_MO_PAYLOAD_IEI			0x02
#define DIRECTIP_MO_LOCATION_INFORMATION_IEI	0x03
#define DIRECTIP_MO_CONFIRMATION_IEI		0x05

// todo: oifovat podle endianes
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;
}

//todo: kontrolvat jestly to jde - tj len=2,4,8,16 ...
void memSwap(uint8_t * data, uint8_t len)
{
	uint8_t temp;
	uint8_t i;

	for (i = 0; i < len / 2; i++)
	{
		temp = data[i];
		data[i] = data[len - 1 - i];
		data[len - 1 - i] = temp;
	}
}


#define SENSOB_TYPE_UINT8	0
#define SENSOB_TYPE_INT8	1
#define SENSOB_TYPE_UINT16	2
#define SENSOB_TYPE_INT16	3
#define SENSOB_TYPE_UINT32	4
#define SENSOB_TYPE_INT32	5
#define SENSOB_TYPE_UINT64	6
#define SENSOB_TYPE_INT64	7
#define SENSOB_TYPE_FLOAT	8
#define SENSOB_TYPE_DOUBLE	9
#define SENSOB_TYPE_TIMESTAMP	10
#define SENSOB_TYPE_ERROR	11
#define SENSOB_TYPE_POSITION	12
#define SENSOB_TYPE_ALERT	13


static uint8_t * ld;
static uint8_t * pd;
static uint16_t sd;

static uint16_t seq = 0;

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

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

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

    ld = pd = NULL;
    sd = 0;
    return 0;
}

/*
#define STATUS_NONE	0


void stateMachine(uint8_t c)
{
    static uint8_t status = STATUS_NONE;
    static uint6
    
    switch (status)
    {
	case STATUS_NONE:
	    if (c == 0xff)
		status = STATUS_DELIMITER;
		size = 1;
		exp
	    break;
	case STATUS_DELIMITER:
	    
	default:
	    break;
    }
}*/

uint8_t * findDelimiter(uint8_t * data, uint16_t len)
{
    uint8_t * res;

    for (res = data; res < data + len; res++)
    {
	if (*res == 0x5a)
	    return res;
    }
    return NULL;
}

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


char directipReportStatus[][256] =
{
    "The SBD session completed successfully",
    "The MO message transfer, if any, was successful. The MT message queued at the Iridium Gateway is too large to be transferred within a single SBD session",
    "The MO message transfer, if any, was successful. The reported location was determined to be of unacceptable quality",
    "",
    "",
    "",
    "",
    "",
    "",
    "",
    "The SBD session timed out before session completion",
    "",
    "The MO message being transferred by the IMEI is too large be transferred within a single SBD session",
    "An RF link loss occurred during the SBD session",
    "An IMEI protocol anomaly occurred during SBD session",
    "The IMEI is prohibited from accessing the Iridium Gateway"
};

int directipHeader(struct directip_mo_header_t * header, char * imei)
{
    feederLog(LOG_DEBUG, "sensob_iridium: Directip header\n");
    memcpy(imei, header->imei, 15);
    imei[15] = '\0';
    feederLog(LOG_DEBUG, "sensob_iridium: Imei %s\n", imei);
    feederLog(LOG_DEBUG, "sensob_iridium: Timestamp %ld, MOMSN %d, MTMSN %d\n", byteSwap32(header->time_of_session), byteSwap16(header->momsn), byteSwap16(header->mtmsn));
    if (header->session_status > 2)
        feederLog(LOG_WARNING, "sensob_iridium: Session status %d, %s\n", header->session_status, directipReportStatus[header->session_status]);
    else
	feederLog(LOG_WARNING, "sensob_iridium: Session status %d, %s\n", header->session_status, directipReportStatus[header->session_status]);
    
    return header->session_status;
}

int directipPayload(struct directip_mo_payload_t * payload, uint8_t ** data, uint16_t * length)
{
    feederLog(LOG_DEBUG, "sensob_iridium: Directip payload\n");
    *data = payload->mo_payload;
    *length = byteSwap16(payload->mo_payload_length);
    return 0;
}

int directipLocationInformation(struct directip_mo_location_information_t * location)
{
    (void)location;

    feederLog(LOG_DEBUG, "sensob_iridium: Directip location information\n");
    return 0;
}


uint32_t map[] =
{
    0,
    690020000,
    340230000,
    690010000,
    570030000,
    340220000,
    460040000,
    340240000,
    410090000,
    710010000,
    690020000,
    340260000,
    360170000
};

uint32_t sensobn_map_to_senso(uint8_t id)
{
    return map[id];
}

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)

{
    static struct sensobn_header_t * header;
    struct sensobn_item_t * item;
    double val;
    unsigned int ret, i;
    
    static double lat, lon;		//todo: do lib_data
    static uint8_t pos_flag = 0;
    uint8_t temp[256];
    
    struct directip_overall_t * overall;
    struct directip_iei_t * iei;
    uint16_t message_length;
    char imei[16];
    uint8_t * data_payload;
    uint16_t data_size;

    unsigned int l;
    char buffer[1024];
/*
    if (data != NULL)
    {
        feederLog(LOG_DEBUG, "sensob_iridium: 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;
	}
    }
*/

    feederLog(LOG_DEBUG, "sensob_iridium: Analyzing data, size %d\n", length);

    ret = 0;
    if ((data != NULL) && (length > 0))
    {

	overall = (directip_overall_t *)data;
	if (overall->protocol_revision_number == 1)
	{
	    message_length = byteSwap16(overall->overall_message_length);
	    feederLog(LOG_DEBUG, "sensob_iridium: Overall message length %d\n", message_length);
	
	    iei = (struct directip_iei_t *)(data + 3);
	
	    while (iei < (struct directip_iei_t *)((char *)overall + 3 + message_length))
	    {
		switch (iei->iei)
		{
		    case DIRECTIP_MO_HEADER_IEI:
			if (directipHeader((struct directip_mo_header_t *)iei, imei) > 2)
			{
			    return 0;
			}
		    break;
	    	    case DIRECTIP_MO_PAYLOAD_IEI:
	    		directipPayload((struct directip_mo_payload_t *)iei, &data_payload, &data_size);
		    break;
		    case DIRECTIP_MO_LOCATION_INFORMATION_IEI:
			directipLocationInformation((struct directip_mo_location_information_t *)iei);
		    break;
		    default:
			feederLog(LOG_WARNING, "sensob_iridium: Unknowen eie %d\n", iei->iei);
			return 0;
	    	    break;
		}
		iei = (struct directip_iei_t *)((char *)iei + 3 + byteSwap16(iei->length));
	    }

    	    feederLog(LOG_DEBUG, "sensob_iridium: Incoming payload: len %d: \n", data_size);
    	    l = data_size;
    	    while (l > 0)
	    {
	        logHex(buffer, data_payload + data_size - l, (l > 16) ? 16:l);
		feederLog(LOG_DEBUG, "%s\n", buffer);
		l -= (l > 16) ? 16:l;
	    }




	    length = data_size;
	    ld = malloc(length);
	    if (ld == NULL)
	    {
    		feederLog(LOG_WARNING, "sensob: Can not allocate data buffer\n");
	    
		return 0;
	    }
	    memcpy(ld, data_payload, length);
	    pd = NULL;
	    sd = length;
	}
	else if (data[0] == 0x5a)		//delimiter
	{
	    feederLog(LOG_DEBUG, "sensobn: Naked data\n");
	    ld = malloc(length);
	    if (ld == NULL)
	    {
    		feederLog(LOG_WARNING, "sensob: Can not allocate data buffer\n");
	    
		return 0;
	    }
	    memcpy(ld, data, length);
	    pd = NULL;
	    sd = length;
	}
	else
	{
	    feederLog(LOG_WARNING, "sensob_iridium: Protocol revision number != 1\n");
	    return 0;
	}

	pd = findDelimiter(ld, sd);
        if (pd == NULL)
	{
    	    feederLog(LOG_WARNING, "sensobn: No delimiter\n");
    	    free(ld);
    	    ld = NULL;
	    return 0;
	}

	header = (sensobn_header_t *)pd;
//        feederLog(LOG_DEBUG, "sensob: Id %llu, seq %d, size %d\n", byteSwap64(header->id), header->seq, header->size);
        feederLog(LOG_DEBUG, "sensobn: Id %llu, size %d\n", header->id, header->size);
	pd += sizeof(struct sensobn_header_t);

    }

    if (pd == NULL)
    {
	return 0;
    }
    

//    *id = byteSwap64(header->id);
    *id = header->id;

    i = 0;
    feederLog(LOG_DEBUG, "sensobn: header size %d\n", header->size);
    while (header->size > 0)
    {
	item = (sensobn_item_t *)pd;
	pd += sizeof(struct sensobn_item_t);
	val = item->value;
	
	feederLog(LOG_DEBUG, "sensobn: item sensor %u, value %f\n", item->sensor, val);
	header->size--;
	
	{
	    if (!isnan(val))//todo: nevalidni mereni
	    {
		sensors[i] = sensobn_map_to_senso(item->sensor >> 4) | (item->sensor & 0x0f);
		result_array[i] = val;
		if ((sensor[i] == 340260002) && (*id == 105000008))
		{
		    temp_hack = val;
		}
		i++;
		*tm = header->timestamp;
		*type = VALUES_TYPE_OBS;
		
		ret = i;
	    }
	    else
		feederLog(LOG_WARNING, "sensob: Invalid item\n");

	    break;		//todo: viz timestamp flag, pokud nebude musi se brat posledni znamej timestamp
	}
    }
    if (header->size == 0)
    {
	free(ld);
	ld = pd = NULL;
	sd = 0;
    }

    return ret;

/*
    if (data != NULL)
    {
    
    if ((data[0] != ':') && (data[0] != ';'))
	raw = 1;
    else
	raw = 0;
	
    if (!raw)
    {
	switch (data[0])
	{
	    case ':':compressed = 1;
		break;
	    case ';':compressed = 0;
		break;
	}
		
        dlength = ((unsigned long)data[1]) << 24;
	dlength += ((unsigned long)data[2]) << 16;
	dlength += ((unsigned long)data[3]) << 8;
	dlength += ((unsigned long)data[4]);
	ddata = malloc(dlength + 1);
	if (ddata == NULL)
	{
	    feederLog(LOG_WARNING, "senso: Can not allocate decompress buffer\n");
	    return 0;
	}
	feederLog(LOG_DEBUG, "senso: Packet length %ld, data length %ld%s\n", length, dlength, compressed ? ", compressed" : "");

	length -= HEADER_SIZE;
	if (ddata != NULL)
	{
	    if (compressed)
	    {
	        dres = uncompress((unsigned char *)ddata, (uLongf *)&dlength, data + HEADER_SIZE, length);
	    }
	    else
	    {
		memcpy(ddata, data + HEADER_SIZE, length);
		dres = Z_OK;
	    }
	    if (dres == Z_OK)
	    {
		ddata[dlength] = 0;
		feederLog(LOG_DEBUG, "senso: Data, cr: %f%% : \n%s", ((double)length / (double)dlength) * 100.0, ddata);
		message = ddata;
    	    }
	    else
	    {
		zerr(dres);
	    }
	}
    }
    else
    {
	ddata = malloc(length + 1);
	if (ddata == NULL)
	{
	    feederLog(LOG_WARNING, "Can not allocate data buffer\n");
	    return 0;
	}
	memcpy(ddata, data, length);
	ddata[length] = 0;
	feederLog(LOG_DEBUG, "Data raw: %s\n", data);
	message = ddata;
    }
    }


    if (message != NULL)
    {
	c = strchr(message, '\n');
	*c = 0;
	*id = dataCheck(message, tm, &result_array[0], (unsigned long int *)&sensors[0]);
	message = c + 1;
	ret = 1;
	if (strlen(message) == 0)
	{
	    free(ddata);
	    message = ddata = NULL;
	}
    }
    
    *type = VALUES_TYPE_OBS;
    return ret;*/
    return 0;
}

int reply(void *lib_data, int socket, unsigned char *data)
{
    (void)lib_data;
    (void)socket;
    (void)data;
/*    struct sensob_ack_t * ack;

    ack = (struct sensob_ack_t *)data;
    ack->delimiter = 0xff;
    ack->seq = seq;
    seq = 0;
    feederLog(LOG_DEBUG, "senso: replying\n");

    return sizeof(struct sensob_ack_t);
    */
    return 0;
}