bayer.c

trident tm5600的linux驱动

	start_with_green = !start_with_green;
    }

    /* render the last line */
    v4lconvert_border_bayer_line_to_bgr24(bayer + width, bayer, bgr, width,
      !start_with_green, !blue_line);
}

void v4lconvert_bayer_to_rgb24(const unsigned char *bayer,
  unsigned char *bgr, int width, int height, unsigned int pixfmt)
{
	bayer_to_rgbbgr24(bayer, bgr, width, height, pixfmt,
		pixfmt == V4L2_PIX_FMT_SGBRG8		/* start with green */
			|| pixfmt == V4L2_PIX_FMT_SGRBG8,
		pixfmt != V4L2_PIX_FMT_SBGGR8		/* blue line */
			&& pixfmt != V4L2_PIX_FMT_SGBRG8);
}

void v4lconvert_bayer_to_bgr24(const unsigned char *bayer,
  unsigned char *bgr, int width, int height, unsigned int pixfmt)
{
	bayer_to_rgbbgr24(bayer, bgr, width, height, pixfmt,
		pixfmt == V4L2_PIX_FMT_SGBRG8		/* start with green */
			|| pixfmt == V4L2_PIX_FMT_SGRBG8,
		pixfmt == V4L2_PIX_FMT_SBGGR8		/* blue line */
			|| pixfmt == V4L2_PIX_FMT_SGBRG8);
}

static void v4lconvert_border_bayer_line_to_y(
  const unsigned char* bayer, const unsigned char* adjacent_bayer,
  unsigned char *y, int width, int start_with_green, int blue_line)
{
  int t0, t1;

  if (start_with_green) {
    /* First pixel */
    if (blue_line) {
      *y++ = (8453*adjacent_bayer[0] + 16594*bayer[0] + 3223*bayer[1] + 524288)
	     >> 15;
    } else {
      *y++ = (8453*bayer[1] + 16594*bayer[0] + 3223*adjacent_bayer[0] + 524288)
	     >> 15;
    }
    /* Second pixel */
    t0 = bayer[0] + bayer[2] + adjacent_bayer[1];
    t1 = adjacent_bayer[0] + adjacent_bayer[2];
    if (blue_line) {
      *y++ = (4226*t1 + 5531*t0 + 3223*bayer[1] + 524288) >> 15;
    } else {
      *y++ = (8453*bayer[1] + 5531*t0 + 1611*t1 + 524288) >> 15;
    }
    bayer++;
    adjacent_bayer++;
    width -= 2;
  } else {
    /* First pixel */
    t0 = bayer[1] + adjacent_bayer[0];
    if (blue_line) {
      *y++ = (8453*adjacent_bayer[1] + 8297*t0 + 3223*bayer[0] + 524288)
	     >> 15;
    } else {
      *y++ = (8453*bayer[0] + 8297*t0 + 3223*adjacent_bayer[1] + 524288)
	     >> 15;
    }
    width--;
  }

  if (blue_line) {
    for ( ; width > 2; width -= 2) {
      t0 = bayer[0] + bayer[2];
      *y++ = (8453*adjacent_bayer[1] + 16594*bayer[1] + 1611*t0 + 524288)
	     >> 15;
      bayer++;
      adjacent_bayer++;

      t0 = bayer[0] + bayer[2] + adjacent_bayer[1];
      t1 = adjacent_bayer[0] + adjacent_bayer[2];
      *y++ = (4226*t1 + 5531*t0 + 3223*bayer[1] + 524288) >> 15;
      bayer++;
      adjacent_bayer++;
    }
  } else {
    for ( ; width > 2; width -= 2) {
      t0 = bayer[0] + bayer[2];
      *y++ = (4226*t0 + 16594*bayer[1] + 3223*adjacent_bayer[1] + 524288)
	     >> 15;
      bayer++;
      adjacent_bayer++;

      t0 = bayer[0] + bayer[2] + adjacent_bayer[1];
      t1 = adjacent_bayer[0] + adjacent_bayer[2];
      *y++ = (8453*bayer[1] + 5531*t0 + 1611*t1 + 524288) >> 15;
      bayer++;
      adjacent_bayer++;
    }
  }

  if (width == 2) {
    /* Second to last pixel */
    t0 = bayer[0] + bayer[2];
    if (blue_line) {
      *y++ = (8453*adjacent_bayer[1] + 16594*bayer[1] + 1611*t0 + 524288)
	     >> 15;
    } else {
      *y++ = (4226*t0 + 16594*bayer[1] + 3223*adjacent_bayer[1] + 524288)
	     >> 15;
    }
    /* Last pixel */
    t0 = bayer[1] + adjacent_bayer[2];
    if (blue_line) {
      *y++ = (8453*adjacent_bayer[1] + 8297*t0 + 3223*bayer[2] + 524288)
	     >> 15;
    } else {
      *y++ = (8453*bayer[2] + 8297*t0 + 3223*adjacent_bayer[1] + 524288)
	     >> 15;
    }
  } else {
    /* Last pixel */
    if (blue_line) {
      *y++ = (8453*adjacent_bayer[1] + 16594*bayer[1] + 3223*bayer[0] + 524288)
	     >> 15;
    } else {
      *y++ = (8453*bayer[0] + 16594*bayer[1] + 3223*adjacent_bayer[1] + 524288)
	     >> 15;
    }
  }
}

