#ifndef __RTL__

#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>

#else /*__RTL__*/

#include <rtl.h>
#include <string.h>
#include <linux/types.h>

#endif /*__RTL__*/

//#include "ul_dbuff.h"
#include "vcasdo_fsm.h"
#include "can_vca.h"
#include "vca_canopen.h"

static const char *sdofsmErrorMessages[] = {"none", "SDO transfer time out.", "SDO FSM of this port & node exists."};
static const int sdofsmErrorMessagesCnt = sizeof(sdofsmErrorMessages) / sizeof(sdofsmErrorMessages[0]);

// only for debug purposes
static const char *fsm_state_names[] = {"IDLE", "RUN", "DONE", "ERROR", "ABORT"};

/**
 * vcasdo_error_msg - translates err_no to the string message
 * @err_no: number of error, if FSM state == %sdofsmError
 */
const char* vcasdo_error_msg(int err_no)
{
    if(err_no < 0 || err_no >= sdofsmErrorMessagesCnt) return "unknown error.";
    return sdofsmErrorMessages[err_no];
}

//======================== logging support ==========================
// -------------- log levels ------------------
#define LOG_FATAL   0
#define LOG_ERR     1
#define LOG_MSG     2
#define LOG_INF     3
#define LOG_DEB     4
/*
 * sdo_log - write message to the log if level <= log_level
 * @level: importance of message (0 is the highest)
 * @format: common printf format
 */
static void mylog(int level, const char *format, ...)
{
    va_list ap; 
    va_start(ap, format); 
    vca_vlog("vcasdo", level, format, ap);
    va_end(ap);
}

static void sdo_log_msg(int level, const struct canmsg_t *msg)
{
    char strmsg[100];
    vca_msg2str(msg, strmsg, 100);
    vca_log("vcasdo", level, "msg dump: %s\n", strmsg);
}

static void fatal_error(const char *format, ...)
{
    va_list ap; 
    va_start(ap, format); 
    vca_vlog("vcasdo", LOG_FATAL, format, ap);
    va_end(ap);
  #ifndef __RTL__
    exit(EXIT_FAILURE);
  #else /*__RTL__*/
    while (1) {
        /*Block all other activities*/
	pthread_wait_np ();
    }
  #endif /*__RTL__*/
}
//---------------------------------------------------------------------------
static void sdo_error(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg)
{
    if(!fsm) return;
    // copy msg to the out_msg
    memcpy(&(fsm->out_msg), msg, sizeof(canmsg_t));
    fsm->state = sdofsmError;
    sdo_log_msg(LOG_ERR, msg);
}

int sdo_check_abort(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg)
{
    int head = msg->data[0];
    int cmd = VCA_SDO_GET_COMMAND(head);
    if(cmd == VCA_SDO_ABORT) {
        // copy msg to the out_msg
        memcpy(&(fsm->out_msg), msg, sizeof(canmsg_t));
        fsm->state = sdofsmAbort;
        return 1;
    }
    return 0;
}

void sdo_fill_multiplexor(byte *mult, unsigned index, unsigned subindex)
{
    mult[0] = index % 256;
    mult[1] = (index >> 8) % 256;
    mult[2] = (byte)subindex;    
}

void vcasdo_read_multiplexor(const byte *mult, unsigned *index, unsigned *subindex)
{
    *index = (mult[1] << 8) + mult[0];
    *subindex = mult[2];    
}

void fill_out_msg_cobid(vcasdo_fsm_t *fsm) 
{
    /* Initialize even flags and cob number to default values,
       random values results in troubles */
    fsm->out_msg.cob = 0;
    fsm->out_msg.flags = 0;
    //fsm->out_msg.id = fsm->tx_cob_id;
    if(fsm->is_server) fsm->out_msg.id = fsm->srvcli_cob_id;
    else               fsm->out_msg.id = fsm->clisrv_cob_id;
}

