#!/bin/sh
# Implementation of QoS setup for Econet platform.

VALID_INTF=""
readonly ETHWAN="$(jsonfilter -i /etc/board.json -e @.network.wan.device)"
readonly LANPORTS="$(jsonfilter -i /etc/board.json -e @.network.lan.ports[*] -e @.network.lan.device | xargs)"
readonly OFFSET=32

get_wan_port_idx() {
    local reg="$(cat /proc/tc3162/fe_reg | grep -E '^FE_WAN_PORT' | awk '{print $4}')"
    local port

    reg=$((reg))
    port=$((reg & 0x1f))

    echo "$port"
}

readonly WAN_TYPE_PORT="$(get_wan_port_idx)"

errmsg() {
    echo "$@" >&2
    return 0
}

get_var() {
    local varname="$1"
    eval echo \$\{${varname}\}
    return 0
}

# Initialized queues
# (Not needed on Econet)
hw_queue_init_all() {
    return 0
}

# populate interfaces available for rate limiting
init_valid_interface_list() {
    local intf tmpintf

    for intf in $(ifconfig -a | awk -F' ' '{if ($1 ~ "eth" ) print $1;}' | xargs); do
        for tmpintf in $LANPORTS; do
                if [ "$intf" = "$tmpintf" ]; then
                        [ -z "$VALID_INTF" ] && VALID_INTF="${intf}" || VALID_INTF="${VALID_INTF} ${intf}"
                        break
                fi
        done
    done

    for intf in $(ifconfig -a | awk -F' ' '{if ($1 ~ "rai" ) print $1;}' | xargs); do
        [ "$(ls -d /sys/class/net/${intf}/lower* 2> /dev/null)" != "" ] && continue
        [ -z "$VALID_INTF" ] && VALID_INTF="${intf}" || VALID_INTF="${VALID_INTF} ${intf}"
    done

    for intf in $(ifconfig -a | awk -F' '  '!/rai/ {if ($1 ~ "ra") print $1;}' | xargs); do
        [ "$(ls -d /sys/class/net/${intf}/lower* 2> /dev/null)" != "" ] && continue
        [ -z "$VALID_INTF" ] && VALID_INTF="${intf}" || VALID_INTF="${VALID_INTF} ${intf}"
    done
}

# Initialize all interfaces
# (Not needed on Econet)
hw_intf_init() {
    return 0
}

set_wan_ingress_rate() {
    local ingress_rate="$1"
    local wanchannel="$(cat /proc/tc3162/eth_portmap | head -n1)"

    ingress_rate=$((ingress_rate))
    wanchannel=$((wanchannel))

    rm -rf "/tmp/qos/wan_ingress_shape_speed"

    if [ "$wanchannel" = "-1" ]; then
        echo "6 0 $ingress_rate 0 $WAN_TYPE_PORT" > "/proc/tc3162/fe_ratelimit"
    else
        # Both directions needs to be cleard for this to work as expected.
        /userfs/bin/switchmgr port ratectl "$wanchannel" 0 0
        /userfs/bin/switchmgr port ratectl "$wanchannel" 1 0
        /userfs/bin/switchmgr port ratectl "$wanchannel" 0 "$ingress_rate"
    fi

    echo "$ingress_rate" > "/tmp/qos/wan_ingress_shape_speed"
}

set_wan_egress_rate() {
    local rate="$1"
    local burstsize="$2"
    local wanchannel="$(cat /proc/tc3162/eth_portmap | head -n1)"
    wanchannel=$((wanchannel))
    [ "$wanchannel" = "-1" ] && wanchannel=0

    if [ "$rate" -gt 0 ]; then
        /userfs/bin/qdmamgr_wan set general_tx_trtcm config "$wanchannel" enable byte fast enable byte fast
        /userfs/bin/qdmamgr_wan set general_tx_trtcm value "$wanchannel" "$rate" "$rate"

        if [ "$burstsize" -gt 1000 ]; then
            /userfs/bin/qdmamgr_wan set general_tx_trtcm bsize "$wanchannel" "$burstsize" "$burstsize"
        fi
    else
        /userfs/bin/qdmamgr_wan set general_tx_trtcm config "$wanchannel" disable byte fast disable byte fast
    fi
}

