selfdrive/ui

selfdrive/ui/.gitignore

@@ -0,0 +1 @@
+ui

selfdrive/ui/SConscript

@@ -0,0 +1,15 @@
+Import('env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal')
+
+src = ['ui.cc', 'paint.cc', '#phonelibs/nanovg/nanovg.c']
+libs = [common, 'zmq', 'czmq', 'capnp', 'capnp_c', 'm', cereal, 'json', messaging, 'OpenCL', gpucommon, visionipc]
+
+if arch == "aarch64":
+  src += ['sound.cc', 'slplay.c']
+  libs += ['EGL', 'GLESv3', 'gnustl_shared', 'log', 'utils', 'gui', 'hardware', 'ui', 'CB', 'gsl', 'adreno_utils', 'OpenSLES', 'cutils', 'uuid']
+else:
+  src += ['linux.cc']
+  libs += ['EGL', 'pthread', 'X11-xcb', 'xcb', 'X11', 'glfw']
+  
+env.Program('_ui', src,
+  LINKFLAGS=['-Wl,-rpath=/system/lib64,-rpath=/system/comma/usr/lib'],
+  LIBS=libs)

selfdrive/ui/linux.cc

@@ -0,0 +1,100 @@
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <string.h>
+
+#include "ui.hpp"
+
+#define GLFW_INCLUDE_ES2
+#define GLFW_INCLUDE_GLEXT
+#include <GLFW/glfw3.h>
+
+typedef struct FramebufferState FramebufferState;
+typedef struct TouchState TouchState;
+
+#define FALSE 0
+#define TRUE 1
+
+#include <xcb/xcb.h>
+#include <X11/Xlib-xcb.h>
+
+extern "C" {
+
+FramebufferState* framebuffer_init(
+    const char* name, int32_t layer, int alpha,
+    int *out_w, int *out_h) {
+  glfwInit();
+
+  glfwWindowHint(GLFW_CLIENT_API, GLFW_OPENGL_ES_API);
+  glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
+  glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0);
+  glfwWindowHint(GLFW_RESIZABLE, 0);
+  GLFWwindow* window;
+  window = glfwCreateWindow(1920, 1080, "ui", NULL, NULL);
+  if (!window) {
+    printf("glfwCreateWindow failed\n");
+  }
+
+  glfwMakeContextCurrent(window);
+  glfwSwapInterval(0);
+
+  // clear screen
+  glClearColor(0.2f, 0.2f, 0.2f, 1.0f );
+  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+  framebuffer_swap((FramebufferState*)window);
+
+  if (out_w) *out_w = 1920;
+  if (out_h) *out_h = 1080;
+
+  return (FramebufferState*)window;
+}
+
+void framebuffer_set_power(FramebufferState *s, int mode) {
+}
+
+void framebuffer_swap(FramebufferState *s) {
+  glfwSwapBuffers((GLFWwindow*)s);
+}
+
+void touch_init(TouchState *s) {
+  printf("touch_init\n");
+}
+
+int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) {
+  return -1;
+}
+
+int touch_read(TouchState *s, int* out_x, int* out_y) {
+  return -1;
+}
+
+}
+
+#include "sound.hpp"
+
+void ui_sound_init() {}
+void ui_sound_destroy() {}
+
+void set_volume(int volume) {}
+
+void play_alert_sound(AudibleAlert alert) {}
+void stop_alert_sound(AudibleAlert alert) {}
+
+#include "common/visionimg.h"
+#include <sys/mman.h>
+
+GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph) {
+  unsigned int texture;
+  glGenTextures(1, &texture);
+  glBindTexture(GL_TEXTURE_2D, texture);
+  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, img->width, img->height, 0, GL_RGB, GL_UNSIGNED_BYTE, *pph);
+  glGenerateMipmap(GL_TEXTURE_2D);
+  *pkhr = (EGLImageKHR *)1; // not NULL
+  return texture;
+}
+
+void visionimg_destroy_gl(EGLImageKHR khr, void *ph) {
+  // empty
+}
+

selfdrive/ui/slplay.c

@@ -0,0 +1,184 @@
+#include <stdio.h>
+#include <assert.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <getopt.h>
+
+#include "common/timing.h"
+#include "slplay.h"
+
+SLEngineItf engineInterface = NULL;
+SLObjectItf outputMix = NULL;
+SLObjectItf engine = NULL;
+uri_player players[32] = {{NULL, NULL, NULL}};
+
+uint64_t loop_start = 0;
+uint64_t loop_start_ctx = 0;
+
+uri_player* get_player_by_uri(const char* uri) {
+  for (uri_player *s = players; s->uri != NULL; s++) {
+    if (strcmp(s->uri, uri) == 0) {
+      return s;
+    }
+  }
+
+  return NULL;
+}
+
+uri_player* slplay_create_player_for_uri(const char* uri, char **error) {
+  uri_player player = { uri, NULL, NULL };
+
+  SLresult result;
+  SLDataLocator_URI locUri = {SL_DATALOCATOR_URI, (SLchar *) uri};
+  SLDataFormat_MIME formatMime = {SL_DATAFORMAT_MIME, NULL, SL_CONTAINERTYPE_UNSPECIFIED};
+  SLDataSource audioSrc = {&locUri, &formatMime};
+
+  SLDataLocator_OutputMix outMix = {SL_DATALOCATOR_OUTPUTMIX, outputMix};
+  SLDataSink audioSnk = {&outMix, NULL};
+
+  result = (*engineInterface)->CreateAudioPlayer(engineInterface, &player.player, &audioSrc, &audioSnk, 0, NULL, NULL);
+  if (result != SL_RESULT_SUCCESS) {
+    *error = "Failed to create audio player";
+    return NULL;
+  }
+
+  result = (*(player.player))->Realize(player.player, SL_BOOLEAN_FALSE);
+  if (result != SL_RESULT_SUCCESS) {
+    *error = "Failed to realize audio player";
+    return NULL;
+  }
+
+  result = (*(player.player))->GetInterface(player.player, SL_IID_PLAY, &(player.playInterface));
+  if (result != SL_RESULT_SUCCESS) {
+    *error = "Failed to get player interface";
+    return NULL;
+  }
+
+  result = (*(player.playInterface))->SetPlayState(player.playInterface, SL_PLAYSTATE_PAUSED);
+  if (result != SL_RESULT_SUCCESS) {
+    *error = "Failed to initialize playstate to SL_PLAYSTATE_PAUSED";
+    return NULL;
+  }
+
+  uri_player *p = players;
+  while (p->uri != NULL) {
+    p++;
+  }
+  *p = player;
+
+  return p;
+}
+
+void slplay_setup(char **error) {
+  SLresult result;
+  SLEngineOption engineOptions[] = {{SL_ENGINEOPTION_THREADSAFE, SL_BOOLEAN_TRUE}};
+  result = slCreateEngine(&engine, 1, engineOptions, 0, NULL, NULL);
+  if (result != SL_RESULT_SUCCESS) {
+    *error = "Failed to create OpenSL engine";
+  }
+
+  result = (*engine)->Realize(engine, SL_BOOLEAN_FALSE);
+  if (result != SL_RESULT_SUCCESS) {
+    *error = "Failed to realize OpenSL engine";
+  }
+
+  result = (*engine)->GetInterface(engine, SL_IID_ENGINE, &engineInterface);
+  if (result != SL_RESULT_SUCCESS) {
+    *error = "Failed to realize OpenSL engine";
+  }
+
+  const SLInterfaceID ids[1] = {SL_IID_VOLUME};
+  const SLboolean req[1] = {SL_BOOLEAN_FALSE};
+  result = (*engineInterface)->CreateOutputMix(engineInterface, &outputMix, 1, ids, req);
+  if (result != SL_RESULT_SUCCESS) {
+    *error = "Failed to create output mix";
+  }
+
+  result = (*outputMix)->Realize(outputMix, SL_BOOLEAN_FALSE);
+  if (result != SL_RESULT_SUCCESS) {
+    *error = "Failed to realize output mix";
+  }
+}
+
+void slplay_destroy() {
+  for (uri_player *player = players; player->uri != NULL; player++) {
+    if (player->player) {
+      (*(player->player))->Destroy(player->player);
+    }
+  }
+
+  (*outputMix)->Destroy(outputMix);
+  (*engine)->Destroy(engine);
+}
+
+void slplay_stop(uri_player* player, char **error) {
+  SLPlayItf playInterface = player->playInterface;
+  SLresult result;
+
+  // stop a loop
+  loop_start = 0;
+
+  result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_PAUSED);
+  if (result != SL_RESULT_SUCCESS) {
+    *error = "Failed to set SL_PLAYSTATE_STOPPED";
+    return;
+  }
+}
+
+void slplay_stop_uri(const char* uri, char **error) {
+  uri_player* player = get_player_by_uri(uri);
+  slplay_stop(player, error);
+}
+
+void SLAPIENTRY slplay_callback(SLPlayItf playItf, void *context, SLuint32 event) {
+  uint64_t cb_loop_start = *((uint64_t*)context);
+  if (event == SL_PLAYEVENT_HEADATEND && cb_loop_start == loop_start) {
+    (*playItf)->SetPlayState(playItf, SL_PLAYSTATE_STOPPED);
+    (*playItf)->SetMarkerPosition(playItf, 0);
+    (*playItf)->SetPlayState(playItf, SL_PLAYSTATE_PLAYING);
+  }
+}
+
+void slplay_play (const char *uri, bool loop, char **error) {
+  SLresult result;
+
+  uri_player* player = get_player_by_uri(uri);
+  if (player == NULL) {
+    player = slplay_create_player_for_uri(uri, error);
+    if (*error) {
+      return;
+    }
+  }
+
+  SLPlayItf playInterface = player->playInterface;
+  if (loop) {
+    loop_start = nanos_since_boot();
+    loop_start_ctx = loop_start;
+    result = (*playInterface)->RegisterCallback(playInterface, slplay_callback, &loop_start_ctx);
+    if (result != SL_RESULT_SUCCESS) {
+      char error[64];
+      snprintf(error, sizeof(error), "Failed to register callback. %d", result);
+      *error = error[0];
+      return;
+    }
+
+    result = (*playInterface)->SetCallbackEventsMask(playInterface, SL_PLAYEVENT_HEADATEND);
+    if (result != SL_RESULT_SUCCESS) {
+      *error = "Failed to set callback event mask";
+      return;
+    }
+  }
+
+  // Reset the audio player
+  result = (*playInterface)->ClearMarkerPosition(playInterface);
+  if (result != SL_RESULT_SUCCESS) {
+    *error = "Failed to clear marker position";
+    return;
+  }
+  result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_PAUSED);
+  result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_STOPPED);
+  result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_PLAYING);
+  if (result != SL_RESULT_SUCCESS) {
+    *error = "Failed to set SL_PLAYSTATE_PLAYING";
+  }
+}

