C# - RGB565 zu RGB888 (RGB24) konvertieren

private byte[] convertRgb565toRgb888(byte[] imageData)
        {
            if (imageData == null)
                return (null);

            byte[] ret = new byte[imageData.Length / 2 * 3];

            int retindex = 0;
            for (int i = 0; i < imageData.Length; i += 2)
            {
                byte[] aValue = new byte[2]{imageData[i], imageData[i+1]};

                byte red = (byte)(aValue[0] >> 3);
                byte green1 = (byte)(aValue[0] << 5);
                green1 = (byte)(green1 >> 2);

                byte blue = (byte)(aValue[1] << 3);
                blue = (byte)(blue >> 3);
                byte green2 = (byte)(aValue[1] >> 5);

                byte green = (byte)(green1 + green2);

                ret[retindex] = (byte)(red*16);
                ret[retindex + 1] = (byte)(green*8);
                ret[retindex + 2] = (byte)(blue*16);
                retindex += 3;                
            }
            return (ret);
        }


//Beispiel:
private BitmapSource convert(byte[] rgb565)
{
byte[] imageDataRgb888 = convertRgb565toRgb888(imageDataRgb565);
return( BitmapSource.Create(160, 120, 96, 96, PixelFormats.Rgb24, null, imageDataRgb888, (160 * 24 + 7) / 8));
}