IQ.Pilot Release Commit @ b6534c0

iqpilot/selfdrive/dmonitoringmodeld/SConscript

@@ -0,0 +1,100 @@
+import os
+import glob
+
+Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'tinygrad_dir')
+lenv = env.Clone()
+lenvCython = envCython.Clone()
+
+libs = [cereal, messaging, visionipc, common, 'capnp', 'kj', 'pthread']
+frameworks = []
+
+common_src = [
+  "models/commonmodel.cc",
+  "transforms/transform.cc",
+]
+
+# OpenCL is a framework on Mac
+if arch == "Darwin":
+  frameworks += ['OpenCL']
+else:
+  libs += ['OpenCL']
+
+# Set path definitions
+for pathdef, fn in {'TRANSFORM': 'transforms/transform.cl'}.items():
+  for xenv in (lenv, lenvCython):
+    xenv['CXXFLAGS'].append(f'-D{pathdef}_PATH=\\"{File(fn).abspath}\\"')
+
+# Compile cython
+cython_libs = envCython["LIBS"] + libs
+commonmodel_lib = lenv.Library('commonmodel', common_src)
+lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks)
+tinygrad_files = sorted(x for x in glob.glob(tinygrad_dir + "/**", recursive=True) if 'pycache' not in x)
+
+def tg_compile(flags, model_name):
+  fn = File(f"models/{model_name}").abspath
+  cmd = lenv.Command(
+    fn + "_tinygrad.pkl",
+    [fn + ".onnx"] + tinygrad_files,
+    lenv.PrettyAction(
+      f'${{PYWARN}} {flags} python3 {Dir("#iqpilot/selfdrive/iqmodeld/tools").abspath}/compile_model.py {fn}.onnx {fn}_tinygrad.pkl',
+      'MODEL', logfile='${TARGET}.log')
+  )
+  # committed pkls must survive a failed rebuild (Precious: no pre-build
+  # delete) and scons -c (NoClean); a failed compile must not brick modeld
+  lenv.Precious(cmd)
+  lenv.NoClean(cmd)
+  return cmd
+
+def host_tinygrad_flags(*, float16=False):
+  if arch == "larch64":
+    base = "DEV=QCOM IMAGE=2 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
+    return base
+  if arch == "Darwin":
+    base = f'DEV=CPU HOME={os.path.expanduser("~")} IMAGE=0'
+  elif arch == "x86_64":
+    base = "DEV=CPU:LLVM IMAGE=0"
+  else:
+    base = "DEV=CPU:LLVM IMAGE=0"
+  return f"{base} FLOAT16=1" if float16 else base
+
+# Compile small models
+for model_name in ['dmonitoring_model']:
+  # The optimization flags are mandatory on QCOM: without FLOAT16/NOLOCALS/JIT_BATCH_SIZE/OPENPILOT_HACKS these
+  # models compile to unoptimized QCOM kernels and run ~20x slower (dmonitoring_model: ~300ms -> ~14ms),
+  # which starves the driving model on the shared Adreno. IMAGE=2 (not upstream's IMAGE=1) because the
+  # pinned tinygrad (fd992d66) hits an IMAGE=1 codegen bug on this Adreno; IMAGE=2 is correct and fast here.
+  flags = host_tinygrad_flags()
+
+  # Shipped prebuilt pkls: on device, a fresh install has no .sconsign, so scons would recompile these
+  # from onnx (5-10 min) even though up-to-date pkls are committed. The check file pins the exact
+  # inputs (onnx + tinygrad revision + flags + metadata script) and output hashes; if it matches, skip
+  # declaring the targets entirely. Any mismatch falls back to a normal on-device compile.
+  if arch == "larch64":
+    from iqpilot.selfdrive.dmonitoringmodeld.prebuilt_models import packaged_prebuilt_matches, verify_prebuilt, outputs_match
+    if packaged_prebuilt_matches(model_name):
+      print(lenv.PrettyNote('SKIP', f"{model_name} — packaged prebuilt pkl"))
+      continue
+    if verify_prebuilt(model_name, flags):
+      print(lenv.PrettyNote('SKIP', f"{model_name} — prebuilt pkl"))
+      continue
+    # Input digest mismatch but the committed artifacts are intact: this is a
+    # device with a mismatched tinygrad package. Recompiling here would DELETE the
+    # known-good pkl and then fail, bricking
+    # modeld. Keep the shipped artifacts and say so.
+    if outputs_match(model_name):
+      print(lenv.PrettyNote('WARN', f"{model_name} — input digest mismatch, keeping committed pkl"))
+      continue
+  elif not os.environ.get("COMPILE_MODELS"):
+    # The committed pkls ARE the device (QCOM) artifacts. A host build would
+    # overwrite them with host-flavor pkls (and `scons -c` deletes them), which
+    # then show up as staged changes and brick devices if committed. Host pkls
+    # only on explicit request: COMPILE_MODELS=1 scons ...
+    print(lenv.PrettyNote('SKIP', f"{model_name} — QCOM pkl kept (COMPILE_MODELS=1 to build host)"))
+    continue
+
+  fn = File(f"models/{model_name}").abspath
+  script_files = [File(Dir("#iqpilot/selfdrive/dmonitoringmodeld").File("get_model_metadata.py").abspath)]
+  metadata_cmd = f'${{PYWARN}} python3 {Dir("#iqpilot/selfdrive/dmonitoringmodeld").abspath}/get_model_metadata.py {fn}.onnx'
+  lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"] + tinygrad_files + script_files,
+               lenv.PrettyAction(metadata_cmd, 'META', logfile='${TARGET}.log'))
+  tg_compile(flags, model_name)

