34 */ |
34 */ |
35 |
35 |
36 |
36 |
37 #include "data.h" |
37 #include "data.h" |
38 #include "sysdep.h" |
38 #include "sysdep.h" |
39 |
39 #include "dcf.h" |
40 extern UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
40 |
41 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize); |
41 typedef struct { |
42 |
42 UNS16 Index; |
43 |
43 UNS8 Subindex; |
44 static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId); |
44 UNS32 Size; |
45 |
45 UNS8 *Data; |
46 /* Seek to next NodeID's DCF */ |
46 } dcf_entry_t; |
47 #define SEEK_NEXT_DCF() \ |
47 |
48 nodeId=(nodeId+1) % d->dcf_odentry->bSubCount; \ |
48 void SaveNode(CO_Data* d, UNS8 nodeId); |
49 if(nodeId==0) nodeId=1; \ |
49 static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId); |
50 d->dcf_cursor = NULL; |
50 static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId); |
51 |
51 UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId); |
52 /** |
52 |
53 ** |
53 /** |
|
54 ** @brief Function to be called from post_SlaveBootup |
54 ** |
55 ** |
55 ** @param d |
56 ** @param d |
56 ** @param nodeId |
57 ** @param nodeId |
57 */ |
58 */ |
58 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId) |
59 UNS8 check_and_start_node(CO_Data* d, UNS8 nodeId) |
59 { |
60 { |
60 UNS32 abortCode = 0; |
61 if(d->dcf_status != DCF_STATUS_INIT) |
61 |
62 return 0; |
62 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) |
63 if((init_consise_dcf(d, nodeId) == 0) || (read_consise_dcf_next_entry(d, nodeId) == 0)){ |
63 { |
64 /* Ask slave node to go in operational mode */ |
64 MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode); |
65 masterSendNMTstateChange (d, nodeId, NMT_Start_Node); |
65 MSG_WAR(0x2A02, "server node : ", nodeId); |
66 d->NMTable[nodeId] = Operational; |
66 } |
67 return 1; |
67 |
68 } |
68 closeSDOtransfer(d, nodeId, SDO_CLIENT); |
69 d->dcf_status = DCF_STATUS_READ_CHECK; |
69 /* Timedout ? */ |
70 return 2; |
70 if(abortCode == SDOABT_TIMED_OUT){ |
71 } |
71 /* Node may not be ready, try another one */ |
|
72 /* Warning, this might leed to endless attempts */ |
|
73 /* if node does never answer */ |
|
74 SEEK_NEXT_DCF() |
|
75 } |
|
76 send_consise_dcf_loop(d,nodeId); |
|
77 } |
|
78 |
|
79 |
72 |
80 /** |
73 /** |
81 ** |
74 ** |
82 ** |
75 ** |
83 ** @param d |
76 ** @param d |
84 ** @param nodeId |
77 ** @param nodeId |
|
78 */ |
|
79 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId) |
|
80 { |
|
81 UNS32 abortCode = 0; |
|
82 UNS8 buf[4], match = 0, node; |
|
83 UNS32 size=4; |
|
84 if(d->dcf_status == DCF_STATUS_READ_CHECK){ |
|
85 // printf("DCF_STATUS_READ_CHECK \n"); |
|
86 if(getReadResultNetworkDict (d, nodeId, buf, &size, &abortCode) != SDO_FINISHED) |
|
87 goto dcferror; |
|
88 /* Check if data received match the DCF */ |
|
89 if(size == d->dcf_size){ |
|
90 match = 1; |
|
91 while(--size) |
|
92 if(buf[size] != d->dcf_data[size]) |
|
93 match = 0; |
|
94 } |
|
95 if(match) { |
|
96 if(read_consise_dcf_next_entry(d, nodeId) == 0){ |
|
97 masterSendNMTstateChange (d, nodeId, NMT_Start_Node); |
|
98 d->NMTable[nodeId] = Operational; |
|
99 d->dcf_status = DCF_STATUS_INIT; |
|
100 /* Look for other nodes waiting to be started */ |
|
101 for(node = 0 ; node<NMT_MAX_NODE_ID ; node++){ |
|
102 if(d->NMTable[node] != Initialisation) |
|
103 continue; |
|
104 if(check_and_start_node(d, node) == 2) |
|
105 break; |
|
106 } |
|
107 } |
|
108 } |
|
109 else { /* Data received does not match : start rewriting all */ |
|
110 if((init_consise_dcf(d, nodeId) == 0) || (write_consise_dcf_next_entry(d, nodeId) == 0)) |
|
111 goto dcferror; |
|
112 d->dcf_status = DCF_STATUS_WRITE; |
|
113 } |
|
114 } |
|
115 else if(d->dcf_status == DCF_STATUS_WRITE){ |
|
116 // printf("DCF_STATUS_WRITE \n"); |
|
117 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) |
|
118 goto dcferror; |
|
119 if(write_consise_dcf_next_entry(d, nodeId) == 0){ |
|
120 SaveNode(d, nodeId); |
|
121 d->dcf_status = DCF_STATUS_SAVED; |
|
122 } |
|
123 } |
|
124 else if(d->dcf_status == DCF_STATUS_SAVED){ |
|
125 // printf("DCF_STATUS_SAVED \n"); |
|
126 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) |
|
127 goto dcferror; |
|
128 masterSendNMTstateChange (d, nodeId, NMT_Reset_Node); |
|
129 d->dcf_status = DCF_STATUS_INIT; |
|
130 d->NMTable[nodeId] = Unknown_state; |
|
131 } |
|
132 return; |
|
133 dcferror: |
|
134 MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode); |
|
135 MSG_WAR(0x2A02, "server node : ", nodeId); |
|
136 d->NMTable[nodeId] = Unknown_state; |
|
137 } |
|
138 |
|
139 /** |
|
140 ** |
|
141 ** |
|
142 ** @param d |
|
143 ** @param nodeId |
85 ** |
144 ** |
86 ** @return |
145 ** @return |
87 */ |
146 */ |
88 UNS8 send_consise_dcf(CO_Data* d,UNS8 nodeId) |
147 UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId) |
89 { |
148 { |
90 UNS32 szData; |
149 /* Fetch DCF OD entry */ |
91 /* Fetch DCF OD entry, if not already done */ |
|
92 if(!d->dcf_odentry) |
|
93 { |
|
94 UNS32 errorCode; |
150 UNS32 errorCode; |
95 ODCallback_t *Callback; |
151 ODCallback_t *Callback; |
96 d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback); |
152 d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback); |
97 /* If DCF entry do not exist... Nothing to do.*/ |
153 /* If DCF entry do not exist... Nothing to do.*/ |
98 if (errorCode != OD_SUCCESSFUL) goto DCF_finish; |
154 if (errorCode != OD_SUCCESSFUL) goto DCF_finish; |
|
155 /* Fix DCF table overflow */ |
|
156 if(nodeId > d->dcf_odentry->bSubCount) goto DCF_finish; |
|
157 /* If DCF empty... Nothing to do */ |
|
158 if(! d->dcf_odentry->pSubindex[nodeId].size) goto DCF_finish; |
|
159 UNS8* dcf = (UNS8*)d->dcf_odentry->pSubindex[nodeId].pObject; |
|
160 // printf("%.2x %.2x %.2x %.2x\n",dcf[0],dcf[1],dcf[2],dcf[3]); |
|
161 d->dcf_cursor = dcf + 4; |
|
162 d->dcf_entries_count = 0; |
|
163 d->dcf_status = DCF_STATUS_INIT; |
|
164 return 1; |
|
165 DCF_finish: |
|
166 return 0; |
|
167 } |
|
168 |
|
169 UNS8 get_next_DCF_data(CO_Data* d, dcf_entry_t *dcf_entry, UNS8 nodeId) |
|
170 { |
|
171 UNS8* dcfend; |
|
172 UNS32 nb_entries; |
|
173 UNS32 szData; |
|
174 if(!d->dcf_odentry) |
|
175 return 0; |
|
176 if(nodeId > d->dcf_odentry->bSubCount) |
|
177 return 0; |
|
178 szData = d->dcf_odentry->pSubindex[nodeId].size; |
|
179 UNS8* dcf = (UNS8*)d->dcf_odentry->pSubindex[nodeId].pObject; |
|
180 nb_entries = UNS32_LE(*((UNS32*)dcf)); |
|
181 dcfend = dcf + szData; |
|
182 if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){ |
|
183 /* DCF data may not be 32/16b aligned, |
|
184 * we cannot directly dereference d->dcf_cursor |
|
185 * as UNS16 or UNS32 |
|
186 * Do it byte per byte taking care on endianess*/ |
|
187 #ifdef CANOPEN_BIG_ENDIAN |
|
188 dcf_entry->Index = *(d->dcf_cursor++) << 8 | |
|
189 *(d->dcf_cursor++); |
|
190 #else |
|
191 memcpy(&dcf_entry->Index, d->dcf_cursor,2); |
|
192 d->dcf_cursor+=2; |
|
193 #endif |
|
194 dcf_entry->Subindex = *(d->dcf_cursor++); |
|
195 #ifdef CANOPEN_BIG_ENDIAN |
|
196 dcf_entry->Size = *(d->dcf_cursor++) << 24 | |
|
197 *(d->dcf_cursor++) << 16 | |
|
198 *(d->dcf_cursor++) << 8 | |
|
199 *(d->dcf_cursor++); |
|
200 #else |
|
201 memcpy(&dcf_entry->Size, d->dcf_cursor,4); |
|
202 d->dcf_cursor+=4; |
|
203 #endif |
|
204 d->dcf_data = dcf_entry->Data = d->dcf_cursor; |
|
205 d->dcf_size = dcf_entry->Size; |
|
206 d->dcf_cursor += dcf_entry->Size; |
|
207 d->dcf_entries_count++; |
|
208 return 1; |
99 } |
209 } |
100 if(d->dcf_odentry->bSubCount<=nodeId) goto DCF_finish; /* Fix DCF table overflow */ |
|
101 szData = d->dcf_odentry->pSubindex[nodeId].size; |
|
102 |
|
103 /* if the entry for the nodeId is not empty. */ |
|
104 if(szData!=0){ |
|
105 /* if the entry for the nodeId is already been processing, quit.*/ |
|
106 if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND) return 1; |
|
107 |
|
108 d->dcf_odentry->pSubindex[nodeId].bAccessType|=DCF_TO_SEND; |
|
109 d->dcf_request++; |
|
110 if(d->dcf_request==1) |
|
111 send_consise_dcf_loop(d,nodeId); |
|
112 return 1; |
|
113 } |
|
114 |
|
115 DCF_finish: |
|
116 return 0; |
210 return 0; |
117 } |
211 } |
118 |
212 |
119 static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId) |
213 static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId) |
120 { |
214 { |
121 if(nodeId > d->dcf_odentry->bSubCount) return; |
215 UNS8 Ret; |
122 /* Loop on all DCF subindexes, corresponding to node ID until there is no request*/ |
216 dcf_entry_t dcf_entry; |
123 //while (nodeId < d->dcf_odentry->bSubCount){ |
217 if(!get_next_DCF_data(d, &dcf_entry, nodeId)) |
124 while (d->dcf_request>0){ |
218 return 0; |
125 if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND){ |
219 Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/ |
126 UNS8* dcfend; |
220 nodeId, /* UNS8 nodeId*/ |
127 UNS32 nb_entries; |
221 dcf_entry.Index, /* UNS16 index*/ |
128 UNS32 szData = d->dcf_odentry->pSubindex[nodeId].size; |
222 dcf_entry.Subindex, /* UNS8 subindex*/ |
129 |
223 (UNS8)dcf_entry.Size, /* UNS8 count*/ |
130 { |
224 0, /* UNS8 dataType*/ |
131 UNS8* dcf = *((UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject); |
225 dcf_entry.Data,/* void *data*/ |
132 dcfend = dcf + szData; |
226 CheckSDOAndContinue,/* Callback*/ |
133 if (!d->dcf_cursor){ |
227 0, /* no endianize*/ |
134 d->dcf_cursor = (UNS8*)dcf + 4; |
228 0); /* no block mode */ |
135 d->dcf_entries_count = 0; |
229 if(Ret) |
136 } |
230 MSG_ERR(0x1A02,"Erreur writeNetworkDictCallBackAI",Ret); |
137 nb_entries = UNS32_LE(*((UNS32*)dcf)); |
231 return 1; |
138 } |
232 } |
139 |
233 |
140 /* condition on consise DCF string for NodeID, if big enough */ |
234 static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId) |
141 if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){ |
235 { |
142 |
236 UNS8 Ret; |
143 UNS16 target_Index; |
237 dcf_entry_t dcf_entry; |
144 UNS8 target_Subindex; |
238 if(!get_next_DCF_data(d, &dcf_entry, nodeId)) |
145 UNS32 target_Size; |
239 return 0; |
146 |
240 Ret = readNetworkDictCallbackAI(d, /* CO_Data* d*/ |
147 /* DCF data may not be 32/16b aligned, |
241 nodeId, /* UNS8 nodeId*/ |
148 * we cannot directly dereference d->dcf_cursor |
242 dcf_entry.Index, /* UNS16 index*/ |
149 * as UNS16 or UNS32 |
243 dcf_entry.Subindex, /* UNS8 subindex*/ |
150 * Do it byte per byte taking care on endianess*/ |
244 0, /* UNS8 dataType*/ |
151 #ifdef CANOPEN_BIG_ENDIAN |
245 CheckSDOAndContinue,/* Callback*/ |
152 target_Index = *(d->dcf_cursor++) << 8 | |
246 0); /* no block mode */ |
153 *(d->dcf_cursor++); |
247 if(Ret) |
154 #else |
248 MSG_ERR(0x1A03,"Erreur readNetworkDictCallbackAI",Ret); |
155 memcpy(&target_Index, d->dcf_cursor,2); |
249 return 1; |
156 d->dcf_cursor+=2; |
250 } |
157 #endif |
251 |
158 |
252 void SaveNode(CO_Data* d, UNS8 nodeId) |
159 target_Subindex = *(d->dcf_cursor++); |
253 { |
160 |
254 UNS8 Ret; |
161 #ifdef CANOPEN_BIG_ENDIAN |
255 UNS32 data=0x65766173; |
162 target_Size = *(d->dcf_cursor++) << 24 | |
256 Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/ |
163 *(d->dcf_cursor++) << 16 | |
257 nodeId, /* UNS8 nodeId*/ |
164 *(d->dcf_cursor++) << 8 | |
258 0x1010, /* UNS16 index*/ |
165 *(d->dcf_cursor++); |
259 1, /* UNS8 subindex*/ |
166 #else |
260 4, /* UNS8 count*/ |
167 memcpy(&target_Size, d->dcf_cursor,4); |
261 0, /* UNS8 dataType*/ |
168 d->dcf_cursor+=4; |
262 (void *)&data,/* void *data*/ |
169 #endif |
263 CheckSDOAndContinue,/* Callback*/ |
170 |
264 0, /* no endianize*/ |
171 _writeNetworkDict(d, /* CO_Data* d*/ |
265 0); /* no block mode */ |
172 nodeId, /* UNS8 nodeId*/ |
266 if(Ret) |
173 target_Index, /* UNS16 index*/ |
267 MSG_ERR(0x1A04,"Erreur writeNetworkDictCallBackAI",Ret); |
174 target_Subindex, /* UNS8 subindex*/ |
268 } |
175 (UNS8)target_Size, /* UNS8 count*/ |
|
176 0, /* UNS8 dataType*/ |
|
177 d->dcf_cursor,/* void *data*/ |
|
178 CheckSDOAndContinue,/* SDOCallback_t |
|
179 Callback*/ |
|
180 0); /* no endianize*/ |
|
181 /* Push d->dcf_cursor to the end of data*/ |
|
182 |
|
183 d->dcf_cursor += target_Size; |
|
184 d->dcf_entries_count++; |
|
185 |
|
186 /* send_consise_dcf_loop will be called by CheckSDOAndContinue for next DCF entry*/ |
|
187 return; |
|
188 } |
|
189 else |
|
190 { |
|
191 /* We have finished with the dcf entry. Change the flag, decrement the request |
|
192 * and execute the bootup callback. */ |
|
193 d->dcf_odentry->pSubindex[nodeId].bAccessType&=~DCF_TO_SEND; |
|
194 d->dcf_request--; |
|
195 (*d->post_SlaveBootup)(d, nodeId); |
|
196 } |
|
197 } |
|
198 |
|
199 SEEK_NEXT_DCF() |
|
200 } |
|
201 |
|
202 } |
|