Agronode_feeder/feeder_lib/at2000/at2000.c

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

#include "tiny-AES128-C-master/aes.h"

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


#define FM4_BUFFER_SIZE 2048

#define AT2000_SAP_ID_ACK			0x00
#define AT2000_SAP_ID_DEVICE_ID 		0x01
#define AT2000_SAP_ID_CONF_REQ			0x82
#define AT2000_SAP_ID_CONF_REQ_RES		0x83
#define AT2000_SAP_ID_REMOTE_CONF		0x84
#define AT2000_SAP_ID_FIRMWARE_DATA		0x85
#define AT2000_SAP_ID_IO_ELEMENTS_REQ		0x86
#define AT2000_SAP_ID_IO_ELEMENTS_RES		0x87
#define AT2000_SAP_ID_TRACK_RECORDS_REQ		0x88
#define AT2000_SAP_ID_TRACK_RECORDS_RES		0x89
#define AT2000_SAP_ID_LOG_REQ			0x8A
#define AT2000_SAP_ID_LOG_RES			0x8B
#define AT2000_SAP_ID_SESSION_END_REQ		0x0C
#define AT2000_SAP_ID_FIRWARE_UPDATE_STATUS_REQ	0x8D
#define AT2000_SAP_ID_FIRWARE_UPDATE_STATUS_RES	0x8E
#define AT2000_SAP_ID_NONE			0xFF

#define AT2000_SAP_ID_NO_REQ			0xFF

#define AT2000_STATUS_IDLE			0x01
#define AT2000_STATUS_INIT			0x02
#define AT2000_STATUS_TRACK			0x03
#define AT2000_STATUS_TRACK_WAIT		0x04
#define AT2000_STATUS_SESSION_END		0x05
#define AT2000_STATUS_CLOSE			0x06


#define AT2000_IO_EVENTS_COORDINATE_RELIABILITY	11


static uint8_t at2000_aes_key[] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F};

//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;
    unsigned char * int_buffer;
    unsigned int int_buffer_pos;
    unsigned int int_buffer_pos_wr;
//    unsigned int int_buffer_size;
    uint8_t IV[16];
    uint8_t tstV[16];
    uint8_t status;
    uint8_t sapid;
    uint32_t data_len;
    uint8_t pos_obs;
} 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;

typedef struct at2000_packet_t
{
//    uint8_t codec_id;
    uint8_t sap;
    uint8_t len[3];
    uint8_t reserv[12];
} __attribute__((packed)) at2000_packet_t;


typedef struct at2000_track_t
{
    uint16_t recID;
    uint8_t reserv[2];
    uint32_t timestamp;
    uint32_t timestamp_frac;
    float lat;
    float lon;
    uint32_t alt;
    uint32_t speed;
    uint32_t course;
    uint32_t geozone_events;
    uint32_t io_events;
    uint32_t geozone_val;
    uint32_t io_val_bits;
    uint16_t operator;
    uint16_t ain1;
    uint16_t ain2;
    uint16_t ext_power;
    uint16_t cellid;
    uint8_t gsm_level;
    uint8_t current_profile;
    uint8_t battery_level;
    uint8_t temperature;
    uint8_t gps_sats;
} __attribute__((packed)) at2000_track_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;
}

//todo: nasledujici dve pole do pole struktur, tedy vcetne multiplu
/* {FM4 type, FEEDER sensor type id, sensor id, alertable (if prio is 1(high))} */

uint8_t sensorTypes[][4] =
{
    {1, TYPE_PERCENTAGE, 2, 0},
    {9, TYPE_ANALOG_INPUT, 7, 0},
    {21, TYPE_SIGNAL_STRENGTH, 7, 0},
    {22, TYPE_PROFILE, 1, 0},
    {53, TYPE_DIGITAL_INPUT, 14, 0},
    {72, TYPE_THERMOMETER, 28, 0},
    {101, TYPE_DIGITAL_INPUT, 15, 0},
    {102, TYPE_DIGITAL_INPUT, 15, 0},
    {103, TYPE_RESISTANCE, 1, 0},
    {200, TYPE_LINK, 8, 0},
    {210, TYPE_SPEED, 8, 0},
    {238, TYPE_DIGITAL_INPUT, 17, 0},
    {239, TYPE_LINK, 9, 0},
    {240, TYPE_MOVE, 2, 0},
    

    {0, 0, 0, 0}
};
/*
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},
    {79, TYPE_ACCELERATION, 2, 0},
    {80, TYPE_SPEED, 3, 0},
    {81, TYPE_CRUISE, 1, 0},
    {82, TYPE_CRUISE, 2, 0},
    {83, TYPE_CRUISE, 3, 0},
    {84, TYPE_ENGINE, 1, 0},
    {85, TYPE_ENGINE, 2, 0},
    {86, TYPE_ENGINE, 3, 0},
    {87, TYPE_LEVEL, 4, 0},
    {88, TYPE_FREQUENCY, 1, 0},
    {104, TYPE_TIME, 1, 0},
    {127, TYPE_TEMP, 20, 0},
    {128, TYPE_TEMP, 21, 0},
    {135, TYPE_FLOW, 6, 0},
    {136, TYPE_FLOW, 7, 0},
    {207, TYPE_ID, 1, 0},
    {249, TYPE_SIGNAL_STRENGTH, 5, 0},
    {0, 0, 0, 0}
};
*/