iqpilot/selfdrive/dmonitoringmodeld/dmonitoringmodeld.py

@@ -0,0 +1,165 @@
+#!/usr/bin/env python3
+import os
+from iqpilot.system.hardware import TICI
+os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
+from tinygrad.tensor import Tensor
+from tinygrad.dtype import dtypes
+import time
+import pickle
+import numpy as np
+from pathlib import Path
+
+from iqpilot.cereal import messaging
+from iqpilot.cereal.messaging import PubMaster, SubMaster
+from iqpilot.cereal.visionipc import VisionStreamType
+from msgq.visionipc import VisionIpcClient, VisionBuf
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.realtime import config_realtime_process
+from iqpilot.common.transformations.model import dmonitoringmodel_intrinsics
+from iqpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
+from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
+from iqpilot.selfdrive.dmonitoringmodeld.math import sigmoid, safe_exp
+from iqpilot.selfdrive.dmonitoringmodeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
+from iqpilot.selfdrive.iqmodeld.runtime.tinygrad import qcom_tensor_from_opencl_address
+
+PROCESS_NAME = "selfdrive.dmonitoringmodeld.dmonitoringmodeld"
+SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
+MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl'
+METADATA_PATH = Path(__file__).parent / 'models/dmonitoring_model_metadata.pkl'
+
+
+class ModelState:
+  inputs: dict[str, np.ndarray]
+  output: np.ndarray
+
+  def __init__(self, cl_ctx):
+    with open(METADATA_PATH, 'rb') as f:
+      model_metadata = pickle.load(f)
+      self.input_shapes = model_metadata['input_shapes']
+      self.output_slices = model_metadata['output_slices']
+
+    self.frame = MonitoringModelFrame(cl_ctx)
+    self.numpy_inputs = {
+      'calib': np.zeros(self.input_shapes['calib'], dtype=np.float32),
+    }
+
+    self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
+    with open(MODEL_PKL_PATH, "rb") as f:
+      self.model_run = pickle.load(f)
+
+  def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]:
+    self.numpy_inputs['calib'][0,:] = calib
+
+    t1 = time.perf_counter()
+
+    input_img_cl = self.frame.prepare(buf, transform.flatten())
+    if TICI:
+      # The imgs tensors are backed by opencl memory, only need init once
+      if 'input_img' not in self.tensor_inputs:
+        self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(input_img_cl.mem_address, self.input_shapes['input_img'], dtype=dtypes.uint8)
+    else:
+      self.tensor_inputs['input_img'] = Tensor(self.frame.buffer_from_cl(input_img_cl).reshape(self.input_shapes['input_img']), dtype=dtypes.uint8).realize()
+
+
+    output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy()
+
+    t2 = time.perf_counter()
+    return output, t2 - t1
+
+def slice_outputs(model_outputs, output_slices):
+  return  {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()}
+
+def parse_model_output(model_output):
+  parsed = {}
+  parsed['wheel_on_right'] = sigmoid(model_output['wheel_on_right'])
+  for ds_suffix in ['lhd', 'rhd']:
+    face_descs = model_output[f'face_descs_{ds_suffix}']
+    parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
+    parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
+    for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
+      parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
+  return parsed
+
+def fill_driver_data(msg, model_output, ds_suffix):
+  msg.faceOrientation = model_output[f'face_descs_{ds_suffix}'][0, :3].tolist()
+  msg.faceOrientationStd = model_output[f'face_descs_{ds_suffix}_std'][0, :3].tolist()
+  msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist()
+  msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist()
+  msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item()
+  msg.leftEyeProb = model_output[f'left_eye_prob_{ds_suffix}'][0, 0].item()
+  msg.rightEyeProb = model_output[f'right_eye_prob_{ds_suffix}'][0, 0].item()
+  msg.leftBlinkProb = model_output[f'left_blink_prob_{ds_suffix}'][0, 0].item()
+  msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
+  msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
+  msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
+
+def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
+  msg = messaging.new_message('driverStateV2', valid=True)
+  ds = msg.driverStateV2
+  ds.frameId = frame_id
+  ds.modelExecutionTime = exec_time
+  ds.gpuExecutionTime = gpu_exec_time
+  ds.rawPredictions = model_output['raw_pred']
+  ds.wheelOnRightProb = model_output['wheel_on_right'][0, 0].item()
+  fill_driver_data(ds.leftDriverData, model_output, 'lhd')
+  fill_driver_data(ds.rightDriverData, model_output, 'rhd')
+  return msg
+
+
+def main():
+  config_realtime_process(7, 5)
+
+  # Set in the child, not at import: manager preimports every process module in the parent,
+  # so an import-time write lands in one shared env that all children inherit (and setdefault
+  # in a child is then a guaranteed no-op). tinygrad reads this lazily at QCOMDevice init.
+  # KGSL: lower value = higher priority. DM has no 50ms deadline; at the driving contexts'
+  # default 8 its kernels interleave with the warp and blow its submit tail 16ms -> 72ms p90.
+  os.environ['QCOM_PRIORITY'] = os.getenv('DMON_QCOM_PRIORITY', '12')
+
+  cl_context = CLContext()
+  model = ModelState(cl_context)
+  cloudlog.warning("models loaded, dmonitoringmodeld starting")
+
+  cloudlog.warning("connecting to driver stream")
+  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True, cl_context)
+  while not vipc_client.connect(False):
+    time.sleep(0.1)
+  assert vipc_client.is_connected()
+  cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}")
+
+  sm = SubMaster(["extrinsicsCalibration"])
+  pm = PubMaster(["driverStateV2"])
+
+  calib = np.zeros(model.numpy_inputs['calib'].size, dtype=np.float32)
+  model_transform = None
+
+  while True:
+    buf = vipc_client.recv()
+    if buf is None:
+      continue
+
+    if model_transform is None:
+      cam = _os_fisheye if buf.width == _os_fisheye.width else _ar_ox_fisheye
+      model_transform = np.linalg.inv(np.dot(dmonitoringmodel_intrinsics, np.linalg.inv(cam.intrinsics))).astype(np.float32)
+
+    sm.update(0)
+    if sm.updated["extrinsicsCalibration"]:
+      calib_rpy = get_calibrated_rpy(sm["extrinsicsCalibration"])
+      calib[:] = calib_rpy if calib_rpy is not None else np.zeros_like(calib)
+
+    t1 = time.perf_counter()
+    model_output, gpu_execution_time = model.run(buf, calib, model_transform)
+    t2 = time.perf_counter()
+    raw_pred = model_output.tobytes() if SEND_RAW_PRED else b''
+    model_output = slice_outputs(model_output, model.output_slices)
+    model_output = parse_model_output(model_output)
+    model_output['raw_pred'] = raw_pred
+    msg = get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time)
+    pm.send("driverStateV2", msg)
+
+
+if __name__ == "__main__":
+  try:
+    main()
+  except KeyboardInterrupt:
+    cloudlog.warning("got SIGINT")

