81 d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback); |
84 d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback); |
82 /* If DCF entry do not exist... Nothing to do.*/ |
85 /* If DCF entry do not exist... Nothing to do.*/ |
83 if (errorCode != OD_SUCCESSFUL) goto DCF_finish; |
86 if (errorCode != OD_SUCCESSFUL) goto DCF_finish; |
84 } |
87 } |
85 |
88 |
86 /* Loop on all DCF subindexes, corresponding to nodes ID */ |
89 UNS8 szData = d->dcf_odentry->pSubindex[nodeId].size; |
87 while (nodeId < d->dcf_odentry->bSubCount){ |
90 /* if the entry for the nodeId is not empty. */ |
88 UNS32 nb_entries; |
91 if(szData!=0){ |
|
92 /* if the entry for the nodeId is already been processing, quit.*/ |
|
93 if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND) return 1; |
|
94 |
|
95 d->dcf_odentry->pSubindex[nodeId].bAccessType|=DCF_TO_SEND; |
|
96 d->dcf_request++; |
|
97 if(d->dcf_request==1) |
|
98 send_consise_dcf_loop(d,nodeId); |
|
99 return 1; |
|
100 } |
|
101 |
|
102 DCF_finish: |
|
103 return 0; |
|
104 } |
89 |
105 |
90 UNS8 szData = d->dcf_odentry->pSubindex[nodeId].size; |
106 static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId) |
91 UNS8* dcfend; |
107 { |
|
108 /* Loop on all DCF subindexes, corresponding to node ID until there is no request*/ |
|
109 //while (nodeId < d->dcf_odentry->bSubCount){ |
|
110 while (d->dcf_request>0){ |
|
111 if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND){ |
|
112 UNS32 nb_entries; |
|
113 UNS8 szData = d->dcf_odentry->pSubindex[nodeId].size; |
|
114 UNS8* dcfend; |
|
115 |
|
116 { |
|
117 UNS8* dcf = *((UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject); |
|
118 dcfend = dcf + szData; |
|
119 if (!d->dcf_cursor){ |
|
120 d->dcf_cursor = (UNS8*)dcf + 4; |
|
121 d->dcf_entries_count = 0; |
|
122 } |
|
123 nb_entries = UNS32_LE(*((UNS32*)dcf)); |
|
124 } |
92 |
125 |
93 { |
126 /* condition on consise DCF string for NodeID, if big enough */ |
94 UNS8* dcf = *((UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject); |
127 if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){ |
95 dcfend = dcf + szData; |
128 |
96 if (!d->dcf_cursor){ |
129 UNS16 target_Index; |
97 d->dcf_cursor = (UNS8*)dcf + 4; |
130 UNS8 target_Subindex; |
98 d->dcf_entries_count = 0; |
131 UNS32 target_Size; |
99 } |
|
100 nb_entries = UNS32_LE(*((UNS32*)dcf)); |
|
101 } |
|
102 |
132 |
103 /* condition on consise DCF string for NodeID, if big enough */ |
133 /* pointer to the DCF string for NodeID */ |
104 if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries) |
134 target_Index = UNS16_LE(*((UNS16*)(d->dcf_cursor))); d->dcf_cursor += 2; |
105 { |
135 target_Subindex = *((UNS8*)((UNS8*)d->dcf_cursor++)); |
106 UNS16 target_Index; |
136 target_Size = UNS32_LE(*((UNS32*)(d->dcf_cursor))); d->dcf_cursor += 4; |
107 UNS8 target_Subindex; |
137 |
108 UNS32 target_Size; |
138 _writeNetworkDict(d, /* CO_Data* d*/ |
109 |
|
110 /* pointer to the DCF string for NodeID */ |
|
111 target_Index = UNS16_LE(*((UNS16*)(d->dcf_cursor))); d->dcf_cursor += 2; |
|
112 target_Subindex = *((UNS8*)((UNS8*)d->dcf_cursor++)); |
|
113 target_Size = UNS32_LE(*((UNS32*)(d->dcf_cursor))); d->dcf_cursor += 4; |
|
114 |
|
115 _writeNetworkDict(d, /* CO_Data* d*/ |
|
116 nodeId, /* UNS8 nodeId*/ |
139 nodeId, /* UNS8 nodeId*/ |
117 target_Index, /* UNS16 index*/ |
140 target_Index, /* UNS16 index*/ |
118 target_Subindex, /* UNS8 subindex*/ |
141 target_Subindex, /* UNS8 subindex*/ |
119 target_Size, /* UNS8 count*/ |
142 target_Size, /* UNS8 count*/ |
120 0, /* UNS8 dataType*/ |
143 0, /* UNS8 dataType*/ |
121 d->dcf_cursor,/* void *data*/ |
144 d->dcf_cursor,/* void *data*/ |
122 CheckSDOAndContinue,/* SDOCallback_t |
145 CheckSDOAndContinue,/* SDOCallback_t |
123 Callback*/ |
146 Callback*/ |
124 0); /* no endianize*/ |
147 0); /* no endianize*/ |
125 /* Push d->dcf_cursor to the end of data*/ |
148 /* Push d->dcf_cursor to the end of data*/ |
126 |
149 |
127 d->dcf_cursor += target_Size; |
150 d->dcf_cursor += target_Size; |
128 d->dcf_entries_count++; |
151 d->dcf_entries_count++; |
129 |
152 |
130 /* send_consise_dcf will be called by CheckSDOAndContinue for next DCF entry*/ |
153 /* send_consise_dcf_loop will be called by CheckSDOAndContinue for next DCF entry*/ |
131 return; |
154 return; |
132 } |
155 } |
133 nodeId++; |
156 else |
134 d->dcf_cursor = NULL; |
157 { |
|
158 /* We have finished with the dcf entry. Change the flag, decrement the request |
|
159 * and execute the bootup callback. */ |
|
160 d->dcf_odentry->pSubindex[nodeId].bAccessType&=~DCF_TO_SEND; |
|
161 d->dcf_request--; |
|
162 (*d->post_SlaveBootup)(nodeId); |
|
163 } |
|
164 } |
|
165 else |
|
166 { |
|
167 /* Check the next element*/ |
|
168 //nodeId++; |
|
169 nodeId=(nodeId+1) % d->dcf_odentry->bSubCount; |
|
170 if(nodeId==d->dcf_odentry->bSubCount)nodeId=1; |
|
171 d->dcf_cursor = NULL; |
|
172 } |
135 } |
173 } |
136 DCF_finish: |
174 |
|
175 //DCF_finish: |
137 /* Switch Master to preOperational state */ |
176 /* Switch Master to preOperational state */ |
138 (*d->preOperational)(); |
177 |
|
178 //(*d->preOperational)(); |
139 } |
179 } |