EtherCAT/ethercatctl.new

#!/bin/bash

# AD82: modified ethercatctl script:
#       replace ethernet drivers with ec_* dedicated ec drivers for selected interfaces only

# Load EtherCAT configuration
ETHERCAT_CONFIG="/etc/ethercat.conf"
if [ -f "$ETHERCAT_CONFIG" ]; then
    . "$ETHERCAT_CONFIG"
fi

# Define commands matching standard EtherLab scripts+apps
ETHERCAT=/opt/etherlab/bin/ethercat
MODPROBE="/sbin/modprobe"
MODINFO="/sbin/modinfo"
MODPROBE_FLAGS=""

IP=/usr/sbin/ip


# -------------------------------------------------------------------------
# Dynamic Parsing of MASTERn_DEVICE and MASTERn_BACKUP
# -------------------------------------------------------------------------
DEVICES=""
BACKUPS=""

# Loop through and build comma-separated lists of MAC addresses
for var in $(compgen -v | grep '^MASTER[0-9]\+_DEVICE'); do
    mac=$(eval echo "\$$var" | tr 'A-Z' 'a-z')
    if [ -n "$mac" ] && [ "$mac" != "none" ]; then
        if [ -z "${DEVICES}" ]; then DEVICES="$mac"; else DEVICES="${DEVICES},$mac"; fi
    fi
done

for var in $(compgen -v | grep '^MASTER[0-9]\+_BACKUP'); do
    mac=$(eval echo "\$$var" | tr 'A-Z' 'a-z')
    if [ -n "$mac" ] && [ "$mac" != "none" ]; then
        if [ -z "${BACKUPS}" ]; then BACKUPS="$mac"; else BACKUPS="${BACKUPS},$mac"; fi
    fi
done

# Create a space-separated list for our bash regex matching later
TARGET_MACS=$(echo "${DEVICES} ${BACKUPS}" | tr ',' ' ')

start_devices() {
    # Check if we actually have devices configured
    if [ -z "${DEVICES}" ]; then
        echo "[Fatal] No MASTERn_DEVICE MAC addresses found in config. Aborting."
        exit 1
    fi

    # 1. Bring up standard interfaces first (needed in case we DO fall back to generic)
    for iface in ${LINK_DEVICES}; do
        echo "  Bringing up interface: ${IP} link set ${iface} up"
        ${IP} link set "${iface}" up 2>/dev/null
    done

    # 2. Load the core master module
    echo "Loading EtherCAT master core module with configured MACs..."
    if ! ${MODPROBE} ${MODPROBE_FLAGS} ec_master \
            main_devices="${DEVICES}" backup_devices="${BACKUPS}"; then
        echo "[Fatal] Failed to load ec_master core module. Aborting."
        exit 1
    fi

    # 3. Target and SWAP PCI drivers BEFORE loading ec_* modules
    echo "Starting targeted EtherCAT device binding..."
    for iface in /sys/class/net/*; do
        [ -e "$iface" ] || continue
        ifname=$(basename "$iface")
        [ "$ifname" = "lo" ] && continue

        if [ -f "$iface/address" ]; then
            iface_mac=$(cat "$iface/address" | tr 'A-Z' 'a-z')
        else
            continue
        fi

        # Check if this interface MAC is one of our targets (Main or Backup)
        if [[ $TARGET_MACS =~ $iface_mac ]]; then
            if [ -e "$iface/device" ]; then
                pci_bus_id=$(basename $(readlink "$iface/device"))
                
                if [[ "$pci_bus_id" =~ ^[0-9a-fA-F]{4}: ]]; then
                    current_driver=$(basename $(readlink "$iface/device/driver") 2>/dev/null)
                    
                    if [ -n "$current_driver" ]; then
                        # If the driver matches a hardware module we have optimized drivers for:
                        if [[ " $DEVICE_MODULES " =~ " $current_driver " ]]; then
                            target_driver="ec_$current_driver"
                            
			    # but do nothing if selected ec_ driver is not installed
			    /sbin/modinfo ${target_driver} >/dev/null 2>/dev/null || continue
			    
                            echo "Matching optimized interface: $ifname ($iface_mac) -> Target: '$target_driver'"
                            echo "  Unbinding $pci_bus_id from $current_driver..."
                            echo -n "$pci_bus_id" > "/sys/bus/pci/drivers/$current_driver/unbind" 2>/dev/null
                            sleep 0.2
                            
                            # Note: Binding might fail here if the module isn't loaded yet,
                            # but loading the module next will auto-probe and grab it.
                            if [ -d "/sys/bus/pci/drivers/$target_driver" ]; then
                                echo "  Binding $pci_bus_id to $target_driver..."
                                echo -n "$pci_bus_id" > "/sys/bus/pci/drivers/$target_driver/bind" 2>/dev/null
                            fi
                        else
                            echo "Interface $ifname ($iface_mac) matches config but uses standard driver '$current_driver'. Leaving for ec_generic."
                        fi
                    fi
                fi
            fi
        fi
    done

    # 4. Load optimized modules first, generic absolute last!
    echo "Loading configured EtherCAT device modules..."
    # Sort modules so 'generic' always goes to the end of the line
    SORTED_MODULES=$(echo "$DEVICE_MODULES" | awk '{for(i=1;i<=NF;i++) if($i!="generic") print $i; print "generic"}')
    
    for mod in $SORTED_MODULES; do
        [ -z "$mod" ] && continue
        echo "  Loading ec_$mod..."
        ${MODPROBE} ${MODPROBE_FLAGS} "ec_$mod" 2>/dev/null || echo "  [Warning] Failed to modprobe ec_$mod"
        sleep 0.1
    done
}

stop_devices() {
    echo "Stopping EtherCAT device binding (restoring standard drivers)..."
    
    # Unbind PCI devices first while drivers are still loaded
    for mod in $DEVICE_MODULES; do
        target_driver="ec_$mod"
        if [ -d "/sys/bus/pci/drivers/$target_driver" ]; then
            for pci_dev in /sys/bus/pci/drivers/$target_driver/[0-9a-fA-F]*; do
                [ -e "$pci_dev" ] || continue
                pci_bus_id=$(basename "$pci_dev")
                
		if [ "$pci_bus_id" != "bind" ]
		then
                  echo "Unbinding $pci_bus_id from $target_driver..."
                  echo -n "$pci_bus_id" > "/sys/bus/pci/drivers/$target_driver/unbind" 2>/dev/null
		
		  echo "Binding $pci_bus_id to ${target_driver#ec_}..."
                  echo -n "$pci_bus_id" > "/sys/bus/pci/drivers/${target_driver#ec_}/bind" 2>/dev/null
		fi
            done
        fi
    done

    # Force PCI bus rescan so standard modules claim their hardware back
    echo "Triggering PCI bus rescan..."
    echo 1 > "/sys/bus/pci/rescan"
    sleep 0.5

    echo "Unloading EtherCAT device modules..."
    for mod in $DEVICE_MODULES; do
        echo "  Unloading ec_$mod..."
        rmmod "ec_$mod" 2>/dev/null
    done

    echo "Unloading EtherCAT master core module..."
    rmmod ec_master 2>/dev/null
}

status_devices() {
    echo "Checking for EtherCAT master 1.6.3 status "

    # 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
}


case "$1" in
    start)
        start_devices
        ;;
    stop)
        stop_devices
        ;;
    status)
        status_devices
	;;

    *)
        echo "Usage: $0 {start|stop}"
        exit 1
        ;;
esac