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