23 #include <core/exception.h>
24 #include <core/threading/mutex.h>
25 #include <core/threading/wait_condition.h>
26 #include <fvutils/compression/jpeg_compressor.h>
27 #include <fvutils/compression/jpeg_compressor_mmal.h>
30 #include <mmal/mmal.h>
31 #include <mmal/mmal_buffer.h>
32 #include <mmal/mmal_logging.h>
33 #include <mmal/util/mmal_connection.h>
34 #include <mmal/util/mmal_default_components.h>
35 #include <mmal/util/mmal_util.h>
36 #include <mmal/util/mmal_util_params.h>
46 namespace firevision {
50 class JpegImageCompressorMMAL::State
55 frame_complete_ =
false;
59 compdest = ImageCompressor::COMP_DEST_MEM;
62 encoder_component = NULL;
63 encoder_pool_in = NULL;
64 encoder_pool_out = NULL;
75 CompressionDestination compdest;
80 unsigned int jpeg_buffer_size;
81 unsigned int jpeg_bytes;
83 MMAL_COMPONENT_T *encoder_component;
85 MMAL_POOL_T *encoder_pool_in;
86 MMAL_POOL_T *encoder_pool_out;
102 encoder_output_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
104 bool complete =
false;
107 JpegImageCompressorMMAL::State *state = (JpegImageCompressorMMAL::State *)port->userdata;
110 size_t bytes_written = buffer->length;
112 if (buffer->length) {
113 mmal_buffer_header_mem_lock(buffer);
114 if (state->compdest == ImageCompressor::COMP_DEST_FILE && state->file_handle) {
115 bytes_written = fwrite(buffer->data, 1, buffer->length, state->file_handle);
116 }
else if (state->compdest == ImageCompressor::COMP_DEST_MEM && state->buffer) {
117 if (state->jpeg_bytes + bytes_written <= state->jpeg_buffer_size) {
118 memcpy(state->buffer, buffer->data, buffer->length);
119 state->buffer += buffer->length;
120 state->jpeg_bytes += buffer->length;
122 printf(
"Buffer overflow: %zu + %zu > %zu\n",
125 state->jpeg_buffer_size);
128 mmal_buffer_header_mem_unlock(buffer);
132 if (bytes_written != buffer->length) {
133 printf(
"Unable to write buffer to file - aborting");
140 & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED)) {
144 printf(
"Received a encoder buffer callback with no state");
148 mmal_buffer_header_release(buffer);
151 if (port->is_enabled) {
152 MMAL_STATUS_T status = MMAL_SUCCESS;
153 MMAL_BUFFER_HEADER_T *new_buffer;
155 new_buffer = mmal_queue_get(state->encoder_pool_out->queue);
158 status = mmal_port_send_buffer(port, new_buffer);
160 if (!new_buffer || status != MMAL_SUCCESS)
161 printf(
"Unable to return a buffer to the encoder port");
165 state->frame_complete_mutex_->lock();
166 state->frame_complete_ =
true;
167 state->frame_complete_waitcond_->wake_all();
168 state->frame_complete_mutex_->unlock();
173 encoder_input_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
176 mmal_buffer_header_release(buffer);
191 JpegImageCompressorMMAL::JpegImageCompressorMMAL(
unsigned int quality)
194 width_ = height_ = 0;
196 state_ =
new State();
202 JpegImageCompressorMMAL::~JpegImageCompressorMMAL()
204 destroy_encoder_component();
209 JpegImageCompressorMMAL::supports_vflip()
215 JpegImageCompressorMMAL::set_vflip(
bool enable)
221 JpegImageCompressorMMAL::compress()
225 MMAL_PORT_T *encoder_input = NULL;
226 MMAL_PORT_T *encoder_output = NULL;
228 MMAL_STATUS_T status = MMAL_SUCCESS;
231 if (mmal_component_enable(state_->encoder_component) != MMAL_SUCCESS) {
232 mmal_component_destroy(state_->encoder_component);
233 throw Exception(
"Unable to enable video encoder component");
236 encoder_input = state_->encoder_component->input[0];
237 encoder_output = state_->encoder_component->output[0];
239 if (state_->compdest == ImageCompressor::COMP_DEST_FILE) {
240 state_->file_handle = fopen(filename_,
"wb");
241 if (!state_->file_handle) {
242 throw Exception(errno,
"Failed to open output file");
246 state_->frame_complete_mutex_->lock();
247 state_->frame_complete_ =
false;
248 state_->frame_complete_mutex_->unlock();
250 encoder_output->userdata = (::MMAL_PORT_USERDATA_T *)state_;
253 status = mmal_port_enable(encoder_output, encoder_output_buffer_callback);
256 int num = mmal_queue_length(state_->encoder_pool_out->queue);
258 for (
int q = 0; q < num; ++q) {
259 MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(state_->encoder_pool_out->queue);
262 printf(
"Unable to get a required buffer %d from pool queue", q);
264 if (mmal_port_send_buffer(encoder_output, buffer) != MMAL_SUCCESS)
265 printf(
"Unable to send a buffer to encoder output port (%d)", q);
269 status = mmal_port_enable(encoder_input, encoder_input_buffer_callback);
271 MMAL_BUFFER_HEADER_T *buffer;
272 if ((buffer = mmal_queue_get(state_->encoder_pool_in->queue)) != NULL) {
273 size_t exp_size = colorspace_buffer_size(YUV422_PLANAR,
274 encoder_input->format->es->video.width,
275 encoder_input->format->es->video.height);
276 if (buffer->alloc_size < exp_size) {
277 printf(
"Too small buffer");
283 char *data = (
char *)buffer->data;
284 char *imgb = (
char *)buffer_;
288 for (h = 0; h < encoder_input->format->es->video.height; ++h) {
289 memcpy(data, imgb + ((height_ - h - 1) * width_), width_);
291 data += encoder_input->format->es->video.width;
294 for (h = 0; h < encoder_input->format->es->video.height; ++h) {
295 memcpy(data, imgb + (width_ * height_) + ((height_ - h - 1) * (width_ / 2)), width_ / 2);
297 data += encoder_input->format->es->video.width / 2;
300 for (h = 0; h < encoder_input->format->es->video.height; ++h) {
302 imgb + (width_ * height_) + ((width_ / 2) * height_)
303 + ((height_ - h - 1) * (width_ / 2)),
307 data += encoder_input->format->es->video.width / 2;
310 for (h = 0; h < encoder_input->format->es->video.height; ++h) {
311 memcpy(data, imgb, width_);
313 data += encoder_input->format->es->video.width;
316 for (h = 0; h < encoder_input->format->es->video.height * 2; ++h) {
317 memcpy(data, imgb, width_ / 2);
319 data += encoder_input->format->es->video.width / 2;
323 buffer->length = (size_t)(data - (
char *)buffer->data);
324 buffer->flags = MMAL_BUFFER_HEADER_FLAG_EOS;
326 status = mmal_port_send_buffer(encoder_input, buffer);
327 if (status != MMAL_SUCCESS) {
328 printf(
"Unable to send input buffer: %x\n", status);
331 state_->frame_complete_mutex_->lock();
332 while (!state_->frame_complete_) {
333 state_->frame_complete_waitcond_->wait();
335 state_->frame_complete_mutex_->unlock();
338 if (encoder_input && encoder_input->is_enabled)
339 mmal_port_disable(encoder_input);
340 if (encoder_output && encoder_output->is_enabled)
341 mmal_port_disable(encoder_output);
343 if (state_->compdest == ImageCompressor::COMP_DEST_FILE) {
344 fclose(state_->file_handle);
345 state_->file_handle = NULL;
350 JpegImageCompressorMMAL::set_image_dimensions(
unsigned int width,
unsigned int height)
352 if (width_ != width || height_ != height) {
355 destroy_encoder_component();
356 create_encoder_component();
361 JpegImageCompressorMMAL::set_image_buffer(colorspace_t cspace,
unsigned char *buffer)
363 if (cspace == YUV422_PLANAR) {
366 throw Exception(
"JpegImageCompressorMMAL: can only accept YUV422_PLANAR buffers");
373 state_->compdest = cd;
377 JpegImageCompressorMMAL::supports_compression_destination(
384 JpegImageCompressorMMAL::set_destination_buffer(
unsigned char *buf,
unsigned int buf_size)
386 state_->jpeg_buffer = (
char *)buf;
387 state_->jpeg_buffer_size = buf_size;
391 JpegImageCompressorMMAL::compressed_size()
393 return state_->jpeg_bytes;
397 JpegImageCompressorMMAL::recommended_compressed_buffer_size()
399 return width_ * height_ * 2;
403 JpegImageCompressorMMAL::set_filename(
const char *filename)
405 filename_ = filename;
410 JpegImageCompressorMMAL::create_encoder_component()
412 MMAL_COMPONENT_T *encoder = 0;
413 MMAL_PORT_T * encoder_input = NULL, *encoder_output = NULL;
414 MMAL_STATUS_T status;
417 status = mmal_component_create(MMAL_COMPONENT_DEFAULT_IMAGE_ENCODER, &encoder);
419 if (status != MMAL_SUCCESS) {
421 mmal_component_destroy(encoder);
422 throw Exception(
"Unable to create JPEG encoder component");
425 if (!encoder->input_num || !encoder->output_num) {
426 mmal_component_destroy(encoder);
427 throw Exception(
"JPEG encoder doesn't have input/output ports");
430 encoder_input = encoder->input[0];
431 encoder_output = encoder->output[0];
433 memset(&encoder_input->format->es->video, 0,
sizeof(MMAL_VIDEO_FORMAT_T));
434 encoder_input->format->es->video.width = width_;
435 encoder_input->format->es->video.height = height_;
436 encoder_input->format->es->video.crop.x = 0;
437 encoder_input->format->es->video.crop.y = 0;
438 encoder_input->format->es->video.crop.width = width_;
439 encoder_input->format->es->video.crop.height = height_;
440 encoder_input->format->es->video.frame_rate.num = 1;
441 encoder_input->format->es->video.frame_rate.den = 1;
444 mmal_format_copy(encoder_output->format, encoder_input->format);
447 encoder_input->format->flags = 0;
448 encoder_input->format->encoding = MMAL_ENCODING_I422;
451 encoder_output->format->encoding = MMAL_ENCODING_JPEG;
453 encoder_output->buffer_size = encoder_output->buffer_size_recommended * 2;
454 if (encoder_output->buffer_size < encoder_output->buffer_size_min)
455 encoder_output->buffer_size = encoder_output->buffer_size_min;
458 status = mmal_port_parameter_set_uint32(encoder_output, MMAL_PARAMETER_JPEG_Q_FACTOR, quality_);
459 if (status != MMAL_SUCCESS) {
460 mmal_component_destroy(encoder);
461 throw Exception(
"Unable to set JPEG quality");
464 status = mmal_port_parameter_set_boolean(encoder_output, MMAL_PARAMETER_EXIF_DISABLE, 1);
465 if (status != MMAL_SUCCESS) {
466 mmal_component_destroy(encoder);
467 throw Exception(
"Unable to disable JPEG EXIF data");
471 status = mmal_port_format_commit(encoder_output);
473 if (status != MMAL_SUCCESS) {
474 mmal_component_destroy(encoder);
475 throw Exception(
"Unable to set format on video encoder output port");
479 status = mmal_port_format_commit(encoder_input);
481 if (status != MMAL_SUCCESS) {
482 mmal_component_destroy(encoder);
483 throw Exception(
"Unable to set format on input encoder port");
487 MMAL_PARAMETER_THUMBNAIL_CONFIG_T param_thumb = {{MMAL_PARAMETER_THUMBNAIL_CONFIGURATION,
488 sizeof(MMAL_PARAMETER_THUMBNAIL_CONFIG_T)},
493 status = mmal_port_parameter_set(encoder->control, ¶m_thumb.hdr);
498 mmal_port_pool_create(encoder_output, encoder_output->buffer_num, encoder_output->buffer_size);
501 mmal_component_destroy(encoder);
502 throw Exception(
"Failed to create buffer header pool for encoder output port %s",
503 encoder_output->name);
506 state_->encoder_pool_out = pool;
510 mmal_port_pool_create(encoder_input, encoder_input->buffer_num, encoder_input->buffer_size);
513 mmal_component_destroy(encoder);
514 throw Exception(
"Failed to create buffer header pool for encoder input port %s",
515 encoder_input->name);
518 state_->encoder_pool_in = pool;
519 state_->encoder_component = encoder;
524 JpegImageCompressorMMAL::destroy_encoder_component()
526 mmal_component_disable(state_->encoder_component);
528 if (state_->encoder_pool_in) {
529 mmal_port_pool_destroy(state_->encoder_component->input[0], state_->encoder_pool_in);
530 state_->encoder_pool_in = NULL;
533 if (state_->encoder_pool_out) {
534 mmal_port_pool_destroy(state_->encoder_component->output[0], state_->encoder_pool_out);
535 state_->encoder_pool_out = NULL;
538 if (state_->encoder_component) {
539 mmal_component_destroy(state_->encoder_component);
540 state_->encoder_component = NULL;