selfdrive/ui/slplay.h

@@ -0,0 +1,21 @@
+#ifndef SLPLAY_H
+#define SLPLAY_H
+
+#include <SLES/OpenSLES.h>
+#include <SLES/OpenSLES_Android.h>
+#include <stdbool.h>
+
+typedef struct {
+  const char* uri;
+  SLObjectItf player;
+  SLPlayItf playInterface;
+} uri_player;
+
+void slplay_setup(char **error);
+uri_player* slplay_create_player_for_uri(const char* uri, char **error);
+void slplay_play (const char *uri, bool loop, char **error);
+void slplay_stop_uri (const char* uri, char **error);
+void slplay_destroy();
+
+#endif
+

selfdrive/ui/sound.cc

@@ -0,0 +1,85 @@
+#include <stdlib.h>
+#include "sound.hpp"
+
+#include "common/swaglog.h"
+
+typedef struct {
+  AudibleAlert alert;
+  const char* uri;
+  bool loop;
+} sound_file;
+
+extern "C"{
+#include "slplay.h"
+}
+
+void set_volume(int volume) {
+  char volume_change_cmd[64];
+  sprintf(volume_change_cmd, "service call audio 3 i32 3 i32 %d i32 1 &", volume);
+
+  // 5 second timeout at 60fps
+  int volume_changed = system(volume_change_cmd);
+}
+
+
+sound_file sound_table[] = {
+  { cereal_CarControl_HUDControl_AudibleAlert_chimeDisengage, "../assets/sounds/disengaged.wav", false },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimeEngage, "../assets/sounds/engaged.wav", false },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimeWarning1, "../assets/sounds/warning_1.wav", false },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimeWarning2, "../assets/sounds/warning_2.wav", false },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat, "../assets/sounds/warning_repeat.wav", true },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimeError, "../assets/sounds/error.wav", false },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimePrompt, "../assets/sounds/error.wav", false },
+  { cereal_CarControl_HUDControl_AudibleAlert_none, NULL, false },
+};
+
+sound_file* get_sound_file(AudibleAlert alert) {
+  for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) {
+    if (s->alert == alert) {
+      return s;
+    }
+  }
+
+  return NULL;
+}
+
+void play_alert_sound(AudibleAlert alert) {
+  sound_file* sound = get_sound_file(alert);
+  char* error = NULL;
+
+  slplay_play(sound->uri, sound->loop, &error);
+  if(error) {
+    LOGW("error playing sound: %s", error);
+  }
+}
+
+void stop_alert_sound(AudibleAlert alert) {
+  sound_file* sound = get_sound_file(alert);
+  char* error = NULL;
+
+  slplay_stop_uri(sound->uri, &error);
+  if(error) {
+    LOGW("error stopping sound: %s", error);
+  }
+}
+
+void ui_sound_init() {
+  char *error = NULL;
+  slplay_setup(&error);
+  if (error) goto fail;
+
+  for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) {
+    slplay_create_player_for_uri(s->uri, &error);
+    if (error) goto fail;
+  }
+  return;
+
+fail:
+  LOGW(error);
+  exit(1);
+}
+
+void ui_sound_destroy() {
+  slplay_destroy();
+}
+

selfdrive/ui/sound.hpp

@@ -0,0 +1,17 @@
+#ifndef __SOUND_HPP
+#define __SOUND_HPP
+
+#include "cereal/gen/c/log.capnp.h"
+
+typedef enum cereal_CarControl_HUDControl_AudibleAlert AudibleAlert;
+
+void ui_sound_init();
+void ui_sound_destroy();
+
+void set_volume(int volume);
+
+void play_alert_sound(AudibleAlert alert);
+void stop_alert_sound(AudibleAlert alert);
+
+#endif
+

selfdrive/ui/spinner/Makefile

@@ -0,0 +1,74 @@
+CC = clang
+CXX = clang++
+
+PHONELIBS = ../../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS)
+
+NANOVG_FLAGS = -I$(PHONELIBS)/nanovg
+
+OPENGL_LIBS = -lGLESv3
+
+FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
+
+OBJS = spinner.o \
+       ../../common/framebuffer.o \
+       $(PHONELIBS)/nanovg/nanovg.o \
+       ../../common/spinner.o \
+       opensans_semibold.o \
+       img_spinner_track.o \
+       img_spinner_comma.o
+
+DEPS := $(OBJS:.o=.d)
+
+.PHONY: all
+all: spinner
+
+spinner: $(OBJS)
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+	      -s \
+        $(FRAMEBUFFER_LIBS) \
+        -L/system/vendor/lib64 \
+        $(OPENGL_LIBS) \
+        -lm -llog
+
+../../common/framebuffer.o: ../../common/framebuffer.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+           -I$(PHONELIBS)/android_frameworks_native/include \
+           -I$(PHONELIBS)/android_system_core/include \
+           -I$(PHONELIBS)/android_hardware_libhardware/include \
+           -c -o '$@' '$<'
+
+opensans_semibold.o: ../../assets/fonts/opensans_semibold.ttf
+	@echo "[ bin2o ] $@"
+	cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
+
+img_spinner_track.o: ../../assets/img_spinner_track.png
+	@echo "[ bin2o ] $@"
+	cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
+
+img_spinner_comma.o: ../../assets/img_spinner_comma.png
+	@echo "[ bin2o ] $@"
+	cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
+
+%.o: %.c
+	@echo "[ CC ] $@"
+	$(CC) $(CFLAGS) -MMD \
+          -I../.. \
+          $(NANOVG_FLAGS) \
+          -c -o '$@' '$<'
+
+.PHONY: clean
+clean:
+	rm -f spinner $(OBJS) $(DEPS)
+
+-include $(DEPS)