double sensorMulti[] =
{
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    0.1,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.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,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0
};
*/
int getSensorType(uint8_t sensor)
{
    int i;

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

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

    sensor_index = getSensorType(sensor);		//todo: check return value 
    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++;
}

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

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;
    unit_new->int_buffer = malloc(FM4_BUFFER_SIZE);
    if (unit_new->int_buffer == NULL)
    {
	free(unit_new);
	return NULL;
    }
//    unit_new->int_buffer_size = FM4_BUFFER_SIZE;
    unit_new->int_buffer_pos = 0;
    unit_new->int_buffer_pos_wr = 0;
    unit_new->status = AT2000_STATUS_IDLE;
    unit_new->sapid = AT2000_SAP_ID_NONE;

    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)
{
    char imei_str[16];

    memcpy(imei_str, data, 15);
    imei_str[15] = 0;
    feederLog(LOG_DEBUG, "at2000: 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;
    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;
    char buffer[32768];
    unsigned int l;
    struct at2000_packet_t * packet;
    uint8_t buffer_dec[512];

    (void)lib_data;
    
    if (data != NULL)
    {
        feederLog(LOG_DEBUG, "at2000: 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, "at2000: new unit for socket %d\n", socket);
    
        unit = unitCreate(socket);	//todo: check if scucsefull
    }
    else
    {
	feederLog(LOG_DEBUG, "at2000: data from known unit on socket %d\n", socket);
    }

    aunit = unit;

    if (aunit->status == AT2000_STATUS_IDLE) 
    {
	if (data[0] == 1)
	{
	
	    packet = (struct at2000_packet_t *)(data + 1);
	
	    aunit->data_len = ((uint32_t)(packet->len[0])) + ((uint32_t)(packet->len[1]) << 8) + ((uint32_t)(packet->len[2]) << 16);
	    feederLog(LOG_DEBUG, "at2000: data size %d\n", aunit->data_len);
	    if (packet->sap == AT2000_SAP_ID_DEVICE_ID)
	    {
		imei(aunit, data + 1 + sizeof(struct at2000_packet_t));
	        memcpy(aunit->IV, data + 1 + sizeof(struct at2000_packet_t) + 15, 16);
		memcpy(aunit->tstV, data + 1 + sizeof(struct at2000_packet_t) + 15 + 16, 16);
	    
	        feederLog(LOG_DEBUG, "at2000: tstV decrypt\n");
	        AES128_CBC_decrypt_buffer(buffer_dec, aunit->tstV, 16, at2000_aes_key, aunit->IV);
		logHex(buffer, buffer_dec, 16);
		feederLog(LOG_DEBUG, "%s\n", buffer);
		
		if (((aunit->imei[14] ^ aunit->IV[0]) != buffer_dec[0]) ||
		    ((aunit->imei[13] ^ aunit->IV[1]) != buffer_dec[1]) ||
		    ((aunit->imei[12] ^ aunit->IV[2]) != buffer_dec[2]) ||
		    ((aunit->imei[11] ^ aunit->IV[3]) != buffer_dec[3]))
		{
		    feederLog(LOG_ERR, "at2000: tstV not passed\n");
		    ret = -1;
		}
	    }
	}
	else
	{
	    feederLog(LOG_ERR, "at2000: bad codec id\n");
	    ret = -1;
	}
    }
    else
    {
	if (data != NULL)
	{
	    aunit->pos_obs = VALUES_TYPE_POS;
	
	    if (aunit->sapid == AT2000_SAP_ID_NONE)
	    {
        	packet = (struct at2000_packet_t *)(data);

		aunit->data_len = ((uint32_t)(packet->len[0])) + ((uint32_t)(packet->len[1]) << 8) + ((uint32_t)(packet->len[2]) << 16);
    	        feederLog(LOG_DEBUG, "at2000: data size %d\n", aunit->data_len);
        
    		if (packet->sap == AT2000_SAP_ID_TRACK_RECORDS_RES)
    		{
        	    feederLog(LOG_DEBUG, "at2000: track log in\n");
        	    aunit->sapid = packet->sap;
/*
		if (data_len < aunit->int_buffer_size)
		{
	    	    aunit->int_buffer = realloc(aunit->int_buffer, data_len);
	    	    if (aunit->int_buffer == NULL)
	    	    {
	    		feederLog(LOG_ERR, "at2000: no more memory for buffer\n");//todo: puvodni pamet je uvolnena nebo jen zustava nerealokovana?
			unit->int_buffer_pos_wr = unit->int_buffer_pos = unit->int_buffer_size = 0;
			ret = -1;
			return ret;
		    }
		
		}
*/		
		
//	    	memcpy(aunit->int_buffer + aunit->int_buffer_pos_wr, data + 16, length - 16);

		    memcpy(data, aunit->tstV, 16);
    		    AES128_CBC_decrypt_buffer(aunit->int_buffer + aunit->int_buffer_pos_wr, data, length, at2000_aes_key, aunit->IV);
		    aunit->int_buffer_pos_wr += (length);
		    aunit->int_buffer_pos = 16;
		
/*		
    		AES128_CBC_decrypt_buffer(aunit->int_buffer + aunit->int_buffer_pos_wr, data + 16, length - 16, at2000_aes_key, aunit->IV);
		aunit->int_buffer_pos_wr += (length - 16);*/
		}
	    }
	    else if (aunit->sapid == AT2000_SAP_ID_TRACK_RECORDS_RES)
	    {
    		AES128_CBC_decrypt_buffer(aunit->int_buffer + aunit->int_buffer_pos_wr, data, length, at2000_aes_key, aunit->IV);
		aunit->int_buffer_pos_wr += (length);
	    }
	}
	
	    if (aunit->sapid == AT2000_SAP_ID_TRACK_RECORDS_RES)
	    {
		//while (aunit->int_buffer_pos + sizeof(struct at2000_track_t) <= aunit->int_buffer_pos_wr)
	    
		if (aunit->int_buffer_pos + sizeof(struct at2000_track_t) <= aunit->int_buffer_pos_wr)
		{
	    	    //AES128_CBC_decrypt_buffer(buffer_dec, aunit->int_buffer + aunit->int_buffer_pos, sizeof(struct at2000_track_t), at2000_aes_key, aunit->IV);
	    	    struct at2000_track_t * track;
	        
	    	    track = (struct at2000_track_t *)(aunit->int_buffer + aunit->int_buffer_pos);

		    switch (aunit->pos_obs)
		    {
			case VALUES_TYPE_POS:
			{
			    logHex(buffer, aunit->int_buffer + aunit->int_buffer_pos, sizeof(struct at2000_track_t));
	    		    feederLog(LOG_DEBUG, "%s\n", buffer);
	        
/*	    		    feederLog(LOG_DEBUG, "track id %d\n", track->recID);
	    		    feederLog(LOG_DEBUG, "timestamp %ld\n", track->timestamp);
	    		    feederLog(LOG_DEBUG, "track lat %f\n", (float)track->lat);
	    		    feederLog(LOG_DEBUG, "track lon %f\n", (float)track->lon);
*/	    	    
	    		    result_array[0] = (float)track->lat;
		    	    result_array[1] = (float)track->lon;
			    sensors[0] = sensors[1] = 0x10;
			    *type = VALUES_TYPE_POS;

			    *id = atol(aunit->imei);
			    *tm = track->timestamp;

			    if ((track->gps_sats != 0xFF) && ((float)track->lat != 0.0) && ((float)track->lon != 0.0) && ((track->io_events & (1 << AT2000_IO_EVENTS_COORDINATE_RELIABILITY)) != 0))
			    {
			        ret = 2;
				aunit->pos_obs = VALUES_TYPE_OBS;
			    }
			    else
			    {
				feederLog(LOG_DEBUG, "at2000: invalid fix\n");
				ret = 0;
				aunit->int_buffer_pos += sizeof(struct at2000_track_t);
	    			aunit->data_len -= sizeof(struct at2000_track_t);
			    }
			    break;
			}
			case VALUES_TYPE_OBS:
			{
	    		    result_array[0] = (float)track->battery_level;
			    sensors[0] = 540070000;
			    *type = VALUES_TYPE_OBS;

			    *id = atol(aunit->imei);
			    *tm = track->timestamp;

			    ret = 1;
			    aunit->pos_obs = VALUES_TYPE_POS;
			    
	    		    aunit->int_buffer_pos += sizeof(struct at2000_track_t);
	    		    aunit->data_len -= sizeof(struct at2000_track_t);
			    
			
			    break;
			}
		    }
	    	}
	    	else
	    	{
	    	    memcpy(aunit->int_buffer, aunit->int_buffer + aunit->int_buffer_pos, aunit->int_buffer_pos_wr - aunit->int_buffer_pos);
	    	    aunit->int_buffer_pos_wr = aunit->int_buffer_pos_wr - aunit->int_buffer_pos;
	    	    aunit->int_buffer_pos = 0;
	    	}

	    }
	
    }
    return ret;
}

void replyBuild(uint8_t * buffer, uint8_t id, uint8_t * payload, uint8_t payload_len)
{
    struct at2000_packet_t * packet;

    packet = (at2000_packet_t *)buffer;
    
    bzero(buffer, 32);
    packet->sap = id;
    packet->len[0] = payload_len;
    packet->len[1] = packet->len[2] = 0;
    if ((payload != NULL) && (payload_len > 0))
	memcpy(packet->reserv, payload, payload_len);
}


void __sleep__(unsigned int sec)
{
    struct timespec tv;
    
    tv.tv_sec = sec;
    tv.tv_nsec = 0;
    
    while (nanosleep(&tv, &tv) == -1) ;
}

int reply(void *lib_data, int socket, unsigned char *data)
{
    unsigned int temp_reply_len = 0;
    uint8_t payload[16];

    (void)lib_data;
    (void)socket;
    feederLog(LOG_DEBUG, "at2000: replying\n");
    
    if (aunit->status == AT2000_STATUS_IDLE)
    {
    	__sleep__(1);

        payload[0] = 0x00;
        replyBuild(data, AT2000_SAP_ID_ACK, payload, 1);
        temp_reply_len = 32;

        aunit->status = AT2000_STATUS_INIT;
    }
    else if (aunit->status == AT2000_STATUS_INIT)
    {
	__sleep__(1);

        replyBuild(data, AT2000_SAP_ID_TRACK_RECORDS_REQ, NULL, 0);
        temp_reply_len = 16;
	    
        aunit->status = AT2000_STATUS_TRACK_WAIT;
    }
    else if (aunit->status == AT2000_STATUS_TRACK)
    {
	__sleep__(1);
        payload[0] = 0x00;
        replyBuild(data, AT2000_SAP_ID_ACK, payload, 1);
        temp_reply_len = 32;

	if (aunit->data_len == 0)
	    aunit->status = AT2000_STATUS_SESSION_END;
	else
    	    aunit->status = AT2000_STATUS_TRACK_WAIT;
    }
    else if (aunit->status == AT2000_STATUS_TRACK_WAIT)
    {
	temp_reply_len = 0;
	
        aunit->status = AT2000_STATUS_TRACK;
    }
    else if (aunit->status == AT2000_STATUS_SESSION_END)
    {
	__sleep__(1);

        replyBuild(data, AT2000_SAP_ID_SESSION_END_REQ, NULL, 0);
        temp_reply_len = 16;
        aunit->status = AT2000_STATUS_CLOSE;
    }
    else if (aunit->status == AT2000_STATUS_CLOSE)
    {
	__sleep__(1);

	temp_reply_len = -1;
    }
    
/*
    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)
{
    (void)lib_data;
    (void)socket;
    feederLog(LOG_DEBUG, "at2000: socket %d opened\n", socket);

    return 0;
}

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

    (void)lib_data;
    (void)socket;
    feederLog(LOG_DEBUG, "at2000: socket %d closed\n", socket);

    unit = unitFind(socket);
    if (unit != NULL)
    {
	free(unit->int_buffer);
	
	unit->imei[0] = 0;		//todo: nebo mozna lepe, zrusit celou jednotku - coz bacha na next
	unit->fd = -1;
	
	if (unit == units)
	    units = unit->next;
	for (unit_parent = units; (unit_parent != NULL); unit_parent = unit_parent->next)
	{
	    if (unit_parent->next == unit)
	    {
		unit_parent->next = unit->next;
	    }
	}
	free(unit);
	unit = NULL;
    }

//    unit->int_buffer_pos_wr = unit->int_buffer_pos = unit->int_buffer_size = 0;
//    unit->int_buffer_pos_wr = unit->int_buffer_pos = 0;
    return 0;
}