Fawkes API  Fawkes Development Version
jpeg_compressor_mmal.cpp
1 
2 /***************************************************************************
3  * jpeg_compressor_mmal.cpp - JPEG image compressor (using MMAL)
4  *
5  * Created: Wed Feb 05 15:13:30 2014
6  * Copyright 2005-2014 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version. A runtime exception applies to
13  * this software (see LICENSE.GPL_WRE file mentioned below for details).
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21  */
22 
23 #include <fvutils/compression/jpeg_compressor.h>
24 #include <fvutils/compression/jpeg_compressor_mmal.h>
25 
26 #include <core/exception.h>
27 #include <core/threading/mutex.h>
28 #include <core/threading/wait_condition.h>
29 
30 extern "C" {
31 #include <bcm_host.h>
32 
33 #include <mmal/mmal.h>
34 #include <mmal/mmal_logging.h>
35 #include <mmal/mmal_buffer.h>
36 #include <mmal/util/mmal_util.h>
37 #include <mmal/util/mmal_util_params.h>
38 #include <mmal/util/mmal_default_components.h>
39 #include <mmal/util/mmal_connection.h>
40 }
41 
42 #include <cstdio>
43 #include <cerrno>
44 
45 using namespace fawkes;
46 
47 namespace firevision {
48 #if 0 /* just to make Emacs auto-indent happy */
49 }
50 #endif
51 
52 ///@cond INTERNALS
53 
54 class JpegImageCompressorMMAL::State {
55  public:
56  State() {
57  frame_complete_ = false;
58  frame_complete_mutex_ = new fawkes::Mutex();
59  frame_complete_waitcond_ = new fawkes::WaitCondition(frame_complete_mutex_);
60 
61  compdest = ImageCompressor::COMP_DEST_MEM;
62  file_handle = NULL;
63  buffer = NULL;
64  encoder_component = NULL;
65  encoder_pool_in = NULL;
66  encoder_pool_out = NULL;
67  reset();
68  }
69 
70  void reset() {
71  jpeg_bytes = 0;
72  buffer = jpeg_buffer;
73  }
74 
75  CompressionDestination compdest;
76  FILE *file_handle;
77  char *buffer;
78 
79  char *jpeg_buffer;
80  unsigned int jpeg_buffer_size;
81  unsigned int jpeg_bytes;
82 
83  MMAL_COMPONENT_T *encoder_component; /// Pointer to the encoder component
84 
85  MMAL_POOL_T *encoder_pool_in; /// Pointer to the pool of buffers used by encoder output port
86  MMAL_POOL_T *encoder_pool_out; /// Pointer to the pool of buffers used by encoder input port
87 
88  bool frame_complete_;
89  fawkes::Mutex * frame_complete_mutex_;
90  fawkes::WaitCondition * frame_complete_waitcond_;
91 };
92 
93 /**
94  * buffer header callback function for encoder
95  *
96  * Callback will dump buffer data to the specific file
97  *
98  * @param port Pointer to port from which callback originated
99  * @param buffer mmal buffer header pointer
100  */
101 static void
102 encoder_output_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
103 {
104  bool complete = false;
105 
106  // We pass our file handle and other stuff in via the userdata field.
107  JpegImageCompressorMMAL::State *state =
108  (JpegImageCompressorMMAL::State *)port->userdata;
109 
110  if (state) {
111  size_t bytes_written = buffer->length;
112 
113  if (buffer->length) {
114  mmal_buffer_header_mem_lock(buffer);
115  if (state->compdest == ImageCompressor::COMP_DEST_FILE && state->file_handle) {
116  bytes_written = fwrite(buffer->data, 1, buffer->length, state->file_handle);
117  } else if (state->compdest == ImageCompressor::COMP_DEST_MEM && state->buffer) {
118  if (state->jpeg_bytes + bytes_written <= state->jpeg_buffer_size) {
119  memcpy(state->buffer, buffer->data, buffer->length);
120  state->buffer += buffer->length;
121  state->jpeg_bytes += buffer->length;
122  } else {
123  printf("Buffer overflow: %zu + %zu > %zu\n", state->jpeg_bytes, bytes_written, state->jpeg_buffer_size);
124  }
125  }
126  mmal_buffer_header_mem_unlock(buffer);
127  }
128 
129  // We need to check we wrote what we wanted - it's possible we have run out of storage.
130  if (bytes_written != buffer->length) {
131  printf("Unable to write buffer to file - aborting");
132 
133  complete = true;
134  }
135 
136  // Now flag if we have completed
137  if (buffer->flags & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED)) {
138  complete = true;
139  }
140  }
141  else
142  {
143  printf("Received a encoder buffer callback with no state");
144  }
145 
146  // release buffer back to the pool
147  mmal_buffer_header_release(buffer);
148 
149  // and send one back to the port (if still open)
150  if (port->is_enabled) {
151  MMAL_STATUS_T status = MMAL_SUCCESS;
152  MMAL_BUFFER_HEADER_T *new_buffer;
153 
154  new_buffer = mmal_queue_get(state->encoder_pool_out->queue);
155 
156  if (new_buffer) {
157  status = mmal_port_send_buffer(port, new_buffer);
158  }
159  if (!new_buffer || status != MMAL_SUCCESS)
160  printf("Unable to return a buffer to the encoder port");
161  }
162 
163  if (complete) {
164  state->frame_complete_mutex_->lock();
165  state->frame_complete_ = true;
166  state->frame_complete_waitcond_->wake_all();
167  state->frame_complete_mutex_->unlock();
168  }
169 }
170 
171 static void encoder_input_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
172 {
173  // The decoder is done with the data, just recycle the buffer header into its pool
174  mmal_buffer_header_release(buffer);
175 }
176 
177 
178 /// @endcond
179 
180 /** @class JpegImageCompressorMMAL <fvutils/compression/jpeg_compressor.h>
181  * Jpeg image compressor.
182  * This JPEG image compressor implementation uses the MMAL hardware encoder
183  * of the Raspberry Pi.
184  * @author Tim Niemueller
185  */
186 
187 
188 /** Constructor.
189  * @param quality JPEG quality in percent (1-100)
190  */
191 JpegImageCompressorMMAL::JpegImageCompressorMMAL(unsigned int quality)
192 {
193  vflip_ = false;
194  width_ = height_ = 0;
195  quality_ = quality;
196  state_ = new State();
197  // we can always do this, it'll just do nothing the second time
198  bcm_host_init();
199 }
200 
201 /** Destructor. */
202 JpegImageCompressorMMAL::~JpegImageCompressorMMAL()
203 {
204  destroy_encoder_component();
205  delete state_;
206 }
207 
208 
209 bool
210 JpegImageCompressorMMAL::supports_vflip()
211 {
212  return true;
213 }
214 
215 
216 void
217 JpegImageCompressorMMAL::set_vflip(bool enable)
218 {
219  vflip_ = enable;
220 }
221 
222 void
223 JpegImageCompressorMMAL::compress()
224 {
225  state_->reset();
226 
227  MMAL_PORT_T *encoder_input = NULL;
228  MMAL_PORT_T *encoder_output = NULL;
229 
230  MMAL_STATUS_T status = MMAL_SUCCESS;
231 
232  // Enable component
233  if (mmal_component_enable(state_->encoder_component) != MMAL_SUCCESS) {
234  mmal_component_destroy(state_->encoder_component);
235  throw Exception("Unable to enable video encoder component");
236  }
237 
238  encoder_input = state_->encoder_component->input[0];
239  encoder_output = state_->encoder_component->output[0];
240 
241  if (state_->compdest == ImageCompressor::COMP_DEST_FILE) {
242  state_->file_handle = fopen(filename_, "wb");
243  if (! state_->file_handle) {
244  throw Exception(errno, "Failed to open output file");
245  }
246  }
247 
248  state_->frame_complete_mutex_->lock();
249  state_->frame_complete_ = false;
250  state_->frame_complete_mutex_->unlock();
251 
252  encoder_output->userdata = (::MMAL_PORT_USERDATA_T *)state_;
253 
254  // Enable the encoder output port and tell it its callback function
255  status = mmal_port_enable(encoder_output, encoder_output_buffer_callback);
256 
257  // Send all the buffers to the encoder output port
258  int num = mmal_queue_length(state_->encoder_pool_out->queue);
259 
260  for (int q = 0; q < num; ++q) {
261  MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(state_->encoder_pool_out->queue);
262 
263  if (!buffer)
264  printf("Unable to get a required buffer %d from pool queue", q);
265 
266  if (mmal_port_send_buffer(encoder_output, buffer)!= MMAL_SUCCESS)
267  printf("Unable to send a buffer to encoder output port (%d)", q);
268  }
269 
270  // Enable the encoder output port and tell it its callback function
271  status = mmal_port_enable(encoder_input, encoder_input_buffer_callback);
272 
273  MMAL_BUFFER_HEADER_T *buffer;
274  if ((buffer = mmal_queue_get(state_->encoder_pool_in->queue)) != NULL)
275  {
276  size_t exp_size = colorspace_buffer_size(YUV422_PLANAR,
277  encoder_input->format->es->video.width,
278  encoder_input->format->es->video.height);
279  if (buffer->alloc_size < exp_size) {
280  printf("Too small buffer");
281  }
282 
283  buffer->cmd = 0;
284  buffer->offset = 0;
285 
286  char *data = (char *)buffer->data;
287  char *imgb = (char *)buffer_;
288 
289  unsigned int h;
290  if (vflip_) {
291  for (h = 0; h < encoder_input->format->es->video.height; ++h) {
292  memcpy(data, imgb + ((height_ - h - 1) * width_), width_);
293  //imgb += width_;
294  data += encoder_input->format->es->video.width;
295  }
296 
297  for (h = 0; h < encoder_input->format->es->video.height; ++h) {
298  memcpy(data, imgb + (width_ * height_) + ((height_ - h - 1) * (width_/2)), width_ / 2);
299  //imgb += width_ / 2;
300  data += encoder_input->format->es->video.width / 2;
301  }
302 
303  for (h = 0; h < encoder_input->format->es->video.height; ++h) {
304  memcpy(data, imgb + (width_ * height_) + ((width_/2) * height_) + ((height_ - h - 1) * (width_/2)), width_ / 2);
305  //memcpy(data, imgb, width_ / 2);
306  //imgb += width_ / 2;
307  data += encoder_input->format->es->video.width / 2;
308  }
309  } else {
310  for (h = 0; h < encoder_input->format->es->video.height; ++h) {
311  memcpy(data, imgb, width_);
312  imgb += width_;
313  data += encoder_input->format->es->video.width;
314  }
315 
316  for (h = 0; h < encoder_input->format->es->video.height * 2; ++h) {
317  memcpy(data, imgb, width_ / 2);
318  imgb += width_ / 2;
319  data += encoder_input->format->es->video.width / 2;
320  }
321  }
322 
323  buffer->length = (size_t)(data - (char *)buffer->data);
324  buffer->flags = MMAL_BUFFER_HEADER_FLAG_EOS;
325 
326  status = mmal_port_send_buffer(encoder_input, buffer);
327  if (status != MMAL_SUCCESS) {
328  printf("Unable to send input buffer: %x\n", status);
329  }
330 
331  state_->frame_complete_mutex_->lock();
332  while (! state_->frame_complete_) {
333  state_->frame_complete_waitcond_->wait();
334  }
335  state_->frame_complete_mutex_->unlock();
336  }
337 
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);
342 
343  if (state_->compdest == ImageCompressor::COMP_DEST_FILE) {
344  fclose(state_->file_handle);
345  state_->file_handle = NULL;
346  }
347 }
348 
349 
350 void
351 JpegImageCompressorMMAL::set_image_dimensions(unsigned int width, unsigned int height)
352 {
353  if (width_ != width || height_ != height) {
354  width_ = width;
355  height_ = height;
356  destroy_encoder_component();
357  create_encoder_component();
358  }
359 }
360 
361 
362 void
363 JpegImageCompressorMMAL::set_image_buffer(colorspace_t cspace, unsigned char *buffer)
364 {
365  if ( cspace == YUV422_PLANAR ) {
366  buffer_ = buffer;
367  } else {
368  throw Exception("JpegImageCompressorMMAL: can only accept YUV422_PLANAR buffers");
369  }
370 }
371 
372 
373 void
374 JpegImageCompressorMMAL::set_compression_destination(ImageCompressor::CompressionDestination cd)
375 {
376  state_->compdest = cd;
377 }
378 
379 
380 bool
381 JpegImageCompressorMMAL::supports_compression_destination(ImageCompressor::CompressionDestination cd)
382 {
383  return true;
384 }
385 
386 
387 void
388 JpegImageCompressorMMAL::set_destination_buffer(unsigned char *buf, unsigned int buf_size)
389 {
390  state_->jpeg_buffer = (char *)buf;
391  state_->jpeg_buffer_size = buf_size;
392 }
393 
394 
395 size_t
396 JpegImageCompressorMMAL::compressed_size()
397 {
398  return state_->jpeg_bytes;
399 }
400 
401 size_t
402 JpegImageCompressorMMAL::recommended_compressed_buffer_size()
403 {
404  return width_ * height_ * 2;
405 }
406 
407 
408 void
409 JpegImageCompressorMMAL::set_filename(const char *filename)
410 {
411  filename_ = filename;
412 }
413 
414 /** Create the encoder component, set up its ports */
415 void
416 JpegImageCompressorMMAL::create_encoder_component()
417 {
418  MMAL_COMPONENT_T *encoder = 0;
419  MMAL_PORT_T *encoder_input = NULL, *encoder_output = NULL;
420  MMAL_STATUS_T status;
421  MMAL_POOL_T *pool;
422 
423  status = mmal_component_create(MMAL_COMPONENT_DEFAULT_IMAGE_ENCODER, &encoder);
424 
425  if (status != MMAL_SUCCESS) {
426  if (encoder)
427  mmal_component_destroy(encoder);
428  throw Exception("Unable to create JPEG encoder component");
429  }
430 
431  if (!encoder->input_num || !encoder->output_num) {
432  mmal_component_destroy(encoder);
433  throw Exception("JPEG encoder doesn't have input/output ports");
434  }
435 
436  encoder_input = encoder->input[0];
437  encoder_output = encoder->output[0];
438 
439  memset(&encoder_input->format->es->video, 0, sizeof(MMAL_VIDEO_FORMAT_T));
440  encoder_input->format->es->video.width = width_;
441  encoder_input->format->es->video.height = height_;
442  encoder_input->format->es->video.crop.x = 0;
443  encoder_input->format->es->video.crop.y = 0;
444  encoder_input->format->es->video.crop.width = width_;
445  encoder_input->format->es->video.crop.height = height_;
446  encoder_input->format->es->video.frame_rate.num = 1;
447  encoder_input->format->es->video.frame_rate.den = 1;
448 
449  // We want same format on input and output
450  mmal_format_copy(encoder_output->format, encoder_input->format);
451 
452  // Specify input format
453  encoder_input->format->flags = 0;
454  encoder_input->format->encoding = MMAL_ENCODING_I422;
455 
456  // Specify out output format
457  encoder_output->format->encoding = MMAL_ENCODING_JPEG;
458 
459  encoder_output->buffer_size = encoder_output->buffer_size_recommended * 2;
460  if (encoder_output->buffer_size < encoder_output->buffer_size_min)
461  encoder_output->buffer_size = encoder_output->buffer_size_min;
462 
463  // Set the JPEG quality level
464  status = mmal_port_parameter_set_uint32(encoder_output, MMAL_PARAMETER_JPEG_Q_FACTOR, quality_);
465  if (status != MMAL_SUCCESS) {
466  mmal_component_destroy(encoder);
467  throw Exception("Unable to set JPEG quality");
468  }
469 
470  status = mmal_port_parameter_set_boolean(encoder_output,
471  MMAL_PARAMETER_EXIF_DISABLE, 1);
472  if (status != MMAL_SUCCESS) {
473  mmal_component_destroy(encoder);
474  throw Exception("Unable to disable JPEG EXIF data");
475  }
476 
477  // Commit the port changes to the output port
478  status = mmal_port_format_commit(encoder_output);
479 
480  if (status != MMAL_SUCCESS) {
481  mmal_component_destroy(encoder);
482  throw Exception("Unable to set format on video encoder output port");
483  }
484 
485  // Commit the port changes to the input port
486  status = mmal_port_format_commit(encoder_input);
487 
488  if (status != MMAL_SUCCESS) {
489  mmal_component_destroy(encoder);
490  throw Exception("Unable to set format on input encoder port");
491  }
492 
493  {
494  MMAL_PARAMETER_THUMBNAIL_CONFIG_T param_thumb =
495  {{MMAL_PARAMETER_THUMBNAIL_CONFIGURATION,
496  sizeof(MMAL_PARAMETER_THUMBNAIL_CONFIG_T)}, 0, 0, 0, 0};
497  status = mmal_port_parameter_set(encoder->control, &param_thumb.hdr);
498  }
499 
500  /* Create pool of buffer headers for the output port to consume */
501  pool = mmal_port_pool_create(encoder_output, encoder_output->buffer_num, encoder_output->buffer_size);
502 
503  if (!pool) {
504  mmal_component_destroy(encoder);
505  throw Exception("Failed to create buffer header pool for encoder output port %s", encoder_output->name);
506  }
507 
508  state_->encoder_pool_out = pool;
509 
510  /* Create pool of buffer headers for the input port to consume */
511  pool = mmal_port_pool_create(encoder_input, encoder_input->buffer_num, encoder_input->buffer_size);
512 
513  if (!pool) {
514  mmal_component_destroy(encoder);
515  throw Exception("Failed to create buffer header pool for encoder input port %s", encoder_input->name);
516  }
517 
518  state_->encoder_pool_in = pool;
519  state_->encoder_component = encoder;
520 }
521 
522 /** Create the encoder component, set up its ports */
523 void
524 JpegImageCompressorMMAL::destroy_encoder_component()
525 {
526  mmal_component_disable(state_->encoder_component);
527 
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;
531  }
532 
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;
536  }
537 
538  if (state_->encoder_component) {
539  mmal_component_destroy(state_->encoder_component);
540  state_->encoder_component = NULL;
541  }
542 }
543 
544 } // end namespace firevision
Wait until a given condition holds.
Fawkes library namespace.
CompressionDestination
Where to put the compressed image.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Mutex mutual exclusion lock.
Definition: mutex.h:32