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 and rtpc_dispatch_call

    47

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

    48

#include <rtnet_port.h>

    49

#include <rtnet_rtpc.h>

    50

    51

#include "../globals.h"

    52

#include "ecdev.h"

    53

    54

#define PFX "ec_rtdmnet: "

    55

    56

#define ETH_P_ETHERCAT 0x88A4

    57

    58

#define EC_GEN_RX_BUF_SIZE 1600

    59

    60

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

    61

    62

int __init ec_gen_init_module(void);

    63

void __exit ec_gen_cleanup_module(void);

    64

    65

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

    66

    67

/** \cond */

    68

    69

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

    70

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

    71

MODULE_LICENSE("GPL");

    72

MODULE_VERSION(EC_MASTER_VERSION);

    73

    74

/** \endcond */

    75

    76

struct list_head generic_devices;

    77

    78

typedef struct {

    79

    struct list_head list;

    80

    struct net_device *netdev;

    81

    struct rtnet_device *used_netdev;

    82

    int socket;

    83

    ec_device_t *ecdev;

    84

    uint8_t *rx_buf;

    85

    // struct sockaddr_ll dest_addr;

    86

} ec_gen_device_t;

    87

    88

typedef struct {

    89

    struct list_head list;

    90

    struct rtnet_device *netdev;

    91

    char name[IFNAMSIZ];

    92

    int ifindex;

    93

    uint8_t dev_addr[ETH_ALEN];

    94

} ec_gen_interface_desc_t;

    95

    96

int ec_gen_device_open(ec_gen_device_t *);

    97

int ec_gen_device_stop(ec_gen_device_t *);

    98

int ec_gen_device_start_xmit(ec_gen_device_t *, struct sk_buff *);

    99

void ec_gen_device_poll(ec_gen_device_t *);

   100

   101

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

   102

   103

static int ec_gen_netdev_open(struct net_device *dev)

   104

{

   105

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

   106

    return ec_gen_device_open(gendev);

   107

}

   108

   109

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

   110

   111

static int ec_gen_netdev_stop(struct net_device *dev)

   112

{

   113

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

   114

    return ec_gen_device_stop(gendev);

   115

}

   116

   117

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

   118

   119

static int ec_gen_netdev_start_xmit(

   120

        struct sk_buff *skb,

   121

        struct net_device *dev

   122

        )

   123

{

   124

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

   125

    return ec_gen_device_start_xmit(gendev, skb);

   126

}

   127

   128

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

   129

   130

void ec_gen_poll(struct net_device *dev)

   131

{

   132

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

   133

    ec_gen_device_poll(gendev);

   134

}

   135

   136

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

   137

   138

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

   139

static const struct net_device_ops ec_gen_netdev_ops = {

   140

    .ndo_open       = ec_gen_netdev_open,

   141

    .ndo_stop       = ec_gen_netdev_stop,

   142

    .ndo_start_xmit = ec_gen_netdev_start_xmit,

   143

};

   144

#endif

   145

   146

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

   147

   148

/** Init generic device.

   149

 */

   150

int ec_gen_device_init(

   151

        ec_gen_device_t *dev

   152

        )

   153

{

   154

    ec_gen_device_t **priv;

   155

    char null = 0x00;

   156

   157

    dev->ecdev = NULL;

   158

    dev->socket = -1;

   159

    dev->rx_buf = NULL;

   160

   161

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

   162

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

   163

            NET_NAME_UNKNOWN, ether_setup);

   164

#else

   165

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

   166

#endif

   167

    if (!dev->netdev) {

   168

        return -ENOMEM;

   169

    }

   170

   171

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

   172

    dev->netdev->netdev_ops = &ec_gen_netdev_ops;

   173

#else

   174

    dev->netdev->open = ec_gen_netdev_open;

   175

    dev->netdev->stop = ec_gen_netdev_stop;

   176

    dev->netdev->hard_start_xmit = ec_gen_netdev_start_xmit;

   177

#endif

   178

   179

    priv = netdev_priv(dev->netdev);

   180

    *priv = dev;

   181

   182

    return 0;

   183

}

   184

   185

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

   186

   187

/** Clear generic device.

   188

 */

   189

void ec_gen_device_clear(

   190

        ec_gen_device_t *dev

   191

        )

   192