selfdrive/ui/spinner/spinner.c

@@ -0,0 +1,21 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <math.h>
+#include <unistd.h>
+#include <assert.h>
+
+#include <GLES3/gl3.h>
+#include <EGL/egl.h>
+#include <EGL/eglext.h>
+
+#include "common/framebuffer.h"
+#include "common/spinner.h"
+
+int main(int argc, char** argv) {
+  int err;
+
+  spin(argc, argv);
+
+  return 0;
+}

selfdrive/ui/test/.gitignore

@@ -0,0 +1,2 @@
+test
+play_sound

selfdrive/ui/test/Makefile

@@ -0,0 +1,69 @@
+CC = clang
+CXX = clang++
+
+PHONELIBS = ../../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
+
+ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
+ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
+           -l:libczmq.a -l:libzmq.a \
+           -lgnustl_shared
+
+NANOVG_FLAGS = -I$(PHONELIBS)/nanovg
+JSON_FLAGS = -I$(PHONELIBS)/json/src
+
+OPENGL_LIBS = -lGLESv3
+
+FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
+
+OBJS = test.o \
+       ../../common/framebuffer.o \
+       ../../common/touch.o
+
+DEPS := $(OBJS:.o=.d)
+
+all: test
+
+test: $(OBJS)
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+	      $(FRAMEBUFFER_LIBS) \
+	      $(CEREAL_LIBS) \
+	      $(ZMQ_LIBS) \
+        -L/system/vendor/lib64 \
+        -lhardware \
+        $(OPENGL_LIBS) \
+        -lcutils -lm -llog
+
+%.o: %.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+           -Iinclude -I.. -I../.. \
+           -I$(PHONELIBS)/android_frameworks_native/include \
+           -I$(PHONELIBS)/android_system_core/include \
+           -I$(PHONELIBS)/android_hardware_libhardware/include \
+           -c -o '$@' '$<'
+
+%.o: %.c
+	@echo "[ CC ] $@"
+	$(CC) $(CFLAGS) -MMD \
+          -I.. -I../.. \
+          $(NANOVG_FLAGS) \
+          $(ZMQ_FLAGS) \
+          $(CEREAL_CFLAGS) \
+          $(JSON_FLAGS) \
+          -c -o '$@' '$<'
+
+.PHONY: clean
+clean:
+	rm -f ui $(OBJS) $(DEPS)
+
+-include $(DEPS)

selfdrive/ui/test/build_play_sound.sh

@@ -0,0 +1,3 @@
+#!/bin/sh
+clang -fPIC -o play_sound play_sound.c ../slplay.c -I ../../ -I ../ -lOpenSLES -Wl,-rpath=/system/lib64
+

selfdrive/ui/test/play_sound.c

@@ -0,0 +1,37 @@
+#include <stdio.h>
+#include "slplay.h"
+
+void play_sound(char *uri, int volume) {
+  char **error = NULL;
+  printf("call slplay_setup\n");
+  slplay_setup(error);
+  if (error) { printf("%s\n", *error); return; }
+
+  printf("call slplay_create_player_for_uri\n");
+  slplay_create_player_for_uri(uri, error);
+  if (error) { printf("%s\n", *error); return; }
+
+  printf("call slplay_play\n");
+
+  while (1) {
+    char volume_change_cmd[64];
+    sprintf(volume_change_cmd, "service call audio 3 i32 3 i32 %d i32 1", volume);
+    system(volume_change_cmd);
+
+    slplay_play(uri, false, error);
+    if (error) { printf("%s\n", *error); return; }
+
+    sleep(1);
+  }
+}
+
+int main(int argc, char *argv[]) {
+  int volume = 10;
+  if (argc > 2) {
+    volume = atoi(argv[2]);
+  }
+  printf("setting volume to %d\n", volume);
+
+  play_sound(argv[1], volume);
+  return 0;
+}

selfdrive/ui/test/test.c

@@ -0,0 +1,48 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+
+#include <GLES3/gl3.h>
+#include <EGL/egl.h>
+#include <EGL/eglext.h>
+
+#include "common/framebuffer.h"
+#include "common/touch.h"
+
+typedef struct UIState {
+  FramebufferState *fb;
+  int fb_w, fb_h;
+  EGLDisplay display;
+  EGLSurface surface;
+} UIState;
+
+TouchState touch = {0};
+
+void wait_for_touch() {
+  int touch_x = -1, touch_y = -1;
+  while (1) {
+    int touched = touch_poll(&touch, &touch_x, &touch_y, 0);
+    if (touched == 1) { break; }
+  }
+}
+
+int main() {
+  UIState uistate;
+  UIState *s = &uistate;
+
+  memset(s, 0, sizeof(UIState));
+  s->fb = framebuffer_init("ui", 0x00010000, true,
+                           &s->display, &s->surface, &s->fb_w, &s->fb_h);
+
+  touch_init(&touch);
+
+  printf("waiting for touch with screen on\n");
+  framebuffer_set_power(s->fb, HWC_POWER_MODE_NORMAL);
+  wait_for_touch();
+
+  printf("waiting for touch with screen off\n");
+  framebuffer_set_power(s->fb, HWC_POWER_MODE_OFF);
+  wait_for_touch();
+  printf("done\n");
+}
+

selfdrive/ui/ui.cc

