type
Post
status
Published
date
Mar 6, 2025
slug
2025/03/06/EtherCAT-Master-IgH-Analysis-(Part-One)---Master-Initialization,-State-Machine-and-EtherCAT-Messages---Mu-Duo
summary
tags
Linux
EtherCAT
category
EtherCAT
created days
new update day
icon
password
Created_time
Feb 25, 2025 08:58 AM
Last edited time
Mar 6, 2025 03:07 AM

0 获取源码

IgH EtherCAT Master现已迁移到GitLab:https://gitlab.com/etherlab.org/ethercat。文档可以在 ethercat/doc 目录下找到。
可以使用以下命令克隆存储库:
git clone https://gitlab.com/etherlab.org/ethercat.git git checkout stable-1.6
此外,还有其他一些EtherCAT主站的实现:
ethercat
ribaldaUpdated Feb 27, 2025
基于官方IgH,功能更为全面的EtherCAT主站。
KickCAT
leducpUpdated Feb 28, 2025
一个用C++编写的全新EtherCAT主站,目前功能尚不完善。
ethercrab
ethercrab-rsUpdated Mar 5, 2025
一个用纯Rust语言编写的全新EtherCAT主站,目前功能尚不完善。
本文主要讲解IgH的实现。

1 启动脚本

IgH通过脚本来启动,支持 systemd 与 init.d 启动方式。这些脚本位于源码的 scripts 目录下。
root@vbox:/home/ethercat# tree ./script/ ./script/ ├── ethercat.bash_completion ├── ethercat.conf ├── ethercatctl.in ├── ethercat.service.in ├── ifup-eoe.sh ├── init.d │ ├── ethercat.in │ └── Makefile.am ├── Makefile.am └── sysconfig ├── ethercat └── Makefile.am 2 directories, 10 files
script/ethercat.service.in
# # EtherCAT master kernel modules # [Unit] Description=EtherCAT Master Kernel Modules # Fine tuning of the startup dependencies below are recommended # to provide a reliable startup routine. # The dependencies below can be either uncommented after copying # this file to /etc/systemd/system or by creating overrides: # Copy the needed dependencies into # /etc/systemd/system/ethercat.service.d/50-dependencies.conf # in a [Unit] section. # # Uncomment this, if the generic Ethernet driver is used. It assures, that the # network interfaces are configured, before the master starts. # #Requires=network.target # Stop master, if network is stopped #After=network.target # Start master, after network is ready # # Uncomment this, if a native Ethernet driver is used. It assures, that the # network interfaces are configured, after the Ethernet drivers have been # replaced. Otherwise, the networking configuration tools could be confused. # #Before=network-pre.target #Wants=network-pre.target [Service] Type=oneshot RemainAfterExit=yes ExecStart=@sbindir@/ethercatctl start ExecStop=@sbindir@/ethercatctl stop [Install] WantedBy=multi-user.target
对于systemd方式,编译时会由 ethercat.service.in 文件生成 ethercat.service
script/ethercatctl.in
#!/bin/bash #------------------------------------------------------------------------------ # # Start script for EtherCAT to use with systemd # # Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH # # 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 # # vim: expandtab sw=4 tw=78 # #------------------------------------------------------------------------------ LSMOD=/sbin/lsmod MODPROBE=/sbin/modprobe RMMOD=/sbin/rmmod MODINFO=/sbin/modinfo IP=/bin/ip ETHERCAT=@bindir@/ethercat #------------------------------------------------------------------------------ ETHERCAT_CONFIG=@sysconfdir@/ethercat.conf if [ ! -r ${ETHERCAT_CONFIG} ]; then echo ${ETHERCAT_CONFIG} not existing; exit 6 fi # shellcheck source=/etc/ethercat.conf . ${ETHERCAT_CONFIG} #------------------------------------------------------------------------------ parse_mac_address() { local DEVICENAMETOMAC if [ -z "${1}" ]; then MAC="" elif echo ${1} | grep -qE '^([0-9a-fA-F]{2}:){5}[0-9a-fA-F]{2}$'; then MAC=${1} else DEVICENAMETOMAC=$("${IP}" address show dev "${1}" | awk '/link\/ether/ { print $2; }') if echo "${DEVICENAMETOMAC}" | grep -qE '^([0-9a-fA-F]{2}:){5}[0-9a-fA-F]{2}$'; then MAC=${DEVICENAMETOMAC} else echo Invalid MAC address or interface name \""${1}"\" \ in ${ETHERCAT_CONFIG} exit 1 fi fi } #------------------------------------------------------------------------------ case "${1}" in start) # construct DEVICES and BACKUPS from configuration variables DEVICES="" BACKUPS="" MASTER_INDEX=0 while true; do DEVICE=$(eval echo "\${MASTER${MASTER_INDEX}_DEVICE}") BACKUP=$(eval echo "\${MASTER${MASTER_INDEX}_BACKUP}") if [ -z "${DEVICE}" ]; then break; fi if [ ${MASTER_INDEX} -gt 0 ]; then DEVICES=${DEVICES}, BACKUPS=${BACKUPS}, fi parse_mac_address "${DEVICE}" DEVICES=${DEVICES}${MAC} parse_mac_address "${BACKUP}" BACKUPS=${BACKUPS}${MAC} MASTER_INDEX=$((${MASTER_INDEX} + 1)) done if [ -z "${DEVICES}" ]; then echo "ERROR: No network cards for EtherCAT specified." echo -n "Please edit ${ETHERCAT_CONFIG} with root permissions" echo -n " and set MASTER0_DEVICE variable to either a " echo "network interface name (like eth0) or to a MAC address." exit 1 fi # load master module if ! ${MODPROBE} ${MODPROBE_FLAGS} ec_master \ main_devices="${DEVICES}" backup_devices="${BACKUPS}"; then exit 1 fi LOADED_MODULES=ec_master # check for modules to replace for MODULE in ${DEVICE_MODULES}; do ECMODULE=ec_${MODULE} if ! ${MODINFO} "${ECMODULE}" > /dev/null; then continue # ec_* module not found fi if [ "${MODULE}" != "generic" ] && [ "${MODULE}" != "ccat" ]; then # unload standard module and check if unloading was successful ${RMMOD} "${MODULE}" 2> /dev/null || true if ${LSMOD} | grep "^${MODULE} " > /dev/null; then # could not unload module ${RMMOD} ${LOADED_MODULES} exit 1 fi fi if ! ${MODPROBE} ${MODPROBE_FLAGS} "${ECMODULE}"; then if [ "${MODULE}" != "generic" ] && [ "${MODULE}" != "ccat" ]; then ${MODPROBE} ${MODPROBE_FLAGS} "${MODULE}" # try to restore fi ${RMMOD} ${LOADED_MODULES} exit 1 fi LOADED_MODULES="${ECMODULE} ${LOADED_MODULES}" done exit 0 ;; #------------------------------------------------------------------------------ stop) # unload EtherCAT device modules for MODULE in ${DEVICE_MODULES} master; do ECMODULE=ec_${MODULE} if ! ${LSMOD} | grep -q "^${ECMODULE} "; then continue # ec_* module not loaded fi if ! ${RMMOD} "${ECMODULE}"; then exit 1 fi; done sleep 1 # load standard modules again for MODULE in ${DEVICE_MODULES}; do if [ "${MODULE}" == "generic" ] || [ "${MODULE}" == "ccat" ]; then continue fi ${MODPROBE} ${MODPROBE_FLAGS} "${MODULE}" done exit 0 ;; #------------------------------------------------------------------------------ restart) $0 stop || exit 1 sleep 1 $0 start ;; #------------------------------------------------------------------------------ status) echo "Checking for EtherCAT master @VERSION@ " # count masters in configuration file MASTER_COUNT=0 while true; do DEVICE=$(eval echo "\${MASTER${MASTER_COUNT}_DEVICE}") if [ -z "${DEVICE}" ]; then break; fi MASTER_COUNT=$((${MASTER_COUNT} + 1)) done RESULT=0 for i in $(seq 0 "$((${MASTER_COUNT} - 1))"); do echo -n "Master${i} " # Check if the master is in idle or operation phase ${ETHERCAT} master --master "${i}" 2>/dev/null | \ grep -qE 'Phase:[[:space:]]*Idle|Phase:[[:space:]]*Operation' EXITCODE=$? if [ ${EXITCODE} -eq 0 ]; then echo " running" else echo " dead" RESULT=1 fi done exit ${RESULT} ;; #------------------------------------------------------------------------------ *) echo "USAGE: $0 {start|stop|restart|status}" exit 1 ;; esac #------------------------------------------------------------------------------
ethercat.service 中指定了执行文件为 ethercatctl,该文件由 ethercatctl.in 生成。
script/init.d/ethercat.in
#!/bin/sh #------------------------------------------------------------------------------ # # Init script for EtherCAT # # Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH # # 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 # # # vim: expandtab # #------------------------------------------------------------------------------ ### BEGIN INIT INFO # Provides: ethercat # Required-Start: $local_fs $syslog $network # Should-Start: $time ntp # Required-Stop: $local_fs $syslog $network # Should-Stop: $time ntp # Default-Start: 3 5 # Default-Stop: 0 1 2 6 # Short-Description: EtherCAT master # Description: EtherCAT master @VERSION@ ### END INIT INFO #------------------------------------------------------------------------------ LSMOD=/sbin/lsmod MODPROBE=/sbin/modprobe RMMOD=/sbin/rmmod MODINFO=/sbin/modinfo ETHERCAT=@prefix@/bin/ethercat MASTER_ARGS= #------------------------------------------------------------------------------ ETHERCAT_CONFIG=/etc/sysconfig/ethercat if [ ! -r ${ETHERCAT_CONFIG} ]; then echo ${ETHERCAT_CONFIG} not existing; if [ "${1}" = "stop" ]; then exit 0 else exit 6 fi fi . ${ETHERCAT_CONFIG} #------------------------------------------------------------------------------ exit_success() { if [ -r /etc/rc.status ]; then rc_reset rc_status -v rc_exit else echo " done" exit 0 fi } #------------------------------------------------------------------------------ exit_fail() { if [ -r /etc/rc.status ]; then rc_failed rc_status -v rc_exit else echo " failed" exit 1 fi } #------------------------------------------------------------------------------ print_running() { if [ -r /etc/rc.status ]; then rc_reset rc_status -v else echo " running" fi } #------------------------------------------------------------------------------ print_dead() { if [ -r /etc/rc.status ]; then rc_failed rc_status -v else echo " dead" fi } #------------------------------------------------------------------------------ parse_mac_address() { if [ -z "${1}" ]; then MAC="" elif echo "${1}" | grep -qE '^([0-9a-fA-F]{2}:){5}[0-9a-fA-F]{2}$'; then MAC=${1} else echo Invalid MAC address \""${1}"\" in ${ETHERCAT_CONFIG} exit_fail fi } #------------------------------------------------------------------------------ if [ -r /etc/rc.status ]; then . /etc/rc.status rc_reset fi case "${1}" in start) echo -n "Starting EtherCAT master @VERSION@ " # construct DEVICES and BACKUPS from configuration variables DEVICES="" BACKUPS="" MASTER_INDEX=0 while true; do DEVICE=$(eval echo "\${MASTER${MASTER_INDEX}_DEVICE}") BACKUP=$(eval echo "\${MASTER${MASTER_INDEX}_BACKUP}") if [ -z "${DEVICE}" ]; then break; fi if [ ${MASTER_INDEX} -gt 0 ]; then DEVICES=${DEVICES}, BACKUPS=${BACKUPS}, fi parse_mac_address "${DEVICE}" DEVICES=${DEVICES}${MAC} parse_mac_address "${BACKUP}" BACKUPS=${BACKUPS}${MAC} MASTER_INDEX=$((${MASTER_INDEX} + 1)) done # load master module if ! ${MODPROBE} ${MODPROBE_FLAGS} ec_master "${MASTER_ARGS}" \ main_devices="${DEVICES}" backup_devices="${BACKUPS}"; then exit_fail fi # check for modules to replace for MODULE in ${DEVICE_MODULES}; do ECMODULE=ec_${MODULE} if ! ${MODINFO} "${ECMODULE}" > /dev/null; then continue # ec_* module not found fi if [ "${MODULE}" != "generic" ]; then if ${LSMOD} | grep "^${MODULE} " > /dev/null; then if ! ${RMMOD} "${MODULE}"; then exit_fail fi fi fi if ! ${MODPROBE} ${MODPROBE_FLAGS} "${ECMODULE}"; then if [ "${MODULE}" != "generic" ]; then ${MODPROBE} ${MODPROBE_FLAGS} "${MODULE}" # try to restore fi exit_fail fi done exit_success ;; stop) echo -n "Shutting down EtherCAT master @VERSION@ " # unload EtherCAT device modules for MODULE in ${DEVICE_MODULES} master; do ECMODULE=ec_${MODULE} if ! ${LSMOD} | grep -q "^${ECMODULE} "; then continue # ec_* module not loaded fi if ! ${RMMOD} "${ECMODULE}"; then exit_fail fi; done sleep 1 # reload previous modules for MODULE in ${DEVICE_MODULES}; do if [ "${MODULE}" != "generic" ]; then if ! ${MODPROBE} ${MODPROBE_FLAGS} "${MODULE}"; then echo Warning: Failed to restore "${MODULE}". fi fi done exit_success ;; restart) $0 stop || exit 1 sleep 1 $0 start ;; status) echo "Checking for EtherCAT master @VERSION@ " # count masters in configuration file MASTER_COUNT=0 while true; do DEVICE=$(eval echo "\${MASTER${MASTER_COUNT}_DEVICE}") if [ -z "${DEVICE}" ]; then break; fi MASTER_COUNT=$((${MASTER_COUNT} + 1)) done RESULT=0 for i in $(seq 0 "$((${MASTER_COUNT} - 1))"); do echo -n "Master${i} " # Check if the master is in idle or operation phase ${ETHERCAT} master --master "${i}" 2>/dev/null | \ grep -qE 'Phase:[[:space:]]*Idle|Phase:[[:space:]]*Operation' EXITCODE=$? if [ ${EXITCODE} -eq 0 ]; then print_running else print_dead RESULT=1 fi done exit ${RESULT} ;; *) echo "USAGE: $0 {start|stop|restart|status}" ;; esac if [ -r /etc/rc.status ]; then rc_exit else exit 1 fi #------------------------------------------------------------------------------
init.d 和 systemd 方式类似,都是生成一个可执行脚本,且脚本完成的工作一致,主要包括加载主站模块、网卡驱动、给主站内核模块传递参数、卸载模块等操作。
script/ethercat.conf
#------------------------------------------------------------------------------ # # EtherCAT master configuration file for use with ethercatctl. # # vim: spelllang=en spell tw=78 # #------------------------------------------------------------------------------ # # Main Ethernet devices. # # The MASTER<X>_DEVICE variable specifies the Ethernet device for a master # with index 'X'. # # Specify the MAC address (hexadecimal with colons) of the Ethernet device to # use. Example: "00:00:08:44:ab:66" # # Alternatively, a network interface name can be specified. The interface # name will be resolved to a MAC address using the 'ip' command. # Example: "eth0" # # The broadcast address "ff:ff:ff:ff:ff:ff" has a special meaning: It tells # the master to accept the first device offered by any Ethernet driver. # # The MASTER<X>_DEVICE variables also determine, how many masters will be # created: A non-empty variable MASTER0_DEVICE will create one master, adding a # non-empty variable MASTER1_DEVICE will create a second master, and so on. # # Examples: # MASTER0_DEVICE="00:00:08:44:ab:66" # MASTER0_DEVICE="eth0" # MASTER0_DEVICE="" #MASTER1_DEVICE="" # # Backup Ethernet devices # # The MASTER<X>_BACKUP variables specify the devices used for redundancy. They # behaves nearly the same as the MASTER<X>_DEVICE variable, except that it # does not interpret the ff:ff:ff:ff:ff:ff address. # #MASTER0_BACKUP="" # # Ethernet driver modules to use for EtherCAT operation. # # Specify a non-empty list of Ethernet drivers, that shall be used for # EtherCAT operation. # # Except for the generic Ethernet driver module, the init script will try to # unload the usual Ethernet driver modules in the list and replace them with # the EtherCAT-capable ones. If a certain (EtherCAT-capable) driver is not # found, a warning will appear. # # Possible values: 8139too, e100, e1000, e1000e, r8169, generic, ccat, igb, igc, genet, dwmac-intel, stmmac-pci. # Separate multiple drivers with spaces. # A list of all matching kernel versions can be found here: # https://docs.etherlab.org/ethercat/1.6/doxygen/devicedrivers.html # # Note: The e100, e1000, e1000e, r8169, ccat, igb and igc drivers are not built by # default. Enable them with the --enable-<driver> configure switches. # # Attention: When using the generic driver, the corresponding Ethernet device # has to be activated (with OS methods, for example 'ip link set ethX up'), # before the master is started, otherwise all frames will time out. # DEVICE_MODULES="" # If you have any issues about network interfaces not being configured # properly, systemd may need some additional infos about your setup. # Have a look at the service file, you'll find some details there. # # # Flags for loading kernel modules. # # This can usually be left empty. Adjust this variable, if you have problems # with module loading. # #MODPROBE_FLAGS="-b" #------------------------------------------------------------------------------
ethercat.conf 是共同的配置文件,用于配置主站使用的网卡、驱动等信息。下面我们来看 start 和 stop 脚本的具体工作。

