131 /** Allocates a domain datagram and appends it to the list. |
131 /** Allocates a domain datagram and appends it to the list. |
132 * |
132 * |
133 * The datagram type and expected working counters are determined by the |
133 * The datagram type and expected working counters are determined by the |
134 * number of input and output fmmus that share the datagram. |
134 * number of input and output fmmus that share the datagram. |
135 * |
135 * |
136 * \return 0 in case of success, else < 0 |
136 * \retval 0 Success. |
|
137 * \retval <0 Error code. |
137 */ |
138 */ |
138 int ec_domain_add_datagram( |
139 int ec_domain_add_datagram( |
139 ec_domain_t *domain, /**< EtherCAT domain. */ |
140 ec_domain_t *domain, /**< EtherCAT domain. */ |
140 uint32_t logical_offset, /**< Logical offset. */ |
141 uint32_t logical_offset, /**< Logical offset. */ |
141 size_t data_size, /**< Size of the data. */ |
142 size_t data_size, /**< Size of the data. */ |
142 uint8_t *data, /**< Process data. */ |
143 uint8_t *data, /**< Process data. */ |
143 const unsigned int used[] /**< Used by inputs/outputs. */ |
144 const unsigned int used[] /**< Used by inputs/outputs. */ |
144 ) |
145 ) |
145 { |
146 { |
146 ec_datagram_t *datagram; |
147 ec_datagram_t *datagram; |
|
148 int ret; |
147 |
149 |
148 if (!(datagram = kmalloc(sizeof(ec_datagram_t), GFP_KERNEL))) { |
150 if (!(datagram = kmalloc(sizeof(ec_datagram_t), GFP_KERNEL))) { |
149 EC_ERR("Failed to allocate domain datagram!\n"); |
151 EC_ERR("Failed to allocate domain datagram!\n"); |
150 return -1; |
152 return -ENOMEM; |
151 } |
153 } |
152 |
154 |
153 ec_datagram_init(datagram); |
155 ec_datagram_init(datagram); |
154 snprintf(datagram->name, EC_DATAGRAM_NAME_SIZE, |
156 snprintf(datagram->name, EC_DATAGRAM_NAME_SIZE, |
155 "domain%u-%u", domain->index, logical_offset); |
157 "domain%u-%u", domain->index, logical_offset); |
156 |
158 |
157 if (used[EC_DIR_OUTPUT] && used[EC_DIR_INPUT]) { // inputs and outputs |
159 if (used[EC_DIR_OUTPUT] && used[EC_DIR_INPUT]) { // inputs and outputs |
158 if (ec_datagram_lrw(datagram, logical_offset, data_size, data)) { |
160 ret = ec_datagram_lrw(datagram, logical_offset, data_size, data); |
|
161 if (ret < 0) { |
159 kfree(datagram); |
162 kfree(datagram); |
160 return -1; |
163 return ret; |
161 } |
164 } |
162 // If LRW is used, output FMMUs increment the working counter by 2, |
165 // If LRW is used, output FMMUs increment the working counter by 2, |
163 // while input FMMUs increment it by 1. |
166 // while input FMMUs increment it by 1. |
164 domain->expected_working_counter += |
167 domain->expected_working_counter += |
165 used[EC_DIR_OUTPUT] * 2 + used[EC_DIR_INPUT]; |
168 used[EC_DIR_OUTPUT] * 2 + used[EC_DIR_INPUT]; |
166 } else if (used[EC_DIR_OUTPUT]) { // outputs only |
169 } else if (used[EC_DIR_OUTPUT]) { // outputs only |
167 if (ec_datagram_lwr(datagram, logical_offset, data_size, data)) { |
170 ret = ec_datagram_lwr(datagram, logical_offset, data_size, data); |
|
171 if (ret < 0) { |
168 kfree(datagram); |
172 kfree(datagram); |
169 return -1; |
173 return ret; |
170 } |
174 } |
171 domain->expected_working_counter += used[EC_DIR_OUTPUT]; |
175 domain->expected_working_counter += used[EC_DIR_OUTPUT]; |
172 } else { // inputs only (or nothing) |
176 } else { // inputs only (or nothing) |
173 if (ec_datagram_lrd(datagram, logical_offset, data_size, data)) { |
177 ret = ec_datagram_lrd(datagram, logical_offset, data_size, data); |
|
178 if (ret < 0) { |
174 kfree(datagram); |
179 kfree(datagram); |
175 return -1; |
180 return ret; |
176 } |
181 } |
177 domain->expected_working_counter += used[EC_DIR_INPUT]; |
182 domain->expected_working_counter += used[EC_DIR_INPUT]; |
178 } |
183 } |
179 |
184 |
180 ec_datagram_zero(datagram); |
185 ec_datagram_zero(datagram); |
189 * This allocates the necessary datagrams and writes the correct logical |
194 * This allocates the necessary datagrams and writes the correct logical |
190 * addresses to every configured FMMU. |
195 * addresses to every configured FMMU. |
191 * |
196 * |
192 * \todo Check for FMMUs that do not fit into any datagram. |
197 * \todo Check for FMMUs that do not fit into any datagram. |
193 * |
198 * |
194 * \retval 0 in case of success |
199 * \retval 0 Success |
195 * \retval <0 on failure. |
200 * \retval <0 Error code. |
196 */ |
201 */ |
197 int ec_domain_finish( |
202 int ec_domain_finish( |
198 ec_domain_t *domain, /**< EtherCAT domain. */ |
203 ec_domain_t *domain, /**< EtherCAT domain. */ |
199 uint32_t base_address /**< Logical base address. */ |
204 uint32_t base_address /**< Logical base address. */ |
200 ) |
205 ) |
203 size_t datagram_size; |
208 size_t datagram_size; |
204 unsigned int datagram_count; |
209 unsigned int datagram_count; |
205 unsigned int datagram_used[EC_DIR_COUNT]; |
210 unsigned int datagram_used[EC_DIR_COUNT]; |
206 ec_fmmu_config_t *fmmu; |
211 ec_fmmu_config_t *fmmu; |
207 const ec_datagram_t *datagram; |
212 const ec_datagram_t *datagram; |
|
213 int ret; |
208 |
214 |
209 domain->logical_base_address = base_address; |
215 domain->logical_base_address = base_address; |
210 |
216 |
211 if (domain->data_size && domain->data_origin == EC_ORIG_INTERNAL) { |
217 if (domain->data_size && domain->data_origin == EC_ORIG_INTERNAL) { |
212 if (!(domain->data = |
218 if (!(domain->data = |
213 (uint8_t *) kmalloc(domain->data_size, GFP_KERNEL))) { |
219 (uint8_t *) kmalloc(domain->data_size, GFP_KERNEL))) { |
214 EC_ERR("Failed to allocate %u bytes internal memory for" |
220 EC_ERR("Failed to allocate %u bytes internal memory for" |
215 " domain %u!\n", domain->data_size, domain->index); |
221 " domain %u!\n", domain->data_size, domain->index); |
216 return -1; |
222 return -ENOMEM; |
217 } |
223 } |
218 } |
224 } |
219 |
225 |
220 // Cycle through all domain FMMUS and |
226 // Cycle through all domain FMMUS and |
221 // - correct the logical base addresses |
227 // - correct the logical base addresses |
235 datagram_used[fmmu->dir]++; |
241 datagram_used[fmmu->dir]++; |
236 |
242 |
237 // If the current FMMU's data do not fit in the current datagram, |
243 // If the current FMMU's data do not fit in the current datagram, |
238 // allocate a new one. |
244 // allocate a new one. |
239 if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) { |
245 if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) { |
240 if (ec_domain_add_datagram(domain, |
246 ret = ec_domain_add_datagram(domain, |
241 domain->logical_base_address + datagram_offset, |
247 domain->logical_base_address + datagram_offset, |
242 datagram_size, domain->data + datagram_offset, |
248 datagram_size, domain->data + datagram_offset, |
243 datagram_used)) |
249 datagram_used); |
244 return -1; |
250 if (ret < 0) |
|
251 return ret; |
245 datagram_offset += datagram_size; |
252 datagram_offset += datagram_size; |
246 datagram_size = 0; |
253 datagram_size = 0; |
247 datagram_count++; |
254 datagram_count++; |
248 datagram_used[EC_DIR_OUTPUT] = 0; |
255 datagram_used[EC_DIR_OUTPUT] = 0; |
249 datagram_used[EC_DIR_INPUT] = 0; |
256 datagram_used[EC_DIR_INPUT] = 0; |
253 } |
260 } |
254 |
261 |
255 // Allocate last datagram, if data are left (this is also the case if the |
262 // Allocate last datagram, if data are left (this is also the case if the |
256 // process data fit into a single datagram) |
263 // process data fit into a single datagram) |
257 if (datagram_size) { |
264 if (datagram_size) { |
258 if (ec_domain_add_datagram(domain, |
265 ret = ec_domain_add_datagram(domain, |
259 domain->logical_base_address + datagram_offset, |
266 domain->logical_base_address + datagram_offset, |
260 datagram_size, domain->data + datagram_offset, |
267 datagram_size, domain->data + datagram_offset, |
261 datagram_used)) |
268 datagram_used); |
262 return -1; |
269 if (ret < 0) |
|
270 return ret; |
263 datagram_count++; |
271 datagram_count++; |
264 } |
272 } |
265 |
273 |
266 EC_INFO("Domain%u: Logical address 0x%08x, %u byte, " |
274 EC_INFO("Domain%u: Logical address 0x%08x, %u byte, " |
267 "expected working counter %u.\n", domain->index, |
275 "expected working counter %u.\n", domain->index, |
326 if (domain->master->debug_level) |
334 if (domain->master->debug_level) |
327 EC_DBG("ecrt_domain_reg_pdo_entry_list(domain = 0x%x, regs = 0x%x)\n", |
335 EC_DBG("ecrt_domain_reg_pdo_entry_list(domain = 0x%x, regs = 0x%x)\n", |
328 (u32) domain, (u32) regs); |
336 (u32) domain, (u32) regs); |
329 |
337 |
330 for (reg = regs; reg->index; reg++) { |
338 for (reg = regs; reg->index; reg++) { |
331 if (!(sc = ecrt_master_slave_config(domain->master, reg->alias, |
339 sc = ecrt_master_slave_config_err(domain->master, reg->alias, |
332 reg->position, reg->vendor_id, reg->product_code))) |
340 reg->position, reg->vendor_id, reg->product_code); |
333 return -1; |
341 if (IS_ERR(sc)) |
334 |
342 return PTR_ERR(sc); |
335 if ((ret = ecrt_slave_config_reg_pdo_entry(sc, reg->index, |
343 |
336 reg->subindex, domain, reg->bit_position)) < 0) |
344 ret = ecrt_slave_config_reg_pdo_entry(sc, reg->index, |
337 return -1; |
345 reg->subindex, domain, reg->bit_position); |
|
346 if (ret < 0) |
|
347 return ret; |
338 |
348 |
339 *reg->offset = ret; |
349 *reg->offset = ret; |
340 } |
350 } |
341 |
351 |
342 return 0; |
352 return 0; |