# Initialize the hardware setup library
hw_init_all() {
    local tc=0
    local tsid=0
    local channel

    export TMP_HW_QUEUE_LIST=""
    echo clear > /proc/ifc_debug
    echo reinit > /proc/ifc_debug

    for tc in $(seq 0 7); do
        rm -rf "/tmp/qos/dscp_values_${tc}_4"
        rm -rf "/tmp/qos/dscp_values_${tc}_6"
    done

    /userfs/bin/qdmamgr_lan set general_rx_init enable trtcm 8 125
    /userfs/bin/qdmamgr_wan set general_rx_init enable trtcm 8 125
    /userfs/bin/qdmamgr_wan set general_tx_init enable trtcm 40 25

    for channel in $(seq 0 30); do
        tsid=$((OFFSET+channel))
        /userfs/bin/qdmamgr_lan set general_rx_trtcm config "$tsid" disable byte fast disable byte fast
        /userfs/bin/qdmamgr_wan set general_rx_trtcm config "$tsid" disable byte fast disable byte fast
    done

    # set_wan_ingress_rate "0" - Not needed, done in policer.sh
    set_wan_egress_rate "0" "0"

    return 0
}

# Remember selected queue options. They will be committed
# during hw_commit_all()
hw_queue_set() {
    local ifname="$1"
    local q_count="$2"
    local order="$3"
    local qsize="$4"
    local wgt="$5"
    local sc_alg="$6"
    local rate="$7"
    local burstsize="$8"
    local index="$((order - 1))"

    #if [ "${ifname}" != "${ethwan}" ] ; then
    #   return 2
    #fi

    export TMP_HW_QUEUE_${order}_no="${q_count}"
    export TMP_HW_QUEUE_${order}_ifname="${ifname}"
    export TMP_HW_QUEUE_${order}_order="${order}"
    export TMP_HW_QUEUE_${order}_qsize="${qsize}"
    export TMP_HW_QUEUE_${order}_wgt="${wgt}"
    export TMP_HW_QUEUE_${order}_sc_alg="${sc_alg}"
    export TMP_HW_QUEUE_${order}_rate="${rate}"
    export TMP_HW_QUEUE_${order}_burstsize="${burstsize}"
    export TMP_HW_QUEUE_LIST="${TMP_HW_QUEUE_LIST} ${order}"

    if [ "${rate}" != "" ] && [ $(($rate != 0)) ] ; then
        errmsg "Per-queue shape rate is not implemented"
    fi

    return 0
}

# Set policer options. In fact, they are not supported.
hw_policer_set() {
    local action="$1"
    local dir="$2"
    local policer_no="$3"

    shift 3

    case "$action" in
        add)
            meter="$1"
            cir="$2"
            cbs="$3"
            ebs="$4"
            pir="$5"
            pbs="$6"

            ;;
        del)
            ;;
    esac
    return 0
}

# get tsid from ifname
get_tsid() {
    if [ -z "$1" ]; then
        echo "-1"
        return
    fi

    local tmpintf
    local intf="$1"
    local idx=0

    for tmpintf in $VALID_INTF; do
        if [ "$tmpintf" = "$intf" ]; then
            if [ "${intf:0:5}" = "eth0." ]; then
                idx=${intf:5:6}
                idx=$((idx-1))
            elif [ "${intf:0:3}" = "eth" ]; then
                idx=${intf:3:4}
                idx=$((idx+4))
            elif [ "${intf:0:3}" = "rai" ]; then
                idx=${intf:3:4}
                idx=$((idx+9))
            elif [ "${intf:0:2}" = "ra" ]; then
                idx=${intf:2:3}
                idx=$((idx+17))
            else
                echo "-1"
                return
            fi
            echo $((OFFSET+idx))
            return
        fi
    done

    echo "-1"
}

