#!/bin/sh

IFS=,

if [ -z "$MDIO_CONTROL" ]; then
	MDIO_CONTROL=/sys/kernel/debug/mdio/f0028000.ethernet/control
fi

verbose=
phy_chipaddr=
phy_page=
phy_address=
phy_device=
phy_offset=
phy_bits=
phy_value=

while [ $# != 0 ]; do
	case $1 in
	-v|--verbose)
		verbose=1
		;;
	-n|--no-write)
		no_write=1
		;;
	-L|--link)
		MDIO_CONTROL=/sys/kernel/debug/mdio/f802c000.ethernet/control
		;;
	-S|--switch)
		MDIO_CONTROL=/sys/kernel/debug/mdio/f0028000.ethernet/control
		;;
	-P|--phy)
		MDIO_CONTROL=$(echo /sys/kernel/debug/mdio/!ahb!apb!ethernet@f0028000!switch0@*!mdio/control)
		;;
	-m|--multichip)
		shift
		phy_chipaddr=$1
		;;
	-p|--page)
		shift
		phy_page=$1
		;;
	-a|--address)
		shift
		phy_addresses=$1
		;;
	-d|--device)
		shift
		phy_device=$1
		;;
	-o|--offset)
		shift
		phy_offset=$1
		;;
	-b|--bits)
		shift
		phy_bits=$1
		;;
	-w|--write)
		shift
		phy_value=$1
		;;
	esac
	shift
done

parse_bits() {
	bit_stop=${1%:*}
	bit_start=${1#*:}
	bit_stop=$((bit_stop + 1))
}

_get_bits() {
	local bit_start bit_stop
	parse_bits "$1"
	value="$2"

	printf "0x%.04x\n" "$(( (value & ((1<<bit_stop) - 1)) >> bit_start ))"
}

_set_bits() {
	local original="$1"
	local bit_start bit_stop
	parse_bits "$2"
	local value="$3"

	local mask="$(( (1<<bit_stop) - (1<<bit_start) ))"
	printf "0x%.04x\n" "$(( (original & ~mask) | ((value << bit_start) & mask) ))"
}

direct_cmd() {
	echo -n "$1" >&3
}

direct_read() {
	direct_cmd $(printf "%.2x:%.2x\n" "$1" "$2")
	echo "0x$(head -n 1 <&3)"
}

direct_write() {
	direct_cmd $(printf "%.2x:%.2x:%.4x\n" "$1" "$2" "$3")
}

indirect_wait() {
	ret=$(direct_read "$phy_chipaddr" 0x00)
	while [ $(( ret & 0x8000 )) != 0 ]; do
		sleep 1
		ret=$(direct_read "$phy_chipaddr" 0x00)
	done
}

indirect_read() {
	address="$1"
	offset="$2"
	cmd=$(printf "0x%.4x" "$(( 0x9800 | ((address & 0x1f) << 5) | (offset & 0x1f) ))")
	indirect_wait
	direct_write "$phy_chipaddr" 0x00 "$cmd"
	indirect_wait
	direct_read "$phy_chipaddr" 0x01
}

indirect_write() {
	address="$1"
	offset="$2"
	data="$3"
	cmd=$(printf "0x%.4x" "$(( 0x9400 | ((address & 0x1f) << 5) | (offset & 0x1f) ))")
	indirect_wait
	direct_write "$phy_chipaddr" 0x01 "$data"
	direct_write "$phy_chipaddr" 0x00 "$cmd"
	indirect_wait
}

mdio_read() {
	local address="$1"
	local offset="$2"
	local bits="$3"
	[ "$verbose" = 1 ] && printf "read: 0x%.2x:0x%.2x\n" "$address" "$offset" >&2
	if [ -n "$phy_chipaddr" ]; then
		value=$(indirect_read "$address" "$offset")
	else
		value=$(direct_read "$address" "$offset")
	fi
	[ $? = 0 ] || echo "Read error: address=$1 offset=$2" >&2

	if [ -z "$bits" ]; then
		echo $value
	else
		_get_bits "$bits" "$value"
	fi
}

mdio_write() {
	local address="$1"
	local offset="$2"
	local bits="$3"
	local value="$4"

	if [ -n "$bits" ]; then
		original="$(mdio_read "$address" "$offset")"
		value="$(_set_bits "$original" "$bits" "$value")"
	fi

	[ -n "$verbose" -a -n "$no_write" ] && printf "no "
	[ -n "$verbose" ] && printf "write: 0x%.2x:0x%.2x = %.4x\n" "$address" "$offset" "$value" >&2
	if [ -z "$no_write" ]; then
		if [ -n "$phy_chipaddr" ]; then
			indirect_write "$address" "$offset" "$value"
		else
			direct_write "$address" "$offset" "$value"
		fi
		[ $? = 0 ] || echo "Write error: address=$1 offset=$2 value=$3" >&2
	fi
}

# TODO: Remember and reset original page rather than 0x0001.
mdio_page_set() {
	if [ -n "$phy_page" ]; then
		phy_orig_page="$(mdio_read "$1" 0x16)"
		mdio_write "$1" 0x16 "$phy_page"
	fi
}

mdio_page_reset() {
	[ -n "$phy_page" ] && mdio_write "$1" 0x16 "$(phy_orig_page)"
}

usage() {
	echo "Get c22 register:"
	echo "  mdio --phy --address 0 --offset 0x00"
	echo "  mdio -P -a 0 -o 0x00"
	echo "Set c22 register:"
	echo "  mdio --phy --address 0 --offset 0x00 --write 0x8000"
	echo "  mdio -P -a 0 -o 0x00 -w 0x8000"
	echo "Note: Read the source."
	exit 1
}

exec 3<>"$MDIO_CONTROL"
[ -n "$phy_addresses" -a -n "$phy_offset" ] || usage

for phy_address in $phy_addresses; do
	[ -n "$phy_device" ] && phy_offset=$(( phy_offset | phy_device << 16 | 0x40000000 ))

	mdio_page_set "$phy_address"
	if [ -n "$phy_value" ]; then
		mdio_write "$phy_address" "$phy_offset" "$phy_bits" "$phy_value"
	else
		mdio_read "$phy_address" "$phy_offset" "$phy_bits"
	fi
	mdio_page_reset "$phy_address"
done