#!/bin/sh
. /lib/functions.sh
include /lib/ethernet

# include common code
. /lib/qos/ip_rule.sh
. /lib/qos/iptables.sh
. /lib/qos/ebtables.sh
. /lib/qos/classify.sh

POLICER_COUNT=0
Q_COUNT=0
SP_Q_PRIO=7

cfg_name=""
cfg_type=""

get_max_linkspeed() {
	local ifname="$1"
	local modes="10000 5000 2500 1000 100 10"
	local speed=""
	local phycap="$(ethtool $ifname | sed -ne '/Supported link modes:/,/:/p' | sed -e 's/.*://' -e 's/^[[:space:]]*//' -e '$d')"

	for i in $modes; do
		if echo "${phycap}" | grep -qe "$i\\D"; then
			speed=$i
			break
		fi
	done

	echo "$speed"
}

get_shaper_rate() {
	rate="$1"

	if [ -z "${rate}" ]; then
		echo "-1"
		return
	fi

	if [ "${rate}" -eq -1 ]; then
		#No shaping
		echo "-1"
		return
	fi

	# Convert the rate from bps to kbps.
	if [ "${rate}" -ge 0 -a "${rate}" -le 100 ];then
		# set as per percentage
		speed=$(get_max_linkspeed "$ifname")
		if [ -z "${speed}" ]; then
			echo "-1"
			return
		fi

		rate=$((speed * rate * 1000 / 100))
	else
		rate=$(($rate / 1000))
	fi

	echo "$rate"
}

get_port_number() {
	[ -z "$1" ] && return
	local ports="0 1 2 3 4 5 6 7 8"
	local units="0 1"
	local port="$1"
	local prt
	local ifname

	for unit in $units; do
		for prt in $ports; do
			ifname="$(ethswctl getifname $unit $prt 2>/dev/null | awk '{print$NF}')"
			if [ "$ifname" == "$port" ]; then
				echo "$unit $prt"
				return
			fi
		done
	done
}

# Function to handle a queue order and
# update total number of queues
handle_q_order() {
	local qid="$1" #queue section ID

	config_get is_enable "$qid" "enable" 1

	# No need to configure disabled queues
	if [ $is_enable == '0' ]; then
		return
	fi

	config_get ifname "$qid" "ifname"
	# If ifname is empty that is good enough to break
	if [ -z "$ifname" ];then
		return
	fi

	# Create precedence file containing queue order per
	# interface.
	local precedence_file="/tmp/qos/$ifname/q_order"
	local q_no=$(cat /tmp/qos/$ifname/q_idx)

	config_get precedence "$qid" "precedence"
	value=${precedence}_q${q_no}
	echo $value >> $precedence_file

	# Update the number of queues per interface.
	q_no=$((q_no + 1))
	echo $q_no > /tmp/qos/$ifname/q_idx
}

# Sort queue, lower value in uci means higher precedence, so this
# function sorts the precedence in decending order
sort_q_by_precedence() {
	ifname="$1"

	local order_file="/tmp/qos/$ifname/q_order"
	local tmp_order_file="/tmp/qos/$ifname/q_order.tmp"

	sort -n -k1 $order_file > $tmp_order_file
	cp $tmp_order_file $order_file
	rm -f $tmp_order_file
}

sort_by_precedence() {
	for interf in $(jsonfilter -i /etc/board.json -e @.network.lan.ports[*] -e @.network.lan.device -e @.network.wan.device | xargs); do
		sort_q_by_precedence $interf
	done
}

# function to handle a queue section shapingrate
handle_queue_shapingrate() {
	local qid="$1" #queue section ID
	local intf_name="$2"

	config_get is_enable "$qid" "enable"

	# no need to configure disabled queues
	if [ $is_enable == '0' ]; then
		return
	fi

	config_get ifname "$qid" "ifname"
	# if ifname is empty that is good enough to break
	if [ -z "$ifname" ];then
		return
	fi

	# This is to get the qid per interface.
	if [ "$intf_name" != "$ifname" ]; then
		return
	fi

	config_get rate "$qid" "rate"

	rate=$(get_shaper_rate "$rate")
	if [ "$rate" -eq -1 ]; then
		return
	fi

	config_get bs "$qid" "burst_size"

	# Call tmctl which is a broadcomm command to configure queues shapingrate.
	tmctl setqcfg --devtype 0 --if $ifname --qid $Q_COUNT --shapingrate $rate --burstsize $bs &>/dev/null

	Q_COUNT=$((Q_COUNT + 1))
}

