#!/bin/sh
set -eu

CONFIG="${PRISM_POWER_PROFILE_CONFIG:-/etc/default/prism-power-profile}"
SYSFS_ROOT="${PRISM_POWER_SYSFS_ROOT:-/sys}"
RUN_DIR="${PRISM_POWER_RUN_DIR:-/run}"
ISP_HELPER="${PRISM_ISP_POWER_HELPER:-/usr/sbin/prism-isp-lowpower}"
IRQ_HELPER="${PRISM_IRQ_AFFINITY_HELPER:-/usr/sbin/prism-irq-affinity}"
AGENT_AFFINITY_HELPER="${PRISM_AGENT_AFFINITY_HELPER:-/usr/bin/taskset}"
AGENT_PID="${PRISM_AGENT_PID:-}"
PPS_CPU="${PRISM_PPS_CPU:-3}"
PPS_WORKQUEUE_CPUMASK="${PRISM_PPS_WORKQUEUE_CPUMASK:-f7}"
IDLE_CPUS="0 1 2 3"
IMU_CPUS="0 1 2 3"
CAMERA_CPUS="0 1 2 3"
OFFLINE_CPUS="4 5 6 7"
CPU_MIN_FREQ_KHZ="408000"
IDLE_CPU_MAX_FREQ_KHZ="816000"
IMU_CPU_MAX_FREQ_KHZ="816000"
CAMERA_CPU_MAX_FREQ_KHZ="1608000"
DDR_MAX_FREQ_HZ="1968000000"
CAMERA_DDR_MAX_FREQ_HZ="1968000000"
CAMERA_ISP_MAX_FREQ_HZ="594000000"
DDR_DEVFREQ_MATCH="dmc ddr"
PERFORMANCE_DEVFREQ_MATCH="dmc ddr npu gpu mali"
MAXIMUM_PERFORMANCE="0"
AUTOMATIC_SCALING="0"

if [ -r "$CONFIG" ]; then
  # shellcheck disable=SC1090
  . "$CONFIG"
fi

# This image's DDR qualification ceiling applies to every performance mode.
# Older configs may still request 2736 MHz; do not let those bypass the cap.
case "$DDR_MAX_FREQ_HZ" in
  ''|*[!0-9]*) DDR_MAX_FREQ_HZ="1968000000" ;;
esac
if [ "$DDR_MAX_FREQ_HZ" -eq 0 ] ||
   [ "$DDR_MAX_FREQ_HZ" -gt 1968000000 ]; then
  DDR_MAX_FREQ_HZ="1968000000"
fi

# Downloads use the validated camera CPU/DDR ceilings without powering up ISP.
# Defaults inherit both low-power and full-performance product configuration.
DOWNLOAD_CPUS="${DOWNLOAD_CPUS:-$CAMERA_CPUS}"
DOWNLOAD_CPU_MAX_FREQ_KHZ="${DOWNLOAD_CPU_MAX_FREQ_KHZ:-$CAMERA_CPU_MAX_FREQ_KHZ}"
DOWNLOAD_DDR_MAX_FREQ_HZ="${DOWNLOAD_DDR_MAX_FREQ_HZ:-$CAMERA_DDR_MAX_FREQ_HZ}"

write_if_writable() {
  path="$1"
  value="$2"
  [ -n "$value" ] || return 0
  [ -w "$path" ] || return 0
  printf '%s\n' "$value" >"$path" 2>/dev/null || true
}

set_cpu_online() {
  cpu="$1"
  value="$2"
  path="$SYSFS_ROOT/devices/system/cpu/cpu${cpu}/online"
  [ -e "$path" ] || return 0
  current="$(cat "$path" 2>/dev/null || true)"
  [ "$current" = "$value" ] && return 0
  write_if_writable "$path" "$value"
}

cpu_in_list() {
  wanted="$1"
  shift
  for item in "$@"; do
    [ "$item" = "$wanted" ] && return 0
  done
  return 1
}

policy_contains_cpu() {
  policy="$1"
  cpu="$2"
  related="$(cat "$policy/related_cpus" 2>/dev/null || true)"
  case " $related " in
    *" $cpu "*) return 0 ;;
  esac
  return 1
}

