1 /******************************************************************************

     2  *

     3  *  $Id$

     4  *

     5  *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH

     6  *  Copyright (C) 2014-2018  Edouard Tisserant

     7  *

     8  *  This file is part of the IgH EtherCAT Master.

     9  *

    10  *  The IgH EtherCAT Master is free software; you can redistribute it and/or

    11  *  modify it under the terms of the GNU General Public License version 2, as

    12  *  published by the Free Software Foundation.

    13  *

    14  *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    17  *  Public License for more details.

    18  *

    19  *  You should have received a copy of the GNU General Public License along

    20  *  with the IgH EtherCAT Master; if not, write to the Free Software

    21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    22  *

    23  *  ---

    24  *

    25  *  The license mentioned above concerns the source code only. Using the

    26  *  EtherCAT technology and brand is only permitted in compliance with the

    27  *  industrial property and similar rights of Beckhoff Automation GmbH.

    28  *

    29  *****************************************************************************/

    30 

    31 /** \file

    32  * EtherCAT generic Xenomai's RTDM RAW Ethernet socket device module.

    33  * Heavily based on generic.c. Should be merged in a single file with #ifdefs

    34  */

    35 

    36 /*****************************************************************************/

    37 

    38 #include <linux/module.h>

    39 #include <linux/device.h>

    40 #include <linux/err.h>

    41 #include <linux/version.h>

    42 #include <linux/if_arp.h> /* ARPHRD_ETHER */

    43 #include <linux/etherdevice.h>

    44 #include <rtdm/rtdm.h>

    45 

    46 // for rtnetif_carrier_ok

    47 // This needs -I@XENOMAI_DIR@/kernel/drivers/net/stack/include in Kbuild.in

    48 #include <rtnet_port.h>

    49 

    50 #include "../globals.h"

    51 #include "ecdev.h"

    52 

    53 #define PFX "ec_rtnet: "

    54 

    55 #define ETH_P_ETHERCAT 0x88A4

    56 

    57 #define EC_GEN_RX_BUF_SIZE 1600

    58 

    59 /*****************************************************************************/

    60 

    61 int __init ec_gen_init_module(void);

    62 void __exit ec_gen_cleanup_module(void);

    63 

    64 /*****************************************************************************/

    65 

    66 /** \cond */

    67 

    68 MODULE_AUTHOR("Edouard Tisserant <edouard.tisserant@gmail.com>");

    69 MODULE_DESCRIPTION("EtherCAT generic Xenomai's RTDM RAW Ethernet socket device module.");

    70 MODULE_LICENSE("GPL");

    71 MODULE_VERSION(EC_MASTER_VERSION);

    72 

    73 /** \endcond */

    74 

    75 struct list_head generic_devices;

    76 

    77 typedef struct {

    78     struct list_head list;

    79     struct net_device *netdev;

    80     struct rtnet_device *used_netdev;

    81     int socket;

    82     ec_device_t *ecdev;

    83     uint8_t *rx_buf;

    84     // struct sockaddr_ll dest_addr;

    85 } ec_gen_device_t;

    86 

    87 typedef struct {

    88     struct list_head list;

    89     struct rtnet_device *netdev;

    90     char name[IFNAMSIZ];

    91     int ifindex;

    92     uint8_t dev_addr[ETH_ALEN];

    93 } ec_gen_interface_desc_t;

    94 

    95 int ec_gen_device_open(ec_gen_device_t *);

    96 int ec_gen_device_stop(ec_gen_device_t *);

    97 int ec_gen_device_start_xmit(ec_gen_device_t *, struct sk_buff *);

    98 void ec_gen_device_poll(ec_gen_device_t *);

    99 

   100 /*****************************************************************************/

   101 

   102 static int ec_gen_netdev_open(struct net_device *dev)

   103 {

   104     ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev));

   105     return ec_gen_device_open(gendev);

   106 }

   107 

   108 /*****************************************************************************/

   109 

   110 static int ec_gen_netdev_stop(struct net_device *dev)

   111 {

   112     ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev));

   113     return ec_gen_device_stop(gendev);

   114 }

   115 

   116 /*****************************************************************************/

   117 

   118 static int ec_gen_netdev_start_xmit(

   119         struct sk_buff *skb,

   120         struct net_device *dev

   121         )

   122 {

   123     ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev));

   124     return ec_gen_device_start_xmit(gendev, skb);

   125 }

   126 

   127 /*****************************************************************************/

   128 

   129 void ec_gen_poll(struct net_device *dev)

   130 {

   131     ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev));

   132     ec_gen_device_poll(gendev);

   133 }

   134 

   135 /*****************************************************************************/

   136 

   137 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29)

   138 static const struct net_device_ops ec_gen_netdev_ops = {

   139     .ndo_open       = ec_gen_netdev_open,

   140     .ndo_stop       = ec_gen_netdev_stop,

   141     .ndo_start_xmit = ec_gen_netdev_start_xmit,

   142 };

   143 #endif

   144 

   145 /*****************************************************************************/

   146 

   147 /** Init generic device.

   148  */

   149 int ec_gen_device_init(

   150         ec_gen_device_t *dev

   151         )

   152 {

   153     ec_gen_device_t **priv;

   154     char null = 0x00;

   155 

   156     dev->ecdev = NULL;

   157     dev->socket = -1;

   158     dev->rx_buf = NULL;

   159 

   160 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 17, 0)

   161     dev->netdev = alloc_netdev(sizeof(ec_gen_device_t *), &null,

   162             NET_NAME_UNKNOWN, ether_setup);

   163 #else

   164     dev->netdev = alloc_netdev(sizeof(ec_gen_device_t *), &null, ether_setup);

   165 #endif

   166     if (!dev->netdev) {

   167         return -ENOMEM;

   168     }

   169 

   170 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29)

   171     dev->netdev->netdev_ops = &ec_gen_netdev_ops;

   172 #else

   173     dev->netdev->open = ec_gen_netdev_open;

   174     dev->netdev->stop = ec_gen_netdev_stop;

   175     dev->netdev->hard_start_xmit = ec_gen_netdev_start_xmit;

   176 #endif

   177 

   178     priv = netdev_priv(dev->netdev);

   179     *priv = dev;

   180 

   181     return 0;

   182 }

   183 

   184 /*****************************************************************************/

   185 

   186 /** Clear generic device.

   187  */

   188 void ec_gen_device_clear(

   189         ec_gen_device_t *dev

   190         )

   191 {

   192     if (dev->ecdev) {

   193         ecdev_close(dev->ecdev);

   194         ecdev_withdraw(dev->ecdev);

   195     }

   196     if (!(dev->socket < 0)) {

   197         rtdm_close(dev->socket);

   198         dev->socket = -1;

   199     }

   200     free_netdev(dev->netdev);

   201 

   202     if (dev->rx_buf) {

   203         kfree(dev->rx_buf);

   204     }

   205 }

   206 

   207 /*****************************************************************************/

   208 

   209 /** Creates a network socket.

   210  */

   211 int ec_gen_device_create_socket(

   212         ec_gen_device_t *dev,

   213         ec_gen_interface_desc_t *desc

   214         )

   215 {

   216     int ret;

   217     struct sockaddr_ll sa;

   218 

   219     dev->rx_buf = kmalloc(EC_GEN_RX_BUF_SIZE, GFP_KERNEL);

   220     if (!dev->rx_buf) {

   221         return -ENOMEM;

   222     }

   223 

   224     /* create rt-socket */

   225     dev->socket = rtdm_socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ETHERCAT));

   226     if (dev->socket < 0) {

   227         printk(" rtdm_socket() = %d!\n", dev->socket);

   228         return dev->socket;

   229     }

   230 

   231     printk(KERN_ERR PFX "Binding socket to interface %i (%s).\n",

   232             desc->ifindex, desc->name);

   233 

   234     memset(&sa, 0x00, sizeof(sa));

   235     sa.sll_family = AF_PACKET;

   236     sa.sll_protocol = htons(ETH_P_ETHERCAT);

   237     sa.sll_ifindex = desc->ifindex;

   238     ret = rtdm_bind(dev->socket, (struct sockaddr *)&sa,

   239                       sizeof(struct sockaddr_ll));

   240     if (ret < 0) {

   241         printk(" rtdm_bind() = %d!\n", ret);

   242         rtdm_close(dev->socket);

   243         dev->socket = -1;

   244         return ret;

   245     }

   246 

   247     return 0;

   248 }

   249 

   250 /*****************************************************************************/

   251 

   252 /** Offer generic device to master.

   253  */

   254 int ec_gen_device_offer(

   255         ec_gen_device_t *dev,

   256         ec_gen_interface_desc_t *desc

   257         )

   258 {

   259     int ret = 0;

   260 

   261     dev->used_netdev = desc->netdev;

   262     memcpy(dev->netdev->dev_addr, desc->dev_addr, ETH_ALEN);

   263 

   264     dev->ecdev = ecdev_offer(dev->netdev, ec_gen_poll, THIS_MODULE);

   265     if (dev->ecdev) {

   266         if (ec_gen_device_create_socket(dev, desc)) {

   267             ecdev_withdraw(dev->ecdev);

   268             dev->ecdev = NULL;

   269         } else if (ecdev_open(dev->ecdev)) {

   270             ecdev_withdraw(dev->ecdev);

   271             dev->ecdev = NULL;

   272         } else {

   273             ecdev_set_link(dev->ecdev, rtnetif_carrier_ok(dev->used_netdev)); // FIXME

   274             ret = 1;

   275         }

   276     }

   277 

   278     return ret;

   279 }

   280 

   281 /*****************************************************************************/

   282 

   283 /** Open the device.

   284  */

   285 int ec_gen_device_open(

   286         ec_gen_device_t *dev

   287         )

   288 {

   289     return 0;

   290 }

   291 

   292 /*****************************************************************************/

   293 

   294 /** Stop the device.

   295  */

   296 int ec_gen_device_stop(

   297         ec_gen_device_t *dev

   298         )

   299 {

   300     return 0;

   301 }

   302 

   303 /*****************************************************************************/

   304 

   305 int ec_gen_device_start_xmit(

   306         ec_gen_device_t *dev,

   307         struct sk_buff *skb

   308         )

   309 {

   310     struct user_msghdr msg;

   311     struct iovec iov;

   312     size_t len = skb->len;

   313     int ret;

   314 

   315     ecdev_set_link(dev->ecdev, rtnetif_carrier_ok(dev->used_netdev));

   316     //ecdev_set_link(dev->ecdev, 1); // FIXME

   317 

   318     iov.iov_base = skb->data;

   319     iov.iov_len = len;

   320     memset(&msg, 0, sizeof(msg));

   321     // msg.msg_name    = &dev->dest_addr;

   322     // msg.msg_namelen = sizeof(dev->dest_addr);

   323     msg.msg_iov     = &iov;

   324     msg.msg_iovlen  = 1;

   325 

   326     ret = rtdm_sendmsg(dev->socket, &msg, 0);

   327     // printk("sendmsg: %d\n", ret);

   328     // if (ret != len && ret != -ENOSYS)

   329     //     printk(" rtdm_sendmsg() = %d!\n", ret);

   330 

   331     return ret == len ? NETDEV_TX_OK : NETDEV_TX_BUSY;

   332 }

   333 

   334 /*****************************************************************************/

   335 

   336 /** Polls the device.

   337  */

   338 void ec_gen_device_poll(

   339         ec_gen_device_t *dev

   340         )

   341 {

   342     struct user_msghdr msg;

   343     struct iovec iov;

   344     int ret, budget = 128; // FIXME

   345 

   346     ecdev_set_link(dev->ecdev, rtnetif_carrier_ok(dev->used_netdev));

   347 

   348     do {

   349         iov.iov_base = dev->rx_buf;

   350         iov.iov_len = EC_GEN_RX_BUF_SIZE;

   351         memset(&msg, 0, sizeof(msg));

   352         msg.msg_iov     = &iov;

   353         msg.msg_iovlen  = 1;

   354 

   355         ret = rtdm_recvmsg(dev->socket, &msg, MSG_DONTWAIT);

   356         if (ret > 0) {

   357             ecdev_receive(dev->ecdev, dev->rx_buf, ret);

   358         } else if (ret < 0) {

   359             break;

   360         }

   361         //printk("revmsg: %d\n", ret);

   362         budget--;

   363     } while (budget);

   364 }

   365 

   366 /*****************************************************************************/

   367 

   368 /** Offer device.

   369  */

   370 int offer_device(

   371         ec_gen_interface_desc_t *desc

   372         )

   373 {

   374     ec_gen_device_t *gendev;

   375     int ret = 0;

   376 

   377     gendev = kmalloc(sizeof(ec_gen_device_t), GFP_KERNEL);

   378     if (!gendev) {

   379         return -ENOMEM;

   380     }

   381 

   382     ret = ec_gen_device_init(gendev);

   383     if (ret) {

   384         kfree(gendev);

   385         return ret;

   386     }

   387 

   388     if (ec_gen_device_offer(gendev, desc)) {

   389         list_add_tail(&gendev->list, &generic_devices);

   390     } else {

   391         ec_gen_device_clear(gendev);

   392         kfree(gendev);

   393     }

   394 

   395     return ret;

   396 }

   397 

   398 /*****************************************************************************/

   399 

   400 /** Clear devices.

   401  */

   402 void clear_devices(void)

   403 {

   404     ec_gen_device_t *gendev, *next;

   405 

   406     list_for_each_entry_safe(gendev, next, &generic_devices, list) {

   407         list_del(&gendev->list);

   408         ec_gen_device_clear(gendev);

   409         kfree(gendev);

   410     }

   411 }

   412 

   413 /*****************************************************************************/

   414 

   415 /** Module initialization.

   416  *

   417  * Initializes \a master_count masters.

   418  * \return 0 on success, else < 0

   419  */

   420 int __init ec_gen_init_module(void)

   421 {

   422     int ret = 0;

   423     int devices = 0;

   424     int i;

   425     struct list_head descs;

   426     struct rtnet_device *rtdev;

   427     ec_gen_interface_desc_t *desc, *next;

   428 

   429     printk(KERN_INFO PFX "EtherCAT master RTnet Ethernet device module %s\n",

   430             EC_MASTER_VERSION);

   431 

   432     INIT_LIST_HEAD(&generic_devices);

   433     INIT_LIST_HEAD(&descs);

   434 

   435     for (i = 0; i < MAX_RT_DEVICES; i++) {

   436 

   437         rtdev = rtdev_get_by_index(i);

   438         if (rtdev != NULL) {

   439             mutex_lock(&rtdev->nrt_lock);

   440 

   441             //if (test_bit(PRIV_FLAG_UP, &rtdev->priv_flags)) {

   442             //    mutex_unlock(&rtdev->nrt_lock);

   443             //    printk(KERN_ERR PFX "%s busy, skipping device!\n", rtdev->name);

   444             //    rtdev_dereference(rtdev);

   445             //    continue;

   446             //}

   447 

   448             desc = kmalloc(sizeof(ec_gen_interface_desc_t), GFP_ATOMIC);

   449             if (!desc) {

   450                 ret = -ENOMEM;

   451                 goto out_err;

   452             }

   453             strncpy(desc->name, rtdev->name, IFNAMSIZ);

   454             desc->netdev = rtdev;

   455             desc->ifindex = rtdev->ifindex;

   456             memcpy(desc->dev_addr, rtdev->dev_addr, ETH_ALEN);

   457             list_add_tail(&desc->list, &descs);

   458               mutex_unlock(&rtdev->nrt_lock);

   459 

   460             devices++;

   461         }

   462     }

   463 

   464     if (devices == 0) {

   465         printk(KERN_ERR PFX "no real-time devices found!\n");

   466         ret = -ENODEV;

   467         goto out_err;

   468     }

   469 

   470     list_for_each_entry_safe(desc, next, &descs, list) {

   471         ret = offer_device(desc);

   472         if (ret) {

   473             goto out_err;

   474         }

   475         kfree(desc);

   476     }

   477     return ret;

   478 

   479 out_err:

   480     list_for_each_entry_safe(desc, next, &descs, list) {

   481         list_del(&desc->list);

   482         kfree(desc);

   483     }

   484     clear_devices();

   485     return ret;

   486 }

   487 

   488 /*****************************************************************************/

   489 

   490 /** Module cleanup.

   491  *

   492  * Clears all master instances.

   493  */

   494 void __exit ec_gen_cleanup_module(void)

   495 {

   496     clear_devices();

   497     printk(KERN_INFO PFX "Unloading.\n");

   498 }

   499 

   500 /*****************************************************************************/

   501 

   502 /** \cond */

   503 

   504 module_init(ec_gen_init_module);

   505 module_exit(ec_gen_cleanup_module);

   506 

   507 /** \endcond */

   508 

   509 /*****************************************************************************/