{

   193

    if (dev->ecdev) {

   194

        ecdev_close(dev->ecdev);

   195

        ecdev_withdraw(dev->ecdev);

   196

    }

   197

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

   198

        rtdm_close(dev->socket);

   199

        dev->socket = -1;

   200

    }

   201

    free_netdev(dev->netdev);

   202

   203

    if (dev->rx_buf) {

   204

        kfree(dev->rx_buf);

   205

    }

   206

}

   207

   208

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

   209

   210

/** Creates a network socket.

   211

 */

   212

int ec_gen_device_create_socket(

   213

        ec_gen_device_t *dev,

   214

        ec_gen_interface_desc_t *desc

   215

        )

   216

{

   217

    int ret;

   218

    struct sockaddr_ll sa;

   219

   220

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

   221

    if (!dev->rx_buf) {

   222

        return -ENOMEM;

   223

    }

   224

   225

    /* create rt-socket */

   226

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

   227

    if (dev->socket < 0) {

   228

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

   229

        return dev->socket;

   230

    }

   231

   232

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

   233

            desc->ifindex, desc->name);

   234

   235

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

   236

    sa.sll_family = AF_PACKET;

   237

    sa.sll_protocol = htons(ETH_P_ETHERCAT);

   238

    sa.sll_ifindex = desc->ifindex;

   239

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

   240

                      sizeof(struct sockaddr_ll));

   241

    if (ret < 0) {

   242

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

   243

        rtdm_close(dev->socket);

   244

        dev->socket = -1;

   245

        return ret;

   246

    }

   247

   248

    return 0;

   249

}

   250

   251

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

   252

   253

/** Offer generic device to master.

   254

 */

   255

int ec_gen_device_offer(

   256

        ec_gen_device_t *dev,

   257

        ec_gen_interface_desc_t *desc

   258

        )

   259

{

   260

    int ret = 0;

   261

   262

    dev->used_netdev = desc->netdev;

   263

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

   264

   265

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

   266

    if (dev->ecdev) {

   267

        if (ec_gen_device_create_socket(dev, desc)) {

   268

            ecdev_withdraw(dev->ecdev);

   269

            dev->ecdev = NULL;

   270

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

   271

            ecdev_withdraw(dev->ecdev);

   272

            dev->ecdev = NULL;

   273

        } else {

   274

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

   275

            ret = 1;

   276

        }

   277

    }

   278

   279

    return ret;

   280

}

   281

   282

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

   283

   284

/** Open the device.

   285

 */

   286

int ec_gen_device_open(

   287

        ec_gen_device_t *dev

   288

        )

   289

{

   290

    return 0;

   291

}

   292

   293

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

   294

   295

/** Stop the device.

   296

 */

   297

int ec_gen_device_stop(

   298

        ec_gen_device_t *dev

   299

        )

   300

{

   301

    return 0;

   302

}

   303

   304

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

   305

   306

// delegate to some rtdm thread when called from nrt context

   307

struct sendmsg_params {

   308

    int socket;

   309

    struct user_msghdr *msg;

   310

};

   311

   312

static int sendmsg_handler(struct rt_proc_call *call)

   313

{

   314

    struct sendmsg_params *params;

   315

   316

    params = rtpc_get_priv(call, struct sendmsg_params);

   317

   318

    return rtdm_sendmsg(params->socket, params->msg, 0);

   319

}

   320

   321

static ssize_t

   322

nrt_rtdm_sendmsg(int socket, struct user_msghdr *msg)

   323

{

   324

    int ret;

   325

    struct sendmsg_params params = {socket, msg};

   326

   327

    ret = rtpc_dispatch_call(sendmsg_handler, 0, &params,

   328

                             sizeof(params), NULL, NULL);

   329

    return ret;

   330

}

   331

   332

int ec_gen_device_start_xmit(

   333

        ec_gen_device_t *dev,

   334

        struct sk_buff *skb

   335

        )

   336

{

   337

    struct user_msghdr msg;

   338

    struct iovec iov;

   339

    size_t len = skb->len;

   340

    int ret;

   341

   342

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

   343

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

   344

   345

    iov.iov_base = skb->data;

   346

    iov.iov_len = len;

   347

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

   348

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

   349

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

   350

    msg.msg_iov     = &iov;

   351

    msg.msg_iovlen  = 1;

   352

   353

	if (rtdm_in_rt_context())

   354

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

   355

	else

   356

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

   357

   358

    return ret == len ? NETDEV_TX_OK : NETDEV_TX_BUSY;

   359

}

   360

   361

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

   362

   363

