23 void ec_domain_init(ec_domain_t *domain, /**< Domäne */ |
23 void ec_domain_init(ec_domain_t *domain, /**< Domäne */ |
24 ec_master_t *master /**< Zugehöriger Master */ |
24 ec_master_t *master /**< Zugehöriger Master */ |
25 ) |
25 ) |
26 { |
26 { |
27 domain->master = master; |
27 domain->master = master; |
28 domain->data = NULL; |
|
29 domain->data_size = 0; |
28 domain->data_size = 0; |
30 domain->commands = NULL; |
|
31 domain->command_count = 0; |
|
32 domain->base_address = 0; |
29 domain->base_address = 0; |
33 domain->response_count = 0xFFFFFFFF; |
30 domain->response_count = 0xFFFFFFFF; |
34 |
31 |
35 INIT_LIST_HEAD(&domain->field_regs); |
32 INIT_LIST_HEAD(&domain->field_regs); |
|
33 INIT_LIST_HEAD(&domain->commands); |
36 } |
34 } |
37 |
35 |
38 /*****************************************************************************/ |
36 /*****************************************************************************/ |
39 |
37 |
40 /** |
38 /** |
41 Destruktor einer EtherCAT-Domäne. |
39 Destruktor einer EtherCAT-Domäne. |
42 */ |
40 */ |
43 |
41 |
44 void ec_domain_clear(ec_domain_t *domain /**< Domäne */) |
42 void ec_domain_clear(ec_domain_t *domain /**< Domäne */) |
45 { |
43 { |
46 if (domain->data) kfree(domain->data); |
44 ec_command_t *command, *next; |
47 if (domain->commands) kfree(domain->commands); |
45 |
|
46 list_for_each_entry_safe(command, next, &domain->commands, list) { |
|
47 ec_command_clear(command); |
|
48 kfree(command); |
|
49 } |
48 |
50 |
49 ec_domain_clear_field_regs(domain); |
51 ec_domain_clear_field_regs(domain); |
50 } |
52 } |
51 |
53 |
52 /*****************************************************************************/ |
54 /*****************************************************************************/ |
105 } |
107 } |
106 |
108 |
107 /*****************************************************************************/ |
109 /*****************************************************************************/ |
108 |
110 |
109 /** |
111 /** |
|
112 Alloziert ein Prozessdatenkommando und fügt es in die Liste ein. |
|
113 */ |
|
114 |
|
115 int ec_domain_add_command(ec_domain_t *domain, /**< Domäne */ |
|
116 uint32_t offset, /**< Logisches Offset */ |
|
117 size_t data_size /**< Größe der Kommando-Daten */ |
|
118 ) |
|
119 { |
|
120 ec_command_t *command; |
|
121 |
|
122 if (!(command = kmalloc(sizeof(ec_command_t), GFP_KERNEL))) { |
|
123 EC_ERR("Failed to allocate domain command!\n"); |
|
124 return -1; |
|
125 } |
|
126 |
|
127 ec_command_init(command); |
|
128 |
|
129 if (ec_command_lrw(command, offset, data_size)) { |
|
130 kfree(command); |
|
131 return -1; |
|
132 } |
|
133 |
|
134 list_add_tail(&command->list, &domain->commands); |
|
135 return 0; |
|
136 } |
|
137 |
|
138 /*****************************************************************************/ |
|
139 |
|
140 /** |
110 Erzeugt eine Domäne. |
141 Erzeugt eine Domäne. |
111 |
142 |
112 Reserviert den Speicher einer Domäne, berechnet die logischen Adressen der |
143 Reserviert den Speicher einer Domäne, berechnet die logischen Adressen der |
113 FMMUs und setzt die Prozessdatenzeiger der registrierten Felder. |
144 FMMUs und setzt die Prozessdatenzeiger der registrierten Felder. |
114 |
145 |
120 ) |
151 ) |
121 { |
152 { |
122 ec_field_reg_t *field_reg; |
153 ec_field_reg_t *field_reg; |
123 ec_slave_t *slave; |
154 ec_slave_t *slave; |
124 ec_fmmu_t *fmmu; |
155 ec_fmmu_t *fmmu; |
125 unsigned int i, j; |
156 unsigned int i, j, cmd_count; |
126 uint32_t data_offset; |
157 uint32_t field_off, field_off_cmd; |
127 |
158 uint32_t cmd_offset; |
128 if (domain->data) { |
159 size_t cmd_data_size; |
129 EC_ERR("Domain already allocated!\n"); |
160 ec_command_t *command; |
130 return -1; |
|
131 } |
|
132 |
161 |
133 domain->base_address = base_address; |
162 domain->base_address = base_address; |
134 |
163 |
135 // Größe der Prozessdaten berechnen |
164 // Größe der Prozessdaten berechnen und Kommandos allozieren |
136 // und logische Adressen der FMMUs setzen |
|
137 domain->data_size = 0; |
165 domain->data_size = 0; |
|
166 cmd_offset = base_address; |
|
167 cmd_data_size = 0; |
|
168 cmd_count = 0; |
138 for (i = 0; i < domain->master->slave_count; i++) { |
169 for (i = 0; i < domain->master->slave_count; i++) { |
139 slave = &domain->master->slaves[i]; |
170 slave = &domain->master->slaves[i]; |
140 for (j = 0; j < slave->fmmu_count; j++) { |
171 for (j = 0; j < slave->fmmu_count; j++) { |
141 fmmu = &slave->fmmus[j]; |
172 fmmu = &slave->fmmus[j]; |
142 if (fmmu->domain == domain) { |
173 if (fmmu->domain == domain) { |
143 fmmu->logical_start_address = base_address + domain->data_size; |
174 fmmu->logical_start_address = base_address + domain->data_size; |
144 domain->data_size += fmmu->sync->size; |
175 domain->data_size += fmmu->sync->size; |
|
176 if (cmd_data_size + fmmu->sync->size > EC_MAX_DATA_SIZE) { |
|
177 if (ec_domain_add_command(domain, cmd_offset, |
|
178 cmd_data_size)) return -1; |
|
179 cmd_offset += cmd_data_size; |
|
180 cmd_data_size = 0; |
|
181 cmd_count++; |
|
182 } |
|
183 cmd_data_size += fmmu->sync->size; |
145 } |
184 } |
146 } |
185 } |
147 } |
186 } |
148 |
187 |
149 if (!domain->data_size) { |
188 // Letztes Kommando allozieren |
|
189 if (cmd_data_size) { |
|
190 if (ec_domain_add_command(domain, cmd_offset, cmd_data_size)) |
|
191 return -1; |
|
192 cmd_count++; |
|
193 } |
|
194 |
|
195 if (!cmd_count) { |
150 EC_WARN("Domain 0x%08X contains no data!\n", (u32) domain); |
196 EC_WARN("Domain 0x%08X contains no data!\n", (u32) domain); |
151 ec_domain_clear_field_regs(domain); |
197 ec_domain_clear_field_regs(domain); |
152 return 0; |
198 return 0; |
153 } |
199 } |
154 |
|
155 // Prozessdaten allozieren |
|
156 if (!(domain->data = kmalloc(domain->data_size, GFP_KERNEL))) { |
|
157 EC_ERR("Failed to allocate domain data!\n"); |
|
158 return -1; |
|
159 } |
|
160 |
|
161 // Prozessdaten mit Nullen vorbelegen |
|
162 memset(domain->data, 0x00, domain->data_size); |
|
163 |
200 |
164 // Alle Prozessdatenzeiger setzen |
201 // Alle Prozessdatenzeiger setzen |
165 list_for_each_entry(field_reg, &domain->field_regs, list) { |
202 list_for_each_entry(field_reg, &domain->field_regs, list) { |
166 for (i = 0; i < field_reg->slave->fmmu_count; i++) { |
203 for (i = 0; i < field_reg->slave->fmmu_count; i++) { |
167 fmmu = &field_reg->slave->fmmus[i]; |
204 fmmu = &field_reg->slave->fmmus[i]; |
168 if (fmmu->domain == domain && fmmu->sync == field_reg->sync) { |
205 if (fmmu->domain == domain && fmmu->sync == field_reg->sync) { |
169 data_offset = fmmu->logical_start_address - base_address |
206 field_off = fmmu->logical_start_address + |
170 + field_reg->field_offset; |
207 field_reg->field_offset; |
171 *field_reg->data_ptr = domain->data + data_offset; |
208 // Kommando suchen |
|
209 list_for_each_entry(command, &domain->commands, list) { |
|
210 field_off_cmd = field_off - command->address.logical; |
|
211 if (field_off >= command->address.logical && |
|
212 field_off_cmd < command->mem_size) { |
|
213 *field_reg->data_ptr = command->data + field_off_cmd; |
|
214 } |
|
215 } |
|
216 if (!field_reg->data_ptr) { |
|
217 EC_ERR("Failed to assign data pointer!\n"); |
|
218 return -1; |
|
219 } |
172 break; |
220 break; |
173 } |
221 } |
174 } |
222 } |
175 } |
223 } |
176 |
224 |
177 // Kommando-Array erzeugen |
|
178 domain->command_count = domain->data_size / EC_MAX_DATA_SIZE + 1; |
|
179 if (!(domain->commands = (ec_command_t *) kmalloc(sizeof(ec_command_t) |
|
180 * domain->command_count, |
|
181 GFP_KERNEL))) { |
|
182 EC_ERR("Failed to allocate domain command array!\n"); |
|
183 return -1; |
|
184 } |
|
185 |
|
186 EC_INFO("Domain %X - Allocated %i bytes in %i command(s)\n", |
225 EC_INFO("Domain %X - Allocated %i bytes in %i command(s)\n", |
187 (u32) domain, domain->data_size, domain->command_count); |
226 (u32) domain, domain->data_size, cmd_count); |
188 |
227 |
189 ec_domain_clear_field_regs(domain); |
228 ec_domain_clear_field_regs(domain); |
190 |
229 |
191 return 0; |
230 return 0; |
192 } |
231 } |
344 Setzt Prozessdaten-Kommandos in die Warteschlange des Masters. |
383 Setzt Prozessdaten-Kommandos in die Warteschlange des Masters. |
345 */ |
384 */ |
346 |
385 |
347 void ecrt_domain_queue(ec_domain_t *domain /**< Domäne */) |
386 void ecrt_domain_queue(ec_domain_t *domain /**< Domäne */) |
348 { |
387 { |
349 unsigned int i; |
388 ec_command_t *command; |
350 size_t size; |
389 |
351 off_t offset; |
390 list_for_each_entry(command, &domain->commands, list) { |
352 |
391 ec_master_queue_command(domain->master, command); |
353 offset = 0; |
|
354 for (i = 0; i < domain->command_count; i++) { |
|
355 size = domain->data_size - offset; |
|
356 if (size > EC_MAX_DATA_SIZE) size = EC_MAX_DATA_SIZE; |
|
357 ec_command_init_lrw(domain->commands + i, |
|
358 domain->base_address + offset, size, |
|
359 domain->data + offset); |
|
360 ec_master_queue_command(domain->master, domain->commands + i); |
|
361 offset += size; |
|
362 } |
392 } |
363 } |
393 } |
364 |
394 |
365 /*****************************************************************************/ |
395 /*****************************************************************************/ |
366 |
396 |
368 Verarbeitet empfangene Prozessdaten. |
398 Verarbeitet empfangene Prozessdaten. |
369 */ |
399 */ |
370 |
400 |
371 void ecrt_domain_process(ec_domain_t *domain /**< Domäne */) |
401 void ecrt_domain_process(ec_domain_t *domain /**< Domäne */) |
372 { |
402 { |
373 unsigned int working_counter_sum, i; |
403 unsigned int working_counter_sum; |
374 ec_command_t *command; |
404 ec_command_t *command; |
375 size_t size; |
|
376 off_t offset; |
|
377 |
405 |
378 working_counter_sum = 0; |
406 working_counter_sum = 0; |
379 |
407 |
380 offset = 0; |
408 list_for_each_entry(command, &domain->commands, list) { |
381 for (i = 0; i < domain->command_count; i++) { |
|
382 command = domain->commands + i; |
|
383 size = domain->data_size - offset; |
|
384 if (size > EC_MAX_DATA_SIZE) size = EC_MAX_DATA_SIZE; |
|
385 if (command->state == EC_CMD_RECEIVED) { |
409 if (command->state == EC_CMD_RECEIVED) { |
386 // Daten vom Kommando- in den Prozessdatenspeicher kopieren |
|
387 memcpy(domain->data + offset, command->data, size); |
|
388 working_counter_sum += command->working_counter; |
410 working_counter_sum += command->working_counter; |
389 } |
411 } |
390 else if (unlikely(domain->master->debug_level)) { |
|
391 EC_DBG("process data command not received!\n"); |
|
392 } |
|
393 offset += size; |
|
394 } |
412 } |
395 |
413 |
396 ec_domain_response_count(domain, working_counter_sum); |
414 ec_domain_response_count(domain, working_counter_sum); |
397 } |
415 } |
398 |
416 |