1.1 start

  1. 加载 ec_master.ko 模块
    1. 模块参数包括:
      • main_devices:主网卡的MAC地址,多个 main_devices 表示创建多个主站,MAC参数个数由 master_count 决定。
      • backup_devices:备用网卡的MAC地址,多个 backup_devices 表示创建多个备用主站,MAC参数个数由 backup_count 决定。
      • debug_level:调试级别,控制调试信息输出级别。
      • eoe_interfaces:EOE接口,eoe_count 表示 eoe_interfaces 的个数。
      • eoe_autocreate:是否自动创建EOE handler。
      • pcap_size:Pcap缓冲区大小。
  1. 读取 /etc/sysconfig/ethercat 中的环境变量 DEVICE_MODULES,该变量位于 ethercat.conf 中。
  1. 在每个 DEVICE_MODULES 前添加前缀 ec_,例如,如果 DEVICE_MODULES 为 igb,则添加前缀后为 ec_igb
  1. 使用 modinfo 检查该模块是否存在。
  1. 对于非genericrtdm的驱动,需要先将网卡与当前驱动unbindunbind后的网卡才能被新驱动接管
  1. 对于 generic 和 rtdm 驱动,需要先将网卡与当前驱动解绑(unbind),解绑后的网卡才能被新驱动接管。
  1. 加载该驱动。
start 脚本加载了两个内核模块:ec_master.ko 和网卡驱动 ec_xxx.koec_master 根据内核参数(网卡MAC地址)创建主站实例,此时主站处于 Orphaned 阶段。随后加载网卡驱动 ec_xxx.ko,执行网卡驱动的 probe 函数,根据MAC地址将网卡与主站实例匹配,此时主站获得操作的网卡设备,进入 Idle 阶段。详细过程将在后文介绍。
notion image

1.2 stop