iqpilot/selfdrive/dmonitoringmodeld/get_model_metadata.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python3
+import sys
+import pathlib
+import onnx
+import codecs
+import pickle
+from typing import Any
+
+def get_name_and_shape(value_info:onnx.ValueInfoProto) -> tuple[str, tuple[int,...]]:
+  shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim])
+  name = value_info.name
+  return name, shape
+
+def get_metadata_value_by_name(model:onnx.ModelProto, name:str) -> str | Any:
+  for prop in model.metadata_props:
+    if prop.key == name:
+      return prop.value
+  return None
+
+if __name__ == "__main__":
+  model_path = pathlib.Path(sys.argv[1])
+  model = onnx.load(str(model_path))
+  output_slices = get_metadata_value_by_name(model, 'output_slices')
+  assert output_slices is not None, 'output_slices not found in metadata'
+
+  metadata = {
+    'model_checkpoint': get_metadata_value_by_name(model, 'model_checkpoint'),
+    'output_slices': pickle.loads(codecs.decode(output_slices.encode(), "base64")),
+    'input_shapes': dict([get_name_and_shape(x) for x in model.graph.input]),
+    'output_shapes': dict([get_name_and_shape(x) for x in model.graph.output])
+  }
+
+  metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl')
+  with open(metadata_path, 'wb') as f:
+    pickle.dump(metadata, f)
+
+  print(f'saved metadata to {metadata_path}')

iqpilot/selfdrive/dmonitoringmodeld/math.py

@@ -0,0 +1,9 @@
+import numpy as np
+
+
+def safe_exp(values, out=None):
+  return np.exp(np.clip(values, -np.inf, 11), out=out)
+
+
+def sigmoid(values):
+  return 1.0 / (1.0 + safe_exp(-values))

iqpilot/selfdrive/dmonitoringmodeld/models/commonmodel.cc