# function to handle a queue section
handle_queue() {
	local qid="$1" #queue section ID
	local intf_name="$2"

	config_get is_enable "$qid" "enable"

	# no need to configure disabled queues
	if [ $is_enable == '0' ]; then
		return
	fi

	config_get ifname "$qid" "ifname"
	# if ifname is empty that is good enough to break
	if [ -z "$ifname" ];then
		return
	fi

	# This is to get the qid per interface.
	if [ "$intf_name" != "$ifname" ]; then
		return
	fi

	local precedence_file="/tmp/qos/$ifname/q_order"

	local temp_order=0
	while read -r line; do
		line_qid=${line: -1}
		if [ "$line_qid" == "$Q_COUNT" ]; then
			break
		fi
		temp_order=$((temp_order + 1))
	done < "$precedence_file"

	# for sp queue, no matter what the precedence value is configured in the uci file
	# broadcom recommends that the highest precedence queue should have priority value
	#7, the next priority q 6 and so on.
	# Here, we have the index of the queue when sorted by precedence value in the file
	# precedence_file so the order is calculated accordingly.
	local order=`expr $SP_Q_PRIO - $temp_order`

	config_get sc_alg "$qid" "scheduling"
	config_get wgt "$qid" "weight" 1
	config_get rate "$qid" "rate"
	config_get bs "$qid" "burst_size"
	config_get qsize "$qid" "queue_size" 1024

	local salg=1

	case "$sc_alg" in
		"SP") salg=1
		;;
		"WRR") salg=2
		;;
		"WDRR") salg=3
		;;
		"WFQ") salg=4
		;;
	esac

	# ignore precedence value in case of WRR, broadcom recommends that WRR queue should
	# always have precedence value set to 0
	if [ $salg -eq 2 ]; then
	       order=0
	fi

	rate=$(get_shaper_rate "$rate")
	if [ "$rate" -eq -1 ]; then
		return
	fi

	config_get bs "$qid" "burst_size"

	# Call tmctl which is a broadcomm command to configure queues on a port.
	tmctl setqcfg --devtype 0 --if $ifname --qid $Q_COUNT --priority $order --qsize $qsize --weight $wgt --schedmode $salg --shapingrate $rate --burstsize $bs
	Q_COUNT=$((Q_COUNT + 1))
}

iptables_set_traffic_class() {
	IP_RULE="$IP_RULE -j MARK --set-xmark 0x$1/0x$1"
}

# marking value be decimal for linux target as it uses set-mark whereas other
# targets uses set-xmark, hence this function can't make it common
ip_rule_get_converted_tos() {
	con_tos=$(printf %x $1)
	echo $con_tos
}

ebt_match_ipv6_dscp() {
	#when ethertype is not configured by user then both proto rules of ipv4
	#and ipv6 to be installed so update BR6_RULE string as well otherwise
	#update BR_RULE only for installation of ipv6 proto rule only.
	if [ ! -z "$BR6_RULE" ]; then
		BR6_RULE="$BR6_RULE --ip6-tclass $1"
	else
		BR_RULE="$BR_RULE --ip6-tclass $1"
	fi
}

broute_filter_on_dscp() {
	# The broadcom option --ip-dscp-extend actually accepts tos
	# and not dscp and that too in hex, hence, perform the conversion
	# from dscp in uci to tos first and then convert to hex
	tos_val=$(($1<<2))
	tos_hex=$(printf "%x" $tos_val)
	BR_RULE="$BR_RULE --ip-dscp-extend $tos_hex"
}