stop 脚本用于卸载内核模块 ec_master.ko 和网卡驱动 ec_xxx.ko
  1. 遍历配置文件中的环境变量 DEVICE_MODULES
  1. 在每个 DEVICE_MODULES 前添加前缀 ec_
  1. 使用 lsmod 检查该模块是否已加载。
  1. 卸载模块。
在后文中,“主站”和“master”均表示主站或主站实例对象,“slave”和“从站”表示从站或从站对象,中英混用,不做刻意区分。

2 主站实例创建

在一个使用IgH的控制器中,可以有多个EtherCAT主站。每个主站可以绑定一个主网卡和一个备用网卡,通常备用网卡不启用,只有在启用线缆冗余功能时才使用备用网卡(本文假设只有一个主网卡)。
notion image
start 过程中执行insmod ec_master.ko,这个时候,先调用的就是 module_init 调用的初始化函数ec_init_module()。先根据参数main_devices 的个数master_count,每个master需要一个设备节点来与应用程序交互,所以master_count决定需要创建多少个matser和多少个字符设备;
这里先分配注册master_count个字符设备的主次设备号device_number和名称,这样每个master对应的设备在文件系统中就是/dev/EtherCAT0/dev/EtherCAT1...(Linux下)。
if (master_count) { if (alloc_chrdev_region(&device_number, 0, master_count, "EtherCAT")) { EC_ERR("Failed to obtain device number(s)!\n"); ... } } class = class_create(THIS_MODULE, "EtherCAT");
解析模块参数得到MAC地址,保存到数组macs中。
for (i = 0; i < master_count; i++) { ret = ec_mac_parse(macs[i][0], main_devices[i], 0); if (i < backup_count) { ret = ec_mac_parse(macs[i][1], backup_devices[i], 1); } }
分配master_count个主站对象的内存,调用ec_master_init()初始化这些实例。
if (master_count) { if (!(masters = kmalloc(sizeof(ec_master_t) * master_count, GFP_KERNEL))) { ... } } for (i = 0; i < master_count; i++) { ret = ec_master_init(&masters[i], i, macs[i][0], macs[i][1], device_number, class, debug_level); ... }

2.1 Master Phases

IgH中,状态机是其核心思想,所有操作都基于状态机执行。每个EtherCAT主站实例都需要经历以下阶段转换(见图2.3),主站各阶段的操作如下:
notion image
  • Orphaned phase:此时主站实例已分配并初始化,正在等待以太网设备连接,即尚未与网卡驱动关联,此时无法进行总线通信。
  • Idle phase:当主站与网卡绑定后,Idle线程 ec_master_idle_thread 开始运行,主站处于IDLE状态。ec_master_idle_thread 主要完成从站拓扑扫描、配置站点地址等工作。该阶段命令行工具能够访问总线,但无法进行过程数据交换,因为总线配置尚未完成。
  • Operation phase:应用程序请求主站提供总线配置并激活主站后,主站进入Operation状态。ec_master_idle_thread 停止运行,内核线程变为 ec_master_operation_thread,之后应用程序可以周期性地交换过程数据。
在继续讲解 ec_master_init 函数之前,我们先了解数据报与状态机的关系,这对后续理解非常重要。

2.2 数据报

2.2.1 EtherCAT 数据报格式

EtherCAT 是以以太网为基础的现场总线系统,EtherCAT 使用标准的 IEEE 802.3 以太网帧,在主站一侧使用标准的以太网控制器,不需要额外的硬件。并在以太网帧头使用以太网类型 0x88A4 来和其他以太网帧相区别(EtherCAT 数据还可以通过 UDP/IP 来传输,本文已忽略),标准的 IEEE 802.3 以太网帧中数据部分为 EtherCAT 的数据,标准的 IEEE 802.3 以太网帧与 EtherCAT 数据帧关系如下:
notion image
EtherCAT 数据位于以太网帧数据区,EtherCAT 数据由 EtherCAT头若干EtherCAT数据报文组成。EtherCAT头中记录了EtherCAT数据报的长度和类型,类型为1表示与从站通信。EtherCAT数据报文内包含多个子报文,每个子报文由报文头、数据和WKC域组成。子报文结构含义如下。
notion image
整个EtherCAT网络形成一个环状,主站与从站之间是通过EtherCAT数据报来交互,一个EtherCAT报文从网卡 TX 发出后,从站 ESC 芯片 EtherCAT 报文进行交换数据,最后该报文回到主站。网上有个经典的EtherCAT动态图(刷新后动态图重新播放).
notion image
认识 EtherCAT 数据帧结构后,我们看 IgH 内是如何表示一个 EtherCAT 数据报文的?EtherCAT数据报文在 igh 中用对象 ec_datagram_t 表示。

2.2.2 ec_datagram_t 结构体