@@ -0,0 +1,22 @@
+#include "selfdrive/dmonitoringmodeld/models/commonmodel.h"
+
+#include <cmath>
+#include <cstring>
+
+#include "common/clutil.h"
+
+MonitoringModelFrame::MonitoringModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) {
+  input_frames = std::make_unique<uint8_t[]>(buf_size);
+  init_transform(device_id, context, MODEL_WIDTH, MODEL_HEIGHT);
+}
+
+cl_mem* MonitoringModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) {
+  run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection);
+  clFinish(q);
+  return &y_cl;
+}
+
+MonitoringModelFrame::~MonitoringModelFrame() {
+  deinit_transform();
+  CL_CHECK(clReleaseCommandQueue(q));
+}

iqpilot/selfdrive/dmonitoringmodeld/models/commonmodel.h

@@ -0,0 +1,76 @@
+#pragma once
+
+#include <cfloat>
+#include <cstdlib>
+#include <cassert>
+
+#include <memory>
+
+#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+#include "common/clutil.h"
+#include "common/mat.h"
+#include "selfdrive/dmonitoringmodeld/transforms/transform.h"
+
+class ModelFrame {
+public:
+  ModelFrame(cl_device_id device_id, cl_context context) {
+    q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
+  }
+  virtual ~ModelFrame() {}
+  virtual cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) { return NULL; }
+  uint8_t* buffer_from_cl(cl_mem *in_frames, int buffer_size) {
+    CL_CHECK(clEnqueueReadBuffer(q, *in_frames, CL_TRUE, 0, buffer_size, input_frames.get(), 0, nullptr, nullptr));
+    clFinish(q);
+    return &input_frames[0];
+  }
+
+  int MODEL_WIDTH;
+  int MODEL_HEIGHT;
+  int MODEL_FRAME_SIZE;
+  int buf_size;
+
+protected:
+  cl_mem y_cl, u_cl, v_cl;
+  Transform transform;
+  cl_command_queue q;
+  std::unique_ptr<uint8_t[]> input_frames;
+
+  void init_transform(cl_device_id device_id, cl_context context, int model_width, int model_height) {
+    y_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, model_width * model_height, NULL, &err));
+    u_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (model_width / 2) * (model_height / 2), NULL, &err));
+    v_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (model_width / 2) * (model_height / 2), NULL, &err));
+    transform_init(&transform, context, device_id);
+  }
+
+  void deinit_transform() {
+    transform_destroy(&transform);
+    CL_CHECK(clReleaseMemObject(v_cl));
+    CL_CHECK(clReleaseMemObject(u_cl));
+    CL_CHECK(clReleaseMemObject(y_cl));
+  }
+
+  void run_transform(cl_mem yuv_cl, int model_width, int model_height, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) {
+    transform_queue(&transform, q,
+        yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset,
+        y_cl, u_cl, v_cl, model_width, model_height, projection);
+  }
+};
+
+class MonitoringModelFrame : public ModelFrame {
+public:
+  MonitoringModelFrame(cl_device_id device_id, cl_context context);
+  ~MonitoringModelFrame();
+  cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection);
+
+  const int MODEL_WIDTH = 1440;
+  const int MODEL_HEIGHT = 960;
+  const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT;
+  const int buf_size = MODEL_FRAME_SIZE;
+
+};

iqpilot/selfdrive/dmonitoringmodeld/models/commonmodel.pxd

@@ -0,0 +1,23 @@
+# distutils: language = c++
+
+from msgq.visionipc.visionipc cimport cl_device_id, cl_context, cl_mem
+
+cdef extern from "common/mat.h":
+  cdef struct mat3:
+    float v[9]
+
+cdef extern from "common/clutil.h":
+  cdef unsigned long CL_DEVICE_TYPE_DEFAULT
+  cl_device_id cl_get_device_id(unsigned long)
+  cl_context cl_create_context(cl_device_id)
+  void cl_release_context(cl_context)
+
+cdef extern from "selfdrive/dmonitoringmodeld/models/commonmodel.h":
+  cppclass ModelFrame:
+    int buf_size
+    unsigned char * buffer_from_cl(cl_mem*, int);
+    cl_mem * prepare(cl_mem, int, int, int, int, mat3)
+
+  cppclass MonitoringModelFrame:
+    int buf_size
+    MonitoringModelFrame(cl_device_id, cl_context)

iqpilot/selfdrive/dmonitoringmodeld/models/commonmodel_pyx.pxd

@@ -0,0 +1,13 @@
+# distutils: language = c++
+
+from msgq.visionipc.visionipc cimport cl_mem
+from msgq.visionipc.visionipc_pyx cimport CLContext as BaseCLContext
+
+cdef class CLContext(BaseCLContext):
+  pass
+
+cdef class CLMem:
+  cdef cl_mem * mem
+
+  @staticmethod
+  cdef create(void*)

iqpilot/selfdrive/dmonitoringmodeld/models/commonmodel_pyx.pyx