//===================== state functions =============================
void vcasdo_fsm_state_init(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg);
void vcasdo_fsm_state_segment(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg);
//--------------- help functions ------------------------------------
void load_data_from_msg(vcasdo_fsm_t *fsm, const canmsg_t *msg)
{
    ul_dbuff_t *db = &fsm->data;
    int expedited = 0;
    int size = 0;
    int last_segment = 0;
    const unsigned char *data;

    mylog(LOG_DEB, "fsm (%p): load_data_from_msg()\n", fsm);
    if(!db) fatal_error("fsm(%p) load_data_from_msg: fsm->data == NULL\n", fsm);
    data = msg->data;
    if(fsm->statefnc == vcasdo_fsm_state_init) {
        int size_ind;
        // check expedited transfer
        expedited = data[0] & VCA_SDO_EXPEDITED_TRANSFER;
        // get size
        size_ind = data[0] & VCA_SDO_SIZE_INDICATOR;
        if(expedited && size_ind) size = VCA_SDO_EXPEDITED_SIZE(data[0]);
        else if(expedited && !size_ind) size = 7; // unspecified number of bytes
        else if(!expedited && !size_ind) size = 0; // reserved for future use
        else {
            // byte 4 LSB, byte 7 MSB
            size = (data[7] << 8) + data[4];
        }
        ul_dbuff_set_capacity(db, size);
        ul_dbuff_set_len(db, 0);
        fsm->bytes_to_load = size;
        if(!expedited) size = 0; // segmented init does not transfer data
        #ifdef _DEBUG
        mylog(LOG_DEB, "\tmsg head: %s\n", vca_byte2str(data[0], 2));
        mylog(LOG_DEB, "\texpedited transfer: %s\n", (expedited)? "1": "0");
        mylog(LOG_DEB, "\tsize indicator: %s\n", (size_ind)? "1": "0");
        mylog(LOG_DEB, "\tbytes_to_load: %i\n", fsm->bytes_to_load);
        #endif
    }
    else if(fsm->statefnc == vcasdo_fsm_state_segment) {
        size = VCA_SDO_SEGMENTED_SIZE(data[0]);
        last_segment = data[0] & VCA_SDO_LAST_SEGMENT_INDICATOR;
        #ifdef _DEBUG
        mylog(LOG_DEB, "\tmsg head: %s\n", vca_byte2str(data[0], 2));
        mylog(LOG_DEB, "\tlast_segment indicator: %s\n", (last_segment)? "1": "0");
        mylog(LOG_DEB, "\tsize: %i\n", size);
        mylog(LOG_DEB, "\tbytes_to_load: %i\n", fsm->bytes_to_load);
        #endif
    }
    
    if(size > fsm->bytes_to_load) {
        mylog(LOG_ERR, "fsm (%p): received more bytes (%lu) than expected (%i)\n", db->len + size, fsm->bytes_to_load);
        size = fsm->bytes_to_load;
    }
    else if(last_segment && size != fsm->bytes_to_load) {
        mylog(LOG_ERR, "fsm (%p): received other number of bytes (%lu) than expected (%i)\n", db->len + size, fsm->bytes_to_load);
        fsm->bytes_to_load = size;
    }
    if(expedited) ul_dbuff_cat(db, data + 4, size);
    else          ul_dbuff_cat(db, data + 1, size);
    fsm->bytes_to_load -= size;
}

