#ifndef __RTL__

#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <stdarg.h>
#include <string.h>

#else /*__RTL__*/

#include <rtl.h>
#include <string.h>
#include <signal.h>
#include <posix/unistd.h>

#define CAN_VCA_WITHOUT_FILE

#define printf rtl_printf
#define strtol simple_strtol
#define strtoll simple_strtoll
#define strtoul simple_strtoul
#define strtoull simple_strtoull

#endif /*__RTL__*/

#include "ul_utmalloc.h"
#include "ul_gavl.h"
#include "ul_gavlcust.h"
#include "sui_event.h"
#include "sui_dinfo.h"
#include "sui_dinfo_dbuff.h"
#include "sui_dievc.h"
#include "can_vca.h"
#include "vca_od.h"
#include "vcasdo_msg.h"
#include "vca_hwmod.h"

static const char* od_item_type2str(int type);
static int get_object_data_size(vcaod_object_t *o, uint32_t *abort_code);

//====================== logging support =======================
static int log_is_cont = 0;

// -------------- log levels ------------------
#define LOG_FATAL   0
#define LOG_ERR     1
#define LOG_MSG     2
#define LOG_INF     3
#define LOG_DEB     4
#define LOG_TRASH   5

#define LOG_MYLOG   LOG_TRASH

static void myvlog(int level, const char *format, va_list ap)
{
    if(log_is_cont) level |= VCA_LOGL_CONT;
    vca_vlog("vcaod", level, format, ap);
}

static void mylog(int level, const char *format, ...)
{
    va_list ap; 
    va_start(ap, format); 
    myvlog(level, format, ap);
    va_end(ap);
}
/*
static void fatal_error(const char *format, ...)
{
    va_list ap; 
    va_start(ap, format); 
    myvlog(LOG_FATAL, format, ap);
    va_end(ap);
    exit(-1);
}
*/
//=================================================================
typedef struct data_type_def_t {
    const char *name;
    int size;
} data_type_def_t;

enum {  data_type_UNKNOWN = 0, data_type_BOOLEAN, 
        data_type_INTEGER8, data_type_INTEGER16, data_type_INTEGER32, 
        data_type_UNSIGNED8, data_type_UNSIGNED16, data_type_UNSIGNED32,
        data_type_REAL32,
        data_type_VISIBLE_STRING, data_type_OCTET_STRING, data_type_UNICODE_STRING,
        data_type_TIME_OF_DAY, data_type_TIME_DIFERENCE,
        data_type_reserved1,
        data_type_DOMAIN,
        data_type_INTEGER24,
        data_type_REAL64,
        data_type_INTEGER40, data_type_INTEGER48, data_type_INTEGER56, data_type_INTEGER64, 
        data_type_UNSIGNED24,
        data_type_reserved2,
        data_type_UNSIGNED40, data_type_UNSIGNED48, data_type_UNSIGNED56, data_type_UNSIGNED64,
        data_type_reserved3, data_type_reserved4, data_type_reserved5, data_type_reserved6,
        data_type_PDO_COMMUNICATION_PARAMETER, data_type_PDO_MAPPING,
        data_type_SDO_PARAMETER,
        data_type_IDENTITY
};

static data_type_def_t data_types[] = 
    {{"UNKNOWN", -1}, {"BOOLEAN", 1}, 
     {"INTEGER8", 1}, {"INTEGER16", 2}, {"INTEGER32", 4}, 
     {"UNSIGNED8", 1}, {"UNSIGNED16", 2}, {"UNSIGNED32", 4},
     {"REAL32", 4},
     {"VISIBLE_STRING", 0}, {"OCTET_STRING", 0}, {"UNICODE_STRING", 0},
     {"TIME_OF_DAY", -1}, {"TIME_DIFERENCE", -1},
     {"reserved", -1},
     {"DOMAIN", -1},
     {"INTEGER24", 3},
     {"REAL64", 8},
     {"INTEGER40", 5}, {"INTEGER48", 6}, {"INTEGER56", 7}, {"INTEGER64", 8}, 
     {"UNSIGNED24", 3},
     {"reserved", -1},
     {"UNSIGNED40", 5}, {"UNSIGNED48", 6}, {"UNSIGNED56", 7}, {"UNSIGNED64", 8},
     {"reserved", -1}, {"reserved", -1}, {"reserved", -1}, {"reserved", -1},
     {"PDO_COMMUNICATION_PARAMETER", -1}, {"PDO_MAPPING", -1},
     {"SDO_PARAMETER", -1},
     {"IDENTITY", -1}
    };