@@ -0,0 +1,65 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii, language_level=3
+
+import numpy as np
+cimport numpy as cnp
+from libc.string cimport memcpy
+from libc.stdint cimport uintptr_t
+
+from msgq.visionipc.visionipc cimport cl_mem
+from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
+from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context, cl_release_context
+from .commonmodel cimport mat3, ModelFrame as cppModelFrame, MonitoringModelFrame as cppMonitoringModelFrame
+
+
+cdef class CLContext(BaseCLContext):
+  def __cinit__(self):
+    self.device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT)
+    self.context = cl_create_context(self.device_id)
+
+  def __dealloc__(self):
+    if self.context:
+      cl_release_context(self.context)
+
+cdef class CLMem:
+  @staticmethod
+  cdef create(void * cmem):
+    mem = CLMem()
+    mem.mem = <cl_mem*> cmem
+    return mem
+
+  @property
+  def mem_address(self):
+    return <uintptr_t>(self.mem)
+
+def cl_from_visionbuf(VisionBuf buf):
+  return CLMem.create(<void*>&buf.buf.buf_cl)
+
+
+cdef class ModelFrame:
+  cdef cppModelFrame * frame
+  cdef int buf_size
+
+  def __dealloc__(self):
+    del self.frame
+
+  def prepare(self, VisionBuf buf, float[:] projection):
+    cdef mat3 cprojection
+    memcpy(cprojection.v, &projection[0], 9*sizeof(float))
+    cdef cl_mem * data
+    data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection)
+    return CLMem.create(data)
+
+  def buffer_from_cl(self, CLMem in_frames):
+    cdef unsigned char * data2
+    data2 = self.frame.buffer_from_cl(in_frames.mem, self.buf_size)
+    return np.asarray(<cnp.uint8_t[:self.buf_size]> data2)
+
+
+cdef class MonitoringModelFrame(ModelFrame):
+  cdef cppMonitoringModelFrame * _frame
+
+  def __cinit__(self, CLContext context):
+    self._frame = new cppMonitoringModelFrame(context.device_id, context.context)
+    self.frame = <cppModelFrame*>(self._frame)
+    self.buf_size = self._frame.buf_size

iqpilot/selfdrive/dmonitoringmodeld/models/prebuilt_check.json

@@ -0,0 +1,9 @@
+{
+  "dmonitoring_model": {
+    "outputs": {
+      "dmonitoring_model_metadata.pkl": "31a86ab7a92dc0af088b15787a440dd3b210aa662e445a15145900e559a1b5c3",
+      "dmonitoring_model_tinygrad.pkl": "de991722fc93036a09595ac72f944f52afa15b097f182b3d5aa30a33d93d2d20"
+    },
+    "signature": "696bc8453926631500feeb8f6d7baa9e8abeac06ee581e439b8edde299b24eb4"
+  }
+}

iqpilot/selfdrive/dmonitoringmodeld/prebuilt_models.py