@@ -0,0 +1,972 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <assert.h>
+#include <sys/mman.h>
+#include <sys/resource.h>
+
+#include <json.h>
+#include <czmq.h>
+
+#include "common/util.h"
+#include "common/messaging.h"
+#include "common/timing.h"
+#include "common/swaglog.h"
+#include "common/touch.h"
+#include "common/visionimg.h"
+#include "common/params.h"
+
+#include "ui.hpp"
+#include "sound.hpp"
+
+static int last_brightness = -1;
+static void set_brightness(UIState *s, int brightness) {
+  if (last_brightness != brightness && (s->awake || brightness == 0)) {
+    FILE *f = fopen("/sys/class/leds/lcd-backlight/brightness", "wb");
+    if (f != NULL) {
+      fprintf(f, "%d", brightness);
+      fclose(f);
+      last_brightness = brightness;
+    }
+  }
+}
+
+static void set_awake(UIState *s, bool awake) {
+#ifdef QCOM
+  if (awake) {
+    // 30 second timeout at 30 fps
+    s->awake_timeout = 30*30;
+  }
+  if (s->awake != awake) {
+    s->awake = awake;
+
+    // TODO: replace command_awake and command_sleep with direct calls to android
+    if (awake) {
+      LOGW("awake normal");
+      system("service call window 18 i32 1");  // enable event processing
+      framebuffer_set_power(s->fb, HWC_POWER_MODE_NORMAL);
+    } else {
+      LOGW("awake off");
+      set_brightness(s, 0);
+      system("service call window 18 i32 0");  // disable event processing
+      framebuffer_set_power(s->fb, HWC_POWER_MODE_OFF);
+    }
+  }
+#else
+  // computer UI doesn't sleep
+  s->awake = true;
+#endif
+}
+
+volatile sig_atomic_t do_exit = 0;
+static void set_do_exit(int sig) {
+  do_exit = 1;
+}
+
+static void read_param_bool(bool* param, const char* param_name) {
+  char *s;
+  const int result = read_db_value(NULL, param_name, &s, NULL);
+  if (result == 0) {
+    *param = s[0] == '1';
+    free(s);
+  }
+}
+
+static void read_param_float(float* param, const char* param_name) {
+  char *s;
+  const int result = read_db_value(NULL, param_name, &s, NULL);
+  if (result == 0) {
+    *param = strtod(s, NULL);
+    free(s);
+  }
+}
+
+static void read_param_bool_timeout(bool* param, const char* param_name, int* timeout) {
+  if (*timeout > 0){
+    (*timeout)--;
+  } else {
+    read_param_bool(param, param_name);
+    *timeout = 2 * UI_FREQ; // 0.5Hz
+  }
+}
+
+static void read_param_float_timeout(float* param, const char* param_name, int* timeout) {
+  if (*timeout > 0){
+    (*timeout)--;
+  } else {
+    read_param_float(param, param_name);
+    *timeout = 2 * UI_FREQ; // 0.5Hz
+  }
+}
+
+static void ui_init(UIState *s) {
+  memset(s, 0, sizeof(UIState));
+
+  pthread_mutex_init(&s->lock, NULL);
+  pthread_cond_init(&s->bg_cond, NULL);
+
+  s->ctx = Context::create();
+  s->model_sock = SubSocket::create(s->ctx, "model");
+  s->controlsstate_sock = SubSocket::create(s->ctx, "controlsState");
+  s->uilayout_sock = SubSocket::create(s->ctx, "uiLayoutState");
+  s->livecalibration_sock = SubSocket::create(s->ctx, "liveCalibration");
+  s->radarstate_sock = SubSocket::create(s->ctx, "radarState");
+
+  assert(s->model_sock != NULL);
+  assert(s->controlsstate_sock != NULL);
+  assert(s->uilayout_sock != NULL);
+  assert(s->livecalibration_sock != NULL);
+  assert(s->radarstate_sock != NULL);
+
+  s->poller = Poller::create({
+                              s->model_sock,
+                              s->controlsstate_sock,
+                              s->uilayout_sock,
+                              s->livecalibration_sock,
+                              s->radarstate_sock
+                             });
+
+#ifdef SHOW_SPEEDLIMIT
+  s->map_data_sock = SubSock::create(s->ctx, "liveMapData");
+  assert(s->map_data_sock != NULL);
+  s->poller.registerSocket(s->map_data_sock);
+#endif
+
+  s->ipc_fd = -1;
+
+  // init display
+  s->fb = framebuffer_init("ui", 0x00010000, true, &s->fb_w, &s->fb_h);
+  assert(s->fb);
+
+  set_awake(s, true);
+
+  s->model_changed = false;
+  s->livempc_or_radarstate_changed = false;
+
+  ui_nvg_init(s);
+}
+
+static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
+                           int num_back_fds, const int *back_fds,
+                           const VisionStreamBufs front_bufs, int num_front_fds,
+                           const int *front_fds) {
+  const VisionUIInfo ui_info = back_bufs.buf_info.ui_info;
+
+  assert(num_back_fds == UI_BUF_COUNT);
+  assert(num_front_fds == UI_BUF_COUNT);
+
+  vipc_bufs_load(s->bufs, &back_bufs, num_back_fds, back_fds);
+  vipc_bufs_load(s->front_bufs, &front_bufs, num_front_fds, front_fds);
+
+  s->cur_vision_idx = -1;
+  s->cur_vision_front_idx = -1;
+
+  s->scene = (UIScene){
+      .frontview = getenv("FRONTVIEW") != NULL,
+      .fullview = getenv("FULLVIEW") != NULL,
+      .transformed_width = ui_info.transformed_width,
+      .transformed_height = ui_info.transformed_height,
+      .front_box_x = ui_info.front_box_x,
+      .front_box_y = ui_info.front_box_y,
+      .front_box_width = ui_info.front_box_width,
+      .front_box_height = ui_info.front_box_height,
+      .world_objects_visible = false,  // Invisible until we receive a calibration message.
+      .gps_planner_active = false,
+  };
+
+  s->rgb_width = back_bufs.width;
+  s->rgb_height = back_bufs.height;
+  s->rgb_stride = back_bufs.stride;
+  s->rgb_buf_len = back_bufs.buf_len;
+
+  s->rgb_front_width = front_bufs.width;
+  s->rgb_front_height = front_bufs.height;
+  s->rgb_front_stride = front_bufs.stride;
+  s->rgb_front_buf_len = front_bufs.buf_len;
+
+  s->rgb_transform = (mat4){{
+    2.0f/s->rgb_width, 0.0f, 0.0f, -1.0f,
+    0.0f, 2.0f/s->rgb_height, 0.0f, -1.0f,
+    0.0f, 0.0f, 1.0f, 0.0f,
+    0.0f, 0.0f, 0.0f, 1.0f,
+  }};
+
+  read_param_float(&s->speed_lim_off, "SpeedLimitOffset");
+  read_param_bool(&s->is_metric, "IsMetric");
+  read_param_bool(&s->longitudinal_control, "LongitudinalControl");
+  read_param_bool(&s->limit_set_speed, "LimitSetSpeed");
+
+  // Set offsets so params don't get read at the same time
+  s->longitudinal_control_timeout = UI_FREQ / 3;
+  s->is_metric_timeout = UI_FREQ / 2;
+  s->limit_set_speed_timeout = UI_FREQ;
+}
+
+static PathData read_path(cereal_ModelData_PathData_ptr pathp) {
+  PathData ret = {0};
+
+  struct cereal_ModelData_PathData pathd;
+  cereal_read_ModelData_PathData(&pathd, pathp);
+
+  ret.prob = pathd.prob;
+  ret.std = pathd.std;
+
+  capn_list32 polyp = pathd.poly;
+  capn_resolve(&polyp.p);
+  for (int i = 0; i < POLYFIT_DEGREE; i++) {
+    ret.poly[i] = capn_to_f32(capn_get32(polyp, i));
+  }
+
+  // Compute points locations
+  for (int i = 0; i < MODEL_PATH_DISTANCE; i++) {
+    ret.points[i] = ret.poly[0] * (i*i*i) + ret.poly[1] * (i*i)+ ret.poly[2] * i + ret.poly[3];
+  }
+
+  return ret;
+}
+
+static ModelData read_model(cereal_ModelData_ptr modelp) {
+  struct cereal_ModelData modeld;
+  cereal_read_ModelData(&modeld, modelp);
+
+  ModelData d = {0};
+
+  d.path = read_path(modeld.path);
+  d.left_lane = read_path(modeld.leftLane);
+  d.right_lane = read_path(modeld.rightLane);
+
+  struct cereal_ModelData_LeadData leadd;
+  cereal_read_ModelData_LeadData(&leadd, modeld.lead);
+  d.lead = (LeadData){
+      .dist = leadd.dist, .prob = leadd.prob, .std = leadd.std,
+  };
+
+  return d;
+}
+
+static void update_status(UIState *s, int status) {
+  if (s->status != status) {
+    s->status = status;
+    // wake up bg thread to change
+    pthread_cond_signal(&s->bg_cond);
+  }
+}
+
+
+void handle_message(UIState *s, Message * msg) {
+  struct capn ctx;
+  capn_init_mem(&ctx, (uint8_t*)msg->getData(), msg->getSize(), 0);
+
+  cereal_Event_ptr eventp;
+  eventp.p = capn_getp(capn_root(&ctx), 0, 1);
+  struct cereal_Event eventd;
+  cereal_read_Event(&eventd, eventp);
+
+  if (eventd.which == cereal_Event_controlsState) {
+    struct cereal_ControlsState datad;
+    cereal_read_ControlsState(&datad, eventd.controlsState);
+
+    s->controls_timeout = 1 * UI_FREQ;
+    s->controls_seen = true;
+
+    if (datad.vCruise != s->scene.v_cruise) {
+      s->scene.v_cruise_update_ts = eventd.logMonoTime;
+    }
+    s->scene.v_cruise = datad.vCruise;
+    s->scene.v_ego = datad.vEgo;
+    s->scene.curvature = datad.curvature;
+    s->scene.engaged = datad.enabled;
+    s->scene.engageable = datad.engageable;
+    s->scene.gps_planner_active = datad.gpsPlannerActive;
+    s->scene.monitoring_active = datad.driverMonitoringOn;
+
+    s->scene.frontview = datad.rearViewCam;
+
+    s->scene.decel_for_model = datad.decelForModel;
+
+    if (datad.alertSound != cereal_CarControl_HUDControl_AudibleAlert_none && datad.alertSound != s->alert_sound) {
+      if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
+        stop_alert_sound(s->alert_sound);
+      }
+      play_alert_sound(datad.alertSound);
+
+      s->alert_sound = datad.alertSound;
+      snprintf(s->alert_type, sizeof(s->alert_type), "%s", datad.alertType.str);
+    } else if ((!datad.alertSound || datad.alertSound == cereal_CarControl_HUDControl_AudibleAlert_none)
+                  && s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
+      stop_alert_sound(s->alert_sound);
+      s->alert_type[0] = '\0';
+      s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
+    }
+
+    if (datad.alertText1.str) {
+      snprintf(s->scene.alert_text1, sizeof(s->scene.alert_text1), "%s", datad.alertText1.str);
+    } else {
+      s->scene.alert_text1[0] = '\0';
+    }
+    if (datad.alertText2.str) {
+      snprintf(s->scene.alert_text2, sizeof(s->scene.alert_text2), "%s", datad.alertText2.str);
+    } else {
+      s->scene.alert_text2[0] = '\0';
+    }
+    s->scene.awareness_status = datad.awarenessStatus;
+
+    s->scene.alert_ts = eventd.logMonoTime;
+
+    s->scene.alert_size = datad.alertSize;
+    if (datad.alertSize == cereal_ControlsState_AlertSize_none) {
+      s->alert_size = ALERTSIZE_NONE;
+    } else if (datad.alertSize == cereal_ControlsState_AlertSize_small) {
+      s->alert_size = ALERTSIZE_SMALL;
+    } else if (datad.alertSize == cereal_ControlsState_AlertSize_mid) {
+      s->alert_size = ALERTSIZE_MID;
+    } else if (datad.alertSize == cereal_ControlsState_AlertSize_full) {
+      s->alert_size = ALERTSIZE_FULL;
+    }
+
+    if (s->status != STATUS_STOPPED) {
+      if (datad.alertStatus == cereal_ControlsState_AlertStatus_userPrompt) {
+        update_status(s, STATUS_WARNING);
+      } else if (datad.alertStatus == cereal_ControlsState_AlertStatus_critical) {
+        update_status(s, STATUS_ALERT);
+      } else if (datad.enabled) {
+        update_status(s, STATUS_ENGAGED);
+      } else {
+        update_status(s, STATUS_DISENGAGED);
+      }
+    }
+
+    s->scene.alert_blinkingrate = datad.alertBlinkingRate;
+    if (datad.alertBlinkingRate > 0.) {
+      if (s->alert_blinked) {
+        if (s->alert_blinking_alpha > 0.0 && s->alert_blinking_alpha < 1.0) {
+          s->alert_blinking_alpha += (0.05*datad.alertBlinkingRate);
+        } else {
+          s->alert_blinked = false;
+        }
+      } else {
+        if (s->alert_blinking_alpha > 0.25) {
+          s->alert_blinking_alpha -= (0.05*datad.alertBlinkingRate);
+        } else {
+          s->alert_blinking_alpha += 0.25;
+          s->alert_blinked = true;
+        }
+      }
+    }
+  } else if (eventd.which == cereal_Event_radarState) {
+    struct cereal_RadarState datad;
+    cereal_read_RadarState(&datad, eventd.radarState);
+    struct cereal_RadarState_LeadData leaddatad;
+    cereal_read_RadarState_LeadData(&leaddatad, datad.leadOne);
+    s->scene.lead_status = leaddatad.status;
+    s->scene.lead_d_rel = leaddatad.dRel;
+    s->scene.lead_y_rel = leaddatad.yRel;
+    s->scene.lead_v_rel = leaddatad.vRel;
+    s->livempc_or_radarstate_changed = true;
+  } else if (eventd.which == cereal_Event_liveCalibration) {
+    s->scene.world_objects_visible = true;
+    struct cereal_LiveCalibrationData datad;
+    cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration);
+
+    capn_list32 extrinsicl = datad.extrinsicMatrix;
+    capn_resolve(&extrinsicl.p);  // is this a bug?
+    for (int i = 0; i < 3 * 4; i++) {
+      s->scene.extrinsic_matrix.v[i] =
+          capn_to_f32(capn_get32(extrinsicl, i));
+    }
+  } else if (eventd.which == cereal_Event_model) {
+    s->scene.model = read_model(eventd.model);
+    s->model_changed = true;
+  } else if (eventd.which == cereal_Event_liveMpc) {
+    struct cereal_LiveMpcData datad;
+    cereal_read_LiveMpcData(&datad, eventd.liveMpc);
+
+    capn_list32 x_list = datad.x;
+    capn_resolve(&x_list.p);
+
+    for (int i = 0; i < 50; i++){
+      s->scene.mpc_x[i] = capn_to_f32(capn_get32(x_list, i));
+    }
+
+    capn_list32 y_list = datad.y;
+    capn_resolve(&y_list.p);
+
+    for (int i = 0; i < 50; i++){
+      s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i));
+    }
+    s->livempc_or_radarstate_changed = true;
+  } else if (eventd.which == cereal_Event_uiLayoutState) {
+    struct cereal_UiLayoutState datad;
+    cereal_read_UiLayoutState(&datad, eventd.uiLayoutState);
+    s->active_app = datad.activeApp;
+    s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed;
+    s->scene.uilayout_mapenabled = datad.mapEnabled;
+
+    bool hasSidebar = !s->scene.uilayout_sidebarcollapsed;
+    bool mapEnabled = s->scene.uilayout_mapenabled;
+    if (mapEnabled) {
+      s->scene.ui_viz_rx = hasSidebar ? (box_x+nav_w) : (box_x+nav_w-(bdr_s*4));
+      s->scene.ui_viz_rw = hasSidebar ? (box_w-nav_w) : (box_w-nav_w+(bdr_s*4));
+      s->scene.ui_viz_ro = -(sbr_w + 4*bdr_s);
+    } else {
+      s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x-sbr_w+bdr_s*2);
+      s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w+sbr_w-(bdr_s*2));
+      s->scene.ui_viz_ro = hasSidebar ? -(sbr_w - 6*bdr_s) : 0;
+    }
+  } else if (eventd.which == cereal_Event_liveMapData) {
+    struct cereal_LiveMapData datad;
+    cereal_read_LiveMapData(&datad, eventd.liveMapData);
+    s->scene.map_valid = datad.mapValid;
+  }
+  capn_free(&ctx);
+}
+
+static void ui_update(UIState *s) {
+  int err;
+
+  if (s->vision_connect_firstrun) {
+    // cant run this in connector thread because opengl.
+    // do this here for now in lieu of a run_on_main_thread event
+
+    for (int i=0; i<UI_BUF_COUNT; i++) {
+      if(s->khr[i] != NULL) {
+        visionimg_destroy_gl(s->khr[i], s->priv_hnds[i]);
+        glDeleteTextures(1, &s->frame_texs[i]);
+      }
+
+      VisionImg img = {
+        .fd = s->bufs[i].fd,
+        .format = VISIONIMG_FORMAT_RGB24,
+        .width = s->rgb_width,
+        .height = s->rgb_height,
+        .stride = s->rgb_stride,
+        .bpp = 3,
+        .size = s->rgb_buf_len,
+      };
+      #ifndef QCOM
+        s->priv_hnds[i] = s->bufs[i].addr;
+      #endif
+      s->frame_texs[i] = visionimg_to_gl(&img, &s->khr[i], &s->priv_hnds[i]);
+
+      glBindTexture(GL_TEXTURE_2D, s->frame_texs[i]);
+      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+
+      // BGR
+      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE);
+      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN);
+      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED);
+    }
+
+    for (int i=0; i<UI_BUF_COUNT; i++) {
+      if(s->khr_front[i] != NULL) {
+        visionimg_destroy_gl(s->khr_front[i], s->priv_hnds_front[i]);
+        glDeleteTextures(1, &s->frame_front_texs[i]);
+      }
+
+      VisionImg img = {
+        .fd = s->front_bufs[i].fd,
+        .format = VISIONIMG_FORMAT_RGB24,
+        .width = s->rgb_front_width,
+        .height = s->rgb_front_height,
+        .stride = s->rgb_front_stride,
+        .bpp = 3,
+        .size = s->rgb_front_buf_len,
+      };
+      #ifndef QCOM
+        s->priv_hnds_front[i] = s->bufs[i].addr;
+      #endif
+      s->frame_front_texs[i] = visionimg_to_gl(&img, &s->khr_front[i], &s->priv_hnds_front[i]);
+
+      glBindTexture(GL_TEXTURE_2D, s->frame_front_texs[i]);
+      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+
+      // BGR
+      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE);
+      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN);
+      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED);
+    }
+
+    assert(glGetError() == GL_NO_ERROR);
+
+    // Default UI Measurements (Assumes sidebar collapsed)
+    s->scene.ui_viz_rx = (box_x-sbr_w+bdr_s*2);
+    s->scene.ui_viz_rw = (box_w+sbr_w-(bdr_s*2));
+    s->scene.ui_viz_ro = 0;
+
+    s->vision_connect_firstrun = false;
+
+    s->alert_blinking_alpha = 1.0;
+    s->alert_blinked = false;
+  }
+
+  zmq_pollitem_t polls[1] = {{0}};
+  // Take an rgb image from visiond if there is one
+  while(true) {
+    assert(s->ipc_fd >= 0);
+    polls[0].fd = s->ipc_fd;
+    polls[0].events = ZMQ_POLLIN;
+    #ifdef UI_60FPS
+      // uses more CPU in both UI and surfaceflinger
+      // 16% / 21%
+      int ret = zmq_poll(polls, 1, 1);
+    #else
+      // 9% / 13% = a 14% savings
+      int ret = zmq_poll(polls, 1, 1000);
+    #endif
+    if (ret < 0) {
+      if (errno == EINTR) continue;
+
+      LOGW("poll failed (%d)", ret);
+      close(s->ipc_fd);
+      s->ipc_fd = -1;
+      s->vision_connected = false;
+      return;
+    } else if (ret == 0) {
+      break;
+    }
+    // vision ipc event
+    VisionPacket rp;
+    err = vipc_recv(s->ipc_fd, &rp);
+    if (err <= 0) {
+      LOGW("vision disconnected");
+      close(s->ipc_fd);
+      s->ipc_fd = -1;
+      s->vision_connected = false;
+      return;
+    }
+    if (rp.type == VIPC_STREAM_ACQUIRE) {
+      bool front = rp.d.stream_acq.type == VISION_STREAM_RGB_FRONT;
+      int idx = rp.d.stream_acq.idx;
+
+      int release_idx;
+      if (front) {
+        release_idx = s->cur_vision_front_idx;
+      } else {
+        release_idx = s->cur_vision_idx;
+      }
+      if (release_idx >= 0) {
+        VisionPacket rep = {
+          .type = VIPC_STREAM_RELEASE,
+          .d = { .stream_rel = {
+            .type = rp.d.stream_acq.type,
+            .idx = release_idx,
+          }},
+        };
+        vipc_send(s->ipc_fd, &rep);
+      }
+
+      if (front) {
+        assert(idx < UI_BUF_COUNT);
+        s->cur_vision_front_idx = idx;
+      } else {
+        assert(idx < UI_BUF_COUNT);
+        s->cur_vision_idx = idx;
+        // printf("v %d\n", ((uint8_t*)s->bufs[idx].addr)[0]);
+      }
+    } else {
+      assert(false);
+    }
+    break;
+  }
+  // peek and consume all events in the zmq queue, then return.
+  while(true) {
+    auto polls = s->poller->poll(0);
+
+    if (polls.size() == 0)
+      return;
+
+    for (auto sock : polls){
+      Message * msg = sock->receive();
+      if (msg == NULL) continue;
+
+      set_awake(s, true);
+
+      handle_message(s, msg);
+
+      delete msg;
+    }
+  }
+}
+
+static int vision_subscribe(int fd, VisionPacket *rp, VisionStreamType type) {
+  int err;
+  LOGW("vision_subscribe type:%d", type);
+
+  VisionPacket p1 = {
+    .type = VIPC_STREAM_SUBSCRIBE,
+    .d = { .stream_sub = { .type = type, .tbuffer = true, }, },
+  };
+  err = vipc_send(fd, &p1);
+  if (err < 0) {
+    close(fd);
+    return 0;
+  }
+
+  do {
+    err = vipc_recv(fd, rp);
+    if (err <= 0) {
+      close(fd);
+      return 0;
+    }
+
+    // release what we aren't ready for yet
+    if (rp->type == VIPC_STREAM_ACQUIRE) {
+      VisionPacket rep = {
+        .type = VIPC_STREAM_RELEASE,
+        .d = { .stream_rel = {
+          .type = rp->d.stream_acq.type,
+          .idx = rp->d.stream_acq.idx,
+        }},
+      };
+      vipc_send(fd, &rep);
+    }
+  } while (rp->type != VIPC_STREAM_BUFS || rp->d.stream_bufs.type != type);
+
+  return 1;
+}
+
+static void* vision_connect_thread(void *args) {
+  int err;
+  set_thread_name("vision_connect");
+
+  UIState *s = (UIState*)args;
+  while (!do_exit) {
+    usleep(100000);
+    pthread_mutex_lock(&s->lock);
+    bool connected = s->vision_connected;
+    pthread_mutex_unlock(&s->lock);
+    if (connected) continue;
+
+    int fd = vipc_connect();
+    if (fd < 0) continue;
+
+    VisionPacket back_rp, front_rp;
+    if (!vision_subscribe(fd, &back_rp, VISION_STREAM_RGB_BACK)) continue;
+    if (!vision_subscribe(fd, &front_rp, VISION_STREAM_RGB_FRONT)) continue;
+
+    pthread_mutex_lock(&s->lock);
+    assert(!s->vision_connected);
+    s->ipc_fd = fd;
+
+    ui_init_vision(s,
+                   back_rp.d.stream_bufs, back_rp.num_fds, back_rp.fds,
+                   front_rp.d.stream_bufs, front_rp.num_fds, front_rp.fds);
+
+    s->vision_connected = true;
+    s->vision_connect_firstrun = true;
+
+    // Drain sockets
+    while (true){
+      auto polls = s->poller->poll(0);
+      if (polls.size() == 0)
+        break;
+
+      for (auto sock : polls){
+        Message * msg = sock->receive();
+        if (msg == NULL) continue;
+        delete msg;
+      }
+    }
+
+    pthread_mutex_unlock(&s->lock);
+  }
+  return NULL;
+}
+
+#ifdef QCOM
+
+#include <cutils/properties.h>
+#include <hardware/sensors.h>
+#include <utils/Timers.h>
+
+static void* light_sensor_thread(void *args) {
+  int err;
+  set_thread_name("light_sensor");
+
+  UIState *s = (UIState*)args;
+  s->light_sensor = 0.0;
+
+  struct sensors_poll_device_t* device;
+  struct sensors_module_t* module;
+
+  hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module);
+  sensors_open(&module->common, &device);
+
+  // need to do this
+  struct sensor_t const* list;
+  int count = module->get_sensors_list(module, &list);
+
+  int SENSOR_LIGHT = 7;
+
+  err = device->activate(device, SENSOR_LIGHT, 0);
+  if (err != 0) goto fail;
+  err = device->activate(device, SENSOR_LIGHT, 1);
+  if (err != 0) goto fail;
+
+  device->setDelay(device, SENSOR_LIGHT, ms2ns(100));
+
+  while (!do_exit) {
+    static const size_t numEvents = 1;
+    sensors_event_t buffer[numEvents];
+
+    int n = device->poll(device, buffer, numEvents);
+    if (n < 0) {
+      LOG_100("light_sensor_poll failed: %d", n);
+    }
+    if (n > 0) {
+      s->light_sensor = buffer[0].light;
+    }
+  }
+
+  return NULL;
+
+fail:
+  LOGE("LIGHT SENSOR IS MISSING");
+  s->light_sensor = 255;
+  return NULL;
+}
+
+
+static void* bg_thread(void* args) {
+  UIState *s = (UIState*)args;
+  set_thread_name("bg");
+
+  FramebufferState *bg_fb = framebuffer_init("bg", 0x00001000, false, NULL, NULL);
+  assert(bg_fb);
+
+  int bg_status = -1;
+  while(!do_exit) {
+    pthread_mutex_lock(&s->lock);
+    if (bg_status == s->status) {
+      // will always be signaled if it changes?
+      pthread_cond_wait(&s->bg_cond, &s->lock);
+    }
+    bg_status = s->status;
+    pthread_mutex_unlock(&s->lock);
+
+    assert(bg_status < ARRAYSIZE(bg_colors));
+    const uint8_t *color = bg_colors[bg_status];
+
+    glClearColor(color[0]/256.0, color[1]/256.0, color[2]/256.0, 0.0);
+    glClear(GL_COLOR_BUFFER_BIT);
+
+    framebuffer_swap(bg_fb);
+  }
+
+  return NULL;
+}
+
+#endif
+
+int is_leon() {
+  #define MAXCHAR 1000
+  FILE *fp;
+  char str[MAXCHAR];
+  const char* filename = "/proc/cmdline";
+
+  fp = fopen(filename, "r");
+  if (fp == NULL){
+    printf("Could not open file %s",filename);
+    return 0;
+  }
+  fgets(str, MAXCHAR, fp);
+  fclose(fp);
+  return strstr(str, "letv") != NULL;
+}
+
+
+
+int main(int argc, char* argv[]) {
+  int err;
+  setpriority(PRIO_PROCESS, 0, -14);
+
+  zsys_handler_set(NULL);
+  signal(SIGINT, (sighandler_t)set_do_exit);
+
+  UIState uistate;
+  UIState *s = &uistate;
+  ui_init(s);
+
+  pthread_t connect_thread_handle;
+  err = pthread_create(&connect_thread_handle, NULL,
+                       vision_connect_thread, s);
+  assert(err == 0);
+
+#ifdef QCOM
+  pthread_t light_sensor_thread_handle;
+  err = pthread_create(&light_sensor_thread_handle, NULL,
+                       light_sensor_thread, s);
+  assert(err == 0);
+
+  pthread_t bg_thread_handle;
+  err = pthread_create(&bg_thread_handle, NULL,
+                       bg_thread, s);
+  assert(err == 0);
+#endif
+
+  TouchState touch = {0};
+  touch_init(&touch);
+  s->touch_fd = touch.fd;
+
+  ui_sound_init();
+
+  // light sensor scaling params
+  const int LEON = is_leon();
+
+  const float BRIGHTNESS_B = LEON ? 10.0 : 5.0;
+  const float BRIGHTNESS_M = LEON ? 2.6 : 1.3;
+
+  float smooth_brightness = BRIGHTNESS_B;
+
+  const int MIN_VOLUME = LEON ? 12 : 9;
+  const int MAX_VOLUME = LEON ? 15 : 12;
+
+  set_volume(MIN_VOLUME);
+  s->volume_timeout = 5 * UI_FREQ;
+  int draws = 0;
+  while (!do_exit) {
+    bool should_swap = false;
+    if (!s->vision_connected) {
+      // Delay a while to avoid 9% cpu usage while car is not started and user is keeping touching on the screen.
+      // Don't hold the lock while sleeping, so that vision_connect_thread have chances to get the lock.
+      usleep(30 * 1000);
+    }
+    pthread_mutex_lock(&s->lock);
+    double u1 = millis_since_boot();
+
+    // light sensor is only exposed on EONs
+    float clipped_brightness = (s->light_sensor*BRIGHTNESS_M) + BRIGHTNESS_B;
+    if (clipped_brightness > 512) clipped_brightness = 512;
+    smooth_brightness = clipped_brightness * 0.01 + smooth_brightness * 0.99;
+    if (smooth_brightness > 255) smooth_brightness = 255;
+    set_brightness(s, (int)smooth_brightness);
+
+    if (!s->vision_connected) {
+      // Car is not started, keep in idle state and awake on touch events
+      zmq_pollitem_t polls[1] = {{0}};
+      polls[0].fd = s->touch_fd;
+      polls[0].events = ZMQ_POLLIN;
+      int ret = zmq_poll(polls, 1, 0);
+      if (ret < 0){
+        if (errno == EINTR) continue;
+        LOGW("poll failed (%d)", ret);
+      } else if (ret > 0) {
+        // awake on any touch
+        int touch_x = -1, touch_y = -1;
+        int touched = touch_read(&touch, &touch_x, &touch_y);
+        if (touched == 1) {
+          set_awake(s, true);
+        }
+      }
+      if (s->status != STATUS_STOPPED) {
+        update_status(s, STATUS_STOPPED);
+      }
+    } else {
+      if (s->status == STATUS_STOPPED) {
+        update_status(s, STATUS_DISENGAGED);
+      }
+      // Car started, fetch a new rgb image from ipc and peek for zmq events.
+      ui_update(s);
+      if(!s->vision_connected) {
+        // Visiond process is just stopped, force a redraw to make screen blank again.
+        ui_draw(s);
+        glFinish();
+        should_swap = true;
+      }
+    }
+
+    // manage wakefulness
+    if (s->awake_timeout > 0) {
+      s->awake_timeout--;
+    } else {
+      set_awake(s, false);
+    }
+
+    // Don't waste resources on drawing in case screen is off or car is not started.
+    if (s->awake && s->vision_connected) {
+      ui_draw(s);
+      glFinish();
+      should_swap = true;
+    }
+
+    if (s->volume_timeout > 0) {
+      s->volume_timeout--;
+    } else {
+      int volume = fmin(MAX_VOLUME, MIN_VOLUME + s->scene.v_ego / 5);  // up one notch every 5 m/s
+      set_volume(volume);
+      s->volume_timeout = 5 * UI_FREQ;
+    }
+
+    if (s->controls_timeout > 0) {
+      s->controls_timeout--;
+    } else {
+      // stop playing alert sound
+      if ((!s->vision_connected || (s->vision_connected && s->alert_sound_timeout == 0)) &&
+            s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
+        stop_alert_sound(s->alert_sound);
+        s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
+      }
+
+      // if visiond is still running and controlsState times out, display an alert
+      // TODO: refactor this to not be here
+      if (s->controls_seen && s->vision_connected && strcmp(s->scene.alert_text2, "Controls Unresponsive") != 0) {
+        s->scene.alert_size = ALERTSIZE_FULL;
+        if (s->status != STATUS_STOPPED) {
+          update_status(s, STATUS_ALERT);
+        }
+        snprintf(s->scene.alert_text1, sizeof(s->scene.alert_text1), "%s", "TAKE CONTROL IMMEDIATELY");
+        snprintf(s->scene.alert_text2, sizeof(s->scene.alert_text2), "%s", "Controls Unresponsive");
+        ui_draw_vision_alert(s, s->scene.alert_size, s->status, s->scene.alert_text1, s->scene.alert_text2);
+
+        s->alert_sound_timeout = 2 * UI_FREQ;
+
+        s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat;
+        play_alert_sound(s->alert_sound);
+      }
+      s->alert_sound_timeout--;
+      s->controls_seen = false;
+    }
+
+    read_param_bool_timeout(&s->is_metric, "IsMetric", &s->is_metric_timeout);
+    read_param_bool_timeout(&s->longitudinal_control, "LongitudinalControl", &s->longitudinal_control_timeout);
+    read_param_bool_timeout(&s->limit_set_speed, "LimitSetSpeed", &s->limit_set_speed_timeout);
+    read_param_float_timeout(&s->speed_lim_off, "SpeedLimitOffset", &s->limit_set_speed_timeout);
+
+    pthread_mutex_unlock(&s->lock);
+
+    // the bg thread needs to be scheduled, so the main thread needs time without the lock
+    // safe to do this outside the lock?
+    if (should_swap) {
+      double u2 = millis_since_boot();
+      if (u2-u1 > 66) {
+        // warn on sub 15fps
+        LOGW("slow frame(%d) time: %.2f", draws, u2-u1);
+      }
+      draws++;
+      framebuffer_swap(s->fb);
+    }
+  }
+
+  set_awake(s, true);
+  ui_sound_destroy();
+
+  // wake up bg thread to exit
+  pthread_mutex_lock(&s->lock);
+  pthread_cond_signal(&s->bg_cond);
+  pthread_mutex_unlock(&s->lock);
+
+#ifdef QCOM
+  // join light_sensor_thread?
+
+  err = pthread_join(bg_thread_handle, NULL);
+  assert(err == 0);
+#endif
+
+  err = pthread_join(connect_thread_handle, NULL);
+  assert(err == 0);
+
+  return 0;
+}

