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

    struct rtdm_fd *rtdm_fd;

    84

    ec_device_t *ecdev;

    85

    uint8_t *rx_buf;

    86

    // struct sockaddr_ll dest_addr;

    87

} ec_gen_device_t;

    88

    89

typedef struct {

    90

    struct list_head list;

    91

    struct rtnet_device *netdev;

    92

    char name[IFNAMSIZ];

    93

    int ifindex;

    94

    uint8_t dev_addr[ETH_ALEN];

    95

} ec_gen_interface_desc_t;

    96

    97

int ec_gen_device_open(ec_gen_device_t *);

    98

int ec_gen_device_stop(ec_gen_device_t *);

    99

int ec_gen_device_start_xmit(ec_gen_device_t *, struct sk_buff *);

   100

void ec_gen_device_poll(ec_gen_device_t *);

   101

   102

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

   103

   104

static int ec_gen_netdev_open(struct net_device *dev)

   105

{

   106

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

   107

    return ec_gen_device_open(gendev);

   108

}

   109

   110

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

   111

   112

static int ec_gen_netdev_stop(struct net_device *dev)

   113

{

   114

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

   115

    return ec_gen_device_stop(gendev);

   116

}

   117

   118

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

   119

   120

static int ec_gen_netdev_start_xmit(

   121

        struct sk_buff *skb,

   122

        struct net_device *dev

   123

        )

   124

{

   125

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

   126

    return ec_gen_device_start_xmit(gendev, skb);

   127

}

   128

   129

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

   130

   131

void ec_gen_poll(struct net_device *dev)

   132

{

   133

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

   134

    ec_gen_device_poll(gendev);

   135

}

   136

   137

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

   138

   139

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

   140

static const struct net_device_ops ec_gen_netdev_ops = {

   141

    .ndo_open       = ec_gen_netdev_open,

   142

    .ndo_stop       = ec_gen_netdev_stop,

   143

    .ndo_start_xmit = ec_gen_netdev_start_xmit,

   144

};

   145

#endif

   146

   147

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

   148

   149

/** Init generic device.

   150

 */

   151

int ec_gen_device_init(

   152

        ec_gen_device_t *dev

   153

        )

   154

{

   155

    ec_gen_device_t **priv;

   156

    char null = 0x00;

   157

   158

    dev->ecdev = NULL;

   159

    dev->socket = -1;

   160

    dev->rx_buf = NULL;

   161

   162

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

   163

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

   164

            NET_NAME_UNKNOWN, ether_setup);

   165

#else

   166

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

   167

#endif

   168

    if (!dev->netdev) {

   169

        return -ENOMEM;

   170

    }

   171

   172

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

   173

    dev->netdev->netdev_ops = &ec_gen_netdev_ops;

   174

#else

   175

    dev->netdev->open = ec_gen_netdev_open;

   176

    dev->netdev->stop = ec_gen_netdev_stop;

   177

    dev->netdev->hard_start_xmit = ec_gen_netdev_start_xmit;

   178

#endif

   179

   180

    priv = netdev_priv(dev->netdev);

   181

    *priv = dev;

   182

   183

    return 0;

   184

}

   185

   186

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

   187

   188

/** Clear generic device.

   189

 */

   190

void ec_gen_device_clear(

   191

        ec_gen_device_t *dev

   192

        )

   193

{

   194

    if (dev->ecdev) {

   195

        ecdev_close(dev->ecdev);

   196

        ecdev_withdraw(dev->ecdev);

   197

    }

   198

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

   199

        rtdm_fd_put(dev->rtdm_fd);

   200

        rtdm_close(dev->socket);

   201

        dev->socket = -1;

   202

    }

   203

    free_netdev(dev->netdev);

   204

   205

    if (dev->rx_buf) {

   206

        kfree(dev->rx_buf);

   207

    }

   208

}

   209

   210

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

   211

   212

/** Creates a network socket.

   213

 */

   214

int ec_gen_device_create_socket(

   215

        ec_gen_device_t *dev,

   216

        ec_gen_interface_desc_t *desc

   217

        )

   218