broute_ipv4_rule_options()
{
	local cid=$1
	config_get protocol "$cid" "proto"
	config_get dscp_filter "$cid" "dscp_filter"
	config_get icmp_type "$cid" "icmp_type"

	set_ip_addr $cid ebt_match_src_ip ebt_match_dst_ip

	if [ ! -z $dscp_filter ]; then
		broute_filter_on_dscp "$dscp_filter"
	fi

	if [ ! -z $protocol ]; then
		local proto_num=$(protocol_string_to_num "$protocol")
		ebt_match_ip_protocol "$proto_num"

		#port installation for protol tcp/udp/sctp
		if [ $proto_num = "6" ] || [ $proto_num = "17" ] || [ $proto_num = "132" ] ; then
			set_ports "$cid" ebt_match_ip_src_port ebt_match_ip_dst_port
		elif [ "$proto_num" = "1" -a -n "$icmp_type" ]; then
			ebt_match_ip_icmp_type "$icmp_type"
		fi
	fi
}

broute_ipv6_rule_options()
{
	local cid=$1

	config_get protocol "$cid" "proto"
	config_get dscp_filter "$cid" "dscp_filter"
	config_get icmp_type "$cid" "icmp_type"

	set_ip_addr $cid ebt_match_ipv6_src_ip ebt_match_ipv6_dst_ip

	if [ ! -z $dscp_filter ]; then
		local tos_val
		local tos_hex

		tos_val=$(($dscp_filter<<2))
		tos_hex=$(printf "%x" $tos_val)
		ebt_match_ipv6_dscp "$tos_hex"
	fi

	if [ ! -z $protocol ]; then
		local proto_num=$(protocol_string_to_num "$protocol")
		ebt_match_ipv6_protocol "$proto_num"

		#port installation for protol tcp/udp/sctp
		if [ $proto_num = "6" ] || [ $proto_num = "17" ] || [ $proto_num = "132" ]; then
			set_ports "$cid" ebt_match_ipv6_src_port ebt_match_ipv6_dst_port
		elif [ "$proto_num" = "58" -a -n "$icmp_type" ]; then
			ebt_match_ipv6_icmp_type "$icmp_type"
		fi
	fi
}

broute_rule_set_traffic_class() {
	#when ethertype is not configured by user then both proto rules of ipv4
	#and ipv6 to be installed so update BR6_RULE string as well otherwise
	#update BR_RULE only for installation of ipv6 proto rule only.
	BR_RULE="$BR_RULE -j mark --mark-or 0x$1 --mark-target ACCEPT"
	if [ ! -z "$BR6_RULE" ]; then
		BR6_RULE="$BR6_RULE -j mark --mark-or 0x$1 --mark-target ACCEPT"
	fi
}

#function to handle a policer section
handle_policer() {
	local p_sec="$1" # policer section ID
	local dir=1 # default direction, upstream

	config_get is_enable "$p_sec" "enable"

	#no need to configure disabled policer
	if [ $is_enable == '0' ]; then
		return
	fi

	config_get cir "$p_sec" "committed_rate"
	config_get cbs "$p_sec" "committed_burst_size" -1
	config_get ebs "$p_sec" "excess_burst_size" 0
	config_get pir "$p_sec" "peak_rate" 0
	config_get pbs "$p_sec" "peak_burst_size" 0
	config_get meter "$p_sec" "meter_type" 0

	# Call tmctl which is a broadcomm command to configure policer.
	tmctl createpolicer --dir $dir --pid $POLICER_COUNT --ptype $meter --cir $cir --cbs $cbs --ebs $ebs --pir $pir --pbs $pbs

	POLICER_COUNT=$((POLICER_COUNT + 1))
}

#function to handle a shaper section
handle_shaper() {
	sid="$1" #queue section ID

	config_get is_enable "$sid" "enable"
	# no need to configure disabled queues
	if [ $is_enable == '0' ]; then
		return
	fi

	config_get ifname "$sid" "ifname"
	# if ifname is empty that is good enough to break
	if [ -z "$ifname" ];then
		return
	fi

	config_get rate "$sid" "rate"

	rate=$(get_shaper_rate "$rate")
	if [ "$rate" -eq -1 ]; then
		return
	fi

	config_get bs "$sid" "burst_size"
	tmctl setportshaper --devtype 0 --if $ifname --shapingrate $rate --burstsize $bs
}

