spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

selfdrive/common

pull/960/head
George Hotz 2020-01-17 11:01:02 -08:00
parent 71ead9adea
commit 368a956b96
41 changed files with 3876 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
selfdrive/common/SConscript 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
View File

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

39
selfdrive/common/visionbuf.h 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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 100644
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);
}