Add support for decoding to 565 to libjpeg_turbo

wrbmp.c

Index: wrbmp.c
diff --git a/wrbmp.c b/wrbmp.c
index 3283b0f15c20ae86b64bc2d021b4f9760cb9de3e..a42e1c13c5170adc2f6d7b083519738f6ddbab6e 100644
--- a/wrbmp.c
+++ b/wrbmp.c
@@ -1,8 +1,11 @@
 /*
  * wrbmp.c
  *
+ * This file was part of the Independent JPEG Group's software.
  * Copyright (C) 1994-1996, Thomas G. Lane.
- * This file is part of the Independent JPEG Group's software.
+ * Modifications:
+ * Copyright (C) 2013, Linaro Limited.
+ * Copyright (C) 2014, D. R. Commander.
  * For conditions of distribution and use, see the accompanying README file.
  *
  * This file contains routines to write output images in Microsoft "BMP"
@@ -62,6 +65,15 @@ LOCAL(void) write_colormap
 	     int map_colors, int map_entry_size));
 
 
+static inline boolean is_big_endian(void)
+{
+  int test_value = 1;
+  if(*(char *)&test_value != 1)
+    return TRUE;
+  return FALSE;
+}
+
+
 /*
  * Write some pixel data.
  * In this module rows_supplied will always be 1.
@@ -89,11 +101,30 @@ put_pixel_rows (j_decompress_ptr cinfo, djpeg_dest_ptr dinfo,
    */
   inptr = dest->pub.buffer[0];
   outptr = image_ptr[0];
-  for (col = cinfo->output_width; col > 0; col--) {
-    outptr[2] = *inptr++;	/* can omit GETJSAMPLE() safely */
-    outptr[1] = *inptr++;
-    outptr[0] = *inptr++;
-    outptr += 3;
+
+  if(cinfo->out_color_space == JCS_RGB565) {
+    boolean big_endian = is_big_endian();
+    unsigned short *inptr2 = (unsigned short *)inptr;
+    for (col = cinfo->output_width; col > 0; col--) {
+      if (big_endian) {
+        outptr[0] = (*inptr2 >> 5) & 0xF8;
+        outptr[1] = ((*inptr2 << 5) & 0xE0) | ((*inptr2 >> 11) & 0x1C);
+        outptr[2] = *inptr2 & 0xF8;
+      } else {
+        outptr[0] = (*inptr2 << 3) & 0xF8;
+        outptr[1] = (*inptr2 >> 3) & 0xFC;
+        outptr[2] = (*inptr2 >> 8) & 0xF8;
+      }
+      outptr += 3;
+      inptr2++;
+    }
+  } else {
+    for (col = cinfo->output_width; col > 0; col--) {
+      outptr[2] = *inptr++;       /* can omit GETJSAMPLE() safely */
+      outptr[1] = *inptr++;
+      outptr[0] = *inptr++;
+      outptr += 3;
+    }
   }
 
   /* Zero out the pad bytes. */
@@ -181,6 +212,9 @@ write_bmp_header (j_decompress_ptr cinfo, bmp_dest_ptr dest)
       bits_per_pixel = 24;
       cmap_entries = 0;
     }
+  } else if (cinfo->out_color_space == JCS_RGB565) {
+    bits_per_pixel = 24;
+    cmap_entries   = 0;
   } else {
     /* Grayscale output.  We need to fake a 256-entry colormap. */
     bits_per_pixel = 8;
@@ -246,6 +280,9 @@ write_os2_header (j_decompress_ptr cinfo, bmp_dest_ptr dest)
       bits_per_pixel = 24;
       cmap_entries = 0;
     }
+  } else if (cinfo->out_color_space == JCS_RGB565) {
+    bits_per_pixel = 24;
+    cmap_entries   = 0;
   } else {
     /* Grayscale output.  We need to fake a 256-entry colormap. */
     bits_per_pixel = 8;
@@ -407,6 +444,8 @@ jinit_write_bmp (j_decompress_ptr cinfo, boolean is_os2)
       dest->pub.put_pixel_rows = put_gray_rows;
     else
       dest->pub.put_pixel_rows = put_pixel_rows;
+  } else if(cinfo->out_color_space == JCS_RGB565 ) {
+      dest->pub.put_pixel_rows = put_pixel_rows;
   } else {
     ERREXIT(cinfo, JERR_BMP_COLORSPACE);
   }
@@ -415,16 +454,26 @@ jinit_write_bmp (j_decompress_ptr cinfo, boolean is_os2)
   jpeg_calc_output_dimensions(cinfo);
 
   /* Determine width of rows in the BMP file (padded to 4-byte boundary). */
-  row_width = cinfo->output_width * cinfo->output_components;
-  dest->data_width = row_width;
-  while ((row_width & 3) != 0) row_width++;
-  dest->row_width = row_width;
-  dest->pad_bytes = (int) (row_width - dest->data_width);
+  if (cinfo->out_color_space == JCS_RGB565) {
+    row_width = cinfo->output_width * 2;
+    dest->row_width = dest->data_width = cinfo->output_width * 3;
+  } else {
+    row_width = cinfo->output_width * cinfo->output_components;
+    dest->row_width = dest->data_width = row_width;
+  }
+  while ((dest->row_width & 3) != 0) dest->row_width++;
+  dest->pad_bytes = (int) (dest->row_width - dest->data_width);
+  if (cinfo->out_color_space == JCS_RGB565) {
+    while ((row_width & 3) != 0) row_width++;
+  } else {
+    row_width = dest->row_width;
+  }
+
 
   /* Allocate space for inversion array, prepare for write pass */
   dest->whole_image = (*cinfo->mem->request_virt_sarray)
     ((j_common_ptr) cinfo, JPOOL_IMAGE, FALSE,
-     row_width, cinfo->output_height, (JDIMENSION) 1);
+     dest->row_width, cinfo->output_height, (JDIMENSION) 1);
   dest->cur_output_row = 0;
   if (cinfo->progress != NULL) {
     cd_progress_ptr progress = (cd_progress_ptr) cinfo->progress;