diff --git a/system/camerad/SConscript b/system/camerad/SConscript index f5cbc15748..8bafd83f08 100644 --- a/system/camerad/SConscript +++ b/system/camerad/SConscript @@ -2,8 +2,8 @@ Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc') libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', messaging, visionipc, gpucommon, 'atomic'] -camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc', - 'cameras/spectra.cc', 'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc']) +camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/spectra.cc', + 'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc']) env.Program('camerad', ['main.cc', camera_obj], LIBS=libs) if GetOption("extras") and arch == "x86_64": diff --git a/system/camerad/cameras/camera_util.cc b/system/camerad/cameras/camera_util.cc deleted file mode 100644 index 4d78556800..0000000000 --- a/system/camerad/cameras/camera_util.cc +++ /dev/null @@ -1,140 +0,0 @@ -#include "system/camerad/cameras/camera_util.h" - -#include - -#include - -#include -#include - -#include "common/swaglog.h" -#include "common/util.h" - -// For debugging: -// echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl - -// ************** low level camera helpers **************** -int do_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 = HANDLE_EINTR(ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol)); - if (ret == -1) { - LOGE("VIDIOC_CAM_CONTROL error: op_code %d - errno %d", op_code, errno); - } - return ret; -} - -std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources) { - struct cam_acquire_dev_cmd cmd = { - .session_handle = session_handle, - .handle_type = CAM_HANDLE_USER_POINTER, - .num_resources = (uint32_t)(data ? num_resources : 0), - .resource_hdl = (uint64_t)data, - }; - int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); - return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt; -} - -int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) { - struct cam_config_dev_cmd cmd = { - .session_handle = session_handle, - .dev_handle = dev_handle, - .packet_handle = packet_handle, - }; - return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd)); -} - -int device_control(int fd, int op_code, int session_handle, int dev_handle) { - // start stop and release are all the same - struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle }; - return do_cam_control(fd, op_code, &cmd, sizeof(cmd)); -} - -void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align, int flags, int mmu_hdl, int mmu_hdl2) { - 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++; - } - - do_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("allocated: %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 release(int video0_fd, uint32_t handle) { - struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0}; - mem_mgr_release_cmd.buf_handle = handle; - - int ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd)); - assert(ret == 0); -} - -// *** MemoryManager *** - -void *MemoryManager::alloc_buf(int size, uint32_t *handle) { - lock.lock(); - void *ptr; - if (!cached_allocations[size].empty()) { - ptr = cached_allocations[size].front(); - cached_allocations[size].pop(); - *handle = handle_lookup[ptr]; - } else { - ptr = alloc_w_mmu_hdl(video0_fd, size, handle); - handle_lookup[ptr] = *handle; - size_lookup[ptr] = size; - } - lock.unlock(); - memset(ptr, 0, size); - return ptr; -} - -void MemoryManager::free(void *ptr) { - lock.lock(); - cached_allocations[size_lookup[ptr]].push(ptr); - lock.unlock(); -} - -MemoryManager::~MemoryManager() { - for (auto& x : cached_allocations) { - while (!x.second.empty()) { - void *ptr = x.second.front(); - x.second.pop(); - LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]); - munmap(ptr, size_lookup[ptr]); - - // release fd - close(handle_lookup[ptr] >> 16); - release(video0_fd, handle_lookup[ptr]); - - handle_lookup.erase(ptr); - size_lookup.erase(ptr); - } - } -} diff --git a/system/camerad/cameras/camera_util.h b/system/camerad/cameras/camera_util.h deleted file mode 100644 index a2e7bfb434..0000000000 --- a/system/camerad/cameras/camera_util.h +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include - -std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1); -int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle); -int device_control(int fd, int op_code, int session_handle, int dev_handle); -int do_cam_control(int fd, int op_code, void *handle, int size); -void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, - int mmu_hdl = 0, int mmu_hdl2 = 0); -void release(int video0_fd, uint32_t handle); - -class MemoryManager { -public: - void init(int _video0_fd) { video0_fd = _video0_fd; } - ~MemoryManager(); - - template - auto alloc(int len, uint32_t *handle) { - return std::unique_ptr>((T*)alloc_buf(len, handle), [this](void *ptr) { this->free(ptr); }); - } - -private: - void *alloc_buf(int len, uint32_t *handle); - void free(void *ptr); - - std::mutex lock; - std::map handle_lookup; - std::map size_lookup; - std::map > cached_allocations; - int video0_fd; -}; diff --git a/system/camerad/cameras/spectra.cc b/system/camerad/cameras/spectra.cc index 609d77bf0f..42aa32c8b0 100644 --- a/system/camerad/cameras/spectra.cc +++ b/system/camerad/cameras/spectra.cc @@ -1,9 +1,7 @@ -#include - +#include #include -#include -#include -#include +#include +#include #include "media/cam_defs.h" #include "media/cam_isp.h" @@ -14,11 +12,94 @@ #include "common/util.h" #include "common/swaglog.h" - #include "system/camerad/cameras/spectra.h" +// For debugging: +// echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl + +// ************** low level camera helpers **************** + +int do_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 = HANDLE_EINTR(ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol)); + if (ret == -1) { + LOGE("VIDIOC_CAM_CONTROL error: op_code %d - errno %d", op_code, errno); + } + return ret; +} + +std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources) { + struct cam_acquire_dev_cmd cmd = { + .session_handle = session_handle, + .handle_type = CAM_HANDLE_USER_POINTER, + .num_resources = (uint32_t)(data ? num_resources : 0), + .resource_hdl = (uint64_t)data, + }; + int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); + return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt; +} + +int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) { + struct cam_config_dev_cmd cmd = { + .session_handle = session_handle, + .dev_handle = dev_handle, + .packet_handle = packet_handle, + }; + return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd)); +} + +int device_control(int fd, int op_code, int session_handle, int dev_handle) { + // start stop and release are all the same + struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle }; + return do_cam_control(fd, op_code, &cmd, sizeof(cmd)); +} + +void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align, int flags, int mmu_hdl, int mmu_hdl2) { + 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++; + } + + do_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("allocated: %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 release(int video0_fd, uint32_t handle) { + struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0}; + mem_mgr_release_cmd.buf_handle = handle; -// *** helpers *** + int ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd)); + assert(ret == 0); +} static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { cam_cmd_unconditional_wait *unconditional_wait = (cam_cmd_unconditional_wait *)((char *)power + (sizeof(struct cam_cmd_power) + (power->count - 1) * sizeof(struct cam_power_settings))); @@ -28,6 +109,49 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { return (struct cam_cmd_power *)(unconditional_wait + 1); } +// *** MemoryManager *** + +void *MemoryManager::alloc_buf(int size, uint32_t *handle) { + lock.lock(); + void *ptr; + if (!cached_allocations[size].empty()) { + ptr = cached_allocations[size].front(); + cached_allocations[size].pop(); + *handle = handle_lookup[ptr]; + } else { + ptr = alloc_w_mmu_hdl(video0_fd, size, handle); + handle_lookup[ptr] = *handle; + size_lookup[ptr] = size; + } + lock.unlock(); + memset(ptr, 0, size); + return ptr; +} + +void MemoryManager::free(void *ptr) { + lock.lock(); + cached_allocations[size_lookup[ptr]].push(ptr); + lock.unlock(); +} + +MemoryManager::~MemoryManager() { + for (auto& x : cached_allocations) { + while (!x.second.empty()) { + void *ptr = x.second.front(); + x.second.pop(); + LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]); + munmap(ptr, size_lookup[ptr]); + + // release fd + close(handle_lookup[ptr] >> 16); + release(video0_fd, handle_lookup[ptr]); + + handle_lookup.erase(ptr); + size_lookup.erase(ptr); + } + } +} + // *** SpectraMaster *** void SpectraMaster::init() { diff --git a/system/camerad/cameras/spectra.h b/system/camerad/cameras/spectra.h index baa5cbc32f..5c84d527dd 100644 --- a/system/camerad/cameras/spectra.h +++ b/system/camerad/cameras/spectra.h @@ -4,10 +4,10 @@ #include #include "media/cam_isp_ife.h" +#include "media/cam_req_mgr.h" #include "common/util.h" #include "system/camerad/cameras/tici.h" -#include "system/camerad/cameras/camera_util.h" #include "system/camerad/cameras/camera_common.h" #include "system/camerad/sensors/sensor.h" @@ -19,6 +19,35 @@ const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py // https://github.com/commaai/agnos-kernel-sdm845 +std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1); +int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle); +int device_control(int fd, int op_code, int session_handle, int dev_handle); +int do_cam_control(int fd, int op_code, void *handle, int size); +void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, + int mmu_hdl = 0, int mmu_hdl2 = 0); +void release(int video0_fd, uint32_t handle); + +class MemoryManager { +public: + void init(int _video0_fd) { video0_fd = _video0_fd; } + ~MemoryManager(); + + template + auto alloc(int len, uint32_t *handle) { + return std::unique_ptr>((T*)alloc_buf(len, handle), [this](void *ptr) { this->free(ptr); }); + } + +private: + void *alloc_buf(int len, uint32_t *handle); + void free(void *ptr); + + std::mutex lock; + std::map handle_lookup; + std::map size_lookup; + std::map > cached_allocations; + int video0_fd; +}; + class SpectraMaster { public: void init(); diff --git a/tools/replay/camera.cc b/tools/replay/camera.cc index ded966da6d..914c18e15f 100644 --- a/tools/replay/camera.cc +++ b/tools/replay/camera.cc @@ -1,7 +1,9 @@ #include "tools/replay/camera.h" -#include #include +#include + +#include #include "third_party/linux/include/msm_media_info.h" #include "tools/replay/util.h"