diff --git a/.gitignore b/.gitignore index 789f2728f3..05c14a98ab 100644 --- a/.gitignore +++ b/.gitignore @@ -20,7 +20,9 @@ a.out .*.un~ *.tmp *.o +*.o-* *.os +*.os-* *.so *.a *.clb diff --git a/SConstruct b/SConstruct index bec87eda54..5894c2ab66 100644 --- a/SConstruct +++ b/SConstruct @@ -14,32 +14,46 @@ AddOption('--asan', arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() if platform.system() == "Darwin": arch = "Darwin" +if arch == "aarch64" and not os.path.isdir("/system"): + arch = "larch64" webcam = bool(ARGUMENTS.get("use_webcam", 0)) -if arch == "aarch64": +if arch == "aarch64" or arch == "larch64": lenv = { "LD_LIBRARY_PATH": '/data/data/com.termux/files/usr/lib', "PATH": os.environ['PATH'], - "ANDROID_DATA": os.environ['ANDROID_DATA'], - "ANDROID_ROOT": os.environ['ANDROID_ROOT'], } + if arch == "aarch64": + # android + lenv["ANDROID_DATA"] = os.environ['ANDROID_DATA'] + lenv["ANDROID_ROOT"] = os.environ['ANDROID_ROOT'] + cpppath = [ "#phonelibs/opencl/include", ] + libpath = [ - "#phonelibs/snpe/aarch64-android-clang3.8", "/usr/lib", "/data/data/com.termux/files/usr/lib", "/system/vendor/lib64", "/system/comma/usr/lib", "#phonelibs/nanovg", - "#phonelibs/libyuv/lib", ] - cflags = ["-DQCOM", "-mcpu=cortex-a57"] - cxxflags = ["-DQCOM", "-mcpu=cortex-a57"] + if arch == "larch64": + cpppath += ["#phonelibs/capnp-cpp/include", "#phonelibs/capnp-c/include"] + libpath += ["#phonelibs/snpe/larch64"] + libpath += ["#phonelibs/libyuv/larch64/lib"] + libpath += ["#external/capnparm/lib", "/usr/lib/aarch64-linux-gnu"] + cflags = ["-DQCOM2", "-mcpu=cortex-a57"] + cxxflags = ["-DQCOM2", "-mcpu=cortex-a57"] + else: + libpath += ["#phonelibs/snpe/aarch64"] + libpath += ["#phonelibs/libyuv/lib"] + cflags = ["-DQCOM", "-mcpu=cortex-a57"] + cxxflags = ["-DQCOM", "-mcpu=cortex-a57"] rpath = ["/system/vendor/lib64"] else: @@ -177,9 +191,11 @@ def abspath(x): # rpath works elsewhere return x[0].path.rsplit("/", 1)[1][:-3] -#zmq = 'zmq' # still needed for apks -zmq = FindFile("libzmq.a", libpath) +if arch == 'larch64': + zmq = 'zmq' +else: + zmq = FindFile("libzmq.a", libpath) Export('env', 'arch', 'zmq', 'SHARED', 'webcam') # cereal and messaging are shared with the system diff --git a/cereal b/cereal index 22986de4a6..35040fe6bb 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 22986de4a65db337f3ae63620d551d48225c4cca +Subproject commit 35040fe6bb9ebc31d38e98faa64d5ec4093ce3d5 diff --git a/external/capnparm/.gitignore b/external/capnparm/.gitignore new file mode 100644 index 0000000000..30acc8dd7a --- /dev/null +++ b/external/capnparm/.gitignore @@ -0,0 +1 @@ +capnproto-c++-0.6.1* diff --git a/external/capnparm/bin/capnp b/external/capnparm/bin/capnp new file mode 100755 index 0000000000..1d63a895f7 Binary files /dev/null and b/external/capnparm/bin/capnp differ diff --git a/external/capnparm/bin/capnpc b/external/capnparm/bin/capnpc new file mode 120000 index 0000000000..5668473f09 --- /dev/null +++ b/external/capnparm/bin/capnpc @@ -0,0 +1 @@ +capnp \ No newline at end of file diff --git a/external/capnparm/bin/capnpc-c b/external/capnparm/bin/capnpc-c new file mode 100755 index 0000000000..cc88361259 Binary files /dev/null and b/external/capnparm/bin/capnpc-c differ diff --git a/external/capnparm/bin/capnpc-c++ b/external/capnparm/bin/capnpc-c++ new file mode 100755 index 0000000000..f630a58ed8 Binary files /dev/null and b/external/capnparm/bin/capnpc-c++ differ diff --git a/external/capnparm/bin/capnpc-capnp b/external/capnparm/bin/capnpc-capnp new file mode 100755 index 0000000000..4cb7443263 Binary files /dev/null and b/external/capnparm/bin/capnpc-capnp differ diff --git a/external/capnparm/bin/capnpc-java b/external/capnparm/bin/capnpc-java new file mode 100755 index 0000000000..a81a7ea59e Binary files /dev/null and b/external/capnparm/bin/capnpc-java differ diff --git a/external/capnparm/build.sh b/external/capnparm/build.sh new file mode 100755 index 0000000000..909a1c20c4 --- /dev/null +++ b/external/capnparm/build.sh @@ -0,0 +1,47 @@ +set -e +echo "Installing capnp" + +ONE=${HOME}/openpilot +VERSION=0.6.1 +wget https://capnproto.org/capnproto-c++-${VERSION}.tar.gz +tar xvf capnproto-c++-${VERSION}.tar.gz +cd capnproto-c++-${VERSION} +CXXFLAGS="-fPIC" ./configure --prefix=${ONE}/external/capnparm +make -j9 + +# manually build binaries statically +g++ -std=gnu++11 -I./src -I./src -DKJ_HEADER_WARNINGS -DCAPNP_HEADER_WARNINGS -DCAPNP_INCLUDE_DIR=\"/usr/local/include\" -pthread -O2 -DNDEBUG -pthread -pthread -o .libs/capnp src/capnp/compiler/module-loader.o src/capnp/compiler/capnp.o ./.libs/libcapnpc.a ./.libs/libcapnp.a ./.libs/libkj.a -lpthread -pthread + +g++ -std=gnu++11 -I./src -I./src -DKJ_HEADER_WARNINGS -DCAPNP_HEADER_WARNINGS -DCAPNP_INCLUDE_DIR=\"/usr/local/include\" -pthread -O2 -DNDEBUG -pthread -pthread -o .libs/capnpc-c++ src/capnp/compiler/capnpc-c++.o ./.libs/libcapnp.a ./.libs/libkj.a -lpthread -pthread + +g++ -std=gnu++11 -I./src -I./src -DKJ_HEADER_WARNINGS -DCAPNP_HEADER_WARNINGS -DCAPNP_INCLUDE_DIR=\"/usr/local/include\" -pthread -O2 -DNDEBUG -pthread -pthread -o .libs/capnpc-capnp src/capnp/compiler/capnpc-capnp.o ./.libs/libcapnp.a ./.libs/libkj.a -lpthread -pthread + + +make -j4 install + +# -------- +echo "Installing c-capnp" + +git clone https://github.com/commaai/c-capnproto.git +cd c-capnproto +git submodule update --init --recursive +autoreconf -f -i -s +CFLAGS="-fPIC" ./configure --prefix=${ONE}/external/capnparm +make -j4 + +# manually build binaries statically +gcc -fPIC -o .libs/capnpc-c compiler/capnpc-c.o compiler/schema.capnp.o compiler/str.o ./.libs/libcapnp_c.a -Wl,-rpath -Wl,${ONE}/external/capnp/lib + +make install + +# -------- +echo "Installing java-capnp" + +git clone https://github.com/dwrensha/capnproto-java.git +cd capnproto-java +git reset --hard 2c43bd712fb218da0eabdf241a750b9c05903e8e +g++ compiler/src/main/cpp/capnpc-java.c++ -std=c++11 -pthread -I${ONE}/external/capnp/include -L${ONE}/external/capnp/lib -l:libcapnp.a -l:libkj.a -pthread -lpthread -o capnpc-java +cp capnpc-java ${ONE}/external/capnp/bin/ + +rm -rf capnproto-c++-${VERSION}.tar.gz +rm -rf capnproto-c++-${VERSION} diff --git a/external/capnparm/include b/external/capnparm/include new file mode 120000 index 0000000000..dd0350f7d9 --- /dev/null +++ b/external/capnparm/include @@ -0,0 +1 @@ +../capnp/include \ No newline at end of file diff --git a/external/capnparm/lib/libcapnp-0.6.1.so b/external/capnparm/lib/libcapnp-0.6.1.so new file mode 100755 index 0000000000..b91dbfa096 Binary files /dev/null and b/external/capnparm/lib/libcapnp-0.6.1.so differ diff --git a/external/capnparm/lib/libcapnp-json-0.6.1.so b/external/capnparm/lib/libcapnp-json-0.6.1.so new file mode 100755 index 0000000000..2fcf1cce3c Binary files /dev/null and b/external/capnparm/lib/libcapnp-json-0.6.1.so differ diff --git a/external/capnparm/lib/libcapnp-json.a b/external/capnparm/lib/libcapnp-json.a new file mode 100644 index 0000000000..0f2ea332ce Binary files /dev/null and b/external/capnparm/lib/libcapnp-json.a differ diff --git a/external/capnparm/lib/libcapnp-json.la b/external/capnparm/lib/libcapnp-json.la new file mode 100755 index 0000000000..9ffd74a410 --- /dev/null +++ b/external/capnparm/lib/libcapnp-json.la @@ -0,0 +1,41 @@ +# libcapnp-json.la - a libtool library file +# Generated by libtool (GNU libtool) 2.4.6 Debian-2.4.6-2 +# +# Please DO NOT delete this file! +# It is necessary for linking the library. + +# The name that we can dlopen(3). +dlname='libcapnp-json-0.6.1.so' + +# Names of this library. +library_names='libcapnp-json-0.6.1.so libcapnp-json-0.6.1.so libcapnp-json.so' + +# The name of the static archive. +old_library='libcapnp-json.a' + +# Linker flags that cannot go in dependency_libs. +inherited_linker_flags=' -pthread' + +# Libraries that this one depends upon. +dependency_libs=' /home/comma/openpilot/external/capnparm/lib/libcapnp.la /home/comma/openpilot/external/capnparm/lib/libkj.la -lpthread' + +# Names of additional weak libraries provided by this library +weak_library_names='' + +# Version information for libcapnp-json. +current=0 +age=0 +revision=0 + +# Is this an already installed library? +installed=yes + +# Should we warn about portability when linking against -modules? +shouldnotlink=no + +# Files to dlopen/dlpreopen +dlopen='' +dlpreopen='' + +# Directory that this library needs to be installed in: +libdir='/home/comma/openpilot/external/capnparm/lib' diff --git a/external/capnparm/lib/libcapnp-json.so b/external/capnparm/lib/libcapnp-json.so new file mode 120000 index 0000000000..439f29db8e --- /dev/null +++ b/external/capnparm/lib/libcapnp-json.so @@ -0,0 +1 @@ +libcapnp-json-0.6.1.so \ No newline at end of file diff --git a/external/capnparm/lib/libcapnp-rpc-0.6.1.so b/external/capnparm/lib/libcapnp-rpc-0.6.1.so new file mode 100755 index 0000000000..46b04fae73 Binary files /dev/null and b/external/capnparm/lib/libcapnp-rpc-0.6.1.so differ diff --git a/external/capnparm/lib/libcapnp-rpc.a b/external/capnparm/lib/libcapnp-rpc.a new file mode 100644 index 0000000000..2330258105 Binary files /dev/null and b/external/capnparm/lib/libcapnp-rpc.a differ diff --git a/external/capnparm/lib/libcapnp-rpc.la b/external/capnparm/lib/libcapnp-rpc.la new file mode 100755 index 0000000000..ac7b61b67c --- /dev/null +++ b/external/capnparm/lib/libcapnp-rpc.la @@ -0,0 +1,41 @@ +# libcapnp-rpc.la - a libtool library file +# Generated by libtool (GNU libtool) 2.4.6 Debian-2.4.6-2 +# +# Please DO NOT delete this file! +# It is necessary for linking the library. + +# The name that we can dlopen(3). +dlname='libcapnp-rpc-0.6.1.so' + +# Names of this library. +library_names='libcapnp-rpc-0.6.1.so libcapnp-rpc-0.6.1.so libcapnp-rpc.so' + +# The name of the static archive. +old_library='libcapnp-rpc.a' + +# Linker flags that cannot go in dependency_libs. +inherited_linker_flags=' -pthread' + +# Libraries that this one depends upon. +dependency_libs=' /home/comma/openpilot/external/capnparm/lib/libcapnp.la /home/comma/openpilot/external/capnparm/lib/libkj-async.la /home/comma/openpilot/external/capnparm/lib/libkj.la -lpthread' + +# Names of additional weak libraries provided by this library +weak_library_names='' + +# Version information for libcapnp-rpc. +current=0 +age=0 +revision=0 + +# Is this an already installed library? +installed=yes + +# Should we warn about portability when linking against -modules? +shouldnotlink=no + +# Files to dlopen/dlpreopen +dlopen='' +dlpreopen='' + +# Directory that this library needs to be installed in: +libdir='/home/comma/openpilot/external/capnparm/lib' diff --git a/external/capnparm/lib/libcapnp-rpc.so b/external/capnparm/lib/libcapnp-rpc.so new file mode 120000 index 0000000000..a4f0e19500 --- /dev/null +++ b/external/capnparm/lib/libcapnp-rpc.so @@ -0,0 +1 @@ +libcapnp-rpc-0.6.1.so \ No newline at end of file diff --git a/external/capnparm/lib/libcapnp.a b/external/capnparm/lib/libcapnp.a new file mode 100644 index 0000000000..95122fd793 Binary files /dev/null and b/external/capnparm/lib/libcapnp.a differ diff --git a/external/capnparm/lib/libcapnp.la b/external/capnparm/lib/libcapnp.la new file mode 100755 index 0000000000..36e866c323 --- /dev/null +++ b/external/capnparm/lib/libcapnp.la @@ -0,0 +1,41 @@ +# libcapnp.la - a libtool library file +# Generated by libtool (GNU libtool) 2.4.6 Debian-2.4.6-2 +# +# Please DO NOT delete this file! +# It is necessary for linking the library. + +# The name that we can dlopen(3). +dlname='libcapnp-0.6.1.so' + +# Names of this library. +library_names='libcapnp-0.6.1.so libcapnp-0.6.1.so libcapnp.so' + +# The name of the static archive. +old_library='libcapnp.a' + +# Linker flags that cannot go in dependency_libs. +inherited_linker_flags=' -pthread' + +# Libraries that this one depends upon. +dependency_libs=' /home/comma/openpilot/external/capnparm/lib/libkj.la -lpthread' + +# Names of additional weak libraries provided by this library +weak_library_names='' + +# Version information for libcapnp. +current=0 +age=0 +revision=0 + +# Is this an already installed library? +installed=yes + +# Should we warn about portability when linking against -modules? +shouldnotlink=no + +# Files to dlopen/dlpreopen +dlopen='' +dlpreopen='' + +# Directory that this library needs to be installed in: +libdir='/home/comma/openpilot/external/capnparm/lib' diff --git a/external/capnparm/lib/libcapnp.so b/external/capnparm/lib/libcapnp.so new file mode 120000 index 0000000000..a14cdcee7b --- /dev/null +++ b/external/capnparm/lib/libcapnp.so @@ -0,0 +1 @@ +libcapnp-0.6.1.so \ No newline at end of file diff --git a/external/capnparm/lib/libcapnp_c.a b/external/capnparm/lib/libcapnp_c.a new file mode 100644 index 0000000000..9f7ee7c0df Binary files /dev/null and b/external/capnparm/lib/libcapnp_c.a differ diff --git a/external/capnparm/lib/libcapnp_c.la b/external/capnparm/lib/libcapnp_c.la new file mode 100755 index 0000000000..392aa7a2de --- /dev/null +++ b/external/capnparm/lib/libcapnp_c.la @@ -0,0 +1,41 @@ +# libcapnp_c.la - a libtool library file +# Generated by libtool (GNU libtool) 2.4.6 Debian-2.4.6-0.1 +# +# Please DO NOT delete this file! +# It is necessary for linking the library. + +# The name that we can dlopen(3). +dlname='libcapnp_c.so.0' + +# Names of this library. +library_names='libcapnp_c.so.0.0.0 libcapnp_c.so.0 libcapnp_c.so' + +# The name of the static archive. +old_library='libcapnp_c.a' + +# Linker flags that cannot go in dependency_libs. +inherited_linker_flags='' + +# Libraries that this one depends upon. +dependency_libs='' + +# Names of additional weak libraries provided by this library +weak_library_names='' + +# Version information for libcapnp_c. +current=0 +age=0 +revision=0 + +# Is this an already installed library? +installed=yes + +# Should we warn about portability when linking against -modules? +shouldnotlink=no + +# Files to dlopen/dlpreopen +dlopen='' +dlpreopen='' + +# Directory that this library needs to be installed in: +libdir='/home/comma/openpilot/external/capnparm/lib' diff --git a/external/capnparm/lib/libcapnp_c.so b/external/capnparm/lib/libcapnp_c.so new file mode 120000 index 0000000000..5ff443389f --- /dev/null +++ b/external/capnparm/lib/libcapnp_c.so @@ -0,0 +1 @@ +libcapnp_c.so.0.0.0 \ No newline at end of file diff --git a/external/capnparm/lib/libcapnp_c.so.0 b/external/capnparm/lib/libcapnp_c.so.0 new file mode 120000 index 0000000000..5ff443389f --- /dev/null +++ b/external/capnparm/lib/libcapnp_c.so.0 @@ -0,0 +1 @@ +libcapnp_c.so.0.0.0 \ No newline at end of file diff --git a/external/capnparm/lib/libcapnp_c.so.0.0.0 b/external/capnparm/lib/libcapnp_c.so.0.0.0 new file mode 100755 index 0000000000..b2b875ceca Binary files /dev/null and b/external/capnparm/lib/libcapnp_c.so.0.0.0 differ diff --git a/external/capnparm/lib/libcapnpc-0.6.1.so b/external/capnparm/lib/libcapnpc-0.6.1.so new file mode 100755 index 0000000000..8438e824bb Binary files /dev/null and b/external/capnparm/lib/libcapnpc-0.6.1.so differ diff --git a/external/capnparm/lib/libcapnpc.a b/external/capnparm/lib/libcapnpc.a new file mode 100644 index 0000000000..8257774180 Binary files /dev/null and b/external/capnparm/lib/libcapnpc.a differ diff --git a/external/capnparm/lib/libcapnpc.la b/external/capnparm/lib/libcapnpc.la new file mode 100755 index 0000000000..1c6299c78b --- /dev/null +++ b/external/capnparm/lib/libcapnpc.la @@ -0,0 +1,41 @@ +# libcapnpc.la - a libtool library file +# Generated by libtool (GNU libtool) 2.4.6 Debian-2.4.6-2 +# +# Please DO NOT delete this file! +# It is necessary for linking the library. + +# The name that we can dlopen(3). +dlname='libcapnpc-0.6.1.so' + +# Names of this library. +library_names='libcapnpc-0.6.1.so libcapnpc-0.6.1.so libcapnpc.so' + +# The name of the static archive. +old_library='libcapnpc.a' + +# Linker flags that cannot go in dependency_libs. +inherited_linker_flags=' -pthread' + +# Libraries that this one depends upon. +dependency_libs=' /home/comma/openpilot/external/capnparm/lib/libcapnp.la /home/comma/openpilot/external/capnparm/lib/libkj.la -lpthread' + +# Names of additional weak libraries provided by this library +weak_library_names='' + +# Version information for libcapnpc. +current=0 +age=0 +revision=0 + +# Is this an already installed library? +installed=yes + +# Should we warn about portability when linking against -modules? +shouldnotlink=no + +# Files to dlopen/dlpreopen +dlopen='' +dlpreopen='' + +# Directory that this library needs to be installed in: +libdir='/home/comma/openpilot/external/capnparm/lib' diff --git a/external/capnparm/lib/libcapnpc.so b/external/capnparm/lib/libcapnpc.so new file mode 120000 index 0000000000..e1be5626dd --- /dev/null +++ b/external/capnparm/lib/libcapnpc.so @@ -0,0 +1 @@ +libcapnpc-0.6.1.so \ No newline at end of file diff --git a/external/capnparm/lib/libkj-0.6.1.so b/external/capnparm/lib/libkj-0.6.1.so new file mode 100755 index 0000000000..bf944a8d03 Binary files /dev/null and b/external/capnparm/lib/libkj-0.6.1.so differ diff --git a/external/capnparm/lib/libkj-async-0.6.1.so b/external/capnparm/lib/libkj-async-0.6.1.so new file mode 100755 index 0000000000..eeb451a6cd Binary files /dev/null and b/external/capnparm/lib/libkj-async-0.6.1.so differ diff --git a/external/capnparm/lib/libkj-async.a b/external/capnparm/lib/libkj-async.a new file mode 100644 index 0000000000..d65ac035b6 Binary files /dev/null and b/external/capnparm/lib/libkj-async.a differ diff --git a/external/capnparm/lib/libkj-async.la b/external/capnparm/lib/libkj-async.la new file mode 100755 index 0000000000..cca7fab9c5 --- /dev/null +++ b/external/capnparm/lib/libkj-async.la @@ -0,0 +1,41 @@ +# libkj-async.la - a libtool library file +# Generated by libtool (GNU libtool) 2.4.6 Debian-2.4.6-2 +# +# Please DO NOT delete this file! +# It is necessary for linking the library. + +# The name that we can dlopen(3). +dlname='libkj-async-0.6.1.so' + +# Names of this library. +library_names='libkj-async-0.6.1.so libkj-async-0.6.1.so libkj-async.so' + +# The name of the static archive. +old_library='libkj-async.a' + +# Linker flags that cannot go in dependency_libs. +inherited_linker_flags=' -pthread' + +# Libraries that this one depends upon. +dependency_libs=' /home/comma/openpilot/external/capnparm/lib/libkj.la -lpthread' + +# Names of additional weak libraries provided by this library +weak_library_names='' + +# Version information for libkj-async. +current=0 +age=0 +revision=0 + +# Is this an already installed library? +installed=yes + +# Should we warn about portability when linking against -modules? +shouldnotlink=no + +# Files to dlopen/dlpreopen +dlopen='' +dlpreopen='' + +# Directory that this library needs to be installed in: +libdir='/home/comma/openpilot/external/capnparm/lib' diff --git a/external/capnparm/lib/libkj-async.so b/external/capnparm/lib/libkj-async.so new file mode 120000 index 0000000000..415bd24bee --- /dev/null +++ b/external/capnparm/lib/libkj-async.so @@ -0,0 +1 @@ +libkj-async-0.6.1.so \ No newline at end of file diff --git a/external/capnparm/lib/libkj-http-0.6.1.so b/external/capnparm/lib/libkj-http-0.6.1.so new file mode 100755 index 0000000000..98dae6f427 Binary files /dev/null and b/external/capnparm/lib/libkj-http-0.6.1.so differ diff --git a/external/capnparm/lib/libkj-http.a b/external/capnparm/lib/libkj-http.a new file mode 100644 index 0000000000..f20efd0494 Binary files /dev/null and b/external/capnparm/lib/libkj-http.a differ diff --git a/external/capnparm/lib/libkj-http.la b/external/capnparm/lib/libkj-http.la new file mode 100755 index 0000000000..6df422a386 --- /dev/null +++ b/external/capnparm/lib/libkj-http.la @@ -0,0 +1,41 @@ +# libkj-http.la - a libtool library file +# Generated by libtool (GNU libtool) 2.4.6 Debian-2.4.6-2 +# +# Please DO NOT delete this file! +# It is necessary for linking the library. + +# The name that we can dlopen(3). +dlname='libkj-http-0.6.1.so' + +# Names of this library. +library_names='libkj-http-0.6.1.so libkj-http-0.6.1.so libkj-http.so' + +# The name of the static archive. +old_library='libkj-http.a' + +# Linker flags that cannot go in dependency_libs. +inherited_linker_flags=' -pthread' + +# Libraries that this one depends upon. +dependency_libs=' /home/comma/openpilot/external/capnparm/lib/libkj-async.la /home/comma/openpilot/external/capnparm/lib/libkj.la -lpthread' + +# Names of additional weak libraries provided by this library +weak_library_names='' + +# Version information for libkj-http. +current=0 +age=0 +revision=0 + +# Is this an already installed library? +installed=yes + +# Should we warn about portability when linking against -modules? +shouldnotlink=no + +# Files to dlopen/dlpreopen +dlopen='' +dlpreopen='' + +# Directory that this library needs to be installed in: +libdir='/home/comma/openpilot/external/capnparm/lib' diff --git a/external/capnparm/lib/libkj-http.so b/external/capnparm/lib/libkj-http.so new file mode 120000 index 0000000000..f66c52a3f5 --- /dev/null +++ b/external/capnparm/lib/libkj-http.so @@ -0,0 +1 @@ +libkj-http-0.6.1.so \ No newline at end of file diff --git a/external/capnparm/lib/libkj-test-0.6.1.so b/external/capnparm/lib/libkj-test-0.6.1.so new file mode 100755 index 0000000000..6dc0a89a5f Binary files /dev/null and b/external/capnparm/lib/libkj-test-0.6.1.so differ diff --git a/external/capnparm/lib/libkj-test.a b/external/capnparm/lib/libkj-test.a new file mode 100644 index 0000000000..50924135c3 Binary files /dev/null and b/external/capnparm/lib/libkj-test.a differ diff --git a/external/capnparm/lib/libkj-test.la b/external/capnparm/lib/libkj-test.la new file mode 100755 index 0000000000..1b939e1ec1 --- /dev/null +++ b/external/capnparm/lib/libkj-test.la @@ -0,0 +1,41 @@ +# libkj-test.la - a libtool library file +# Generated by libtool (GNU libtool) 2.4.6 Debian-2.4.6-2 +# +# Please DO NOT delete this file! +# It is necessary for linking the library. + +# The name that we can dlopen(3). +dlname='libkj-test-0.6.1.so' + +# Names of this library. +library_names='libkj-test-0.6.1.so libkj-test-0.6.1.so libkj-test.so' + +# The name of the static archive. +old_library='libkj-test.a' + +# Linker flags that cannot go in dependency_libs. +inherited_linker_flags=' -pthread' + +# Libraries that this one depends upon. +dependency_libs=' /home/comma/openpilot/external/capnparm/lib/libkj.la -lpthread' + +# Names of additional weak libraries provided by this library +weak_library_names='' + +# Version information for libkj-test. +current=0 +age=0 +revision=0 + +# Is this an already installed library? +installed=yes + +# Should we warn about portability when linking against -modules? +shouldnotlink=no + +# Files to dlopen/dlpreopen +dlopen='' +dlpreopen='' + +# Directory that this library needs to be installed in: +libdir='/home/comma/openpilot/external/capnparm/lib' diff --git a/external/capnparm/lib/libkj-test.so b/external/capnparm/lib/libkj-test.so new file mode 120000 index 0000000000..8474e67019 --- /dev/null +++ b/external/capnparm/lib/libkj-test.so @@ -0,0 +1 @@ +libkj-test-0.6.1.so \ No newline at end of file diff --git a/external/capnparm/lib/libkj.a b/external/capnparm/lib/libkj.a new file mode 100644 index 0000000000..a1b496ae95 Binary files /dev/null and b/external/capnparm/lib/libkj.a differ diff --git a/external/capnparm/lib/libkj.la b/external/capnparm/lib/libkj.la new file mode 100755 index 0000000000..bc6e99882d --- /dev/null +++ b/external/capnparm/lib/libkj.la @@ -0,0 +1,41 @@ +# libkj.la - a libtool library file +# Generated by libtool (GNU libtool) 2.4.6 Debian-2.4.6-2 +# +# Please DO NOT delete this file! +# It is necessary for linking the library. + +# The name that we can dlopen(3). +dlname='libkj-0.6.1.so' + +# Names of this library. +library_names='libkj-0.6.1.so libkj-0.6.1.so libkj.so' + +# The name of the static archive. +old_library='libkj.a' + +# Linker flags that cannot go in dependency_libs. +inherited_linker_flags=' -pthread' + +# Libraries that this one depends upon. +dependency_libs=' -lpthread' + +# Names of additional weak libraries provided by this library +weak_library_names='' + +# Version information for libkj. +current=0 +age=0 +revision=0 + +# Is this an already installed library? +installed=yes + +# Should we warn about portability when linking against -modules? +shouldnotlink=no + +# Files to dlopen/dlpreopen +dlopen='' +dlpreopen='' + +# Directory that this library needs to be installed in: +libdir='/home/comma/openpilot/external/capnparm/lib' diff --git a/external/capnparm/lib/libkj.so b/external/capnparm/lib/libkj.so new file mode 120000 index 0000000000..07c6d3bb08 --- /dev/null +++ b/external/capnparm/lib/libkj.so @@ -0,0 +1 @@ +libkj-0.6.1.so \ No newline at end of file diff --git a/external/capnparm/lib/pkgconfig/c-capnproto.pc b/external/capnparm/lib/pkgconfig/c-capnproto.pc new file mode 100644 index 0000000000..95f2d975bf --- /dev/null +++ b/external/capnparm/lib/pkgconfig/c-capnproto.pc @@ -0,0 +1,12 @@ +prefix=/home/comma/openpilot/external/capnparm +exec_prefix=${prefix} +libdir=${exec_prefix}/lib +bindir=${exec_prefix}/bin +includedir=${prefix}/include +codegen=${bindir}/capnpc-c + +Name: c-capnproto +Description: Cap'n Proto C bindings +Version: 0.2 +Libs: -L${libdir} -lcapnp_c +Cflags: -I${includedir} diff --git a/external/capnparm/lib/pkgconfig/capnp-rpc.pc b/external/capnparm/lib/pkgconfig/capnp-rpc.pc new file mode 100644 index 0000000000..56f45029ca --- /dev/null +++ b/external/capnparm/lib/pkgconfig/capnp-rpc.pc @@ -0,0 +1,11 @@ +prefix=/home/comma/openpilot/external/capnparm +exec_prefix=${prefix} +libdir=${exec_prefix}/lib +includedir=${prefix}/include + +Name: Cap'n Proto RPC +Description: Fast object-oriented RPC system +Version: 0.6.1 +Libs: -L${libdir} -lcapnp-rpc +Requires: capnp = 0.6.1 kj-async = 0.6.1 +Cflags: -I${includedir} diff --git a/external/capnparm/lib/pkgconfig/capnp.pc b/external/capnparm/lib/pkgconfig/capnp.pc new file mode 100644 index 0000000000..3813d23a39 --- /dev/null +++ b/external/capnparm/lib/pkgconfig/capnp.pc @@ -0,0 +1,12 @@ +prefix=/home/comma/openpilot/external/capnparm +exec_prefix=${prefix} +libdir=${exec_prefix}/lib +includedir=${prefix}/include + +Name: Cap'n Proto +Description: Insanely fast serialization system +Version: 0.6.1 +Libs: -L${libdir} -lcapnp -pthread -lpthread +Libs.private: -lpthread +Requires: kj = 0.6.1 +Cflags: -I${includedir} -pthread diff --git a/external/capnparm/lib/pkgconfig/kj-async.pc b/external/capnparm/lib/pkgconfig/kj-async.pc new file mode 100644 index 0000000000..ffcd024b9a --- /dev/null +++ b/external/capnparm/lib/pkgconfig/kj-async.pc @@ -0,0 +1,11 @@ +prefix=/home/comma/openpilot/external/capnparm +exec_prefix=${prefix} +libdir=${exec_prefix}/lib +includedir=${prefix}/include + +Name: KJ Async Framework Library +Description: Basic utility library called KJ (async part) +Version: 0.6.1 +Libs: -L${libdir} -lkj-async -pthread -lpthread +Requires: kj = 0.6.1 +Cflags: -I${includedir} -pthread diff --git a/external/capnparm/lib/pkgconfig/kj.pc b/external/capnparm/lib/pkgconfig/kj.pc new file mode 100644 index 0000000000..a1610d0779 --- /dev/null +++ b/external/capnparm/lib/pkgconfig/kj.pc @@ -0,0 +1,10 @@ +prefix=/home/comma/openpilot/external/capnparm +exec_prefix=${prefix} +libdir=${exec_prefix}/lib +includedir=${prefix}/include + +Name: KJ Framework Library +Description: Basic utility library called KJ +Version: 0.6.1 +Libs: -L${libdir} -lkj -pthread -lpthread +Cflags: -I${includedir} -pthread diff --git a/phonelibs/capnp-cpp/larch64/include b/phonelibs/capnp-cpp/larch64/include new file mode 120000 index 0000000000..198e3f2957 --- /dev/null +++ b/phonelibs/capnp-cpp/larch64/include @@ -0,0 +1 @@ +../../../external/capnparm/include \ No newline at end of file diff --git a/phonelibs/capnp-cpp/larch64/lib b/phonelibs/capnp-cpp/larch64/lib new file mode 120000 index 0000000000..bf768993ee --- /dev/null +++ b/phonelibs/capnp-cpp/larch64/lib @@ -0,0 +1 @@ +../../../external/capnparm/lib \ No newline at end of file diff --git a/phonelibs/libyuv/larch64/lib/libyuv.a b/phonelibs/libyuv/larch64/lib/libyuv.a new file mode 100644 index 0000000000..fdbcda2517 Binary files /dev/null and b/phonelibs/libyuv/larch64/lib/libyuv.a differ diff --git a/phonelibs/linux/include/msm_ion.h b/phonelibs/linux/include/msm_ion.h index e41b3170fd..d5caed5eb3 100644 --- a/phonelibs/linux/include/msm_ion.h +++ b/phonelibs/linux/include/msm_ion.h @@ -166,7 +166,7 @@ struct ion_flush_data { struct ion_prefetch_regions { unsigned int vmid; - size_t __user *sizes; + size_t *sizes; unsigned int nr_sizes; }; @@ -174,7 +174,7 @@ struct ion_prefetch_data { int heap_id; unsigned long len; /* Is unsigned long bad? 32bit compiler vs 64 bit compiler*/ - struct ion_prefetch_regions __user *regions; + struct ion_prefetch_regions *regions; unsigned int nr_regions; }; diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libSNPE.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libSNPE.so new file mode 100644 index 0000000000..45cf75d44f Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libSNPE.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libatomic.so.1 b/phonelibs/snpe/aarch64-linux-gcc4.9/libatomic.so.1 new file mode 100644 index 0000000000..809fd4ef6f Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libatomic.so.1 differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator.so new file mode 100644 index 0000000000..9452b0e4ae Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so new file mode 100644 index 0000000000..eb3d336ea0 Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so new file mode 100644 index 0000000000..61a910ff58 Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so new file mode 100644 index 0000000000..c12328c7d1 Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libsymphony-cpu.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libsymphony-cpu.so new file mode 100755 index 0000000000..202d7e9710 Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libsymphony-cpu.so differ diff --git a/phonelibs/snpe/larch64 b/phonelibs/snpe/larch64 new file mode 120000 index 0000000000..e5c222b1c9 --- /dev/null +++ b/phonelibs/snpe/larch64 @@ -0,0 +1 @@ +aarch64-linux-gcc4.9 \ No newline at end of file diff --git a/scripts/tici_prep.sh b/scripts/tici_prep.sh new file mode 100755 index 0000000000..8cf2f58ce2 --- /dev/null +++ b/scripts/tici_prep.sh @@ -0,0 +1,3 @@ +#!/bin/bash +sudo chmod 666 /dev/ion /dev/kgsl-3d0 /dev/video* /dev/v4l-subdev* + diff --git a/selfdrive/boardd/boardd_setup.py b/selfdrive/boardd/boardd_setup.py index 1da81997c3..b8997f8542 100644 --- a/selfdrive/boardd/boardd_setup.py +++ b/selfdrive/boardd/boardd_setup.py @@ -21,7 +21,10 @@ if ARCH == "x86_64": ARCH_DIR = 'x64' else: libraries = [':libcan_list_to_can_capnp.a', 'capnp', 'kj'] - ARCH_DIR = 'aarch64' + if os.path.isdir("/system"): + ARCH_DIR = 'aarch64' + else: + ARCH_DIR = 'larch64' setup(name='Boardd API Implementation', cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript index 336fc784db..374ae139c5 100644 --- a/selfdrive/camerad/SConscript +++ b/selfdrive/camerad/SConscript @@ -5,6 +5,9 @@ libs = ['m', 'pthread', common, 'jpeg', 'json', cereal, 'OpenCL', messaging, 'cz if arch == "aarch64": libs += ['gsl', 'CB', 'adreno_utils', 'EGL', 'GLESv3', 'cutils', 'ui'] cameras = ['cameras/camera_qcom.c'] +elif arch == "larch64": + libs += [] + cameras = ['cameras/camera_qcom2.c'] else: if webcam: libs += ['opencv_core', 'opencv_highgui', 'opencv_imgproc', 'opencv_videoio'] diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 6d1017fe2b..1bd8ac6c77 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -12,7 +12,8 @@ #define CAMERA_ID_OV10640 5 #define CAMERA_ID_LGC920 6 #define CAMERA_ID_LGC615 7 -#define CAMERA_ID_MAX 8 +#define CAMERA_ID_AR0231 8 +#define CAMERA_ID_MAX 9 #ifdef __cplusplus extern "C" { diff --git a/selfdrive/camerad/cameras/camera_qcom2.c b/selfdrive/camerad/cameras/camera_qcom2.c new file mode 100644 index 0000000000..20c786a94f --- /dev/null +++ b/selfdrive/camerad/cameras/camera_qcom2.c @@ -0,0 +1,944 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/util.h" +#include "common/swaglog.h" +#include "camera_qcom2.h" + +#include "media/cam_defs.h" +#include "media/cam_isp.h" +#include "media/cam_isp_ife.h" +#include "media/cam_sensor_cmn_header.h" +#include "media/cam_sensor.h" +#include "media/cam_sync.h" + +#include "sensor2_i2c.h" + +#define FRAME_WIDTH 1928 +#define FRAME_HEIGHT 1208 +//#define FRAME_STRIDE 1936 // for 8 bit output +#define FRAME_STRIDE 2416 // for 10 bit output + +static void hexdump(uint8_t *data, int len) { + for (int i = 0; i < len; i++) { + if (i!=0&&i%0x10==0) printf("\n"); + printf("%02X ", data[i]); + } + printf("\n"); +} + + +extern volatile sig_atomic_t do_exit; + +CameraInfo cameras_supported[CAMERA_ID_MAX] = { + [CAMERA_ID_AR0231] = { + .frame_width = FRAME_WIDTH, + .frame_height = FRAME_HEIGHT, + .frame_stride = FRAME_STRIDE, + .bayer = true, + .bayer_flip = 0, + .hdr = false + }, +}; + +// ************** low level camera helpers **************** + +int cam_control(int fd, int op_code, void *handle, int size) { + struct cam_control camcontrol = {0}; + camcontrol.op_code = op_code; + camcontrol.handle = (uint64_t)handle; + if (size == 0) { camcontrol.size = 8; + camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE; + } else { + camcontrol.size = size; + camcontrol.handle_type = CAM_HANDLE_USER_POINTER; + } + + int ret = ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol); + if (ret == -1) { + perror("wat"); + } + return ret; +} + +int device_control(int fd, int op_code, int session_handle, int dev_handle) { + // start stop and release are all the same + static struct cam_release_dev_cmd release_dev_cmd; + release_dev_cmd.session_handle = session_handle; + release_dev_cmd.dev_handle = dev_handle; + return cam_control(fd, op_code, &release_dev_cmd, sizeof(release_dev_cmd)); +} + +void *alloc_w_mmu_hdl(int video0_fd, int len, int align, int flags, uint32_t *handle, int mmu_hdl, int mmu_hdl2) { + int ret; + + static struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0}; + mem_mgr_alloc_cmd.len = len; + mem_mgr_alloc_cmd.align = align; + mem_mgr_alloc_cmd.flags = flags; + mem_mgr_alloc_cmd.num_hdl = 0; + if (mmu_hdl != 0) { + mem_mgr_alloc_cmd.mmu_hdls[0] = mmu_hdl; + mem_mgr_alloc_cmd.num_hdl++; + } + if (mmu_hdl2 != 0) { + mem_mgr_alloc_cmd.mmu_hdls[1] = mmu_hdl2; + mem_mgr_alloc_cmd.num_hdl++; + } + + cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd)); + *handle = mem_mgr_alloc_cmd.out.buf_handle; + + void *ptr = NULL; + if (mem_mgr_alloc_cmd.out.fd > 0) { + ptr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, mem_mgr_alloc_cmd.out.fd, 0); + assert(ptr != MAP_FAILED); + } + + LOGD("alloced: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr); + + return ptr; +} + +void *alloc(int video0_fd, int len, int align, int flags, uint32_t *handle) { + return alloc_w_mmu_hdl(video0_fd, len, align, flags, handle, 0, 0); +} + +void release(int video0_fd, uint32_t handle) { + int ret; + static struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0}; + mem_mgr_release_cmd.buf_handle = handle; + + ret = cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd)); + assert(ret == 0); +} + + +void release_fd(int video0_fd, uint32_t handle) { + // handle to fd + close(handle>>16); + release(video0_fd, handle); +} + +// ************** high level camera helpers **************** + +void sensors_poke(struct CameraState *s, int request_id) { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet); + struct cam_packet *pkt = alloc(s->video0_fd, size, 8, + CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle); + pkt->num_cmd_buf = 1; + pkt->kmd_cmd_buf_index = -1; + pkt->header.size = size; + pkt->header.op_code = 0x7f; + pkt->header.request_id = request_id; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + + static struct cam_config_dev_cmd config_dev_cmd = {}; + config_dev_cmd.session_handle = s->session_handle; + config_dev_cmd.dev_handle = s->sensor_dev_handle; + config_dev_cmd.offset = 0; + config_dev_cmd.packet_handle = cam_packet_handle; + + int ret = cam_control(s->sensor_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd)); + assert(ret == 0); + + release_fd(s->video0_fd, cam_packet_handle); +} + +void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int len, int op_code) { + LOGD("sensors_i2c: %d", len); + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; + struct cam_packet *pkt = alloc(s->video0_fd, size, 8, + CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle); + pkt->num_cmd_buf = 1; + pkt->kmd_cmd_buf_index = -1; + pkt->header.size = size; + pkt->header.op_code = op_code; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + + buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_random_wr) + (len-1)*sizeof(struct i2c_random_wr_payload); + buf_desc[0].type = CAM_CMD_BUF_I2C; + struct cam_cmd_power *power = alloc(s->video0_fd, buf_desc[0].size, 8, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &buf_desc[0].mem_handle); + struct cam_cmd_i2c_random_wr *i2c_random_wr = (void*)power; + i2c_random_wr->header.count = len; + i2c_random_wr->header.op_code = 1; + i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR; + i2c_random_wr->header.data_type = CAMERA_SENSOR_I2C_TYPE_WORD; + i2c_random_wr->header.addr_type = CAMERA_SENSOR_I2C_TYPE_WORD; + memcpy(i2c_random_wr->random_wr_payload, dat, len*sizeof(struct i2c_random_wr_payload)); + + static struct cam_config_dev_cmd config_dev_cmd = {}; + config_dev_cmd.session_handle = s->session_handle; + config_dev_cmd.dev_handle = s->sensor_dev_handle; + config_dev_cmd.offset = 0; + config_dev_cmd.packet_handle = cam_packet_handle; + + int ret = cam_control(s->sensor_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd)); + assert(ret == 0); + + release_fd(s->video0_fd, buf_desc[0].mem_handle); + release_fd(s->video0_fd, cam_packet_handle); +} + +void sensors_init(int video0_fd, int sensor_fd, int camera_num) { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; + struct cam_packet *pkt = alloc(video0_fd, size, 8, + CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle); + pkt->num_cmd_buf = 2; + pkt->kmd_cmd_buf_index = -1; + pkt->header.op_code = 0x1000003; + pkt->header.size = size; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + + buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe); + buf_desc[0].type = CAM_CMD_BUF_LEGACY; + struct cam_cmd_i2c_info *i2c_info = alloc(video0_fd, buf_desc[0].size, 8, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &buf_desc[0].mem_handle); + struct cam_cmd_probe *probe = (struct cam_cmd_probe *)((uint8_t *)i2c_info) + sizeof(struct cam_cmd_i2c_info); + + switch (camera_num) { + case 0: + // port 0 + i2c_info->slave_addr = 0x20; + probe->camera_id = 0; + break; + case 1: + // port 1 + i2c_info->slave_addr = 0x30; + probe->camera_id = 1; + break; + case 2: + // port 2 + i2c_info->slave_addr = 0x20; + probe->camera_id = 2; + break; + } + + // 0(I2C_STANDARD_MODE) = 100khz, 1(I2C_FAST_MODE) = 400khz + //i2c_info->i2c_freq_mode = I2C_STANDARD_MODE; + i2c_info->i2c_freq_mode = I2C_FAST_MODE; + i2c_info->cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_INFO; + + probe->data_type = CAMERA_SENSOR_I2C_TYPE_WORD; + probe->addr_type = CAMERA_SENSOR_I2C_TYPE_WORD; + probe->op_code = 3; // don't care? + probe->cmd_type = CAMERA_SENSOR_CMD_TYPE_PROBE; + probe->reg_addr = 0x3000; //0x300a; //0x300b; + probe->expected_data = 0x354; //0x7750; //0x885a; + probe->data_mask = 0; + + //buf_desc[1].size = buf_desc[1].length = 148; + buf_desc[1].size = buf_desc[1].length = 196; + buf_desc[1].type = CAM_CMD_BUF_I2C; + struct cam_cmd_power *power = alloc(video0_fd, buf_desc[1].size, 8, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &buf_desc[1].mem_handle); + memset(power, 0, buf_desc[1].size); + struct cam_cmd_unconditional_wait *unconditional_wait; + + void *ptr = power; + // 7750 + /*power->count = 2; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 2; + power->power_settings[1].power_seq_type = 8; + power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings));*/ + + // 885a + power->count = 4; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 3; // clock?? + power->power_settings[1].power_seq_type = 1; // analog + power->power_settings[2].power_seq_type = 2; // digital + power->power_settings[3].power_seq_type = 8; // reset low + power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)); + + unconditional_wait = (void*)power; + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = 5; + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + power = (void*)power + sizeof(struct cam_cmd_unconditional_wait); + + // set clock + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 0; + power->power_settings[0].config_val_low = 24000000; //Hz + power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)); + + unconditional_wait = (void*)power; + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = 10; // ms + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + power = (void*)power + sizeof(struct cam_cmd_unconditional_wait); + + // 8,1 is this reset? + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 8; + power->power_settings[0].config_val_low = 1; + power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)); + + unconditional_wait = (void*)power; + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = 100; // ms + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + power = (void*)power + sizeof(struct cam_cmd_unconditional_wait); + + // probe happens here + + // disable clock + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 0; + power->power_settings[0].config_val_low = 0; + power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)); + + unconditional_wait = (void*)power; + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = 1; + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + power = (void*)power + sizeof(struct cam_cmd_unconditional_wait); + + // reset high + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 8; + power->power_settings[0].config_val_low = 1; + power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)); + + unconditional_wait = (void*)power; + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = 1; + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + power = (void*)power + sizeof(struct cam_cmd_unconditional_wait); + + // reset low + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 8; + power->power_settings[0].config_val_low = 0; + power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)); + + unconditional_wait = (void*)power; + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = 1; + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + power = (void*)power + sizeof(struct cam_cmd_unconditional_wait); + + // 7750 + /*power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 2; + power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings));*/ + + // 885a + power->count = 3; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 2; + power->power_settings[1].power_seq_type = 1; + power->power_settings[2].power_seq_type = 3; + power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)); + + LOGD("probing the sensor"); + int ret = cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)cam_packet_handle, 0); + assert(ret == 0); + + release_fd(video0_fd, buf_desc[0].mem_handle); + release_fd(video0_fd, buf_desc[1].mem_handle); + release_fd(video0_fd, cam_packet_handle); +} + +void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; + if (io_mem_handle != 0) { + size += sizeof(struct cam_buf_io_cfg); + } + struct cam_packet *pkt = alloc(s->video0_fd, size, 8, + CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle); + pkt->num_cmd_buf = 2; + pkt->kmd_cmd_buf_index = 0; + + if (io_mem_handle != 0) { + pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*2; + pkt->num_io_configs = 1; + } + + if (io_mem_handle != 0) { + pkt->header.op_code = 0xf000001; + pkt->header.request_id = request_id; + } else { + pkt->header.op_code = 0xf000000; + } + pkt->header.size = size; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + struct cam_buf_io_cfg *io_cfg = (void*)&pkt->payload + pkt->io_configs_offset; + + // TODO: support MMU + buf_desc[0].size = 65624; + buf_desc[0].length = 0; + buf_desc[0].type = CAM_CMD_BUF_DIRECT; + buf_desc[0].meta_data = 3; + buf_desc[0].mem_handle = buf0_mem_handle; + buf_desc[0].offset = buf0_offset; + + buf_desc[1].size = 324; + if (io_mem_handle != 0) { + buf_desc[1].length = 228; // 0 works here too + buf_desc[1].offset = 0x60; + } else { + buf_desc[1].length = 324; + } + buf_desc[1].type = CAM_CMD_BUF_GENERIC; + buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON; + uint32_t *buf2 = alloc(s->video0_fd, buf_desc[1].size, 0x20, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &buf_desc[1].mem_handle); + + // cam_isp_packet_generic_blob_handler + uint32_t tmp[] = { + // size is 0x20, type is 0(CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG) + 0x2000, + 0x1, 0x0, CAM_ISP_IFE_OUT_RES_RDI_0, 0x1, 0x0, 0x1, 0x0, 0x0, // 1 port, CAM_ISP_IFE_OUT_RES_RDI_0 + // size is 0x38, type is 1(CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG), clocks + 0x3801, + 0x1, 0x4, // Dual mode, 4 RDI wires + 0x18148d00, 0x0, 0x18148d00, 0x0, 0x18148d00, 0x0, // rdi clock + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // junk? + // offset 0x60 + // size is 0xe0, type is 2(CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG), bandwidth + 0xe002, + 0x1, 0x4, // 4 RDI + 0x0, 0x0, 0x1ad27480, 0x0, 0x1ad27480, 0x0, // left_pix_vote + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // right_pix_vote + 0x0, 0x0, 0x6ee11c0, 0x2, 0x6ee11c0, 0x2, // rdi_vote + 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; + memcpy(buf2, tmp, sizeof(tmp)); + + if (io_mem_handle != 0) { + io_cfg[0].mem_handle[0] = io_mem_handle; + io_cfg[0].planes[0] = (struct cam_plane_cfg){ + .width = FRAME_WIDTH, + .height = FRAME_HEIGHT, + .plane_stride = FRAME_STRIDE, + .slice_height = FRAME_HEIGHT, + .meta_stride = 0x0, + .meta_size = 0x0, + .meta_offset = 0x0, + .packer_config = 0x0, + .mode_config = 0x0, + .tile_config = 0x0, + .h_init = 0x0, + .v_init = 0x0, + }; + io_cfg[0].format = 0x3; + io_cfg[0].color_pattern = 0x5; + io_cfg[0].bpp = 0xc; + io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; + io_cfg[0].fence = fence; + io_cfg[0].direction = 0x2; + io_cfg[0].subsample_pattern = 0x1; + io_cfg[0].framedrop_pattern = 0x1; + } + + static struct cam_config_dev_cmd config_dev_cmd = {}; + config_dev_cmd.session_handle = s->session_handle; + config_dev_cmd.dev_handle = s->isp_dev_handle; + config_dev_cmd.offset = 0; + config_dev_cmd.packet_handle = cam_packet_handle; + + int ret = cam_control(s->isp_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd)); + if (ret != 0) { + printf("ISP CONFIG FAILED\n"); + } + + release_fd(s->video0_fd, buf_desc[1].mem_handle); + //release(s->video0_fd, buf_desc[0].mem_handle); + release_fd(s->video0_fd, cam_packet_handle); +} + +void enqueue_buffer(struct CameraState *s, int i) { + int ret; + int request_id = (++s->sched_request_id); + bool first = true; + + if (s->buf_handle[i]) { + first = false; + release(s->video0_fd, s->buf_handle[i]); + + // destroy old output fence + static struct cam_sync_info sync_destroy = {0}; + strcpy(sync_destroy.name, "NodeOutputPortFence"); + sync_destroy.sync_obj = s->sync_objs[i]; + ret = cam_control(s->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); + LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj); + } + + // new request_ids + s->request_ids[i] = request_id; + + // do stuff + static struct cam_req_mgr_sched_request req_mgr_sched_request = {0}; + req_mgr_sched_request.session_hdl = s->session_handle; + req_mgr_sched_request.link_hdl = s->link_handle; + req_mgr_sched_request.req_id = request_id; + ret = cam_control(s->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request)); + LOGD("sched req: %d %d", ret, request_id); + + // create output fence + static struct cam_sync_info sync_create = {0}; + strcpy(sync_create.name, "NodeOutputPortFence"); + ret = cam_control(s->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); + LOGD("fence req: %d %d", ret, sync_create.sync_obj); + s->sync_objs[i] = sync_create.sync_obj; + + // configure ISP to put the image in place + static struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; + mem_mgr_map_cmd.mmu_hdls[0] = s->device_iommu; + mem_mgr_map_cmd.num_hdl = 1; + mem_mgr_map_cmd.flags = 1; + mem_mgr_map_cmd.fd = s->bufs[i].fd; + ret = cam_control(s->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); + LOGD("map buf req: (fd: %d) 0x%x %d", s->bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); + s->buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; + + // poke sensor + sensors_poke(s, request_id); + LOGD("Poked sensor"); + + // push the buffer + config_isp(s, s->buf_handle[i], s->sync_objs[i], request_id, s->buf0_handle, 65632*(i+1)); +} + + +// ******************* camera ******************* + +static void camera_release_buffer(void* cookie, int i) { + int ret; + CameraState *s = cookie; + enqueue_buffer(s, i); +} + +static void camera_init(CameraState *s, int camera_id, int camera_num, unsigned int fps) { + LOGD("camera init %d", camera_num); + + // TODO: this is copied code from camera_webcam + assert(camera_id < ARRAYSIZE(cameras_supported)); + s->ci = cameras_supported[camera_id]; + assert(s->ci.frame_width != 0); + + s->camera_num = camera_num; + s->frame_size = s->ci.frame_height * s->ci.frame_stride; + + tbuffer_init2(&s->camera_tb, FRAME_BUF_COUNT, "frame", camera_release_buffer, s); + + s->transform = (mat3){{ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + }}; + s->digital_gain = 1.0; +} + +static void camera_open(CameraState *s, VisionBuf* b) { + int ret; + s->bufs = b; + + // /dev/v4l-subdev10 is sensor, 11, 12, 13 are the other sensors + switch (s->camera_num) { + case 0: + s->sensor_fd = open("/dev/v4l-subdev10", O_RDWR | O_NONBLOCK); + break; + case 1: + s->sensor_fd = open("/dev/v4l-subdev11", O_RDWR | O_NONBLOCK); + break; + case 2: + s->sensor_fd = open("/dev/v4l-subdev12", O_RDWR | O_NONBLOCK); + break; + } + assert(s->sensor_fd >= 0); + LOGD("opened sensor"); + + // also at /dev/v4l-subdev3, 4, 5, 6 + switch (s->camera_num) { + case 0: + s->csiphy_fd = open("/dev/v4l-subdev3", O_RDWR | O_NONBLOCK); + break; + case 1: + s->csiphy_fd = open("/dev/v4l-subdev4", O_RDWR | O_NONBLOCK); + break; + case 2: + s->csiphy_fd = open("/dev/v4l-subdev5", O_RDWR | O_NONBLOCK); + break; + } + assert(s->csiphy_fd >= 0); + LOGD("opened csiphy"); + + // probe the sensor + LOGD("-- Probing sensor %d", s->camera_num); + sensors_init(s->video0_fd, s->sensor_fd, s->camera_num); + + memset(&s->req_mgr_session_info, 0, sizeof(s->req_mgr_session_info)); + ret = cam_control(s->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info)); + LOGD("get session: %d 0x%X", ret, s->req_mgr_session_info.session_hdl); + s->session_handle = s->req_mgr_session_info.session_hdl; + + // access the sensor + LOGD("-- Accessing sensor"); + static struct cam_acquire_dev_cmd acquire_dev_cmd = {0}; + acquire_dev_cmd.session_handle = s->session_handle; + acquire_dev_cmd.handle_type = CAM_HANDLE_USER_POINTER; + ret = cam_control(s->sensor_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd)); + LOGD("acquire sensor dev: %d", ret); + s->sensor_dev_handle = acquire_dev_cmd.dev_handle; + + static struct cam_isp_resource isp_resource = {0}; + + acquire_dev_cmd.session_handle = s->session_handle; + acquire_dev_cmd.handle_type = CAM_HANDLE_USER_POINTER; + acquire_dev_cmd.num_resources = 1; + acquire_dev_cmd.resource_hdl = (uint64_t)&isp_resource; + + isp_resource.resource_id = CAM_ISP_RES_ID_PORT; + isp_resource.length = sizeof(struct cam_isp_in_port_info) + sizeof(struct cam_isp_out_port_info)*(1-1); + isp_resource.handle_type = CAM_HANDLE_USER_POINTER; + + struct cam_isp_in_port_info *in_port_info = malloc(isp_resource.length); + isp_resource.res_hdl = (uint64_t)in_port_info; + + switch (s->camera_num) { + case 0: + in_port_info->res_type = CAM_ISP_IFE_IN_RES_PHY_0; + break; + case 1: + in_port_info->res_type = CAM_ISP_IFE_IN_RES_PHY_1; + break; + case 2: + in_port_info->res_type = CAM_ISP_IFE_IN_RES_PHY_2; + break; + } + + in_port_info->lane_type = CAM_ISP_LANE_TYPE_DPHY; + in_port_info->lane_num = 4; + in_port_info->lane_cfg = 0x3210; + + in_port_info->vc = 0x0; + //in_port_info->dt = 0x2C; //CSI_RAW12 + //in_port_info->format = CAM_FORMAT_MIPI_RAW_12; + + in_port_info->dt = 0x2B; //CSI_RAW10 + in_port_info->format = CAM_FORMAT_MIPI_RAW_10; + + in_port_info->test_pattern = 0x2; // 0x3? + in_port_info->usage_type = 0x0; + + in_port_info->left_start = 0x0; + in_port_info->left_stop = FRAME_WIDTH - 1; + in_port_info->left_width = FRAME_WIDTH; + + in_port_info->right_start = 0x0; + in_port_info->right_stop = FRAME_WIDTH - 1; + in_port_info->right_width = FRAME_WIDTH; + + in_port_info->line_start = 0x0; + in_port_info->line_stop = FRAME_HEIGHT - 1; + in_port_info->height = FRAME_HEIGHT; + + in_port_info->pixel_clk = 0x0; + in_port_info->batch_size = 0x0; + in_port_info->dsp_mode = 0x0; + in_port_info->hbi_cnt = 0x0; + in_port_info->custom_csid = 0x0; + + in_port_info->num_out_res = 0x1; + in_port_info->data[0] = (struct cam_isp_out_port_info){ + .res_type = CAM_ISP_IFE_OUT_RES_RDI_0, + //.format = CAM_FORMAT_MIPI_RAW_12, + .format = CAM_FORMAT_MIPI_RAW_10, + .width = FRAME_WIDTH, + .height = FRAME_HEIGHT, + .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, + }; + + ret = cam_control(s->isp_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd)); + LOGD("acquire isp dev: %d", ret); + free(in_port_info); + s->isp_dev_handle = acquire_dev_cmd.dev_handle; + + static struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {0}; + csiphy_acquire_dev_info.combo_mode = 0; + + acquire_dev_cmd.session_handle = s->session_handle; + acquire_dev_cmd.handle_type = CAM_HANDLE_USER_POINTER; + acquire_dev_cmd.num_resources = 1; + acquire_dev_cmd.resource_hdl = (uint64_t)&csiphy_acquire_dev_info; + + ret = cam_control(s->csiphy_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd)); + LOGD("acquire csiphy dev: %d", ret); + s->csiphy_dev_handle = acquire_dev_cmd.dev_handle; + + // acquires done + + // config ISP + void *buf0 = alloc_w_mmu_hdl(s->video0_fd, 984480, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &s->buf0_handle, s->device_iommu, s->cdm_iommu); + config_isp(s, 0, 0, 1, s->buf0_handle, 0); + + LOG("-- Configuring sensor"); + sensors_i2c(s, init_array_ar0231, sizeof(init_array_ar0231)/sizeof(struct i2c_random_wr_payload), + CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + sensors_i2c(s, start_reg_array, sizeof(start_reg_array)/sizeof(struct i2c_random_wr_payload), + CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON); + sensors_i2c(s, stop_reg_array, sizeof(stop_reg_array)/sizeof(struct i2c_random_wr_payload), + CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF); + + // config csiphy + LOG("-- Config CSI PHY"); + { + uint32_t cam_packet_handle = 0; + struct cam_packet *pkt = alloc(s->video0_fd, sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1, 8, + CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle); + pkt->num_cmd_buf = 1; + pkt->kmd_cmd_buf_index = -1; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + + buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info); + buf_desc[0].type = CAM_CMD_BUF_GENERIC; + struct cam_csiphy_info *csiphy_info = alloc(s->video0_fd, buf_desc[0].size, 8, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &buf_desc[0].mem_handle); + + csiphy_info->lane_mask = 0x1f; + csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels?? + csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane + csiphy_info->combo_mode = 0x0; + csiphy_info->lane_cnt = 0x4; + csiphy_info->secure_mode = 0x0; + csiphy_info->settle_time = 2800000000; + csiphy_info->data_rate = 44000000; + + static struct cam_config_dev_cmd config_dev_cmd = {}; + config_dev_cmd.session_handle = s->session_handle; + config_dev_cmd.dev_handle = s->csiphy_dev_handle; + config_dev_cmd.offset = 0; + config_dev_cmd.packet_handle = cam_packet_handle; + + int ret = cam_control(s->csiphy_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd)); + assert(ret == 0); + + release(s->video0_fd, buf_desc[0].mem_handle); + release(s->video0_fd, cam_packet_handle); + } + + // link devices + LOG("-- Link devices"); + static struct cam_req_mgr_link_info req_mgr_link_info = {0}; + req_mgr_link_info.session_hdl = s->session_handle; + req_mgr_link_info.num_devices = 2; + req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle; + req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle; + ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info)); + LOGD("link: %d", ret); + s->link_handle = req_mgr_link_info.link_hdl; + + static struct cam_req_mgr_link_control req_mgr_link_control = {0}; + req_mgr_link_control.ops = 0; + req_mgr_link_control.session_hdl = s->session_handle; + req_mgr_link_control.num_links = 1; + req_mgr_link_control.link_hdls[0] = s->link_handle; + ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); + LOGD("link control: %d", ret); + + // start devices + LOG("-- Start devices"); + ret = device_control(s->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle); + LOGD("start isp: %d", ret); + ret = device_control(s->csiphy_fd, CAM_START_DEV, s->session_handle, s->csiphy_dev_handle); + LOGD("start csiphy: %d", ret); + ret = device_control(s->sensor_fd, CAM_START_DEV, s->session_handle, s->sensor_dev_handle); + LOGD("start sensor: %d", ret); + + for (int i = 0; i < FRAME_BUF_COUNT; i++) { + LOG("-- Initting buffer %d", i); + enqueue_buffer(s, i); + } +} + +void cameras_init(DualCameraState *s) { + camera_init(&s->rear, CAMERA_ID_AR0231, 0, 20); + camera_init(&s->wide, CAMERA_ID_AR0231, 1, 20); + camera_init(&s->front, CAMERA_ID_AR0231, 2, 20); +} + +void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front) { + int ret; + + LOG("-- Opening devices"); + // video0 is the target of many ioctls + s->video0_fd = open("/dev/video0", O_RDWR | O_NONBLOCK); + assert(s->video0_fd >= 0); + LOGD("opened video0"); + s->rear.video0_fd = s->front.video0_fd = s->wide.video0_fd = s->video0_fd; + + // video1 is the target of some ioctls + s->video1_fd = open("/dev/video1", O_RDWR | O_NONBLOCK); + assert(s->video1_fd >= 0); + LOGD("opened video1"); + s->rear.video1_fd = s->front.video1_fd = s->wide.video1_fd = s->video1_fd; + + // looks like there's only one of these + s->isp_fd = open("/dev/v4l-subdev1", O_RDWR | O_NONBLOCK); + assert(s->isp_fd >= 0); + LOGD("opened isp"); + s->rear.isp_fd = s->front.isp_fd = s->wide.isp_fd = s->isp_fd; + + // query icp for MMU handles + LOG("-- Query ICP for MMU handles"); + static struct cam_isp_query_cap_cmd isp_query_cap_cmd = {0}; + static struct cam_query_cap_cmd query_cap_cmd = {0}; + query_cap_cmd.handle_type = 1; + query_cap_cmd.caps_handle = (uint64_t)&isp_query_cap_cmd; + query_cap_cmd.size = sizeof(isp_query_cap_cmd); + ret = cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd)); + assert(ret == 0); + LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure); + LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure); + int device_iommu = isp_query_cap_cmd.device_iommu.non_secure; + int cdm_iommu = isp_query_cap_cmd.cdm_iommu.non_secure; + s->rear.device_iommu = s->front.device_iommu = s->wide.device_iommu = device_iommu; + s->rear.cdm_iommu = s->front.cdm_iommu = s->wide.cdm_iommu = cdm_iommu; + + // subscribe + LOG("-- Subscribing"); + static struct v4l2_event_subscription sub = {0}; + sub.type = 0x8000000; + sub.id = 0; + ret = ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub); + LOGD("isp subscribe: %d", ret); + sub.id = 1; + ret = ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub); + LOGD("isp subscribe: %d", ret); + + camera_open(&s->rear, camera_bufs_rear); + //camera_open(&s->front, camera_bufs_front); + // TODO: add bufs for camera wide +} + +static void camera_close(CameraState *s) { + int ret; + + // stop devices + LOG("-- Stop devices"); + ret = device_control(s->sensor_fd, CAM_STOP_DEV, s->session_handle, s->sensor_dev_handle); + LOGD("stop sensor: %d", ret); + ret = device_control(s->isp_fd, CAM_STOP_DEV, s->session_handle, s->isp_dev_handle); + LOGD("stop isp: %d", ret); + ret = device_control(s->csiphy_fd, CAM_STOP_DEV, s->session_handle, s->csiphy_dev_handle); + LOGD("stop csiphy: %d", ret); + + // link control stop + LOG("-- Stop link control"); + static struct cam_req_mgr_link_control req_mgr_link_control = {0}; + req_mgr_link_control.ops = 1; + req_mgr_link_control.session_hdl = s->session_handle; + req_mgr_link_control.num_links = 1; + req_mgr_link_control.link_hdls[0] = s->link_handle; + ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); + LOGD("link control stop: %d", ret); + + // unlink + LOG("-- Unlink"); + static struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0}; + req_mgr_unlink_info.session_hdl = s->session_handle; + req_mgr_unlink_info.link_hdl = s->link_handle; + ret = cam_control(s->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info)); + LOGD("unlink: %d", ret); + + // release devices + LOGD("-- Release devices"); + ret = device_control(s->sensor_fd, CAM_RELEASE_DEV, s->session_handle, s->sensor_dev_handle); + LOGD("release sensor: %d", ret); + ret = device_control(s->isp_fd, CAM_RELEASE_DEV, s->session_handle, s->isp_dev_handle); + LOGD("release isp: %d", ret); + ret = device_control(s->csiphy_fd, CAM_RELEASE_DEV, s->session_handle, s->csiphy_dev_handle); + LOGD("release csiphy: %d", ret); + + ret = cam_control(s->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info)); + LOGD("destroyed session: %d", ret); + + tbuffer_stop(&s->camera_tb); +} + +static void cameras_close(DualCameraState *s) { + camera_close(&s->rear); + //camera_close(&s->front); + //camera_close(&s->wide); +} + +struct video_event_data { + int32_t session_hdl; + int32_t link_hdl; + int32_t frame_id; + int32_t reserved; + uint64_t tv_sec; + uint64_t tv_usec; +}; + +void cameras_run(DualCameraState *s) { + LOG("-- Dequeueing Video events"); + + while (!do_exit) { + struct pollfd fds[2] = {{0}}; + + fds[0].fd = s->video0_fd; + fds[0].events = POLLPRI; + + fds[1].fd = s->video1_fd; + fds[1].events = POLLPRI; + + int ret = poll(fds, ARRAYSIZE(fds), 1000); + if (ret <= 0) { + if (errno == EINTR || errno == EAGAIN) continue; + LOGE("poll failed (%d - %d)", ret, errno); + break; + } + + for (int i=0; i<2; i++) { + if (!fds[i].revents) continue; + static struct v4l2_event ev = {0}; + ret = ioctl(fds[i].fd, VIDIOC_DQEVENT, &ev); + if (ev.type == 0x8000000) { + struct video_event_data *event_data = (struct video_event_data *)ev.u.data; + uint64_t timestamp = (event_data->tv_sec*1000000000ULL + + event_data->tv_usec*1000); + LOGD("video%d dqevent: %d type:0x%x frame_id:%d timestamp: %llu", i, ret, ev.type, event_data->frame_id, timestamp); + + if (event_data->frame_id != 0) { + for (int j = 0; j < FRAME_BUF_COUNT; j++) { + if (s->rear.request_ids[j] == event_data->frame_id) { + // TODO: support more than rear camera + tbuffer_dispatch(&s->rear.camera_tb, j); + break; + } + } + } + } + } + } + + LOG(" ************** STOPPING **************"); + cameras_close(s); +} + +void camera_autoexposure(CameraState *s, float grey_frac) { +} + diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h new file mode 100644 index 0000000000..b15ea8b128 --- /dev/null +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -0,0 +1,86 @@ +#ifndef CAMERA_H +#define CAMERA_H + +#include +#include +#include + +#include "common/mat.h" +#include "common/visionbuf.h" +#include "common/buffering.h" + +#include "camera_common.h" + +#include "media/cam_req_mgr.h" + +#define FRAME_BUF_COUNT 4 +#define METADATA_BUF_COUNT 4 + +#ifdef __cplusplus +extern "C" { +#endif + + +typedef struct CameraState { + CameraInfo ci; + FrameMetadata camera_bufs_metadata[FRAME_BUF_COUNT]; + TBuffer camera_tb; + + int frame_size; + float digital_gain; + mat3 transform; + + int device_iommu; + int cdm_iommu; + + int video0_fd; + int video1_fd; + int isp_fd; + + int sensor_fd; + int csiphy_fd; + + int camera_num; + + VisionBuf *bufs; + + uint32_t session_handle; + + uint32_t sensor_dev_handle; + uint32_t isp_dev_handle; + uint32_t csiphy_dev_handle; + + int32_t link_handle; + + int buf0_handle; + int buf_handle[FRAME_BUF_COUNT]; + int sched_request_id; + int request_ids[FRAME_BUF_COUNT]; + int sync_objs[FRAME_BUF_COUNT]; + + struct cam_req_mgr_session_info req_mgr_session_info; +} CameraState; + +typedef struct DualCameraState { + int device; + + int video0_fd; + int video1_fd; + int isp_fd; + + CameraState rear; + CameraState front; + CameraState wide; +} DualCameraState; + +void cameras_init(DualCameraState *s); +void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front); +void cameras_run(DualCameraState *s); +void camera_autoexposure(CameraState *s, float grey_frac); + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif + diff --git a/selfdrive/camerad/cameras/debayer.cl b/selfdrive/camerad/cameras/debayer.cl index 344ead0356..c9e3716dfd 100644 --- a/selfdrive/camerad/cameras/debayer.cl +++ b/selfdrive/camerad/cameras/debayer.cl @@ -81,17 +81,28 @@ __kernel void debayer10(__global uchar const * const in, float4 p = convert_float4(pint); +#if NEW == 1 + // now it's 0x2a + const float black_level = 42.0f; + // max on black level + p = max(0.0, p - black_level); +#else // 64 is the black level of the sensor, remove // (changed to 56 for HDR) const float black_level = 56.0f; + // TODO: switch to max here? p = (p - black_level); +#endif + +#if NEW == 0 // correct vignetting (no pow function?) // see https://www.eecis.udel.edu/~jye/lab_research/09/JiUp.pdf the A (4th order) const float r = ((oy - RGB_HEIGHT/2)*(oy - RGB_HEIGHT/2) + (ox - RGB_WIDTH/2)*(ox - RGB_WIDTH/2)); const float fake_f = 700.0f; // should be 910, but this fits... const float lil_a = (1.0f + r/(fake_f*fake_f)); p = p * lil_a * lil_a; +#endif // rescale to 1.0 #if HDR @@ -114,8 +125,12 @@ __kernel void debayer10(__global uchar const * const in, float3 c1 = (float3)(p.s0, (p.s1+p.s2)/2.0f, p.s3); #endif +#if NEW + // TODO: new color correction +#else // color correction c1 = color_correct(c1); +#endif #if HDR // srgb gamma isn't right for YUV, so it's disabled for now diff --git a/selfdrive/camerad/cameras/sensor2_i2c.h b/selfdrive/camerad/cameras/sensor2_i2c.h new file mode 100644 index 0000000000..621d28251e --- /dev/null +++ b/selfdrive/camerad/cameras/sensor2_i2c.h @@ -0,0 +1,690 @@ +struct i2c_random_wr_payload start_reg_array[] = {{0x301a, 0x1c}}; +//struct i2c_random_wr_payload stop_reg_array[] = {{0x301a, 0x10d8}}; +struct i2c_random_wr_payload stop_reg_array[] = {{0x301a, 0x18}};; + +struct i2c_random_wr_payload init_array_ar0231[] = { + {0x3092, 0x0C24}, + {0x337A, 0x0C80}, + {0x3520, 0x1288}, + {0x3522, 0x880C}, + {0x3524, 0x0C12}, + {0x352C, 0x1212}, + {0x354A, 0x007F}, + {0x350C, 28}, + {0x3506, 0x3333}, + {0x3508, 0x3333}, + {0x3100, 0x4000}, + {0x3280, 0x0FA0}, + {0x3282, 0x0FA0}, + {0x3284, 0x0FA0}, + {0x3286, 0x0FA0}, + {0x3288, 0x0FA0}, + {0x328A, 0x0FA0}, + {0x328C, 0x0FA0}, + {0x328E, 0x0FA0}, + {0x3290, 0x0FA0}, + {0x3292, 0x0FA0}, + {0x3294, 0x0FA0}, + {0x3296, 0x0FA0}, + {0x3298, 0x0FA0}, + {0x329A, 0x0FA0}, + {0x329C, 0x0FA0}, + {0x329E, 0x0FA0}, + + // SEQ_DATA_PORT? + {0x2512, 0x8000}, + {0x2510, 0x0905}, + {0x2510, 0x3350}, + {0x2510, 0x2004}, + {0x2510, 0x1460}, + {0x2510, 0x1578}, + {0x2510, 0x0901}, + {0x2510, 0x7B24}, + {0x2510, 0xFF24}, + {0x2510, 0xFF24}, + {0x2510, 0xEA24}, + {0x2510, 0x1022}, + {0x2510, 0x2410}, + {0x2510, 0x155A}, + {0x2510, 0x0901}, + {0x2510, 0x1400}, + {0x2510, 0x24FF}, + {0x2510, 0x24FF}, + {0x2510, 0x24EA}, + {0x2510, 0x2324}, + {0x2510, 0x647A}, + {0x2510, 0x2404}, + {0x2510, 0x052C}, + {0x2510, 0x400A}, + {0x2510, 0xFF0A}, + {0x2510, 0xFF0A}, + {0x2510, 0x1008}, + {0x2510, 0x3851}, + {0x2510, 0x1440}, + {0x2510, 0x0004}, + {0x2510, 0x0801}, + {0x2510, 0x0408}, + {0x2510, 0x1180}, + {0x2510, 0x2652}, + {0x2510, 0x1518}, + {0x2510, 0x0906}, + {0x2510, 0x1348}, + {0x2510, 0x1002}, + {0x2510, 0x1016}, + {0x2510, 0x1181}, + {0x2510, 0x1189}, + {0x2510, 0x1056}, + {0x2510, 0x1210}, + {0x2510, 0x0901}, + {0x2510, 0x0D09}, + {0x2510, 0x1413}, + {0x2510, 0x8809}, + {0x2510, 0x2B15}, + {0x2510, 0x8809}, + {0x2510, 0x0311}, + {0x2510, 0xD909}, + {0x2510, 0x1214}, + {0x2510, 0x4109}, + {0x2510, 0x0312}, + {0x2510, 0x1409}, + {0x2510, 0x0110}, + {0x2510, 0xD612}, + {0x2510, 0x1012}, + {0x2510, 0x1212}, + {0x2510, 0x1011}, + {0x2510, 0xDD11}, + {0x2510, 0xD910}, + {0x2510, 0x5609}, + {0x2510, 0x1511}, + {0x2510, 0xDB09}, + {0x2510, 0x1511}, + {0x2510, 0x9B09}, + {0x2510, 0x0F11}, + {0x2510, 0xBB12}, + {0x2510, 0x1A12}, + {0x2510, 0x1014}, + {0x2510, 0x6012}, + {0x2510, 0x5010}, + {0x2510, 0x7610}, + {0x2510, 0xE609}, + {0x2510, 0x0812}, + {0x2510, 0x4012}, + {0x2510, 0x6009}, + {0x2510, 0x290B}, + {0x2510, 0x0904}, + {0x2510, 0x1440}, + {0x2510, 0x0923}, + {0x2510, 0x15C8}, + {0x2510, 0x13C8}, + {0x2510, 0x092C}, + {0x2510, 0x1588}, + {0x2510, 0x1388}, + {0x2510, 0x0C09}, + {0x2510, 0x0C14}, + {0x2510, 0x4109}, + {0x2510, 0x1112}, + {0x2510, 0x6212}, + {0x2510, 0x6011}, + {0x2510, 0xBF11}, + {0x2510, 0xBB10}, + {0x2510, 0x6611}, + {0x2510, 0xFB09}, + {0x2510, 0x3511}, + {0x2510, 0xBB12}, + {0x2510, 0x6312}, + {0x2510, 0x6014}, + {0x2510, 0x0015}, + {0x2510, 0x0011}, + {0x2510, 0xB812}, + {0x2510, 0xA012}, + {0x2510, 0x0010}, + {0x2510, 0x2610}, + {0x2510, 0x0013}, + {0x2510, 0x0011}, + {0x2510, 0x0008}, + {0x2510, 0x3053}, + {0x2510, 0x4215}, + {0x2510, 0x4013}, + {0x2510, 0x4010}, + {0x2510, 0x0210}, + {0x2510, 0x1611}, + {0x2510, 0x8111}, + {0x2510, 0x8910}, + {0x2510, 0x5612}, + {0x2510, 0x1009}, + {0x2510, 0x010D}, + {0x2510, 0x0815}, + {0x2510, 0xC015}, + {0x2510, 0xD013}, + {0x2510, 0x5009}, + {0x2510, 0x1313}, + {0x2510, 0xD009}, + {0x2510, 0x0215}, + {0x2510, 0xC015}, + {0x2510, 0xC813}, + {0x2510, 0xC009}, + {0x2510, 0x0515}, + {0x2510, 0x8813}, + {0x2510, 0x8009}, + {0x2510, 0x0213}, + {0x2510, 0x8809}, + {0x2510, 0x0411}, + {0x2510, 0xC909}, + {0x2510, 0x0814}, + {0x2510, 0x0109}, + {0x2510, 0x0B11}, + {0x2510, 0xD908}, + {0x2510, 0x1400}, + {0x2510, 0x091A}, + {0x2510, 0x1440}, + {0x2510, 0x0903}, + {0x2510, 0x1214}, + {0x2510, 0x0901}, + {0x2510, 0x10D6}, + {0x2510, 0x1210}, + {0x2510, 0x1212}, + {0x2510, 0x1210}, + {0x2510, 0x11DD}, + {0x2510, 0x11D9}, + {0x2510, 0x1056}, + {0x2510, 0x0917}, + {0x2510, 0x11DB}, + {0x2510, 0x0913}, + {0x2510, 0x11FB}, + {0x2510, 0x0905}, + {0x2510, 0x11BB}, + {0x2510, 0x121A}, + {0x2510, 0x1210}, + {0x2510, 0x1460}, + {0x2510, 0x1250}, + {0x2510, 0x1076}, + {0x2510, 0x10E6}, + {0x2510, 0x0901}, + {0x2510, 0x15A8}, + {0x2510, 0x0901}, + {0x2510, 0x13A8}, + {0x2510, 0x1240}, + {0x2510, 0x1260}, + {0x2510, 0x0925}, + {0x2510, 0x13AD}, + {0x2510, 0x0902}, + {0x2510, 0x0907}, + {0x2510, 0x1588}, + {0x2510, 0x0901}, + {0x2510, 0x138D}, + {0x2510, 0x0B09}, + {0x2510, 0x0914}, + {0x2510, 0x4009}, + {0x2510, 0x0B13}, + {0x2510, 0x8809}, + {0x2510, 0x1C0C}, + {0x2510, 0x0920}, + {0x2510, 0x1262}, + {0x2510, 0x1260}, + {0x2510, 0x11BF}, + {0x2510, 0x11BB}, + {0x2510, 0x1066}, + {0x2510, 0x090A}, + {0x2510, 0x11FB}, + {0x2510, 0x093B}, + {0x2510, 0x11BB}, + {0x2510, 0x1263}, + {0x2510, 0x1260}, + {0x2510, 0x1400}, + {0x2510, 0x1508}, + {0x2510, 0x11B8}, + {0x2510, 0x12A0}, + {0x2510, 0x1200}, + {0x2510, 0x1026}, + {0x2510, 0x1000}, + {0x2510, 0x1300}, + {0x2510, 0x1100}, + {0x2510, 0x437A}, + {0x2510, 0x0609}, + {0x2510, 0x0B05}, + {0x2510, 0x0708}, + {0x2510, 0x4137}, + {0x2510, 0x502C}, + {0x2510, 0x2CFE}, + {0x2510, 0x15FE}, + {0x2510, 0x0C2C}, + + // end SEQ_DATA_PORT + {0x32e6,0xe0}, + + // exposure time + {0x1008,0x36f}, + {0x100c,0x58f}, + {0x100e,0x7af}, + {0x1010,0x14f}, + {0x3230,0x312}, + {0x3232,0x532}, + {0x3234,0x752}, + {0x3236,0xf2}, + + {0x3566, 0x0}, + {0x32D0, 0x3A02}, + {0x32D2, 0x3508}, + {0x32D4, 0x3702}, + {0x32D6, 0x3C04}, + {0x32DC, 0x370A}, + {0x30b0, 0x200}, + {0x3082, 0x0}, + {0x33E0, 0x0080}, + {0x3180, 0x0080}, + {0x33E4, 0x0080}, + {0x33E0, 0x0C00}, + {0x33E0, 0x0000}, + + {0x31B4, 0x2185}, + {0x31B6, 0x1146}, + {0x31B8, 0x3047}, + + {0x31BA, 0x186}, + {0x31BC, 0x805}, + + // additions + // mipi + 4 lanes + {0x31AE, 0x0204}, + // DATA_FORMAT_RAW = 12 + // DATA_FORMAT_OUTPUT = 0? + //{0x31AC, 0x0C08}, + {0x31AC, 0x0C0A}, + + // 0x2B = CSI_RAW10 + {0x3342, 0x122B}, + {0x3346, 0x122B}, + {0x334a, 0x122B}, + {0x334e, 0x122B}, + + // 10-bit + {0x3036, 0xA}, +}; + +struct i2c_random_wr_payload poke_array_ov7750[] = { + {0x3208, 0x0}, {0x380e, 0x1a}, {0x380f, 0xf0}, {0x3500, 0x0}, {0x3501, 0x0}, {0x3502, 0x10}, {0x350a, 0x0}, {0x350b, 0x10}, {0x3208, 0x10}, {0x3208, 0xa0}, + //{0x3208, 0x0}, {0x380e, 0x1a}, {0x380f, 0xf0}, {0x3500, 0x0}, {0x3501, 0x0}, {0x3502, 0x10}, {0x350a, 0x0}, {0x350b, 0x10}, {0x3208, 0x10}, {0x3208, 0xa0}, +}; + +struct i2c_random_wr_payload preinit_array_ov7750[] = { + {0x103, 0x1}, + {0x303b, 0x2}, + {0x302b, 0x80}, +}; + +struct i2c_random_wr_payload init_array_ov7750[] = { + // 2nd batch + {0x3005, 0x0}, + {0x3012, 0xc0}, + {0x3013, 0xd2}, + {0x3014, 0x4}, + {0x3016, 0xf0}, + {0x3017, 0xf0}, + {0x3018, 0xf0}, + {0x301a, 0xf0}, + {0x301b, 0xf0}, + {0x301c, 0xf0}, + {0x3023, 0x5}, + {0x3037, 0xf0}, + {0x3098, 0x4}, + {0x3099, 0x28}, + {0x309a, 0x5}, + {0x309b, 0x4}, + {0x30b0, 0xa}, + {0x30b1, 0x1}, + {0x30b3, 0x64}, + {0x30b4, 0x3}, + {0x30b5, 0x5}, + {0x3106, 0xda}, + {0x3500, 0x0}, + {0x3501, 0x1f}, + {0x3502, 0x80}, + {0x3503, 0x7}, + {0x3509, 0x10}, + {0x350b, 0x10}, + {0x3600, 0x1c}, + {0x3602, 0x62}, + {0x3620, 0xb7}, + {0x3622, 0x4}, + {0x3626, 0x21}, + {0x3627, 0x30}, + {0x3630, 0x44}, + {0x3631, 0x35}, + {0x3634, 0x60}, + {0x3636, 0x0}, + {0x3662, 0x1}, + {0x3663, 0x70}, + {0x3664, 0xf0}, + {0x3666, 0xa}, + {0x3669, 0x1a}, + {0x366a, 0x0}, + {0x366b, 0x50}, + {0x3673, 0x1}, + {0x3674, 0xff}, + {0x3675, 0x3}, + {0x3705, 0xc1}, + {0x3709, 0x40}, + {0x373c, 0x8}, + {0x3742, 0x0}, + {0x3757, 0xb3}, + {0x3788, 0x0}, + {0x37a8, 0x1}, + {0x37a9, 0xc0}, + {0x3800, 0x0}, + {0x3801, 0x4}, + {0x3802, 0x0}, + {0x3803, 0x4}, + {0x3804, 0x2}, + {0x3805, 0x8b}, + {0x3806, 0x1}, + {0x3807, 0xeb}, + {0x3808, 0x2}, + {0x3809, 0x80}, + {0x380a, 0x1}, + {0x380b, 0xe0}, + {0x380c, 0x3}, + {0x380d, 0xa0}, + {0x380e, 0x6}, + {0x380f, 0xbc}, + {0x3810, 0x0}, + {0x3811, 0x4}, + {0x3812, 0x0}, + {0x3813, 0x5}, + {0x3814, 0x11}, + {0x3815, 0x11}, + {0x3820, 0x40}, + {0x3821, 0x0}, + {0x382f, 0xe}, + {0x3832, 0x0}, + {0x3833, 0x5}, + {0x3834, 0x0}, + {0x3835, 0xc}, + {0x3837, 0x0}, + {0x3b80, 0x0}, + {0x3b81, 0xa5}, + {0x3b82, 0x10}, + {0x3b83, 0x0}, + {0x3b84, 0x8}, + {0x3b85, 0x0}, + {0x3b86, 0x1}, + {0x3b87, 0x0}, + {0x3b88, 0x0}, + {0x3b89, 0x0}, + {0x3b8a, 0x0}, + {0x3b8b, 0x5}, + {0x3b8c, 0x0}, + {0x3b8d, 0x0}, + {0x3b8e, 0x0}, + {0x3b8f, 0x1a}, + {0x3b94, 0x5}, + {0x3b95, 0xf2}, + {0x3b96, 0x40}, + {0x3c00, 0x89}, + {0x3c01, 0x63}, + {0x3c02, 0x1}, + {0x3c03, 0x0}, + {0x3c04, 0x0}, + {0x3c05, 0x3}, + {0x3c06, 0x0}, + {0x3c07, 0x6}, + {0x3c0c, 0x1}, + {0x3c0d, 0xd0}, + {0x3c0e, 0x2}, + {0x3c0f, 0xa}, + {0x4001, 0x42}, + {0x4004, 0x4}, + {0x4005, 0x0}, + {0x404e, 0x1}, + {0x4300, 0xff}, + {0x4301, 0x0}, + {0x4315, 0x0}, + {0x4501, 0x48}, + {0x4600, 0x0}, + {0x4601, 0x4e}, + {0x4801, 0xf}, + {0x4806, 0xf}, + {0x4819, 0xaa}, + {0x4823, 0x3e}, + {0x4837, 0x19}, + {0x4a0d, 0x0}, + {0x4a47, 0x7f}, + {0x4a49, 0xf0}, + {0x4a4b, 0x30}, + {0x5000, 0x85}, + {0x5001, 0x80}, +}; + +struct i2c_random_wr_payload init_array_ov8856[] = { + // part 1 184 + {0x103, 0x1}, + {0x302, 0x3c}, + {0x303, 0x1}, + {0x31e, 0xc}, + {0x3000, 0x0}, + {0x300e, 0x0}, + {0x3010, 0x0}, + {0x3015, 0x84}, + {0x3018, 0x72}, + {0x3033, 0x24}, + {0x3500, 0x0}, + {0x3501, 0x4c}, + {0x3502, 0xe0}, + {0x3503, 0x8}, + {0x3505, 0x83}, + {0x3508, 0x1}, + {0x3509, 0x80}, + {0x350c, 0x0}, + {0x350d, 0x80}, + {0x350e, 0x4}, + {0x350f, 0x0}, + {0x3510, 0x0}, + {0x3511, 0x2}, + {0x3512, 0x0}, + {0x3600, 0x72}, + {0x3601, 0x40}, + {0x3602, 0x30}, + {0x3610, 0xc5}, + {0x3611, 0x58}, + {0x3612, 0x5c}, + {0x3613, 0x5a}, + {0x3614, 0x60}, + {0x3628, 0xff}, + {0x3629, 0xff}, + {0x362a, 0xff}, + {0x3633, 0x10}, + {0x3634, 0x10}, + {0x3635, 0x10}, + {0x3636, 0x10}, + {0x3663, 0x8}, + {0x3669, 0x34}, + {0x366e, 0x8}, + {0x3706, 0x86}, + {0x370b, 0x7e}, + {0x3714, 0x27}, + {0x3730, 0x12}, + {0x3733, 0x10}, + {0x3764, 0x0}, + {0x3765, 0x0}, + {0x3769, 0x62}, + {0x376a, 0x2a}, + {0x376b, 0x3b}, + {0x3780, 0x0}, + {0x3781, 0x24}, + {0x3782, 0x0}, + {0x3783, 0x23}, + {0x3798, 0x2f}, + {0x37a1, 0x60}, + {0x37a8, 0x6a}, + {0x37ab, 0x3f}, + {0x37c2, 0x14}, + {0x37c3, 0xf1}, + {0x37c9, 0x80}, + {0x37cb, 0x3}, + {0x37cc, 0xa}, + {0x37cd, 0x16}, + {0x37ce, 0x1f}, + {0x3800, 0x0}, + {0x3801, 0x0}, + {0x3802, 0x0}, + {0x3803, 0xc}, + {0x3804, 0xc}, + {0x3805, 0xdf}, + {0x3806, 0x9}, + {0x3807, 0xa3}, + {0x3808, 0x6}, + {0x3809, 0x60}, + {0x380a, 0x4}, + {0x380b, 0xc8}, + {0x380c, 0x7}, + {0x380d, 0x8c}, + {0x380e, 0x9}, + {0x380f, 0xb2}, + {0x3810, 0x0}, + {0x3811, 0x8}, + {0x3812, 0x0}, + {0x3813, 0x2}, + {0x3814, 0x3}, + {0x3815, 0x1}, + {0x3816, 0x0}, + {0x3817, 0x0}, + {0x3818, 0x0}, + {0x3819, 0x0}, + {0x3820, 0x90}, + {0x3821, 0x67}, + {0x382a, 0x3}, + {0x382b, 0x1}, + {0x3830, 0x6}, + {0x3836, 0x2}, + {0x3862, 0x4}, + {0x3863, 0x8}, + {0x3cc0, 0x33}, + {0x3d85, 0x17}, + {0x3d8c, 0x73}, + {0x3d8d, 0xde}, + {0x4001, 0xe0}, + {0x4003, 0x40}, + {0x4008, 0x0}, + {0x4009, 0x5}, + {0x400f, 0x80}, + {0x4010, 0xf0}, + {0x4011, 0xff}, + {0x4012, 0x2}, + {0x4013, 0x1}, + {0x4014, 0x1}, + {0x4015, 0x1}, + {0x4042, 0x0}, + {0x4043, 0x80}, + {0x4044, 0x0}, + {0x4045, 0x80}, + {0x4046, 0x0}, + {0x4047, 0x80}, + {0x4048, 0x0}, + {0x4049, 0x80}, + {0x4041, 0x3}, + {0x404c, 0x20}, + {0x404d, 0x0}, + {0x404e, 0x20}, + {0x4203, 0x80}, + {0x4307, 0x30}, + {0x4317, 0x0}, + {0x4503, 0x8}, + {0x4601, 0x80}, + {0x4816, 0x53}, + {0x481b, 0x58}, + {0x481f, 0x27}, + {0x4837, 0x16}, + {0x5000, 0x77}, + {0x5001, 0xe}, + {0x5004, 0x0}, + {0x502e, 0x0}, + {0x5030, 0x41}, + {0x5795, 0x0}, + {0x5796, 0x10}, + {0x5797, 0x10}, + {0x5798, 0x73}, + {0x5799, 0x73}, + {0x579a, 0x0}, + {0x579b, 0x28}, + {0x579c, 0x0}, + {0x579d, 0x16}, + {0x579e, 0x6}, + {0x579f, 0x20}, + {0x57a0, 0x4}, + {0x57a1, 0xa0}, + {0x5780, 0x14}, + {0x5781, 0xf}, + {0x5782, 0x44}, + {0x5783, 0x2}, + {0x5784, 0x1}, + {0x5785, 0x1}, + {0x5786, 0x0}, + {0x5787, 0x4}, + {0x5788, 0x2}, + {0x5789, 0xf}, + {0x578a, 0xfd}, + {0x578b, 0xf5}, + {0x578c, 0xf5}, + {0x578d, 0x3}, + {0x578e, 0x8}, + {0x578f, 0xc}, + {0x5790, 0x8}, + {0x5791, 0x4}, + {0x5792, 0x0}, + {0x5793, 0x52}, + {0x5794, 0xa3}, + {0x5a08, 0x2}, + {0x5b00, 0x2}, + {0x5b01, 0x10}, + {0x5b02, 0x3}, + {0x5b03, 0xcf}, + {0x5b05, 0x6c}, + {0x5e00, 0x0}, + + // part 2 45 + {0x3501, 0x9a}, + {0x3502, 0x20}, + {0x366d, 0x0}, + {0x366e, 0x10}, + {0x3714, 0x23}, + {0x37c2, 0x4}, + {0x3800, 0x0}, + {0x3801, 0x0}, + {0x3802, 0x0}, + {0x3803, 0xc}, + {0x3804, 0xc}, + {0x3805, 0xdf}, + {0x3806, 0x9}, + {0x3807, 0xa3}, + {0x3808, 0xc}, + {0x3809, 0xc0}, + {0x380a, 0x9}, + {0x380b, 0x90}, + {0x380c, 0x7}, + {0x380d, 0x8c}, + {0x380e, 0x9}, + {0x380f, 0xb2}, + {0x3811, 0x10}, + {0x3813, 0x4}, + {0x3814, 0x1}, + {0x3820, 0xc6}, + {0x3821, 0x40}, + {0x382a, 0x1}, + {0x4009, 0xb}, + {0x4601, 0x80}, + {0x5003, 0xc8}, + {0x5006, 0x0}, + {0x5007, 0x0}, + {0x5795, 0x2}, + {0x5796, 0x20}, + {0x5797, 0x20}, + {0x5798, 0xd5}, + {0x5799, 0xd5}, + {0x579b, 0x50}, + {0x579d, 0x2c}, + {0x579e, 0xc}, + {0x579f, 0x40}, + {0x57a0, 0x9}, + {0x57a1, 0x40}, + {0x5e10, 0xfc}, +}; + diff --git a/selfdrive/camerad/include/media/cam_cpas.h b/selfdrive/camerad/include/media/cam_cpas.h new file mode 100644 index 0000000000..c5cbac82e7 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_cpas.h @@ -0,0 +1,25 @@ +#ifndef __UAPI_CAM_CPAS_H__ +#define __UAPI_CAM_CPAS_H__ + +#include "cam_defs.h" + +#define CAM_FAMILY_CAMERA_SS 1 +#define CAM_FAMILY_CPAS_SS 2 + +/** + * struct cam_cpas_query_cap - CPAS query device capability payload + * + * @camera_family : Camera family type + * @reserved : Reserved field for alignment + * @camera_version : Camera platform version + * @cpas_version : Camera CPAS version within camera platform + * + */ +struct cam_cpas_query_cap { + uint32_t camera_family; + uint32_t reserved; + struct cam_hw_version camera_version; + struct cam_hw_version cpas_version; +}; + +#endif /* __UAPI_CAM_CPAS_H__ */ diff --git a/selfdrive/camerad/include/media/cam_defs.h b/selfdrive/camerad/include/media/cam_defs.h new file mode 100644 index 0000000000..e006463d2a --- /dev/null +++ b/selfdrive/camerad/include/media/cam_defs.h @@ -0,0 +1,477 @@ +#ifndef __UAPI_CAM_DEFS_H__ +#define __UAPI_CAM_DEFS_H__ + +#include +#include +#include + + +/* camera op codes */ +#define CAM_COMMON_OPCODE_BASE 0x100 +#define CAM_QUERY_CAP (CAM_COMMON_OPCODE_BASE + 0x1) +#define CAM_ACQUIRE_DEV (CAM_COMMON_OPCODE_BASE + 0x2) +#define CAM_START_DEV (CAM_COMMON_OPCODE_BASE + 0x3) +#define CAM_STOP_DEV (CAM_COMMON_OPCODE_BASE + 0x4) +#define CAM_CONFIG_DEV (CAM_COMMON_OPCODE_BASE + 0x5) +#define CAM_RELEASE_DEV (CAM_COMMON_OPCODE_BASE + 0x6) +#define CAM_SD_SHUTDOWN (CAM_COMMON_OPCODE_BASE + 0x7) +#define CAM_FLUSH_REQ (CAM_COMMON_OPCODE_BASE + 0x8) +#define CAM_COMMON_OPCODE_MAX (CAM_COMMON_OPCODE_BASE + 0x9) + +#define CAM_EXT_OPCODE_BASE 0x200 +#define CAM_CONFIG_DEV_EXTERNAL (CAM_EXT_OPCODE_BASE + 0x1) + +/* camera handle type */ +#define CAM_HANDLE_USER_POINTER 1 +#define CAM_HANDLE_MEM_HANDLE 2 + +/* Generic Blob CmdBuffer header properties */ +#define CAM_GENERIC_BLOB_CMDBUFFER_SIZE_MASK 0xFFFFFF00 +#define CAM_GENERIC_BLOB_CMDBUFFER_SIZE_SHIFT 8 +#define CAM_GENERIC_BLOB_CMDBUFFER_TYPE_MASK 0xFF +#define CAM_GENERIC_BLOB_CMDBUFFER_TYPE_SHIFT 0 + +/* Command Buffer Types */ +#define CAM_CMD_BUF_DMI 0x1 +#define CAM_CMD_BUF_DMI16 0x2 +#define CAM_CMD_BUF_DMI32 0x3 +#define CAM_CMD_BUF_DMI64 0x4 +#define CAM_CMD_BUF_DIRECT 0x5 +#define CAM_CMD_BUF_INDIRECT 0x6 +#define CAM_CMD_BUF_I2C 0x7 +#define CAM_CMD_BUF_FW 0x8 +#define CAM_CMD_BUF_GENERIC 0x9 +#define CAM_CMD_BUF_LEGACY 0xA + +/** + * enum flush_type_t - Identifies the various flush types + * + * @CAM_FLUSH_TYPE_REQ: Flush specific request + * @CAM_FLUSH_TYPE_ALL: Flush all requests belonging to a context + * @CAM_FLUSH_TYPE_MAX: Max enum to validate flush type + * + */ +enum flush_type_t { + CAM_FLUSH_TYPE_REQ, + CAM_FLUSH_TYPE_ALL, + CAM_FLUSH_TYPE_MAX +}; + +/** + * struct cam_control - Structure used by ioctl control for camera + * + * @op_code: This is the op code for camera control + * @size: Control command size + * @handle_type: User pointer or shared memory handle + * @reserved: Reserved field for 64 bit alignment + * @handle: Control command payload + */ +struct cam_control { + uint32_t op_code; + uint32_t size; + uint32_t handle_type; + uint32_t reserved; + uint64_t handle; +}; + +/* camera IOCTL */ +#define VIDIOC_CAM_CONTROL \ + _IOWR('V', BASE_VIDIOC_PRIVATE, struct cam_control) + +/** + * struct cam_hw_version - Structure for HW version of camera devices + * + * @major : Hardware version major + * @minor : Hardware version minor + * @incr : Hardware version increment + * @reserved : Reserved for 64 bit aligngment + */ +struct cam_hw_version { + uint32_t major; + uint32_t minor; + uint32_t incr; + uint32_t reserved; +}; + +/** + * struct cam_iommu_handle - Structure for IOMMU handles of camera hw devices + * + * @non_secure: Device Non Secure IOMMU handle + * @secure: Device Secure IOMMU handle + * + */ +struct cam_iommu_handle { + int32_t non_secure; + int32_t secure; +}; + +/* camera secure mode */ +#define CAM_SECURE_MODE_NON_SECURE 0 +#define CAM_SECURE_MODE_SECURE 1 + +/* Camera Format Type */ +#define CAM_FORMAT_BASE 0 +#define CAM_FORMAT_MIPI_RAW_6 1 +#define CAM_FORMAT_MIPI_RAW_8 2 +#define CAM_FORMAT_MIPI_RAW_10 3 +#define CAM_FORMAT_MIPI_RAW_12 4 +#define CAM_FORMAT_MIPI_RAW_14 5 +#define CAM_FORMAT_MIPI_RAW_16 6 +#define CAM_FORMAT_MIPI_RAW_20 7 +#define CAM_FORMAT_QTI_RAW_8 8 +#define CAM_FORMAT_QTI_RAW_10 9 +#define CAM_FORMAT_QTI_RAW_12 10 +#define CAM_FORMAT_QTI_RAW_14 11 +#define CAM_FORMAT_PLAIN8 12 +#define CAM_FORMAT_PLAIN16_8 13 +#define CAM_FORMAT_PLAIN16_10 14 +#define CAM_FORMAT_PLAIN16_12 15 +#define CAM_FORMAT_PLAIN16_14 16 +#define CAM_FORMAT_PLAIN16_16 17 +#define CAM_FORMAT_PLAIN32_20 18 +#define CAM_FORMAT_PLAIN64 19 +#define CAM_FORMAT_PLAIN128 20 +#define CAM_FORMAT_ARGB 21 +#define CAM_FORMAT_ARGB_10 22 +#define CAM_FORMAT_ARGB_12 23 +#define CAM_FORMAT_ARGB_14 24 +#define CAM_FORMAT_DPCM_10_6_10 25 +#define CAM_FORMAT_DPCM_10_8_10 26 +#define CAM_FORMAT_DPCM_12_6_12 27 +#define CAM_FORMAT_DPCM_12_8_12 28 +#define CAM_FORMAT_DPCM_14_8_14 29 +#define CAM_FORMAT_DPCM_14_10_14 30 +#define CAM_FORMAT_NV21 31 +#define CAM_FORMAT_NV12 32 +#define CAM_FORMAT_TP10 33 +#define CAM_FORMAT_YUV422 34 +#define CAM_FORMAT_PD8 35 +#define CAM_FORMAT_PD10 36 +#define CAM_FORMAT_UBWC_NV12 37 +#define CAM_FORMAT_UBWC_NV12_4R 38 +#define CAM_FORMAT_UBWC_TP10 39 +#define CAM_FORMAT_UBWC_P010 40 +#define CAM_FORMAT_PLAIN8_SWAP 41 +#define CAM_FORMAT_PLAIN8_10 42 +#define CAM_FORMAT_PLAIN8_10_SWAP 43 +#define CAM_FORMAT_YV12 44 +#define CAM_FORMAT_Y_ONLY 45 +#define CAM_FORMAT_MAX 46 + +/* camera rotaion */ +#define CAM_ROTATE_CW_0_DEGREE 0 +#define CAM_ROTATE_CW_90_DEGREE 1 +#define CAM_RORATE_CW_180_DEGREE 2 +#define CAM_ROTATE_CW_270_DEGREE 3 + +/* camera Color Space */ +#define CAM_COLOR_SPACE_BASE 0 +#define CAM_COLOR_SPACE_BT601_FULL 1 +#define CAM_COLOR_SPACE_BT601625 2 +#define CAM_COLOR_SPACE_BT601525 3 +#define CAM_COLOR_SPACE_BT709 4 +#define CAM_COLOR_SPACE_DEPTH 5 +#define CAM_COLOR_SPACE_MAX 6 + +/* camera buffer direction */ +#define CAM_BUF_INPUT 1 +#define CAM_BUF_OUTPUT 2 +#define CAM_BUF_IN_OUT 3 + +/* camera packet device Type */ +#define CAM_PACKET_DEV_BASE 0 +#define CAM_PACKET_DEV_IMG_SENSOR 1 +#define CAM_PACKET_DEV_ACTUATOR 2 +#define CAM_PACKET_DEV_COMPANION 3 +#define CAM_PACKET_DEV_EEPOM 4 +#define CAM_PACKET_DEV_CSIPHY 5 +#define CAM_PACKET_DEV_OIS 6 +#define CAM_PACKET_DEV_FLASH 7 +#define CAM_PACKET_DEV_FD 8 +#define CAM_PACKET_DEV_JPEG_ENC 9 +#define CAM_PACKET_DEV_JPEG_DEC 10 +#define CAM_PACKET_DEV_VFE 11 +#define CAM_PACKET_DEV_CPP 12 +#define CAM_PACKET_DEV_CSID 13 +#define CAM_PACKET_DEV_ISPIF 14 +#define CAM_PACKET_DEV_IFE 15 +#define CAM_PACKET_DEV_ICP 16 +#define CAM_PACKET_DEV_LRME 17 +#define CAM_PACKET_DEV_MAX 18 + + +/* constants */ +#define CAM_PACKET_MAX_PLANES 3 + +/** + * struct cam_plane_cfg - Plane configuration info + * + * @width: Plane width in pixels + * @height: Plane height in lines + * @plane_stride: Plane stride in pixel + * @slice_height: Slice height in line (not used by ISP) + * @meta_stride: UBWC metadata stride + * @meta_size: UBWC metadata plane size + * @meta_offset: UBWC metadata offset + * @packer_config: UBWC packer config + * @mode_config: UBWC mode config + * @tile_config: UBWC tile config + * @h_init: UBWC horizontal initial coordinate in pixels + * @v_init: UBWC vertical initial coordinate in lines + * + */ +struct cam_plane_cfg { + uint32_t width; + uint32_t height; + uint32_t plane_stride; + uint32_t slice_height; + uint32_t meta_stride; + uint32_t meta_size; + uint32_t meta_offset; + uint32_t packer_config; + uint32_t mode_config; + uint32_t tile_config; + uint32_t h_init; + uint32_t v_init; +}; + +/** + * struct cam_cmd_buf_desc - Command buffer descriptor + * + * @mem_handle: Command buffer handle + * @offset: Command start offset + * @size: Size of the command buffer in bytes + * @length: Used memory in command buffer in bytes + * @type: Type of the command buffer + * @meta_data: Data type for private command buffer + * Between UMD and KMD + * + */ +struct cam_cmd_buf_desc { + int32_t mem_handle; + uint32_t offset; + uint32_t size; + uint32_t length; + uint32_t type; + uint32_t meta_data; +}; + +/** + * struct cam_buf_io_cfg - Buffer io configuration for buffers + * + * @mem_handle: Mem_handle array for the buffers. + * @offsets: Offsets for each planes in the buffer + * @planes: Per plane information + * @width: Main plane width in pixel + * @height: Main plane height in lines + * @format: Format of the buffer + * @color_space: Color space for the buffer + * @color_pattern: Color pattern in the buffer + * @bpp: Bit per pixel + * @rotation: Rotation information for the buffer + * @resource_type: Resource type associated with the buffer + * @fence: Fence handle + * @early_fence: Fence handle for early signal + * @aux_cmd_buf: An auxiliary command buffer that may be + * used for programming the IO + * @direction: Direction of the config + * @batch_size: Batch size in HFR mode + * @subsample_pattern: Subsample pattern. Used in HFR mode. It + * should be consistent with batchSize and + * CAMIF programming. + * @subsample_period: Subsample period. Used in HFR mode. It + * should be consistent with batchSize and + * CAMIF programming. + * @framedrop_pattern: Framedrop pattern + * @framedrop_period: Framedrop period + * @flag: Flags for extra information + * @direction: Buffer direction: input or output + * @padding: Padding for the structure + * + */ +struct cam_buf_io_cfg { + int32_t mem_handle[CAM_PACKET_MAX_PLANES]; + uint32_t offsets[CAM_PACKET_MAX_PLANES]; + struct cam_plane_cfg planes[CAM_PACKET_MAX_PLANES]; + uint32_t format; + uint32_t color_space; + uint32_t color_pattern; + uint32_t bpp; + uint32_t rotation; + uint32_t resource_type; + int32_t fence; + int32_t early_fence; + struct cam_cmd_buf_desc aux_cmd_buf; + uint32_t direction; + uint32_t batch_size; + uint32_t subsample_pattern; + uint32_t subsample_period; + uint32_t framedrop_pattern; + uint32_t framedrop_period; + uint32_t flag; + uint32_t padding; +}; + +/** + * struct cam_packet_header - Camera packet header + * + * @op_code: Camera packet opcode + * @size: Size of the camera packet in bytes + * @request_id: Request id for this camera packet + * @flags: Flags for the camera packet + * @padding: Padding + * + */ +struct cam_packet_header { + uint32_t op_code; + uint32_t size; + uint64_t request_id; + uint32_t flags; + uint32_t padding; +}; + +/** + * struct cam_patch_desc - Patch structure + * + * @dst_buf_hdl: Memory handle for the dest buffer + * @dst_offset: Offset byte in the dest buffer + * @src_buf_hdl: Memory handle for the source buffer + * @src_offset: Offset byte in the source buffer + * + */ +struct cam_patch_desc { + int32_t dst_buf_hdl; + uint32_t dst_offset; + int32_t src_buf_hdl; + uint32_t src_offset; +}; + +/** + * struct cam_packet - Camera packet structure + * + * @header: Camera packet header + * @cmd_buf_offset: Command buffer start offset + * @num_cmd_buf: Number of the command buffer in the packet + * @io_config_offset: Buffer io configuration start offset + * @num_io_configs: Number of the buffer io configurations + * @patch_offset: Patch offset for the patch structure + * @num_patches: Number of the patch structure + * @kmd_cmd_buf_index: Command buffer index which contains extra + * space for the KMD buffer + * @kmd_cmd_buf_offset: Offset from the beginning of the command + * buffer for KMD usage. + * @payload: Camera packet payload + * + */ +struct cam_packet { + struct cam_packet_header header; + uint32_t cmd_buf_offset; + uint32_t num_cmd_buf; + uint32_t io_configs_offset; + uint32_t num_io_configs; + uint32_t patch_offset; + uint32_t num_patches; + uint32_t kmd_cmd_buf_index; + uint32_t kmd_cmd_buf_offset; + uint64_t payload[1]; + +}; + +/** + * struct cam_release_dev_cmd - Control payload for release devices + * + * @session_handle: Session handle for the release + * @dev_handle: Device handle for the release + */ +struct cam_release_dev_cmd { + int32_t session_handle; + int32_t dev_handle; +}; + +/** + * struct cam_start_stop_dev_cmd - Control payload for start/stop device + * + * @session_handle: Session handle for the start/stop command + * @dev_handle: Device handle for the start/stop command + * + */ +struct cam_start_stop_dev_cmd { + int32_t session_handle; + int32_t dev_handle; +}; + +/** + * struct cam_config_dev_cmd - Command payload for configure device + * + * @session_handle: Session handle for the command + * @dev_handle: Device handle for the command + * @offset: Offset byte in the packet handle. + * @packet_handle: Packet memory handle for the actual packet: + * struct cam_packet. + * + */ +struct cam_config_dev_cmd { + int32_t session_handle; + int32_t dev_handle; + uint64_t offset; + uint64_t packet_handle; +}; + +/** + * struct cam_query_cap_cmd - Payload for query device capability + * + * @size: Handle size + * @handle_type: User pointer or shared memory handle + * @caps_handle: Device specific query command payload + * + */ +struct cam_query_cap_cmd { + uint32_t size; + uint32_t handle_type; + uint64_t caps_handle; +}; + +/** + * struct cam_acquire_dev_cmd - Control payload for acquire devices + * + * @session_handle: Session handle for the acquire command + * @dev_handle: Device handle to be returned + * @handle_type: Resource handle type: + * 1 = user pointer, 2 = mem handle + * @num_resources: Number of the resources to be acquired + * @resources_hdl: Resource handle that refers to the actual + * resource array. Each item in this + * array is device specific resource structure + * + */ +struct cam_acquire_dev_cmd { + int32_t session_handle; + int32_t dev_handle; + uint32_t handle_type; + uint32_t num_resources; + uint64_t resource_hdl; +}; + +/** + * struct cam_flush_dev_cmd - Control payload for flush devices + * + * @version: Version + * @session_handle: Session handle for the acquire command + * @dev_handle: Device handle to be returned + * @flush_type: Flush type: + * 0 = flush specific request + * 1 = flush all + * @reserved: Reserved for 64 bit aligngment + * @req_id: Request id that needs to cancel + * + */ +struct cam_flush_dev_cmd { + uint64_t version; + int32_t session_handle; + int32_t dev_handle; + uint32_t flush_type; + uint32_t reserved; + int64_t req_id; +}; + +#endif /* __UAPI_CAM_DEFS_H__ */ diff --git a/selfdrive/camerad/include/media/cam_fd.h b/selfdrive/camerad/include/media/cam_fd.h new file mode 100644 index 0000000000..8feb6e4da8 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_fd.h @@ -0,0 +1,127 @@ +#ifndef __UAPI_CAM_FD_H__ +#define __UAPI_CAM_FD_H__ + +#include "cam_defs.h" + +#define CAM_FD_MAX_FACES 35 +#define CAM_FD_RAW_RESULT_ENTRIES 512 + +/* FD Op Codes */ +#define CAM_PACKET_OPCODES_FD_FRAME_UPDATE 0x0 + +/* FD Command Buffer identifiers */ +#define CAM_FD_CMD_BUFFER_ID_GENERIC 0x0 +#define CAM_FD_CMD_BUFFER_ID_CDM 0x1 +#define CAM_FD_CMD_BUFFER_ID_MAX 0x2 + +/* FD Blob types */ +#define CAM_FD_BLOB_TYPE_SOC_CLOCK_BW_REQUEST 0x0 +#define CAM_FD_BLOB_TYPE_RAW_RESULTS_REQUIRED 0x1 + +/* FD Resource IDs */ +#define CAM_FD_INPUT_PORT_ID_IMAGE 0x0 +#define CAM_FD_INPUT_PORT_ID_MAX 0x1 + +#define CAM_FD_OUTPUT_PORT_ID_RESULTS 0x0 +#define CAM_FD_OUTPUT_PORT_ID_RAW_RESULTS 0x1 +#define CAM_FD_OUTPUT_PORT_ID_WORK_BUFFER 0x2 +#define CAM_FD_OUTPUT_PORT_ID_MAX 0x3 + +/** + * struct cam_fd_soc_clock_bw_request - SOC clock, bandwidth request info + * + * @clock_rate : Clock rate required while processing frame + * @bandwidth : Bandwidth required while processing frame + * @reserved : Reserved for future use + */ +struct cam_fd_soc_clock_bw_request { + uint64_t clock_rate; + uint64_t bandwidth; + uint64_t reserved[4]; +}; + +/** + * struct cam_fd_face - Face properties + * + * @prop1 : Property 1 of face + * @prop2 : Property 2 of face + * @prop3 : Property 3 of face + * @prop4 : Property 4 of face + * + * Do not change this layout, this is inline with how HW writes + * these values directly when the buffer is programmed to HW + */ +struct cam_fd_face { + uint32_t prop1; + uint32_t prop2; + uint32_t prop3; + uint32_t prop4; +}; + +/** + * struct cam_fd_results - FD results layout + * + * @faces : Array of faces with face properties + * @face_count : Number of faces detected + * @reserved : Reserved for alignment + * + * Do not change this layout, this is inline with how HW writes + * these values directly when the buffer is programmed to HW + */ +struct cam_fd_results { + struct cam_fd_face faces[CAM_FD_MAX_FACES]; + uint32_t face_count; + uint32_t reserved[3]; +}; + +/** + * struct cam_fd_hw_caps - Face properties + * + * @core_version : FD core version + * @wrapper_version : FD wrapper version + * @raw_results_available : Whether raw results are available on this HW + * @supported_modes : Modes supported by this HW. + * @reserved : Reserved for future use + */ +struct cam_fd_hw_caps { + struct cam_hw_version core_version; + struct cam_hw_version wrapper_version; + uint32_t raw_results_available; + uint32_t supported_modes; + uint64_t reserved; +}; + +/** + * struct cam_fd_query_cap_cmd - FD Query capabilities information + * + * @device_iommu : FD IOMMU handles + * @cdm_iommu : CDM iommu handles + * @hw_caps : FD HW capabilities + * @reserved : Reserved for alignment + */ +struct cam_fd_query_cap_cmd { + struct cam_iommu_handle device_iommu; + struct cam_iommu_handle cdm_iommu; + struct cam_fd_hw_caps hw_caps; + uint64_t reserved; +}; + +/** + * struct cam_fd_acquire_dev_info - FD acquire device information + * + * @clk_bw_request : SOC clock, bandwidth request + * @priority : Priority for this acquire + * @mode : Mode in which to run FD HW. + * @get_raw_results : Whether this acquire needs face raw results + * while frame processing + * @reserved : Reserved field for 64 bit alignment + */ +struct cam_fd_acquire_dev_info { + struct cam_fd_soc_clock_bw_request clk_bw_request; + uint32_t priority; + uint32_t mode; + uint32_t get_raw_results; + uint32_t reserved[13]; +}; + +#endif /* __UAPI_CAM_FD_H__ */ diff --git a/selfdrive/camerad/include/media/cam_icp.h b/selfdrive/camerad/include/media/cam_icp.h new file mode 100644 index 0000000000..680d05b630 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_icp.h @@ -0,0 +1,179 @@ +#ifndef __UAPI_CAM_ICP_H__ +#define __UAPI_CAM_ICP_H__ + +#include "cam_defs.h" + +/* icp, ipe, bps, cdm(ipe/bps) are used in querycap */ +#define CAM_ICP_DEV_TYPE_A5 1 +#define CAM_ICP_DEV_TYPE_IPE 2 +#define CAM_ICP_DEV_TYPE_BPS 3 +#define CAM_ICP_DEV_TYPE_IPE_CDM 4 +#define CAM_ICP_DEV_TYPE_BPS_CDM 5 +#define CAM_ICP_DEV_TYPE_MAX 5 + +/* definitions needed for icp aquire device */ +#define CAM_ICP_RES_TYPE_BPS 1 +#define CAM_ICP_RES_TYPE_IPE_RT 2 +#define CAM_ICP_RES_TYPE_IPE 3 +#define CAM_ICP_RES_TYPE_MAX 4 + +/* packet opcode types */ +#define CAM_ICP_OPCODE_IPE_UPDATE 0 +#define CAM_ICP_OPCODE_BPS_UPDATE 1 + +/* IPE input port resource type */ +#define CAM_ICP_IPE_INPUT_IMAGE_FULL 0x0 +#define CAM_ICP_IPE_INPUT_IMAGE_DS4 0x1 +#define CAM_ICP_IPE_INPUT_IMAGE_DS16 0x2 +#define CAM_ICP_IPE_INPUT_IMAGE_DS64 0x3 +#define CAM_ICP_IPE_INPUT_IMAGE_FULL_REF 0x4 +#define CAM_ICP_IPE_INPUT_IMAGE_DS4_REF 0x5 +#define CAM_ICP_IPE_INPUT_IMAGE_DS16_REF 0x6 +#define CAM_ICP_IPE_INPUT_IMAGE_DS64_REF 0x7 + +/* IPE output port resource type */ +#define CAM_ICP_IPE_OUTPUT_IMAGE_DISPLAY 0x8 +#define CAM_ICP_IPE_OUTPUT_IMAGE_VIDEO 0x9 +#define CAM_ICP_IPE_OUTPUT_IMAGE_FULL_REF 0xA +#define CAM_ICP_IPE_OUTPUT_IMAGE_DS4_REF 0xB +#define CAM_ICP_IPE_OUTPUT_IMAGE_DS16_REF 0xC +#define CAM_ICP_IPE_OUTPUT_IMAGE_DS64_REF 0xD + +#define CAM_ICP_IPE_IMAGE_MAX 0xE + +/* BPS input port resource type */ +#define CAM_ICP_BPS_INPUT_IMAGE 0x0 + +/* BPS output port resource type */ +#define CAM_ICP_BPS_OUTPUT_IMAGE_FULL 0x1 +#define CAM_ICP_BPS_OUTPUT_IMAGE_DS4 0x2 +#define CAM_ICP_BPS_OUTPUT_IMAGE_DS16 0x3 +#define CAM_ICP_BPS_OUTPUT_IMAGE_DS64 0x4 +#define CAM_ICP_BPS_OUTPUT_IMAGE_STATS_BG 0x5 +#define CAM_ICP_BPS_OUTPUT_IMAGE_STATS_BHIST 0x6 +#define CAM_ICP_BPS_OUTPUT_IMAGE_REG1 0x7 +#define CAM_ICP_BPS_OUTPUT_IMAGE_REG2 0x8 + +#define CAM_ICP_BPS_IO_IMAGES_MAX 0x9 + +/* Command meta types */ +#define CAM_ICP_CMD_META_GENERIC_BLOB 0x1 + +/* Generic blob types */ +#define CAM_ICP_CMD_GENERIC_BLOB_CLK 0x1 +#define CAM_ICP_CMD_GENERIC_BLOB_CFG_IO 0x2 + +/** + * struct cam_icp_clk_bw_request + * + * @budget_ns: Time required to process frame + * @frame_cycles: Frame cycles needed to process the frame + * @rt_flag: Flag to indicate real time stream + * @uncompressed_bw: Bandwidth required to process frame + * @compressed_bw: Compressed bandwidth to process frame + */ +struct cam_icp_clk_bw_request { + uint64_t budget_ns; + uint32_t frame_cycles; + uint32_t rt_flag; + uint64_t uncompressed_bw; + uint64_t compressed_bw; +}; + +/** + * struct cam_icp_dev_ver - Device information for particular hw type + * + * This is used to get device version info of + * ICP, IPE, BPS and CDM related IPE and BPS from firmware + * and use this info in CAM_QUERY_CAP IOCTL + * + * @dev_type: hardware type for the cap info(icp, ipe, bps, cdm(ipe/bps)) + * @reserved: reserved field + * @hw_ver: major, minor and incr values of a device version + */ +struct cam_icp_dev_ver { + uint32_t dev_type; + uint32_t reserved; + struct cam_hw_version hw_ver; +}; + +/** + * struct cam_icp_ver - ICP version info + * + * This strcuture is used for fw and api version + * this is used to get firmware version and api version from firmware + * and use this info in CAM_QUERY_CAP IOCTL + * + * @major: FW version major + * @minor: FW version minor + * @revision: FW version increment + */ +struct cam_icp_ver { + uint32_t major; + uint32_t minor; + uint32_t revision; + uint32_t reserved; +}; + +/** + * struct cam_icp_query_cap_cmd - ICP query device capability payload + * + * @dev_iommu_handle: icp iommu handles for secure/non secure modes + * @cdm_iommu_handle: iommu handles for secure/non secure modes + * @fw_version: firmware version info + * @api_version: api version info + * @num_ipe: number of ipes + * @num_bps: number of bps + * @dev_ver: returned device capability array + */ +struct cam_icp_query_cap_cmd { + struct cam_iommu_handle dev_iommu_handle; + struct cam_iommu_handle cdm_iommu_handle; + struct cam_icp_ver fw_version; + struct cam_icp_ver api_version; + uint32_t num_ipe; + uint32_t num_bps; + struct cam_icp_dev_ver dev_ver[CAM_ICP_DEV_TYPE_MAX]; +}; + +/** + * struct cam_icp_res_info - ICP output resource info + * + * @format: format of the resource + * @width: width in pixels + * @height: height in lines + * @fps: fps + */ +struct cam_icp_res_info { + uint32_t format; + uint32_t width; + uint32_t height; + uint32_t fps; +}; + +/** + * struct cam_icp_acquire_dev_info - An ICP device info + * + * @scratch_mem_size: Output param - size of scratch memory + * @dev_type: device type (IPE_RT/IPE_NON_RT/BPS) + * @io_config_cmd_size: size of IO config command + * @io_config_cmd_handle: IO config command for each acquire + * @secure_mode: camera mode (secure/non secure) + * @chain_info: chaining info of FW device handles + * @in_res: resource info used for clock and bandwidth calculation + * @num_out_res: number of output resources + * @out_res: output resource + */ +struct cam_icp_acquire_dev_info { + uint32_t scratch_mem_size; + uint32_t dev_type; + uint32_t io_config_cmd_size; + int32_t io_config_cmd_handle; + uint32_t secure_mode; + int32_t chain_info; + struct cam_icp_res_info in_res; + uint32_t num_out_res; + struct cam_icp_res_info out_res[1]; +} __attribute__((__packed__)); + +#endif /* __UAPI_CAM_ICP_H__ */ diff --git a/selfdrive/camerad/include/media/cam_isp.h b/selfdrive/camerad/include/media/cam_isp.h new file mode 100644 index 0000000000..266840d38c --- /dev/null +++ b/selfdrive/camerad/include/media/cam_isp.h @@ -0,0 +1,379 @@ +#ifndef __UAPI_CAM_ISP_H__ +#define __UAPI_CAM_ISP_H__ + +#include "cam_defs.h" +#include "cam_isp_vfe.h" +#include "cam_isp_ife.h" + + +/* ISP driver name */ +#define CAM_ISP_DEV_NAME "cam-isp" + +/* HW type */ +#define CAM_ISP_HW_BASE 0 +#define CAM_ISP_HW_CSID 1 +#define CAM_ISP_HW_VFE 2 +#define CAM_ISP_HW_IFE 3 +#define CAM_ISP_HW_ISPIF 4 +#define CAM_ISP_HW_MAX 5 + +/* Color Pattern */ +#define CAM_ISP_PATTERN_BAYER_RGRGRG 0 +#define CAM_ISP_PATTERN_BAYER_GRGRGR 1 +#define CAM_ISP_PATTERN_BAYER_BGBGBG 2 +#define CAM_ISP_PATTERN_BAYER_GBGBGB 3 +#define CAM_ISP_PATTERN_YUV_YCBYCR 4 +#define CAM_ISP_PATTERN_YUV_YCRYCB 5 +#define CAM_ISP_PATTERN_YUV_CBYCRY 6 +#define CAM_ISP_PATTERN_YUV_CRYCBY 7 +#define CAM_ISP_PATTERN_MAX 8 + +/* Usage Type */ +#define CAM_ISP_RES_USAGE_SINGLE 0 +#define CAM_ISP_RES_USAGE_DUAL 1 +#define CAM_ISP_RES_USAGE_MAX 2 + +/* Resource ID */ +#define CAM_ISP_RES_ID_PORT 0 +#define CAM_ISP_RES_ID_CLK 1 +#define CAM_ISP_RES_ID_MAX 2 + +/* Resource Type - Type of resource for the resource id + * defined in cam_isp_vfe.h, cam_isp_ife.h + */ + +/* Lane Type in input resource for Port */ +#define CAM_ISP_LANE_TYPE_DPHY 0 +#define CAM_ISP_LANE_TYPE_CPHY 1 +#define CAM_ISP_LANE_TYPE_MAX 2 + +/* ISP Resurce Composite Group ID */ +#define CAM_ISP_RES_COMP_GROUP_NONE 0 +#define CAM_ISP_RES_COMP_GROUP_ID_0 1 +#define CAM_ISP_RES_COMP_GROUP_ID_1 2 +#define CAM_ISP_RES_COMP_GROUP_ID_2 3 +#define CAM_ISP_RES_COMP_GROUP_ID_3 4 +#define CAM_ISP_RES_COMP_GROUP_ID_4 5 +#define CAM_ISP_RES_COMP_GROUP_ID_5 6 +#define CAM_ISP_RES_COMP_GROUP_ID_MAX 6 + +/* ISP packet opcode for ISP */ +#define CAM_ISP_PACKET_OP_BASE 0 +#define CAM_ISP_PACKET_INIT_DEV 1 +#define CAM_ISP_PACKET_UPDATE_DEV 2 +#define CAM_ISP_PACKET_OP_MAX 3 + +/* ISP packet meta_data type for command buffer */ +#define CAM_ISP_PACKET_META_BASE 0 +#define CAM_ISP_PACKET_META_LEFT 1 +#define CAM_ISP_PACKET_META_RIGHT 2 +#define CAM_ISP_PACKET_META_COMMON 3 +#define CAM_ISP_PACKET_META_DMI_LEFT 4 +#define CAM_ISP_PACKET_META_DMI_RIGHT 5 +#define CAM_ISP_PACKET_META_DMI_COMMON 6 +#define CAM_ISP_PACKET_META_CLOCK 7 +#define CAM_ISP_PACKET_META_CSID 8 +#define CAM_ISP_PACKET_META_DUAL_CONFIG 9 +#define CAM_ISP_PACKET_META_GENERIC_BLOB_LEFT 10 +#define CAM_ISP_PACKET_META_GENERIC_BLOB_RIGHT 11 +#define CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON 12 + +/* DSP mode */ +#define CAM_ISP_DSP_MODE_NONE 0 +#define CAM_ISP_DSP_MODE_ONE_WAY 1 +#define CAM_ISP_DSP_MODE_ROUND 2 + +/* ISP Generic Cmd Buffer Blob types */ +#define CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG 0 +#define CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG 1 +#define CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG 2 + +/* Query devices */ +/** + * struct cam_isp_dev_cap_info - A cap info for particular hw type + * + * @hw_type: Hardware type for the cap info + * @reserved: reserved field for alignment + * @hw_version: Hardware version + * + */ +struct cam_isp_dev_cap_info { + uint32_t hw_type; + uint32_t reserved; + struct cam_hw_version hw_version; +}; + +/** + * struct cam_isp_query_cap_cmd - ISP query device capability payload + * + * @device_iommu: returned iommu handles for device + * @cdm_iommu: returned iommu handles for cdm + * @num_dev: returned number of device capabilities + * @reserved: reserved field for alignment + * @dev_caps: returned device capability array + * + */ +struct cam_isp_query_cap_cmd { + struct cam_iommu_handle device_iommu; + struct cam_iommu_handle cdm_iommu; + int32_t num_dev; + uint32_t reserved; + struct cam_isp_dev_cap_info dev_caps[CAM_ISP_HW_MAX]; +}; + +/* Acquire Device */ +/** + * struct cam_isp_out_port_info - An output port resource info + * + * @res_type: output resource type defined in file + * cam_isp_vfe.h or cam_isp_ife.h + * @format: output format of the resource + * @wdith: output width in pixels + * @height: output height in lines + * @comp_grp_id: composite group id for the resource. + * @split_point: split point in pixels for the dual VFE. + * @secure_mode: flag to tell if output should be run in secure + * mode or not. See cam_defs.h for definition + * @reserved: reserved field for alignment + * + */ +struct cam_isp_out_port_info { + uint32_t res_type; + uint32_t format; + uint32_t width; + uint32_t height; + uint32_t comp_grp_id; + uint32_t split_point; + uint32_t secure_mode; + uint32_t reserved; +}; + +/** + * struct cam_isp_in_port_info - An input port resource info + * + * @res_type: input resource type define in file + * cam_isp_vfe.h or cam_isp_ife.h + * @lane_type: lane type: c-phy or d-phy. + * @lane_num: active lane number + * @lane_cfg: lane configurations: 4 bits per lane + * @vc: input virtual channel number + * @dt: input data type number + * @format: input format + * @test_pattern: test pattern for the testgen + * @usage_type: whether dual vfe is required + * @left_start: left input start offset in pixels + * @left_stop: left input stop offset in pixels + * @left_width: left input width in pixels + * @right_start: right input start offset in pixels. + * Only for Dual VFE + * @right_stop: right input stop offset in pixels. + * Only for Dual VFE + * @right_width: right input width in pixels. + * Only for dual VFE + * @line_start: top of the line number + * @line_stop: bottome of the line number + * @height: input height in lines + * @pixel_clk; sensor output clock + * @batch_size: batch size for HFR mode + * @dsp_mode: DSP stream mode (Defines as CAM_ISP_DSP_MODE_*) + * @hbi_cnt: HBI count for the camif input + * @reserved: Reserved field for alignment + * @num_out_res: number of the output resource associated + * @data: payload that contains the output resources + * + */ +struct cam_isp_in_port_info { + uint32_t res_type; + uint32_t lane_type; + uint32_t lane_num; + uint32_t lane_cfg; + uint32_t vc; + uint32_t dt; + uint32_t format; + uint32_t test_pattern; + uint32_t usage_type; + uint32_t left_start; + uint32_t left_stop; + uint32_t left_width; + uint32_t right_start; + uint32_t right_stop; + uint32_t right_width; + uint32_t line_start; + uint32_t line_stop; + uint32_t height; + uint32_t pixel_clk; + uint32_t batch_size; + uint32_t dsp_mode; + uint32_t hbi_cnt; + uint32_t custom_csid; + uint32_t reserved; + uint32_t num_out_res; + struct cam_isp_out_port_info data[1]; +}; + +/** + * struct cam_isp_resource - A resource bundle + * + * @resoruce_id: resource id for the resource bundle + * @length: length of the while resource blob + * @handle_type: type of the resource handle + * @reserved: reserved field for alignment + * @res_hdl: resource handle that points to the + * resource array; + * + */ +struct cam_isp_resource { + uint32_t resource_id; + uint32_t length; + uint32_t handle_type; + uint32_t reserved; + uint64_t res_hdl; +}; + +/** + * struct cam_isp_port_hfr_config - HFR configuration for this port + * + * @resource_type: Resource type + * @subsample_pattern: Subsample pattern. Used in HFR mode. It + * should be consistent with batchSize and + * CAMIF programming. + * @subsample_period: Subsample period. Used in HFR mode. It + * should be consistent with batchSize and + * CAMIF programming. + * @framedrop_pattern: Framedrop pattern + * @framedrop_period: Framedrop period + * @reserved: Reserved for alignment + */ +struct cam_isp_port_hfr_config { + uint32_t resource_type; + uint32_t subsample_pattern; + uint32_t subsample_period; + uint32_t framedrop_pattern; + uint32_t framedrop_period; + uint32_t reserved; +} __attribute__((packed)); + +/** + * struct cam_isp_resource_hfr_config - Resource HFR configuration + * + * @num_ports: Number of ports + * @reserved: Reserved for alignment + * @port_hfr_config: HFR configuration for each IO port + */ +struct cam_isp_resource_hfr_config { + uint32_t num_ports; + uint32_t reserved; + struct cam_isp_port_hfr_config port_hfr_config[1]; +} __attribute__((packed)); + +/** + * struct cam_isp_dual_split_params - dual isp spilt parameters + * + * @split_point: Split point information x, where (0 < x < width) + * left ISP's input ends at x + righ padding and + * Right ISP's input starts at x - left padding + * @right_padding: Padding added past the split point for left + * ISP's input + * @left_padding: Padding added before split point for right + * ISP's input + * @reserved: Reserved filed for alignment + * + */ +struct cam_isp_dual_split_params { + uint32_t split_point; + uint32_t right_padding; + uint32_t left_padding; + uint32_t reserved; +}; + +/** + * struct cam_isp_dual_stripe_config - stripe config per bus client + * + * @offset: Start horizontal offset relative to + * output buffer + * In UBWC mode, this value indicates the H_INIT + * value in pixel + * @width: Width of the stripe in bytes + * @tileconfig Ubwc meta tile config. Contain the partial + * tile info + * @port_id: port id of ISP output + * + */ +struct cam_isp_dual_stripe_config { + uint32_t offset; + uint32_t width; + uint32_t tileconfig; + uint32_t port_id; +}; + +/** + * struct cam_isp_dual_config - dual isp configuration + * + * @num_ports Number of isp output ports + * @reserved Reserved field for alignment + * @split_params: Inpput split parameters + * @stripes: Stripe information + * + */ +struct cam_isp_dual_config { + uint32_t num_ports; + uint32_t reserved; + struct cam_isp_dual_split_params split_params; + struct cam_isp_dual_stripe_config stripes[1]; +} __attribute__((packed)); + +/** + * struct cam_isp_clock_config - Clock configuration + * + * @usage_type: Usage type (Single/Dual) + * @num_rdi: Number of RDI votes + * @left_pix_hz: Pixel Clock for Left ISP + * @right_pix_hz: Pixel Clock for Right ISP, valid only if Dual + * @rdi_hz: RDI Clock. ISP clock will be max of RDI and + * PIX clocks. For a particular context which ISP + * HW the RDI is allocated to is not known to UMD. + * Hence pass the clock and let KMD decide. + */ +struct cam_isp_clock_config { + uint32_t usage_type; + uint32_t num_rdi; + uint64_t left_pix_hz; + uint64_t right_pix_hz; + uint64_t rdi_hz[1]; +} __attribute__((packed)); + +/** + * struct cam_isp_bw_vote - Bandwidth vote information + * + * @resource_id: Resource ID + * @reserved: Reserved field for alignment + * @cam_bw_bps: Bandwidth vote for CAMNOC + * @ext_bw_bps: Bandwidth vote for path-to-DDR after CAMNOC + */ + +struct cam_isp_bw_vote { + uint32_t resource_id; + uint32_t reserved; + uint64_t cam_bw_bps; + uint64_t ext_bw_bps; +} __attribute__((packed)); + +/** + * struct cam_isp_bw_config - Bandwidth configuration + * + * @usage_type: Usage type (Single/Dual) + * @num_rdi: Number of RDI votes + * @left_pix_vote: Bandwidth vote for left ISP + * @right_pix_vote: Bandwidth vote for right ISP + * @rdi_vote: RDI bandwidth requirements + */ + +struct cam_isp_bw_config { + uint32_t usage_type; + uint32_t num_rdi; + struct cam_isp_bw_vote left_pix_vote; + struct cam_isp_bw_vote right_pix_vote; + struct cam_isp_bw_vote rdi_vote[1]; +} __attribute__((packed)); + +#endif /* __UAPI_CAM_ISP_H__ */ diff --git a/selfdrive/camerad/include/media/cam_isp_ife.h b/selfdrive/camerad/include/media/cam_isp_ife.h new file mode 100644 index 0000000000..f5e72813fc --- /dev/null +++ b/selfdrive/camerad/include/media/cam_isp_ife.h @@ -0,0 +1,39 @@ +#ifndef __UAPI_CAM_ISP_IFE_H__ +#define __UAPI_CAM_ISP_IFE_H__ + +/* IFE output port resource type (global unique)*/ +#define CAM_ISP_IFE_OUT_RES_BASE 0x3000 + +#define CAM_ISP_IFE_OUT_RES_FULL (CAM_ISP_IFE_OUT_RES_BASE + 0) +#define CAM_ISP_IFE_OUT_RES_DS4 (CAM_ISP_IFE_OUT_RES_BASE + 1) +#define CAM_ISP_IFE_OUT_RES_DS16 (CAM_ISP_IFE_OUT_RES_BASE + 2) +#define CAM_ISP_IFE_OUT_RES_RAW_DUMP (CAM_ISP_IFE_OUT_RES_BASE + 3) +#define CAM_ISP_IFE_OUT_RES_FD (CAM_ISP_IFE_OUT_RES_BASE + 4) +#define CAM_ISP_IFE_OUT_RES_PDAF (CAM_ISP_IFE_OUT_RES_BASE + 5) +#define CAM_ISP_IFE_OUT_RES_RDI_0 (CAM_ISP_IFE_OUT_RES_BASE + 6) +#define CAM_ISP_IFE_OUT_RES_RDI_1 (CAM_ISP_IFE_OUT_RES_BASE + 7) +#define CAM_ISP_IFE_OUT_RES_RDI_2 (CAM_ISP_IFE_OUT_RES_BASE + 8) +#define CAM_ISP_IFE_OUT_RES_RDI_3 (CAM_ISP_IFE_OUT_RES_BASE + 9) +#define CAM_ISP_IFE_OUT_RES_STATS_HDR_BE (CAM_ISP_IFE_OUT_RES_BASE + 10) +#define CAM_ISP_IFE_OUT_RES_STATS_HDR_BHIST (CAM_ISP_IFE_OUT_RES_BASE + 11) +#define CAM_ISP_IFE_OUT_RES_STATS_TL_BG (CAM_ISP_IFE_OUT_RES_BASE + 12) +#define CAM_ISP_IFE_OUT_RES_STATS_BF (CAM_ISP_IFE_OUT_RES_BASE + 13) +#define CAM_ISP_IFE_OUT_RES_STATS_AWB_BG (CAM_ISP_IFE_OUT_RES_BASE + 14) +#define CAM_ISP_IFE_OUT_RES_STATS_BHIST (CAM_ISP_IFE_OUT_RES_BASE + 15) +#define CAM_ISP_IFE_OUT_RES_STATS_RS (CAM_ISP_IFE_OUT_RES_BASE + 16) +#define CAM_ISP_IFE_OUT_RES_STATS_CS (CAM_ISP_IFE_OUT_RES_BASE + 17) +#define CAM_ISP_IFE_OUT_RES_STATS_IHIST (CAM_ISP_IFE_OUT_RES_BASE + 18) +#define CAM_ISP_IFE_OUT_RES_MAX (CAM_ISP_IFE_OUT_RES_BASE + 19) + + +/* IFE input port resource type (global unique) */ +#define CAM_ISP_IFE_IN_RES_BASE 0x4000 + +#define CAM_ISP_IFE_IN_RES_TPG (CAM_ISP_IFE_IN_RES_BASE + 0) +#define CAM_ISP_IFE_IN_RES_PHY_0 (CAM_ISP_IFE_IN_RES_BASE + 1) +#define CAM_ISP_IFE_IN_RES_PHY_1 (CAM_ISP_IFE_IN_RES_BASE + 2) +#define CAM_ISP_IFE_IN_RES_PHY_2 (CAM_ISP_IFE_IN_RES_BASE + 3) +#define CAM_ISP_IFE_IN_RES_PHY_3 (CAM_ISP_IFE_IN_RES_BASE + 4) +#define CAM_ISP_IFE_IN_RES_MAX (CAM_ISP_IFE_IN_RES_BASE + 5) + +#endif /* __UAPI_CAM_ISP_IFE_H__ */ diff --git a/selfdrive/camerad/include/media/cam_isp_vfe.h b/selfdrive/camerad/include/media/cam_isp_vfe.h new file mode 100644 index 0000000000..e48db2f98d --- /dev/null +++ b/selfdrive/camerad/include/media/cam_isp_vfe.h @@ -0,0 +1,44 @@ +#ifndef __UAPI_CAM_ISP_VFE_H__ +#define __UAPI_CAM_ISP_VFE_H__ + +/* VFE output port resource type (global unique) */ +#define CAM_ISP_VFE_OUT_RES_BASE 0x1000 + +#define CAM_ISP_VFE_OUT_RES_ENC (CAM_ISP_VFE_OUT_RES_BASE + 0) +#define CAM_ISP_VFE_OUT_RES_VIEW (CAM_ISP_VFE_OUT_RES_BASE + 1) +#define CAM_ISP_VFE_OUT_RES_VID (CAM_ISP_VFE_OUT_RES_BASE + 2) +#define CAM_ISP_VFE_OUT_RES_RDI_0 (CAM_ISP_VFE_OUT_RES_BASE + 3) +#define CAM_ISP_VFE_OUT_RES_RDI_1 (CAM_ISP_VFE_OUT_RES_BASE + 4) +#define CAM_ISP_VFE_OUT_RES_RDI_2 (CAM_ISP_VFE_OUT_RES_BASE + 5) +#define CAM_ISP_VFE_OUT_RES_RDI_3 (CAM_ISP_VFE_OUT_RES_BASE + 6) +#define CAM_ISP_VFE_OUT_RES_STATS_AEC (CAM_ISP_VFE_OUT_RES_BASE + 7) +#define CAM_ISP_VFE_OUT_RES_STATS_AF (CAM_ISP_VFE_OUT_RES_BASE + 8) +#define CAM_ISP_VFE_OUT_RES_STATS_AWB (CAM_ISP_VFE_OUT_RES_BASE + 9) +#define CAM_ISP_VFE_OUT_RES_STATS_RS (CAM_ISP_VFE_OUT_RES_BASE + 10) +#define CAM_ISP_VFE_OUT_RES_STATS_CS (CAM_ISP_VFE_OUT_RES_BASE + 11) +#define CAM_ISP_VFE_OUT_RES_STATS_IHIST (CAM_ISP_VFE_OUT_RES_BASE + 12) +#define CAM_ISP_VFE_OUT_RES_STATS_SKIN (CAM_ISP_VFE_OUT_RES_BASE + 13) +#define CAM_ISP_VFE_OUT_RES_STATS_BG (CAM_ISP_VFE_OUT_RES_BASE + 14) +#define CAM_ISP_VFE_OUT_RES_STATS_BF (CAM_ISP_VFE_OUT_RES_BASE + 15) +#define CAM_ISP_VFE_OUT_RES_STATS_BE (CAM_ISP_VFE_OUT_RES_BASE + 16) +#define CAM_ISP_VFE_OUT_RES_STATS_BHIST (CAM_ISP_VFE_OUT_RES_BASE + 17) +#define CAM_ISP_VFE_OUT_RES_STATS_BF_SCALE (CAM_ISP_VFE_OUT_RES_BASE + 18) +#define CAM_ISP_VFE_OUT_RES_STATS_HDR_BE (CAM_ISP_VFE_OUT_RES_BASE + 19) +#define CAM_ISP_VFE_OUT_RES_STATS_HDR_BHIST (CAM_ISP_VFE_OUT_RES_BASE + 20) +#define CAM_ISP_VFE_OUT_RES_STATS_AEC_BG (CAM_ISP_VFE_OUT_RES_BASE + 21) +#define CAM_ISP_VFE_OUT_RES_CAMIF_RAW (CAM_ISP_VFE_OUT_RES_BASE + 22) +#define CAM_ISP_VFE_OUT_RES_IDEAL_RAW (CAM_ISP_VFE_OUT_RES_BASE + 23) +#define CAM_ISP_VFE_OUT_RES_MAX (CAM_ISP_VFE_OUT_RES_BASE + 24) + +/* VFE input port_ resource type (global unique) */ +#define CAM_ISP_VFE_IN_RES_BASE 0x2000 + +#define CAM_ISP_VFE_IN_RES_TPG (CAM_ISP_VFE_IN_RES_BASE + 0) +#define CAM_ISP_VFE_IN_RES_PHY_0 (CAM_ISP_VFE_IN_RES_BASE + 1) +#define CAM_ISP_VFE_IN_RES_PHY_1 (CAM_ISP_VFE_IN_RES_BASE + 2) +#define CAM_ISP_VFE_IN_RES_PHY_2 (CAM_ISP_VFE_IN_RES_BASE + 3) +#define CAM_ISP_VFE_IN_RES_PHY_3 (CAM_ISP_VFE_IN_RES_BASE + 4) +#define CAM_ISP_VFE_IN_RES_FE (CAM_ISP_VFE_IN_RES_BASE + 5) +#define CAM_ISP_VFE_IN_RES_MAX (CAM_ISP_VFE_IN_RES_BASE + 6) + +#endif /* __UAPI_CAM_ISP_VFE_H__ */ diff --git a/selfdrive/camerad/include/media/cam_jpeg.h b/selfdrive/camerad/include/media/cam_jpeg.h new file mode 100644 index 0000000000..f3082f3bfe --- /dev/null +++ b/selfdrive/camerad/include/media/cam_jpeg.h @@ -0,0 +1,117 @@ +#ifndef __UAPI_CAM_JPEG_H__ +#define __UAPI_CAM_JPEG_H__ + +#include "cam_defs.h" + +/* enc, dma, cdm(enc/dma) are used in querycap */ +#define CAM_JPEG_DEV_TYPE_ENC 0 +#define CAM_JPEG_DEV_TYPE_DMA 1 +#define CAM_JPEG_DEV_TYPE_MAX 2 + +#define CAM_JPEG_NUM_DEV_PER_RES_MAX 1 + +/* definitions needed for jpeg aquire device */ +#define CAM_JPEG_RES_TYPE_ENC 0 +#define CAM_JPEG_RES_TYPE_DMA 1 +#define CAM_JPEG_RES_TYPE_MAX 2 + +/* packet opcode types */ +#define CAM_JPEG_OPCODE_ENC_UPDATE 0 +#define CAM_JPEG_OPCODE_DMA_UPDATE 1 + +/* ENC input port resource type */ +#define CAM_JPEG_ENC_INPUT_IMAGE 0x0 + +/* ENC output port resource type */ +#define CAM_JPEG_ENC_OUTPUT_IMAGE 0x1 + +#define CAM_JPEG_ENC_IO_IMAGES_MAX 0x2 + +/* DMA input port resource type */ +#define CAM_JPEG_DMA_INPUT_IMAGE 0x0 + +/* DMA output port resource type */ +#define CAM_JPEG_DMA_OUTPUT_IMAGE 0x1 + +#define CAM_JPEG_DMA_IO_IMAGES_MAX 0x2 + +#define CAM_JPEG_IMAGE_MAX 0x2 + +/** + * struct cam_jpeg_dev_ver - Device information for particular hw type + * + * This is used to get device version info of JPEG ENC, JPEG DMA + * from hardware and use this info in CAM_QUERY_CAP IOCTL + * + * @size : Size of struct passed + * @dev_type: Hardware type for the cap info(jpeg enc, jpeg dma) + * @hw_ver: Major, minor and incr values of a device version + */ +struct cam_jpeg_dev_ver { + uint32_t size; + uint32_t dev_type; + struct cam_hw_version hw_ver; +}; + +/** + * struct cam_jpeg_query_cap_cmd - JPEG query device capability payload + * + * @dev_iommu_handle: Jpeg iommu handles for secure/non secure + * modes + * @cdm_iommu_handle: Iommu handles for secure/non secure modes + * @num_enc: Number of encoder + * @num_dma: Number of dma + * @dev_ver: Returned device capability array + */ +struct cam_jpeg_query_cap_cmd { + struct cam_iommu_handle dev_iommu_handle; + struct cam_iommu_handle cdm_iommu_handle; + uint32_t num_enc; + uint32_t num_dma; + struct cam_jpeg_dev_ver dev_ver[CAM_JPEG_DEV_TYPE_MAX]; +}; + +/** + * struct cam_jpeg_res_info - JPEG output resource info + * + * @format: Format of the resource + * @width: Width in pixels + * @height: Height in lines + * @fps: Fps + */ +struct cam_jpeg_res_info { + uint32_t format; + uint32_t width; + uint32_t height; + uint32_t fps; +}; + +/** + * struct cam_jpeg_acquire_dev_info - An JPEG device info + * + * @dev_type: Device type (ENC/DMA) + * @reserved: Reserved Bytes + * @in_res: In resource info + * @in_res: Iut resource info + */ +struct cam_jpeg_acquire_dev_info { + uint32_t dev_type; + uint32_t reserved; + struct cam_jpeg_res_info in_res; + struct cam_jpeg_res_info out_res; +}; + +/** + * struct cam_jpeg_config_inout_param_info - JPEG Config time + * input output params + * + * @clk_index: Input Param- clock selection index.(-1 default) + * @output_size: Output Param - jpeg encode/dma output size in + * bytes + */ +struct cam_jpeg_config_inout_param_info { + int32_t clk_index; + int32_t output_size; +}; + +#endif /* __UAPI_CAM_JPEG_H__ */ diff --git a/selfdrive/camerad/include/media/cam_lrme.h b/selfdrive/camerad/include/media/cam_lrme.h new file mode 100644 index 0000000000..97d957835e --- /dev/null +++ b/selfdrive/camerad/include/media/cam_lrme.h @@ -0,0 +1,65 @@ +#ifndef __UAPI_CAM_LRME_H__ +#define __UAPI_CAM_LRME_H__ + +#include "cam_defs.h" + +/* LRME Resource Types */ + +enum CAM_LRME_IO_TYPE { + CAM_LRME_IO_TYPE_TAR, + CAM_LRME_IO_TYPE_REF, + CAM_LRME_IO_TYPE_RES, + CAM_LRME_IO_TYPE_DS2, +}; + +#define CAM_LRME_INPUT_PORT_TYPE_TAR (1 << 0) +#define CAM_LRME_INPUT_PORT_TYPE_REF (1 << 1) + +#define CAM_LRME_OUTPUT_PORT_TYPE_DS2 (1 << 0) +#define CAM_LRME_OUTPUT_PORT_TYPE_RES (1 << 1) + +#define CAM_LRME_DEV_MAX 1 + + +struct cam_lrme_hw_version { + uint32_t gen; + uint32_t rev; + uint32_t step; +}; + +struct cam_lrme_dev_cap { + struct cam_lrme_hw_version clc_hw_version; + struct cam_lrme_hw_version bus_rd_hw_version; + struct cam_lrme_hw_version bus_wr_hw_version; + struct cam_lrme_hw_version top_hw_version; + struct cam_lrme_hw_version top_titan_version; +}; + +/** + * struct cam_lrme_query_cap_cmd - LRME query device capability payload + * + * @dev_iommu_handle: LRME iommu handles for secure/non secure + * modes + * @cdm_iommu_handle: Iommu handles for secure/non secure modes + * @num_devices: number of hardware devices + * @dev_caps: Returned device capability array + */ +struct cam_lrme_query_cap_cmd { + struct cam_iommu_handle device_iommu; + struct cam_iommu_handle cdm_iommu; + uint32_t num_devices; + struct cam_lrme_dev_cap dev_caps[CAM_LRME_DEV_MAX]; +}; + +struct cam_lrme_soc_info { + uint64_t clock_rate; + uint64_t bandwidth; + uint64_t reserved[4]; +}; + +struct cam_lrme_acquire_args { + struct cam_lrme_soc_info lrme_soc_info; +}; + +#endif /* __UAPI_CAM_LRME_H__ */ + diff --git a/selfdrive/camerad/include/media/cam_req_mgr.h b/selfdrive/camerad/include/media/cam_req_mgr.h new file mode 100644 index 0000000000..ae65649964 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_req_mgr.h @@ -0,0 +1,436 @@ +#ifndef __UAPI_LINUX_CAM_REQ_MGR_H +#define __UAPI_LINUX_CAM_REQ_MGR_H + +#include +#include +#include +#include +#include + +#define CAM_REQ_MGR_VNODE_NAME "cam-req-mgr-devnode" + +#define CAM_DEVICE_TYPE_BASE (MEDIA_ENT_F_OLD_BASE) +#define CAM_VNODE_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE) +#define CAM_SENSOR_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 1) +#define CAM_IFE_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 2) +#define CAM_ICP_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 3) +#define CAM_LRME_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 4) +#define CAM_JPEG_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 5) +#define CAM_FD_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 6) +#define CAM_CPAS_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 7) +#define CAM_CSIPHY_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 8) +#define CAM_ACTUATOR_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 9) +#define CAM_CCI_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 10) +#define CAM_FLASH_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 11) +#define CAM_EEPROM_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 12) +#define CAM_OIS_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 13) + +/* cam_req_mgr hdl info */ +#define CAM_REQ_MGR_HDL_IDX_POS 8 +#define CAM_REQ_MGR_HDL_IDX_MASK ((1 << CAM_REQ_MGR_HDL_IDX_POS) - 1) +#define CAM_REQ_MGR_GET_HDL_IDX(hdl) (hdl & CAM_REQ_MGR_HDL_IDX_MASK) + +/** + * Max handles supported by cam_req_mgr + * It includes both session and device handles + */ +#define CAM_REQ_MGR_MAX_HANDLES 64 +#define MAX_LINKS_PER_SESSION 2 + +/* V4L event type which user space will subscribe to */ +#define V4L_EVENT_CAM_REQ_MGR_EVENT (V4L2_EVENT_PRIVATE_START + 0) + +/* Specific event ids to get notified in user space */ +#define V4L_EVENT_CAM_REQ_MGR_SOF 0 +#define V4L_EVENT_CAM_REQ_MGR_ERROR 1 +#define V4L_EVENT_CAM_REQ_MGR_SOF_BOOT_TS 2 + +/* SOF Event status */ +#define CAM_REQ_MGR_SOF_EVENT_SUCCESS 0 +#define CAM_REQ_MGR_SOF_EVENT_ERROR 1 + +/* Link control operations */ +#define CAM_REQ_MGR_LINK_ACTIVATE 0 +#define CAM_REQ_MGR_LINK_DEACTIVATE 1 + +/** + * Request Manager : flush_type + * @CAM_REQ_MGR_FLUSH_TYPE_ALL: Req mgr will remove all the pending + * requests from input/processing queue. + * @CAM_REQ_MGR_FLUSH_TYPE_CANCEL_REQ: Req mgr will remove only particular + * request id from input/processing queue. + * @CAM_REQ_MGR_FLUSH_TYPE_MAX: Max number of the flush type + * @opcode: CAM_REQ_MGR_FLUSH_REQ + */ +#define CAM_REQ_MGR_FLUSH_TYPE_ALL 0 +#define CAM_REQ_MGR_FLUSH_TYPE_CANCEL_REQ 1 +#define CAM_REQ_MGR_FLUSH_TYPE_MAX 2 + +/** + * Request Manager : Sync Mode type + * @CAM_REQ_MGR_SYNC_MODE_NO_SYNC: Req mgr will apply non-sync mode for this + * request. + * @CAM_REQ_MGR_SYNC_MODE_SYNC: Req mgr will apply sync mode for this request. + */ +#define CAM_REQ_MGR_SYNC_MODE_NO_SYNC 0 +#define CAM_REQ_MGR_SYNC_MODE_SYNC 1 + +/** + * struct cam_req_mgr_event_data + * @session_hdl: session handle + * @link_hdl: link handle + * @frame_id: frame id + * @reserved: reserved for 64 bit aligngment + * @req_id: request id + * @tv_sec: timestamp in seconds + * @tv_usec: timestamp in micro seconds + */ +struct cam_req_mgr_event_data { + int32_t session_hdl; + int32_t link_hdl; + int32_t frame_id; + int32_t reserved; + int64_t req_id; + uint64_t tv_sec; + uint64_t tv_usec; +}; + +/** + * struct cam_req_mgr_session_info + * @session_hdl: In/Output param - session_handle + * @opcode1: CAM_REQ_MGR_CREATE_SESSION + * @opcode2: CAM_REQ_MGR_DESTROY_SESSION + */ +struct cam_req_mgr_session_info { + int32_t session_hdl; + int32_t reserved; +}; + +/** + * struct cam_req_mgr_link_info + * @session_hdl: Input param - Identifier for CSL session + * @num_devices: Input Param - Num of devices to be linked + * @dev_hdls: Input param - List of device handles to be linked + * @link_hdl: Output Param -Identifier for link + * @opcode: CAM_REQ_MGR_LINK + */ +struct cam_req_mgr_link_info { + int32_t session_hdl; + uint32_t num_devices; + int32_t dev_hdls[CAM_REQ_MGR_MAX_HANDLES]; + int32_t link_hdl; +}; + +/** + * struct cam_req_mgr_unlink_info + * @session_hdl: input param - session handle + * @link_hdl: input param - link handle + * @opcode: CAM_REQ_MGR_UNLINK + */ +struct cam_req_mgr_unlink_info { + int32_t session_hdl; + int32_t link_hdl; +}; + +/** + * struct cam_req_mgr_flush_info + * @brief: User can tell drivers to flush a particular request id or + * flush all requests from its pending processing queue. Flush is a + * blocking call and driver shall ensure all requests are flushed + * before returning. + * @session_hdl: Input param - Identifier for CSL session + * @link_hdl: Input Param -Identifier for link + * @flush_type: User can cancel a particular req id or can flush + * all requests in queue + * @reserved: reserved for 64 bit aligngment + * @req_id: field is valid only if flush type is cancel request + * for flush all this field value is not considered. + * @opcode: CAM_REQ_MGR_FLUSH_REQ + */ +struct cam_req_mgr_flush_info { + int32_t session_hdl; + int32_t link_hdl; + uint32_t flush_type; + uint32_t reserved; + int64_t req_id; +}; + +/** struct cam_req_mgr_sched_info + * @session_hdl: Input param - Identifier for CSL session + * @link_hdl: Input Param -Identifier for link + * inluding itself. + * @bubble_enable: Input Param - Cam req mgr will do bubble recovery if this + * flag is set. + * @sync_mode: Type of Sync mode for this request + * @req_id: Input Param - Request Id from which all requests will be flushed + */ +struct cam_req_mgr_sched_request { + int32_t session_hdl; + int32_t link_hdl; + int32_t bubble_enable; + int32_t sync_mode; + int64_t req_id; +}; + +/** + * struct cam_req_mgr_sync_mode + * @session_hdl: Input param - Identifier for CSL session + * @sync_mode: Input Param - Type of sync mode + * @num_links: Input Param - Num of links in sync mode (Valid only + * when sync_mode is one of SYNC enabled modes) + * @link_hdls: Input Param - Array of link handles to be in sync mode + * (Valid only when sync_mode is one of SYNC + * enabled modes) + * @master_link_hdl: Input Param - To dictate which link's SOF drives system + * (Valid only when sync_mode is one of SYNC + * enabled modes) + * + * @opcode: CAM_REQ_MGR_SYNC_MODE + */ +struct cam_req_mgr_sync_mode { + int32_t session_hdl; + int32_t sync_mode; + int32_t num_links; + int32_t link_hdls[MAX_LINKS_PER_SESSION]; + int32_t master_link_hdl; + int32_t reserved; +}; + +/** + * struct cam_req_mgr_link_control + * @ops: Link operations: activate/deactive + * @session_hdl: Input param - Identifier for CSL session + * @num_links: Input Param - Num of links + * @reserved: reserved field + * @link_hdls: Input Param - Links to be activated/deactivated + * + * @opcode: CAM_REQ_MGR_LINK_CONTROL + */ +struct cam_req_mgr_link_control { + int32_t ops; + int32_t session_hdl; + int32_t num_links; + int32_t reserved; + int32_t link_hdls[MAX_LINKS_PER_SESSION]; +}; + +/** + * cam_req_mgr specific opcode ids + */ +#define CAM_REQ_MGR_CREATE_DEV_NODES (CAM_COMMON_OPCODE_MAX + 1) +#define CAM_REQ_MGR_CREATE_SESSION (CAM_COMMON_OPCODE_MAX + 2) +#define CAM_REQ_MGR_DESTROY_SESSION (CAM_COMMON_OPCODE_MAX + 3) +#define CAM_REQ_MGR_LINK (CAM_COMMON_OPCODE_MAX + 4) +#define CAM_REQ_MGR_UNLINK (CAM_COMMON_OPCODE_MAX + 5) +#define CAM_REQ_MGR_SCHED_REQ (CAM_COMMON_OPCODE_MAX + 6) +#define CAM_REQ_MGR_FLUSH_REQ (CAM_COMMON_OPCODE_MAX + 7) +#define CAM_REQ_MGR_SYNC_MODE (CAM_COMMON_OPCODE_MAX + 8) +#define CAM_REQ_MGR_ALLOC_BUF (CAM_COMMON_OPCODE_MAX + 9) +#define CAM_REQ_MGR_MAP_BUF (CAM_COMMON_OPCODE_MAX + 10) +#define CAM_REQ_MGR_RELEASE_BUF (CAM_COMMON_OPCODE_MAX + 11) +#define CAM_REQ_MGR_CACHE_OPS (CAM_COMMON_OPCODE_MAX + 12) +#define CAM_REQ_MGR_LINK_CONTROL (CAM_COMMON_OPCODE_MAX + 13) +/* end of cam_req_mgr opcodes */ + +#define CAM_MEM_FLAG_HW_READ_WRITE (1<<0) +#define CAM_MEM_FLAG_HW_READ_ONLY (1<<1) +#define CAM_MEM_FLAG_HW_WRITE_ONLY (1<<2) +#define CAM_MEM_FLAG_KMD_ACCESS (1<<3) +#define CAM_MEM_FLAG_UMD_ACCESS (1<<4) +#define CAM_MEM_FLAG_PROTECTED_MODE (1<<5) +#define CAM_MEM_FLAG_CMD_BUF_TYPE (1<<6) +#define CAM_MEM_FLAG_PIXEL_BUF_TYPE (1<<7) +#define CAM_MEM_FLAG_STATS_BUF_TYPE (1<<8) +#define CAM_MEM_FLAG_PACKET_BUF_TYPE (1<<9) +#define CAM_MEM_FLAG_CACHE (1<<10) +#define CAM_MEM_FLAG_HW_SHARED_ACCESS (1<<11) + +#define CAM_MEM_MMU_MAX_HANDLE 16 + +/* Maximum allowed buffers in existence */ +#define CAM_MEM_BUFQ_MAX 1024 + +#define CAM_MEM_MGR_SECURE_BIT_POS 15 +#define CAM_MEM_MGR_HDL_IDX_SIZE 15 +#define CAM_MEM_MGR_HDL_FD_SIZE 16 +#define CAM_MEM_MGR_HDL_IDX_END_POS 16 +#define CAM_MEM_MGR_HDL_FD_END_POS 32 + +#define CAM_MEM_MGR_HDL_IDX_MASK ((1 << CAM_MEM_MGR_HDL_IDX_SIZE) - 1) + +#define GET_MEM_HANDLE(idx, fd) \ + ((idx & CAM_MEM_MGR_HDL_IDX_MASK) | \ + (fd << (CAM_MEM_MGR_HDL_FD_END_POS - CAM_MEM_MGR_HDL_FD_SIZE))) \ + +#define GET_FD_FROM_HANDLE(hdl) \ + (hdl >> (CAM_MEM_MGR_HDL_FD_END_POS - CAM_MEM_MGR_HDL_FD_SIZE)) \ + +#define CAM_MEM_MGR_GET_HDL_IDX(hdl) (hdl & CAM_MEM_MGR_HDL_IDX_MASK) + +#define CAM_MEM_MGR_SET_SECURE_HDL(hdl, flag) \ + ((flag) ? (hdl |= (1 << CAM_MEM_MGR_SECURE_BIT_POS)) : \ + ((hdl) &= ~(1 << CAM_MEM_MGR_SECURE_BIT_POS))) + +#define CAM_MEM_MGR_IS_SECURE_HDL(hdl) \ + (((hdl) & \ + (1<> CAM_MEM_MGR_SECURE_BIT_POS) + +/** + * memory allocation type + */ +#define CAM_MEM_DMA_NONE 0 +#define CAM_MEM_DMA_BIDIRECTIONAL 1 +#define CAM_MEM_DMA_TO_DEVICE 2 +#define CAM_MEM_DMA_FROM_DEVICE 3 + + +/** + * memory cache operation + */ +#define CAM_MEM_CLEAN_CACHE 1 +#define CAM_MEM_INV_CACHE 2 +#define CAM_MEM_CLEAN_INV_CACHE 3 + + +/** + * struct cam_mem_alloc_out_params + * @buf_handle: buffer handle + * @fd: output buffer file descriptor + * @vaddr: virtual address pointer + */ +struct cam_mem_alloc_out_params { + uint32_t buf_handle; + int32_t fd; + uint64_t vaddr; +}; + +/** + * struct cam_mem_map_out_params + * @buf_handle: buffer handle + * @reserved: reserved for future + * @vaddr: virtual address pointer + */ +struct cam_mem_map_out_params { + uint32_t buf_handle; + uint32_t reserved; + uint64_t vaddr; +}; + +/** + * struct cam_mem_mgr_alloc_cmd + * @len: size of buffer to allocate + * @align: alignment of the buffer + * @mmu_hdls: array of mmu handles + * @num_hdl: number of handles + * @flags: flags of the buffer + * @out: out params + */ +/* CAM_REQ_MGR_ALLOC_BUF */ +struct cam_mem_mgr_alloc_cmd { + uint64_t len; + uint64_t align; + int32_t mmu_hdls[CAM_MEM_MMU_MAX_HANDLE]; + uint32_t num_hdl; + uint32_t flags; + struct cam_mem_alloc_out_params out; +}; + +/** + * struct cam_mem_mgr_map_cmd + * @mmu_hdls: array of mmu handles + * @num_hdl: number of handles + * @flags: flags of the buffer + * @fd: output buffer file descriptor + * @reserved: reserved field + * @out: out params + */ + +/* CAM_REQ_MGR_MAP_BUF */ +struct cam_mem_mgr_map_cmd { + int32_t mmu_hdls[CAM_MEM_MMU_MAX_HANDLE]; + uint32_t num_hdl; + uint32_t flags; + int32_t fd; + uint32_t reserved; + struct cam_mem_map_out_params out; +}; + +/** + * struct cam_mem_mgr_map_cmd + * @buf_handle: buffer handle + * @reserved: reserved field + */ +/* CAM_REQ_MGR_RELEASE_BUF */ +struct cam_mem_mgr_release_cmd { + int32_t buf_handle; + uint32_t reserved; +}; + +/** + * struct cam_mem_mgr_map_cmd + * @buf_handle: buffer handle + * @ops: cache operations + */ +/* CAM_REQ_MGR_CACHE_OPS */ +struct cam_mem_cache_ops_cmd { + int32_t buf_handle; + uint32_t mem_cache_ops; +}; + +/** + * Request Manager : error message type + * @CAM_REQ_MGR_ERROR_TYPE_DEVICE: Device error message, fatal to session + * @CAM_REQ_MGR_ERROR_TYPE_REQUEST: Error on a single request, not fatal + * @CAM_REQ_MGR_ERROR_TYPE_BUFFER: Buffer was not filled, not fatal + */ +#define CAM_REQ_MGR_ERROR_TYPE_DEVICE 0 +#define CAM_REQ_MGR_ERROR_TYPE_REQUEST 1 +#define CAM_REQ_MGR_ERROR_TYPE_BUFFER 2 + +/** + * struct cam_req_mgr_error_msg + * @error_type: type of error + * @request_id: request id of frame + * @device_hdl: device handle + * @linke_hdl: link_hdl + * @resource_size: size of the resource + */ +struct cam_req_mgr_error_msg { + uint32_t error_type; + uint32_t request_id; + int32_t device_hdl; + int32_t link_hdl; + uint64_t resource_size; +}; + +/** + * struct cam_req_mgr_frame_msg + * @request_id: request id of the frame + * @frame_id: frame id of the frame + * @timestamp: timestamp of the frame + * @link_hdl: link handle associated with this message + * @sof_status: sof status success or fail + */ +struct cam_req_mgr_frame_msg { + uint64_t request_id; + uint64_t frame_id; + uint64_t timestamp; + int32_t link_hdl; + uint32_t sof_status; +}; + +/** + * struct cam_req_mgr_message + * @session_hdl: session to which the frame belongs to + * @reserved: reserved field + * @u: union which can either be error or frame message + */ +struct cam_req_mgr_message { + int32_t session_hdl; + int32_t reserved; + union { + struct cam_req_mgr_error_msg err_msg; + struct cam_req_mgr_frame_msg frame_msg; + } u; +}; +#endif /* __UAPI_LINUX_CAM_REQ_MGR_H */ diff --git a/selfdrive/camerad/include/media/cam_sensor.h b/selfdrive/camerad/include/media/cam_sensor.h new file mode 100644 index 0000000000..f5af6047f5 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_sensor.h @@ -0,0 +1,477 @@ +#ifndef __UAPI_CAM_SENSOR_H__ +#define __UAPI_CAM_SENSOR_H__ + +#include +#include +#include + +#define CAM_SENSOR_PROBE_CMD (CAM_COMMON_OPCODE_MAX + 1) +#define CAM_FLASH_MAX_LED_TRIGGERS 3 +#define MAX_OIS_NAME_SIZE 32 +#define CAM_CSIPHY_SECURE_MODE_ENABLED 1 +/** + * struct cam_sensor_query_cap - capabilities info for sensor + * + * @slot_info : Indicates about the slotId or cell Index + * @secure_camera : Camera is in secure/Non-secure mode + * @pos_pitch : Sensor position pitch + * @pos_roll : Sensor position roll + * @pos_yaw : Sensor position yaw + * @actuator_slot_id : Actuator slot id which connected to sensor + * @eeprom_slot_id : EEPROM slot id which connected to sensor + * @ois_slot_id : OIS slot id which connected to sensor + * @flash_slot_id : Flash slot id which connected to sensor + * @csiphy_slot_id : CSIphy slot id which connected to sensor + * + */ +struct cam_sensor_query_cap { + uint32_t slot_info; + uint32_t secure_camera; + uint32_t pos_pitch; + uint32_t pos_roll; + uint32_t pos_yaw; + uint32_t actuator_slot_id; + uint32_t eeprom_slot_id; + uint32_t ois_slot_id; + uint32_t flash_slot_id; + uint32_t csiphy_slot_id; +} __attribute__((packed)); + +/** + * struct cam_csiphy_query_cap - capabilities info for csiphy + * + * @slot_info : Indicates about the slotId or cell Index + * @version : CSIphy version + * @clk lane : Of the 5 lanes, informs lane configured + * as clock lane + * @reserved + */ +struct cam_csiphy_query_cap { + uint32_t slot_info; + uint32_t version; + uint32_t clk_lane; + uint32_t reserved; +} __attribute__((packed)); + +/** + * struct cam_actuator_query_cap - capabilities info for actuator + * + * @slot_info : Indicates about the slotId or cell Index + * @reserved + */ +struct cam_actuator_query_cap { + uint32_t slot_info; + uint32_t reserved; +} __attribute__((packed)); + +/** + * struct cam_eeprom_query_cap_t - capabilities info for eeprom + * + * @slot_info : Indicates about the slotId or cell Index + * @eeprom_kernel_probe : Indicates about the kernel or userspace probe + */ +struct cam_eeprom_query_cap_t { + uint32_t slot_info; + uint16_t eeprom_kernel_probe; + uint16_t reserved; +} __attribute__((packed)); + +/** + * struct cam_ois_query_cap_t - capabilities info for ois + * + * @slot_info : Indicates about the slotId or cell Index + */ +struct cam_ois_query_cap_t { + uint32_t slot_info; + uint16_t reserved; +} __attribute__((packed)); + +/** + * struct cam_cmd_i2c_info - Contains slave I2C related info + * + * @slave_addr : Slave address + * @i2c_freq_mode : 4 bits are used for I2c freq mode + * @cmd_type : Explains type of command + */ +struct cam_cmd_i2c_info { + uint16_t slave_addr; + uint8_t i2c_freq_mode; + uint8_t cmd_type; +} __attribute__((packed)); + +/** + * struct cam_ois_opcode - Contains OIS opcode + * + * @prog : OIS FW prog register address + * @coeff : OIS FW coeff register address + * @pheripheral : OIS pheripheral + * @memory : OIS memory + */ +struct cam_ois_opcode { + uint32_t prog; + uint32_t coeff; + uint32_t pheripheral; + uint32_t memory; +} __attribute__((packed)); + +/** + * struct cam_cmd_ois_info - Contains OIS slave info + * + * @slave_addr : OIS i2c slave address + * @i2c_freq_mode : i2c frequency mode + * @cmd_type : Explains type of command + * @ois_fw_flag : indicates if fw is present or not + * @is_ois_calib : indicates the calibration data is available + * @ois_name : OIS name + * @opcode : opcode + */ +struct cam_cmd_ois_info { + uint16_t slave_addr; + uint8_t i2c_freq_mode; + uint8_t cmd_type; + uint8_t ois_fw_flag; + uint8_t is_ois_calib; + char ois_name[MAX_OIS_NAME_SIZE]; + struct cam_ois_opcode opcode; +} __attribute__((packed)); + +/** + * struct cam_cmd_probe - Contains sensor slave info + * + * @data_type : Slave register data type + * @addr_type : Slave register address type + * @op_code : Don't Care + * @cmd_type : Explains type of command + * @reg_addr : Slave register address + * @expected_data : Data expected at slave register address + * @data_mask : Data mask if only few bits are valid + * @camera_id : Indicates the slot to which camera + * needs to be probed + * @reserved + */ +struct cam_cmd_probe { + uint8_t data_type; + uint8_t addr_type; + uint8_t op_code; + uint8_t cmd_type; + uint32_t reg_addr; + uint32_t expected_data; + uint32_t data_mask; + uint16_t camera_id; + uint16_t reserved; +} __attribute__((packed)); + +/** + * struct cam_power_settings - Contains sensor power setting info + * + * @power_seq_type : Type of power sequence + * @reserved + * @config_val_low : Lower 32 bit value configuration value + * @config_val_high : Higher 32 bit value configuration value + * + */ +struct cam_power_settings { + uint16_t power_seq_type; + uint16_t reserved; + uint32_t config_val_low; + uint32_t config_val_high; +} __attribute__((packed)); + +/** + * struct cam_cmd_power - Explains about the power settings + * + * @count : Number of power settings follows + * @reserved + * @cmd_type : Explains type of command + * @power_settings : Contains power setting info + */ +struct cam_cmd_power { + uint16_t count; + uint8_t reserved; + uint8_t cmd_type; + struct cam_power_settings power_settings[1]; +} __attribute__((packed)); + +/** + * struct i2c_rdwr_header - header of READ/WRITE I2C command + * + * @ count : Number of registers / data / reg-data pairs + * @ op_code : Operation code + * @ cmd_type : Command buffer type + * @ data_type : I2C data type + * @ addr_type : I2C address type + * @ reserved + */ +struct i2c_rdwr_header { + uint16_t count; + uint8_t op_code; + uint8_t cmd_type; + uint8_t data_type; + uint8_t addr_type; + uint16_t reserved; +} __attribute__((packed)); + +/** + * struct i2c_random_wr_payload - payload for I2C random write + * + * @ reg_addr : Register address + * @ reg_data : Register data + * + */ +struct i2c_random_wr_payload { + uint32_t reg_addr; + uint32_t reg_data; +} __attribute__((packed)); + +/** + * struct cam_cmd_i2c_random_wr - I2C random write command + * @ header : header of READ/WRITE I2C command + * @ random_wr_payload : payload for I2C random write + */ +struct cam_cmd_i2c_random_wr { + struct i2c_rdwr_header header; + struct i2c_random_wr_payload random_wr_payload[1]; +} __attribute__((packed)); + +/** + * struct cam_cmd_read - I2C read command + * @ reg_data : Register data + * @ reserved + */ +struct cam_cmd_read { + uint32_t reg_data; + uint32_t reserved; +} __attribute__((packed)); + +/** + * struct cam_cmd_i2c_continuous_wr - I2C continuous write command + * @ header : header of READ/WRITE I2C command + * @ reg_addr : Register address + * @ data_read : I2C read command + */ +struct cam_cmd_i2c_continuous_wr { + struct i2c_rdwr_header header; + uint32_t reg_addr; + struct cam_cmd_read data_read[1]; +} __attribute__((packed)); + +/** + * struct cam_cmd_i2c_random_rd - I2C random read command + * @ header : header of READ/WRITE I2C command + * @ data_read : I2C read command + */ +struct cam_cmd_i2c_random_rd { + struct i2c_rdwr_header header; + struct cam_cmd_read data_read[1]; +} __attribute__((packed)); + +/** + * struct cam_cmd_i2c_continuous_rd - I2C continuous continuous read command + * @ header : header of READ/WRITE I2C command + * @ reg_addr : Register address + * + */ +struct cam_cmd_i2c_continuous_rd { + struct i2c_rdwr_header header; + uint32_t reg_addr; +} __attribute__((packed)); + +/** + * struct cam_cmd_conditional_wait - Conditional wait command + * @data_type : Data type + * @addr_type : Address type + * @op_code : Opcode + * @cmd_type : Explains type of command + * @timeout : Timeout for retries + * @reserved + * @reg_addr : Register Address + * @reg_data : Register data + * @data_mask : Data mask if only few bits are valid + * @camera_id : Indicates the slot to which camera + * needs to be probed + * + */ +struct cam_cmd_conditional_wait { + uint8_t data_type; + uint8_t addr_type; + uint8_t op_code; + uint8_t cmd_type; + uint16_t timeout; + uint16_t reserved; + uint32_t reg_addr; + uint32_t reg_data; + uint32_t data_mask; +} __attribute__((packed)); + +/** + * struct cam_cmd_unconditional_wait - Un-conditional wait command + * @delay : Delay + * @op_code : Opcode + * @cmd_type : Explains type of command + */ +struct cam_cmd_unconditional_wait { + int16_t delay; + uint8_t op_code; + uint8_t cmd_type; +} __attribute__((packed)); + +/** + * cam_csiphy_info: Provides cmdbuffer structre + * @lane_mask : Lane mask details + * @lane_assign : Lane sensor will be using + * @csiphy_3phase : Total number of lanes + * @combo_mode : Info regarding combo_mode is enable / disable + * @lane_cnt : Total number of lanes + * @secure_mode : Secure mode flag to enable / disable + * @3phase : Details whether 3Phase / 2Phase operation + * @settle_time : Settling time in ms + * @data_rate : Data rate + * + */ +struct cam_csiphy_info { + uint16_t lane_mask; + uint16_t lane_assign; + uint8_t csiphy_3phase; + uint8_t combo_mode; + uint8_t lane_cnt; + uint8_t secure_mode; + uint64_t settle_time; + uint64_t data_rate; +} __attribute__((packed)); + +/** + * cam_csiphy_acquire_dev_info : Information needed for + * csiphy at the time of acquire + * @combo_mode : Indicates the device mode of operation + * @reserved + * + */ +struct cam_csiphy_acquire_dev_info { + uint32_t combo_mode; + uint32_t reserved; +} __attribute__((packed)); + +/** + * cam_sensor_acquire_dev : Updates sensor acuire cmd + * @device_handle : Updates device handle + * @session_handle : Session handle for acquiring device + * @handle_type : Resource handle type + * @reserved + * @info_handle : Handle to additional info + * needed for sensor sub modules + * + */ +struct cam_sensor_acquire_dev { + uint32_t session_handle; + uint32_t device_handle; + uint32_t handle_type; + uint32_t reserved; + uint64_t info_handle; +} __attribute__((packed)); + +/** + * cam_sensor_streamon_dev : StreamOn command for the sensor + * @session_handle : Session handle for acquiring device + * @device_handle : Updates device handle + * @handle_type : Resource handle type + * @reserved + * @info_handle : Information Needed at the time of streamOn + * + */ +struct cam_sensor_streamon_dev { + uint32_t session_handle; + uint32_t device_handle; + uint32_t handle_type; + uint32_t reserved; + uint64_t info_handle; +} __attribute__((packed)); + +/** + * struct cam_flash_init : Init command for the flash + * @flash_type : flash hw type + * @reserved + * @cmd_type : command buffer type + */ +struct cam_flash_init { + uint8_t flash_type; + uint16_t reserved; + uint8_t cmd_type; +} __attribute__((packed)); + +/** + * struct cam_flash_set_rer : RedEyeReduction command buffer + * + * @count : Number of flash leds + * @opcode : Command buffer opcode + * CAM_FLASH_FIRE_RER + * @cmd_type : command buffer operation type + * @num_iteration : Number of led turn on/off sequence + * @reserved + * @led_on_delay_ms : flash led turn on time in ms + * @led_off_delay_ms : flash led turn off time in ms + * @led_current_ma : flash led current in ma + * + */ +struct cam_flash_set_rer { + uint16_t count; + uint8_t opcode; + uint8_t cmd_type; + uint16_t num_iteration; + uint16_t reserved; + uint32_t led_on_delay_ms; + uint32_t led_off_delay_ms; + uint32_t led_current_ma[CAM_FLASH_MAX_LED_TRIGGERS]; +} __attribute__((packed)); + +/** + * struct cam_flash_set_on_off : led turn on/off command buffer + * + * @count : Number of Flash leds + * @opcode : command buffer opcodes + * CAM_FLASH_FIRE_LOW + * CAM_FLASH_FIRE_HIGH + * CAM_FLASH_OFF + * @cmd_type : command buffer operation type + * @led_current_ma : flash led current in ma + * + */ +struct cam_flash_set_on_off { + uint16_t count; + uint8_t opcode; + uint8_t cmd_type; + uint32_t led_current_ma[CAM_FLASH_MAX_LED_TRIGGERS]; +} __attribute__((packed)); + +/** + * struct cam_flash_query_curr : query current command buffer + * + * @reserved + * @opcode : command buffer opcode + * @cmd_type : command buffer operation type + * @query_current_ma : battery current in ma + * + */ +struct cam_flash_query_curr { + uint16_t reserved; + uint8_t opcode; + uint8_t cmd_type; + uint32_t query_current_ma; +} __attribute__ ((packed)); + +/** + * struct cam_flash_query_cap : capabilities info for flash + * + * @slot_info : Indicates about the slotId or cell Index + * @max_current_flash : max supported current for flash + * @max_duration_flash : max flash turn on duration + * @max_current_torch : max supported current for torch + * + */ +struct cam_flash_query_cap_info { + uint32_t slot_info; + uint32_t max_current_flash[CAM_FLASH_MAX_LED_TRIGGERS]; + uint32_t max_duration_flash[CAM_FLASH_MAX_LED_TRIGGERS]; + uint32_t max_current_torch[CAM_FLASH_MAX_LED_TRIGGERS]; +} __attribute__ ((packed)); + +#endif diff --git a/selfdrive/camerad/include/media/cam_sensor_cmn_header.h b/selfdrive/camerad/include/media/cam_sensor_cmn_header.h new file mode 100644 index 0000000000..24617b39bd --- /dev/null +++ b/selfdrive/camerad/include/media/cam_sensor_cmn_header.h @@ -0,0 +1,391 @@ +/* Copyright (c) 2017-2018, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#ifndef _CAM_SENSOR_CMN_HEADER_ +#define _CAM_SENSOR_CMN_HEADER_ + +#include +#include +#include + +#define MAX_REGULATOR 5 +#define MAX_POWER_CONFIG 12 + +#define MAX_PER_FRAME_ARRAY 32 +#define BATCH_SIZE_MAX 16 + +#define CAM_SENSOR_NAME "cam-sensor" +#define CAM_ACTUATOR_NAME "cam-actuator" +#define CAM_CSIPHY_NAME "cam-csiphy" +#define CAM_FLASH_NAME "cam-flash" +#define CAM_EEPROM_NAME "cam-eeprom" +#define CAM_OIS_NAME "cam-ois" + +#define MAX_SYSTEM_PIPELINE_DELAY 2 + +#define CAM_PKT_NOP_OPCODE 127 + +enum camera_sensor_cmd_type { + CAMERA_SENSOR_CMD_TYPE_INVALID, + CAMERA_SENSOR_CMD_TYPE_PROBE, + CAMERA_SENSOR_CMD_TYPE_PWR_UP, + CAMERA_SENSOR_CMD_TYPE_PWR_DOWN, + CAMERA_SENSOR_CMD_TYPE_I2C_INFO, + CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR, + CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_RD, + CAMERA_SENSOR_CMD_TYPE_I2C_CONT_WR, + CAMERA_SENSOR_CMD_TYPE_I2C_CONT_RD, + CAMERA_SENSOR_CMD_TYPE_WAIT, + CAMERA_SENSOR_FLASH_CMD_TYPE_INIT_INFO, + CAMERA_SENSOR_FLASH_CMD_TYPE_FIRE, + CAMERA_SENSOR_FLASH_CMD_TYPE_RER, + CAMERA_SENSOR_FLASH_CMD_TYPE_QUERYCURR, + CAMERA_SENSOR_FLASH_CMD_TYPE_WIDGET, + CAMERA_SENSOR_CMD_TYPE_RD_DATA, + CAMERA_SENSOR_FLASH_CMD_TYPE_INIT_FIRE, + CAMERA_SENSOR_CMD_TYPE_MAX, +}; + +enum camera_sensor_i2c_op_code { + CAMERA_SENSOR_I2C_OP_INVALID, + CAMERA_SENSOR_I2C_OP_RNDM_WR, + CAMERA_SENSOR_I2C_OP_RNDM_WR_VERF, + CAMERA_SENSOR_I2C_OP_CONT_WR_BRST, + CAMERA_SENSOR_I2C_OP_CONT_WR_BRST_VERF, + CAMERA_SENSOR_I2C_OP_CONT_WR_SEQN, + CAMERA_SENSOR_I2C_OP_CONT_WR_SEQN_VERF, + CAMERA_SENSOR_I2C_OP_MAX, +}; + +enum camera_sensor_wait_op_code { + CAMERA_SENSOR_WAIT_OP_INVALID, + CAMERA_SENSOR_WAIT_OP_COND, + CAMERA_SENSOR_WAIT_OP_HW_UCND, + CAMERA_SENSOR_WAIT_OP_SW_UCND, + CAMERA_SENSOR_WAIT_OP_MAX, +}; + +enum camera_flash_opcode { + CAMERA_SENSOR_FLASH_OP_INVALID, + CAMERA_SENSOR_FLASH_OP_OFF, + CAMERA_SENSOR_FLASH_OP_FIRELOW, + CAMERA_SENSOR_FLASH_OP_FIREHIGH, + CAMERA_SENSOR_FLASH_OP_MAX, +}; + +enum camera_sensor_i2c_type { + CAMERA_SENSOR_I2C_TYPE_INVALID, + CAMERA_SENSOR_I2C_TYPE_BYTE, + CAMERA_SENSOR_I2C_TYPE_WORD, + CAMERA_SENSOR_I2C_TYPE_3B, + CAMERA_SENSOR_I2C_TYPE_DWORD, + CAMERA_SENSOR_I2C_TYPE_MAX, +}; + +enum i2c_freq_mode { + I2C_STANDARD_MODE, + I2C_FAST_MODE, + I2C_CUSTOM_MODE, + I2C_FAST_PLUS_MODE, + I2C_MAX_MODES, +}; + +enum position_roll { + ROLL_0 = 0, + ROLL_90 = 90, + ROLL_180 = 180, + ROLL_270 = 270, + ROLL_INVALID = 360, +}; + +enum position_yaw { + FRONT_CAMERA_YAW = 0, + REAR_CAMERA_YAW = 180, + INVALID_YAW = 360, +}; + +enum position_pitch { + LEVEL_PITCH = 0, + INVALID_PITCH = 360, +}; + +enum sensor_sub_module { + SUB_MODULE_SENSOR, + SUB_MODULE_ACTUATOR, + SUB_MODULE_EEPROM, + SUB_MODULE_LED_FLASH, + SUB_MODULE_CSID, + SUB_MODULE_CSIPHY, + SUB_MODULE_OIS, + SUB_MODULE_EXT, + SUB_MODULE_MAX, +}; + +enum msm_camera_power_seq_type { + SENSOR_MCLK, + SENSOR_VANA, + SENSOR_VDIG, + SENSOR_VIO, + SENSOR_VAF, + SENSOR_VAF_PWDM, + SENSOR_CUSTOM_REG1, + SENSOR_CUSTOM_REG2, + SENSOR_RESET, + SENSOR_STANDBY, + SENSOR_CUSTOM_GPIO1, + SENSOR_CUSTOM_GPIO2, + SENSOR_SEQ_TYPE_MAX, +}; + +enum cam_sensor_packet_opcodes { + CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON, + CAM_SENSOR_PACKET_OPCODE_SENSOR_UPDATE, + CAM_SENSOR_PACKET_OPCODE_SENSOR_INITIAL_CONFIG, + CAM_SENSOR_PACKET_OPCODE_SENSOR_PROBE, + CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, + CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF, + CAM_SENSOR_PACKET_OPCODE_SENSOR_NOP = 127 +}; + +enum cam_actuator_packet_opcodes { + CAM_ACTUATOR_PACKET_OPCODE_INIT, + CAM_ACTUATOR_PACKET_AUTO_MOVE_LENS, + CAM_ACTUATOR_PACKET_MANUAL_MOVE_LENS +}; + +enum cam_eeprom_packet_opcodes { + CAM_EEPROM_PACKET_OPCODE_INIT +}; + +enum cam_ois_packet_opcodes { + CAM_OIS_PACKET_OPCODE_INIT, + CAM_OIS_PACKET_OPCODE_OIS_CONTROL +}; + +enum msm_bus_perf_setting { + S_INIT, + S_PREVIEW, + S_VIDEO, + S_CAPTURE, + S_ZSL, + S_STEREO_VIDEO, + S_STEREO_CAPTURE, + S_DEFAULT, + S_LIVESHOT, + S_DUAL, + S_EXIT +}; + +enum msm_camera_device_type_t { + MSM_CAMERA_I2C_DEVICE, + MSM_CAMERA_PLATFORM_DEVICE, + MSM_CAMERA_SPI_DEVICE, +}; + +enum cam_flash_device_type { + CAMERA_FLASH_DEVICE_TYPE_PMIC = 0, + CAMERA_FLASH_DEVICE_TYPE_I2C, + CAMERA_FLASH_DEVICE_TYPE_GPIO, +}; + +enum cci_i2c_master_t { + MASTER_0, + MASTER_1, + MASTER_MAX, +}; + +enum camera_vreg_type { + VREG_TYPE_DEFAULT, + VREG_TYPE_CUSTOM, +}; + +enum cam_sensor_i2c_cmd_type { + CAM_SENSOR_I2C_WRITE_RANDOM, + CAM_SENSOR_I2C_WRITE_BURST, + CAM_SENSOR_I2C_WRITE_SEQ, + CAM_SENSOR_I2C_READ, + CAM_SENSOR_I2C_POLL +}; + +struct common_header { + uint16_t first_word; + uint8_t third_byte; + uint8_t cmd_type; +}; + +struct camera_vreg_t { + const char *reg_name; + int min_voltage; + int max_voltage; + int op_mode; + uint32_t delay; + const char *custom_vreg_name; + enum camera_vreg_type type; +}; + +struct msm_camera_gpio_num_info { + uint16_t gpio_num[SENSOR_SEQ_TYPE_MAX]; + uint8_t valid[SENSOR_SEQ_TYPE_MAX]; +}; + +struct msm_cam_clk_info { + const char *clk_name; + long clk_rate; + uint32_t delay; +}; + +struct msm_pinctrl_info { + struct pinctrl *pinctrl; + struct pinctrl_state *gpio_state_active; + struct pinctrl_state *gpio_state_suspend; + bool use_pinctrl; +}; + +struct cam_sensor_i2c_reg_array { + uint32_t reg_addr; + uint32_t reg_data; + uint32_t delay; + uint32_t data_mask; +}; + +struct cam_sensor_i2c_reg_setting { + struct cam_sensor_i2c_reg_array *reg_setting; + unsigned short size; + enum camera_sensor_i2c_type addr_type; + enum camera_sensor_i2c_type data_type; + unsigned short delay; +}; + +/*struct i2c_settings_list { + struct cam_sensor_i2c_reg_setting i2c_settings; + enum cam_sensor_i2c_cmd_type op_code; + struct list_head list; +}; + +struct i2c_settings_array { + struct list_head list_head; + int32_t is_settings_valid; + int64_t request_id; +}; + +struct i2c_data_settings { + struct i2c_settings_array init_settings; + struct i2c_settings_array config_settings; + struct i2c_settings_array streamon_settings; + struct i2c_settings_array streamoff_settings; + struct i2c_settings_array *per_frame; +};*/ + +struct cam_sensor_power_ctrl_t { + struct device *dev; + struct cam_sensor_power_setting *power_setting; + uint16_t power_setting_size; + struct cam_sensor_power_setting *power_down_setting; + uint16_t power_down_setting_size; + struct msm_camera_gpio_num_info *gpio_num_info; + struct msm_pinctrl_info pinctrl_info; + uint8_t cam_pinctrl_status; +}; + +struct cam_camera_slave_info { + uint16_t sensor_slave_addr; + uint16_t sensor_id_reg_addr; + uint16_t sensor_id; + uint16_t sensor_id_mask; +}; + +struct msm_sensor_init_params { + int modes_supported; + unsigned int sensor_mount_angle; +}; + +enum msm_sensor_camera_id_t { + CAMERA_0, + CAMERA_1, + CAMERA_2, + CAMERA_3, + CAMERA_4, + CAMERA_5, + CAMERA_6, + MAX_CAMERAS, +}; + +struct msm_sensor_id_info_t { + unsigned short sensor_id_reg_addr; + unsigned short sensor_id; + unsigned short sensor_id_mask; +}; + +enum msm_sensor_output_format_t { + MSM_SENSOR_BAYER, + MSM_SENSOR_YCBCR, + MSM_SENSOR_META, +}; + +struct cam_sensor_power_setting { + enum msm_camera_power_seq_type seq_type; + unsigned short seq_val; + long config_val; + unsigned short delay; + void *data[10]; +}; + +struct cam_sensor_board_info { + struct cam_camera_slave_info slave_info; + int32_t sensor_mount_angle; + int32_t secure_mode; + int modes_supported; + int32_t pos_roll; + int32_t pos_yaw; + int32_t pos_pitch; + int32_t subdev_id[SUB_MODULE_MAX]; + int32_t subdev_intf[SUB_MODULE_MAX]; + const char *misc_regulator; + struct cam_sensor_power_ctrl_t power_info; +}; + +enum msm_camera_vreg_name_t { + CAM_VDIG, + CAM_VIO, + CAM_VANA, + CAM_VAF, + CAM_V_CUSTOM1, + CAM_V_CUSTOM2, + CAM_VREG_MAX, +}; + +struct msm_camera_gpio_conf { + void *cam_gpiomux_conf_tbl; + uint8_t cam_gpiomux_conf_tbl_size; + struct gpio *cam_gpio_common_tbl; + uint8_t cam_gpio_common_tbl_size; + struct gpio *cam_gpio_req_tbl; + uint8_t cam_gpio_req_tbl_size; + uint32_t gpio_no_mux; + uint32_t *camera_off_table; + uint8_t camera_off_table_size; + uint32_t *camera_on_table; + uint8_t camera_on_table_size; + struct msm_camera_gpio_num_info *gpio_num_info; +}; + +/*for tof camera Begin*/ +enum EEPROM_DATA_OP_T{ + EEPROM_DEFAULT_DATA = 0, + EEPROM_INIT_DATA, + EEPROM_CONFIG_DATA, + EEPROM_STREAMON_DATA, + EEPROM_STREAMOFF_DATA, + EEPROM_OTHER_DATA, +}; +/*for tof camera End*/ +#endif /* _CAM_SENSOR_CMN_HEADER_ */ diff --git a/selfdrive/camerad/include/media/cam_sync.h b/selfdrive/camerad/include/media/cam_sync.h new file mode 100644 index 0000000000..4a8781fc82 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_sync.h @@ -0,0 +1,134 @@ +#ifndef __UAPI_CAM_SYNC_H__ +#define __UAPI_CAM_SYNC_H__ + +#include +#include +#include +#include + +#define CAM_SYNC_DEVICE_NAME "cam_sync_device" + +/* V4L event which user space will subscribe to */ +#define CAM_SYNC_V4L_EVENT (V4L2_EVENT_PRIVATE_START + 0) + +/* Specific event ids to get notified in user space */ +#define CAM_SYNC_V4L_EVENT_ID_CB_TRIG 0 + +/* Size of opaque payload sent to kernel for safekeeping until signal time */ +#define CAM_SYNC_USER_PAYLOAD_SIZE 2 + +/* Device type for sync device needed for device discovery */ +#define CAM_SYNC_DEVICE_TYPE (MEDIA_ENT_F_OLD_BASE) + +#define CAM_SYNC_GET_PAYLOAD_PTR(ev, type) \ + (type *)((char *)ev.u.data + sizeof(struct cam_sync_ev_header)) + +#define CAM_SYNC_GET_HEADER_PTR(ev) \ + ((struct cam_sync_ev_header *)ev.u.data) + +#define CAM_SYNC_STATE_INVALID 0 +#define CAM_SYNC_STATE_ACTIVE 1 +#define CAM_SYNC_STATE_SIGNALED_SUCCESS 2 +#define CAM_SYNC_STATE_SIGNALED_ERROR 3 + +/** + * struct cam_sync_ev_header - Event header for sync event notification + * + * @sync_obj: Sync object + * @status: Status of the object + */ +struct cam_sync_ev_header { + int32_t sync_obj; + int32_t status; +}; + +/** + * struct cam_sync_info - Sync object creation information + * + * @name: Optional string representation of the sync object + * @sync_obj: Sync object returned after creation in kernel + */ +struct cam_sync_info { + char name[64]; + int32_t sync_obj; +}; + +/** + * struct cam_sync_signal - Sync object signaling struct + * + * @sync_obj: Sync object to be signaled + * @sync_state: State of the sync object to which it should be signaled + */ +struct cam_sync_signal { + int32_t sync_obj; + uint32_t sync_state; +}; + +/** + * struct cam_sync_merge - Merge information for sync objects + * + * @sync_objs: Pointer to sync objects + * @num_objs: Number of objects in the array + * @merged: Merged sync object + */ +struct cam_sync_merge { + __u64 sync_objs; + uint32_t num_objs; + int32_t merged; +}; + +/** + * struct cam_sync_userpayload_info - Payload info from user space + * + * @sync_obj: Sync object for which payload has to be registered for + * @reserved: Reserved + * @payload: Pointer to user payload + */ +struct cam_sync_userpayload_info { + int32_t sync_obj; + uint32_t reserved; + __u64 payload[CAM_SYNC_USER_PAYLOAD_SIZE]; +}; + +/** + * struct cam_sync_wait - Sync object wait information + * + * @sync_obj: Sync object to wait on + * @reserved: Reserved + * @timeout_ms: Timeout in milliseconds + */ +struct cam_sync_wait { + int32_t sync_obj; + uint32_t reserved; + uint64_t timeout_ms; +}; + +/** + * struct cam_private_ioctl_arg - Sync driver ioctl argument + * + * @id: IOCTL command id + * @size: Size of command payload + * @result: Result of command execution + * @reserved: Reserved + * @ioctl_ptr: Pointer to user data + */ +struct cam_private_ioctl_arg { + __u32 id; + __u32 size; + __u32 result; + __u32 reserved; + __u64 ioctl_ptr; +}; + +#define CAM_PRIVATE_IOCTL_CMD \ + _IOWR('V', BASE_VIDIOC_PRIVATE, struct cam_private_ioctl_arg) + +#define CAM_SYNC_CREATE 0 +#define CAM_SYNC_DESTROY 1 +#define CAM_SYNC_SIGNAL 2 +#define CAM_SYNC_MERGE 3 +#define CAM_SYNC_REGISTER_PAYLOAD 4 +#define CAM_SYNC_DEREGISTER_PAYLOAD 5 +#define CAM_SYNC_WAIT 6 + +#endif /* __UAPI_CAM_SYNC_H__ */ diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index bdce18de24..73f9a85dda 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -4,6 +4,8 @@ #ifdef QCOM #include "cameras/camera_qcom.h" +#elif QCOM2 +#include "cameras/camera_qcom2.h" #elif WEBCAM #include "cameras/camera_webcam.h" #else @@ -456,6 +458,8 @@ void* processing_thread(void *arg) { framed.setFocusConf(focus_confs); #endif +// TODO: add this back +//#if !defined(QCOM) && !defined(QCOM2) #ifndef QCOM framed.setImage(kj::arrayPtr((const uint8_t*)s->yuv_ion[yuv_idx].addr, s->yuv_buf_size)); #endif @@ -469,6 +473,8 @@ void* processing_thread(void *arg) { } } +#ifndef QCOM2 + // TODO: fix on QCOM2, giving scanline error // one thumbnail per 5 seconds (instead of %5 == 0 posenet) if (cnt % 100 == 3) { uint8_t* thumbnail_buffer = NULL; @@ -532,6 +538,7 @@ void* processing_thread(void *arg) { free(thumbnail_buffer); } +#endif tbuffer_dispatch(&s->ui_tb, ui_idx); @@ -873,15 +880,21 @@ cl_program build_debayer_program(VisionState *s, assert(rgb_width == frame_width/2); assert(rgb_height == frame_height/2); + #ifdef QCOM2 + int dnew = 1; + #else + int dnew = 0; + #endif + char args[4096]; snprintf(args, sizeof(args), "-cl-fast-relaxed-math -cl-denorms-are-zero " "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d " "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d " - "-DBAYER_FLIP=%d -DHDR=%d", + "-DBAYER_FLIP=%d -DHDR=%d -DNEW=%d", frame_width, frame_height, frame_stride, rgb_width, rgb_height, rgb_stride, - bayer_flip, hdr); + bayer_flip, hdr, dnew); return CLU_LOAD_FROM_FILE(s->context, s->device_id, "cameras/debayer.cl", args); } @@ -919,9 +932,11 @@ void init_buffers(VisionState *s) { for (int i=0; icamera_bufs[i] = visionbuf_allocate_cl(s->frame_size, s->device_id, s->context, &s->camera_bufs_cl[i]); - // TODO: make lengths correct - s->focus_bufs[i] = visionbuf_allocate(0xb80); - s->stats_bufs[i] = visionbuf_allocate(0xb80); + #ifndef QCOM2 + // TODO: make lengths correct + s->focus_bufs[i] = visionbuf_allocate(0xb80); + s->stats_bufs[i] = visionbuf_allocate(0xb80); + #endif } for (int i=0; iterminate_pub, 0); +#ifndef QCOM2 LOG("joining frontview_thread"); err = pthread_join(frontview_thread_handle, NULL); assert(err == 0); +#endif LOG("joining visionserver_thread"); err = pthread_join(visionserver_thread_handle, NULL); @@ -1126,7 +1146,7 @@ int main(int argc, char *argv[]) { init_buffers(s); -#ifdef QCOM +#if defined(QCOM) || defined(QCOM2) s->msg_context = Context::create(); s->frame_sock = PubSocket::create(s->msg_context, "frame"); s->front_frame_sock = PubSocket::create(s->msg_context, "frontFrame"); @@ -1140,7 +1160,7 @@ int main(int argc, char *argv[]) { party(s); -#ifdef QCOM +#if defined(QCOM) || defined(QCOM2) delete s->frame_sock; delete s->front_frame_sock; delete s->thumbnail_sock; diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript index 6f40e6a8f0..4d77e7e9e3 100644 --- a/selfdrive/common/SConscript +++ b/selfdrive/common/SConscript @@ -24,6 +24,12 @@ if arch == "aarch64": 'visionbuf_ion.c', ] _gpu_libs = ['gui', 'adreno_utils'] +elif arch == "larch64": + defines = {"CLU_NO_CACHE": None} + files += [ + 'visionbuf_ion.c', + ] + _gpu_libs = ['GL'] else: defines = {"CLU_NO_CACHE": None} files += [ diff --git a/selfdrive/common/visionbuf_ion.c b/selfdrive/common/visionbuf_ion.c index 724e75e9b0..e5dddfad22 100644 --- a/selfdrive/common/visionbuf_ion.c +++ b/selfdrive/common/visionbuf_ion.c @@ -1,8 +1,12 @@ #include #include +#include #include #include #include +#include +#include +#include #include #include diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 4c58e278f0..0872b653a9 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -11,6 +11,8 @@ common_src = [ if arch == "aarch64": libs += ['gsl', 'CB', 'gnustl_shared'] +elif arch == "larch64": + libs += ['gsl', 'CB', 'symphony-cpu', 'pthread'] else: libs += ['symphony-cpu', 'pthread'] diff --git a/selfdrive/modeld/dmonitoringmodeld b/selfdrive/modeld/dmonitoringmodeld index a7aa664a2a..51b520d743 100755 --- a/selfdrive/modeld/dmonitoringmodeld +++ b/selfdrive/modeld/dmonitoringmodeld @@ -1,5 +1,5 @@ #!/bin/sh -export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$HOME/openpilot/phonelibs/snpe/larch64:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/" exec ./_dmonitoringmodeld diff --git a/selfdrive/modeld/modeld b/selfdrive/modeld/modeld index 9de1569dec..5369034dfe 100755 --- a/selfdrive/modeld/modeld +++ b/selfdrive/modeld/modeld @@ -1,4 +1,4 @@ #!/bin/sh -export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$HOME/openpilot/phonelibs/snpe/larch64:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" exec ./_modeld diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index d3457c72a0..58464b5357 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -10,14 +10,14 @@ #define MODEL_HEIGHT 320 #define FULL_W 426 -#ifdef QCOM +#if defined(QCOM) || defined(QCOM2) #define input_lambda(x) (x - 128.f) * 0.0078125f #else #define input_lambda(x) x // for non SNPE running platforms, assume keras model instead has lambda layer #endif void dmonitoring_init(DMonitoringModelState* s) { -#ifdef QCOM +#if defined(QCOM) || defined(QCOM2) const char* model_path = "../../models/dmonitoring_model_q.dlc"; #else const char* model_path = "../../models/dmonitoring_model.dlc";