25 #define EC_FUNC_WRITE_FOOTER \ |
26 #define EC_FUNC_WRITE_FOOTER \ |
26 frame->data_length = length; \ |
27 frame->data_length = length; \ |
27 memcpy(frame->data, data, length); |
28 memcpy(frame->data, data, length); |
28 |
29 |
29 #define EC_FUNC_READ_FOOTER \ |
30 #define EC_FUNC_READ_FOOTER \ |
30 frame->data_length = length; |
31 frame->data_length = length; \ |
|
32 memset(frame->data, 0x00, length); |
31 |
33 |
32 /*****************************************************************************/ |
34 /*****************************************************************************/ |
33 |
35 |
34 /** |
36 /** |
35 Initialisiert ein EtherCAT-NPRD-Kommando. |
37 Initialisiert ein EtherCAT-NPRD-Kommando. |
287 |
289 |
288 // Zeiger auf Socket-Buffer holen |
290 // Zeiger auf Socket-Buffer holen |
289 data = ec_device_prepare(&frame->master->device); |
291 data = ec_device_prepare(&frame->master->device); |
290 |
292 |
291 // EtherCAT frame header |
293 // EtherCAT frame header |
292 data[0] = command_size & 0xFF; |
294 EC_WRITE_U16(data, (command_size & 0x7FF) | 0x1000); |
293 data[1] = ((command_size & 0x700) >> 8) | 0x10; |
|
294 data += EC_FRAME_HEADER_SIZE; |
295 data += EC_FRAME_HEADER_SIZE; |
295 |
296 |
296 // EtherCAT command header |
297 // EtherCAT command header |
297 data[0] = frame->type; |
298 EC_WRITE_U8 (data, frame->type); |
298 data[1] = frame->index; |
299 EC_WRITE_U8 (data + 1, frame->index); |
299 data[2] = frame->address.raw[0]; |
300 EC_WRITE_U32(data + 2, frame->address.logical); |
300 data[3] = frame->address.raw[1]; |
301 EC_WRITE_U16(data + 6, frame->data_length & 0x7FF); |
301 data[4] = frame->address.raw[2]; |
302 EC_WRITE_U16(data + 8, 0x0000); |
302 data[5] = frame->address.raw[3]; |
|
303 data[6] = frame->data_length & 0xFF; |
|
304 data[7] = (frame->data_length & 0x700) >> 8; |
|
305 data[8] = 0x00; |
|
306 data[9] = 0x00; |
|
307 data += EC_COMMAND_HEADER_SIZE; |
303 data += EC_COMMAND_HEADER_SIZE; |
308 |
304 |
309 if (likely(frame->type == ec_frame_type_apwr // Write commands |
305 // EtherCAT command data |
310 || frame->type == ec_frame_type_npwr |
306 memcpy(data, frame->data, frame->data_length); |
311 || frame->type == ec_frame_type_bwr |
307 data += frame->data_length; |
312 || frame->type == ec_frame_type_lrw)) { |
|
313 memcpy(data, frame->data, frame->data_length); |
|
314 } |
|
315 else { // Read commands |
|
316 memset(data, 0x00, frame->data_length); |
|
317 } |
|
318 |
308 |
319 // EtherCAT command footer |
309 // EtherCAT command footer |
320 data += frame->data_length; |
310 EC_WRITE_U16(data, frame->working_counter); |
321 data[0] = frame->working_counter & 0xFF; |
|
322 data[1] = (frame->working_counter & 0xFF00) >> 8; |
|
323 data += EC_COMMAND_FOOTER_SIZE; |
311 data += EC_COMMAND_FOOTER_SIZE; |
324 |
312 |
325 // Pad with zeros |
313 // Pad with zeros |
326 for (i = EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE |
314 for (i = EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE |
327 + frame->data_length + EC_COMMAND_FOOTER_SIZE; |
315 + frame->data_length + EC_COMMAND_FOOTER_SIZE; |
328 i < EC_MIN_FRAME_SIZE; i++) { |
316 i < EC_MIN_FRAME_SIZE; i++) |
329 *data++ = 0x00; |
317 EC_WRITE_U8(data++, 0x00); |
330 } |
|
331 |
318 |
332 // Send frame |
319 // Send frame |
333 ec_device_send(&frame->master->device, frame_size); |
320 ec_device_send(&frame->master->device, frame_size); |
334 |
321 |
335 return 0; |
322 return 0; |
369 } |
356 } |
370 |
357 |
371 data = ec_device_data(device); |
358 data = ec_device_data(device); |
372 |
359 |
373 // Länge des gesamten Frames prüfen |
360 // Länge des gesamten Frames prüfen |
374 frame_length = (data[0] & 0xFF) | ((data[1] & 0x07) << 8); |
361 frame_length = EC_READ_U16(data) & 0x07FF; |
|
362 data += EC_FRAME_HEADER_SIZE; |
375 |
363 |
376 if (unlikely(frame_length > received_length)) { |
364 if (unlikely(frame_length > received_length)) { |
377 printk(KERN_ERR "EtherCAT: Received corrupted frame (length does" |
365 printk(KERN_ERR "EtherCAT: Received corrupted frame (length does" |
378 " not match)!\n"); |
366 " not match)!\n"); |
379 ec_frame_print(frame); |
367 ec_frame_print(frame); |
380 return -1; |
368 return -1; |
381 } |
369 } |
382 |
370 |
383 // Command header |
371 // Command header |
384 data += EC_FRAME_HEADER_SIZE; |
372 command_type = EC_READ_U8(data); |
385 command_type = data[0]; |
373 command_index = EC_READ_U8(data + 1); |
386 command_index = data[1]; |
374 data_length = EC_READ_U16(data + 6) & 0x07FF; |
387 data_length = (data[6] & 0xFF) | ((data[7] & 0x07) << 8); |
375 data += EC_COMMAND_HEADER_SIZE; |
388 |
376 |
389 if (unlikely(EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE |
377 if (unlikely(EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE |
390 + data_length + EC_COMMAND_FOOTER_SIZE > received_length)) { |
378 + data_length + EC_COMMAND_FOOTER_SIZE > received_length)) { |
391 printk(KERN_ERR "EtherCAT: Received frame with incomplete command" |
379 printk(KERN_ERR "EtherCAT: Received frame with incomplete command" |
392 " data!\n"); |
380 " data!\n"); |
397 if (unlikely(frame->type != command_type |
385 if (unlikely(frame->type != command_type |
398 || frame->index != command_index |
386 || frame->index != command_index |
399 || frame->data_length != data_length)) |
387 || frame->data_length != data_length)) |
400 { |
388 { |
401 printk(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n"); |
389 printk(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n"); |
402 ec_frame_print(frame); |
390 ec_frame_print(frame); // FIXME uninteressant... |
403 ec_device_call_isr(device); // Empfangenes "vergessen" |
391 ec_device_call_isr(device); // Empfangenes "vergessen" |
404 return -1; |
392 return -1; |
405 } |
393 } |
406 |
394 |
407 frame->state = ec_frame_received; |
395 frame->state = ec_frame_received; |
408 |
396 |
409 // Empfangene Daten in Kommandodatenspeicher kopieren |
397 // Empfangene Daten in Kommandodatenspeicher kopieren |
410 data += EC_COMMAND_HEADER_SIZE; |
|
411 memcpy(frame->data, data, data_length); |
398 memcpy(frame->data, data, data_length); |
412 data += data_length; |
399 data += data_length; |
413 |
400 |
414 // Working-Counter setzen |
401 // Working-Counter setzen |
415 frame->working_counter = (data[0] & 0xFF) | ((data[1] & 0xFF) << 8); |
402 frame->working_counter = EC_READ_U16(data); |
416 |
403 |
417 if (unlikely(frame->master->debug_level > 1)) { |
404 if (unlikely(frame->master->debug_level > 1)) { |
418 ec_frame_print(frame); |
405 ec_frame_print(frame); |
419 } |
406 } |
420 |
407 |