/** Polls the device.

   364

 */

   365

   366

// delegate to some rtdm thread when called from nrt context

   367

struct recvmsg_params {

   368

    int socket;

   369

    struct user_msghdr *msg;

   370

};

   371

   372

static int recvmsg_handler(struct rt_proc_call *call)

   373

{

   374

    struct recvmsg_params *params;

   375

   376

    params = rtpc_get_priv(call, struct recvmsg_params);

   377

   378

    return  rtdm_recvmsg(params->socket, params->msg, MSG_DONTWAIT);

   379

}

   380

   381

static ssize_t

   382

nrt_rtdm_recvmsg(int socket, struct user_msghdr *msg)

   383

{

   384

    int ret;

   385

    struct recvmsg_params params = {socket,msg};

   386

   387

    ret = rtpc_dispatch_call(recvmsg_handler, 0, &params,

   388

                             sizeof(params), NULL, NULL);

   389

    return ret;

   390

}

   391

   392

void ec_gen_device_poll(

   393

        ec_gen_device_t *dev

   394

        )

   395

{

   396

    struct user_msghdr msg;

   397

    struct iovec iov;

   398

    int ret, budget = 128; // FIXME

   399

   400

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

   401

   402

    do {

   403

        iov.iov_base = dev->rx_buf;

   404

        iov.iov_len = EC_GEN_RX_BUF_SIZE;

   405

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

   406

        msg.msg_iov     = &iov;

   407

        msg.msg_iovlen  = 1;

   408

   409

        if (rtdm_in_rt_context())

   410

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

   411

        else

   412

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

   413

   414

        if (ret > 0) {

   415

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

   416

        } else if (ret < 0) {

   417

            break;

   418

        }

   419

        budget--;

   420

    } while (budget);

   421

}

   422

   423

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

   424

   425

/** Offer device.

   426

 */

   427

int offer_device(

   428

        ec_gen_interface_desc_t *desc

   429

        )

   430

{

   431

    ec_gen_device_t *gendev;

   432

    int ret = 0;

   433

   434

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

   435

    if (!gendev) {

   436

        return -ENOMEM;

   437

    }

   438

   439

    ret = ec_gen_device_init(gendev);

   440

    if (ret) {

   441

        kfree(gendev);

   442

        return ret;

   443

    }

   444

   445

    if (ec_gen_device_offer(gendev, desc)) {

   446

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

   447

    } else {

   448

        ec_gen_device_clear(gendev);

   449

        kfree(gendev);

   450

    }

   451

   452

    return ret;

   453

}

   454

   455

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

   456

   457

/** Clear devices.

   458

 */

   459

void clear_devices(void)

   460

{

   461

    ec_gen_device_t *gendev, *next;

   462

   463

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

   464

        list_del(&gendev->list);

   465

        ec_gen_device_clear(gendev);

   466

        kfree(gendev);

   467

    }

   468

}

   469

   470

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

   471

   472

/** Module initialization.

   473

 *

   474

 * Initializes \a master_count masters.

   475

 * \return 0 on success, else < 0

   476

 */

   477

int __init ec_gen_init_module(void)

   478

{

   479

    int ret = 0;

   480

    int devices = 0;

   481

    int i;

   482

    struct list_head descs;

   483

    struct rtnet_device *rtdev;

   484

    ec_gen_interface_desc_t *desc, *next;

   485

   486

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

   487

            EC_MASTER_VERSION);

   488

   489

    INIT_LIST_HEAD(&generic_devices);

   490

    INIT_LIST_HEAD(&descs);

   491

   492

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

   493

   494

        rtdev = rtdev_get_by_index(i);

   495

        if (rtdev != NULL) {

   496

            mutex_lock(&rtdev->nrt_lock);

   497

   498

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

   499

            //    mutex_unlock(&rtdev->nrt_lock);

   500

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

   501

            //    rtdev_dereference(rtdev);

   502

            //    continue;

   503

            //}

   504

   505

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

   506

            if (!desc) {

   507

                ret = -ENOMEM;

   508

                goto out_err;

   509

            }

   510

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

   511

            desc->netdev = rtdev;

   512

            desc->ifindex = rtdev->ifindex;

   513

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

   514

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

   515

              mutex_unlock(&rtdev->nrt_lock);

   516

   517

            devices++;

   518

        }

   519

    }

   520

   521

    if (devices == 0) {

   522

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

   523

        ret = -ENODEV;

   524

        goto out_err;