sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 64 Packages Projects Releases Wiki Activity

selfdrive/common

Browse Source
pull/214/head
George Hotz 6 years ago
parent 71ead9adea
commit 368a956b96
41 changed files with 3876 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 36
      selfdrive/common/SConscript
  2. 438
      selfdrive/common/buffering.c
  3. 123
      selfdrive/common/buffering.h
  4. 418
      selfdrive/common/clutil.c
  5. 87
      selfdrive/common/clutil.h
  6. 52
      selfdrive/common/cqueue.c
  7. 33
      selfdrive/common/cqueue.h
  8. 56
      selfdrive/common/efd.c
  9. 17
      selfdrive/common/efd.h
  10. 142
      selfdrive/common/framebuffer.cc
  11. 50
      selfdrive/common/framebuffer.h
  12. 71
      selfdrive/common/glutil.c
  13. 8
      selfdrive/common/glutil.h
  14. 119
      selfdrive/common/ipc.c
  15. 19
      selfdrive/common/ipc.h
  16. 88
      selfdrive/common/mat.h
  17. 15
      selfdrive/common/messaging.h
  18. 40
      selfdrive/common/modeldata.h
  19. 13
      selfdrive/common/mutex.h
  20. 307
      selfdrive/common/params.cc
  21. 47
      selfdrive/common/params.h
  22. 182
      selfdrive/common/spinner.c
  23. 14
      selfdrive/common/spinner.h
  24. 119
      selfdrive/common/swaglog.c
  25. 68
      selfdrive/common/swaglog.h
  26. 64
      selfdrive/common/test_params.c
  27. 54
      selfdrive/common/timing.h
  28. 120
      selfdrive/common/touch.c
  29. 21
      selfdrive/common/touch.h
  30. 61
      selfdrive/common/util.c
  31. 48
      selfdrive/common/util.h
  32. 66
      selfdrive/common/utilpp.h
  33. 1
      selfdrive/common/version.h
  34. 39
      selfdrive/common/visionbuf.h
  35. 110
      selfdrive/common/visionbuf_cl.c
  36. 141
      selfdrive/common/visionbuf_ion.c
  37. 122
      selfdrive/common/visionimg.cc
  38. 36
      selfdrive/common/visionimg.h
  39. 194
      selfdrive/common/visionipc.c
  40. 113
      selfdrive/common/visionipc.h
  41. 124
      selfdrive/common/visionstream.c

36
selfdrive/common/SConscript
Unescape View File

@ -0,0 +1,36 @@
Import('env', 'arch', 'SHARED')
if SHARED:
fxn = env.SharedLibrary
else:
fxn = env.Library
_common = fxn('common', ['params.cc', 'swaglog.c', 'util.c', 'cqueue.c'], LIBS="json")
_visionipc = fxn('visionipc', ['visionipc.c', 'ipc.c'])
files = [
'buffering.c',
'clutil.c',
'efd.c',
'glutil.c',
'visionimg.cc',
]
if arch == "aarch64":
defines = {}
files += [
'framebuffer.cc',
'touch.c',
'visionbuf_ion.c',
]
_gpu_libs = ['gui', 'adreno_utils']
else:
defines = {"CLU_NO_CACHE": None}
files += [
'visionbuf_cl.c',
]
_gpu_libs = ["GL"]
_gpucommon = fxn('gpucommon', files, CPPDEFINES=defines, LIBS=_gpu_libs)
Export('_common', '_visionipc', '_gpucommon', '_gpu_libs')

438
selfdrive/common/buffering.c
Unescape View File

@ -0,0 +1,438 @@
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <assert.h>
#include <string.h>
#include <unistd.h>
#include "common/efd.h"
#include "buffering.h"
void tbuffer_init(TBuffer *tb, int num_bufs, const char* name) {
assert(num_bufs >= 3);
memset(tb, 0, sizeof(TBuffer));
tb->reading = (bool*)calloc(num_bufs, sizeof(bool));
assert(tb->reading);
tb->pending_idx = -1;
tb->num_bufs = num_bufs;
tb->name = name;
pthread_mutex_init(&tb->lock, NULL);
pthread_cond_init(&tb->cv, NULL);
tb->efd = efd_init();
assert(tb->efd >= 0);
}
void tbuffer_init2(TBuffer *tb, int num_bufs, const char* name,
void (*release_cb)(void* c, int idx),
void* cb_cookie) {
tbuffer_init(tb, num_bufs, name);
tb->release_cb = release_cb;
tb->cb_cookie = cb_cookie;
}
int tbuffer_efd(TBuffer *tb) {
return tb->efd;
}
int tbuffer_select(TBuffer *tb) {
pthread_mutex_lock(&tb->lock);
int i;
for (i=0; i<tb->num_bufs; i++) {
if (!tb->reading[i] && i != tb->pending_idx) {
break;
}
}
assert(i < tb->num_bufs);
pthread_mutex_unlock(&tb->lock);
return i;
}
void tbuffer_dispatch(TBuffer *tb, int idx) {
pthread_mutex_lock(&tb->lock);
if (tb->pending_idx != -1) {
//printf("tbuffer (%s) dropped!\n", tb->name ? tb->name : "?");
if (tb->release_cb) {
tb->release_cb(tb->cb_cookie, tb->pending_idx);
}
tb->pending_idx = -1;
}
tb->pending_idx = idx;
efd_write(tb->efd);
pthread_cond_signal(&tb->cv);
pthread_mutex_unlock(&tb->lock);
}
int tbuffer_acquire(TBuffer *tb) {
pthread_mutex_lock(&tb->lock);
if (tb->stopped) {
pthread_mutex_unlock(&tb->lock);
return -1;
}
while (tb->pending_idx == -1) {
pthread_cond_wait(&tb->cv, &tb->lock);
if (tb->stopped) {
pthread_mutex_unlock(&tb->lock);
return -1;
}
}
efd_clear(tb->efd);
int ret = tb->pending_idx;
assert(ret < tb->num_bufs);
tb->reading[ret] = true;
tb->pending_idx = -1;
pthread_mutex_unlock(&tb->lock);
return ret;
}
static void tbuffer_release_locked(TBuffer *tb, int idx) {
assert(idx < tb->num_bufs);
if (!tb->reading[idx]) {
printf("!! releasing tbuffer we aren't reading %d\n", idx);
}
if (tb->release_cb) {
tb->release_cb(tb->cb_cookie, idx);
}
tb->reading[idx] = false;
}
void tbuffer_release(TBuffer *tb, int idx) {
pthread_mutex_lock(&tb->lock);
tbuffer_release_locked(tb, idx);
pthread_mutex_unlock(&tb->lock);
}
void tbuffer_release_all(TBuffer *tb) {
pthread_mutex_lock(&tb->lock);
for (int i=0; i<tb->num_bufs; i++) {
if (tb->reading[i]) {
tbuffer_release_locked(tb, i);
}
}
pthread_mutex_unlock(&tb->lock);
}
void tbuffer_stop(TBuffer *tb) {
pthread_mutex_lock(&tb->lock);
tb->stopped = true;
efd_write(tb->efd);
pthread_cond_signal(&tb->cv);
pthread_mutex_unlock(&tb->lock);
}
void pool_init(Pool *s, int num_bufs) {
assert(num_bufs > 3);
memset(s, 0, sizeof(*s));
s->num_bufs = num_bufs;
s->refcnt = (int*)calloc(num_bufs, sizeof(int));
s->ts = (int*)calloc(num_bufs, sizeof(int));
s->counter = 1;
pthread_mutex_init(&s->lock, NULL);
}
void pool_init2(Pool *s, int num_bufs,
void (*release_cb)(void* c, int idx), void* cb_cookie) {
pool_init(s, num_bufs);
s->cb_cookie = cb_cookie;
s->release_cb = release_cb;
}
void pool_acquire(Pool *s, int idx) {
pthread_mutex_lock(&s->lock);
assert(idx >= 0 && idx < s->num_bufs);
s->refcnt[idx]++;
pthread_mutex_unlock(&s->lock);
}
static void pool_release_locked(Pool *s, int idx) {
// printf("release %d refcnt %d\n", idx, s->refcnt[idx]);
assert(idx >= 0 && idx < s->num_bufs);
assert(s->refcnt[idx] > 0);
s->refcnt[idx]--;
// printf("release %d -> %d, %p\n", idx, s->refcnt[idx], s->release_cb);
if (s->refcnt[idx] == 0 && s->release_cb) {
// printf("call %p\b", s->release_cb);
s->release_cb(s->cb_cookie, idx);
}
}
void pool_release(Pool *s, int idx) {
pthread_mutex_lock(&s->lock);
pool_release_locked(s, idx);
pthread_mutex_unlock(&s->lock);
}
TBuffer* pool_get_tbuffer(Pool *s) {
pthread_mutex_lock(&s->lock);
assert(s->num_tbufs < POOL_MAX_TBUFS);
TBuffer* tbuf = &s->tbufs[s->num_tbufs];
s->num_tbufs++;
tbuffer_init2(tbuf, s->num_bufs,
"pool", (void (*)(void *, int))pool_release, s);
bool stopped = s->stopped;
pthread_mutex_unlock(&s->lock);
// Stop the tbuffer so we can return a valid object.
// We must stop here because the pool_stop may have already been called,
// in which case tbuffer_stop may never be called again.
if (stopped) {
tbuffer_stop(tbuf);
}
return tbuf;
}
PoolQueue* pool_get_queue(Pool *s) {
pthread_mutex_lock(&s->lock);
int i;
for (i = 0; i < POOL_MAX_QUEUES; i++) {
if (!s->queues[i].inited) {
break;
}
}
assert(i < POOL_MAX_QUEUES);
PoolQueue *c = &s->queues[i];
memset(c, 0, sizeof(*c));
c->pool = s;
c->inited = true;
c->efd = efd_init();
assert(c->efd >= 0);
c->num_bufs = s->num_bufs;
c->num = c->num_bufs+1;
c->idx = (int*)malloc(sizeof(int)*c->num);
memset(c->idx, -1, sizeof(int)*c->num);
pthread_mutex_init(&c->lock, NULL);
pthread_cond_init(&c->cv, NULL);
pthread_mutex_unlock(&s->lock);
return c;
}
void pool_release_queue(PoolQueue *c) {
Pool *s = c->pool;
pthread_mutex_lock(&s->lock);
pthread_mutex_lock(&c->lock);
for (int i=0; i<c->num; i++) {
if (c->idx[i] != -1) {
pool_release_locked(s, c->idx[i]);
}
}
close(c->efd);
free(c->idx);
c->inited = false;
pthread_mutex_unlock(&c->lock);
pthread_mutex_destroy(&c->lock);
pthread_cond_destroy(&c->cv);
pthread_mutex_unlock(&s->lock);
}
int pool_select(Pool *s) {
pthread_mutex_lock(&s->lock);
int i;
for (i=0; i<s->num_bufs; i++) {
if (s->refcnt[i] == 0) {
break;
}
}
if (i >= s->num_bufs) {
// overwrite the oldest
// still being using in a queue or tbuffer :/
int min_k = 0;
int min_ts = s->ts[0];
for (int k=1; k<s->num_bufs; k++) {
if (s->ts[k] < min_ts) {
min_ts = s->ts[k];
min_k = k;
}
}
i = min_k;
printf("pool is full! evicted %d\n", min_k);
// might be really bad if the user is doing pointery stuff
if (s->release_cb) {
s->release_cb(s->cb_cookie, min_k);
}
}
s->refcnt[i]++;
s->ts[i] = s->counter;
s->counter++;
pthread_mutex_unlock(&s->lock);
return i;
}
void pool_push(Pool *s, int idx) {
pthread_mutex_lock(&s->lock);
// printf("push %d head %d tail %d\n", idx, s->head, s->tail);
assert(idx >= 0 && idx < s->num_bufs);
s->ts[idx] = s->counter;
s->counter++;
assert(s->refcnt[idx] > 0);
s->refcnt[idx]--; //push is a implcit release
int num_tbufs = s->num_tbufs;
s->refcnt[idx] += num_tbufs;
// dispatch pool queues
for (int i=0; i<POOL_MAX_QUEUES; i++) {
PoolQueue *c = &s->queues[i];
if (!c->inited) continue;
pthread_mutex_lock(&c->lock);
if (((c->head+1) % c->num) == c->tail) {
// queue is full. skip for now
pthread_mutex_unlock(&c->lock);
continue;
}
s->refcnt[idx]++;
c->idx[c->head] = idx;
c->head = (c->head+1) % c->num;
assert(c->head != c->tail);
pthread_mutex_unlock(&c->lock);
efd_write(c->efd);
pthread_cond_signal(&c->cv);
}
pthread_mutex_unlock(&s->lock);
for (int i=0; i<num_tbufs; i++) {
tbuffer_dispatch(&s->tbufs[i], idx);
}
}
int poolq_pop(PoolQueue *c) {
pthread_mutex_lock(&c->lock);
if (c->stopped) {
pthread_mutex_unlock(&c->lock);
return -1;
}
while (c->head == c->tail) {
pthread_cond_wait(&c->cv, &c->lock);
if (c->stopped) {
pthread_mutex_unlock(&c->lock);
return -1;
}
}
// printf("pop head %d tail %d\n", s->head, s->tail);
assert(c->head != c->tail);
int r = c->idx[c->tail];
c->idx[c->tail] = -1;
c->tail = (c->tail+1) % c->num;
// queue event is level triggered
if (c->head == c->tail) {
efd_clear(c->efd);
}
// printf("pop %d head %d tail %d\n", r, s->head, s->tail);
assert(r >= 0 && r < c->num_bufs);
pthread_mutex_unlock(&c->lock);
return r;
}
int poolq_efd(PoolQueue *c) {
return c->efd;
}
void poolq_release(PoolQueue *c, int idx) {
pool_release(c->pool, idx);
}
void pool_stop(Pool *s) {
for (int i=0; i<s->num_tbufs; i++) {
tbuffer_stop(&s->tbufs[i]);
}
pthread_mutex_lock(&s->lock);
s->stopped = true;
for (int i=0; i<POOL_MAX_QUEUES; i++) {
PoolQueue *c = &s->queues[i];
if (!c->inited) continue;
pthread_mutex_lock(&c->lock);
c->stopped = true;
pthread_mutex_unlock(&c->lock);
efd_write(c->efd);
pthread_cond_signal(&c->cv);
}
pthread_mutex_unlock(&s->lock);
}

