meta-bletchley: Add services to control step motor

For Bletchley platform, we can only power on system by step motor to press power key.
Add tools and service to initialize step motor and control system power
by motor.

Signed-off-by: Allen.Wang <Allen_Wang@quantatw.com>
Change-Id: Ic75352a037566d701b2e362743c527c370b0c2e5
Signed-off-by: Patrick Williams <patrick@stwcx.xyz>

1 files changed, 185 insertions, 0 deletions

diff --git a/meta-facebook/meta-bletchley/recipes-bletchley/motor-ctrl/files/motor-init b/meta-facebook/meta-bletchley/recipes-bletchley/motor-ctrl/files/motor-init
new file mode 100755
index 000000000..cecb99618
--- /dev/null
+++ b/meta-facebook/meta-bletchley/recipes-bletchley/motor-ctrl/files/motor-init
@@ -0,0 +1,185 @@
+#!/bin/bash -e
+
+# Initialize for step motor of sled:
+#   Enable pwm and setup pwm duty
+#   Setup gpio pins for step motor control
+#   Moving step motor back to initial position
+
+export PATH=$PATH:/usr/libexec
+
+PWM_BASE_ADDR="0x1e61"
+DEV_FILE="/dev/mem"
+CALIBRATE_TIMEOUT=120
+
+#Get pwm register address for sledN
+function get_pwm_offset() {
+  local offset=0
+  case $1 in
+    0)
+      offset="0080"
+    ;;
+    1)
+      offset="0090"
+    ;;
+    2)
+      offset="00a0"
+    ;;
+    3)
+      offset="00b0"
+    ;;
+    4)
+      offset="00c0"
+    ;;
+    5)
+      offset="00d0"
+    ;;
+  esac
+
+  echo ${PWM_BASE_ADDR}${offset}
+}
+
+#Get pwm duty register address for sledN
+function get_duty_offset() {
+  local offset=0
+  case $1 in
+    0)
+      offset="0084"
+    ;;
+    1)
+      offset="0094"
+    ;;
+    2)
+      offset="00a4"
+    ;;
+    3)
+      offset="00b4"
+    ;;
+    4)
+      offset="00c4"
+    ;;
+    5)
+      offset="00d4"
+    ;;
+  esac
+
+  echo ${PWM_BASE_ADDR}${offset}
+}
+
+#Enable pwm for sledN
+function open_pwm() {
+    local SLED_NUM="$1"
+    echo "Open pwm of sled$SLED_NUM"
+    #enable BMC PWM
+    if [ ! -c "$DEV_FILE" ]; then
+       mknod /dev/mem c 1 1
+    fi
+
+    PWM_OFFSET=$(get_pwm_offset "$SLED_NUM")
+    DUTY_OFFSET=$(get_duty_offset "$SLED_NUM")
+    echo "setting ${PWM_OFFSET} to 0x000113F3"
+    echo "setting ${DUTY_OFFSET} to 0xFF006400"
+    devmem "$PWM_OFFSET"  32 0x000113F3
+    devmem "$DUTY_OFFSET" 32 0xFF006400
+}
+
+function set_gpio()
+{
+    NET_NAME=$1
+    OUT_VAL=$2
+    mapfile -t -d " " GPIO_INFO < <(gpiofind "$NET_NAME")
+    if [ "${#GPIO_INFO[@]}" -ne 2 ]; then
+        echo "set_gpio: can not find gpio, $NET_NAME"
+        return 1
+    fi
+    echo -n "set_gpio: set $NET_NAME = $OUT_VAL"
+    if ! gpioset "${GPIO_INFO[0]}" "${GPIO_INFO[1]%$'\n'}"="$OUT_VAL"; then
+        echo " failed"
+        return 1
+    fi
+    echo " success"
+    return 0
+}
+
+function get_gpio()
+{
+    NET_NAME=$1
+    RET_VAL=2
+
+    mapfile -t -d " " GPIO_INFO < <(gpiofind "$NET_NAME")
+    if [ "${#GPIO_INFO[@]}" -ne 2 ]; then
+        echo "get_gpio: can not find gpio, $NET_NAME" >&2
+        return 1
+    fi
+    if ! RET_VAL=$(gpioget "${GPIO_INFO[0]}" "${GPIO_INFO[1]%$'\n'}") ; then
+        echo "get_gpio: get ${NET_NAME} failed" >&2
+        return 1
+    fi
+    echo "${RET_VAL}"
+    return 0
+}
+
+#Init gpio pins for step motor control
+function init_gpios() {
+    echo "Init GPIOs:"
+    motor_ctrl_gpio_pins_names=(    "SLED${1}_MD_STBY_RESET"
+                                    "SLED${1}_MD_IOEXP_EN_FAULT"
+                                    "SLED${1}_MD_DIR"
+                                    "SLED${1}_MD_DECAY"
+                                    "SLED${1}_MD_MODE1"
+                                    "SLED${1}_MD_MODE2"
+                                    "SLED${1}_MD_MODE3" )
+
+    for  gpio_name in "${motor_ctrl_gpio_pins_names[@]}"; do
+        set_gpio "$gpio_name"   0
+    done
+}
+
+if [[ "$1" =~ ^(slot[0-5]{1})$ ]] || [[ "$1" =~ ^(sled[0-5]{1})$ ]]; then
+  SLED=$1
+  SLED_NUM=${SLED:4}
+else
+  #show_usage
+  echo "invalid sled name: ${1}"
+  exit 1;
+fi
+
+#Check if sled is present
+SLED_PRESENT=$(get_gpio "presence-sled${SLED_NUM}")
+if [ "$SLED_PRESENT" != 0 ];then
+    echo "${SLED} is not present, skip motor initialize"
+    exit 1
+fi
+
+#Init gpios
+init_gpios "$SLED_NUM"
+
+#enable pwm
+open_pwm "$SLED_NUM"
+
+#SLED{N}_MS_DETECT1  (initial position)
+DETECT_PIN1="SLED${SLED_NUM}_MS_DETECT1"
+INIT_POS=$(get_gpio "$DETECT_PIN1")
+
+if [ "$INIT_POS" -eq 1 ];then
+    time_count=0
+    echo "Making motor back to initial position..."
+    motor-ctrl "$SLED" r >/dev/null
+    while  [ "$INIT_POS" -eq 1 ]  ;do
+        INIT_POS=$(get_gpio "$DETECT_PIN1")
+        sleep 0.1
+        time_count=$(( time_count + 1 ))
+        if [  $time_count -gt $CALIBRATE_TIMEOUT ];then
+            echo "Error: Step motor run over 1 cycle but switch never triggered"
+            break
+        fi
+    done
+    motor-ctrl "$SLED" s >/dev/null
+fi
+
+if [ "$INIT_POS" -eq 0 ];then
+    echo "Motor calibrated to initial position."
+    exit 0
+else
+    echo "Find motor initial position failed"
+    exit 1
+fi