Prepare for using the ISP (#23621)
* parse out isp packet with structs * dsp mode * support only driver / comments for yuv * minor touchups * DEBUG_FRAMES Co-authored-by: Comma Device <device@comma.ai>pull/23636/head
George Hotz
GitHub
parent
013cccf6a8
commit
cb6a68373b
|
|
@ -41,6 +41,11 @@ const bool env_send_driver = getenv("SEND_DRIVER") != NULL;
|
|||
const bool env_send_road = getenv("SEND_ROAD") != NULL;
|
||||
const bool env_send_wide_road = getenv("SEND_WIDE_ROAD") != NULL;
|
||||
|
||||
// for debugging
|
||||
// note: ONLY_ROAD doesn't work, likely due to a mixup with wideRoad cam in the kernel
|
||||
const bool env_only_driver = getenv("ONLY_DRIVER") != NULL;
|
||||
const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL;
|
||||
|
||||
typedef void (*release_cb)(void *cookie, int buf_idx);
|
||||
|
||||
typedef struct CameraInfo {
|
||||
|
|
|
|||
|
|
@ -60,7 +60,7 @@ 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) {
|
||||
if (size == 0) {
|
||||
camcontrol.size = 8;
|
||||
camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE;
|
||||
} else {
|
||||
|
|
@ -353,6 +353,9 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
|
|||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 2;
|
||||
pkt->kmd_cmd_buf_index = 0;
|
||||
// YUV has kmd_cmd_buf_offset = 1780
|
||||
// I guess this is the ISP command
|
||||
// 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;
|
||||
|
|
@ -377,28 +380,65 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
|
|||
buf_desc[0].mem_handle = buf0_mem_handle;
|
||||
buf_desc[0].offset = buf0_offset;
|
||||
|
||||
// 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};
|
||||
// parsed by cam_isp_packet_generic_blob_handler
|
||||
struct isp_packet {
|
||||
uint32_t type_0;
|
||||
cam_isp_resource_hfr_config resource_hfr;
|
||||
|
||||
uint32_t type_1;
|
||||
cam_isp_clock_config clock;
|
||||
uint64_t extra_rdi_hz[3];
|
||||
|
||||
uint32_t type_2;
|
||||
cam_isp_bw_config bw;
|
||||
struct cam_isp_bw_vote extra_rdi_vote[6];
|
||||
} __attribute__((packed)) tmp;
|
||||
memset(&tmp, 0, sizeof(tmp));
|
||||
|
||||
tmp.type_0 = CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG;
|
||||
tmp.type_0 |= sizeof(cam_isp_resource_hfr_config) << 8;
|
||||
static_assert(sizeof(cam_isp_resource_hfr_config) == 0x20);
|
||||
tmp.resource_hfr = {
|
||||
.num_ports = 1, // 10 for YUV (but I don't think we need them)
|
||||
.port_hfr_config[0] = {
|
||||
.resource_type = CAM_ISP_IFE_OUT_RES_RDI_0, // CAM_ISP_IFE_OUT_RES_FULL for YUV
|
||||
.subsample_pattern = 1,
|
||||
.subsample_period = 0,
|
||||
.framedrop_pattern = 1,
|
||||
.framedrop_period = 0,
|
||||
}};
|
||||
|
||||
tmp.type_1 = CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG;
|
||||
tmp.type_1 |= (sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) << 8;
|
||||
static_assert((sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) == 0x38);
|
||||
tmp.clock = {
|
||||
.usage_type = 1, // dual mode
|
||||
.num_rdi = 4,
|
||||
.left_pix_hz = 404000000,
|
||||
.right_pix_hz = 404000000,
|
||||
.rdi_hz[0] = 404000000,
|
||||
};
|
||||
|
||||
|
||||
tmp.type_2 = CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG;
|
||||
tmp.type_2 |= (sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) << 8;
|
||||
static_assert((sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) == 0xe0);
|
||||
tmp.bw = {
|
||||
.usage_type = 1, // dual mode
|
||||
.num_rdi = 4,
|
||||
.left_pix_vote = {
|
||||
.resource_id = 0,
|
||||
.cam_bw_bps = 450000000,
|
||||
.ext_bw_bps = 450000000,
|
||||
},
|
||||
.rdi_vote[0] = {
|
||||
.resource_id = 0,
|
||||
.cam_bw_bps = 8706200000,
|
||||
.ext_bw_bps = 8706200000,
|
||||
},
|
||||
};
|
||||
|
||||
static_assert(offsetof(struct isp_packet, type_2) == 0x60);
|
||||
|
||||
buf_desc[1].size = sizeof(tmp);
|
||||
buf_desc[1].offset = io_mem_handle != 0 ? 0x60 : 0;
|
||||
|
|
@ -406,7 +446,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
|
|||
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(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
|
||||
memcpy(buf2, tmp, sizeof(tmp));
|
||||
memcpy(buf2, &tmp, sizeof(tmp));
|
||||
|
||||
if (io_mem_handle != 0) {
|
||||
io_cfg[0].mem_handle[0] = io_mem_handle;
|
||||
|
|
@ -415,19 +455,20 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
|
|||
.height = FRAME_HEIGHT,
|
||||
.plane_stride = FRAME_STRIDE,
|
||||
.slice_height = FRAME_HEIGHT,
|
||||
.meta_stride = 0x0,
|
||||
.meta_stride = 0x0, // YUV has meta(stride=0x400, size=0x5000)
|
||||
.meta_size = 0x0,
|
||||
.meta_offset = 0x0,
|
||||
.packer_config = 0x0,
|
||||
.mode_config = 0x0,
|
||||
.packer_config = 0x0, // 0xb for YUV
|
||||
.mode_config = 0x0, // 0x9ef for YUV
|
||||
.tile_config = 0x0,
|
||||
.h_init = 0x0,
|
||||
.v_init = 0x0,
|
||||
};
|
||||
io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10;
|
||||
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].format = CAM_FORMAT_MIPI_RAW_10; // CAM_FORMAT_UBWC_TP10 for YUV
|
||||
io_cfg[0].color_space = CAM_COLOR_SPACE_BASE; // CAM_COLOR_SPACE_BT601_FULL for YUV
|
||||
io_cfg[0].color_pattern = 0x5; // 0x0 for YUV
|
||||
io_cfg[0].bpp = 0xa;
|
||||
io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; // CAM_ISP_IFE_OUT_RES_FULL for YUV
|
||||
io_cfg[0].fence = fence;
|
||||
io_cfg[0].direction = CAM_BUF_OUTPUT;
|
||||
io_cfg[0].subsample_pattern = 0x1;
|
||||
|
|
@ -553,18 +594,14 @@ int open_v4l_by_name_and_index(const char name[], int index, int flags = O_RDWR
|
|||
static void camera_open(CameraState *s) {
|
||||
s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num);
|
||||
assert(s->sensor_fd >= 0);
|
||||
LOGD("opened sensor");
|
||||
|
||||
s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num);
|
||||
assert(s->csiphy_fd >= 0);
|
||||
LOGD("opened csiphy");
|
||||
LOGD("opened sensor for %d", s->camera_num);
|
||||
|
||||
// probe the sensor
|
||||
LOGD("-- Probing sensor %d", s->camera_num);
|
||||
sensors_init(s->multi_cam_state->video0_fd, s->sensor_fd, s->camera_num);
|
||||
|
||||
// create session
|
||||
struct cam_req_mgr_session_info session_info = {};
|
||||
struct cam_req_mgr_session_info session_info = {};
|
||||
int ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
|
||||
LOGD("get session: %d 0x%X", ret, session_info.session_hdl);
|
||||
s->session_handle = session_info.session_hdl;
|
||||
|
|
@ -605,7 +642,7 @@ static void camera_open(CameraState *s) {
|
|||
|
||||
.pixel_clk = 0x0,
|
||||
.batch_size = 0x0,
|
||||
.dsp_mode = 0x0,
|
||||
.dsp_mode = CAM_ISP_DSP_MODE_NONE,
|
||||
.hbi_cnt = 0x0,
|
||||
.custom_csid = 0x0,
|
||||
|
||||
|
|
@ -627,9 +664,13 @@ static void camera_open(CameraState *s) {
|
|||
|
||||
auto isp_dev_handle = device_acquire(s->multi_cam_state->isp_fd, s->session_handle, &isp_resource);
|
||||
assert(isp_dev_handle);
|
||||
s->isp_dev_handle = *isp_dev_handle;
|
||||
s->isp_dev_handle = *isp_dev_handle;
|
||||
LOGD("acquire isp dev");
|
||||
|
||||
s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num);
|
||||
assert(s->csiphy_fd >= 0);
|
||||
LOGD("opened csiphy for %d", s->camera_num);
|
||||
|
||||
struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0};
|
||||
auto csiphy_dev_handle = device_acquire(s->csiphy_fd, s->session_handle, &csiphy_acquire_dev_info);
|
||||
assert(csiphy_dev_handle);
|
||||
|
|
@ -645,6 +686,7 @@ static void camera_open(CameraState *s) {
|
|||
//sensors_i2c(s, start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
|
||||
//sensors_i2c(s, stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
|
||||
|
||||
|
||||
// config csiphy
|
||||
LOG("-- Config CSI PHY");
|
||||
{
|
||||
|
|
@ -686,8 +728,8 @@ static void camera_open(CameraState *s) {
|
|||
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->multi_cam_state->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;
|
||||
LOGD("link: %d hdl: 0x%X", ret, s->link_handle);
|
||||
|
||||
struct cam_req_mgr_link_control req_mgr_link_control = {0};
|
||||
req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE;
|
||||
|
|
@ -708,15 +750,17 @@ static void camera_open(CameraState *s) {
|
|||
}
|
||||
|
||||
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
|
||||
camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
|
||||
printf("road camera initted \n");
|
||||
camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
|
||||
printf("wide road camera initted \n");
|
||||
camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
|
||||
printf("driver camera initted \n");
|
||||
if (!env_only_driver) {
|
||||
camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
|
||||
printf("road camera initted \n");
|
||||
camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
|
||||
printf("wide road camera initted \n");
|
||||
}
|
||||
|
||||
s->sm = new SubMaster({"driverState"});
|
||||
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"});
|
||||
|
|
@ -763,12 +807,14 @@ void cameras_open(MultiCameraState *s) {
|
|||
ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub));
|
||||
printf("req mgr subscribe: %d\n", ret);
|
||||
|
||||
camera_open(&s->road_cam);
|
||||
printf("road camera opened \n");
|
||||
camera_open(&s->wide_road_cam);
|
||||
printf("wide road camera opened \n");
|
||||
camera_open(&s->driver_cam);
|
||||
printf("driver camera opened \n");
|
||||
if (!env_only_driver) {
|
||||
camera_open(&s->road_cam);
|
||||
printf("road camera opened \n");
|
||||
camera_open(&s->wide_road_cam);
|
||||
printf("wide road camera opened \n");
|
||||
}
|
||||
}
|
||||
|
||||
static void camera_close(CameraState *s) {
|
||||
|
|
@ -816,9 +862,11 @@ static void camera_close(CameraState *s) {
|
|||
}
|
||||
|
||||
void cameras_close(MultiCameraState *s) {
|
||||
camera_close(&s->road_cam);
|
||||
camera_close(&s->wide_road_cam);
|
||||
camera_close(&s->driver_cam);
|
||||
if (!env_only_driver) {
|
||||
camera_close(&s->road_cam);
|
||||
camera_close(&s->wide_road_cam);
|
||||
}
|
||||
|
||||
delete s->sm;
|
||||
delete s->pm;
|
||||
|
|
@ -1010,16 +1058,20 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
|
|||
void cameras_run(MultiCameraState *s) {
|
||||
LOG("-- Starting threads");
|
||||
std::vector<std::thread> threads;
|
||||
threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
|
||||
threads.push_back(start_process_thread(s, &s->driver_cam, common_process_driver_camera));
|
||||
threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera));
|
||||
if (!env_only_driver) {
|
||||
threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
|
||||
threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera));
|
||||
}
|
||||
|
||||
// start devices
|
||||
LOG("-- Starting devices");
|
||||
int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload);
|
||||
sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
sensors_i2c(&s->driver_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
if (!env_only_driver) {
|
||||
sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
}
|
||||
|
||||
// poll events
|
||||
LOG("-- Dequeueing Video events");
|
||||
|
|
@ -1043,8 +1095,10 @@ void cameras_run(MultiCameraState *s) {
|
|||
if (ret == 0) {
|
||||
if (ev.type == V4L_EVENT_CAM_REQ_MGR_EVENT) {
|
||||
struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)ev.u.data;
|
||||
// LOGD("v4l2 event: sess_hdl %d, link_hdl %d, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
|
||||
// printf("sess_hdl %d, link_hdl %d, frame_id %lu, req_id %lu, timestamp 0x%lx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
|
||||
// LOGD("v4l2 event: sess_hdl 0x%X, link_hdl 0x%X, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
|
||||
if (env_debug_frames) {
|
||||
printf("sess_hdl 0x%X, link_hdl 0x%X, frame_id %lu, req_id %lu, timestamp 0x%lx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
|
||||
}
|
||||
|
||||
if (event_data->session_hdl == s->road_cam.session_handle) {
|
||||
handle_camera_event(&s->road_cam, event_data);
|
||||
|
|
|
|||
Loading…
Reference in New Issue