123
selfdrive/common/buffering.h
Unescape View File

@ -0,0 +1,123 @@
#ifndef BUFFERING_H
#define BUFFERING_H
#include <stdbool.h>
#include <pthread.h>
#ifdef __cplusplus
extern "C" {
#endif
// Tripple buffering helper
typedef struct TBuffer {
pthread_mutex_t lock;
pthread_cond_t cv;
int efd;
bool* reading;
int pending_idx;
int num_bufs;
const char* name;
void (*release_cb)(void* c, int idx);
void *cb_cookie;
bool stopped;
} TBuffer;
// num_bufs must be at least the number of buffers that can be acquired simultaniously plus two
void tbuffer_init(TBuffer *tb, int num_bufs, const char* name);
void tbuffer_init2(TBuffer *tb, int num_bufs, const char* name,
void (*release_cb)(void* c, int idx),
void* cb_cookie);
// returns an eventfd that signals if a buffer is ready and tbuffer_acquire shouldn't to block.
// useful to polling on multiple tbuffers.
int tbuffer_efd(TBuffer *tb);
// Chooses a buffer that's not reading or pending
int tbuffer_select(TBuffer *tb);
// Called when the writer is done with their buffer
// - Wakes up the reader if it's waiting
// - releases the pending buffer if the reader's too slow
void tbuffer_dispatch(TBuffer *tb, int idx);
// Called when the reader wants a new buffer, will return -1 when stopped
int tbuffer_acquire(TBuffer *tb);
// Called when the reader is done with their buffer
void tbuffer_release(TBuffer *tb, int idx);
void tbuffer_release_all(TBuffer *tb);
void tbuffer_stop(TBuffer *tb);
// pool: buffer pool + queue thing...
#define POOL_MAX_TBUFS 8
#define POOL_MAX_QUEUES 8
typedef struct Pool Pool;
typedef struct PoolQueue {
pthread_mutex_t lock;
pthread_cond_t cv;
Pool* pool;
bool inited;
bool stopped;
int efd;
int num_bufs;
int num;
int head, tail;
int* idx;
} PoolQueue;
int poolq_pop(PoolQueue *s);
int poolq_efd(PoolQueue *s);
void poolq_release(PoolQueue *c, int idx);
typedef struct Pool {
pthread_mutex_t lock;
bool stopped;
int num_bufs;
int counter;
int* ts;
int* refcnt;
void (*release_cb)(void* c, int idx);
void *cb_cookie;
int num_tbufs;
TBuffer tbufs[POOL_MAX_TBUFS];
PoolQueue queues[POOL_MAX_QUEUES];
} Pool;
void pool_init(Pool *s, int num_bufs);
void pool_init2(Pool *s, int num_bufs,
void (*release_cb)(void* c, int idx), void* cb_cookie);
TBuffer* pool_get_tbuffer(Pool *s);
PoolQueue* pool_get_queue(Pool *s);
void pool_release_queue(PoolQueue *q);
int pool_select(Pool *s);
void pool_push(Pool *s, int idx);
void pool_acquire(Pool *s, int idx);
void pool_release(Pool *s, int idx);
void pool_stop(Pool *s);
#ifdef __cplusplus
} // extern "C"
#endif
#endif

418
selfdrive/common/clutil.c
Unescape View File

@ -0,0 +1,418 @@
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <assert.h>
#include <string.h>
#include <unistd.h>
#include <inttypes.h>
#include <sys/stat.h>
#ifdef __APPLE__
#include <OpenCL/cl.h>
#else
#include <CL/cl.h>
#endif
#include "common/util.h"
#include "clutil.h"
typedef struct CLUProgramIndex {
uint64_t index_hash;
const uint8_t* bin_data;
const uint8_t* bin_end;
} CLUProgramIndex;
#ifdef CLU_NO_SRC
#include "clcache_bins.h"
#else
static const CLUProgramIndex clu_index[] = {};
#endif
void clu_init(void) {
#ifndef CLU_NO_SRC
mkdir("/tmp/clcache", 0777);
unlink("/tmp/clcache/index.cli");
#endif
}
cl_program cl_create_program_from_file(cl_context ctx, const char* path) {
char* src_buf = read_file(path, NULL);
assert(src_buf);
int err = 0;
cl_program ret = clCreateProgramWithSource(ctx, 1, (const char**)&src_buf, NULL, &err);
assert(err == 0);
free(src_buf);
return ret;
}
static char* get_version_string(cl_platform_id platform) {
size_t size = 0;
int err;
err = clGetPlatformInfo(platform, CL_PLATFORM_VERSION, 0, NULL, &size);
assert(err == 0);
char *str = malloc(size);
assert(str);
err = clGetPlatformInfo(platform, CL_PLATFORM_VERSION, size, str, NULL);
assert(err == 0);
return str;
}
void cl_print_info(cl_platform_id platform, cl_device_id device) {
char str[4096];
clGetPlatformInfo(platform, CL_PLATFORM_VENDOR, sizeof(str), str, NULL);
printf("vendor: '%s'\n", str);
char* version = get_version_string(platform);
printf("platform version: '%s'\n", version);
free(version);
clGetPlatformInfo(platform, CL_PLATFORM_PROFILE, sizeof(str), str, NULL);
printf("profile: '%s'\n", str);
clGetPlatformInfo(platform, CL_PLATFORM_EXTENSIONS, sizeof(str), str, NULL);
printf("extensions: '%s'\n", str);
clGetDeviceInfo(device, CL_DEVICE_NAME, sizeof(str), str, NULL);
printf("name: '%s'\n", str);
clGetDeviceInfo(device, CL_DEVICE_VERSION, sizeof(str), str, NULL);
printf("device version: '%s'\n", str);
size_t sz;
clGetDeviceInfo(device, CL_DEVICE_MAX_WORK_GROUP_SIZE, sizeof(sz), &sz, NULL);
printf("max work group size: %u\n", sz);
cl_device_type type;
clGetDeviceInfo(device, CL_DEVICE_TYPE, sizeof(type), &type, NULL);
printf("type = 0x%04x = ", (unsigned int)type);
switch(type) {
case CL_DEVICE_TYPE_CPU:
printf("CL_DEVICE_TYPE_CPU\n");
break;
case CL_DEVICE_TYPE_GPU:
printf("CL_DEVICE_TYPE_GPU\n");
break;
case CL_DEVICE_TYPE_ACCELERATOR:
printf("CL_DEVICE_TYPE_ACCELERATOR\n");
break;
default:
printf("Other...\n" );
break;
}
}
void cl_print_build_errors(cl_program program, cl_device_id device) {
cl_build_status status;
clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_STATUS,
sizeof(cl_build_status), &status, NULL);
size_t log_size;
clGetProgramBuildInfo(program, device,
CL_PROGRAM_BUILD_LOG, 0, NULL, &log_size);
char* log = calloc(log_size+1, 1);
assert(log);
clGetProgramBuildInfo(program, device,
CL_PROGRAM_BUILD_LOG, log_size+1, log, NULL);
printf("build failed; status=%d, log:\n%s\n",
status, log);
free(log);
}
uint64_t clu_index_hash(const char* s) {
size_t sl = strlen(s);
assert(sl < 128);
uint64_t x = 0;
for (int i=127; i>=0; i--) {
x *= 65599ULL;
x += (uint8_t)s[i<sl ? sl-1-i : sl];
}
return x ^ (x >> 32);
}
uint64_t clu_fnv_hash(const uint8_t *data, size_t len) {
/* 64 bit Fowler/Noll/Vo FNV-1a hash code */
uint64_t hval = 0xcbf29ce484222325ULL;
const uint8_t *dp = data;
const uint8_t *de = data + len;
while (dp < de) {
hval ^= (uint64_t) *dp++;
hval += (hval << 1) + (hval << 4) + (hval << 5) +
(hval << 7) + (hval << 8) + (hval << 40);
}
return hval;
}
cl_program cl_cached_program_from_hash(cl_context ctx, cl_device_id device_id, uint64_t hash) {
int err;
char cache_path[1024];
snprintf(cache_path, sizeof(cache_path), "/tmp/clcache/%016" PRIx64 ".clb", hash);
size_t bin_size;
uint8_t *bin = read_file(cache_path, &bin_size);
if (!bin) {
return NULL;
}
cl_program prg = clCreateProgramWithBinary(ctx, 1, &device_id, &bin_size, (const uint8_t**)&bin, NULL, &err);
assert(err == 0);
free(bin);
err = clBuildProgram(prg, 1, &device_id, NULL, NULL, NULL);
assert(err == 0);
return prg;
}
static uint8_t* get_program_binary(cl_program prg, size_t *out_size) {
int err;
cl_uint num_devices;
err = clGetProgramInfo(prg, CL_PROGRAM_NUM_DEVICES, sizeof(num_devices), &num_devices, NULL);
assert(err == 0);
assert(num_devices == 1);
size_t binary_size = 0;
err = clGetProgramInfo(prg, CL_PROGRAM_BINARY_SIZES, sizeof(binary_size), &binary_size, NULL);
assert(err == 0);
assert(binary_size > 0);
uint8_t *binary_buf = malloc(binary_size);
assert(binary_buf);
uint8_t* bufs[1] = { binary_buf, };
err = clGetProgramInfo(prg, CL_PROGRAM_BINARIES, sizeof(bufs), &bufs, NULL);
assert(err == 0);
*out_size = binary_size;
return binary_buf;
}
cl_program cl_cached_program_from_string(cl_context ctx, cl_device_id device_id,
const char* src, const char* args,
uint64_t *out_hash) {
int err;
cl_platform_id platform;
err = clGetDeviceInfo(device_id, CL_DEVICE_PLATFORM, sizeof(platform), &platform, NULL);
assert(err == 0);
const char* platform_version = get_version_string(platform);
const size_t hash_len = strlen(platform_version)+1+strlen(src)+1+strlen(args)+1;
char* hash_buf = malloc(hash_len);
assert(hash_buf);
memset(hash_buf, 0, hash_len);
snprintf(hash_buf, hash_len, "%s%c%s%c%s", platform_version, 1, src, 1, args);
free((void*)platform_version);
uint64_t hash = clu_fnv_hash((uint8_t*)hash_buf, hash_len);
free(hash_buf);
cl_program prg = NULL;
#ifndef CLU_NO_CACHE
prg = cl_cached_program_from_hash(ctx, device_id, hash);
#endif
if (prg == NULL) {
prg = clCreateProgramWithSource(ctx, 1, (const char**)&src, NULL, &err);
assert(err == 0);
err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL);
if (err != 0) {
cl_print_build_errors(prg, device_id);
}
assert(err == 0);
#ifndef CLU_NO_CACHE
// write program binary to cache
size_t binary_size;
uint8_t *binary_buf = get_program_binary(prg, &binary_size);
char cache_path[1024];
snprintf(cache_path, sizeof(cache_path), "/tmp/clcache/%016" PRIx64 ".clb", hash);
FILE* of = fopen(cache_path, "wb");
assert(of);
fwrite(binary_buf, 1, binary_size, of);
fclose(of);
free(binary_buf);
#endif
}
if (out_hash) *out_hash = hash;
return prg;
}
cl_program cl_cached_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args,
uint64_t *out_hash) {
char* src_buf = read_file(path, NULL);
assert(src_buf);
cl_program ret = cl_cached_program_from_string(ctx, device_id, src_buf, args, out_hash);
free(src_buf);
return ret;
}
static void add_index(uint64_t index_hash, uint64_t src_hash) {
FILE *f = fopen("/tmp/clcache/index.cli", "a");
assert(f);
fprintf(f, "%016" PRIx64 " %016" PRIx64 "\n", index_hash, src_hash);
fclose(f);
}
cl_program cl_program_from_index(cl_context ctx, cl_device_id device_id, uint64_t index_hash) {
int err;
int i;
for (i=0; i<ARRAYSIZE(clu_index); i++) {
if (clu_index[i].index_hash == index_hash) {
break;
}
}
if (i >= ARRAYSIZE(clu_index)) {
assert(false);
}
size_t bin_size = clu_index[i].bin_end - clu_index[i].bin_data;
const uint8_t *bin_data = clu_index[i].bin_data;
cl_program prg = clCreateProgramWithBinary(ctx, 1, &device_id, &bin_size, (const uint8_t**)&bin_data, NULL, &err);
assert(err == 0);
err = clBuildProgram(prg, 1, &device_id, NULL, NULL, NULL);
assert(err == 0);
return prg;
}
cl_program cl_index_program_from_string(cl_context ctx, cl_device_id device_id,
const char* src, const char* args,
uint64_t index_hash) {
uint64_t src_hash = 0;
cl_program ret = cl_cached_program_from_string(ctx, device_id, src, args, &src_hash);
#ifndef CLU_NO_CACHE
add_index(index_hash, src_hash);
#endif
return ret;
}
cl_program cl_index_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args,
uint64_t index_hash) {
uint64_t src_hash = 0;
cl_program ret = cl_cached_program_from_file(ctx, device_id, path, args, &src_hash);
#ifndef CLU_NO_CACHE
add_index(index_hash, src_hash);
#endif
return ret;
}
/*
* Given a cl code and return a string represenation
*/
const char* cl_get_error_string(int err) {
switch (err) {
case 0: return "CL_SUCCESS";
case -1: return "CL_DEVICE_NOT_FOUND";
case -2: return "CL_DEVICE_NOT_AVAILABLE";
case -3: return "CL_COMPILER_NOT_AVAILABLE";
case -4: return "CL_MEM_OBJECT_ALLOCATION_FAILURE";
case -5: return "CL_OUT_OF_RESOURCES";
case -6: return "CL_OUT_OF_HOST_MEMORY";
case -7: return "CL_PROFILING_INFO_NOT_AVAILABLE";
case -8: return "CL_MEM_COPY_OVERLAP";
case -9: return "CL_IMAGE_FORMAT_MISMATCH";
case -10: return "CL_IMAGE_FORMAT_NOT_SUPPORTED";
case -12: return "CL_MAP_FAILURE";
case -13: return "CL_MISALIGNED_SUB_BUFFER_OFFSET";
case -14: return "CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST";
case -15: return "CL_COMPILE_PROGRAM_FAILURE";
case -16: return "CL_LINKER_NOT_AVAILABLE";
case -17: return "CL_LINK_PROGRAM_FAILURE";
case -18: return "CL_DEVICE_PARTITION_FAILED";
case -19: return "CL_KERNEL_ARG_INFO_NOT_AVAILABLE";
case -30: return "CL_INVALID_VALUE";
case -31: return "CL_INVALID_DEVICE_TYPE";
case -32: return "CL_INVALID_PLATFORM";
case -33: return "CL_INVALID_DEVICE";
case -34: return "CL_INVALID_CONTEXT";
case -35: return "CL_INVALID_QUEUE_PROPERTIES";
case -36: return "CL_INVALID_COMMAND_QUEUE";
case -37: return "CL_INVALID_HOST_PTR";
case -38: return "CL_INVALID_MEM_OBJECT";
case -39: return "CL_INVALID_IMAGE_FORMAT_DESCRIPTOR";
case -40: return "CL_INVALID_IMAGE_SIZE";
case -41: return "CL_INVALID_SAMPLER";
case -42: return "CL_INVALID_BINARY";
case -43: return "CL_INVALID_BUILD_OPTIONS";
case -44: return "CL_INVALID_PROGRAM";
case -45: return "CL_INVALID_PROGRAM_EXECUTABLE";
case -46: return "CL_INVALID_KERNEL_NAME";
case -47: return "CL_INVALID_KERNEL_DEFINITION";
case -48: return "CL_INVALID_KERNEL";
case -49: return "CL_INVALID_ARG_INDEX";
case -50: return "CL_INVALID_ARG_VALUE";
case -51: return "CL_INVALID_ARG_SIZE";
case -52: return "CL_INVALID_KERNEL_ARGS";
case -53: return "CL_INVALID_WORK_DIMENSION";
case -54: return "CL_INVALID_WORK_GROUP_SIZE";
case -55: return "CL_INVALID_WORK_ITEM_SIZE";
case -56: return "CL_INVALID_GLOBAL_OFFSET";
case -57: return "CL_INVALID_EVENT_WAIT_LIST";
case -58: return "CL_INVALID_EVENT";
case -59: return "CL_INVALID_OPERATION";
case -60: return "CL_INVALID_GL_OBJECT";
case -61: return "CL_INVALID_BUFFER_SIZE";
case -62: return "CL_INVALID_MIP_LEVEL";
case -63: return "CL_INVALID_GLOBAL_WORK_SIZE";
case -64: return "CL_INVALID_PROPERTY";
case -65: return "CL_INVALID_IMAGE_DESCRIPTOR";
case -66: return "CL_INVALID_COMPILER_OPTIONS";
case -67: return "CL_INVALID_LINKER_OPTIONS";
case -68: return "CL_INVALID_DEVICE_PARTITION_COUNT";
case -69: return "CL_INVALID_PIPE_SIZE";
case -70: return "CL_INVALID_DEVICE_QUEUE";
case -71: return "CL_INVALID_SPEC_ID";
case -72: return "CL_MAX_SIZE_RESTRICTION_EXCEEDED";
case -1002: return "CL_INVALID_D3D10_DEVICE_KHR";
case -1003: return "CL_INVALID_D3D10_RESOURCE_KHR";
case -1004: return "CL_D3D10_RESOURCE_ALREADY_ACQUIRED_KHR";
case -1005: return "CL_D3D10_RESOURCE_NOT_ACQUIRED_KHR";
case -1006: return "CL_INVALID_D3D11_DEVICE_KHR";
case -1007: return "CL_INVALID_D3D11_RESOURCE_KHR";
case -1008: return "CL_D3D11_RESOURCE_ALREADY_ACQUIRED_KHR";
case -1009: return "CL_D3D11_RESOURCE_NOT_ACQUIRED_KHR";
case -1010: return "CL_INVALID_DX9_MEDIA_ADAPTER_KHR";
case -1011: return "CL_INVALID_DX9_MEDIA_SURFACE_KHR";
case -1012: return "CL_DX9_MEDIA_SURFACE_ALREADY_ACQUIRED_KHR";
case -1013: return "CL_DX9_MEDIA_SURFACE_NOT_ACQUIRED_KHR";
case -1093: return "CL_INVALID_EGL_OBJECT_KHR";
case -1092: return "CL_EGL_RESOURCE_NOT_ACQUIRED_KHR";
case -1001: return "CL_PLATFORM_NOT_FOUND_KHR";
case -1057: return "CL_DEVICE_PARTITION_FAILED_EXT";
case -1058: return "CL_INVALID_PARTITION_COUNT_EXT";
case -1059: return "CL_INVALID_PARTITION_NAME_EXT";
case -1094: return "CL_INVALID_ACCELERATOR_INTEL";
case -1095: return "CL_INVALID_ACCELERATOR_TYPE_INTEL";
case -1096: return "CL_INVALID_ACCELERATOR_DESCRIPTOR_INTEL";
case -1097: return "CL_ACCELERATOR_TYPE_NOT_SUPPORTED_INTEL";
case -1000: return "CL_INVALID_GL_SHAREGROUP_REFERENCE_KHR";
case -1098: return "CL_INVALID_VA_API_MEDIA_ADAPTER_INTEL";
case -1099: return "CL_INVALID_VA_API_MEDIA_SURFACE_INTEL";
case -1100: return "CL_VA_API_MEDIA_SURFACE_ALREADY_ACQUIRED_INTEL";
case -1101: return "CL_VA_API_MEDIA_SURFACE_NOT_ACQUIRED_INTEL";
default: return "CL_UNKNOWN_ERROR";
}
}

