115 else if(d->dcf_status == DCF_STATUS_WRITE){ |
115 else if(d->dcf_status == DCF_STATUS_WRITE){ |
116 // printf("DCF_STATUS_WRITE \n"); |
116 // printf("DCF_STATUS_WRITE \n"); |
117 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) |
117 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) |
118 goto dcferror; |
118 goto dcferror; |
119 if(write_consise_dcf_next_entry(d, nodeId) == 0){ |
119 if(write_consise_dcf_next_entry(d, nodeId) == 0){ |
|
120 #ifdef DCF_SAVE_NODE |
120 SaveNode(d, nodeId); |
121 SaveNode(d, nodeId); |
121 d->dcf_status = DCF_STATUS_SAVED; |
122 d->dcf_status = DCF_STATUS_SAVED; |
|
123 #else //DCF_SAVE_NODE |
|
124 masterSendNMTstateChange (d, nodeId, NMT_Start_Node); |
|
125 d->NMTable[nodeId] = Operational; |
|
126 d->dcf_status = DCF_STATUS_INIT; |
|
127 /* Look for other nodes waiting to be started */ |
|
128 for(node = 0 ; node<NMT_MAX_NODE_ID ; node++){ |
|
129 if(d->NMTable[node] != Initialisation) |
|
130 continue; |
|
131 if(check_and_start_node(d, node) == 2) |
|
132 break; |
|
133 } |
|
134 #endif //DCF_SAVE_NODE |
122 } |
135 } |
123 } |
136 } |
124 else if(d->dcf_status == DCF_STATUS_SAVED){ |
137 else if(d->dcf_status == DCF_STATUS_SAVED){ |
125 // printf("DCF_STATUS_SAVED \n"); |
138 // printf("DCF_STATUS_SAVED \n"); |
126 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) |
139 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) |
147 UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId) |
160 UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId) |
148 { |
161 { |
149 /* Fetch DCF OD entry */ |
162 /* Fetch DCF OD entry */ |
150 UNS32 errorCode; |
163 UNS32 errorCode; |
151 ODCallback_t *Callback; |
164 ODCallback_t *Callback; |
|
165 UNS8* dcf; |
152 d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback); |
166 d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback); |
153 /* If DCF entry do not exist... Nothing to do.*/ |
167 /* If DCF entry do not exist... Nothing to do.*/ |
154 if (errorCode != OD_SUCCESSFUL) goto DCF_finish; |
168 if (errorCode != OD_SUCCESSFUL) goto DCF_finish; |
155 /* Fix DCF table overflow */ |
169 /* Fix DCF table overflow */ |
156 if(nodeId > d->dcf_odentry->bSubCount) goto DCF_finish; |
170 if(nodeId > d->dcf_odentry->bSubCount) goto DCF_finish; |
157 /* If DCF empty... Nothing to do */ |
171 /* If DCF empty... Nothing to do */ |
158 if(! d->dcf_odentry->pSubindex[nodeId].size) goto DCF_finish; |
172 if(! d->dcf_odentry->pSubindex[nodeId].size) goto DCF_finish; |
159 UNS8* dcf = (UNS8*)d->dcf_odentry->pSubindex[nodeId].pObject; |
173 dcf = *(UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject; |
160 // printf("%.2x %.2x %.2x %.2x\n",dcf[0],dcf[1],dcf[2],dcf[3]); |
174 // printf("%.2x %.2x %.2x %.2x\n",dcf[0],dcf[1],dcf[2],dcf[3]); |
161 d->dcf_cursor = dcf + 4; |
175 d->dcf_cursor = dcf + 4; |
162 d->dcf_entries_count = 0; |
176 d->dcf_entries_count = 0; |
163 d->dcf_status = DCF_STATUS_INIT; |
177 d->dcf_status = DCF_STATUS_INIT; |
164 return 1; |
178 return 1; |
169 UNS8 get_next_DCF_data(CO_Data* d, dcf_entry_t *dcf_entry, UNS8 nodeId) |
183 UNS8 get_next_DCF_data(CO_Data* d, dcf_entry_t *dcf_entry, UNS8 nodeId) |
170 { |
184 { |
171 UNS8* dcfend; |
185 UNS8* dcfend; |
172 UNS32 nb_entries; |
186 UNS32 nb_entries; |
173 UNS32 szData; |
187 UNS32 szData; |
|
188 UNS8* dcf; |
174 if(!d->dcf_odentry) |
189 if(!d->dcf_odentry) |
175 return 0; |
190 return 0; |
176 if(nodeId > d->dcf_odentry->bSubCount) |
191 if(nodeId > d->dcf_odentry->bSubCount) |
177 return 0; |
192 return 0; |
178 szData = d->dcf_odentry->pSubindex[nodeId].size; |
193 szData = d->dcf_odentry->pSubindex[nodeId].size; |
179 UNS8* dcf = (UNS8*)d->dcf_odentry->pSubindex[nodeId].pObject; |
194 dcf = *(UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject; |
180 nb_entries = UNS32_LE(*((UNS32*)dcf)); |
195 nb_entries = UNS32_LE(*((UNS32*)dcf)); |
181 dcfend = dcf + szData; |
196 dcfend = dcf + szData; |
182 if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){ |
197 if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){ |
183 /* DCF data may not be 32/16b aligned, |
198 /* DCF data may not be 32/16b aligned, |
184 * we cannot directly dereference d->dcf_cursor |
199 * we cannot directly dereference d->dcf_cursor |