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

/*****************************************************************************/