87
selfdrive/common/clutil.h
Unescape View File

@ -0,0 +1,87 @@
#ifndef CLUTIL_H
#define CLUTIL_H
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#ifdef __APPLE__
#include <OpenCL/cl.h>
#else
#include <CL/cl.h>
#endif
#ifdef __cplusplus
extern "C" {
#endif
void clu_init(void);
cl_program cl_create_program_from_file(cl_context ctx, const char* path);
void cl_print_info(cl_platform_id platform, cl_device_id device);
void cl_print_build_errors(cl_program program, cl_device_id device);
void cl_print_build_errors(cl_program program, cl_device_id device);
cl_program cl_cached_program_from_hash(cl_context ctx, cl_device_id device_id, uint64_t hash);
cl_program cl_cached_program_from_string(cl_context ctx, cl_device_id device_id,
const char* src, const char* args,
uint64_t *out_hash);
cl_program cl_cached_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args,
uint64_t *out_hash);
cl_program cl_program_from_index(cl_context ctx, cl_device_id device_id, uint64_t index_hash);
cl_program cl_index_program_from_string(cl_context ctx, cl_device_id device_id,
const char* src, const char* args,
uint64_t index_hash);
cl_program cl_index_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args,
uint64_t index_hash);
uint64_t clu_index_hash(const char *s);
uint64_t clu_fnv_hash(const uint8_t *data, size_t len);
const char* cl_get_error_string(int err);
static inline int cl_check_error(int err) {
if (err != 0) {
fprintf(stderr, "%s\n", cl_get_error_string(err));
exit(1);
}
return err;
}
// // string hash macro. compiler, I'm so sorry.
#define CLU_H1(s,i,x) (x*65599ULL+(uint8_t)s[(i)<strlen(s)?strlen(s)-1-(i):strlen(s)])
#define CLU_H4(s,i,x) CLU_H1(s,i,CLU_H1(s,i+1,CLU_H1(s,i+2,CLU_H1(s,i+3,x))))
#define CLU_H16(s,i,x) CLU_H4(s,i,CLU_H4(s,i+4,CLU_H4(s,i+8,CLU_H4(s,i+12,x))))
#define CLU_H64(s,i,x) CLU_H16(s,i,CLU_H16(s,i+16,CLU_H16(s,i+32,CLU_H16(s,i+48,x))))
// #define CLU_H256(s,i,x) CLU_H64(s,i,CLU_H64(s,i+64,CLU_H64(s,i+128,CLU_H64(s,i+192,x))))
#define CLU_H128(s,i,x) CLU_H64(s,i,CLU_H64(s,i+64,x))
#define CLU_HASH(s) ((uint64_t)(CLU_H128(s,0,0)^(CLU_H128(s,0,0)>>32)))
#define CLU_STRINGIFY(x) #x
#define CLU_STRINGIFY2(x) CLU_STRINGIFY(x)
#define CLU_LINESTR CLU_STRINGIFY2(__LINE__)
#ifdef CLU_NO_SRC
#define CLU_LOAD_FROM_STRING(ctx, device_id, src, args) \
cl_program_from_index(ctx, device_id, CLU_HASH("\1" __FILE__ "\1" CLU_LINESTR) ^ clu_fnv_hash((const uint8_t*)__func__, strlen(__func__)) ^ clu_fnv_hash((const uint8_t*)args, strlen(args)))
#define CLU_LOAD_FROM_FILE(ctx, device_id, path, args) \
cl_program_from_index(ctx, device_id, CLU_HASH("\2" path) ^ clu_fnv_hash((const uint8_t*)args, strlen(args)))
#else
#define CLU_LOAD_FROM_STRING(ctx, device_id, src, args) \
cl_index_program_from_string(ctx, device_id, src, args, clu_index_hash("\1" __FILE__ "\1" CLU_LINESTR) ^ clu_fnv_hash((const uint8_t*)__func__, strlen(__func__)) ^ clu_fnv_hash((const uint8_t*)args, strlen(args)))
#define CLU_LOAD_FROM_FILE(ctx, device_id, path, args) \
cl_index_program_from_file(ctx, device_id, path, args, clu_index_hash("\2" path) ^ clu_fnv_hash((const uint8_t*)args, strlen(args)))
#endif
#ifdef __cplusplus
}
#endif
#endif

52
selfdrive/common/cqueue.c
Unescape View File

@ -0,0 +1,52 @@
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include "cqueue.h"
void queue_init(Queue *q) {
memset(q, 0, sizeof(*q));
TAILQ_INIT(&q->q);
pthread_mutex_init(&q->lock, NULL);
pthread_cond_init(&q->cv, NULL);
}
void* queue_pop(Queue *q) {
pthread_mutex_lock(&q->lock);
while (TAILQ_EMPTY(&q->q)) {
pthread_cond_wait(&q->cv, &q->lock);
}
QueueEntry *entry = TAILQ_FIRST(&q->q);
TAILQ_REMOVE(&q->q, entry, entries);
pthread_mutex_unlock(&q->lock);
void* r = entry->data;
free(entry);
return r;
}
void* queue_try_pop(Queue *q) {
pthread_mutex_lock(&q->lock);
void* r = NULL;
if (!TAILQ_EMPTY(&q->q)) {
QueueEntry *entry = TAILQ_FIRST(&q->q);
TAILQ_REMOVE(&q->q, entry, entries);
r = entry->data;
free(entry);
}
pthread_mutex_unlock(&q->lock);
return r;
}
void queue_push(Queue *q, void *data) {
QueueEntry *entry = calloc(1, sizeof(QueueEntry));
assert(entry);
entry->data = data;
pthread_mutex_lock(&q->lock);
TAILQ_INSERT_TAIL(&q->q, entry, entries);
pthread_cond_signal(&q->cv);
pthread_mutex_unlock(&q->lock);
}

33
selfdrive/common/cqueue.h
Unescape View File

@ -0,0 +1,33 @@
#ifndef COMMON_CQUEUE_H
#define COMMON_CQUEUE_H
#include <sys/queue.h>
#include <pthread.h>
#ifdef __cplusplus
extern "C" {
#endif
// a blocking queue
typedef struct QueueEntry {
TAILQ_ENTRY(QueueEntry) entries;
void *data;
} QueueEntry;
typedef struct Queue {
TAILQ_HEAD(queue, QueueEntry) q;
pthread_mutex_t lock;
pthread_cond_t cv;
} Queue;
void queue_init(Queue *q);
void* queue_pop(Queue *q);
void* queue_try_pop(Queue *q);
void queue_push(Queue *q, void *data);
#ifdef __cplusplus
} // extern "C"
#endif
#endif

56
selfdrive/common/efd.c
Unescape View File

@ -0,0 +1,56 @@
#include <stdlib.h>
#include <assert.h>
#ifdef __linux__
#include <sys/eventfd.h>
#else
#include <sys/time.h>
#include <sys/event.h>
#define EVENT_IDENT 42
#endif
#include "efd.h"
int efd_init() {
#ifdef __linux__
return eventfd(0, EFD_CLOEXEC);
#else
int fd = kqueue();
assert(fd >= 0);
struct kevent kev;
EV_SET(&kev, EVENT_IDENT, EVFILT_USER, EV_ADD | EV_CLEAR, 0, 0, NULL);
struct timespec timeout = {0, 0};
int err = kevent(fd, &kev, 1, NULL, 0, &timeout);
assert(err != -1);
return fd;
#endif
}
void efd_write(int fd) {
#ifdef __linux__
eventfd_write(fd, 1);
#else
struct kevent kev;
EV_SET(&kev, EVENT_IDENT, EVFILT_USER, 0, NOTE_TRIGGER, 0, NULL);
struct timespec timeout = {0, 0};
int err = kevent(fd, &kev, 1, NULL, 0, &timeout);
assert(err != -1);
#endif
}
void efd_clear(int fd) {
#ifdef __linux__
eventfd_t efd_cnt;
eventfd_read(fd, &efd_cnt);
#else
struct kevent kev;
struct timespec timeout = {0, 0};
int nfds = kevent(fd, NULL, 0, &kev, 1, &timeout);
assert(nfds != -1);
#endif
}

