CANOpen服务数据对象报文

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

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

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

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

SDO报文格式如下:

CANOpen服务数据对象报文_第1张图片



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

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

CANOpen服务数据对象报文_第2张图片

CANOpen服务数据对象报文_第3张图片

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


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


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


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


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


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


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



[cpp]  view plain  copy
  1. #include   
  2. #include "canfestival.h"  
  3. #include "sysdep.h"  
  4.   
  5. #define NO_INLINE  
  6.   
  7. #ifdef NO_INLINE  
  8. #define INLINE  
  9. #else  
  10. #define INLINE inline  
  11. #endif  
  12.   
  13. /* 通过节点号查找客户端号 */  
  14. UNS8 GetSDOClientFromNodeId(CO_Data *d, UNS8 nodeId);  
  15.   
  16. /* 写服务器字典 */  
  17. INLINE UNS8 _writeNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex,   
  18.                                                             UNS32 count, UNS8 dataType, void *data,   
  19.                                                             SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode);  
  20.   
  21. /* 读服务器字典 */  
  22. INLINE UNS8 _readNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode);  
  23.   
  24. #define getSDOcs(byte) (byte >> 5)  
  25. #define getSDOn2(byte) ((byte >> 2) & 3)  
  26. #define getSDOn3(byte) ((byte >> 1) & 7)  
  27. #define getSDOe(byte) ((byte >> 1) & 1)  
  28. #define getSDOs(byte) (byte & 1)  
  29. #define getSDOc(byte) (byte & 1)  
  30. #define getSDOt(byte) ((byte >> 4) & 1)  
  31. #define getSDOindex(byte1, byte2) (((UNS16)byte2 << 8) | ((UNS16)byte1))  
  32. #define getSDOsubIndex(byte3) (byte3)  
  33. #define getSDOblockSC(byte) (byte & 3)  
  34.   
  35.   
  36. /* sdo传输超时回调函数 */  
  37. void SDOTimeoutAlarm(CO_Data *d, UNS32 id)  
  38. {  
  39.     UNS16 offset;  
  40.     UNS8 nodeId;  
  41.       
  42.     /* 第一个sdo客户端在字典中的下标 */  
  43.     offset = d->firstIndex->SDO_CLT;  
  44.     if((offset == 0) || ((offset + d->transfers[id].CliServNbr) > d->lastIndex->SDO_CLT))   
  45.     {  
  46.         return;  
  47.     }  
  48.       
  49.     /* 从字典中取出服务器节点id */  
  50.     nodeId = (UNS8)*((UNS32*)d->objdict[offset + d->transfers[id].CliServNbr].pSubindex[3].pObject);  
  51.       
  52.     MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);  
  53.     MSG_WAR(0x2A02, "server node id : ", nodeId);  
  54.     MSG_WAR(0x2A02, "         index : ", d->transfers[id].index);  
  55.     MSG_WAR(0x2A02, "      subIndex : ", d->transfers[id].subIndex);  
  56.       
  57.     /* 传输通道超时定时器置空 */  
  58.     d->transfers[id].timer = TIMER_NONE;  
  59.     /* 传输通道状态设置为内部中止 */  
  60.     d->transfers[id].state = SDO_ABORTED_INTERNAL;  
  61.     /* 向主站发送中止报文 */  
  62.     sendSDOabort(d, d->transfers[id].whoami, d->transfers[id].CliServNbr, d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);  
  63.     /* 错误码设置为超时 */  
  64.     d->transfers[id].abortCode = SDOABT_TIMED_OUT;  
  65.       
  66.     /* 如果传输通道超时,则调用回调函数 */  
  67.     if(d->transfers[id].Callback)  
  68.         (*d->transfers[id].Callback)(d, nodeId);  
  69.   
  70.     /* 复位sdo传输通道 */  
  71.     if(d->transfers[id].abortCode == SDOABT_TIMED_OUT)   
  72.         resetSDOline(d, (UNS8)id);  
  73. }  
  74.   
  75. /* 停止sdo超时定时器 */  
  76. #define StopSDO_TIMER(id) \  
  77.     MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\  
  78. d->transfers[id].timer = DelAlarm(d->transfers[id].timer);  
  79.   
  80. /* 启动sdo超时定时器 */  
  81. #define StartSDO_TIMER(id) \  
  82.     MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\  
  83.     d->transfers[id].timer = SetAlarm(d, id, &SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0);  
  84.   
  85. /* 重启sdo超时定时器 */  
  86. #define RestartSDO_TIMER(id) \  
  87.     MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\  
  88. if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) }  
  89.   
  90. /* 复位所有sdo传输通道 */  
  91. void resetSDO(CO_Data *d)  
  92. {  
  93.     UNS8 j;  
  94.   
  95.     /* 复位所有sdo传输通道 */  
  96.     for(j = 0; j < SDO_MAX_SIMULTANEOUS_TRANSFERS; j++)  
  97.     {  
  98.         resetSDOline(d, j);  
  99.     }  
  100. }  
  101.   
  102. /* 将数据从传输通道缓冲区拷贝到字典中 */  
  103. UNS32 SDOlineToObjdict(CO_Data *d, UNS8 line)  
  104. {  
  105.     UNS32 size;  
  106.     UNS32 errorCode;  
  107.       
  108.     MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);  
  109.       
  110.     /* 确定该传输通道传输了多少字节 */  
  111.     if(d->transfers[line].count == 0)  
  112.     {  
  113.         d->transfers[line].count = d->transfers[line].offset;  
  114.     }  
  115.     size = d->transfers[line].count;  
  116.   
  117.     /* 将数据拷贝到字典中 */  
  118. #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION  
  119.     if(size > SDO_MAX_LENGTH_TRANSFER)  
  120.     {  
  121.         errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,   
  122.                                                      (void *)d->transfers[line].dynamicData, &size, 1);  
  123.     }  
  124.     else  
  125.     {  
  126.         errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,  
  127.                                                      (void *)d->transfers[line].data, &size, 1);  
  128.     }  
  129. #else  
  130.     errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,  
  131.                                                  (void *)d->transfers[line].data, &size, 1);  
  132. #endif  
  133.     if(errorCode != OD_SUCCESSFUL)  
  134.         return errorCode;  
  135.   
  136.     MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);  
  137.   
  138.     return 0;  
  139.   
  140. }  
  141.   
  142. /* 将数据从字典中拷贝到传输通道缓冲区 */  
  143. UNS32 objdictToSDOline(CO_Data *d, UNS8 line)  
  144. {  
  145.     UNS32 size = SDO_MAX_LENGTH_TRANSFER;  
  146.     UNS8 dataType;  
  147.     UNS32 errorCode;  
  148.   
  149.     MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index);  
  150.     MSG_WAR(0x3A06, "  subIndex : ", d->transfers[line].subIndex);  
  151.   
  152.     /* 支持动态内存分配 */  
  153. #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION  
  154.     /* 从字典中将数据拷贝出来 */  
  155.     errorCode = getODentry(d, d->transfers[line].index, d->transfers[line].subIndex,   
  156.                                                  (void *)d->transfers[line].data, &size, &dataType, 1);  
  157.     /* 如果内存不够拷贝 */  
  158.     if(errorCode == SDOABT_OUT_OF_MEMORY)   
  159.     {  
  160.         if(size <= SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE)   
  161.         {  
  162.             /* 动态分配内存 */  
  163.             d->transfers[line].dynamicData = (UNS8 *)malloc(size * sizeof(UNS8));  
  164.             if(d->transfers[line].dynamicData != NULL)   
  165.             {  
  166.                 d->transfers[line].dynamicDataSize = size;  
  167.                 /* 从字典中将数据拷贝出来 */  
  168.                 errorCode = getODentry(d, d->transfers[line].index, d->transfers[line].subIndex, (void *)d->transfers[line].dynamicData,  
  169.                                                              &d->transfers[line].dynamicDataSize, &dataType, 1);  
  170.             }  
  171.         }  
  172.     }  
  173. #else  
  174.     /* 从字典中将数据拷贝出来 */  
  175.     errorCode = getODentry(d, d->transfers[line].index, d->transfers[line].subIndex,   
  176.                                                  (void *)d->transfers[line].data, &size, &dataType, 1);  
  177. #endif  
  178.     if(errorCode != OD_SUCCESSFUL)  
  179.         return errorCode;  
  180.   
  181.     d->transfers[line].count = size;  
  182.     d->transfers[line].offset = 0;  
  183.   
  184.     return 0;  
  185. }  
  186.   
  187. /* 将数据从传输通道缓冲区拷贝出来 */  
  188. UNS8 lineToSDO(CO_Data *d, UNS8 line, UNS32 nbBytes, UNS8 *data)   
  189. {  
  190.     UNS8 i;  
  191.     UNS32 offset;  
  192.   
  193. #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION  
  194.     /* 如果不支持内存动态分配,并且字节数大于静态缓冲区,则报错 */  
  195.     if((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFER)   
  196.     {  
  197.         MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFER", nbBytes);  
  198.         return 0xFF;  
  199.     }  
  200. #endif  
  201.   
  202.     /* 如果传输通道字节数大于配置的大小 */  
  203.     if((d->transfers[line].offset + nbBytes) > d->transfers[line].count)   
  204.     {  
  205.         MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);  
  206.         return 0xFF;  
  207.     }  
  208.     offset = d->transfers[line].offset;  
  209. #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION  
  210.     /* 如果说sdo传输通道字节数小于静态缓冲区 */  
  211.     if(d->transfers[line].count <= SDO_MAX_LENGTH_TRANSFER)  
  212.     {  
  213.         /* 将数据从静态缓冲区拷贝出来 */  
  214.         for(i = 0; i < nbBytes; i++)  
  215.             *(data + i) = d->transfers[line].data[offset + i];  
  216.     }  
  217.     /* 如果说sdo传输通道字节数大于静态缓冲区 */  
  218.     else  
  219.     {  
  220.         if(d->transfers[line].dynamicData == NULL)  
  221.         {  
  222.             MSG_ERR(0x1A11,"SDO's dynamic buffer not allocated. Line", line);  
  223.             return 0xFF;  
  224.         }  
  225.           
  226.         /* 将数据从动态缓冲区拷贝出来 */  
  227.         for(i = 0; i < nbBytes; i++)  
  228.             *(data + i) = d->transfers[line].dynamicData[offset + i];  
  229.     }  
  230. #else  
  231.     /* 将数据拷贝出来 */  
  232.     for(i = 0; i < nbBytes; i++)  
  233.         *(data + i) = d->transfers[line].data[offset + i];  
  234. #endif  
  235.     /* 偏移量增大 */  
  236.     d->transfers[line].offset = d->transfers[line].offset + nbBytes;  
  237.       
  238.     return 0;  
  239. }  
  240.   
  241. /* 将数据拷贝到传输通道缓冲区 */  
  242. UNS8 SDOtoLine(CO_Data *d, UNS8 line, UNS32 nbBytes, UNS8 *data)  
  243. {  
  244.     UNS8 i;  
  245.     UNS32 offset;  
  246.   
  247. #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION  
  248.     /* 不允许动态分配时数据量不能大于缓冲区 */  
  249.     if((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFER)   
  250.     {  
  251.         MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFER", nbBytes);  
  252.         return 0xFF;  
  253.     }  
  254. #endif  
  255.   
  256.     /* 缓冲区偏移量 */  
  257.     offset = d->transfers[line].offset;  
  258.       
  259. #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION  
  260.     {  
  261.         UNS8 *lineData = d->transfers[line].data;  
  262.           
  263.         /* 当数据量大于缓冲区时 */  
  264.         if((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFER)   
  265.         {  
  266.             /* 如果还没有动态分配内存 */  
  267.             if(d->transfers[line].dynamicData == NULL)   
  268.             {  
  269.                 /* 动态分配内存 */  
  270.                 d->transfers[line].dynamicData = (UNS8 *)malloc(SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);  
  271.                 d->transfers[line].dynamicDataSize = SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE;  
  272.                 if(d->transfers[line].dynamicData == NULL)   
  273.                 {  
  274.                     MSG_ERR(0x1A15,"SDO allocating dynamic buffer failed, size", SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);  
  275.                     return 0xFF;  
  276.                 }  
  277.                   
  278.                 /* 将静态缓冲区数据拷贝到动态内存区 */  
  279.                 memcpy(d->transfers[line].dynamicData, d->transfers[line].data, offset);  
  280.             }  
  281.             /* 如果已经动态分配内存,则要重新分配 */  
  282.             else if((d->transfers[line].offset + nbBytes) > d->transfers[line].dynamicDataSize)  
  283.             {  
  284.                 UNS8 *newDynamicBuffer = (UNS8*)realloc(d->transfers[line].dynamicData, d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);  
  285.                 if(newDynamicBuffer == NULL)   
  286.                 {  
  287.                     MSG_ERR(0x1A15,"SDO reallocating dynamic buffer failed, size", d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);  
  288.                     return 0xFF;  
  289.                 }  
  290.                 d->transfers[line].dynamicData = newDynamicBuffer;  
  291.                 d->transfers[line].dynamicDataSize += SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE;  
  292.             }  
  293.             lineData = d->transfers[line].dynamicData;  
  294.         }  
  295.   
  296.         /* 将数据拷贝到缓冲区 */  
  297.         for(i = 0; i < nbBytes; i++)  
  298.         {  
  299.             lineData[offset + i] = *(data + i);  
  300.       
  301.         }  
  302.     }  
  303. #else  
  304.     /* 将数据拷贝到缓冲区 */  
  305.     for (i = 0; i < nbBytes; i++)  
  306.         d->transfers[line].data[offset + i] = *(data + i);  
  307. #endif  
  308.   
  309.     /* 缓冲区偏移量增加 */  
  310.     d->transfers[line].offset = d->transfers[line].offset + nbBytes;  
  311.       
  312.     return 0;  
  313. }  
  314.   
  315. /* sdo传输通道失败 */  
  316. UNS8 failedSDO(CO_Data *d, UNS8 CliServNbr, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode)  
  317. {  
  318.     UNS8 err;  
  319.     UNS8 line;  
  320.       
  321.     /* 通过客户端/服务器号获取sdo传输通道号 */  
  322.     err = getSDOlineOnUse(d, CliServNbr, whoami, &line);  
  323.     if(!err)  
  324.     {  
  325.         MSG_WAR(0x3A20, "FailedSDO : line found : ", line);  
  326.     }  
  327.       
  328.     /* 如果自身是服务器,直接复位传输通道 */  
  329.     if((!err) && (whoami == SDO_SERVER))   
  330.     {  
  331.         resetSDOline(d, line);  
  332.         MSG_WAR(0x3A21, "FailedSDO : line released : ", line);  
  333.     }  
  334.     /* 如果自身是客户端,则停止sdo传输超时定时器并将状态置为中止并设置错误码 */  
  335.     if((!err) && (whoami == SDO_CLIENT))   
  336.     {  
  337.         StopSDO_TIMER(line);  
  338.         d->transfers[line].state = SDO_ABORTED_INTERNAL;  
  339.         d->transfers[line].abortCode = abortCode;  
  340.     }  
  341.       
  342.     MSG_WAR(0x3A22, "Sending SDO abort ", 0);  
  343.       
  344.     /* 发送sdo中止报文 */  
  345.     err = sendSDOabort(d, whoami, CliServNbr, index, subIndex, abortCode);  
  346.     if(err)   
  347.     {  
  348.         MSG_WAR(0x3A23, "Unable to send the SDO abort", 0);  
  349.         return 0xFF;  
  350.     }  
  351.   
  352.     return 0;  
  353. }  
  354.   
  355. /* 复位sdo通道 */  
  356. void resetSDOline(CO_Data *d, UNS8 line)  
  357. {  
  358.     UNS32 i;  
  359.       
  360.     MSG_WAR(0x3A25, "reset SDO line nb : ", line);  
  361.       
  362.     /* 初始化sdo通道 */  
  363.     initSDOline(d, line, 0, 0, 0, SDO_RESET);  
  364.       
  365.     /* 将sdo传输通道缓冲区清空 */  
  366.     for(i = 0; i < SDO_MAX_LENGTH_TRANSFER; i++)  
  367.         d->transfers[line].data[i] = 0;  
  368.       
  369.     /* 传输通道归属(服务器/客户端)清空 */  
  370.     d->transfers[line].whoami = 0;  
  371.       
  372.     /* 传输通道错误码清空 */  
  373.     d->transfers[line].abortCode = 0;  
  374. }  
  375.   
  376. /* 初始化sdo传输通道 */  
  377. UNS8 initSDOline(CO_Data *d, UNS8 line, UNS8 CliServNbr, UNS16 index, UNS8 subIndex, UNS8 state)  
  378. {  
  379.     MSG_WAR(0x3A25, "init SDO line nb : ", line);  
  380.       
  381.     /* 判断是否该SDO传输通道在域下载/域上传/块下载/块上传过程中 */  
  382.     if(state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS ||   
  383.          state == SDO_BLOCK_DOWNLOAD_IN_PROGRESS || state == SDO_BLOCK_UPLOAD_IN_PROGRESS)  
  384.     {  
  385.         /* 开启sdo定时器,通讯超时做相应处理 */  
  386.         StartSDO_TIMER(line)  
  387.     }  
  388.     /* 不在传输过程中,关闭定时器 */  
  389.     else  
  390.     {  
  391.         StopSDO_TIMER(line)  
  392.     }  
  393.   
  394.     /* 初始化客户端/服务器号 */  
  395.     d->transfers[line].CliServNbr = CliServNbr;  
  396.     /* 初始化对象索引 */  
  397.     d->transfers[line].index = index;  
  398.     /* 初始化对象子索引 */  
  399.     d->transfers[line].subIndex = subIndex;  
  400.     /* 初始化传输通道状态 */  
  401.     d->transfers[line].state = state;  
  402.     d->transfers[line].toggle = 0;  
  403.     /* 初始化字节数 */  
  404.     d->transfers[line].count = 0;  
  405.     d->transfers[line].offset = 0;  
  406.     d->transfers[line].peerCRCsupport = 0;  
  407.     d->transfers[line].blksize = 0;  
  408.     d->transfers[line].ackseq = 0;  
  409.     d->transfers[line].objsize = 0;  
  410.     d->transfers[line].lastblockoffset = 0;  
  411.     d->transfers[line].seqno = 0;  
  412.     d->transfers[line].endfield = 0;  
  413.     /* 初始化块传输接收状态 */  
  414.     d->transfers[line].rxstep = RXSTEP_INIT;  
  415.     d->transfers[line].dataType = 0;  
  416.     /* 超时回调函数 */  
  417.     d->transfers[line].Callback = NULL;  
  418. #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION  
  419.     free(d->transfers[line].dynamicData);  
  420.     d->transfers[line].dynamicData = 0;  
  421.     d->transfers[line].dynamicDataSize = 0;  
  422. #endif  
  423.     return 0;  
  424. }  
  425.   
  426. /* 获取空闲的sdo传输通道 */  
  427. UNS8 getSDOfreeLine(CO_Data *d, UNS8 whoami, UNS8 *line)  
  428. {  
  429.     UNS8 i;  
  430.   
  431.     /* 遍历所有传输通道,查找空闲的传输通道 */  
  432.     for(i = 0; i < SDO_MAX_SIMULTANEOUS_TRANSFERS; i++)  
  433.     {  
  434.         if(d->transfers[i].state == SDO_RESET)   
  435.         {  
  436.             *line = i;  
  437.             d->transfers[i].whoami = whoami;  
  438.             return 0;  
  439.         }  
  440.     }  
  441.       
  442.     MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);  
  443.   
  444.     return 0xFF;  
  445. }  
  446.   
  447. /* 通过客户端/服务器号获取sdo传输通道号 */  
  448. UNS8 getSDOlineOnUse(CO_Data *d, UNS8 CliServNbr, UNS8 whoami, UNS8 *line)  
  449. {  
  450.     UNS8 i;  
  451.   
  452.     /* 遍历所有sdo传输通道 */  
  453.     for(i = 0; i < SDO_MAX_SIMULTANEOUS_TRANSFERS; i++)  
  454.     {  
  455.         /* 如果该通道处于正常状态并且客户端/服务器号匹配,则说明找到了该通道 */  
  456.         if((d->transfers[i].state != SDO_RESET) &&  
  457.              (d->transfers[i].state != SDO_ABORTED_INTERNAL) &&  
  458.              (d->transfers[i].CliServNbr == CliServNbr) &&  
  459.              (d->transfers[i].whoami == whoami))   
  460.         {  
  461.             /* 获取该通道号 */  
  462.             if(line)  
  463.             {                 
  464.                 *line = i;  
  465.             }  
  466.               
  467.             return 0;  
  468.         }  
  469.     }  
  470.   
  471.     return 0xFF;  
  472. }  
  473.   
  474. /* 通过客户端/服务器号获取sdo传输通道号 */  
  475. UNS8 getSDOlineToClose(CO_Data *d, UNS8 CliServNbr, UNS8 whoami, UNS8 *line)  
  476. {  
  477.     UNS8 i;  
  478.   
  479.     for(i = 0; i < SDO_MAX_SIMULTANEOUS_TRANSFERS; i++)  
  480.     {  
  481.         if((d->transfers[i].state != SDO_RESET) &&  
  482.              (d->transfers[i].CliServNbr == CliServNbr) &&  
  483.              (d->transfers[i].whoami == whoami))   
  484.         {  
  485.             if(line)   
  486.                 *line = i;  
  487.               
  488.             return 0;  
  489.         }  
  490.     }  
  491.       
  492.     return 0xFF;  
  493. }  
  494.   
  495. /* 关闭sdo传输通道 */  
  496. UNS8 closeSDOtransfer(CO_Data *d, UNS8 nodeId, UNS8 whoami)  
  497. {  
  498.     UNS8 err;  
  499.     UNS8 line;  
  500.     UNS8 CliNbr;  
  501.       
  502.     /* 通过节点号查找客户端号 */  
  503.     CliNbr = GetSDOClientFromNodeId(d, nodeId);  
  504.     if(CliNbr >= 0xFE)  
  505.         return SDO_ABORTED_INTERNAL;  
  506.       
  507.     /* 通过客户端/服务器号获取sdo传输通道号 */  
  508.     err = getSDOlineToClose(d, CliNbr, whoami, &line);  
  509.     if(err)   
  510.     {  
  511.         MSG_WAR(0x2A30, "No SDO communication to close", 0);  
  512.         return 0xFF;  
  513.     }  
  514.   
  515.     /* 复位sdo通道 */  
  516.     resetSDOline(d, line);  
  517.       
  518.     return 0;  
  519. }  
  520.   
  521. /* 获取传输通道剩余字节数 */  
  522. UNS8 getSDOlineRestBytes(CO_Data *d, UNS8 line, UNS32 *nbBytes)  
  523. {  
  524.     if(d->transfers[line].count == 0)  
  525.         *nbBytes = 0;  
  526.     else  
  527.         *nbBytes = d->transfers[line].count - d->transfers[line].offset;  
  528.     return 0;  
  529. }  
  530.   
  531. /* sdo传输通道剩余字节数 */  
  532. UNS8 setSDOlineRestBytes(CO_Data *d, UNS8 line, UNS32 nbBytes)  
  533. {  
  534. #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION  
  535.     if(nbBytes > SDO_MAX_LENGTH_TRANSFER)   
  536.     {  
  537.         MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFER", nbBytes);  
  538.         return 0xFF;  
  539.     }  
  540. #endif  
  541.     d->transfers[line].count = nbBytes;  
  542.   
  543.     return 0;  
  544. }  
  545.   
  546. /* 发送sdo报文 */  
  547. UNS8 sendSDO(CO_Data *d, UNS8 whoami, UNS8 CliServNbr, UNS8 *pData)  
  548. {  
  549.     UNS16 offset;  
  550.     UNS8 i;  
  551.     Message m;  
  552.   
  553.     MSG_WAR(0x3A38, "sendSDO",0);  
  554.   
  555.     /* 运行态或预运行态才可以使用sdo报文 */  
  556.     if(!((d->nodeState == Operational) || (d->nodeState == Pre_operational)))   
  557.     {  
  558.         MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState);  
  559.         return 0xFF;  
  560.     }  
  561.   
  562.     /* 服务器端发送sdo报文 */  
  563.     if(whoami == SDO_SERVER)      
  564.     {  
  565.         /* 取出sdo服务器端的发送cob_id */  
  566.         offset = d->firstIndex->SDO_SVR;  
  567.         if((offset == 0) || ((offset + CliServNbr) > d->lastIndex->SDO_SVR))   
  568.         {  
  569.             MSG_ERR(0x1A42, "SendSDO : SDO server not found", 0);  
  570.             return 0xFF;  
  571.         }  
  572.         m.cob_id = (UNS16)*((UNS32*)d->objdict[offset + CliServNbr].pSubindex[2].pObject);  
  573.         MSG_WAR(0x3A41, "I am server Tx cobId : ", m.cob_id);  
  574.     }  
  575.     /* 客户端发送sdo报文 */  
  576.     else   
  577.     {  
  578.         /* 取出sdo客户端发送cob_id */  
  579.         offset = d->firstIndex->SDO_CLT;  
  580.         if((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_CLT))   
  581.         {  
  582.             MSG_ERR(0x1A42, "SendSDO : SDO client not found", 0);  
  583.             return 0xFF;  
  584.         }  
  585.         m.cob_id = (UNS16)*((UNS32*)d->objdict[offset + CliServNbr].pSubindex[1].pObject);  
  586.         MSG_WAR(0x3A41, "I am client Tx cobId : ", m.cob_id);  
  587.     }  
  588.     /* 数据帧 */  
  589.     m.rtr = NOT_A_REQUEST;  
  590.     /* sdo报文固定8字节 */  
  591.     m.len = 8;  
  592.     /* 数据 */  
  593.     for(i = 0; i < 8; i++)   
  594.     {  
  595.         m.data[i] =  pData[i];  
  596.     }  
  597.       
  598.     /* 发送报文 */  
  599.     return canSend(d->canHandle,&m);  
  600. }  
  601.   
  602. /* 发送sdo中止报文 */  
  603. UNS8 sendSDOabort(CO_Data* d, UNS8 whoami, UNS8 CliServNbr, UNS16 index, UNS8 subIndex, UNS32 abortCode)  
  604. {  
  605.     UNS8 data[8];  
  606.     UNS8 ret;  
  607.   
  608.     MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);  
  609.   
  610.     data[0] = 0x80;  
  611.     data[1] = index & 0xFF;  
  612.     data[2] = (index >> 8) & 0xFF;  
  613.     data[3] = subIndex;  
  614.     data[4] = (UNS8)(abortCode & 0xFF);  
  615.     data[5] = (UNS8)((abortCode >> 8) & 0xFF);  
  616.     data[6] = (UNS8)((abortCode >> 16) & 0xFF);  
  617.     data[7] = (UNS8)((abortCode >> 24) & 0xFF);  
  618.   
  619.     /* 发送sdo报文 */  
  620.     ret = sendSDO(d, whoami, CliServNbr, data);  
  621.   
  622.     return ret;  
  623. }  
  624.   
  625. /* 处理sdo报文 */  
  626. UNS8 proceedSDO(CO_Data *d, Message *m)  
  627. {  
  628.     UNS8 err;  
  629.     UNS8 cs;  
  630.     UNS8 line;  
  631.     UNS32 nbBytes;  
  632.     UNS8 nodeId = 0;  
  633.     UNS8 CliServNbr;  
  634.     UNS8 whoami = SDO_UNKNOWN;  
  635.     UNS32 errorCode;  
  636.     UNS8 data[8];  
  637.     UNS16 index;  
  638.     UNS8 subIndex;  
  639.     UNS32 abortCode;  
  640.     UNS32 i;  
  641.     UNS8    j;  
  642.     UNS32 *pCobId = NULL;  
  643.     UNS16 offset;  
  644.     UNS16 lastIndex;  
  645.     UNS8 SubCommand;  
  646.   UNS8 SeqNo;  
  647.   UNS8 AckSeq;  
  648.     UNS8 NbBytesNoData;  
  649.   
  650.     MSG_WAR(0x3A60, "proceedSDO ", 0);  
  651.   
  652.     whoami = SDO_UNKNOWN;  
  653.       
  654.     /* 第一个sdo服务器在字典中的下标 */  
  655.     offset = d->firstIndex->SDO_SVR;  
  656.     /* 最后一个sdo服务器在字典中的下标 */  
  657.     lastIndex = d->lastIndex->SDO_SVR;  
  658.       
  659.     j = 0;  
  660.       
  661.     /* 如果字典中配置了sdo服务器 */  
  662.     if(offset)   
  663.     {  
  664.         /* 遍历所有sdo服务器 */  
  665.         while(offset <= lastIndex)   
  666.         {  
  667.             /* 服务器端子索引不能低于1个 */  
  668.             if(d->objdict[offset].bSubCount <= 1)   
  669.             {  
  670.                 MSG_ERR(0x1A61, "Subindex 1  not found at index ", 0x1200 + j);  
  671.                 return 0xFF;  
  672.             }  
  673.               
  674.             /* 取出服务器端的接收CobId */  
  675.             pCobId = (UNS32*)d->objdict[offset].pSubindex[1].pObject;  
  676.             /* 如果和接收到的报文匹配 */  
  677.             if(*pCobId == UNS16_LE(m->cob_id))   
  678.             {  
  679.                 /* 说明自己是服务器 */  
  680.                 whoami = SDO_SERVER;  
  681.                 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);  
  682.                 /* 客户端/服务器号 */  
  683.                 CliServNbr = j;  
  684.                 break;  
  685.             }  
  686.             j++;  
  687.             offset++;  
  688.         }  
  689.     }  
  690.       
  691.     /* 如果不是服务器,判断自己是不是客户端 */  
  692.     if(whoami == SDO_UNKNOWN)   
  693.     {  
  694.         /* 第一个客户端在字典中的下标 */  
  695.         offset = d->firstIndex->SDO_CLT;  
  696.         /* 最后一个客户端在字典中的下标 */  
  697.         lastIndex = d->lastIndex->SDO_CLT;  
  698.           
  699.         j = 0;  
  700.           
  701.         /* 如果配置了客户端 */  
  702.         if(offset)   
  703.         {  
  704.             /* 遍历所有sdo客户端 */  
  705.             while(offset <= lastIndex)   
  706.             {  
  707.                 /* 子索引不可以低于3个 */  
  708.                 if(d->objdict[offset].bSubCount <= 3)   
  709.                 {  
  710.                     MSG_ERR(0x1A63, "Subindex 3  not found at index ", 0x1280 + j);  
  711.                     return 0xFF;  
  712.                 }  
  713.                 /* 取出客户端的接收CobId */  
  714.                 pCobId = (UNS32*)d->objdict[offset].pSubindex[2].pObject;  
  715.                 /* 如果和接收到的报文匹配 */  
  716.                 if(*pCobId == UNS16_LE(m->cob_id))   
  717.                 {  
  718.                     /* 自己为客户端 */  
  719.                     whoami = SDO_CLIENT;  
  720.                       
  721.                     MSG_WAR(0x3A64, "proceedSDO. I am client index : ", 0x1280 + j);  
  722.                       
  723.                     /* 服务器/客户端号 */  
  724.                     CliServNbr = j;  
  725.                       
  726.                     /* 服务器端的节点号 */  
  727.                     nodeId = *((UNS8*) d->objdict[offset].pSubindex[3].pObject);  
  728.                     break;  
  729.                 }  
  730.                 j++;  
  731.                 offset++;  
  732.             }  
  733.         }  
  734.     }  
  735.       
  736.     /* 如果没有配置服务器,也没有配置客户端,则不用处理该保文 */  
  737.     if(whoami == SDO_UNKNOWN)   
  738.     {  
  739.         return 0xFF;  
  740.     }  
  741.   
  742.     /* SDO报文一定是8字节 */  
  743.     if((*m).len != 8)   
  744.     {  
  745.         MSG_ERR(0x1A67, "Error size SDO", 0);  
  746.         failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_GENERAL_ERROR);  
  747.         return 0xFF;  
  748.     }  
  749.   
  750.     if(whoami == SDO_CLIENT)   
  751.     {  
  752.         MSG_WAR(0x3A68, "I am CLIENT number ", CliServNbr);  
  753.     }  
  754.     else   
  755.     {  
  756.         MSG_WAR(0x3A69, "I am SERVER number ", CliServNbr);  
  757.     }  
  758.   
  759.     /* 通服务器/客户端号获取通道号 */  
  760.     err = getSDOlineOnUse(d, CliServNbr, whoami, &line);  
  761.   
  762.     cs = 0xFF;   
  763.   if(!err)   
  764.     {  
  765.         /* 客户端->服务器:块下载过程中 服务器->客户端:上传过程中 */  
  766.         if(((whoami == SDO_SERVER) && (d->transfers[line].state == SDO_BLOCK_DOWNLOAD_IN_PROGRESS)) ||  
  767.              ((whoami == SDO_CLIENT) && (d->transfers[line].state == SDO_BLOCK_UPLOAD_IN_PROGRESS)))   
  768.         {  
  769.             if(m->data[0] == 0x80)  
  770.                 cs = 4;  
  771.             else  
  772.                 cs = 6;  
  773.         }  
  774.     }  
  775.     if(cs == 0xFF)  
  776.     {  
  777.         cs = getSDOcs(m->data[0]);  
  778.     }  
  779.       
  780.     /* 判断指令 */  
  781.     switch(cs)   
  782.     {  
  783.         /* 客户端->服务器:域分段下载/服务器->客户端:域分段上传 */  
  784.         case 0:  
  785.             /* 客户端->服务器的域分段下载 */  
  786.             if(whoami == SDO_SERVER)   
  787.             {  
  788.                 /* 验证传输通道是否在域分段下载中 */  
  789.                 if(!err)  
  790.                 {  
  791.                     err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;  
  792.                 }  
  793.                 if(err)   
  794.                 {  
  795.                     MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", CliServNbr);  
  796.                     failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  797.                     return 0xFF;  
  798.                 }  
  799.                 /* 重启传输通道超时定时器 */  
  800.                 RestartSDO_TIMER(line)  
  801.                   
  802.                 MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", CliServNbr);  
  803.                   
  804.                 /* 取出索引号 */  
  805.                 index = d->transfers[line].index;  
  806.                 /* 取出子索引号 */  
  807.                 subIndex = d->transfers[line].subIndex;  
  808.                   
  809.                 /* 校验触发位是否同步 */  
  810.                 if(d->transfers[line].toggle != getSDOt(m->data[0]))   
  811.                 {  
  812.                     MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0]));  
  813.                     failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);  
  814.                     return 0xFF;  
  815.                 }  
  816.                   
  817.                 /* 取出字节数 */  
  818.                 nbBytes = 7 - getSDOn3(m->data[0]);  
  819.                 /* 将数据从sdo数据包中拷贝到传输通道缓冲区 */  
  820.                 err = SDOtoLine(d, line, nbBytes, (*m).data + 1);  
  821.                 if(err)  
  822.                 {  
  823.                     failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  824.                     return 0xFF;  
  825.                 }  
  826.                   
  827.                 /* 构建响应包 */  
  828.                 data[0] = (1 << 5) | (d->transfers[line].toggle << 4);  
  829.                 for(i = 1 ; i < 8 ; i++)  
  830.                 {  
  831.                     data[i] = 0;  
  832.                 }  
  833.                 MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", CliServNbr);  
  834.                   
  835.                 /* 发送响应包 */  
  836.                 sendSDO(d, whoami, CliServNbr, data);  
  837.                   
  838.                 /* 触发位取反 */  
  839.                 d->transfers[line].toggle = !d->transfers[line].toggle & 1;  
  840.                 /* 检查该段是否是最后一段 */  
  841.                 if(getSDOc(m->data[0]))   
  842.                 {  
  843.                     /* 将数据从sdo传输通道缓冲区拷贝到字典中 */  
  844.                     errorCode = SDOlineToObjdict(d, line);  
  845.                     if(errorCode)   
  846.                     {  
  847.                         MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0);  
  848.                         /* sdo传输失败 */  
  849.                         failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);  
  850.                         return 0xFF;  
  851.                     }  
  852.                     /* 复位sdo传输通道 */  
  853.                     resetSDOline(d, line);  
  854.                     MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", CliServNbr);  
  855.                 }  
  856.             }  
  857.             /* 服务器->客户端:域分段上传 */  
  858.             else   
  859.             {  
  860.                 /* 验证传输通道是否在域分段上传中 */  
  861.                 if(!err)  
  862.                 {  
  863.                     err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;  
  864.                 }  
  865.                 if(err)   
  866.                 {  
  867.                     MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);  
  868.                     failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  869.                       
  870.                     return 0xFF;  
  871.                 }  
  872.                 /* 重启传输通道超时定时器 */  
  873.                 RestartSDO_TIMER(line)  
  874.                   
  875.                 /* 取出索引号 */  
  876.                 index = d->transfers[line].index;  
  877.                 /* 取出子索引号 */  
  878.                 subIndex = d->transfers[line].subIndex;  
  879.                   
  880.                 /* 校验触发位是否同步 */  
  881.                 if(d->transfers[line].toggle != getSDOt(m->data[0]))   
  882.                 {  
  883.                     MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId);  
  884.                     failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);  
  885.                     return 0xFF;  
  886.                 }  
  887.                 /* 取出字节数 */  
  888.                 nbBytes = 7 - getSDOn3(m->data[0]);  
  889.                 /* 将数据从sdo数据包中拷贝到传输通道缓冲区 */  
  890.                 err = SDOtoLine(d, line, nbBytes, (*m).data + 1);  
  891.                 if(err)   
  892.                 {  
  893.                     failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  894.                     return 0xFF;  
  895.                 }  
  896.                 /* 触发位取反 */  
  897.                 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;  
  898.                 /* 检查该段是否是最后一段 */  
  899.                 if(getSDOc(m->data[0]))   
  900.                 {  
  901.                     /* 停止超时定时器 */  
  902.                     StopSDO_TIMER(line)  
  903.                     /* 传输完成 */  
  904.                     d->transfers[line].state = SDO_FINISHED;  
  905.                     /* 传输完成回调函数 */  
  906.                     if(d->transfers[line].Callback)  
  907.                     {  
  908.                         (*d->transfers[line].Callback)(d,nodeId);  
  909.                     }  
  910.                       
  911.                     MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);  
  912.                 }  
  913.                 /* 如果不是最后一段,继续请求上传 */  
  914.                 else   
  915.                 {  
  916.                     data[0] = (3 << 5) | (d->transfers[line].toggle << 4);  
  917.                     for(i = 1; i < 8; i++)  
  918.                         data[i] = 0;  
  919.                     sendSDO(d, whoami, CliServNbr, data);  
  920.                     MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId);  
  921.                 }  
  922.             }  
  923.             break;  
  924.   
  925.         /* 客户端->服务器:启动域下载/服务器->客户端:域分段下载 */  
  926.         case 1:  
  927.             /* 客户端->服务器:启动域下载 */  
  928.             if(whoami == SDO_SERVER)   
  929.             {  
  930.                 /* 索引 */  
  931.                 index = getSDOindex(m->data[1], m->data[2]);  
  932.                 /* 子索引 */  
  933.                 subIndex = getSDOsubIndex(m->data[3]);  
  934.                 MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", CliServNbr);  
  935.                 MSG_WAR(0x3A80, "Writing at index : ", index);  
  936.                 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);  
  937.                 if(!err)   
  938.                 {  
  939.                     MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0);  
  940.                     failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);  
  941.                     return 0xFF;  
  942.                 }  
  943.                   
  944.                 /* 查找空闲传输通道 */  
  945.                 err = getSDOfreeLine(d, whoami, &line);  
  946.                 if(err)   
  947.                 {  
  948.                     MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);  
  949.                     failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);  
  950.                     return 0xFF;  
  951.                 }  
  952.                 /* 初始化sdo传输通道 */  
  953.                 initSDOline(d, line, CliServNbr, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);  
  954.   
  955.                 /* 如果是快速传输 */  
  956.                 if(getSDOe(m->data[0]))   
  957.                 {  
  958.                     /* 取出字节数 */  
  959.                     nbBytes = 4 - getSDOn2(m->data[0]);  
  960.                     /* 存储字节数 */  
  961.                     d->transfers[line].count = nbBytes;  
  962.                     /* 将数据数据拷贝到传输通道缓冲区 */  
  963.                     err = SDOtoLine(d, line, nbBytes, (*m).data + 4);  
  964.                     if(err)   
  965.                     {  
  966.                         failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  967.                         return 0xFF;  
  968.                     }  
  969.   
  970.                     MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfer. Finished. ", 0);  
  971.                       
  972.                     /* 将数据从传输通道缓冲区拷贝到字典中 */  
  973.                     errorCode = SDOlineToObjdict(d, line);  
  974.                     if(errorCode)   
  975.                     {  
  976.                         MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0);  
  977.                         failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);  
  978.                         return 0xFF;  
  979.                     }  
  980.                     /* 复位sdo通道 */  
  981.                     resetSDOline(d, line);  
  982.                 }  
  983.                 /* 如果是正常传输 */  
  984.                 else   
  985.                 {  
  986.                     /* 数据字节为字节计数器 */  
  987.                     if(getSDOs(m->data[0]))   
  988.                     {  
  989.                         /* 取出字节数 */  
  990.                         nbBytes = (m->data[4]) + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24);  
  991.                         /* 设置sdo传输通道字节数剩余 */  
  992.                         err = setSDOlineRestBytes(d, line, nbBytes);  
  993.                         if(err)  
  994.                         {  
  995.                             failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  996.                             return 0xFF;  
  997.                         }  
  998.                     }  
  999.                 }  
  1000.                 /* 发送启动域下载响应 */  
  1001.                 data[0] = 3 << 5;  
  1002.                 data[1] = index & 0xFF;  
  1003.                 data[2] = (index >> 8) & 0xFF;  
  1004.                 data[3] = subIndex;  
  1005.                 for(i = 4 ; i < 8 ; i++)  
  1006.                     data[i] = 0;  
  1007.                 sendSDO(d, whoami, CliServNbr, data);  
  1008.             }  
  1009.             /* 服务器->客户端:域分段下载 */  
  1010.             else   
  1011.             {  
  1012.                 /* 验证传输通道是否在域分段下载中 */  
  1013.                 if(!err)  
  1014.                     err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;  
  1015.                 if(err)   
  1016.                 {  
  1017.                     MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);  
  1018.                     failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  1019.                     return 0xFF;  
  1020.                 }  
  1021.                   
  1022.                 /* 重启传输通道超时定时器 */  
  1023.                 RestartSDO_TIMER(line)  
  1024.                 /* 索引 */      
  1025.                 index = d->transfers[line].index;  
  1026.                 /* 子索引 */  
  1027.                 subIndex = d->transfers[line].subIndex;  
  1028.                 /* 校验触发位是否同步 */  
  1029.                 if(d->transfers[line].toggle != getSDOt(m->data[0]))   
  1030.                 {  
  1031.                     MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId);  
  1032.                     failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);  
  1033.                     return 0xFF;  
  1034.                 }  
  1035.   
  1036.                 /* 获取传输通道剩余字节数 */  
  1037.                 getSDOlineRestBytes(d, line, &nbBytes);  
  1038.                 /* 如果已经传输完 */  
  1039.                 if(nbBytes == 0)   
  1040.                 {  
  1041.                     MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId);  
  1042.                     /* 停止超时定时器 */  
  1043.                     StopSDO_TIMER(line)  
  1044.                     /* 将状态置为完成 */  
  1045.                     d->transfers[line].state = SDO_FINISHED;  
  1046.                     /* 传输完成回调函数 */  
  1047.                     if(d->transfers[line].Callback)   
  1048.                     {  
  1049.                         (*d->transfers[line].Callback)(d,nodeId);  
  1050.                     }  
  1051.                       
  1052.                     return 0x00;  
  1053.                 }  
  1054.   
  1055.                 /* 如果至少还有7个字节没传输完毕  */  
  1056.                 if(nbBytes > 7)   
  1057.                 {  
  1058.                     /* 触发位取反 */  
  1059.                     d->transfers[line].toggle = ! d->transfers[line].toggle & 1;  
  1060.                     /* 构建报文 */  
  1061.                     data[0] = (d->transfers[line].toggle << 4);  
  1062.                     /* 将数据从传输通道缓冲区拷贝到sdo报文中 */  
  1063.                     err = lineToSDO(d, line, 7, data + 1);  
  1064.                     if(err)   
  1065.                     {  
  1066.                         failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1067.                         return 0xFF;  
  1068.                     }  
  1069.                 }  
  1070.                 /* 如果数据量小于7个字节 */  
  1071.                 else   
  1072.                 {  
  1073.                     /* 触发位取反 */  
  1074.                     d->transfers[line].toggle = !d->transfers[line].toggle & 1;  
  1075.                     /* 构建报文 */  
  1076.                     data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1);  
  1077.                     /* 将数据从传输通道缓冲区拷贝到sdo报文中 */  
  1078.                     err = lineToSDO(d, line, nbBytes, data + 1);  
  1079.                     if(err)   
  1080.                     {  
  1081.                         failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1082.                         return 0xFF;  
  1083.                     }  
  1084.                       
  1085.                     /* 剩余字节填0 */  
  1086.                     for(i = nbBytes + 1; i < 8; i++)  
  1087.                     {  
  1088.                         data[i] = 0;  
  1089.                     }  
  1090.                 }  
  1091.                 MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId);  
  1092.                   
  1093.                 /* 发送sdo报文 */  
  1094.                 sendSDO(d, whoami, CliServNbr, data);  
  1095.             }  
  1096.             break;  
  1097.   
  1098.         /* 客户端->服务器:启动域上传/服务器->客户端:启动域上传 */  
  1099.         case 2:  
  1100.             /* 客户端->服务器:启动域上传 */  
  1101.             if(whoami == SDO_SERVER)   
  1102.             {  
  1103.                 /* 索引 */  
  1104.                 index = getSDOindex(m->data[1], m->data[2]);  
  1105.                 /* 子索引 */  
  1106.                 subIndex = getSDOsubIndex(m->data[3]);  
  1107.                 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", CliServNbr);  
  1108.                 MSG_WAR(0x3A90, "Reading at index : ", index);  
  1109.                 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);  
  1110.                 if(!err)   
  1111.                 {  
  1112.                     MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line);  
  1113.                     MSG_WAR(0x3A93, "Server Nbr = ", CliServNbr);  
  1114.                     failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);  
  1115.                     return 0xFF;  
  1116.                 }  
  1117.                 /* 获取sdo空闲传输通道 */  
  1118.                 err = getSDOfreeLine(d, whoami, &line);  
  1119.                 if(err)   
  1120.                 {  
  1121.                     MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);  
  1122.                     failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);  
  1123.                     return 0xFF;  
  1124.                 }  
  1125.                 /* 初始化传输通道 */  
  1126.                 initSDOline(d, line, CliServNbr, index, subIndex, SDO_UPLOAD_IN_PROGRESS);  
  1127.                 /* 将数据从字典中拷贝到传输通道缓冲区 */  
  1128.                 errorCode = objdictToSDOline(d, line);  
  1129.                 if(errorCode)   
  1130.                 {  
  1131.                     MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", errorCode);  
  1132.                     failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);  
  1133.                     return 0xFF;  
  1134.                 }  
  1135.                 /* 获取传输通道剩余字节数 */  
  1136.                 getSDOlineRestBytes(d, line, &nbBytes);  
  1137.                 /* 字节数大于4,正常传输 */  
  1138.                 if(nbBytes > 4)   
  1139.                 {  
  1140.                     /* 命令 */  
  1141.                     data[0] = (2 << 5) | 1;  
  1142.                     /* 索引 */  
  1143.                     data[1] = index & 0xFF;  
  1144.                     data[2] = (index >> 8) & 0xFF;  
  1145.                     /* 子索引 */  
  1146.                     data[3] = subIndex;  
  1147.                     /* 字节计数器 */  
  1148.                     data[4] = (UNS8)nbBytes;  
  1149.                     data[5] = (UNS8)(nbBytes >> 8);  
  1150.                     data[6] = (UNS8)(nbBytes >> 16);  
  1151.                     data[7] = (UNS8)(nbBytes >> 24);  
  1152.                     MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId);  
  1153.                     /* 发送sdo报文 */  
  1154.                     sendSDO(d, whoami, CliServNbr, data);  
  1155.                 }  
  1156.                 /* 字节数不高于4,快速传输 */  
  1157.                 else  
  1158.                 {  
  1159.                     /* 命令 */  
  1160.                     data[0] = (UNS8)((2 << 5) | ((4 - nbBytes) << 2) | 3);  
  1161.                     /* 索引 */  
  1162.                     data[1] = index & 0xFF;  
  1163.                     data[2] = (index >> 8) & 0xFF;  
  1164.                     /* 子索引 */  
  1165.                     data[3] = subIndex;  
  1166.                     /* 将数据从传输通道缓冲区拷贝到sdo报文 */  
  1167.                     err = lineToSDO(d, line, nbBytes, data + 4);  
  1168.                     if(err)   
  1169.                     {  
  1170.                         failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1171.                         return 0xFF;  
  1172.                     }  
  1173.                     /* 不足8字节填0 */  
  1174.                     for(i = 4 + nbBytes; i < 8; i++)  
  1175.                     {  
  1176.                         data[i] = 0;  
  1177.                     }  
  1178.                       
  1179.                     MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", CliServNbr);  
  1180.                       
  1181.                     /* 发送sdo报文 */  
  1182.                     sendSDO(d, whoami, CliServNbr, data);  
  1183.                       
  1184.                     /* 复位sdo传输通道 */  
  1185.                     resetSDOline(d, line);  
  1186.                 }  
  1187.             }  
  1188.             /* 服务器->客户端:启动域上传 */  
  1189.             else   
  1190.             {  
  1191.                 /* 验证传输通道是否在域上传中 */  
  1192.                 if(!err)  
  1193.                     err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;  
  1194.                 if(err)   
  1195.                 {  
  1196.                     MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId);  
  1197.                     failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  1198.                     return 0xFF;  
  1199.                 }  
  1200.                   
  1201.                 /* 重置传输超时定时器 */  
  1202.                 RestartSDO_TIMER(line)  
  1203.                   
  1204.                 /* 索引 */  
  1205.                 index = d->transfers[line].index;  
  1206.                 /* 子索引 */  
  1207.                 subIndex = d->transfers[line].subIndex;  
  1208.   
  1209.                 /* 加速传输完成 */  
  1210.                 if(getSDOe(m->data[0]))   
  1211.                 {  
  1212.                     /* 字节数 */  
  1213.                     nbBytes = 4 - getSDOn2(m->data[0]);  
  1214.                     /* 将数据拷贝到传输通道缓冲区 */  
  1215.                     err = SDOtoLine(d, line, nbBytes, (*m).data + 4);  
  1216.                     if(err)   
  1217.                     {  
  1218.                         failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1219.                         return 0xFF;  
  1220.                     }  
  1221.   
  1222.                     MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);  
  1223.                       
  1224.                     /* 停止sdo传输通道超时定时器 */  
  1225.                     StopSDO_TIMER(line)  
  1226.                       
  1227.                     /* 接收到的字节数 */  
  1228.                     d->transfers[line].count = nbBytes;  
  1229.                       
  1230.                     /* sdo传输通道传输完成 */  
  1231.                     d->transfers[line].state = SDO_FINISHED;  
  1232.                       
  1233.                     /* 传输完成后调用回调函数 */  
  1234.                     if(d->transfers[line].Callback)   
  1235.                         (*d->transfers[line].Callback)(d,nodeId);  
  1236.                       
  1237.                     return 0;  
  1238.                 }  
  1239.                 /* 正常传输 */  
  1240.                 else   
  1241.                 {  
  1242.                     if(getSDOs(m->data[0]))   
  1243.                     {  
  1244.                         /* 字节计数器 */  
  1245.                         nbBytes = m->data[4] + ((UNS32)(m->data[5]) << 8) + ((UNS32)(m->data[6]) << 16) + ((UNS32)(m->data[7]) << 24);  
  1246.                         /* 设置sdo传输通道剩余字节数 */  
  1247.                         err = setSDOlineRestBytes(d, line, nbBytes);  
  1248.                         if(err)   
  1249.                         {  
  1250.                             failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1251.                             return 0xFF;  
  1252.                         }  
  1253.                     }  
  1254.   
  1255.                     /* 请求:域分段上传 */  
  1256.                     data[0] = 3 << 5;  
  1257.                     for(i = 1; i < 8; i++)  
  1258.                         data[i] = 0;  
  1259.   
  1260.                     MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId);  
  1261.                       
  1262.                     /* 发送报文 */  
  1263.                     sendSDO(d, whoami, CliServNbr, data);  
  1264.                 }  
  1265.             }  
  1266.             break;  
  1267.   
  1268.         /* 客户端->服务器:域分段上传/服务器->客户端:启动域下载 */  
  1269.         case 3:  
  1270.             /* 客户端->服务器:域分段上传 */  
  1271.             if(whoami == SDO_SERVER)   
  1272.             {  
  1273.                 if(!err)  
  1274.                     err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;  
  1275.                 if(err)   
  1276.                 {  
  1277.                     MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", CliServNbr);  
  1278.                     failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  1279.                       
  1280.                     return 0xFF;  
  1281.                 }  
  1282.   
  1283.                 /* 复位sdo传输通道超时定时器 */  
  1284.                 RestartSDO_TIMER(line)  
  1285.                       
  1286.                 MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", CliServNbr);  
  1287.                   
  1288.                 /* 索引 */  
  1289.                 index = d->transfers[line].index;  
  1290.                 /* 子索引 */  
  1291.                 subIndex = d->transfers[line].subIndex;  
  1292.                   
  1293.                 /* 校验触发位是否同步 */  
  1294.                 if(d->transfers[line].toggle != getSDOt(m->data[0]))   
  1295.                 {  
  1296.                     MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0]));  
  1297.                     failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);  
  1298.                       
  1299.                     return 0xFF;  
  1300.                 }  
  1301.   
  1302.                 /* 获取传输通道剩余字节数 */  
  1303.                 getSDOlineRestBytes(d, line, &nbBytes);  
  1304.                 /* 如果至少还有7个字节没传输完毕  */  
  1305.                 if(nbBytes > 7)   
  1306.                 {  
  1307.                     /* 构建报文 */  
  1308.                     data[0] = (d->transfers[line].toggle << 4);  
  1309.                     /* 将数据从sdo传输通道缓冲区拷贝到sdo报文中 */  
  1310.                     err = lineToSDO(d, line, 7, data + 1);  
  1311.                     if(err)   
  1312.                     {  
  1313.                         failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1314.                         return 0xFF;  
  1315.                     }  
  1316.   
  1317.                     /* 将触发位取反 */  
  1318.                     d->transfers[line].toggle = !d->transfers[line].toggle & 1;  
  1319.                       
  1320.                     MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", CliServNbr);  
  1321.                       
  1322.                     /* 发送报文 */  
  1323.                     sendSDO(d, whoami, CliServNbr, data);  
  1324.                 }  
  1325.                 /* 不大于7字节,则说明是最后一段 */  
  1326.                 else   
  1327.                 {  
  1328.                     /* 构建报文 */  
  1329.                     data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1);  
  1330.                     /* 将数据从sdo传输通道缓冲区拷贝到sdo报文中 */  
  1331.                     err = lineToSDO(d, line, nbBytes, data + 1);  
  1332.                     if (err) {  
  1333.                         failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1334.                         return 0xFF;  
  1335.                     }  
  1336.                     /* 不满8字节补0 */  
  1337.                     for(i = nbBytes + 1; i < 8; i++)  
  1338.                         data[i] = 0;  
  1339.                       
  1340.                     MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", CliServNbr);  
  1341.                       
  1342.                     /* 发送报文 */  
  1343.                     sendSDO(d, whoami, CliServNbr, data);  
  1344.                       
  1345.                     /* 复位传输通道 */  
  1346.                     resetSDOline(d, line);  
  1347.                 }  
  1348.             }  
  1349.             /* 服务器->客户端:启动域下载 */  
  1350.             else   
  1351.             {  
  1352.                 /* 校验传输通道是否在域下载过程中 */  
  1353.                 if(!err)  
  1354.                     err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;  
  1355.                 if(err)   
  1356.                 {  
  1357.                     MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId);  
  1358.                     failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  1359.                     return 0xFF;  
  1360.                 }  
  1361.                   
  1362.                 /* 复位sdo传输通道定时器 */  
  1363.                 RestartSDO_TIMER(line)  
  1364.                   
  1365.                 /* 索引 */  
  1366.                 index = d->transfers[line].index;  
  1367.                 /* 子索引 */  
  1368.                 subIndex = d->transfers[line].subIndex;  
  1369.                   
  1370.                 /* 获取sdo传输通道剩余字节数 */  
  1371.                 getSDOlineRestBytes(d, line, &nbBytes);  
  1372.                   
  1373.                 /* 传输完成 */  
  1374.                 if(nbBytes == 0)   
  1375.                 {  
  1376.                     MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId);  
  1377.                       
  1378.                     /* 停止传输通道超时定时器 */  
  1379.                     StopSDO_TIMER(line)  
  1380.                       
  1381.                     /* 将传输通道状态设置为完成 */  
  1382.                     d->transfers[line].state = SDO_FINISHED;  
  1383.                       
  1384.                     /* 传输完成回调函数 */  
  1385.                     if(d->transfers[line].Callback)   
  1386.                         (*d->transfers[line].Callback)(d,nodeId);  
  1387.                       
  1388.                     return 0x00;  
  1389.                 }  
  1390.                   
  1391.                 /* 剩余字节数大于7个:域分段下载 */  
  1392.                 if(nbBytes > 7)   
  1393.                 {  
  1394.                     /* 构建报文 */  
  1395.                     data[0] = (d->transfers[line].toggle << 4);  
  1396.                       
  1397.                     /* 将数据从传输通道中拷贝到sdo报文中 */  
  1398.                     err = lineToSDO(d, line, 7, data + 1);  
  1399.                     if(err)   
  1400.                     {  
  1401.                         failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1402.                         return 0xFF;  
  1403.                     }  
  1404.                 }  
  1405.                 /* 剩余字节数不大于7个:域分段下载,最后一段 */  
  1406.                 else   
  1407.                 {  
  1408.                     /* 构建报文 */  
  1409.                     data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1);  
  1410.                       
  1411.                     /* 将数据从传输通道中拷贝到sdo报文中 */  
  1412.                     err = lineToSDO(d, line, nbBytes, data + 1);  
  1413.                     if(err)   
  1414.                     {  
  1415.                         failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1416.                         return 0xFF;  
  1417.                     }  
  1418.   
  1419.                     /* 不足8字节补0 */  
  1420.                     for(i = nbBytes + 1; i < 8; i++)  
  1421.                         data[i] = 0;  
  1422.                 }  
  1423.                   
  1424.                 MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId);  
  1425.                   
  1426.                 /* 发送sdo报文 */  
  1427.                 sendSDO(d, whoami, CliServNbr, data);  
  1428.             }  
  1429.             break;  
  1430.   
  1431.         /* 中止 */  
  1432.         case 4:  
  1433.             /* 中止码 */  
  1434.             abortCode = (UNS32)m->data[4] | ((UNS32)m->data[5] << 8) | ((UNS32)m->data[6] << 16) | ((UNS32)m->data[7] << 24);  
  1435.             /* 客户端->服务器:中止 */  
  1436.             if(whoami == SDO_SERVER)   
  1437.             {  
  1438.                 /* 复位sdo传输通道 */  
  1439.                 if(!err)   
  1440.                 {  
  1441.                     resetSDOline(d, line);  
  1442.                     MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);  
  1443.                 }  
  1444.                 else  
  1445.                     MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);  
  1446.             }  
  1447.             /* 服务器->客户端:中止 */  
  1448.             else   
  1449.             {  
  1450.                 if(!err)   
  1451.                 {  
  1452.                     /* 停止sdo传输通道超时定时器 */  
  1453.                     StopSDO_TIMER(line)  
  1454.                     /* 将传输通道状态设置为中止 */  
  1455.                     d->transfers[line].state = SDO_ABORTED_RCV;  
  1456.                     /* 中止码 */  
  1457.                     d->transfers[line].abortCode = abortCode;  
  1458.                       
  1459.                     MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);  
  1460.                       
  1461.                     /* 传输结束调用回调函数 */  
  1462.                     if(d->transfers[line].Callback)   
  1463.                         (*d->transfers[line].Callback)(d, nodeId);  
  1464.                 }  
  1465.                 else  
  1466.                     MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);  
  1467.             }  
  1468.             break;  
  1469.   
  1470.         /* 服务器->客户端:块下载初始化/服务器->客户端:块下载/服务器->客户端:块下载结束 */  
  1471.         /* 客户端->服务器:块上传初始化/客户端->服务器:块上传开始命令/客户端->服务器:块上传/客户端->服务器:块上传结束 */  
  1472.         case 5:  
  1473.             /* 子命令 */  
  1474.             SubCommand = getSDOblockSC(m->data[0]);  
  1475.               
  1476.             /* 客户端->服务器:块上传初始化/客户端->服务器:块上传/客户端->服务器:块上传结束 */  
  1477.             if(whoami == SDO_SERVER)   
  1478.             {  
  1479.                 /* 客户端->服务器:块上传初始化 */  
  1480.                 if(SubCommand == SDO_BCS_INITIATE_UPLOAD_REQUEST)   
  1481.                 {  
  1482.                     /* 索引 */  
  1483.                     index = getSDOindex(m->data[1], m->data[2]);  
  1484.                     /* 子索引 */  
  1485.                     subIndex = getSDOsubIndex(m->data[3]);  
  1486.                       
  1487.                     MSG_WAR(0x3AB2, "Received SDO Initiate block upload defined at index 0x1200 + ", CliServNbr);  
  1488.                     MSG_WAR(0x3AB3, "Reading at index : ", index);  
  1489.                     MSG_WAR(0x3AB4, "Reading at subIndex : ", subIndex);  
  1490.                     if(!err)   
  1491.                     {  
  1492.                         MSG_ERR(0x1A93, "SDO error : Transmission yet started at line : ", line);  
  1493.                         MSG_WAR(0x3AB5, "Server Nbr = ", CliServNbr);  
  1494.                         failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);  
  1495.                         return 0xFF;  
  1496.                     }  
  1497.                       
  1498.                     /* 获取sdo空闲传输通道 */  
  1499.                     err = getSDOfreeLine(d, whoami, &line);  
  1500.                     if(err)   
  1501.                     {  
  1502.                         MSG_ERR(0x1A73, "SDO error : No line free, too many SDO in progress. Aborted.", 0);  
  1503.                         failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);  
  1504.                         return 0xFF;  
  1505.                     }  
  1506.                       
  1507.                     /* 初始化sdo传输通道 */  
  1508.                     initSDOline(d, line, CliServNbr, index, subIndex, SDO_BLOCK_UPLOAD_IN_PROGRESS);  
  1509.                       
  1510.                     /* 是否支持crc校验 */  
  1511.                     d->transfers[line].peerCRCsupport = ((m->data[0]) >> 2) & 1;  
  1512.                       
  1513.                     /* 块数量 */  
  1514.                     d->transfers[line].blksize = m->data[4];  
  1515.                     /* 将数据从字典中拷贝到sdo传输通道缓冲区 */  
  1516.                     errorCode = objdictToSDOline(d, line);  
  1517.                     if(errorCode)   
  1518.                     {  
  1519.                         MSG_ERR(0x1A95, "SDO error : Unable to copy the data from object dictionary. Err code : ", errorCode);  
  1520.                         failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);  
  1521.                         return 0xFF;  
  1522.                     }  
  1523.                     /* 获取sdo传输剩余字节数 */  
  1524.                     getSDOlineRestBytes(d, line, &nbBytes);  
  1525.                     /* 设置对象大小 */  
  1526.                     d->transfers[line].objsize = nbBytes;  
  1527.                       
  1528.                     /* sdo块上传初始化响应 */  
  1529.                     data[0] = (6 << 5) | (1 << 1) | SDO_BSS_INITIATE_UPLOAD_RESPONSE;  
  1530.                     /* 索引 */  
  1531.                     data[1] = index & 0xFF;  
  1532.                     data[2] = (index >> 8) & 0xFF;  
  1533.                     /* 子索引 */  
  1534.                     data[3] = subIndex;  
  1535.                     /* 字节数 */  
  1536.                     data[4] = (UNS8)nbBytes;  
  1537.                     data[5] = (UNS8)(nbBytes >> 8);  
  1538.                     data[6] = (UNS8)(nbBytes >> 16);  
  1539.                     data[7] = (UNS8)(nbBytes >> 24);  
  1540.                       
  1541.                     MSG_WAR(0x3A9A, "SDO. Sending normal block upload initiate response defined at index 0x1200 + ", nodeId);  
  1542.                       
  1543.                     /* 发送sdo报文 */  
  1544.                     sendSDO(d, whoami, CliServNbr, data);  
  1545.                 }  
  1546.                 /* 客户端->服务器:块上传结束响应 */  
  1547.                 else if(SubCommand == SDO_BCS_END_UPLOAD_REQUEST)   
  1548.                 {  
  1549.                     MSG_WAR(0x3AA2, "Received SDO block END upload request defined at index 0x1200 + ", CliServNbr);  
  1550.                       
  1551.                     /* 校验是否在块上传过程中 */  
  1552.                     if(!err)  
  1553.                         err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS;  
  1554.                     if(err)   
  1555.                     {  
  1556.                         MSG_ERR(0x1AA1, "SDO error : Received block upload request for unstarted trans. index 0x1200 + ", CliServNbr);  
  1557.                         failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  1558.                         return 0xFF;  
  1559.                     }  
  1560.                                       
  1561.                     /* 复位sdo传输通道 */  
  1562.                     resetSDOline(d, line);  
  1563.                 }  
  1564.                 /* 客户端->服务器:块上传/客户端->服务器:块上传开始命令 */  
  1565.                 else if((SubCommand == SDO_BCS_UPLOAD_RESPONSE) || (SubCommand == SDO_BCS_START_UPLOAD))   
  1566.                 {  
  1567.                     /* 校验是否在块上传过程中 */  
  1568.                     if(!err)  
  1569.                         err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS;  
  1570.                     if(err)   
  1571.                     {  
  1572.                         MSG_ERR(0x1AA1, "SDO error : Received block upload response for unstarted trans. index 0x1200 + ", CliServNbr);  
  1573.                         failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  1574.                         return 0xFF;  
  1575.                     }  
  1576.                       
  1577.                     /* 重启传输通道超时定时器 */  
  1578.                     RestartSDO_TIMER(line);  
  1579.                       
  1580.                     /* 索引 */  
  1581.                     index = d->transfers[line].index;  
  1582.                     /* 子索引 */  
  1583.                     subIndex = d->transfers[line].subIndex;  
  1584.   
  1585.                     /* 块上传响应 */  
  1586.                     if(SubCommand == SDO_BCS_UPLOAD_RESPONSE)   
  1587.                     {  
  1588.                         MSG_WAR(0x3AA2, "Received SDO block upload response defined at index 0x1200 + ", CliServNbr);  
  1589.                           
  1590.                         /* 一次最大上传块数 */  
  1591.                         d->transfers[line].blksize = m->data[2];  
  1592.                         /* 响应序列号 */  
  1593.                         AckSeq = (m->data[1]) & 0x7f;  
  1594.                         /* 获取剩余字节数 */  
  1595.                         getSDOlineRestBytes(d, line, &nbBytes);  
  1596.                         /* 如果传输完 */  
  1597.                         if((nbBytes == 0) && (AckSeq == d->transfers[line].seqno))  
  1598.                         {  
  1599.                             /* 上传结束指令 */  
  1600.                             data[0] = (6 << 5) | ((d->transfers[line].endfield) << 2) | SDO_BSS_END_UPLOAD_RESPONSE;  
  1601.                             for(i = 1; i < 8; i++)  
  1602.                                 data[i] = 0;  
  1603.               
  1604.                             MSG_WAR(0x3AA5, "SDO. Sending block END upload response defined at index 0x1200 + ", CliServNbr);  
  1605.   
  1606.                             /* 发送sdo报文 */  
  1607.                             sendSDO(d, whoami, CliServNbr, data);  
  1608.                             break;  
  1609.                         }  
  1610.                         else  
  1611.                             d->transfers[line].offset = d->transfers[line].lastblockoffset + 7 * AckSeq;  
  1612.   
  1613.                         if(d->transfers[line].offset > d->transfers[line].count)   
  1614.                         {  
  1615.                             MSG_ERR(0x1AA1, "SDO error : Received upload response with bad ackseq index 0x1200 + ", CliServNbr);  
  1616.                             failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  1617.                             return 0xFF;  
  1618.                         }  
  1619.                     }  
  1620.                     else  
  1621.                         MSG_WAR(0x3AA2, "Received SDO block START upload defined at index 0x1200 + ", CliServNbr);  
  1622.   
  1623.                     /* 上一次块传输后数据偏移量 */  
  1624.                     d->transfers[line].lastblockoffset = (UNS8) d->transfers[line].offset;  
  1625.   
  1626.                     /* 进行一次块上传数据 */  
  1627.                     for(SeqNo = 1; SeqNo <= d->transfers[line].blksize; SeqNo++)   
  1628.                     {  
  1629.                         /* 块序列号 */  
  1630.                         d->transfers[line].seqno = SeqNo;  
  1631.                           
  1632.                         /* 获取传输通道剩余字节数 */  
  1633.                         getSDOlineRestBytes(d, line, &nbBytes);  
  1634.                         /* 如果大于7字节,则说明不是最后一块数据 */  
  1635.                         if(nbBytes > 7)   
  1636.                         {  
  1637.                             /* 块序列号 */  
  1638.                             data[0] = SeqNo;  
  1639.                             /* 将传输通道上的数据拷贝到sdo报文中 */  
  1640.                             err = lineToSDO(d, line, 7, data + 1);  
  1641.                             if(err)   
  1642.                             {  
  1643.                                 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1644.                                 return 0xFF;  
  1645.                             }  
  1646.                               
  1647.                             MSG_WAR(0x3AA5, "SDO. Sending upload segment defined at index 0x1200 + ", CliServNbr);  
  1648.                               
  1649.                             /* 发送sdo报文 */  
  1650.                             sendSDO(d, whoami, CliServNbr, data);  
  1651.                         }  
  1652.                         /* 如果不大于7字节,则说明是最后一块数据 */  
  1653.                         else   
  1654.                         {  
  1655.                             /* 块序列号,最高位表示最后一块 */  
  1656.                             data[0] = 0x80 | SeqNo;  
  1657.                               
  1658.                             /* 将传输通道上的数据拷贝到sdo报文中 */  
  1659.                             err = lineToSDO(d, line, nbBytes, data + 1);  
  1660.                             if(err)   
  1661.                             {  
  1662.                                 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1663.                                 return 0xFF;  
  1664.                             }  
  1665.                               
  1666.                             /* 不满8字节补0 */  
  1667.                             for(i = nbBytes + 1 ; i < 8 ; i++)  
  1668.                                 data[i] = 0;  
  1669.                               
  1670.                             MSG_WAR(0x3AA5, "SDO. Sending last upload segment defined at index 0x1200 + ", CliServNbr);  
  1671.                               
  1672.                             /* 发送sdo报文 */  
  1673.                             sendSDO(d, whoami, CliServNbr, data);  
  1674.                               
  1675.                             /* 记录补零个数 */  
  1676.                             d->transfers[line].endfield = (UNS8)(7 - nbBytes);  
  1677.                             break;  
  1678.                         }  
  1679.                     }  
  1680.                 }  
  1681.             }  
  1682.             /* 服务器->客户端:块下载初始化/服务器->客户端:块下载/服务器->客户端:块下载结束 */  
  1683.             else   
  1684.             {  
  1685.                 /* 服务器->客户端:块下载初始化/服务器->客户端:块下载 */  
  1686.                 if((SubCommand == SDO_BSS_INITIATE_DOWNLOAD_RESPONSE) ||   
  1687.                      (SubCommand == SDO_BSS_DOWNLOAD_RESPONSE))   
  1688.                 {  
  1689.                     /* 判断sdo传输通道是否在块下载过程中 */  
  1690.                     if(!err)  
  1691.                         err = d->transfers[line].state != SDO_BLOCK_DOWNLOAD_IN_PROGRESS;  
  1692.                     if(err)  
  1693.                     {  
  1694.                         MSG_ERR(0x1AAA, "SDO error : Received response for unknown block download request from node id", nodeId);  
  1695.                         failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  1696.                         return 0xFF;  
  1697.                     }  
  1698.                       
  1699.                     /* 复位sdo传输超时定时器 */  
  1700.                     RestartSDO_TIMER(line)  
  1701.                       
  1702.                     /* 服务器->客户端:块下载初始化 */  
  1703.                     if(SubCommand == SDO_BSS_INITIATE_DOWNLOAD_RESPONSE)   
  1704.                     {  
  1705.                         /* 索引 */  
  1706.                         index = d->transfers[line].index;  
  1707.                         /* 子索引 */  
  1708.                         subIndex = d->transfers[line].subIndex;  
  1709.                         /* 是否支持crc校验 */  
  1710.                         d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1;  
  1711.                         /* 块数量 */  
  1712.                         d->transfers[line].blksize = m->data[4];  
  1713.                     }  
  1714.                     /* 服务器->客户端:块下载 */  
  1715.                     else   
  1716.                     {  
  1717.                         /* 块数量 */  
  1718.                         d->transfers[line].blksize = m->data[2];  
  1719.                         /* 序列号 */  
  1720.                         AckSeq = (m->data[1]) & 0x7f;  
  1721.                         /* 获取sdo传输通道缓冲区剩余字节数 */  
  1722.                         getSDOlineRestBytes(d, line, &nbBytes);  
  1723.                         /* 如果剩余字节数为0,并且序列号校验通过 */  
  1724.                         if((nbBytes == 0) && (AckSeq == d->transfers[line].seqno))  
  1725.                         {  
  1726.                             /* 块下载结束 */  
  1727.                             data[0] = (6 << 5) | ((d->transfers[line].endfield) << 2) | SDO_BCS_END_DOWNLOAD_REQUEST;  
  1728.                             /* 数据填0 */  
  1729.                             for(i = 1; i < 8; i++)  
  1730.                                 data[i] = 0;  
  1731.                               
  1732.                             MSG_WAR(0x3AA5, "SDO. Sending block END download request defined at index 0x1200 + ", CliServNbr);  
  1733.                               
  1734.                             /* 发送sdo报文 */  
  1735.                             sendSDO(d, whoami, CliServNbr, data);  
  1736.                             break;  
  1737.                         }  
  1738.                         /* 如果剩余字节数不为0 */  
  1739.                         else  
  1740.                             d->transfers[line].offset = d->transfers[line].lastblockoffset + 7 * AckSeq;  
  1741.                           
  1742.                         if(d->transfers[line].offset > d->transfers[line].count)   
  1743.                         {  
  1744.                             MSG_ERR(0x1AA1, "SDO error : Received upload segment with bad ackseq index 0x1200 + ", CliServNbr);  
  1745.                             failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  1746.                             return 0xFF;  
  1747.                         }  
  1748.                     }  
  1749.                     /* 上一次内存块发送完后传输通道的偏移量 */  
  1750.                     d->transfers[line].lastblockoffset = (UNS8)d->transfers[line].offset;  
  1751.                     /* 将数据分块发送出去,不能超过从站指定的最大块数量 */  
  1752.                     for(SeqNo = 1; SeqNo <= d->transfers[line].blksize; SeqNo++)   
  1753.                     {  
  1754.                         /* 块序列号 */  
  1755.                         d->transfers[line].seqno = SeqNo;  
  1756.                         /* 获取传输通道剩余字节数 */  
  1757.                         getSDOlineRestBytes(d, line, &nbBytes);  
  1758.                         /* 如果剩余字节数大于7 */  
  1759.                         if(nbBytes > 7)   
  1760.                         {  
  1761.                             /* 块序列号,最高位表示是否最后一块 */  
  1762.                             data[0] = SeqNo;  
  1763.                             /* 将传输通道缓冲区数据拷贝到sdo报文中 */  
  1764.                             err = lineToSDO(d, line, 7, data + 1);  
  1765.                             if(err)   
  1766.                             {  
  1767.                                 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1768.                                 return 0xFF;  
  1769.                             }  
  1770.                               
  1771.                             MSG_WAR(0x3AAB, "SDO. Sending download segment to node id ", nodeId);  
  1772.                               
  1773.                             /* 发送报文 */  
  1774.                             sendSDO(d, whoami, CliServNbr, data);  
  1775.                         }  
  1776.                         /* 如果剩余字节数不大于7,说明这是最后一块 */  
  1777.                         else   
  1778.                         {  
  1779.                             /* 块序列号,最高位表示是否最后一块 */  
  1780.                             data[0] = 0x80 | SeqNo;  
  1781.                             /* 将传输通道缓冲区数据拷贝到sdo报文中 */  
  1782.                             err = lineToSDO(d, line, nbBytes, data + 1);  
  1783.                             if(err)   
  1784.                             {  
  1785.                                 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);  
  1786.                                 return 0xFF;  
  1787.                             }  
  1788.                               
  1789.                             /* 不足8字节补0 */  
  1790.                             for(i = nbBytes + 1; i < 8; i++)  
  1791.                                 data[i] = 0;  
  1792.                               
  1793.                             MSG_WAR(0x3AAB, "SDO. Sending last download segment to node id ", nodeId);  
  1794.                               
  1795.                             /* 发送报文 */  
  1796.                             sendSDO(d, whoami, CliServNbr, data);  
  1797.                               
  1798.                             /* 最后一个块数据包中,补0的字节数 */  
  1799.                             d->transfers[line].endfield = (UNS8)(7 - nbBytes);  
  1800.                             break;  
  1801.                         }  
  1802.                     }  
  1803.                 }  
  1804.                 /* 服务器->客户端:块下载结束 */  
  1805.                 else if(SubCommand == SDO_BSS_END_DOWNLOAD_RESPONSE)   
  1806.                 {  
  1807.                     MSG_WAR(0x3AAC, "SDO End block download response from nodeId", nodeId);  
  1808.                       
  1809.                     /* 停止sdo传输超时定时器 */  
  1810.                     StopSDO_TIMER(line)  
  1811.                     /* 传输通道状态置为传输完成 */  
  1812.                     d->transfers[line].state = SDO_FINISHED;  
  1813.                       
  1814.                     /* 传输完成调用回调函数 */  
  1815.                     if(d->transfers[line].Callback)   
  1816.                         (*d->transfers[line].Callback)(d, nodeId);  
  1817.                     return 0x00;  
  1818.                 }  
  1819.                 /* 错误指令 */  
  1820.                 else   
  1821.                 {  
  1822.                     MSG_ERR(0x1AAB, "SDO error block download : Received wrong subcommand from nodeId", nodeId);  
  1823.                     failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  1824.                 return 0xFF;  
  1825.                 }  
  1826.             }  
  1827.             break;  
  1828.   
  1829.         /* 客户端->服务器:块下载初始化/客户端->服务器:块下载/客户端->服务器:块下载结束 */  
  1830.         /* 服务器->客户端:块上传初始化/服务器->客户端:块上传/服务器->客户端:块上传结束 */  
  1831.         case 6:  
  1832.             /* 客户端->服务器:块下载初始化/客户端->服务器:块下载/客户端->服务器:块下载结束 */  
  1833.             if(whoami == SDO_SERVER)   
  1834.             {  
  1835.                 /* 如果传输通道未建立,则说明客户端->服务器:块下载初始化 */  
  1836.                 if(err)   
  1837.                 {  
  1838.                     /* 取出子命令,判断是否为块下载初始化 */  
  1839.                     SubCommand = (m->data[0]) & 1;  
  1840.                     if(SubCommand != SDO_BCS_INITIATE_DOWNLOAD_REQUEST)   
  1841.                     {  
  1842.                         MSG_ERR(0x1AAC, "SDO error block download : Received wrong subcommand from node id", nodeId);  
  1843.                         failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);  
  1844.                           
  1845.                         return 0xFF;  
  1846.                   }  
  1847.                       
  1848.                     /* 取出索引 */  
  1849.                     index = getSDOindex(m->data[1], m->data[2]);  
  1850.                     /* 取出子索引 */  
  1851.                     subIndex = getSDOsubIndex(m->data[3]);  

你可能感兴趣的:(STM32)