CANOpen服务数据对象报文

https://blog.csdn.net/lushoumin/article/details/79668443

 SDO是服务数据对象接口(ServiceData Object)的缩写，顾名思义提供服务数据的访问接口，所谓服务数据指一些实时性要求不高的数据，一般是指节点配置参数，因此，SDO一般用来配置和获得节点的配置参数，充当OD对外的接口。

SDO基于CS模式，所有报文都需要确认。通常从节点作为SDO服务器，主节点作为客户端。客户端通过索引和子索引，访问服务器上的任意对象字典，SDO的上传与下载，是从server的角度去理解的，上传：client对server的OD进行读操作；下载：client对server的OD进行写操作。

传送机制有两种：域传送和块传送。

SDO报文格式如下：

SDO域传输一共实现了5个请求/应答协议：启动域下载，启动域上传，域分段下载，域分段上传，域传送中止。

SDO命令字包含如下信息：该报文是上传还是下载，该报文是请求还是应答，该报文是分段还是加速，CAN帧数据字节长度，后续分段的触发位。

块传输这里不进行详细讨论，只给出块下载流程：
 1.客户端：初始化传输通道
           发送块下载初始化指令，包括索引、子索引、字节数

 2.服务器：初始化传输通道
           发送块下载初始化响应，包括索引、子索引、一次传输块数量

 3.客户端：复位传输超时定时器
           发送多个数据包块下载，包括是否最后一块、序列号、数据

 4.服务器：复位传输超时定时器
           将数据拷贝到传输通道
           发送数据包块下载响应，包括序列号、一次传输块数量

 5.客户端：如果没有下载完，重复步骤3、4
           如果下载完，发送块下载结束指令，包括最后一块补零数

 6.服务器：发送块下载结束响应
           将数据拷贝到字典中
           复位传输通道

 7.客户端：停止传输超时定时器
           设置传输通道为完成状态
           调用传输完成回调函数

#include
#include "canfestival.h"
#include "sysdep.h"

#define NO_INLINE

#ifdef NO_INLINE
#define INLINE
#else
#define INLINE inline
#endif

/* 通过节点号查找客户端号 */
UNS8 GetSDOClientFromNodeId(CO_Data *d, UNS8 nodeId);

/* 写服务器字典 */
INLINE UNS8 _writeNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex,
                                                            UNS32 count, UNS8 dataType, void *data,
                                                            SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode);

/* 读服务器字典 */
INLINE UNS8 _readNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode);

#define getSDOcs(byte) (byte >> 5)
#define getSDOn2(byte) ((byte >> 2) & 3)
#define getSDOn3(byte) ((byte >> 1) & 7)
#define getSDOe(byte) ((byte >> 1) & 1)
#define getSDOs(byte) (byte & 1)
#define getSDOc(byte) (byte & 1)
#define getSDOt(byte) ((byte >> 4) & 1)
#define getSDOindex(byte1, byte2) (((UNS16)byte2 << 8) | ((UNS16)byte1))
#define getSDOsubIndex(byte3) (byte3)
#define getSDOblockSC(byte) (byte & 3)


/* sdo传输超时回调函数 */
void SDOTimeoutAlarm(CO_Data *d, UNS32 id)
{
    UNS16 offset;
    UNS8 nodeId;

    /* 第一个sdo客户端在字典中的下标 */
    offset = d->firstIndex->SDO_CLT;
    if((offset == 0) || ((offset + d->transfers[id].CliServNbr) > d->lastIndex->SDO_CLT))
    {
        return;
    }

    /* 从字典中取出服务器节点id */
    nodeId = (UNS8)*((UNS32*)d->objdict[offset + d->transfers[id].CliServNbr].pSubindex[3].pObject);

    MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
    MSG_WAR(0x2A02, "server node id : ", nodeId);
    MSG_WAR(0x2A02, "         index : ", d->transfers[id].index);
    MSG_WAR(0x2A02, "      subIndex : ", d->transfers[id].subIndex);

    /* 传输通道超时定时器置空 */
    d->transfers[id].timer = TIMER_NONE;
    /* 传输通道状态设置为内部中止 */
    d->transfers[id].state = SDO_ABORTED_INTERNAL;
    /* 向主站发送中止报文 */
    sendSDOabort(d, d->transfers[id].whoami, d->transfers[id].CliServNbr, d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
    /* 错误码设置为超时 */
    d->transfers[id].abortCode = SDOABT_TIMED_OUT;

    /* 如果传输通道超时，则调用回调函数 */
    if(d->transfers[id].Callback)
        (*d->transfers[id].Callback)(d, nodeId);

    /* 复位sdo传输通道 */
    if(d->transfers[id].abortCode == SDOABT_TIMED_OUT)
        resetSDOline(d, (UNS8)id);
}

/* 停止sdo超时定时器 */
#define StopSDO_TIMER(id) \
    MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\
d->transfers[id].timer = DelAlarm(d->transfers[id].timer);

/* 启动sdo超时定时器 */
#define StartSDO_TIMER(id) \
    MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\
    d->transfers[id].timer = SetAlarm(d, id, &SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0);

/* 重启sdo超时定时器 */
#define RestartSDO_TIMER(id) \
    MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\
if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) }

/* 复位所有sdo传输通道 */
void resetSDO(CO_Data *d)
{
    UNS8 j;

    /* 复位所有sdo传输通道 */
    for(j = 0; j < SDO_MAX_SIMULTANEOUS_TRANSFERS; j++)
    {
        resetSDOline(d, j);
    }
}

/* 将数据从传输通道缓冲区拷贝到字典中 */
UNS32 SDOlineToObjdict(CO_Data *d, UNS8 line)
{
    UNS32 size;
    UNS32 errorCode;

    MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);

    /* 确定该传输通道传输了多少字节 */
    if(d->transfers[line].count == 0)
    {
        d->transfers[line].count = d->transfers[line].offset;
    }
    size = d->transfers[line].count;

    /* 将数据拷贝到字典中 */
#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
    if(size > SDO_MAX_LENGTH_TRANSFER)
    {
        errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
                                                     (void *)d->transfers[line].dynamicData, &size, 1);
    }
    else
    {
        errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
                                                     (void *)d->transfers[line].data, &size, 1);
    }
#else
    errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
                                                 (void *)d->transfers[line].data, &size, 1);
#endif
    if(errorCode != OD_SUCCESSFUL)
        return errorCode;

    MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);

    return 0;

}

/* 将数据从字典中拷贝到传输通道缓冲区 */
UNS32 objdictToSDOline(CO_Data *d, UNS8 line)
{
    UNS32 size = SDO_MAX_LENGTH_TRANSFER;
    UNS8 dataType;
    UNS32 errorCode;

    MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index);
    MSG_WAR(0x3A06, "  subIndex : ", d->transfers[line].subIndex);

    /* 支持动态内存分配 */
#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
    /* 从字典中将数据拷贝出来 */
    errorCode = getODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
                                                 (void *)d->transfers[line].data, &size, &dataType, 1);
    /* 如果内存不够拷贝 */
    if(errorCode == SDOABT_OUT_OF_MEMORY)
    {
        if(size <= SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE)
        {
            /* 动态分配内存 */
            d->transfers[line].dynamicData = (UNS8 *)malloc(size * sizeof(UNS8));
            if(d->transfers[line].dynamicData != NULL)
            {
                d->transfers[line].dynamicDataSize = size;
                /* 从字典中将数据拷贝出来 */
                errorCode = getODentry(d, d->transfers[line].index, d->transfers[line].subIndex, (void *)d->transfers[line].dynamicData,
                                                             &d->transfers[line].dynamicDataSize, &dataType, 1);
            }
        }
    }