{

   219

    int ret;

   220

    struct sockaddr_ll sa;

   221

   222

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

   223

    if (!dev->rx_buf) {

   224

        return -ENOMEM;

   225

    }

   226

   227

    /* create rt-socket */

   228

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

   229

    if (dev->socket < 0) {

   230

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

   231

        return dev->socket;

   232

    }

   233

   234

    /* HACK :

   235

       When this is called, process is 'kernel', i.e. 

   236

         cobalt_ppd_get(0) == &cobalt_kernel_ppd

   237

   238

       This makes a problem later when recvmsg or sendmsg is indirectly called

   239

       from application's cobalt thread through RTDM IOCTL. From such thread, 

   240

       RTDM can't resolv socket's 'ufd' that rtdm_socket just returned.

   241

   242

       To keep a usable file descriptor for that socket, even when calling 

   243

       from cobalt thread (i.e. if (cobalt_ppd_get(0) != &cobalt_kernel_ppd))

   244

       we resolve it here in advance */

   245

    dev->rtdm_fd = rtdm_fd_get(dev->socket,0);

   247

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

   250

    }

   251

   252

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

   253

            desc->ifindex, desc->name);

   254

   255

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

   256

    sa.sll_family = AF_PACKET;

   257

    sa.sll_protocol = htons(ETH_P_ETHERCAT);

   258

    sa.sll_ifindex = desc->ifindex;

   259

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

   260

                      sizeof(struct sockaddr_ll));

   261

    if (ret < 0) {

   262

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

   264

    }

   265

   266

    return 0;

   267

   269

    rtdm_fd_put(dev->rtdm_fd);

   271

    rtdm_close(dev->socket);

   272

    dev->socket = -1;

   273

    return ret;

   274

   275

}

   276

   277

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

   278

   279

/** Offer generic device to master.

   280

 */

   281

int ec_gen_device_offer(

   282

        ec_gen_device_t *dev,

   283

        ec_gen_interface_desc_t *desc

   284

        )

   285

{

   286

    int ret = 0;

   287

   288

    dev->used_netdev = desc->netdev;

   289

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

   290

   291

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

   292

    if (dev->ecdev) {

   293

        if (ec_gen_device_create_socket(dev, desc)) {

   294

            ecdev_withdraw(dev->ecdev);

   295

            dev->ecdev = NULL;

   296

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

   297

            ecdev_withdraw(dev->ecdev);

   298

            dev->ecdev = NULL;

   299

        } else {

   300

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

   301

            ret = 1;

   302

        }

   303

    }

   304

   305

    return ret;

   306

}

   307

   308

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

   309

   310

/** Open the device.

   311

 */

   312

int ec_gen_device_open(

   313

        ec_gen_device_t *dev

   314

        )

   315

{

   316

    return 0;

   317

}

   318

   319

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

   320

   321

/** Stop the device.

   322

 */

   323

int ec_gen_device_stop(

   324

        ec_gen_device_t *dev

   325

        )

   326

{

   327

    return 0;

   328

}

   329

   330

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

   331

   332

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

   333

struct sendmsg_params {

   334

    int socket;

   335

    struct user_msghdr *msg;

   336

};

   337

   338

static int sendmsg_handler(struct rt_proc_call *call)

   339

{

   340

    struct sendmsg_params *params;

   341

   342

    params = rtpc_get_priv(call, struct sendmsg_params);

   343

   344

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

   345

}

   346

   347

static ssize_t

   348

nrt_rtdm_sendmsg(int socket, struct user_msghdr *msg)

   349

{

   350

    int ret;

   351

    struct sendmsg_params params = {socket, msg};

   352

   353

    ret = rtpc_dispatch_call(sendmsg_handler, 0, &params,

   354

                             sizeof(params), NULL, NULL);

   355

    return ret;

   356

}

   357

   358

int ec_gen_device_start_xmit(

   359

        ec_gen_device_t *dev,

   360

        struct sk_buff *skb

   361

        )

   362

