197 { |
197 { |
198 ec_data_reg_t *data_reg; |
198 ec_data_reg_t *data_reg; |
199 const ec_sii_sync_t *sync; |
199 const ec_sii_sync_t *sync; |
200 const ec_sii_pdo_t *other_pdo; |
200 const ec_sii_pdo_t *other_pdo; |
201 const ec_sii_pdo_entry_t *other_entry; |
201 const ec_sii_pdo_entry_t *other_entry; |
202 unsigned int bit_offset, byte_offset, sync_found; |
202 unsigned int bit_offset, byte_offset; |
203 |
203 |
204 // Find sync manager for PDO |
204 // Find sync manager for PDO |
205 sync_found = 0; |
205 if (pdo->sync_index >= slave->sii_sync_count) { |
206 list_for_each_entry(sync, &slave->sii_syncs, list) { |
|
207 if (sync->index == pdo->sync_index) { |
|
208 sync_found = 1; |
|
209 break; |
|
210 } |
|
211 } |
|
212 |
|
213 if (!sync_found) { |
|
214 EC_ERR("No sync manager for PDO 0x%04X:%i.", |
206 EC_ERR("No sync manager for PDO 0x%04X:%i.", |
215 pdo->index, entry->subindex); |
207 pdo->index, entry->subindex); |
216 return -1; |
208 return -1; |
217 } |
209 } |
|
210 |
|
211 sync = &slave->sii_syncs[pdo->sync_index]; |
218 |
212 |
219 // Calculate offset (in sync manager) for process data pointer |
213 // Calculate offset (in sync manager) for process data pointer |
220 bit_offset = 0; |
214 bit_offset = 0; |
221 byte_offset = 0; |
215 byte_offset = 0; |
222 list_for_each_entry(other_pdo, &slave->sii_pdos, list) { |
216 list_for_each_entry(other_pdo, &slave->sii_pdos, list) { |
270 pointer */ |
264 pointer */ |
271 ) |
265 ) |
272 { |
266 { |
273 ec_data_reg_t *data_reg; |
267 ec_data_reg_t *data_reg; |
274 ec_sii_sync_t *sync; |
268 ec_sii_sync_t *sync; |
275 unsigned int sync_found, sync_index; |
269 unsigned int sync_index; |
276 uint16_t sync_length; |
270 uint16_t sync_length; |
277 |
271 |
278 switch (dir) { |
272 switch (dir) { // FIXME |
279 case EC_DIR_OUTPUT: sync_index = 2; break; |
273 case EC_DIR_OUTPUT: sync_index = 2; break; |
280 case EC_DIR_INPUT: sync_index = 3; break; |
274 case EC_DIR_INPUT: sync_index = 3; break; |
281 default: |
275 default: |
282 EC_ERR("Invalid direction!\n"); |
276 EC_ERR("Invalid direction!\n"); |
283 return -1; |
277 return -1; |
284 } |
278 } |
285 |
279 |
286 // Find sync manager |
280 if (sync_index >= slave->sii_sync_count) { |
287 sync_found = 0; |
|
288 list_for_each_entry(sync, &slave->sii_syncs, list) { |
|
289 if (sync->index == sync_index) { |
|
290 sync_found = 1; |
|
291 break; |
|
292 } |
|
293 } |
|
294 |
|
295 if (!sync_found) { |
|
296 EC_ERR("No sync manager found for PDO range.\n"); |
281 EC_ERR("No sync manager found for PDO range.\n"); |
297 return -1; |
282 return -1; |
298 } |
283 } |
299 |
284 sync = &slave->sii_syncs[sync_index]; |
300 // Allocate memory for data registration object |
285 |
|
286 // Allocate memory for data registration object |
301 if (!(data_reg = |
287 if (!(data_reg = |
302 (ec_data_reg_t *) kmalloc(sizeof(ec_data_reg_t), GFP_KERNEL))) { |
288 (ec_data_reg_t *) kmalloc(sizeof(ec_data_reg_t), GFP_KERNEL))) { |
303 EC_ERR("Failed to allocate data registration.\n"); |
289 EC_ERR("Failed to allocate data registration.\n"); |
304 return -1; |
290 return -1; |
305 } |
291 } |