get_numeric_value() {
    local val="$1"

    case $val in
        ''|*[!0-9]*) echo 0 ;;
        *) echo "${val}" ;;
    esac
}

set_lan_rate() {
    local ifname="$1"
    local rate="$(get_numeric_value "${2}")"
    local burst_size="$(get_numeric_value "${3}")"
    local direction="$4"
    local qdma_exe=""

    local tsid="$(get_tsid "${ifname}")"

    if [ "$tsid" -lt 0 ]; then
        errmsg "could not get tsid for ${ifname}"
        return
    fi

    if [ "$direction" -eq 0 ]; then
        qdma_exe="/userfs/bin/qdmamgr_wan"
    else
        qdma_exe="/userfs/bin/qdmamgr_lan"
    fi

    if [ "$rate" -eq 0 ]; then
        return
    else
        "${qdma_exe}" set general_rx_trtcm config "$tsid" enable byte fast enable byte fast
        "${qdma_exe}" set general_rx_trtcm value "$tsid" "$rate" "$rate"

        if [ "$burst_size" -gt 0 ]; then
            "${qdma_exe}" set general_rx_trtcm bsize "$tsid" "$burst_size" "$burst_size"
        fi
    fi
}

# Set ingress rate. In fact, it is not supported
hw_policer_set_ingress_rate() {
    local ifname="$1"
    local ingress_rate="$2"
    local in_burst_size="$3"

    if [ "$ifname" = "$ETHWAN" ]; then
        set_wan_ingress_rate "$ingress_rate"
    else
        set_lan_rate "$ifname" "$ingress_rate" "$in_burst_size" "0"
    fi
}

# Configure shaper rate that will be committed during hw_commit_all()
hw_shaper_set() {
    local ifname="$1"
    local action="$2"
    local rate="$3"
    local burstsize="$4"

    case "${action}" in
        add)
            if [ "$ifname" = "$ETHWAN" ]; then
                export TMP_HW_SHAPE_RATE="$rate"
                set_wan_egress_rate "$rate" "$burstsize"
            else
                set_lan_rate "$ifname" "$rate" "$burstsize" "1"
            fi
            ;;
        del)
            if [ "$ifname" = "$ETHWAN" ]; then
                export TMP_HW_SHAPE_RATE=""
                set_wan_egress_rate "0" "0"
            else
                set_lan_rate "$ifname" "0" "0" "1"
            fi
            ;;
        *)
            return 1
            ;;
    esac

    return 0
}

# Convert shaper in UCI terms to Econet terms
hw_sc_alg2str() {
    local sc_alg="$1"

    case "${sc_alg}" in
        SP)
            echo "PQ"
            ;;
        WRR)
            echo "WRR"
            ;;
        *)
            echo ""
            return 1
    esac

    return 0
}

