55 /*****************************************************************************/ |
55 /*****************************************************************************/ |
56 |
56 |
57 static char *main; /**< main devices parameter */ |
57 static char *main; /**< main devices parameter */ |
58 static char *backup; /**< backup devices parameter */ |
58 static char *backup; /**< backup devices parameter */ |
59 |
59 |
60 static LIST_HEAD(main_device_ids); /**< list of main device IDs */ |
60 static LIST_HEAD(main_ids); /**< list of main device IDs */ |
61 static LIST_HEAD(backup_device_ids); /**< list of main device IDs */ |
61 static LIST_HEAD(backup_ids); /**< list of main device IDs */ |
62 static LIST_HEAD(masters); /**< list of masters */ |
62 static LIST_HEAD(masters); /**< list of masters */ |
63 static dev_t device_number; /**< XML character device number */ |
63 static dev_t device_number; /**< XML character device number */ |
64 ec_xmldev_t xmldev; /**< XML character device */ |
64 ec_xmldev_t xmldev; /**< XML character device */ |
65 |
65 |
66 char *ec_master_version_str = EC_MASTER_VERSION; |
66 char *ec_master_version_str = EC_MASTER_VERSION; |
81 |
81 |
82 /** \endcond */ |
82 /** \endcond */ |
83 |
83 |
84 /*****************************************************************************/ |
84 /*****************************************************************************/ |
85 |
85 |
86 void clear_device_ids(struct list_head *device_ids) |
|
87 { |
|
88 ec_device_id_t *dev_id, *next_dev_id; |
|
89 |
|
90 list_for_each_entry_safe(dev_id, next_dev_id, device_ids, list) { |
|
91 list_del(&dev_id->list); |
|
92 kfree(dev_id); |
|
93 } |
|
94 } |
|
95 |
|
96 /*****************************************************************************/ |
|
97 |
|
98 static int parse_device_id_mac(ec_device_id_t *dev_id, |
|
99 const char *src, const char **remainder) |
|
100 { |
|
101 unsigned int i, value; |
|
102 char *rem; |
|
103 |
|
104 for (i = 0; i < ETH_ALEN; i++) { |
|
105 value = simple_strtoul(src, &rem, 16); |
|
106 if (rem != src + 2 |
|
107 || value > 0xFF |
|
108 || (i < ETH_ALEN - 1 && *rem != ':')) { |
|
109 return -1; |
|
110 } |
|
111 dev_id->octets[i] = value; |
|
112 if (i < ETH_ALEN - 1) |
|
113 src = rem + 1; |
|
114 } |
|
115 |
|
116 dev_id->type = ec_device_id_mac; |
|
117 *remainder = rem; |
|
118 return 0; |
|
119 } |
|
120 |
|
121 /*****************************************************************************/ |
|
122 |
|
123 static int parse_device_ids(struct list_head *device_ids, const char *src) |
|
124 { |
|
125 const char *rem; |
|
126 ec_device_id_t *dev_id; |
|
127 unsigned int index = 0; |
|
128 |
|
129 while (*src) { |
|
130 // allocate new device ID |
|
131 if (!(dev_id = kmalloc(sizeof(ec_device_id_t), GFP_KERNEL))) { |
|
132 EC_ERR("Out of memory!\n"); |
|
133 goto out_free; |
|
134 } |
|
135 |
|
136 if (*src == ';') { // empty device ID |
|
137 dev_id->type = ec_device_id_empty; |
|
138 } |
|
139 else if (*src == 'M') { |
|
140 src++; |
|
141 if (parse_device_id_mac(dev_id, src, &rem)) { |
|
142 EC_ERR("Device ID %u: Invalid MAC syntax!\n", index); |
|
143 kfree(dev_id); |
|
144 goto out_free; |
|
145 } |
|
146 src = rem; |
|
147 } |
|
148 else { |
|
149 EC_ERR("Device ID %u: Unknown format \'%c\'!\n", index, *src); |
|
150 kfree(dev_id); |
|
151 goto out_free; |
|
152 } |
|
153 |
|
154 list_add_tail(&dev_id->list, device_ids); |
|
155 if (*src) { |
|
156 if (*src != ';') { |
|
157 EC_ERR("Invalid delimiter '%c' after device ID %i!\n", |
|
158 *src, index); |
|
159 goto out_free; |
|
160 } |
|
161 src++; // skip delimiter |
|
162 } |
|
163 index++; |
|
164 } |
|
165 |
|
166 return 0; |
|
167 |
|
168 out_free: |
|
169 clear_device_ids(device_ids); |
|
170 return -1; |
|
171 } |
|
172 |
|
173 /*****************************************************************************/ |
|
174 |
|
175 static int create_device_ids(void) |
|
176 { |
|
177 ec_device_id_t *id; |
|
178 unsigned int main_count = 0, backup_count = 0; |
|
179 |
|
180 if (parse_device_ids(&main_device_ids, main)) |
|
181 return -1; |
|
182 |
|
183 if (parse_device_ids(&backup_device_ids, main)) |
|
184 return -1; |
|
185 |
|
186 // count main device IDs and check for empty ones |
|
187 list_for_each_entry(id, &main_device_ids, list) { |
|
188 if (id->type == ec_device_id_empty) { |
|
189 EC_ERR("Main device IDs may not be empty!\n"); |
|
190 return -1; |
|
191 } |
|
192 main_count++; |
|
193 } |
|
194 |
|
195 // count backup device IDs |
|
196 list_for_each_entry(id, &backup_device_ids, list) { |
|
197 backup_count++; |
|
198 } |
|
199 |
|
200 // fill up backup device IDs |
|
201 while (backup_count < main_count) { |
|
202 if (!(id = kmalloc(sizeof(ec_device_id_t), GFP_KERNEL))) { |
|
203 EC_ERR("Out of memory!\n"); |
|
204 return -1; |
|
205 } |
|
206 |
|
207 id->type = ec_device_id_empty; |
|
208 list_add_tail(&id->list, &backup_device_ids); |
|
209 backup_count++; |
|
210 } |
|
211 |
|
212 return 0; |
|
213 } |
|
214 |
|
215 /*****************************************************************************/ |
|
216 |
|
217 static int device_id_check(const ec_device_id_t *dev_id, |
|
218 const struct net_device *dev, const char *driver_name, |
|
219 unsigned int device_index) |
|
220 { |
|
221 unsigned int i; |
|
222 |
|
223 switch (dev_id->type) { |
|
224 case ec_device_id_mac: |
|
225 for (i = 0; i < ETH_ALEN; i++) |
|
226 if (dev->dev_addr[i] != dev_id->octets[i]) |
|
227 return 0; |
|
228 return 1; |
|
229 default: |
|
230 return 0; |
|
231 } |
|
232 } |
|
233 |
|
234 |
|
235 /*****************************************************************************/ |
|
236 |
|
237 /** |
86 /** |
238 Module initialization. |
87 Module initialization. |
239 Initializes \a ec_master_count masters. |
88 Initializes \a ec_master_count masters. |
240 \return 0 on success, else < 0 |
89 \return 0 on success, else < 0 |
241 */ |
90 */ |
251 if (alloc_chrdev_region(&device_number, 0, 1, "EtherCAT")) { |
100 if (alloc_chrdev_region(&device_number, 0, 1, "EtherCAT")) { |
252 EC_ERR("Failed to obtain device number!\n"); |
101 EC_ERR("Failed to obtain device number!\n"); |
253 goto out_return; |
102 goto out_return; |
254 } |
103 } |
255 |
104 |
256 if (create_device_ids()) |
105 if (ec_device_id_process_params(main, backup, &main_ids, &backup_ids)) |
257 goto out_free_ids; |
106 goto out_cdev; |
258 |
107 |
259 if (!list_empty(&main_device_ids)) { |
108 // create as many masters as main device IDs present |
|
109 if (!list_empty(&main_ids)) { |
|
110 // main_ids and backup_ids are of equal size at this point |
260 main_dev_id = |
111 main_dev_id = |
261 list_entry(main_device_ids.next, ec_device_id_t, list); |
112 list_entry(main_ids.next, ec_device_id_t, list); |
262 backup_dev_id = |
113 backup_dev_id = |
263 list_entry(backup_device_ids.next, ec_device_id_t, list); |
114 list_entry(backup_ids.next, ec_device_id_t, list); |
264 |
115 |
265 while (1) { |
116 while (1) { |
266 if (!(master = (ec_master_t *) |
117 if (!(master = (ec_master_t *) |
267 kmalloc(sizeof(ec_master_t), GFP_KERNEL))) { |
118 kmalloc(sizeof(ec_master_t), GFP_KERNEL))) { |
268 EC_ERR("Failed to allocate memory for EtherCAT master %i.\n", |
119 EC_ERR("Failed to allocate memory for EtherCAT master %i.\n", |
294 return 0; |
145 return 0; |
295 |
146 |
296 out_free_masters: |
147 out_free_masters: |
297 list_for_each_entry_safe(master, next, &masters, list) { |
148 list_for_each_entry_safe(master, next, &masters, list) { |
298 list_del(&master->list); |
149 list_del(&master->list); |
299 kobject_del(&master->kobj); |
150 ec_master_destroy(master); |
300 kobject_put(&master->kobj); |
151 } |
301 } |
152 ec_device_id_clear_list(&main_ids); |
302 out_free_ids: |
153 ec_device_id_clear_list(&backup_ids); |
303 clear_device_ids(&main_device_ids); |
154 out_cdev: |
304 clear_device_ids(&backup_device_ids); |
|
305 unregister_chrdev_region(device_number, 1); |
155 unregister_chrdev_region(device_number, 1); |
306 out_return: |
156 out_return: |
307 return -1; |
157 return -1; |
308 } |
158 } |
309 |
159 |
316 |
166 |
317 void __exit ec_cleanup_module(void) |
167 void __exit ec_cleanup_module(void) |
318 { |
168 { |
319 ec_master_t *master, *next; |
169 ec_master_t *master, *next; |
320 |
170 |
321 EC_INFO("Cleaning up master driver...\n"); |
171 EC_INFO("Cleaning up master module...\n"); |
322 |
172 |
323 list_for_each_entry_safe(master, next, &masters, list) { |
173 list_for_each_entry_safe(master, next, &masters, list) { |
324 list_del(&master->list); |
174 list_del(&master->list); |
325 ec_master_destroy(master); |
175 ec_master_destroy(master); |
326 } |
176 } |
327 |
177 |
|
178 ec_device_id_clear_list(&main_ids); |
|
179 ec_device_id_clear_list(&backup_ids); |
328 unregister_chrdev_region(device_number, 1); |
180 unregister_chrdev_region(device_number, 1); |
329 |
181 |
330 EC_INFO("Master driver cleaned up.\n"); |
182 EC_INFO("Master module cleaned up.\n"); |
331 } |
183 } |
332 |
184 |
333 /*****************************************************************************/ |
185 /*****************************************************************************/ |
334 |
186 |
335 /** |
187 /** |
472 if (down_interruptible(&master->device_sem)) { |
324 if (down_interruptible(&master->device_sem)) { |
473 EC_ERR("Interrupted while waiting for device semaphore!\n"); |
325 EC_ERR("Interrupted while waiting for device semaphore!\n"); |
474 goto out_return; |
326 goto out_return; |
475 } |
327 } |
476 |
328 |
477 if (device_id_check(master->main_device_id, net_dev, |
329 if (ec_device_id_check(master->main_device_id, net_dev, |
478 driver_name, device_index)) { |
330 driver_name, device_index)) { |
479 |
331 |
480 EC_INFO("Accepting device %s:%u (", driver_name, device_index); |
332 EC_INFO("Accepting device %s:%u (", driver_name, device_index); |
481 for (i = 0; i < ETH_ALEN; i++) { |
333 for (i = 0; i < ETH_ALEN; i++) { |
482 printk("%02X", net_dev->dev_addr[i]); |
334 printk("%02X", net_dev->dev_addr[i]); |