void store_data_to_out_msg(vcasdo_fsm_t *fsm)
{
    int data_start;
    int expedited = 0;
    ul_dbuff_t *db;
    unsigned char *data;
    int size; 

    //if(!fsm->data) fatal_error("fsm(%p) store_data_to_out_msg: fsm->data == NULL\n", fsm);
    db = &fsm->data;
    data = fsm->out_msg.data;
    if(fsm->statefnc == vcasdo_fsm_state_init) {
        fsm->bytes_to_load = db->len;
        if(fsm->bytes_to_load <= 4) {
            expedited = 1;
            size = 4; // expedited transfer contains 4 bytes of data
        }
        else {
            size = 0; // first packet of segmented transfer does not contains data
        }
    }
    else {
        size = 7; // next segmented packet contains up to 7 bytes of data
    }
    if(size > fsm->bytes_to_load) size = fsm->bytes_to_load;
    data_start = db->len - fsm->bytes_to_load;
    fsm->bytes_to_load -= size;
    if(expedited) {
        data[0] = 0;
        data[0] |= VCA_SDO_EXPEDITED_TRANSFER;
        data[0] |= VCA_SDO_SIZE_INDICATOR;
        data[0] |= (unsigned char)(((4 - size) & 0x3) << 2);
        VCA_SDO_SET_EXPEDITED_SIZE(data[0], size)
        sdo_fill_multiplexor(data + 1, fsm->index, fsm->subindex);
        memcpy(data + 4, db->data + data_start, size);
        fsm->out_msg.length = size + 4;
        
        #ifdef _DEBUG
        mylog(LOG_DEB, "store_data_to_out_msg: expedited %i + %i pcs of data stored\n", 4, size);
        mylog(LOG_DEB, "\tobject length: %i, data start: %i, bytes to load: %i\n", db->len, data_start, fsm->bytes_to_load);
        mylog(LOG_DEB, "\tmsg head: %s (withouth command specifier)\n", vca_byte2str(data[0], 2));
        mylog(LOG_DEB, "\texpedited transfer: %s\n", (data[0] & VCA_SDO_EXPEDITED_TRANSFER)? "1": "0");
        mylog(LOG_DEB, "\tsize indicator: %s\n", (data[0] & VCA_SDO_SIZE_INDICATOR)? "1": "0");
        mylog(LOG_DEB, "\tsize: %i\n", size);
        #endif
    }
    else {
        mylog(LOG_DEB, "store_data_to_out_msg: segmented\n");
        #ifdef _DEBUG
        mylog(LOG_DEB, "\tobject length: %i, data start: %i, bytes to load: %i\n", db->len, data_start, fsm->bytes_to_load);
        #endif
        data[0] = 0;
        if(fsm->statefnc == vcasdo_fsm_state_init) {
	    int l;
            // init segment transfer
            data[0] |= VCA_SDO_SIZE_INDICATOR;
            l = db->len;
            data[4] = l % 256;
            data[7] = l / 256;
            fsm->toggle_bit = 0;
            #ifdef _DEBUG
            mylog(LOG_DEB, "\t%i/%i pcs of data stored\n", size, fsm->bytes_to_load + size);
            mylog(LOG_DEB, "\texpedited transfer: %s\n", (data[0] & VCA_SDO_EXPEDITED_TRANSFER)? "1": "0");
            mylog(LOG_DEB, "\tsize indicator: %s\n", (data[0] & VCA_SDO_SIZE_INDICATOR)? "1": "0");
            mylog(LOG_DEB, "\tsize: %i\n", size);
            #endif
            sdo_fill_multiplexor(data + 1, fsm->index, fsm->subindex);
            fsm->out_msg.length = 8;
        }
        else {
            // next segment transfer
            VCA_SDO_SET_SEGMENTED_SIZE(data[0], size);
            if(fsm->bytes_to_load == 0) data[0] |= VCA_SDO_LAST_SEGMENT_INDICATOR;
            if(fsm->toggle_bit) {
                data[0] &= ~VCA_SDO_TOGGLE_BIT;
                data[0] |= VCA_SDO_TOGGLE_BIT;
            }
            fsm->toggle_bit = !fsm->toggle_bit;
            #ifdef _DEBUG
            mylog(LOG_DEB, "\t%i/%i pcs of data stored\n", size, fsm->bytes_to_load + size);
            mylog(LOG_DEB, "\tlast segment indicator: %s\n", (data[0] & VCA_SDO_LAST_SEGMENT_INDICATOR)? "1": "0");
            mylog(LOG_DEB, "\tsize: %i\n", size);
            #endif
            fsm->out_msg.length = 1 + size;
        }
        memcpy(data + 1, db->data + data_start, size);
        //fsm->out_msg.length = size + 1;
        #ifdef _DEBUG
        mylog(LOG_DEB, "\tmsg head: %s (withouth command specifier)\n", vca_byte2str(data[0], 2));
        #endif
    }
}
//-------------------------------------------------------------------
/*
 * vcasdo_fsm_state_init - SDO upload/download init state function 
 *
 * Returns 0 if no answer is in fsm->out_msg, 
 */
