edouard@2709: /****************************************************************************** edouard@2709: * edouard@2709: * $Id$ edouard@2709: * edouard@2709: * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH edouard@2709: * Copyright (C) 2014-2018 Edouard Tisserant edouard@2709: * edouard@2709: * This file is part of the IgH EtherCAT Master. edouard@2709: * edouard@2709: * The IgH EtherCAT Master is free software; you can redistribute it and/or edouard@2709: * modify it under the terms of the GNU General Public License version 2, as edouard@2709: * published by the Free Software Foundation. edouard@2709: * edouard@2709: * The IgH EtherCAT Master is distributed in the hope that it will be useful, edouard@2709: * but WITHOUT ANY WARRANTY; without even the implied warranty of edouard@2709: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General edouard@2709: * Public License for more details. edouard@2709: * edouard@2709: * You should have received a copy of the GNU General Public License along edouard@2709: * with the IgH EtherCAT Master; if not, write to the Free Software edouard@2709: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA edouard@2709: * edouard@2709: * --- edouard@2709: * edouard@2709: * The license mentioned above concerns the source code only. Using the edouard@2709: * EtherCAT technology and brand is only permitted in compliance with the edouard@2709: * industrial property and similar rights of Beckhoff Automation GmbH. edouard@2709: * edouard@2709: *****************************************************************************/ edouard@2709: edouard@2709: /** \file edouard@2709: * EtherCAT generic Xenomai's RTDM RAW Ethernet socket device module. edouard@2709: * Heavily based on generic.c. Should be merged in a single file with #ifdefs edouard@2709: */ edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: #include edouard@2709: #include edouard@2709: #include edouard@2709: #include edouard@2709: #include /* ARPHRD_ETHER */ edouard@2709: #include edouard@2709: #include edouard@2709: edouard@2717: // for rtnetif_carrier_ok and rtpc_dispatch_call edouard@2709: // This needs -I@XENOMAI_DIR@/kernel/drivers/net/stack/include in Kbuild.in edouard@2709: #include edouard@2717: #include edouard@2709: edouard@2709: #include "../globals.h" edouard@2709: #include "ecdev.h" edouard@2709: edouard@2711: #define PFX "ec_rtdmnet: " edouard@2709: edouard@2709: #define ETH_P_ETHERCAT 0x88A4 edouard@2709: edouard@2709: #define EC_GEN_RX_BUF_SIZE 1600 edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: int __init ec_gen_init_module(void); edouard@2709: void __exit ec_gen_cleanup_module(void); edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** \cond */ edouard@2709: edouard@2709: MODULE_AUTHOR("Edouard Tisserant "); edouard@2709: MODULE_DESCRIPTION("EtherCAT generic Xenomai's RTDM RAW Ethernet socket device module."); edouard@2709: MODULE_LICENSE("GPL"); edouard@2709: MODULE_VERSION(EC_MASTER_VERSION); edouard@2709: edouard@2709: /** \endcond */ edouard@2709: edouard@2709: struct list_head generic_devices; edouard@2709: edouard@2709: typedef struct { edouard@2709: struct list_head list; edouard@2709: struct net_device *netdev; edouard@2709: struct rtnet_device *used_netdev; edouard@2709: int socket; edouard@2709: ec_device_t *ecdev; edouard@2709: uint8_t *rx_buf; edouard@2709: // struct sockaddr_ll dest_addr; edouard@2709: } ec_gen_device_t; edouard@2709: edouard@2709: typedef struct { edouard@2709: struct list_head list; edouard@2709: struct rtnet_device *netdev; edouard@2709: char name[IFNAMSIZ]; edouard@2709: int ifindex; edouard@2709: uint8_t dev_addr[ETH_ALEN]; edouard@2709: } ec_gen_interface_desc_t; edouard@2709: edouard@2709: int ec_gen_device_open(ec_gen_device_t *); edouard@2709: int ec_gen_device_stop(ec_gen_device_t *); edouard@2709: int ec_gen_device_start_xmit(ec_gen_device_t *, struct sk_buff *); edouard@2709: void ec_gen_device_poll(ec_gen_device_t *); edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: static int ec_gen_netdev_open(struct net_device *dev) edouard@2709: { edouard@2709: ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev)); edouard@2709: return ec_gen_device_open(gendev); edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: static int ec_gen_netdev_stop(struct net_device *dev) edouard@2709: { edouard@2709: ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev)); edouard@2709: return ec_gen_device_stop(gendev); edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: static int ec_gen_netdev_start_xmit( edouard@2709: struct sk_buff *skb, edouard@2709: struct net_device *dev edouard@2709: ) edouard@2709: { edouard@2709: ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev)); edouard@2709: return ec_gen_device_start_xmit(gendev, skb); edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: void ec_gen_poll(struct net_device *dev) edouard@2709: { edouard@2709: ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev)); edouard@2709: ec_gen_device_poll(gendev); edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29) edouard@2709: static const struct net_device_ops ec_gen_netdev_ops = { edouard@2709: .ndo_open = ec_gen_netdev_open, edouard@2709: .ndo_stop = ec_gen_netdev_stop, edouard@2709: .ndo_start_xmit = ec_gen_netdev_start_xmit, edouard@2709: }; edouard@2709: #endif edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** Init generic device. edouard@2709: */ edouard@2709: int ec_gen_device_init( edouard@2709: ec_gen_device_t *dev edouard@2709: ) edouard@2709: { edouard@2709: ec_gen_device_t **priv; edouard@2709: char null = 0x00; edouard@2709: edouard@2709: dev->ecdev = NULL; edouard@2709: dev->socket = -1; edouard@2709: dev->rx_buf = NULL; edouard@2709: edouard@2709: #if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 17, 0) edouard@2709: dev->netdev = alloc_netdev(sizeof(ec_gen_device_t *), &null, edouard@2709: NET_NAME_UNKNOWN, ether_setup); edouard@2709: #else edouard@2709: dev->netdev = alloc_netdev(sizeof(ec_gen_device_t *), &null, ether_setup); edouard@2709: #endif edouard@2709: if (!dev->netdev) { edouard@2709: return -ENOMEM; edouard@2709: } edouard@2709: edouard@2709: #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29) edouard@2709: dev->netdev->netdev_ops = &ec_gen_netdev_ops; edouard@2709: #else edouard@2709: dev->netdev->open = ec_gen_netdev_open; edouard@2709: dev->netdev->stop = ec_gen_netdev_stop; edouard@2709: dev->netdev->hard_start_xmit = ec_gen_netdev_start_xmit; edouard@2709: #endif edouard@2709: edouard@2709: priv = netdev_priv(dev->netdev); edouard@2709: *priv = dev; edouard@2709: edouard@2709: return 0; edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** Clear generic device. edouard@2709: */ edouard@2709: void ec_gen_device_clear( edouard@2709: ec_gen_device_t *dev edouard@2709: ) edouard@2709: { edouard@2709: if (dev->ecdev) { edouard@2709: ecdev_close(dev->ecdev); edouard@2709: ecdev_withdraw(dev->ecdev); edouard@2709: } edouard@2709: if (!(dev->socket < 0)) { edouard@2709: rtdm_close(dev->socket); edouard@2709: dev->socket = -1; edouard@2709: } edouard@2709: free_netdev(dev->netdev); edouard@2709: edouard@2709: if (dev->rx_buf) { edouard@2709: kfree(dev->rx_buf); edouard@2709: } edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** Creates a network socket. edouard@2709: */ edouard@2709: int ec_gen_device_create_socket( edouard@2709: ec_gen_device_t *dev, edouard@2709: ec_gen_interface_desc_t *desc edouard@2709: ) edouard@2709: { edouard@2709: int ret; edouard@2709: struct sockaddr_ll sa; edouard@2709: edouard@2709: dev->rx_buf = kmalloc(EC_GEN_RX_BUF_SIZE, GFP_KERNEL); edouard@2709: if (!dev->rx_buf) { edouard@2709: return -ENOMEM; edouard@2709: } edouard@2709: edouard@2709: /* create rt-socket */ edouard@2709: dev->socket = rtdm_socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ETHERCAT)); edouard@2709: if (dev->socket < 0) { edouard@2709: printk(" rtdm_socket() = %d!\n", dev->socket); edouard@2709: return dev->socket; edouard@2709: } edouard@2709: edouard@2709: printk(KERN_ERR PFX "Binding socket to interface %i (%s).\n", edouard@2709: desc->ifindex, desc->name); edouard@2709: edouard@2709: memset(&sa, 0x00, sizeof(sa)); edouard@2709: sa.sll_family = AF_PACKET; edouard@2709: sa.sll_protocol = htons(ETH_P_ETHERCAT); edouard@2709: sa.sll_ifindex = desc->ifindex; edouard@2709: ret = rtdm_bind(dev->socket, (struct sockaddr *)&sa, edouard@2709: sizeof(struct sockaddr_ll)); edouard@2709: if (ret < 0) { edouard@2709: printk(" rtdm_bind() = %d!\n", ret); edouard@2709: rtdm_close(dev->socket); edouard@2709: dev->socket = -1; edouard@2709: return ret; edouard@2709: } edouard@2709: edouard@2709: return 0; edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** Offer generic device to master. edouard@2709: */ edouard@2709: int ec_gen_device_offer( edouard@2709: ec_gen_device_t *dev, edouard@2709: ec_gen_interface_desc_t *desc edouard@2709: ) edouard@2709: { edouard@2709: int ret = 0; edouard@2709: edouard@2709: dev->used_netdev = desc->netdev; edouard@2709: memcpy(dev->netdev->dev_addr, desc->dev_addr, ETH_ALEN); edouard@2709: edouard@2709: dev->ecdev = ecdev_offer(dev->netdev, ec_gen_poll, THIS_MODULE); edouard@2709: if (dev->ecdev) { edouard@2709: if (ec_gen_device_create_socket(dev, desc)) { edouard@2709: ecdev_withdraw(dev->ecdev); edouard@2709: dev->ecdev = NULL; edouard@2709: } else if (ecdev_open(dev->ecdev)) { edouard@2709: ecdev_withdraw(dev->ecdev); edouard@2709: dev->ecdev = NULL; edouard@2709: } else { edouard@2709: ecdev_set_link(dev->ecdev, rtnetif_carrier_ok(dev->used_netdev)); // FIXME edouard@2709: ret = 1; edouard@2709: } edouard@2709: } edouard@2709: edouard@2709: return ret; edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** Open the device. edouard@2709: */ edouard@2709: int ec_gen_device_open( edouard@2709: ec_gen_device_t *dev edouard@2709: ) edouard@2709: { edouard@2709: return 0; edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** Stop the device. edouard@2709: */ edouard@2709: int ec_gen_device_stop( edouard@2709: ec_gen_device_t *dev edouard@2709: ) edouard@2709: { edouard@2709: return 0; edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2717: // delegate to some rtdm thread when called from nrt context edouard@2717: struct sendmsg_params { edouard@2717: int socket; edouard@2717: struct user_msghdr *msg; edouard@2717: }; edouard@2717: edouard@2717: static int sendmsg_handler(struct rt_proc_call *call) edouard@2717: { edouard@2717: struct sendmsg_params *params; edouard@2717: edouard@2717: params = rtpc_get_priv(call, struct sendmsg_params); edouard@2717: edouard@2717: return rtdm_sendmsg(params->socket, params->msg, 0); edouard@2717: } edouard@2717: edouard@2717: static ssize_t edouard@2717: nrt_rtdm_sendmsg(int socket, struct user_msghdr *msg) edouard@2717: { edouard@2717: int ret; edouard@2717: struct sendmsg_params params = {socket, msg}; edouard@2717: edouard@2717: ret = rtpc_dispatch_call(sendmsg_handler, 0, ¶ms, edouard@2717: sizeof(params), NULL, NULL); edouard@2717: return ret; edouard@2717: } edouard@2717: edouard@2709: int ec_gen_device_start_xmit( edouard@2709: ec_gen_device_t *dev, edouard@2709: struct sk_buff *skb edouard@2709: ) edouard@2709: { edouard@2709: struct user_msghdr msg; edouard@2709: struct iovec iov; edouard@2709: size_t len = skb->len; edouard@2709: int ret; edouard@2709: edouard@2709: ecdev_set_link(dev->ecdev, rtnetif_carrier_ok(dev->used_netdev)); edouard@2709: //ecdev_set_link(dev->ecdev, 1); // FIXME edouard@2709: edouard@2709: iov.iov_base = skb->data; edouard@2709: iov.iov_len = len; edouard@2709: memset(&msg, 0, sizeof(msg)); edouard@2709: // msg.msg_name = &dev->dest_addr; edouard@2709: // msg.msg_namelen = sizeof(dev->dest_addr); edouard@2709: msg.msg_iov = &iov; edouard@2709: msg.msg_iovlen = 1; edouard@2709: edouard@2718: if (rtdm_in_rt_context()) edouard@2717: ret = nrt_rtdm_sendmsg(dev->socket, &msg); edouard@2717: else edouard@2717: ret = rtdm_sendmsg(dev->socket, &msg, 0); edouard@2709: edouard@2709: return ret == len ? NETDEV_TX_OK : NETDEV_TX_BUSY; edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** Polls the device. edouard@2709: */ edouard@2717: edouard@2717: // delegate to some rtdm thread when called from nrt context edouard@2717: struct recvmsg_params { edouard@2717: int socket; edouard@2717: struct user_msghdr *msg; edouard@2717: }; edouard@2717: edouard@2717: static int recvmsg_handler(struct rt_proc_call *call) edouard@2717: { edouard@2717: struct recvmsg_params *params; edouard@2717: edouard@2717: params = rtpc_get_priv(call, struct recvmsg_params); edouard@2717: edouard@2717: return rtdm_recvmsg(params->socket, params->msg, MSG_DONTWAIT); edouard@2717: } edouard@2717: edouard@2717: static ssize_t edouard@2717: nrt_rtdm_recvmsg(int socket, struct user_msghdr *msg) edouard@2717: { edouard@2717: int ret; edouard@2717: struct recvmsg_params params = {socket,msg}; edouard@2717: edouard@2717: ret = rtpc_dispatch_call(recvmsg_handler, 0, ¶ms, edouard@2717: sizeof(params), NULL, NULL); edouard@2717: return ret; edouard@2717: } edouard@2717: edouard@2709: void ec_gen_device_poll( edouard@2709: ec_gen_device_t *dev edouard@2709: ) edouard@2709: { edouard@2709: struct user_msghdr msg; edouard@2709: struct iovec iov; edouard@2709: int ret, budget = 128; // FIXME edouard@2709: edouard@2709: ecdev_set_link(dev->ecdev, rtnetif_carrier_ok(dev->used_netdev)); edouard@2709: edouard@2709: do { edouard@2709: iov.iov_base = dev->rx_buf; edouard@2709: iov.iov_len = EC_GEN_RX_BUF_SIZE; edouard@2709: memset(&msg, 0, sizeof(msg)); edouard@2709: msg.msg_iov = &iov; edouard@2709: msg.msg_iovlen = 1; edouard@2709: edouard@2718: if (rtdm_in_rt_context()) edouard@2717: ret = nrt_rtdm_recvmsg(dev->socket, &msg); edouard@2717: else edouard@2717: ret = rtdm_recvmsg(dev->socket, &msg, MSG_DONTWAIT); edouard@2717: edouard@2709: if (ret > 0) { edouard@2709: ecdev_receive(dev->ecdev, dev->rx_buf, ret); edouard@2709: } else if (ret < 0) { edouard@2709: break; edouard@2709: } edouard@2709: budget--; edouard@2709: } while (budget); edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** Offer device. edouard@2709: */ edouard@2709: int offer_device( edouard@2709: ec_gen_interface_desc_t *desc edouard@2709: ) edouard@2709: { edouard@2709: ec_gen_device_t *gendev; edouard@2709: int ret = 0; edouard@2709: edouard@2709: gendev = kmalloc(sizeof(ec_gen_device_t), GFP_KERNEL); edouard@2709: if (!gendev) { edouard@2709: return -ENOMEM; edouard@2709: } edouard@2709: edouard@2709: ret = ec_gen_device_init(gendev); edouard@2709: if (ret) { edouard@2709: kfree(gendev); edouard@2709: return ret; edouard@2709: } edouard@2709: edouard@2709: if (ec_gen_device_offer(gendev, desc)) { edouard@2709: list_add_tail(&gendev->list, &generic_devices); edouard@2709: } else { edouard@2709: ec_gen_device_clear(gendev); edouard@2709: kfree(gendev); edouard@2709: } edouard@2709: edouard@2709: return ret; edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** Clear devices. edouard@2709: */ edouard@2709: void clear_devices(void) edouard@2709: { edouard@2709: ec_gen_device_t *gendev, *next; edouard@2709: edouard@2709: list_for_each_entry_safe(gendev, next, &generic_devices, list) { edouard@2709: list_del(&gendev->list); edouard@2709: ec_gen_device_clear(gendev); edouard@2709: kfree(gendev); edouard@2709: } edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** Module initialization. edouard@2709: * edouard@2709: * Initializes \a master_count masters. edouard@2709: * \return 0 on success, else < 0 edouard@2709: */ edouard@2709: int __init ec_gen_init_module(void) edouard@2709: { edouard@2709: int ret = 0; edouard@2709: int devices = 0; edouard@2709: int i; edouard@2709: struct list_head descs; edouard@2709: struct rtnet_device *rtdev; edouard@2709: ec_gen_interface_desc_t *desc, *next; edouard@2709: edouard@2709: printk(KERN_INFO PFX "EtherCAT master RTnet Ethernet device module %s\n", edouard@2709: EC_MASTER_VERSION); edouard@2709: edouard@2709: INIT_LIST_HEAD(&generic_devices); edouard@2709: INIT_LIST_HEAD(&descs); edouard@2709: edouard@2709: for (i = 0; i < MAX_RT_DEVICES; i++) { edouard@2709: edouard@2709: rtdev = rtdev_get_by_index(i); edouard@2709: if (rtdev != NULL) { edouard@2709: mutex_lock(&rtdev->nrt_lock); edouard@2709: edouard@2709: //if (test_bit(PRIV_FLAG_UP, &rtdev->priv_flags)) { edouard@2709: // mutex_unlock(&rtdev->nrt_lock); edouard@2709: // printk(KERN_ERR PFX "%s busy, skipping device!\n", rtdev->name); edouard@2709: // rtdev_dereference(rtdev); edouard@2709: // continue; edouard@2709: //} edouard@2709: edouard@2709: desc = kmalloc(sizeof(ec_gen_interface_desc_t), GFP_ATOMIC); edouard@2709: if (!desc) { edouard@2709: ret = -ENOMEM; edouard@2709: goto out_err; edouard@2709: } edouard@2709: strncpy(desc->name, rtdev->name, IFNAMSIZ); edouard@2709: desc->netdev = rtdev; edouard@2709: desc->ifindex = rtdev->ifindex; edouard@2709: memcpy(desc->dev_addr, rtdev->dev_addr, ETH_ALEN); edouard@2709: list_add_tail(&desc->list, &descs); edouard@2709: mutex_unlock(&rtdev->nrt_lock); edouard@2709: edouard@2709: devices++; edouard@2709: } edouard@2709: } edouard@2709: edouard@2709: if (devices == 0) { edouard@2709: printk(KERN_ERR PFX "no real-time devices found!\n"); edouard@2709: ret = -ENODEV; edouard@2709: goto out_err; edouard@2709: } edouard@2709: edouard@2709: list_for_each_entry_safe(desc, next, &descs, list) { edouard@2709: ret = offer_device(desc); edouard@2709: if (ret) { edouard@2709: goto out_err; edouard@2709: } edouard@2709: kfree(desc); edouard@2709: } edouard@2709: return ret; edouard@2709: edouard@2709: out_err: edouard@2709: list_for_each_entry_safe(desc, next, &descs, list) { edouard@2709: list_del(&desc->list); edouard@2709: kfree(desc); edouard@2709: } edouard@2709: clear_devices(); edouard@2709: return ret; edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** Module cleanup. edouard@2709: * edouard@2709: * Clears all master instances. edouard@2709: */ edouard@2709: void __exit ec_gen_cleanup_module(void) edouard@2709: { edouard@2709: clear_devices(); edouard@2709: printk(KERN_INFO PFX "Unloading.\n"); edouard@2709: } edouard@2709: edouard@2709: /*****************************************************************************/ edouard@2709: edouard@2709: /** \cond */ edouard@2709: edouard@2709: module_init(ec_gen_init_module); edouard@2709: module_exit(ec_gen_cleanup_module); edouard@2709: edouard@2709: /** \endcond */ edouard@2709: edouard@2709: /*****************************************************************************/