@@ -0,0 +1,140 @@
+#!/usr/bin/env python3
+import functools
+import hashlib
+import json
+import re
+import sys
+from pathlib import Path
+
+MODELD_DIR = Path(__file__).resolve().parent
+MODELS_DIR = MODELD_DIR / 'models'
+BASEDIR = MODELD_DIR.parents[2]
+METADATA_SCRIPT = MODELD_DIR / 'get_model_metadata.py'
+PYPROJECT = BASEDIR / 'pyproject.toml'
+TINYGRAD_REVISION_FILE = BASEDIR / 'artifacts/package_sources/tinygrad/.iqpilot-revision'
+
+MODEL_NAMES = ['dmonitoring_model']
+
+
+def _hash_file(h, path: Path) -> None:
+  with open(path, 'rb') as f:
+    for chunk in iter(lambda: f.read(1024 * 1024), b''):
+      h.update(chunk)
+
+
+def _file_sha256(path: Path) -> str:
+  h = hashlib.sha256()
+  _hash_file(h, path)
+  return h.hexdigest()
+
+
+@functools.lru_cache(maxsize=1)
+def _tinygrad_revision() -> str:
+  match = re.search(r'"tinygrad @ git\+https://[^@]+@([0-9a-f]{40})"', PYPROJECT.read_text())
+  if match is not None:
+    return match.group(1)
+
+  try:
+    revision = TINYGRAD_REVISION_FILE.read_text().strip()
+  except OSError as e:
+    raise RuntimeError("missing pinned tinygrad revision") from e
+  if re.fullmatch(r'[0-9a-f]{40}', revision) is None:
+    raise RuntimeError("invalid pinned tinygrad revision")
+  return revision
+
+
+CHECK_PATH = MODELS_DIR / 'prebuilt_check.json'
+
+
+def _load_checks() -> dict:
+  try:
+    data = json.loads(CHECK_PATH.read_text())
+  except (json.JSONDecodeError, OSError):
+    return {}
+  return data if isinstance(data, dict) else {}
+
+
+def _output_names(model_name: str) -> list[str]:
+  return [f'{model_name}_tinygrad.pkl', f'{model_name}_metadata.pkl']
+
+
+def compute_signature(model_name: str, flags: str) -> str:
+  h = hashlib.sha256()
+  h.update(flags.encode())
+  h.update(_tinygrad_revision().encode())
+  _hash_file(h, METADATA_SCRIPT)
+  _hash_file(h, MODELS_DIR / f'{model_name}.onnx')
+  return h.hexdigest()
+
+
+def outputs_match(model_name: str) -> bool:
+  """The committed artifacts on disk are exactly the ones the check file pins."""
+  data = _load_checks().get(model_name, {})
+  outputs = data.get('outputs', {})
+  if set(outputs) != set(_output_names(model_name)):
+    return False
+  for fn, expected in outputs.items():
+    p = MODELS_DIR / fn
+    if not p.is_file() or _file_sha256(p) != expected:
+      return False
+  return True
+
+
+def packaged_prebuilt_matches(model_name: str) -> bool:
+  return not (MODELS_DIR / f'{model_name}.onnx').is_file() and outputs_match(model_name)
+
+
+def verify_prebuilt(model_name: str, flags: str) -> bool:
+  if not (MODELS_DIR / f'{model_name}.onnx').is_file():
+    return False
+  data = _load_checks().get(model_name, {})
+  if data.get('signature') != compute_signature(model_name, flags):
+    return False
+  return outputs_match(model_name)
+
+
+def verification_details(model_name: str, flags: str) -> list[str]:
+  data = _load_checks().get(model_name, {})
+  details = [
+    f'signature expected={data.get("signature", "missing")} actual={compute_signature(model_name, flags)}',
+    f'tinygrad={_tinygrad_revision()}',
+    f'onnx={_file_sha256(MODELS_DIR / f"{model_name}.onnx")}',
+    f'metadata_script={_file_sha256(METADATA_SCRIPT)}',
+  ]
+  outputs = data.get('outputs', {})
+  for fn in _output_names(model_name):
+    path = MODELS_DIR / fn
+    actual = _file_sha256(path) if path.is_file() else 'missing'
+    details.append(f'{fn} expected={outputs.get(fn, "missing")} actual={actual}')
+  return details
+
+
+def write_check(model_name: str, flags: str) -> None:
+  outputs = {}
+  for fn in _output_names(model_name):
+    p = MODELS_DIR / fn
+    if not p.is_file():
+      raise FileNotFoundError(f'missing build output: {p}')
+    outputs[fn] = _file_sha256(p)
+  checks = _load_checks()
+  checks[model_name] = {'signature': compute_signature(model_name, flags), 'outputs': outputs}
+  CHECK_PATH.write_text(json.dumps(checks, indent=2, sort_keys=True) + '\n')
+
+
+def _larch64_flags() -> str:
+  return "DEV=QCOM IMAGE=2 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
+
+
+if __name__ == '__main__':
+  mode = sys.argv[1] if len(sys.argv) > 1 else 'verify'
+  flags = _larch64_flags()
+  for name in MODEL_NAMES:
+    if mode == 'write':
+      write_check(name, flags)
+      print(f'{name}: check written')
+    else:
+      valid = verify_prebuilt(name, flags)
+      print(f'{name}: {"OK" if valid else "STALE"}')
+      if not valid:
+        for detail in verification_details(name, flags):
+          print(f'  {detail}')

iqpilot/selfdrive/dmonitoringmodeld/test_prebuilt_models.py

@@ -0,0 +1,65 @@
+import hashlib
+import json
+
+from iqpilot.selfdrive.dmonitoringmodeld import prebuilt_models
+
+
+def write_outputs(models_dir, check_path):
+  outputs = {}
+  for name, contents in {
+    'dmonitoring_model_tinygrad.pkl': b'tinygrad',
+    'dmonitoring_model_metadata.pkl': b'metadata',
+  }.items():
+    (models_dir / name).write_bytes(contents)
+    outputs[name] = hashlib.sha256(contents).hexdigest()
+  check_path.write_text(json.dumps({'dmonitoring_model': {'outputs': outputs}}))
+
+
+def test_packaged_prebuilt_without_onnx(tmp_path, monkeypatch):
+  models_dir = tmp_path / 'models'
+  models_dir.mkdir()
+  check_path = models_dir / 'prebuilt_check.json'
+  write_outputs(models_dir, check_path)
+  monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir)
+  monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path)
+
+  assert prebuilt_models.packaged_prebuilt_matches('dmonitoring_model')
+  assert not prebuilt_models.verify_prebuilt('dmonitoring_model', 'flags')
+
+
+def test_packaged_prebuilt_rejects_corrupt_output(tmp_path, monkeypatch):
+  models_dir = tmp_path / 'models'
+  models_dir.mkdir()
+  check_path = models_dir / 'prebuilt_check.json'
+  write_outputs(models_dir, check_path)
+  (models_dir / 'dmonitoring_model_tinygrad.pkl').write_bytes(b'corrupt')
+  monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir)
+  monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path)
+
+  assert not prebuilt_models.packaged_prebuilt_matches('dmonitoring_model')
+
+
+def test_source_checkout_is_not_packaged_prebuilt(tmp_path, monkeypatch):
+  models_dir = tmp_path / 'models'
+  models_dir.mkdir()
+  check_path = models_dir / 'prebuilt_check.json'
+  write_outputs(models_dir, check_path)
+  (models_dir / 'dmonitoring_model.onnx').write_bytes(b'onnx')
+  monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir)
+  monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path)
+
+  assert not prebuilt_models.packaged_prebuilt_matches('dmonitoring_model')
+
+
+def test_vendored_tinygrad_revision(tmp_path, monkeypatch):
+  revision = '0123456789abcdef0123456789abcdef01234567'
+  pyproject = tmp_path / 'pyproject.toml'
+  revision_file = tmp_path / '.iqpilot-revision'
+  pyproject.write_text('dependencies = ["tinygrad"]\n')
+  revision_file.write_text(f'{revision}\n')
+  monkeypatch.setattr(prebuilt_models, 'PYPROJECT', pyproject)
+  monkeypatch.setattr(prebuilt_models, 'TINYGRAD_REVISION_FILE', revision_file)
+  prebuilt_models._tinygrad_revision.cache_clear()
+
+  assert prebuilt_models._tinygrad_revision() == revision
+  prebuilt_models._tinygrad_revision.cache_clear()

