From 0c2a527b3e9a4c60c55109d384fe19edfd27911a Mon Sep 17 00:00:00 2001 From: George Hotz <72895+geohot@users.noreply.github.com> Date: Thu, 28 Apr 2022 11:32:32 -0700 Subject: [PATCH] camerad: don't remap everything every time (#24334) * 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 old-commit-hash: fb7d84875bc993beaf75645a7e4b311704742f02 --- selfdrive/camerad/cameras/camera_qcom2.cc | 160 +++++++++++++--------- selfdrive/camerad/cameras/camera_qcom2.h | 17 ++- selfdrive/test/test_onroad.py | 2 +- 3 files changed, 113 insertions(+), 66 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 8986351fc1..1d092eb6a6 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -87,11 +87,11 @@ int do_cam_control(int fd, int op_code, void *handle, int size) { return ret; } -std::optional device_acquire(int fd, int32_t session_handle, void *data) { +std::optional 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); diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h index 199aa40e71..a7c64c0665 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.h +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -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 handle_lookup; + std::map size_lookup; + std::map > 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; diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index c8c1a34375..b845ee0e09 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -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,