selfdrive/ui/ui.hpp

@@ -0,0 +1,253 @@
+#ifndef _UI_H
+#define _UI_H
+
+#include <GLES3/gl3.h>
+#include <EGL/egl.h>
+
+#include "nanovg.h"
+
+#include "common/mat.h"
+#include "common/visionipc.h"
+#include "common/framebuffer.h"
+#include "common/modeldata.h"
+#include "messaging.hpp"
+
+#include "cereal/gen/c/log.capnp.h"
+
+#include "sound.hpp"
+
+#define STATUS_STOPPED 0
+#define STATUS_DISENGAGED 1
+#define STATUS_ENGAGED 2
+#define STATUS_WARNING 3
+#define STATUS_ALERT 4
+
+#define ALERTSIZE_NONE 0
+#define ALERTSIZE_SMALL 1
+#define ALERTSIZE_MID 2
+#define ALERTSIZE_FULL 3
+
+#ifndef QCOM
+  #define UI_60FPS
+#endif
+
+#define UI_BUF_COUNT 4
+//#define SHOW_SPEEDLIMIT 1
+//#define DEBUG_TURN
+
+const int vwp_w = 1920;
+const int vwp_h = 1080;
+const int nav_w = 640;
+const int nav_ww= 760;
+const int sbr_w = 300;
+const int bdr_s = 30;
+const int box_x = sbr_w+bdr_s;
+const int box_y = bdr_s;
+const int box_w = vwp_w-sbr_w-(bdr_s*2);
+const int box_h = vwp_h-(bdr_s*2);
+const int viz_w = vwp_w-(bdr_s*2);
+const int header_h = 420;
+const int footer_h = 280;
+const int footer_y = vwp_h-bdr_s-footer_h;
+
+const int UI_FREQ = 30;   // Hz
+
+const int MODEL_PATH_MAX_VERTICES_CNT = 98;
+const int MODEL_LANE_PATH_CNT = 3;
+const int TRACK_POINTS_MAX_CNT = 50 * 2;
+
+const int SET_SPEED_NA = 255;
+
+const uint8_t bg_colors[][4] = {
+  [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff},
+  [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xff},
+  [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xff},
+  [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xff},
+  [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xff},
+};
+
+
+typedef struct UIScene {
+  int frontview;
+  int fullview;
+
+  int transformed_width, transformed_height;
+
+  ModelData model;
+
+  float mpc_x[50];
+  float mpc_y[50];
+
+  bool world_objects_visible;
+  mat4 extrinsic_matrix;      // Last row is 0 so we can use mat4.
+
+  float v_cruise;
+  uint64_t v_cruise_update_ts;
+  float v_ego;
+  bool decel_for_model;
+
+  float speedlimit;
+  bool speedlimit_valid;
+  bool map_valid;
+
+  float curvature;
+  int engaged;
+  bool engageable;
+  bool monitoring_active;
+
+  bool uilayout_sidebarcollapsed;
+  bool uilayout_mapenabled;
+  // responsive layout
+  int ui_viz_rx;
+  int ui_viz_rw;
+  int ui_viz_ro;
+
+  int lead_status;
+  float lead_d_rel, lead_y_rel, lead_v_rel;
+
+  int front_box_x, front_box_y, front_box_width, front_box_height;
+
+  uint64_t alert_ts;
+  char alert_text1[1024];
+  char alert_text2[1024];
+  uint8_t alert_size;
+  float alert_blinkingrate;
+
+  float awareness_status;
+
+  // Used to show gps planner status
+  bool gps_planner_active;
+} UIScene;
+
+typedef struct {
+  float x, y;
+}vertex_data;
+
+typedef struct {
+  vertex_data v[MODEL_PATH_MAX_VERTICES_CNT];
+  int cnt;
+} model_path_vertices_data;
+
+typedef struct {
+  vertex_data v[TRACK_POINTS_MAX_CNT];
+  int cnt;
+} track_vertices_data;
+
+
+typedef struct UIState {
+  pthread_mutex_t lock;
+  pthread_cond_t bg_cond;
+
+  // framebuffer
+  FramebufferState *fb;
+  int fb_w, fb_h;
+  EGLDisplay display;
+  EGLSurface surface;
+
+  // NVG
+  NVGcontext *vg;
+
+  // fonts and images
+  int font_courbd;
+  int font_sans_regular;
+  int font_sans_semibold;
+  int font_sans_bold;
+  int img_wheel;
+  int img_turn;
+  int img_face;
+  int img_map;
+
+  // sockets
+  Context *ctx;
+  SubSocket *model_sock;
+  SubSocket *controlsstate_sock;
+  SubSocket *livecalibration_sock;
+  SubSocket *radarstate_sock;
+  SubSocket *map_data_sock;
+  SubSocket *uilayout_sock;
+  Poller * poller;
+
+  int active_app;
+
+  // vision state
+  bool vision_connected;
+  bool vision_connect_firstrun;
+  int ipc_fd;
+
+  VIPCBuf bufs[UI_BUF_COUNT];
+  VIPCBuf front_bufs[UI_BUF_COUNT];
+  int cur_vision_idx;
+  int cur_vision_front_idx;
+
+  GLuint frame_program;
+  GLuint frame_texs[UI_BUF_COUNT];
+  EGLImageKHR khr[UI_BUF_COUNT];
+  void *priv_hnds[UI_BUF_COUNT];
+  GLuint frame_front_texs[UI_BUF_COUNT];
+  EGLImageKHR khr_front[UI_BUF_COUNT];
+  void *priv_hnds_front[UI_BUF_COUNT];
+
+  GLint frame_pos_loc, frame_texcoord_loc;
+  GLint frame_texture_loc, frame_transform_loc;
+
+  GLuint line_program;
+  GLint line_pos_loc, line_color_loc;
+  GLint line_transform_loc;
+
+  int rgb_width, rgb_height, rgb_stride;
+  size_t rgb_buf_len;
+  mat4 rgb_transform;
+
+  int rgb_front_width, rgb_front_height, rgb_front_stride;
+  size_t rgb_front_buf_len;
+
+  UIScene scene;
+  bool awake;
+
+  // timeouts
+  int awake_timeout;
+  int volume_timeout;
+  int controls_timeout;
+  int alert_sound_timeout;
+  int speed_lim_off_timeout;
+  int is_metric_timeout;
+  int longitudinal_control_timeout;
+  int limit_set_speed_timeout;
+
+  bool controls_seen;
+
+  int status;
+  bool is_metric;
+  bool longitudinal_control;
+  bool limit_set_speed;
+  float speed_lim_off;
+  bool is_ego_over_limit;
+  char alert_type[64];
+  AudibleAlert alert_sound;
+  int alert_size;
+  float alert_blinking_alpha;
+  bool alert_blinked;
+
+  float light_sensor;
+
+  int touch_fd;
+
+  // Hints for re-calculations and redrawing
+  bool model_changed;
+  bool livempc_or_radarstate_changed;
+
+  GLuint frame_vao[2], frame_vbo[2], frame_ibo[2];
+  mat4 rear_frame_mat, front_frame_mat;
+
+  model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2];
+
+  track_vertices_data track_vertices[2];
+} UIState;
+
+// API
+void ui_draw_vision_alert(UIState *s, int va_size, int va_color,
+                          const char* va_text1, const char* va_text2); 
+void ui_draw(UIState *s);
+void ui_nvg_init(UIState *s);
+
+#endif