diff --git a/SConstruct b/SConstruct index c49f55c1e8..5ac384187a 100644 --- a/SConstruct +++ b/SConstruct @@ -17,6 +17,10 @@ AddOption('--asan', action='store_true', help='turn on ASAN') +AddOption('--ubsan', + action='store_true', + help='turn on UBSan') + AddOption('--clazy', action='store_true', help='build with clazy') @@ -121,11 +125,14 @@ else: rpath = [os.path.join(os.getcwd(), x) for x in rpath] if GetOption('asan'): - ccflags_asan = ["-fsanitize=address", "-fno-omit-frame-pointer"] - ldflags_asan = ["-fsanitize=address"] + ccflags = ["-fsanitize=address", "-fno-omit-frame-pointer"] + ldflags = ["-fsanitize=address"] +elif GetOption('ubsan'): + ccflags = ["-fsanitize=undefined"] + ldflags = ["-fsanitize=undefined"] else: - ccflags_asan = [] - ldflags_asan = [] + ccflags = [] + ldflags = [] # change pythonpath to this lenv["PYTHONPATH"] = Dir("#").path @@ -144,7 +151,7 @@ env = Environment( "-Wno-inconsistent-missing-override", "-Wno-c99-designator", "-Wno-reorder-init-list", - ] + cflags + ccflags_asan, + ] + cflags + ccflags, CPPPATH=cpppath + [ "#", @@ -177,7 +184,7 @@ env = Environment( CC='clang', CXX='clang++', - LINKFLAGS=ldflags_asan, + LINKFLAGS=ldflags, RPATH=rpath, diff --git a/release/files_common b/release/files_common index 9ad4c627fb..f15b8472f5 100644 --- a/release/files_common +++ b/release/files_common @@ -300,9 +300,9 @@ selfdrive/proclogd/SConscript selfdrive/proclogd/proclogd.cc selfdrive/loggerd/SConscript -selfdrive/loggerd/encoder.cc selfdrive/loggerd/encoder.h -selfdrive/loggerd/frame_logger.h +selfdrive/loggerd/omx_encoder.cc +selfdrive/loggerd/omx_encoder.h selfdrive/loggerd/logger.cc selfdrive/loggerd/logger.h selfdrive/loggerd/loggerd.cc diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index ed76f3f8dc..f99316255d 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -6,7 +6,7 @@ libs = ['zmq', 'capnp', 'kj', 'z', 'yuv', 'bz2', 'OpenCL', common, cereal, messaging, visionipc] if arch in ["aarch64", "larch64"]: - src += ['encoder.cc'] + src += ['omx_encoder.cc'] libs += ['OmxCore', 'gsl', 'CB'] + gpucommon if arch == "aarch64": libs += ['OmxVenc', 'cutils'] diff --git a/selfdrive/loggerd/encoder.h b/selfdrive/loggerd/encoder.h index bd402e7259..bda737920a 100644 --- a/selfdrive/loggerd/encoder.h +++ b/selfdrive/loggerd/encoder.h @@ -1,71 +1,18 @@ #pragma once -#include -#include -#include +#include -#include -#include +#include +#include -extern "C" { - #include -} - -#include "common/cqueue.h" #include "visionipc.h" -// encoder: lossey codec using hardware hevc - - -struct EncoderState { - pthread_mutex_t lock; - int width, height, fps; - const char* path; - char vid_path[1024]; - char lock_path[1024]; - bool open; - bool dirty; - int counter; - int segment; - - const char* filename; - FILE *of; - - size_t codec_config_len; - uint8_t *codec_config; - bool wrote_codec_config; - - pthread_mutex_t state_lock; - pthread_cond_t state_cv; - OMX_STATETYPE state; - - OMX_HANDLETYPE handle; - - int num_in_bufs; - OMX_BUFFERHEADERTYPE** in_buf_headers; - - int num_out_bufs; - OMX_BUFFERHEADERTYPE** out_buf_headers; - - uint64_t last_t; - - Queue free_in; - Queue done_out; - - AVFormatContext *ofmt_ctx; - AVCodecContext *codec_ctx; - AVStream *out_stream; - bool remuxing; - - bool downscale; - uint8_t *y_ptr2, *u_ptr2, *v_ptr2; +class VideoEncoder { +public: + virtual ~VideoEncoder() {} + virtual int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, + int in_width, int in_height, + int *frame_segment, VisionIpcBufExtra *extra) = 0; + virtual void encoder_open(const char* path, int segment) = 0; + virtual void encoder_close() = 0; }; - -void encoder_init(EncoderState *s, const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale); -int encoder_encode_frame(EncoderState *s, - const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width, int in_height, - int *frame_segment, VisionIpcBufExtra *extra); -void encoder_open(EncoderState *s, const char* path, int segment); -void encoder_close(EncoderState *s); -void encoder_destroy(EncoderState *s); diff --git a/selfdrive/loggerd/frame_logger.h b/selfdrive/loggerd/frame_logger.h deleted file mode 100644 index 85ddabafdc..0000000000 --- a/selfdrive/loggerd/frame_logger.h +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef FRAMELOGGER_H -#define FRAMELOGGER_H - -#include - -#include -#include - -class FrameLogger { -public: - virtual ~FrameLogger() {} - - virtual void Open(const std::string &path) = 0; - virtual void Close() = 0; - - int LogFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, int *frame_segment) { - std::lock_guard guard(lock); - - if (opening) { - Open(next_path); - opening = false; - } - - if (!is_open) return -1; - - if (rotating) { - Close(); - Open(next_path); - segment = next_segment; - rotating = false; - } - - int ret = ProcessFrame(ts, y_ptr, u_ptr, v_ptr); - - if (ret >= 0 && frame_segment) { - *frame_segment = segment; - } - - if (closing) { - Close(); - closing = false; - } - - return ret; - } - - void Rotate(const std::string &new_path, int new_segment) { - std::lock_guard guard(lock); - - next_path = new_path; - next_segment = new_segment; - if (is_open) { - if (next_segment == -1) { - closing = true; - } else { - rotating = true; - } - } else { - segment = next_segment; - opening = true; - } - } - -protected: - - virtual int ProcessFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr) = 0; - - std::recursive_mutex lock; - - bool is_open = false; - int segment = -1; - - std::string vid_path, lock_path; - -private: - int next_segment = -1; - bool opening = false, closing = false, rotating = false; - std::string next_path; -}; - -#endif diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 1b219d38e4..e745a7995c 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -41,8 +41,9 @@ #define DISABLE_ENCODER // TODO: loggerd for PC #endif -#ifndef DISABLE_ENCODER #include "encoder.h" +#ifndef DISABLE_ENCODER +#include "omx_encoder.h" #endif constexpr int MAIN_BITRATE = 5000000; @@ -199,7 +200,7 @@ void encoder_thread(int cam_idx) { int cnt = 0; LoggerHandle *lh = NULL; - std::vector encoders; + std::vector encoders; VisionIpcClient vipc_client = VisionIpcClient("camerad", cam_info.stream_type, false); while (!do_exit) { @@ -215,16 +216,15 @@ void encoder_thread(int cam_idx) { LOGD("encoder init %dx%d", buf_info.width, buf_info.height); // main encoder - encoders.push_back(new EncoderState()); - encoder_init(encoders[0], cam_info.filename, buf_info.width, buf_info.height, - cam_info.fps, cam_info.bitrate, cam_info.is_h265, cam_info.downscale); + encoders.push_back(new OmxEncoder(cam_info.filename, buf_info.width, buf_info.height, + cam_info.fps, cam_info.bitrate, cam_info.is_h265, cam_info.downscale)); // qcamera encoder if (cam_info.has_qcamera) { LogCameraInfo &qcam_info = cameras_logged[LOG_CAMERA_ID_QCAMERA]; - encoders.push_back(new EncoderState()); - encoder_init(encoders[1], qcam_info.filename, qcam_info.frame_width, qcam_info.frame_height, - qcam_info.fps, qcam_info.bitrate, qcam_info.is_h265, qcam_info.downscale); + encoders.push_back(new OmxEncoder(qcam_info.filename, + qcam_info.frame_width, qcam_info.frame_height, + qcam_info.fps, qcam_info.bitrate, qcam_info.is_h265, qcam_info.downscale)); } } @@ -263,8 +263,8 @@ void encoder_thread(int cam_idx) { pthread_mutex_lock(&s.rotate_lock); for (auto &e : encoders) { - encoder_close(e); - encoder_open(e, s.segment_path, s.rotate_segment); + e->encoder_close(); + e->encoder_open(s.segment_path, s.rotate_segment); } pthread_mutex_unlock(&s.rotate_lock); rotate_state.finish_rotate(); @@ -276,16 +276,14 @@ void encoder_thread(int cam_idx) { // encode a frame { int out_segment = -1; - int out_id = encoder_encode_frame(encoders[0], - buf->y, buf->u, buf->v, - buf->width, buf->height, - &out_segment, &extra); + int out_id = encoders[0]->encode_frame(buf->y, buf->u, buf->v, + buf->width, buf->height, + &out_segment, &extra); if (encoders.size() > 1) { int out_segment_alt = -1; - encoder_encode_frame(encoders[1], - buf->y, buf->u, buf->v, - buf->width, buf->height, - &out_segment_alt, &extra); + encoders[1]->encode_frame(buf->y, buf->u, buf->v, + buf->width, buf->height, + &out_segment_alt, &extra); } // publish encode index @@ -301,7 +299,6 @@ void encoder_thread(int cam_idx) { #else eidx.setType(cam_idx == LOG_CAMERA_ID_DCAMERA ? cereal::EncodeIndex::Type::FRONT : cereal::EncodeIndex::Type::FULL_H_E_V_C); #endif - eidx.setEncodeId(cnt); eidx.setSegmentNum(out_segment); eidx.setSegmentId(out_id); @@ -323,8 +320,7 @@ void encoder_thread(int cam_idx) { LOG("encoder destroy"); for(auto &e : encoders) { - encoder_close(e); - encoder_destroy(e); + e->encoder_close(); delete e; } } diff --git a/selfdrive/loggerd/encoder.cc b/selfdrive/loggerd/omx_encoder.cc similarity index 57% rename from selfdrive/loggerd/encoder.cc rename to selfdrive/loggerd/omx_encoder.cc index abe7a239f2..c3533a2849 100644 --- a/selfdrive/loggerd/encoder.cc +++ b/selfdrive/loggerd/omx_encoder.cc @@ -21,31 +21,37 @@ #include "common/mutex.h" #include "common/swaglog.h" -#include "encoder.h" +#include "omx_encoder.h" // ***** OMX callback functions ***** -static void wait_for_state(EncoderState *s, OMX_STATETYPE state) { - pthread_mutex_lock(&s->state_lock); - while (s->state != state) { - pthread_cond_wait(&s->state_cv, &s->state_lock); +void OmxEncoder::wait_for_state(OMX_STATETYPE state) { + pthread_mutex_lock(&this->state_lock); + while (this->state != state) { + pthread_cond_wait(&this->state_cv, &this->state_lock); } - pthread_mutex_unlock(&s->state_lock); + pthread_mutex_unlock(&this->state_lock); } -static OMX_ERRORTYPE event_handler(OMX_HANDLETYPE component, OMX_PTR app_data, OMX_EVENTTYPE event, +static OMX_CALLBACKTYPE omx_callbacks = { + .EventHandler = OmxEncoder::event_handler, + .EmptyBufferDone = OmxEncoder::empty_buffer_done, + .FillBufferDone = OmxEncoder::fill_buffer_done, +}; + +OMX_ERRORTYPE OmxEncoder::event_handler(OMX_HANDLETYPE component, OMX_PTR app_data, OMX_EVENTTYPE event, OMX_U32 data1, OMX_U32 data2, OMX_PTR event_data) { - EncoderState *s = (EncoderState*)app_data; + OmxEncoder *e = (OmxEncoder*)app_data; switch (event) { case OMX_EventCmdComplete: assert(data1 == OMX_CommandStateSet); LOG("set state event 0x%x", data2); - pthread_mutex_lock(&s->state_lock); - s->state = (OMX_STATETYPE)data2; - pthread_cond_broadcast(&s->state_cv); - pthread_mutex_unlock(&s->state_lock); + pthread_mutex_lock(&e->state_lock); + e->state = (OMX_STATETYPE)data2; + pthread_cond_broadcast(&e->state_cv); + pthread_mutex_unlock(&e->state_lock); break; case OMX_EventError: LOGE("OMX error 0x%08x", data1); @@ -60,28 +66,22 @@ static OMX_ERRORTYPE event_handler(OMX_HANDLETYPE component, OMX_PTR app_data, O return OMX_ErrorNone; } -static OMX_ERRORTYPE empty_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, - OMX_BUFFERHEADERTYPE *buffer) { +OMX_ERRORTYPE OmxEncoder::empty_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, + OMX_BUFFERHEADERTYPE *buffer) { // printf("empty_buffer_done\n"); - EncoderState *s = (EncoderState*)app_data; - queue_push(&s->free_in, (void*)buffer); + OmxEncoder *e = (OmxEncoder*)app_data; + queue_push(&e->free_in, (void*)buffer); return OMX_ErrorNone; } -static OMX_ERRORTYPE fill_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, - OMX_BUFFERHEADERTYPE *buffer) { +OMX_ERRORTYPE OmxEncoder::fill_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, + OMX_BUFFERHEADERTYPE *buffer) { // printf("fill_buffer_done\n"); - EncoderState *s = (EncoderState*)app_data; - queue_push(&s->done_out, (void*)buffer); + OmxEncoder *e = (OmxEncoder*)app_data; + queue_push(&e->done_out, (void*)buffer); return OMX_ErrorNone; } -static OMX_CALLBACKTYPE omx_callbacks = { - .EventHandler = event_handler, - .EmptyBufferDone = empty_buffer_done, - .FillBufferDone = fill_buffer_done, -}; - #define PORT_INDEX_IN 0 #define PORT_INDEX_OUT 1 @@ -158,92 +158,77 @@ static const char* omx_color_fomat_name(uint32_t format) { // ***** encoder functions ***** -void encoder_init(EncoderState *s, const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale) { +OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale) { int err; - memset(s, 0, sizeof(*s)); - s->filename = filename; - s->width = width; - s->height = height; - s->fps = fps; - mutex_init_reentrant(&s->lock); - - if (!h265) { - s->remuxing = true; - } - - if (downscale) { - s->downscale = true; - s->y_ptr2 = (uint8_t *)malloc(s->width*s->height); - s->u_ptr2 = (uint8_t *)malloc(s->width*s->height/4); - s->v_ptr2 = (uint8_t *)malloc(s->width*s->height/4); - } - - s->segment = -1; - - s->state = OMX_StateLoaded; + this->filename = filename; + this->width = width; + this->height = height; + this->fps = fps; + this->remuxing = !h265; - s->codec_config = NULL; + queue_init(&this->free_in); + queue_init(&this->done_out); - queue_init(&s->free_in); - queue_init(&s->done_out); + mutex_init_reentrant(&this->lock); + pthread_mutex_init(&this->state_lock, NULL); + pthread_cond_init(&this->state_cv, NULL); - pthread_mutex_init(&s->state_lock, NULL); - pthread_cond_init(&s->state_cv, NULL); - - if (h265) { - err = OMX_GetHandle(&s->handle, (OMX_STRING)"OMX.qcom.video.encoder.hevc", - s, &omx_callbacks); - } else { - err = OMX_GetHandle(&s->handle, (OMX_STRING)"OMX.qcom.video.encoder.avc", - s, &omx_callbacks); + this->downscale = downscale; + if (this->downscale) { + this->y_ptr2 = (uint8_t *)malloc(this->width*this->height); + this->u_ptr2 = (uint8_t *)malloc(this->width*this->height/4); + this->v_ptr2 = (uint8_t *)malloc(this->width*this->height/4); } + + auto component = (OMX_STRING)(h265 ? "OMX.qcom.video.encoder.hevc" : "OMX.qcom.video.encoder.avc"); + err = OMX_GetHandle(&this->handle, component, this, &omx_callbacks); if (err != OMX_ErrorNone) { LOGE("error getting codec: %x", err); } assert(err == OMX_ErrorNone); - // printf("handle: %p\n", s->handle); + // printf("handle: %p\n", this->handle); // setup input port OMX_PARAM_PORTDEFINITIONTYPE in_port = {0}; in_port.nSize = sizeof(in_port); in_port.nPortIndex = (OMX_U32) PORT_INDEX_IN; - err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition, + err = OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port); assert(err == OMX_ErrorNone); - in_port.format.video.nFrameWidth = s->width; - in_port.format.video.nFrameHeight = s->height; - in_port.format.video.nStride = VENUS_Y_STRIDE(COLOR_FMT_NV12, s->width); - in_port.format.video.nSliceHeight = s->height; - // in_port.nBufferSize = (s->width * s->height * 3) / 2; - in_port.nBufferSize = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, s->width, s->height); - in_port.format.video.xFramerate = (s->fps * 65536); + in_port.format.video.nFrameWidth = this->width; + in_port.format.video.nFrameHeight = this->height; + in_port.format.video.nStride = VENUS_Y_STRIDE(COLOR_FMT_NV12, this->width); + in_port.format.video.nSliceHeight = this->height; + // in_port.nBufferSize = (this->width * this->height * 3) / 2; + in_port.nBufferSize = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, this->width, this->height); + in_port.format.video.xFramerate = (this->fps * 65536); in_port.format.video.eCompressionFormat = OMX_VIDEO_CodingUnused; // in_port.format.video.eColorFormat = OMX_COLOR_FormatYUV420SemiPlanar; in_port.format.video.eColorFormat = (OMX_COLOR_FORMATTYPE)QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m; - err = OMX_SetParameter(s->handle, OMX_IndexParamPortDefinition, + err = OMX_SetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port); assert(err == OMX_ErrorNone); - err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition, + err = OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port); assert(err == OMX_ErrorNone); - s->num_in_bufs = in_port.nBufferCountActual; + this->num_in_bufs = in_port.nBufferCountActual; // setup output port OMX_PARAM_PORTDEFINITIONTYPE out_port = {0}; out_port.nSize = sizeof(out_port); out_port.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition, + err = OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR)&out_port); assert(err == OMX_ErrorNone); - out_port.format.video.nFrameWidth = s->width; - out_port.format.video.nFrameHeight = s->height; + out_port.format.video.nFrameWidth = this->width; + out_port.format.video.nFrameHeight = this->height; out_port.format.video.xFramerate = 0; out_port.format.video.nBitrate = bitrate; if (h265) { @@ -253,26 +238,26 @@ void encoder_init(EncoderState *s, const char* filename, int width, int height, } out_port.format.video.eColorFormat = OMX_COLOR_FormatUnused; - err = OMX_SetParameter(s->handle, OMX_IndexParamPortDefinition, + err = OMX_SetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port); assert(err == OMX_ErrorNone); - err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition, + err = OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port); assert(err == OMX_ErrorNone); - s->num_out_bufs = out_port.nBufferCountActual; + this->num_out_bufs = out_port.nBufferCountActual; OMX_VIDEO_PARAM_BITRATETYPE bitrate_type = {0}; bitrate_type.nSize = sizeof(bitrate_type); bitrate_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - err = OMX_GetParameter(s->handle, OMX_IndexParamVideoBitrate, + err = OMX_GetParameter(this->handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type); assert(err == OMX_ErrorNone); bitrate_type.eControlRate = OMX_Video_ControlRateVariable; bitrate_type.nTargetBitrate = bitrate; - err = OMX_SetParameter(s->handle, OMX_IndexParamVideoBitrate, + err = OMX_SetParameter(this->handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type); assert(err == OMX_ErrorNone); @@ -287,14 +272,14 @@ void encoder_init(EncoderState *s, const char* filename, int width, int height, #endif hevc_type.nSize = sizeof(hevc_type); hevc_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - err = OMX_GetParameter(s->handle, index_type, + err = OMX_GetParameter(this->handle, index_type, (OMX_PTR) &hevc_type); assert(err == OMX_ErrorNone); hevc_type.eProfile = OMX_VIDEO_HEVCProfileMain; hevc_type.eLevel = OMX_VIDEO_HEVCHighTierLevel5; - err = OMX_SetParameter(s->handle, index_type, + err = OMX_SetParameter(this->handle, index_type, (OMX_PTR) &hevc_type); assert(err == OMX_ErrorNone); } else { @@ -302,7 +287,7 @@ void encoder_init(EncoderState *s, const char* filename, int width, int height, OMX_VIDEO_PARAM_AVCTYPE avc = { 0 }; avc.nSize = sizeof(avc); avc.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - err = OMX_GetParameter(s->handle, OMX_IndexParamVideoAvc, &avc); + err = OMX_GetParameter(this->handle, OMX_IndexParamVideoAvc, &avc); assert(err == OMX_ErrorNone); avc.nBFrames = 0; @@ -314,7 +299,7 @@ void encoder_init(EncoderState *s, const char* filename, int width, int height, avc.nAllowedPictureTypes |= OMX_VIDEO_PictureTypeB; avc.eLoopFilterMode = OMX_VIDEO_AVCLoopFilterEnable; - err = OMX_SetParameter(s->handle, OMX_IndexParamVideoAvc, &avc); + err = OMX_SetParameter(this->handle, OMX_IndexParamVideoAvc, &avc); assert(err == OMX_ErrorNone); } @@ -324,7 +309,7 @@ void encoder_init(EncoderState *s, const char* filename, int width, int height, // video_port_format.nSize = sizeof(video_port_format); // video_port_format.nIndex = i; // video_port_format.nPortIndex = PORT_INDEX_IN; - // if (OMX_GetParameter(s->handle, OMX_IndexParamVideoPortFormat, &video_port_format) != OMX_ErrorNone) + // if (OMX_GetParameter(this->handle, OMX_IndexParamVideoPortFormat, &video_port_format) != OMX_ErrorNone) // break; // printf("in %d: compression 0x%x format 0x%x %s\n", i, // video_port_format.eCompressionFormat, video_port_format.eColorFormat, @@ -336,83 +321,83 @@ void encoder_init(EncoderState *s, const char* filename, int width, int height, // params.nSize = sizeof(params); // params.nPortIndex = PORT_INDEX_OUT; // params.nProfileIndex = i; - // if (OMX_GetParameter(s->handle, OMX_IndexParamVideoProfileLevelQuerySupported, ¶ms) != OMX_ErrorNone) + // if (OMX_GetParameter(this->handle, OMX_IndexParamVideoProfileLevelQuerySupported, ¶ms) != OMX_ErrorNone) // break; // printf("profile %d level 0x%x\n", params.eProfile, params.eLevel); // } - err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateIdle, NULL); + err = OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateIdle, NULL); assert(err == OMX_ErrorNone); - s->in_buf_headers = (OMX_BUFFERHEADERTYPE **)calloc(s->num_in_bufs, sizeof(OMX_BUFFERHEADERTYPE*)); - for (int i=0; inum_in_bufs; i++) { - err = OMX_AllocateBuffer(s->handle, &s->in_buf_headers[i], PORT_INDEX_IN, s, + this->in_buf_headers = (OMX_BUFFERHEADERTYPE **)calloc(this->num_in_bufs, sizeof(OMX_BUFFERHEADERTYPE*)); + for (int i=0; inum_in_bufs; i++) { + err = OMX_AllocateBuffer(this->handle, &this->in_buf_headers[i], PORT_INDEX_IN, this, in_port.nBufferSize); assert(err == OMX_ErrorNone); } - s->out_buf_headers = (OMX_BUFFERHEADERTYPE **)calloc(s->num_out_bufs, sizeof(OMX_BUFFERHEADERTYPE*)); - for (int i=0; inum_out_bufs; i++) { - err = OMX_AllocateBuffer(s->handle, &s->out_buf_headers[i], PORT_INDEX_OUT, s, + this->out_buf_headers = (OMX_BUFFERHEADERTYPE **)calloc(this->num_out_bufs, sizeof(OMX_BUFFERHEADERTYPE*)); + for (int i=0; inum_out_bufs; i++) { + err = OMX_AllocateBuffer(this->handle, &this->out_buf_headers[i], PORT_INDEX_OUT, this, out_port.nBufferSize); assert(err == OMX_ErrorNone); } - wait_for_state(s, OMX_StateIdle); + wait_for_state(OMX_StateIdle); - err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateExecuting, NULL); + err = OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateExecuting, NULL); assert(err == OMX_ErrorNone); - wait_for_state(s, OMX_StateExecuting); + wait_for_state(OMX_StateExecuting); // give omx all the output buffers - for (int i = 0; i < s->num_out_bufs; i++) { - // printf("fill %p\n", s->out_buf_headers[i]); - err = OMX_FillThisBuffer(s->handle, s->out_buf_headers[i]); + for (int i = 0; i < this->num_out_bufs; i++) { + // printf("fill %p\n", this->out_buf_headers[i]); + err = OMX_FillThisBuffer(this->handle, this->out_buf_headers[i]); assert(err == OMX_ErrorNone); } // fill the input free queue - for (int i = 0; i < s->num_in_bufs; i++) { - queue_push(&s->free_in, (void*)s->in_buf_headers[i]); + for (int i = 0; i < this->num_in_bufs; i++) { + queue_push(&this->free_in, (void*)this->in_buf_headers[i]); } } -static void handle_out_buf(EncoderState *s, OMX_BUFFERHEADERTYPE *out_buf) { +void OmxEncoder::handle_out_buf(OmxEncoder *e, OMX_BUFFERHEADERTYPE *out_buf) { int err; uint8_t *buf_data = out_buf->pBuffer + out_buf->nOffset; if (out_buf->nFlags & OMX_BUFFERFLAG_CODECCONFIG) { - if (s->codec_config_len < out_buf->nFilledLen) { - s->codec_config = (uint8_t *)realloc(s->codec_config, out_buf->nFilledLen); + if (e->codec_config_len < out_buf->nFilledLen) { + e->codec_config = (uint8_t *)realloc(e->codec_config, out_buf->nFilledLen); } - s->codec_config_len = out_buf->nFilledLen; - memcpy(s->codec_config, buf_data, out_buf->nFilledLen); + e->codec_config_len = out_buf->nFilledLen; + memcpy(e->codec_config, buf_data, out_buf->nFilledLen); #ifdef QCOM2 out_buf->nTimeStamp = 0; #endif } - if (s->of) { + if (e->of) { //printf("write %d flags 0x%x\n", out_buf->nFilledLen, out_buf->nFlags); - fwrite(buf_data, out_buf->nFilledLen, 1, s->of); + fwrite(buf_data, out_buf->nFilledLen, 1, e->of); } - if (s->remuxing) { - if (!s->wrote_codec_config && s->codec_config_len > 0) { - if (s->codec_ctx->extradata_size < s->codec_config_len) { - s->codec_ctx->extradata = (uint8_t *)realloc(s->codec_ctx->extradata, s->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE); + if (e->remuxing) { + if (!e->wrote_codec_config && e->codec_config_len > 0) { + if (e->codec_ctx->extradata_size < e->codec_config_len) { + e->codec_ctx->extradata = (uint8_t *)realloc(e->codec_ctx->extradata, e->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE); } - s->codec_ctx->extradata_size = s->codec_config_len; - memcpy(s->codec_ctx->extradata, s->codec_config, s->codec_config_len); - memset(s->codec_ctx->extradata + s->codec_ctx->extradata_size, 0, AV_INPUT_BUFFER_PADDING_SIZE); + e->codec_ctx->extradata_size = e->codec_config_len; + memcpy(e->codec_ctx->extradata, e->codec_config, e->codec_config_len); + memset(e->codec_ctx->extradata + e->codec_ctx->extradata_size, 0, AV_INPUT_BUFFER_PADDING_SIZE); - err = avcodec_parameters_from_context(s->out_stream->codecpar, s->codec_ctx); + err = avcodec_parameters_from_context(e->out_stream->codecpar, e->codec_ctx); assert(err >= 0); - err = avformat_write_header(s->ofmt_ctx, NULL); + err = avformat_write_header(e->ofmt_ctx, NULL); assert(err >= 0); - s->wrote_codec_config = true; + e->wrote_codec_config = true; } if (out_buf->nTimeStamp > 0) { @@ -425,14 +410,14 @@ static void handle_out_buf(EncoderState *s, OMX_BUFFERHEADERTYPE *out_buf) { pkt.size = out_buf->nFilledLen; enum AVRounding rnd = static_cast(AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX); - pkt.pts = pkt.dts = av_rescale_q_rnd(out_buf->nTimeStamp, in_timebase, s->ofmt_ctx->streams[0]->time_base, rnd); - pkt.duration = av_rescale_q(50*1000, in_timebase, s->ofmt_ctx->streams[0]->time_base); + pkt.pts = pkt.dts = av_rescale_q_rnd(out_buf->nTimeStamp, in_timebase, e->ofmt_ctx->streams[0]->time_base, rnd); + pkt.duration = av_rescale_q(50*1000, in_timebase, e->ofmt_ctx->streams[0]->time_base); if (out_buf->nFlags & OMX_BUFFERFLAG_SYNCFRAME) { pkt.flags |= AV_PKT_FLAG_KEY; } - err = av_write_frame(s->ofmt_ctx, &pkt); + err = av_write_frame(e->ofmt_ctx, &pkt); if (err < 0) { LOGW("ts encoder write issue"); } av_free_packet(&pkt); @@ -445,113 +430,106 @@ static void handle_out_buf(EncoderState *s, OMX_BUFFERHEADERTYPE *out_buf) { out_buf->nTimeStamp = 0; } #endif - err = OMX_FillThisBuffer(s->handle, out_buf); + err = OMX_FillThisBuffer(e->handle, out_buf); assert(err == OMX_ErrorNone); } -int encoder_encode_frame(EncoderState *s, - const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width, int in_height, - int *frame_segment, VisionIpcBufExtra *extra) { +int OmxEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, + int in_width, int in_height, + int *frame_segment, VisionIpcBufExtra *extra) { int err; - pthread_mutex_lock(&s->lock); + pthread_mutex_lock(&this->lock); - if (!s->open) { - pthread_mutex_unlock(&s->lock); + if (!this->is_open) { + pthread_mutex_unlock(&this->lock); return -1; } // this sometimes freezes... put it outside the encoder lock so we can still trigger rotates... // THIS IS A REALLY BAD IDEA, but apparently the race has to happen 30 times to trigger this - //pthread_mutex_unlock(&s->lock); - OMX_BUFFERHEADERTYPE* in_buf = (OMX_BUFFERHEADERTYPE *)queue_pop(&s->free_in); - //pthread_mutex_lock(&s->lock); - - // if (s->rotating) { - // encoder_close(s); - // encoder_open(s, s->next_path); - // s->segment = s->next_segment; - // s->rotating = false; - // } - int ret = s->counter; + //pthread_mutex_unlock(&this->lock); + OMX_BUFFERHEADERTYPE* in_buf = (OMX_BUFFERHEADERTYPE *)queue_pop(&this->free_in); + //pthread_mutex_lock(&this->lock); + + int ret = this->counter; uint8_t *in_buf_ptr = in_buf->pBuffer; // printf("in_buf ptr %p\n", in_buf_ptr); uint8_t *in_y_ptr = in_buf_ptr; - int in_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, s->width); - int in_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, s->width); - // uint8_t *in_uv_ptr = in_buf_ptr + (s->width * s->height); - uint8_t *in_uv_ptr = in_buf_ptr + (in_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, s->height)); + int in_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, this->width); + int in_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, this->width); + // uint8_t *in_uv_ptr = in_buf_ptr + (this->width * this->height); + uint8_t *in_uv_ptr = in_buf_ptr + (in_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, this->height)); - if (s->downscale) { + if (this->downscale) { I420Scale(y_ptr, in_width, u_ptr, in_width/2, v_ptr, in_width/2, in_width, in_height, - s->y_ptr2, s->width, - s->u_ptr2, s->width/2, - s->v_ptr2, s->width/2, - s->width, s->height, + this->y_ptr2, this->width, + this->u_ptr2, this->width/2, + this->v_ptr2, this->width/2, + this->width, this->height, libyuv::kFilterNone); - y_ptr = s->y_ptr2; - u_ptr = s->u_ptr2; - v_ptr = s->v_ptr2; + y_ptr = this->y_ptr2; + u_ptr = this->u_ptr2; + v_ptr = this->v_ptr2; } - err = libyuv::I420ToNV12(y_ptr, s->width, - u_ptr, s->width/2, - v_ptr, s->width/2, + err = libyuv::I420ToNV12(y_ptr, this->width, + u_ptr, this->width/2, + v_ptr, this->width/2, in_y_ptr, in_y_stride, in_uv_ptr, in_uv_stride, - s->width, s->height); + this->width, this->height); assert(err == 0); - // in_buf->nFilledLen = (s->width*s->height) + (s->width*s->height/2); - in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, s->width, s->height); + // in_buf->nFilledLen = (this->width*this->height) + (this->width*this->height/2); + in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, this->width, this->height); in_buf->nFlags = OMX_BUFFERFLAG_ENDOFFRAME; in_buf->nOffset = 0; in_buf->nTimeStamp = extra->timestamp_eof/1000LL; // OMX_TICKS, in microseconds - s->last_t = in_buf->nTimeStamp; + this->last_t = in_buf->nTimeStamp; - err = OMX_EmptyThisBuffer(s->handle, in_buf); + err = OMX_EmptyThisBuffer(this->handle, in_buf); assert(err == OMX_ErrorNone); // pump output while (true) { - OMX_BUFFERHEADERTYPE *out_buf = (OMX_BUFFERHEADERTYPE *)queue_try_pop(&s->done_out); + OMX_BUFFERHEADERTYPE *out_buf = (OMX_BUFFERHEADERTYPE *)queue_try_pop(&this->done_out); if (!out_buf) { break; } - handle_out_buf(s, out_buf); + handle_out_buf(this, out_buf); } - s->dirty = true; + this->dirty = true; - s->counter++; + this->counter++; if (frame_segment) { - *frame_segment = s->segment; + *frame_segment = this->segment; } - pthread_mutex_unlock(&s->lock); + pthread_mutex_unlock(&this->lock); return ret; } -void encoder_open(EncoderState *s, const char* path, int segment) { +void OmxEncoder::encoder_open(const char* path, int segment) { int err; - pthread_mutex_lock(&s->lock); + pthread_mutex_lock(&this->lock); - s->segment = segment; - snprintf(s->vid_path, sizeof(s->vid_path), "%s/%s", path, s->filename); - LOGD("encoder_open %s remuxing:%d", s->vid_path, s->remuxing); + this->segment = segment; + snprintf(this->vid_path, sizeof(this->vid_path), "%s/%s", path, this->filename); + LOGD("encoder_open %s remuxing:%d", this->vid_path, this->remuxing); - if (s->remuxing) { - avformat_alloc_output_context2(&s->ofmt_ctx, NULL, NULL, s->vid_path); - assert(s->ofmt_ctx); + if (this->remuxing) { + avformat_alloc_output_context2(&this->ofmt_ctx, NULL, NULL, this->vid_path); + assert(this->ofmt_ctx); - s->out_stream = avformat_new_stream(s->ofmt_ctx, NULL); - assert(s->out_stream); + this->out_stream = avformat_new_stream(this->ofmt_ctx, NULL); + assert(this->out_stream); // set codec correctly av_register_all(); @@ -560,124 +538,124 @@ void encoder_open(EncoderState *s, const char* path, int segment) { codec = avcodec_find_encoder(AV_CODEC_ID_H264); assert(codec); - s->codec_ctx = avcodec_alloc_context3(codec); - assert(s->codec_ctx); - s->codec_ctx->width = s->width; - s->codec_ctx->height = s->height; - s->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P; - s->codec_ctx->time_base = (AVRational){ 1, s->fps }; + this->codec_ctx = avcodec_alloc_context3(codec); + assert(this->codec_ctx); + this->codec_ctx->width = this->width; + this->codec_ctx->height = this->height; + this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P; + this->codec_ctx->time_base = (AVRational){ 1, this->fps }; - err = avio_open(&s->ofmt_ctx->pb, s->vid_path, AVIO_FLAG_WRITE); + err = avio_open(&this->ofmt_ctx->pb, this->vid_path, AVIO_FLAG_WRITE); assert(err >= 0); - s->wrote_codec_config = false; + this->wrote_codec_config = false; } else { - s->of = fopen(s->vid_path, "wb"); - assert(s->of); + this->of = fopen(this->vid_path, "wb"); + assert(this->of); #ifndef QCOM2 - if (s->codec_config_len > 0) { - fwrite(s->codec_config, s->codec_config_len, 1, s->of); + if (this->codec_config_len > 0) { + fwrite(this->codec_config, this->codec_config_len, 1, this->of); } #endif } // create camera lock file - snprintf(s->lock_path, sizeof(s->lock_path), "%s/%s.lock", path, s->filename); - int lock_fd = open(s->lock_path, O_RDWR | O_CREAT, 0777); + snprintf(this->lock_path, sizeof(this->lock_path), "%s/%s.lock", path, this->filename); + int lock_fd = open(this->lock_path, O_RDWR | O_CREAT, 0777); assert(lock_fd >= 0); close(lock_fd); - s->open = true; - s->counter = 0; + this->is_open = true; + this->counter = 0; - pthread_mutex_unlock(&s->lock); + pthread_mutex_unlock(&this->lock); } -void encoder_close(EncoderState *s) { +void OmxEncoder::encoder_close() { int err; - pthread_mutex_lock(&s->lock); + pthread_mutex_lock(&this->lock); - if (s->open) { - if (s->dirty) { + if (this->is_open) { + if (this->dirty) { // drain output only if there could be frames in the encoder - OMX_BUFFERHEADERTYPE* in_buf = (OMX_BUFFERHEADERTYPE *)queue_pop(&s->free_in); + OMX_BUFFERHEADERTYPE* in_buf = (OMX_BUFFERHEADERTYPE *)queue_pop(&this->free_in); in_buf->nFilledLen = 0; in_buf->nOffset = 0; in_buf->nFlags = OMX_BUFFERFLAG_EOS; - in_buf->nTimeStamp = s->last_t + 1000000LL/s->fps; + in_buf->nTimeStamp = this->last_t + 1000000LL/this->fps; - err = OMX_EmptyThisBuffer(s->handle, in_buf); + err = OMX_EmptyThisBuffer(this->handle, in_buf); assert(err == OMX_ErrorNone); while (true) { - OMX_BUFFERHEADERTYPE *out_buf = (OMX_BUFFERHEADERTYPE *)queue_pop(&s->done_out); + OMX_BUFFERHEADERTYPE *out_buf = (OMX_BUFFERHEADERTYPE *)queue_pop(&this->done_out); - handle_out_buf(s, out_buf); + handle_out_buf(this, out_buf); if (out_buf->nFlags & OMX_BUFFERFLAG_EOS) { break; } } - s->dirty = false; + this->dirty = false; } - if (s->remuxing) { - av_write_trailer(s->ofmt_ctx); - avcodec_free_context(&s->codec_ctx); - avio_closep(&s->ofmt_ctx->pb); - avformat_free_context(s->ofmt_ctx); + if (this->remuxing) { + av_write_trailer(this->ofmt_ctx); + avcodec_free_context(&this->codec_ctx); + avio_closep(&this->ofmt_ctx->pb); + avformat_free_context(this->ofmt_ctx); } else { - fclose(s->of); + fclose(this->of); } - unlink(s->lock_path); + unlink(this->lock_path); } - s->open = false; + this->is_open = false; - pthread_mutex_unlock(&s->lock); + pthread_mutex_unlock(&this->lock); } -void encoder_destroy(EncoderState *s) { +OmxEncoder::~OmxEncoder() { int err; - assert(!s->open); + assert(!this->is_open); - err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateIdle, NULL); + err = OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateIdle, NULL); assert(err == OMX_ErrorNone); - wait_for_state(s, OMX_StateIdle); + wait_for_state(OMX_StateIdle); - err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateLoaded, NULL); + err = OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateLoaded, NULL); assert(err == OMX_ErrorNone); - for (int i=0; inum_in_bufs; i++) { - err = OMX_FreeBuffer(s->handle, PORT_INDEX_IN, s->in_buf_headers[i]); + for (int i=0; inum_in_bufs; i++) { + err = OMX_FreeBuffer(this->handle, PORT_INDEX_IN, this->in_buf_headers[i]); assert(err == OMX_ErrorNone); } - free(s->in_buf_headers); + free(this->in_buf_headers); - for (int i=0; inum_out_bufs; i++) { - err = OMX_FreeBuffer(s->handle, PORT_INDEX_OUT, s->out_buf_headers[i]); + for (int i=0; inum_out_bufs; i++) { + err = OMX_FreeBuffer(this->handle, PORT_INDEX_OUT, this->out_buf_headers[i]); assert(err == OMX_ErrorNone); } - free(s->out_buf_headers); + free(this->out_buf_headers); - wait_for_state(s, OMX_StateLoaded); + wait_for_state(OMX_StateLoaded); - err = OMX_FreeHandle(s->handle); + err = OMX_FreeHandle(this->handle); assert(err == OMX_ErrorNone); - while (queue_try_pop(&s->free_in)); - while (queue_try_pop(&s->done_out)); + while (queue_try_pop(&this->free_in)); + while (queue_try_pop(&this->done_out)); - if (s->codec_config) { - free(s->codec_config); + if (this->codec_config) { + free(this->codec_config); } - if (s->downscale) { - free(s->y_ptr2); - free(s->u_ptr2); - free(s->v_ptr2); + if (this->downscale) { + free(this->y_ptr2); + free(this->u_ptr2); + free(this->v_ptr2); } } diff --git a/selfdrive/loggerd/omx_encoder.h b/selfdrive/loggerd/omx_encoder.h new file mode 100644 index 0000000000..b8fd37928a --- /dev/null +++ b/selfdrive/loggerd/omx_encoder.h @@ -0,0 +1,81 @@ +#pragma once + +#include +#include +#include + +#include +#include + +extern "C" { + #include +} + +#include "encoder.h" +#include "common/cqueue.h" +#include "visionipc.h" + +// OmxEncoder, lossey codec using hardware hevc +class OmxEncoder : public VideoEncoder { +public: + OmxEncoder(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale); + ~OmxEncoder(); + int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, + int in_width, int in_height, + int *frame_segment, VisionIpcBufExtra *extra); + void encoder_open(const char* path, int segment); + void encoder_close(); + + // OMX callbacks + static OMX_ERRORTYPE event_handler(OMX_HANDLETYPE component, OMX_PTR app_data, OMX_EVENTTYPE event, + OMX_U32 data1, OMX_U32 data2, OMX_PTR event_data); + static OMX_ERRORTYPE empty_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, + OMX_BUFFERHEADERTYPE *buffer); + static OMX_ERRORTYPE fill_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, + OMX_BUFFERHEADERTYPE *buffer); + +private: + void wait_for_state(OMX_STATETYPE state); + static void handle_out_buf(OmxEncoder *e, OMX_BUFFERHEADERTYPE *out_buf); + + pthread_mutex_t lock; + int width, height, fps; + char vid_path[1024]; + char lock_path[1024]; + bool is_open = false; + bool dirty = false; + int counter = 0; + int segment = -1; + + const char* filename; + FILE *of; + + size_t codec_config_len; + uint8_t *codec_config = NULL; + bool wrote_codec_config; + + pthread_mutex_t state_lock; + pthread_cond_t state_cv; + OMX_STATETYPE state = OMX_StateLoaded; + + OMX_HANDLETYPE handle; + + int num_in_bufs; + OMX_BUFFERHEADERTYPE** in_buf_headers; + + int num_out_bufs; + OMX_BUFFERHEADERTYPE** out_buf_headers; + + uint64_t last_t; + + Queue free_in; + Queue done_out; + + AVFormatContext *ofmt_ctx; + AVCodecContext *codec_ctx; + AVStream *out_stream; + bool remuxing; + + bool downscale; + uint8_t *y_ptr2, *u_ptr2, *v_ptr2; +};