# Commit all options preserved during
hw_commit_all() {
    local sorted_list="$(echo $TMP_HW_QUEUE_LIST | tr ' ' '\n' | sort | xargs)"
    local weight_list=""
    local glob_alg=""
    local shape_rate="$TMP_HW_SHAPE_RATE"
    local q_count="0"
    local mac_qos_flag=""
    local pbit=0
    local tc=0

    # Reorder queues
    for q in ${sorted_list} ; do
        local sc_alg="$(get_var TMP_HW_QUEUE_${q}_sc_alg)"
        local wgt="$(get_var TMP_HW_QUEUE_${q}_wgt)"

        if [ "$glob_alg" != "" ] && [ "$sc_alg" != "$glob_alg" ] ; then
            errmsg "Not matching scheduling algorithm: $sc_alg vs $glob_alg"
            return 1
        fi

        glob_alg="$sc_alg"

        case "${sc_alg}" in
            WRR)
                if [ $(($q_count >= 8)) != 0 ] ; then
                    errmsg "Too many queues, next queues will be ignored"
                else
                    weight_list="$weight_list $wgt"
                fi
                ;;
        esac

        q_count=$((q_count + 1))
    done

    case "${glob_alg}" in
        WRR)
            mac_qos_flag="8QWRR"
            while [ $((q_count < 8)) != 0 ] ; do
                weight_list="$weight_list 1"
                q_count=$((q_count + 1))
            done

            echo ${mac_qos_flag} > /proc/qdma_wan/mac_qos_flag
            echo "1 ${weight_list}" > /proc/qdma_wan/mac_qos
            ;;
        SP)
            mac_qos_flag="8QPQ"
            q_count="8"

            echo ${mac_qos_flag} > /proc/qdma_wan/mac_qos_flag
            echo "0 0 0 0 0 0 0 0 0" > /proc/qdma_wan/mac_qos
            ;;
    esac

    echo 255 > /proc/qdma_wan/mac_default_queuemask

    rm -f "/tmp/qos/wan_link_shape_rate"
    rm -f "/tmp/qos/wan_link_speed"
    if [ "${glob_alg}" != "" ] ; then
        if [ -n "${shape_rate}" ]; then
            echo "${shape_rate}" > "/tmp/qos/wan_link_shape_rate"
        else
            /usr/sbin/qos-uplink-bandwidth
        fi
    else
        /userfs/bin/qosrule discpline Enable 0
    fi

    if ! strings /proc/device-tree/compatible | grep -qFx econet,en7523; then
        if [ -x /userfs/bin/blapi_cmd ]; then
            echo 1 > /proc/ifc_send_to_ppe
            for tc in $(seq 0 7); do
                if [ -s "/tmp/qos/dscp_values_${tc}_4" ]; then
                    sort -un "/tmp/qos/dscp_values_${tc}_4" | awk 'NR==1{first=$1;last=$1;next}
                                                                   $1 == last+1 {last=$1;next}
                                                                   {system("/userfs/bin/blapi_cmd traffic set_traffic_class DSCP " first*4 " " or(last*4, 0x3) " 1");first=$1;last=first}
                                                                   END{system("/userfs/bin/blapi_cmd traffic set_traffic_class DSCP " first*4 " " or(last*4, 0x3) " 1")}'
                fi
                if [ -s "/tmp/qos/dscp_values_${tc}_6" ]; then
                    [ -s "/tmp/qos/dscp_values_${tc}_4" ] && sort -un "/tmp/qos/dscp_values_${tc}_6" | awk 'NR==1{first=$1;last=$1;next}
                                                                                                            $1 == last+1 {last=$1;next}
                                                                                                            {system("/userfs/bin/blapi_cmd traffic set_traffic_class DSCP " first*4 " " or(last*4, 0x3) " 0");first=$1;last=first}
                                                                                                            END{system("/userfs/bin/blapi_cmd traffic set_traffic_class DSCP " first*4 " " or(last*4, 0x3) " 0")}'
                    sort -un "/tmp/qos/dscp_values_${tc}_6" | awk 'NR==1{first=$1;last=$1;next}
                                                                   $1 == last+1 {last=$1;next}
                                                                   {system("/userfs/bin/blapi_cmd traffic set_traffic_class DSCP " first*4 " " or(last*4, 0x3) " 1");first=$1;last=first}
                                                                   END{system("/userfs/bin/blapi_cmd traffic set_traffic_class DSCP " first*4 " " or(last*4, 0x3) " 1")}'
                fi
            done
        fi

        if [ -x /userfs/bin/ifc ]; then
            echo 1 > /proc/ifc_send_to_ppe
            for pbit in $(seq 0 7); do
                /userfs/bin/ifc add vip pbit $pbit
            done
        fi
    fi

    hw_nat -! > /dev/null 2>&1
}