80 |
80 |
81 EC_INFO("Initializing %i EtherCAT master(s)...\n", ec_master_count); |
81 EC_INFO("Initializing %i EtherCAT master(s)...\n", ec_master_count); |
82 |
82 |
83 if (!(ec_masters = (ec_master_t *) |
83 if (!(ec_masters = (ec_master_t *) |
84 kmalloc(sizeof(ec_master_t) * ec_master_count, GFP_KERNEL))) { |
84 kmalloc(sizeof(ec_master_t) * ec_master_count, GFP_KERNEL))) { |
85 EC_ERR("Could not allocate memory for EtherCAT master(s)!\n"); |
85 EC_ERR("Failed to allocate master(s)!\n"); |
86 goto out_return; |
86 goto out_return; |
87 } |
87 } |
88 |
88 |
89 if (!(ec_masters_reserved = |
89 if (!(ec_masters_reserved = (unsigned int *) |
90 (unsigned int *) kmalloc(sizeof(int) * ec_master_count, |
90 kmalloc(sizeof(int) * ec_master_count, GFP_KERNEL))) { |
91 GFP_KERNEL))) { |
91 EC_ERR("Failed to allocate reservation flags!\n"); |
92 EC_ERR("Could not allocate memory for reservation flags!\n"); |
|
93 goto out_free_masters; |
92 goto out_free_masters; |
94 } |
93 } |
95 |
94 |
96 for (i = 0; i < ec_master_count; i++) { |
95 for (i = 0; i < ec_master_count; i++) { |
97 ec_master_init(ec_masters + i); |
96 ec_master_init(ec_masters + i); |
239 /**< EtherCAT-Master-Index */ |
238 /**< EtherCAT-Master-Index */ |
240 ) |
239 ) |
241 { |
240 { |
242 ec_master_t *master; |
241 ec_master_t *master; |
243 |
242 |
244 EC_INFO("===== Starting master %i... =====\n", index); |
243 EC_INFO("Requesting master %i...\n", index); |
245 |
244 |
246 if (index < 0 || index >= ec_master_count) { |
245 if (index < 0 || index >= ec_master_count) { |
247 EC_ERR("Master %i does not exist!\n", index); |
246 EC_ERR("Master %i does not exist!\n", index); |
248 goto req_return; |
247 goto req_return; |
249 } |
248 } |
250 |
249 |
251 if (ec_masters_reserved[index]) { |
250 if (ec_masters_reserved[index]) { |
252 EC_ERR("Master %i already in use!\n", index); |
251 EC_ERR("Master %i is already in use!\n", index); |
253 goto req_return; |
252 goto req_return; |
254 } |
253 } |
255 |
254 |
|
255 ec_masters_reserved[index] = 1; |
256 master = &ec_masters[index]; |
256 master = &ec_masters[index]; |
257 |
257 |
258 if (!master->device) { |
258 if (!master->device) { |
259 EC_ERR("Master %i has no device assigned yet!\n", index); |
259 EC_ERR("Master %i has no assigned device!\n", index); |
260 goto req_return; |
260 goto req_release; |
261 } |
261 } |
262 |
262 |
263 if (!try_module_get(master->device->module)) { |
263 if (!try_module_get(master->device->module)) { |
264 EC_ERR("Failed to reserve device module!\n"); |
264 EC_ERR("Failed to reserve device module!\n"); |
265 goto req_return; |
265 goto req_release; |
266 } |
266 } |
267 |
267 |
268 if (ec_master_open(master)) { |
268 if (ec_device_open(master->device)) { |
269 EC_ERR("Failed to open device!\n"); |
269 EC_ERR("Failed to open device!\n"); |
270 goto req_module_put; |
270 goto req_module_put; |
271 } |
271 } |
272 |
272 |
273 if (!master->device->link_state) EC_WARN("Link is DOWN.\n"); |
273 if (!master->device->link_state) |
|
274 EC_WARN("Link is DOWN.\n"); |
274 |
275 |
275 if (ec_master_bus_scan(master) != 0) { |
276 if (ec_master_bus_scan(master) != 0) { |
276 EC_ERR("Bus scan failed!\n"); |
277 EC_ERR("Bus scan failed!\n"); |
277 goto req_close; |
278 goto req_close; |
278 } |
279 } |
279 |
280 |
280 ec_masters_reserved[index] = 1; |
281 EC_INFO("Master %i is ready.\n", index); |
281 EC_INFO("===== Master %i ready. =====\n", index); |
|
282 |
|
283 return master; |
282 return master; |
284 |
283 |
285 req_close: |
284 req_close: |
286 ec_master_close(master); |
285 if (ec_device_close(master->device)) |
287 |
286 EC_WARN("Warning - Failed to close device!\n"); |
288 req_module_put: |
287 req_module_put: |
289 module_put(master->device->module); |
288 module_put(master->device->module); |
290 ec_master_reset(master); |
289 ec_master_reset(master); |
291 |
290 req_release: |
|
291 ec_masters_reserved[index] = 0; |
292 req_return: |
292 req_return: |
293 EC_INFO("===== Failed to start master %i =====\n", index); |
293 EC_ERR("Failed requesting master %i.\n", index); |
294 return NULL; |
294 return NULL; |
295 } |
295 } |
296 |
296 |
297 /*****************************************************************************/ |
297 /*****************************************************************************/ |
298 |
298 |
299 /** |
299 /** |
300 Gibt einen zuvor angeforderten EtherCAT-Master wieder frei. |
300 Gibt einen zuvor angeforderten EtherCAT-Master wieder frei. |
301 */ |
301 */ |
302 |
302 |
303 void ecrt_release_master(ec_master_t *master /**< EtherCAT-Masdter */) |
303 void ecrt_release_master(ec_master_t *master /**< EtherCAT-Master */) |
304 { |
304 { |
305 unsigned int i, found; |
305 unsigned int i, found; |
306 |
306 |
307 found = 0; |
307 found = 0; |
308 for (i = 0; i < ec_master_count; i++) { |
308 for (i = 0; i < ec_master_count; i++) { |
311 break; |
311 break; |
312 } |
312 } |
313 } |
313 } |
314 |
314 |
315 if (!found) { |
315 if (!found) { |
316 EC_WARN("Master %X was never requested!\n", (u32) master); |
316 EC_WARN("Master 0x%08X was never requested!\n", (u32) master); |
317 return; |
317 return; |
318 } |
318 } |
319 |
319 |
320 EC_INFO("===== Stopping master %i... =====\n", i); |
320 EC_INFO("Releasing master %i...\n", i); |
321 |
321 |
322 ec_master_close(master); |
|
323 ec_master_reset(master); |
322 ec_master_reset(master); |
|
323 |
|
324 if (ec_device_close(master->device)) |
|
325 EC_WARN("Warning - Failed to close device!\n"); |
324 |
326 |
325 module_put(master->device->module); |
327 module_put(master->device->module); |
326 ec_masters_reserved[i] = 0; |
328 ec_masters_reserved[i] = 0; |
327 |
329 |
328 EC_INFO("===== Master %i stopped. =====\n", i); |
330 EC_INFO("Released master %i.\n", i); |
329 return; |
331 return; |
330 } |
332 } |
331 |
333 |
332 /*****************************************************************************/ |
334 /*****************************************************************************/ |
333 |
335 |