17
selfdrive/common/efd.h
Unescape View File

@ -0,0 +1,17 @@
#ifndef EFD_H
#define EFD_H
#ifdef __cplusplus
extern "C" {
#endif
// event fd: a semaphore that can be poll()'d
int efd_init();
void efd_write(int fd);
void efd_clear(int fd);
#ifdef __cplusplus
}
#endif
#endif

142
selfdrive/common/framebuffer.cc
Unescape View File

@ -0,0 +1,142 @@
#include <cstdio>
#include <cstdlib>
#include <cassert>
#include <ui/DisplayInfo.h>
#include <gui/ISurfaceComposer.h>
#include <gui/Surface.h>
#include <gui/SurfaceComposerClient.h>
#include <GLES2/gl2.h>
#include <EGL/eglext.h>
#define BACKLIGHT_CONTROL "/sys/class/leds/lcd-backlight/brightness"
#define BACKLIGHT_LEVEL "205"
using namespace android;
struct FramebufferState {
sp<SurfaceComposerClient> session;
sp<IBinder> dtoken;
DisplayInfo dinfo;
sp<SurfaceControl> control;
sp<Surface> s;
EGLDisplay display;
EGLint egl_major, egl_minor;
EGLConfig config;
EGLSurface surface;
EGLContext context;
};
extern "C" void framebuffer_set_power(FramebufferState *s, int mode) {
SurfaceComposerClient::setDisplayPowerMode(s->dtoken, mode);
}
extern "C" FramebufferState* framebuffer_init(
const char* name, int32_t layer, int alpha,
int *out_w, int *out_h) {
status_t status;
int success;
FramebufferState *s = new FramebufferState;
s->session = new SurfaceComposerClient();
assert(s->session != NULL);
s->dtoken = SurfaceComposerClient::getBuiltInDisplay(
ISurfaceComposer::eDisplayIdMain);
assert(s->dtoken != NULL);
status = SurfaceComposerClient::getDisplayInfo(s->dtoken, &s->dinfo);
assert(status == 0);
//int orientation = 3; // rotate framebuffer 270 degrees
int orientation = 1; // rotate framebuffer 90 degrees
if(orientation == 1 || orientation == 3) {
int temp = s->dinfo.h;
s->dinfo.h = s->dinfo.w;
s->dinfo.w = temp;
}
printf("dinfo %dx%d\n", s->dinfo.w, s->dinfo.h);
Rect destRect(s->dinfo.w, s->dinfo.h);
s->session->setDisplayProjection(s->dtoken, orientation, destRect, destRect);
s->control = s->session->createSurface(String8(name),
s->dinfo.w, s->dinfo.h, PIXEL_FORMAT_RGBX_8888);
assert(s->control != NULL);
SurfaceComposerClient::openGlobalTransaction();
status = s->control->setLayer(layer);
SurfaceComposerClient::closeGlobalTransaction();
assert(status == 0);
s->s = s->control->getSurface();
assert(s->s != NULL);
// init opengl and egl
const EGLint attribs[] = {
EGL_RED_SIZE, 8,
EGL_GREEN_SIZE, 8,
EGL_BLUE_SIZE, 8,
EGL_ALPHA_SIZE, alpha ? 8 : 0,
EGL_DEPTH_SIZE, 0,
EGL_STENCIL_SIZE, 8,
EGL_RENDERABLE_TYPE, EGL_OPENGL_ES3_BIT_KHR,
EGL_NONE,
};
s->display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
assert(s->display != EGL_NO_DISPLAY);
success = eglInitialize(s->display, &s->egl_major, &s->egl_minor);
assert(success);
printf("egl version %d.%d\n", s->egl_major, s->egl_minor);
EGLint num_configs;
success = eglChooseConfig(s->display, attribs, &s->config, 1, &num_configs);
assert(success);
s->surface = eglCreateWindowSurface(s->display, s->config, s->s.get(), NULL);
assert(s->surface != EGL_NO_SURFACE);
const EGLint context_attribs[] = {
EGL_CONTEXT_CLIENT_VERSION, 3,
EGL_NONE,
};
s->context = eglCreateContext(s->display, s->config, NULL, context_attribs);
assert(s->context != EGL_NO_CONTEXT);
EGLint w, h;
eglQuerySurface(s->display, s->surface, EGL_WIDTH, &w);
eglQuerySurface(s->display, s->surface, EGL_HEIGHT, &h);
printf("egl w %d h %d\n", w, h);
success = eglMakeCurrent(s->display, s->surface, s->surface, s->context);
assert(success);
printf("gl version %s\n", glGetString(GL_VERSION));
// set brightness
int brightness_fd = open(BACKLIGHT_CONTROL, O_RDWR);
const char brightness_level[] = BACKLIGHT_LEVEL;
write(brightness_fd, brightness_level, strlen(brightness_level));
if (out_w) *out_w = w;
if (out_h) *out_h = h;
return s;
}
extern "C" void framebuffer_swap(FramebufferState *s) {
eglSwapBuffers(s->display, s->surface);
assert(glGetError() == GL_NO_ERROR);
}

50
selfdrive/common/framebuffer.h
Unescape View File

@ -0,0 +1,50 @@
#ifndef FRAMEBUFFER_H
#define FRAMEBUFFER_H
#include <EGL/eglext.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef struct FramebufferState FramebufferState;
FramebufferState* framebuffer_init(
const char* name, int32_t layer, int alpha,
int *out_w, int *out_h);
void framebuffer_set_power(FramebufferState *s, int mode);
void framebuffer_swap(FramebufferState *s);
/* Display power modes */
enum {
/* The display is turned off (blanked). */
HWC_POWER_MODE_OFF = 0,
/* The display is turned on and configured in a low power state
* that is suitable for presenting ambient information to the user,
* possibly with lower fidelity than normal but greater efficiency. */
HWC_POWER_MODE_DOZE = 1,
/* The display is turned on normally. */
HWC_POWER_MODE_NORMAL = 2,
/* The display is configured as in HWC_POWER_MODE_DOZE but may
* stop applying frame buffer updates from the graphics subsystem.
* This power mode is effectively a hint from the doze dream to
* tell the hardware that it is done drawing to the display for the
* time being and that the display should remain on in a low power
* state and continue showing its current contents indefinitely
* until the mode changes.
*
* This mode may also be used as a signal to enable hardware-based doze
* functionality. In this case, the doze dream is effectively
* indicating that the hardware is free to take over the display
* and manage it autonomously to implement low power always-on display
* functionality. */
HWC_POWER_MODE_DOZE_SUSPEND = 3,
};
#ifdef __cplusplus
}
#endif
#endif

71
selfdrive/common/glutil.c
Unescape View File

@ -0,0 +1,71 @@
#include <stdlib.h>
#include <stdio.h>
#include <GLES3/gl3.h>
#include "glutil.h"
GLuint load_shader(GLenum shaderType, const char *src) {
GLint status = 0, len = 0;
GLuint shader;
if (!(shader = glCreateShader(shaderType)))
return 0;
glShaderSource(shader, 1, &src, NULL);
glCompileShader(shader);
glGetShaderiv(shader, GL_COMPILE_STATUS, &status);
if (status)
return shader;
glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &len);
if (len) {
char *msg = (char*)malloc(len);
if (msg) {
glGetShaderInfoLog(shader, len, NULL, msg);
msg[len-1] = 0;
fprintf(stderr, "error compiling shader:\n%s\n", msg);
free(msg);
}
}
glDeleteShader(shader);
return 0;
}
GLuint load_program(const char *vert_src, const char *frag_src) {
GLuint vert, frag, prog;
GLint status = 0, len = 0;
if (!(vert = load_shader(GL_VERTEX_SHADER, vert_src)))
return 0;
if (!(frag = load_shader(GL_FRAGMENT_SHADER, frag_src)))
goto fail_frag;
if (!(prog = glCreateProgram()))
goto fail_prog;
glAttachShader(prog, vert);
glAttachShader(prog, frag);
glLinkProgram(prog);
glGetProgramiv(prog, GL_LINK_STATUS, &status);
if (status)
return prog;
glGetProgramiv(prog, GL_INFO_LOG_LENGTH, &len);
if (len) {
char *buf = (char*) malloc(len);
if (buf) {
glGetProgramInfoLog(prog, len, NULL, buf);
buf[len-1] = 0;
fprintf(stderr, "error linking program:\n%s\n", buf);
free(buf);
}
}
glDeleteProgram(prog);
fail_prog:
glDeleteShader(frag);
fail_frag:
glDeleteShader(vert);
return 0;
}

8
selfdrive/common/glutil.h
Unescape View File

@ -0,0 +1,8 @@
#ifndef COMMON_GLUTIL_H
#define COMMON_GLUTIL_H
#include <GLES3/gl3.h>
GLuint load_shader(GLenum shaderType, const char *src);
GLuint load_program(const char *vert_src, const char *frag_src);
#endif

119
selfdrive/common/ipc.c
Unescape View File

@ -0,0 +1,119 @@
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <unistd.h>
#include <assert.h>
#include <errno.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <sys/un.h>
#include "ipc.h"
int ipc_connect(const char* socket_path) {
int err;
int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
assert(sock >= 0);
struct sockaddr_un addr = {
.sun_family = AF_UNIX,
};
snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", socket_path);
err = connect(sock, (struct sockaddr*)&addr, sizeof(addr));
if (err != 0) {
close(sock);
return -1;
}
return sock;
}
int ipc_bind(const char* socket_path) {
int err;
unlink(socket_path);
int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
struct sockaddr_un addr = {
.sun_family = AF_UNIX,
};
snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", socket_path);
err = bind(sock, (struct sockaddr *)&addr, sizeof(addr));
assert(err == 0);
err = listen(sock, 3);
assert(err == 0);
return sock;
}
int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
int *out_num_fds) {
int err;
char control_buf[CMSG_SPACE(sizeof(int) * num_fds)];
memset(control_buf, 0, CMSG_SPACE(sizeof(int) * num_fds));
struct iovec iov = {
.iov_base = buf,
.iov_len = buf_size,
};
struct msghdr msg = {
.msg_iov = &iov,
.msg_iovlen = 1,
};
if (num_fds > 0) {
assert(fds);
msg.msg_control = control_buf;
msg.msg_controllen = CMSG_SPACE(sizeof(int) * num_fds);
}
if (send) {
if (num_fds) {
struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
assert(cmsg);
cmsg->cmsg_level = SOL_SOCKET;
cmsg->cmsg_type = SCM_RIGHTS;
cmsg->cmsg_len = CMSG_LEN(sizeof(int) * num_fds);
memcpy(CMSG_DATA(cmsg), fds, sizeof(int) * num_fds);
// printf("send clen %d -> %d\n", num_fds, cmsg->cmsg_len);
}
return sendmsg(fd, &msg, 0);
} else {
int r = recvmsg(fd, &msg, 0);
if (r < 0) return r;
int recv_fds = 0;
if (msg.msg_controllen > 0) {
struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
assert(cmsg);
assert(cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS);
recv_fds = (cmsg->cmsg_len - CMSG_LEN(0));
assert(recv_fds > 0 && (recv_fds % sizeof(int)) == 0);
recv_fds /= sizeof(int);
// printf("recv clen %d -> %d\n", cmsg->cmsg_len, recv_fds);
// assert(cmsg->cmsg_len == CMSG_LEN(sizeof(int) * num_fds));
assert(fds && recv_fds <= num_fds);
memcpy(fds, CMSG_DATA(cmsg), sizeof(int) * recv_fds);
}
if (msg.msg_flags) {
for (int i=0; i<recv_fds; i++) {
close(fds[i]);
}
return -1;
}
if (fds) {
assert(out_num_fds);
*out_num_fds = recv_fds;
}
return r;
}
}

19
selfdrive/common/ipc.h
Unescape View File

@ -0,0 +1,19 @@
#ifndef IPC_H
#define IPC_H
#include <stdbool.h>
#ifdef __cplusplus
extern "C" {
#endif
int ipc_connect(const char* socket_path);
int ipc_bind(const char* socket_path);
int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
int *out_num_fds);
#ifdef __cplusplus
} // extern "C"
#endif
#endif

88
selfdrive/common/mat.h
Unescape View File

@ -0,0 +1,88 @@
#ifndef COMMON_MAT_H
#define COMMON_MAT_H
typedef struct vec3 {
float v[3];
} vec3;
typedef struct vec4 {
float v[4];
} vec4;
typedef struct mat3 {
float v[3*3];
} mat3;
typedef struct mat4 {
float v[4*4];
} mat4;
static inline mat3 matmul3(const mat3 a, const mat3 b) {
mat3 ret = {{0.0}};
for (int r=0; r<3; r++) {
for (int c=0; c<3; c++) {
float v = 0.0;
for (int k=0; k<3; k++) {
v += a.v[r*3+k] * b.v[k*3+c];
}
ret.v[r*3+c] = v;
}
}
return ret;
}
static inline vec3 matvecmul3(const mat3 a, const vec3 b) {
vec3 ret = {{0.0}};
for (int r=0; r<3; r++) {
for (int c=0; c<3; c++) {
ret.v[r] += a.v[r*3+c] * b.v[c];
}
}
return ret;
}
static inline mat4 matmul(const mat4 a, const mat4 b) {
mat4 ret = {{0.0}};
for (int r=0; r<4; r++) {
for (int c=0; c<4; c++) {
float v = 0.0;
for (int k=0; k<4; k++) {
v += a.v[r*4+k] * b.v[k*4+c];
}
ret.v[r*4+c] = v;
}
}
return ret;
}
static inline vec4 matvecmul(const mat4 a, const vec4 b) {
vec4 ret = {{0.0}};
for (int r=0; r<4; r++) {
for (int c=0; c<4; c++) {
ret.v[r] += a.v[r*4+c] * b.v[c];
}
}
return ret;
}
// scales the input and output space of a transformation matrix
// that assumes pixel-center origin.
static inline mat3 transform_scale_buffer(const mat3 in, float s) {
// in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s
mat3 transform_out = {{
1.0f/s, 0.0f, 0.5f,
0.0f, 1.0f/s, 0.5f,
0.0f, 0.0f, 1.0f,
}};
mat3 transform_in = {{
s, 0.0f, -0.5f*s,
0.0f, s, -0.5f*s,
0.0f, 0.0f, 1.0f,
}};
return matmul3(transform_in, matmul3(in, transform_out));
}
#endif