void vcasdo_fsm_state_init(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg)
{
    int cmd;
    mylog(LOG_DEB, "*** STATE *** vcasdo_fsm_state_init(fsm: %p)\n", fsm);
    fsm->out_msg.length = 0;
    if(sdo_check_abort(fsm, msg)) return;
    fsm->toggle_bit = 0;
    cmd = VCA_SDO_GET_COMMAND(msg->data[0]);
    // I must check fsm->is_server,
    // because cmds are not unique
    // for ex. VCA_SDO_INIT_UPLOAD_R and VCA_SDO_INIT_UPLOAD_C have the same value (2)
    if(cmd == VCA_SDO_INIT_UPLOAD_C && !fsm->is_server) {
        // ich bin master
        mylog(LOG_DEB, "\tfsm(%p): MASTER got INIT UPLOAD confirmation\n", fsm);
        load_data_from_msg(fsm, msg);
        if(fsm->bytes_to_load == 0) {
            // expedited transfer
            fsm->state = sdofsmDone;
            fsm->err_no = sdofsmDoneUpload;
        }
        else {
            // send upload SDO segment msg
            byte *data = fsm->out_msg.data; 
            memset(data, 0, 8);
            fsm->out_msg.length = 1;
            data[0] = VCA_SDO_COMMAND(VCA_SDO_UPLOAD_SEGMENT_R);
            fsm->statefnc = vcasdo_fsm_state_segment;
        }
        mylog(LOG_DEB, "\t\t%i bytes to load\n", fsm->bytes_to_load);
    }
    else if(cmd == VCA_SDO_INIT_DOWNLOAD_C && !fsm->is_server) {
        mylog(LOG_DEB, "\tfsm(%p): MASTER got INIT DOWNLOAD confirmation\n", fsm);
        if(fsm->bytes_to_load == 0) {
            // expedited transfer
            fsm->state = sdofsmDone;
            fsm->err_no = sdofsmDoneDownload;
        }
        else {
            // send upload SDO segment msg
            byte *data;
            fsm->statefnc = vcasdo_fsm_state_segment;
            store_data_to_out_msg(fsm);
            data = fsm->out_msg.data; 
            data[0] |= VCA_SDO_COMMAND(VCA_SDO_DOWNLOAD_SEGMENT_R);
            fsm->out_msg.id = fsm->srvcli_cob_id;
        }
    }
    else if(cmd == VCA_SDO_INIT_UPLOAD_R && fsm->is_server) {
        // ich bin slave
	byte *data;
        mylog(LOG_DEB, "\tfsm(%p): SLAVE got INIT UPLOAD request\n", fsm);
        fsm->is_uploader = 1;
        vcasdo_read_multiplexor(msg->data + 1, &(fsm->index), &(fsm->subindex)); 
        store_data_to_out_msg(fsm);
        data = fsm->out_msg.data; 
        data[0] &= ~VCA_SDO_COMMAND_MASK;
        data[0] |= VCA_SDO_COMMAND(VCA_SDO_INIT_UPLOAD_C);
        if(fsm->bytes_to_load == 0) {
            // expedited transfer
            //data[0] |= VCA_SDO_EXPEDITED_TRANSFER;
            fsm->state = sdofsmDone;
        }
        else {
            fsm->statefnc = vcasdo_fsm_state_segment;
        }
        sdo_fill_multiplexor(data + 1, fsm->index, fsm->subindex);
    }
    else if(cmd == VCA_SDO_INIT_DOWNLOAD_R && fsm->is_server) {
        // ich bin slave
	byte *data;
        mylog(LOG_DEB, "\tfsm(%p): SLAVE got INIT DOWNLOAD request\n", fsm);
        fsm->is_uploader = 0;
        vcasdo_read_multiplexor(msg->data + 1, &(fsm->index), &(fsm->subindex)); 
        load_data_from_msg(fsm, msg);
        // prepare response msg
        data = fsm->out_msg.data; 
        memset(data, 0, 8);
        data[0] = VCA_SDO_COMMAND(VCA_SDO_INIT_DOWNLOAD_C);
        sdo_fill_multiplexor(data + 1, fsm->index, fsm->subindex);
        if(fsm->bytes_to_load == 0) {
            // expedited transfer
            //data[0] |= VCA_SDO_EXPEDITED_TRANSFER;
            fsm->state = sdofsmDone;
            fsm->out_msg.length = 4;
        }
        else {
            fsm->statefnc = vcasdo_fsm_state_segment;
            fsm->out_msg.length = 4;
        }
    }
    else {
        mylog(LOG_ERR, "fsm(%p): vcasdo_fsm_state_init - unexpected command 0x%x\n", fsm, cmd);
        sdo_error(fsm, msg);
    }
    //fsm->out_msg.length = 8;
    if(fsm->out_msg.length) {
        fill_out_msg_cobid(fsm);
    }
    mylog(LOG_DEB, "\tfsm(%p): out_msg.length: %i\n", fsm, fsm->out_msg.length);
    gettimeofday(&(fsm->last_activity), NULL);
}

