//------------------------------------------------------------------------------
tOplkError veth_init(const UINT8 aSrcMac_p[6])
{
#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 17, 0))
// allocate net device structure with priv pointing to stats structure
pVEthNetDevice_g = alloc_netdev(sizeof(struct net_device_stats), PLK_VETH_NAME,
ether_setup);
#else
// allocate net device structure with priv pointing to stats structure
pVEthNetDevice_g = alloc_netdev(sizeof(struct net_device_stats), PLK_VETH_NAME,
NET_NAME_UNKNOWN, ether_setup);
#endif
if (pVEthNetDevice_g == NULL)
return kErrorNoResource;
pVEthNetDevice_g->netdev_ops = &oplk_netdev_ops;
pVEthNetDevice_g->watchdog_timeo = VETH_TX_TIMEOUT;
pVEthNetDevice_g->destructor = free_netdev;
// copy own MAC address to net device structure
OPLK_MEMCPY(pVEthNetDevice_g->dev_addr, aSrcMac_p, 6);
//register VEth to the network subsystem
if (register_netdev(pVEthNetDevice_g))
DEBUG_LVL_VETH_TRACE("veth_init: Could not register VEth...\n");
else
DEBUG_LVL_VETH_TRACE("veth_init: Register VEth successful...\n");
return kErrorOk;
}
//------------------------------------------------------------------------------
static int veth_xmit(struct sk_buff* pSkb_p, struct net_device* pNetDevice_p)
{
tOplkError ret = kErrorOk;
tFrameInfo frameInfo;
//transmit function
struct net_device_stats* pStats = netdev_priv(pNetDevice_p);
//save timestemp
pNetDevice_p->trans_start = jiffies;
frameInfo.pFrame = (tPlkFrame*)pSkb_p->data;
frameInfo.frameSize = pSkb_p->len;
//call send fkt on DLL
ret = dllkcal_sendAsyncFrame(&frameInfo, kDllAsyncReqPrioGeneric);
if (ret != kErrorOk)
{
DEBUG_LVL_VETH_TRACE("veth_xmit: dllkcal_sendAsyncFrame returned 0x%02X\n", ret);
netif_stop_queue(pNetDevice_p);
goto Exit;
}
else
{
DEBUG_LVL_VETH_TRACE("veth_xmit: frame passed to DLL\n");
dev_kfree_skb(pSkb_p);
//set stats for the device
pStats->tx_packets++;
pStats->tx_bytes += frameInfo.frameSize;
}
Exit:
return 0;
}
//------------------------------------------------------------------------------
static void getMacAdrs(UINT8* pMac_p)
{
struct ifreq ifr;
int sock;
sock = socket(AF_INET, SOCK_DGRAM, 0);
if (sock < 0)
{
DEBUG_LVL_VETH_TRACE("%s: Cannot open udp socket for MAC reading: %s\n",
__func__,
strerror(errno));
return;
}
OPLK_MEMSET(&ifr, 0, sizeof(struct ifreq));
strncpy(ifr.ifr_name, "plk", IFNAMSIZ);
if (ioctl(sock, SIOCGIFHWADDR, &ifr) < 0)
{
DEBUG_LVL_VETH_TRACE("Cannot get MAC address: '%s' (%d)\n", strerror(errno), errno);
}
DEBUG_LVL_VETH_TRACE("Get Mac addr %02x:%02x:%02x:%02x:%02x:%02x\n",
ifr.ifr_hwaddr.sa_data[0],
ifr.ifr_hwaddr.sa_data[1],
ifr.ifr_hwaddr.sa_data[2],
ifr.ifr_hwaddr.sa_data[3],
ifr.ifr_hwaddr.sa_data[4],
ifr.ifr_hwaddr.sa_data[5]);
close(sock);
OPLK_MEMCPY(pMac_p, &ifr.ifr_hwaddr.sa_data[0], ETH_ALEN);
}
//------------------------------------------------------------------------------
static int veth_close(struct net_device* pNetDevice_p)
{
DEBUG_LVL_VETH_TRACE("veth_close\n");
dllk_deregAsyncHandler(veth_receiveFrame);
netif_stop_queue(pNetDevice_p); //stop the interface queue for the network subsystem
return 0;
}
//------------------------------------------------------------------------------
static void vethTxTimeout(struct net_device* pNetDevice_p)
{
DEBUG_LVL_VETH_TRACE("%s()\n", __func__);
// $$$ d.k.: move to extra function, which is called by DLL when new space is available in TxFifo
if (netif_queue_stopped(pNetDevice_p))
netif_wake_queue(pNetDevice_p);
}
//------------------------------------------------------------------------------
static int vethStop(struct net_device* pNetDevice_p)
{
DEBUG_LVL_VETH_TRACE("%s()\n", __func__);
dllk_deregAsyncHandler(receiveFrameCb);
netif_stop_queue(pNetDevice_p); //stop the interface queue for the network subsystem
return 0;
}
//------------------------------------------------------------------------------
static tOplkError receiveFrameCb(tFrameInfo* pFrameInfo_p,
tEdrvReleaseRxBuffer* pReleaseRxBuffer_p)
{
tOplkError ret = kErrorOk;
struct net_device* pNetDevice = pVEthNetDevice_g;
struct net_device_stats* pStats = netdev_priv(pNetDevice);
struct sk_buff* pSkb;
DEBUG_LVL_VETH_TRACE("%s(): FrameSize=%u\n", __func__, pFrameInfo_p->frameSize);
if ((pSkb = dev_alloc_skb(pFrameInfo_p->frameSize + 2)) == NULL)
{
pStats->rx_dropped++;
goto Exit;
}
pSkb->dev = pNetDevice;
skb_reserve(pSkb, 2);
OPLK_MEMCPY((void*)skb_put(pSkb, pFrameInfo_p->frameSize), pFrameInfo_p->frame.pBuffer, pFrameInfo_p->frameSize);
pSkb->protocol = eth_type_trans(pSkb, pNetDevice);
pSkb->ip_summed = CHECKSUM_UNNECESSARY;
netif_rx(pSkb); // call netif_rx with skb
DEBUG_LVL_VETH_TRACE("%s(): SrcMAC: %02X:%02X:%02x:%02X:%02X:%02x\n",
__func__,
pFrameInfo_p->frame.pBuffer->aSrcMac[0],
pFrameInfo_p->frame.pBuffer->aSrcMac[1],
pFrameInfo_p->frame.pBuffer->aSrcMac[2],
pFrameInfo_p->frame.pBuffer->aSrcMac[3],
pFrameInfo_p->frame.pBuffer->aSrcMac[4],
pFrameInfo_p->frame.pBuffer->aSrcMac[5]);
// update receive statistics
pStats->rx_packets++;
pStats->rx_bytes += pFrameInfo_p->frameSize;
Exit:
*pReleaseRxBuffer_p = kEdrvReleaseRxBufferImmediately;
return ret;
}
//------------------------------------------------------------------------------
tOplkError veth_init(const UINT8 aSrcMac_p[6])
{
tOplkError ret;
struct ifreq ifr;
int err;
if ((vethInstance_l.fd = open(TUN_DEV_NAME, O_RDWR)) < 0)
{
DEBUG_LVL_VETH_TRACE("Error opening %s\n", TUN_DEV_NAME);
return kErrorNoFreeInstance;
}
OPLK_MEMSET(&ifr, 0, sizeof(ifr));
ifr.ifr_flags = IFF_TAP | IFF_NO_PI;
strncpy(ifr.ifr_name, PLK_VETH_NAME, IFNAMSIZ);
if ((err = ioctl(vethInstance_l.fd, TUNSETIFF, (void*)&ifr)) < 0)
{
DEBUG_LVL_VETH_TRACE("Error setting TUN IFF options\n");
close(vethInstance_l.fd);
return err;
}
// save MAC address of TAP device and Ethernet device to be able to
// exchange them
OPLK_MEMCPY(vethInstance_l.macAdrs, aSrcMac_p, 6);
getMacAdrs(vethInstance_l.tapMacAdrs);
// start tap receive thread
vethInstance_l.fStop = FALSE;
if (pthread_create(&vethInstance_l.threadHandle, NULL, vethRecvThread, (void*)&vethInstance_l) != 0)
return kErrorNoFreeInstance;
#if (defined(__GLIBC__) && (__GLIBC__ >= 2) && (__GLIBC_MINOR__ >= 12))
pthread_setname_np(vethInstance_l.threadHandle, "oplk-veth");
#endif
// register callback function in DLL
ret = dllk_regAsyncHandler(receiveFrameCb);
return ret;
}
//------------------------------------------------------------------------------
static int vethStartXmit(struct sk_buff* pSkb_p, struct net_device* pNetDevice_p)
{
tOplkError ret;
tFrameInfo frameInfo;
//transmit function
struct net_device_stats* pStats = netdev_priv(pNetDevice_p);
//save time stamp
#if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 7, 0))
pNetDevice_p->trans_start = jiffies;
#else
netif_trans_update(pNetDevice_p);
#endif
frameInfo.frame.pBuffer = (tPlkFrame*)pSkb_p->data;
frameInfo.frameSize = pSkb_p->len;
//call send fkt on DLL
ret = dllkcal_sendAsyncFrame(&frameInfo, kDllAsyncReqPrioGeneric);
if (ret != kErrorOk)
{
DEBUG_LVL_VETH_TRACE("%s(): dllkcal_sendAsyncFrame returned 0x%04X\n", __func__, ret);
netif_stop_queue(pNetDevice_p);
goto Exit;
}
else
{
DEBUG_LVL_VETH_TRACE("%s(): frame passed to DLL\n", __func__);
dev_kfree_skb(pSkb_p);
//set stats for the device
pStats->tx_packets++;
pStats->tx_bytes += frameInfo.frameSize;
}
Exit:
return 0;
}
//------------------------------------------------------------------------------
static int veth_open(struct net_device* pNetDevice_p)
{
tOplkError ret = kErrorOk;
//open the device
//start the interface queue for the network subsystem
netif_start_queue(pNetDevice_p);
// register callback function in DLL
ret = dllk_regAsyncHandler(veth_receiveFrame);
DEBUG_LVL_VETH_TRACE("veth_open: dllk_regAsyncHandler returned 0x%02X\n", ret);
return 0;
}
//------------------------------------------------------------------------------
static tOplkError receiveFrameCb(tFrameInfo* pFrameInfo_p,
tEdrvReleaseRxBuffer* pReleaseRxBuffer_p)
{
UINT nwrite;
// replace the MAC address of the POWERLINK Ethernet interface with virtual
// Ethernet MAC address before forwarding it into the virtual Ethernet interface
if (OPLK_MEMCMP(pFrameInfo_p->frame.pBuffer->aDstMac, vethInstance_l.macAdrs, ETH_ALEN) == 0)
{
OPLK_MEMCPY(pFrameInfo_p->frame.pBuffer->aDstMac, vethInstance_l.tapMacAdrs, ETH_ALEN);
}
nwrite = write(vethInstance_l.fd, pFrameInfo_p->frame.pBuffer, pFrameInfo_p->frameSize);
if (nwrite != pFrameInfo_p->frameSize)
{
DEBUG_LVL_VETH_TRACE("Error writing data to virtual Ethernet interface!\n");
}
*pReleaseRxBuffer_p = kEdrvReleaseRxBufferImmediately;
return kErrorOk;
}
//------------------------------------------------------------------------------
static struct net_device_stats* veth_getStats(struct net_device* pNetDevice_p)
{
DEBUG_LVL_VETH_TRACE("veth_getStats\n");
return netdev_priv(pNetDevice_p);
}
//------------------------------------------------------------------------------
static struct net_device_stats* vethGetStats(struct net_device* pNetDevice_p)
{
DEBUG_LVL_VETH_TRACE("%s()\n", __func__);
return netdev_priv(pNetDevice_p);
}
//------------------------------------------------------------------------------
static void* vethRecvThread(void* pArg_p)
{
UINT8 buffer[ETH_DATA_LEN];
UINT nread;
tFrameInfo frameInfo;
tOplkError ret = kErrorOk;
tVethInstance* pInstance = (tVethInstance*)pArg_p;
fd_set readFds;
int result;
struct timeval timeout;
while (!pInstance->fStop)
{
timeout.tv_sec = 0;
timeout.tv_usec = 400000;
FD_ZERO(&readFds);
FD_SET(pInstance->fd, &readFds);
result = select(pInstance->fd + 1, &readFds, NULL, NULL, &timeout);
switch (result)
{
case 0: // timeout
//DEBUG_LVL_VETH_TRACE("select timeout\n");
break;
case -1: // error
DEBUG_LVL_VETH_TRACE("select error: %s\n", strerror(errno));
break;
default: // data from tun/tap ready for read
nread = read(pInstance->fd, buffer, ETH_DATA_LEN);
if (nread > 0)
{
DEBUG_LVL_VETH_TRACE("VETH: Read %d bytes from the tap interface\n", nread);
DEBUG_LVL_VETH_TRACE("SRC MAC: %02X:%02X:%02x:%02X:%02X:%02x\n",
buffer[6],
buffer[7],
buffer[8],
buffer[9],
buffer[10],
buffer[11]);
DEBUG_LVL_VETH_TRACE("DST MAC: %02X:%02X:%02x:%02X:%02X:%02x\n",
buffer[0],
buffer[1],
buffer[2],
buffer[3],
buffer[4],
buffer[5]);
// replace src MAC address with MAC address of virtual Ethernet interface
OPLK_MEMCPY(&buffer[6], pInstance->macAdrs, ETH_ALEN);
frameInfo.frame.pBuffer = (tPlkFrame*)buffer;
frameInfo.frameSize = nread;
ret = dllkcal_sendAsyncFrame(&frameInfo, kDllAsyncReqPrioGeneric);
if (ret != kErrorOk)
{
DEBUG_LVL_VETH_TRACE("%s(): dllkcal_sendAsyncFrame returned 0x%04X\n", __func__, ret);
}
}
break;
}
}
pthread_exit(NULL);
return NULL;
}