assign_policer_to_port() {
	local ifname="$1"
	local pindex="$2"
	local portorder="$(jsonfilter -i /etc/board.json -e @.network.lan.ports[*] -e @.network.lan.device -e @.network.wan.device | xargs)"

	for port in $portorder; do
		if [ "$ifname" == "$port" ]; then
			bdmf_shell -c `cat /var/bdmf_sh_id` -cmd /b/configure port/name=$port ingress_rate_limit={traffic_types=8,policer={policer/dir=us,index=$pindex}}
			break
		fi
	done
}

handle_policer_rules() {
	local c_sec=$1
	local policer_name
	local ifname
	local pname
	local pindex=-1
	local ingress_rate=0
	local in_burst_size=0

	config_get policer_name "$c_sec" "policer"
	if [ -z "$policer_name" ];then
		# no need to apply policer if policer not present in this
		# classification rule
		return
	fi

	config_get ifname "$c_sec" "ifname"
	if [ -z "$ifname" ]; then
		# cannot associate policer as interface is not mentioned
		return
	fi

	local i=0
	local max_policer_inst=$(cat /tmp/qos/max_policer_inst)
	while :
	do
		if [ $i -eq $max_policer_inst ]; then
			break
		fi

		pname="$(uci -q get qos.@policer[$i].name)"
		if [ "$policer_name" == "$pname" ]; then
			pindex=$i
			ingress_rate=$(uci -q get qos.@policer[$i].committed_rate)
			in_burst_rate=$(uci -q get qos.@policer[$i].committed_burst_size)
			break
		fi
		i=$((i + 1))
	done

	if [ $pindex -lt 0 ]; then
		# policer not found, no need to proceed further
		return
	fi

	config_ingress_rate_limit $ifname $ingress_rate $in_burst_size

}

config_ingress_rate_limit() {
	local ifname="$1"
	local ingress_rate=$2
	local in_burst_size=$3
	local wanport="$(jsonfilter -i /etc/board.json -e @.network.wan.device)"

	# Unit in uci file is in bps while that accepted by ethswctl is kbits
	if [ $ingress_rate -lt 1000 ]; then
		return
	fi

	ingress_rate=$((ingress_rate / 1000))

	if [ $in_burst_size -eq 0 ]; then
		in_burst_size=$ingress_rate
	else
		in_burst_size=$((in_burst_size / 1000))
	fi

	local unitport="$(get_port_number $ifname)"
	local unit=$(echo $unitport | cut -d ' ' -f 1)
	local port=$(echo $unitport | cut -d ' ' -f 2)
	ethswctl -c rxratectrl -n $unit -p $port -x $ingress_rate -y $in_burst_size
}

configure_shaper() {
	# Delete existing shaper
	for intf in $(jsonfilter -i /etc/board.json -e @.network.lan.ports[*] -e @.network.lan.device -e @.network.wan.device | xargs); do
		tmctl setportshaper --devtype 0 --if $intf --shapingrate 0 --burstsize -1
	done
	# Load UCI file
	config_load qos
	# Processing shaper section(s)
	config_foreach handle_shaper shaper
}

pre_configure_queue() {
	# Delete queues
	for intf in $(jsonfilter -i /etc/board.json -e @.network.lan.ports[*] -e @.network.lan.device -e @.network.wan.device | xargs); do
		rm -rf /tmp/qos/$intf

		mkdir -p /tmp/qos/$intf
		touch /tmp/qos/$intf/q_order
		touch /tmp/qos/$intf/q_idx
		echo 0 > /tmp/qos/$intf/q_idx

		tmctl porttminit --devtype 0 --if $intf --numqueues 8

		i=0
		for i in 0 1 2 3 4 5 6 7; do
			tmctl delqcfg --devtype 0 --if $intf --qid $i &>/dev/null
		done
	done
}

