https://blog.csdn.net/lushoumin/article/details/79668443
SDO基于CS模式,所有报文都需要确认。通常从节点作为SDO服务器,主节点作为客户端。客户端通过索引和子索引,访问服务器上的任意对象字典,SDO的上传与下载,是从server的角度去理解的,上传:client对server的OD进行读操作;下载:client对server的OD进行写操作。
传送机制有两种:域传送和块传送。
SDO报文格式如下:
SDO命令字包含如下信息:该报文是上传还是下载,该报文是请求还是应答,该报文是分段还是加速,CAN帧数据字节长度,后续分段的触发位。
块传输这里不进行详细讨论,只给出块下载流程:
1.客户端:初始化传输通道
发送块下载初始化指令,包括索引、子索引、字节数
2.服务器:初始化传输通道
发送块下载初始化响应,包括索引、子索引、一次传输块数量
3.客户端:复位传输超时定时器
发送多个数据包块下载,包括是否最后一块、序列号、数据
4.服务器:复位传输超时定时器
将数据拷贝到传输通道
发送数据包块下载响应,包括序列号、一次传输块数量
5.客户端:如果没有下载完,重复步骤3、4
如果下载完,发送块下载结束指令,包括最后一块补零数
6.服务器:发送块下载结束响应
将数据拷贝到字典中
复位传输通道
7.客户端:停止传输超时定时器
设置传输通道为完成状态
调用传输完成回调函数
- #include <stdlib.h>
- #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]);
- MSG_WAR(0x3A9B, "Received SDO block download initiate defined at index 0x1200 + ", CliServNbr);
- MSG_WAR(0x3A9B, "Writing at index : ", index);
- MSG_WAR(0x3A9B, "Writing at subIndex : ", subIndex);
- /* 获取空闲的sdo传输通道 */
- err = getSDOfreeLine(d, whoami, &line);
- if(err)
- {
- MSG_ERR(0x1A89, "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_DOWNLOAD_IN_PROGRESS);
- /* 块传输接收状态 */
- d->transfers[line].rxstep = RXSTEP_STARTED;
- /* 是否crc校验 */
- d->transfers[line].peerCRCsupport = ((m->data[0]) >> 2) & 1;
- /* 是否指明字节大小 */
- if((m->data[0]) & 2)
- {
- d->transfers[line].objsize = (UNS32)m->data[4] + (UNS32)m->data[5] * 256 + (UNS32)m->data[6] * 256 * 256 +
- (UNS32)m->data[7] * 256 * 256 * 256;
- }
- /* 块下载初始化响应 */
- data[0] = (5 << 5) | SDO_BSS_INITIATE_DOWNLOAD_RESPONSE;
- /* 索引 */
- data[1] = (UNS8)index;
- data[2] = (UNS8)(index >> 8);
- /* 子索引 */
- data[3] = subIndex;
- /* 块数量 */
- data[4] = SDO_BLOCK_SIZE;
- /* 不足8字节补0 */
- data[5] = data[6] = data[7] = 0;
- MSG_WAR(0x3AAD, "SDO. Sending block download initiate response - index 0x1200 + ", CliServNbr);
- /* 发送sdo报文 */
- sendSDO(d, whoami, CliServNbr, data);
- }
- /* 客户端->服务器:块下载 */
- else if(d->transfers[line].rxstep == RXSTEP_STARTED)
- {
- MSG_WAR(0x3A9B, "Received SDO block download data segment - index 0x1200 + ", CliServNbr);
- /* 重启sdo传输通道超时定时器 */
- RestartSDO_TIMER(line)
- /* 取出序列号 */
- SeqNo = m->data[0] & 0x7F;
- /* 检查是否最后一块 */
- if(m->data[0] & 0x80)
- {
- /* 对序列号进行校验 */
- if(SeqNo == (d->transfers[line].seqno + 1))
- {
- /* 接收结束 */
- d->transfers[line].rxstep = RXSTEP_END;
- /* 设置序列号 */
- d->transfers[line].seqno = SeqNo;
- /* 将数据拷贝到传输通道临时数据缓冲 */
- memcpy(d->transfers[line].tmpData, m->data, 8);
- }
- /* 块传输响应 */
- data[0] = (5 << 5) | SDO_BSS_DOWNLOAD_RESPONSE;
- /* 序列号 */
- data[1] = d->transfers[line].seqno;
- /* 块数量 */
- data[2] = SDO_BLOCK_SIZE;
- /* 不足8字节补0 */
- data[3] = data[4] = data[5] = data[6] = data[7] = 0;
- MSG_WAR(0x3AAE, "SDO. Sending block download response - index 0x1200 + ", CliServNbr);
- /* 发送sdo数据包 */
- sendSDO(d, whoami, CliServNbr, data);
- /* 将序列号置0 */
- d->transfers[line].seqno = 0;
- }
- /* 如果不是最后一块 */
- else
- {
- /* 对序列号进行校验 */
- if(SeqNo == (d->transfers[line].seqno + 1))
- {
- /* 设置序列号 */
- d->transfers[line].seqno = SeqNo;
- /* 将数据从sdo报文中拷贝到传输通道缓冲区*/
- err = SDOtoLine(d, line, 7, (*m).data + 1);
- if(err)
- {
- failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR);
- return 0xFF;
- }
- }
- /* 如果一次传输还不够传,则响应 */
- if(SeqNo == SDO_BLOCK_SIZE)
- {
- /* 块传输响应 */
- data[0] = (5 << 5) | SDO_BSS_DOWNLOAD_RESPONSE;
- /* 序列号 */
- data[1] = d->transfers[line].seqno;
- /* 块数量 */
- data[2] = SDO_BLOCK_SIZE;
- /* 不足8字节补0 */
- data[3] = data[4] = data[5] = data[6] = data[7] = 0;
- MSG_WAR(0x3AAE, "SDO. Sending block download response - index 0x1200 + ", CliServNbr);
- /* 发送sdo数据包 */
- sendSDO(d, whoami, CliServNbr, data);
- /* 将序列号置0 */
- d->transfers[line].seqno = 0;
- }
- }
- }
- /* 客户端->服务器:块下载结束 */
- else if(d->transfers[line].rxstep == RXSTEP_END)
- {
- MSG_WAR(0x3A9B, "Received SDO block download end request - index 0x1200 + ", CliServNbr);
- /* 验证数据包是不是块下载结束 */
- if((m->data[0] & 1) != SDO_BCS_END_DOWNLOAD_REQUEST)
- {
- MSG_ERR(0x1AAD, "SDO error block download : Received wrong subcommand - index 0x1200 + ", CliServNbr);
- failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
- return 0xFF;
- }
- /* 重启sdo传输通道超时定时器 */
- RestartSDO_TIMER(line)
- /* 取出补零个数 */
- NbBytesNoData = (m->data[0] >> 2) & 0x07;
- /* 将数据从传输通道临时缓冲区拷贝到传输通道缓冲区 */
- err = SDOtoLine(d, line, 7 - NbBytesNoData, d->transfers[line].tmpData + 1);
- if(err)
- {
- failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR);
- return 0xFF;
- }
- /* 判断接收到的数据和初始化发过来的数据个数是否一致 */
- if(d->transfers[line].objsize)
- {
- if(d->transfers[line].objsize != d->transfers[line].offset)
- {
- MSG_ERR(0x1AAE, "SDO error block download : sizes do not match - index 0x1200 + ", CliServNbr);
- failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_LOCAL_CTRL_ERROR);
- return 0xFF;
- }
- }
- /* 块下载传输结束 */
- data[0] = (5 << 5) | SDO_BSS_END_DOWNLOAD_RESPONSE;
- /* 数据填0 */
- for(i = 1; i < 8; i++)
- data[i] = 0;
- MSG_WAR(0x3AAF, "SDO. Sending block download end response - index 0x1200 + ", CliServNbr);
- /* 发送sdo报文 */
- sendSDO(d, whoami, CliServNbr, data);
- /* 将数据从sdo传输通道拷贝到字典中 */
- errorCode = SDOlineToObjdict(d, line);
- if(errorCode)
- {
- MSG_ERR(0x1AAF, "SDO error : Unable to copy the data in the object dictionary", 0);
- failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, errorCode);
- return 0xFF;
- }
- /* 复位sdo传输通道 */
- resetSDOline(d, line);
- MSG_WAR(0x3AAF, "SDO. End of block download defined at index 0x1200 + ", CliServNbr);
- }
- }
- /* 服务器->客户端:块上传初始化/服务器->客户端:块上传/服务器->客户端:块上传结束 */
- else
- {
- if(err)
- {
- MSG_ERR(0x1AAD, "SDO error block upload : no transmission started", nodeId);
- failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
- return 0xFF;
- }
- /* 重启传输通道超时定时器 */
- RestartSDO_TIMER(line)
- /* 服务器->客户端:块上传初始化 */
- if(d->transfers[line].rxstep == RXSTEP_INIT)
- {
- /* 校验数据包是不是块上传响应 */
- if((m->data[0] & 1) == SDO_BSS_INITIATE_UPLOAD_RESPONSE)
- {
- MSG_WAR(0x3A9C, "Received SDO block upload response from node id ", nodeId);
- /* 块传输开始 */
- d->transfers[line].rxstep = RXSTEP_STARTED;
- /* 是否支持crc */
- d->transfers[line].peerCRCsupport = ((m->data[0]) >> 2) & 1;
- /* 是否指明数据长度 */
- if((m->data[0]) & 2)
- {
- d->transfers[line].objsize = (UNS32)m->data[4] + (UNS32)m->data[5] * 256 + (UNS32)m->data[6] * 256 * 256 +
- (UNS32)m->data[7] * 256 * 256 * 256;
- }
- /* 开始上传请求 */
- data[0] = (5 << 5) | SDO_BCS_START_UPLOAD;
- for(i = 1; i < 8; i++)
- data[i] = 0;
- MSG_WAR(0x3AB6, "SDO. Sending block upload start to node id ", nodeId);
- /* 发送手动报文 */
- sendSDO(d, whoami, CliServNbr, data);
- }
- }
- /* 服务器->客户端:块上传 */
- else if(d->transfers[line].rxstep == RXSTEP_STARTED)
- {
- /* 序列号 */
- SeqNo = m->data[0] & 0x7F;
- /* 最后一块 */
- if(m->data[0] & 0x80)
- {
- /* 校验序列号,如果通过则将状态置为结束 */
- if(SeqNo == (d->transfers[line].seqno + 1))
- {
- d->transfers[line].rxstep = RXSTEP_END;
- d->transfers[line].seqno = SeqNo;
- /* 将数据拷贝到传输通道临时缓冲区 */
- memcpy(d->transfers[line].tmpData, m->data, 8);
- }
- /* 对块传输进行相应 */
- data[0] = (5 << 5) | SDO_BCS_UPLOAD_RESPONSE;
- /* 序列号 */
- data[1] = d->transfers[line].seqno;
- /* 一次传输最大块数量 */
- data[2] = SDO_BLOCK_SIZE;
- /* 补零 */
- data[3] = data[4] = data[5] = data[6] = data[7] = 0;
- MSG_WAR(0x3AB7, "SDO. Sending block upload response to node id ", nodeId);
- /* 发送报文 */
- sendSDO(d, whoami, CliServNbr, data);
- /* 序列号置零 */
- d->transfers[line].seqno = 0;
- }
- /* 如果不是最后一块 */
- else
- {
- /* 校验序列号,如果通过则将数据拷贝到传输通道 */
- if(SeqNo == (d->transfers[line].seqno + 1))
- {
- d->transfers[line].seqno = SeqNo;
- /* 将数据拷贝到传输通道 */
- err = SDOtoLine(d, line, 7, (*m).data + 1);
- if(err)
- {
- failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR);
- return 0xFF;
- }
- }
- /* 如果一次传输结束 */
- if(SeqNo == SDO_BLOCK_SIZE)
- {
- /* 响应 */
- data[0] = (5 << 5) | SDO_BCS_UPLOAD_RESPONSE;
- data[1] = d->transfers[line].seqno;
- data[2] = SDO_BLOCK_SIZE;
- data[3] = data[4] = data[5] = data[6] = data[7] = 0;
- MSG_WAR(0x3AAE, "SDO. Sending block upload response to node id ", nodeId);
- sendSDO(d, whoami, CliServNbr, data);
- d->transfers[line].seqno = 0;
- }
- }
- }
- /* 服务器->客户端:块上传结束 */
- else if(d->transfers[line].rxstep == RXSTEP_END)
- {
- /* 校验上传结束 */
- if((m->data[0] & 1) != SDO_BSS_END_UPLOAD_RESPONSE)
- {
- MSG_ERR(0x1AAD, "SDO error block upload : Received wrong subcommand from node id ", nodeId);
- failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
- return 0xFF;
- }
- /* 补零字节数 */
- NbBytesNoData = (m->data[0] >> 2) & 0x07;
- /* 将数据从sdo报文中拷贝到传输通道缓冲区 */
- err = SDOtoLine(d, line, 7 - NbBytesNoData, d->transfers[line].tmpData + 1);
- if(err)
- {
- failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR);
- return 0xFF;
- }
- /* 校验一下数据长度 */
- if(d->transfers[line].objsize)
- {
- if(d->transfers[line].objsize != d->transfers[line].offset)
- {
- MSG_ERR(0x1AAE, "SDO error block download : sizes do not match - from node id ", nodeId);
- failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_LOCAL_CTRL_ERROR);
- return 0xFF;
- }
- }
- /* 块上传结束请求 */
- data[0] = (5 << 5) | SDO_BCS_END_UPLOAD_REQUEST;
- for(i = 1; i < 8; i++)
- data[i] = 0;
- MSG_WAR(0x3AAF, "SDO. Sending block upload end request to node id ", nodeId);
- /* 发送数据包 */
- sendSDO(d, whoami, CliServNbr, data);
- MSG_WAR(0x3AAF, "SDO. End of block upload request", 0);
- /* 停止超时定时器 */
- StopSDO_TIMER(line)
- /* 传输完成 */
- d->transfers[line].state = SDO_FINISHED;
- /* 调用回调函数 */
- if(d->transfers[line].Callback)
- (*d->transfers[line].Callback)(d,nodeId);
- }
- }
- break;
- default:
- MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", cs);
- return 0xFF;
- }
- return 0;
- }
- /* 通过节点号查找客户端号 */
- UNS8 GetSDOClientFromNodeId(CO_Data *d, UNS8 nodeId)
- {
- UNS8 SDOfound = 0;
- UNS8 CliNbr;
- UNS16 lastIndex;
- UNS16 offset;
- UNS8 nodeIdServer;
- /* 第一个sdo客户端在字典中的下标 */
- offset = d->firstIndex->SDO_CLT;
- /* 最后一个sdo客户端在字典中的下标 */
- lastIndex = d->lastIndex->SDO_CLT;
- if(offset == 0)
- {
- MSG_ERR(0x1AC6, "No SDO client index found for nodeId ", nodeId);
- return 0xFF;
- }
- /* 客户端号置零 */
- CliNbr = 0;
- /* 遍历字典中所有客户端条目 */
- while(offset <= lastIndex)
- {
- /* 客户端条目中子索引个数大于3 */
- if(d->objdict[offset].bSubCount <= 3)
- {
- MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + CliNbr);
- return 0xFF;
- }
- /* 从字典中取出服务器节点id */
- nodeIdServer = *((UNS8*)d->objdict[offset].pSubindex[3].pObject);
- MSG_WAR(0x1AD2, "index : ", 0x1280 + CliNbr);
- MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
- /* 如果节点id和服务器节点id匹配,相当于找到了对应的客户端号,因为每个客户端对应一个服务器 */
- if(nodeIdServer == nodeId)
- {
- SDOfound = 1;
- break;
- }
- offset++;
- CliNbr++;
- }
- /* 如果没找到对应的服务器。则退出 */
- if(!SDOfound)
- {
- MSG_WAR(0x1AC9, "SDO No preset client found to communicate with node : ", nodeId);
- return 0xFE;
- }
- MSG_WAR(0x3AD0, " SDO client defined at index : ", 0x1280 + CliNbr);
- return CliNbr;
- }
- /* 写服务器字典 */
- 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)
- {
- UNS8 err;
- UNS8 line;
- UNS8 CliNbr;
- UNS32 j;
- UNS8 i;
- UNS8 buf[8];
- MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
- MSG_WAR(0x3AC1, " At index : ", index);
- MSG_WAR(0x3AC2, " subIndex : ", subIndex);
- MSG_WAR(0x3AC3, " nb bytes : ", count);
- /* 通过服务器节点ID查找客户端号 */
- CliNbr = GetSDOClientFromNodeId(d, nodeId);
- /* 客户端不能大于等于254个 */
- if(CliNbr >= 0xFE)
- return CliNbr;
- /* 通过客户端号获取sdo传输通道,检查该客户端是否正在传输过程中,如果在直接退出 */
- err = getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line);
- if(!err)
- {
- MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId);
- return 0xFF;
- }
- /* 获取空闲的sdo传输通道 */
- err = getSDOfreeLine(d, SDO_CLIENT, &line);
- if(err)
- {
- MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
- return (0xFF);
- }
- else
- {
- MSG_WAR(0x3AE1, "Transmission on line : ", line);
- }
- /* 块模式 */
- if(useBlockMode)
- {
- /* 初始化sdo传输通道 */
- initSDOline(d, line, CliNbr, index, subIndex, SDO_BLOCK_DOWNLOAD_IN_PROGRESS);
- d->transfers[line].objsize = count;
- }
- else
- {
- /* 初始化sdo传输通道 */
- initSDOline(d, line, CliNbr, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
- }
- /* 字节数 */
- d->transfers[line].count = count;
- /* 数据类型 */
- d->transfers[line].dataType = dataType;
- /* 支持内存动态分配 */
- #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
- {
- /* 缓冲区指针 */
- UNS8 *lineData = d->transfers[line].data;
- /* 如果字节数大于缓冲区长度 */
- if(count > SDO_MAX_LENGTH_TRANSFER)
- {
- /* 动态分配内存 */
- d->transfers[line].dynamicData = (UNS8*)malloc(count);
- d->transfers[line].dynamicDataSize = count;
- if(d->transfers[line].dynamicData == NULL)
- {
- MSG_ERR(0x1AC9, "SDO. Error. Could not allocate enough bytes : ", count);
- return 0xFE;
- }
- lineData = d->transfers[line].dynamicData;
- }
- #endif
- /* 将数据拷贝到缓冲区 */
- for(j = 0; j < count; j++)
- {
- #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
- #ifdef CANOPEN_BIG_ENDIAN
- if(dataType == 0 && endianize)
- lineData[count - 1 - j] = ((char *)data)[j];
- else
- lineData[j] = ((char *)data)[j];
- #else
- lineData[j] = ((char *)data)[j];
- #endif
- }
- #else
- #ifdef CANOPEN_BIG_ENDIAN
- if(dataType == 0 && endianize)
- d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
- else
- d->transfers[line].data[j] = ((char *)data)[j];
- #else
- d->transfers[line].data[j] = ((char *)data)[j];
- #endif
- #endif
- }
- /* 块下载 */
- if(useBlockMode)
- {
- /* 块下载:110 */
- buf[0] = (6 << 5) | (1 << 1);
- /* 字节数 */
- for(i = 0; i < 4; i++)
- {
- buf[i + 4] = (UNS8)((count >> (i << 3)));
- }
- }
- /* 域下载 */
- else
- {
- /* 字节数少于4字节,加速下载 */
- if(count <= 4)
- {
- /* 下载:001 + 无意义字节数 + 加速下载 + 指明数据长度 */
- buf[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3);
- /* 拷贝数据 */
- for(i = 4; i < 8; i++)
- buf[i] = d->transfers[line].data[i - 4];
- d->transfers[line].offset = count;
- }
- /* 字节数大于4字节,正常下载 */
- else
- {
- /* 下载:001 + 数据字节为字节计数器 */
- buf[0] = (1 << 5) | 1;
- /* 数据字节数 */
- for(i = 0; i < 4; i++)
- buf[i + 4] = (UNS8)((count >> (i << 3)));
- }
- }
- /* 索引 */
- buf[1] = index & 0xFF;
- buf[2] = (index >> 8) & 0xFF;
- /* 子索引 */
- buf[3] = subIndex;
- /* 注册传输完成回调函数 */
- d->transfers[line].Callback = Callback;
- /* 发送sdo报文 */
- err = sendSDO(d, SDO_CLIENT, CliNbr, buf);
- if(err)
- {
- MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
- /* 复位该sdo传输通道 */
- resetSDOline(d, line);
- return 0xFF;
- }
- return 0;
- }
- /* 写服务器字典 */
- UNS8 writeNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex,
- UNS32 count, UNS8 dataType, void *data, UNS8 useBlockMode)
- {
- return _writeNetworkDict(d, nodeId, index, subIndex, count, dataType, data, NULL, 1, useBlockMode);
- }
- /* 写服务器字典,写完调用回调函数 */
- UNS8 writeNetworkDictCallBack(CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS32 count,
- UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 useBlockMode)
- {
- return _writeNetworkDict(d, nodeId, index, subIndex, count, dataType, data, Callback, 1, useBlockMode);
- }
- /* 自动初始化客户端,并且写服务器字典 */
- UNS8 writeNetworkDictCallBackAI(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS32 count, UNS8 dataType,
- void *data, SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode)
- {
- UNS8 ret;
- UNS16 lastIndex;
- UNS16 offset;
- UNS8 nodeIdServer;
- /* 写服务器字典 */
- ret = _writeNetworkDict(d, nodeId, index, subIndex, count, dataType, data, Callback, endianize, useBlockMode);
- /* 如果写失败 */
- if(ret == 0xFE)
- {
- /* 自动初始化客户端,并且写字典 */
- offset = d->firstIndex->SDO_CLT;
- lastIndex = d->lastIndex->SDO_CLT;
- if(offset == 0)
- {
- MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0);
- return 0xFF;
- }
- while(offset <= lastIndex)
- {
- if(d->objdict[offset].bSubCount <= 3)
- {
- MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + i);
- return 0xFF;
- }
- nodeIdServer = *(UNS8*) d->objdict[offset].pSubindex[3].pObject;
- if(nodeIdServer == 0)
- {
- *(UNS32*)d->objdict[offset].pSubindex[1].pObject = (UNS32)(0x600 + nodeId);
- *(UNS32*)d->objdict[offset].pSubindex[2].pObject = (UNS32)(0x580 + nodeId);
- *(UNS8*) d->objdict[offset].pSubindex[3].pObject = nodeId;
- return _writeNetworkDict(d, nodeId, index, subIndex, count, dataType, data, Callback, endianize, useBlockMode);
- }
- offset++;
- }
- return 0xFF;
- }
- else if(ret == 0)
- {
- return 0;
- }
- else
- {
- return 0xFF;
- }
- }
- /* 读服务器字典 */
- INLINE UNS8 _readNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode)
- {
- UNS8 err;
- UNS8 i;
- UNS8 CliNbr;
- UNS8 line;
- UNS8 data[8];
- MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
- MSG_WAR(0x3AD6, " At index : ", index);
- MSG_WAR(0x3AD7, " subIndex : ", subIndex);
- /* 通过节点号查找客户端号 */
- CliNbr = GetSDOClientFromNodeId(d, nodeId);
- if(CliNbr >= 0xFE)
- return CliNbr;
- /* 通过客户端/服务器号获取sdo传输通道号,检查客户端是否在通讯中 */
- err = getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line);
- if(!err)
- {
- MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId);
- return 0xFF;
- }
- /* 获取空闲的sdo传输通道 */
- err = getSDOfreeLine(d, SDO_CLIENT, &line );
- if(err)
- {
- MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
- return (0xFF);
- }
- else
- MSG_WAR(0x3AE0, "Transmission on line : ", line);
- /* 块传输模式 */
- if(useBlockMode)
- {
- /* 初始化sdo传输通道 */
- initSDOline(d, line, CliNbr, index, subIndex, SDO_BLOCK_UPLOAD_IN_PROGRESS);
- /* 设置数据类型 */
- d->transfers[line].dataType = dataType;
- /* 块上传初始化 */
- data[0] = (5 << 5) | SDO_BCS_INITIATE_UPLOAD_REQUEST;
- /* 索引 */
- data[1] = index & 0xFF;
- data[2] = (index >> 8) & 0xFF;
- /* 子索引 */
- data[3] = subIndex;
- /* 块数量 */
- data[4] = SDO_BLOCK_SIZE;
- /* 不足8字节部分填0 */
- for(i = 5; i < 8; i++)
- data[i] = 0;
- }
- else
- {
- /* 初始化sdo传输通道 */
- initSDOline(d, line, CliNbr, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
- /* 设置数据类型 */
- d->transfers[line].dataType = dataType;
- /* 启动域上传 */
- data[0] = (2 << 5);
- /* 索引 */
- data[1] = index & 0xFF;
- data[2] = (index >> 8) & 0xFF;
- /* 子索引 */
- data[3] = subIndex;
- /* 不足8字节部分填0 */
- for(i = 4; i < 8; i++)
- data[i] = 0;
- }
- /* 注册传输完成回调函数 */
- d->transfers[line].Callback = Callback;
- /* 发送sdo数据包 */
- err = sendSDO(d, SDO_CLIENT, CliNbr, data);
- if(err)
- {
- MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
- resetSDOline(d, line);
- return 0xFF;
- }
- return 0;
- }
- /* 读服务器字典 */
- UNS8 readNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, UNS8 useBlockMode)
- {
- return _readNetworkDict(d, nodeId, index, subIndex, dataType, NULL, useBlockMode);
- }
- /* 读服务器字典,完成调用回调函数 */
- UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode)
- {
- return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback, useBlockMode);
- }
- /* 自动初始化客户端,并且读服务器字典 */
- UNS8 readNetworkDictCallbackAI(CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode)
- {
- UNS8 ret;
- UNS16 lastIndex;
- UNS16 offset;
- UNS8 nodeIdServer;
- /* 读服务器字典 */
- ret = _readNetworkDict(d, nodeId, index, subIndex, dataType, Callback, useBlockMode);
- /* 如果读失败 */
- if(ret == 0xFE)
- {
- /* 自动初始化客户端,并且读字典 */
- offset = d->firstIndex->SDO_CLT;
- lastIndex = d->lastIndex->SDO_CLT;
- if(offset == 0)
- {
- MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0);
- return 0xFF;
- }
- while(offset <= lastIndex)
- {
- if (d->objdict[offset].bSubCount <= 3)
- {
- MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + i);
- return 0xFF;
- }
- nodeIdServer = *(UNS8*)d->objdict[offset].pSubindex[3].pObject;
- if(nodeIdServer == 0)
- {
- *(UNS32*)d->objdict[offset].pSubindex[1].pObject = (UNS32)(0x600 + nodeId);
- *(UNS32*)d->objdict[offset].pSubindex[2].pObject = (UNS32)(0x580 + nodeId);
- *(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId;
- return _readNetworkDict(d, nodeId, index, subIndex, dataType, Callback, useBlockMode);
- }
- offset++;
- }
- return 0xFF;
- }
- else if(ret == 0)
- {
- return 0;
- }
- else
- {
- return 0xFF;
- }
- }
- /* 获取读服务器字典结果 */
- UNS8 getReadResultNetworkDict(CO_Data *d, UNS8 nodeId, void *data, UNS32 *size, UNS32 *abortCode)
- {
- UNS32 i;
- UNS8 err;
- UNS8 CliNbr;
- UNS8 line;
- *abortCode = 0;
- /* 通过节点号查找客户端号 */
- CliNbr = GetSDOClientFromNodeId(d, nodeId);
- if(CliNbr >= 0xFE)
- {
- *size = 0;
- return SDO_ABORTED_INTERNAL;
- }
- /* 通过客户端/服务器号获取sdo传输通道号 */
- err = getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line);
- if(err)
- {
- MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId);
- *size = 0;
- return SDO_ABORTED_INTERNAL;
- }
- /* 如果写字典未完成,则返回传输通道状态 */
- if(d->transfers[line].state != SDO_FINISHED)
- {
- if((d->transfers[line].state == SDO_ABORTED_RCV) || (d->transfers[line].state == SDO_ABORTED_INTERNAL))
- {
- *abortCode = d->transfers[line].abortCode;
- *size = 0;
- }
- return d->transfers[line].state;
- }
- /* 检查数据传输是否完整 */
- if(d->transfers[line].count == 0)
- d->transfers[line].count = d->transfers[line].offset;
- if(*size < d->transfers[line].count)
- {
- *size = 0;
- return SDO_PROVIDED_BUFFER_TOO_SMALL;
- }
- /* 把字节数传回去 */
- *size = d->transfers[line].count;
- /* 将数据传回去 */
- #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
- {
- UNS8 *lineData = d->transfers[line].data;
- if(d->transfers[line].dynamicData && d->transfers[line].dynamicDataSize)
- {
- lineData = d->transfers[line].dynamicData;
- }
- for(i = 0; i < *size; i++)
- {
- #ifdef CANOPEN_BIG_ENDIAN
- if(d->transfers[line].dataType != visible_string)
- ((char *)data)[*size - 1 - i] = lineData[i];
- else
- ((char *)data)[i] = lineData[i];
- #else
- ((char *)data)[i] = lineData[i];
- #endif
- }
- }
- #else
- for(i = 0 ; i < *size; i++)
- {
- #ifdef CANOPEN_BIG_ENDIAN
- if(d->transfers[line].dataType != visible_string)
- ((char *)data)[*size - 1 - i] = d->transfers[line].data[i];
- else
- ((char *)data)[i] = d->transfers[line].data[i];
- #else
- ((char *)data)[i] = d->transfers[line].data[i];
- #endif
- }
- #endif
- /* 复位sdo传输通道 */
- resetSDOline(d, line);
- /* 返回传输完成 */
- return SDO_FINISHED;
- }
- /* 获取写服务器字典结果 */
- UNS8 getWriteResultNetworkDict(CO_Data *d, UNS8 nodeId, UNS32 *abortCode)
- {
- UNS8 line = 0;
- UNS8 err;
- UNS8 CliNbr;
- *abortCode = 0;
- /* 通过节点号查找客户端号 */
- CliNbr = GetSDOClientFromNodeId(d, nodeId);
- if(CliNbr >= 0xFE)
- return SDO_ABORTED_INTERNAL;
- /* 通过客户端/服务器号获取sdo传输通道号 */
- err = getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line);
- if(err)
- {
- MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId);
- return SDO_ABORTED_INTERNAL;
- }
- /* 中止码 */
- *abortCode = d->transfers[line].abortCode;
- /* 如果写字典未完成,则返回传输通道状态 */
- if(d->transfers[line].state != SDO_FINISHED)
- return d->transfers[line].state;
- /* 复位sdo传输通道 */
- resetSDOline(d, line);
- /* 返回传输完成 */
- return SDO_FINISHED;
- }