425 lines
9.8 KiB
C
425 lines
9.8 KiB
C
|
|
#include <errno.h>
|
|
|
|
#include "mavl.h"
|
|
|
|
#include "dbg.h"
|
|
#include "log.h"
|
|
|
|
#include "msgset.h"
|
|
#include "val.h"
|
|
#include "mavltypes.h"
|
|
|
|
static int cmp_cw_elemhandler_by_id(const void *elem1, const void *elem2)
|
|
{
|
|
const struct cw_ElemHandler *e1 = elem1;
|
|
const struct cw_ElemHandler *e2 = elem2;
|
|
int r;
|
|
r = e1->id - e2->id;
|
|
if (r != 0)
|
|
return r;
|
|
r = e1->vendor - e2->vendor;
|
|
if (r != 0)
|
|
return r;
|
|
r = e1->proto - e2->proto;
|
|
if (r != 0)
|
|
return r;
|
|
return 0;
|
|
}
|
|
|
|
static int cmp_cw_elemhandler_by_key(const void *elem1, const void *elem2)
|
|
{
|
|
const struct cw_ElemHandler *e1 = elem1;
|
|
const struct cw_ElemHandler *e2 = elem2;
|
|
return strcmp(e1->key, e2->key);
|
|
}
|
|
|
|
static int cmp_msgdata(const void *elem1, const void *elem2)
|
|
{
|
|
const struct cw_MsgData *e1 = elem1;
|
|
const struct cw_MsgData *e2 = elem2;
|
|
return e1->type - e2->type;
|
|
}
|
|
|
|
static int cmp_elemdata(const void *elem1, const void *elem2)
|
|
{
|
|
const struct cw_ElemData *e1 = elem1;
|
|
const struct cw_ElemData *e2 = elem2;
|
|
int r;
|
|
r = e1->id - e2->id;
|
|
if (r != 0)
|
|
return r;
|
|
r = e1->vendor - e2->vendor;
|
|
if (r != 0)
|
|
return r;
|
|
r = e1->proto - e2->proto;
|
|
if (r != 0)
|
|
return r;
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
static int cmp_machinestate(const void *state1, const void *state2)
|
|
{
|
|
const struct cw_StateMachineState *s1 = state1;
|
|
const struct cw_StateMachineState *s2 = state2;
|
|
int rc;
|
|
rc = s1->state - s2->state;
|
|
if (rc!=0)
|
|
return rc;
|
|
return s1->prevstate-s2->prevstate;
|
|
}
|
|
|
|
|
|
static void del_mavl_msgdata(void * d)
|
|
{
|
|
struct cw_MsgData *data = (struct cw_MsgData *) d;
|
|
if (data->elements_list)
|
|
mlist_destroy(data->elements_list);
|
|
|
|
if (data->elements_tree)
|
|
mavl_destroy(data->elements_tree);
|
|
|
|
if (data->mand_keys)
|
|
mlist_destroy(data->mand_keys);
|
|
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Destroy a message set
|
|
* @param set Message set to destroy
|
|
*/
|
|
void cw_msgset_destroy(struct cw_MsgSet *set)
|
|
{
|
|
if (set->msgdata)
|
|
mavl_destroy(set->msgdata);
|
|
if (set->handlers_by_id)
|
|
mavl_destroy(set->handlers_by_id);
|
|
if (set->handlers_by_key)
|
|
mavl_destroy(set->handlers_by_key);
|
|
if (set->statemachine_states)
|
|
mavl_destroy(set->statemachine_states);
|
|
|
|
/* if (set->types_tree)
|
|
mavl_destroy(set->types_tree);*/
|
|
|
|
free(set);
|
|
}
|
|
|
|
/**
|
|
* @brief Create a message set
|
|
* @return Message set create, NULL if an error has occured
|
|
*/
|
|
struct cw_MsgSet *cw_msgset_create()
|
|
{
|
|
//const cw_Type_t **ti;
|
|
|
|
/* allocate memory for a message_set */
|
|
struct cw_MsgSet *set = malloc(sizeof(struct cw_MsgSet));
|
|
if (set == NULL)
|
|
return NULL;
|
|
|
|
memset(set, 0, sizeof(struct cw_MsgSet));
|
|
|
|
|
|
|
|
/* create mavl for all_elems by id */
|
|
set->handlers_by_id = mavl_create(cmp_cw_elemhandler_by_id, NULL,
|
|
sizeof(struct cw_ElemHandler));
|
|
if (set->handlers_by_id == NULL) {
|
|
cw_msgset_destroy(set);
|
|
return NULL;
|
|
}
|
|
|
|
/* create mavl for all_elems by key */
|
|
set->handlers_by_key = mavl_create(cmp_cw_elemhandler_by_key, NULL,
|
|
sizeof(struct cw_ElemHandler));
|
|
if (set->handlers_by_key == NULL) {
|
|
cw_msgset_destroy(set);
|
|
return NULL;
|
|
}
|
|
|
|
/* create mavl for messages */
|
|
set->msgdata = mavl_create(cmp_msgdata, del_mavl_msgdata,
|
|
sizeof(struct cw_MsgData));
|
|
if (set->msgdata == NULL) {
|
|
cw_msgset_destroy(set);
|
|
return NULL;
|
|
}
|
|
|
|
/* set->types_tree = cw_ktv_create_types_tree();
|
|
if (set->types_tree == NULL){
|
|
cw_msgset_destroy(set);
|
|
return NULL;
|
|
}
|
|
/ * insert default types * /
|
|
for (ti=CW_KTV_STD_TYPES;*ti;ti++){
|
|
mavl_insert_ptr(set->types_tree,*ti);
|
|
}
|
|
*/
|
|
|
|
|
|
set->statemachine_states = mavl_create(cmp_machinestate,NULL,sizeof(cw_StateMachineState_t));
|
|
if (set->statemachine_states == NULL)
|
|
{
|
|
cw_msgset_destroy(set);
|
|
return NULL;
|
|
}
|
|
|
|
return set;
|
|
}
|
|
|
|
struct cw_ElemHandler *cw_msgset_get_elemhandler(struct cw_MsgSet *set,
|
|
int proto, int vendor, int id)
|
|
{
|
|
struct cw_ElemHandler search;
|
|
search.proto = proto;
|
|
search.vendor = vendor;
|
|
search.id = id;
|
|
return mavl_get(set->handlers_by_id, &search);
|
|
}
|
|
|
|
|
|
static int update_msgdata(struct cw_MsgSet *set, struct cw_MsgData *msgdata,
|
|
struct cw_MsgDef *msgdef)
|
|
{
|
|
struct cw_ElemDef *elemdef;
|
|
struct cw_ElemData ed, *result;
|
|
mavliter_t it;
|
|
|
|
/* iterate through all defined elements */
|
|
for (elemdef = msgdef->elements; elemdef->id; elemdef++) {
|
|
struct cw_ElemHandler *handler;
|
|
int replaced;
|
|
|
|
handler = cw_msgset_get_elemhandler(set,
|
|
elemdef->proto,
|
|
elemdef->vendor, elemdef->id);
|
|
/* check if a handler for our element already exists */
|
|
if (!handler) {
|
|
cw_log(LOG_ERR, "Creating message set: No handler for message element: %d %d %d [%s]",
|
|
elemdef->proto, elemdef->vendor, elemdef->id, cw_strelem(elemdef->id));
|
|
continue;
|
|
}
|
|
|
|
/* if (handler->type != NULL){
|
|
if (mavl_insert_ptr( set->types_tree, handler->type ) == NULL){
|
|
cw_log(LOG_ERR, "Can't add type from handler: %s", strerror(errno));
|
|
continue;
|
|
}
|
|
}
|
|
*/
|
|
|
|
ed.id = elemdef->id;
|
|
ed.proto = elemdef->proto;
|
|
ed.vendor = elemdef->vendor;
|
|
ed.mand = elemdef->mand;
|
|
|
|
/* add/delete/replace message elemeent to/from/in the elements list */
|
|
switch ( elemdef->op & 0xff){
|
|
case CW_IGNORE:
|
|
break;
|
|
continue;
|
|
case CW_DELETE:
|
|
cw_dbg(DBG_MOD_DETAIL, " deleting message element %d %d %d - %s",
|
|
elemdef->proto,
|
|
elemdef->vendor, elemdef->id, handler->name);
|
|
|
|
mlist_delete(msgdata->elements_list, &ed);
|
|
mavl_del(msgdata->elements_tree,&ed);
|
|
continue;
|
|
break;
|
|
case CW_APPEND:
|
|
mlist_append(msgdata->elements_list, &ed);
|
|
break;
|
|
default:
|
|
case CW_REPLACE:
|
|
if (mlist_replace(msgdata->elements_list, &ed)==NULL){
|
|
mlist_append(msgdata->elements_list, &ed);
|
|
}
|
|
break;
|
|
}
|
|
|
|
|
|
|
|
/* add message element to the elements tree */
|
|
result = mavl_replace(msgdata->elements_tree, &ed, &replaced);
|
|
|
|
if (!replaced) {
|
|
cw_dbg(DBG_MOD_DETAIL, " adding message element %d %d %d - %s",
|
|
elemdef->proto,
|
|
elemdef->vendor, elemdef->id, handler->name);
|
|
} else {
|
|
cw_dbg(DBG_MOD_DETAIL, " replacing message element %d %d %d - %s",
|
|
elemdef->proto,
|
|
elemdef->vendor, elemdef->id, handler->name);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
if (msgdata->mand_keys!=NULL){
|
|
mlist_destroy(msgdata->mand_keys);
|
|
}
|
|
msgdata->mand_keys = mlist_create_conststr();
|
|
|
|
mavliter_init(&it,msgdata->elements_tree);
|
|
mavliter_foreach(&it){
|
|
struct cw_ElemHandler *handler;
|
|
result = mavliter_get(&it);
|
|
|
|
handler = cw_msgset_get_elemhandler(set,
|
|
result->proto,
|
|
result->vendor, result->id);
|
|
if (result->mand){
|
|
mlist_append_ptr(msgdata->mand_keys,(void*)handler->key);
|
|
cw_dbg(DBG_MOD_DETAIL," Add mandatory key: %s",handler->key);
|
|
}
|
|
/*//printf("Have Result %d %d - %s\n",result->id,result->mand, handler->key);*/
|
|
}
|
|
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
int cw_msgset_add(struct cw_MsgSet *set,
|
|
struct cw_MsgDef messages[], struct cw_ElemHandler handlers[]
|
|
)
|
|
{
|
|
|
|
struct cw_ElemHandler *handler;
|
|
struct cw_MsgDef *msgdef;
|
|
/* Create mavl for all handlers */
|
|
for (handler = handlers; handler->id; handler++) {
|
|
cw_dbg(DBG_MOD_DETAIL, "Adding handler for element %d - %s - with key: %s",
|
|
handler->id, handler->name, handler->key);
|
|
mavl_replace(set->handlers_by_id, handler, NULL);
|
|
mavl_replace(set->handlers_by_key, handler, NULL);
|
|
}
|
|
|
|
for (msgdef = messages; msgdef->type != 0; msgdef++) {
|
|
struct cw_MsgData search;
|
|
struct cw_MsgData *msg;
|
|
int exists;
|
|
/* add the message */
|
|
memset(&search,0,sizeof(struct cw_MsgData));
|
|
search.type = msgdef->type;
|
|
|
|
msg = mavl_insert(set->msgdata, &search, &exists);
|
|
if (msg == NULL) {
|
|
cw_log(LOG_ERR, "Can't create messae");
|
|
return 0;
|
|
}
|
|
|
|
/* Look if message already exists */
|
|
if (!exists ) {
|
|
/* message is fresh createt, initialize data */
|
|
msg->elements_tree = mavl_create(cmp_elemdata, NULL,
|
|
sizeof(struct cw_ElemData));
|
|
msg->mand_keys=NULL;
|
|
msg->elements_list = mlist_create(cmp_elemdata,NULL,sizeof(struct cw_ElemData));
|
|
msg->postprocess=NULL;
|
|
msg->preprocess=NULL;
|
|
}
|
|
/* Overwrite the found message */
|
|
if (msgdef->name != NULL)
|
|
msg->name = msgdef->name;
|
|
if (msgdef->states != NULL)
|
|
msg->states = msgdef->states;
|
|
if (msgdef->postprocess != NULL)
|
|
msg->postprocess = msgdef->postprocess;
|
|
if (msgdef->preprocess != NULL)
|
|
msg->preprocess = msgdef->preprocess;
|
|
/* if (msgdef->next_state)
|
|
msg->next_state=msgdef->next_state;
|
|
*/
|
|
msg->receiver = msgdef->receiver;
|
|
|
|
|
|
cw_dbg(DBG_MOD_DETAIL, "Add message Type:%d - %s ", msgdef->type, msgdef->name);
|
|
|
|
|
|
update_msgdata(set, msg, msgdef);
|
|
}
|
|
|
|
/* {
|
|
mavliter_t it;
|
|
cw_dbg(DBG_MOD," Known types:");
|
|
mavliter_init(&it,set->types_tree);
|
|
mavliter_foreach(&it){
|
|
struct cw_Type * t = mavliter_get_ptr(&it);
|
|
cw_dbg(DBG_MOD, " Type: %s", t->name);
|
|
}
|
|
}
|
|
*/
|
|
|
|
|
|
return 0;
|
|
}
|
|
|
|
int cw_msgset_add_states(struct cw_MsgSet * set, cw_StateMachineState_t * states)
|
|
{
|
|
cw_StateMachineState_t * s;
|
|
int replaced;
|
|
|
|
s=states;
|
|
while (s->state != 0){
|
|
const char * repstr;
|
|
mavl_replace(set->statemachine_states,s,&replaced);
|
|
|
|
if (replaced){
|
|
repstr = "Replacing";
|
|
}
|
|
else{
|
|
repstr = "Adding";
|
|
}
|
|
cw_dbg(DBG_MOD_DETAIL,"%s machine state : [%s->%s]",repstr,
|
|
cw_strstate(s->prevstate),
|
|
cw_strstate(s->state));
|
|
s++;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
* @brief Find message data to a specific message
|
|
* @param set message set
|
|
* @param type message type to search for
|
|
* @return message data or NULL if not found
|
|
*/
|
|
struct cw_MsgData *cw_msgset_get_msgdata(struct cw_MsgSet *set, int type)
|
|
{
|
|
|
|
struct cw_MsgData search, *result;
|
|
search.type = type;
|
|
result = mavl_get(set->msgdata, &search);
|
|
if (result != NULL)
|
|
return result;
|
|
|
|
return NULL;
|
|
}
|
|
|
|
typedef int (*cw_MsgCallbackFun)(struct cw_ElemHandlerParams * params, uint8_t * elems_ptr, int elems_len);
|
|
cw_MsgCallbackFun cw_msgset_set_postprocess(struct cw_MsgSet * set,int msg_id,
|
|
cw_MsgCallbackFun fun)
|
|
|
|
{
|
|
struct cw_MsgData * md;
|
|
cw_MsgCallbackFun old_callback;
|
|
md = cw_msgset_get_msgdata(set,msg_id);
|
|
if (md != NULL){
|
|
old_callback = md->postprocess;
|
|
md->postprocess=fun;
|
|
}
|
|
return old_callback;
|
|
}
|
|
|
|
|
|
|