15
selfdrive/common/messaging.h
Unescape View File

@ -0,0 +1,15 @@
// the c version of cereal/messaging.py
#include <zmq.h>
// TODO: refactor to take in service instead of endpoint?
void *sub_sock(void *ctx, const char *endpoint) {
void* sock = zmq_socket(ctx, ZMQ_SUB);
assert(sock);
zmq_setsockopt(sock, ZMQ_SUBSCRIBE, "", 0);
int reconnect_ivl = 500;
zmq_setsockopt(sock, ZMQ_RECONNECT_IVL_MAX, &reconnect_ivl, sizeof(reconnect_ivl));
zmq_connect(sock, endpoint);
return sock;
}

40
selfdrive/common/modeldata.h
Unescape View File

@ -0,0 +1,40 @@
#ifndef MODELDATA_H
#define MODELDATA_H
#define MODEL_PATH_DISTANCE 192
#define POLYFIT_DEGREE 4
#define SPEED_PERCENTILES 10
#define DESIRE_PRED_SIZE 32
#define OTHER_META_SIZE 4
typedef struct PathData {
float points[MODEL_PATH_DISTANCE];
float prob;
float std;
float stds[MODEL_PATH_DISTANCE];
float poly[POLYFIT_DEGREE];
} PathData;
typedef struct LeadData {
float dist;
float prob;
float std;
float rel_y;
float rel_y_std;
float rel_v;
float rel_v_std;
float rel_a;
float rel_a_std;
} LeadData;
typedef struct ModelData {
PathData path;
PathData left_lane;
PathData right_lane;
LeadData lead;
LeadData lead_future;
float meta[OTHER_META_SIZE + DESIRE_PRED_SIZE];
float speed[SPEED_PERCENTILES];
} ModelData;
#endif

13
selfdrive/common/mutex.h
Unescape View File

@ -0,0 +1,13 @@
#ifndef COMMON_MUTEX_H
#define COMMON_MUTEX_H
#include <pthread.h>
static inline void mutex_init_reentrant(pthread_mutex_t *mutex) {
pthread_mutexattr_t attr;
pthread_mutexattr_init(&attr);
pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
pthread_mutex_init(mutex, &attr);
}
#endif

307
selfdrive/common/params.cc
Unescape View File

@ -0,0 +1,307 @@
#include "common/params.h"
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif // _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <dirent.h>
#include <sys/file.h>
#include <sys/stat.h>
#include <map>
#include <string>
#include "common/util.h"
#include "common/utilpp.h"
namespace {
template <typename T>
T* null_coalesce(T* a, T* b) {
return a != NULL ? a : b;
}
static const char* default_params_path = null_coalesce(
const_cast<const char*>(getenv("PARAMS_PATH")), "/data/params");
} // namespace
static int fsync_dir(const char* path){
int result = 0;
int fd = open(path, O_RDONLY);
if (fd < 0){
result = -1;
goto cleanup;
}
result = fsync(fd);
if (result < 0) {
goto cleanup;
}
cleanup:
int result_close = 0;
if (fd >= 0){
result_close = close(fd);
}
if (result_close < 0) {
return result_close;
} else {
return result;
}
}
int write_db_value(const char* params_path, const char* key, const char* value,
size_t value_size) {
// Information about safely and atomically writing a file: https://lwn.net/Articles/457667/
// 1) Create temp file
// 2) Write data to temp file
// 3) fsync() the temp file
// 4) rename the temp file to the real name
// 5) fsync() the containing directory
int lock_fd = -1;
int tmp_fd = -1;
int result;
char tmp_path[1024];
char path[1024];
ssize_t bytes_written;
if (params_path == NULL) {
params_path = default_params_path;
}
// Write value to temp.
result =
snprintf(tmp_path, sizeof(tmp_path), "%s/.tmp_value_XXXXXX", params_path);
if (result < 0) {
goto cleanup;
}
tmp_fd = mkstemp(tmp_path);
bytes_written = write(tmp_fd, value, value_size);
if (bytes_written != value_size) {
result = -20;
goto cleanup;
}
// Build lock path
result = snprintf(path, sizeof(path), "%s/.lock", params_path);
if (result < 0) {
goto cleanup;
}
lock_fd = open(path, 0);
// Build key path
result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key);
if (result < 0) {
goto cleanup;
}
// Take lock.
result = flock(lock_fd, LOCK_EX);
if (result < 0) {
goto cleanup;
}
// change permissions to 0666 for apks
result = fchmod(tmp_fd, 0666);
if (result < 0) {
goto cleanup;
}
// fsync to force persist the changes.
result = fsync(tmp_fd);
if (result < 0) {
goto cleanup;
}
// Move temp into place.
result = rename(tmp_path, path);
if (result < 0) {
goto cleanup;
}
// fsync parent directory
result = snprintf(path, sizeof(path), "%s/d", params_path);
if (result < 0) {
goto cleanup;
}
result = fsync_dir(path);
if (result < 0) {
goto cleanup;
}
cleanup:
// Release lock.
if (lock_fd >= 0) {
close(lock_fd);
}
if (tmp_fd >= 0) {
if (result < 0) {
remove(tmp_path);
}
close(tmp_fd);
}
return result;
}
int delete_db_value(const char* params_path, const char* key) {
int lock_fd = -1;
int result;
char path[1024];
if (params_path == NULL) {
params_path = default_params_path;
}
// Build lock path, and open lockfile
result = snprintf(path, sizeof(path), "%s/.lock", params_path);
if (result < 0) {
goto cleanup;
}
lock_fd = open(path, 0);
// Take lock.
result = flock(lock_fd, LOCK_EX);
if (result < 0) {
goto cleanup;
}
// Build key path
result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key);
if (result < 0) {
goto cleanup;
}
// Delete value.
result = remove(path);
if (result != 0) {
result = ERR_NO_VALUE;
goto cleanup;
}
// fsync parent directory
result = snprintf(path, sizeof(path), "%s/d", params_path);
if (result < 0) {
goto cleanup;
}
result = fsync_dir(path);
if (result < 0) {
goto cleanup;
}
cleanup:
// Release lock.
if (lock_fd >= 0) {
close(lock_fd);
}
return result;
}
int read_db_value(const char* params_path, const char* key, char** value,
size_t* value_sz) {
int lock_fd = -1;
int result;
char path[1024];
if (params_path == NULL) {
params_path = default_params_path;
}
result = snprintf(path, sizeof(path), "%s/.lock", params_path);
if (result < 0) {
goto cleanup;
}
lock_fd = open(path, 0);
result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key);
if (result < 0) {
goto cleanup;
}
// Take lock.
result = flock(lock_fd, LOCK_EX);
if (result < 0) {
goto cleanup;
}
// Read value.
// TODO(mgraczyk): If there is a lot of contention, we can release the lock
// after opening the file, before reading.
*value = static_cast<char*>(read_file(path, value_sz));
if (*value == NULL) {
result = -22;
goto cleanup;
}
result = 0;
cleanup:
// Release lock.
if (lock_fd >= 0) {
close(lock_fd);
}
return result;
}
void read_db_value_blocking(const char* params_path, const char* key,
char** value, size_t* value_sz) {
while (1) {
const int result = read_db_value(params_path, key, value, value_sz);
if (result == 0) {
return;
} else {
// Sleep for 0.1 seconds.
usleep(100000);
}
}
}
int read_db_all(const char* params_path, std::map<std::string, std::string> *params) {
int err = 0;
if (params_path == NULL) {
params_path = default_params_path;
}
std::string lock_path = util::string_format("%s/.lock", params_path);
int lock_fd = open(lock_path.c_str(), 0);
if (lock_fd < 0) return -1;
err = flock(lock_fd, LOCK_EX);
if (err < 0) return err;
std::string key_path = util::string_format("%s/d", params_path);
DIR *d = opendir(key_path.c_str());
if (!d) {
close(lock_fd);
return -1;
}
struct dirent *de = NULL;
while ((de = readdir(d))) {
if (!isalnum(de->d_name[0])) continue;
std::string key = std::string(de->d_name);
if (key == "AccessToken") continue;
std::string value = util::read_file(util::string_format("%s/%s", key_path.c_str(), key.c_str()));
(*params)[key] = value;
}
closedir(d);
close(lock_fd);
return 0;
}

47
selfdrive/common/params.h
Unescape View File

@ -0,0 +1,47 @@
#ifndef _SELFDRIVE_COMMON_PARAMS_H_
#define _SELFDRIVE_COMMON_PARAMS_H_
#include <stddef.h>
#ifdef __cplusplus
extern "C" {
#endif
#define ERR_NO_VALUE -33
int write_db_value(const char* params_path, const char* key, const char* value,
size_t value_size);
// Reads a value from the params database.
// Inputs:
// params_path: The path of the database, or NULL to use the default.
// key: The key to read.
// value: A pointer where a newly allocated string containing the db value will
// be written.
// value_sz: A pointer where the size of value will be written. Does not
// include the NULL terminator.
//
// Returns: Negative on failure, otherwise 0.
int read_db_value(const char* params_path, const char* key, char** value,
size_t* value_sz);
// Delete a value from the params database.
// Inputs are the same as read_db_value, without value and value_sz.
int delete_db_value(const char* params_path, const char* key);
// Reads a value from the params database, blocking until successful.
// Inputs are the same as read_db_value.
void read_db_value_blocking(const char* params_path, const char* key,
char** value, size_t* value_sz);
#ifdef __cplusplus
} // extern "C"
#endif
#ifdef __cplusplus
#include <map>
#include <string>
int read_db_all(const char* params_path, std::map<std::string, std::string> *params);
#endif
#endif // _SELFDRIVE_COMMON_PARAMS_H_

182
selfdrive/common/spinner.c
Unescape View File

@ -0,0 +1,182 @@
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <unistd.h>
#include <assert.h>
#include <ctype.h>
#include <GLES3/gl3.h>
#include <EGL/egl.h>
#include <EGL/eglext.h>
#include "nanovg.h"
#define NANOVG_GLES3_IMPLEMENTATION
#include "nanovg_gl.h"
#include "nanovg_gl_utils.h"
#include "framebuffer.h"
#include "spinner.h"
#define SPINTEXT_LENGTH 128
// external resources linked in
extern const unsigned char _binary_opensans_semibold_ttf_start[];
extern const unsigned char _binary_opensans_semibold_ttf_end[];
extern const unsigned char _binary_img_spinner_track_png_start[];
extern const unsigned char _binary_img_spinner_track_png_end[];
extern const unsigned char _binary_img_spinner_comma_png_start[];
extern const unsigned char _binary_img_spinner_comma_png_end[];
bool stdin_input_available() {
struct timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 0;
fd_set fds;
FD_ZERO(&fds);
FD_SET(STDIN_FILENO, &fds);
select(STDIN_FILENO+1, &fds, NULL, NULL, &timeout);
return (FD_ISSET(0, &fds));
}
int spin(int argc, char** argv) {
int err;
bool draw_progress = false;
float progress_val = 0.0;
char spintext[SPINTEXT_LENGTH];
spintext[0] = 0;
const char* spintext_arg = NULL;
if (argc >= 2) {
strncpy(spintext, argv[1], SPINTEXT_LENGTH);
}
// spinner
int fb_w, fb_h;
FramebufferState *fb = framebuffer_init("spinner", 0x00001000, false,
&fb_w, &fb_h);
assert(fb);
NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES);
assert(vg);
int font = nvgCreateFontMem(vg, "Bold", (unsigned char*)_binary_opensans_semibold_ttf_start, _binary_opensans_semibold_ttf_end-_binary_opensans_semibold_ttf_start, 0);
assert(font >= 0);
int spinner_img = nvgCreateImageMem(vg, 0, (unsigned char*)_binary_img_spinner_track_png_start, _binary_img_spinner_track_png_end - _binary_img_spinner_track_png_start);
assert(spinner_img >= 0);
int spinner_img_s = 360;
int spinner_img_x = ((fb_w/2)-(spinner_img_s/2));
int spinner_img_y = 260;
int spinner_img_xc = (fb_w/2);
int spinner_img_yc = (fb_h/2)-100;
int spinner_comma_img = nvgCreateImageMem(vg, 0, (unsigned char*)_binary_img_spinner_comma_png_start, _binary_img_spinner_comma_png_end - _binary_img_spinner_comma_png_start);
assert(spinner_comma_img >= 0);
for (int cnt = 0; ; cnt++) {
// Check stdin for new text
if (stdin_input_available()){
fgets(spintext, SPINTEXT_LENGTH, stdin);
spintext[strcspn(spintext, "\n")] = 0;
// Check if number (update progress bar)
size_t len = strlen(spintext);
bool is_number = len > 0;
for (int i = 0; i < len; i++){
if (!isdigit(spintext[i])){
is_number = false;
break;
}
}
if (is_number) {
progress_val = (float)(atoi(spintext)) / 100.0;
progress_val = fmin(1.0, progress_val);
progress_val = fmax(0.0, progress_val);
}
draw_progress = is_number;
}
glClearColor(0.1, 0.1, 0.1, 1.0);
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
nvgBeginFrame(vg, fb_w, fb_h, 1.0f);
// background
nvgBeginPath(vg);
NVGpaint bg = nvgLinearGradient(vg, fb_w, 0, fb_w, fb_h,
nvgRGBA(0, 0, 0, 175), nvgRGBA(0, 0, 0, 255));
nvgFillPaint(vg, bg);
nvgRect(vg, 0, 0, fb_w, fb_h);
nvgFill(vg);
// spin track
nvgSave(vg);
nvgTranslate(vg, spinner_img_xc, spinner_img_yc);
nvgRotate(vg, (3.75*M_PI * cnt/120.0));
nvgTranslate(vg, -spinner_img_xc, -spinner_img_yc);
NVGpaint spinner_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
spinner_img_s, spinner_img_s, 0, spinner_img, 0.6f);
nvgBeginPath(vg);
nvgFillPaint(vg, spinner_imgPaint);
nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
nvgFill(vg);
nvgRestore(vg);
// comma
NVGpaint comma_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
spinner_img_s, spinner_img_s, 0, spinner_comma_img, 1.0f);
nvgBeginPath(vg);
nvgFillPaint(vg, comma_imgPaint);
nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
nvgFill(vg);
if (draw_progress){
// draw progress bar
int progress_width = 1000;
int progress_x = fb_w/2-progress_width/2;
int progress_y = 775;
int progress_height = 25;
NVGpaint paint = nvgBoxGradient(
vg, progress_x + 1, progress_y + 1,
progress_width - 2, progress_height, 3, 4, nvgRGB(27, 27, 27), nvgRGB(27, 27, 27));
nvgBeginPath(vg);
nvgRoundedRect(vg, progress_x, progress_y, progress_width, progress_height, 12);
nvgFillPaint(vg, paint);
nvgFill(vg);
int bar_pos = ((progress_width - 2) * progress_val);
paint = nvgBoxGradient(
vg, progress_x, progress_y,
bar_pos+1.5f, progress_height-1, 3, 4,
nvgRGB(245, 245, 245), nvgRGB(105, 105, 105));
nvgBeginPath(vg);
nvgRoundedRect(
vg, progress_x+1, progress_y+1,
bar_pos, progress_height-2, 12);
nvgFillPaint(vg, paint);
nvgFill(vg);
} else {
// message
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP);
nvgFontSize(vg, 96.0f);
nvgText(vg, fb_w/2, (fb_h*2/3)+24, spintext, NULL);
}
nvgEndFrame(vg);
framebuffer_swap(fb);
assert(glGetError() == GL_NO_ERROR);
}
return 0;
}

