mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-29 02:43:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

camera_qcom2: move util functions to separate files (#25720)

Browse Source 
old-commit-hash: 1c8aa8816189b464329e12187a81eef85893a724
This commit is contained in:
Dean Lee
2022-09-11 05:22:43 +08:00
committed by GitHub
parent 8e260b430f
commit 158f9f0cde
6 changed files with 169 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files
release/files_tici
+2
View File
@@ -9,6 +9,8 @@ selfdrive/assets/training_wide/*
system/camerad/cameras/camera_qcom2.cc
system/camerad/cameras/camera_qcom2.h
system/camerad/cameras/camera_util.cc
system/camerad/cameras/camera_util.h
system/camerad/cameras/real_debayer.cl
selfdrive/sensord/rawgps/*
system/camerad/SConscript
+1
View File
@@ -10,6 +10,7 @@ if arch == "larch64":
env.Program('camerad', [
'main.cc',
'cameras/camera_common.cc',
'cameras/camera_util.cc',
'imgproc/utils.cc',
cameras,
], LIBS=libs)
system/camerad/cameras/camera_qcom2.cc
-127
View File
@@ -103,133 +103,6 @@ const int EXPOSURE_TIME_MAX_OX03C10 = 2016;
const uint32_t VS_TIME_MIN_OX03C10 = 1;
const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35
// ************** 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<int32_t> device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1) {
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 = 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) {
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) {
int ret;
struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0};
mem_mgr_release_cmd.buf_handle = handle;
ret = do_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);
}
void *MemoryManager::alloc(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();
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(video0_fd, handle_lookup[ptr]);
handle_lookup.erase(ptr);
size_lookup.erase(ptr);
}
}
}
int CameraState::clear_req_queue() {
struct cam_req_mgr_flush_info req_mgr_flush_request = {0};
req_mgr_flush_request.session_hdl = session_handle;
system/camerad/cameras/camera_qcom2.h
+1 -14
View File
@@ -7,25 +7,12 @@
#include <media/cam_req_mgr.h>
#include "system/camerad/cameras/camera_common.h"
#include "system/camerad/cameras/camera_util.h"
#include "common/params.h"
#include "common/util.h"
#define FRAME_BUF_COUNT 4
class MemoryManager {
public:
void init(int _video0_fd) { video0_fd = _video0_fd; }
void *alloc(int len, uint32_t *handle);
void free(void *ptr);
~MemoryManager();
private:
std::mutex lock;
std::map<void *, uint32_t> handle_lookup;
std::map<void *, int> size_lookup;
std::map<int, std::queue<void *> > cached_allocations;
int video0_fd;
};
class CameraState {
public:
MultiCameraState *multi_cam_state;
system/camerad/cameras/camera_util.cc
+135
View File
@@ -0,0 +1,135 @@
#include "system/camerad/cameras/camera_util.h"
#include <cassert>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include "common/swaglog.h"
#include "common/util.h"
// ************** 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<int32_t> 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) {
int ret;
struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0};
mem_mgr_release_cmd.buf_handle = handle;
ret = do_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);
}
void *MemoryManager::alloc(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();
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(video0_fd, handle_lookup[ptr]);
handle_lookup.erase(ptr);
size_lookup.erase(ptr);
}
}
}
system/camerad/cameras/camera_util.h
+30
View File
@@ -0,0 +1,30 @@
#pragma once
#include <map>
#include <mutex>
#include <optional>
#include <queue>
#include <media/cam_req_mgr.h>
std::optional<int32_t> 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; }
void *alloc(int len, uint32_t *handle);
void free(void *ptr);
~MemoryManager();
private:
std::mutex lock;
std::map<void *, uint32_t> handle_lookup;
std::map<void *, int> size_lookup;
std::map<int, std::queue<void *> > cached_allocations;
int video0_fd;
};