94 { |
94 { |
95 unsigned int tries_left; |
95 unsigned int tries_left; |
96 |
96 |
97 if (EtherCAT_simple_send(master, cmd) < 0) return -1; |
97 if (EtherCAT_simple_send(master, cmd) < 0) return -1; |
98 |
98 |
99 tries_left = 100; |
99 EtherCAT_device_call_isr(master->dev); |
100 while (cmd->state == ECAT_CS_SENT && tries_left) |
100 |
101 { |
101 tries_left = 1000; |
102 udelay(10); |
102 while (master->dev->state == ECAT_DS_SENT && tries_left) |
|
103 { |
|
104 udelay(1); |
103 EtherCAT_device_call_isr(master->dev); |
105 EtherCAT_device_call_isr(master->dev); |
104 tries_left--; |
106 tries_left--; |
105 } |
107 } |
106 |
108 |
107 if (EtherCAT_simple_receive(master, cmd) < 0) return -1; |
109 if (EtherCAT_simple_receive(master, cmd) < 0) return -1; |
135 EC_DBG(KERN_WARNING "EtherCAT_send_receive_command: Command not in ready state!\n"); |
137 EC_DBG(KERN_WARNING "EtherCAT_send_receive_command: Command not in ready state!\n"); |
136 } |
138 } |
137 |
139 |
138 length = cmd->data_length + 12; |
140 length = cmd->data_length + 12; |
139 framelength = length + 2; |
141 framelength = length + 2; |
|
142 |
|
143 if (framelength > ECAT_FRAME_BUFFER_SIZE) |
|
144 { |
|
145 EC_DBG(KERN_ERR "EtherCAT: Frame too long (%i)!\n", framelength); |
|
146 return -1; |
|
147 } |
|
148 |
140 if (framelength < 46) framelength = 46; |
149 if (framelength < 46) framelength = 46; |
141 |
150 |
142 if (master->debug_level > 0) |
151 if (master->debug_level > 0) |
143 { |
152 { |
144 EC_DBG(KERN_DEBUG "Frame length: %i\n", framelength); |
153 EC_DBG(KERN_DEBUG "Frame length: %i\n", framelength); |
221 */ |
230 */ |
222 |
231 |
223 int EtherCAT_simple_receive(EtherCAT_master_t *master, |
232 int EtherCAT_simple_receive(EtherCAT_master_t *master, |
224 EtherCAT_command_t *cmd) |
233 EtherCAT_command_t *cmd) |
225 { |
234 { |
226 unsigned int rx_data_length, length; |
235 unsigned int length; |
|
236 int receive_ret; |
227 unsigned char command_type, command_index; |
237 unsigned char command_type, command_index; |
228 |
238 |
229 rx_data_length = EtherCAT_device_receive(master->dev, master->rx_data, |
239 if ((receive_ret = EtherCAT_device_receive(master->dev, |
230 ECAT_FRAME_BUFFER_SIZE); |
240 master->rx_data)) < 0) |
231 |
241 { |
232 if (rx_data_length < 2) |
242 return -1; |
|
243 } |
|
244 |
|
245 master->rx_data_length = (unsigned int) receive_ret; |
|
246 |
|
247 if (master->rx_data_length < 2) |
233 { |
248 { |
234 EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete EtherCAT header!\n"); |
249 EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete EtherCAT header!\n"); |
235 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
250 output_debug_data(master); |
236 output_debug_data(master->tx_data, master->tx_data_length); |
|
237 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); |
|
238 output_debug_data(master->rx_data, rx_data_length); |
|
239 return -1; |
251 return -1; |
240 } |
252 } |
241 |
253 |
242 // Länge des gesamten Frames prüfen |
254 // Länge des gesamten Frames prüfen |
243 length = ((master->rx_data[1] & 0x07) << 8) | (master->rx_data[0] & 0xFF); |
255 length = ((master->rx_data[1] & 0x07) << 8) | (master->rx_data[0] & 0xFF); |
244 |
256 |
245 if (length > rx_data_length) |
257 if (length > master->rx_data_length) |
246 { |
258 { |
247 EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (length does not match)!\n"); |
259 EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (length does not match)!\n"); |
248 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
260 output_debug_data(master); |
249 output_debug_data(master->tx_data, master->tx_data_length); |
|
250 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); |
|
251 output_debug_data(master->rx_data, rx_data_length); |
|
252 return -1; |
261 return -1; |
253 } |
262 } |
254 |
263 |
255 command_type = master->rx_data[2]; |
264 command_type = master->rx_data[2]; |
256 command_index = master->rx_data[2 + 1]; |
265 command_index = master->rx_data[2 + 1]; |
257 length = (master->rx_data[2 + 6] & 0xFF) | ((master->rx_data[2 + 7] & 0x07) << 8); |
266 length = (master->rx_data[2 + 6] & 0xFF) | ((master->rx_data[2 + 7] & 0x07) << 8); |
258 |
267 |
259 if (rx_data_length - 2 < length + 12) |
268 if (master->rx_data_length - 2 < length + 12) |
260 { |
269 { |
261 EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command data!\n"); |
270 EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command data!\n"); |
262 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
271 output_debug_data(master); |
263 output_debug_data(master->tx_data, master->tx_data_length); |
|
264 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); |
|
265 output_debug_data(master->rx_data, rx_data_length); |
|
266 return -1; |
272 return -1; |
267 } |
273 } |
268 |
274 |
269 if (cmd->state == ECAT_CS_SENT |
275 if (cmd->state == ECAT_CS_SENT |
270 && cmd->type == command_type |
276 && cmd->type == command_type |
989 @return 0 bei Erfolg, sonst < 0 |
992 @return 0 bei Erfolg, sonst < 0 |
990 */ |
993 */ |
991 |
994 |
992 int EtherCAT_read_process_data(EtherCAT_master_t *master) |
995 int EtherCAT_read_process_data(EtherCAT_master_t *master) |
993 { |
996 { |
|
997 unsigned int tries_left; |
|
998 |
994 EtherCAT_device_call_isr(master->dev); |
999 EtherCAT_device_call_isr(master->dev); |
|
1000 |
|
1001 tries_left = 1000; |
|
1002 while (master->dev->state == ECAT_DS_SENT && tries_left) |
|
1003 { |
|
1004 udelay(1); |
|
1005 EtherCAT_device_call_isr(master->dev); |
|
1006 tries_left--; |
|
1007 } |
|
1008 |
|
1009 if (!tries_left) |
|
1010 { |
|
1011 EC_DBG(KERN_ERR "EtherCAT: Timeout while receiving process data!\n"); |
|
1012 return -1; |
|
1013 } |
995 |
1014 |
996 if (EtherCAT_simple_receive(master, &master->process_data_command) < 0) |
1015 if (EtherCAT_simple_receive(master, &master->process_data_command) < 0) |
997 { |
1016 { |
998 EC_DBG(KERN_ERR "EtherCAT: Could not receive cyclic command!\n"); |
1017 EC_DBG(KERN_ERR "EtherCAT: Could not receive cyclic command!\n"); |
999 return -1; |
1018 return -1; |
1027 } |
1046 } |
1028 |
1047 |
1029 /***************************************************************/ |
1048 /***************************************************************/ |
1030 |
1049 |
1031 /** |
1050 /** |
1032 Setzt einen Byte-Wert im Speicher. |
|
1033 |
|
1034 @param data Startadresse |
|
1035 @param offset Byte-Offset |
|
1036 @param value Wert |
|
1037 */ |
|
1038 |
|
1039 void set_byte(unsigned char *data, |
|
1040 unsigned int offset, |
|
1041 unsigned char value) |
|
1042 { |
|
1043 data[offset] = value; |
|
1044 } |
|
1045 |
|
1046 /***************************************************************/ |
|
1047 |
|
1048 /** |
|
1049 Setzt einen Word-Wert im Speicher. |
|
1050 |
|
1051 @param data Startadresse |
|
1052 @param offset Byte-Offset |
|
1053 @param value Wert |
|
1054 */ |
|
1055 |
|
1056 void set_word(unsigned char *data, |
|
1057 unsigned int offset, |
|
1058 unsigned int value) |
|
1059 { |
|
1060 data[offset] = value & 0xFF; |
|
1061 data[offset + 1] = (value & 0xFF00) >> 8; |
|
1062 } |
|
1063 |
|
1064 /***************************************************************/ |
|
1065 |
|
1066 /** |
|
1067 Gibt Frame-Inhalte zwecks Debugging aus. |
1051 Gibt Frame-Inhalte zwecks Debugging aus. |
1068 |
1052 |
1069 @param data Startadresse |
1053 @param data Startadresse |
1070 @param length Länge der Daten |
1054 @param length Länge der Daten |
1071 */ |
1055 */ |
1072 |
1056 |
1073 void output_debug_data(unsigned char *data, unsigned int length) |
1057 void output_debug_data(const EtherCAT_master_t *master) |
1074 { |
1058 { |
1075 unsigned int i; |
1059 unsigned int i; |
1076 |
1060 |
|
1061 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content (%i Bytes):\n", |
|
1062 master->tx_data_length); |
|
1063 |
1077 EC_DBG(KERN_DEBUG); |
1064 EC_DBG(KERN_DEBUG); |
1078 for (i = 0; i < length; i++) |
1065 for (i = 0; i < master->tx_data_length; i++) |
1079 { |
1066 { |
1080 EC_DBG("%02X ", data[i]); |
1067 EC_DBG("%02X ", master->tx_data[i]); |
1081 if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); |
1068 if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); |
1082 } |
1069 } |
1083 EC_DBG("\n"); |
1070 EC_DBG("\n"); |
1084 } |
1071 |
1085 |
1072 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content (%i Bytes):\n", |
1086 /***************************************************************/ |
1073 master->rx_data_length); |
|
1074 |
|
1075 EC_DBG(KERN_DEBUG); |
|
1076 for (i = 0; i < master->rx_data_length; i++) |
|
1077 { |
|
1078 EC_DBG("%02X ", master->rx_data[i]); |
|
1079 if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); |
|
1080 } |
|
1081 EC_DBG("\n"); |
|
1082 } |
|
1083 |
|
1084 /***************************************************************/ |