sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

camerad: less ioctls try 2 (#24371)

Browse Source 
* premap the buffers

* memory manager

* free buffers properly, alignment seems okay

* update camerad CPU usage

* cam_sync_fd

* useless line, and use the define

* cheap prereqs for multistream

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: b7ddce8bac
taco
George Hotz 4 years ago committed by GitHub
parent e7aac92b21
commit 3a6a5485f2
3 changed files with 113 additions and 66 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 160
      selfdrive/camerad/cameras/camera_qcom2.cc
  2. 17
      selfdrive/camerad/cameras/camera_qcom2.h
  3. 2
      selfdrive/test/test_onroad.py

160
selfdrive/camerad/cameras/camera_qcom2.cc
Unescape View File

@ -87,11 +87,11 @@ int do_cam_control(int fd, int op_code, void *handle, int size) {
return ret;
}
std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data) {
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 ? 1 : 0),
.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));
@ -158,6 +158,42 @@ void release_fd(int video0_fd, uint32_t handle) {
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;
@ -186,7 +222,7 @@ void CameraState::sensors_start() {
void CameraState::sensors_poke(int request_id) {
uint32_t cam_packet_handle = 0;
int size = sizeof(struct cam_packet);
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle);
pkt->num_cmd_buf = 0;
pkt->kmd_cmd_buf_index = -1;
pkt->header.size = size;
@ -200,15 +236,14 @@ void CameraState::sensors_poke(int request_id) {
return;
}
munmap(pkt, size);
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
mm.free(pkt);
}
void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) {
// 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 = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle);
pkt->num_cmd_buf = 1;
pkt->kmd_cmd_buf_index = -1;
pkt->header.size = size;
@ -218,7 +253,7 @@ void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op
buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload);
buf_desc[0].type = CAM_CMD_BUF_I2C;
struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
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;
@ -233,10 +268,8 @@ void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op
return;
}
munmap(i2c_random_wr, buf_desc[0].size);
release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
munmap(pkt, size);
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
mm.free(i2c_random_wr);
mm.free(pkt);
}
static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
@ -248,10 +281,9 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
};
int CameraState::sensors_init() {
int video0_fd = multi_cam_state->video0_fd;
uint32_t cam_packet_handle = 0;
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(video0_fd, size, &cam_packet_handle);
struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle);
pkt->num_cmd_buf = 2;
pkt->kmd_cmd_buf_index = -1;
pkt->header.op_code = 0x1000000 | CAM_SENSOR_PACKET_OPCODE_SENSOR_PROBE;
@ -260,7 +292,7 @@ int CameraState::sensors_init() {
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 = (struct cam_cmd_i2c_info *)alloc_w_mmu_hdl(video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
auto probe = (struct cam_cmd_probe *)(i2c_info + 1);
probe->camera_id = camera_num;
@ -302,7 +334,7 @@ int CameraState::sensors_init() {
//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_settings = (struct cam_cmd_power *)alloc_w_mmu_hdl(video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
struct cam_cmd_power *power_settings = (struct cam_cmd_power *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
memset(power_settings, 0, buf_desc[1].size);
// power on
@ -363,12 +395,9 @@ int CameraState::sensors_init() {
LOGD("probing the sensor");
int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
munmap(i2c_info, buf_desc[0].size);
release_fd(video0_fd, buf_desc[0].mem_handle);
munmap(power_settings, buf_desc[1].size);
release_fd(video0_fd, buf_desc[1].mem_handle);
munmap(pkt, size);
release_fd(video0_fd, cam_packet_handle);
mm.free(i2c_info);
mm.free(power_settings);
mm.free(pkt);
return ret;
}
@ -379,7 +408,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b
if (io_mem_handle != 0) {
size += sizeof(struct cam_buf_io_cfg);
}
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle);
pkt->num_cmd_buf = 2;
pkt->kmd_cmd_buf_index = 0;
// YUV has kmd_cmd_buf_offset = 1780
@ -387,7 +416,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b
// YUV also has patch_offset = 0x1030 and num_patches = 10
if (io_mem_handle != 0) {
pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*2;
pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf;
pkt->num_io_configs = 1;
}
@ -474,7 +503,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b
buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset;
buf_desc[1].type = CAM_CMD_BUF_GENERIC;
buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON;
uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
uint32_t *buf2 = (uint32_t *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
memcpy(buf2, &tmp, sizeof(tmp));
if (io_mem_handle != 0) {
@ -510,24 +539,20 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b
printf("ISP CONFIG FAILED\n");
}
munmap(buf2, buf_desc[1].size);
release_fd(multi_cam_state->video0_fd, buf_desc[1].mem_handle);
// release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
munmap(pkt, size);
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
mm.free(buf2);
mm.free(pkt);
}
void CameraState::enqueue_buffer(int i, bool dp) {
int ret;
int request_id = request_ids[i];
if (buf_handle[i]) {
release(multi_cam_state->video0_fd, buf_handle[i]);
if (buf_handle[i] && sync_objs[i]) {
// wait
struct cam_sync_wait sync_wait = {0};
sync_wait.sync_obj = sync_objs[i];
sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23
ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
// LOGD("fence wait: %d %d", ret, sync_wait.sync_obj);
buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
@ -535,13 +560,19 @@ void CameraState::enqueue_buffer(int i, bool dp) {
// destroy old output fence
struct cam_sync_info sync_destroy = {0};
strcpy(sync_destroy.name, "NodeOutputPortFence");
sync_destroy.sync_obj = sync_objs[i];
ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
// LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj);
}
// do stuff
// create output fence
struct cam_sync_info sync_create = {0};
strcpy(sync_create.name, "NodeOutputPortFence");
ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
// LOGD("fence req: %d %d", ret, sync_create.sync_obj);
sync_objs[i] = sync_create.sync_obj;
// schedule request with camera request manager
struct cam_req_mgr_sched_request req_mgr_sched_request = {0};
req_mgr_sched_request.session_hdl = session_handle;
req_mgr_sched_request.link_hdl = link_handle;
@ -549,28 +580,11 @@ void CameraState::enqueue_buffer(int i, bool dp) {
ret = do_cam_control(multi_cam_state->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
struct cam_sync_info sync_create = {0};
strcpy(sync_create.name, "NodeOutputPortFence");
ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
// LOGD("fence req: %d %d", ret, sync_create.sync_obj);
sync_objs[i] = sync_create.sync_obj;
// configure ISP to put the image in place
struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0};
mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu;
mem_mgr_map_cmd.num_hdl = 1;
mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE;
mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd;
ret = do_cam_control(multi_cam_state->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", bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
// poke sensor
// poke sensor, must happen after schedule
sensors_poke(request_id);
// LOGD("Poked sensor");
// push the buffer
// submit request to the ife
config_isp(buf_handle[i], sync_objs[i], request_id, buf0_handle, 65632*(i+1));
}
@ -616,6 +630,9 @@ void CameraState::camera_open() {
assert(sensor_fd >= 0);
LOGD("opened sensor for %d", camera_num);
// init memorymanager for this camera
mm.init(multi_cam_state->video0_fd);
// probe the sensor
LOGD("-- Probing sensor %d", camera_num);
ret = sensors_init();
@ -729,7 +746,7 @@ void CameraState::camera_open() {
{
uint32_t cam_packet_handle = 0;
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle);
pkt->num_cmd_buf = 1;
pkt->kmd_cmd_buf_index = -1;
pkt->header.size = size;
@ -738,7 +755,7 @@ void CameraState::camera_open() {
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 = (struct cam_csiphy_info *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)mm.alloc(buf_desc[0].size, (uint32_t*)&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
@ -751,10 +768,8 @@ void CameraState::camera_open() {
int ret_ = device_config(csiphy_fd, session_handle, csiphy_dev_handle, cam_packet_handle);
assert(ret_ == 0);
munmap(csiphy_info, buf_desc[0].size);
release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
munmap(pkt, size);
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
mm.free(csiphy_info);
mm.free(pkt);
}
// link devices
@ -781,6 +796,18 @@ void CameraState::camera_open() {
ret = device_control(multi_cam_state->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle);
LOGD("start isp: %d", ret);
for (int i = 0; i < FRAME_BUF_COUNT; i++) {
// configure ISP to put the image in place
struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0};
mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu;
mem_mgr_map_cmd.num_hdl = 1;
mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE;
mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd;
ret = do_cam_control(multi_cam_state->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", buf.camera_bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
}
// TODO: this is unneeded, should we be doing the start i2c in a different way?
//ret = device_control(sensor_fd, CAM_START_DEV, session_handle, sensor_dev_handle);
//LOGD("start sensor: %d", ret);
@ -807,9 +834,9 @@ void cameras_open(MultiCameraState *s) {
LOGD("opened video0");
// video1 is cam_sync, the target of some ioctls
s->video1_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK));
assert(s->video1_fd >= 0);
LOGD("opened video1");
s->cam_sync_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK));
assert(s->cam_sync_fd >= 0);
LOGD("opened video1 (cam_sync)");
// looks like there's only one of these
s->isp_fd = open_v4l_by_name_and_index("cam-isp");
@ -834,7 +861,7 @@ void cameras_open(MultiCameraState *s) {
LOG("-- Subscribing");
static struct v4l2_event_subscription sub = {0};
sub.type = V4L_EVENT_CAM_REQ_MGR_EVENT;
sub.id = 2; // should use boot time for sof
sub.id = V4L_EVENT_CAM_REQ_MGR_SOF_BOOT_TS;
ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub));
printf("req mgr subscribe: %d\n", ret);
@ -883,6 +910,11 @@ void CameraState::camera_close() {
LOGD("release isp: %d", ret);
ret = device_control(csiphy_fd, CAM_RELEASE_DEV, session_handle, csiphy_dev_handle);
LOGD("release csiphy: %d", ret);
for (int i = 0; i < FRAME_BUF_COUNT; i++) {
release(multi_cam_state->video0_fd, buf_handle[i]);
}
LOGD("released buffers");
}
ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle);

17
selfdrive/camerad/cameras/camera_qcom2.h
Unescape View File

@ -9,6 +9,20 @@
#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;
@ -60,6 +74,7 @@ public:
int camera_id;
CameraBuf buf;
MemoryManager mm;
private:
void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset);
void enqueue_req_multi(int start, int n, bool dp);
@ -73,7 +88,7 @@ private:
typedef struct MultiCameraState {
unique_fd video0_fd;
unique_fd video1_fd;
unique_fd cam_sync_fd;
unique_fd isp_fd;
int device_iommu;
int cdm_iommu;

2
selfdrive/test/test_onroad.py
Unescape View File

@ -23,7 +23,7 @@ from tools.lib.logreader import LogReader
PROCS = {
"selfdrive.controls.controlsd": 31.0,
"./loggerd": 70.0,
"./camerad": 41.0,
"./camerad": 26.0,
"./locationd": 9.1,
"selfdrive.controls.plannerd": 11.7,
"./_ui": 18.4,

Write Preview
Loading…
Cancel
Save