mjw tlv

#include <stdio.h>
#include <stdlib.h>

typedef struct {
    char *buf;
    int offset;
       unsigned short type;
    unsigned  short len;
}ControlMessag;
int __expandMessage(ControlMessag *msg,int data_len);
#define TYPE1 1
#define TYPE2 2
#define UNKNOWN_TYPE 3
#define MessageMalloc(size) malloc(size)
#define __DEBUG__
#ifdef __DEBUG__
#define DEBUG(format,...) printf("Fun: %s, Line: %d: "format"\n",__FUNCTION__, __LINE__, ##__VA_ARGS__)
#else
#define DEBUG(format,...)
#endif

void MessageFree(ControlMessag *msg){
    if (msg->buf)
        free(msg->buf);
    return;
}
#define expandMessage(msg,len)    {if( !__expandMessage(msg,len)) return 0;}
int __expandMessage(ControlMessag *msg, int data_len){
    DEBUG();
    char *p = NULL;
    int size = msg->offset + sizeof(msg->type) + sizeof(msg->len) + data_len;
    p = (char *)MessageMalloc(size);
    if (!p){
        printf("FUNC=%s err \n",__FUNCTION__);
        return 0;
    }
    memset(p, 0, size);
    memcpy(p, msg->buf, msg->offset);
    MessageFree(msg);
    msg->buf = p;
    return 1;
}

void msgStroeType(ControlMessag *msg, int val){
    memcpy(&(msg->buf[msg->offset]), (char *)&val, sizeof(msg->type));
    (msg->offset) += sizeof(msg->type);
}
void msgStroeLen(ControlMessag *msg, int val){
    memcpy(&(msg->buf[msg->offset]), (char *)&val, sizeof(msg->len));
    (msg->offset) += sizeof(msg->len);
}
int msgStoreData(ControlMessag *msg, void *data, int data_len){
    if (!data)
        return 0;
    memcpy(&(msg->buf[msg->offset]), (char *)data, data_len);
    (msg->offset) += data_len;
    return 1;
}
int addElemMsg(ControlMessag *msg, int type, int len, void *data){
    DEBUG();
    if (len < 0 || (!data && len))
        return 0;
    expandMessage(msg, len);
    msgStroeType(msg, type);
    msgStroeLen(msg, len);
    msgStoreData(msg, data, len);
    return 1;
}
void parseMessage(ControlMessag *msg){
    memcpy(&msg->type, &(msg->buf[msg->offset]), sizeof(msg->type));
    (msg->offset) += sizeof(msg->type);

    memcpy(&msg->len, &(msg->buf[msg->offset]), sizeof(msg->len));
    (msg->offset) += sizeof(msg->len);
    return;
}
void showMsgData(ControlMessag *msg){
    printf("this elem type=%d,len=%d,val=%.*s\n", msg->type, msg->len, msg->len, &msg->buf[msg->offset]);
}
int handleMessage(char *recvbuf, int recvbuf_size){
    DEBUG("recvbuf_size =%d",recvbuf_size);
    if (!recvbuf || !recvbuf_size)
        return 0;
    ControlMessag msg;
    msg.buf = recvbuf;
    msg.offset = 0;
    int size = recvbuf_size;
    while((size -= sizeof(msg.type)+sizeof(msg.len)) >= 0){
        parseMessage(&msg);
        if ((size -= msg.len) < 0){
            printf(" size %d, this elem type=%d,len=%d, offset =%d \n", size , msg.type, msg.len, msg.offset);
            return 0;
        }
        switch (msg.type){
            case TYPE1:
                //TODO:
                DEBUG("   type %d \n",msg.type);
                showMsgData(&msg);
                break;
            case TYPE2:
                //TODO:
                DEBUG(" type %d \n",msg.type);
                showMsgData(&msg);
                break;
            default:
                DEBUG(" unrecognise this type %d \n",msg.type);
        }
        msg.offset += msg.len;//jump data
    }
    return 1;
}
int main(){
    ControlMessag msg;
    memset(&msg, 0, sizeof(msg));
    DEBUG();
    addElemMsg(&msg, TYPE1, 2, "abc");
    addElemMsg(&msg, TYPE2, 3, "abc");
    addElemMsg(&msg, UNKNOWN_TYPE, 3, "abc");
    handleMessage(msg.buf,msg.offset);
    return 1;
}