configure_queue_shaping_rate() {
	# Load UCI file
	config_load qos
	for interf in $(jsonfilter -i /etc/board.json -e @.network.lan.ports[*] -e @.network.lan.device -e @.network.wan.device | xargs); do
		Q_COUNT=0
		config_foreach handle_queue_shapingrate queue $interf
	done
}

configure_queue() {
	# Load UCI file
	config_load qos
	config_foreach handle_q_order queue
	sort_by_precedence
	for interf in $(jsonfilter -i /etc/board.json -e @.network.lan.ports[*] -e @.network.lan.device -e @.network.wan.device | xargs); do
		Q_COUNT=0
		# sp queue have max priority value = no. of queue configured on the port
		# hence read and update SP_Q_PRIO here
		local q_no=$(cat /tmp/qos/$interf/q_idx)
		SP_Q_PRIO=`expr $q_no - 1`
		config_foreach handle_queue queue $interf
	done
}

configure_policer() {
	# The policer object is not available on non BCM968* chips, just clean up
	# the old config if any and return
	for intf in $(jsonfilter -i /etc/board.json -e @.network.lan.ports[*] -e @.network.lan.device -e @.network.wan.device | xargs); do
		local unitport="$(get_port_number $intf)"
		local unit=$(echo $unitport | cut -d ' ' -f 1)
		local port=$(echo $unitport | cut -d ' ' -f 2)
		# setting rate and burst size to 0 disables rate limiting
		if [ $port != "" -a $unit != "" ]; then
			ethswctl -c rxratectrl -n $unit -p $port -x 0 -y 0 &>/dev/null
		fi
	done

	# Delete policer
	local i=0
	local max_p_inst=0
	if [ -f "/tmp/qos/max_policer_inst" ]; then
		max_p_inst=$(cat /tmp/qos/max_policer_inst)
	fi

	while :
	do
		if [ $i -eq $max_p_inst ]; then
			break
		fi
		tmctl deletepolicer --dir 1 --pid $i &>/dev/null
		i=$((i + 1))
	done

	# reset the policer counter
	echo 0 > /tmp/qos/max_policer_inst
	# Load UCI file
	config_load qos
	config_foreach handle_policer policer
	echo $POLICER_COUNT > /tmp/qos/max_policer_inst
}

configure_qos() {
	#queue configuration is being done after shaper configuration,
	#If port shapingrate configuration on DISC device is called after queue configuration then
	#driver overwrites the queue shaping rate with default value of port shaping rate.
	pre_configure_queue
	configure_shaper
	configure_queue
	configure_policer
	configure_classify
	if [ -f "/tmp/qos/classify.ebtables" ]; then
		sh /tmp/qos/classify.ebtables
	fi
	# broadcom recommends that each time traffic class is set,
	# the flows should be flushed for the new mapping to take
	# effect, it then makes sense to make it a part of the
	# qosmngr package itself.
	fcctl flush
}

reload_qos() {
	local service_name="$1"

	if [ -z "$service_name" ]; then
		configure_qos
	elif [ "$service_name" == "shaper" ]; then
		configure_shaper
		# call to configure queue shaping rate as in some platform port shaping rate
		# configuration overwrites the queue shaping rate in driver to default value
		# this will restore the queue shping rate
		configure_queue_shaping_rate
	elif [ "$service_name" == "queue" ]; then
		pre_configure_queue
		configure_queue
	elif [ "$service_name" == "classify" ]; then
		configure_classify
		if [ -f "/tmp/qos/classify.ebtables" ]; then
			sh /tmp/qos/classify.ebtables
		fi
		# broadcom recommends that each time traffic class is set,
		# the flows should be flushed for the new mapping to take
		# effect, it then makes sense to make it a part of the
		# qosmngr package itself.
		fcctl flush
	elif [ "$service_name" == "policer" ]; then
		configure_policer
	fi
}

