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_rtdmnet: "

    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

   328

    return ret == len ? NETDEV_TX_OK : NETDEV_TX_BUSY;

   329

}

   330

   331

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

   332

   333

/** Polls the device.

   334

 */

   335

void ec_gen_device_poll(

   336

        ec_gen_device_t *dev

   337

        )

   338

{

   339

    struct user_msghdr msg;

   340

    struct iovec iov;

   341

    int ret, budget = 128; // FIXME

   342

   343

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

   344

   345

    do {

   346

        iov.iov_base = dev->rx_buf;

   347

        iov.iov_len = EC_GEN_RX_BUF_SIZE;

   348

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

   349

        msg.msg_iov     = &iov;

   350

        msg.msg_iovlen  = 1;

   351

   352

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

   353

        if (ret > 0) {

   354

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

   355

        } else if (ret < 0) {

   356

            break;

   357

        }

   358

        budget--;

   359

    } while (budget);

   360

}

   361

   362

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

   363

   364

/** Offer device.

   365

 */

   366

int offer_device(

   367

        ec_gen_interface_desc_t *desc

   368

        )

   369

{

   370

    ec_gen_device_t *gendev;

   371

    int ret = 0;

   372

   373

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

   374

    if (!gendev) {

   375

        return -ENOMEM;

   376

    }

   377

   378

    ret = ec_gen_device_init(gendev);

   379

    if (ret) {

   380

        kfree(gendev);

   381

        return ret;

   382

    }

   383

   384

    if (ec_gen_device_offer(gendev, desc)) {

   385

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

   386

    } else {

   387

        ec_gen_device_clear(gendev);

   388

        kfree(gendev);

   389

    }

   390

   391

    return ret;

   392

}

   393

   394

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

   395

   396

/** Clear devices.

   397

 */

   398

void clear_devices(void)

   399

{

   400

    ec_gen_device_t *gendev, *next;

   401

   402

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

   403

        list_del(&gendev->list);

   404

        ec_gen_device_clear(gendev);

   405

        kfree(gendev);

   406

    }

   407

}

   408

   409

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

   410

   411

/** Module initialization.

   412

 *

   413

 * Initializes \a master_count masters.

   414

 * \return 0 on success, else < 0

   415

 */

   416

int __init ec_gen_init_module(void)

   417

{

   418

    int ret = 0;

   419

    int devices = 0;

   420

    int i;

   421

    struct list_head descs;

   422

    struct rtnet_device *rtdev;

   423

    ec_gen_interface_desc_t *desc, *next;

   424

   425

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

   426

            EC_MASTER_VERSION);

   427

   428

    INIT_LIST_HEAD(&generic_devices);

   429

    INIT_LIST_HEAD(&descs);

   430

   431

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

   432

   433

        rtdev = rtdev_get_by_index(i);

   434

        if (rtdev != NULL) {

   435

            mutex_lock(&rtdev->nrt_lock);

   436

   437

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

   438

            //    mutex_unlock(&rtdev->nrt_lock);

   439

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

   440

            //    rtdev_dereference(rtdev);

   441

            //    continue;

   442

            //}

   443

   444

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

   445

            if (!desc) {

   446

                ret = -ENOMEM;

   447

                goto out_err;

   448

            }

   449

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

   450

            desc->netdev = rtdev;

   451

            desc->ifindex = rtdev->ifindex;

   452

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

   453

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

   454

              mutex_unlock(&rtdev->nrt_lock);

   455

   456

            devices++;

   457

        }

   458

    }

   459

   460

    if (devices == 0) {

   461

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

   462

        ret = -ENODEV;

   463

        goto out_err;

   464

    }

   465

   466

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

   467

        ret = offer_device(desc);

   468

        if (ret) {

   469

            goto out_err;

   470

        }

   471

        kfree(desc);

   472

    }

   473

    return ret;

   474

   475

out_err:

   476

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

   477

        list_del(&desc->list);

   478

        kfree(desc);

   479

    }

   480

    clear_devices();

   481

    return ret;

   482

}

   483

   484

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

   485

   486

/** Module cleanup.

   487

 *

   488

 * Clears all master instances.

   489

 */

   490

void __exit ec_gen_cleanup_module(void)

   491

{

   492

    clear_devices();

   493

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

   494

}

   495

   496

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

   497

   498

/** \cond */

   499

   500

module_init(ec_gen_init_module);

   501

module_exit(ec_gen_cleanup_module);

   502

   503

/** \endcond */

   504

   505

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