set_cpu_profile() {
  online_cpus="$1"
  max_freq="$2"

  for cpu in $online_cpus; do set_cpu_online "$cpu" 1; done
  for cpu in $OFFLINE_CPUS 0 1; do
    # shellcheck disable=SC2086
    if ! cpu_in_list "$cpu" $online_cpus; then set_cpu_online "$cpu" 0; fi
  done

  for policy in "$SYSFS_ROOT"/devices/system/cpu/cpufreq/policy*; do
    [ -d "$policy" ] || continue
    applies=0
    for cpu in $online_cpus; do
      if policy_contains_cpu "$policy" "$cpu"; then applies=1; break; fi
    done
    [ "$applies" -eq 1 ] || continue
    write_if_writable "$policy/scaling_governor" schedutil
    write_if_writable "$policy/scaling_min_freq" "$CPU_MIN_FREQ_KHZ"
    write_if_writable "$policy/scaling_max_freq" "$max_freq"
  done
}

configure_pps_realtime_cpu() {
  # CPU3 is reserved for the GPIO0 PPS parent interrupt. Keep generic unbound
  # work off it, and use WFI instead of the 370 us cpu-sleep state so the edge
  # timestamp is not delayed while image traffic is active.
  write_if_writable \
    "$SYSFS_ROOT/devices/virtual/workqueue/cpumask" \
    "$PPS_WORKQUEUE_CPUMASK"

  online="$SYSFS_ROOT/devices/system/cpu/cpu${PPS_CPU}/online"
  if [ -e "$online" ] && [ "$(cat "$online" 2>/dev/null || printf 0)" != 1 ]; then
    return 0
  fi
  for state in \
    "$SYSFS_ROOT/devices/system/cpu/cpu${PPS_CPU}/cpuidle/"state[1-9]*; do
    [ -e "$state/disable" ] || continue
    write_if_writable "$state/disable" 1
  done
}

apply_agent_affinity() {
  online_cpus="$1"
  affinity=""
  pid="$AGENT_PID"

  # Keep userspace work away from the CPU reserved for the PPS interrupt.
  # Apply the mask only after the profile has brought its CPUs online.  A
  # systemd CPUAffinity applied while only CPU0 is online is permanently
  # reduced to CPU0 and does not expand when the camera profile starts.
  for cpu in $online_cpus; do
    [ "$cpu" = "$PPS_CPU" ] && continue
    [ -z "$affinity" ] || affinity="$affinity,"
    affinity="${affinity}${cpu}"
  done
  [ -n "$affinity" ] || return 0
  [ -x "$AGENT_AFFINITY_HELPER" ] || return 0

  if [ -z "$pid" ]; then
    pids="$(pidof prism-agent 2>/dev/null || true)"
    for candidate in $pids; do
      pid="$candidate"
      break
    done
  fi
  [ -n "$pid" ] || return 0
  "$AGENT_AFFINITY_HELPER" -apc "$affinity" "$pid" >/dev/null 2>&1 || true
}

node_matches_ddr() {
  node="$1"
  description="${node##*/} $(cat "$node/name" 2>/dev/null || true)"
  description="$(printf '%s' "$description" | tr '[:upper:]' '[:lower:]')"
  for token in $DDR_DEVFREQ_MATCH; do
    case "$description" in *"$token"*) return 0 ;; esac
  done
  return 1
}

lowest_frequency() {
  lowest=""
  for frequency in "$@"; do
    case "$frequency" in ''|*[!0-9]*) continue ;; esac
    if [ -z "$lowest" ] || [ "$frequency" -lt "$lowest" ]; then
      lowest="$frequency"
    fi
  done
  printf '%s' "$lowest"
}

highest_frequency() {
  highest=""
  for frequency in "$@"; do
    case "$frequency" in ''|*[!0-9]*) continue ;; esac
    if [ -z "$highest" ] || [ "$frequency" -gt "$highest" ]; then
      highest="$frequency"
    fi
  done
  printf '%s' "$highest"
}

governor_available() {
  node="$1"
  governor="$2"
  [ -r "$node/available_governors" ] || return 0
  grep -qw "$governor" "$node/available_governors" 2>/dev/null
}

set_maximum_cpu_performance() {
  for cpu in $IDLE_CPUS $IMU_CPUS $CAMERA_CPUS; do
    set_cpu_online "$cpu" 1
  done

  for policy in "$SYSFS_ROOT"/devices/system/cpu/cpufreq/policy*; do
    [ -d "$policy" ] || continue
    frequencies="$(cat "$policy/scaling_available_frequencies" 2>/dev/null || true)"
    # shellcheck disable=SC2086
    maximum="$(highest_frequency $frequencies)"
    if [ -z "$maximum" ]; then
      maximum="$(cat "$policy/cpuinfo_max_freq" 2>/dev/null || true)"
    fi
    [ -n "$maximum" ] || continue
    if governor_available "$policy" performance; then
      write_if_writable "$policy/scaling_governor" performance
    fi
    write_if_writable "$policy/scaling_max_freq" "$maximum"
    write_if_writable "$policy/scaling_min_freq" "$maximum"
  done
}

