/******************************************************************************
*
* $Id$
*
* Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
* Copyright (C) 2014-2018 Edouard Tisserant
*
* This file is part of the IgH EtherCAT Master.
*
* The IgH EtherCAT Master is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License version 2, as
* published by the Free Software Foundation.
*
* The IgH EtherCAT Master is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with the IgH EtherCAT Master; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
* ---
*
* The license mentioned above concerns the source code only. Using the
* EtherCAT technology and brand is only permitted in compliance with the
* industrial property and similar rights of Beckhoff Automation GmbH.
*
*****************************************************************************/
/** \file
* EtherCAT generic Xenomai's RTDM RAW Ethernet socket device module.
* Heavily based on generic.c. Should be merged in a single file with #ifdefs
*/
/*****************************************************************************/
#include <linux/module.h>
#include <linux/device.h>
#include <linux/err.h>
#include <linux/version.h>
#include <linux/if_arp.h> /* ARPHRD_ETHER */
#include <linux/etherdevice.h>
#include <rtdm/rtdm.h>
// for rtnetif_carrier_ok and rtpc_dispatch_call
// This needs -I@XENOMAI_DIR@/kernel/drivers/net/stack/include in Kbuild.in
#include <rtnet_port.h>
#include <rtnet_rtpc.h>
#include "../globals.h"
#include "ecdev.h"
#define PFX "ec_rtdmnet: "
#define ETH_P_ETHERCAT 0x88A4
#define EC_GEN_RX_BUF_SIZE 1600
/*****************************************************************************/
int __init ec_gen_init_module(void);
void __exit ec_gen_cleanup_module(void);
/*****************************************************************************/
/** \cond */
MODULE_AUTHOR("Edouard Tisserant <edouard.tisserant@gmail.com>");
MODULE_DESCRIPTION("EtherCAT generic Xenomai's RTDM RAW Ethernet socket device module.");
MODULE_LICENSE("GPL");
MODULE_VERSION(EC_MASTER_VERSION);
/** \endcond */
struct list_head generic_devices;
typedef struct {
struct list_head list;
struct net_device *netdev;
struct rtnet_device *used_netdev;
int socket;
ec_device_t *ecdev;
uint8_t *rx_buf;
// struct sockaddr_ll dest_addr;
} ec_gen_device_t;
typedef struct {
struct list_head list;
struct rtnet_device *netdev;
char name[IFNAMSIZ];
int ifindex;
uint8_t dev_addr[ETH_ALEN];
} ec_gen_interface_desc_t;
int ec_gen_device_open(ec_gen_device_t *);
int ec_gen_device_stop(ec_gen_device_t *);
int ec_gen_device_start_xmit(ec_gen_device_t *, struct sk_buff *);
void ec_gen_device_poll(ec_gen_device_t *);
/*****************************************************************************/
static int ec_gen_netdev_open(struct net_device *dev)
{
ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev));
return ec_gen_device_open(gendev);
}
/*****************************************************************************/
static int ec_gen_netdev_stop(struct net_device *dev)
{
ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev));
return ec_gen_device_stop(gendev);
}
/*****************************************************************************/
static int ec_gen_netdev_start_xmit(
struct sk_buff *skb,
struct net_device *dev
)
{
ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev));
return ec_gen_device_start_xmit(gendev, skb);
}
/*****************************************************************************/
void ec_gen_poll(struct net_device *dev)
{
ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev));
ec_gen_device_poll(gendev);
}
/*****************************************************************************/
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29)
static const struct net_device_ops ec_gen_netdev_ops = {
.ndo_open = ec_gen_netdev_open,
.ndo_stop = ec_gen_netdev_stop,
.ndo_start_xmit = ec_gen_netdev_start_xmit,
};
#endif
/*****************************************************************************/
/** Init generic device.
*/
int ec_gen_device_init(
ec_gen_device_t *dev
)
{
ec_gen_device_t **priv;
char null = 0x00;
dev->ecdev = NULL;
dev->socket = -1;
dev->rx_buf = NULL;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 17, 0)
dev->netdev = alloc_netdev(sizeof(ec_gen_device_t *), &null,
NET_NAME_UNKNOWN, ether_setup);
#else
dev->netdev = alloc_netdev(sizeof(ec_gen_device_t *), &null, ether_setup);
#endif
if (!dev->netdev) {
return -ENOMEM;
}
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29)
dev->netdev->netdev_ops = &ec_gen_netdev_ops;
#else
dev->netdev->open = ec_gen_netdev_open;
dev->netdev->stop = ec_gen_netdev_stop;
dev->netdev->hard_start_xmit = ec_gen_netdev_start_xmit;
#endif
priv = netdev_priv(dev->netdev);
*priv = dev;
return 0;
}
/*****************************************************************************/
/** Clear generic device.
*/
void ec_gen_device_clear(
ec_gen_device_t *dev
)
{
if (dev->ecdev) {
ecdev_close(dev->ecdev);
ecdev_withdraw(dev->ecdev);
}
if (!(dev->socket < 0)) {
rtdm_close(dev->socket);
dev->socket = -1;
}
free_netdev(dev->netdev);
if (dev->rx_buf) {
kfree(dev->rx_buf);
}
}
/*****************************************************************************/
/** Creates a network socket.
*/
int ec_gen_device_create_socket(
ec_gen_device_t *dev,
ec_gen_interface_desc_t *desc
)
{
int ret;
struct sockaddr_ll sa;
dev->rx_buf = kmalloc(EC_GEN_RX_BUF_SIZE, GFP_KERNEL);
if (!dev->rx_buf) {
return -ENOMEM;
}
/* create rt-socket */
dev->socket = rtdm_socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ETHERCAT));
if (dev->socket < 0) {
printk(" rtdm_socket() = %d!\n", dev->socket);
return dev->socket;
}
printk(KERN_ERR PFX "Binding socket to interface %i (%s).\n",
desc->ifindex, desc->name);
memset(&sa, 0x00, sizeof(sa));
sa.sll_family = AF_PACKET;
sa.sll_protocol = htons(ETH_P_ETHERCAT);
sa.sll_ifindex = desc->ifindex;
ret = rtdm_bind(dev->socket, (struct sockaddr *)&sa,
sizeof(struct sockaddr_ll));
if (ret < 0) {
printk(" rtdm_bind() = %d!\n", ret);
rtdm_close(dev->socket);
dev->socket = -1;
return ret;
}
return 0;
}
/*****************************************************************************/
/** Offer generic device to master.
*/
int ec_gen_device_offer(
ec_gen_device_t *dev,
ec_gen_interface_desc_t *desc
)
{
int ret = 0;
dev->used_netdev = desc->netdev;
memcpy(dev->netdev->dev_addr, desc->dev_addr, ETH_ALEN);
dev->ecdev = ecdev_offer(dev->netdev, ec_gen_poll, THIS_MODULE);
if (dev->ecdev) {
if (ec_gen_device_create_socket(dev, desc)) {
ecdev_withdraw(dev->ecdev);
dev->ecdev = NULL;
} else if (ecdev_open(dev->ecdev)) {
ecdev_withdraw(dev->ecdev);
dev->ecdev = NULL;
} else {
ecdev_set_link(dev->ecdev, rtnetif_carrier_ok(dev->used_netdev)); // FIXME
ret = 1;
}
}
return ret;
}
/*****************************************************************************/
/** Open the device.
*/
int ec_gen_device_open(
ec_gen_device_t *dev
)
{
return 0;
}
/*****************************************************************************/
/** Stop the device.
*/
int ec_gen_device_stop(
ec_gen_device_t *dev
)
{
return 0;
}
/*****************************************************************************/
// delegate to some rtdm thread when called from nrt context
struct sendmsg_params {
int socket;
struct user_msghdr *msg;
};
static int sendmsg_handler(struct rt_proc_call *call)
{
struct sendmsg_params *params;
params = rtpc_get_priv(call, struct sendmsg_params);
return rtdm_sendmsg(params->socket, params->msg, 0);
}
static ssize_t
nrt_rtdm_sendmsg(int socket, struct user_msghdr *msg)
{
int ret;
struct sendmsg_params params = {socket, msg};
ret = rtpc_dispatch_call(sendmsg_handler, 0, ¶ms,
sizeof(params), NULL, NULL);
return ret;
}
int ec_gen_device_start_xmit(
ec_gen_device_t *dev,
struct sk_buff *skb
)
{
struct user_msghdr msg;
struct iovec iov;
size_t len = skb->len;
int ret;
ecdev_set_link(dev->ecdev, rtnetif_carrier_ok(dev->used_netdev));
//ecdev_set_link(dev->ecdev, 1); // FIXME
iov.iov_base = skb->data;
iov.iov_len = len;
memset(&msg, 0, sizeof(msg));
// msg.msg_name = &dev->dest_addr;
// msg.msg_namelen = sizeof(dev->dest_addr);
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
if (rtdm_in_rt_context())
ret = nrt_rtdm_sendmsg(dev->socket, &msg);
else
ret = rtdm_sendmsg(dev->socket, &msg, 0);
return ret == len ? NETDEV_TX_OK : NETDEV_TX_BUSY;
}
/*****************************************************************************/
/** Polls the device.
*/
// delegate to some rtdm thread when called from nrt context
struct recvmsg_params {
int socket;
struct user_msghdr *msg;
};
static int recvmsg_handler(struct rt_proc_call *call)
{
struct recvmsg_params *params;
params = rtpc_get_priv(call, struct recvmsg_params);
return rtdm_recvmsg(params->socket, params->msg, MSG_DONTWAIT);
}
static ssize_t
nrt_rtdm_recvmsg(int socket, struct user_msghdr *msg)
{
int ret;
struct recvmsg_params params = {socket,msg};
ret = rtpc_dispatch_call(recvmsg_handler, 0, ¶ms,
sizeof(params), NULL, NULL);
return ret;
}
void ec_gen_device_poll(
ec_gen_device_t *dev
)
{
struct user_msghdr msg;
struct iovec iov;
int ret, budget = 128; // FIXME
ecdev_set_link(dev->ecdev, rtnetif_carrier_ok(dev->used_netdev));
do {
iov.iov_base = dev->rx_buf;
iov.iov_len = EC_GEN_RX_BUF_SIZE;
memset(&msg, 0, sizeof(msg));
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
if (rtdm_in_rt_context())
ret = nrt_rtdm_recvmsg(dev->socket, &msg);
else
ret = rtdm_recvmsg(dev->socket, &msg, MSG_DONTWAIT);
if (ret > 0) {
ecdev_receive(dev->ecdev, dev->rx_buf, ret);
} else if (ret < 0) {
break;
}
budget--;
} while (budget);
}
/*****************************************************************************/
/** Offer device.
*/
int offer_device(
ec_gen_interface_desc_t *desc
)
{
ec_gen_device_t *gendev;
int ret = 0;
gendev = kmalloc(sizeof(ec_gen_device_t), GFP_KERNEL);
if (!gendev) {
return -ENOMEM;
}
ret = ec_gen_device_init(gendev);
if (ret) {
kfree(gendev);
return ret;
}
if (ec_gen_device_offer(gendev, desc)) {
list_add_tail(&gendev->list, &generic_devices);
} else {
ec_gen_device_clear(gendev);
kfree(gendev);
}
return ret;
}
/*****************************************************************************/
/** Clear devices.
*/
void clear_devices(void)
{
ec_gen_device_t *gendev, *next;
list_for_each_entry_safe(gendev, next, &generic_devices, list) {
list_del(&gendev->list);
ec_gen_device_clear(gendev);
kfree(gendev);
}
}
/*****************************************************************************/
/** Module initialization.
*
* Initializes \a master_count masters.
* \return 0 on success, else < 0
*/
int __init ec_gen_init_module(void)
{
int ret = 0;
int devices = 0;
int i;
struct list_head descs;
struct rtnet_device *rtdev;
ec_gen_interface_desc_t *desc, *next;
printk(KERN_INFO PFX "EtherCAT master RTnet Ethernet device module %s\n",
EC_MASTER_VERSION);
INIT_LIST_HEAD(&generic_devices);
INIT_LIST_HEAD(&descs);
for (i = 0; i < MAX_RT_DEVICES; i++) {
rtdev = rtdev_get_by_index(i);
if (rtdev != NULL) {
mutex_lock(&rtdev->nrt_lock);
//if (test_bit(PRIV_FLAG_UP, &rtdev->priv_flags)) {
// mutex_unlock(&rtdev->nrt_lock);
// printk(KERN_ERR PFX "%s busy, skipping device!\n", rtdev->name);
// rtdev_dereference(rtdev);
// continue;
//}
desc = kmalloc(sizeof(ec_gen_interface_desc_t), GFP_ATOMIC);
if (!desc) {
ret = -ENOMEM;
goto out_err;
}
strncpy(desc->name, rtdev->name, IFNAMSIZ);
desc->netdev = rtdev;
desc->ifindex = rtdev->ifindex;
memcpy(desc->dev_addr, rtdev->dev_addr, ETH_ALEN);
list_add_tail(&desc->list, &descs);
mutex_unlock(&rtdev->nrt_lock);
devices++;
}
}
if (devices == 0) {
printk(KERN_ERR PFX "no real-time devices found!\n");
ret = -ENODEV;
goto out_err;
}
list_for_each_entry_safe(desc, next, &descs, list) {
ret = offer_device(desc);
if (ret) {
goto out_err;
}
kfree(desc);
}
return ret;
out_err:
list_for_each_entry_safe(desc, next, &descs, list) {
list_del(&desc->list);
kfree(desc);
}
clear_devices();
return ret;
}
/*****************************************************************************/
/** Module cleanup.
*
* Clears all master instances.
*/
void __exit ec_gen_cleanup_module(void)
{
clear_devices();
printk(KERN_INFO PFX "Unloading.\n");
}
/*****************************************************************************/
/** \cond */
module_init(ec_gen_init_module);
module_exit(ec_gen_cleanup_module);
/** \endcond */
/*****************************************************************************/