//-------------------------------------------------------------------
/*
 * vcasdo_fsm_state_segment - SDO upload/download segment state function 
 *
 * Returns 0 if no answer is in fsm->out_msg, 
 */
void vcasdo_fsm_state_segment(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg)
{
    int cmd;
    mylog(LOG_DEB, "*** STATE *** vcasdo_fsm_state_segment(fsm: %p)\n", fsm);
    fsm->out_msg.length = 0;
    // is it init domain upload response ?
    if(sdo_check_abort(fsm, msg)) return;
    cmd = VCA_SDO_GET_COMMAND(msg->data[0]);

    if(cmd == VCA_SDO_UPLOAD_SEGMENT_C && !fsm->is_server) {
        int tb;
        mylog(LOG_DEB, "fsm(%p): MASTER got UPLOAD SEGMENT confirmation\n", fsm);
        // check toggle bit
        tb = msg->data[0] & VCA_SDO_TOGGLE_BIT;
        if((tb && !fsm->toggle_bit) || (!tb && fsm->toggle_bit)) {
            mylog(LOG_ERR, "\tERROR: bad toggle bit\n");
            vcasdo_fsm_abort(fsm, 0x05030000);
        }
        load_data_from_msg(fsm, msg);
        if(fsm->bytes_to_load == 0) {
            fsm->state = sdofsmDone;
            fsm->err_no = sdofsmDoneUpload;
        }
        else {
            byte *data = fsm->out_msg.data; 
            memset(data, 0, 8);
            data[0] = VCA_SDO_COMMAND(VCA_SDO_UPLOAD_SEGMENT_R);
            // toggle toggle bit
            fsm->toggle_bit = !fsm->toggle_bit;
            data[0] &= ~VCA_SDO_TOGGLE_BIT;
            if(fsm->toggle_bit) data[0] |= VCA_SDO_TOGGLE_BIT;
            fsm->out_msg.length = 1;
        }
    }
    else if(cmd == VCA_SDO_DOWNLOAD_SEGMENT_C && !fsm->is_server) {
        mylog(LOG_DEB, "fsm(%p): MASTER got DOWNLOAD SEGMENT confirmation\n", fsm);
        if(fsm->bytes_to_load == 0) {
            fsm->state = sdofsmDone;
        }
        else {
	    byte *data;
            store_data_to_out_msg(fsm);
            data = fsm->out_msg.data; 
            data[0] &= ~VCA_SDO_COMMAND_MASK;
            data[0] |= VCA_SDO_COMMAND(VCA_SDO_DOWNLOAD_SEGMENT_R);
        }
    }
    else if(cmd == VCA_SDO_UPLOAD_SEGMENT_R && fsm->is_server) {
        // ich bin slave
	byte *data;
        mylog(LOG_DEB, "fsm(%p): SLAVE got UPLOAD SEGMENT request\n", fsm);
        store_data_to_out_msg(fsm);
        if(fsm->bytes_to_load == 0) {
            fsm->state = sdofsmDone;
            fsm->err_no = sdofsmDoneUpload;
        }
        data = fsm->out_msg.data; 
        //data[0] &= ~VCA_SDO_TOGGLE_BIT;
        //data[0] |= VCA_SDO_TOGGLE_BIT;
        data[0] &= ~VCA_SDO_COMMAND_MASK;
        data[0] |= VCA_SDO_COMMAND(VCA_SDO_UPLOAD_SEGMENT_C);
    }
    else if(cmd == VCA_SDO_DOWNLOAD_SEGMENT_R && fsm->is_server) {
        byte *data;
        // ich bin slave
        mylog(LOG_DEB, "fsm(%p): SLAVE got DOWNLOAD SEGMENT request\n", fsm);
        load_data_from_msg(fsm, msg);
        data = fsm->out_msg.data; 
        memset(data, 0, 8);
        data[0] &= ~VCA_SDO_TOGGLE_BIT;
        if(msg->data[0] &  VCA_SDO_TOGGLE_BIT) {
            data[0] |= VCA_SDO_TOGGLE_BIT;
        }
        data[0] &= ~VCA_SDO_COMMAND_MASK;
        data[0] |= VCA_SDO_COMMAND(VCA_SDO_DOWNLOAD_SEGMENT_C);
        fsm->out_msg.length = 1;
        if(fsm->bytes_to_load == 0) {
            fsm->state = sdofsmDone;
            fsm->err_no = sdofsmDoneDownload;
        }
    }
    else {
        mylog(LOG_ERR, "fsm(%p): vcasdo_fsm_state_segment - unexpected command 0x%x\n", cmd);
        sdo_error(fsm, msg);
    }

    if(fsm->out_msg.length) fill_out_msg_cobid(fsm);
    gettimeofday(&(fsm->last_activity), NULL);
}
//-------------------------------------------------------------------