node_matches_performance_domain() {
  node="$1"
  description="${node##*/} $(cat "$node/name" 2>/dev/null || true)"
  description="$(printf '%s' "$description" | tr '[:upper:]' '[:lower:]')"
  for token in $PERFORMANCE_DEVFREQ_MATCH; do
    case "$description" in *"$token"*) return 0 ;; esac
  done
  return 1
}

set_maximum_devfreq_performance() {
  for node in "$SYSFS_ROOT"/class/devfreq/*; do
    [ -d "$node" ] || continue
    node_matches_performance_domain "$node" || continue
    frequencies="$(cat "$node/available_frequencies" 2>/dev/null || true)"
    # shellcheck disable=SC2086
    maximum="$(highest_frequency $frequencies)"
    [ -n "$maximum" ] || maximum="$(cat "$node/max_freq" 2>/dev/null || true)"
    [ -n "$maximum" ] || continue
    if node_matches_ddr "$node"; then
      # shellcheck disable=SC2086
      maximum="$(ddr_limited_frequency "$maximum" $frequencies)" || return 1
      # Lower a previous pinned minimum before applying a lower maximum.
      write_if_writable "$node/min_freq" "$maximum"
    fi
    write_if_writable "$node/max_freq" "$maximum"
    write_if_writable "$node/min_freq" "$maximum"
    if governor_available "$node" performance; then
      write_if_writable "$node/governor" performance
    elif governor_available "$node" userspace; then
      write_if_writable "$node/governor" userspace
      write_if_writable "$node/userspace/set_freq" "$maximum"
    fi
    write_if_writable "$node/device/power/control" on
  done
}

apply_maximum_performance() {
  set_maximum_cpu_performance
  configure_pps_realtime_cpu
  apply_agent_affinity "$CAMERA_CPUS"
  set_maximum_devfreq_performance
  set_isp_profile camera
}

set_automatic_cpu_scaling() {
  # Keep every CPU available; schedutil chooses frequencies separately for
  # each cluster. Do not apply a little-core frequency ceiling to big cores.
  for auto_cpu_dir in "$SYSFS_ROOT"/devices/system/cpu/cpu[0-9]*; do
    [ -d "$auto_cpu_dir" ] || continue
    set_cpu_online "${auto_cpu_dir##*cpu}" 1
  done
  for auto_policy in "$SYSFS_ROOT"/devices/system/cpu/cpufreq/policy*; do
    [ -d "$auto_policy" ] || continue
    auto_governor=""
    for candidate in schedutil ondemand; do
      if [ ! -r "$auto_policy/scaling_available_governors" ] ||
         grep -qw "$candidate" "$auto_policy/scaling_available_governors"; then
        auto_governor="$candidate"
        break
      fi
    done
    if [ -z "$auto_governor" ]; then
      echo "prism-power-profile: no supported demand governor for $auto_policy" >&2
      continue
    fi
    auto_frequencies="$(cat "$auto_policy/scaling_available_frequencies" 2>/dev/null || true)"
    auto_min="$(cat "$auto_policy/cpuinfo_min_freq" 2>/dev/null || true)"
    auto_max="$(cat "$auto_policy/cpuinfo_max_freq" 2>/dev/null || true)"
    # shellcheck disable=SC2086
    [ -n "$auto_min" ] || auto_min="$(lowest_frequency $auto_frequencies)"
    # shellcheck disable=SC2086
    [ -n "$auto_max" ] || auto_max="$(highest_frequency $auto_frequencies)"
    write_if_writable "$auto_policy/scaling_governor" "$auto_governor"
    write_if_writable "$auto_policy/scaling_min_freq" "$auto_min"
    write_if_writable "$auto_policy/scaling_max_freq" "$auto_max"
  done
}

set_automatic_devfreq_scaling() {
  for auto_node in "$SYSFS_ROOT"/class/devfreq/*; do
    [ -d "$auto_node" ] || continue
    node_matches_performance_domain "$auto_node" || continue
    case "$description" in
      *npu*) auto_candidates="rknpu_ondemand simple_ondemand" ;;
      *gpu*|*mali*) auto_candidates="simple_ondemand mali_ondemand" ;;
      *dmc*|*ddr*) auto_candidates="dmc_ondemand simple_ondemand" ;;
      *) continue ;;
    esac
    auto_governor=""
    for candidate in $auto_candidates; do
      if governor_available "$auto_node" "$candidate"; then
        auto_governor="$candidate"
        break
      fi
    done
    if [ -n "$auto_governor" ]; then
      write_if_writable "$auto_node/governor" "$auto_governor"
    else
      echo "prism-power-profile: no supported demand governor for $auto_node" >&2
    fi
    auto_frequencies="$(cat "$auto_node/available_frequencies" 2>/dev/null || true)"
    # shellcheck disable=SC2086
    auto_min="$(lowest_frequency $auto_frequencies)"
    # shellcheck disable=SC2086
    auto_max="$(highest_frequency $auto_frequencies)"
    if node_matches_ddr "$auto_node"; then
      # shellcheck disable=SC2086
      auto_max="$(ddr_limited_frequency "$auto_max" $auto_frequencies)" || return 1
      [ -n "$auto_min" ] || auto_min="$auto_max"
    fi
    write_if_writable "$auto_node/min_freq" "$auto_min"
    write_if_writable "$auto_node/max_freq" "$auto_max"
    # Runtime suspend does not disable the accelerator: the driver resumes
    # it on demand. Never pin it powered on or write userspace/set_freq.
    case "$description" in
      *npu*|*gpu*|*mali*) write_if_writable "$auto_node/device/power/control" auto ;;
    esac
  done
}

apply_automatic_scaling() {
  set_automatic_cpu_scaling
  configure_pps_realtime_cpu
  apply_agent_affinity "$CAMERA_CPUS"
  set_automatic_devfreq_scaling
  # Remove the userspace ISP clock override; the existing kernel driver
  # chooses its operating clock from the active camera workload.
  if [ -x "$ISP_HELPER" ]; then
    "$ISP_HELPER" restore >/dev/null 2>&1 || true
  fi
}

best_frequency_at_or_below() {
  ceiling="$1"
  shift
  case "$ceiling" in
    ''|*[!0-9]*)
      highest_frequency "$@"
      return
      ;;
  esac

  best=""
  for frequency in "$@"; do
    case "$frequency" in ''|*[!0-9]*) continue ;; esac
    if [ "$frequency" -le "$ceiling" ] &&
       { [ -z "$best" ] || [ "$frequency" -gt "$best" ]; }; then
      best="$frequency"
    fi
  done
  if [ -n "$best" ]; then
    printf '%s' "$best"
  else
    # A ceiling below the lowest hardware OPP cannot be honored. Use the
    # lowest advertised OPP rather than accidentally restoring the maximum.
    lowest_frequency "$@"
  fi
}

ddr_limited_frequency() {
  requested="$1"
  shift
  case "$requested" in
    ''|*[!0-9]*) requested="$DDR_MAX_FREQ_HZ" ;;
  esac
  if [ "$requested" -eq 0 ] || [ "$requested" -gt "$DDR_MAX_FREQ_HZ" ]; then
    requested="$DDR_MAX_FREQ_HZ"
  fi
  target="$(best_frequency_at_or_below "$requested" "$@")"
  # Without an OPP list, the sysfs ceiling still constrains the governor.
  [ -n "$target" ] || target="$requested"
  if [ "$target" -gt "$DDR_MAX_FREQ_HZ" ]; then
    echo "prism-power-profile: no DDR OPP within the qualified ceiling" >&2
    return 1
  fi
  printf '%s' "$target"
}

set_ddr_profile() {
  mode="$1"
  ceiling="${2:-}"
  for node in "$SYSFS_ROOT"/class/devfreq/*; do
    [ -d "$node" ] || continue
    node_matches_ddr "$node" || continue
    frequencies="$(cat "$node/available_frequencies" 2>/dev/null || true)"
    # shellcheck disable=SC2086
    lowest="$(lowest_frequency $frequencies)"
    # shellcheck disable=SC2086
    target="$(ddr_limited_frequency "$ceiling" $frequencies)" || return 1
    [ -n "$lowest" ] || lowest="$target"
    write_if_writable "$node/governor" dmc_ondemand
    [ -n "$lowest" ] && write_if_writable "$node/min_freq" "$lowest"
    if [ "$mode" = low ]; then
      [ -n "$lowest" ] && write_if_writable "$node/max_freq" "$lowest"
    else
      # shellcheck disable=SC2086
      [ -n "$target" ] && write_if_writable "$node/max_freq" "$target"
    fi
  done
}

set_isp_profile() {
  mode="$1"
  [ -x "$ISP_HELPER" ] || return 0
  if [ "$mode" = camera ]; then
    "$ISP_HELPER" set "$CAMERA_ISP_MAX_FREQ_HZ" >/dev/null 2>&1 || true
  else
    "$ISP_HELPER" apply >/dev/null 2>&1 || true
  fi
}

set_irq_profile() {
  mode="$1"
  [ "$mode" != download ] || mode=camera
  [ -x "$IRQ_HELPER" ] || return 0
  "$IRQ_HELPER" "$mode" >/dev/null 2>&1 || true
}

show_status() {
  printf 'profile='
  cat "$RUN_DIR/prism-power-profile" 2>/dev/null || printf 'unknown\n'
  cat "$SYSFS_ROOT/devices/system/cpu/online" 2>/dev/null || true
  for policy in "$SYSFS_ROOT"/devices/system/cpu/cpufreq/policy*; do
    [ -d "$policy" ] || continue
    printf '%s min=' "$policy"
    cat "$policy/scaling_min_freq" 2>/dev/null || true
    printf ' max='
    cat "$policy/scaling_max_freq" 2>/dev/null || true
  done
  for node in "$SYSFS_ROOT"/class/devfreq/*; do
    [ -d "$node" ] || continue
    node_matches_ddr "$node" || continue
    printf '%s max=' "$node"
    cat "$node/max_freq" 2>/dev/null || true
  done
}

profile="${1:-status}"
case "$profile" in
  idle-raw|imu-raw|camera-raw|download-raw)
    profile="${profile%-raw}"
    # Raw sensors bypass the ISP in both image variants.
    ISP_HELPER=/bin/true
    # Qualified 415M packed12 capture policy: add one A76, cap both clusters
    # at1608MHz, retain CPU3's PPS reservation. Full-performance keeps its
    # explicit CPU/GPU mode; DDR is capped independently in every mode.
    if [ "$AUTOMATIC_SCALING" != 1 ] && [ "$MAXIMUM_PERFORMANCE" != 1 ]; then
      CAMERA_CPUS="0 1 2 3 4"
      CAMERA_CPU_MAX_FREQ_KHZ="1608000"
      DOWNLOAD_CPUS="$CAMERA_CPUS"
      DOWNLOAD_CPU_MAX_FREQ_KHZ="$CAMERA_CPU_MAX_FREQ_KHZ"
    fi
    ;;
esac
if [ "$AUTOMATIC_SCALING" = 1 ]; then
  case "$profile" in
    idle|imu|camera|download)
      apply_automatic_scaling
      set_irq_profile "$profile"
      mkdir -p "$RUN_DIR"
      printf '%s\n' "$profile" >"$RUN_DIR/prism-power-profile"
      show_status
      exit 0
      ;;
  esac
fi
if [ "$MAXIMUM_PERFORMANCE" = 1 ]; then
  case "$profile" in
    idle|imu|camera|download)
      apply_maximum_performance
      set_irq_profile "$profile"
      mkdir -p "$RUN_DIR"
      printf '%s\n' "$profile" >"$RUN_DIR/prism-power-profile"
      show_status
      exit 0
      ;;
  esac
fi
case "$profile" in
  download)
    set_cpu_profile "$DOWNLOAD_CPUS" "$DOWNLOAD_CPU_MAX_FREQ_KHZ"
    configure_pps_realtime_cpu
    apply_agent_affinity "$DOWNLOAD_CPUS"
    set_ddr_profile download "$DOWNLOAD_DDR_MAX_FREQ_HZ"
    set_isp_profile idle
    set_irq_profile download
    ;;
  idle)
    set_cpu_profile "$IDLE_CPUS" "$IDLE_CPU_MAX_FREQ_KHZ"
    configure_pps_realtime_cpu
    apply_agent_affinity "$IDLE_CPUS"
    set_ddr_profile low
    set_isp_profile idle
    set_irq_profile idle
    ;;
  imu)
    set_cpu_profile "$IMU_CPUS" "$IMU_CPU_MAX_FREQ_KHZ"
    configure_pps_realtime_cpu
    apply_agent_affinity "$IMU_CPUS"
    set_ddr_profile low
    set_isp_profile imu
    set_irq_profile imu
    ;;
  camera)
    set_cpu_profile "$CAMERA_CPUS" "$CAMERA_CPU_MAX_FREQ_KHZ"
    configure_pps_realtime_cpu
    apply_agent_affinity "$CAMERA_CPUS"
    set_ddr_profile camera "$CAMERA_DDR_MAX_FREQ_HZ"
    set_isp_profile camera
    set_irq_profile camera
    ;;
  status)
    show_status
    exit 0
    ;;
  *)
    echo "Usage: $0 [idle|imu|camera|download|status]" >&2
    exit 2
    ;;
esac

mkdir -p "$RUN_DIR"
printf '%s\n' "$profile" >"$RUN_DIR/prism-power-profile"
show_status
