drivers/video/rockchip/bmp_helper.c

   1 /*
   2  * linux/drivers/video/rockchip/bmp_helper.c
   3  *
   4  * Copyright (C) 2012 Rockchip Corporation
   5  * Author: Mark Yao <mark.yao@rock-chips.com>
   6  *
   7  * This program is free software; you can redistribute it and/or modify it
   8  * under the terms of the GNU General Public License version 2 as published by
   9  * the Free Software Foundation.
  10  *
  11  * This program is distributed in the hope that it will be useful, but WITHOUT
  12  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  13  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  14  * more details.
  15  *
  16  * You should have received a copy of the GNU General Public License along with
  17  * this program.  If not, see <http://www.gnu.org/licenses/>.
  18  */
  19
  20 #include <linux/sysfs.h>
  21 #include <linux/uaccess.h>
  22 #include <linux/kernel.h>
  23 #include <linux/rk_fb.h>
  24
  25 #include "bmp_helper.h"
  26
  27 static void yuv_to_rgb(int y, int u, int v, int *r, int *g, int *b)
  28 {
  29         int rdif, invgdif, bdif;
  30
  31         u -= 128;
  32         v -= 128;
  33         rdif = v + ((v * 103) >> 8);
  34         invgdif = ((u * 88) >> 8) + ((v * 183) >> 8);
  35         bdif = u + ((u*198) >> 8);
  36         *r = range(y + rdif, 0, 0xff);
  37         *g = range(y - invgdif, 0, 0xff);
  38         *b = range(y + bdif, 0, 0xff);
  39 }
  40
  41 int datatobmp(void *__iomem *vaddr, int width, int height, u8 data_format,
  42               void *data, void (*fn)(void *, void *, int))
  43 {
  44         uint32_t *d, *d1, *d2;
  45         uint8_t *dst, *yrgb, *uv, *y1, *y2;
  46         int y, u, v, r, g, b;
  47
  48         int yu = width * 4 % 4;
  49         int byteperline;
  50         unsigned int size;
  51         BITMAPHEADER header;
  52         BITMAPINFOHEADER infoheader;
  53         void *buf;
  54         int i, j;
  55
  56         yu = yu != 0 ? 4 - yu : yu;
  57         byteperline = width * 4 + yu;
  58         size = byteperline * height + 54;
  59         memset(&header, 0, sizeof(header));
  60         memset(&infoheader, 0, sizeof(infoheader));
  61         header.type = 'M'<<8|'B';
  62         header.size = size;
  63         header.offset = 54;
  64
  65         infoheader.size = 40;
  66         infoheader.width = width;
  67         infoheader.height = 0 - height;
  68         infoheader.bitcount = 4 * 8;
  69         infoheader.compression = 0;
  70         infoheader.imagesize = byteperline * height;
  71         infoheader.xpelspermeter = 0;
  72         infoheader.ypelspermeter = 0;
  73         infoheader.colors = 0;
  74         infoheader.colorsimportant = 0;
  75         fn(data, (void *)&header, sizeof(header));
  76         fn(data, (void *)&infoheader, sizeof(infoheader));
  77
  78         /*
  79          * if data_format is ARGB888 or XRGB888, not need convert.
  80          */
  81         if (data_format == ARGB888 || data_format == XRGB888) {
  82                 fn(data, (char *)vaddr, width * height * 4);
  83                 return 0;
  84         }
  85         /*
  86          * alloc 2 line buffer.
  87          */
  88         buf = kmalloc(width * 2 * 4, GFP_KERNEL);
  89         if (!buf)
  90                 return -ENOMEM;
  91         yrgb = (uint8_t *)vaddr;
  92         uv = yrgb + width * height;
  93         for (j = 0; j < height; j++) {
  94                 if (j % 2 == 0) {
  95                         dst = buf;
  96                         y1 = yrgb + j * width;
  97                         y2 = y1 + width;
  98                         d1 = buf;
  99                         d2 = d1 + width;
 100                 }
 101
 102                 for (i = 0; i < width; i++) {
 103                         switch (data_format) {
 104                         case XBGR888:
 105                         case ABGR888:
 106                                 dst[0] = yrgb[2];
 107                                 dst[1] = yrgb[1];
 108                                 dst[2] = yrgb[0];
 109                                 dst[3] = yrgb[3];
 110                                 dst += 4;
 111                                 yrgb += 4;
 112                                 break;
 113                         case RGB888:
 114                                 dst[0] = yrgb[0];
 115                                 dst[1] = yrgb[1];
 116                                 dst[2] = yrgb[2];
 117                                 dst[3] = 0xff;
 118                                 dst += 4;
 119                                 yrgb += 3;
 120                                 break;
 121                         case RGB565:
 122                                 dst[0] = (yrgb[0] & 0x1f) << 3;
 123                                 dst[1] = (yrgb[0] & 0xe0) >> 3 |
 124                                                 (yrgb[1] & 0x7) << 5;
 125                                 dst[2] = yrgb[1] & 0xf8;
 126                                 dst[3] = 0xff;
 127                                 dst += 4;
 128                                 yrgb += 2;
 129                                 break;
 130                         case YUV420:
 131                         case YUV422:
 132                         case YUV444:
 133                                 if (data_format == YUV420) {
 134                                         if (i % 2 == 0) {
 135                                                 d = d1++;
 136                                                 y = *y1++;
 137                                         } else {
 138                                                 d = d2++;
 139                                                 y = *y2++;
 140                                         }
 141                                         if (i % 4 == 0) {
 142                                                 u = *uv++;
 143                                                 v = *uv++;
 144                                         }
 145                                 } else if (data_format == YUV422) {
 146                                         if (i % 2 == 0) {
 147                                                 u = *uv++;
 148                                                 v = *uv++;
 149                                         }
 150                                         d = d1++;
 151                                 } else {
 152                                         u = *uv++;
 153                                         v = *uv++;
 154                                         d = d1++;
 155                                 }
 156                                 yuv_to_rgb(y, u, v, &r, &g, &b);
 157                                 *d = 0xff<<24 | r << 16 | g << 8 | b;
 158                                 break;
 159                         case YUV422_A:
 160                         case YUV444_A:
 161                         default:
 162                                 pr_err("unsupport now\n");
 163                                 return -EINVAL;
 164                         }
 165                 }
 166                 if (j % 2 == 1)
 167                         fn(data, (char *)buf, 2 * width * 4);
 168         }
 169
 170         return 0;
 171 }