196 {2, EC_DIR_OUTPUT, 2, el6002_pdos + 0, EC_WD_DISABLE}, |
229 {2, EC_DIR_OUTPUT, 2, el6002_pdos + 0, EC_WD_DISABLE}, |
197 {3, EC_DIR_INPUT, 2, el6002_pdos + 2, EC_WD_DISABLE}, |
230 {3, EC_DIR_INPUT, 2, el6002_pdos + 2, EC_WD_DISABLE}, |
198 {0xff} |
231 {0xff} |
199 }; |
232 }; |
200 |
233 |
|
234 typedef enum { |
|
235 PAR_NONE, |
|
236 PAR_ODD, |
|
237 PAR_EVEN |
|
238 } parity_t; |
|
239 |
|
240 typedef struct { |
|
241 u8 value; |
|
242 unsigned int data_bits; |
|
243 parity_t parity; |
|
244 unsigned int stop_bits; |
|
245 } el600x_data_frame_t; |
|
246 |
|
247 /** EL600x supported values for data frame SDO. |
|
248 */ |
|
249 el600x_data_frame_t el600x_data_frame[] = { |
|
250 {0x01, 7, PAR_EVEN, 1}, |
|
251 {0x09, 7, PAR_EVEN, 2}, |
|
252 {0x02, 7, PAR_ODD, 1}, |
|
253 {0x0a, 7, PAR_ODD, 2}, |
|
254 {0x03, 8, PAR_NONE, 1}, |
|
255 {0x0b, 8, PAR_NONE, 2}, |
|
256 {0x04, 8, PAR_EVEN, 1}, |
|
257 {0x0c, 8, PAR_EVEN, 2}, |
|
258 {0x05, 8, PAR_ODD, 1}, |
|
259 {0x0d, 8, PAR_ODD, 2}, |
|
260 }; |
|
261 |
|
262 typedef struct { |
|
263 u8 value; |
|
264 unsigned int baud; |
|
265 tcflag_t cbaud; |
|
266 } el600x_baud_rate_t; |
|
267 |
|
268 /** EL600x supported values for baud rate SDO. |
|
269 */ |
|
270 el600x_baud_rate_t el600x_baud_rate[] = { |
|
271 {1, 300, B300}, |
|
272 {2, 600, B600}, |
|
273 {3, 1200, B1200}, |
|
274 {4, 2400, B2400}, |
|
275 {5, 4800, B4800}, |
|
276 {6, 9600, B9600}, |
|
277 {7, 19200, B19200}, |
|
278 {8, 38400, B38400}, |
|
279 {9, 57600, B57600}, |
|
280 {10, 115200, B115200} |
|
281 }; |
|
282 |
201 /****************************************************************************/ |
283 /****************************************************************************/ |
202 |
284 |
203 int el6002_init(el6002_t *ser, ec_master_t *master, u16 position, |
285 int el60xx_cflag_changed(void *data, tcflag_t cflag) |
204 ec_domain_t *domain) |
286 { |
|
287 el60xx_port_t *port = (el60xx_port_t *) data; |
|
288 unsigned int data_bits, stop_bits; |
|
289 tcflag_t cbaud, rtscts; |
|
290 parity_t par; |
|
291 unsigned int i; |
|
292 el600x_baud_rate_t *b_to_use = NULL; |
|
293 el600x_data_frame_t *df_to_use = NULL; |
|
294 |
|
295 #if DEBUG |
|
296 printk(KERN_INFO PFX "%s(%s, cflag=%x).\n", __func__, port->name, cflag); |
|
297 #endif |
|
298 |
|
299 rtscts = cflag & CRTSCTS; |
|
300 printk(KERN_INFO PFX "%s: Requested RTS/CTS: %s.\n", |
|
301 port->name, rtscts ? "yes" : "no"); |
|
302 |
|
303 cbaud = cflag & CBAUD; |
|
304 |
|
305 for (i = 0; i < sizeof(el600x_baud_rate) / sizeof(el600x_baud_rate_t); |
|
306 i++) { |
|
307 el600x_baud_rate_t *b = el600x_baud_rate + i; |
|
308 if (b->cbaud == cbaud) { |
|
309 b_to_use = b; |
|
310 break; |
|
311 } |
|
312 } |
|
313 |
|
314 if (b_to_use) { |
|
315 printk(KERN_INFO PFX "%s: Requested baud rate: %u.\n", |
|
316 port->name, b_to_use->baud); |
|
317 } else { |
|
318 printk(KERN_ERR PFX "Error: %s does not support" |
|
319 " baud rate index %x.\n", port->name, cbaud); |
|
320 return -EINVAL; |
|
321 } |
|
322 |
|
323 switch (cflag & CSIZE) { |
|
324 case CS5: |
|
325 data_bits = 5; |
|
326 break; |
|
327 case CS6: |
|
328 data_bits = 6; |
|
329 break; |
|
330 case CS7: |
|
331 data_bits = 7; |
|
332 break; |
|
333 case CS8: |
|
334 data_bits = 8; |
|
335 break; |
|
336 default: /* CS5 or CS6 */ |
|
337 data_bits = 0; |
|
338 } |
|
339 |
|
340 if (cflag & PARENB) { |
|
341 par = (cflag & PARODD) ? PAR_ODD : PAR_EVEN; |
|
342 } else { |
|
343 par = PAR_NONE; |
|
344 } |
|
345 |
|
346 stop_bits = (cflag & CSTOPB) ? 2 : 1; |
|
347 |
|
348 printk(KERN_INFO PFX "%s: Requested Data frame: %u%c%u.\n", |
|
349 port->name, data_bits, |
|
350 (par == PAR_NONE ? 'N' : (par == PAR_ODD ? 'O' : 'E')), |
|
351 stop_bits); |
|
352 |
|
353 for (i = 0; i < sizeof(el600x_data_frame) / sizeof(el600x_data_frame_t); |
|
354 i++) { |
|
355 el600x_data_frame_t *df = el600x_data_frame + i; |
|
356 if (df->data_bits == data_bits && |
|
357 df->parity == par && |
|
358 df->stop_bits == stop_bits) { |
|
359 df_to_use = df; |
|
360 break; |
|
361 } |
|
362 } |
|
363 |
|
364 if (!df_to_use) { |
|
365 printk(KERN_ERR PFX "Error: %s does not support data frame type.\n", |
|
366 port->name); |
|
367 return -EINVAL; |
|
368 } |
|
369 |
|
370 port->requested_rtscts = rtscts != 0; |
|
371 port->requested_baud_rate = b_to_use->value; |
|
372 port->requested_data_frame = df_to_use->value; |
|
373 port->config_error = 0; |
|
374 return 0; |
|
375 } |
|
376 |
|
377 /****************************************************************************/ |
|
378 |
|
379 static ec_tty_operations_t el60xx_tty_ops = { |
|
380 .cflag_changed = el60xx_cflag_changed, |
|
381 }; |
|
382 |
|
383 /****************************************************************************/ |
|
384 |
|
385 int el60xx_port_init(el60xx_port_t *port, ec_slave_config_t *sc, |
|
386 ec_domain_t *domain, unsigned int slot_offset, const char *name) |
205 { |
387 { |
206 int ret = 0; |
388 int ret = 0; |
207 |
389 |
208 ser->tty = ectty_create(); |
390 strncpy(port->name, name, EL6002_PORT_NAME_SIZE); |
209 if (IS_ERR(ser->tty)) { |
391 |
210 printk(KERN_ERR PFX "Failed to create tty.\n"); |
392 port->tty = ectty_create(&el60xx_tty_ops, port); |
211 ret = PTR_ERR(ser->tty); |
393 if (IS_ERR(port->tty)) { |
|
394 printk(KERN_ERR PFX "Failed to create tty for %s.\n", |
|
395 port->name); |
|
396 ret = PTR_ERR(port->tty); |
212 goto out_return; |
397 goto out_return; |
213 } |
398 } |
214 |
399 |
215 ser->sc = NULL; |
400 port->max_tx_data_size = 22; |
216 ser->max_tx_data_size = 22; |
401 port->max_rx_data_size = 22; |
217 ser->max_rx_data_size = 22; |
402 port->tx_data = NULL; |
218 ser->tx_data = NULL; |
403 port->tx_data_size = 0; |
219 ser->tx_data_size = 0; |
404 port->state = SER_REQUEST_INIT; |
220 ser->state = SER_REQUEST_INIT; |
405 port->tx_request_toggle = 0; |
221 ser->tx_request_toggle = 0; |
406 port->rx_accepted_toggle = 0; |
222 ser->rx_accepted_toggle = 0; |
407 port->control = 0x0000; |
223 ser->control = 0x0000; |
408 port->off_ctrl = 0; |
224 ser->off_ctrl = 0; |
409 port->off_tx = 0; |
225 ser->off_tx = 0; |
410 port->off_status = 0; |
226 ser->off_status = 0; |
411 port->off_rx = 0; |
227 ser->off_rx = 0; |
412 port->requested_rtscts = 0x00; // no hardware handshake |
228 |
413 port->current_rtscts = 0xff; |
229 if (!(ser->sc = ecrt_master_slave_config( |
414 port->requested_baud_rate = 6; // 9600 |
230 master, 0, position, Beckhoff_EL6002))) { |
415 port->current_baud_rate = 0; |
231 printk(KERN_ERR PFX "Failed to create slave configuration.\n"); |
416 port->requested_data_frame = 0x03; // 8N1 |
232 ret = -EBUSY; |
417 port->current_data_frame = 0x00; |
233 goto out_free_tty; |
418 port->config_error = 0; |
234 } |
419 |
235 |
420 if (!(port->rtscts_sdo = ecrt_slave_config_create_sdo_request(sc, |
236 if (ecrt_slave_config_pdos(ser->sc, EC_END, el6002_syncs)) { |
421 0x8000 + slot_offset, 0x01, 1))) { |
237 printk(KERN_ERR PFX "Failed to configure PDOs.\n"); |
422 printk(KERN_ERR PFX "Failed to create SDO request for %s.\n", |
|
423 port->name); |
238 ret = -ENOMEM; |
424 ret = -ENOMEM; |
239 goto out_free_tty; |
425 goto out_free_tty; |
240 } |
426 } |
241 |
427 |
|
428 if (!(port->baud_sdo = ecrt_slave_config_create_sdo_request(sc, |
|
429 0x8000 + slot_offset, 0x11, 1))) { |
|
430 printk(KERN_ERR PFX "Failed to create SDO request for %s.\n", |
|
431 port->name); |
|
432 ret = -ENOMEM; |
|
433 goto out_free_tty; |
|
434 } |
|
435 |
|
436 if (!(port->frame_sdo = ecrt_slave_config_create_sdo_request(sc, |
|
437 0x8000 + slot_offset, 0x15, 1))) { |
|
438 printk(KERN_ERR PFX "Failed to create SDO request for %s\n", |
|
439 port->name); |
|
440 ret = -ENOMEM; |
|
441 goto out_free_tty; |
|
442 } |
|
443 |
242 ret = ecrt_slave_config_reg_pdo_entry( |
444 ret = ecrt_slave_config_reg_pdo_entry( |
243 ser->sc, 0x7001, 0x01, domain, NULL); |
445 sc, 0x7001 + slot_offset, 0x01, domain, NULL); |
244 if (ret < 0) { |
446 if (ret < 0) { |
245 printk(KERN_ERR PFX "Failed to register PDO entry.\n"); |
447 printk(KERN_ERR PFX "Failed to register PDO entry of %s\n", |
|
448 port->name); |
246 goto out_free_tty; |
449 goto out_free_tty; |
247 } |
450 } |
248 ser->off_ctrl = ret; |
451 port->off_ctrl = ret; |
249 |
452 |
250 ret = ecrt_slave_config_reg_pdo_entry( |
453 ret = ecrt_slave_config_reg_pdo_entry( |
251 ser->sc, 0x7000, 0x11, domain, NULL); |
454 sc, 0x7000 + slot_offset, 0x11, domain, NULL); |
252 if (ret < 0) { |
455 if (ret < 0) { |
253 printk(KERN_ERR PFX "Failed to register PDO entry.\n"); |
456 printk(KERN_ERR PFX "Failed to register PDO entry of %s.\n", |
|
457 port->name); |
254 goto out_free_tty; |
458 goto out_free_tty; |
255 } |
459 } |
256 ser->off_tx = ret; |
460 port->off_tx = ret; |
257 |
461 |
258 ret = ecrt_slave_config_reg_pdo_entry( |
462 ret = ecrt_slave_config_reg_pdo_entry( |
259 ser->sc, 0x6001, 0x01, domain, NULL); |
463 sc, 0x6001 + slot_offset, 0x01, domain, NULL); |
260 if (ret < 0) { |
464 if (ret < 0) { |
261 printk(KERN_ERR PFX "Failed to register PDO entry.\n"); |
465 printk(KERN_ERR PFX "Failed to register PDO entry of %s.\n", |
|
466 port->name); |
262 goto out_free_tty; |
467 goto out_free_tty; |
263 } |
468 } |
264 ser->off_status = ret; |
469 port->off_status = ret; |
265 |
470 |
266 ret = ecrt_slave_config_reg_pdo_entry( |
471 ret = ecrt_slave_config_reg_pdo_entry( |
267 ser->sc, 0x6000, 0x11, domain, NULL); |
472 sc, 0x6000 + slot_offset, 0x11, domain, NULL); |
268 if (ret < 0) { |
473 if (ret < 0) { |
269 printk(KERN_ERR PFX "Failed to register PDO entry.\n"); |
474 printk(KERN_ERR PFX "Failed to register PDO entry of %s.\n", |
|
475 port->name); |
270 goto out_free_tty; |
476 goto out_free_tty; |
271 } |
477 } |
272 ser->off_rx = ret; |
478 port->off_rx = ret; |
273 |
479 |
274 if (ser->max_tx_data_size > 0) { |
480 if (port->max_tx_data_size > 0) { |
275 ser->tx_data = kmalloc(ser->max_tx_data_size, GFP_KERNEL); |
481 port->tx_data = kmalloc(port->max_tx_data_size, GFP_KERNEL); |
276 if (ser->tx_data == NULL) { |
482 if (port->tx_data == NULL) { |
|
483 printk(KERN_ERR PFX "Failed to allocate %u bytes of TX" |
|
484 " memory for %s.\n", port->max_tx_data_size, port->name); |
277 ret = -ENOMEM; |
485 ret = -ENOMEM; |
278 goto out_free_tty; |
486 goto out_free_tty; |
279 } |
487 } |
280 } |
488 } |
281 |
489 |
282 return 0; |
490 return 0; |
283 |
491 |
284 out_free_tty: |
492 out_free_tty: |
285 ectty_free(ser->tty); |
493 ectty_free(port->tty); |
286 out_return: |
494 out_return: |
287 return ret; |
495 return ret; |
288 } |
496 } |
289 |
497 |
290 /****************************************************************************/ |
498 /****************************************************************************/ |
291 |
499 |
292 void el6002_clear(el6002_t *ser) |
500 void el60xx_port_clear(el60xx_port_t *port) |
293 { |
501 { |
294 ectty_free(ser->tty); |
502 ectty_free(port->tty); |
295 if (ser->tx_data) { |
503 if (port->tx_data) { |
296 kfree(ser->tx_data); |
504 kfree(port->tx_data); |
297 } |
505 } |
298 } |
506 } |
299 |
507 |
300 /****************************************************************************/ |
508 /****************************************************************************/ |
301 |
509 |
302 void el6002_run(el6002_t *ser, u8 *pd) |
510 void el60xx_port_run(el60xx_port_t *port, u8 *pd) |
303 { |
511 { |
304 u16 status = EC_READ_U16(pd + ser->off_status); |
512 u16 status = EC_READ_U16(pd + port->off_status); |
305 u8 *rx_data = pd + ser->off_rx; |
513 u8 *rx_data = pd + port->off_rx; |
306 uint8_t tx_accepted_toggle, rx_request_toggle; |
514 uint8_t tx_accepted_toggle, rx_request_toggle; |
307 |
515 |
308 switch (ser->state) { |
516 switch (port->state) { |
309 case SER_READY: |
517 case SER_READY: |
|
518 |
|
519 /* Check, if hardware handshaking has to be configured. */ |
|
520 if (!port->config_error && |
|
521 port->requested_rtscts != port->current_rtscts) { |
|
522 EC_WRITE_U8(ecrt_sdo_request_data(port->rtscts_sdo), |
|
523 port->requested_rtscts); |
|
524 ecrt_sdo_request_write(port->rtscts_sdo); |
|
525 port->state = SER_SET_RTSCTS; |
|
526 break; |
|
527 } |
|
528 |
|
529 /* Check, if the baud rate has to be configured. */ |
|
530 if (!port->config_error && |
|
531 port->requested_baud_rate != port->current_baud_rate) { |
|
532 EC_WRITE_U8(ecrt_sdo_request_data(port->baud_sdo), |
|
533 port->requested_baud_rate); |
|
534 ecrt_sdo_request_write(port->baud_sdo); |
|
535 port->state = SER_SET_BAUD_RATE; |
|
536 break; |
|
537 } |
|
538 |
|
539 /* Check, if the data frame has to be configured. */ |
|
540 if (!port->config_error && |
|
541 port->requested_data_frame != port->current_data_frame) { |
|
542 EC_WRITE_U8(ecrt_sdo_request_data(port->frame_sdo), |
|
543 port->requested_data_frame); |
|
544 ecrt_sdo_request_write(port->frame_sdo); |
|
545 port->state = SER_SET_DATA_FRAME; |
|
546 break; |
|
547 } |
310 |
548 |
311 /* Send data */ |
549 /* Send data */ |
312 |
550 |
313 tx_accepted_toggle = status & 0x0001; |
551 tx_accepted_toggle = status & 0x0001; |
314 if (tx_accepted_toggle != ser->tx_accepted_toggle) { // ready |
552 if (tx_accepted_toggle != port->tx_accepted_toggle) { // ready |
315 ser->tx_data_size = |
553 port->tx_data_size = |
316 ectty_tx_data(ser->tty, ser->tx_data, ser->max_tx_data_size); |
554 ectty_tx_data(port->tty, port->tx_data, port->max_tx_data_size); |
317 if (ser->tx_data_size) { |
555 if (port->tx_data_size) { |
318 printk(KERN_INFO PFX "Sending %u bytes.\n", ser->tx_data_size); |
556 #if DEBUG |
319 ser->tx_request_toggle = !ser->tx_request_toggle; |
557 printk(KERN_INFO PFX "%s: Sending %u bytes.\n", |
320 ser->tx_accepted_toggle = tx_accepted_toggle; |
558 port->name, port->tx_data_size); |
|
559 #endif |
|
560 port->tx_request_toggle = !port->tx_request_toggle; |
|
561 port->tx_accepted_toggle = tx_accepted_toggle; |
321 } |
562 } |
322 } |
563 } |
323 |
564 |
324 /* Receive data */ |
565 /* Receive data */ |
325 |
566 |
326 rx_request_toggle = status & 0x0002; |
567 rx_request_toggle = status & 0x0002; |
327 if (rx_request_toggle != ser->rx_request_toggle) { |
568 if (rx_request_toggle != port->rx_request_toggle) { |
328 uint8_t rx_data_size = status >> 8; |
569 uint8_t rx_data_size = status >> 8; |
329 ser->rx_request_toggle = rx_request_toggle; |
570 port->rx_request_toggle = rx_request_toggle; |
330 printk(KERN_INFO PFX "Received %u bytes.\n", rx_data_size); |
571 #if DEBUG |
331 ectty_rx_data(ser->tty, rx_data, rx_data_size); |
572 printk(KERN_INFO PFX "%s: Received %u bytes.\n", |
332 ser->rx_accepted_toggle = !ser->rx_accepted_toggle; |
573 port->name, rx_data_size); |
333 } |
574 #endif |
334 |
575 ectty_rx_data(port->tty, rx_data, rx_data_size); |
335 ser->control = |
576 port->rx_accepted_toggle = !port->rx_accepted_toggle; |
336 ser->tx_request_toggle | |
577 } |
337 ser->rx_accepted_toggle << 1 | |
578 |
338 ser->tx_data_size << 8; |
579 port->control = |
|
580 port->tx_request_toggle | |
|
581 port->rx_accepted_toggle << 1 | |
|
582 port->tx_data_size << 8; |
339 break; |
583 break; |
340 |
584 |
341 case SER_REQUEST_INIT: |
585 case SER_REQUEST_INIT: |
342 if (status & (1 << 2)) { |
586 if (status & (1 << 2)) { |
343 ser->control = 0x0000; |
587 port->control = 0x0000; |
344 ser->state = SER_WAIT_FOR_INIT_RESPONSE; |
588 port->state = SER_WAIT_FOR_INIT_RESPONSE; |
345 } else { |
589 } else { |
346 ser->control = 1 << 2; // CW.2, request initialization |
590 port->control = 1 << 2; // CW.2, request initialization |
347 } |
591 } |
348 break; |
592 break; |
349 |
593 |
350 case SER_WAIT_FOR_INIT_RESPONSE: |
594 case SER_WAIT_FOR_INIT_RESPONSE: |
351 if (!(status & (1 << 2))) { |
595 if (!(status & (1 << 2))) { |
352 printk(KERN_INFO PFX "Init successful.\n"); |
596 printk(KERN_INFO PFX "%s: Init successful.\n", port->name); |
353 ser->tx_accepted_toggle = 1; |
597 port->tx_accepted_toggle = 1; |
354 ser->control = 0x0000; |
598 port->control = 0x0000; |
355 ser->state = SER_READY; |
599 port->state = SER_READY; |
356 } |
600 } |
357 break; |
601 break; |
358 } |
602 |
359 |
603 case SER_SET_RTSCTS: |
360 EC_WRITE_U16(pd + ser->off_ctrl, ser->control); |
604 switch (ecrt_sdo_request_state(port->rtscts_sdo)) { |
361 memcpy(pd + ser->off_tx, ser->tx_data, ser->tx_data_size); |
605 case EC_REQUEST_SUCCESS: |
|
606 printk(KERN_INFO PFX "%s: Accepted RTS/CTS.\n", |
|
607 port->name); |
|
608 port->current_rtscts = port->requested_rtscts; |
|
609 port->state = SER_REQUEST_INIT; |
|
610 break; |
|
611 case EC_REQUEST_ERROR: |
|
612 printk(KERN_ERR PFX "Failed to set RTS/CTS on %s!\n", |
|
613 port->name); |
|
614 port->state = SER_REQUEST_INIT; |
|
615 port->config_error = 1; |
|
616 break; |
|
617 default: |
|
618 break; |
|
619 } |
|
620 break; |
|
621 |
|
622 case SER_SET_BAUD_RATE: |
|
623 switch (ecrt_sdo_request_state(port->baud_sdo)) { |
|
624 case EC_REQUEST_SUCCESS: |
|
625 printk(KERN_INFO PFX "%s: Accepted baud rate.\n", |
|
626 port->name); |
|
627 port->current_baud_rate = port->requested_baud_rate; |
|
628 port->state = SER_REQUEST_INIT; |
|
629 break; |
|
630 case EC_REQUEST_ERROR: |
|
631 printk(KERN_ERR PFX "Failed to set baud rate on %s!\n", |
|
632 port->name); |
|
633 port->state = SER_REQUEST_INIT; |
|
634 port->config_error = 1; |
|
635 break; |
|
636 default: |
|
637 break; |
|
638 } |
|
639 break; |
|
640 |
|
641 case SER_SET_DATA_FRAME: |
|
642 switch (ecrt_sdo_request_state(port->frame_sdo)) { |
|
643 case EC_REQUEST_SUCCESS: |
|
644 printk(KERN_INFO PFX "%s: Accepted data frame.\n", |
|
645 port->name); |
|
646 port->current_data_frame = port->requested_data_frame; |
|
647 port->state = SER_REQUEST_INIT; |
|
648 break; |
|
649 case EC_REQUEST_ERROR: |
|
650 printk(KERN_ERR PFX "Failed to set data frame on %s!\n", |
|
651 port->name); |
|
652 port->state = SER_REQUEST_INIT; |
|
653 port->config_error = 1; |
|
654 break; |
|
655 default: |
|
656 break; |
|
657 } |
|
658 break; |
|
659 } |
|
660 |
|
661 EC_WRITE_U16(pd + port->off_ctrl, port->control); |
|
662 memcpy(pd + port->off_tx, port->tx_data, port->tx_data_size); |
|
663 } |
|
664 |
|
665 /****************************************************************************/ |
|
666 |
|
667 int el6002_init(el6002_t *el6002, ec_master_t *master, u16 position, |
|
668 ec_domain_t *domain, u32 vendor, u32 product) |
|
669 { |
|
670 int ret = 0, i; |
|
671 |
|
672 if (!(el6002->sc = ecrt_master_slave_config( |
|
673 master, 0, position, vendor, product))) { |
|
674 printk(KERN_ERR PFX "EL6002(%u): Failed to create" |
|
675 " slave configuration.\n", position); |
|
676 ret = -EBUSY; |
|
677 goto out_return; |
|
678 } |
|
679 |
|
680 if (ecrt_slave_config_pdos(el6002->sc, EC_END, el6002_syncs)) { |
|
681 printk(KERN_ERR PFX "EL6002(%u): Failed to configure PDOs.\n", |
|
682 position); |
|
683 ret = -ENOMEM; |
|
684 goto out_return; |
|
685 } |
|
686 |
|
687 for (i = 0; i < EL6002_PORTS; i++) { |
|
688 char name[EL6002_PORT_NAME_SIZE]; |
|
689 snprintf(name, EL6002_PORT_NAME_SIZE, "EL6002(%u) X%u", |
|
690 position, i + 1); |
|
691 if (el60xx_port_init(el6002->port + i, el6002->sc, domain, i * 0x10, |
|
692 name)) { |
|
693 printk(KERN_ERR PFX "EL6002(%u): Failed to init port X%u.\n", |
|
694 position, i); |
|
695 goto out_ports; |
|
696 } |
|
697 } |
|
698 |
|
699 return 0; |
|
700 |
|
701 out_ports: |
|
702 for (i--; i >= 0; i--) { |
|
703 el60xx_port_clear(el6002->port + i); |
|
704 } |
|
705 out_return: |
|
706 return ret; |
|
707 } |
|
708 |
|
709 /****************************************************************************/ |
|
710 |
|
711 void el6002_clear(el6002_t *el6002) |
|
712 { |
|
713 int i; |
|
714 |
|
715 for (i = 0; i < EL6002_PORTS; i++) { |
|
716 el60xx_port_clear(el6002->port + i); |
|
717 } |
|
718 } |
|
719 |
|
720 /****************************************************************************/ |
|
721 |
|
722 void el6002_run(el6002_t *el6002, u8 *pd) |
|
723 { |
|
724 int i; |
|
725 |
|
726 for (i = 0; i < EL6002_PORTS; i++) { |
|
727 el60xx_port_run(el6002->port + i, pd); |
|
728 } |
362 } |
729 } |
363 |
730 |
364 /*****************************************************************************/ |
731 /*****************************************************************************/ |
365 |
732 |
366 void run_serial_devices(u8 *pd) |
733 void run_serial_devices(u8 *pd) |
367 { |
734 { |
368 el6002_t *ser; |
735 el6002_t *el6002; |
369 |
736 |
370 list_for_each_entry(ser, &handlers, list) { |
737 list_for_each_entry(el6002, &handlers, list) { |
371 el6002_run(ser, pd); |
738 el6002_run(el6002, pd); |
372 } |
739 } |
|
740 } |
|
741 |
|
742 /*****************************************************************************/ |
|
743 |
|
744 int create_el6002_handler(ec_master_t *master, ec_domain_t *domain, |
|
745 u16 position, u32 vendor, u32 product) |
|
746 { |
|
747 el6002_t *el6002; |
|
748 int ret; |
|
749 |
|
750 printk(KERN_INFO PFX "Creating handler for EL6002 at position %u\n", |
|
751 position); |
|
752 |
|
753 el6002 = kmalloc(sizeof(*el6002), GFP_KERNEL); |
|
754 if (!el6002) { |
|
755 printk(KERN_ERR PFX "Failed to allocate serial device object.\n"); |
|
756 return -ENOMEM; |
|
757 } |
|
758 |
|
759 ret = el6002_init(el6002, master, position, domain, vendor, product); |
|
760 if (ret) { |
|
761 kfree(el6002); |
|
762 return ret; |
|
763 } |
|
764 |
|
765 list_add_tail(&el6002->list, &handlers); |
|
766 return 0; |
373 } |
767 } |
374 |
768 |
375 /*****************************************************************************/ |
769 /*****************************************************************************/ |
376 |
770 |
377 int create_serial_devices(ec_master_t *master, ec_domain_t *domain) |
771 int create_serial_devices(ec_master_t *master, ec_domain_t *domain) |