44 #include "pdo.h" |
43 #include "pdo.h" |
45 #include "sync.h" |
44 #include "sync.h" |
46 |
45 |
47 /*****************************************************************************/ |
46 /*****************************************************************************/ |
48 |
47 |
49 /** |
48 /** Constructor. |
50 * Constructor. |
|
51 */ |
49 */ |
52 |
|
53 void ec_sync_init( |
50 void ec_sync_init( |
54 ec_sync_t *sync, /**< EtherCAT sync manager */ |
51 ec_sync_t *sync, /**< EtherCAT sync manager. */ |
55 ec_slave_t *slave, /**< EtherCAT slave */ |
52 ec_slave_t *slave, /**< EtherCAT slave. */ |
56 unsigned int index /**< sync manager index */ |
53 unsigned int index /**< Sync manager index. */ |
57 ) |
54 ) |
58 { |
55 { |
59 sync->slave = slave; |
56 sync->slave = slave; |
60 sync->index = index; |
57 sync->index = index; |
61 |
58 |
62 sync->est_length = 0; |
59 ec_pdo_mapping_init(&sync->mapping); |
63 INIT_LIST_HEAD(&sync->pdos); |
|
64 sync->alt_mapping = 0; |
|
65 sync->mapping_source = EC_SYNC_MAPPING_NONE; |
60 sync->mapping_source = EC_SYNC_MAPPING_NONE; |
66 } |
61 } |
67 |
62 |
68 /*****************************************************************************/ |
63 /*****************************************************************************/ |
69 |
64 |
70 /** |
65 /** Destructor. |
71 * Destructor. |
|
72 */ |
66 */ |
73 |
|
74 void ec_sync_clear( |
67 void ec_sync_clear( |
75 ec_sync_t *sync /**< EtherCAT sync manager */ |
68 ec_sync_t *sync /**< EtherCAT sync manager. */ |
76 ) |
69 ) |
77 { |
70 { |
78 ec_sync_clear_pdos(sync); |
71 ec_pdo_mapping_clear(&sync->mapping); |
79 } |
72 } |
80 |
73 |
81 /*****************************************************************************/ |
74 /*****************************************************************************/ |
82 |
75 |
83 /** |
76 /** Initializes a sync manager configuration page with EEPROM data. |
84 * Calculates the size of a sync manager by evaluating PDO sizes. |
77 * |
85 * \return sync manager size |
78 * The referenced memory (\a data) must be at least \a EC_SYNC_SIZE bytes. |
86 */ |
79 */ |
87 |
80 void ec_sync_config( |
88 uint16_t ec_sync_size( |
81 const ec_sync_t *sync, /**< Sync manager. */ |
89 const ec_sync_t *sync /**< sync manager */ |
82 uint16_t data_size, /**< Data size. */ |
|
83 uint8_t *data /**> Configuration memory. */ |
90 ) |
84 ) |
91 { |
85 { |
92 const ec_pdo_t *pdo; |
|
93 const ec_pdo_entry_t *pdo_entry; |
|
94 unsigned int bit_size, byte_size; |
|
95 |
|
96 if (sync->length) return sync->length; |
|
97 if (sync->est_length) return sync->est_length; |
|
98 |
|
99 bit_size = 0; |
|
100 list_for_each_entry(pdo, &sync->pdos, list) { |
|
101 list_for_each_entry(pdo_entry, &pdo->entries, list) { |
|
102 bit_size += pdo_entry->bit_length; |
|
103 } |
|
104 } |
|
105 |
|
106 if (bit_size % 8) // round up to full bytes |
|
107 byte_size = bit_size / 8 + 1; |
|
108 else |
|
109 byte_size = bit_size / 8; |
|
110 |
|
111 return byte_size; |
|
112 } |
|
113 |
|
114 /*****************************************************************************/ |
|
115 |
|
116 /** |
|
117 Initializes a sync manager configuration page with EEPROM data. |
|
118 The referenced memory (\a data) must be at least EC_SYNC_SIZE bytes. |
|
119 */ |
|
120 |
|
121 void ec_sync_config( |
|
122 const ec_sync_t *sync, /**< sync manager */ |
|
123 uint8_t *data /**> configuration memory */ |
|
124 ) |
|
125 { |
|
126 size_t sync_size = ec_sync_size(sync); |
|
127 |
|
128 if (sync->slave->master->debug_level) { |
86 if (sync->slave->master->debug_level) { |
129 EC_DBG("SM%i: Addr 0x%04X, Size %3i, Ctrl 0x%02X, En %i\n", |
87 EC_DBG("SM%i: Addr 0x%04X, Size %3i, Ctrl 0x%02X, En %i\n", |
130 sync->index, sync->physical_start_address, |
88 sync->index, sync->physical_start_address, |
131 sync_size, sync->control_register, sync->enable); |
89 data_size, sync->control_register, sync->enable); |
132 } |
90 } |
133 |
91 |
134 EC_WRITE_U16(data, sync->physical_start_address); |
92 EC_WRITE_U16(data, sync->physical_start_address); |
135 EC_WRITE_U16(data + 2, sync_size); |
93 EC_WRITE_U16(data + 2, data_size); |
136 EC_WRITE_U8 (data + 4, sync->control_register); |
94 EC_WRITE_U8 (data + 4, sync->control_register); |
137 EC_WRITE_U8 (data + 5, 0x00); // status byte (read only) |
95 EC_WRITE_U8 (data + 5, 0x00); // status byte (read only) |
138 EC_WRITE_U16(data + 6, sync->enable ? 0x0001 : 0x0000); // enable |
96 EC_WRITE_U16(data + 6, sync->enable ? 0x0001 : 0x0000); // enable |
139 } |
97 } |
140 |
98 |
141 /*****************************************************************************/ |
99 /*****************************************************************************/ |
142 |
100 |
143 /** |
101 /** Adds a PDO to the list of known mapped PDOs. |
144 * Adds a PDO to the list of known mapped PDOs. |
102 * |
145 * \return 0 on success, else < 0 |
103 * \return 0 on success, else < 0 |
146 */ |
104 */ |
147 |
|
148 int ec_sync_add_pdo( |
105 int ec_sync_add_pdo( |
149 ec_sync_t *sync, /**< EtherCAT sync manager */ |
106 ec_sync_t *sync, /**< EtherCAT sync manager. */ |
150 const ec_pdo_t *pdo /**< PDO to map */ |
107 const ec_pdo_t *pdo /**< PDO to map. */ |
151 ) |
108 ) |
152 { |
109 { |
153 ec_pdo_t *mapped_pdo; |
110 return ec_pdo_mapping_add_pdo(&sync->mapping, pdo); |
154 |
|
155 // PDO already mapped? |
|
156 list_for_each_entry(mapped_pdo, &sync->pdos, list) { |
|
157 if (mapped_pdo->index != pdo->index) continue; |
|
158 EC_ERR("PDO 0x%04X is already mapped!\n", pdo->index); |
|
159 return -1; |
|
160 } |
|
161 |
|
162 if (!(mapped_pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
|
163 EC_ERR("Failed to allocate memory for PDO mapping.\n"); |
|
164 return -1; |
|
165 } |
|
166 |
|
167 ec_pdo_init(mapped_pdo); |
|
168 if (ec_pdo_copy(mapped_pdo, pdo)) { |
|
169 ec_pdo_clear(mapped_pdo); |
|
170 kfree(mapped_pdo); |
|
171 return -1; |
|
172 } |
|
173 |
|
174 // set appropriate sync manager index |
|
175 mapped_pdo->sync_index = sync->index; |
|
176 |
|
177 list_add_tail(&mapped_pdo->list, &sync->pdos); |
|
178 return 0; |
|
179 } |
111 } |
180 |
112 |
181 /*****************************************************************************/ |
113 /*****************************************************************************/ |
182 |
114 |
183 /** |
115 /** Get direction covered by sync manager. |
184 * Clears the list of known mapped PDOs. |
116 * |
|
117 * \return Direction covered by the given sync manager. |
185 */ |
118 */ |
186 |
119 ec_direction_t ec_sync_direction( |
187 void ec_sync_clear_pdos( |
120 const ec_sync_t *sync /**< EtherCAT sync manager. */ |
188 ec_sync_t *sync /**< EtherCAT sync manager */ |
|
189 ) |
|
190 { |
|
191 ec_pdo_t *pdo, *next; |
|
192 |
|
193 // free all mapped PDOs |
|
194 list_for_each_entry_safe(pdo, next, &sync->pdos, list) { |
|
195 list_del(&pdo->list); |
|
196 ec_pdo_clear(pdo); |
|
197 kfree(pdo); |
|
198 } |
|
199 } |
|
200 |
|
201 /*****************************************************************************/ |
|
202 |
|
203 /** |
|
204 * \return Type of PDOs covered by the given sync manager. |
|
205 */ |
|
206 |
|
207 ec_pdo_type_t ec_sync_get_pdo_type( |
|
208 const ec_sync_t *sync /**< EtherCAT sync manager */ |
|
209 ) |
121 ) |
210 { |
122 { |
211 int index = sync->index; |
123 int index = sync->index; |
212 |
124 |
213 if (sync->slave && sync->slave->sii_mailbox_protocols) { |
125 if (sync->slave && sync->slave->sii_mailbox_protocols) { |
214 index -= 2; |
126 index -= 2; |
215 } |
127 } |
216 |
128 |
217 if (index < 0 || index > 1) { |
129 if (index < 0 || index > 1) { |
218 EC_WARN("ec_sync_get_pdo_type(): invalid sync manager index.\n"); |
130 EC_WARN("ec_sync_get_direction(): invalid sync manager index.\n"); |
219 return EC_RX_PDO; |
131 return EC_DIR_OUTPUT; |
220 } |
132 } |
221 |
133 |
222 return (ec_pdo_type_t) index; |
134 return (ec_direction_t) index; |
223 } |
135 } |
224 |
136 |
225 /*****************************************************************************/ |
137 /*****************************************************************************/ |