#else
    /* 从字典中将数据拷贝出来 */
    errorCode = getODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
                                                 (void *)d->transfers[line].data, &size, &dataType, 1);
#endif
    if(errorCode != OD_SUCCESSFUL)
        return errorCode;

    d->transfers[line].count = size;
    d->transfers[line].offset = 0;

    return 0;
}

/* 将数据从传输通道缓冲区拷贝出来 */
UNS8 lineToSDO(CO_Data *d, UNS8 line, UNS32 nbBytes, UNS8 *data)
{
    UNS8 i;
    UNS32 offset;

#ifndef SDO_DYNAMIC_BUFFER_ALLOCATION
    /* 如果不支持内存动态分配，并且字节数大于静态缓冲区，则报错 */
    if((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFER)
    {
        MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFER", nbBytes);
        return 0xFF;
    }
#endif

    /* 如果传输通道字节数大于配置的大小 */
    if((d->transfers[line].offset + nbBytes) > d->transfers[line].count)
    {
        MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);
        return 0xFF;
    }
    offset = d->transfers[line].offset;
#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
    /* 如果说sdo传输通道字节数小于静态缓冲区 */
    if(d->transfers[line].count <= SDO_MAX_LENGTH_TRANSFER)
    {
        /* 将数据从静态缓冲区拷贝出来 */
        for(i = 0; i < nbBytes; i++)
            *(data + i) = d->transfers[line].data[offset + i];
    }
    /* 如果说sdo传输通道字节数大于静态缓冲区 */
    else
    {
        if(d->transfers[line].dynamicData == NULL)
        {
            MSG_ERR(0x1A11,"SDO's dynamic buffer not allocated. Line", line);
            return 0xFF;
        }

        /* 将数据从动态缓冲区拷贝出来 */
        for(i = 0; i < nbBytes; i++)
            *(data + i) = d->transfers[line].dynamicData[offset + i];
    }
#else
    /* 将数据拷贝出来 */
    for(i = 0; i < nbBytes; i++)
        *(data + i) = d->transfers[line].data[offset + i];
#endif
    /* 偏移量增大 */
    d->transfers[line].offset = d->transfers[line].offset + nbBytes;

    return 0;
}

