37 |
37 |
38 /* Uncomment if your compiler does not support inline functions */ |
38 /* Uncomment if your compiler does not support inline functions */ |
39 #define NO_INLINE |
39 #define NO_INLINE |
40 |
40 |
41 #ifdef NO_INLINE |
41 #ifdef NO_INLINE |
42 #define INLINE |
42 #define INLINE |
43 #else |
43 #else |
44 #define INLINE inline |
44 #define INLINE inline |
45 #endif |
45 #endif |
46 |
46 |
47 /*Internals prototypes*/ |
47 /*Internals prototypes*/ |
48 |
48 |
49 /*! |
49 /*! |
50 ** Called by writeNetworkDict |
50 ** Called by writeNetworkDict |
51 ** |
51 ** |
52 ** @param d |
52 ** @param d |
53 ** @param nodeId |
53 ** @param nodeId |
54 ** @param index |
54 ** @param index |
55 ** @param subIndex |
55 ** @param subIndex |
56 ** @param count |
56 ** @param count |
57 ** @param dataType |
57 ** @param dataType |
58 ** @param data |
58 ** @param data |
59 ** @param Callback |
59 ** @param Callback |
60 ** @param endianize |
60 ** @param endianize |
61 ** |
61 ** |
62 ** @return |
62 ** @return |
63 **/ |
63 **/ |
64 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
64 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
65 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize); |
65 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize); |
66 |
66 |
67 /*! |
67 /*! |
68 ** Called by readNetworkDict |
68 ** Called by readNetworkDict |
69 ** |
69 ** |
70 ** @param d |
70 ** @param d |
71 ** @param nodeId |
71 ** @param nodeId |
72 ** @param index |
72 ** @param index |
73 ** @param subIndex |
73 ** @param subIndex |
74 ** @param dataType |
74 ** @param dataType |
75 ** @param Callback |
75 ** @param Callback |
76 ** |
76 ** |
77 ** @return |
77 ** @return |
78 **/ |
78 **/ |
79 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, |
79 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, |
80 UNS8 dataType, SDOCallback_t Callback); |
80 UNS8 dataType, SDOCallback_t Callback); |
81 |
81 |
82 |
82 |
83 /***************************************************************************/ |
83 /***************************************************************************/ |
84 /* SDO (un)packing macros */ |
84 /* SDO (un)packing macros */ |
85 |
85 |
86 /** Returns the command specifier (cs, ccs, scs) from the first byte of the SDO |
86 /** Returns the command specifier (cs, ccs, scs) from the first byte of the SDO |
87 */ |
87 */ |
88 #define getSDOcs(byte) (byte >> 5) |
88 #define getSDOcs(byte) (byte >> 5) |
89 |
89 |
90 /** Returns the number of bytes without data from the first byte of the SDO. Coded in 2 bits |
90 /** Returns the number of bytes without data from the first byte of the SDO. Coded in 2 bits |
91 */ |
91 */ |
92 #define getSDOn2(byte) ((byte >> 2) & 3) |
92 #define getSDOn2(byte) ((byte >> 2) & 3) |
93 |
93 |
94 /** Returns the number of bytes without data from the first byte of the SDO. Coded in 3 bits |
94 /** Returns the number of bytes without data from the first byte of the SDO. Coded in 3 bits |
95 */ |
95 */ |
96 #define getSDOn3(byte) ((byte >> 1) & 7) |
96 #define getSDOn3(byte) ((byte >> 1) & 7) |
97 |
97 |
98 /** Returns the transfer type from the first byte of the SDO |
98 /** Returns the transfer type from the first byte of the SDO |
99 */ |
99 */ |
100 #define getSDOe(byte) ((byte >> 1) & 1) |
100 #define getSDOe(byte) ((byte >> 1) & 1) |
101 |
101 |
102 /** Returns the size indicator from the first byte of the SDO |
102 /** Returns the size indicator from the first byte of the SDO |
103 */ |
103 */ |
104 #define getSDOs(byte) (byte & 1) |
104 #define getSDOs(byte) (byte & 1) |
105 |
105 |
106 /** Returns the indicator of end transmission from the first byte of the SDO |
106 /** Returns the indicator of end transmission from the first byte of the SDO |
107 */ |
107 */ |
108 #define getSDOc(byte) (byte & 1) |
108 #define getSDOc(byte) (byte & 1) |
109 |
109 |
110 /** Returns the toggle from the first byte of the SDO |
110 /** Returns the toggle from the first byte of the SDO |
111 */ |
111 */ |
112 #define getSDOt(byte) ((byte >> 4) & 1) |
112 #define getSDOt(byte) ((byte >> 4) & 1) |
113 |
113 |
114 /** Returns the index from the bytes 1 and 2 of the SDO |
114 /** Returns the index from the bytes 1 and 2 of the SDO |
115 */ |
115 */ |
116 #define getSDOindex(byte1, byte2) (((UNS16)byte2 << 8) | ((UNS16)byte1)) |
116 #define getSDOindex(byte1, byte2) (((UNS16)byte2 << 8) | ((UNS16)byte1)) |
117 |
117 |
118 /** Returns the subIndex from the byte 3 of the SDO |
118 /** Returns the subIndex from the byte 3 of the SDO |
119 */ |
119 */ |
120 #define getSDOsubIndex(byte3) (byte3) |
120 #define getSDOsubIndex(byte3) (byte3) |
121 |
121 |
122 /*! |
122 /*! |
123 ** |
123 ** |
124 ** |
124 ** |
125 ** @param d |
125 ** @param d |
126 ** @param id |
126 ** @param id |
127 **/ |
127 **/ |
128 void SDOTimeoutAlarm(CO_Data* d, UNS32 id) |
128 void SDOTimeoutAlarm(CO_Data* d, UNS32 id) |
129 { |
129 { |
130 MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0); |
130 UNS16 offset; |
131 MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId); |
131 UNS8 nodeId; |
132 MSG_WAR(0x2A02, " index : ", d->transfers[id].index); |
132 /* Get the client->server cobid.*/ |
133 MSG_WAR(0x2A02, " subIndex : ", d->transfers[id].subIndex); |
133 offset = d->firstIndex->SDO_CLT; |
134 /* Reset timer handler */ |
134 if ((offset == 0) || ((offset+d->transfers[id].CliServNbr) > d->lastIndex->SDO_CLT)) { |
135 d->transfers[id].timer = TIMER_NONE; |
135 return ; |
136 /*Set aborted state*/ |
136 } |
137 d->transfers[id].state = SDO_ABORTED_INTERNAL; |
137 nodeId = *((UNS32*) d->objdict[offset+d->transfers[id].CliServNbr].pSubindex[3].pObject); |
138 /* Sending a SDO abort */ |
138 MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0); |
139 sendSDOabort(d, d->transfers[id].whoami, d->transfers[id].nodeId, |
139 MSG_WAR(0x2A02, "server node : ", NodeId); |
140 d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT); |
140 MSG_WAR(0x2A02, " index : ", d->transfers[id].index); |
141 d->transfers[id].abortCode = SDOABT_TIMED_OUT; |
141 MSG_WAR(0x2A02, " subIndex : ", d->transfers[id].subIndex); |
142 /* Call the user function to inform of the problem.*/ |
142 /* Reset timer handler */ |
143 if(d->transfers[id].Callback) |
143 d->transfers[id].timer = TIMER_NONE; |
144 /*If ther is a callback, it is responsible to close SDO transfer (client)*/ |
144 /*Set aborted state*/ |
145 (*d->transfers[id].Callback)(d,d->transfers[id].nodeId); |
145 d->transfers[id].state = SDO_ABORTED_INTERNAL; |
146 |
146 /* Sending a SDO abort */ |
147 /*Reset the line if (whoami == SDO_SERVER) or the callback did not close the line. |
147 sendSDOabort(d, d->transfers[id].whoami, d->transfers[id].CliServNbr, |
148 Otherwise this sdo transfer would never be closed. */ |
148 d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT); |
149 resetSDOline(d, (UNS8)id); |
149 d->transfers[id].abortCode = SDOABT_TIMED_OUT; |
|
150 /* Call the user function to inform of the problem.*/ |
|
151 if(d->transfers[id].Callback) |
|
152 /*If ther is a callback, it is responsible to close SDO transfer (client)*/ |
|
153 (*d->transfers[id].Callback)(d, nodeId); |
|
154 |
|
155 /*Reset the line if (whoami == SDO_SERVER) or the callback did not close the line. |
|
156 Otherwise this sdo transfer would never be closed. */ |
|
157 resetSDOline(d, (UNS8)id); |
150 } |
158 } |
151 |
159 |
152 #define StopSDO_TIMER(id) \ |
160 #define StopSDO_TIMER(id) \ |
153 MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\ |
161 MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\ |
154 d->transfers[id].timer = DelAlarm(d->transfers[id].timer); |
162 d->transfers[id].timer = DelAlarm(d->transfers[id].timer); |
155 |
163 |
156 #define StartSDO_TIMER(id) \ |
164 #define StartSDO_TIMER(id) \ |
157 MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\ |
165 MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\ |
158 d->transfers[id].timer = SetAlarm(d,id,&SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0); |
166 d->transfers[id].timer = SetAlarm(d,id,&SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0); |
159 |
167 |
160 #define RestartSDO_TIMER(id) \ |
168 #define RestartSDO_TIMER(id) \ |
161 MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\ |
169 MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\ |
162 if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) } |
170 if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) } |
163 |
171 |
164 /*! |
172 /*! |
165 ** Reset all sdo buffers |
173 ** Reset all sdo buffers |
166 ** |
174 ** |
167 ** @param d |
175 ** @param d |
168 **/ |
176 **/ |
169 void resetSDO (CO_Data* d) |
177 void resetSDO (CO_Data* d) |
170 { |
178 { |
171 UNS8 j; |
179 UNS8 j; |
172 |
180 |
173 /* transfer structure initialization */ |
181 /* transfer structure initialization */ |
174 for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++) |
182 for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++) |
175 resetSDOline(d, j); |
183 resetSDOline(d, j); |
176 } |
184 } |
177 |
185 |
178 /*! |
186 /*! |
179 ** |
187 ** |
180 ** |
188 ** |
181 ** @param d |
189 ** @param d |
182 ** @param line |
190 ** @param line |
183 ** |
191 ** |
184 ** @return |
192 ** @return |
185 **/ |
193 **/ |
186 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line) |
194 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line) |
187 { |
195 { |
188 UNS32 size; |
196 UNS32 size; |
189 UNS32 errorCode; |
197 UNS32 errorCode; |
190 MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line); |
198 MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line); |
191 /* if SDO initiated with e=0 and s=0 count is null, offset carry effective size*/ |
199 /* if SDO initiated with e=0 and s=0 count is null, offset carry effective size*/ |
192 if( d->transfers[line].count == 0) |
200 if( d->transfers[line].count == 0) |
193 d->transfers[line].count = d->transfers[line].offset; |
201 d->transfers[line].count = d->transfers[line].offset; |
194 size = d->transfers[line].count; |
202 size = d->transfers[line].count; |
195 |
203 |
196 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
204 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
197 if (size > SDO_MAX_LENGTH_TRANSFERT) |
205 if (size > SDO_MAX_LENGTH_TRANSFERT) |
198 { |
206 { |
199 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
207 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
200 (void *) d->transfers[line].dynamicData, &size, 1); |
208 (void *) d->transfers[line].dynamicData, &size, 1); |
201 } |
209 } |
202 else |
210 else |
203 { |
211 { |
204 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
212 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
205 (void *) d->transfers[line].data, &size, 1); |
213 (void *) d->transfers[line].data, &size, 1); |
206 } |
214 } |
207 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
215 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
208 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
216 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
209 (void *) d->transfers[line].data, &size, 1); |
217 (void *) d->transfers[line].data, &size, 1); |
210 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
218 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
211 |
219 |
212 if (errorCode != OD_SUCCESSFUL) |
220 if (errorCode != OD_SUCCESSFUL) |
213 return errorCode; |
221 return errorCode; |
214 MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line); |
222 MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line); |
215 return 0; |
223 return 0; |
216 |
224 |
217 } |
225 } |
218 |
226 |
219 /*! |
227 /*! |
220 ** |
228 ** |
221 ** |
229 ** |
222 ** @param d |
230 ** @param d |
223 ** @param line |
231 ** @param line |
224 ** |
232 ** |
225 ** @return |
233 ** @return |
226 **/ |
234 **/ |
227 UNS32 objdictToSDOline (CO_Data* d, UNS8 line) |
235 UNS32 objdictToSDOline (CO_Data* d, UNS8 line) |
228 { |
236 { |
229 UNS32 size = 0; |
237 UNS32 size = 0; |
230 UNS8 dataType; |
238 UNS8 dataType; |
231 UNS32 errorCode; |
239 UNS32 errorCode; |
232 |
240 |
233 MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index); |
241 MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index); |
234 MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex); |
242 MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex); |
235 |
243 |
236 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
244 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
237 //TODO: Read the size of the object. Depending o it put data into data or dynamicData |
245 //TODO: Read the size of the object. Depending o it put data into data or dynamicData |
238 errorCode = getODentry(d, d->transfers[line].index, |
246 errorCode = getODentry(d, d->transfers[line].index, |
239 d->transfers[line].subIndex, |
247 d->transfers[line].subIndex, |
240 (void *)d->transfers[line].data, |
248 (void *)d->transfers[line].data, |
241 &size, &dataType, 1); |
249 &size, &dataType, 1); |
242 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
250 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
243 errorCode = getODentry(d, d->transfers[line].index, |
251 errorCode = getODentry(d, d->transfers[line].index, |
244 d->transfers[line].subIndex, |
252 d->transfers[line].subIndex, |
245 (void *)d->transfers[line].data, |
253 (void *)d->transfers[line].data, |
246 &size, &dataType, 1); |
254 &size, &dataType, 1); |
247 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
255 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
248 |
256 |
249 if (errorCode != OD_SUCCESSFUL) |
257 if (errorCode != OD_SUCCESSFUL) |
250 return errorCode; |
258 return errorCode; |
251 |
259 |
252 d->transfers[line].count = size; |
260 d->transfers[line].count = size; |
253 d->transfers[line].offset = 0; |
261 d->transfers[line].offset = 0; |
254 |
262 |
255 return 0; |
263 return 0; |
256 } |
264 } |
257 |
265 |
258 /*! |
266 /*! |
259 ** |
267 ** |
260 ** |
268 ** |
261 ** @param d |
269 ** @param d |
262 ** @param line |
270 ** @param line |
263 ** @param nbBytes |
271 ** @param nbBytes |
264 ** @param data |
272 ** @param data |
265 ** |
273 ** |
266 ** @return |
274 ** @return |
267 **/ |
275 **/ |
268 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) { |
276 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) { |
269 UNS8 i; |
277 UNS8 i; |
270 UNS32 offset; |
278 UNS32 offset; |
271 |
279 |
272 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
280 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
273 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
281 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
274 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
282 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
275 return 0xFF; |
283 return 0xFF; |
276 } |
284 } |
277 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
285 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
278 |
286 |
279 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) { |
287 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) { |
280 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes); |
288 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes); |
281 return 0xFF; |
289 return 0xFF; |
282 } |
290 } |
283 offset = d->transfers[line].offset; |
291 offset = d->transfers[line].offset; |
284 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
292 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
285 if (d->transfers[line].count <= SDO_MAX_LENGTH_TRANSFERT) |
293 if (d->transfers[line].count <= SDO_MAX_LENGTH_TRANSFERT) |
286 { |
294 { |
287 for (i = 0 ; i < nbBytes ; i++) |
295 for (i = 0 ; i < nbBytes ; i++) |
288 * (data + i) = d->transfers[line].data[offset + i]; |
296 * (data + i) = d->transfers[line].data[offset + i]; |
289 } |
297 } |
290 else |
298 else |
291 { |
299 { |
292 if (d->transfers[line].dynamicData == NULL) |
300 if (d->transfers[line].dynamicData == NULL) |
293 { |
301 { |
294 MSG_ERR(0x1A11,"SDO's dynamic buffer not allocated. Line", line); |
302 MSG_ERR(0x1A11,"SDO's dynamic buffer not allocated. Line", line); |
295 return 0xFF; |
303 return 0xFF; |
296 } |
304 } |
297 for (i = 0 ; i < nbBytes ; i++) |
305 for (i = 0 ; i < nbBytes ; i++) |
298 * (data + i) = d->transfers[line].dynamicData[offset + i]; |
306 * (data + i) = d->transfers[line].dynamicData[offset + i]; |
299 } |
307 } |
300 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
308 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
301 for (i = 0 ; i < nbBytes ; i++) |
309 for (i = 0 ; i < nbBytes ; i++) |
302 * (data + i) = d->transfers[line].data[offset + i]; |
310 * (data + i) = d->transfers[line].data[offset + i]; |
303 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
311 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
304 d->transfers[line].offset = d->transfers[line].offset + nbBytes; |
312 d->transfers[line].offset = d->transfers[line].offset + nbBytes; |
305 return 0; |
313 return 0; |
306 } |
314 } |
307 |
315 |
308 /*! |
316 /*! |
309 ** |
317 ** |
310 ** |
318 ** |
311 ** @param d |
319 ** @param d |
312 ** @param line |
320 ** @param line |
313 ** @param nbBytes |
321 ** @param nbBytes |
314 ** @param data |
322 ** @param data |
315 ** |
323 ** |
316 ** @return |
324 ** @return |
317 **/ |
325 **/ |
318 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) |
326 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) |
319 { |
327 { |
320 UNS8 i; |
328 UNS8 i; |
321 UNS32 offset; |
329 UNS32 offset; |
322 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
330 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
323 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
331 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
324 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
332 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
325 return 0xFF; |
333 return 0xFF; |
326 } |
334 } |
327 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
335 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
328 |
336 |
329 offset = d->transfers[line].offset; |
337 offset = d->transfers[line].offset; |
330 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
338 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
331 { |
339 { |
332 UNS8* lineData = d->transfers[line].data; |
340 UNS8* lineData = d->transfers[line].data; |
333 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
341 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
334 if (d->transfers[line].dynamicData == NULL) { |
342 if (d->transfers[line].dynamicData == NULL) { |
335 d->transfers[line].dynamicData = (UNS8*) malloc(SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
343 d->transfers[line].dynamicData = (UNS8*) malloc(SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
336 d->transfers[line].dynamicDataSize = SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE; |
344 d->transfers[line].dynamicDataSize = SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE; |
337 |
345 |
338 if (d->transfers[line].dynamicData == NULL) { |
346 if (d->transfers[line].dynamicData == NULL) { |
339 MSG_ERR(0x1A15,"SDO allocating dynamic buffer failed, size", SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
347 MSG_ERR(0x1A15,"SDO allocating dynamic buffer failed, size", SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
340 return 0xFF; |
348 return 0xFF; |
341 } |
349 } |
342 //Copy present data |
350 //Copy present data |
343 memcpy(d->transfers[line].dynamicData, d->transfers[line].data, offset); |
351 memcpy(d->transfers[line].dynamicData, d->transfers[line].data, offset); |
344 } |
352 } |
345 else if ((d->transfers[line].offset + nbBytes) > d->transfers[line].dynamicDataSize) |
353 else if ((d->transfers[line].offset + nbBytes) > d->transfers[line].dynamicDataSize) |
346 { |
354 { |
347 UNS8* newDynamicBuffer = (UNS8*) realloc(d->transfers[line].dynamicData, d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
355 UNS8* newDynamicBuffer = (UNS8*) realloc(d->transfers[line].dynamicData, d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
348 if (newDynamicBuffer == NULL) { |
356 if (newDynamicBuffer == NULL) { |
349 MSG_ERR(0x1A15,"SDO reallocating dynamic buffer failed, size", d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
357 MSG_ERR(0x1A15,"SDO reallocating dynamic buffer failed, size", d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
350 return 0xFF; |
358 return 0xFF; |
351 } |
359 } |
352 d->transfers[line].dynamicData = newDynamicBuffer; |
360 d->transfers[line].dynamicData = newDynamicBuffer; |
353 d->transfers[line].dynamicDataSize += SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE; |
361 d->transfers[line].dynamicDataSize += SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE; |
354 } |
362 } |
355 lineData = d->transfers[line].dynamicData; |
363 lineData = d->transfers[line].dynamicData; |
356 } |
364 } |
357 |
365 |
358 for (i = 0 ; i < nbBytes ; i++) |
366 for (i = 0 ; i < nbBytes ; i++) |
359 lineData[offset + i] = * (data + i); |
367 lineData[offset + i] = * (data + i); |
360 } |
368 } |
361 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
369 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
362 for (i = 0 ; i < nbBytes ; i++) |
370 for (i = 0 ; i < nbBytes ; i++) |
363 d->transfers[line].data[offset + i] = * (data + i); |
371 d->transfers[line].data[offset + i] = * (data + i); |
364 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
372 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
365 |
373 |
366 d->transfers[line].offset = d->transfers[line].offset + nbBytes; |
374 d->transfers[line].offset = d->transfers[line].offset + nbBytes; |
367 return 0; |
375 return 0; |
368 } |
376 } |
369 |
377 |
370 /*! |
378 /*! |
371 ** |
379 ** |
372 ** |
380 ** |
373 ** @param d |
381 ** @param d |
374 ** @param nodeId |
382 ** @param CliServNbr |
375 ** @param whoami |
383 ** @param whoami |
376 ** @param index |
384 ** @param index |
377 ** @param subIndex |
385 ** @param subIndex |
378 ** @param abortCode |
386 ** @param abortCode |
379 ** |
387 ** |
380 ** @return |
388 ** @return |
381 **/ |
389 **/ |
382 UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index, |
390 UNS8 failedSDO (CO_Data* d, UNS8 CliServNbr, UNS8 whoami, UNS16 index, |
383 UNS8 subIndex, UNS32 abortCode) |
391 UNS8 subIndex, UNS32 abortCode) |
384 { |
392 { |
385 UNS8 err; |
393 UNS8 err; |
386 UNS8 line; |
394 UNS8 line; |
387 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
395 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
388 if (!err) /* If a line on use have been found.*/ |
396 if (!err) /* If a line on use have been found.*/ |
389 MSG_WAR(0x3A20, "FailedSDO : line found : ", line); |
397 MSG_WAR(0x3A20, "FailedSDO : line found : ", line); |
390 if ((! err) && (whoami == SDO_SERVER)) { |
398 if ((! err) && (whoami == SDO_SERVER)) { |
391 resetSDOline( d, line ); |
399 resetSDOline( d, line ); |
392 MSG_WAR(0x3A21, "FailedSDO : line released : ", line); |
400 MSG_WAR(0x3A21, "FailedSDO : line released : ", line); |
393 } |
401 } |
394 if ((! err) && (whoami == SDO_CLIENT)) { |
402 if ((! err) && (whoami == SDO_CLIENT)) { |
395 StopSDO_TIMER(line); |
403 StopSDO_TIMER(line); |
396 d->transfers[line].state = SDO_ABORTED_INTERNAL; |
404 d->transfers[line].state = SDO_ABORTED_INTERNAL; |
397 } |
405 } |
398 MSG_WAR(0x3A22, "Sending SDO abort ", 0); |
406 MSG_WAR(0x3A22, "Sending SDO abort ", 0); |
399 err = sendSDOabort(d, whoami, nodeId, index, subIndex, abortCode); |
407 err = sendSDOabort(d, whoami, CliServNbr, index, subIndex, abortCode); |
400 if (err) { |
408 if (err) { |
401 MSG_WAR(0x3A23, "Unable to send the SDO abort", 0); |
409 MSG_WAR(0x3A23, "Unable to send the SDO abort", 0); |
402 return 0xFF; |
410 return 0xFF; |
403 } |
411 } |
404 return 0; |
412 return 0; |
405 } |
413 } |
406 |
414 |
407 /*! |
415 /*! |
408 ** |
416 ** |
409 ** |
417 ** |
410 ** @param d |
418 ** @param d |
411 ** @param line |
419 ** @param line |
412 **/ |
420 **/ |
413 void resetSDOline ( CO_Data* d, UNS8 line ) |
421 void resetSDOline ( CO_Data* d, UNS8 line ) |
414 { |
422 { |
415 UNS32 i; |
423 UNS32 i; |
416 MSG_WAR(0x3A25, "reset SDO line nb : ", line); |
424 MSG_WAR(0x3A25, "reset SDO line nb : ", line); |
417 initSDOline(d, line, 0, 0, 0, SDO_RESET); |
425 initSDOline(d, line, 0, 0, 0, SDO_RESET); |
418 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++) |
426 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++) |
419 d->transfers[line].data[i] = 0; |
427 d->transfers[line].data[i] = 0; |
420 d->transfers[line].whoami = 0; |
428 d->transfers[line].whoami = 0; |
421 d->transfers[line].abortCode = 0; |
429 d->transfers[line].abortCode = 0; |
422 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
430 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
423 free(d->transfers[line].dynamicData); |
431 free(d->transfers[line].dynamicData); |
424 d->transfers[line].dynamicData = 0; |
432 d->transfers[line].dynamicData = 0; |
425 d->transfers[line].dynamicDataSize = 0; |
433 d->transfers[line].dynamicDataSize = 0; |
426 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
434 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
427 } |
435 } |
428 |
436 |
429 /*! |
437 /*! |
430 ** |
438 ** |
431 ** |
439 ** |
432 ** @param d |
440 ** @param d |
433 ** @param line |
441 ** @param line |
434 ** @param nodeId |
442 ** @param CliServNbr |
435 ** @param index |
443 ** @param index |
436 ** @param subIndex |
444 ** @param subIndex |
437 ** @param state |
445 ** @param state |
438 ** |
446 ** |
439 ** @return |
447 ** @return |
440 **/ |
448 **/ |
441 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state) |
449 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 CliServNbr, UNS16 index, UNS8 subIndex, UNS8 state) |
442 { |
450 { |
443 MSG_WAR(0x3A25, "init SDO line nb : ", line); |
451 MSG_WAR(0x3A25, "init SDO line nb : ", line); |
444 if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){ |
452 if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){ |
445 StartSDO_TIMER(line) |
453 StartSDO_TIMER(line) |
446 }else{ |
454 }else{ |
447 StopSDO_TIMER(line) |
455 StopSDO_TIMER(line) |
448 } |
456 } |
449 d->transfers[line].nodeId = nodeId; |
457 d->transfers[line].CliServNbr = CliServNbr; |
450 d->transfers[line].index = index; |
458 d->transfers[line].index = index; |
451 d->transfers[line].subIndex = subIndex; |
459 d->transfers[line].subIndex = subIndex; |
452 d->transfers[line].state = state; |
460 d->transfers[line].state = state; |
453 d->transfers[line].toggle = 0; |
461 d->transfers[line].toggle = 0; |
454 d->transfers[line].count = 0; |
462 d->transfers[line].count = 0; |
455 d->transfers[line].offset = 0; |
463 d->transfers[line].offset = 0; |
456 d->transfers[line].dataType = 0; |
464 d->transfers[line].dataType = 0; |
457 d->transfers[line].Callback = NULL; |
465 d->transfers[line].Callback = NULL; |
458 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
466 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
459 free(d->transfers[line].dynamicData); |
467 free(d->transfers[line].dynamicData); |
460 d->transfers[line].dynamicData = 0; |
468 d->transfers[line].dynamicData = 0; |
461 d->transfers[line].dynamicDataSize = 0; |
469 d->transfers[line].dynamicDataSize = 0; |
462 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
470 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
463 return 0; |
471 return 0; |
464 } |
472 } |
465 |
473 |
466 /*! |
474 /*! |
467 ** |
475 ** |
468 ** |
476 ** |
469 ** @param d |
477 ** @param d |
470 ** @param whoami |
478 ** @param whoami |
471 ** @param line |
479 ** @param line |
472 ** |
480 ** |
473 ** @return |
481 ** @return |
474 **/ |
482 **/ |
475 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line ) |
483 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line ) |
476 { |
484 { |
477 |
485 |
478 UNS8 i; |
486 UNS8 i; |
479 |
487 |
480 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ |
488 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ |
481 if ( d->transfers[i].state == SDO_RESET ) { |
489 if ( d->transfers[i].state == SDO_RESET ) { |
482 *line = i; |
490 *line = i; |
483 d->transfers[i].whoami = whoami; |
491 d->transfers[i].whoami = whoami; |
484 return 0; |
492 return 0; |
485 } /* end if */ |
493 } /* end if */ |
486 } /* end for */ |
494 } /* end for */ |
487 MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i); |
495 MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i); |
488 return 0xFF; |
496 return 0xFF; |
489 } |
497 } |
490 |
498 |
491 /*! |
499 /*! |
492 ** |
500 ** |
493 ** |
501 ** |
494 ** @param d |
502 ** @param d |
495 ** @param nodeId |
503 ** @param CliServNbr |
496 ** @param whoami |
504 ** @param whoami |
497 ** @param line |
505 ** @param line |
498 ** |
506 ** |
499 ** @return |
507 ** @return |
500 **/ |
508 **/ |
501 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line) |
509 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 CliServNbr, UNS8 whoami, UNS8 *line) |
502 { |
510 { |
503 |
511 |
504 UNS8 i; |
512 UNS8 i; |
505 |
513 |
506 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ |
514 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ |
507 if ( (d->transfers[i].state != SDO_RESET) && |
515 if ( (d->transfers[i].state != SDO_RESET) && |
508 (d->transfers[i].state != SDO_ABORTED_INTERNAL) && |
516 (d->transfers[i].state != SDO_ABORTED_INTERNAL) && |
509 (d->transfers[i].nodeId == nodeId) && |
517 (d->transfers[i].CliServNbr == CliServNbr) && |
510 (d->transfers[i].whoami == whoami) ) { |
518 (d->transfers[i].whoami == whoami) ) { |
511 if (line) *line = i; |
519 if (line) *line = i; |
512 return 0; |
520 return 0; |
513 } |
521 } |
514 } |
522 } |
515 return 0xFF; |
523 return 0xFF; |
516 } |
524 } |
517 |
525 |
518 /*! |
526 /*! |
519 ** |
527 ** |
520 ** |
528 ** |
521 ** @param d |
529 ** @param d |
522 ** @param nodeId |
530 ** @param CliServNbr |
523 ** @param whoami |
531 ** @param whoami |
524 ** @param line |
532 ** @param line |
525 ** |
533 ** |
526 ** @return |
534 ** @return |
527 **/ |
535 **/ |
528 UNS8 getSDOlineToClose (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line) |
536 UNS8 getSDOlineToClose (CO_Data* d, UNS8 CliServNbr, UNS8 whoami, UNS8 *line) |
529 { |
537 { |
530 |
538 |
531 UNS8 i; |
539 UNS8 i; |
532 |
540 |
533 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ |
541 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ |
534 if ( (d->transfers[i].state != SDO_RESET) && |
542 if ( (d->transfers[i].state != SDO_RESET) && |
535 (d->transfers[i].nodeId == nodeId) && |
543 (d->transfers[i].CliServNbr == CliServNbr) && |
536 (d->transfers[i].whoami == whoami) ) { |
544 (d->transfers[i].whoami == whoami) ) { |
537 if (line) *line = i; |
545 if (line) *line = i; |
538 return 0; |
546 return 0; |
539 } |
547 } |
540 } |
548 } |
541 return 0xFF; |
549 return 0xFF; |
542 } |
550 } |
543 |
551 |
544 |
552 |
545 /*! |
553 /*! |
546 ** |
554 ** |
547 ** |
555 ** |
548 ** @param d |
556 ** @param d |
549 ** @param nodeId |
557 ** @param CliServNbr |
550 ** @param whoami |
558 ** @param whoami |
551 ** |
559 ** |
552 ** @return |
560 ** @return |
553 **/ |
561 **/ |
554 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami) |
562 UNS8 closeSDOtransfer (CO_Data* d, UNS8 CliServNbr, UNS8 whoami) |
555 { |
563 { |
556 UNS8 err; |
564 UNS8 err; |
557 UNS8 line; |
565 UNS8 line; |
558 err = getSDOlineToClose(d, nodeId, whoami, &line); |
566 err = getSDOlineToClose(d, CliServNbr, whoami, &line); |
559 if (err) { |
567 if (err) { |
560 MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId); |
568 MSG_WAR(0x2A30, "No SDO communication to close", 0); |
561 return 0xFF; |
569 return 0xFF; |
562 } |
570 } |
563 resetSDOline(d, line); |
571 resetSDOline(d, line); |
564 return 0; |
572 return 0; |
565 } |
573 } |
566 |
574 |
567 /*! |
575 /*! |
568 ** |
576 ** |
569 ** |
577 ** |
570 ** @param d |
578 ** @param d |
571 ** @param line |
579 ** @param line |
572 ** @param nbBytes |
580 ** @param nbBytes |
573 ** |
581 ** |
574 ** @return |
582 ** @return |
575 **/ |
583 **/ |
576 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS32 * nbBytes) |
584 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS32 * nbBytes) |
577 { |
585 { |
578 /* SDO initiated with e=0 and s=0 have count set to null */ |
586 /* SDO initiated with e=0 and s=0 have count set to null */ |
579 if (d->transfers[line].count == 0) |
587 if (d->transfers[line].count == 0) |
580 * nbBytes = 0; |
588 * nbBytes = 0; |
581 else |
589 else |
582 * nbBytes = d->transfers[line].count - d->transfers[line].offset; |
590 * nbBytes = d->transfers[line].count - d->transfers[line].offset; |
583 return 0; |
591 return 0; |
584 } |
592 } |
585 |
593 |
586 /*! |
594 /*! |
587 ** |
595 ** |
588 ** |
596 ** |
589 ** @param d |
597 ** @param d |
590 ** @param line |
598 ** @param line |
591 ** @param nbBytes |
599 ** @param nbBytes |
592 ** |
600 ** |
593 ** @return |
601 ** @return |
594 **/ |
602 **/ |
595 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS32 nbBytes) |
603 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS32 nbBytes) |
596 { |
604 { |
597 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
605 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
598 if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) { |
606 if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) { |
599 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
607 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
600 return 0xFF; |
608 return 0xFF; |
601 } |
609 } |
602 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
610 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
603 |
611 |
604 d->transfers[line].count = nbBytes; |
612 d->transfers[line].count = nbBytes; |
605 return 0; |
613 return 0; |
606 } |
614 } |
607 |
615 |
608 /*! |
616 /*! |
609 ** |
617 ** |
610 ** |
618 ** |
611 ** @param d |
619 ** @param d |
612 ** @param whoami |
620 ** @param whoami |
613 ** @param sdo |
621 ** @param CliServNbr |
614 ** |
622 ** @param pData |
615 ** @return |
623 ** |
616 **/ |
624 ** @return |
617 UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo) |
625 **/ |
618 { |
626 UNS8 sendSDO (CO_Data* d, UNS8 whoami, UNS8 CliServNbr, UNS8 *pData) |
619 UNS16 offset; |
627 { |
620 UNS16 lastIndex; |
628 UNS16 offset; |
621 UNS8 found = 0; |
629 UNS8 i; |
622 Message m; |
630 Message m; |
623 UNS8 i; |
631 |
624 UNS32 * pwCobId = NULL; |
632 MSG_WAR(0x3A38, "sendSDO",0); |
625 UNS8 * pwNodeId = NULL; |
633 if( !((d->nodeState == Operational) || (d->nodeState == Pre_operational ))) { |
626 |
634 MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState); |
627 MSG_WAR(0x3A38, "sendSDO",0); |
635 return 0xFF; |
628 if( !((d->nodeState == Operational) || (d->nodeState == Pre_operational ))) { |
636 } |
629 MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState); |
637 |
630 return 0xFF; |
638 /*get the server->client cobid*/ |
631 } |
639 if ( whoami == SDO_SERVER ) { |
632 |
640 offset = d->firstIndex->SDO_SVR; |
633 /*get the server->client cobid*/ |
641 if ((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_SVR)) { |
634 if ( whoami == SDO_SERVER ) {/*case server. only one SDO server*/ |
642 MSG_ERR(0x1A42, "SendSDO : SDO server not found", 0); |
635 offset = d->firstIndex->SDO_SVR; |
643 return 0xFF; |
636 if (offset == 0) { |
644 } |
637 MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0); |
645 m.cob_id = *((UNS32*) d->objdict[offset+CliServNbr].pSubindex[2].pObject); |
638 return 0xFF; |
646 MSG_WAR(0x3A41, "I am server cobId : ", m.cob_id); |
639 } |
647 } |
640 pwCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject; |
648 else { /*case client*/ |
641 MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId); |
649 /* Get the client->server cobid.*/ |
642 } |
650 offset = d->firstIndex->SDO_CLT; |
643 else { /*case client*/ |
651 if ((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_CLT)) { |
644 /* Get the client->server cobid.*/ |
652 MSG_ERR(0x1A42, "SendSDO : SDO client not found", 0); |
645 UNS16 sdoNum = 0; |
653 return 0xFF; |
646 offset = d->firstIndex->SDO_CLT; |
654 } |
647 lastIndex = d->lastIndex->SDO_CLT; |
655 m.cob_id = *((UNS32*) d->objdict[offset+CliServNbr].pSubindex[1].pObject); |
648 if (offset == 0) { |
656 MSG_WAR(0x3A41, "I am client cobId : ", m.cob_id); |
649 MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0); |
657 } |
650 return 0xFF; |
658 /* message copy for sending */ |
651 } |
659 m.rtr = NOT_A_REQUEST; |
652 /* find index for communication server node */ |
660 /* the length of SDO must be 8 */ |
653 while (offset <= lastIndex){ |
661 m.len = 8; |
654 MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum); |
662 for (i = 0 ; i < 8 ; i++) { |
655 if (d->objdict[offset].bSubCount <= 3) { |
663 m.data[i] = pData[i]; |
656 MSG_ERR(0x1A28, "Subindex 3 not found at index ", 0x1280 + sdoNum); |
664 } |
657 return 0xFF; |
665 return canSend(d->canHandle,&m); |
658 } |
666 } |
659 pwNodeId = (UNS8*) d->objdict[offset].pSubindex[3].pObject; |
667 |
660 MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId); |
668 /*! |
661 if(*pwNodeId == sdo.nodeId) { |
669 ** |
662 found = 1; |
670 ** |
663 break; |
671 ** @param d |
664 } |
672 ** @param whoami |
665 offset ++; |
673 ** @param index |
666 sdoNum ++; |
674 ** @param subIndex |
667 } |
675 ** @param abortCode |
668 if (! found){ |
676 ** |
669 MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId); |
677 ** @return |
670 return 0xFF; |
678 **/ |
671 } |
679 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS8 CliServNbr, UNS16 index, UNS8 subIndex, UNS32 abortCode) |
672 /* read the client->server cobid */ |
680 { |
673 pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
681 UNS8 data[8]; |
674 } |
682 UNS8 ret; |
675 /* message copy for sending */ |
683 |
676 m.cob_id = (UNS16)UNS16_LE(*pwCobId); |
684 MSG_WAR(0x2A50,"Sending SDO abort ", abortCode); |
677 m.rtr = NOT_A_REQUEST; |
685 data[0] = 0x80; |
678 /* the length of SDO must be 8 */ |
686 /* Index */ |
679 m.len = 8; |
687 data[1] = index & 0xFF; /* LSB */ |
680 for (i = 0 ; i < 8 ; i++) { |
688 data[2] = (index >> 8) & 0xFF; /* MSB */ |
681 m.data[i] = sdo.body.data[i]; |
689 /* Subindex */ |
682 } |
690 data[3] = subIndex; |
683 return canSend(d->canHandle,&m); |
691 /* Data */ |
684 } |
692 data[4] = (UNS8)(abortCode & 0xFF); |
685 |
693 data[5] = (UNS8)((abortCode >> 8) & 0xFF); |
686 /*! |
694 data[6] = (UNS8)((abortCode >> 16) & 0xFF); |
687 ** |
695 data[7] = (UNS8)((abortCode >> 24) & 0xFF); |
688 ** |
696 ret = sendSDO(d, whoami, CliServNbr, data); |
689 ** @param d |
697 |
690 ** @param whoami |
698 return ret; |
691 ** @param index |
699 } |
692 ** @param subIndex |
700 |
693 ** @param abortCode |
701 /*! |
694 ** |
702 ** |
695 ** @return |
703 ** |
696 **/ |
704 ** @param d |
697 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS8 nodeID, UNS16 index, UNS8 subIndex, UNS32 abortCode) |
705 ** @param m |
698 { |
706 ** |
699 s_SDO sdo; |
707 ** @return |
700 UNS8 ret; |
708 **/ |
701 |
|
702 MSG_WAR(0x2A50,"Sending SDO abort ", abortCode); |
|
703 if(whoami == SDO_SERVER) |
|
704 { |
|
705 sdo.nodeId = *d->bDeviceNodeId; |
|
706 } |
|
707 else |
|
708 { |
|
709 sdo.nodeId = nodeID; |
|
710 } |
|
711 sdo.body.data[0] = 0x80; |
|
712 /* Index */ |
|
713 sdo.body.data[1] = index & 0xFF; /* LSB */ |
|
714 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
|
715 /* Subindex */ |
|
716 sdo.body.data[3] = subIndex; |
|
717 /* Data */ |
|
718 sdo.body.data[4] = (UNS8)(abortCode & 0xFF); |
|
719 sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF); |
|
720 sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF); |
|
721 sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF); |
|
722 ret = sendSDO(d, whoami, sdo); |
|
723 |
|
724 return ret; |
|
725 } |
|
726 |
|
727 /*! |
|
728 ** |
|
729 ** |
|
730 ** @param d |
|
731 ** @param m |
|
732 ** |
|
733 ** @return |
|
734 **/ |
|
735 UNS8 proceedSDO (CO_Data* d, Message *m) |
709 UNS8 proceedSDO (CO_Data* d, Message *m) |
736 { |
710 { |
737 UNS8 err; |
711 UNS8 err; |
738 UNS8 line; |
712 UNS8 line; |
739 UNS32 nbBytes; /* received or to be transmited. */ |
713 UNS32 nbBytes; /* received or to be transmited. */ |
740 UNS8 nodeId = 0; /* The node from which the SDO is received */ |
714 UNS8 nodeId = 0; /* The node Id of the server if client otherwise unused */ |
741 UNS8 *pNodeId = NULL; |
715 UNS8 CliServNbr; |
742 UNS8 whoami = SDO_UNKNOWN; /* SDO_SERVER or SDO_CLIENT.*/ |
716 UNS8 whoami = SDO_UNKNOWN; /* SDO_SERVER or SDO_CLIENT.*/ |
743 UNS32 errorCode; /* while reading or writing in the local object dictionary.*/ |
717 UNS32 errorCode; /* while reading or writing in the local object dictionary.*/ |
744 s_SDO sdo; /* SDO to transmit */ |
718 UNS8 data[8]; /* data for SDO to transmit */ |
745 UNS16 index; |
719 UNS16 index; |
746 UNS8 subIndex; |
720 UNS8 subIndex; |
747 UNS32 abortCode; |
721 UNS32 abortCode; |
748 UNS32 i; |
722 UNS32 i; |
749 UNS8 j; |
723 UNS8 j; |
750 UNS32 *pCobId = NULL; |
724 UNS32 *pCobId = NULL; |
751 UNS16 offset; |
725 UNS16 offset; |
752 UNS16 lastIndex; |
726 UNS16 lastIndex; |
753 |
727 |
754 MSG_WAR(0x3A60, "proceedSDO ", 0); |
728 MSG_WAR(0x3A60, "proceedSDO ", 0); |
755 whoami = SDO_UNKNOWN; |
729 whoami = SDO_UNKNOWN; |
756 /* Looking for the cobId in the object dictionary. */ |
730 /* Looking for the cobId in the object dictionary. */ |
757 /* Am-I a server ? */ |
731 /* Am-I a server ? */ |
758 offset = d->firstIndex->SDO_SVR; |
732 offset = d->firstIndex->SDO_SVR; |
759 lastIndex = d->lastIndex->SDO_SVR; |
733 lastIndex = d->lastIndex->SDO_SVR; |
760 j = 0; |
734 j = 0; |
761 if(offset) while (offset <= lastIndex) { |
735 if(offset) while (offset <= lastIndex) { |
762 if (d->objdict[offset].bSubCount <= 1) { |
736 if (d->objdict[offset].bSubCount <= 1) { |
763 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j); |
737 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j); |
764 return 0xFF; |
738 return 0xFF; |
765 } |
739 } |
766 pCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
740 /* Looking for the cobid received. */ |
767 if ( *pCobId == UNS16_LE(m->cob_id) ) { |
741 pCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
768 whoami = SDO_SERVER; |
742 if ( *pCobId == UNS16_LE(m->cob_id) ) { |
769 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j); |
743 whoami = SDO_SERVER; |
770 /* In case of server, the node id of the client may be unknown. So we put the index minus offset */ |
744 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j); |
771 /* 0x1200 where the cobid received is defined. */ |
745 /* Defining Server number = index minus 0x1200 where the cobid received is defined. */ |
772 nodeId = j; |
746 CliServNbr = j; |
773 break; |
747 break; |
774 } |
748 } |
775 j++; |
749 j++; |
776 offset++; |
750 offset++; |
777 } /* end while */ |
751 } /* end while */ |
778 if (whoami == SDO_UNKNOWN) { |
752 if (whoami == SDO_UNKNOWN) { |
779 /* Am-I client ? */ |
753 /* Am-I client ? */ |
780 offset = d->firstIndex->SDO_CLT; |
754 offset = d->firstIndex->SDO_CLT; |
781 lastIndex = d->lastIndex->SDO_CLT; |
755 lastIndex = d->lastIndex->SDO_CLT; |
782 j = 0; |
756 j = 0; |
783 if(offset) while (offset <= lastIndex) { |
757 if(offset) while (offset <= lastIndex) { |
784 if (d->objdict[offset].bSubCount <= 3) { |
758 if (d->objdict[offset].bSubCount <= 3) { |
785 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j); |
759 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j); |
786 return 0xFF; |
760 return 0xFF; |
787 } |
761 } |
788 /* a) Looking for the cobid received. */ |
762 /* Looking for the cobid received. */ |
789 pCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject; |
763 pCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject; |
790 if (*pCobId == UNS16_LE(m->cob_id) ) { |
764 if (*pCobId == UNS16_LE(m->cob_id) ) { |
791 /* b) cobid found, so reading the node id of the server. */ |
765 whoami = SDO_CLIENT; |
792 pNodeId = (UNS8*) d->objdict[offset].pSubindex[3].pObject; |
766 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j); |
793 whoami = SDO_CLIENT; |
767 /* Defining Client number = index minus 0x1280 where the cobid received is defined. */ |
794 nodeId = *pNodeId; |
768 CliServNbr = j; |
795 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j); |
769 /* Reading the server node ID, if client it is mandatory in the OD */ |
796 MSG_WAR(0x3A65, " Server nodeId : ", nodeId); |
770 nodeId = *((UNS8*) d->objdict[offset].pSubindex[3].pObject); |
797 break; |
771 break; |
798 } |
772 } |
799 j++; |
773 j++; |
800 offset++; |
774 offset++; |
801 } /* end while */ |
775 } /* end while */ |
802 } |
776 } |
803 if (whoami == SDO_UNKNOWN) { |
777 if (whoami == SDO_UNKNOWN) { |
804 return 0xFF;/* This SDO was not for us ! */ |
778 return 0xFF;/* This SDO was not for us ! */ |
805 } |
779 } |
806 |
780 |
807 /* Test if the size of the SDO is ok */ |
781 /* Test if the size of the SDO is ok */ |
808 if ( (*m).len != 8) { |
782 if ( (*m).len != 8) { |
809 MSG_ERR(0x1A67, "Error size SDO. CobId : ", UNS16_LE(m->cob_id)); |
783 MSG_ERR(0x1A67, "Error size SDO", 0); |
810 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR); |
784 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_GENERAL_ERROR); |
811 return 0xFF; |
785 return 0xFF; |
812 } |
786 } |
813 |
787 |
814 if (whoami == SDO_CLIENT) { |
788 if (whoami == SDO_CLIENT) { |
815 MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId); |
789 MSG_WAR(0x3A68, "I am CLIENT number ", CliServNbr); |
816 } |
790 } |
817 else { |
791 else { |
818 MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", UNS16_LE(m->cob_id)); |
792 MSG_WAR(0x3A69, "I am SERVER number ", CliServNbr); |
819 } |
793 } |
820 |
794 |
821 /* Testing the command specifier */ |
795 /* Testing the command specifier */ |
822 /* Allowed : cs = 0, 1, 2, 3, 4. (= all except those for block tranfert). */ |
796 /* Allowed : cs = 0, 1, 2, 3, 4. (= all except those for block tranfert). */ |
823 /* cs = other : Not allowed -> abort. */ |
797 /* cs = other : Not allowed -> abort. */ |
824 switch (getSDOcs(m->data[0])) { |
798 switch (getSDOcs(m->data[0])) { |
825 |
799 |
826 case 0: |
800 case 0: |
827 /* I am SERVER */ |
801 /* I am SERVER */ |
828 if (whoami == SDO_SERVER) { |
802 if (whoami == SDO_SERVER) { |
829 /* Receiving a download segment data. */ |
803 /* Receiving a download segment data. */ |
830 /* A SDO transfert should have been yet initiated. */ |
804 /* A SDO transfert should have been yet initiated. */ |
831 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
805 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
832 if (!err) |
806 if (!err) |
833 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
807 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
834 if (err) { |
808 if (err) { |
835 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", |
809 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", |
836 nodeId); |
810 CliServNbr); |
837 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
811 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
838 return 0xFF; |
812 return 0xFF; |
839 } |
813 } |
840 /* Reset the wathdog */ |
814 /* Reset the wathdog */ |
841 RestartSDO_TIMER(line) |
815 RestartSDO_TIMER(line) |
842 MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId); |
816 MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", CliServNbr); |
843 index = d->transfers[line].index; |
817 index = d->transfers[line].index; |
844 subIndex = d->transfers[line].subIndex; |
818 subIndex = d->transfers[line].subIndex; |
845 /* Toggle test. */ |
819 /* Toggle test. */ |
846 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
820 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
847 MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); |
821 MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); |
848 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
822 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
849 return 0xFF; |
823 return 0xFF; |
850 } |
824 } |
851 /* Nb of data to be downloaded */ |
825 /* Nb of data to be downloaded */ |
852 nbBytes = 7 - getSDOn3(m->data[0]); |
826 nbBytes = 7 - getSDOn3(m->data[0]); |
853 /* Store the data in the transfert structure. */ |
827 /* Store the data in the transfert structure. */ |
854 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); |
828 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); |
855 if (err) { |
829 if (err) { |
856 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
830 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
857 return 0xFF; |
831 return 0xFF; |
858 } |
832 } |
859 /* Sending the SDO response, CS = 1 */ |
833 /* Sending the SDO response, CS = 1 */ |
860 sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender). */ |
834 data[0] = (1 << 5) | (d->transfers[line].toggle << 4); |
861 sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4); |
835 for (i = 1 ; i < 8 ; i++) |
862 for (i = 1 ; i < 8 ; i++) |
836 data[i] = 0; |
863 sdo.body.data[i] = 0; |
837 MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", CliServNbr); |
864 MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId); |
838 sendSDO(d, whoami, CliServNbr, data); |
865 sendSDO(d, whoami, sdo); |
839 /* Inverting the toggle for the next segment. */ |
866 /* Inverting the toggle for the next segment. */ |
840 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
867 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
841 /* If it was the last segment, */ |
868 /* If it was the last segment, */ |
842 if (getSDOc(m->data[0])) { |
869 if (getSDOc(m->data[0])) { |
843 /* Transfering line data to object dictionary. */ |
870 /* Transfering line data to object dictionary. */ |
844 /* The code does not use the "d" of initiate frame. So it is safe if e=s=0 */ |
871 /* The code does not use the "d" of initiate frame. So it is safe if e=s=0 */ |
845 errorCode = SDOlineToObjdict(d, line); |
872 errorCode = SDOlineToObjdict(d, line); |
846 if (errorCode) { |
873 if (errorCode) { |
847 MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); |
874 MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); |
848 failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode); |
875 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); |
849 return 0xFF; |
876 return 0xFF; |
850 } |
877 } |
851 /* Release of the line */ |
878 /* Release of the line */ |
852 resetSDOline(d, line); |
879 resetSDOline(d, line); |
853 MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", CliServNbr); |
880 MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId); |
854 } |
881 } |
855 } /* end if SERVER */ |
882 } /* end if SERVER */ |
856 else { /* if CLIENT */ |
883 else { /* if CLIENT */ |
857 /* I am CLIENT */ |
884 /* I am CLIENT */ |
858 /* It is a request for a previous upload segment. We should find a line opened for this.*/ |
885 /* It is a request for a previous upload segment. We should find a line opened for this.*/ |
859 err = getSDOlineOnUse( d, CliServNbr, whoami, &line); |
886 err = getSDOlineOnUse( d, nodeId, whoami, &line); |
860 if (!err) |
887 if (!err) |
861 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
888 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
862 if (err) { |
889 if (err) { |
863 MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); |
890 MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); |
864 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
891 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
865 return 0xFF; |
892 return 0xFF; |
866 } |
893 } |
867 /* Reset the wathdog */ |
894 /* Reset the wathdog */ |
868 RestartSDO_TIMER(line) |
895 RestartSDO_TIMER(line) |
869 index = d->transfers[line].index; |
896 index = d->transfers[line].index; |
870 subIndex = d->transfers[line].subIndex; |
897 subIndex = d->transfers[line].subIndex; |
871 /* test of the toggle; */ |
898 /* test of the toggle; */ |
872 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
899 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
873 MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); |
900 MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); |
874 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
901 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
875 return 0xFF; |
902 return 0xFF; |
876 } |
903 } |
877 /* nb of data to be uploaded */ |
904 /* nb of data to be uploaded */ |
878 nbBytes = 7 - getSDOn3(m->data[0]); |
905 nbBytes = 7 - getSDOn3(m->data[0]); |
879 /* Storing the data in the line structure. */ |
906 /* Storing the data in the line structure. */ |
880 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); |
907 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); |
881 if (err) { |
908 if (err) { |
882 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
909 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
883 return 0xFF; |
910 return 0xFF; |
884 } |
911 } |
885 /* Inverting the toggle for the next segment. */ |
912 /* Inverting the toggle for the next segment. */ |
886 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
913 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
887 /* If it was the last segment,*/ |
914 /* If it was the last segment,*/ |
888 if ( getSDOc(m->data[0])) { |
915 if ( getSDOc(m->data[0])) { |
889 /* Put in state finished */ |
916 /* Put in state finished */ |
890 /* The code is safe for the case e=s=0 in initiate frame. */ |
917 /* The code is safe for the case e=s=0 in initiate frame. */ |
891 StopSDO_TIMER(line) |
918 StopSDO_TIMER(line) |
892 d->transfers[line].state = SDO_FINISHED; |
919 d->transfers[line].state = SDO_FINISHED; |
893 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
920 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
894 |
921 |
895 MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId); |
922 MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId); |
896 } |
923 } |
897 else { /* more segments to receive */ |
924 else { /* more segments to receive */ |
898 /* Sending the request for the next segment. */ |
925 /* Sending the request for the next segment. */ |
899 data[0] = (3 << 5) | (d->transfers[line].toggle << 4); |
926 sdo.nodeId = nodeId; |
900 for (i = 1 ; i < 8 ; i++) |
927 sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4); |
901 data[i] = 0; |
928 for (i = 1 ; i < 8 ; i++) |
902 sendSDO(d, whoami, CliServNbr, data); |
929 sdo.body.data[i] = 0; |
903 MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId); |
930 sendSDO(d, whoami, sdo); |
904 } |
931 MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId); |
905 } /* End if CLIENT */ |
932 } |
906 break; |
933 } /* End if CLIENT */ |
907 |
934 break; |
908 case 1: |
935 |
909 /* I am SERVER */ |
936 case 1: |
910 /* Receive of an initiate download */ |
937 /* I am SERVER */ |
911 if (whoami == SDO_SERVER) { |
938 /* Receive of an initiate download */ |
912 index = getSDOindex(m->data[1],m->data[2]); |
939 if (whoami == SDO_SERVER) { |
913 subIndex = getSDOsubIndex(m->data[3]); |
940 index = getSDOindex(m->data[1],m->data[2]); |
914 MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", |
941 subIndex = getSDOsubIndex(m->data[3]); |
915 CliServNbr); |
942 MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", |
916 MSG_WAR(0x3A80, "Writing at index : ", index); |
943 nodeId); |
917 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex); |
944 MSG_WAR(0x3A80, "Writing at index : ", index); |
918 |
945 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex); |
919 /* Search if a SDO transfert have been yet initiated */ |
946 |
920 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
947 /* Search if a SDO transfert have been yet initiated */ |
921 if (! err) { |
948 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
922 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); |
949 if (! err) { |
923 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
950 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); |
924 return 0xFF; |
951 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
925 } |
952 return 0xFF; |
926 /* No line on use. Great ! */ |
953 } |
927 /* Try to open a new line. */ |
954 /* No line on use. Great ! */ |
928 err = getSDOfreeLine( d, whoami, &line ); |
955 /* Try to open a new line. */ |
929 if (err) { |
956 err = getSDOfreeLine( d, whoami, &line ); |
930 MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0); |
957 if (err) { |
931 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
958 MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0); |
932 return 0xFF; |
959 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
933 } |
960 return 0xFF; |
934 initSDOline(d, line, CliServNbr, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); |
961 } |
935 |
962 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); |
936 if (getSDOe(m->data[0])) { /* If SDO expedited */ |
963 |
937 /* nb of data to be downloaded */ |
964 if (getSDOe(m->data[0])) { /* If SDO expedited */ |
938 nbBytes = 4 - getSDOn2(m->data[0]); |
965 /* nb of data to be downloaded */ |
939 /* Storing the data in the line structure. */ |
966 nbBytes = 4 - getSDOn2(m->data[0]); |
940 d->transfers[line].count = nbBytes; |
967 /* Storing the data in the line structure. */ |
941 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); |
968 d->transfers[line].count = nbBytes; |
942 |
969 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); |
943 if (err) { |
970 |
944 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
945 return 0xFF; |
|
946 } |
|
947 |
|
948 /* SDO expedited -> transfert finished. Data can be stored in the dictionary. */ |
|
949 /*The line will be reseted when it is downloading in the dictionary. */ |
|
950 MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished. ", 0); |
|
951 /* Transfering line data to object dictionary. */ |
|
952 errorCode = SDOlineToObjdict(d, line); |
|
953 if (errorCode) { |
|
954 MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); |
|
955 failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode); |
|
956 return 0xFF; |
|
957 } |
|
958 /* Release of the line. */ |
|
959 resetSDOline(d, line); |
|
960 } |
|
961 else {/* So, if it is not an expedited transfert */ |
|
962 if (getSDOs(m->data[0])) { |
|
963 nbBytes = (m->data[4]) + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24); |
|
964 err = setSDOlineRestBytes(d, CliServNbr, nbBytes); |
|
965 if (err) { |
|
966 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
967 return 0xFF; |
|
968 } |
|
969 } |
|
970 } |
|
971 /*Sending a SDO, cs=3*/ |
|
972 data[0] = 3 << 5; |
|
973 data[1] = index & 0xFF; /* LSB */ |
|
974 data[2] = (index >> 8) & 0xFF; /* MSB */ |
|
975 data[3] = subIndex; |
|
976 for (i = 4 ; i < 8 ; i++) |
|
977 data[i] = 0; |
|
978 sendSDO(d, whoami, CliServNbr, data); |
|
979 } /* end if I am SERVER */ |
|
980 else { |
|
981 /* I am CLIENT */ |
|
982 /* It is a response for a previous download segment. We should find a line opened for this. */ |
|
983 err = getSDOlineOnUse( d, CliServNbr, whoami, &line); |
|
984 if (!err) |
|
985 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
|
986 if (err) { |
|
987 MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); |
|
988 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
989 return 0xFF; |
|
990 } |
|
991 /* Reset the wathdog */ |
|
992 RestartSDO_TIMER(line) |
|
993 index = d->transfers[line].index; |
|
994 subIndex = d->transfers[line].subIndex; |
|
995 /* test of the toggle; */ |
|
996 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
|
997 MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); |
|
998 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
|
999 return 0xFF; |
|
1000 } |
|
1001 |
|
1002 /* End transmission or downloading next segment. We need to know if it will be the last one. */ |
|
1003 getSDOlineRestBytes(d, line, &nbBytes); |
|
1004 if (nbBytes == 0) { |
|
1005 MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); |
|
1006 StopSDO_TIMER(line) |
|
1007 d->transfers[line].state = SDO_FINISHED; |
|
1008 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
|
1009 return 0x00; |
|
1010 } |
|
1011 /* At least one transfer to send. */ |
|
1012 if (nbBytes > 7) { |
|
1013 /* several segments to download.*/ |
|
1014 /* code to send the next segment. (cs = 0; c = 0) */ |
|
1015 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
|
1016 data[0] = (d->transfers[line].toggle << 4); |
|
1017 err = lineToSDO(d, line, 7, data + 1); |
|
1018 if (err) { |
|
1019 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1020 return 0xFF; |
|
1021 } |
|
1022 } |
|
1023 else { |
|
1024 /* Last segment. */ |
|
1025 /* code to send the last segment. (cs = 0; c = 1)*/ |
|
1026 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
|
1027 data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1); |
|
1028 err = lineToSDO(d, line, nbBytes, data + 1); |
|
1029 if (err) { |
|
1030 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1031 return 0xFF; |
|
1032 } |
|
1033 for (i = nbBytes + 1 ; i < 8 ; i++) |
|
1034 data[i] = 0; |
|
1035 } |
|
1036 MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); |
|
1037 sendSDO(d, whoami, CliServNbr, data); |
|
1038 } /* end if I am a CLIENT */ |
|
1039 break; |
|
1040 |
|
1041 case 2: |
|
1042 /* I am SERVER */ |
|
1043 /* Receive of an initiate upload.*/ |
|
1044 if (whoami == SDO_SERVER) { |
|
1045 index = getSDOindex(m->data[1],m->data[2]); |
|
1046 subIndex = getSDOsubIndex(m->data[3]); |
|
1047 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", |
|
1048 CliServNbr); |
|
1049 MSG_WAR(0x3A90, "Reading at index : ", index); |
|
1050 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex); |
|
1051 /* Search if a SDO transfert have been yet initiated*/ |
|
1052 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
|
1053 if (! err) { |
|
1054 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); |
|
1055 MSG_WAR(0x3A93, "Server Nbr = ", CliServNbr); |
|
1056 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
|
1057 return 0xFF; |
|
1058 } |
|
1059 /* No line on use. Great !*/ |
|
1060 /* Try to open a new line.*/ |
|
1061 err = getSDOfreeLine( d, whoami, &line ); |
|
1062 if (err) { |
|
1063 MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0); |
|
1064 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
|
1065 return 0xFF; |
|
1066 } |
|
1067 initSDOline(d, line, CliServNbr, index, subIndex, SDO_UPLOAD_IN_PROGRESS); |
|
1068 /* Transfer data from dictionary to the line structure. */ |
|
1069 errorCode = objdictToSDOline(d, line); |
|
1070 |
|
1071 if (errorCode) { |
|
1072 MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", |
|
1073 errorCode); |
|
1074 failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode); |
|
1075 return 0xFF; |
|
1076 } |
|
1077 /* Preparing the response.*/ |
|
1078 getSDOlineRestBytes(d, line, &nbBytes); /* Nb bytes to transfer ? */ |
|
1079 if (nbBytes > 4) { |
|
1080 /* normal transfert. (segmented). */ |
|
1081 /* code to send the initiate upload response. (cs = 2) */ |
|
1082 data[0] = (2 << 5) | 1; |
|
1083 data[1] = index & 0xFF; /* LSB */ |
|
1084 data[2] = (index >> 8) & 0xFF; /* MSB */ |
|
1085 data[3] = subIndex; |
|
1086 data[4] = (UNS8)nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/ |
|
1087 /* It takes too much memory to upgrate to 2^32 because the size of data is also coded */ |
|
1088 /* in the object dictionary, at every index and subindex. */ |
|
1089 for (i = 5 ; i < 8 ; i++) |
|
1090 data[i] = 0; |
|
1091 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); |
|
1092 sendSDO(d, whoami, CliServNbr, data); |
|
1093 } |
|
1094 else { |
|
1095 /* Expedited upload. (cs = 2 ; e = 1) */ |
|
1096 data[0] = (UNS8)((2 << 5) | ((4 - nbBytes) << 2) | 3); |
|
1097 data[1] = index & 0xFF; /* LSB */ |
|
1098 data[2] = (index >> 8) & 0xFF; /* MSB */ |
|
1099 data[3] = subIndex; |
|
1100 err = lineToSDO(d, line, nbBytes, data + 4); |
|
1101 if (err) { |
|
1102 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1103 return 0xFF; |
|
1104 } |
|
1105 for (i = 4 + nbBytes ; i < 8 ; i++) |
|
1106 data[i] = 0; |
|
1107 MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", |
|
1108 CliServNbr); |
|
1109 sendSDO(d, whoami, CliServNbr, data); |
|
1110 /* Release the line.*/ |
|
1111 resetSDOline(d, line); |
|
1112 } |
|
1113 } /* end if I am SERVER*/ |
|
1114 else { |
|
1115 /* I am CLIENT */ |
|
1116 /* It is the response for the previous initiate upload request.*/ |
|
1117 /* We should find a line opened for this. */ |
|
1118 err = getSDOlineOnUse( d, CliServNbr, whoami, &line); |
|
1119 if (!err) |
|
1120 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
|
1121 if (err) { |
|
1122 MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); |
|
1123 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1124 return 0xFF; |
|
1125 } |
|
1126 /* Reset the wathdog */ |
|
1127 RestartSDO_TIMER(line) |
|
1128 index = d->transfers[line].index; |
|
1129 subIndex = d->transfers[line].subIndex; |
|
1130 |
|
1131 if (getSDOe(m->data[0])) { /* If SDO expedited */ |
|
1132 /* nb of data to be uploaded */ |
|
1133 nbBytes = 4 - getSDOn2(m->data[0]); |
|
1134 /* Storing the data in the line structure. */ |
|
1135 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); |
|
1136 if (err) { |
|
1137 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1138 return 0xFF; |
|
1139 } |
|
1140 /* SDO expedited -> transfert finished. data are available via getReadResultNetworkDict(). */ |
|
1141 MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId); |
|
1142 StopSDO_TIMER(line) |
|
1143 d->transfers[line].count = nbBytes; |
|
1144 d->transfers[line].state = SDO_FINISHED; |
|
1145 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
|
1146 return 0; |
|
1147 } |
|
1148 else { /* So, if it is not an expedited transfert */ |
|
1149 /* Storing the nb of data to receive. */ |
|
1150 if (getSDOs(m->data[0])) { |
|
1151 nbBytes = m->data[4] + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24); |
|
1152 err = setSDOlineRestBytes(d, line, nbBytes); |
|
1153 if (err) { |
|
1154 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1155 return 0xFF; |
|
1156 } |
|
1157 } |
|
1158 /* Requesting next segment. (cs = 3) */ |
|
1159 data[0] = 3 << 5; |
|
1160 for (i = 1 ; i < 8 ; i++) |
|
1161 data[i] = 0; |
|
1162 MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); |
|
1163 sendSDO(d, whoami, CliServNbr, data); |
|
1164 } |
|
1165 } /* End if CLIENT */ |
|
1166 break; |
|
1167 |
|
1168 case 3: |
|
1169 /* I am SERVER */ |
|
1170 if (whoami == SDO_SERVER) { |
|
1171 /* Receiving a upload segment. */ |
|
1172 /* A SDO transfert should have been yet initiated. */ |
|
1173 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
|
1174 if (!err) |
|
1175 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
|
1176 if (err) { |
|
1177 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", |
|
1178 CliServNbr); |
|
1179 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1180 return 0xFF; |
|
1181 } |
|
1182 /* Reset the wathdog */ |
|
1183 RestartSDO_TIMER(line) |
|
1184 MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", CliServNbr); |
|
1185 index = d->transfers[line].index; |
|
1186 subIndex = d->transfers[line].subIndex; |
|
1187 /* Toggle test.*/ |
|
1188 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
|
1189 MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); |
|
1190 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
|
1191 return 0xFF; |
|
1192 } |
|
1193 /* Uploading next segment. We need to know if it will be the last one. */ |
|
1194 getSDOlineRestBytes(d, line, &nbBytes); |
|
1195 if (nbBytes > 7) { |
|
1196 /* The segment to transfer is not the last one.*/ |
|
1197 /* code to send the next segment. (cs = 0; c = 0) */ |
|
1198 data[0] = (d->transfers[line].toggle << 4); |
|
1199 err = lineToSDO(d, line, 7, data + 1); |
|
1200 if (err) { |
|
1201 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1202 return 0xFF; |
|
1203 } |
|
1204 /* Inverting the toggle for the next tranfert. */ |
|
1205 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
|
1206 MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", CliServNbr); |
|
1207 sendSDO(d, whoami, CliServNbr, data); |
|
1208 } |
|
1209 else { |
|
1210 /* Last segment. */ |
|
1211 /* code to send the last segment. (cs = 0; c = 1) */ |
|
1212 data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1); |
|
1213 err = lineToSDO(d, line, nbBytes, data + 1); |
|
1214 if (err) { |
|
1215 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1216 return 0xFF; |
|
1217 } |
|
1218 for (i = nbBytes + 1 ; i < 8 ; i++) |
|
1219 data[i] = 0; |
|
1220 MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", CliServNbr); |
|
1221 sendSDO(d, whoami, CliServNbr, data); |
|
1222 /* Release the line */ |
|
1223 resetSDOline(d, line); |
|
1224 } |
|
1225 } /* end if SERVER*/ |
|
1226 else { |
|
1227 /* I am CLIENT */ |
|
1228 /* It is the response for the previous initiate download request. */ |
|
1229 /* We should find a line opened for this. */ |
|
1230 err = getSDOlineOnUse( d, CliServNbr, whoami, &line); |
|
1231 if (!err) |
|
1232 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
|
1233 if (err) { |
|
1234 MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); |
|
1235 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1236 return 0xFF; |
|
1237 } |
|
1238 /* Reset the watchdog */ |
|
1239 RestartSDO_TIMER(line) |
|
1240 index = d->transfers[line].index; |
|
1241 subIndex = d->transfers[line].subIndex; |
|
1242 /* End transmission or requesting next segment. */ |
|
1243 getSDOlineRestBytes(d, line, &nbBytes); |
|
1244 if (nbBytes == 0) { |
|
1245 MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); |
|
1246 StopSDO_TIMER(line) |
|
1247 d->transfers[line].state = SDO_FINISHED; |
|
1248 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
|
1249 return 0x00; |
|
1250 } |
|
1251 if (nbBytes > 7) { |
|
1252 /* more than one request to send */ |
|
1253 /* code to send the next segment. (cs = 0; c = 0) */ |
|
1254 data[0] = (d->transfers[line].toggle << 4); |
|
1255 err = lineToSDO(d, line, 7, data + 1); |
|
1256 if (err) { |
|
1257 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1258 return 0xFF; |
|
1259 } |
|
1260 } |
|
1261 else { |
|
1262 /* Last segment.*/ |
|
1263 /* code to send the last segment. (cs = 0; c = 1) */ |
|
1264 data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1); |
|
1265 err = lineToSDO(d, line, nbBytes, data + 1); |
|
1266 if (err) { |
|
1267 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1268 return 0xFF; |
|
1269 } |
|
1270 for (i = nbBytes + 1 ; i < 8 ; i++) |
|
1271 data[i] = 0; |
|
1272 } |
|
1273 MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId); |
|
1274 sendSDO(d, whoami, CliServNbr, data); |
|
1275 |
|
1276 } /* end if I am a CLIENT */ |
|
1277 break; |
|
1278 |
|
1279 case 4: |
|
1280 abortCode = |
|
1281 (UNS32)m->data[4] | |
|
1282 ((UNS32)m->data[5] << 8) | |
|
1283 ((UNS32)m->data[6] << 16) | |
|
1284 ((UNS32)m->data[7] << 24); |
|
1285 /* Received SDO abort. */ |
|
1286 /* Looking for the line concerned. */ |
|
1287 if (whoami == SDO_SERVER) { |
|
1288 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
|
1289 if (!err) { |
|
1290 resetSDOline( d, line ); |
|
1291 MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode); |
|
1292 } |
|
1293 else |
|
1294 MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode); |
|
1295 /* Tips : The end user has no way to know that the server node has received an abort SDO. */ |
|
1296 /* Its is ok, I think.*/ |
|
1297 } |
|
1298 else { /* If I am CLIENT */ |
|
1299 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
|
1300 if (!err) { |
|
1301 /* The line *must* be released by the core program. */ |
|
1302 StopSDO_TIMER(line) |
|
1303 d->transfers[line].state = SDO_ABORTED_RCV; |
|
1304 d->transfers[line].abortCode = abortCode; |
|
1305 MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode); |
|
1306 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
|
1307 } |
|
1308 else |
|
1309 MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode); |
|
1310 } |
|
1311 break; |
|
1312 default: |
|
1313 /* Error : Unknown cs */ |
|
1314 MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0])); |
|
1315 return 0xFF; |
|
1316 |
|
1317 } /* End switch */ |
|
1318 return 0; |
|
1319 } |
|
1320 |
|
1321 |
|
1322 /*! |
|
1323 ** |
|
1324 ** |
|
1325 ** @param d |
|
1326 ** @param nodeId |
|
1327 ** |
|
1328 ** @return |
|
1329 ** 0xFF : No SDO client available |
|
1330 ** 0xFE : Not found |
|
1331 ** otherwise : SDO client number |
|
1332 **/ |
|
1333 UNS8 GetSDOClientFromNodeId( CO_Data* d, UNS8 nodeId ) |
|
1334 { |
|
1335 UNS8 SDOfound = 0; |
|
1336 UNS8 CliNbr; |
|
1337 UNS16 lastIndex; |
|
1338 UNS16 offset; |
|
1339 UNS8 nodeIdServer; |
|
1340 |
|
1341 offset = d->firstIndex->SDO_CLT; |
|
1342 lastIndex = d->lastIndex->SDO_CLT; |
|
1343 if (offset == 0) { |
|
1344 MSG_ERR(0x1AC6, "No SDO client index found for nodeId ", nodeId); |
|
1345 return 0xFF; |
|
1346 } |
|
1347 CliNbr = 0; |
|
1348 while (offset <= lastIndex) { |
|
1349 if (d->objdict[offset].bSubCount <= 3) { |
|
1350 MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + CliNbr); |
|
1351 return 0xFF; |
|
1352 } |
|
1353 /* looking for the server nodeId */ |
|
1354 nodeIdServer = *((UNS8*) d->objdict[offset].pSubindex[3].pObject); |
|
1355 MSG_WAR(0x1AD2, "index : ", 0x1280 + CliNbr); |
|
1356 MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer); |
|
1357 |
|
1358 if(nodeIdServer == nodeId) { |
|
1359 SDOfound = 1; |
|
1360 break; |
|
1361 } |
|
1362 offset++; |
|
1363 CliNbr++; |
|
1364 } /* end while */ |
|
1365 if (!SDOfound) { |
|
1366 MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId); |
|
1367 return 0xFE; |
|
1368 } |
|
1369 MSG_WAR(0x3AD0," SDO client defined at index : ", 0x1280 + CliNbr); |
|
1370 |
|
1371 return CliNbr; |
|
1372 } |
|
1373 |
|
1374 |
|
1375 /*! |
|
1376 ** |
|
1377 ** |
|
1378 ** @param d |
|
1379 ** @param nodeId |
|
1380 ** @param index |
|
1381 ** @param subIndex |
|
1382 ** @param count |
|
1383 ** @param dataType |
|
1384 ** @param data |
|
1385 ** @param Callback |
|
1386 ** @param endianize |
|
1387 ** |
|
1388 ** @return |
|
1389 **/ |
|
1390 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
|
1391 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) |
|
1392 { |
|
1393 UNS8 err; |
|
1394 UNS8 line; |
|
1395 UNS8 CliNbr; |
|
1396 UNS32 j; |
|
1397 UNS8 i; |
|
1398 UNS8 buf[8]; |
|
1399 |
|
1400 MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId); |
|
1401 MSG_WAR(0x3AC1, " At index : ", index); |
|
1402 MSG_WAR(0x3AC2, " subIndex : ", subIndex); |
|
1403 MSG_WAR(0x3AC3, " nb bytes : ", count); |
|
1404 |
|
1405 /* First let's find the corresponding SDO client in our OD */ |
|
1406 CliNbr = GetSDOClientFromNodeId( d, nodeId); |
|
1407 if(CliNbr >= 0xFE) |
|
1408 return CliNbr; |
|
1409 /* Verify that there is no SDO communication yet. */ |
|
1410 err = getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line); |
|
1411 if (!err) { |
|
1412 MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId); |
|
1413 return 0xFF; |
|
1414 } |
|
1415 /* Taking the line ... */ |
|
1416 err = getSDOfreeLine( d, SDO_CLIENT, &line ); |
971 if (err) { |
1417 if (err) { |
972 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1418 MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); |
973 return 0xFF; |
1419 return (0xFF); |
974 } |
1420 } |
975 |
1421 initSDOline(d, line, CliNbr, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); |
976 /* SDO expedited -> transfert finished. Data can be stored in the dictionary. */ |
1422 d->transfers[line].count = count; |
977 /*The line will be reseted when it is downloading in the dictionary. */ |
1423 d->transfers[line].dataType = dataType; |
978 MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId); |
|
979 /* Transfering line data to object dictionary. */ |
|
980 errorCode = SDOlineToObjdict(d, line); |
|
981 if (errorCode) { |
|
982 MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); |
|
983 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); |
|
984 return 0xFF; |
|
985 } |
|
986 /* Release of the line. */ |
|
987 resetSDOline(d, line); |
|
988 } |
|
989 else {/* So, if it is not an expedited transfert */ |
|
990 if (getSDOs(m->data[0])) { |
|
991 nbBytes = (m->data[4]) + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24); |
|
992 err = setSDOlineRestBytes(d, nodeId, nbBytes); |
|
993 if (err) { |
|
994 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
995 return 0xFF; |
|
996 } |
|
997 } |
|
998 } |
|
999 /*Sending a SDO, cs=3*/ |
|
1000 sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender).*/ |
|
1001 sdo.body.data[0] = 3 << 5; |
|
1002 sdo.body.data[1] = index & 0xFF; /* LSB */ |
|
1003 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
|
1004 sdo.body.data[3] = subIndex; |
|
1005 for (i = 4 ; i < 8 ; i++) |
|
1006 sdo.body.data[i] = 0; |
|
1007 sendSDO(d, whoami, sdo); |
|
1008 } /* end if I am SERVER */ |
|
1009 else { |
|
1010 /* I am CLIENT */ |
|
1011 /* It is a response for a previous download segment. We should find a line opened for this. */ |
|
1012 err = getSDOlineOnUse( d, nodeId, whoami, &line); |
|
1013 if (!err) |
|
1014 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
|
1015 if (err) { |
|
1016 MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); |
|
1017 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1018 return 0xFF; |
|
1019 } |
|
1020 /* Reset the wathdog */ |
|
1021 RestartSDO_TIMER(line) |
|
1022 index = d->transfers[line].index; |
|
1023 subIndex = d->transfers[line].subIndex; |
|
1024 /* test of the toggle; */ |
|
1025 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
|
1026 MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); |
|
1027 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
|
1028 return 0xFF; |
|
1029 } |
|
1030 |
|
1031 /* End transmission or downloading next segment. We need to know if it will be the last one. */ |
|
1032 getSDOlineRestBytes(d, line, &nbBytes); |
|
1033 if (nbBytes == 0) { |
|
1034 MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); |
|
1035 StopSDO_TIMER(line) |
|
1036 d->transfers[line].state = SDO_FINISHED; |
|
1037 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
|
1038 return 0x00; |
|
1039 } |
|
1040 /* At least one transfer to send. */ |
|
1041 if (nbBytes > 7) { |
|
1042 /* several segments to download.*/ |
|
1043 /* code to send the next segment. (cs = 0; c = 0) */ |
|
1044 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
|
1045 sdo.nodeId = nodeId; /* The server node Id; */ |
|
1046 sdo.body.data[0] = (d->transfers[line].toggle << 4); |
|
1047 err = lineToSDO(d, line, 7, sdo.body.data + 1); |
|
1048 if (err) { |
|
1049 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1050 return 0xFF; |
|
1051 } |
|
1052 } |
|
1053 else { |
|
1054 /* Last segment. */ |
|
1055 /* code to send the last segment. (cs = 0; c = 1)*/ |
|
1056 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
|
1057 sdo.nodeId = nodeId; /* The server node Id; */ |
|
1058 sdo.body.data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1); |
|
1059 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
|
1060 if (err) { |
|
1061 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1062 return 0xFF; |
|
1063 } |
|
1064 for (i = nbBytes + 1 ; i < 8 ; i++) |
|
1065 sdo.body.data[i] = 0; |
|
1066 } |
|
1067 MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); |
|
1068 sendSDO(d, whoami, sdo); |
|
1069 } /* end if I am a CLIENT */ |
|
1070 break; |
|
1071 |
|
1072 case 2: |
|
1073 /* I am SERVER */ |
|
1074 /* Receive of an initiate upload.*/ |
|
1075 if (whoami == SDO_SERVER) { |
|
1076 index = getSDOindex(m->data[1],m->data[2]); |
|
1077 subIndex = getSDOsubIndex(m->data[3]); |
|
1078 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", |
|
1079 nodeId); |
|
1080 MSG_WAR(0x3A90, "Reading at index : ", index); |
|
1081 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex); |
|
1082 /* Search if a SDO transfert have been yet initiated*/ |
|
1083 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
|
1084 if (! err) { |
|
1085 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); |
|
1086 MSG_WAR(0x3A93, "nodeId = ", nodeId); |
|
1087 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
|
1088 return 0xFF; |
|
1089 } |
|
1090 /* No line on use. Great !*/ |
|
1091 /* Try to open a new line.*/ |
|
1092 err = getSDOfreeLine( d, whoami, &line ); |
|
1093 if (err) { |
|
1094 MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0); |
|
1095 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
|
1096 return 0xFF; |
|
1097 } |
|
1098 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS); |
|
1099 /* Transfer data from dictionary to the line structure. */ |
|
1100 errorCode = objdictToSDOline(d, line); |
|
1101 |
|
1102 if (errorCode) { |
|
1103 MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", |
|
1104 errorCode); |
|
1105 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); |
|
1106 return 0xFF; |
|
1107 } |
|
1108 /* Preparing the response.*/ |
|
1109 getSDOlineRestBytes(d, line, &nbBytes); /* Nb bytes to transfer ? */ |
|
1110 sdo.nodeId = nodeId; /* The server node Id; */ |
|
1111 if (nbBytes > 4) { |
|
1112 /* normal transfert. (segmented). */ |
|
1113 /* code to send the initiate upload response. (cs = 2) */ |
|
1114 sdo.body.data[0] = (2 << 5) | 1; |
|
1115 sdo.body.data[1] = index & 0xFF; /* LSB */ |
|
1116 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
|
1117 sdo.body.data[3] = subIndex; |
|
1118 sdo.body.data[4] = (UNS8)nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/ |
|
1119 /* It takes too much memory to upgrate to 2^32 because the size of data is also coded */ |
|
1120 /* in the object dictionary, at every index and subindex. */ |
|
1121 for (i = 5 ; i < 8 ; i++) |
|
1122 sdo.body.data[i] = 0; |
|
1123 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); |
|
1124 sendSDO(d, whoami, sdo); |
|
1125 } |
|
1126 else { |
|
1127 /* Expedited upload. (cs = 2 ; e = 1) */ |
|
1128 sdo.body.data[0] = (UNS8)((2 << 5) | ((4 - nbBytes) << 2) | 3); |
|
1129 sdo.body.data[1] = index & 0xFF; /* LSB */ |
|
1130 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
|
1131 sdo.body.data[3] = subIndex; |
|
1132 err = lineToSDO(d, line, nbBytes, sdo.body.data + 4); |
|
1133 if (err) { |
|
1134 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1135 return 0xFF; |
|
1136 } |
|
1137 for (i = 4 + nbBytes ; i < 8 ; i++) |
|
1138 sdo.body.data[i] = 0; |
|
1139 MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", |
|
1140 nodeId); |
|
1141 sendSDO(d, whoami, sdo); |
|
1142 /* Release the line.*/ |
|
1143 resetSDOline(d, line); |
|
1144 } |
|
1145 } /* end if I am SERVER*/ |
|
1146 else { |
|
1147 /* I am CLIENT */ |
|
1148 /* It is the response for the previous initiate upload request.*/ |
|
1149 /* We should find a line opened for this. */ |
|
1150 err = getSDOlineOnUse( d, nodeId, whoami, &line); |
|
1151 if (!err) |
|
1152 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
|
1153 if (err) { |
|
1154 MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); |
|
1155 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1156 return 0xFF; |
|
1157 } |
|
1158 /* Reset the wathdog */ |
|
1159 RestartSDO_TIMER(line) |
|
1160 index = d->transfers[line].index; |
|
1161 subIndex = d->transfers[line].subIndex; |
|
1162 |
|
1163 if (getSDOe(m->data[0])) { /* If SDO expedited */ |
|
1164 /* nb of data to be uploaded */ |
|
1165 nbBytes = 4 - getSDOn2(m->data[0]); |
|
1166 /* Storing the data in the line structure. */ |
|
1167 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); |
|
1168 if (err) { |
|
1169 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1170 return 0xFF; |
|
1171 } |
|
1172 /* SDO expedited -> transfert finished. data are available via getReadResultNetworkDict(). */ |
|
1173 MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId); |
|
1174 StopSDO_TIMER(line) |
|
1175 d->transfers[line].count = nbBytes; |
|
1176 d->transfers[line].state = SDO_FINISHED; |
|
1177 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
|
1178 return 0; |
|
1179 } |
|
1180 else { /* So, if it is not an expedited transfert */ |
|
1181 /* Storing the nb of data to receive. */ |
|
1182 if (getSDOs(m->data[0])) { |
|
1183 nbBytes = m->data[4] + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24); |
|
1184 err = setSDOlineRestBytes(d, line, nbBytes); |
|
1185 if (err) { |
|
1186 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1187 return 0xFF; |
|
1188 } |
|
1189 } |
|
1190 /* Requesting next segment. (cs = 3) */ |
|
1191 sdo.nodeId = nodeId; |
|
1192 sdo.body.data[0] = 3 << 5; |
|
1193 for (i = 1 ; i < 8 ; i++) |
|
1194 sdo.body.data[i] = 0; |
|
1195 MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); |
|
1196 sendSDO(d, whoami, sdo); |
|
1197 } |
|
1198 } /* End if CLIENT */ |
|
1199 break; |
|
1200 |
|
1201 case 3: |
|
1202 /* I am SERVER */ |
|
1203 if (whoami == SDO_SERVER) { |
|
1204 /* Receiving a upload segment. */ |
|
1205 /* A SDO transfert should have been yet initiated. */ |
|
1206 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
|
1207 if (!err) |
|
1208 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
|
1209 if (err) { |
|
1210 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", |
|
1211 nodeId); |
|
1212 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1213 return 0xFF; |
|
1214 } |
|
1215 /* Reset the wathdog */ |
|
1216 RestartSDO_TIMER(line) |
|
1217 MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId); |
|
1218 index = d->transfers[line].index; |
|
1219 subIndex = d->transfers[line].subIndex; |
|
1220 /* Toggle test.*/ |
|
1221 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
|
1222 MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); |
|
1223 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
|
1224 return 0xFF; |
|
1225 } |
|
1226 /* Uploading next segment. We need to know if it will be the last one. */ |
|
1227 getSDOlineRestBytes(d, line, &nbBytes); |
|
1228 if (nbBytes > 7) { |
|
1229 /* The segment to transfer is not the last one.*/ |
|
1230 /* code to send the next segment. (cs = 0; c = 0) */ |
|
1231 sdo.nodeId = nodeId; /* The server node Id; */ |
|
1232 sdo.body.data[0] = (d->transfers[line].toggle << 4); |
|
1233 err = lineToSDO(d, line, 7, sdo.body.data + 1); |
|
1234 if (err) { |
|
1235 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1236 return 0xFF; |
|
1237 } |
|
1238 /* Inverting the toggle for the next tranfert. */ |
|
1239 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
|
1240 MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId); |
|
1241 sendSDO(d, whoami, sdo); |
|
1242 } |
|
1243 else { |
|
1244 /* Last segment. */ |
|
1245 /* code to send the last segment. (cs = 0; c = 1) */ |
|
1246 sdo.nodeId = nodeId; /** The server node Id; */ |
|
1247 sdo.body.data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1); |
|
1248 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
|
1249 if (err) { |
|
1250 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1251 return 0xFF; |
|
1252 } |
|
1253 for (i = nbBytes + 1 ; i < 8 ; i++) |
|
1254 sdo.body.data[i] = 0; |
|
1255 MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId); |
|
1256 sendSDO(d, whoami, sdo); |
|
1257 /* Release the line */ |
|
1258 resetSDOline(d, line); |
|
1259 } |
|
1260 } /* end if SERVER*/ |
|
1261 else { |
|
1262 /* I am CLIENT */ |
|
1263 /* It is the response for the previous initiate download request. */ |
|
1264 /* We should find a line opened for this. */ |
|
1265 err = getSDOlineOnUse( d, nodeId, whoami, &line); |
|
1266 if (!err) |
|
1267 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
|
1268 if (err) { |
|
1269 MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); |
|
1270 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1271 return 0xFF; |
|
1272 } |
|
1273 /* Reset the watchdog */ |
|
1274 RestartSDO_TIMER(line) |
|
1275 index = d->transfers[line].index; |
|
1276 subIndex = d->transfers[line].subIndex; |
|
1277 /* End transmission or requesting next segment. */ |
|
1278 getSDOlineRestBytes(d, line, &nbBytes); |
|
1279 if (nbBytes == 0) { |
|
1280 MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); |
|
1281 StopSDO_TIMER(line) |
|
1282 d->transfers[line].state = SDO_FINISHED; |
|
1283 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
|
1284 return 0x00; |
|
1285 } |
|
1286 if (nbBytes > 7) { |
|
1287 /* more than one request to send */ |
|
1288 /* code to send the next segment. (cs = 0; c = 0) */ |
|
1289 sdo.nodeId = nodeId; /** The server node Id; */ |
|
1290 sdo.body.data[0] = (d->transfers[line].toggle << 4); |
|
1291 err = lineToSDO(d, line, 7, sdo.body.data + 1); |
|
1292 if (err) { |
|
1293 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1294 return 0xFF; |
|
1295 } |
|
1296 } |
|
1297 else { |
|
1298 /* Last segment.*/ |
|
1299 /* code to send the last segment. (cs = 0; c = 1) */ |
|
1300 sdo.nodeId = nodeId; /* The server node Id; */ |
|
1301 sdo.body.data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1); |
|
1302 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
|
1303 if (err) { |
|
1304 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1305 return 0xFF; |
|
1306 } |
|
1307 for (i = nbBytes + 1 ; i < 8 ; i++) |
|
1308 sdo.body.data[i] = 0; |
|
1309 } |
|
1310 MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId); |
|
1311 sendSDO(d, whoami, sdo); |
|
1312 |
|
1313 } /* end if I am a CLIENT */ |
|
1314 break; |
|
1315 |
|
1316 case 4: |
|
1317 abortCode = |
|
1318 (UNS32)m->data[4] | |
|
1319 ((UNS32)m->data[5] << 8) | |
|
1320 ((UNS32)m->data[6] << 16) | |
|
1321 ((UNS32)m->data[7] << 24); |
|
1322 /* Received SDO abort. */ |
|
1323 /* Looking for the line concerned. */ |
|
1324 if (whoami == SDO_SERVER) { |
|
1325 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
|
1326 if (!err) { |
|
1327 resetSDOline( d, line ); |
|
1328 MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode); |
|
1329 } |
|
1330 else |
|
1331 MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode); |
|
1332 /* Tips : The end user has no way to know that the server node has received an abort SDO. */ |
|
1333 /* Its is ok, I think.*/ |
|
1334 } |
|
1335 else { /* If I am CLIENT */ |
|
1336 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
|
1337 if (!err) { |
|
1338 /* The line *must* be released by the core program. */ |
|
1339 StopSDO_TIMER(line) |
|
1340 d->transfers[line].state = SDO_ABORTED_RCV; |
|
1341 d->transfers[line].abortCode = abortCode; |
|
1342 MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode); |
|
1343 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
|
1344 } |
|
1345 else |
|
1346 MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode); |
|
1347 } |
|
1348 break; |
|
1349 default: |
|
1350 /* Error : Unknown cs */ |
|
1351 MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0])); |
|
1352 return 0xFF; |
|
1353 |
|
1354 } /* End switch */ |
|
1355 return 0; |
|
1356 } |
|
1357 |
|
1358 /*! |
|
1359 ** |
|
1360 ** |
|
1361 ** @param d |
|
1362 ** @param nodeId |
|
1363 ** @param index |
|
1364 ** @param subIndex |
|
1365 ** @param count |
|
1366 ** @param dataType |
|
1367 ** @param data |
|
1368 ** @param Callback |
|
1369 ** @param endianize |
|
1370 ** |
|
1371 ** @return |
|
1372 **/ |
|
1373 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
|
1374 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) |
|
1375 { |
|
1376 UNS8 err; |
|
1377 UNS8 SDOfound = 0; |
|
1378 UNS8 line; |
|
1379 s_SDO sdo; /* SDO to transmit */ |
|
1380 UNS8 i; |
|
1381 UNS32 j; |
|
1382 UNS16 lastIndex; |
|
1383 UNS16 offset; |
|
1384 UNS8 *pNodeIdServer; |
|
1385 UNS8 nodeIdServer; |
|
1386 |
|
1387 MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId); |
|
1388 MSG_WAR(0x3AC1, " At index : ", index); |
|
1389 MSG_WAR(0x3AC2, " subIndex : ", subIndex); |
|
1390 MSG_WAR(0x3AC3, " nb bytes : ", count); |
|
1391 |
|
1392 /* Verify that there is no SDO communication yet. */ |
|
1393 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); |
|
1394 if (!err) { |
|
1395 MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId); |
|
1396 return 0xFF; |
|
1397 } |
|
1398 /* Taking the line ... */ |
|
1399 err = getSDOfreeLine( d, SDO_CLIENT, &line ); |
|
1400 if (err) { |
|
1401 MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); |
|
1402 return (0xFF); |
|
1403 } |
|
1404 /* Check which SDO to use to communicate with the node */ |
|
1405 offset = d->firstIndex->SDO_CLT; |
|
1406 lastIndex = d->lastIndex->SDO_CLT; |
|
1407 if (offset == 0) { |
|
1408 MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0); |
|
1409 return 0xFF; |
|
1410 } |
|
1411 i = 0; |
|
1412 while (offset <= lastIndex) { |
|
1413 if (d->objdict[offset].bSubCount <= 3) { |
|
1414 MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + i); |
|
1415 return 0xFF; |
|
1416 } |
|
1417 /* looking for the nodeId server */ |
|
1418 pNodeIdServer = (UNS8*) d->objdict[offset].pSubindex[3].pObject; |
|
1419 nodeIdServer = *pNodeIdServer; |
|
1420 MSG_WAR(0x1AD2, "index : ", 0x1280 + i); |
|
1421 MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer); |
|
1422 |
|
1423 if(nodeIdServer == nodeId) { |
|
1424 SDOfound = 1; |
|
1425 break; |
|
1426 } |
|
1427 offset++; |
|
1428 i++; |
|
1429 } /* end while */ |
|
1430 if (!SDOfound) { |
|
1431 MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId); |
|
1432 return 0xFE; |
|
1433 } |
|
1434 MSG_WAR(0x3AD0," SDO client defined at index : ", 0x1280 + i); |
|
1435 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); |
|
1436 d->transfers[line].count = count; |
|
1437 d->transfers[line].dataType = dataType; |
|
1438 |
1424 |
1439 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
1425 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
1440 { |
1426 { |
1441 UNS8* lineData = d->transfers[line].data; |
1427 UNS8* lineData = d->transfers[line].data; |
1442 if (count > SDO_MAX_LENGTH_TRANSFERT) |
1428 if (count > SDO_MAX_LENGTH_TRANSFERT) |
1443 { |
1429 { |
1444 d->transfers[line].dynamicData = (UNS8*) malloc(count); |
1430 d->transfers[line].dynamicData = (UNS8*) malloc(count); |
1445 d->transfers[line].dynamicDataSize = count; |
1431 d->transfers[line].dynamicDataSize = count; |
1446 if (d->transfers[line].dynamicData == NULL) |
1432 if (d->transfers[line].dynamicData == NULL) |
1447 { |
1433 { |
1448 MSG_ERR(0x1AC9, "SDO. Error. Could not allocate enough bytes : ", count); |
1434 MSG_ERR(0x1AC9, "SDO. Error. Could not allocate enough bytes : ", count); |
1449 return 0xFE; |
1435 return 0xFE; |
1450 } |
1436 } |
1451 lineData = d->transfers[line].dynamicData; |
1437 lineData = d->transfers[line].dynamicData; |
1452 } |
1438 } |
1453 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
1439 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
1454 |
1440 |
1455 /* Copy data to transfers structure. */ |
1441 /* Copy data to transfers structure. */ |
1456 for (j = 0 ; j < count ; j++) { |
1442 for (j = 0 ; j < count ; j++) { |
1457 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
1443 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
1458 # ifdef CANOPEN_BIG_ENDIAN |
1444 # ifdef CANOPEN_BIG_ENDIAN |
1459 if (dataType == 0 && endianize) |
1445 if (dataType == 0 && endianize) |
1460 lineData[count - 1 - j] = ((char *)data)[j]; |
1446 lineData[count - 1 - j] = ((char *)data)[j]; |
1461 else /* String of bytes. */ |
1447 else /* String of bytes. */ |
1462 lineData[j] = ((char *)data)[j]; |
1448 lineData[j] = ((char *)data)[j]; |
1463 # else |
1449 # else |
1464 lineData[j] = ((char *)data)[j]; |
1450 lineData[j] = ((char *)data)[j]; |
1465 # endif |
1451 # endif |
1466 } |
1452 } |
1467 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
1453 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
1468 # ifdef CANOPEN_BIG_ENDIAN |
1454 # ifdef CANOPEN_BIG_ENDIAN |
1469 if (dataType == 0 && endianize) |
1455 if (dataType == 0 && endianize) |
1470 d->transfers[line].data[count - 1 - j] = ((char *)data)[j]; |
1456 d->transfers[line].data[count - 1 - j] = ((char *)data)[j]; |
1471 else /* String of bytes. */ |
1457 else /* String of bytes. */ |
1472 d->transfers[line].data[j] = ((char *)data)[j]; |
1458 d->transfers[line].data[j] = ((char *)data)[j]; |
1473 # else |
1459 # else |
1474 d->transfers[line].data[j] = ((char *)data)[j]; |
1460 d->transfers[line].data[j] = ((char *)data)[j]; |
1475 # endif |
1461 # endif |
1476 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
1462 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
1477 } |
1463 } |
1478 /* Send the SDO to the server. Initiate download, cs=1. */ |
1464 /* Send the SDO to the server. Initiate download, cs=1. */ |
1479 sdo.nodeId = nodeId; |
1465 if (count <= 4) { /* Expedited transfert */ |
1480 if (count <= 4) { /* Expedited transfert */ |
1466 buf[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3); |
1481 sdo.body.data[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3); |
1467 for (i = 4 ; i < 8 ; i++) |
1482 for (i = 4 ; i < 8 ; i++) |
1468 buf[i] = d->transfers[line].data[i - 4]; |
1483 sdo.body.data[i] = d->transfers[line].data[i - 4]; |
1469 d->transfers[line].offset = count; |
1484 d->transfers[line].offset = count; |
1470 } |
1485 } |
1471 else { /** Normal transfert */ |
1486 else { /** Normal transfert */ |
1472 buf[0] = (1 << 5) | 1; |
1487 sdo.body.data[0] = (1 << 5) | 1; |
1473 for (i = 0 ; i < 4 ; i++) |
1488 for (i = 0 ; i < 4 ; i++) |
1474 buf[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */ |
1489 sdo.body.data[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */ |
1475 } |
1490 } |
1476 buf[1] = index & 0xFF; /* LSB */ |
1491 sdo.body.data[1] = index & 0xFF; /* LSB */ |
1477 buf[2] = (index >> 8) & 0xFF; /* MSB */ |
1492 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
1478 buf[3] = subIndex; |
1493 sdo.body.data[3] = subIndex; |
1479 |
1494 |
1480 d->transfers[line].Callback = Callback; |
1495 d->transfers[line].Callback = Callback; |
1481 |
1496 |
1482 err = sendSDO(d, SDO_CLIENT, CliNbr, buf); |
1497 err = sendSDO(d, SDO_CLIENT, sdo); |
1483 if (err) { |
1498 if (err) { |
1484 MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId); |
1499 MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId); |
1485 /* release the line */ |
1500 /* release the line */ |
1486 resetSDOline(d, line); |
1501 resetSDOline(d, line); |
1487 return 0xFF; |
1502 return 0xFF; |
1488 } |
1503 } |
1489 |
1504 |
1490 |
1505 |
1491 return 0; |
1506 return 0; |
1492 } |
1507 } |
1493 |
1508 |
1494 /*! |
1509 /*! |
1495 ** |
1510 ** |
1496 ** |
1511 ** |
1497 ** @param d |
1512 ** @param d |
1498 ** @param nodeId |
1513 ** @param nodeId |
1499 ** @param index |
1514 ** @param index |
1500 ** @param subIndex |
1515 ** @param subIndex |
1501 ** @param count |
1516 ** @param count |
1502 ** @param dataType |
1517 ** @param dataType |
1503 ** @param data |
1518 ** @param data |
1504 ** |
1519 ** |
1505 ** @return |
1520 ** @return |
1506 **/ |
1521 **/ |
|
1522 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
1507 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
1523 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data) |
1508 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data) |
1524 { |
1509 { |
1525 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1); |
1510 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1); |
1526 } |
1511 } |
1527 |
1512 |
1528 /*! |
1513 /*! |
1529 ** |
1514 ** |
1530 ** |
1515 ** |
1531 ** @param d |
1516 ** @param d |
1532 ** @param nodeId |
1517 ** @param nodeId |
1533 ** @param index |
1518 ** @param index |
1534 ** @param subIndex |
1519 ** @param subIndex |
1535 ** @param count |
1520 ** @param count |
1536 ** @param dataType |
1521 ** @param dataType |
1537 ** @param data |
1522 ** @param data |
1538 ** @param Callback |
1523 ** @param Callback |
1539 ** |
1524 ** |
1540 ** @return |
1525 ** @return |
1541 **/ |
1526 **/ |
1542 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, |
1527 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, |
1543 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback) |
1528 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback) |
1544 { |
1529 { |
1545 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1); |
1530 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1); |
1546 } |
1531 } |
1547 |
1532 |
1548 UNS8 writeNetworkDictCallBackAI (CO_Data* d, UNS8 nodeId, UNS16 index, |
1533 UNS8 writeNetworkDictCallBackAI (CO_Data* d, UNS8 nodeId, UNS16 index, |
1549 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) |
1534 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) |
1550 { |
1535 { |
1551 UNS8 ret; |
1536 UNS8 ret; |
1552 UNS16 lastIndex; |
1537 UNS16 lastIndex; |
1553 UNS16 offset; |
1538 UNS16 offset; |
1554 UNS8 nodeIdServer; |
1539 UNS8 nodeIdServer; |