14
selfdrive/common/spinner.h
Unescape View File

@ -0,0 +1,14 @@
#ifndef COMMON_SPINNER_H
#define COMMON_SPINNER_H
#ifdef __cplusplus
extern "C" {
#endif
int spin(int argc, char** argv);
#ifdef __cplusplus
}
#endif
#endif

119
selfdrive/common/swaglog.c
Unescape View File

@ -0,0 +1,119 @@
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>
#include <pthread.h>
#include <zmq.h>
#include <json.h>
#include "common/timing.h"
#include "common/version.h"
#include "swaglog.h"
typedef struct LogState {
pthread_mutex_t lock;
bool inited;
JsonNode *ctx_j;
void *zctx;
void *sock;
int print_level;
} LogState;
static LogState s = {
.lock = PTHREAD_MUTEX_INITIALIZER,
};
static void cloudlog_bind_locked(const char* k, const char* v) {
json_append_member(s.ctx_j, k, json_mkstring(v));
}
static void cloudlog_init() {
if (s.inited) return;
s.ctx_j = json_mkobject();
s.zctx = zmq_ctx_new();
s.sock = zmq_socket(s.zctx, ZMQ_PUSH);
zmq_connect(s.sock, "ipc:///tmp/logmessage");
s.print_level = CLOUDLOG_WARNING;
const char* print_level = getenv("LOGPRINT");
if (print_level) {
if (strcmp(print_level, "debug") == 0) {
s.print_level = CLOUDLOG_DEBUG;
} else if (strcmp(print_level, "info") == 0) {
s.print_level = CLOUDLOG_INFO;
} else if (strcmp(print_level, "warning") == 0) {
s.print_level = CLOUDLOG_WARNING;
}
}
// openpilot bindings
char* dongle_id = getenv("DONGLE_ID");
if (dongle_id) {
cloudlog_bind_locked("dongle_id", dongle_id);
}
cloudlog_bind_locked("version", COMMA_VERSION);
bool dirty = !getenv("CLEAN");
json_append_member(s.ctx_j, "dirty", json_mkbool(dirty));
s.inited = true;
}
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...) {
pthread_mutex_lock(&s.lock);
cloudlog_init();
char* msg_buf = NULL;
va_list args;
va_start(args, fmt);
vasprintf(&msg_buf, fmt, args);
va_end(args);
if (!msg_buf) {
pthread_mutex_unlock(&s.lock);
return;
}
if (levelnum >= s.print_level) {
printf("%s: %s\n", filename, msg_buf);
}
JsonNode *log_j = json_mkobject();
assert(log_j);
json_append_member(log_j, "msg", json_mkstring(msg_buf));
json_append_member(log_j, "ctx", s.ctx_j);
json_append_member(log_j, "levelnum", json_mknumber(levelnum));
json_append_member(log_j, "filename", json_mkstring(filename));
json_append_member(log_j, "lineno", json_mknumber(lineno));
json_append_member(log_j, "funcname", json_mkstring(func));
json_append_member(log_j, "created", json_mknumber(seconds_since_epoch()));
char* log_s = json_encode(log_j);
assert(log_s);
json_remove_from_parent(s.ctx_j);
json_delete(log_j);
free(msg_buf);
char levelnum_c = levelnum;
zmq_send(s.sock, &levelnum_c, 1, ZMQ_NOBLOCK | ZMQ_SNDMORE);
zmq_send(s.sock, log_s, strlen(log_s), ZMQ_NOBLOCK);
free(log_s);
pthread_mutex_unlock(&s.lock);
}
void cloudlog_bind(const char* k, const char* v) {
pthread_mutex_lock(&s.lock);
cloudlog_init();
cloudlog_bind_locked(k, v);
pthread_mutex_unlock(&s.lock);
}

68
selfdrive/common/swaglog.h
Unescape View File

@ -0,0 +1,68 @@
#ifndef SWAGLOG_H
#define SWAGLOG_H
#include "selfdrive/common/timing.h"
#define CLOUDLOG_DEBUG 10
#define CLOUDLOG_INFO 20
#define CLOUDLOG_WARNING 30
#define CLOUDLOG_ERROR 40
#define CLOUDLOG_CRITICAL 50
#ifdef __cplusplus
extern "C" {
#endif
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
void cloudlog_bind(const char* k, const char* v);
#ifdef __cplusplus
}
#endif
#define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
__func__, \
fmt, ## __VA_ARGS__)
#define cloudlog_rl(burst, millis, lvl, fmt, ...) \
{ \
static uint64_t __begin = 0; \
static int __printed = 0; \
static int __missed = 0; \
\
int __burst = (burst); \
int __millis = (millis); \
uint64_t __ts = nanos_since_boot(); \
\
if (!__begin) __begin = __ts; \
\
if (__begin + __millis*1000000ULL < __ts) { \
if (__missed) { \
cloudlog(CLOUDLOG_WARNING, "cloudlog: %d messages supressed", __missed); \
} \
__begin = 0; \
__printed = 0; \
__missed = 0; \
} \
\
if (__printed < __burst) { \
cloudlog(lvl, fmt, ## __VA_ARGS__); \
__printed++; \
} else { \
__missed++; \
} \
}
#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
#define LOGD_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
#endif

64
selfdrive/common/test_params.c
Unescape View File

@ -0,0 +1,64 @@
#include "selfdrive/common/params.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
static const char* const kUsage = "%s: read|write|read_block params_path key [value]\n";
int main(int argc, const char* argv[]) {
if (argc < 4) {
printf(kUsage, argv[0]);
return 0;
}
const char* params_path = argv[2];
const char* key = argv[3];
if (strcmp(argv[1], "read") == 0) {
char* value;
size_t value_size;
int result = read_db_value(params_path, key, &value, &value_size);
if (result >= 0) {
fprintf(stdout, "Read %zu bytes: ", value_size);
fwrite(value, 1, value_size, stdout);
fprintf(stdout, "\n");
free(value);
} else {
fprintf(stderr, "Error reading: %d\n", result);
return -1;
}
} else if (strcmp(argv[1], "write") == 0) {
if (argc < 5) {
fprintf(stderr, "Error: write value required\n");
return 1;
}
const char* value = argv[4];
const size_t value_size = strlen(value);
int result = write_db_value(params_path, key, value, value_size);
if (result >= 0) {
fprintf(stdout, "Wrote %s to %s\n", value, key);
} else {
fprintf(stderr, "Error writing: %d\n", result);
return -1;
}
} else if (strcmp(argv[1], "read_block") == 0) {
char* value;
size_t value_size;
read_db_value_blocking(params_path, key, &value, &value_size);
fprintf(stdout, "Read %zu bytes: ", value_size);
fwrite(value, 1, value_size, stdout);
fprintf(stdout, "\n");
free(value);
} else {
printf(kUsage, argv[0]);
return 1;
}
return 0;
}
// BUILD:
// $ gcc -I$HOME/one selfdrive/common/test_params.c selfdrive/common/params.c selfdrive/common/util.c -o ./test_params
// $ seq 0 100000 | xargs -P20 -I{} ./test_params write /data/params DongleId {} && sleep 0.1 &
// $ while ./test_params read /data/params DongleId; do sleep 0.05; done

54
selfdrive/common/timing.h
Unescape View File

@ -0,0 +1,54 @@
#ifndef COMMON_TIMING_H
#define COMMON_TIMING_H
#include <stdint.h>
#include <time.h>
#ifdef __APPLE__
#define CLOCK_BOOTTIME CLOCK_MONOTONIC
#endif
static inline uint64_t nanos_since_boot() {
struct timespec t;
clock_gettime(CLOCK_BOOTTIME, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
static inline double millis_since_boot() {
struct timespec t;
clock_gettime(CLOCK_BOOTTIME, &t);
return t.tv_sec * 1000.0 + t.tv_nsec * 1e-6;
}
static inline double seconds_since_boot() {
struct timespec t;
clock_gettime(CLOCK_BOOTTIME, &t);
return (double)t.tv_sec + t.tv_nsec * 1e-9;;
}
static inline uint64_t nanos_since_epoch() {
struct timespec t;
clock_gettime(CLOCK_REALTIME, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
static inline double seconds_since_epoch() {
struct timespec t;
clock_gettime(CLOCK_REALTIME, &t);
return (double)t.tv_sec + t.tv_nsec * 1e-9;
}
// you probably should use nanos_since_boot instead
static inline uint64_t nanos_monotonic() {
struct timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
static inline uint64_t nanos_monotonic_raw() {
struct timespec t;
clock_gettime(CLOCK_MONOTONIC_RAW, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
#endif

120
selfdrive/common/touch.c
Unescape View File

@ -0,0 +1,120 @@
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <assert.h>
#include <unistd.h>
#include <fcntl.h>
#include <dirent.h>
#include <sys/poll.h>
#include <linux/input.h>
#include "touch.h"
static int find_dev() {
int err;
int ret = -1;
DIR *dir = opendir("/dev/input");
assert(dir);
struct dirent* de = NULL;
while ((de = readdir(dir))) {
if (strncmp(de->d_name, "event", 5)) continue;
int fd = openat(dirfd(dir), de->d_name, O_RDONLY);
assert(fd >= 0);
unsigned char ev_bits[KEY_MAX / 8 + 1];
err = ioctl(fd, EVIOCGBIT(EV_ABS, sizeof(ev_bits)), ev_bits);
assert(err >= 0);
const int x_key = ABS_MT_POSITION_X / 8;
const int y_key = ABS_MT_POSITION_Y / 8;
if ((ev_bits[x_key] & (ABS_MT_POSITION_X - x_key)) &&
(ev_bits[y_key] & (ABS_MT_POSITION_Y - y_key))) {
ret = fd;
break;
}
close(fd);
}
closedir(dir);
return ret;
}
void touch_init(TouchState *s) {
s->fd = find_dev();
assert(s->fd >= 0);
}
int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) {
assert(out_x && out_y);
bool up = false;
while (true) {
struct pollfd polls[] = {{
.fd = s->fd,
.events = POLLIN,
}};
int err = poll(polls, 1, timeout);
if (err < 0) {
return -1;
}
if (!(polls[0].revents & POLLIN)) {
break;
}
struct input_event event;
err = read(polls[0].fd, &event, sizeof(event));
if (err < sizeof(event)) {
return -1;
}
switch (event.type) {
case EV_ABS:
if (event.code == ABS_MT_POSITION_X) {
s->last_x = event.value;
} else if (event.code == ABS_MT_POSITION_Y) {
s->last_y = event.value;
} else if (event.code == ABS_MT_TRACKING_ID && event.value != -1) {
up = true;
}
break;
default:
break;
}
}
if (up) {
// adjust for flippening
*out_x = s->last_y;
*out_y = 1080 - s->last_x;
}
return up;
}
int touch_read(TouchState *s, int* out_x, int* out_y) {
assert(out_x && out_y);
struct input_event event;
int err = read(s->fd, &event, sizeof(event));
if (err < sizeof(event)) {
return -1;
}
bool up = false;
switch (event.type) {
case EV_ABS:
if (event.code == ABS_MT_POSITION_X) {
s->last_x = event.value;
} else if (event.code == ABS_MT_POSITION_Y) {
s->last_y = event.value;
}
up = true;
break;
default:
break;
}
if (up) {
// adjust for flippening
*out_x = s->last_y;
*out_y = 1080 - s->last_x;
}
return up;
}

21
selfdrive/common/touch.h
Unescape View File

@ -0,0 +1,21 @@
#ifndef TOUCH_H
#define TOUCH_H
#ifdef __cplusplus
extern "C" {
#endif
typedef struct TouchState {
int fd;
int last_x, last_y;
} TouchState;
void touch_init(TouchState *s);
int touch_poll(TouchState *s, int *out_x, int *out_y, int timeout);
int touch_read(TouchState *s, int* out_x, int* out_y);
#ifdef __cplusplus
}
#endif
#endif

61
selfdrive/common/util.c
Unescape View File

@ -0,0 +1,61 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <unistd.h>
#ifdef __linux__
#include <sys/prctl.h>
#include <sys/syscall.h>
#include <sched.h>
#endif
void* read_file(const char* path, size_t* out_len) {
FILE* f = fopen(path, "r");
if (!f) {
return NULL;
}
fseek(f, 0, SEEK_END);
long f_len = ftell(f);
rewind(f);
char* buf = (char*)calloc(f_len, 1);
assert(buf);
size_t num_read = fread(buf, f_len, 1, f);
fclose(f);
if (num_read != 1) {
free(buf);
return NULL;
}
if (out_len) {
*out_len = f_len;
}
return buf;
}
void set_thread_name(const char* name) {
#ifdef __linux__
// pthread_setname_np is dumb (fails instead of truncates)
prctl(PR_SET_NAME, (unsigned long)name, 0, 0, 0);
#endif
}
int set_realtime_priority(int level) {
#ifdef __linux__
long tid = syscall(SYS_gettid);
// should match python using chrt
struct sched_param sa;
memset(&sa, 0, sizeof(sa));
sa.sched_priority = level;
return sched_setscheduler(tid, SCHED_FIFO, &sa);
#else
return -1;
#endif
}

48
selfdrive/common/util.h
Unescape View File

@ -0,0 +1,48 @@
#ifndef COMMON_UTIL_H
#define COMMON_UTIL_H
#include <stdio.h>
#ifndef __cplusplus
#define min(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a < _b ? _a : _b; })
#define max(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
#endif
#define clamp(a,b,c) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
__typeof__ (c) _c = (c); \
_a < _b ? _b : (_a > _c ? _c : _a); })
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
#define ALIGN(x, align) (((x) + (align)-1) & ~((align)-1))
#ifdef __cplusplus
extern "C" {
#endif
// Reads a file into a newly allocated buffer.
//
// Returns NULL on failure, otherwise the NULL-terminated file contents.
// The result must be freed by the caller.
void* read_file(const char* path, size_t* out_len);
void set_thread_name(const char* name);
int set_realtime_priority(int level);
#ifdef __cplusplus
}
#endif
#endif

66
selfdrive/common/utilpp.h
Unescape View File

@ -0,0 +1,66 @@
#ifndef UTILPP_H
#define UTILPP_H
#include <cstdio>
#include <unistd.h>
#include <string>
#include <memory>
#include <sstream>
#include <fstream>
namespace util {
inline bool starts_with(std::string s, std::string prefix) {
return s.compare(0, prefix.size(), prefix) == 0;
}
template<typename ... Args>
inline std::string string_format( const std::string& format, Args ... args ) {
size_t size = snprintf( nullptr, 0, format.c_str(), args ... ) + 1;
std::unique_ptr<char[]> buf( new char[ size ] );
snprintf( buf.get(), size, format.c_str(), args ... );
return std::string( buf.get(), buf.get() + size - 1 );
}
inline std::string read_file(std::string fn) {
std::ifstream t(fn);
std::stringstream buffer;
buffer << t.rdbuf();
return buffer.str();
}
inline std::string tohex(const uint8_t* buf, size_t buf_size) {
std::unique_ptr<char[]> hexbuf(new char[buf_size*2+1]);
for (size_t i=0; i < buf_size; i++) {
sprintf(&hexbuf[i*2], "%02x", buf[i]);
}
hexbuf[buf_size*2] = 0;
return std::string(hexbuf.get(), hexbuf.get() + buf_size*2);
}
inline std::string base_name(std::string const & path) {
size_t pos = path.find_last_of("/");
if (pos == std::string::npos) return path;
return path.substr(pos + 1);
}
inline std::string dir_name(std::string const & path) {
size_t pos = path.find_last_of("/");
if (pos == std::string::npos) return "";
return path.substr(0, pos);
}
inline std::string readlink(std::string path) {
char buff[4096];
ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1);
if (len != -1) {
buff[len] = '\0';
return std::string(buff);
}
return "";
}
}
#endif

1
selfdrive/common/version.h
Unescape View File

@ -0,0 +1 @@
#define COMMA_VERSION "0.7.2"

39
selfdrive/common/visionbuf.h
Unescape View File

@ -0,0 +1,39 @@
#ifndef IONBUF_H
#define IONBUF_H
#ifdef __APPLE__
#include <OpenCL/cl.h>
#else
#include <CL/cl.h>
#endif
#ifdef __cplusplus
extern "C" {
#endif
typedef struct VisionBuf {
size_t len;
void* addr;
int handle;
int fd;
cl_context ctx;
cl_device_id device_id;
cl_mem buf_cl;
cl_command_queue copy_q;
} VisionBuf;
#define VISIONBUF_SYNC_FROM_DEVICE 0
#define VISIONBUF_SYNC_TO_DEVICE 1
VisionBuf visionbuf_allocate(size_t len);
VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem);
cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx);
void visionbuf_sync(const VisionBuf* buf, int dir);
void visionbuf_free(const VisionBuf* buf);
#ifdef __cplusplus
}
#endif
#endif

110
selfdrive/common/visionbuf_cl.c
Unescape View File

@ -0,0 +1,110 @@
#include "visionbuf.h"
#include <stdio.h>
#include <fcntl.h>
#include <assert.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/types.h>
#ifdef __APPLE__
#include <OpenCL/cl.h>
#else
#include <CL/cl.h>
#endif
int offset = 0;
void *malloc_with_fd(size_t len, int *fd) {
char full_path[0x100];
snprintf(full_path, sizeof(full_path)-1, "/dev/shm/visionbuf_%d_%d", getpid(), offset++);
*fd = open(full_path, O_RDWR | O_CREAT, 0777);
unlink(full_path);
ftruncate(*fd, len);
void *addr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0);
return addr;
}
VisionBuf visionbuf_allocate(size_t len) {
// const size_t alignment = 4096;
// void* addr = aligned_alloc(alignment, alignment * ((len - 1) / alignment + 1));
//void* addr = calloc(1, len);
int fd;
void *addr = malloc_with_fd(len, &fd);
return (VisionBuf){
.len = len, .addr = addr, .handle = 1, .fd = fd,
};
}
cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx) {
// HACK because this platform is just for convenience
VisionBuf *w_buf = (VisionBuf*)buf;
cl_mem ret;
*w_buf = visionbuf_allocate_cl(buf->len, device_id, ctx, &ret);
return ret;
}
VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem) {
int err;
assert(out_mem);
#if __OPENCL_VERSION__ >= 200
void* host_ptr =
clSVMAlloc(ctx, CL_MEM_READ_WRITE | CL_MEM_SVM_FINE_GRAIN_BUFFER, len, 0);
assert(host_ptr);
#else
int fd;
void* host_ptr = malloc_with_fd(len, &fd);
cl_command_queue q = clCreateCommandQueue(ctx, device_id, 0, &err);
assert(err == 0);
#endif
cl_mem mem = clCreateBuffer(ctx, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, len, host_ptr, &err);
assert(err == 0);
*out_mem = mem;
return (VisionBuf){
.len = len, .addr = host_ptr, .handle = 0, .fd = fd,
.device_id = device_id, .ctx = ctx, .buf_cl = mem,
#if __OPENCL_VERSION__ < 200
.copy_q = q,
#endif
};
}
void visionbuf_sync(const VisionBuf* buf, int dir) {
int err = 0;
if (!buf->buf_cl) return;
#if __OPENCL_VERSION__ < 200
if (dir == VISIONBUF_SYNC_FROM_DEVICE) {
err = clEnqueueReadBuffer(buf->copy_q, buf->buf_cl, CL_FALSE, 0, buf->len, buf->addr, 0, NULL, NULL);
} else {
err = clEnqueueWriteBuffer(buf->copy_q, buf->buf_cl, CL_FALSE, 0, buf->len, buf->addr, 0, NULL, NULL);
}
assert(err == 0);
clFinish(buf->copy_q);
#endif
}
void visionbuf_free(const VisionBuf* buf) {
if (buf->handle) {
munmap(buf->addr, buf->len);
close(buf->fd);
} else {
int err = clReleaseMemObject(buf->buf_cl);
assert(err == 0);
#if __OPENCL_VERSION__ >= 200
clSVMFree(buf->ctx, buf->addr);
#else
munmap(buf->addr, buf->len);
close(buf->fd);
#endif
}
}