//========================= FSM functions ===========================
void set_fsm_ports(vcasdo_fsm_t *fsm, unsigned srvcli_cob_id,
     unsigned clisrv_cob_id, unsigned node)
{
    if(srvcli_cob_id == 0) srvcli_cob_id = 0x580 | node;
    if(clisrv_cob_id == 0) clisrv_cob_id = 0x600 | node;
    fsm->srvcli_cob_id = srvcli_cob_id;
    fsm->clisrv_cob_id = clisrv_cob_id;
    fsm->node = node;
    /*
    if(fsm->is_server){
       fsm->tx_cob_id = srvcli_cob_id;
       fsm->rx_cob_id = clisrv_cob_id;
    }else{
       fsm->tx_cob_id = clisrv_cob_id;
       fsm->rx_cob_id = srvcli_cob_id;
    }
    */
}
//...................................................................

/**
 * vcasdo_init_fsm -  init SDO FSM 
 * @fsm: fsm to init
 * @srvcli_cob_id: port to use for server->client communication (default 0x850 used if %srvcli_cob_id==0)
 * @clisrv_cob_id: port to use for client->server communication (default 0x600 used if %clisrv_cob_id==0)
 * @node: number of node on CAN bus to communicate with
 */
void vcasdo_init_fsm(vcasdo_fsm_t *fsm, unsigned srvcli_cob_id,
    unsigned clisrv_cob_id, unsigned node)
{
    if(!fsm) fatal_error("vcasdo_fsm_run: fsm == NULL\n");
    ul_dbuff_init(&fsm->data, 0);
    vcasdo_fsm_idle(fsm);
    set_fsm_ports(fsm, srvcli_cob_id, clisrv_cob_id, node);
    //fsm->node = node;
    fsm->statefnc = NULL;
    fsm->is_server = 1;
}

/**
 * vcasdo_destroy_fsm -  frees all SDO FSM resources (destructor) 
 * @fsm: fsm to destroy
 */
void vcasdo_destroy_fsm(vcasdo_fsm_t *fsm)
{
    if(!fsm) return;
    ul_dbuff_destroy(&fsm->data);
    //free(fsm);
}

/**
 * vcasdo_fsm_idle - sets SDO FSM to idle state
 * @fsm: SDO FSM
 */
void vcasdo_fsm_idle(vcasdo_fsm_t *fsm)
{
    mylog(LOG_DEB, "*** ENTERING *** vcasdo_fsm_idle(fsm: %p)\n", fsm);
    if(!fsm) fatal_error("vcasdo_fsm_idle: fsm == NULL\n");
    fsm->state = sdofsmIdle;
    fsm->statefnc = NULL;
    ul_dbuff_set_len(&fsm->data, 0);
    gettimeofday(&(fsm->last_activity), NULL);
}

/**
 * vcasdo_run_fsm - starts SDO communication protocol for this FSM
 * @fsm: SDO FSM
 */