{

   363

    struct user_msghdr msg;

   364

    struct iovec iov;

   365

    size_t len = skb->len;

   366

    int ret;

   367

   368

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

   369

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

   370

   371

    iov.iov_base = skb->data;

   372

    iov.iov_len = len;

   373

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

   374

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

   375

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

   376

    msg.msg_iov     = &iov;

   377

    msg.msg_iovlen  = 1;

   378

   380

        /* HACK : call fd ops directly as in rtdm's fd.c */

   381

        ret = dev->rtdm_fd->ops->sendmsg_rt(dev->rtdm_fd, &msg, 0);

   383

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

   384

   385

    return ret == len ? NETDEV_TX_OK : NETDEV_TX_BUSY;

   386

}

   387

   388

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

   389

   390

/** Polls the device.

   391

 */

   392

   393

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

   394

struct recvmsg_params {

   395

    int socket;

   396

    struct user_msghdr *msg;

   397

};

   398

   399

static int recvmsg_handler(struct rt_proc_call *call)

   400

{

   401

    struct recvmsg_params *params;

   402

   403

    params = rtpc_get_priv(call, struct recvmsg_params);

   404

   405

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

   406

}

   407

   408

static ssize_t

   409

nrt_rtdm_recvmsg(int socket, struct user_msghdr *msg)

   410

{

   411

    int ret;

   412

    struct recvmsg_params params = {socket,msg};

   413

   414

    ret = rtpc_dispatch_call(recvmsg_handler, 0, &params,

   415

                             sizeof(params), NULL, NULL);

   416

    return ret;

   417

}

   418

   419

void ec_gen_device_poll(

   420

        ec_gen_device_t *dev

   421

        )

   422

{

   423

    struct user_msghdr msg;

   424

    struct iovec iov;

   425

    int ret, budget = 128; // FIXME

   426

   427

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

   428

   429

    do {

   430

        iov.iov_base = dev->rx_buf;

   431

        iov.iov_len = EC_GEN_RX_BUF_SIZE;

   432

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

   433

        msg.msg_iov     = &iov;

   434

        msg.msg_iovlen  = 1;

   435

   436

        if (rtdm_in_rt_context()){

   437

            /* HACK : call fd ops directly as in rtdm's fd.c */

   438

            ret = dev->rtdm_fd->ops->recvmsg_rt(dev->rtdm_fd, &msg, MSG_DONTWAIT);

   439

        }else

   440

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

   441

   442

        if (ret > 0) {

   443

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

   444

        } else if (ret < 0) {

   445

            break;

   446

        }

   447

        budget--;

   448

    } while (budget);

   449

}

   450

   451

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

   452

   453

/** Offer device.

   454

 */

   455

int offer_device(

   456

        ec_gen_interface_desc_t *desc

   457

        )

   458

{

   459

    ec_gen_device_t *gendev;

   460

    int ret = 0;

   461

   462

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

   463

    if (!gendev) {

   464

        return -ENOMEM;

   465

    }

   466

   467

    ret = ec_gen_device_init(gendev);

   468

    if (ret) {

   469

        kfree(gendev);

   470

        return ret;

   471

    }

   472

   473

    if (ec_gen_device_offer(gendev, desc)) {

   474

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

   475

    } else {

   476

        ec_gen_device_clear(gendev);

   477

        kfree(gendev);

   478

    }

   479

   480

    return ret;

   481

}

   482

   483

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

   484

   485

/** Clear devices.

   486

 */

   487

void clear_devices(void)

   488

{

   489

    ec_gen_device_t *gendev, *next;

   490

   491

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

   492

        list_del(&gendev->list);

   493

        ec_gen_device_clear(gendev);

   494

        kfree(gendev);

   495

    }

   496

}

   497

   498

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

   499

   500

/** Module initialization.

   501

 *

   502

 * Initializes \a master_count masters.

   503

 * \return 0 on success, else < 0

   504

 */

   505

int __init ec_gen_init_module(void)

   506

{

   507

    int ret = 0;

   508

    int devices = 0;

   509

    int i;

   510

    struct list_head descs;

   511

    struct rtnet_device *rtdev;

   512

    ec_gen_interface_desc_t *desc, *next;

   513

   514

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

   515

            EC_MASTER_VERSION);

   516

   517

    INIT_LIST_HEAD(&generic_devices);

   518

    INIT_LIST_HEAD(&descs);

   519

   520

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

   521