141
selfdrive/common/visionbuf_ion.c
Unescape View File

@ -0,0 +1,141 @@
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <linux/ion.h>
#include <CL/cl_ext.h>
#include <msm_ion.h>
#include "visionbuf.h"
// just hard-code these for convenience
// size_t device_page_size = 0;
// clGetDeviceInfo(device_id, CL_DEVICE_PAGE_SIZE_QCOM,
// sizeof(device_page_size), &device_page_size,
// NULL);
// size_t padding_cl = 0;
// clGetDeviceInfo(device_id, CL_DEVICE_EXT_MEM_PADDING_IN_BYTES_QCOM,
// sizeof(padding_cl), &padding_cl,
// NULL);
#define DEVICE_PAGE_SIZE_CL 4096
#define PADDING_CL 0
static int ion_fd = -1;
static void ion_init() {
if (ion_fd == -1) {
ion_fd = open("/dev/ion", O_RDWR | O_NONBLOCK);
}
}
VisionBuf visionbuf_allocate(size_t len) {
int err;
ion_init();
struct ion_allocation_data ion_alloc = {0};
ion_alloc.len = len + PADDING_CL;
ion_alloc.align = 4096;
ion_alloc.heap_id_mask = 1 << ION_IOMMU_HEAP_ID;
ion_alloc.flags = ION_FLAG_CACHED;
err = ioctl(ion_fd, ION_IOC_ALLOC, &ion_alloc);
assert(err == 0);
struct ion_fd_data ion_fd_data = {0};
ion_fd_data.handle = ion_alloc.handle;
err = ioctl(ion_fd, ION_IOC_SHARE, &ion_fd_data);
assert(err == 0);
void *addr = mmap(NULL, ion_alloc.len,
PROT_READ | PROT_WRITE,
MAP_SHARED, ion_fd_data.fd, 0);
assert(addr != MAP_FAILED);
memset(addr, 0, ion_alloc.len);
return (VisionBuf){
.len = len,
.addr = addr,
.handle = ion_alloc.handle,
.fd = ion_fd_data.fd,
};
}
VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem) {
VisionBuf r = visionbuf_allocate(len);
*out_mem = visionbuf_to_cl(&r, device_id, ctx);
return r;
}
cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx) {
int err = 0;
assert(((uintptr_t)buf->addr % DEVICE_PAGE_SIZE_CL) == 0);
cl_mem_ion_host_ptr ion_cl = {0};
ion_cl.ext_host_ptr.allocation_type = CL_MEM_ION_HOST_PTR_QCOM;
ion_cl.ext_host_ptr.host_cache_policy = CL_MEM_HOST_UNCACHED_QCOM;
ion_cl.ion_filedesc = buf->fd;
ion_cl.ion_hostptr = buf->addr;
cl_mem mem = clCreateBuffer(ctx,
CL_MEM_USE_HOST_PTR | CL_MEM_EXT_HOST_PTR_QCOM,
buf->len, &ion_cl, &err);
assert(err == 0);
return mem;
}
void visionbuf_sync(const VisionBuf* buf, int dir) {
int err;
struct ion_fd_data fd_data = {0};
fd_data.fd = buf->fd;
err = ioctl(ion_fd, ION_IOC_IMPORT, &fd_data);
assert(err == 0);
struct ion_flush_data flush_data = {0};
flush_data.handle = fd_data.handle;
flush_data.vaddr = buf->addr;
flush_data.offset = 0;
flush_data.length = buf->len;
// ION_IOC_INV_CACHES ~= DMA_FROM_DEVICE
// ION_IOC_CLEAN_CACHES ~= DMA_TO_DEVICE
// ION_IOC_CLEAN_INV_CACHES ~= DMA_BIDIRECTIONAL
struct ion_custom_data custom_data = {0};
switch (dir) {
case VISIONBUF_SYNC_FROM_DEVICE:
custom_data.cmd = ION_IOC_INV_CACHES;
break;
case VISIONBUF_SYNC_TO_DEVICE:
custom_data.cmd = ION_IOC_CLEAN_CACHES;
break;
default:
assert(0);
}
custom_data.arg = (unsigned long)&flush_data;
err = ioctl(ion_fd, ION_IOC_CUSTOM, &custom_data);
assert(err == 0);
struct ion_handle_data handle_data = {0};
handle_data.handle = fd_data.handle;
err = ioctl(ion_fd, ION_IOC_FREE, &handle_data);
assert(err == 0);
}
void visionbuf_free(const VisionBuf* buf) {
struct ion_handle_data handle_data = {
.handle = buf->handle,
};
int ret = ioctl(ion_fd, ION_IOC_FREE, &handle_data);
assert(ret == 0);
}

122
selfdrive/common/visionimg.cc
Unescape View File

