fp@1613: /****************************************************************************** fp@1613: * fp@1613: * $Id$ fp@1613: * fp@1613: * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH fp@1613: * fp@1613: * This file is part of the IgH EtherCAT Master. fp@1613: * fp@1613: * The IgH EtherCAT Master is free software; you can redistribute it and/or fp@1613: * modify it under the terms of the GNU General Public License version 2, as fp@1613: * published by the Free Software Foundation. fp@1613: * fp@1613: * The IgH EtherCAT Master is distributed in the hope that it will be useful, fp@1613: * but WITHOUT ANY WARRANTY; without even the implied warranty of fp@1613: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General fp@1613: * Public License for more details. fp@1613: * fp@1613: * You should have received a copy of the GNU General Public License along fp@1613: * with the IgH EtherCAT Master; if not, write to the Free Software fp@1613: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA fp@1613: * fp@1613: * --- fp@1613: * fp@1613: * The license mentioned above concerns the source code only. Using the fp@1613: * EtherCAT technology and brand is only permitted in compliance with the fp@1613: * industrial property and similar rights of Beckhoff Automation GmbH. fp@1613: * fp@1613: *****************************************************************************/ fp@1613: fp@1613: #include fp@1613: #include fp@1613: fp@1613: #include "../../include/ecrt.h" // EtherCAT realtime interface fp@1613: #include "../../include/ectty.h" // EtherCAT TTY interface fp@1613: fp@1613: /*****************************************************************************/ fp@1613: fp@1613: // Optional features fp@1613: #define PFX "ec_tty_example: " fp@1613: fp@1613: /*****************************************************************************/ fp@1613: fp@1613: #define VendorIdBeckhoff 0x00000002 fp@1613: #define ProductCodeBeckhoffEL6002 0x17723052 fp@1613: #define Beckhoff_EL6002 VendorIdBeckhoff, ProductCodeBeckhoffEL6002 fp@1613: fp@1613: typedef enum { fp@1613: SER_REQUEST_INIT, fp@1613: SER_WAIT_FOR_INIT_RESPONSE, fp@1613: SER_READY fp@1613: } serial_state_t; fp@1613: fp@1613: typedef struct { fp@1613: ec_tty_t *tty; fp@1613: ec_slave_config_t *sc; fp@1613: fp@1613: size_t max_tx_data_size; fp@1613: size_t max_rx_data_size; fp@1613: fp@1613: u8 *tx_data; fp@1613: u8 tx_data_size; fp@1613: fp@1613: serial_state_t state; fp@1613: fp@1613: u8 tx_request_toggle; fp@1613: u8 tx_accepted_toggle; fp@1613: fp@1613: u8 rx_request_toggle; fp@1613: u8 rx_accepted_toggle; fp@1613: fp@1613: u16 control; fp@1613: fp@1613: u32 off_ctrl; fp@1613: u32 off_tx; fp@1613: u32 off_status; fp@1613: u32 off_rx; fp@1613: } el6002_t; fp@1613: fp@1613: static el6002_t *ser = NULL; fp@1613: fp@1613: /*****************************************************************************/ fp@1613: fp@1613: /* Beckhoff EL6002 fp@1613: * Vendor ID: 0x00000002 fp@1613: * Product code: 0x17723052 fp@1613: * Revision number: 0x00100000 fp@1613: */ fp@1613: fp@1613: ec_pdo_entry_info_t el6002_pdo_entries[] = { fp@1613: {0x7001, 0x01, 16}, /* Ctrl */ fp@1613: {0x7000, 0x11, 8}, /* Data Out 0 */ fp@1613: {0x7000, 0x12, 8}, /* Data Out 1 */ fp@1613: {0x7000, 0x13, 8}, /* Data Out 2 */ fp@1613: {0x7000, 0x14, 8}, /* Data Out 3 */ fp@1613: {0x7000, 0x15, 8}, /* Data Out 4 */ fp@1613: {0x7000, 0x16, 8}, /* Data Out 5 */ fp@1613: {0x7000, 0x17, 8}, /* Data Out 6 */ fp@1613: {0x7000, 0x18, 8}, /* Data Out 7 */ fp@1613: {0x7000, 0x19, 8}, /* Data Out 8 */ fp@1613: {0x7000, 0x1a, 8}, /* Data Out 9 */ fp@1613: {0x7000, 0x1b, 8}, /* Data Out 10 */ fp@1613: {0x7000, 0x1c, 8}, /* Data Out 11 */ fp@1613: {0x7000, 0x1d, 8}, /* Data Out 12 */ fp@1613: {0x7000, 0x1e, 8}, /* Data Out 13 */ fp@1613: {0x7000, 0x1f, 8}, /* Data Out 14 */ fp@1613: {0x7000, 0x20, 8}, /* Data Out 15 */ fp@1613: {0x7000, 0x21, 8}, /* Data Out 16 */ fp@1613: {0x7000, 0x22, 8}, /* Data Out 17 */ fp@1613: {0x7000, 0x23, 8}, /* Data Out 18 */ fp@1613: {0x7000, 0x24, 8}, /* Data Out 19 */ fp@1613: {0x7000, 0x25, 8}, /* Data Out 20 */ fp@1613: {0x7000, 0x26, 8}, /* Data Out 21 */ fp@1613: {0x7011, 0x01, 16}, /* Ctrl */ fp@1613: {0x7010, 0x11, 8}, /* Data Out 0 */ fp@1613: {0x7010, 0x12, 8}, /* Data Out 1 */ fp@1613: {0x7010, 0x13, 8}, /* Data Out 2 */ fp@1613: {0x7010, 0x14, 8}, /* Data Out 3 */ fp@1613: {0x7010, 0x15, 8}, /* Data Out 4 */ fp@1613: {0x7010, 0x16, 8}, /* Data Out 5 */ fp@1613: {0x7010, 0x17, 8}, /* Data Out 6 */ fp@1613: {0x7010, 0x18, 8}, /* Data Out 7 */ fp@1613: {0x7010, 0x19, 8}, /* Data Out 8 */ fp@1613: {0x7010, 0x1a, 8}, /* Data Out 9 */ fp@1613: {0x7010, 0x1b, 8}, /* Data Out 10 */ fp@1613: {0x7010, 0x1c, 8}, /* Data Out 11 */ fp@1613: {0x7010, 0x1d, 8}, /* Data Out 12 */ fp@1613: {0x7010, 0x1e, 8}, /* Data Out 13 */ fp@1613: {0x7010, 0x1f, 8}, /* Data Out 14 */ fp@1613: {0x7010, 0x20, 8}, /* Data Out 15 */ fp@1613: {0x7010, 0x21, 8}, /* Data Out 16 */ fp@1613: {0x7010, 0x22, 8}, /* Data Out 17 */ fp@1613: {0x7010, 0x23, 8}, /* Data Out 18 */ fp@1613: {0x7010, 0x24, 8}, /* Data Out 19 */ fp@1613: {0x7010, 0x25, 8}, /* Data Out 20 */ fp@1613: {0x7010, 0x26, 8}, /* Data Out 21 */ fp@1613: {0x6001, 0x01, 16}, /* Status */ fp@1613: {0x6000, 0x11, 8}, /* Data In 0 */ fp@1613: {0x6000, 0x12, 8}, /* Data In 1 */ fp@1613: {0x6000, 0x13, 8}, /* Data In 2 */ fp@1613: {0x6000, 0x14, 8}, /* Data In 3 */ fp@1613: {0x6000, 0x15, 8}, /* Data In 4 */ fp@1613: {0x6000, 0x16, 8}, /* Data In 5 */ fp@1613: {0x6000, 0x17, 8}, /* Data In 6 */ fp@1613: {0x6000, 0x18, 8}, /* Data In 7 */ fp@1613: {0x6000, 0x19, 8}, /* Data In 8 */ fp@1613: {0x6000, 0x1a, 8}, /* Data In 9 */ fp@1613: {0x6000, 0x1b, 8}, /* Data In 10 */ fp@1613: {0x6000, 0x1c, 8}, /* Data In 11 */ fp@1613: {0x6000, 0x1d, 8}, /* Data In 12 */ fp@1613: {0x6000, 0x1e, 8}, /* Data In 13 */ fp@1613: {0x6000, 0x1f, 8}, /* Data In 14 */ fp@1613: {0x6000, 0x20, 8}, /* Data In 15 */ fp@1613: {0x6000, 0x21, 8}, /* Data In 16 */ fp@1613: {0x6000, 0x22, 8}, /* Data In 17 */ fp@1613: {0x6000, 0x23, 8}, /* Data In 18 */ fp@1613: {0x6000, 0x24, 8}, /* Data In 19 */ fp@1613: {0x6000, 0x25, 8}, /* Data In 20 */ fp@1613: {0x6000, 0x26, 8}, /* Data In 21 */ fp@1613: {0x6011, 0x01, 16}, /* Status */ fp@1613: {0x6010, 0x11, 8}, /* Data In 0 */ fp@1613: {0x6010, 0x12, 8}, /* Data In 1 */ fp@1613: {0x6010, 0x13, 8}, /* Data In 2 */ fp@1613: {0x6010, 0x14, 8}, /* Data In 3 */ fp@1613: {0x6010, 0x15, 8}, /* Data In 4 */ fp@1613: {0x6010, 0x16, 8}, /* Data In 5 */ fp@1613: {0x6010, 0x17, 8}, /* Data In 6 */ fp@1613: {0x6010, 0x18, 8}, /* Data In 7 */ fp@1613: {0x6010, 0x19, 8}, /* Data In 8 */ fp@1613: {0x6010, 0x1a, 8}, /* Data In 9 */ fp@1613: {0x6010, 0x1b, 8}, /* Data In 10 */ fp@1613: {0x6010, 0x1c, 8}, /* Data In 11 */ fp@1613: {0x6010, 0x1d, 8}, /* Data In 12 */ fp@1613: {0x6010, 0x1e, 8}, /* Data In 13 */ fp@1613: {0x6010, 0x1f, 8}, /* Data In 14 */ fp@1613: {0x6010, 0x20, 8}, /* Data In 15 */ fp@1613: {0x6010, 0x21, 8}, /* Data In 16 */ fp@1613: {0x6010, 0x22, 8}, /* Data In 17 */ fp@1613: {0x6010, 0x23, 8}, /* Data In 18 */ fp@1613: {0x6010, 0x24, 8}, /* Data In 19 */ fp@1613: {0x6010, 0x25, 8}, /* Data In 20 */ fp@1613: {0x6010, 0x26, 8}, /* Data In 21 */ fp@1613: }; fp@1613: fp@1613: ec_pdo_info_t el6002_pdos[] = { fp@1613: {0x1604, 23, el6002_pdo_entries + 0}, /* COM RxPDO-Map Outputs Ch.1 */ fp@1613: {0x1605, 23, el6002_pdo_entries + 23}, /* COM RxPDO-Map Outputs Ch.2 */ fp@1613: {0x1a04, 23, el6002_pdo_entries + 46}, /* COM TxPDO-Map Inputs Ch.1 */ fp@1613: {0x1a05, 23, el6002_pdo_entries + 69}, /* COM TxPDO-Map Inputs Ch.2 */ fp@1613: }; fp@1613: fp@1613: ec_sync_info_t el6002_syncs[] = { fp@1613: {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, fp@1613: {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, fp@1613: {2, EC_DIR_OUTPUT, 2, el6002_pdos + 0, EC_WD_DISABLE}, fp@1613: {3, EC_DIR_INPUT, 2, el6002_pdos + 2, EC_WD_DISABLE}, fp@1613: {0xff} fp@1613: }; fp@1613: fp@1613: /****************************************************************************/ fp@1613: fp@1613: int el6002_init(el6002_t *ser, ec_master_t *master, u16 position, fp@1613: ec_domain_t *domain) fp@1613: { fp@1613: int ret = 0; fp@1613: fp@1613: ser->tty = ectty_create(); fp@1613: if (IS_ERR(ser->tty)) { fp@1613: printk(KERN_ERR PFX "Failed to create tty.\n"); fp@1613: ret = PTR_ERR(ser->tty); fp@1613: goto out_return; fp@1613: } fp@1613: fp@1613: ser->sc = NULL; fp@1613: ser->max_tx_data_size = 22; fp@1613: ser->max_rx_data_size = 22; fp@1613: ser->tx_data = NULL; fp@1613: ser->tx_data_size = 0; fp@1613: ser->state = SER_REQUEST_INIT; fp@1613: ser->tx_request_toggle = 0; fp@1613: ser->rx_accepted_toggle = 0; fp@1613: ser->control = 0x0000; fp@1613: ser->off_ctrl = 0; fp@1613: ser->off_tx = 0; fp@1613: ser->off_status = 0; fp@1613: ser->off_rx = 0; fp@1613: fp@1613: if (!(ser->sc = ecrt_master_slave_config( fp@1613: master, 0, position, Beckhoff_EL6002))) { fp@1613: printk(KERN_ERR PFX "Failed to create slave configuration.\n"); fp@1613: ret = -EBUSY; fp@1613: goto out_free_tty; fp@1613: } fp@1613: fp@1613: if (ecrt_slave_config_pdos(ser->sc, EC_END, el6002_syncs)) { fp@1613: printk(KERN_ERR PFX "Failed to configure PDOs.\n"); fp@1613: ret = -ENOMEM; fp@1613: goto out_free_tty; fp@1613: } fp@1613: fp@1613: ret = ecrt_slave_config_reg_pdo_entry( fp@1613: ser->sc, 0x7001, 0x01, domain, NULL); fp@1613: if (ret < 0) { fp@1613: printk(KERN_ERR PFX "Failed to register PDO entry.\n"); fp@1613: goto out_free_tty; fp@1613: } fp@1613: ser->off_ctrl = ret; fp@1613: fp@1613: ret = ecrt_slave_config_reg_pdo_entry( fp@1613: ser->sc, 0x7000, 0x11, domain, NULL); fp@1613: if (ret < 0) { fp@1613: printk(KERN_ERR PFX "Failed to register PDO entry.\n"); fp@1613: goto out_free_tty; fp@1613: } fp@1613: ser->off_tx = ret; fp@1613: fp@1613: ret = ecrt_slave_config_reg_pdo_entry( fp@1613: ser->sc, 0x6001, 0x01, domain, NULL); fp@1613: if (ret < 0) { fp@1613: printk(KERN_ERR PFX "Failed to register PDO entry.\n"); fp@1613: goto out_free_tty; fp@1613: } fp@1613: ser->off_status = ret; fp@1613: fp@1613: ret = ecrt_slave_config_reg_pdo_entry( fp@1613: ser->sc, 0x6000, 0x11, domain, NULL); fp@1613: if (ret < 0) { fp@1613: printk(KERN_ERR PFX "Failed to register PDO entry.\n"); fp@1613: goto out_free_tty; fp@1613: } fp@1613: ser->off_rx = ret; fp@1613: fp@1613: if (ser->max_tx_data_size > 0) { fp@1613: ser->tx_data = kmalloc(ser->max_tx_data_size, GFP_KERNEL); fp@1613: if (ser->tx_data == NULL) { fp@1613: ret = -ENOMEM; fp@1613: goto out_free_tty; fp@1613: } fp@1613: } fp@1613: fp@1613: return 0; fp@1613: fp@1613: out_free_tty: fp@1613: ectty_free(ser->tty); fp@1613: out_return: fp@1613: return ret; fp@1613: } fp@1613: fp@1613: /****************************************************************************/ fp@1613: fp@1613: void el6002_clear(el6002_t *ser) fp@1613: { fp@1613: ectty_free(ser->tty); fp@1613: if (ser->tx_data) { fp@1613: kfree(ser->tx_data); fp@1613: } fp@1613: } fp@1613: fp@1613: /****************************************************************************/ fp@1613: fp@1613: void el6002_run(el6002_t *ser, u8 *pd) fp@1613: { fp@1613: u16 status = EC_READ_U16(pd + ser->off_status); fp@1613: u8 *rx_data = pd + ser->off_rx; fp@1613: uint8_t tx_accepted_toggle, rx_request_toggle; fp@1613: fp@1613: switch (ser->state) { fp@1613: case SER_READY: fp@1613: fp@1613: /* Send data */ fp@1613: fp@1613: tx_accepted_toggle = status & 0x0001; fp@1613: if (tx_accepted_toggle != ser->tx_accepted_toggle) { // ready fp@1613: ser->tx_data_size = fp@1613: ectty_tx_data(ser->tty, ser->tx_data, ser->max_tx_data_size); fp@1613: if (ser->tx_data_size) { fp@1613: printk(KERN_INFO PFX "Sending %u bytes.\n", ser->tx_data_size); fp@1613: ser->tx_request_toggle = !ser->tx_request_toggle; fp@1613: ser->tx_accepted_toggle = tx_accepted_toggle; fp@1613: } fp@1613: } fp@1613: fp@1613: /* Receive data */ fp@1613: fp@1613: rx_request_toggle = status & 0x0002; fp@1613: if (rx_request_toggle != ser->rx_request_toggle) { fp@1613: uint8_t rx_data_size = status >> 8; fp@1613: ser->rx_request_toggle = rx_request_toggle; fp@1613: printk(KERN_INFO PFX "Received %u bytes.\n", rx_data_size); fp@1613: ectty_rx_data(ser->tty, rx_data, rx_data_size); fp@1613: ser->rx_accepted_toggle = !ser->rx_accepted_toggle; fp@1613: } fp@1613: fp@1613: ser->control = fp@1613: ser->tx_request_toggle | fp@1613: ser->rx_accepted_toggle << 1 | fp@1613: ser->tx_data_size << 8; fp@1613: break; fp@1613: fp@1613: case SER_REQUEST_INIT: fp@1613: if (status & (1 << 2)) { fp@1613: ser->control = 0x0000; fp@1613: ser->state = SER_WAIT_FOR_INIT_RESPONSE; fp@1613: } else { fp@1613: ser->control = 1 << 2; // CW.2, request initialization fp@1613: } fp@1613: break; fp@1613: fp@1613: case SER_WAIT_FOR_INIT_RESPONSE: fp@1613: if (!(status & (1 << 2))) { fp@1613: printk(KERN_INFO PFX "Init successful.\n"); fp@1613: ser->tx_accepted_toggle = 1; fp@1613: ser->control = 0x0000; fp@1613: ser->state = SER_READY; fp@1613: } fp@1613: break; fp@1613: } fp@1613: fp@1613: EC_WRITE_U16(pd + ser->off_ctrl, ser->control); fp@1613: memcpy(pd + ser->off_tx, ser->tx_data, ser->tx_data_size); fp@1613: } fp@1613: fp@1613: /*****************************************************************************/ fp@1613: fp@1613: void run_serial_devices(u8 *pd) fp@1613: { fp@1613: if (ser) { fp@1613: el6002_run(ser, pd); fp@1613: } fp@1613: } fp@1613: fp@1613: /*****************************************************************************/ fp@1613: fp@1613: int create_serial_devices(ec_master_t *master, ec_domain_t *domain) fp@1613: { fp@1613: int i, ret; fp@1613: ec_master_info_t master_info; fp@1613: ec_slave_info_t slave_info; fp@1613: fp@1613: printk(KERN_INFO PFX "Registering serial devices...\n"); fp@1613: fp@1613: ret = ecrt_master(master, &master_info); fp@1613: if (ret) { fp@1613: printk(KERN_ERR PFX "Failed to obtain master information.\n"); fp@1613: return ret; fp@1613: } fp@1613: fp@1613: for (i = 0; i < master_info.slave_count; i++) { fp@1613: ret = ecrt_master_get_slave(master, i, &slave_info); fp@1613: if (ret) { fp@1613: printk(KERN_ERR PFX "Failed to obtain master information.\n"); fp@1613: return ret; fp@1613: } fp@1613: fp@1613: if (slave_info.vendor_id != VendorIdBeckhoff fp@1613: || slave_info.product_code != ProductCodeBeckhoffEL6002) { fp@1613: continue; fp@1613: } fp@1613: fp@1613: printk(KERN_INFO PFX "Creating handler for serial device" fp@1613: " at position %i\n", i); fp@1613: fp@1613: ser = kmalloc(sizeof(*ser), GFP_KERNEL); fp@1613: if (!ser) { fp@1613: printk(KERN_ERR PFX "Failed to allocate serial device object.\n"); fp@1613: return -ENOMEM; fp@1613: } fp@1613: fp@1613: ret = el6002_init(ser, master, i, domain); fp@1613: if (ret) { fp@1613: printk(KERN_ERR PFX "Failed to init serial device object.\n"); fp@1613: kfree(ser); fp@1613: ser = NULL; fp@1613: return ret; fp@1613: } fp@1613: fp@1613: break; // FIXME fp@1613: } fp@1613: fp@1613: fp@1613: printk(KERN_INFO PFX "Finished.\n"); fp@1613: return 0; fp@1613: } fp@1613: fp@1613: /*****************************************************************************/ fp@1613: fp@1613: void cleanup_serial_devices(void) fp@1613: { fp@1613: printk(KERN_INFO PFX "Cleaning up serial devices...\n"); fp@1613: fp@1613: if (ser) { fp@1613: el6002_clear(ser); fp@1613: kfree(ser); fp@1613: } fp@1613: fp@1613: printk(KERN_INFO PFX "Finished cleaning up serial devices.\n"); fp@1613: } fp@1613: fp@1613: /*****************************************************************************/