iqpilot/selfdrive/dmonitoringmodeld/transforms/transform.cc

@@ -0,0 +1,97 @@
+#include "selfdrive/dmonitoringmodeld/transforms/transform.h"
+
+#include <cassert>
+#include <cstring>
+
+#include "common/clutil.h"
+
+void transform_init(Transform* s, cl_context ctx, cl_device_id device_id) {
+  memset(s, 0, sizeof(*s));
+
+  cl_program prg = cl_program_from_file(ctx, device_id, TRANSFORM_PATH, "");
+  s->krnl = CL_CHECK_ERR(clCreateKernel(prg, "warpPerspective", &err));
+  // done with this
+  CL_CHECK(clReleaseProgram(prg));
+
+  s->m_y_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err));
+  s->m_uv_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err));
+}
+
+void transform_destroy(Transform* s) {
+  CL_CHECK(clReleaseMemObject(s->m_y_cl));
+  CL_CHECK(clReleaseMemObject(s->m_uv_cl));
+  CL_CHECK(clReleaseKernel(s->krnl));
+}
+
+void transform_queue(Transform* s,
+                     cl_command_queue q,
+                     cl_mem in_yuv, int in_width, int in_height, int in_stride, int in_uv_offset,
+                     cl_mem out_y, cl_mem out_u, cl_mem out_v,
+                     int out_width, int out_height,
+                     const mat3& projection) {
+  const int zero = 0;
+
+  // sampled using pixel center origin
+  // (because that's how fastcv and opencv does it)
+
+  mat3 projection_y = projection;
+
+  // in and out uv is half the size of y.
+  mat3 projection_uv = transform_scale_buffer(projection, 0.5);
+
+  CL_CHECK(clEnqueueWriteBuffer(q, s->m_y_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_y.v, 0, NULL, NULL));
+  CL_CHECK(clEnqueueWriteBuffer(q, s->m_uv_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_uv.v, 0, NULL, NULL));
+
+  const int in_y_width = in_width;
+  const int in_y_height = in_height;
+  const int in_y_px_stride = 1;
+  const int in_uv_width = in_width/2;
+  const int in_uv_height = in_height/2;
+  const int in_uv_px_stride = 2;
+  const int in_u_offset = in_uv_offset;
+  const int in_v_offset = in_uv_offset + 1;
+
+  const int out_y_width = out_width;
+  const int out_y_height = out_height;
+  const int out_uv_width = out_width/2;
+  const int out_uv_height = out_height/2;
+
+  CL_CHECK(clSetKernelArg(s->krnl, 0, sizeof(cl_mem), &in_yuv));  // src
+  CL_CHECK(clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_stride));  // src_row_stride
+  CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_y_px_stride));  // src_px_stride
+  CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &zero));  // src_offset
+  CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_y_height));  // src_rows
+  CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_int), &in_y_width));  // src_cols
+  CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_y));  // dst
+  CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &out_y_width));  // dst_row_stride
+  CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &zero));  // dst_offset
+  CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_y_height));  // dst_rows
+  CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_int), &out_y_width));  // dst_cols
+  CL_CHECK(clSetKernelArg(s->krnl, 11, sizeof(cl_mem), &s->m_y_cl));  // M
+
+  const size_t work_size_y[2] = {(size_t)out_y_width, (size_t)out_y_height};
+
+  CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
+                              (const size_t*)&work_size_y, NULL, 0, 0, NULL));
+
+  const size_t work_size_uv[2] = {(size_t)out_uv_width, (size_t)out_uv_height};
+
+  CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_uv_px_stride));  // src_px_stride
+  CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_u_offset));  // src_offset
+  CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_uv_height));  // src_rows
+  CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_int), &in_uv_width));  // src_cols
+  CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_u));  // dst
+  CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &out_uv_width));  // dst_row_stride
+  CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &zero));  // dst_offset
+  CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_uv_height));  // dst_rows
+  CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_int), &out_uv_width));  // dst_cols
+  CL_CHECK(clSetKernelArg(s->krnl, 11, sizeof(cl_mem), &s->m_uv_cl));  // M
+
+  CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
+                              (const size_t*)&work_size_uv, NULL, 0, 0, NULL));
+  CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_v_offset));  // src_ofset
+  CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_v));  // dst
+
+  CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
+                              (const size_t*)&work_size_uv, NULL, 0, 0, NULL));
+}