@ -0,0 +1,122 @@
#include <cassert>
#ifdef QCOM
#include <system/graphics.h>
#include <ui/GraphicBuffer.h>
#include <ui/PixelFormat.h>
#include <gralloc_priv.h>
#include <GLES3/gl3.h>
#define GL_GLEXT_PROTOTYPES
#include <GLES2/gl2ext.h>
#include <EGL/egl.h>
#define EGL_EGLEXT_PROTOTYPES
#include <EGL/eglext.h>
#endif
#include "common/util.h"
#include "common/visionbuf.h"
#include "common/visionimg.h"
#ifdef QCOM
using namespace android;
// from libadreno_utils.so
extern "C" void compute_aligned_width_and_height(int width,
int height,
int bpp,
int tile_mode,
int raster_mode,
int padding_threshold,
int *aligned_w,
int *aligned_h);
#endif
void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h) {
#ifdef QCOM
compute_aligned_width_and_height(ALIGN(width, 32), ALIGN(height, 32), 3, 0, 0, 512, aligned_w, aligned_h);
#else
*aligned_w = width; *aligned_h = height;
#endif
}
VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf) {
int aligned_w = 0, aligned_h = 0;
visionimg_compute_aligned_width_and_height(width, height, &aligned_w, &aligned_h);
int stride = aligned_w * 3;
size_t size = aligned_w * aligned_h * 3;
VisionBuf buf = visionbuf_allocate(size);
*out_buf = buf;
return (VisionImg){
.fd = buf.fd,
.format = VISIONIMG_FORMAT_RGB24,
.width = width,
.height = height,
.stride = stride,
.size = size,
.bpp = 3,
};
}
#ifdef QCOM
EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph) {
assert((img->size % img->stride) == 0);
assert((img->stride % img->bpp) == 0);
int format = 0;
if (img->format == VISIONIMG_FORMAT_RGB24) {
format = HAL_PIXEL_FORMAT_RGB_888;
} else {
assert(false);
}
private_handle_t* hnd = new private_handle_t(img->fd, img->size,
private_handle_t::PRIV_FLAGS_USES_ION|private_handle_t::PRIV_FLAGS_FRAMEBUFFER,
0, format,
img->stride/img->bpp, img->size/img->stride,
img->width, img->height);
GraphicBuffer* gb = new GraphicBuffer(img->width, img->height, (PixelFormat)format,
GraphicBuffer::USAGE_HW_TEXTURE, img->stride/img->bpp, hnd, false);
// GraphicBuffer is ref counted by EGLClientBuffer(ANativeWindowBuffer), no need and not possible to release.
*pph = hnd;
return (EGLClientBuffer) gb->getNativeBuffer();
}
GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph) {
EGLClientBuffer buf = visionimg_to_egl(img, pph);
EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
assert(display != EGL_NO_DISPLAY);
EGLint img_attrs[] = { EGL_IMAGE_PRESERVED_KHR, EGL_TRUE, EGL_NONE };
EGLImageKHR image = eglCreateImageKHR(display, EGL_NO_CONTEXT,
EGL_NATIVE_BUFFER_ANDROID, buf, img_attrs);
assert(image != EGL_NO_IMAGE_KHR);
GLuint tex = 0;
glGenTextures(1, &tex);
glBindTexture(GL_TEXTURE_2D, tex);
glEGLImageTargetTexture2DOES(GL_TEXTURE_2D, image);
*pkhr = image;
return tex;
}
void visionimg_destroy_gl(EGLImageKHR khr, void *ph) {
EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
assert(display != EGL_NO_DISPLAY);
eglDestroyImageKHR(display, khr);
delete (private_handle_t*)ph;
}
#endif

36
selfdrive/common/visionimg.h
Unescape View File

@ -0,0 +1,36 @@
#ifndef VISIONIMG_H
#define VISIONIMG_H
#include "common/visionbuf.h"
#include <GLES3/gl3.h>
#include <EGL/egl.h>
#include <EGL/eglext.h>
#undef Status
#ifdef __cplusplus
extern "C" {
#endif
#define VISIONIMG_FORMAT_RGB24 1
typedef struct VisionImg {
int fd;
int format;
int width, height, stride;
int bpp;
size_t size;
} VisionImg;
void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h);
VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf);
EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph);
GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph);
void visionimg_destroy_gl(EGLImageKHR khr, void *ph);
#ifdef __cplusplus
} // extern "C"
#endif
#endif

194
selfdrive/common/visionipc.c
Unescape View File

@ -0,0 +1,194 @@
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <unistd.h>
#include <assert.h>
#include <errno.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <sys/un.h>
#include "ipc.h"
#include "visionipc.h"
typedef struct VisionPacketWire {
int type;
VisionPacketData d;
} VisionPacketWire;
int vipc_connect() {
return ipc_connect(VIPC_SOCKET_PATH);
}
int vipc_recv(int fd, VisionPacket *out_p) {
VisionPacketWire p = {0};
VisionPacket p2 = {0};
int ret = ipc_sendrecv_with_fds(false, fd, &p, sizeof(p), (int*)p2.fds, VIPC_MAX_FDS, &p2.num_fds);
if (ret < 0) {
printf("vipc_recv err: %s\n", strerror(errno));
} else {
p2.type = p.type;
p2.d = p.d;
*out_p = p2;
}
//printf("%d = vipc_recv(%d, %d): %d %d %d %u\n", ret, fd, p2.num_fds, out_p->d.stream_bufs.type, out_p->d.stream_bufs.width, out_p->d.stream_bufs.height, out_p->d.stream_bufs.buf_len);
return ret;
}
int vipc_send(int fd, const VisionPacket *p2) {
assert(p2->num_fds <= VIPC_MAX_FDS);
VisionPacketWire p = {
.type = p2->type,
.d = p2->d,
};
int ret = ipc_sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL);
//printf("%d = vipc_send(%d, %d): %d %d %d %u\n", ret, fd, p2->num_fds, p2->d.stream_bufs.type, p2->d.stream_bufs.width, p2->d.stream_bufs.height, p2->d.stream_bufs.buf_len);
return ret;
}
void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
int num_fds, const int* fds) {
for (int i=0; i<num_fds; i++) {
if (bufs[i].addr) {
munmap(bufs[i].addr, bufs[i].len);
bufs[i].addr = NULL;
close(bufs[i].fd);
}
bufs[i].fd = fds[i];
bufs[i].len = stream_bufs->buf_len;
bufs[i].addr = mmap(NULL, bufs[i].len,
PROT_READ | PROT_WRITE,
MAP_SHARED, bufs[i].fd, 0);
// printf("b %d %zu -> %p\n", bufs[i].fd, bufs[i].len, bufs[i].addr);
assert(bufs[i].addr != MAP_FAILED);
}
}
int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info) {
int err;
memset(s, 0, sizeof(*s));
s->last_idx = -1;
s->ipc_fd = vipc_connect();
if (s->ipc_fd < 0) return -1;
VisionPacket p = {
.type = VIPC_STREAM_SUBSCRIBE,
.d = { .stream_sub = {
.type = type,
.tbuffer = tbuffer,
}, },
};
err = vipc_send(s->ipc_fd, &p);
if (err < 0) {
close(s->ipc_fd);
return -1;
}
VisionPacket rp;
err = vipc_recv(s->ipc_fd, &rp);
if (err <= 0) {
close(s->ipc_fd);
return -1;
}
assert(rp.type = VIPC_STREAM_BUFS);
assert(rp.d.stream_bufs.type == type);
s->bufs_info = rp.d.stream_bufs;
s->num_bufs = rp.num_fds;
s->bufs = calloc(s->num_bufs, sizeof(VIPCBuf));
assert(s->bufs);
vipc_bufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds);
if (out_bufs_info) {
*out_bufs_info = s->bufs_info;
}
return 0;
}
void visionstream_release(VisionStream *s) {
int err;
if (s->last_idx >= 0) {
VisionPacket rep = {
.type = VIPC_STREAM_RELEASE,
.d = { .stream_rel = {
.type = s->last_type,
.idx = s->last_idx,
}}
};
err = vipc_send(s->ipc_fd, &rep);
s->last_idx = -1;
}
}
VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra) {
int err;
VisionPacket rp;
err = vipc_recv(s->ipc_fd, &rp);
if (err <= 0) {
return NULL;
}
assert(rp.type == VIPC_STREAM_ACQUIRE);
if (s->last_idx >= 0) {
VisionPacket rep = {
.type = VIPC_STREAM_RELEASE,
.d = { .stream_rel = {
.type = s->last_type,
.idx = s->last_idx,
}}
};
err = vipc_send(s->ipc_fd, &rep);
if (err <= 0) {
return NULL;
}
}
s->last_type = rp.d.stream_acq.type;
s->last_idx = rp.d.stream_acq.idx;
assert(s->last_idx < s->num_bufs);
if (out_extra) {
*out_extra = rp.d.stream_acq.extra;
}
return &s->bufs[s->last_idx];
}
void visionstream_destroy(VisionStream *s) {
int err;
if (s->last_idx >= 0) {
VisionPacket rep = {
.type = VIPC_STREAM_RELEASE,
.d = { .stream_rel = {
.type = s->last_type,
.idx = s->last_idx,
}}
};
err = vipc_send(s->ipc_fd, &rep);
s->last_idx = -1;
}
for (int i=0; i<s->num_bufs; i++) {
if (s->bufs[i].addr) {
munmap(s->bufs[i].addr, s->bufs[i].len);
s->bufs[i].addr = NULL;
close(s->bufs[i].fd);
}
}
if (s->bufs) free(s->bufs);
close(s->ipc_fd);
}

113
selfdrive/common/visionipc.h
Unescape View File

@ -0,0 +1,113 @@
#ifndef VISIONIPC_H
#define VISIONIPC_H
#include <stddef.h>
#include <stdint.h>
#include <stdbool.h>
#define VIPC_SOCKET_PATH "/tmp/vision_socket"
#define VIPC_MAX_FDS 64
#ifdef __cplusplus
extern "C" {
#endif
typedef enum VisionIPCPacketType {
VIPC_INVALID = 0,
VIPC_STREAM_SUBSCRIBE,
VIPC_STREAM_BUFS,
VIPC_STREAM_ACQUIRE,
VIPC_STREAM_RELEASE,
} VisionIPCPacketType;
typedef enum VisionStreamType {
VISION_STREAM_RGB_BACK,
VISION_STREAM_RGB_FRONT,
VISION_STREAM_YUV,
VISION_STREAM_YUV_FRONT,
VISION_STREAM_MAX,
} VisionStreamType;
typedef struct VisionUIInfo {
int big_box_x, big_box_y;
int big_box_width, big_box_height;
int transformed_width, transformed_height;
int front_box_x, front_box_y;
int front_box_width, front_box_height;
} VisionUIInfo;
typedef struct VisionStreamBufs {
VisionStreamType type;
int width, height, stride;
size_t buf_len;
union {
VisionUIInfo ui_info;
} buf_info;
} VisionStreamBufs;
typedef struct VIPCBufExtra {
// only for yuv
uint32_t frame_id;
uint64_t timestamp_eof;
} VIPCBufExtra;
typedef union VisionPacketData {
struct {
VisionStreamType type;
bool tbuffer;
} stream_sub;
VisionStreamBufs stream_bufs;
struct {
VisionStreamType type;
int idx;
VIPCBufExtra extra;
} stream_acq;
struct {
VisionStreamType type;
int idx;
} stream_rel;
} VisionPacketData;
typedef struct VisionPacket {
int type;
VisionPacketData d;
int num_fds;
int fds[VIPC_MAX_FDS];
} VisionPacket;
int vipc_connect(void);
int vipc_recv(int fd, VisionPacket *out_p);
int vipc_send(int fd, const VisionPacket *p);
typedef struct VIPCBuf {
int fd;
size_t len;
void* addr;
} VIPCBuf;
void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
int num_fds, const int* fds);
typedef struct VisionStream {
int ipc_fd;
int last_idx;
int last_type;
int num_bufs;
VisionStreamBufs bufs_info;
VIPCBuf *bufs;
} VisionStream;
int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info);
void visionstream_release(VisionStream *s);
VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra);
void visionstream_destroy(VisionStream *s);
#ifdef __cplusplus
}
#endif
#endif

124
selfdrive/common/visionstream.c
Unescape View File

@ -0,0 +1,124 @@
int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info) {
int err;
memset(s, 0, sizeof(*s));
s->last_idx = -1;
s->ipc_fd = vipc_connect();
if (s->ipc_fd < 0) return -1;
VisionPacket p = {
.type = VIPC_STREAM_SUBSCRIBE,
.d = { .stream_sub = {
.type = type,
.tbuffer = tbuffer,
}, },
};
err = vipc_send(s->ipc_fd, &p);
if (err < 0) {
close(s->ipc_fd);
return -1;
}
VisionPacket rp;
err = vipc_recv(s->ipc_fd, &rp);
if (err <= 0) {
close(s->ipc_fd);
return -1;
}
assert(rp.type = VIPC_STREAM_BUFS);
assert(rp.d.stream_bufs.type == type);
s->bufs_info = rp.d.stream_bufs;
s->num_bufs = rp.num_fds;
s->bufs = calloc(s->num_bufs, sizeof(VIPCBuf));
assert(s->bufs);
vipc_bufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds);
if (out_bufs_info) {
*out_bufs_info = s->bufs_info;
}
return 0;
}
void visionstream_release(VisionStream *s) {
int err;
if (s->last_idx >= 0) {
VisionPacket rep = {
.type = VIPC_STREAM_RELEASE,
.d = { .stream_rel = {
.type = s->last_type,
.idx = s->last_idx,
}}
};
err = vipc_send(s->ipc_fd, &rep);
s->last_idx = -1;
}
}
VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra) {
int err;
VisionPacket rp;
err = vipc_recv(s->ipc_fd, &rp);
if (err <= 0) {
return NULL;
}
assert(rp.type == VIPC_STREAM_ACQUIRE);
if (s->last_idx >= 0) {
VisionPacket rep = {
.type = VIPC_STREAM_RELEASE,
.d = { .stream_rel = {
.type = s->last_type,
.idx = s->last_idx,
}}
};
err = vipc_send(s->ipc_fd, &rep);
if (err <= 0) {
return NULL;
}
}
s->last_type = rp.d.stream_acq.type;
s->last_idx = rp.d.stream_acq.idx;
assert(s->last_idx < s->num_bufs);
if (out_extra) {
*out_extra = rp.d.stream_acq.extra;
}
return &s->bufs[s->last_idx];
}
void visionstream_destroy(VisionStream *s) {
int err;
if (s->last_idx >= 0) {
VisionPacket rep = {
.type = VIPC_STREAM_RELEASE,
.d = { .stream_rel = {
.type = s->last_type,
.idx = s->last_idx,
}}
};
err = vipc_send(s->ipc_fd, &rep);
s->last_idx = -1;
}
for (int i=0; i<s->num_bufs; i++) {
if (s->bufs[i].addr) {
munmap(s->bufs[i].addr, s->bufs[i].len);
s->bufs[i].addr = NULL;
close(s->bufs[i].fd);
}
}
if (s->bufs) free(s->bufs);
close(s->ipc_fd);
}
Write Preview
Loading…
Cancel
Save