/* 将数据拷贝到传输通道缓冲区 */
UNS8 SDOtoLine(CO_Data *d, UNS8 line, UNS32 nbBytes, UNS8 *data)
{
    UNS8 i;
    UNS32 offset;

#ifndef SDO_DYNAMIC_BUFFER_ALLOCATION
    /* 不允许动态分配时数据量不能大于缓冲区 */
    if((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFER)
    {
        MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFER", nbBytes);
        return 0xFF;
    }
#endif

    /* 缓冲区偏移量 */
    offset = d->transfers[line].offset;

#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
    {
        UNS8 *lineData = d->transfers[line].data;

        /* 当数据量大于缓冲区时 */
        if((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFER)
        {
            /* 如果还没有动态分配内存 */
            if(d->transfers[line].dynamicData == NULL)
            {
                /* 动态分配内存 */
                d->transfers[line].dynamicData = (UNS8 *)malloc(SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);
                d->transfers[line].dynamicDataSize = SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE;
                if(d->transfers[line].dynamicData == NULL)
                {
                    MSG_ERR(0x1A15,"SDO allocating dynamic buffer failed, size", SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);
                    return 0xFF;
                }

                /* 将静态缓冲区数据拷贝到动态内存区 */
                memcpy(d->transfers[line].dynamicData, d->transfers[line].data, offset);
            }
            /* 如果已经动态分配内存，则要重新分配 */
            else if((d->transfers[line].offset + nbBytes) > d->transfers[line].dynamicDataSize)
            {
                UNS8 *newDynamicBuffer = (UNS8*)realloc(d->transfers[line].dynamicData, d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);
                if(newDynamicBuffer == NULL)
                {
                    MSG_ERR(0x1A15,"SDO reallocating dynamic buffer failed, size", d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);
                    return 0xFF;
                }
                d->transfers[line].dynamicData = newDynamicBuffer;
                d->transfers[line].dynamicDataSize += SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE;
            }
            lineData = d->transfers[line].dynamicData;
        }

        /* 将数据拷贝到缓冲区 */
        for(i = 0; i < nbBytes; i++)
        {
            lineData[offset + i] = *(data + i);

        }
    }
#else
    /* 将数据拷贝到缓冲区 */
    for (i = 0; i < nbBytes; i++)
        d->transfers[line].data[offset + i] = *(data + i);
#endif

    /* 缓冲区偏移量增加 */
    d->transfers[line].offset = d->transfers[line].offset + nbBytes;

    return 0;
}

/* sdo传输通道失败 */
UNS8 failedSDO(CO_Data *d, UNS8 CliServNbr, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode)
{
    UNS8 err;
    UNS8 line;

    /* 通过客户端/服务器号获取sdo传输通道号 */
    err = getSDOlineOnUse(d, CliServNbr, whoami, &line);
    if(!err)
    {
        MSG_WAR(0x3A20, "FailedSDO : line found : ", line);
    }

    /* 如果自身是服务器，直接复位传输通道 */
    if((!err) && (whoami == SDO_SERVER))
    {
        resetSDOline(d, line);
        MSG_WAR(0x3A21, "FailedSDO : line released : ", line);
    }
    /* 如果自身是客户端，则停止sdo传输超时定时器并将状态置为中止并设置错误码 */
    if((!err) && (whoami == SDO_CLIENT))
    {
        StopSDO_TIMER(line);
        d->transfers[line].state = SDO_ABORTED_INTERNAL;
        d->transfers[line].abortCode = abortCode;
    }

    MSG_WAR(0x3A22, "Sending SDO abort ", 0);

    /* 发送sdo中止报文 */
    err = sendSDOabort(d, whoami, CliServNbr, index, subIndex, abortCode);
    if(err)
    {
        MSG_WAR(0x3A23, "Unable to send the SDO abort", 0);
        return 0xFF;
    }

    return 0;
}

/* 复位sdo通道 */
void resetSDOline(CO_Data *d, UNS8 line)
{
    UNS32 i;

    MSG_WAR(0x3A25, "reset SDO line nb : ", line);

    /* 初始化sdo通道 */
    initSDOline(d, line, 0, 0, 0, SDO_RESET);

    /* 将sdo传输通道缓冲区清空 */
    for(i = 0; i < SDO_MAX_LENGTH_TRANSFER; i++)
        d->transfers[line].data[i] = 0;

    /* 传输通道归属(服务器/客户端)清空 */
    d->transfers[line].whoami = 0;

    /* 传输通道错误码清空 */
    d->transfers[line].abortCode = 0;
}

/* 初始化sdo传输通道 */
UNS8 initSDOline(CO_Data *d, UNS8 line, UNS8 CliServNbr, UNS16 index, UNS8 subIndex, UNS8 state)
{
    MSG_WAR(0x3A25, "init SDO line nb : ", line);

    /* 判断是否该SDO传输通道在域下载/域上传/块下载/块上传过程中 */
    if(state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS ||
         state == SDO_BLOCK_DOWNLOAD_IN_PROGRESS || state == SDO_BLOCK_UPLOAD_IN_PROGRESS)
    {
        /* 开启sdo定时器，通讯超时做相应处理 */
        StartSDO_TIMER(line)
    }
    /* 不在传输过程中，关闭定时器 */
    else
    {
        StopSDO_TIMER(line)
    }

    /* 初始化客户端/服务器号 */
    d->transfers[line].CliServNbr = CliServNbr;
    /* 初始化对象索引 */
    d->transfers[line].index = index;
    /* 初始化对象子索引 */
    d->transfers[line].subIndex = subIndex;
    /* 初始化传输通道状态 */
    d->transfers[line].state = state;
    d->transfers[line].toggle = 0;
    /* 初始化字节数 */
    d->transfers[line].count = 0;
    d->transfers[line].offset = 0;
    d->transfers[line].peerCRCsupport = 0;
    d->transfers[line].blksize = 0;
    d->transfers[line].ackseq = 0;
    d->transfers[line].objsize = 0;
    d->transfers[line].lastblockoffset = 0;
    d->transfers[line].seqno = 0;
    d->transfers[line].endfield = 0;
    /* 初始化块传输接收状态 */
    d->transfers[line].rxstep = RXSTEP_INIT;
    d->transfers[line].dataType = 0;
    /* 超时回调函数 */
    d->transfers[line].Callback = NULL;
#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
    free(d->transfers[line].dynamicData);
    d->transfers[line].dynamicData = 0;
    d->transfers[line].dynamicDataSize = 0;
#endif
    return 0;
}

/* 获取空闲的sdo传输通道 */
UNS8 getSDOfreeLine(CO_Data *d, UNS8 whoami, UNS8 *line)
{
    UNS8 i;

    /* 遍历所有传输通道，查找空闲的传输通道 */
    for(i = 0; i < SDO_MAX_SIMULTANEOUS_TRANSFERS; i++)
    {
        if(d->transfers[i].state == SDO_RESET)
        {
            *line = i;
            d->transfers[i].whoami = whoami;
            return 0;
        }
    }

    MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);

    return 0xFF;
}

/* 通过客户端/服务器号获取sdo传输通道号 */
UNS8 getSDOlineOnUse(CO_Data *d, UNS8 CliServNbr, UNS8 whoami, UNS8 *line)
{
    UNS8 i;

    /* 遍历所有sdo传输通道 */
    for(i = 0; i < SDO_MAX_SIMULTANEOUS_TRANSFERS; i++)
    {
        /* 如果该通道处于正常状态并且客户端/服务器号匹配，则说明找到了该通道 */
        if((d->transfers[i].state != SDO_RESET) &&
             (d->transfers[i].state != SDO_ABORTED_INTERNAL) &&
             (d->transfers[i].CliServNbr == CliServNbr) &&
             (d->transfers[i].whoami == whoami))
        {
            /* 获取该通道号 */
            if(line)
            {
                *line = i;
            }

            return 0;
        }
    }

    return 0xFF;
}

/* 通过客户端/服务器号获取sdo传输通道号 */
UNS8 getSDOlineToClose(CO_Data *d, UNS8 CliServNbr, UNS8 whoami, UNS8 *line)
{
    UNS8 i;

    for(i = 0; i < SDO_MAX_SIMULTANEOUS_TRANSFERS; i++)
    {
        if((d->transfers[i].state != SDO_RESET) &&
             (d->transfers[i].CliServNbr == CliServNbr) &&
             (d->transfers[i].whoami == whoami))
        {
            if(line)
                *line = i;

            return 0;
        }
    }

    return 0xFF;
}

/* 关闭sdo传输通道 */
UNS8 closeSDOtransfer(CO_Data *d, UNS8 nodeId, UNS8 whoami)
{
    UNS8 err;
    UNS8 line;
    UNS8 CliNbr;

    /* 通过节点号查找客户端号 */
    CliNbr = GetSDOClientFromNodeId(d, nodeId);
    if(CliNbr >= 0xFE)
        return SDO_ABORTED_INTERNAL;

    /* 通过客户端/服务器号获取sdo传输通道号 */
    err = getSDOlineToClose(d, CliNbr, whoami, &line);
    if(err)
    {
        MSG_WAR(0x2A30, "No SDO communication to close", 0);
        return 0xFF;
    }

    /* 复位sdo通道 */
    resetSDOline(d, line);

    return 0;
}

/* 获取传输通道剩余字节数 */
UNS8 getSDOlineRestBytes(CO_Data *d, UNS8 line, UNS32 *nbBytes)
{
    if(d->transfers[line].count == 0)
        *nbBytes = 0;
    else
        *nbBytes = d->transfers[line].count - d->transfers[line].offset;
    return 0;
}

/* sdo传输通道剩余字节数 */
UNS8 setSDOlineRestBytes(CO_Data *d, UNS8 line, UNS32 nbBytes)
{
#ifndef SDO_DYNAMIC_BUFFER_ALLOCATION
    if(nbBytes > SDO_MAX_LENGTH_TRANSFER)
    {
        MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFER", nbBytes);
        return 0xFF;
    }
#endif
    d->transfers[line].count = nbBytes;

    return 0;
}

/* 发送sdo报文 */
UNS8 sendSDO(CO_Data *d, UNS8 whoami, UNS8 CliServNbr, UNS8 *pData)
{
    UNS16 offset;
    UNS8 i;
    Message m;

    MSG_WAR(0x3A38, "sendSDO",0);

    /* 运行态或预运行态才可以使用sdo报文 */
    if(!((d->nodeState == Operational) || (d->nodeState == Pre_operational)))
    {
        MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState);
        return 0xFF;
    }

    /* 服务器端发送sdo报文 */
    if(whoami == SDO_SERVER)
    {
        /* 取出sdo服务器端的发送cob_id */
        offset = d->firstIndex->SDO_SVR;
        if((offset == 0) || ((offset + CliServNbr) > d->lastIndex->SDO_SVR))
        {
            MSG_ERR(0x1A42, "SendSDO : SDO server not found", 0);
            return 0xFF;
        }
        m.cob_id = (UNS16)*((UNS32*)d->objdict[offset + CliServNbr].pSubindex[2].pObject);
        MSG_WAR(0x3A41, "I am server Tx cobId : ", m.cob_id);
    }
    /* 客户端发送sdo报文 */
    else
    {
        /* 取出sdo客户端发送cob_id */
        offset = d->firstIndex->SDO_CLT;
        if((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_CLT))
        {
            MSG_ERR(0x1A42, "SendSDO : SDO client not found", 0);
            return 0xFF;
        }
        m.cob_id = (UNS16)*((UNS32*)d->objdict[offset + CliServNbr].pSubindex[1].pObject);
        MSG_WAR(0x3A41, "I am client Tx cobId : ", m.cob_id);
    }
    /* 数据帧 */
    m.rtr = NOT_A_REQUEST;
    /* sdo报文固定8字节 */
    m.len = 8;
    /* 数据 */
    for(i = 0; i < 8; i++)
    {
        m.data[i] =  pData[i];
    }

    /* 发送报文 */
    return canSend(d->canHandle,&m);
}

/* 发送sdo中止报文 */
UNS8 sendSDOabort(CO_Data* d, UNS8 whoami, UNS8 CliServNbr, UNS16 index, UNS8 subIndex, UNS32 abortCode)
{
    UNS8 data[8];
    UNS8 ret;

    MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);

    data[0] = 0x80;
    data[1] = index & 0xFF;
    data[2] = (index >> 8) & 0xFF;
    data[3] = subIndex;
    data[4] = (UNS8)(abortCode & 0xFF);
    data[5] = (UNS8)((abortCode >> 8) & 0xFF);
    data[6] = (UNS8)((abortCode >> 16) & 0xFF);
    data[7] = (UNS8)((abortCode >> 24) & 0xFF);

    /* 发送sdo报文 */
    ret = sendSDO(d, whoami, CliServNbr, data);

    return ret;
}

