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 <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>
28 
29 extern "C" {
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>
37 
38 #include <bcm_host.h>
39 }
40 
41 #include <cerrno>
42 #include <cstdio>
43 
44 using namespace fawkes;
45 
46 namespace firevision {
47 
48 ///@cond INTERNALS
49 
50 class JpegImageCompressorMMAL::State
51 {
52 public:
53  State()
54  {
55  frame_complete_ = false;
56  frame_complete_mutex_ = new fawkes::Mutex();
57  frame_complete_waitcond_ = new fawkes::WaitCondition(frame_complete_mutex_);
58 
59  compdest = ImageCompressor::COMP_DEST_MEM;
60  file_handle = NULL;
61  buffer = NULL;
62  encoder_component = NULL;
63  encoder_pool_in = NULL;
64  encoder_pool_out = NULL;
65  reset();
66  }
67 
68  void
69  reset()
70  {
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 = (JpegImageCompressorMMAL::State *)port->userdata;
108 
109  if (state) {
110  size_t bytes_written = buffer->length;
111 
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;
121  } else {
122  printf("Buffer overflow: %zu + %zu > %zu\n",
123  state->jpeg_bytes,
124  bytes_written,
125  state->jpeg_buffer_size);
126  }
127  }
128  mmal_buffer_header_mem_unlock(buffer);
129  }
130 
131  // We need to check we wrote what we wanted - it's possible we have run out of storage.
132  if (bytes_written != buffer->length) {
133  printf("Unable to write buffer to file - aborting");
134 
135  complete = true;
136  }
137 
138  // Now flag if we have completed
139  if (buffer->flags
140  & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED)) {
141  complete = true;
142  }
143  } else {
144  printf("Received a encoder buffer callback with no state");
145  }
146 
147  // release buffer back to the pool
148  mmal_buffer_header_release(buffer);
149 
150  // and send one back to the port (if still open)
151  if (port->is_enabled) {
152  MMAL_STATUS_T status = MMAL_SUCCESS;
153  MMAL_BUFFER_HEADER_T *new_buffer;
154 
155  new_buffer = mmal_queue_get(state->encoder_pool_out->queue);
156 
157  if (new_buffer) {
158  status = mmal_port_send_buffer(port, new_buffer);
159  }
160  if (!new_buffer || status != MMAL_SUCCESS)
161  printf("Unable to return a buffer to the encoder port");
162  }
163 
164  if (complete) {
165  state->frame_complete_mutex_->lock();
166  state->frame_complete_ = true;
167  state->frame_complete_waitcond_->wake_all();
168  state->frame_complete_mutex_->unlock();
169  }
170 }
171 
172 static void
173 encoder_input_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
174 {
175  // The decoder is done with the data, just recycle the buffer header into its pool
176  mmal_buffer_header_release(buffer);
177 }
178 
179 /// @endcond
180 
181 /** @class JpegImageCompressorMMAL <fvutils/compression/jpeg_compressor.h>
182  * Jpeg image compressor.
183  * This JPEG image compressor implementation uses the MMAL hardware encoder
184  * of the Raspberry Pi.
185  * @author Tim Niemueller
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 bool
209 JpegImageCompressorMMAL::supports_vflip()
210 {
211  return true;
212 }
213 
214 void
215 JpegImageCompressorMMAL::set_vflip(bool enable)
216 {
217  vflip_ = enable;
218 }
219 
220 void
221 JpegImageCompressorMMAL::compress()
222 {
223  state_->reset();
224 
225  MMAL_PORT_T *encoder_input = NULL;
226  MMAL_PORT_T *encoder_output = NULL;
227 
228  MMAL_STATUS_T status = MMAL_SUCCESS;
229 
230  // Enable component
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");
234  }
235 
236  encoder_input = state_->encoder_component->input[0];
237  encoder_output = state_->encoder_component->output[0];
238 
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");
243  }
244  }
245 
246  state_->frame_complete_mutex_->lock();
247  state_->frame_complete_ = false;
248  state_->frame_complete_mutex_->unlock();
249 
250  encoder_output->userdata = (::MMAL_PORT_USERDATA_T *)state_;
251 
252  // Enable the encoder output port and tell it its callback function
253  status = mmal_port_enable(encoder_output, encoder_output_buffer_callback);
254 
255  // Send all the buffers to the encoder output port
256  int num = mmal_queue_length(state_->encoder_pool_out->queue);
257 
258  for (int q = 0; q < num; ++q) {
259  MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(state_->encoder_pool_out->queue);
260 
261  if (!buffer)
262  printf("Unable to get a required buffer %d from pool queue", q);
263 
264  if (mmal_port_send_buffer(encoder_output, buffer) != MMAL_SUCCESS)
265  printf("Unable to send a buffer to encoder output port (%d)", q);
266  }
267 
268  // Enable the encoder output port and tell it its callback function
269  status = mmal_port_enable(encoder_input, encoder_input_buffer_callback);
270 
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");
278  }
279 
280  buffer->cmd = 0;
281  buffer->offset = 0;
282 
283  char *data = (char *)buffer->data;
284  char *imgb = (char *)buffer_;
285 
286  unsigned int h;
287  if (vflip_) {
288  for (h = 0; h < encoder_input->format->es->video.height; ++h) {
289  memcpy(data, imgb + ((height_ - h - 1) * width_), width_);
290  //imgb += width_;
291  data += encoder_input->format->es->video.width;
292  }
293 
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);
296  //imgb += width_ / 2;
297  data += encoder_input->format->es->video.width / 2;
298  }
299 
300  for (h = 0; h < encoder_input->format->es->video.height; ++h) {
301  memcpy(data,
302  imgb + (width_ * height_) + ((width_ / 2) * height_)
303  + ((height_ - h - 1) * (width_ / 2)),
304  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 void
350 JpegImageCompressorMMAL::set_image_dimensions(unsigned int width, unsigned int height)
351 {
352  if (width_ != width || height_ != height) {
353  width_ = width;
354  height_ = height;
355  destroy_encoder_component();
356  create_encoder_component();
357  }
358 }
359 
360 void
361 JpegImageCompressorMMAL::set_image_buffer(colorspace_t cspace, unsigned char *buffer)
362 {
363  if (cspace == YUV422_PLANAR) {
364  buffer_ = buffer;
365  } else {
366  throw Exception("JpegImageCompressorMMAL: can only accept YUV422_PLANAR buffers");
367  }
368 }
369 
370 void
371 JpegImageCompressorMMAL::set_compression_destination(ImageCompressor::CompressionDestination cd)
372 {
373  state_->compdest = cd;
374 }
375 
376 bool
377 JpegImageCompressorMMAL::supports_compression_destination(
379 {
380  return true;
381 }
382 
383 void
384 JpegImageCompressorMMAL::set_destination_buffer(unsigned char *buf, unsigned int buf_size)
385 {
386  state_->jpeg_buffer = (char *)buf;
387  state_->jpeg_buffer_size = buf_size;
388 }
389 
390 size_t
391 JpegImageCompressorMMAL::compressed_size()
392 {
393  return state_->jpeg_bytes;
394 }
395 
396 size_t
397 JpegImageCompressorMMAL::recommended_compressed_buffer_size()
398 {
399  return width_ * height_ * 2;
400 }
401 
402 void
403 JpegImageCompressorMMAL::set_filename(const char *filename)
404 {
405  filename_ = filename;
406 }
407 
408 /** Create the encoder component, set up its ports */
409 void
410 JpegImageCompressorMMAL::create_encoder_component()
411 {
412  MMAL_COMPONENT_T *encoder = 0;
413  MMAL_PORT_T * encoder_input = NULL, *encoder_output = NULL;
414  MMAL_STATUS_T status;
415  MMAL_POOL_T * pool;
416 
417  status = mmal_component_create(MMAL_COMPONENT_DEFAULT_IMAGE_ENCODER, &encoder);
418 
419  if (status != MMAL_SUCCESS) {
420  if (encoder)
421  mmal_component_destroy(encoder);
422  throw Exception("Unable to create JPEG encoder component");
423  }
424 
425  if (!encoder->input_num || !encoder->output_num) {
426  mmal_component_destroy(encoder);
427  throw Exception("JPEG encoder doesn't have input/output ports");
428  }
429 
430  encoder_input = encoder->input[0];
431  encoder_output = encoder->output[0];
432 
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;
442 
443  // We want same format on input and output
444  mmal_format_copy(encoder_output->format, encoder_input->format);
445 
446  // Specify input format
447  encoder_input->format->flags = 0;
448  encoder_input->format->encoding = MMAL_ENCODING_I422;
449 
450  // Specify out output format
451  encoder_output->format->encoding = MMAL_ENCODING_JPEG;
452 
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;
456 
457  // Set the JPEG quality level
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");
462  }
463 
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");
468  }
469 
470  // Commit the port changes to the output port
471  status = mmal_port_format_commit(encoder_output);
472 
473  if (status != MMAL_SUCCESS) {
474  mmal_component_destroy(encoder);
475  throw Exception("Unable to set format on video encoder output port");
476  }
477 
478  // Commit the port changes to the input port
479  status = mmal_port_format_commit(encoder_input);
480 
481  if (status != MMAL_SUCCESS) {
482  mmal_component_destroy(encoder);
483  throw Exception("Unable to set format on input encoder port");
484  }
485 
486  {
487  MMAL_PARAMETER_THUMBNAIL_CONFIG_T param_thumb = {{MMAL_PARAMETER_THUMBNAIL_CONFIGURATION,
488  sizeof(MMAL_PARAMETER_THUMBNAIL_CONFIG_T)},
489  0,
490  0,
491  0,
492  0};
493  status = mmal_port_parameter_set(encoder->control, &param_thumb.hdr);
494  }
495 
496  /* Create pool of buffer headers for the output port to consume */
497  pool =
498  mmal_port_pool_create(encoder_output, encoder_output->buffer_num, encoder_output->buffer_size);
499 
500  if (!pool) {
501  mmal_component_destroy(encoder);
502  throw Exception("Failed to create buffer header pool for encoder output port %s",
503  encoder_output->name);
504  }
505 
506  state_->encoder_pool_out = pool;
507 
508  /* Create pool of buffer headers for the input port to consume */
509  pool =
510  mmal_port_pool_create(encoder_input, encoder_input->buffer_num, encoder_input->buffer_size);
511 
512  if (!pool) {
513  mmal_component_destroy(encoder);
514  throw Exception("Failed to create buffer header pool for encoder input port %s",
515  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
fawkes::Mutex
Mutex mutual exclusion lock.
Definition: mutex.h:33
fawkes::WaitCondition
Wait until a given condition holds.
Definition: wait_condition.h:37
fawkes
Fawkes library namespace.
firevision::ImageCompressor::CompressionDestination
CompressionDestination
Where to put the compressed image.
Definition: imagecompressor.h:36
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36