void v4lconvert_bayer_to_yuv420(const unsigned char *bayer,
  unsigned char *yuv, int width, int height, unsigned int pixfmt)
{
  int blue_line = 0, start_with_green = 0, x, y;
  unsigned char *ydst = yuv;
  unsigned char *udst = yuv + width * height;
  unsigned char *vdst = udst + width * height / 4;

  /* First calculate the u and v planes 2x2 pixels at a time */
  switch (pixfmt) {
    case V4L2_PIX_FMT_SBGGR8:
      for (y = 0; y < height; y += 2) {
	for (x = 0; x < width; x += 2) {
	  int b, g, r;
	  b  = bayer[x];
	  g  = bayer[x+1];
	  g += bayer[x+width];
	  r  = bayer[x+width+1];
	  *udst++ = (-4878 * r - 4789 * g + 14456 * b + 4210688) >> 15;
	  *vdst++ = (14456 * r - 6052 * g -  2351 * b + 4210688) >> 15;
	}
	bayer += 2 * width;
      }
      blue_line = 1;
      break;

    case V4L2_PIX_FMT_SRGGB8:
      for (y = 0; y < height; y += 2) {
	for (x = 0; x < width; x += 2) {
	  int b, g, r;
	  r  = bayer[x];
	  g  = bayer[x+1];
	  g += bayer[x+width];
	  b  = bayer[x+width+1];
	  *udst++ = (-4878 * r - 4789 * g + 14456 * b + 4210688) >> 15;
	  *vdst++ = (14456 * r - 6052 * g -  2351 * b + 4210688) >> 15;
	}
	bayer += 2 * width;
      }
      break;

    case V4L2_PIX_FMT_SGBRG8:
      for (y = 0; y < height; y += 2) {
	for (x = 0; x < width; x += 2) {
	  int b, g, r;
	  g  = bayer[x];
	  b  = bayer[x+1];
	  r  = bayer[x+width];
	  g += bayer[x+width+1];
	  *udst++ = (-4878 * r - 4789 * g + 14456 * b + 4210688) >> 15;
	  *vdst++ = (14456 * r - 6052 * g -  2351 * b + 4210688) >> 15;
	}
	bayer += 2 * width;
      }
      blue_line = 1;
      start_with_green = 1;
      break;

    case V4L2_PIX_FMT_SGRBG8:
      for (y = 0; y < height; y += 2) {
	for (x = 0; x < width; x += 2) {
	  int b, g, r;
	  g  = bayer[x];
	  r  = bayer[x+1];
	  b  = bayer[x+width];
	  g += bayer[x+width+1];
	  *udst++ = (-4878 * r - 4789 * g + 14456 * b + 4210688) >> 15;
	  *vdst++ = (14456 * r - 6052 * g -  2351 * b + 4210688) >> 15;
	}
	bayer += 2 * width;
      }
      start_with_green = 1;
      break;
  }

  bayer -= width * height;

  /* render the first line */
  v4lconvert_border_bayer_line_to_y(bayer, bayer + width, ydst, width,
    start_with_green, blue_line);
  ydst += width;

  /* reduce height by 2 because of the border */
  for (height -= 2; height; height--) {
    int t0, t1;
    /* (width - 2) because of the border */
    const unsigned char *bayerEnd = bayer + (width - 2);

    if (start_with_green) {
      t0 = bayer[1] + bayer[width * 2 + 1];
      /* Write first pixel */
      t1 = bayer[0] + bayer[width * 2] + bayer[width + 1];
      if (blue_line)
	*ydst++ = (8453*bayer[width] + 5516*t1 + 1661*t0 + 524288) >> 15;
      else
	*ydst++ = (4226*t0 + 5516*t1 + 3223*bayer[width] + 524288) >> 15;

      /* Write second pixel */
      t1 = bayer[width] + bayer[width + 2];
      if (blue_line)
	*ydst++ = (4226*t1 + 16594*bayer[width+1] + 1611*t0 + 524288) >> 15;
      else
	*ydst++ = (4226*t0 + 16594*bayer[width+1] + 1611*t1 + 524288) >> 15;
      bayer++;
    } else {
      /* Write first pixel */
      t0 = bayer[0] + bayer[width * 2];
      if (blue_line) {
	*ydst++ = (8453*bayer[width+1] + 16594*bayer[width] + 1661*t0 +
		   524288) >> 15;
      } else {
	*ydst++ = (4226*t0 + 16594*bayer[width] + 3223*bayer[width+1] +
		   524288) >> 15;
      }
    }

    if (blue_line) {
      for (; bayer <= bayerEnd - 2; bayer += 2) {
	t0 = bayer[0] + bayer[2] + bayer[width * 2] + bayer[width * 2 + 2];
	t1 = bayer[1] + bayer[width] + bayer[width + 2] + bayer[width * 2 + 1];
	*ydst++ = (8453*bayer[width+1] + 4148*t1 + 806*t0 + 524288) >> 15;

	t0 = bayer[2] + bayer[width * 2 + 2];
	t1 = bayer[width + 1] + bayer[width + 3];
	*ydst++ = (4226*t1 + 16594*bayer[width+2] + 1611*t0 + 524288) >> 15;
      }
    } else {
      for (; bayer <= bayerEnd - 2; bayer += 2) {
	t0 = bayer[0] + bayer[2] + bayer[width * 2] + bayer[width * 2 + 2];
	t1 = bayer[1] + bayer[width] + bayer[width + 2] + bayer[width * 2 + 1];
	*ydst++ = (2113*t0 + 4148*t1 + 3223*bayer[width+1] + 524288) >> 15;

	t0 = bayer[2] + bayer[width * 2 + 2];
	t1 = bayer[width + 1] + bayer[width + 3];
	*ydst++ = (4226*t0 + 16594*bayer[width+2] + 1611*t1 + 524288) >> 15;
      }
    }

    if (bayer < bayerEnd) {
      /* Write second to last pixel */
      t0 = bayer[0] + bayer[2] + bayer[width * 2] + bayer[width * 2 + 2];
      t1 = bayer[1] + bayer[width] + bayer[width + 2] + bayer[width * 2 + 1];
      if (blue_line)
	*ydst++ = (8453*bayer[width+1] + 4148*t1 + 806*t0 + 524288) >> 15;
      else
	*ydst++ = (2113*t0 + 4148*t1 + 3223*bayer[width+1] + 524288) >> 15;

      /* write last pixel */
      t0 = bayer[2] + bayer[width * 2 + 2];
      if (blue_line) {
	*ydst++ = (8453*bayer[width+1] + 16594*bayer[width+2] + 1661*t0 +
		   524288) >> 15;
      } else {
	*ydst++ = (4226*t0 + 16594*bayer[width+2] + 3223*bayer[width+1] +
		   524288) >> 15;
      }
      bayer++;
    } else {
      /* write last pixel */
      t0 = bayer[0] + bayer[width * 2];
      t1 = bayer[1] + bayer[width * 2 + 1] + bayer[width];
      if (blue_line)
	*ydst++ = (8453*bayer[width+1] + 5516*t1 + 1661*t0 + 524288) >> 15;
      else
	*ydst++ = (4226*t0 + 5516*t1 + 3223*bayer[width+1] + 524288) >> 15;
    }

    /* skip 2 border pixels */
    bayer += 2;

    blue_line = !blue_line;
    start_with_green = !start_with_green;
  }

  /* render the last line */
  v4lconvert_border_bayer_line_to_y(bayer + width, bayer, ydst, width,
    !start_with_green, !blue_line);
}