/* 处理sdo报文 */
UNS8 proceedSDO(CO_Data *d, Message *m)
{
    UNS8 err;
    UNS8 cs;
    UNS8 line;
    UNS32 nbBytes;
    UNS8 nodeId = 0;
    UNS8 CliServNbr;
    UNS8 whoami = SDO_UNKNOWN;
    UNS32 errorCode;
    UNS8 data[8];
    UNS16 index;
    UNS8 subIndex;
    UNS32 abortCode;
    UNS32 i;
    UNS8    j;
    UNS32 *pCobId = NULL;
    UNS16 offset;
    UNS16 lastIndex;
    UNS8 SubCommand;
  UNS8 SeqNo;
  UNS8 AckSeq;
    UNS8 NbBytesNoData;

    MSG_WAR(0x3A60, "proceedSDO ", 0);

    whoami = SDO_UNKNOWN;

    /* 第一个sdo服务器在字典中的下标 */
    offset = d->firstIndex->SDO_SVR;
    /* 最后一个sdo服务器在字典中的下标 */
    lastIndex = d->lastIndex->SDO_SVR;

    j = 0;

    /* 如果字典中配置了sdo服务器 */
    if(offset)
    {
        /* 遍历所有sdo服务器 */
        while(offset <= lastIndex)
        {
            /* 服务器端子索引不能低于1个 */
            if(d->objdict[offset].bSubCount <= 1)
            {
                MSG_ERR(0x1A61, "Subindex 1  not found at index ", 0x1200 + j);
                return 0xFF;
            }

            /* 取出服务器端的接收CobId */
            pCobId = (UNS32*)d->objdict[offset].pSubindex[1].pObject;
            /* 如果和接收到的报文匹配 */
            if(*pCobId == UNS16_LE(m->cob_id))
            {
                /* 说明自己是服务器 */
                whoami = SDO_SERVER;
                MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);
                /* 客户端/服务器号 */
                CliServNbr = j;
                break;
            }
            j++;
            offset++;
        }
    }

    /* 如果不是服务器，判断自己是不是客户端 */
    if(whoami == SDO_UNKNOWN)
    {
        /* 第一个客户端在字典中的下标 */
        offset = d->firstIndex->SDO_CLT;
        /* 最后一个客户端在字典中的下标 */
        lastIndex = d->lastIndex->SDO_CLT;

        j = 0;

        /* 如果配置了客户端 */
        if(offset)
        {
            /* 遍历所有sdo客户端 */
            while(offset <= lastIndex)
            {
                /* 子索引不可以低于3个 */
                if(d->objdict[offset].bSubCount <= 3)
                {
                    MSG_ERR(0x1A63, "Subindex 3  not found at index ", 0x1280 + j);
                    return 0xFF;
                }
                /* 取出客户端的接收CobId */
                pCobId = (UNS32*)d->objdict[offset].pSubindex[2].pObject;
                /* 如果和接收到的报文匹配 */
                if(*pCobId == UNS16_LE(m->cob_id))
                {
                    /* 自己为客户端 */
                    whoami = SDO_CLIENT;

                    MSG_WAR(0x3A64, "proceedSDO. I am client index : ", 0x1280 + j);

                    /* 服务器/客户端号 */
                    CliServNbr = j;

                    /* 服务器端的节点号 */
                    nodeId = *((UNS8*) d->objdict[offset].pSubindex[3].pObject);
                    break;
                }
                j++;
                offset++;
            }
        }
    }

    /* 如果没有配置服务器，也没有配置客户端，则不用处理该保文 */
    if(whoami == SDO_UNKNOWN)
    {
        return 0xFF;
    }

    /* SDO报文一定是8字节 */
    if((*m).len != 8)
    {
        MSG_ERR(0x1A67, "Error size SDO", 0);
        failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_GENERAL_ERROR);
        return 0xFF;
    }

    if(whoami == SDO_CLIENT)
    {
        MSG_WAR(0x3A68, "I am CLIENT number ", CliServNbr);
    }
    else
    {
        MSG_WAR(0x3A69, "I am SERVER number ", CliServNbr);
    }

    /* 通服务器/客户端号获取通道号 */
    err = getSDOlineOnUse(d, CliServNbr, whoami, &line);

    cs = 0xFF;
  if(!err)
    {
        /* 客户端->服务器：块下载过程中 服务器->客户端：上传过程中 */
        if(((whoami == SDO_SERVER) && (d->transfers[line].state == SDO_BLOCK_DOWNLOAD_IN_PROGRESS)) ||
             ((whoami == SDO_CLIENT) && (d->transfers[line].state == SDO_BLOCK_UPLOAD_IN_PROGRESS)))
        {
            if(m->data[0] == 0x80)
                cs = 4;
            else
                cs = 6;
        }
    }
    if(cs == 0xFF)
    {
        cs = getSDOcs(m->data[0]);
    }

    /* 判断指令 */
    switch(cs)
    {
        /* 客户端->服务器:域分段下载/服务器->客户端:域分段上传 */
        case 0:
            /* 客户端->服务器的域分段下载 */
            if(whoami == SDO_SERVER)
            {
                /* 验证传输通道是否在域分段下载中 */
                if(!err)
                {
                    err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
                }
                if(err)
                {
                    MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", CliServNbr);
                    failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
                    return 0xFF;
                }
                /* 重启传输通道超时定时器 */
                RestartSDO_TIMER(line)

                MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", CliServNbr);

                /* 取出索引号 */
                index = d->transfers[line].index;
                /* 取出子索引号 */
                subIndex = d->transfers[line].subIndex;

                /* 校验触发位是否同步 */
                if(d->transfers[line].toggle != getSDOt(m->data[0]))
                {
                    MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0]));
                    failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
                    return 0xFF;
                }

                /* 取出字节数 */
                nbBytes = 7 - getSDOn3(m->data[0]);
                /* 将数据从sdo数据包中拷贝到传输通道缓冲区 */
                err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
                if(err)
                {
                    failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                    return 0xFF;
                }

                /* 构建响应包 */
                data[0] = (1 << 5) | (d->transfers[line].toggle << 4);
                for(i = 1 ; i < 8 ; i++)
                {
                    data[i] = 0;
                }
                MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", CliServNbr);

                /* 发送响应包 */
                sendSDO(d, whoami, CliServNbr, data);

                /* 触发位取反 */
                d->transfers[line].toggle = !d->transfers[line].toggle & 1;
                /* 检查该段是否是最后一段 */
                if(getSDOc(m->data[0]))
                {
                    /* 将数据从sdo传输通道缓冲区拷贝到字典中 */
                    errorCode = SDOlineToObjdict(d, line);
                    if(errorCode)
                    {
                        MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0);
                        /* sdo传输失败 */
                        failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);
                        return 0xFF;
                    }
                    /* 复位sdo传输通道 */
                    resetSDOline(d, line);
                    MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", CliServNbr);
                }
            }
            /* 服务器->客户端:域分段上传 */
            else
            {
                /* 验证传输通道是否在域分段上传中 */
                if(!err)
                {
                    err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
                }
                if(err)
                {
                    MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);
                    failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);

                    return 0xFF;
                }
                /* 重启传输通道超时定时器 */
                RestartSDO_TIMER(line)

                /* 取出索引号 */
                index = d->transfers[line].index;
                /* 取出子索引号 */
                subIndex = d->transfers[line].subIndex;

                /* 校验触发位是否同步 */
                if(d->transfers[line].toggle != getSDOt(m->data[0]))
                {
                    MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId);
                    failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
                    return 0xFF;
                }
                /* 取出字节数 */
                nbBytes = 7 - getSDOn3(m->data[0]);
                /* 将数据从sdo数据包中拷贝到传输通道缓冲区 */
                err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
                if(err)
                {
                    failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                    return 0xFF;
                }
                /* 触发位取反 */
                d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
                /* 检查该段是否是最后一段 */
                if(getSDOc(m->data[0]))
                {
                    /* 停止超时定时器 */
                    StopSDO_TIMER(line)
                    /* 传输完成 */
                    d->transfers[line].state = SDO_FINISHED;
                    /* 传输完成回调函数 */
                    if(d->transfers[line].Callback)
                    {
                        (*d->transfers[line].Callback)(d,nodeId);
                    }

                    MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);
                }
                /* 如果不是最后一段，继续请求上传 */
                else
                {
                    data[0] = (3 << 5) | (d->transfers[line].toggle << 4);
                    for(i = 1; i < 8; i++)
                        data[i] = 0;
                    sendSDO(d, whoami, CliServNbr, data);
                    MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId);
                }
            }
            break;

        /* 客户端->服务器:启动域下载/服务器->客户端:域分段下载 */
        case 1:
            /* 客户端->服务器:启动域下载 */
            if(whoami == SDO_SERVER)
            {
                /* 索引 */
                index = getSDOindex(m->data[1], m->data[2]);
                /* 子索引 */
                subIndex = getSDOsubIndex(m->data[3]);
                MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", CliServNbr);
                MSG_WAR(0x3A80, "Writing at index : ", index);
                MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
                if(!err)
                {
                    MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0);
                    failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
                    return 0xFF;
                }

                /* 查找空闲传输通道 */
                err = getSDOfreeLine(d, whoami, &line);
                if(err)
                {
                    MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
                    failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
                    return 0xFF;
                }
                /* 初始化sdo传输通道 */
                initSDOline(d, line, CliServNbr, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);

                /* 如果是快速传输 */
                if(getSDOe(m->data[0]))
                {
                    /* 取出字节数 */
                    nbBytes = 4 - getSDOn2(m->data[0]);
                    /* 存储字节数 */
                    d->transfers[line].count = nbBytes;
                    /* 将数据数据拷贝到传输通道缓冲区 */
                    err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
                    if(err)
                    {
                        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                        return 0xFF;
                    }

                    MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfer. Finished. ", 0);

                    /* 将数据从传输通道缓冲区拷贝到字典中 */
                    errorCode = SDOlineToObjdict(d, line);
                    if(errorCode)
                    {
                        MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0);
                        failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);
                        return 0xFF;
                    }
                    /* 复位sdo通道 */
                    resetSDOline(d, line);
                }
                /* 如果是正常传输 */
                else
                {
                    /* 数据字节为字节计数器 */
                    if(getSDOs(m->data[0]))
                    {
                        /* 取出字节数 */
                        nbBytes = (m->data[4]) + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24);
                        /* 设置sdo传输通道字节数剩余 */
                        err = setSDOlineRestBytes(d, line, nbBytes);
                        if(err)
                        {
                            failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                            return 0xFF;
                        }
                    }
                }
                /* 发送启动域下载响应 */
                data[0] = 3 << 5;
                data[1] = index & 0xFF;
                data[2] = (index >> 8) & 0xFF;
                data[3] = subIndex;
                for(i = 4 ; i < 8 ; i++)
                    data[i] = 0;
                sendSDO(d, whoami, CliServNbr, data);
            }
            /* 服务器->客户端:域分段下载 */
            else
            {
                /* 验证传输通道是否在域分段下载中 */
                if(!err)
                    err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
                if(err)
                {
                    MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);
                    failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
                    return 0xFF;
                }

                /* 重启传输通道超时定时器 */
                RestartSDO_TIMER(line)
                /* 索引 */
                index = d->transfers[line].index;
                /* 子索引 */
                subIndex = d->transfers[line].subIndex;
                /* 校验触发位是否同步 */
                if(d->transfers[line].toggle != getSDOt(m->data[0]))
                {
                    MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId);
                    failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
                    return 0xFF;
                }

                /* 获取传输通道剩余字节数 */
                getSDOlineRestBytes(d, line, &nbBytes);
                /* 如果已经传输完 */
                if(nbBytes == 0)
                {
                    MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId);
                    /* 停止超时定时器 */
                    StopSDO_TIMER(line)
                    /* 将状态置为完成 */
                    d->transfers[line].state = SDO_FINISHED;
                    /* 传输完成回调函数 */
                    if(d->transfers[line].Callback)
                    {
                        (*d->transfers[line].Callback)(d,nodeId);
                    }

                    return 0x00;
                }

                /* 如果至少还有7个字节没传输完毕  */
                if(nbBytes > 7)
                {
                    /* 触发位取反 */
                    d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
                    /* 构建报文 */
                    data[0] = (d->transfers[line].toggle << 4);
                    /* 将数据从传输通道缓冲区拷贝到sdo报文中 */
                    err = lineToSDO(d, line, 7, data + 1);
                    if(err)
                    {
                        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                        return 0xFF;
                    }
                }
                /* 如果数据量小于7个字节 */
                else
                {
                    /* 触发位取反 */
                    d->transfers[line].toggle = !d->transfers[line].toggle & 1;
                    /* 构建报文 */
                    data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1);
                    /* 将数据从传输通道缓冲区拷贝到sdo报文中 */
                    err = lineToSDO(d, line, nbBytes, data + 1);
                    if(err)
                    {
                        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                        return 0xFF;
                    }

                    /* 剩余字节填0 */
                    for(i = nbBytes + 1; i < 8; i++)
                    {
                        data[i] = 0;
                    }
                }
                MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId);

                /* 发送sdo报文 */
                sendSDO(d, whoami, CliServNbr, data);
            }
            break;

        /* 客户端->服务器:启动域上传/服务器->客户端:启动域上传 */
        case 2:
            /* 客户端->服务器:启动域上传 */
            if(whoami == SDO_SERVER)
            {
                /* 索引 */
                index = getSDOindex(m->data[1], m->data[2]);
                /* 子索引 */
                subIndex = getSDOsubIndex(m->data[3]);
                MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", CliServNbr);
                MSG_WAR(0x3A90, "Reading at index : ", index);
                MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);
                if(!err)
                {
                    MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line);
                    MSG_WAR(0x3A93, "Server Nbr = ", CliServNbr);
                    failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
                    return 0xFF;
                }
                /* 获取sdo空闲传输通道 */
                err = getSDOfreeLine(d, whoami, &line);
                if(err)
                {
                    MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
                    failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
                    return 0xFF;
                }
                /* 初始化传输通道 */
                initSDOline(d, line, CliServNbr, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
                /* 将数据从字典中拷贝到传输通道缓冲区 */
                errorCode = objdictToSDOline(d, line);
                if(errorCode)
                {
                    MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", errorCode);
                    failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);
                    return 0xFF;
                }
                /* 获取传输通道剩余字节数 */
                getSDOlineRestBytes(d, line, &nbBytes);
                /* 字节数大于4，正常传输 */
                if(nbBytes > 4)
                {
                    /* 命令 */
                    data[0] = (2 << 5) | 1;
                    /* 索引 */
                    data[1] = index & 0xFF;
                    data[2] = (index >> 8) & 0xFF;
                    /* 子索引 */
                    data[3] = subIndex;
                    /* 字节计数器 */
                    data[4] = (UNS8)nbBytes;
                    data[5] = (UNS8)(nbBytes >> 8);
                    data[6] = (UNS8)(nbBytes >> 16);
                    data[7] = (UNS8)(nbBytes >> 24);
                    MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId);
                    /* 发送sdo报文 */
                    sendSDO(d, whoami, CliServNbr, data);
                }
                /* 字节数不高于4，快速传输 */
                else
                {
                    /* 命令 */
                    data[0] = (UNS8)((2 << 5) | ((4 - nbBytes) << 2) | 3);
                    /* 索引 */
                    data[1] = index & 0xFF;
                    data[2] = (index >> 8) & 0xFF;
                    /* 子索引 */
                    data[3] = subIndex;
                    /* 将数据从传输通道缓冲区拷贝到sdo报文 */
                    err = lineToSDO(d, line, nbBytes, data + 4);
                    if(err)
                    {
                        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                        return 0xFF;
                    }
                    /* 不足8字节填0 */
                    for(i = 4 + nbBytes; i < 8; i++)
                    {
                        data[i] = 0;
                    }

                    MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", CliServNbr);

                    /* 发送sdo报文 */
                    sendSDO(d, whoami, CliServNbr, data);

                    /* 复位sdo传输通道 */
                    resetSDOline(d, line);
                }
            }
            /* 服务器->客户端:启动域上传 */
            else
            {
                /* 验证传输通道是否在域上传中 */
                if(!err)
                    err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
                if(err)
                {
                    MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId);
                    failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
                    return 0xFF;
                }

                /* 重置传输超时定时器 */
                RestartSDO_TIMER(line)

                /* 索引 */
                index = d->transfers[line].index;
                /* 子索引 */
                subIndex = d->transfers[line].subIndex;

                /* 加速传输完成 */
                if(getSDOe(m->data[0]))
                {
                    /* 字节数 */
                    nbBytes = 4 - getSDOn2(m->data[0]);
                    /* 将数据拷贝到传输通道缓冲区 */
                    err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
                    if(err)
                    {
                        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                        return 0xFF;
                    }

                    MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);

                    /* 停止sdo传输通道超时定时器 */
                    StopSDO_TIMER(line)

                    /* 接收到的字节数 */
                    d->transfers[line].count = nbBytes;

                    /* sdo传输通道传输完成 */
                    d->transfers[line].state = SDO_FINISHED;

                    /* 传输完成后调用回调函数 */
                    if(d->transfers[line].Callback)
                        (*d->transfers[line].Callback)(d,nodeId);

                    return 0;
                }
                /* 正常传输 */
                else
                {
                    if(getSDOs(m->data[0]))
                    {
                        /* 字节计数器 */
                        nbBytes = m->data[4] + ((UNS32)(m->data[5]) << 8) + ((UNS32)(m->data[6]) << 16) + ((UNS32)(m->data[7]) << 24);
                        /* 设置sdo传输通道剩余字节数 */
                        err = setSDOlineRestBytes(d, line, nbBytes);
                        if(err)
                        {
                            failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                            return 0xFF;
                        }
                    }

                    /* 请求：域分段上传 */
                    data[0] = 3 << 5;
                    for(i = 1; i < 8; i++)
                        data[i] = 0;

                    MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId);

                    /* 发送报文 */
                    sendSDO(d, whoami, CliServNbr, data);
                }
            }
            break;

        /* 客户端->服务器：域分段上传/服务器->客户端：启动域下载 */
        case 3:
            /* 客户端->服务器：域分段上传 */
            if(whoami == SDO_SERVER)
            {
                if(!err)
                    err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
                if(err)
                {
                    MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", CliServNbr);
                    failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);

                    return 0xFF;
                }

                /* 复位sdo传输通道超时定时器 */
                RestartSDO_TIMER(line)

                MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", CliServNbr);

                /* 索引 */
                index = d->transfers[line].index;
                /* 子索引 */
                subIndex = d->transfers[line].subIndex;

                /* 校验触发位是否同步 */
                if(d->transfers[line].toggle != getSDOt(m->data[0]))
                {
                    MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0]));
                    failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);

                    return 0xFF;
                }

                /* 获取传输通道剩余字节数 */
                getSDOlineRestBytes(d, line, &nbBytes);
                /* 如果至少还有7个字节没传输完毕  */
                if(nbBytes > 7)
                {
                    /* 构建报文 */
                    data[0] = (d->transfers[line].toggle << 4);
                    /* 将数据从sdo传输通道缓冲区拷贝到sdo报文中 */
                    err = lineToSDO(d, line, 7, data + 1);
                    if(err)
                    {
                        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                        return 0xFF;
                    }

                    /* 将触发位取反 */
                    d->transfers[line].toggle = !d->transfers[line].toggle & 1;

                    MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", CliServNbr);

                    /* 发送报文 */
                    sendSDO(d, whoami, CliServNbr, data);
                }
                /* 不大于7字节，则说明是最后一段 */
                else
                {
                    /* 构建报文 */
                    data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1);
                    /* 将数据从sdo传输通道缓冲区拷贝到sdo报文中 */
                    err = lineToSDO(d, line, nbBytes, data + 1);
                    if (err) {
                        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                        return 0xFF;
                    }
                    /* 不满8字节补0 */
                    for(i = nbBytes + 1; i < 8; i++)
                        data[i] = 0;

                    MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", CliServNbr);

                    /* 发送报文 */
                    sendSDO(d, whoami, CliServNbr, data);

                    /* 复位传输通道 */
                    resetSDOline(d, line);
                }
            }
            /* 服务器->客户端：启动域下载 */
            else
            {
                /* 校验传输通道是否在域下载过程中 */
                if(!err)
                    err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
                if(err)
                {
                    MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId);
                    failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
                    return 0xFF;
                }

                /* 复位sdo传输通道定时器 */
                RestartSDO_TIMER(line)

                /* 索引 */
                index = d->transfers[line].index;
                /* 子索引 */
                subIndex = d->transfers[line].subIndex;

                /* 获取sdo传输通道剩余字节数 */
                getSDOlineRestBytes(d, line, &nbBytes);

                /* 传输完成 */
                if(nbBytes == 0)
                {
                    MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId);

                    /* 停止传输通道超时定时器 */
                    StopSDO_TIMER(line)

                    /* 将传输通道状态设置为完成 */
                    d->transfers[line].state = SDO_FINISHED;

                    /* 传输完成回调函数 */
                    if(d->transfers[line].Callback)
                        (*d->transfers[line].Callback)(d,nodeId);

                    return 0x00;
                }

                /* 剩余字节数大于7个：域分段下载 */
                if(nbBytes > 7)
                {
                    /* 构建报文 */
                    data[0] = (d->transfers[line].toggle << 4);

                    /* 将数据从传输通道中拷贝到sdo报文中 */
                    err = lineToSDO(d, line, 7, data + 1);
                    if(err)
                    {
                        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                        return 0xFF;
                    }
                }
                /* 剩余字节数不大于7个：域分段下载，最后一段 */
                else
                {
                    /* 构建报文 */
                    data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1);

                    /* 将数据从传输通道中拷贝到sdo报文中 */
                    err = lineToSDO(d, line, nbBytes, data + 1);
                    if(err)
                    {
                        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                        return 0xFF;
                    }

                    /* 不足8字节补0 */
                    for(i = nbBytes + 1; i < 8; i++)
                        data[i] = 0;
                }

                MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId);

                /* 发送sdo报文 */
                sendSDO(d, whoami, CliServNbr, data);
            }
            break;

        /* 中止 */
        case 4:
            /* 中止码 */
            abortCode = (UNS32)m->data[4] | ((UNS32)m->data[5] << 8) | ((UNS32)m->data[6] << 16) | ((UNS32)m->data[7] << 24);
            /* 客户端->服务器：中止 */
            if(whoami == SDO_SERVER)
            {
                /* 复位sdo传输通道 */
                if(!err)
                {
                    resetSDOline(d, line);
                    MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
                }
                else
                    MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);
            }
            /* 服务器->客户端：中止 */
            else
            {
                if(!err)
                {
                    /* 停止sdo传输通道超时定时器 */
                    StopSDO_TIMER(line)
                    /* 将传输通道状态设置为中止 */
                    d->transfers[line].state = SDO_ABORTED_RCV;
                    /* 中止码 */
                    d->transfers[line].abortCode = abortCode;

                    MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);

                    /* 传输结束调用回调函数 */
                    if(d->transfers[line].Callback)
                        (*d->transfers[line].Callback)(d, nodeId);
                }
                else
                    MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
            }
            break;

        /* 服务器->客户端：块下载初始化/服务器->客户端：块下载/服务器->客户端：块下载结束 */
        /* 客户端->服务器：块上传初始化/客户端->服务器：块上传开始命令/客户端->服务器：块上传/客户端->服务器：块上传结束 */
        case 5:
            /* 子命令 */
            SubCommand = getSDOblockSC(m->data[0]);

            /* 客户端->服务器：块上传初始化/客户端->服务器：块上传/客户端->服务器：块上传结束 */
            if(whoami == SDO_SERVER)
            {
                /* 客户端->服务器：块上传初始化 */
                if(SubCommand == SDO_BCS_INITIATE_UPLOAD_REQUEST)
                {
                    /* 索引 */
                    index = getSDOindex(m->data[1], m->data[2]);
                    /* 子索引 */
                    subIndex = getSDOsubIndex(m->data[3]);

                    MSG_WAR(0x3AB2, "Received SDO Initiate block upload defined at index 0x1200 + ", CliServNbr);
                    MSG_WAR(0x3AB3, "Reading at index : ", index);
                    MSG_WAR(0x3AB4, "Reading at subIndex : ", subIndex);
                    if(!err)
                    {
                        MSG_ERR(0x1A93, "SDO error : Transmission yet started at line : ", line);
                        MSG_WAR(0x3AB5, "Server Nbr = ", CliServNbr);
                        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
                        return 0xFF;
                    }

                    /* 获取sdo空闲传输通道 */
                    err = getSDOfreeLine(d, whoami, &line);
                    if(err)
                    {
                        MSG_ERR(0x1A73, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
                        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
                        return 0xFF;
                    }

                    /* 初始化sdo传输通道 */
                    initSDOline(d, line, CliServNbr, index, subIndex, SDO_BLOCK_UPLOAD_IN_PROGRESS);

                    /* 是否支持crc校验 */
                    d->transfers[line].peerCRCsupport = ((m->data[0]) >> 2) & 1;

                    /* 块数量 */
                    d->transfers[line].blksize = m->data[4];
                    /* 将数据从字典中拷贝到sdo传输通道缓冲区 */
                    errorCode = objdictToSDOline(d, line);
                    if(errorCode)
                    {
                        MSG_ERR(0x1A95, "SDO error : Unable to copy the data from object dictionary. Err code : ", errorCode);
                        failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);
                        return 0xFF;
                    }
                    /* 获取sdo传输剩余字节数 */
                    getSDOlineRestBytes(d, line, &nbBytes);
                    /* 设置对象大小 */
                    d->transfers[line].objsize = nbBytes;

                    /* sdo块上传初始化响应 */
                    data[0] = (6 << 5) | (1 << 1) | SDO_BSS_INITIATE_UPLOAD_RESPONSE;
                    /* 索引 */
                    data[1] = index & 0xFF;
                    data[2] = (index >> 8) & 0xFF;
                    /* 子索引 */
                    data[3] = subIndex;
                    /* 字节数 */
                    data[4] = (UNS8)nbBytes;
                    data[5] = (UNS8)(nbBytes >> 8);
                    data[6] = (UNS8)(nbBytes >> 16);
                    data[7] = (UNS8)(nbBytes >> 24);

                    MSG_WAR(0x3A9A, "SDO. Sending normal block upload initiate response defined at index 0x1200 + ", nodeId);

                    /* 发送sdo报文 */
                    sendSDO(d, whoami, CliServNbr, data);
                }
                /* 客户端->服务器：块上传结束响应 */
                else if(SubCommand == SDO_BCS_END_UPLOAD_REQUEST)
                {
                    MSG_WAR(0x3AA2, "Received SDO block END upload request defined at index 0x1200 + ", CliServNbr);

                    /* 校验是否在块上传过程中 */
                    if(!err)
                        err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS;
                    if(err)
                    {
                        MSG_ERR(0x1AA1, "SDO error : Received block upload request for unstarted trans. index 0x1200 + ", CliServNbr);
                        failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
                        return 0xFF;
                    }

                    /* 复位sdo传输通道 */
                    resetSDOline(d, line);
                }
                /* 客户端->服务器：块上传/客户端->服务器：块上传开始命令 */
                else if((SubCommand == SDO_BCS_UPLOAD_RESPONSE) || (SubCommand == SDO_BCS_START_UPLOAD))
                {
                    /* 校验是否在块上传过程中 */
                    if(!err)
                        err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS;
                    if(err)
                    {
                        MSG_ERR(0x1AA1, "SDO error : Received block upload response for unstarted trans. index 0x1200 + ", CliServNbr);
                        failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
                        return 0xFF;
                    }

                    /* 重启传输通道超时定时器 */
                    RestartSDO_TIMER(line);

                    /* 索引 */
                    index = d->transfers[line].index;
                    /* 子索引 */
                    subIndex = d->transfers[line].subIndex;

                    /* 块上传响应 */
                    if(SubCommand == SDO_BCS_UPLOAD_RESPONSE)
                    {
                        MSG_WAR(0x3AA2, "Received SDO block upload response defined at index 0x1200 + ", CliServNbr);

                        /* 一次最大上传块数 */
                        d->transfers[line].blksize = m->data[2];
                        /* 响应序列号 */
                        AckSeq = (m->data[1]) & 0x7f;
                        /* 获取剩余字节数 */
                        getSDOlineRestBytes(d, line, &nbBytes);
                        /* 如果传输完 */
                        if((nbBytes == 0) && (AckSeq == d->transfers[line].seqno))
                        {
                            /* 上传结束指令 */
                            data[0] = (6 << 5) | ((d->transfers[line].endfield) << 2) | SDO_BSS_END_UPLOAD_RESPONSE;
                            for(i = 1; i < 8; i++)
                                data[i] = 0;

                            MSG_WAR(0x3AA5, "SDO. Sending block END upload response defined at index 0x1200 + ", CliServNbr);

                            /* 发送sdo报文 */
                            sendSDO(d, whoami, CliServNbr, data);
                            break;
                        }
                        else
                            d->transfers[line].offset = d->transfers[line].lastblockoffset + 7 * AckSeq;

                        if(d->transfers[line].offset > d->transfers[line].count)
                        {
                            MSG_ERR(0x1AA1, "SDO error : Received upload response with bad ackseq index 0x1200 + ", CliServNbr);
                            failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
                            return 0xFF;
                        }
                    }
                    else
                        MSG_WAR(0x3AA2, "Received SDO block START upload defined at index 0x1200 + ", CliServNbr);

                    /* 上一次块传输后数据偏移量 */
                    d->transfers[line].lastblockoffset = (UNS8) d->transfers[line].offset;

                    /* 进行一次块上传数据 */
                    for(SeqNo = 1; SeqNo <= d->transfers[line].blksize; SeqNo++)
                    {
                        /* 块序列号 */
                        d->transfers[line].seqno = SeqNo;

                        /* 获取传输通道剩余字节数 */
                        getSDOlineRestBytes(d, line, &nbBytes);
                        /* 如果大于7字节，则说明不是最后一块数据 */
                        if(nbBytes > 7)
                        {
                            /* 块序列号 */
                            data[0] = SeqNo;
                            /* 将传输通道上的数据拷贝到sdo报文中 */
                            err = lineToSDO(d, line, 7, data + 1);
                            if(err)
                            {
                                failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                                return 0xFF;
                            }

                            MSG_WAR(0x3AA5, "SDO. Sending upload segment defined at index 0x1200 + ", CliServNbr);

                            /* 发送sdo报文 */
                            sendSDO(d, whoami, CliServNbr, data);
                        }
                        /* 如果不大于7字节，则说明是最后一块数据 */
                        else
                        {
                            /* 块序列号，最高位表示最后一块 */
                            data[0] = 0x80 | SeqNo;

                            /* 将传输通道上的数据拷贝到sdo报文中 */
                            err = lineToSDO(d, line, nbBytes, data + 1);
                            if(err)
                            {
                                failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                                return 0xFF;
                            }

                            /* 不满8字节补0 */
                            for(i = nbBytes + 1 ; i < 8 ; i++)
                                data[i] = 0;

                            MSG_WAR(0x3AA5, "SDO. Sending last upload segment defined at index 0x1200 + ", CliServNbr);

                            /* 发送sdo报文 */
                            sendSDO(d, whoami, CliServNbr, data);

                            /* 记录补零个数 */
                            d->transfers[line].endfield = (UNS8)(7 - nbBytes);
                            break;
                        }
                    }
                }
            }
            /* 服务器->客户端：块下载初始化/服务器->客户端：块下载/服务器->客户端：块下载结束 */
            else
            {
                /* 服务器->客户端：块下载初始化/服务器->客户端：块下载 */
                if((SubCommand == SDO_BSS_INITIATE_DOWNLOAD_RESPONSE) ||
                     (SubCommand == SDO_BSS_DOWNLOAD_RESPONSE))
                {
                    /* 判断sdo传输通道是否在块下载过程中 */
                    if(!err)
                        err = d->transfers[line].state != SDO_BLOCK_DOWNLOAD_IN_PROGRESS;
                    if(err)
                    {
                        MSG_ERR(0x1AAA, "SDO error : Received response for unknown block download request from node id", nodeId);
                        failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
                        return 0xFF;
                    }

                    /* 复位sdo传输超时定时器 */
                    RestartSDO_TIMER(line)

                    /* 服务器->客户端：块下载初始化 */
                    if(SubCommand == SDO_BSS_INITIATE_DOWNLOAD_RESPONSE)
                    {
                        /* 索引 */
                        index = d->transfers[line].index;
                        /* 子索引 */
                        subIndex = d->transfers[line].subIndex;
                        /* 是否支持crc校验 */
                        d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1;
                        /* 块数量 */
                        d->transfers[line].blksize = m->data[4];
                    }
                    /* 服务器->客户端：块下载 */
                    else
                    {
                        /* 块数量 */
                        d->transfers[line].blksize = m->data[2];
                        /* 序列号 */
                        AckSeq = (m->data[1]) & 0x7f;
                        /* 获取sdo传输通道缓冲区剩余字节数 */
                        getSDOlineRestBytes(d, line, &nbBytes);
                        /* 如果剩余字节数为0，并且序列号校验通过 */
                        if((nbBytes == 0) && (AckSeq == d->transfers[line].seqno))
                        {
                            /* 块下载结束 */
                            data[0] = (6 << 5) | ((d->transfers[line].endfield) << 2) | SDO_BCS_END_DOWNLOAD_REQUEST;
                            /* 数据填0 */
                            for(i = 1; i < 8; i++)
                                data[i] = 0;

                            MSG_WAR(0x3AA5, "SDO. Sending block END download request defined at index 0x1200 + ", CliServNbr);

                            /* 发送sdo报文 */
                            sendSDO(d, whoami, CliServNbr, data);
                            break;
                        }
                        /* 如果剩余字节数不为0 */
                        else
                            d->transfers[line].offset = d->transfers[line].lastblockoffset + 7 * AckSeq;

                        if(d->transfers[line].offset > d->transfers[line].count)
                        {
                            MSG_ERR(0x1AA1, "SDO error : Received upload segment with bad ackseq index 0x1200 + ", CliServNbr);
                            failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
                            return 0xFF;
                        }
                    }
                    /* 上一次内存块发送完后传输通道的偏移量 */
                    d->transfers[line].lastblockoffset = (UNS8)d->transfers[line].offset;
                    /* 将数据分块发送出去，不能超过从站指定的最大块数量 */
                    for(SeqNo = 1; SeqNo <= d->transfers[line].blksize; SeqNo++)
                    {
                        /* 块序列号 */
                        d->transfers[line].seqno = SeqNo;
                        /* 获取传输通道剩余字节数 */
                        getSDOlineRestBytes(d, line, &nbBytes);
                        /* 如果剩余字节数大于7 */
                        if(nbBytes > 7)
                        {
                            /* 块序列号，最高位表示是否最后一块 */
                            data[0] = SeqNo;
                            /* 将传输通道缓冲区数据拷贝到sdo报文中 */
                            err = lineToSDO(d, line, 7, data + 1);
                            if(err)
                            {
                                failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                                return 0xFF;
                            }

                            MSG_WAR(0x3AAB, "SDO. Sending download segment to node id ", nodeId);

                            /* 发送报文 */
                            sendSDO(d, whoami, CliServNbr, data);
                        }
                        /* 如果剩余字节数不大于7，说明这是最后一块 */
                        else
                        {
                            /* 块序列号，最高位表示是否最后一块 */
                            data[0] = 0x80 | SeqNo;
                            /* 将传输通道缓冲区数据拷贝到sdo报文中 */
                            err = lineToSDO(d, line, nbBytes, data + 1);
                            if(err)
                            {
                                failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
                                return 0xFF;
                            }

                            /* 不足8字节补0 */
                            for(i = nbBytes + 1; i < 8; i++)
                                data[i] = 0;

                            MSG_WAR(0x3AAB, "SDO. Sending last download segment to node id ", nodeId);

                            /* 发送报文 */
                            sendSDO(d, whoami, CliServNbr, data);

                            /* 最后一个块数据包中，补0的字节数 */
                            d->transfers[line].endfield = (UNS8)(7 - nbBytes);
                            break;
                        }
                    }
                }
                /* 服务器->客户端：块下载结束 */
                else if(SubCommand == SDO_BSS_END_DOWNLOAD_RESPONSE)
                {
                    MSG_WAR(0x3AAC, "SDO End block download response from nodeId", nodeId);

                    /* 停止sdo传输超时定时器 */
                    StopSDO_TIMER(line)
                    /* 传输通道状态置为传输完成 */
                    d->transfers[line].state = SDO_FINISHED;

                    /* 传输完成调用回调函数 */
                    if(d->transfers[line].Callback)
                        (*d->transfers[line].Callback)(d, nodeId);
                    return 0x00;
                }
                /* 错误指令 */
                else
                {
                    MSG_ERR(0x1AAB, "SDO error block download : Received wrong subcommand from nodeId", nodeId);
                    failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
                return 0xFF;
                }
            }
            break;

        /* 客户端->服务器：块下载初始化/客户端->服务器：块下载/客户端->服务器：块下载结束 */
        /* 服务器->客户端：块上传初始化/服务器->客户端：块上传/服务器->客户端：块上传结束 */
        case 6:
            /* 客户端->服务器：块下载初始化/客户端->服务器：块下载/客户端->服务器：块下载结束 */
            if(whoami == SDO_SERVER)
            {
                /* 如果传输通道未建立，则说明客户端->服务器：块下载初始化 */
                if(err)
                {
                    /* 取出子命令，判断是否为块下载初始化 */
                    SubCommand = (m->data[0]) & 1;
                    if(SubCommand != SDO_BCS_INITIATE_DOWNLOAD_REQUEST)
                    {
                        MSG_ERR(0x1AAC, "SDO error block download : Received wrong subcommand from node id", nodeId);
                        failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);

                        return 0xFF;
                  }

                    /* 取出索引 */
                    index = getSDOindex(m->data[1], m->data[2]);
                    /* 取出子索引 */
                    subIndex = getSDOsubIndex(m->data[3]);