author | Florian Pose <fp@igh-essen.com> |
Fri, 24 Feb 2006 10:19:26 +0000 | |
changeset 77 | 677967864795 |
parent 73 | 9f4ea66d89a3 |
child 80 | 8cd08d73b560 |
permissions | -rw-r--r-- |
64 | 1 |
/****************************************************************************** |
2 |
* |
|
3 |
* c a n o p e n . c |
|
4 |
* |
|
5 |
* CANopen over EtherCAT |
|
6 |
* |
|
7 |
* $Id$ |
|
8 |
* |
|
9 |
*****************************************************************************/ |
|
10 |
||
11 |
#include <linux/delay.h> |
|
12 |
||
77
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
13 |
#include "../include/EtherCAT_si.h" |
64 | 14 |
#include "master.h" |
15 |
||
16 |
/*****************************************************************************/ |
|
17 |
||
18 |
// Prototypen |
|
19 |
||
20 |
/*****************************************************************************/ |
|
21 |
||
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
22 |
/** |
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
23 |
Schreibt ein CANopen-SDO (service data object). |
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
24 |
*/ |
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
25 |
|
77
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
26 |
int EtherCAT_rt_canopen_sdo_write(ec_slave_t *slave, /**< EtherCAT-Slave */ |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
27 |
uint16_t sdo_index, /**< SDO-Index */ |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
28 |
uint8_t sdo_subindex, /**< SDO-Subindex */ |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
29 |
uint32_t value, /**< Neuer Wert */ |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
30 |
size_t size /**< Größe des Datenfeldes */ |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
31 |
) |
64 | 32 |
{ |
33 |
unsigned char data[0xF6]; |
|
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
34 |
ec_frame_t frame; |
64 | 35 |
unsigned int tries_left, i; |
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
36 |
ec_master_t *master; |
64 | 37 |
|
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
38 |
memset(data, 0x00, 0xF6); |
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
39 |
|
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
40 |
master = slave->master; |
64 | 41 |
|
42 |
if (size == 0 || size > 4) { |
|
43 |
printk(KERN_ERR "EtherCAT: Illegal SDO data size: %i!\n", size); |
|
44 |
return -1; |
|
45 |
} |
|
46 |
||
77
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
47 |
EC_WRITE_U16(data, 0x000A); // Length of the Mailbox service data |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
48 |
EC_WRITE_U16(data + 2, slave->station_address); // Station address |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
49 |
EC_WRITE_U8 (data + 4, 0x00); // Channel & priority |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
50 |
EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
51 |
EC_WRITE_U16(data + 6, 0x2000); // Number & Service |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
52 |
EC_WRITE_U8 (data + 8, 0x13 | ((4 - size) << 2)); // Spec., exp., init. |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
53 |
EC_WRITE_U16(data + 9, sdo_index); |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
54 |
EC_WRITE_U8 (data + 11, sdo_subindex); |
64 | 55 |
|
56 |
for (i = 0; i < size; i++) { |
|
77
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
57 |
EC_WRITE_U8(data + 12 + i, value & 0xFF); |
64 | 58 |
value >>= 8; |
59 |
} |
|
60 |
||
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
61 |
ec_frame_init_npwr(&frame, master, slave->station_address, 0x1800, 0xF6, |
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
62 |
data); |
64 | 63 |
|
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
64 |
if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
64 | 65 |
|
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
66 |
if (unlikely(frame.working_counter != 1)) { |
64 | 67 |
printk(KERN_ERR "EtherCAT: Mailbox send - Slave %i did not respond!\n", |
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
68 |
slave->ring_position); |
64 | 69 |
return -1; |
70 |
} |
|
71 |
||
72 |
// Read "written bit" of Sync-Manager |
|
73 |
||
74 |
tries_left = 10; |
|
75 |
while (tries_left) |
|
76 |
{ |
|
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
77 |
ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
64 | 78 |
|
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
79 |
if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
64 | 80 |
|
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
81 |
if (unlikely(frame.working_counter != 1)) { |
64 | 82 |
printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not" |
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
83 |
" respond!\n", slave->ring_position); |
64 | 84 |
return -1; |
85 |
} |
|
86 |
||
77
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
87 |
if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high |
64 | 88 |
break; |
89 |
} |
|
90 |
||
91 |
udelay(1000); |
|
92 |
tries_left--; |
|
93 |
} |
|
94 |
||
95 |
if (!tries_left) { |
|
96 |
printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i timed out.\n", |
|
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
97 |
slave->ring_position); |
64 | 98 |
return -1; |
99 |
} |
|
100 |
||
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
101 |
ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
64 | 102 |
|
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
103 |
if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
64 | 104 |
|
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
105 |
if (unlikely(frame.working_counter != 1)) { |
64 | 106 |
printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not" |
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
107 |
" respond!\n", slave->ring_position); |
64 | 108 |
return -1; |
109 |
} |
|
110 |
||
77
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
111 |
if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
112 |
EC_READ_U16(frame.data + 6) != 0x3000 || // SDO response |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
113 |
EC_READ_U8 (frame.data + 8) >> 5 != 0x03 || // Download response |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
114 |
EC_READ_U16(frame.data + 9) != sdo_index || // Index |
677967864795
EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet.
Florian Pose <fp@igh-essen.com>
parents:
73
diff
changeset
|
115 |
EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex |
64 | 116 |
{ |
117 |
printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n", |
|
73
9f4ea66d89a3
Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst.
Florian Pose <fp@igh-essen.com>
parents:
66
diff
changeset
|
118 |
slave->ring_position); |
64 | 119 |
return -1; |
120 |
} |
|
121 |
||
122 |
return 0; |
|
123 |
} |
|
124 |
||
125 |
/*****************************************************************************/ |
|
126 |
||
127 |
EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_write); |
|
128 |
||
129 |
/*****************************************************************************/ |
|
130 |
||
131 |
/* Emacs-Konfiguration |
|
132 |
;;; Local Variables: *** |
|
133 |
;;; c-basic-offset:4 *** |
|
134 |
;;; End: *** |
|
135 |
*/ |