ec_init
SOEM主站一切开始的地方始于ec_init, 它是EtherCAT主站初始化的入口。初始化SOEM 主站,并绑定到socket到ifname。
/** Initialise lib in single NIC mode* @param[in] ifname = Dev name, f.e. "eth0"* @return >0 if OK* @see ecx_init*/
int ec_init(const char * ifname)
{return ecx_init(&ecx_context, ifname);
}
- ifname 名词解释
ifname 是网络接口名称(interface name)的缩写。在 SOEM 的 slaveinfo 程序中,ifname 用于指定要绑定和通信的本地网络接口,比如 “eth0”、“enp2s0”、“eno1” 等。 - NIC名词解释
NIC(Network Interface Card,网络接口卡)是计算机或其他设备用于连接计算机网络的硬件设备。它通常也被称为“网卡”。NIC 负责在设备和网络之间进行数据的收发和转换,实现设备与局域网(LAN)、广域网(WAN)等网络的物理连接。常见的 NIC 类型包括以太网卡(Ethernet Card)、无线网卡(Wi-Fi Card)等。在 EtherCAT 等工业通信场景中,NIC 就是主站或从站与网络通信的物理接口。
ecx_init
这段代码定义了一个名为 ecx_init 的函数,用于初始化 EtherCAT 通信上下文。函数有两个参数:context 是指向 ecx_contextt 结构体的指针,表示 EtherCAT 的上下文环境;ifname 是一个字符串,表示要使用的网络接口名称(如 “eth0”)。在函数体内,ecx_init 调用了 ecx_setupnic 函数,并传递了 context->port、ifname 和 FALSE 作为参数。这里,context->port 是ecx_portt类型的指针,指向了ethercatmain.c中的ecx_port,ifname 指定了网络接口名称,FALSE 通常表示不启用冗余模式。ecx_setupnic 负责实际的网络接口初始化工作。
/** Initialise lib in single NIC mode* @param[in] context = context struct* @param[in] ifname = Dev name, f.e. "eth0"* @return >0 if OK*/
int ecx_init(ecx_contextt *context, const char * ifname)
{return ecx_setupnic(context->port, ifname, FALSE);
}
ecx_setupnic 的实现因平台而异,这部分实现位于oshw层,支持多种不同的平台,譬如linux, rtk, vxworks, macosx,win32等等。
ecx_conext定义
从ecx_conext变量定义可以看出,其内容几乎都是指向具体业务逻辑的指针,在初始化时根据实际情况进行初始化。
ecx_contextt ecx_context = {&ecx_port, // .port =&ec_slave[0], // .slavelist =&ec_slavecount, // .slavecount =EC_MAXSLAVE, // .maxslave =&ec_group[0], // .grouplist =EC_MAXGROUP, // .maxgroup =&ec_esibuf[0], // .esibuf =&ec_esimap[0], // .esimap =0, // .esislave =&ec_elist, // .elist =&ec_idxstack, // .idxstack =&EcatError, // .ecaterror =&ec_DCtime, // .DCtime =&ec_SMcommtype[0], // .SMcommtype =&ec_PDOassign[0], // .PDOassign =&ec_PDOdesc[0], // .PDOdesc =&ec_SM, // .eepSM =&ec_FMMU, // .eepFMMU =NULL, // .FOEhook()NULL, // .EOEhook()0, // .manualstatechangeNULL, // .userdata
};
ecx_setupnic(linux)
这段代码实现了 EtherCAT 主站库中用于将网络接口卡(NIC)与原始套接字(RAW socket)连接的基础设置函数 ecx_setupnic。其主要作用是为 EtherCAT 通信初始化网络端口,支持主/冗余两种模式。secondary为FALSE,这里暂不分析冗余模式。在非冗余模式中,初始化互斥锁(用于多线程安全),重置端口状态和缓冲区指针,并同样清空接收缓冲区状态。
接下来,函数通过 socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ECAT)) 创建一个原始套接字,用于直接收发 EtherCAT 协议的数据包。如果套接字创建失败,函数直接返回 0 表示失败。
随后,代码设置了套接字的接收和发送超时时间,并启用 SO_DONTROUTE 选项,确保数据包不经过路由器。通过 ioctl 系列调用,函数获取并设置指定网卡(ifname)的接口索引和标志位,将网卡设置为混杂模式和广播模式,以便能够收发所有经过的数据包。
然后,函数将套接字绑定到指定的网络接口和 EtherCAT 协议。最后,为所有发送缓冲区预先设置以太网头部,并将接收缓冲区状态初始化为“空”。如果所有操作都成功,函数返回 1,否则返回 0。
/** Basic setup to connect NIC to socket.* @param[in] port = port context struct* @param[in] ifname = Name of NIC device, f.e. "eth0"* @param[in] secondary = if >0 then use secondary stack instead of primary* @return >0 if succeeded*/
int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary)
{int i;int r, rval, ifindex;struct timeval timeout;struct ifreq ifr;struct sockaddr_ll sll;int *psock;pthread_mutexattr_t mutexattr;rval = 0;if (secondary){/* secondary port struct available? */if (port->redport){/* when using secondary socket it is automatically a redundant setup */psock = &(port->redport->sockhandle);*psock = -1;port->redstate = ECT_RED_DOUBLE;port->redport->stack.sock = &(port->redport->sockhandle);port->redport->stack.txbuf = &(port->txbuf);port->redport->stack.txbuflength = &(port->txbuflength);port->redport->stack.tempbuf = &(port->redport->tempinbuf);port->redport->stack.rxbuf = &(port->redport->rxbuf);port->redport->stack.rxbufstat = &(port->redport->rxbufstat);port->redport->stack.rxsa = &(port->redport->rxsa);ecx_clear_rxbufstat(&(port->redport->rxbufstat[0]));}else{/* fail */return 0;}}else{pthread_mutexattr_init(&mutexattr);pthread_mutexattr_setprotocol(&mutexattr , PTHREAD_PRIO_INHERIT);pthread_mutex_init(&(port->getindex_mutex), &mutexattr);pthread_mutex_init(&(port->tx_mutex) , &mutexattr);pthread_mutex_init(&(port->rx_mutex) , &mutexattr);port->sockhandle = -1;port->lastidx = 0;port->redstate = ECT_RED_NONE; // No redundancy, single NIC modeport->stack.sock = &(port->sockhandle);port->stack.txbuf = &(port->txbuf);port->stack.txbuflength = &(port->txbuflength);port->stack.tempbuf = &(port->tempinbuf);port->stack.rxbuf = &(port->rxbuf);port->stack.rxbufstat = &(port->rxbufstat);port->stack.rxsa = &(port->rxsa);ecx_clear_rxbufstat(&(port->rxbufstat[0]));psock = &(port->sockhandle);}/* we use RAW packet socket, with packet type ETH_P_ECAT */*psock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ECAT));if(*psock < 0)return 0;timeout.tv_sec = 0;timeout.tv_usec = 1;r = 0;r |= setsockopt(*psock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));r |= setsockopt(*psock, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout));i = 1;r |= setsockopt(*psock, SOL_SOCKET, SO_DONTROUTE, &i, sizeof(i));/* connect socket to NIC by name */strcpy(ifr.ifr_name, ifname);r |= ioctl(*psock, SIOCGIFINDEX, &ifr);ifindex = ifr.ifr_ifindex;strcpy(ifr.ifr_name, ifname);ifr.ifr_flags = 0;/* reset flags of NIC interface */r |= ioctl(*psock, SIOCGIFFLAGS, &ifr);/* set flags of NIC interface, here promiscuous and broadcast */ifr.ifr_flags = ifr.ifr_flags | IFF_PROMISC | IFF_BROADCAST;r |= ioctl(*psock, SIOCSIFFLAGS, &ifr);/* bind socket to protocol, in this case RAW EtherCAT */sll.sll_family = AF_PACKET;sll.sll_ifindex = ifindex;sll.sll_protocol = htons(ETH_P_ECAT);r |= bind(*psock, (struct sockaddr *)&sll, sizeof(sll));/* setup ethernet headers in tx buffers so we don't have to repeat it */for (i = 0; i < EC_MAXBUF; i++){ec_setupheader(&(port->txbuf[i]));port->rxbufstat[i] = EC_BUF_EMPTY;}ec_setupheader(&(port->txbuf2));if (r == 0) rval = 1;return rval;
}
互斥锁
pthread_mutexattr_setprotocol 是 POSIX 线程库中的一个函数,用于为互斥锁属性对象 mutexattr 指定互斥锁的协议类型。第二个参数 PTHREAD_PRIO_INHERIT 表示启用优先级继承机制。当多个线程竞争同一个互斥锁时,如果低优先级线程持有锁而高优先级线程等待锁,低优先级线程会临时“继承”高优先级线程的优先级,直到释放锁为止。这可以有效防止优先级反转问题,提高实时系统的可靠性。
pthread_mutexattr_init(&mutexattr);
pthread_mutexattr_setprotocol(&mutexattr , PTHREAD_PRIO_INHERIT);
pthread_mutex_init(&(port->getindex_mutex), &mutexattr);
pthread_mutex_init(&(port->tx_mutex) , &mutexattr);
pthread_mutex_init(&(port->rx_mutex) , &mutexattr);
原始套接字
这一行代码的作用是创建一个原始套接字(RAW socket),用于直接收发 EtherCAT 协议的数据帧。
- PF_PACKET 指定使用数据链路层(即直接操作以太网帧),
- SOCK_RAW 表示原始套接字,可以收发未经内核协议栈处理的原始数据包,
- htons(ETH_P_ECAT) 指定协议类型为 EtherCAT(以太网类型 0x88A4)。
这与 EtherCAT 主站的底层通信密切相关。只有通过这种方式创建的套接字,主站程序才能直接与网卡进行 EtherCAT 帧的收发,而不经过 TCP/IP 协议栈,实现实时性和协议兼容性。这一行是整个 SOEM 网络初始化和数据收发的基础,后续所有的 bind、ioctl、setsockopt 等操作,都是围绕这个套接字进行配置和使用的。
*psock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ECAT));
与NIC绑定
通过 strcpy(ifr.ifr_name, ifname); 将网络接口名(如 “eth0”)复制到 ifr 结构体中。随后,ioctl(*psock, SIOCGIFINDEX, &ifr); 获取该接口的索引号,并存储到 ifindex 变量中。再次设置接口名后,将 ifr.ifr_flags 置零,并通过 ioctl(*psock, SIOCGIFFLAGS, &ifr); 重置接口标志位。
接下来,将接口标志位设置为混杂模式(IFF_PROMISC)和广播模式(IFF_BROADCAST),这样网卡可以接收所有经过的数据包,包括不是发给本机的数据包,这对于 EtherCAT 这类工业实时通信协议非常重要。通过 ioctl(*psock, SIOCSIFFLAGS, &ifr); 应用这些标志设置。
最后,配置 sll 结构体,将其设置为数据链路层(AF_PACKET),指定接口索引和 EtherCAT 协议类型(ETH_P_ECAT),并通过 bind 函数将套接字绑定到该网络接口和协议上。这样,后续通过该套接字收发的数据包就会直接在指定网卡和 EtherCAT 协议下进行传输。
/* connect socket to NIC by name */
strcpy(ifr.ifr_name, ifname);
r |= ioctl(*psock, SIOCGIFINDEX, &ifr);
ifindex = ifr.ifr_ifindex;
strcpy(ifr.ifr_name, ifname);
ifr.ifr_flags = 0;
/* reset flags of NIC interface */
r |= ioctl(*psock, SIOCGIFFLAGS, &ifr);
/* set flags of NIC interface, here promiscuous and broadcast */
ifr.ifr_flags = ifr.ifr_flags | IFF_PROMISC | IFF_BROADCAST;
r |= ioctl(*psock, SIOCSIFFLAGS, &ifr);
/* bind socket to protocol, in this case RAW EtherCAT */
sll.sll_family = AF_PACKET;
sll.sll_ifindex = ifindex;
sll.sll_protocol = htons(ETH_P_ECAT);
r |= bind(*psock, (struct sockaddr *)&sll, sizeof(sll));
初始化默认Ethernet headers
首先,for (i = 0; i < EC_MAXBUF; i++) 循环遍历所有发送缓冲区(txbuf)和接收缓冲区状态(rxbufstat)。在每次循环中,调用 ec_setupheader(&(port->txbuf[i])) 为每个发送缓冲区设置以太网头部,确保每个数据包都带有正确的EtherCAT帧头。随后,将对应的接收缓冲区状态 port->rxbufstat[i] 设置为 EC_BUF_EMPTY,表示该缓冲区当前为空,可以安全接收新数据。
循环结束后,单独对 port->txbuf2 也调用了一次 ec_setupheader,这通常是用于特殊用途的额外发送缓冲区,也需要初始化帧头。
for (i = 0; i < EC_MAXBUF; i++)
{ec_setupheader(&(port->txbuf[i]));port->rxbufstat[i] = EC_BUF_EMPTY;
}
ec_setupheader(&(port->txbuf2));
ec_setupheader
用于初始化以太网帧头部。参数 p 是一个指向以太网头部结构体(ec_etherheadert)的指针。
在函数内部,首先将 p 强制类型转换为 ec_etherheadert * 类型,并赋值给 bp。接下来,bp->da0、bp->da1 和 bp->da2 被设置为 0xffff,并通过 htons 函数进行主机字节序到网络字节序的转换。这三个字段共同表示以太网帧的目标MAC地址,这里被设置为广播地址(所有位为1),意味着该帧会被网络上的所有设备接收。
随后,bp->sa0、bp->sa1 和 bp->sa2 分别被设置为主站MAC地址(priMAC 数组中的值),同样经过字节序转换。这些字段表示以太网帧的源MAC地址。
最后,bp->etype 被设置为 ETH_P_ECAT,同样经过字节序转换,表示该帧的数据类型为 EtherCAT 协议。
/** Fill buffer with ethernet header structure.* Destination MAC is always broadcast.* Ethertype is always ETH_P_ECAT.* @param[out] p = buffer*/
void ec_setupheader(void *p)
{ec_etherheadert *bp;bp = p;bp->da0 = htons(0xffff);bp->da1 = htons(0xffff);bp->da2 = htons(0xffff);bp->sa0 = htons(priMAC[0]);bp->sa1 = htons(priMAC[1]);bp->sa2 = htons(priMAC[2]);bp->etype = htons(ETH_P_ECAT);
}/** ethernet header definition */
PACKED_BEGIN
typedef struct PACKED
{/** destination MAC */uint16 da0,da1,da2;/** source MAC */uint16 sa0,sa1,sa2;/** ethernet type */uint16 etype;
} ec_etherheadert;
PACKED_END