      assert(OMX_ErrorNone == _expr); \
    } while (0)
  
  // ***** OMX callback functions *****
  
  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);
 
</s> remove void CameraBuf::queue(size_t buf_idx){
  {
    std::lock_guard<std::mutex> lk(frame_queue_mutex);
    frame_queue.push(buf_idx);
  }
  frame_queue_cv.notify_one();
 </s> add void CameraBuf::queue(size_t buf_idx) {
  safe_queue.push(buf_idx); </s> remove   while (queue_try_pop(&this->free_in)); 
  while (queue_try_pop(&this->done_out)); 
 </s> add   OMX_BUFFERHEADERTYPE *out_buf;
  while (this->free_in.try_pop(out_buf)); 
  while (this->done_out.try_pop(out_buf));  </s> remove     OMX_BUFFERHEADERTYPE *out_buf = (OMX_BUFFERHEADERTYPE *)queue_try_pop(&this->done_out);
    if (!out_buf) {
 </s> add     OMX_BUFFERHEADERTYPE *out_buf;
    if (!this->done_out.try_pop(out_buf)) { </s> remove         if (encoders.size() > 1) {
          int out_segment_alt = -1;
          encoders[1]->encode_frame(buf->y, buf->u, buf->v,
                                    buf->width, buf->height,
                                    &out_segment_alt, extra.timestamp_eof);
        }

        // publish encode index
        MessageBuilder msg;
        // this is really ugly
        auto eidx = cam_idx == LOG_CAMERA_ID_DCAMERA ? msg.initEvent().initFrontEncodeIdx() :
                    (cam_idx == LOG_CAMERA_ID_ECAMERA ? msg.initEvent().initWideEncodeIdx() : msg.initEvent().initEncodeIdx());
        eidx.setFrameId(extra.frame_id);
        eidx.setTimestampSof(extra.timestamp_sof);
        eidx.setTimestampEof(extra.timestamp_eof);
  #ifdef QCOM2
        eidx.setType(cereal::EncodeIndex::Type::FULL_H_E_V_C);
  #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);

        if (lh) {
          auto bytes = msg.toBytes();
          lh_log(lh, bytes.begin(), bytes.size(), false);
 </s> add         if (i == 0 && out_id != -1) {
          // publish encode index
          MessageBuilder msg;
          // this is really ugly
          auto eidx = cam_idx == LOG_CAMERA_ID_DCAMERA ? msg.initEvent().initFrontEncodeIdx() :
                      (cam_idx == LOG_CAMERA_ID_ECAMERA ? msg.initEvent().initWideEncodeIdx() : msg.initEvent().initEncodeIdx());
          eidx.setFrameId(extra.frame_id);
          eidx.setTimestampSof(extra.timestamp_sof);
          eidx.setTimestampEof(extra.timestamp_eof);
    #ifdef QCOM2
          eidx.setType(cereal::EncodeIndex::Type::FULL_H_E_V_C);
    #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);
          if (lh) {
            auto bytes = msg.toBytes();
            lh_log(lh, bytes.begin(), bytes.size(), false);
          } </s> remove     queue_push(&this->free_in, (void*)buf);
 </s> add     this->free_in.push(buf);
extern ExitHandler do_exit;

      assert(OMX_ErrorNone == _expr); \
    } while (0)
  
extern ExitHandler do_exit;

  // ***** OMX callback functions *****
  
  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);
keep keep add keep keep keep keep keep keep
 remove cqueue, use class SafeQueue (#19774)

* class SafeQueue

* cleanup includes

* space

* add timeout

use try_poll

* add function empty() & size()

* class SafeQueue

* add timeout

use try_poll

* add function empty() & size()

* rebase master

* rebase master

* for loop

* fix bug 
selfdrive/loggerd/omx_encoder.cc