Skip to content
Snippets Groups Projects
Commit 4e90e0b3 authored by Florian Pose's avatar Florian Pose
Browse files

EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.

parent 65a12a2b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/EtherCAT_rt.h
+ 4
− 4
View file @ 4e90e0b3
...@@ -87,10 +87,10 @@ int EtherCAT_rt_domain_xio(ec_domain_t *domain); ...@@ -87,10 +87,10 @@ int EtherCAT_rt_domain_xio(ec_domain_t *domain);
// Slave Methods // Slave Methods
int EtherCAT_rt_canopen_sdo_write(ec_slave_t *slave, int EtherCAT_rt_canopen_sdo_write(ec_slave_t *slave,
unsigned int sdo_index, uint16_t sdo_index,
unsigned char sdo_subindex, uint8_t sdo_subindex,
unsigned int value, uint32_t value,
unsigned int size); size_t size);
/*****************************************************************************/ /*****************************************************************************/
......
include/EtherCAT_si.h
+ 12
− 29
View file @ 4e90e0b3
...@@ -8,6 +8,8 @@ ...@@ -8,6 +8,8 @@
* *
*****************************************************************************/ *****************************************************************************/
#include <asm/byteorder.h>
/*****************************************************************************/ /*****************************************************************************/
// Bitwise read/write macros // Bitwise read/write macros
...@@ -24,31 +26,14 @@ ...@@ -24,31 +26,14 @@
// Read macros // Read macros
#define EC_READ_U8(PD) \ #define EC_READ_U8(PD) ((uint8_t) *((uint8_t *) (PD)))
(*((uint8_t *) (PD))) #define EC_READ_S8(PD) ((int8_t) *((uint8_t *) (PD)))
#define EC_READ_S8(PD) \
((int8_t) *((uint8_t *) (PD)))
#define EC_READ_U16(PD) \
((uint16_t) (*((uint8_t *) (PD) + 0) << 0 | \
*((uint8_t *) (PD) + 1) << 8))
#define EC_READ_S16(PD) \
((int16_t) (*((uint8_t *) (PD) + 0) << 0 | \
*((uint8_t *) (PD) + 1) << 8))
#define EC_READ_U32(PD) \ #define EC_READ_U16(PD) ((uint16_t) le16_to_cpup((void *) (PD)))
((uint32_t) (*((uint8_t *) (PD) + 0) << 0 | \ #define EC_READ_S16(PD) ((int16_t) le16_to_cpup((void *) (PD)))
*((uint8_t *) (PD) + 1) << 8 | \
*((uint8_t *) (PD) + 2) << 16 | \
*((uint8_t *) (PD) + 3) << 24))
#define EC_READ_S32(PD) \ #define EC_READ_U32(PD) ((uint32_t) le32_to_cpup((void *) (PD)))
((int32_t) (*((uint8_t *) (PD) + 0) << 0 | \ #define EC_READ_S32(PD) ((int32_t) le32_to_cpup((void *) (PD)))
*((uint8_t *) (PD) + 1) << 8 | \
*((uint8_t *) (PD) + 2) << 16 | \
*((uint8_t *) (PD) + 3) << 24))
/*****************************************************************************/ /*****************************************************************************/
...@@ -63,18 +48,16 @@ ...@@ -63,18 +48,16 @@
#define EC_WRITE_U16(PD, VAL) \ #define EC_WRITE_U16(PD, VAL) \
do { \ do { \
*((uint8_t *) (PD) + 0) = ((uint16_t) (VAL) >> 0) & 0xFF; \ *((uint16_t *) (PD)) = (uint16_t) (VAL); \
*((uint8_t *) (PD) + 1) = ((uint16_t) (VAL) >> 8) & 0xFF; \ cpu_to_le16s(PD); \
} while (0) } while (0)
#define EC_WRITE_S16(PD, VAL) EC_WRITE_U16(PD, VAL) #define EC_WRITE_S16(PD, VAL) EC_WRITE_U16(PD, VAL)
#define EC_WRITE_U32(PD, VAL) \ #define EC_WRITE_U32(PD, VAL) \
do { \ do { \
*((uint8_t *) (PD) + 0) = ((uint32_t) (VAL) >> 0) & 0xFF; \ *((uint32_t *) (PD)) = (uint32_t) (VAL); \
*((uint8_t *) (PD) + 1) = ((uint32_t) (VAL) >> 8) & 0xFF; \ cpu_to_le16s(PD); \
*((uint8_t *) (PD) + 2) = ((uint32_t) (VAL) >> 16) & 0xFF; \
*((uint8_t *) (PD) + 3) = ((uint32_t) (VAL) >> 24) & 0xFF; \
} while (0) } while (0)
#define EC_WRITE_S32(PD, VAL) EC_WRITE_U32(PD, VAL) #define EC_WRITE_S32(PD, VAL) EC_WRITE_U32(PD, VAL)
......
master/canopen.c
+ 22
− 30
View file @ 4e90e0b3
...@@ -10,6 +10,7 @@ ...@@ -10,6 +10,7 @@
#include <linux/delay.h> #include <linux/delay.h>
#include "../include/EtherCAT_si.h"
#include "master.h" #include "master.h"
/*****************************************************************************/ /*****************************************************************************/
...@@ -22,13 +23,12 @@ ...@@ -22,13 +23,12 @@
Schreibt ein CANopen-SDO (service data object). Schreibt ein CANopen-SDO (service data object).
*/ */
int EtherCAT_rt_canopen_sdo_write( int EtherCAT_rt_canopen_sdo_write(ec_slave_t *slave, /**< EtherCAT-Slave */
ec_slave_t *slave, /**< EtherCAT-Slave */ uint16_t sdo_index, /**< SDO-Index */
unsigned int sdo_index, /**< SDO-Index */ uint8_t sdo_subindex, /**< SDO-Subindex */
unsigned char sdo_subindex, /**< SDO-Subindex */ uint32_t value, /**< Neuer Wert */
unsigned int value, /**< Neuer Wert */ size_t size /**< Gre des Datenfeldes */
unsigned int size /**< Gre des Datenfeldes */ )
)
{ {
unsigned char data[0xF6]; unsigned char data[0xF6];
ec_frame_t frame; ec_frame_t frame;
...@@ -44,24 +44,17 @@ int EtherCAT_rt_canopen_sdo_write( ...@@ -44,24 +44,17 @@ int EtherCAT_rt_canopen_sdo_write(
return -1; return -1;
} }
data[0] = 0x0A; // Length of the Mailbox service data EC_WRITE_U16(data, 0x000A); // Length of the Mailbox service data
data[1] = 0x00; EC_WRITE_U16(data + 2, slave->station_address); // Station address
data[2] = slave->station_address & 0xFF; // Station address EC_WRITE_U8 (data + 4, 0x00); // Channel & priority
data[3] = (slave->station_address >> 8) & 0xFF; EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT
data[4] = 0x00; // Channel & priority EC_WRITE_U16(data + 6, 0x2000); // Number & Service
data[5] = 0x03; // CANopen over EtherCAT EC_WRITE_U8 (data + 8, 0x13 | ((4 - size) << 2)); // Spec., exp., init.
data[6] = 0x00; // Number(7-0) EC_WRITE_U16(data + 9, sdo_index);
data[7] = 0x2 << 4; // Number(8) & Service = SDO Request (0x02) EC_WRITE_U8 (data + 11, sdo_subindex);
data[8] = 0x01 // Size specified
| (0x1 << 1) // Transfer type = Expedited
| ((4 - size) << 2) // Data Set Size
| (0x1 << 5); // Command specifier = Initiate download request (0x01)
data[9] = sdo_index & 0xFF;
data[10] = (sdo_index >> 8) & 0xFF;
data[11] = sdo_subindex;
for (i = 0; i < size; i++) { for (i = 0; i < size; i++) {
data[12 + i] = value & 0xFF; EC_WRITE_U8(data + 12 + i, value & 0xFF);
value >>= 8; value >>= 8;
} }
...@@ -91,7 +84,7 @@ int EtherCAT_rt_canopen_sdo_write( ...@@ -91,7 +84,7 @@ int EtherCAT_rt_canopen_sdo_write(
return -1; return -1;
} }
if (frame.data[5] & 8) { // Written bit is high if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high
break; break;
} }
...@@ -115,12 +108,11 @@ int EtherCAT_rt_canopen_sdo_write( ...@@ -115,12 +108,11 @@ int EtherCAT_rt_canopen_sdo_write(
return -1; return -1;
} }
if (frame.data[5] != 0x03 // COE if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE
|| (frame.data[7] >> 4) != 0x03 // SDO response EC_READ_U16(frame.data + 6) != 0x3000 || // SDO response
|| (frame.data[8] >> 5) != 0x03 // Initiate download response EC_READ_U8 (frame.data + 8) >> 5 != 0x03 || // Download response
|| (frame.data[9] != (sdo_index & 0xFF)) // Index EC_READ_U16(frame.data + 9) != sdo_index || // Index
|| (frame.data[10] != ((sdo_index >> 8) & 0xFF)) EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex
|| (frame.data[11] != sdo_subindex)) // Subindex
{ {
printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n", printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n",
slave->ring_position); slave->ring_position);
......
master/frame.c
+ 23
− 36
View file @ 4e90e0b3
...@@ -11,6 +11,7 @@ ...@@ -11,6 +11,7 @@
#include <linux/slab.h> #include <linux/slab.h>
#include <linux/delay.h> #include <linux/delay.h>
#include "../include/EtherCAT_si.h"
#include "frame.h" #include "frame.h"
#include "master.h" #include "master.h"
...@@ -27,7 +28,8 @@ ...@@ -27,7 +28,8 @@
memcpy(frame->data, data, length); memcpy(frame->data, data, length);
#define EC_FUNC_READ_FOOTER \ #define EC_FUNC_READ_FOOTER \
frame->data_length = length; frame->data_length = length; \
memset(frame->data, 0x00, length);
/*****************************************************************************/ /*****************************************************************************/
...@@ -289,45 +291,30 @@ int ec_frame_send(ec_frame_t *frame /**< Rahmen zum Senden */) ...@@ -289,45 +291,30 @@ int ec_frame_send(ec_frame_t *frame /**< Rahmen zum Senden */)
data = ec_device_prepare(&frame->master->device); data = ec_device_prepare(&frame->master->device);
// EtherCAT frame header // EtherCAT frame header
data[0] = command_size & 0xFF; EC_WRITE_U16(data, (command_size & 0x7FF) | 0x1000);
data[1] = ((command_size & 0x700) >> 8) | 0x10;
data += EC_FRAME_HEADER_SIZE; data += EC_FRAME_HEADER_SIZE;
// EtherCAT command header // EtherCAT command header
data[0] = frame->type; EC_WRITE_U8 (data, frame->type);
data[1] = frame->index; EC_WRITE_U8 (data + 1, frame->index);
data[2] = frame->address.raw[0]; EC_WRITE_U32(data + 2, frame->address.logical);
data[3] = frame->address.raw[1]; EC_WRITE_U16(data + 6, frame->data_length & 0x7FF);
data[4] = frame->address.raw[2]; EC_WRITE_U16(data + 8, 0x0000);
data[5] = frame->address.raw[3];
data[6] = frame->data_length & 0xFF;
data[7] = (frame->data_length & 0x700) >> 8;
data[8] = 0x00;
data[9] = 0x00;
data += EC_COMMAND_HEADER_SIZE; data += EC_COMMAND_HEADER_SIZE;
if (likely(frame->type == ec_frame_type_apwr // Write commands // EtherCAT command data
|| frame->type == ec_frame_type_npwr memcpy(data, frame->data, frame->data_length);
|| frame->type == ec_frame_type_bwr data += frame->data_length;
|| frame->type == ec_frame_type_lrw)) {
memcpy(data, frame->data, frame->data_length);
}
else { // Read commands
memset(data, 0x00, frame->data_length);
}
// EtherCAT command footer // EtherCAT command footer
data += frame->data_length; EC_WRITE_U16(data, frame->working_counter);
data[0] = frame->working_counter & 0xFF;
data[1] = (frame->working_counter & 0xFF00) >> 8;
data += EC_COMMAND_FOOTER_SIZE; data += EC_COMMAND_FOOTER_SIZE;
// Pad with zeros // Pad with zeros
for (i = EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE for (i = EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE
+ frame->data_length + EC_COMMAND_FOOTER_SIZE; + frame->data_length + EC_COMMAND_FOOTER_SIZE;
i < EC_MIN_FRAME_SIZE; i++) { i < EC_MIN_FRAME_SIZE; i++)
*data++ = 0x00; EC_WRITE_U8(data++, 0x00);
}
// Send frame // Send frame
ec_device_send(&frame->master->device, frame_size); ec_device_send(&frame->master->device, frame_size);
...@@ -371,7 +358,8 @@ int ec_frame_receive(ec_frame_t *frame /**< Gesendeter Rahmen */) ...@@ -371,7 +358,8 @@ int ec_frame_receive(ec_frame_t *frame /**< Gesendeter Rahmen */)
data = ec_device_data(device); data = ec_device_data(device);
// Lnge des gesamten Frames prfen // Lnge des gesamten Frames prfen
frame_length = (data[0] & 0xFF) | ((data[1] & 0x07) << 8); frame_length = EC_READ_U16(data) & 0x07FF;
data += EC_FRAME_HEADER_SIZE;
if (unlikely(frame_length > received_length)) { if (unlikely(frame_length > received_length)) {
printk(KERN_ERR "EtherCAT: Received corrupted frame (length does" printk(KERN_ERR "EtherCAT: Received corrupted frame (length does"
...@@ -381,10 +369,10 @@ int ec_frame_receive(ec_frame_t *frame /**< Gesendeter Rahmen */) ...@@ -381,10 +369,10 @@ int ec_frame_receive(ec_frame_t *frame /**< Gesendeter Rahmen */)
} }
// Command header // Command header
data += EC_FRAME_HEADER_SIZE; command_type = EC_READ_U8(data);
command_type = data[0]; command_index = EC_READ_U8(data + 1);
command_index = data[1]; data_length = EC_READ_U16(data + 6) & 0x07FF;
data_length = (data[6] & 0xFF) | ((data[7] & 0x07) << 8); data += EC_COMMAND_HEADER_SIZE;
if (unlikely(EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE if (unlikely(EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE
+ data_length + EC_COMMAND_FOOTER_SIZE > received_length)) { + data_length + EC_COMMAND_FOOTER_SIZE > received_length)) {
...@@ -399,7 +387,7 @@ int ec_frame_receive(ec_frame_t *frame /**< Gesendeter Rahmen */) ...@@ -399,7 +387,7 @@ int ec_frame_receive(ec_frame_t *frame /**< Gesendeter Rahmen */)
|| frame->data_length != data_length)) || frame->data_length != data_length))
{ {
printk(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n"); printk(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n");
ec_frame_print(frame); ec_frame_print(frame); // FIXME uninteressant...
ec_device_call_isr(device); // Empfangenes "vergessen" ec_device_call_isr(device); // Empfangenes "vergessen"
return -1; return -1;
} }
...@@ -407,12 +395,11 @@ int ec_frame_receive(ec_frame_t *frame /**< Gesendeter Rahmen */) ...@@ -407,12 +395,11 @@ int ec_frame_receive(ec_frame_t *frame /**< Gesendeter Rahmen */)
frame->state = ec_frame_received; frame->state = ec_frame_received;
// Empfangene Daten in Kommandodatenspeicher kopieren // Empfangene Daten in Kommandodatenspeicher kopieren
data += EC_COMMAND_HEADER_SIZE;
memcpy(frame->data, data, data_length); memcpy(frame->data, data, data_length);
data += data_length; data += data_length;
// Working-Counter setzen // Working-Counter setzen
frame->working_counter = (data[0] & 0xFF) | ((data[1] & 0xFF) << 8); frame->working_counter = EC_READ_U16(data);
if (unlikely(frame->master->debug_level > 1)) { if (unlikely(frame->master->debug_level > 1)) {
ec_frame_print(frame); ec_frame_print(frame);
......
master/frame.h
+ 1
− 2
View file @ 4e90e0b3
...@@ -68,13 +68,12 @@ typedef union ...@@ -68,13 +68,12 @@ typedef union
{ {
struct struct
{ {
uint16_t slave; /**< Adresse des Slaves */ uint16_t slave; /**< Adresse des Slaves (Ringposition oder Knoten) */
uint16_t mem; /**< Physikalische Speicheradresse im Slave */ uint16_t mem; /**< Physikalische Speicheradresse im Slave */
} }
physical; /**< Physikalische Adresse */ physical; /**< Physikalische Adresse */
uint32_t logical; /**< Logische Adresse */ uint32_t logical; /**< Logische Adresse */
uint8_t raw[4]; /**< Rohdaten fr die Generierung des Frames */
} }
ec_address_t; ec_address_t;
......
master/master.c
+ 18
− 28
View file @ 4e90e0b3
...@@ -15,6 +15,7 @@ ...@@ -15,6 +15,7 @@
#include <linux/delay.h> #include <linux/delay.h>
#include "../include/EtherCAT_rt.h" #include "../include/EtherCAT_rt.h"
#include "../include/EtherCAT_si.h"
#include "globals.h" #include "globals.h"
#include "master.h" #include "master.h"
#include "slave.h" #include "slave.h"
...@@ -201,11 +202,10 @@ int ec_scan_for_slaves(ec_master_t *master /**< EtherCAT-Master */) ...@@ -201,11 +202,10 @@ int ec_scan_for_slaves(ec_master_t *master /**< EtherCAT-Master */)
slave = master->slaves + i; slave = master->slaves + i;
// Write station address // Write station address
data[0] = slave->station_address & 0x00FF; EC_WRITE_U16(data, slave->station_address);
data[1] = (slave->station_address & 0xFF00) >> 8;
ec_frame_init_apwr(&frame, master, slave->ring_position, 0x0010, 2, ec_frame_init_apwr(&frame, master, slave->ring_position, 0x0010,
data); sizeof(uint16_t), data);
if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
...@@ -366,14 +366,11 @@ void ec_sync_config(const ec_sync_t *sync, /**< Sync-Manager */ ...@@ -366,14 +366,11 @@ void ec_sync_config(const ec_sync_t *sync, /**< Sync-Manager */
uint8_t *data /**> Zeiger auf Konfigurationsspeicher */ uint8_t *data /**> Zeiger auf Konfigurationsspeicher */
) )
{ {
data[0] = sync->physical_start_address & 0xFF; EC_WRITE_U16(data, sync->physical_start_address);
data[1] = (sync->physical_start_address >> 8) & 0xFF; EC_WRITE_U16(data + 2, sync->size);
data[2] = sync->size & 0xFF; EC_WRITE_U8 (data + 4, sync->control_byte);
data[3] = (sync->size >> 8) & 0xFF; EC_WRITE_U8 (data + 5, 0x00); // status byte (read only)
data[4] = sync->control_byte; EC_WRITE_U16(data + 6, 0x0001); // enable
data[5] = 0x00;
data[6] = 0x01; // enable
data[7] = 0x00;
} }
/*****************************************************************************/ /*****************************************************************************/
...@@ -389,22 +386,15 @@ void ec_fmmu_config(const ec_fmmu_t *fmmu, /**< Sync-Manager */ ...@@ -389,22 +386,15 @@ void ec_fmmu_config(const ec_fmmu_t *fmmu, /**< Sync-Manager */
uint8_t *data /**> Zeiger auf Konfigurationsspeicher */ uint8_t *data /**> Zeiger auf Konfigurationsspeicher */
) )
{ {
data[0] = fmmu->logical_start_address & 0xFF; EC_WRITE_U32(data, fmmu->logical_start_address);
data[1] = (fmmu->logical_start_address >> 8) & 0xFF; EC_WRITE_U16(data + 4, fmmu->sync->size);
data[2] = (fmmu->logical_start_address >> 16) & 0xFF; EC_WRITE_U8 (data + 6, 0x00); // Logical start bit
data[3] = (fmmu->logical_start_address >> 24) & 0xFF; EC_WRITE_U8 (data + 7, 0x07); // Logical end bit
data[4] = fmmu->sync->size & 0xFF; EC_WRITE_U16(data + 8, fmmu->sync->physical_start_address);
data[5] = (fmmu->sync->size >> 8) & 0xFF; EC_WRITE_U8 (data + 10, 0x00); // Physical start bit
data[6] = 0x00; // Logical start bit EC_WRITE_U8 (data + 11, (fmmu->sync->control_byte & 0x04) ? 0x02 : 0x01);
data[7] = 0x07; // Logical end bit EC_WRITE_U16(data + 12, 0x0001); // Enable
data[8] = fmmu->sync->physical_start_address & 0xFF; EC_WRITE_U16(data + 14, 0x0000); // res.
data[9] = (fmmu->sync->physical_start_address >> 8) & 0xFF;
data[10] = 0x00; // Physical start bit
data[11] = (fmmu->sync->control_byte & 0x04) ? 0x02 : 0x01;
data[12] = 0x01; // Enable
data[13] = 0x00; // res.
data[14] = 0x00; // res.
data[15] = 0x00; // res.
} }
/****************************************************************************** /******************************************************************************
......
master/slave.c
+ 25
− 30
View file @ 4e90e0b3
...@@ -11,6 +11,7 @@ ...@@ -11,6 +11,7 @@
#include <linux/module.h> #include <linux/module.h>
#include <linux/delay.h> #include <linux/delay.h>
#include "../include/EtherCAT_si.h"
#include "globals.h" #include "globals.h"
#include "slave.h" #include "slave.h"
#include "frame.h" #include "frame.h"
...@@ -75,11 +76,11 @@ int ec_slave_fetch(ec_slave_t *slave /**< EtherCAT-Slave */) ...@@ -75,11 +76,11 @@ int ec_slave_fetch(ec_slave_t *slave /**< EtherCAT-Slave */)
return -1; return -1;
} }
slave->base_type = frame.data[0]; slave->base_type = EC_READ_U8 (frame.data);
slave->base_revision = frame.data[1]; slave->base_revision = EC_READ_U8 (frame.data + 1);
slave->base_build = frame.data[2] | (frame.data[3] << 8); slave->base_build = EC_READ_U16(frame.data + 2);
slave->base_fmmu_count = frame.data[4]; slave->base_fmmu_count = EC_READ_U8 (frame.data + 4);
slave->base_sync_count = frame.data[5]; slave->base_sync_count = EC_READ_U8 (frame.data + 5);
if (slave->base_fmmu_count > EC_MAX_FMMUS) if (slave->base_fmmu_count > EC_MAX_FMMUS)
slave->base_fmmu_count = EC_MAX_FMMUS; slave->base_fmmu_count = EC_MAX_FMMUS;
...@@ -135,12 +136,10 @@ int ec_slave_sii_read(ec_slave_t *slave, ...@@ -135,12 +136,10 @@ int ec_slave_sii_read(ec_slave_t *slave,
// Initiate read operation // Initiate read operation
data[0] = 0x00; EC_WRITE_U8 (data, 0x00);
data[1] = 0x01; EC_WRITE_U8 (data + 1, 0x01);
data[2] = offset & 0xFF; EC_WRITE_U16(data + 2, offset);
data[3] = (offset & 0xFF00) >> 8; EC_WRITE_U16(data + 4, 0x0000);
data[4] = 0x00;
data[5] = 0x00;
ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x502, 6, ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x502, 6,
data); data);
...@@ -173,7 +172,7 @@ int ec_slave_sii_read(ec_slave_t *slave, ...@@ -173,7 +172,7 @@ int ec_slave_sii_read(ec_slave_t *slave,
return -1; return -1;
} }
if (likely((frame.data[1] & 0x81) == 0)) { if (likely((EC_READ_U8(frame.data + 1) & 0x81) == 0)) {
memcpy(target, frame.data + 6, 4); memcpy(target, frame.data + 6, 4);
break; break;
} }
...@@ -208,8 +207,7 @@ void ec_slave_state_ack(ec_slave_t *slave, ...@@ -208,8 +207,7 @@ void ec_slave_state_ack(ec_slave_t *slave,
unsigned char data[2]; unsigned char data[2];
unsigned int tries_left; unsigned int tries_left;
data[0] = state | EC_ACK; EC_WRITE_U16(data, state | EC_ACK);
data[1] = 0x00;
ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x0120, ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x0120,
2, data); 2, data);
...@@ -247,14 +245,14 @@ void ec_slave_state_ack(ec_slave_t *slave, ...@@ -247,14 +245,14 @@ void ec_slave_state_ack(ec_slave_t *slave,
return; return;
} }
if (unlikely(frame.data[0] != state)) { if (unlikely(EC_READ_U8(frame.data) != state)) {
printk(KERN_ERR "EtherCAT: Could not acknowledge state %02X on" printk(KERN_ERR "EtherCAT: Could not acknowledge state %02X on"
" slave %i (code %02X)!\n", state, slave->ring_position, " slave %i (code %02X)!\n", state, slave->ring_position,
frame.data[0]); EC_READ_U8(frame.data));
return; return;
} }
if (likely(frame.data[0] == state)) { if (likely(EC_READ_U8(frame.data) == state)) {
printk(KERN_INFO "EtherCAT: Acknowleged state %02X on slave %i.\n", printk(KERN_INFO "EtherCAT: Acknowleged state %02X on slave %i.\n",
state, slave->ring_position); state, slave->ring_position);
return; return;
...@@ -289,8 +287,7 @@ int ec_slave_state_change(ec_slave_t *slave, ...@@ -289,8 +287,7 @@ int ec_slave_state_change(ec_slave_t *slave,
unsigned char data[2]; unsigned char data[2];
unsigned int tries_left; unsigned int tries_left;
data[0] = state; EC_WRITE_U16(data, state);
data[1] = 0x00;
ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x0120, ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x0120,
2, data); 2, data);
...@@ -327,15 +324,15 @@ int ec_slave_state_change(ec_slave_t *slave, ...@@ -327,15 +324,15 @@ int ec_slave_state_change(ec_slave_t *slave,
return -1; return -1;
} }
if (unlikely(frame.data[0] & 0x10)) { // State change error if (unlikely(EC_READ_U8(frame.data) & 0x10)) { // State change error
printk(KERN_ERR "EtherCAT: Could not set state %02X - Slave %i" printk(KERN_ERR "EtherCAT: Could not set state %02X - Slave %i"
" refused state change (code %02X)!\n", state, " refused state change (code %02X)!\n", state,
slave->ring_position, frame.data[0]); slave->ring_position, EC_READ_U8(frame.data));
ec_slave_state_ack(slave, frame.data[0] & 0x0F); ec_slave_state_ack(slave, EC_READ_U8(frame.data) & 0x0F);
return -1; return -1;
} }
if (likely(frame.data[0] == (state & 0x0F))) { if (likely(EC_READ_U8(frame.data) == (state & 0x0F))) {
// State change successful // State change successful
break; break;
} }
...@@ -442,7 +439,6 @@ int ec_slave_check_crc(ec_slave_t *slave /**< EtherCAT-Slave */) ...@@ -442,7 +439,6 @@ int ec_slave_check_crc(ec_slave_t *slave /**< EtherCAT-Slave */)
{ {
ec_frame_t frame; ec_frame_t frame;
uint8_t data[4]; uint8_t data[4];
uint16_t crc[2];
ec_frame_init_nprd(&frame, slave->master, slave->station_address, 0x0300, ec_frame_init_nprd(&frame, slave->master, slave->station_address, 0x0300,
4); 4);
...@@ -460,17 +456,16 @@ int ec_slave_check_crc(ec_slave_t *slave /**< EtherCAT-Slave */) ...@@ -460,17 +456,16 @@ int ec_slave_check_crc(ec_slave_t *slave /**< EtherCAT-Slave */)
return -1; return -1;
} }
crc[0] = frame.data[0] | (frame.data[1] << 8);
crc[1] = frame.data[2] | (frame.data[3] << 8);
// No CRC faults. // No CRC faults.
if (!crc[0] && !crc[1]) return 0; if (!EC_READ_U16(frame.data) && !EC_READ_U16(frame.data + 2)) return 0;
printk(KERN_INFO "EtherCAT: CRC faults on slave %i. A: %i, B: %i\n", printk(KERN_INFO "EtherCAT: CRC faults on slave %i. A: %i, B: %i\n",
slave->ring_position, crc[0], crc[1]); slave->ring_position, EC_READ_U16(frame.data),
EC_READ_U16(frame.data + 2));
// Reset CRC counters // Reset CRC counters
memset(data, 0x00, 4); EC_WRITE_U16(data, 0x0000);
EC_WRITE_U16(data + 2, 0x0000);
ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x0300, ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x0300,
4, data); 4, data);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment