#!/bin/bash

# Isolates CPU cores for codesys
# sudo codesys_isolcpus [core]
# 1. Clear CPU Isolation
#   > sudo codesys_isolcpus
# 2. Isolates the fourth  CPU core
#   > sudo codesys_isolcpus 3
# 3. Isolates CPU third and fourth cores
#   > sudo codesys_isolcpus 2,3
# 4. Isolates CPU second to fourth cores
#   > sudo codesys_isolcpus 1-3

G_CMDLINE=/boot/firmware/cmdline.txt
G_CODESYS=/etc/init.d/codesyscontrol
G_CODESYS_CONFIG=/etc/CODESYSControl.cfg

function update_cmdline(){
    local t_cmdline=${G_CMDLINE}
    local key=$1

    local t_value=$(cat ${t_cmdline} | sed -r "s/(^| )${key}=[^ ]*( |\$)/ /g")
    if [ $# -eq 2 ];then
        local value=$2
        t_value="${t_value} ${key}=${value}"
    fi
    echo ${t_value} > ${t_cmdline}
}

function load_irqaffinity(){
    local t_isolcpus=$1
    local t_value=""
    if echo "${t_isolcpus}" | grep -q ","; then
        for i in `seq 0 3`;do
            echo "${t_isolcpus}" | grep -q "\(^\|,\)${i}\(,\|\$\)"
            if [ $? -ne 0 ];then
                if [ ! -z "${t_value}" ];then
                    t_value="${t_value},"
                fi
                t_value="${t_value}${i}"
            fi
        done
        echo "${t_value}"
        return 0
    fi
    if echo "${t_isolcpus}" | grep -q "-"; then
        t_array=($(echo "${t_isolcpus}" | awk -F- '{for(i=1;i<=NF;i++) print $i}'))

        if [ ${#t_array[*]} -ne 2 ];then
            echo "${t_value}"
            return 0
        fi
        t_low=$(( ${t_array[0]} - 1 ))
        t_high=$(( ${t_array[1]} + 1 ))
        for i in `seq 0 ${t_low}`;do
            if [ ! -z "${t_value}" ];then
                t_value="${t_value},"
            fi
            t_value="${t_value}${i}"
        done
        for i in `seq ${t_high} 3`;do
            if [ ! -z "${t_value}" ];then
                t_value="${t_value},"
            fi
            t_value="${t_value}${i}"
        done
        echo "${t_value}"
        return 0
    fi

    for i in `seq 0 3`;do
        echo "${t_isolcpus}" | grep -q "\(^\|,\)${i}\(,\|\$\)"
        if [ $? -ne 0 ];then
            if [ ! -z "${t_value}" ];then
                t_value="${t_value},"
            fi
            t_value="${t_value}${i}"
        fi
    done
    echo "${t_value}"
    return 0
}

function codesys_isolcpus(){
    local t_codesys=${G_CODESYS}
    local t_codesys_config=${G_CODESYS_CONFIG}
    local t_isolcpus=$1
    local t_value_codesys=""
    
    if [ $# -gt 0 ]; then
        t_value_codesys=" taskset -c ${t_isolcpus}"
        update_cmdline isolcpus ${t_isolcpus}
        update_cmdline nohz_full ${t_isolcpus}
        update_cmdline irqaffinity $(load_irqaffinity ${t_isolcpus})
    else
        update_cmdline isolcpus
        update_cmdline nohz_full
        update_cmdline irqaffinity
    fi


    local codesys_line=$(sed -n '/$DAEMON $DAEMON_ARGS $EXEC $CONFIGFILE/=' ${t_codesys})
    sed -i "${codesys_line}s/( .* \$D/(${t_value_codesys} \$D/" ${t_codesys}

    if [ -f "${t_codesys_config}" ];then
        sed -i '/Linux.DisableCpuDmaLatency=/c\Linux.DisableCpuDmaLatency=0' ${t_codesys_config}
    fi
}


codesys_isolcpus $1