ec_datagram_t 是 IgH EtherCAT 主站中用于表示 EtherCAT 数据报的结构体。EtherCAT 数据报是主站与从站之间通信的基本单元,包含了数据、地址、类型等信息。该结构体定义了数据报的各种属性和状态,用于管理和操作 EtherCAT 通信过程中的数据报。
typedef struct { struct list_head queue; /**< Master datagram queue item, 用于将数据报插入主站的发送队列中。 通过用户提供的互斥锁(mutex)进行保护,确保多线程环境下的线程安全。 */ struct list_head ext_queue; /**< 用于将数据报插入外部队列中。 保护机制:通过信号量 ext_queue_sem 进行保护,确保外部队列的线程安全。 */ struct list_head sent; /**< 用于将已发送的数据报插入主站的已发送列表中。 主站通过该列表跟踪已发送但尚未收到响应的数据报。 */ ec_device_index_t device_index; /**< 表示数据报将通过哪个网络设备(网卡)发送或接收。 主站可能支持多个网卡设备,device_index 用于指定具体的设备。 */ ec_datagram_type_t type; /**< 表示数据报的类型。 常见类型: APRD:自动增量读取。 BWR:广播写。 */ uint8_t address[EC_ADDR_LEN]; /**< 表示数据报的目标地址(从站地址)。 EC_ADDR_LEN 是地址的长度,通常为 4 字节。*/ uint8_t *data; /**< 指向数据报的有效载荷(payload),有效载荷是实际传输的数据内容,大小由 data_size 决定。 */ ec_origin_t data_origin; /**< 表示数据报有效载荷的内存来源。可能的值包括: EC_ORIG_INTERNAL:数据来自主站内部。 EC_ORIG_EXTERNAL:数据来自外部(如用户空间)。 */ size_t mem_size; /**< 表示数据报有效载荷的内存大小。 这是实际分配的内存大小,可能大于 data_size。 */ size_t data_size; /**< 表示数据报有效载荷中实际使用的数据大小。 data_size 必须小于或等于 mem_size。 */ uint8_t index; /**< Index (set by master). */ uint16_t working_counter; /**< 表示数据报的工作计数器(Working Counter, WKC) WKC 是 EtherCAT 协议中的一个重要字段,用于确认从站是否正确处理了数据报。 */ ec_datagram_state_t state; /**< 表示数据报的当前状态。 常见状态: EC_DATAGRAM_INIT:数据报已初始化。 EC_DATAGRAM_QUEUED:数据报已插入发送队列。 EC_DATAGRAM_SENT:数据报已发送。 EC_DATAGRAM_RECEIVED:数据报已接收。 EC_DATAGRAM_TIMED_OUT:数据报超时。 EC_DATAGRAM_ERROR:数据报发送或接收过程中出错。 */ #ifdef EC_HAVE_CYCLES cycles_t cycles_sent; /**< 记录数据报发送的时间。 使用高精度计时器(cycles)记录的时间。 */ #endif unsigned long jiffies_sent; /**< 记录数据报发送的时间。 使用内核的 jiffies 记录的时间。 */ #ifdef EC_HAVE_CYCLES cycles_t cycles_received; /**< 记录数据报接收的时间。 使用高精度计时器(cycles)记录的时间。 */ #endif unsigned long jiffies_received; /**< 记录数据报接收的时间。 使用内核的 jiffies 记录的时间。 */ unsigned int skip_count; /**< 记录数据报未被接收时的重试次数。 当数据报未被接收时,主站会重新将其加入队列,skip_count 记录重试次数。 */ unsigned long stats_output_jiffies; /**< 记录最后一次统计输出的时间。 用于性能统计和调试。 */ char name[EC_DATAGRAM_NAME_SIZE]; /**< 数据报的描述信息。 用于调试和日志记录,便于识别数据报的用途。 */ } ec_datagram_t;
可以看到,上述子报文中的各个字段在 ec_datagram_t 结构体中都有对应的表示。为了避免重复,这里不再详细介绍这些字段,而是重点解释其他几个关键成员的作用。
  1. device_index
      • 作用:表示该数据报是通过哪个网卡设备发送或接收的。
      • 背景:在 IgH 中,一个 master 实例可以绑定多个网卡设备(例如主网卡和备用网卡),用于发送和接收 EtherCAT 数据帧。device_index 用于标识当前数据报所属的网卡设备。
      • 示例:如果 master 绑定了两个网卡,device_index 可以区分数据报是通过主网卡还是备用网卡发送的。
  1. data_origin
      • 作用:表示该数据报的内存来源类别。
      • 背景:在 IgH 中,master 管理着多个空闲的 ec_datagram_t 对象,这些对象根据用途被分为不同的类别(例如内部使用、外部使用等)。data_origin 用于标识当前数据报属于哪一类别。
      • 后续说明:具体的分类和用途将在后文详细讨论。
  1. index
      • 作用:表示该数据报在 EtherCAT 数据区中的子报文索引。
      • 背景:一个完整的 EtherCAT 数据帧可能包含多个子报文,每个子报文都有一个唯一的索引。index 用于标识当前数据报是第几个子报文。
      • 使用场景:在 master 组装以太网数据帧时,会根据 index 将多个子报文按顺序组合成一个完整的数据帧。
  1. data
      • 作用:指向子报文的数据内存区。
      • 背景:每个子报文的数据内容可能不同,因此数据区是动态分配的。data 指针指向实际的数据内存区域。
      • 相关字段mem_size 表示分配的内存大小,而 data_size 表示实际使用的数据大小。
  1. mem_size
      • 作用:表示数据报数据区的分配内存大小。
      • 说明:由于每个子报文的数据大小可能不同,mem_size 记录了为数据区分配的总内存大小,以确保有足够的空间存储数据。
  1. data_size
      • 作用:表示数据报中实际使用的数据大小。
      • 示例:如果该数据报用于读取从站的某个寄存器,data_size 就是该寄存器的大小。
      • 与 mem_size 的关系data_size 必须小于或等于 mem_size
  1. jiffies_sentjiffies_receivedcycles_sentcycles_received
      • 作用:记录数据报的发送和接收时间,用于统计和调试。
      • 背景
        • jiffies_sent 和 jiffies_received 使用内核的 jiffies 计时方式,记录数据报的发送和接收时间。
        • cycles_sent 和 cycles_received 使用高精度计时器(cycles),提供更精确的时间记录。
      • 使用场景:这些时间戳用于分析数据报的传输延迟、网络性能等。
notion image
ec_datagram_state_t 表示数据报的状态,每个数据报(ec_datagram_t)也是基于状态来处理,有6种状态:
  • EC_DATAGRAM_INIT :数据报已经初始化
  • EC_DATAGRAM_QUEUED :数据报已插入发送队列
  • EC_DATAGRAM_SENT :数据报已经发送(还存在队列中)
  • EC_DATAGRAM_RECEIVED:该数据报已接收,并从发送队列删除
  • EC_DATAGRAM_TIMED_OUT :该数据报发送后,接收超时,从发送队列删除
  • EC_DATAGRAM_ERROR : 发送和接收过程中出错(从队列删除),校验错误、不匹配等。

2.2.3 M 位的作用

  • 发送时:在主站组装 EtherCAT 数据帧时,M 位(More 位)用于指示当前子报文是否是最后一个子报文。如果 M 位为 1,表示后面还有更多的子报文;如果 M 位为 0,表示当前子报文是最后一个。
  • 接收时:从站根据 M 位判断是否还有后续子报文需要处理。如果 M 位为 1,从站会继续处理下一个子报文;如果 M 位为 0,从站会停止处理并返回响应。
数据报对象初始化由函数 ec_datagram_init() 完成:
/** Constructor. */ void ec_datagram_init(ec_datagram_t *datagram /**< EtherCAT datagram. */) { // 初始化链表头,标记该数据报尚未加入任何队列 INIT_LIST_HEAD(&datagram->queue); // mark as unqueued datagram->device_index = EC_DEVICE_MAIN; /* 默认主设备使用 */ datagram->type = EC_DATAGRAM_NONE; /* EtherCAT datagram type. */ memset(datagram->address, 0x00, EC_ADDR_LEN); /* 数据报地址清零 */ datagram->data = NULL; /* 数据指针初始化为空 */ datagram->data_origin = EC_ORIG_INTERNAL; /* 设置数据来源为内部 */ datagram->mem_size = 0; /* 内存大小初始化为0 */ datagram->data_size = 0; /* 数据大小初始化为0 */ datagram->index = 0x00; /* 索引初始化为0 */ datagram->working_counter = 0x0000; /* 工作计数器初始化为0 */ datagram->state = EC_DATAGRAM_INIT; /* 状态设置为初始化状态 */ #ifdef EC_HAVE_CYCLES datagram->cycles_sent = 0; #endif datagram->jiffies_sent = 0; #ifdef EC_HAVE_CYCLES datagram->cycles_received = 0; #endif datagram->jiffies_received = 0; datagram->skip_count = 0; datagram->stats_output_jiffies = 0; memset(datagram->name, 0x00, EC_DATAGRAM_NAME_SIZE); /* 将数据报名称字段清零 */ }

2.3 状态机

2.3.1 有限状态机(FSM概述

有限状态机(Finite State Machine, FSM)是一种表示有限个状态以及在这些状态之间转移和动作的数学模型。它在数字系统设计中具有重要作用,广泛应用于协议处理、控制器设计、菜单系统等领域。无论是单片机、FPGA,还是现代计算机系统,状态机都是核心的设计模式之一。例如,Linux 的 TCP 协议栈就是基于状态机实现的。
在 IgH EtherCAT 主站中,状态机同样是核心机制。几乎所有功能都是通过状态机来实现的,每个状态机负责管理某个对象的状态和功能实现的状态转换。这些状态转换基于 EtherCAT 数据报进行,具体流程如下:
  1. 状态机在某个状态(如 A0)中填充数据报(datagram)。
  1. 数据报通过 EtherCAT 数据帧发送到从站,经过从站的 ESC(EtherCAT Slave Controller)处理。
  1. 处理后的数据报返回到主站的网卡接收端。
  1. 主站将接收到的数据报交给状态机的下一个状态(如 A1)进行解析和处理。

2.3.2 IgH 中状态机的基本表示

在 IgH 中,状态机的基本结构如下:
struct ec_fsm_change { ec_slave_t *slave; /**< slave the FSM runs on */ ec_datagram_t *datagram; /**< datagram used in the state machine */ unsigned int retries; /**< retries upon datagram timeout */ void (*state)(ec_fsm_change_t *); /**< slave state change state function */ ec_fsm_change_mode_t mode; /**< full state change, or ack only. */ ec_slave_state_t requested_state; /**< input: state */ ec_slave_state_t old_state; /**< prior slave state */ unsigned long jiffies_start; /**< change timer */ uint8_t take_time; /**< take sending timestamp */ uint8_t spontaneous_change; /**< spontaneous state change detected */ };
struct ec_fsm_xxx { ec_datagram_t *datagram; /**< 主站状态机使用的数据报对象 */ void (*state)(ec_fsm_master_t *); /**< 状态函数 */ .... };
  • datagram:指向状态机操作的数据报对象。每个状态机都需要操作一个数据报对象,用于与从站进行数据交换。
  • state:状态函数指针,指向当前状态的处理函数。状态函数负责执行当前状态的操作,并决定状态机的下一步转移。
IgH EtherCAT协议栈几乎所有功能通过状态机实现,每个状态机管理着某个对象的状态、功能实现的状态装换,而这些状态转换是基于EtherCAT数据报来进行的,如状态机A0状态函数填充datagram,经EtherCAT数据帧发出后,经过slave ESC处理,回到网卡接收端接收后,交给状态机A1状态的下一个状态函数解析处理。所以每个状态机内都包含有指向该状态机操作的数据报对象指针datagram和状态执行的状态函数void (*state)(ec_fsm_master_t *)
总结一句话:状态机是根据数据报的状态来执行,每个状态机都需要操作一个数据报对象
现在知道了状态机与数据报的关系,下面介绍IgH EtherCAT协议栈中有哪些状态机,及状态机使用的数据报对象是从哪里分配如何管理的。

2.3.3 master状态机

前面说到主站具有的三个阶段,当主站与网卡设备attach后进入Idle phase,处于Idle phase后,开始执行主站状态机。
主站状态机包含1个主状态机和许多子状态机,matser状态机主要目的是:
  • Bus monitoring 监控EtherCAT总线拓扑结构,如果发生改变,则重新扫描。
  • Slave con fguration 监视从站的应用程序层状态。如果从站未处于其应有的状态,则从站将被(重新)配置
  • Request handling 请求处理(源自应用程序或外部来源),主站任务应该处理异步请求,例如:SII访问,SDO访问或类似。
主状态机ec_fsm_master_t结构如下:
/** Finite state machine of an EtherCAT master. */ struct ec_fsm_master { ec_master_t *master; /**< 指向当前状态机所属的主站实例。 每个主站状态机都与一个特定的主站实例关联,通过该指针可以访问主站的其他属性和方法。 */ ec_datagram_t *datagram; /**< 指向状态机使用的数据报对象。 状态机通过数据报与从站进行通信,datagram 是状态机操作的核心对象。 */ unsigned int retries; /**< 记录数据报超时时的重试次数。 当数据报发送后未收到响应时,状态机会根据 retries 决定是否重试。 */ void (*state)(ec_fsm_master_t *); /**< 指向当前状态的处理函数。 状态机通过该函数指针执行当前状态的操作,并根据操作结果决定状态转移。 */ ec_device_index_t dev_idx; /**< 表示当前状态机使用的网卡设备索引。 主站可能绑定多个网卡设备,dev_idx 用于标识当前操作使用的设备。 */ int idle; /**< 表示状态机是否处于空闲阶段(Idle Phase)。 在空闲阶段,主站会执行从站扫描和配置等操作。 */ unsigned long scan_jiffies; /**< 记录从站扫描开始的时间(以 jiffies 为单位)。 用于计算从站扫描的耗时。 */ uint8_t link_state[EC_MAX_NUM_DEVICES]; /**< 记录每个网卡设备的链路状态。 EC_MAX_NUM_DEVICES 是主站支持的最大网卡设备数量,为1。 */ unsigned int slaves_responding[EC_MAX_NUM_DEVICES]; /**< 记录每个网卡设备响应的从站数量。 用于监控总线上从站的响应情况。 */ unsigned int rescan_required; /**< 表示是否需要重新扫描总线。 当总线拓扑发生变化时,该标志会被设置。 */ ec_slave_state_t slave_states[EC_MAX_NUM_DEVICES]; /**< 记录每个网卡设备上从站的应用层(AL)状态。 用于监控从站的状态是否符合预期。 */ ec_slave_t *slave; /**< 指向当前正在处理的从站对象。 在从站扫描或配置过程中,状态机会操作特定的从站对象。 */ ec_sii_write_request_t *sii_request; /**< 指向 SII(Slave Information Interface)写请求对象。 用于处理从站的 SII 写操作。 */ off_t sii_index; /**< 表示 SII 写请求数据的索引。 用于定位 SII 写请求的数据位置。 */ ec_sdo_request_t *sdo_request; /**< 指向 SDO(Service Data Object)请求对象。 用于处理从站的 SDO 读写操作。 */ ec_soe_request_t *soe_request; /**< 指向 SoE(Servo Drive over EtherCAT)请求对象。 用于处理从站的 SoE 操作。 */ ec_fsm_coe_t fsm_coe; /**< 表示 CoE(CANopen over EtherCAT)状态机。 用于处理 CoE 协议相关的状态转换。 */ ec_fsm_soe_t fsm_soe; /**< 表示 SoE 状态机。 用于处理 SoE 协议相关的状态转换。 */ ec_fsm_pdo_t fsm_pdo; /**< 表示 PDO(Process Data Object)配置状态机。 用于处理 PDO 的配置和映射。 */ ec_fsm_eoe_t fsm_eoe; /**< 表示 EoE(Ethernet over EtherCAT)状态机。 用于处理 EoE 协议相关的状态转换。 */ ec_fsm_change_t fsm_change; /**< 表示状态变更状态机。 用于处理从站状态的变更。 */ ec_fsm_slave_config_t fsm_slave_config; /**< 表示从站配置状态机。 用于处理从站的配置过程。 */ ec_fsm_slave_scan_t fsm_slave_scan; /**< 表示从站扫描状态机。 用于处理从站的扫描过程。 */ ec_fsm_sii_t fsm_sii; /**< 表示 SII 状态机。 用于处理从站的 SII 读写操作。 */ };
可以看到,主站状态机结构下还有很多子状态机,想象一下如果主站的所有功能通过一个状态机来完成,那么这个状态机的状态数量、各状态之间的联系会有多恐怖,复杂性级别将会提高到无法管理的水平。为此,IgH中,将EtherCAT主状态机的某些功能用子状态机完成。这有助于封装相关工作流,并且避免“状态爆炸”现象。这样当主站完成coe功能时,可以由子状态机fsm_coe去完成。具体各功能是如何通过状态机完成的,文章后面会介绍。

2.3.4 slave 状态机

slave 状态机管理着每个从站的状态,所以位于从站对象 (ec_slave_t) 内:
struct ec_slave { ec_master_t *master; /**< Master owning the slave. */ ..... ec_fsm_slave_t fsm; /**< Slave state machine. */ ..... }; struct ec_fsm_slave { ec_slave_t *slave; /**< slave the FSM runs on */ struct list_head list; /**< Used for execution list. */ ec_dict_request_t int_dict_request; /**< Internal dictionary request. */ void (*state)(ec_fsm_slave_t *, ec_datagram_t *); /**< State function. */ ec_datagram_t *datagram; /**< Previous state datagram. */ ec_sdo_request_t *sdo_request; /**< SDO request to process. */ ec_reg_request_t *reg_request; /**< Register request to process. */ ec_foe_request_t *foe_request; /**< FoE request to process. */ off_t foe_index; /**< Index to FoE write request data. */ ec_soe_request_t *soe_request; /**< SoE request to process. */ ec_eoe_request_t *eoe_request; /**< EoE request to process. */ ec_mbg_request_t *mbg_request; /**< MBox Gateway request to process. */ ec_dict_request_t *dict_request; /**< Dictionary request to process. */ ec_fsm_coe_t fsm_coe; /**< CoE state machine. */ ec_fsm_foe_t fsm_foe; /**< FoE state machine. */ ec_fsm_soe_t fsm_soe; /**< SoE state machine. */ ec_fsm_eoe_t fsm_eoe; /**< EoE state machine. */ ec_fsm_mbg_t fsm_mbg; /**< MBox Gateway state machine. */ ec_fsm_pdo_t fsm_pdo; /**< PDO configuration state machine. */ ec_fsm_change_t fsm_change; /**< State change state machine */ ec_fsm_slave_scan_t fsm_slave_scan; /**< slave scan state machine */ ec_fsm_slave_config_t fsm_slave_config; /**< slave config state machine. */ };
slave状态机和master状态机类似,slave状态机内还包含许多子状态机。slave状态机主要目的是:
  • 主站管理从站状态
  • 主站与从站应用层(AL)通讯。比如具有EoE功能的从站,主站通过该从站下的子状态机fsm_eoe来管理主站与从站应用层的EOE通讯。

2.3.5 数据报对象的管理

上面简单介绍了IgH内的状态机,状态机输入输出的对象是datagram,fsm对象内只有数据报对象的指针,那fsm工作过程中的数据报对象从哪里分配?
由于每个循环周期都需要操作数据报对象,IgH为减少datagram的动态分配操作,提高主站性能,在master初始化的时候预分配了主站运行需要的所有datagram对象。在master实例我们可以看到下面的数据报对象:
struct ec_master { ... ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */ ... ec_datagram_t ref_sync_datagram; /**< Datagram used for synchronizing the reference clock to the master clock.*/ ec_datagram_t sync_datagram; /**< Datagram used for DC drift compensation. */ ec_datagram_t sync_mon_datagram; /**< Datagram used for DC synchronisation monitoring. */ ... ec_datagram_t ext_datagram_ring[EC_EXT_RING_SIZE]; /* 32 */ }
这些数据报对象都是已经分配内存的,但由于报文不同,报文操作的数据大小不同,所以datagram数据区大小随状态机的具体操作而变化,在具体使用时才分配数据区内存。
以上数据报对象给状态机使用,别忘了还有过程数据也需要数据报对象,所以IgH中数据报类型分为以下四类:
分为三类(非常重要):
数据报对象
用途
Datagram_pairs
过程数据报
fsm_datagram[]
Fsm_master及子状态机专用的数据报对象。
ext_datagram_ring[]
动态分配给fsm_slave及其子fsm。
ref_sync_datagram sync_datagram sync64_datagram sync_mon_datagram
应用专用数据报用于时钟同步。
其中 fsm_datagram 为 master 状态机及 master 下的子状态机执行过程中操作的对象。
ext_datagram_ring[]是一个环形队列,当fsm_slave从站状态机处于ready状态,可以开始处理与slave相关请求,如配置、扫描、SDO、PDO等,这时会从ext_datagram_ring[]中给该fsm_slave分配一个数据报,并运行fsm_slave状态机检查并处理请求。
应用专用数据报用于时钟同步,与时钟强相关,它们比较特殊,它们的数据区大小是恒定的,所以其数据区在主站初始化时就已分配内存,应用调用时直接填数据发送,避免linux的内存分配带来时钟的偏差。
数据报数据区(data)内存通过ec_datagram_prealloc()来分配.
int ec_datagram_prealloc( ec_datagram_t *datagram, /**< EtherCAT datagram. */ size_t size /**< New payload size in bytes. */ ) { if (datagram->data_origin == EC_ORIG_EXTERNAL || size <= datagram->mem_size) return 0; ...... if (!(datagram->data = kmalloc(size, GFP_KERNEL))) { ...... } datagram->mem_size = size; return 0; }
数据区的大小为一个以太网帧中单个Ethercat数据报的最大数据大小EC_MAX_DATA_SIZE
/** Size of an EtherCAT frame header. */ #define EC_FRAME_HEADER_SIZE 2 /** Size of an EtherCAT datagram header. */ #define EC_DATAGRAM_HEADER_SIZE 10 /** Size of an EtherCAT datagram footer. */ #define EC_DATAGRAM_FOOTER_SIZE 2 /** Size of the EtherCAT address field. */ #define EC_ADDR_LEN 4 /** Resulting maximum data size of a single datagram in a frame. */ #define EC_MAX_DATA_SIZE (ETH_DATA_LEN - EC_FRAME_HEADER_SIZE \ - EC_DATAGRAM_HEADER_SIZE - EC_DATAGRAM_FOOTER_SIZE)
由于以太网帧的大小有限,因此数据报的最大大小受到限制,即以太网帧长度 1500 - ethercat头2byte- ethercat子数据报报头10字节-WKC 2字节,如图:
notion image
如果过程数据镜像的大小超过该限制,就必须发送多个帧,并且必须对映像进行分区以使用多个数据报。 Domain自动进行管理。

2.4 master状态机及数据报初始化

2.4.1 模块入口函数 ec_init_module(void)

我们看一下 ec_master.ko 模块的入口函数 ec_init_module(void)
ec_init_module(void)
/** Module initialization. * * Initializes \a master_count masters. * \return 0 on success, else < 0 */ int __init ec_init_module(void) { int i, ret = 0; EC_INFO("Master driver %s\n", EC_MASTER_VERSION); sema_init(&master_sem, 1); if (master_count) { if (alloc_chrdev_region(&device_number, 0, master_count, "EtherCAT")) { EC_ERR("Failed to obtain device number(s)!\n"); ret = -EBUSY; goto out_return; } } #if LINUX_VERSION_CODE < KERNEL_VERSION(6, 4, 0) class = class_create(THIS_MODULE, "EtherCAT"); #else class = class_create("EtherCAT"); #endif if (IS_ERR(class)) { EC_ERR("Failed to create device class.\n"); ret = PTR_ERR(class); goto out_cdev; } // zero MAC addresses memset(macs, 0x00, sizeof(uint8_t) * EC_MAX_MASTERS * 2 * ETH_ALEN); // process MAC parameters for (i = 0; i < master_count; i++) { ret = ec_mac_parse(macs[i][0], main_devices[i], 0); if (ret) goto out_class; if (i < backup_count) { ret = ec_mac_parse(macs[i][1], backup_devices[i], 1); if (ret) goto out_class; } } // initialize static master variables ec_master_init_static(); if (master_count) { if (!(masters = kmalloc(sizeof(ec_master_t) * master_count, GFP_KERNEL))) { EC_ERR("Failed to allocate memory" " for EtherCAT masters.\n"); ret = -ENOMEM; goto out_class; } } for (i = 0; i < master_count; i++) { ret = ec_master_init(&masters[i], i, macs[i][0], macs[i][1], device_number, class, debug_level, run_on_cpu); if (ret) goto out_free_masters; } EC_INFO("%u master%s waiting for devices.\n", master_count, (master_count == 1 ? "" : "s")); return ret; out_free_masters: for (i--; i >= 0; i--) ec_master_clear(&masters[i]); kfree(masters); out_class: class_destroy(class); out_cdev: if (master_count) unregister_chrdev_region(device_number, master_count); out_return: return ret; }
ec_init_module() 是 EtherCAT 主站驱动模块的初始化函数,主要完成以下任务:
  1. 初始化全局信号量。
  1. 分配字符设备号。
  1. 创建设备类。
  1. 解析主站和备用网卡的 MAC 地址。
  1. 初始化主站的静态变量。
  1. 分配并初始化主站实例。
  1. 处理错误情况并释放资源。

2.4.2 ec_master_init_static(void)

/** Static variables initializer. */ void ec_master_init_static(void) { #ifdef EC_HAVE_CYCLES timeout_cycles = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000); ext_injection_timeout_cycles = (cycles_t) EC_SDO_INJECTION_TIMEOUT /* us */ * (cpu_khz / 1000); #else // one jiffy may always elapse between time measurement timeout_jiffies = max(EC_IO_TIMEOUT * HZ / 1000000, 1); ext_injection_timeout_jiffies = max(EC_SDO_INJECTION_TIMEOUT * HZ / 1000000, 1); #endif }
ec_master_init_static() 是 EtherCAT 主站的静态变量初始化函数,主要完成以下任务:
  1. 根据是否支持高精度计时器(EC_HAVE_CYCLES),计算超时时间。
  1. 设置 I/O 操作超时时间和外部注入超时时间。
  • 说明
    • jiffies 是 Linux 内核中的一种时间单位,表示系统启动以来的时钟滴答数。
    • timeout_jiffies 是 I/O 操作的超时时间,单位为 jiffies
    • ext_injection_timeout_jiffies 是外部注入(如 SDO 操作)的超时时间,单位为 jiffies
    • HZ 是内核的时钟频率(每秒的时钟滴答数)。
    • 公式:超时时间(jiffies) = 超时时间(us) * HZ / 1000000
    • max() 函数确保超时时间至少为 1 个 jiffy,避免超时时间为 0。
ec_master_init_static() 函数的主要作用是初始化 EtherCAT 主站的静态变量,特别是与超时时间相关的变量。根据系统是否支持高精度计时器,函数会使用不同的方式计算超时时间:
  • 如果支持高精度计时器,使用 CPU 周期数(cycles)作为超时时间的单位。
  • 如果不支持高精度计时器,使用 jiffies 作为超时时间的单位。

2.4.3 ec_master_init()

ec_master_init() 是 EtherCAT 主站的构造函数,主要完成以下任务:
  1. master初始化:使用sema_init()和ec_master_clear_device_stats()函数来初始化master及其相关的信号量和统计信息。
  1. MAC地址分配:通过检查EC_MAX_NUM_DEVICES是否大于1,将main_mac和backup_mac分别赋值给master->macs[EC_DEVICE_MAIN]和master->macs[EC_DEVICE_BACKUP]。
  1. 初始化设备:通过调用ec_device_init()函数来初始化master的所有设备,如果其中任何一个失败,代码会执行goto out_clear_devices跳转到清理部分。
  1. 状态机和数据报文初始化:使用ec_datagram_init()和ec_fsm_master_init()函数来初始化FSM和数据报文。
  1. 外部数据报文环的初始化:创建一个EC_EXT_RING_SIZE大小的数组,并使用ec_datagram_prealloc()函数为每个元素分配内存空间。
  1. 同步和引用同步数据报文的初始化:通过调用ec_datagram_init()和ec_datagram_prealloc()函数来初始化这些数据报文,并预分配它们所需的大小。
  1. 字符设备初始化:使用ec_cdev_init()函数创建一个新的字符设备用于与用户空间通信。
  1. RTDM设备的初始化(如果定义了EC_RTDM):尝试通过调用ec_rtdm_dev_init()函数来初始化RTDM设备,如果失败会跳转到out_unregister_class_device标签。
  1. 清理部分(在任何一个步骤中初始化失败时执行):如果初始化过程中的某一步失败,代码将使用一系列的ec_datagram_clear()、ec_fsm_master_clear()和ec_device_clear()调用来清除所有已经分配的资源。
ec_master_init()
static unsigned int run_on_cpu = 0xffffffff; /**< Bind created kernel threads to a cpu. Default do not bind. */ int __init ec_init_module(void) { ........ for (i = 0; i < master_count; i++) { ret = ec_master_init(&masters[i], i, macs[i][0], macs[i][1], device_number, class, debug_level, run_on_cpu); if (ret) goto out_free_masters; } ....... } /** Master constructor. \return 0 in case of success, else < 0 */ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ unsigned int index, /**< master index */ const uint8_t *main_mac, /**< MAC address of main device */ const uint8_t *backup_mac, /**< MAC address of backup device */ dev_t device_number, /**< Character device number. */ struct class *class, /**< Device class. */ unsigned int debug_level, /**< Debug level (module parameter). */ unsigned int run_on_cpu /**< bind created kernel threads to a cpu */ ) { int ret; unsigned int dev_idx, i; master->index = index; master->reserved = 0; sema_init(&master->master_sem, 1); // 每个主设备只有一个网卡设备,EC_MAX_NUM_DEVICES 为 1 for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_MAX_NUM_DEVICES; dev_idx++) { master->macs[dev_idx] = NULL; } master->macs[EC_DEVICE_MAIN] = main_mac; #if EC_MAX_NUM_DEVICES > 1 master->macs[EC_DEVICE_BACKUP] = backup_mac; master->num_devices = 1 + !ec_mac_is_zero(backup_mac); #else if (!ec_mac_is_zero(backup_mac)) { EC_MASTER_WARN(master, "Ignoring backup MAC address!"); } #endif /** index 是主站的索引,用于标识不同的主站实例。 master_sem 是主站的信号量,用于保护对主站资源的并发访问。 macs 数组存储主站的主网卡和备用网卡的 MAC 地址。 如果支持备用网卡(EC_MAX_NUM_DEVICES > 1),则初始化备用网卡的 MAC 地址;否则忽略备用网卡的 MAC 地址。 */ ec_master_clear_device_stats(master); sema_init(&master->device_sem, 1); master->phase = EC_ORPHANED; master->active = 0; master->config_changed = 0; master->injection_seq_fsm = 0; master->injection_seq_rt = 0; master->slaves = NULL; master->slave_count = 0; INIT_LIST_HEAD(&master->configs); INIT_LIST_HEAD(&master->domains); master->app_time = 0ULL; master->dc_ref_time = 0ULL; master->scan_busy = 0; master->scan_index = 0; master->allow_scan = 1; sema_init(&master->scan_sem, 1); init_waitqueue_head(&master->scan_queue); master->config_busy = 0; sema_init(&master->config_sem, 1); init_waitqueue_head(&master->config_queue); INIT_LIST_HEAD(&master->datagram_queue); master->datagram_index = 0; INIT_LIST_HEAD(&master->ext_datagram_queue); sema_init(&master->ext_queue_sem, 1); master->ext_ring_idx_rt = 0; master->ext_ring_idx_fsm = 0; /** 作用:初始化主站的信号量、队列和链表。 说明: device_sem 是设备操作的信号量。 phase 表示主站的当前阶段(如 EC_ORPHANED)。 slaves 和 slave_count 用于管理从站。 configs 和 domains 是配置和域的链表。 scan_sem 和 config_sem 是扫描和配置操作的信号量。 datagram_queue 和 ext_datagram_queue 是数据报的队列。 */ // init external datagram ring for (i = 0; i < EC_EXT_RING_SIZE; i++) { ec_datagram_t *datagram = &master->ext_datagram_ring[i]; ec_datagram_init(datagram); snprintf(datagram->name, EC_DATAGRAM_NAME_SIZE, "ext-%u", i); } /** 作用:初始化外部数据报环形缓冲区。 说明: ext_datagram_ring 是一个环形缓冲区,用于存储外部数据报。 每个数据报通过 ec_datagram_init() 初始化,并设置名称。 */ // send interval in IDLE phase ec_master_set_send_interval(master, 1000000 / HZ); /** 作用:设置 IDLE 阶段的发送间隔。 说明: send_interval 是数据报的发送间隔,单位为微秒。 HZ 是内核的时钟频率(每秒的时钟滴答数)。 */ master->fsm_slave = NULL; INIT_LIST_HEAD(&master->fsm_exec_list); master->fsm_exec_count = 0U; master->debug_level = debug_level; master->run_on_cpu = run_on_cpu; master->stats.timeouts = 0; master->stats.corrupted = 0; master->stats.unmatched = 0; master->stats.output_jiffies = 0; master->thread = NULL; #ifdef EC_EOE master->eoe_thread = NULL; INIT_LIST_HEAD(&master->eoe_handlers); #endif rt_mutex_init(&master->io_mutex); master->send_cb = NULL; master->receive_cb = NULL; master->cb_data = NULL; master->app_send_cb = NULL; master->app_receive_cb = NULL; master->app_cb_data = NULL; INIT_LIST_HEAD(&master->sii_requests); INIT_LIST_HEAD(&master->emerg_reg_requests); init_waitqueue_head(&master->request_queue); // init devices for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master); dev_idx++) { ret = ec_device_init(&master->devices[dev_idx], master); if (ret < 0) { goto out_clear_devices; } } // init state machine datagram ec_datagram_init(&master->fsm_datagram); snprintf(master->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, "master-fsm"); ret = ec_datagram_prealloc(&master->fsm_datagram, EC_MAX_DATA_SIZE); if (ret < 0) { ec_datagram_clear(&master->fsm_datagram); EC_MASTER_ERR(master, "Failed to allocate FSM datagram.\n"); goto out_clear_devices; } /** 作用:初始化主站状态机和数据报。 说明: fsm_datagram 是主站状态机使用的数据报。 ec_fsm_master_init() 初始化主站状态机,并指定使用的数据报。 */ // create state machine object ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram); // alloc external datagram ring for (i = 0; i < EC_EXT_RING_SIZE; i++) { ec_datagram_t *datagram = &master->ext_datagram_ring[i]; ret = ec_datagram_prealloc(datagram, EC_MAX_DATA_SIZE); if (ret) { EC_MASTER_ERR(master, "Failed to allocate external" " datagram %u.\n", i); goto out_clear_ext_datagrams; } } // init reference sync datagram ec_datagram_init(&master->ref_sync_datagram); snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "refsync"); ret = ec_datagram_prealloc(&master->ref_sync_datagram, 4); if (ret < 0) { ec_datagram_clear(&master->ref_sync_datagram); EC_MASTER_ERR(master, "Failed to allocate reference" " synchronisation datagram.\n"); goto out_clear_ext_datagrams; } // init sync datagram ec_datagram_init(&master->sync_datagram); snprintf(master->sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "sync"); ret = ec_datagram_prealloc(&master->sync_datagram, 4); if (ret < 0) { ec_datagram_clear(&master->sync_datagram); EC_MASTER_ERR(master, "Failed to allocate" " synchronisation datagram.\n"); goto out_clear_ref_sync; } // init sync monitor datagram ec_datagram_init(&master->sync_mon_datagram); snprintf(master->sync_mon_datagram.name, EC_DATAGRAM_NAME_SIZE, "syncmon"); ret = ec_datagram_brd(&master->sync_mon_datagram, 0x092c, 4); if (ret < 0) { ec_datagram_clear(&master->sync_mon_datagram); EC_MASTER_ERR(master, "Failed to allocate sync" " monitoring datagram.\n"); goto out_clear_sync; } master->dc_ref_config = NULL; master->dc_ref_clock = NULL; INIT_WORK(&master->sc_reset_work, sc_reset_task); init_irq_work(&master->sc_reset_work_kicker, sc_reset_task_kicker); // init character device ret = ec_cdev_init(&master->cdev, master, device_number); if (ret) goto out_clear_sync_mon; master->class_device = device_create(class, NULL, MKDEV(MAJOR(device_number), master->index), NULL, "EtherCAT%u", master->index); if (IS_ERR(master->class_device)) { EC_MASTER_ERR(master, "Failed to create class device!\n"); ret = PTR_ERR(master->class_device); goto out_clear_cdev; } #ifdef EC_RTDM // init RTDM device ret = ec_rtdm_dev_init(&master->rtdm_dev, master); if (ret) { goto out_unregister_class_device; } #endif /** 作用:初始化字符设备和 RTDM 设备。 说明: ec_cdev_init() 初始化字符设备。 device_create() 创建设备节点。 如果启用 RTDM(实时设备模型),则初始化 RTDM 设备。 */ return 0; #ifdef EC_RTDM out_unregister_class_device: device_unregister(master->class_device); #endif out_clear_cdev: ec_cdev_clear(&master->cdev); out_clear_sync_mon: ec_datagram_clear(&master->sync_mon_datagram); out_clear_sync: ec_datagram_clear(&master->sync_datagram); out_clear_ref_sync: ec_datagram_clear(&master->ref_sync_datagram); out_clear_ext_datagrams: for (i = 0; i < EC_EXT_RING_SIZE; i++) { ec_datagram_clear(&master->ext_datagram_ring[i]); } ec_fsm_master_clear(&master->fsm); ec_datagram_clear(&master->fsm_datagram); out_clear_devices: for (; dev_idx > 0; dev_idx--) { ec_device_clear(&master->devices[dev_idx - 1]); } return ret; }
对状态机及数据报对象有初步认识后,我们回到 ec_master.ko 模块入口函数 ec_init_module() 主站实例初始化 ec_master_init(),主要完成主站状态机初始化及数据报:
// init state machine datagram ec_datagram_init(&master->fsm_datagram); /*初始化数据报对象*/ snprintf(master->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, "master-fsm"); ret = ec_datagram_prealloc(&master->fsm_datagram, EC_MAX_DATA_SIZE); // create state machine object ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram); /*初始化master fsm*/
其中ec_fsm_master_init初始化master fsm和子状态机,并指定了master fsm使用的数据报对象fsm_datagram
void ec_fsm_master_init( ec_fsm_master_t *fsm, /**< Master state machine. */ ec_master_t *master, /**< EtherCAT master. */ ec_datagram_t *datagram /**< Datagram object to use. */ ) { fsm->master = master; fsm->datagram = datagram; // inits the member variables state, idle, dev_idx, link_state, // slaves_responding, slave_states and rescan_required ec_fsm_master_reset(fsm); // init sub-state-machines ec_fsm_coe_init(&fsm->fsm_coe); ec_fsm_soe_init(&fsm->fsm_soe); ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe); ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram, &fsm->fsm_change, &fsm->fsm_coe, &fsm->fsm_soe, &fsm->fsm_pdo, &fsm->fsm_eoe); ec_fsm_slave_scan_init(&fsm->fsm_slave_scan, fsm->datagram, &fsm->fsm_slave_config, &fsm->fsm_pdo); ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram); }

2.4.4 初始化外部数据报队列

外部数据报队列用于从站状态机,每个状态机执行期间使用的数据报从该区域分配,下面是初始化ext_datagram_ring中每个结构:
// alloc external datagram ring for (i = 0; i < EC_EXT_RING_SIZE; i++) { ec_datagram_t *datagram = &master->ext_datagram_ring[i]; ret = ec_datagram_prealloc(datagram, EC_MAX_DATA_SIZE); if (ret) { EC_MASTER_ERR(master, "Failed to allocate external" " datagram %u.\n", i); goto out_clear_ext_datagrams; } }
非应用数据报队列链表,如EOE数据报会插入该队列后发送。
INIT_LIST_HEAD(&master->ext_datagram_queue);
同样初始化几个时钟相关数据报对象,它们功能固定,所以数据区大小固定,就不贴代码了,比如sync_mon_datagram,它的作用是用于同步监控,获取从站系统时间差,所以是一个BRD数据报,在此直接将数据报操作偏移地址初始化,使用时能快速填充发送。
ec_datagram_init(&master->sync_mon_datagram); ...... ret = ec_datagram_brd(&master->sync_mon_datagram, 0x092c, 4);
地址
名称
描述
复位值
0x092c~0x092F
0~30
系统时间差
本地系统时间副本与参考时钟系统时间值之差
0
31
符号
0:本地系统时间≥参考时钟时间1:本地系统时间<参考时钟时间
0
另外比较重要的是将使用的网卡MAC地址放到macs[]中,在网卡驱动probe过程中根据MAC来匹配主站使用哪个网卡。
for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_MAX_NUM_DEVICES; dev_idx++) { master->macs[dev_idx] = NULL; } master->macs[EC_DEVICE_MAIN] = main_mac;

2.5 初始化EtherCAT device

master协议栈主要完成EtherCAT数据报的解析和组装,然后需要再添加EtherNet报头和FCS组成一个完整的以太网帧,最后通过网卡设备发送出去。为与以太网设备驱动层解耦,igh使用ec_device_t来封装底层以太网设备,一般来说每个master只有一个ec_device_t,这个编译时配置决定,若启用线缆冗余功能,可指定多个网卡设备:
struct ec_device { ec_master_t *master; /**< EtherCAT master */ struct net_device *dev; /**< 使用的网络设备 */ ec_pollfunc_t poll; /**< pointer to the device's poll function */ struct module *module; /**< pointer to the device's owning module */ uint8_t open; /**< true, if the net_device has been opened */ uint8_t link_state; /**< device link state */ struct sk_buff *tx_skb[EC_TX_RING_SIZE]; /**< transmit skb ring */ unsigned int tx_ring_index; /**< last ring entry used to transmit */ #ifdef EC_HAVE_CYCLES cycles_t cycles_poll; /**< cycles of last poll */ #endif #ifdef EC_DEBUG_RING struct timeval timeval_poll; #endif unsigned long jiffies_poll; /**< jiffies of last poll */ // Frame statistics u64 tx_count; /**< 发送的帧数 */ u64 last_tx_count; /**<上次统计周期发送的帧数。 */ u64 rx_count; /**< 接收的帧数 */ u64 last_rx_count; /**< 上一个统计周期收到的帧数。 */ u64 tx_bytes; /**< 发送的字节数 */ u64 last_tx_bytes; /**< 上一个统计周期发送的字节数。 */ u64 rx_bytes; /**< Number of bytes received. */ u64 last_rx_bytes; /**< Number of bytes received of last statistics cycle. */ u64 tx_errors; /**< Number of transmit errors. */ s32 tx_frame_rates[EC_RATE_COUNT]; /**< Transmit rates in frames/s for different statistics cycle periods. */ s32 rx_frame_rates[EC_RATE_COUNT]; /**< Receive rates in frames/s for different statistics cycle periods. */ s32 tx_byte_rates[EC_RATE_COUNT]; /**< Transmit rates in byte/s for different statistics cycle periods. */ s32 rx_byte_rates[EC_RATE_COUNT]; /**< Receive rates in byte/s for different statistics cycle periods. */ ...... };
成员*master表示改对象属于哪个master,*dev指向使用的以太网设备net_device,poll该网络设备poll函数,tx_skb[]以太网帧发送缓冲区队列,需要发送的以太网帧会先放到该队里,tx_ring_index管理tx_skb[],以及一些网络统计变量,下面初始化ec_device_t对象:
/*\master\master.c*/ for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master); dev_idx++) { ret = ec_device_init(&master->devices[dev_idx], master); if (ret < 0) { goto out_clear_devices; } } /*\master\device.c*/ int ec_device_init( ec_device_t *device, /**< EtherCAT device */ ec_master_t *master /**< master owning the device */ ) { int ret; unsigned int i; struct ethhdr *eth; .... device->master = master; device->dev = NULL; device->poll = NULL; device->module = NULL; device->open = 0; device->link_state = 0; for (i = 0; i < EC_TX_RING_SIZE; i++) { device->tx_skb[i] = NULL; } ...... ec_device_clear_stats(device); ...... for (i = 0; i < EC_TX_RING_SIZE; i++) { if (!(device->tx_skb[i] = dev_alloc_skb(ETH_FRAME_LEN))) { ...... } // add Ethernet-II-header skb_reserve(device->tx_skb[i], ETH_HLEN); eth = (struct ethhdr *) skb_push(device->tx_skb[i], ETH_HLEN); eth->h_proto = htons(0x88A4); memset(eth->h_dest, 0xFF, ETH_ALEN); } ..... }
主要关注分配以太网帧发送队列内存tx_skb[],并填充Ethernet报头中的以太网类型字段为0x88A4,目标MAC地址0xFFFFFFFF FFFF,对于源MAC地址、sk_buff所属网络设备、ec_device_t对象使用的网络设备net_device,将在网卡驱动初始化与master建立联系过程中设置。

2.6 设置IDLE 线程的发送间隔:

ec_master_set_send_interval(master, 1000000 / HZ);
根据网卡速率计算:
void ec_master_set_send_interval( ec_master_t *master, /**< EtherCAT master */ unsigned int send_interval /**< Send interval */ ) { master->send_interval = send_interval; //发送间隔 us master->max_queue_size = (send_interval * 1000) / EC_BYTE_TRANSMISSION_TIME_NS; master->max_queue_size -= master->max_queue_size / 10; }
100Mbps网卡发送一字节数据需要的时间EC_BYTE_TRANSMISSION_TIME_NS: 1/(100 MBit/s / 8 bit/byte) = 80 ns/byte.

2.7 初始化字符设备

由于主站位于内核空间,用户空间应用与主站交互通过字符设备来交互;
创建普通字符设备,给普通linux应用和Ethercat tool使用。若使用xenomai或RTAI,则再创建实时字符设备,提供给实时应用使用。
...... master->class_device = device_create(class, NULL, MKDEV(MAJOR(device_number), master->index), NULL, "EtherCAT%u", master->index); ...... #ifdef EC_RTDM // init RTDM device ret = ec_rtdm_dev_init(&master->rtdm_dev, master); ... #endif
到这里明白了IgH中的状态机与数据报之间的关系,主站对象也创建好了,但是主站还没有网卡设备与之关联,主站也还没有工作,下面简单看一下ecdev_offer流程。
关于网卡驱动代码详细解析推荐这两篇文章:

3 网卡

  1. 网卡probe
    1. static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent) { ...... adapter->ecdev = ecdev_offer(netdev, ec_poll, THIS_MODULE); if (adapter->ecdev) { /*注册打开ec_net设备*/ err = ecdev_open(adapter->ecdev); ..... adapter->ec_watchdog_jiffies = jiffies; } else { /*注册普通网络设备*/ ...... err = register_netdev(netdev); ...... } ...... }
  1. 给主站提供网络设备:ecdev_offer ,根据MAC地址找到master下的ec_device_t对象
    1. device->dev = net_dev; device->poll = poll; device->module = module;
      上面我们只设置了ec_device_t->tx_skb[]sk_buff的以太网类型和目的地址,现在继续填充源MAC地址为网卡的MAC地址、sk_buff所属的net_device:
      for (i = 0; i < EC_TX_RING_SIZE; i++) { device->tx_skb[i]->dev = net_dev; eth = (struct ethhdr *) (device->tx_skb[i]->data); memcpy(eth->h_source, net_dev->dev_addr, ETH_ALEN); }
  1. 调用网络设备接口打开网络设备
    1. int ec_device_open( ec_device_t *device /**< EtherCAT device */ ) { int ret; ..... ret = device->dev->open(device->dev); if (!ret) device->open = 1; .... return ret; }
  1. 当master下的所有的网络设备都open后,master从ORPHANED转到IDLE阶段
    1. int ec_master_enter_idle_phase( ec_master_t *master /**< EtherCAT master */ ) { int ret; ec_device_index_t dev_idx; ...... master->send_cb = ec_master_internal_send_cb; master->receive_cb = ec_master_internal_receive_cb; master->cb_data = master; master->phase = EC_IDLE; /*更新master状态*/ // reset number of responding slaves to trigger scanning for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master); dev_idx++) { master->fsm.slaves_responding[dev_idx] = 0; } ret = ec_master_nrthread_start(master, ec_master_idle_thread, "EtherCAT-IDLE"); .... return ret; }
其中主要设置master发送和接收回调函数,应用通过发送和接收数据时,将通过这两接口直接发送和接收。创建master idle线程ec_master_idle_thread

4 IDLE阶段内核线程

综上,状态机操作对象是datagram,datagram发送出去后回到主站交给状态机的下一个状态处理,所以主站需要循环地执行状态机、发送EtherCAT数据帧、接收EtherCAT数据帧、执行状态机、发送EtherCAT数据帧、……来驱动状态机运行,这个循环由内核线程来完成。
notion image
当主站与网卡绑定后,应用还没有请求主站,主站处于IDLE状态,这时循环由内核线程ec_master_idle_thread来完成,主要完成从站拓扑扫描、配置站点地址等工作。
static int ec_master_idle_thread(void *priv_data) { ec_master_t *master = (ec_master_t *) priv_data; int fsm_exec; #ifdef EC_USE_HRTIMER size_t sent_bytes; #endif // send interval in IDLE phase ec_master_set_send_interval(master, 250000 / HZ); while (!kthread_should_stop()) { // receive ecrt_master_receive(master); /*接收上个循环发送的数据帧*/ ...... // execute master & slave state machines ...... fsm_exec = ec_fsm_master_exec(&master->fsm); /*执行master状态机*/ ec_master_exec_slave_fsms(master); /*为从站状态机分配datagram,并执行从站状态机*/ ...... if (fsm_exec) { ec_master_queue_datagram(master, &master->fsm_datagram); /*将master状态机处理的datagram插入发送链表*/ } // send ecrt_master_send(master); /*组装以太网帧并调用网卡发送*/ sent_bytes = master->devices[EC_DEVICE_MAIN].tx_skb[ master->devices[EC_DEVICE_MAIN].tx_ring_index]->len; up(&master->io_sem); if (ec_fsm_master_idle(&master->fsm)) { ec_master_nanosleep(master->send_interval * 1000); set_current_state(TASK_INTERRUPTIBLE); schedule_timeout(1); } else { ec_master_nanosleep(sent_bytes * EC_BYTE_TRANSMISSION_TIME_NS); } } EC_MASTER_DBG(master, 1, "Master IDLE thread exiting...\n"); return 0; }
整个过程简单概述如下。

4.1 数据报发送

下面介绍IgH中状态机处理后数据报的发送流程(ecrt_master_send())。
notion image
master使用一个链表datagram_queue来管理要发送的子报文对象datagram,需要发送的子报文对象会先插入该链表中,统一发送时,分配一个sock_buff,从datagram_queue上取出报文对象,设置indexindex是发送后接收回来与原报文对应的标识之一),将一个个报文对象按EtherCAT数据帧结构填充到sock_buff中,最后通过网卡设备驱动函数hard_start_xmit,将sock_buff从网卡发送出去。
notion image

4.2 数据报接收

notion image
接收数据时,通过网卡设备驱动ec_poll函数取出Packet得到以太网数据,然后解析其中的EtherCAT数据帧,解析流程如下:
  1. 得到子报文index,遍历发送链表datagram_queue找到index对应的datagram
  1. 将子报文数据拷贝到datagram数据区。
  1. 将以太网帧内子报文中的WKC值复制到datagram中的WKC。
  1. datagram从链表datagram_queue删除。
  1. 根据子报文头M位判断还有没有子报文,有则跳转1继续处理下一个子报文,否则完成接收。
接收完成后,进入下一个循环,内核线程运行状态机或周期应用进行下一个周期,处理接收的Ethercat报文。
先简单介绍到这,敬请关注后续文章。。。。
作者:wsg1100
本文版权归作者和博客园共有,欢迎转载,但必须给出原文链接,并保留此段声明,否则保留追究法律责任的权利。
 
 
欢迎加入喵星计算机技术研究院,原创技术文章第一时间推送。
notion image
 
SET_NETDEV_DEV 宏详解Kernel configuration :: Xenomai 3