static int data_types_cnt = sizeof(data_types) / sizeof(data_types[0]);

//==================== vca OD iplementation =======================
inline int vcaod_cmp_fnc(const unsigned *a, const unsigned *b)
{
  if (*a > *b) return 1;
  if (*a < *b) return -1;
  return 0;
}

GAVL_CUST_NODE_INT_DEC(vcaod, vcaod_root_t, vcaod_object_t, unsigned,
	my_root, my_node, index, vcaod_cmp_fnc)

GAVL_CUST_NODE_INT_IMP(vcaod, vcaod_root_t, vcaod_object_t, unsigned,
	my_root, my_node, index, vcaod_cmp_fnc)
//=================================================================

#ifndef CAN_VCA_WITHOUT_FILE
//=================================================================
static int eds_line_no = 0; // counter of lines read from EDS
static int get_line(FILE *in, ul_dbuff_t *line)
{
    int c;
    ul_dbuff_set_len(line, 0);
    while(1) {
        c = getc(in);
        if(c == EOF) break;
        ul_dbuff_append_byte(line, (unsigned char)c);
        if(c == '\n') break;
    }
    if(line->len) eds_line_no++;
    ul_dbuff_append_byte(line, '\0');
    return line->len - 1;   // ignore terminating zero
}
#endif /*CAN_VCA_WITHOUT_FILE*/
//.................................................................
static vcaod_object_t* od_item_init(vcaod_object_t *item)
{
    if(!item) item = (vcaod_object_t*)malloc(sizeof(vcaod_object_t));
    if(item) {
        memset(item, 0, sizeof(*item));
        item->subindex = -1;
        ul_dbuff_init(&item->value, 0);
        item->dinfo = NULL;
    }
    return item;
}
//.................................................................
static void od_item_destroy(vcaod_object_t *o)
{
    if(o) {
        #ifdef _DEBUG_FULL
        mylog(LOG_DEB, "od_item_destroy(): object [%04x:%02x]\n", o->index, o->subindex);
        #endif
        if(o->dinfo) {
            if(o->flags & VCAOD_OBJECT_FLAG_WEAK_DINFO) sui_dinfo_dec_refcnt(o->dinfo);
            else sui_dinfo_del_weakptr(o->dinfo, &(o->dinfo));
        }
        if(o->subobjects) {
            int i;
            for(i=0; i<o->subcnt; i++) {
                vcaod_object_t *o1 = o->subobjects + i;
                if(o1) od_item_destroy(o1);
            }
            free(o->subobjects);
        }
        ul_dbuff_destroy(&o->value);
    }
}
//.................................................................
void od_item_set_value_u64(vcaod_object_t *item, uint64_t val)
{
    int size = get_object_data_size(item, 0);
    ul_dbuff_set_len(&(item->value), size);
    memset(item->value.data, 0, item->value.len);
    if(size > sizeof(val)) size = sizeof(val);
    memcpy(item->value.data, &val, size);
    //ul_dbuff_fix_endianning(&(item->value));
}
//.................................................................
void od_item_set_value_as_str(vcaod_object_t *item, const char* valstr)
{
    if(!item) return;
    if(item->data_type == data_type_VISIBLE_STRING) {
        int size = strlen(valstr);
        ul_dbuff_set_len(&(item->value), size);
        memcpy(item->value.data, valstr, item->value.len);
    }
    else if(item->data_type >= data_type_BOOLEAN && item->data_type <= data_type_INTEGER32) {
        long i = strtol(valstr, NULL, 0);
        od_item_set_value_u64(item, i);
    }
    else if(item->data_type >= data_type_UNSIGNED8 && item->data_type <= data_type_UNSIGNED32) {
        unsigned long i = strtoul(valstr, NULL, 0);
        od_item_set_value_u64(item, i);
    }
    else if(item->data_type == data_type_INTEGER24 || (item->data_type >= data_type_INTEGER40 && item->data_type <= data_type_INTEGER64)) {
        //printf("od_item_set_value_as_str(): item_data_type: 0x%x, value: '%s'\n", item->data_type, valstr);
        long long i = strtoll(valstr, NULL, 0);
        od_item_set_value_u64(item, i);
    }
    else if(item->data_type == data_type_UNSIGNED24 || (item->data_type >= data_type_UNSIGNED40 && item->data_type <= data_type_UNSIGNED64)) {
        unsigned long long i = strtoull(valstr, NULL, 0);
        od_item_set_value_u64(item, i);
    }
}
#ifndef CAN_VCA_WITHOUT_FILE
//.................................................................
// reads object definition from EDS file
// object definition lines after [....]
// after return line contains next unprocessed line from EDS file
static void get_od_item_def(FILE *in, ul_dbuff_t *line, vcaod_object_t *item)
{
    int l=0;
    int is_subindex;
    int type_size = 0;
    int default_val_is_set = 0;

    if(!item) return;
    is_subindex = (item->subcnt == 0);
    while(1) {
        if(!line->len || !line->data[0]) {
            l = get_line(in, line); // get new line only if the old is processed
            if(!l) break; // EOF
        }
        ul_dbuff_trim(line);
        if(line->data[0] == '[') break; // next index/subindex definition
        l = ul_str_cpos(line->data, '=', 0);
        if(l >= 0) {
            line->data[l] = '\0'; 
            if(vca_strmatch(line->data, "ParameterName") > 0) {
                ul_str_ncpy(item->name, line->data + l + 1, VCAOD_OBJECT_NAME_LEN);
            }
            else if(vca_strmatch(line->data, "SubNumber") > 0) { 
                errno = 0;
                item->subcnt = strtol(line->data + l + 1, NULL, 0);
                if(errno) item->subcnt = 0;
            }
            else if(vca_strmatch(line->data, "ObjectType") > 0) { 
                errno = 0;
                item->object_type = strtol(line->data + l + 1, NULL, 0);
                if(errno) item->object_type = 0;
                if(is_subindex) switch(item->object_type) {
                    case 0:
                    case vcaod_object_type_VAR: break;
                    default: 
                        mylog(LOG_MYLOG, "[%04x:%02x] subindex type should not be 0x%x (%s)\n", 
                                item->index, item->subindex, item->object_type, od_item_type2str(item->object_type));
                        mylog(LOG_MYLOG, "\tchanging it to 0x7 (VAR)\n"); 
                        item->object_type = vcaod_object_type_VAR;
                        break;
                }
                type_size = get_object_data_size(item, 0);
            }
            else if(vca_strmatch(line->data, "DataType") > 0) { 
                errno = 0;
                item->data_type = strtol(line->data + l + 1, NULL, 0);
                if(errno) item->data_type = 0;
            }
            else if(vca_strmatch(line->data, "AccessType") > 0) {
                unsigned char *pc = line->data + l + 1;
                if(vca_strmatch(pc, "ro") >= 0) item->access = vcaod_access_RO;
                else if(vca_strmatch(pc, "wo") >= 0) item->access = vcaod_access_WO;
                else if(vca_strmatch(pc, "rw") >= 0) item->access = vcaod_access_RW;
                else if(vca_strmatch(pc, "rwr") >= 0) item->access = vcaod_access_RWR;
                else if(vca_strmatch(pc, "rww") >= 0) item->access = vcaod_access_RWW;
                else if(vca_strmatch(pc, "const") >= 0) item->access = vcaod_access_CONST;
            }
            else if(vca_strmatch(line->data, "DefaultValue") > 0) {
                od_item_set_value_as_str(item, line->data + l + 1);
                default_val_is_set = 1;
            }
            else if(vca_strmatch(line->data, "PDOMapping") > 0) {
                long li;
                errno = 0;
                li = strtol(line->data + l + 1, NULL, 0);
                item->flags &= ~VCAOD_OBJECT_FLAG_PDO_MAPPING;
                if(!errno) if(li) item->flags |= VCAOD_OBJECT_FLAG_PDO_MAPPING;
            }
            else {
                #ifdef _DEBUG_FULL
                mylog(LOG_DEB, "[%04x:%02x] get_od_item_def() - line %i: flushing: %s\n", item->index, item->subindex, eds_line_no, line->data);
                #endif
            }
            //LowLimit=0x01, HighLimit=0x088, is not implemented yet
        }
        line->data[0] = 0;
    }
    if(!default_val_is_set) {
        od_item_set_value_as_str(item, "");
    }
}
#endif /*CAN_VCA_WITHOUT_FILE*/
//-----------------------------------------------------------------
void vcaod_od_free(vcaod_root_t *odroot)
{
    vcaod_object_t *o;
    if(!odroot) return;
    while((o = vcaod_first(odroot)) != NULL) {
        vcaod_delete(odroot, o);
        od_item_destroy(o);
        free(o);
    }
}
#ifndef CAN_VCA_WITHOUT_FILE
//-----------------------------------------------------------------
int vcaod_load_eds(vcaod_root_t *odroot, const char* eds_file_name)
{
    FILE *in;
    ul_dbuff_t line;
    eds_line_no = 0;
    mylog(LOG_INF, "Loading EDS from file '%s'\n", eds_file_name);
    if(!vcaod_is_empty(odroot)) {
        mylog(LOG_ERR, "vcaod_load_eds: OD root is not empty");
        return -1;
    }
    in = fopen(eds_file_name, "rb");
    if(!in) {
        mylog(LOG_MSG, "vcaod_load_eds: can't open file %s for reading.", eds_file_name);
        return -2;
    }
    
    ul_dbuff_init(&line, 0);
    while(1) {
        //int l=0;
        vcaod_object_t *item;
	
        if(!line.len || !line.data[0]) { 
            int l = get_line(in, &line); // get new line only if the old is processed
            if(!l) break; // eof
        }
        
        ul_dbuff_trim(&line);
        if(!line.data[0]) continue; // skip empty lines
        
        if(line.data[0] == '[') {
            // index
            unsigned ix;
            char *pc;
            ix = strtol(line.data+1, &pc, 16);
            if(*pc != ']') { // other than index declaration
                if(ul_str_pos(pc, "sub", 0) >= 0) {
                    mylog(LOG_ERR, "EDS read error at line %i: '%s'\n"
                                   "subindex declaration should not be here\n", 
                                   eds_line_no, line.data);
                }
                else {
                    #ifdef _DEBUG_FULL
                    mylog(LOG_DEB, "line %i: flushing: %s\n", eds_line_no, line.data);
                    #endif
                }
                line.data[0] = 0; // mark line as processed
                continue;
            }
            
            *pc = '\0'; // cutoff trailing ']'
            
            // index declaration
            #ifdef _DEBUG_FULL
            mylog(LOG_DEB, "+++++ processing line %i: %s\nparsing index [%04x]\n", eds_line_no, line.data, ix);
            #endif
            item = od_item_init(NULL);
            item->index = ix;
            line.data[0] = 0;
            get_od_item_def(in, &line, item);
            if(item->subcnt > 0) {
                // read subindicies
                int subcnt = item->subcnt;
                // make space also for optional FF subindex
                item->subobjects = malloc(sizeof(vcaod_object_t) * subcnt);
                if(item->subobjects) {
                    int i;
                    for(i=0; i<subcnt; i++) od_item_init(item->subobjects + i);
                    while(line.len) {
                        ul_dbuff_trim(&line);
                        if(line.data[0] == '[' && ul_str_pos(line.data, "sub", 0) == 5) {
                            int subix = strtol(line.data+8, NULL, 16);
                            #ifdef _DEBUG_FULL
                            mylog(LOG_DEB, "----- processing line %i: %s\nparsing index [%04x:%02x]\n", eds_line_no, line.data, item->index, subix);
                            #endif
                            if(subix < subcnt || subix == 0xFF) {
                                int six = subix;
                                if(subix == 0xFF) {
                                    six = subcnt; // optional subindex FF (see CANopen docs)
                                    if(subcnt == item->subcnt) {
                                        // append only one FF subindex
                                        item->subcnt++;
                                        item->subobjects = realloc(item->subobjects, sizeof(vcaod_object_t) * item->subcnt);
                                        od_item_init(item->subobjects + subcnt);
                                    }
                                }
                                item->subobjects[six].index = item->index;
                                item->subobjects[six].subindex = subix;
                                line.data[0] = 0;
                                get_od_item_def(in, &line, item->subobjects + six);
                            }
                            else {
                                mylog(LOG_ERR, "EDS parser error line %i: %s\nsubindex [%02x] out of range (%x)\n", eds_line_no, line.data, subix, subcnt);
                            }
                        }
                        else {
                            // next object
                            break;
                        }
                    }
                    // check if all subindicies are filled
                    for(i=0; i<subcnt; i++) if(item->subobjects[i].subindex < 0) {
                        mylog(LOG_ERR, "EDS read error at line %i: index[%04x]subobject[%02x] not filled\n", eds_line_no, ix, i);
                    }
                }
            }
            vcaod_insert(odroot, item);
        }        
        else {
            line.data[0] = 0; // mark line as processed
        }
    }
    ul_dbuff_destroy(&line);
    return 0;
}
#endif /*CAN_VCA_WITHOUT_FILE*/
//-------------------------------------------------------------------
static const char* od_item_type2str(int type)
{
    const char *pc;
    switch(type) {
        case vcaod_object_type_DOMAIN: pc = "DOMAIN"; break;
        case vcaod_object_type_DEFTYPE: pc = "DEFTYPE"; break;
        case vcaod_object_type_DEFSTRUCT: pc = "DEFSTRUCT"; break;
        case vcaod_object_type_VAR: pc = "VAR"; break;
        case vcaod_object_type_ARRAY: pc = "ARRAY"; break;
        case vcaod_object_type_RECORD: pc = "RECORD"; break;
        default: pc = "UNKNOWN";
    }
    return pc;
}
//...................................................................
static void dump_item(const vcaod_object_t *item)
{
    char indent = '\t';
    if(item->subindex < 0) indent = ' ';
    //mylog(LOG_INF, "%c\tParameterName='%s'\n", indent, item->name);
    printf("%c\tParameterName='%s'\n", indent, item->name);
    if(item->subcnt > 0) {
        //mylog(LOG_INF, "%c\tSubNumber=%i\n", indent, item->subcnt);
        printf("%c\tSubNumber=%i\n", indent, item->subcnt);
    }
    else {
        int type;
        int i;
        unsigned char *buf;
        const char *pc = od_item_type2str(item->object_type);
	//mylog(LOG_INF, "%c\tObjectType=%02x(%s)\n", indent, item->object_type, pc);
        printf("%c\tObjectType=%02x(%s)\n", indent, item->object_type, pc);
        type = item->data_type;
        if(type >= data_types_cnt) type = 0;
        //mylog(LOG_INF, "%c\tDataType=%04x(%s)\n", indent, item->data_type, data_types[type].name);
        printf("%c\tDataType=%04x(%s)\n", indent, item->data_type, data_types[type].name);
        switch(item->access) {
            case vcaod_access_RW: pc = "rw"; break;
            case vcaod_access_RWR: pc = "rwr"; break;
            case vcaod_access_RWW: pc = "rww"; break;
            case vcaod_access_RO: pc = "ro"; break;
            case vcaod_access_WO: pc = "wo"; break;
            case vcaod_access_CONST: pc = "const"; break;
            default: pc = "unknown";
        }
        //mylog(LOG_INF, "%c\tAccessType=%s\n", indent, pc);
        printf("%c\tAccessType=%s\n", indent, pc);
        //mylog(LOG_INF, "%c\tPDOMapping=%i\n", indent, item->pdo_mapping);
        printf("%c\tPDOMapping=%s\n", indent, (item->flags & VCAOD_OBJECT_FLAG_PDO_MAPPING)? "yes": "no");
        printf("%c\tValue= ", indent);
        buf = item->value.data;
        for(i=0; i<item->value.len; i++) {
            if(i == 0) printf("dbuff [");
            else       printf(" ");
            printf("%02x", buf[i]);
        }
        printf("]\n");
    }
}
//...................................................................
void vcaod_dump_od(vcaod_root_t *odroot)
{
    vcaod_object_t *item;
    for(item=vcaod_first(odroot); item; item=vcaod_next(odroot, item)) {
        //mylog(LOG_INF, "[%04x]\n", item->index);
        printf("[%04x]\n", item->index);
        dump_item(item);
        if(item->subcnt) {
            int i;
            // item->subobjects[subcnt] can be optional FF subindex
            for(i=0; i<item->subcnt; i++) {
                int six = item->subobjects[i].subindex;
                // skip subindicies not specified in EDS
                if(six < 0) continue;
                //mylog(LOG_INF, "\n");
                //mylog(LOG_INF, "\tsubindex[%02x]\n", six);
                printf("\n\tsubindex[%02x]\n", six);
                dump_item(item->subobjects + i);
            }
        }
    }
}
//-------------------------------------------------------------------
vcaod_object_t* _vcaod_find_object(vcaod_root_t *odroot, unsigned ix, unsigned subix, uint32_t *abort_code)
{
    vcaod_object_t *o = vcaod_find(odroot, &ix);
    if(!o) {
        //mylog(LOG_DEB, "_vcaod_find_object(): object %04x:%02x index not found in OD\n", ix, subix);
        if(abort_code) *abort_code = 0x6020000;
        return NULL;
    }
    if(o->subcnt == 0) return o;
    if(o->object_type != vcaod_object_type_ARRAY) {
        int s = -1;
        if(subix == 0xFF) s = o->subcnt - 1;
        else if(subix < o->subcnt) s = subix;
        if(s == -1) {
            //mylog(LOG_MSG, "vcaod_get_value(): object %04x:%02x subindex not found in OD\n", ix, subix);
            if(abort_code) *abort_code = 0x6090011;
            return NULL;
        }
        if(o->subobjects[s].subindex != subix) {
            //mylog(LOG_MSG, "vcaod_get_value(): object %04x:%02x subobject[%i] does not have subindex %x\n", ix, subix, s, subix);
            if(abort_code) *abort_code = 0x6090011;
            return NULL;
        }
        o = o->subobjects + s;
    }
    return o;
}
//..........................................................................
static int get_object_data_size(vcaod_object_t *o, uint32_t *abort_code)
{
    int dsize = -1;
    if(o->data_type < data_types_cnt) dsize = data_types[o->data_type].size;
    if(dsize == -1) {
        if(abort_code) {
            mylog(LOG_ERR, "get_object_size(): [%04x:%02x] size of data type 0x%x not known\n", o->index, o->subindex, o->data_type);
            *abort_code = 0x8000000;
        }
        else {
            dsize = 0;
        }
    }
    return dsize;
}
//..........................................................................
int vcaod_get_value(vcaod_root_t *odroot, unsigned ix, unsigned subix, void *buff, int len, uint32_t *abort_code)
{
    vcaod_object_t *o = _vcaod_find_object(odroot, ix, subix, abort_code);
    mylog(LOG_DEB, "vcaod_get_value(): reading object %04x:%02x value\n", ix, subix);
    if(!o) {
        mylog(LOG_MSG, "vcaod_get_value(): object %04x:%02x index not found in OD\n", ix, subix);
        return -1;
    }
    // check write only
    if(o->access == vcaod_access_WO) {
        mylog(LOG_MSG, "vcaod_set_value(): [%04x:%02x] attempt to read WRITE ONLY object\n", ix, subix);
        if(abort_code) *abort_code = 0x6010001;
        return -1;
    }
    
    if(o->dinfo) {
        // use dinfo if it does exists
        // TODO: support reading other objects than long via dinfo
        // sui_wr_long() fires object change if necessary
        long val;
        int max = (sizeof(val) > len)? len: sizeof(val);
        if(sui_rd_long(o->dinfo, 0, &val) == SUDI_DATA_OK) {
            mylog(LOG_DEB, "vcaod_get_value(): object value read using dinfo\n");
            memset(buff, 0, len);
            memcpy(buff, &val, max);
        }
        else {
            mylog(LOG_DEB, "vcaod_get_value(): ERROR reading object value using dinfo\n");
            if(abort_code) *abort_code = 0x08000020; // "Data cannot be transferred or stored to the application.
            return -1;
        }
        return sizeof(long);
    }
    else {
        int dstart;
        int dsize;
        int is_array;
        ul_dbuff_t *db;
        dstart = -1;
        dsize = get_object_data_size(o, abort_code);
        if(dsize == -1) {
            mylog(LOG_ERR, "vcaod_get_value(): object %04x:%02x size of data type 0x%x not known", ix, subix, o->data_type);
            return -1;
        }
        is_array = (o->object_type == vcaod_object_type_ARRAY);
        if(is_array) {
            if(subix < o->subcnt) {
                dstart = subix * dsize;
            }
            else {
                mylog(LOG_ERR, "vcaod_get_value(): object %04x:%02x array index [%i] out of bounds (%i)\n", ix, subix, subix, o->subcnt);
                if(abort_code) *abort_code = 0x6090011;
                return -1;
            }
        }
        else {
            dstart = 0;
        }
        db = &(o->value);
        if(dstart + dsize > db->len) {
            mylog(LOG_ERR, "vcaod_get_value(): object %04x:%02x dbuff len(%i) shorter than object size(%i)\n", ix, subix, db->len, dstart + dsize);
            mylog(LOG_ERR, "\tpossible reason: OD entry wasn't initialized, try to write to it first.\n");
            if(abort_code) *abort_code = 0x8000000;
            return -1;
        }
        memset(buff, 0, len);
        if(dsize <= len) memcpy(buff, db->data + dstart, dsize);
        return dsize;
    }
    return -1;
}
//-------------------------------------------------------------------
int vcaod_set_value(vcaod_root_t *odroot, unsigned ix, unsigned subix, const void *buff, int len, uint32_t *abort_code)
{
    vcaod_object_t *o = _vcaod_find_object(odroot, ix, subix, abort_code);
    mylog(LOG_DEB, "vcaod_set_value(): set object %04x:%02x value\n", ix, subix);
    if(!o) {
        mylog(LOG_MSG, "vcaod_set_value(): object %04x:%02x index not found in OD\n", ix, subix);
        return -1;
    }
    // check read only
    if(o->access < vcaod_access_RW) {
        mylog(LOG_MSG, "vcaod_set_value(): [%04x:%02x] attempt to write READ ONLY object\n", ix, subix);
        if(abort_code) *abort_code = 0x6010002;
        return -1;
    }

    if(o->dinfo) {
        // use dinfo if it does exists
        // TODO: support updating other objects than long via dinfo
        // sui_wr_long() fires object change if necessary
        if(sui_wr_long(o->dinfo, 0, buff) == SUDI_DATA_OK) {
            mylog(LOG_DEB, "vcaod_set_value(): object value set using dinfo\n");
        }
        else {
            mylog(LOG_DEB, "vcaod_set_value(): ERROR seting object value using dinfo\n");
            if(abort_code) *abort_code = 0x08000020; // "Data cannot be transferred or stored to the application.
            return -1;
        }
        return sizeof(long);
    }
    else {
        int dsize;
        int is_array;
        int sz;
        int dstart;
        ul_dbuff_t *db;
        
        dsize = get_object_data_size(o, abort_code);
        if(dsize < 0) return -1;
        is_array = (o->object_type == vcaod_object_type_ARRAY);
        sz = dsize;
        dstart = 0;
        if(is_array) {
            sz *= o->subcnt;
            if(subix < o->subcnt) {
                dstart = subix * dsize;
            }
            else {
                mylog(LOG_ERR, "vcaod_set_value(): object %04x:%02x array index [%i] out of bounds (%i)\n", ix, subix, subix, o->subcnt);
                if(abort_code) *abort_code = 0x6090011;
                return -1;
            }
        }
        db = &(o->value);
        ul_dbuff_set_len(db, sz);
        if(dstart + dsize > db->len) {
            mylog(LOG_ERR, "vcaod_set_value(): object %04x:%02x dbuff len (%i) exceded (%i)\n", ix, subix, db->len, dstart + dsize);
            if(abort_code) *abort_code = 0x8000000;
            return -1;
        }
        memset(db->data + dstart, 0, dsize);
        if(dsize > len) dsize = len; // stored data can be shorter than object size
        memcpy(db->data + dstart, buff, dsize);
        return dsize;
    }
    return -1;
}
//-------------------------------------------------------------------
sui_dinfo_t *vcaod_get_dinfo_ref(vcaod_object_t *obj, int create_weak)
{
    sui_dinfo_t *d = obj->dinfo;
    
    if(d){
        /* dinfo already exists, it is either HW type or weak */
        sui_dinfo_inc_refcnt(d);
	return d;
    }
    if(!create_weak) return NULL;

    d = sui_dinfo_dbuff_create(&obj->value, 0);
    if(d == NULL) return NULL;
    mylog(LOG_INF, "\tCreated WEAK dbuff dinfo (%p) for object %04x:%02x.\n", 
                     d, obj->index, obj->subindex);
    obj->dinfo = d;
    obj->flags |= VCAOD_OBJECT_FLAG_WEAK_DINFO;
    sui_dinfo_add_weakptr(obj->dinfo, &(obj->dinfo));
    return d;
}   
//-------------------------------------------------------------------
void vcaod_connect_hw_dinfos_to_OD(vcaod_root_t *odroot)
{
    int i;
    mylog(LOG_INF, "--- connecting HW dinfos to OD objects ---\n");
    for(i=0; i<vcaHwModule_getDinfoCnt(); i++) {
        vcaod_object_t *o;
        int ix, six;
        uint32_t err_code;
        uint32_t u = vcaHwModule_getDinfoObjectIndex(i);
        ix = u / 0x100; six = u % 0x100;
        o = _vcaod_find_object(odroot, ix, six, &err_code);
        if(!o) {
            mylog(LOG_ERR, "vcaod_connect_hw_dinfos_to_OD(): [ix=%i] cann't find object %04x:%02x in OD\n", i, ix, six);
            mylog(LOG_ERR, "\t%s\n", vcasdo_abort_msg(err_code));
            continue;
        }
        if(o->dinfo) {
            mylog(LOG_ERR, "vcaod_connect_hw_dinfos_to_OD(): object %04x:%02x already has dinfo asigned\n", ix, six);
            continue;
        }
        o->dinfo = vcaHwModule_getDinfoRef(i);
        o->flags &= ~VCAOD_OBJECT_FLAG_WEAK_DINFO;        
        mylog(LOG_DEB, "vcaod_connect_hw_dinfos_to_OD(): added HW dinfo (%p) to object %04x:%02x\n", o->dinfo, ix, six);
    }
}   
//-------------------------------------------------------------------