iqpilot/selfdrive/dmonitoringmodeld/transforms/transform.cl

@@ -0,0 +1,54 @@
+#define INTER_BITS 5
+#define INTER_TAB_SIZE (1 << INTER_BITS)
+#define INTER_SCALE 1.f / INTER_TAB_SIZE
+
+#define INTER_REMAP_COEF_BITS 15
+#define INTER_REMAP_COEF_SCALE (1 << INTER_REMAP_COEF_BITS)
+
+__kernel void warpPerspective(__global const uchar * src,
+                              int src_row_stride, int src_px_stride, int src_offset, int src_rows, int src_cols,
+                              __global uchar * dst,
+                              int dst_row_stride, int dst_offset, int dst_rows, int dst_cols,
+                              __constant float * M)
+{
+    int dx = get_global_id(0);
+    int dy = get_global_id(1);
+
+    if (dx < dst_cols && dy < dst_rows)
+    {
+        float X0 = M[0] * dx + M[1] * dy + M[2];
+        float Y0 = M[3] * dx + M[4] * dy + M[5];
+        float W = M[6] * dx + M[7] * dy + M[8];
+        W = W != 0.0f ? INTER_TAB_SIZE / W : 0.0f;
+        int X = rint(X0 * W), Y = rint(Y0 * W);
+
+        int sx = convert_short_sat(X >> INTER_BITS);
+        int sy = convert_short_sat(Y >> INTER_BITS);
+
+        short sx_clamp = clamp(sx, 0, src_cols - 1);
+        short sx_p1_clamp = clamp(sx + 1, 0, src_cols - 1);
+        short sy_clamp = clamp(sy, 0, src_rows - 1);
+        short sy_p1_clamp = clamp(sy + 1, 0, src_rows - 1);
+        int v0 = convert_int(src[mad24(sy_clamp, src_row_stride, src_offset + sx_clamp*src_px_stride)]);
+        int v1 = convert_int(src[mad24(sy_clamp, src_row_stride, src_offset + sx_p1_clamp*src_px_stride)]);
+        int v2 = convert_int(src[mad24(sy_p1_clamp, src_row_stride, src_offset + sx_clamp*src_px_stride)]);
+        int v3 = convert_int(src[mad24(sy_p1_clamp, src_row_stride, src_offset + sx_p1_clamp*src_px_stride)]);
+
+        short ay = (short)(Y & (INTER_TAB_SIZE - 1));
+        short ax = (short)(X & (INTER_TAB_SIZE - 1));
+        float taby = 1.f/INTER_TAB_SIZE*ay;
+        float tabx = 1.f/INTER_TAB_SIZE*ax;
+
+        int dst_index = mad24(dy, dst_row_stride, dst_offset + dx);
+
+        int itab0 = convert_short_sat_rte( (1.0f-taby)*(1.0f-tabx) * INTER_REMAP_COEF_SCALE );
+        int itab1 = convert_short_sat_rte( (1.0f-taby)*tabx * INTER_REMAP_COEF_SCALE );
+        int itab2 = convert_short_sat_rte( taby*(1.0f-tabx) * INTER_REMAP_COEF_SCALE );
+        int itab3 = convert_short_sat_rte( taby*tabx * INTER_REMAP_COEF_SCALE );
+
+        int val = v0 * itab0 +  v1 * itab1 + v2 * itab2 + v3 * itab3;
+
+        uchar pix = convert_uchar_sat((val + (1 << (INTER_REMAP_COEF_BITS-1))) >> INTER_REMAP_COEF_BITS);
+        dst[dst_index] = pix;
+    }
+}

iqpilot/selfdrive/dmonitoringmodeld/transforms/transform.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+#include "common/mat.h"
+
+typedef struct {
+  cl_kernel krnl;
+  cl_mem m_y_cl, m_uv_cl;
+} Transform;
+
+void transform_init(Transform* s, cl_context ctx, cl_device_id device_id);
+
+void transform_destroy(Transform* transform);
+
+void transform_queue(Transform* s, cl_command_queue q,
+                     cl_mem yuv, int in_width, int in_height, int in_stride, int in_uv_offset,
+                     cl_mem out_y, cl_mem out_u, cl_mem out_v,
+                     int out_width, int out_height,
+                     const mat3& projection);