#!/bin/bash

ID_VENDOR="0x1d6b"
ID_PRODUCT="0x0104"
ID_MANU="Zefie Networks"
ID_PROD="Raspberry Pi Zero W Gadget"
USB_FILE="/dev/mmcblk0p3"
USB_IS_ISO=0
USB_IS_RO=0

functions=("rndis.usb0" "mass_storage.usb0" "acm.usb0")

g="/sys/kernel/config/usb_gadget/g"


# Get serial from Pi /proc/cpuinfo

SERIAL="$(grep Serial /proc/cpuinfo | sed 's/Serial\s*: 0000\(\w*\)/\1/')"
if [ -z "${SERIAL}" ]; then
        # Minimum serial length is 8 (for MAC conversion)
        SERIAL=00000001
fi

# Generate MAC from last 8 characters of serial
# Dev = serial, Host = serial + 1 hex byte
# eg: Serial: 1234f54b Dev: B8:27:EB:34:F5:4B Host: B8:27:EB:34:F5:4C

MAC="$(echo "${SERIAL}" | rev | sed 's/\(\w\w\)/:\1/g' | cut -b 2- | rev)"
MAC_DEV="02:00:$(echo "${MAC}" | rev | cut -b -11 | rev)"
MAC_HOST_PRE=$(perl -le 'print hex("'"$(echo "${MAC_DEV}" | rev | cut -d':' -f1 | rev)"'")')
MAC_HOST="$(echo "${MAC_DEV}" | rev | cut -d':' -f2- | rev):$(printf "%02X" "$((MAC_HOST_PRE + 1))")"

check_debug() {
        if [ ! -z "${GADGET_DEBUG}" ]; then
                echo 1;
                return 1;
        fi
        echo 0;
        return 0;
}

read_ffs() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "  ${FUNCNAME[0]} $*"
                return;
        fi
        if [ -e "${g}/${1}" ]; then
                if [ -d "${g}/${1}" ]; then
                        ls -l "${g}/${1}"
                else
                        echo "${1}: $(cat "${g}/${1}")"
                fi
        fi
}

write_ffs() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "  ${FUNCNAME[0]} $*"
                echo "     echo \"${2}\" > \"${g}/${1}\""
                return;
        fi

        if [ ! -d "${g}/$(dirname "${1}")" ]; then
                mkdir -p "${g}/$(dirname "${1}")"
        fi
        if [ -f "${g}/${1}" ]; then
                if [ "$(cat "${g}/${1}")" != "${2}" ]; then
                        echo "${2}" > "${g}/${1}"
                        if [ $? -ne 0 ]; then
                                echo "Debug: echo \"${2}\" > \"${g}/${1}\""
                        fi
                fi
        else
                echo "${2}" > "${g}/${1}"
        fi
}

config_ffs() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]} $*"
        fi
        write_ffs "configs/c.1/${1}" "${2}"
}

activate_function() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]} $*"
                echo "  ln -s \"${g}/functions/${1}\" \"${g}/configs/c.1/\""
                return;
        fi
        ln -s "${g}/functions/${1}" "${g}/configs/c.1/"
}

activate_usb() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]}"
                return;
        fi
        udevadm settle -t 5 || :
        write_ffs UDC "$(ls -1 /sys/class/udc)"
}

config_function() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]} $*"
        fi
        write_ffs "functions/${1}/${2}" "${3}"
}

create_function() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]} $*"
                echo "  mkdir -p \"${g}/functions/${1}\""
                return;
        fi
        mkdir -p "${g}/functions/${1}"
}

deactivate_function() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]} $*"
                return;
        fi
        rm "${g}/configs/c.1/${1}"
}

deactivate_usb() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]}"
        fi
        write_ffs UDC ""
}

destroy_function() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]} $*"
                return;
        fi
        if [ "$(read_ffs "configs/c.1/${1}" | wc -l)" -gt 0 ]; then
                deactivate_function "${f}"
        fi
        rmdir "${g}/functions/${1}"
}

link_osdesc() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]}"
                echo "  ln -s \"${g}/configs/c.1\" \"${g}/os_desc/\""
                return;
        fi
        ln -s "${g}/configs/c.1" "${g}/os_desc/"
}

ms_update() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "  ${FUNCNAME[0]} $*"
        fi
        deactivate_usb
        function_init mass_storage.usb0 1
        config_function mass_storage.usb0 "lun.${1}/cdrom" "${2}"
        config_function mass_storage.usb0 "lun.${1}/ro" "${3}"
        config_function mass_storage.usb0 "lun.${1}/file" "${4}"
        activate_function mass_storage.usb0
        sleep 1
        activate_usb
}

ms_mount_iso() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]} $*"
        fi
        ms_update 0 1 1 "${1}"
}

ms_mount_rw() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]} $*"
        fi
        ms_update 0 0 0 "${1}"
}

ms_mount_ro() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]} $*"
        fi
        ms_update 0 0 1 "${1}"
}

restart_function() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]} $*"
        fi
        deactivate_usb
        function_init "${1}"
        sleep 1
        activate_usb
}

function_init() {
        if [ "$(check_debug)" -eq 1 ]; then
                echo "${FUNCNAME[0]} $*"
        fi
        if [ ! -z "${1}" ]; then
                if [ "$(read_ffs "configs/c.1/${1}" | wc -l)" -gt 0 ]; then
                        deactivate_function "${1}"
                fi

                if [ "$(read_ffs "functions/${1}" | wc -l)" -gt 0 ]; then
                        destroy_function "${1}"
                fi

                create_function "${1}"
                case "${1}" in
                        mass_storage.usb0)
                                config_function "${1}" stall 0
                                config_function "${1}" lun.0/cdrom "${USB_IS_ISO}"
                                config_function "${1}" lun.0/nofua 0
                                config_function "${1}" lun.0/ro "${USB_IS_RO}"
                                config_function "${1}" lun.0/removable 1
                                config_function "${1}" lun.0/file "${USB_FILE}"
                                ;;

                        rndis.usb0)
                                config_function "${1}" host_addr "${MAC_HOST}"
                                config_function "${1}" dev_addr "${MAC_DEV}"
                                config_function "${1}" os_desc/interface.rndis/compatible_id RNDIS
                                config_function "${1}" os_desc/interface.rndis/sub_compatible_id 5162001
                                ;;
                esac
                if [ -z "${2}" ]; then
                        activate_function "${1}"
                fi
        fi
}

if [ "${USB_IS_ISO}" -eq 1 ]; then
        USB_IS_RO=1
fi

if [ -z "${1}" ]; then

        modprobe libcomposite

        deactivate_usb

        write_ffs idVendor "${ID_VENDOR}"
        write_ffs idProduct "${ID_PRODUCT}"
        write_ffs bcdDevice 0x0100
        write_ffs bcdUSB 0x0200
        write_ffs bDeviceClass 0xEF
        write_ffs bDeviceSubClass 0x02
        write_ffs bDeviceProtocol 0x01

        write_ffs strings/0x409/serialnumber "${SERIAL}"
        write_ffs strings/0x409/manufacturer "${ID_MANU}"
        write_ffs strings/0x409/product "${ID_PROD}"

        write_ffs os_desc/use 1
        write_ffs os_desc/b_vendor_code 0xcd
        write_ffs os_desc/qw_sign MSFT100

        config_ffs MaxPower 250

        link_osdesc

        for f in "${functions[@]}"; do
                function_init "${f}"
        done

        activate_usb
else
        $@
fi