void vcasdo_fsm_run(vcasdo_fsm_t *fsm)
{
    mylog(LOG_DEB, "*** ENTERING *** vcasdo_fsm_run(fsm: %p)\n", fsm);
    if(!fsm) fatal_error("vcasdo_fsm_run: fsm == NULL\n");
    fsm->state = sdofsmRun;
    fsm->statefnc = vcasdo_fsm_state_init;
    gettimeofday(&(fsm->last_activity), NULL);
}

/**
 * vcasdo_run_fsm - aborts SDO communication for this FSM, fill abort out_msg
 * @fsm: SDO FSM
 * @abort_code: code to fill to out_msg
 */
void vcasdo_fsm_abort(vcasdo_fsm_t *fsm, uint32_t abort_code)
{
    unsigned char *data;
    mylog(LOG_DEB, "*** ENTERING *** vcasdo_fsm_abort(fsm: %p)\n", fsm);
    if(!fsm) return;
    fsm->state = sdofsmAbort;
    fsm->err_no = abort_code;
    data = fsm->out_msg.data;
    data[0] = VCA_SDO_COMMAND(VCA_SDO_ABORT);
    sdo_fill_multiplexor(data + 1, fsm->index, fsm->subindex);
    fill_out_msg_cobid(fsm);
    *((uint32_t*)(data + 4)) = abort_code;
    fsm->out_msg.length = 8;
}

/**
 * vcasdo_fsm_upload - starts upload SDO communication protocol for this FSM
 * @fsm:         SDO FSM
 * @db:        pointer to a ul_dbuff structure to store received/transmitted data
 * @node:        CANopen device node to upload from
 * @index:       uploaded object index
 * @subindex:    uploaded object subindex
 * @srvcli_cob_id: port to use for server->client communication (default 0x850 used if %srvcli_cob_id==0)
 * @clisrv_cob_id: port to use for client->server communication (default 0x600 used if %clisrv_cob_id==0)
 *
 * Returns not 0 if %fsm->out_msg contains CAN message to sent
 */
int vcasdo_fsm_upload(vcasdo_fsm_t *fsm, int node, unsigned index, byte subindex, unsigned srvcli_cob_id, unsigned clisrv_cob_id)
{
    byte *data;
    set_fsm_ports(fsm, srvcli_cob_id, clisrv_cob_id, node);
    //fsm->node = node;
    fsm->index = index; fsm->subindex = subindex;
    fsm->is_server = 0; // only master can invoke SDO communication
    data = fsm->out_msg.data;
    memset(data, 0, CAN_MSG_LENGTH);
    data[0] = VCA_SDO_COMMAND(VCA_SDO_INIT_UPLOAD_R);
    sdo_fill_multiplexor(data + 1, fsm->index, fsm->subindex);
    // SDO command should probably have 8 bytes
    //fsm->out_msg.length = 4;
    fsm->out_msg.length = CAN_MSG_LENGTH;
    fsm->is_uploader = 1;
    fill_out_msg_cobid(fsm);
    vcasdo_fsm_run(fsm);
    return 1;
}

/**
 * vcasdo_fsm_download - starts download SDO communication protocol for this FSM
 * @fsm:         SDO FSM
 * @dbuff:       pointer to a ul_dbuff structure to store received/transmitted data
 * @node:        CANopen device node to upload from
 * @index:       uploaded object index
 * @subindex:    uploaded object subindex
 * @srvcli_cob_id: port to use for server->client communication (default 0x850 used if %srvcli_cob_id==0)
 * @clisrv_cob_id: port to use for client->server communication (default 0x600 used if %clisrv_cob_id==0)
 *
 * Returns not 0 if %fsm->out_msg contains CAN message to sent
 */
