diff --git a/master/device.c b/master/device.c index 3133b0c5..c7b8d0c8 100644 --- a/master/device.c +++ b/master/device.c @@ -54,24 +54,10 @@ */ int ec_device_init(ec_device_t *device, /**< EtherCAT device */ - ec_master_t *master, /**< master owning the device */ - struct net_device *net_dev, /**< net_device structure */ - ec_pollfunc_t poll, /**< pointer to device's poll function */ - struct module *module /**< the device's module */ - ) + ec_master_t *master /**< master owning the device */ + ) { - struct ethhdr *eth; - device->master = master; - device->dev = net_dev; - device->poll = poll; - device->module = module; - - device->open = 0; - device->link_state = 0; // down - - device->tx_count = 0; - device->rx_count = 0; #ifdef EC_DBG_IF if (ec_debug_init(&device->dbg)) { @@ -89,15 +75,13 @@ int ec_device_init(ec_device_t *device, /**< EtherCAT device */ #endif } - device->tx_skb->dev = net_dev; - // add Ethernet-II-header skb_reserve(device->tx_skb, ETH_HLEN); - eth = (struct ethhdr *) skb_push(device->tx_skb, ETH_HLEN); - eth->h_proto = htons(0x88A4); - memcpy(eth->h_source, net_dev->dev_addr, net_dev->addr_len); - memset(eth->h_dest, 0xFF, net_dev->addr_len); + device->eth = (struct ethhdr *) skb_push(device->tx_skb, ETH_HLEN); + device->eth->h_proto = htons(0x88A4); + memset(device->eth->h_dest, 0xFF, ETH_ALEN); + ec_device_detach(device); // resets remaining fields return 0; #ifdef EC_DBG_IF @@ -117,7 +101,7 @@ int ec_device_init(ec_device_t *device, /**< EtherCAT device */ void ec_device_clear(ec_device_t *device /**< EtherCAT device */) { if (device->open) ec_device_close(device); - if (device->tx_skb) dev_kfree_skb(device->tx_skb); + dev_kfree_skb(device->tx_skb); #ifdef EC_DBG_IF ec_debug_clear(&device->dbg); #endif @@ -125,6 +109,46 @@ void ec_device_clear(ec_device_t *device /**< EtherCAT device */) /*****************************************************************************/ +/** + Associate with net_device. +*/ + +void ec_device_attach(ec_device_t *device, /**< EtherCAT device */ + struct net_device *net_dev, /**< net_device structure */ + ec_pollfunc_t poll, /**< pointer to device's poll function */ + struct module *module /**< the device's module */ + ) +{ + ec_device_detach(device); // resets fields + + device->dev = net_dev; + device->poll = poll; + device->module = module; + + device->tx_skb->dev = net_dev; + memcpy(device->eth->h_source, net_dev->dev_addr, ETH_ALEN); +} + +/*****************************************************************************/ + +/** + Disconnect from net_device. +*/ + +void ec_device_detach(ec_device_t *device /**< EtherCAT device */) +{ + device->dev = NULL; + device->poll = NULL; + device->module = NULL; + device->open = 0; + device->link_state = 0; // down + device->tx_count = 0; + device->rx_count = 0; + device->tx_skb->dev = NULL; +} + +/*****************************************************************************/ + /** Opens the EtherCAT device. \return 0 in case of success, else < 0 diff --git a/master/device.h b/master/device.h index 452e98b3..08a5b529 100644 --- a/master/device.h +++ b/master/device.h @@ -63,13 +63,14 @@ struct ec_device { ec_master_t *master; /**< EtherCAT master */ struct net_device *dev; /**< pointer to the assigned net_device */ - uint8_t open; /**< true, if the net_device has been opened */ - struct sk_buff *tx_skb; /**< transmit socket buffer */ ec_pollfunc_t poll; /**< pointer to the device's poll function */ + struct module *module; /**< pointer to the device's owning module */ + uint8_t open; /**< true, if the net_device has been opened */ + uint8_t link_state; /**< device link state */ + struct sk_buff *tx_skb; /**< transmit socket buffer */ + struct ethhdr *eth; /**< pointer to ethernet header in socket buffer */ cycles_t cycles_poll; /**< cycles of last poll */ unsigned long jiffies_poll; /**< jiffies of last poll */ - struct module *module; /**< pointer to the device's owning module */ - uint8_t link_state; /**< device link state */ unsigned int tx_count; /**< number of frames sent */ unsigned int rx_count; /**< number of frames received */ #ifdef EC_DBG_IF @@ -79,10 +80,13 @@ struct ec_device /*****************************************************************************/ -int ec_device_init(ec_device_t *, ec_master_t *, struct net_device *, - ec_pollfunc_t, struct module *); +int ec_device_init(ec_device_t *, ec_master_t *); void ec_device_clear(ec_device_t *); +void ec_device_attach(ec_device_t *, struct net_device *, ec_pollfunc_t, + struct module *); +void ec_device_detach(ec_device_t *); + int ec_device_open(ec_device_t *); int ec_device_close(ec_device_t *); diff --git a/master/ethernet.c b/master/ethernet.c index 7c1a65c8..53edc505 100644 --- a/master/ethernet.c +++ b/master/ethernet.c @@ -317,7 +317,7 @@ int ec_eoe_active(const ec_eoe_t *eoe /**< EoE handler */) void ec_eoe_state_rx_start(ec_eoe_t *eoe /**< EoE handler */) { - if (!eoe->slave->online || !eoe->slave->master->device->link_state) + if (!eoe->slave->online || !eoe->slave->master->main_device.link_state) return; ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram); @@ -518,7 +518,7 @@ void ec_eoe_state_tx_start(ec_eoe_t *eoe /**< EoE handler */) unsigned int wakeup = 0; #endif - if (!eoe->slave->online || !eoe->slave->master->device->link_state) + if (!eoe->slave->online || !eoe->slave->master->main_device.link_state) return; spin_lock_bh(&eoe->tx_queue_lock); diff --git a/master/master.c b/master/master.c index adc48309..f27d5e14 100644 --- a/master/master.c +++ b/master/master.c @@ -114,9 +114,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ atomic_set(&master->available, 1); master->index = index; - master->device = NULL; master->main_device_id = main_id; - master->backup_device = NULL; master->backup_device_id = backup_id; init_MUTEX(&master->device_sem); @@ -167,6 +165,13 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ master->sdo_timer.data = (unsigned long) master; init_completion(&master->sdo_complete); + // init devices + if (ec_device_init(&master->main_device, master)) + goto out_return; + + if (ec_device_init(&master->backup_device, master)) + goto out_clear_main; + // create EoE handlers for (i = 0; i < eoeif_count; i++) { if (!(eoe = (ec_eoe_t *) kmalloc(sizeof(ec_eoe_t), GFP_KERNEL))) { @@ -216,6 +221,10 @@ out_clear_eoe: ec_eoe_clear(eoe); kfree(eoe); } + ec_device_clear(&master->backup_device); +out_clear_main: + ec_device_clear(&master->main_device); +out_return: return -1; } @@ -267,6 +276,9 @@ void ec_master_clear(struct kobject *kobj /**< kobject of the master */) kfree(eoe); } + ec_device_clear(&master->backup_device); + ec_device_clear(&master->main_device); + EC_INFO("Master %i freed.\n", master->index); kfree(master); @@ -594,7 +606,7 @@ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) do { // fetch pointer to transmit socket buffer - frame_data = ec_device_tx_data(master->device); + frame_data = ec_device_tx_data(&master->main_device); cur_data = frame_data + EC_FRAME_HEADER_SIZE; follows_word = NULL; more_datagrams_waiting = 0; @@ -657,7 +669,7 @@ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) EC_DBG("frame size: %i\n", cur_data - frame_data); // send frame - ec_device_send(master->device, cur_data - frame_data); + ec_device_send(&master->main_device, cur_data - frame_data); cycles_sent = get_cycles(); jiffies_sent = jiffies; @@ -764,8 +776,8 @@ void ec_master_receive_datagrams(ec_master_t *master, /**< EtherCAT master */ // dequeue the received datagram datagram->state = EC_DATAGRAM_RECEIVED; - datagram->cycles_received = master->device->cycles_poll; - datagram->jiffies_received = master->device->jiffies_poll; + datagram->cycles_received = master->main_device.cycles_poll; + datagram->jiffies_received = master->main_device.jiffies_poll; list_del_init(&datagram->queue); } } @@ -862,7 +874,7 @@ ssize_t ec_master_device_info(const ec_device_t *device, off += ec_device_id_print(dev_id, buffer + off); - if (device) { + if (device->dev) { off += sprintf(buffer + off, " (connected).\n"); off += sprintf(buffer + off, " Frames sent: %u\n", device->tx_count); @@ -921,10 +933,10 @@ ssize_t ec_master_info(ec_master_t *master, /**< EtherCAT master */ } off += sprintf(buffer + off, " Main: "); - off += ec_master_device_info(master->device, + off += ec_master_device_info(&master->main_device, master->main_device_id, buffer + off); off += sprintf(buffer + off, " Backup: "); - off += ec_master_device_info(master->backup_device, + off += ec_master_device_info(&master->backup_device, master->backup_device_id, buffer + off); up(&master->device_sem); @@ -1465,7 +1477,7 @@ void ecrt_master_send(ec_master_t *master /**< EtherCAT master */) { ec_datagram_t *datagram, *n; - if (unlikely(!master->device->link_state)) { + if (unlikely(!master->main_device.link_state)) { // link is down, no datagram can be sent list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { datagram->state = EC_DATAGRAM_ERROR; @@ -1473,7 +1485,7 @@ void ecrt_master_send(ec_master_t *master /**< EtherCAT master */) } // query link state - ec_device_poll(master->device); + ec_device_poll(&master->main_device); return; } @@ -1494,7 +1506,7 @@ void ecrt_master_receive(ec_master_t *master /**< EtherCAT master */) cycles_t cycles_timeout; // receive datagrams - ec_device_poll(master->device); + ec_device_poll(&master->main_device); cycles_timeout = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000); @@ -1502,7 +1514,7 @@ void ecrt_master_receive(ec_master_t *master /**< EtherCAT master */) list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) { if (datagram->state != EC_DATAGRAM_SENT) continue; - if (master->device->cycles_poll - datagram->cycles_sent + if (master->main_device.cycles_poll - datagram->cycles_sent > cycles_timeout) { list_del_init(&datagram->queue); datagram->state = EC_DATAGRAM_TIMED_OUT; diff --git a/master/master.h b/master/master.h index 9204dea9..cdac9117 100644 --- a/master/master.h +++ b/master/master.h @@ -99,9 +99,9 @@ struct ec_master struct kobject kobj; /**< kobject */ - ec_device_t *device; /**< EtherCAT device */ + ec_device_t main_device; /**< EtherCAT device */ const ec_device_id_t *main_device_id; /**< ID of main device */ - ec_device_t *backup_device; /**< EtherCAT backup device */ + ec_device_t backup_device; /**< EtherCAT backup device */ const ec_device_id_t *backup_device_id; /**< ID of backup device */ struct semaphore device_sem; /**< device semaphore */ diff --git a/master/module.c b/master/module.c index 0b8069e4..024947f4 100644 --- a/master/module.c +++ b/master/module.c @@ -342,54 +342,35 @@ int ecdev_offer(struct net_device *net_dev, /**< net_device to offer */ char str[50]; // FIXME list_for_each_entry(master, &masters, list) { - if (down_interruptible(&master->device_sem)) { - EC_ERR("Interrupted while waiting for device semaphore!\n"); - goto out_return; - } - if (ec_device_id_check(master->main_device_id, net_dev, driver_name, device_index)) { - ec_device_id_print(master->main_device_id, str); EC_INFO("Accepting device %s for master %u.\n", str, master->index); - if (master->device) { - EC_ERR("Master %u already has a device.\n", master->index); - goto out_up; + if (down_interruptible(&master->device_sem)) { + EC_ERR("Interrupted while waiting for device semaphore!\n"); + return -1; + } + + if (master->main_device.dev) { + EC_ERR("Master %u already has a device attached.\n", + master->index); + up(&master->device_sem); + return -1; } - if (!(master->device = (ec_device_t *) - kmalloc(sizeof(ec_device_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate device!\n"); - goto out_up; - } - - if (ec_device_init(master->device, master, - net_dev, poll, module)) { - EC_ERR("Failed to init device!\n"); - goto out_free; - } - + ec_device_attach(&master->main_device, net_dev, poll, module); up(&master->device_sem); + sprintf(net_dev->name, "ec%u", master->index); - *ecdev = master->device; // offer accepted + *ecdev = &master->main_device; // offer accepted return 0; // no error } - - up(&master->device_sem); } *ecdev = NULL; // offer declined return 0; // no error - - out_free: - kfree(master->device); - master->device = NULL; - out_up: - up(&master->device_sem); - out_return: - return 1; } /*****************************************************************************/ @@ -413,11 +394,8 @@ void ecdev_withdraw(ec_device_t *device /**< EtherCAT device */) EC_INFO("Master %u releasing main device %s.\n", master->index, str); down(&master->device_sem); - master->device = NULL; + ec_device_detach(device); up(&master->device_sem); - - ec_device_clear(device); - kfree(device); } /*****************************************************************************/ @@ -503,13 +481,13 @@ ec_master_t *ecrt_request_master(unsigned int master_index goto out_release; } - if (!master->device) { + if (master->mode != EC_MASTER_MODE_IDLE) { up(&master->device_sem); - EC_ERR("Master %i has no assigned device!\n", master_index); + EC_ERR("Master %i still waiting for devices!\n", master_index); goto out_release; } - if (!try_module_get(master->device->module)) { + if (!try_module_get(master->main_device.module)) { up(&master->device_sem); EC_ERR("Device module is unloading!\n"); goto out_release; @@ -517,7 +495,7 @@ ec_master_t *ecrt_request_master(unsigned int master_index up(&master->device_sem); - if (!master->device->link_state) { + if (!master->main_device.link_state) { EC_ERR("Link is DOWN.\n"); goto out_module_put; } @@ -531,7 +509,7 @@ ec_master_t *ecrt_request_master(unsigned int master_index return master; out_module_put: - module_put(master->device->module); + module_put(master->main_device.module); out_release: atomic_inc(&master->available); out_return: @@ -556,7 +534,7 @@ void ecrt_release_master(ec_master_t *master /**< EtherCAT master */) ec_master_leave_operation_mode(master); - module_put(master->device->module); + module_put(master->main_device.module); atomic_inc(&master->available); EC_INFO("Released master %i.\n", master->index);