get_cfg_added_deleted()
{
	# return true if there is difference in number of queue configuration
	# in /etc/config/qos and /tmp/qos/qos, false is returned if both file
	# has same count of queue configuration.
	local queue=0
	local classify=0
	local shaper=0
	local policer=0
	local old_cfg="false"

        config_cb() {
		# invoked on the just previous config_load, get the count of
		# queue configuration in /etc/config/qos and /tmp/qos/qos.
                cfg_type="$1"
                cfg_name="$2"
                if [ -z $cfg_name ] || [ -z $cfg_type ]; then
                        return
                fi

                if [ "$old_cfg" == "false" ]; then
			eval $cfg_type=$(($cfg_type + 1))
		else
			eval $cfg_type=$(($cfg_type - 1))
		fi
                option_cb() {
                        local option="$1"
                        local value="$2"
                        if [ -z "$option" ] || [ -z "$value" ]; then
                                return
                        fi
                        if [ "$old_cfg" == "false" ]; then
				eval $cfg_type=$(($cfg_type + 1))
			else
				eval $cfg_type=$(($cfg_type - 1))
			fi
		}
	}
	# config_load will trigger call for config_cb that is getting count
	# of qos configuration, respective config counts will come.
	config_load qos
	# config_load will trigger call for config_cb that is decreasing count
	# of qos configuration.
	old_cfg="true"
	UCI_CONFIG_DIR="/tmp/qos"
	config_load qos
	UCI_CONFIG_DIR="/etc/config"
	reset_cb

        if [ $classify -ne 0 ]; then
                modified_config="classify"
	fi
        if [ $shaper -ne 0 ]; then
                modified_config="$modified_config shaper"
	fi
        if [ $policer -ne  0 ]; then
                modified_config="$modified_config policer"
	fi
	if [ $queue -eq 0 ]; then
		echo "$modified_config"
		return
	else
		echo "queue"
		return
	fi
}

# reload_qos_service is invoked on qos service reload.
reload_qos_service() {
	q_cfg_restart="false"
	policer="false"
	classify="false"
	shaper="false"

	setup_qos

        if ! [[ -f "/etc/config/qos"  && -f "/tmp/qos/qos" ]]; then
		configure_qos
		cp /etc/config/qos /tmp/qos/qos
	fi

        config_cb() {
		# this is invoked when config_load is called.
                cfg_type="$1"
                cfg_name="$2"
                if [ -z $cfg_name ] || [ -z $cfg_type ]; then
                        return
                fi
                option_cb() {
                        # checking for if any parameters value is modified in queue cfg
                        # comparsion is done between /etc/config/qos and /tmp/qos/qos
                        local option="$1"
                        local value="$2"
			local old_value=""
                        if [ -z "$option" ] || [ -z "$value" ]; then
                                return
                        fi
                        old_value=$(uci -q -c "/tmp/qos" get qos.$cfg_name.$option)
                        if ! [ "$old_value" == "$value" ]; then
                                if [ "$cfg_type" == "queue" ]; then
                                        q_cfg_restart="true"
                                else
                                        eval $cfg_type="true"
                                fi
                        fi
                }
        }
	# if there is new addition/deletion of queue configuration
	# then return is queue to trigger restart of qos.
	# Otehrwise shaper policer classify
	# respective operation config is invoked that does not change queue statistics
	cfg_added_deleted=$(get_cfg_added_deleted)
	if [ "$cfg_added_deleted" == "queue" ]; then
		configure_qos
	else
		q_cfg_restart="false"
		# config_load will trigger call for config_cb that is checking
		# for modification in config value of queue config.
		# if change of value of queue config is there then q_cfg_restart
		# is set as true, else other qos config flag is set as true.
		config_load qos
		reset_cb
		if [ "$q_cfg_restart" == "true" ]; then
			configure_qos
		else
			for config in $cfg_added_deleted
			do
				eval $config="true"
			done
			if [ "$shaper" == "true" ]; then
				reload_qos "shaper"
			fi
			if [ "$policer" == "true" ]; then
				reload_qos "policer"
				# change in policer config may need reconfiguration 
				# of classifier
				reload_qos "classify"
			fi
			if [ "$classify" == "true" ]; then
				reload_qos "classify"
			fi
		fi
	fi
	cp /etc/config/qos /tmp/qos/qos
}