int vcasdo_fsm_download(vcasdo_fsm_t *fsm, ul_dbuff_t *dbuff, int node, unsigned index, byte subindex, unsigned srvcli_cob_id, unsigned clisrv_cob_id)
{
    byte *data;
    if(!fsm) fatal_error("vcasdo_fsm_download: fsm is NULL\n");
    if(!dbuff) fatal_error("vcasdo_fsm_download: data is NULL\n");
    set_fsm_ports(fsm, srvcli_cob_id, clisrv_cob_id, node);
    //fsm->node = node;
    fsm->index = index; fsm->subindex = subindex;
    if(&fsm->data != dbuff) ul_dbuff_cpy(&fsm->data, dbuff->data, dbuff->len);
    fsm->is_server = 0; // only master can invoke SDO communication
    fsm->is_uploader = 0;
    fill_out_msg_cobid(fsm);
    vcasdo_fsm_run(fsm);
    // store_data_to_out_msg() MUST be after vcasdo_fsm_run()
    // store_data_to_out_msg() does not work correctly if fsm->statefnc==NULL
    data = fsm->out_msg.data;
    memset(data, 0, CAN_MSG_LENGTH);
    store_data_to_out_msg(fsm);
    data[0] &= ~VCA_SDO_COMMAND_MASK;
    data[0] |= VCA_SDO_COMMAND(VCA_SDO_INIT_DOWNLOAD_R);
    // SDO command should probably have 8 bytes
    fsm->out_msg.length = CAN_MSG_LENGTH;
    return 1;
}

//----------------------------------------------------------------------
// 0 not for me
static int is_for_me(const vcasdo_fsm_t *fsm, const struct canmsg_t *msg)
{
    int ret = 0;
    if(!fsm) return ret;
    if(!msg) return ret;
    if(!msg->length) return ret;
    
//    unsigned char cmd = msg->data[0];
//    unsigned char sdonumber = cmd >> VCA_SDO_COMMAND_OFFSET;
    
    // check if message is for me
    if(fsm->is_server) {
        mylog(LOG_DEB, "SLAVE: is_for_me(): checking msg cob-id %x against fsm cob-id %x\n", msg->id, fsm->clisrv_cob_id);
        if(msg->id != fsm->clisrv_cob_id) return ret;
    }
    else {
        mylog(LOG_DEB, "MASTER: is_for_me(): checking msg cob-id %x against fsm cob-id %x\n", msg->id, fsm->srvcli_cob_id);
        if(msg->id != fsm->srvcli_cob_id) return ret;
    }
    //if(msg->id != fsm->rx_cob_id) return ret;
    /*
    if(fsm->is_server) {
        if(VCA_SDO_GET_PORT(msg->id) != fsm->clisrv_cob_id) return ret;
    }
    else {
        if(VCA_SDO_GET_PORT(msg->id) != fsm->srvcli_cob_id) return ret;
    }
    if(VCA_SDO_GET_NODE(msg->id) != fsm->node) return ret;
    */
    return 1;
}

//-------------------------------------------------------------------
/**
 * vcasdo_fsm_taste_msg - try to process msg in FSM
 * @fsm: fsm to process msg
 * @msg: tried msg
 *
 * Return Value: zero if msg is not eatable for FSM
 */
 /*
 *%sdofsmMsgRefuse if FSM refuses %msg, 
 *         %sdofsmMsgEat       if FSM proceses %msg, 
 *         %sdofsmMsgEatAnswer if FSM proceses %msg and it has answer in %fsm->out_msg prepared
 *         %sdofsmMsgEatError  %msg was eaten, but it does not match communication protocol or abort msg was detected
 */
int vcasdo_fsm_taste_msg(vcasdo_fsm_t *fsm, const canmsg_t *msg)
{
    #ifdef _DEBUG
    char msgstr[100];
    vca_msg2str(msg, msgstr, 100);
    mylog(LOG_DEB, "tasting message: '%s' in state: %s\n", msgstr, fsm_state_names[fsm->state]);
    #endif
    if(!is_for_me(fsm, msg)) return 0;// sdofsmMsgRefuse;
    
    if(fsm->statefnc && fsm->state == sdofsmRun) fsm->statefnc(fsm, msg);
    // if out msg is shorter than 8 byte, append zeros
    // SDO command should probably have 8 bytes
    if(fsm->out_msg.length > 0 && fsm->out_msg.length < CAN_MSG_LENGTH) {
        byte *data = fsm->out_msg.data;
        int l = fsm->out_msg.length;
        memset(data + l, 0, CAN_MSG_LENGTH - 1);
    }
    return 1;
    /*
    if(fsm->statefnc) {
        if(fsm->state == sdofsmError) return sdofsmMsgEatError;
        if(fsm->state == sdofsmAbort) return sdofsmMsgEatError;
    }
    return sdofsmMsgEat;
    */
}