[libcamera-devel,RFC,v2,2/4] libcamera: swisp: The software ISP class
diff mbox series

Message ID tencent_BF476F7B7C811ABED21EA0949CB140575509@qq.com
State New
Headers show
Series
  • pipeline: isp: The software ISP module
Related show

Commit Message

Siyuan Fan Aug. 11, 2021, 6:12 a.m. UTC
From: Fan Siyuan <siyuan.fan@foxmail.com>

 Changes in V2:
 -Design the abstract class ISP, CPU_ISP and future GPU_ISP inherit from it.
 -Add the rgb buf alloc and thread control member function

 Now the ISP must implement the below process:
 BLC-->demosaic-->AWB-->tone mapping-->gamma-->Noise reduction

 Plan to add:
 lens shading correct-->Color correct-->color space transform

Signed-off-by: Fan Siyuan <siyuan.fan@foxmail.com>
---
 src/libcamera/swisp/isp.cpp | 643 ++++++++++++++++++++++++++++++++++++
 src/libcamera/swisp/isp.h   |  92 ++++++
 2 files changed, 735 insertions(+)
 create mode 100644 src/libcamera/swisp/isp.cpp
 create mode 100644 src/libcamera/swisp/isp.h

Comments

Paul Elder Aug. 11, 2021, 10:15 a.m. UTC | #1
Hi Siyuan,

On Wed, Aug 11, 2021 at 07:12:56AM +0100, Siyuan Fan wrote:
> From: Fan Siyuan <siyuan.fan@foxmail.com>
> 
>  Changes in V2:

You skipped all of my comments from v1, so I'll only comment on the
ISP interface.

>  -Design the abstract class ISP, CPU_ISP and future GPU_ISP inherit from it.
>  -Add the rgb buf alloc and thread control member function
> 
>  Now the ISP must implement the below process:
>  BLC-->demosaic-->AWB-->tone mapping-->gamma-->Noise reduction
> 
>  Plan to add:
>  lens shading correct-->Color correct-->color space transform
> 
> Signed-off-by: Fan Siyuan <siyuan.fan@foxmail.com>
> ---
>  src/libcamera/swisp/isp.cpp | 643 ++++++++++++++++++++++++++++++++++++
>  src/libcamera/swisp/isp.h   |  92 ++++++
>  2 files changed, 735 insertions(+)
>  create mode 100644 src/libcamera/swisp/isp.cpp
>  create mode 100644 src/libcamera/swisp/isp.h
> 
> diff --git a/src/libcamera/swisp/isp.cpp b/src/libcamera/swisp/isp.cpp
> new file mode 100644
> index 00000000..6d073040
> --- /dev/null
> +++ b/src/libcamera/swisp/isp.cpp
> @@ -0,0 +1,643 @@
> +/* SPDX-License-Identifier: LGPL-2.1-or-later */
> +/*
> + * Copyright (C) 2021, Siyuan Fan <siyuan.fan@foxmail.com>
> + *
> + * isp.cpp - The software ISP class
> + */
> +
> +#include "isp.h"
> +
> +#include <math.h>
> +#include <stdlib.h>
> +#include <string.h>
> +#include <sys/mman.h>
> +#include <unistd.h>
> +
> +#include <libcamera/request.h>
> +#include <libcamera/file_descriptor.h>
> +
> +#include "libcamera/base/log.h"
> +
> +namespace libcamera{
> +
> +LOG_DECLARE_CATEGORY(ISP)
> +
> +void CPU_ISP::autoContrast(uint16_t *data, float lowCut, float highCut, int width, int height)
> +{
> +        int blue, gr, gb, red;
> +        int histBlue[1024] = {0}, histGb[1024] = {0}, histGr[1024] = {0}, histRed[1024] = {0};
> +
> +        int index;
> +        for (int i = 0; i < height; i++) {
> +                index = i * width;
> +                for (int j = 0; j < width; j++) {
> +                        if (i % 2 == 0 && j % 2 == 0) {
> +                                blue = data[index];
> +                                histBlue[blue]++;
> +                        }
> +                        else if ((i % 2 == 0 && j % 2 == 1)) {
> +                                gb = data[index];
> +                                histGb[gb]++;
> +                        }
> +                        else if ((i % 2 == 1 && j % 2 == 0)) {
> +                                gr = data[index];
> +                                histGr[gr]++;
> +                        }
> +                        else {
> +                                red = data[index];
> +                                histRed[red]++;
> +                        }
> +                        index++;
> +                }    
> +        }
> +
> +        int pixelAmount = width * height;
> +        int sum = 0;
> +        int minBlue;
> +        for (int i = 0; i < 1024; i++){
> +                sum = sum + histBlue[i];
> +                if (sum >= pixelAmount * lowCut * 0.01) {
> +                        minBlue = i;
> +                        break;
> +                }
> +        }
> +
> +        sum = 0;
> +        int maxBlue;
> +        for (int i = 1023; i >= 0; i--){
> +                sum = sum + histBlue[i];
> +                if (sum >= pixelAmount * highCut * 0.01) {
> +                        maxBlue = i;
> +                        break;
> +                }
> +        }
> +
> +        sum = 0;
> +        int minGb;
> +        for (int i = 0; i < 1024; i++){
> +                sum = sum + histGb[i];
> +                if (sum >= pixelAmount * lowCut * 0.01) {
> +                        minGb = i;
> +                        break;
> +                }
> +        }
> +
> +        sum = 0;
> +        int maxGb;
> +        for (int i = 1023; i >= 0; i--){
> +                sum = sum + histGb[i];
> +                if (sum >= pixelAmount * highCut * 0.01) {
> +                        maxGb = i;
> +                        break;
> +                }
> +        }
> +
> +        sum = 0;
> +        int minGr;
> +        for (int i = 0; i < 1024; i++){
> +                sum = sum + histGr[i];
> +                if (sum >= pixelAmount * lowCut * 0.01) {
> +                        minGr = i;
> +                        break;
> +                }
> +        }
> +
> +        sum = 0;
> +        int maxGr;
> +        for (int i = 1023; i >= 0; i--){
> +                sum = sum + histGr[i];
> +                if (sum >= pixelAmount * highCut * 0.01) {
> +                        maxGr = i;
> +                        break;
> +                }
> +        }
> +
> +        sum = 0;
> +        int minRed;
> +        for (int i = 0; i < 1024; i++){
> +                sum = sum + histRed[i];
> +                if (sum >= pixelAmount * lowCut * 0.01) {
> +                        minRed = i;
> +                        break;
> +                }
> +        }
> +
> +        sum = 0;
> +        int maxRed;
> +        for (int i = 1023; i >= 0; i--){
> +                sum = sum + histRed[i];
> +                if (sum >= pixelAmount * highCut * 0.01) {
> +                        maxRed = i;
> +                        break;
> +                }
> +        }
> +
> +        int blueMap[1024];
> +        float norb = 1.0 / (maxBlue - minBlue);
> +        for (int i = 0; i < 1024; i++) {
> +                if (i < minBlue) {
> +                        blueMap[i] = 0;
> +                }
> +                else if (i > maxBlue) {
> +                        blueMap[i] = 1023;
> +                }
> +                else {
> +                        blueMap[i] = (i - minBlue) * norb * 1023;
> +                }
> +                if (blueMap[i] > 1023) blueMap[i] = 1023;
> +        }
> +
> +        int gbMap[1024];
> +        float norgb = 1.0 / (maxGb - minGb);
> +        for (int i = 0; i < 1024; i++) {
> +                if (i < minGb) {
> +                        gbMap[i] = 0;
> +                }
> +                else if (i > maxGb) {
> +                        gbMap[i] = 1023;
> +                }
> +                else {
> +                        gbMap[i] = (i - minGb) * norgb * 1023;
> +                }
> +                if (gbMap[i] > 1023) gbMap[i] = 1023;
> +        }
> +
> +        int grMap[1024];
> +        float norgr = 1.0 / (maxGr - minGr);
> +        for (int i = 0; i < 1024; i++) {
> +                if (i < minGr) {
> +                        grMap[i] = 0;
> +                }
> +                else if (i > maxGr) {
> +                        grMap[i] = 1023;
> +                }
> +                else {
> +                        grMap[i] = (i - minGr) * norgr * 1023;
> +                }
> +                if (grMap[i] > 1023) grMap[i] = 1023;
> +        }
> +
> +        int redMap[1024];
> +        float norr = 1.0 / (maxRed - minRed);
> +        for (int i = 0; i < 1024; i++) {
> +                if (i < minRed) {
> +                        redMap[i] = 0;
> +                }
> +                else if (i > maxRed) {
> +                        redMap[i] = 1023;
> +                }
> +                else{
> +                        redMap[i] = (i - minRed) * norr * 1023;
> +                }
> +                if (redMap[i] > 1023) redMap[i] = 1023;
> +        }
> +
> +        for (int i = 0;i < height; i++) {
> +                for (int j = 0; j < width; j++){
> +                        index = i * width;
> +                        if (i % 2 == 0 && j % 2 == 0) {
> +                                data[index] = blueMap[data[index]];
> +                        }    
> +                        else if (i % 2 == 0 && j % 2 == 1) {
> +                                data[index] = gbMap[data[index]];
> +                        }    
> +                        else if (i % 2 == 1 && j % 2 == 0) {
> +                                data[index] = grMap[data[index]];
> +                        }    
> +                        else {
> +                                data[index] = redMap[data[index]];
> +                        }
> +                        index++;
> +                }
> +        }
> +}
> +
> +void CPU_ISP::blackLevelCorrect(uint16_t *data, uint16_t offset, int width, int height)
> +{
> +        int len = width * height;
> +        for(int i = 0; i < len; i++) {
> +                if (data[i] < offset){
> +                        data[i] = 0;
> +                }
> +                else {
> +                        data[i] -= offset;
> +                }
> +        }
> +}
> +
> +void CPU_ISP::readChannels(uint16_t *data, uint16_t *R, uint16_t *G, uint16_t *B,
> +                       int width, int height)
> +{
> +        int index;
> +        for (int i = 0; i < height; i++) {
> +                index = i * width;
> +                for (int j = 0; j < width; j++) {
> +                        if (i % 2 == 0 && j % 2 == 0) {
> +                                B[index] = data[index];
> +                        }
> +                        else if ((i % 2 == 0 && j % 2 == 1) || (i % 2 == 1 && j % 2 == 0)){
> +                                G[index] = data[index];
> +                        }
> +                        else {
> +                                R[index] = data[index];
> +                        }
> +                        index++;
> +                }
> +        }
> +}
> +
> +void CPU_ISP::firstPixelInsert(uint16_t *src, uint16_t *dst, int width, int height)
> +{
> +        int index;
> +        for (int i = 0; i < height; i++) {
> +                index = i * width;
> +                for (int j = 0; j < width; j++){
> +                        if (i % 2 == 0 && j % 2 == 1) {
> +                                if (j == (width - 1)) {
> +                                        dst[index] = src[index - 1];
> +                                }
> +                                else {
> +                                        dst[index] = (src[index - 1] +
> +                                                src[index + 1]) >> 1;
> +                                }
> +                        }
> +
> +                        if (i % 2 == 1 && j % 2 == 0) {
> +                                if(i == height - 1) {
> +                                        dst[index] = src[index - width];
> +                                }
> +                                else {
> +                                        dst[index] = (src[index - width]+
> +                                                src[index + width]) >> 1;
> +                                }
> +                        }
> +    
> +                        if (i % 2 == 1 && j % 2 == 1) {
> +                                if (j < width - 1 && i < height - 1) {
> +                                        dst[index] = (src[index - width - 1] +
> +                                                src[index - width + 1] +
> +                                                src[index + width - 1] +
> +                                                src[index + width + 1]) >> 2;
> +                                }
> +                                else if (i == height - 1 && j < width - 1) {
> +                                        dst[index] = (src[index - width - 1] +
> +                                                src[index - width + 1]) >> 1;
> +                                }
> +                                else if (i < height - 1 && j == width - 1) {
> +                                        dst[index] = (src[index - width - 1] +
> +                                                src[index + width - 1]) >> 1;
> +                                }
> +                                else {
> +                                        dst[index] = src[index - width - 1];
> +                                }          
> +                        }
> +                        index++;
> +                }
> +        }
> +}
> +
> +void CPU_ISP::twoPixelInsert(uint16_t *src, uint16_t *dst, int width, int height)
> +{
> +        int index;
> +        for (int i = 0; i < height; i++) {
> +                index = i * width;
> +                for (int j = 0; j < width; j++) {
> +                        if (i == 0 && j == 0) {
> +                                dst[index] = (src[index + width] +
> +                                        src[index + 1]) >> 1;
> +                        }
> +                        else if (i == 0 && j > 0 && j % 2 == 0) {
> +                                dst[index] = (src[index - 1] +
> +                                        src[index + width] +
> +                                        src[index + 1]) / 3;
> +                        }
> +                        else if (i > 0 && j == 0 && i % 2 == 0) {
> +                                dst[index] = (src[index - width] +
> +                                        src[index + 1] +
> +                                        src[index + width]) / 3;
> +                        }
> +                        else if (i == (height - 1) && j < (width - 1) && j % 2 == 1) {
> +                                dst[index] = (src[index - 1] +
> +                                        src[index - width] +
> +                                        src[index + 1]) / 3;
> +                        }
> +                        else if (i < (height - 1) && j == (width - 1) && i % 2 == 1) {
> +                                dst[index] = (src[index - width] +
> +                                        src[index - 1] +
> +                                        src[index + width]) / 3;
> +                        }
> +                        else if (i == (height - 1) && j == (width - 1)) {
> +                                dst[index] = (src[index - width] +
> +                                        src[index - 1]) >> 1;
> +                        }
> +                        else if ((i % 2 == 0 && j % 2 == 0) || (i % 2 == 1 && j % 2 == 1)) {
> +                                dst[index] = (src[index - 1] +
> +                                        src[index + 1] +
> +                                        src[index - width] +
> +                                        src[index + width]) / 4;
> +                        }
> +                        index++;
> +                }
> +        }
> +}
> +
> +void CPU_ISP::lastPixelInsert(uint16_t *src, uint16_t *dst, int width, int height)
> +{
> +        int index;
> +        for (int i = 0; i < height; i++) {
> +                index = i * width;
> +                for (int j = 0; j < width; j++){
> +                        if (i % 2 == 1 && j % 2 == 0) {
> +                                if (j == 0) {
> +                                        dst[index] = src[index + 1];
> +                                }
> +                                else {
> +                                        dst[index] = (src[index - 1] +
> +                                                src[index + 1]) >> 1;
> +                                }
> +                        }
> +
> +                        if (i % 2 == 0 && j % 2 == 1) {
> +                                if(i == 0) {
> +                                        dst[index] = src[index + width];
> +                                }
> +                                else {
> +                                        dst[index] = (src[index - width]+
> +                                                src[index + width]) >> 1;
> +                                }
> +                        }
> +
> +                        if (i % 2 == 0 && j % 2 == 0) {
> +                                if (i > 0 && j > 0) {
> +                                        dst[index] = (src[index - width - 1] +
> +                                                src[index - width + 1] +
> +                                                src[index + width - 1] +
> +                                                src[index + width + 1]) >> 2;
> +                                }
> +                                else if (i == 0 && j > 0) {
> +                                        dst[index] = (src[index + width - 1] +
> +                                                src[index + width + 1]) >> 1;
> +                                }
> +                                else if (i > 0 && j == 0) {
> +                                        dst[index] = (src[index - width + 1] +
> +                                                src[index + width + 1]) >> 1;
> +                                }
> +                                else {
> +                                        dst[index] = src[index + width + 1];
> +                                }
> +                        }
> +                        index++;
> +                }
> +        }
> +}
> +
> +void CPU_ISP::demosaic(uint16_t *data, uint16_t *R, uint16_t *G, uint16_t *B,
> +                       int width, int height)
> +{
> +        firstPixelInsert(data, B, width, height);
> +        twoPixelInsert(data, G, width, height);
> +        lastPixelInsert(data, R, width, height);
> +}
> +
> +void CPU_ISP::autoWhiteBalance(uint16_t *R, uint16_t *G, uint16_t *B, int width, int height)
> +{
> +        float aveB = 0, aveG = 0, aveR = 0;
> +        float Kb, Kg, Kr;
> +
> +        for (int i = 0; i < width * height; i++) {
> +                aveB += 1.0 * B[i];
> +                aveG += 1.0 * G[i];
> +                aveR += 1.0 * R[i];
> +        }
> +
> +        aveB *= (1.0 / (width * height));
> +        aveG *= (1.0 / (width * height));
> +        aveR *= (1.0 / (width * height));
> +
> +        Kr = (aveB + aveG + aveR) / aveR * (1.0 / 3.0);
> +        Kg = (aveB + aveG + aveR) / aveG * (1.0 / 3.0);
> +        Kb = (aveB + aveG + aveR) / aveB * (1.0 / 3.0);
> +
> +        for (int i = 0; i < width * height; i++) {
> +                B[i] = B[i] * Kb;
> +                G[i] = G[i] * Kg;
> +                R[i] = R[i] * Kr;
> +
> +                if (R[i] > 1023) R[i] = 1023;
> +                if (G[i] > 1023) G[i] = 1023;
> +                if (R[i] > 1023) B[i] = 1023;
> +        }
> +}
> +
> +void CPU_ISP::gammaCorrect(uint16_t *R, uint16_t *G, uint16_t *B, float val, int width, int height)
> +{
> +        float nor = 1.0 / 1023.0;
> +        float gamma = 1.0 / val;
> +        for (int i = 0; i < width * height; i++) {
> +                R[i] = pow(R[i] * nor, gamma) * 1023;
> +                G[i] = pow(G[i] * nor, gamma) * 1023;
> +                B[i] = pow(B[i] * nor, gamma) * 1023;
> +
> +                if (R[i] > 1023) R[i] = 1023;
> +                if (G[i] > 1023) G[i] = 1023;
> +                if (B[i] > 1023) B[i] = 1023;
> +        }
> +}
> +
> +void CPU_ISP::compress_10bit_to_8bit (uint16_t *src, uint8_t *dst, int width, int height)
> +{
> +        for (int i = 0; i < width * height; i++) {
> +                dst[i] = src[i] >> 2 & 0xff;
> +        }
> +}
> +
> +float CPU_ISP::distance(int x, int y, int i, int j)
> +{
> +    return float(sqrt(pow(x - i, 2) + pow(y - j, 2)));
> +}
> +
> +double CPU_ISP::gaussian(float x, double sigma)
> +{
> +    return exp(-(pow(x, 2)) / (2 * pow(sigma, 2))) / (2 * 3.1415926 * pow(sigma, 2));
> +}
> +
> +void CPU_ISP::bilateralFilter(uint16_t *R, uint16_t *G, uint16_t *B,
> +                          int diameter, double sigmaI,
> +                          double sigmaS, int width, int height)
> +{
> +        for (int i = 2; i < height - 2; i++) {
> +                for (int j = 2; j < width - 2; j++) {
> +                        double iFiltered = 0;
> +                        double wp = 0;
> +                        int neighbor_x = 0;
> +                        int neighbor_y = 0;
> +                        int half = diameter / 2;
> +
> +                        for (int k = 0; k < diameter; k++) {
> +                                for (int l = 0; l < diameter; l++) {
> +                                        neighbor_x = i - (half - k);
> +                                        neighbor_y = j - (half - l);
> +                                        double gi = gaussian(R[neighbor_x * width + neighbor_y] - R[i * width +j], sigmaI);
> +                                        double gs = gaussian(distance(i, j, neighbor_x, neighbor_y), sigmaS);
> +                                        double w = gi * gs;
> +                                        iFiltered = iFiltered + R[neighbor_x * width + neighbor_y] * w;
> +                                        wp = wp + w;
> +                                }    
> +                        }
> +
> +                        iFiltered = iFiltered / wp;
> +                        R[i * width + j] = iFiltered;
> +                }
> +        }
> +
> +        for (int i = 2; i < height - 2; i++) {
> +                for (int j = 2; j < width - 2; j++) {
> +                        double iFiltered = 0;
> +                        double wp = 0;
> +                        int neighbor_x = 0;
> +                        int neighbor_y = 0;
> +                        int half = diameter / 2;
> +
> +                        for (int k = 0; k < diameter; k++) {
> +                                for (int l = 0; l < diameter; l++) {
> +                                        neighbor_x = i - (half - k);
> +                                        neighbor_y = j - (half - l);
> +                                        double gi = gaussian(G[neighbor_x * width + neighbor_y] - G[i * width +j], sigmaI);
> +                                        double gs = gaussian(distance(i, j, neighbor_x, neighbor_y), sigmaS);
> +                                        double w = gi * gs;
> +                                        iFiltered = iFiltered + G[neighbor_x * width + neighbor_y] * w;
> +                                        wp = wp + w;
> +                                }    
> +                        }
> +
> +                        iFiltered = iFiltered / wp;
> +                        G[i * width + j] = iFiltered;
> +                }
> +        }
> +
> +        for (int i = 2; i < height - 2; i++) {
> +                for (int j = 2; j < width - 2; j++) {
> +                        double iFiltered = 0;
> +                        double wp = 0;
> +                        int neighbor_x = 0;
> +                        int neighbor_y = 0;
> +                        int half = diameter / 2;
> +
> +                        for (int k = 0; k < diameter; k++) {
> +                                for (int l = 0; l < diameter; l++) {
> +                                        neighbor_x = i - (half - k);
> +                                        neighbor_y = j - (half - l);
> +                                        double gi = gaussian(B[neighbor_x * width + neighbor_y] - B[i * width +j], sigmaI);
> +                                        double gs = gaussian(distance(i, j, neighbor_x, neighbor_y), sigmaS);
> +                                        double w = gi * gs;
> +                                        iFiltered = iFiltered + B[neighbor_x * width + neighbor_y] * w;
> +                                        wp = wp + w;
> +                                }    
> +                        }
> +
> +                        iFiltered = iFiltered / wp;
> +                        B[i * width + j] = iFiltered;
> +                }
> +        }
> +}
> +
> +void CPU_ISP::noiseReduction(uint16_t *R, uint16_t *G, uint16_t *B, int width, int height)
> +{
> +        bilateralFilter(R, G, B, 5, 24.0, 32.0, width, height);
> +}
> +
> +void CPU_ISP::processing(FrameBuffer *srcBuffer, FrameBuffer *dstBuffer, int width, int height)
> +{
> +        uint8_t *rgb_buf;
> +        uint16_t *rawData;
> +        uint16_t *rgbData = new std::uint16_t[width * height * 3];
> +
> +        uint16_t *rData = rgbData;
> +        uint16_t *gData = rData + width * height;
> +        uint16_t *bData = gData + width * height;
> +        memset(rgbData, 0x0, width * height * 3);
> +
> +        const FrameBuffer::Plane &plane =  srcBuffer->planes()[0]; 
> +        rawData = (uint16_t *)mmap(NULL, plane.length, PROT_READ|PROT_WRITE, MAP_SHARED, plane.fd.fd(), 0);
> +        LOG(ISP, Debug) << plane.length << " " << plane.fd.fd();
> +        if (rawData == MAP_FAILED) {
> +            LOG(ISP, Error) << "Read raw data failed";
> +            ispCompleted.emit(dstBuffer);
> +        }
> +
> +        blackLevelCorrect(rawData, 16, width, height);
> +        readChannels(rawData, rData, gData, bData, width, height);
> +        demosaic(rawData, rData, gData, bData, width, height);
> +        autoWhiteBalance(rData, gData, bData, width, height);
> +        autoContrast(rData, 0.01, 0.01, width, height);
> +        autoContrast(gData, 0.01, 0.01, width, height);
> +        autoContrast(bData, 0.01, 0.01, width, height);
> +        gammaCorrect(rData, gData, bData, 2.2, width, height);
> +        //bilateralFilter(rData, gData, bData, 5, 24.0, 32.0, width, height);
> +    
> +        const FrameBuffer::Plane &rgbPlane = dstBuffer->planes()[0];
> +        rgb_buf = (uint8_t *)mmap(NULL, rgbPlane.length, PROT_READ|PROT_WRITE, MAP_SHARED, rgbPlane.fd.fd(), 0);
> +        if (rgb_buf == MAP_FAILED) {
> +                LOG(ISP, Error) << "Read rgb data failed";
> +                ispCompleted.emit(dstBuffer);
> +        }
> +
> +        uint8_t *rData_8 = rgb_buf;
> +        uint8_t *gData_8 = rData_8 + width * height;
> +        uint8_t *bData_8 = gData_8 + width * height;
> +
> +        compress_10bit_to_8bit(rData, rData_8, width, height);
> +        compress_10bit_to_8bit(gData, gData_8, width, height);
> +        compress_10bit_to_8bit(bData, bData_8, width, height);
> +
> +        dstBuffer->metadata_.status = srcBuffer->metadata().status;
> +        dstBuffer->metadata_.sequence = srcBuffer->metadata().sequence;
> +        dstBuffer->metadata_.timestamp = srcBuffer->metadata().timestamp;
> +
> +        dstBuffer->metadata_.planes.clear();
> +        dstBuffer->metadata_.planes.push_back({rgbPlane.length});
> +
> +        delete[] rgbData;
> +
> +        ispCompleted.emit(dstBuffer);
> +}
> +
> +int CPU_ISP::exportBuffers(std::vector<std::unique_ptr<FrameBuffer>> *buffers,
> +                       unsigned int count, int width, int height)
> +{
> +        int bufferByte = width * height * 3;
> +
> +        for (unsigned int i = 0; i < count; i++) {
> +                std::string name = "frame-" + std::to_string(i);
> +
> +                const int isp_fd = memfd_create(name.c_str(), 0);
> +                int ret = ftruncate(isp_fd, bufferByte);
> +                if (ret < 0) {
> +                        LOG(ISP, Error) << "Failed to resize memfd" << strerror(-ret);
> +                        return ret;
> +                }
> +
> +                FrameBuffer::Plane rgbPlane;
> +                rgbPlane.fd = FileDescriptor(std::move(isp_fd));
> +                rgbPlane.length = bufferByte;
> +
> +                std::vector<FrameBuffer::Plane> planes{rgbPlane};
> +                buffers->emplace_back(std::make_unique<FrameBuffer>(std::move(planes)));
> +        }
> +
> +        return count;
> +}
> +
> +void CPU_ISP::startThreadISP()
> +{
> +        moveToThread(&thread_);
> +        thread_.start();
> +}
> +
> +void CPU_ISP::stopThreadISP()
> +{
> +        thread_.exit();
> +        thread_.wait();
> +}
> +
> +} /* namespace libcamera */
> \ No newline at end of file
> diff --git a/src/libcamera/swisp/isp.h b/src/libcamera/swisp/isp.h
> new file mode 100644
> index 00000000..4099e2c6
> --- /dev/null
> +++ b/src/libcamera/swisp/isp.h
> @@ -0,0 +1,92 @@
> +/* SPDX-License-Identifier: LGPL-2.1-or-later */
> +/*
> + * Copyright (C) 2021, Siyuan Fan <siyuan.fan@foxmail.com>
> + *
> + * isp.h - The software ISP class
> + */
> +#ifndef __LIBCAMERA_SWISP_ISP_H__
> +#define __LIBCAMERA_SWISP_ISP_H__
> +
> +#include <libcamera/framebuffer.h>
> +
> +#include "libcamera/base/object.h"
> +#include "libcamera/base/signal.h"
> +#include "libcamera/base/thread.h"
> +
> +namespace libcamera{
> +
> +using std::uint16_t;
> +using std::uint8_t;
> +
> +class ISP : public Object
> +{
> +public:
> +        ISP() {}

You're missing the functions that form the interface to the ISP. At the
moment these are:
- processing()
- exportBuffers()
- startThreadISP()
- stopThreadISP()
- Signal<FrameBuffer *> ispCompleted

In addition to these I think you need:
- formats() (supportedFormats()? I'm not sure what a good name is for this)
- configure()

Obviously for your implementation they'll be super simple, but they need
to be defined in the common software ISP interface.

Maybe some function for reporting supported sizes would be good too?
Your ISP doesn't look like it has such restriction, but I'm wondering if
GPU-based ones would. Or maybe GPU-based ISPs would only have a
restriction on size for specific performace ranges.

> +
> +        virtual void blackLevelCorrect(uint16_t *data, uint16_t offset, int width, int height) = 0;
> +
> +        virtual void demosaic(uint16_t *data, uint16_t *R, uint16_t *G, uint16_t *B,
> +                              int width, int height) = 0;
> +
> +        virtual void autoWhiteBalance(uint16_t *R, uint16_t *G, uint16_t *B, int width, int height) = 0;
> +
> +        virtual void gammaCorrect(uint16_t *R, uint16_t *G, uint16_t *B, float val, int width, int height)  = 0;
> +
> +        virtual void noiseReduction(uint16_t *R, uint16_t *G, uint16_t *B, int width, int height) = 0;

Will these be called directly by the pipeline handler? On one hand that
allows flexibility on the side of the pipeline handler, but on the other
hand it adds more inter-thread calls. Maybe it's better to just expose
one function (like you do now) and take a vector of operations that the
pipeline handler wants the ISP to do? Like

isp_->invokeMethod(processing, { BLC, Demosaic, AWB, NR }, rawBuf, rgbBuf, ...)

If these functions won't be called directly by the pipeline handler, and
software ISPs aren't required to implement these, then they should not
go here.

> +
> +        virtual ~ISP() {}

This should go right below ISP() {}

> +};
> +
> +class CPU_ISP : public ISP

When you sort class names alphabetically, it's nice for related classes
to be grouped together, which is why the common name goes first (like
the PipelineHandlers do), so I would call this ISPCPU.

> +{
> +public:
> +        void autoContrast(uint16_t *data, float lowCut, float highCut, int width, int height);
> +
> +        void blackLevelCorrect(uint16_t *data, uint16_t offset, int width, int height) override;
> +
> +        void readChannels(uint16_t *data, uint16_t *R, uint16_t *G, uint16_t *B,
> +                          int width, int height);
> +
> +        void firstPixelInsert(uint16_t *src, uint16_t *dst, int width, int height);
> +
> +        void twoPixelInsert(uint16_t *src, uint16_t *dst, int width, int height);
> +
> +        void lastPixelInsert(uint16_t *src, uint16_t *dst, int width, int height);
> +
> +        void demosaic(uint16_t *data, uint16_t *R, uint16_t *G, uint16_t *B,
> +                      int width, int height) override;
> +
> +        void autoWhiteBalance(uint16_t *R, uint16_t *G, uint16_t *B, int width, int height) override;
> +
> +        void gammaCorrect(uint16_t *R, uint16_t *G, uint16_t *B, float val, int width, int height) override;
> +
> +        float distance(int x, int y, int i, int j);
> +
> +        double gaussian(float x, double sigma);
> +
> +        void bilateralFilter(uint16_t *R, uint16_t *G, uint16_t *B,
> +                             int diameter, double sigmaI, double sigmaS,
> +                             int width, int height);
> +
> +        void noiseReduction(uint16_t *R, uint16_t *G, uint16_t *B, int width, int height) override;
> +
> +        void compress_10bit_to_8bit(uint16_t *src, uint8_t *dst, int width, int height);    

All of these should be private.

> +
> +        void processing(FrameBuffer *srcBuffer, FrameBuffer *dstBuffer, int width, int height);
> +
> +        int exportBuffers(std::vector<std::unique_ptr<FrameBuffer>> *buffers,
> +                           unsigned int count, int width, int height);
> +
> +        void startThreadISP();
> +
> +        void stopThreadISP();
> +
> +        Signal<FrameBuffer *> ispCompleted;
> +
> +private:
> +        Thread thread_;
> +};
> +
> +} /* namespace libcamera */
> +
> +#endif /* __LIBCAMERA_SWISP_ISP_H__ */
> \ No newline at end of file

You should have a newline at end of file.


Paul

> -- 
> 2.20.1
>

Patch
diff mbox series

diff --git a/src/libcamera/swisp/isp.cpp b/src/libcamera/swisp/isp.cpp
new file mode 100644
index 00000000..6d073040
--- /dev/null
+++ b/src/libcamera/swisp/isp.cpp
@@ -0,0 +1,643 @@ 
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2021, Siyuan Fan <siyuan.fan@foxmail.com>
+ *
+ * isp.cpp - The software ISP class
+ */
+
+#include "isp.h"
+
+#include <math.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <unistd.h>
+
+#include <libcamera/request.h>
+#include <libcamera/file_descriptor.h>
+
+#include "libcamera/base/log.h"
+
+namespace libcamera{
+
+LOG_DECLARE_CATEGORY(ISP)
+
+void CPU_ISP::autoContrast(uint16_t *data, float lowCut, float highCut, int width, int height)
+{
+        int blue, gr, gb, red;
+        int histBlue[1024] = {0}, histGb[1024] = {0}, histGr[1024] = {0}, histRed[1024] = {0};
+
+        int index;
+        for (int i = 0; i < height; i++) {
+                index = i * width;
+                for (int j = 0; j < width; j++) {
+                        if (i % 2 == 0 && j % 2 == 0) {
+                                blue = data[index];
+                                histBlue[blue]++;
+                        }
+                        else if ((i % 2 == 0 && j % 2 == 1)) {
+                                gb = data[index];
+                                histGb[gb]++;
+                        }
+                        else if ((i % 2 == 1 && j % 2 == 0)) {
+                                gr = data[index];
+                                histGr[gr]++;
+                        }
+                        else {
+                                red = data[index];
+                                histRed[red]++;
+                        }
+                        index++;
+                }    
+        }
+
+        int pixelAmount = width * height;
+        int sum = 0;
+        int minBlue;
+        for (int i = 0; i < 1024; i++){
+                sum = sum + histBlue[i];
+                if (sum >= pixelAmount * lowCut * 0.01) {
+                        minBlue = i;
+                        break;
+                }
+        }
+
+        sum = 0;
+        int maxBlue;
+        for (int i = 1023; i >= 0; i--){
+                sum = sum + histBlue[i];
+                if (sum >= pixelAmount * highCut * 0.01) {
+                        maxBlue = i;
+                        break;
+                }
+        }
+
+        sum = 0;
+        int minGb;
+        for (int i = 0; i < 1024; i++){
+                sum = sum + histGb[i];
+                if (sum >= pixelAmount * lowCut * 0.01) {
+                        minGb = i;
+                        break;
+                }
+        }
+
+        sum = 0;
+        int maxGb;
+        for (int i = 1023; i >= 0; i--){
+                sum = sum + histGb[i];
+                if (sum >= pixelAmount * highCut * 0.01) {
+                        maxGb = i;
+                        break;
+                }
+        }
+
+        sum = 0;
+        int minGr;
+        for (int i = 0; i < 1024; i++){
+                sum = sum + histGr[i];
+                if (sum >= pixelAmount * lowCut * 0.01) {
+                        minGr = i;
+                        break;
+                }
+        }
+
+        sum = 0;
+        int maxGr;
+        for (int i = 1023; i >= 0; i--){
+                sum = sum + histGr[i];
+                if (sum >= pixelAmount * highCut * 0.01) {
+                        maxGr = i;
+                        break;
+                }
+        }
+
+        sum = 0;
+        int minRed;
+        for (int i = 0; i < 1024; i++){
+                sum = sum + histRed[i];
+                if (sum >= pixelAmount * lowCut * 0.01) {
+                        minRed = i;
+                        break;
+                }
+        }
+
+        sum = 0;
+        int maxRed;
+        for (int i = 1023; i >= 0; i--){
+                sum = sum + histRed[i];
+                if (sum >= pixelAmount * highCut * 0.01) {
+                        maxRed = i;
+                        break;
+                }
+        }
+
+        int blueMap[1024];
+        float norb = 1.0 / (maxBlue - minBlue);
+        for (int i = 0; i < 1024; i++) {
+                if (i < minBlue) {
+                        blueMap[i] = 0;
+                }
+                else if (i > maxBlue) {
+                        blueMap[i] = 1023;
+                }
+                else {
+                        blueMap[i] = (i - minBlue) * norb * 1023;
+                }
+                if (blueMap[i] > 1023) blueMap[i] = 1023;
+        }
+
+        int gbMap[1024];
+        float norgb = 1.0 / (maxGb - minGb);
+        for (int i = 0; i < 1024; i++) {
+                if (i < minGb) {
+                        gbMap[i] = 0;
+                }
+                else if (i > maxGb) {
+                        gbMap[i] = 1023;
+                }
+                else {
+                        gbMap[i] = (i - minGb) * norgb * 1023;
+                }
+                if (gbMap[i] > 1023) gbMap[i] = 1023;
+        }
+
+        int grMap[1024];
+        float norgr = 1.0 / (maxGr - minGr);
+        for (int i = 0; i < 1024; i++) {
+                if (i < minGr) {
+                        grMap[i] = 0;
+                }
+                else if (i > maxGr) {
+                        grMap[i] = 1023;
+                }
+                else {
+                        grMap[i] = (i - minGr) * norgr * 1023;
+                }
+                if (grMap[i] > 1023) grMap[i] = 1023;
+        }
+
+        int redMap[1024];
+        float norr = 1.0 / (maxRed - minRed);
+        for (int i = 0; i < 1024; i++) {
+                if (i < minRed) {
+                        redMap[i] = 0;
+                }
+                else if (i > maxRed) {
+                        redMap[i] = 1023;
+                }
+                else{
+                        redMap[i] = (i - minRed) * norr * 1023;
+                }
+                if (redMap[i] > 1023) redMap[i] = 1023;
+        }
+
+        for (int i = 0;i < height; i++) {
+                for (int j = 0; j < width; j++){
+                        index = i * width;
+                        if (i % 2 == 0 && j % 2 == 0) {
+                                data[index] = blueMap[data[index]];
+                        }    
+                        else if (i % 2 == 0 && j % 2 == 1) {
+                                data[index] = gbMap[data[index]];
+                        }    
+                        else if (i % 2 == 1 && j % 2 == 0) {
+                                data[index] = grMap[data[index]];
+                        }    
+                        else {
+                                data[index] = redMap[data[index]];
+                        }
+                        index++;
+                }
+        }
+}
+
+void CPU_ISP::blackLevelCorrect(uint16_t *data, uint16_t offset, int width, int height)
+{
+        int len = width * height;
+        for(int i = 0; i < len; i++) {
+                if (data[i] < offset){
+                        data[i] = 0;
+                }
+                else {
+                        data[i] -= offset;
+                }
+        }
+}
+
+void CPU_ISP::readChannels(uint16_t *data, uint16_t *R, uint16_t *G, uint16_t *B,
+                       int width, int height)
+{
+        int index;
+        for (int i = 0; i < height; i++) {
+                index = i * width;
+                for (int j = 0; j < width; j++) {
+                        if (i % 2 == 0 && j % 2 == 0) {
+                                B[index] = data[index];
+                        }
+                        else if ((i % 2 == 0 && j % 2 == 1) || (i % 2 == 1 && j % 2 == 0)){
+                                G[index] = data[index];
+                        }
+                        else {
+                                R[index] = data[index];
+                        }
+                        index++;
+                }
+        }
+}
+
+void CPU_ISP::firstPixelInsert(uint16_t *src, uint16_t *dst, int width, int height)
+{
+        int index;
+        for (int i = 0; i < height; i++) {
+                index = i * width;
+                for (int j = 0; j < width; j++){
+                        if (i % 2 == 0 && j % 2 == 1) {
+                                if (j == (width - 1)) {
+                                        dst[index] = src[index - 1];
+                                }
+                                else {
+                                        dst[index] = (src[index - 1] +
+                                                src[index + 1]) >> 1;
+                                }
+                        }
+
+                        if (i % 2 == 1 && j % 2 == 0) {
+                                if(i == height - 1) {
+                                        dst[index] = src[index - width];
+                                }
+                                else {
+                                        dst[index] = (src[index - width]+
+                                                src[index + width]) >> 1;
+                                }
+                        }
+    
+                        if (i % 2 == 1 && j % 2 == 1) {
+                                if (j < width - 1 && i < height - 1) {
+                                        dst[index] = (src[index - width - 1] +
+                                                src[index - width + 1] +
+                                                src[index + width - 1] +
+                                                src[index + width + 1]) >> 2;
+                                }
+                                else if (i == height - 1 && j < width - 1) {
+                                        dst[index] = (src[index - width - 1] +
+                                                src[index - width + 1]) >> 1;
+                                }
+                                else if (i < height - 1 && j == width - 1) {
+                                        dst[index] = (src[index - width - 1] +
+                                                src[index + width - 1]) >> 1;
+                                }
+                                else {
+                                        dst[index] = src[index - width - 1];
+                                }          
+                        }
+                        index++;
+                }
+        }
+}
+
+void CPU_ISP::twoPixelInsert(uint16_t *src, uint16_t *dst, int width, int height)
+{
+        int index;
+        for (int i = 0; i < height; i++) {
+                index = i * width;
+                for (int j = 0; j < width; j++) {
+                        if (i == 0 && j == 0) {
+                                dst[index] = (src[index + width] +
+                                        src[index + 1]) >> 1;
+                        }
+                        else if (i == 0 && j > 0 && j % 2 == 0) {
+                                dst[index] = (src[index - 1] +
+                                        src[index + width] +
+                                        src[index + 1]) / 3;
+                        }
+                        else if (i > 0 && j == 0 && i % 2 == 0) {
+                                dst[index] = (src[index - width] +
+                                        src[index + 1] +
+                                        src[index + width]) / 3;
+                        }
+                        else if (i == (height - 1) && j < (width - 1) && j % 2 == 1) {
+                                dst[index] = (src[index - 1] +
+                                        src[index - width] +
+                                        src[index + 1]) / 3;
+                        }
+                        else if (i < (height - 1) && j == (width - 1) && i % 2 == 1) {
+                                dst[index] = (src[index - width] +
+                                        src[index - 1] +
+                                        src[index + width]) / 3;
+                        }
+                        else if (i == (height - 1) && j == (width - 1)) {
+                                dst[index] = (src[index - width] +
+                                        src[index - 1]) >> 1;
+                        }
+                        else if ((i % 2 == 0 && j % 2 == 0) || (i % 2 == 1 && j % 2 == 1)) {
+                                dst[index] = (src[index - 1] +
+                                        src[index + 1] +
+                                        src[index - width] +
+                                        src[index + width]) / 4;
+                        }
+                        index++;
+                }
+        }
+}
+
+void CPU_ISP::lastPixelInsert(uint16_t *src, uint16_t *dst, int width, int height)
+{
+        int index;
+        for (int i = 0; i < height; i++) {
+                index = i * width;
+                for (int j = 0; j < width; j++){
+                        if (i % 2 == 1 && j % 2 == 0) {
+                                if (j == 0) {
+                                        dst[index] = src[index + 1];
+                                }
+                                else {
+                                        dst[index] = (src[index - 1] +
+                                                src[index + 1]) >> 1;
+                                }
+                        }
+
+                        if (i % 2 == 0 && j % 2 == 1) {
+                                if(i == 0) {
+                                        dst[index] = src[index + width];
+                                }
+                                else {
+                                        dst[index] = (src[index - width]+
+                                                src[index + width]) >> 1;
+                                }
+                        }
+
+                        if (i % 2 == 0 && j % 2 == 0) {
+                                if (i > 0 && j > 0) {
+                                        dst[index] = (src[index - width - 1] +
+                                                src[index - width + 1] +
+                                                src[index + width - 1] +
+                                                src[index + width + 1]) >> 2;
+                                }
+                                else if (i == 0 && j > 0) {
+                                        dst[index] = (src[index + width - 1] +
+                                                src[index + width + 1]) >> 1;
+                                }
+                                else if (i > 0 && j == 0) {
+                                        dst[index] = (src[index - width + 1] +
+                                                src[index + width + 1]) >> 1;
+                                }
+                                else {
+                                        dst[index] = src[index + width + 1];
+                                }
+                        }
+                        index++;
+                }
+        }
+}
+
+void CPU_ISP::demosaic(uint16_t *data, uint16_t *R, uint16_t *G, uint16_t *B,
+                       int width, int height)
+{
+        firstPixelInsert(data, B, width, height);
+        twoPixelInsert(data, G, width, height);
+        lastPixelInsert(data, R, width, height);
+}
+
+void CPU_ISP::autoWhiteBalance(uint16_t *R, uint16_t *G, uint16_t *B, int width, int height)
+{
+        float aveB = 0, aveG = 0, aveR = 0;
+        float Kb, Kg, Kr;
+
+        for (int i = 0; i < width * height; i++) {
+                aveB += 1.0 * B[i];
+                aveG += 1.0 * G[i];
+                aveR += 1.0 * R[i];
+        }
+
+        aveB *= (1.0 / (width * height));
+        aveG *= (1.0 / (width * height));
+        aveR *= (1.0 / (width * height));
+
+        Kr = (aveB + aveG + aveR) / aveR * (1.0 / 3.0);
+        Kg = (aveB + aveG + aveR) / aveG * (1.0 / 3.0);
+        Kb = (aveB + aveG + aveR) / aveB * (1.0 / 3.0);
+
+        for (int i = 0; i < width * height; i++) {
+                B[i] = B[i] * Kb;
+                G[i] = G[i] * Kg;
+                R[i] = R[i] * Kr;
+
+                if (R[i] > 1023) R[i] = 1023;
+                if (G[i] > 1023) G[i] = 1023;
+                if (R[i] > 1023) B[i] = 1023;
+        }
+}
+
+void CPU_ISP::gammaCorrect(uint16_t *R, uint16_t *G, uint16_t *B, float val, int width, int height)
+{
+        float nor = 1.0 / 1023.0;
+        float gamma = 1.0 / val;
+        for (int i = 0; i < width * height; i++) {
+                R[i] = pow(R[i] * nor, gamma) * 1023;
+                G[i] = pow(G[i] * nor, gamma) * 1023;
+                B[i] = pow(B[i] * nor, gamma) * 1023;
+
+                if (R[i] > 1023) R[i] = 1023;
+                if (G[i] > 1023) G[i] = 1023;
+                if (B[i] > 1023) B[i] = 1023;
+        }
+}
+
+void CPU_ISP::compress_10bit_to_8bit (uint16_t *src, uint8_t *dst, int width, int height)
+{
+        for (int i = 0; i < width * height; i++) {
+                dst[i] = src[i] >> 2 & 0xff;
+        }
+}
+
+float CPU_ISP::distance(int x, int y, int i, int j)
+{
+    return float(sqrt(pow(x - i, 2) + pow(y - j, 2)));
+}
+
+double CPU_ISP::gaussian(float x, double sigma)
+{
+    return exp(-(pow(x, 2)) / (2 * pow(sigma, 2))) / (2 * 3.1415926 * pow(sigma, 2));
+}
+
+void CPU_ISP::bilateralFilter(uint16_t *R, uint16_t *G, uint16_t *B,
+                          int diameter, double sigmaI,
+                          double sigmaS, int width, int height)
+{
+        for (int i = 2; i < height - 2; i++) {
+                for (int j = 2; j < width - 2; j++) {
+                        double iFiltered = 0;
+                        double wp = 0;
+                        int neighbor_x = 0;
+                        int neighbor_y = 0;
+                        int half = diameter / 2;
+
+                        for (int k = 0; k < diameter; k++) {
+                                for (int l = 0; l < diameter; l++) {
+                                        neighbor_x = i - (half - k);
+                                        neighbor_y = j - (half - l);
+                                        double gi = gaussian(R[neighbor_x * width + neighbor_y] - R[i * width +j], sigmaI);
+                                        double gs = gaussian(distance(i, j, neighbor_x, neighbor_y), sigmaS);
+                                        double w = gi * gs;
+                                        iFiltered = iFiltered + R[neighbor_x * width + neighbor_y] * w;
+                                        wp = wp + w;
+                                }    
+                        }
+
+                        iFiltered = iFiltered / wp;
+                        R[i * width + j] = iFiltered;
+                }
+        }
+
+        for (int i = 2; i < height - 2; i++) {
+                for (int j = 2; j < width - 2; j++) {
+                        double iFiltered = 0;
+                        double wp = 0;
+                        int neighbor_x = 0;
+                        int neighbor_y = 0;
+                        int half = diameter / 2;
+
+                        for (int k = 0; k < diameter; k++) {
+                                for (int l = 0; l < diameter; l++) {
+                                        neighbor_x = i - (half - k);
+                                        neighbor_y = j - (half - l);
+                                        double gi = gaussian(G[neighbor_x * width + neighbor_y] - G[i * width +j], sigmaI);
+                                        double gs = gaussian(distance(i, j, neighbor_x, neighbor_y), sigmaS);
+                                        double w = gi * gs;
+                                        iFiltered = iFiltered + G[neighbor_x * width + neighbor_y] * w;
+                                        wp = wp + w;
+                                }    
+                        }
+
+                        iFiltered = iFiltered / wp;
+                        G[i * width + j] = iFiltered;
+                }
+        }
+
+        for (int i = 2; i < height - 2; i++) {
+                for (int j = 2; j < width - 2; j++) {
+                        double iFiltered = 0;
+                        double wp = 0;
+                        int neighbor_x = 0;
+                        int neighbor_y = 0;
+                        int half = diameter / 2;
+
+                        for (int k = 0; k < diameter; k++) {
+                                for (int l = 0; l < diameter; l++) {
+                                        neighbor_x = i - (half - k);
+                                        neighbor_y = j - (half - l);
+                                        double gi = gaussian(B[neighbor_x * width + neighbor_y] - B[i * width +j], sigmaI);
+                                        double gs = gaussian(distance(i, j, neighbor_x, neighbor_y), sigmaS);
+                                        double w = gi * gs;
+                                        iFiltered = iFiltered + B[neighbor_x * width + neighbor_y] * w;
+                                        wp = wp + w;
+                                }    
+                        }
+
+                        iFiltered = iFiltered / wp;
+                        B[i * width + j] = iFiltered;
+                }
+        }
+}
+
+void CPU_ISP::noiseReduction(uint16_t *R, uint16_t *G, uint16_t *B, int width, int height)
+{
+        bilateralFilter(R, G, B, 5, 24.0, 32.0, width, height);
+}
+
+void CPU_ISP::processing(FrameBuffer *srcBuffer, FrameBuffer *dstBuffer, int width, int height)
+{
+        uint8_t *rgb_buf;
+        uint16_t *rawData;
+        uint16_t *rgbData = new std::uint16_t[width * height * 3];
+
+        uint16_t *rData = rgbData;
+        uint16_t *gData = rData + width * height;
+        uint16_t *bData = gData + width * height;
+        memset(rgbData, 0x0, width * height * 3);
+
+        const FrameBuffer::Plane &plane =  srcBuffer->planes()[0]; 
+        rawData = (uint16_t *)mmap(NULL, plane.length, PROT_READ|PROT_WRITE, MAP_SHARED, plane.fd.fd(), 0);
+        LOG(ISP, Debug) << plane.length << " " << plane.fd.fd();
+        if (rawData == MAP_FAILED) {
+            LOG(ISP, Error) << "Read raw data failed";
+            ispCompleted.emit(dstBuffer);
+        }
+
+        blackLevelCorrect(rawData, 16, width, height);
+        readChannels(rawData, rData, gData, bData, width, height);
+        demosaic(rawData, rData, gData, bData, width, height);
+        autoWhiteBalance(rData, gData, bData, width, height);
+        autoContrast(rData, 0.01, 0.01, width, height);
+        autoContrast(gData, 0.01, 0.01, width, height);
+        autoContrast(bData, 0.01, 0.01, width, height);
+        gammaCorrect(rData, gData, bData, 2.2, width, height);
+        //bilateralFilter(rData, gData, bData, 5, 24.0, 32.0, width, height);
+    
+        const FrameBuffer::Plane &rgbPlane = dstBuffer->planes()[0];
+        rgb_buf = (uint8_t *)mmap(NULL, rgbPlane.length, PROT_READ|PROT_WRITE, MAP_SHARED, rgbPlane.fd.fd(), 0);
+        if (rgb_buf == MAP_FAILED) {
+                LOG(ISP, Error) << "Read rgb data failed";
+                ispCompleted.emit(dstBuffer);
+        }
+
+        uint8_t *rData_8 = rgb_buf;
+        uint8_t *gData_8 = rData_8 + width * height;
+        uint8_t *bData_8 = gData_8 + width * height;
+
+        compress_10bit_to_8bit(rData, rData_8, width, height);
+        compress_10bit_to_8bit(gData, gData_8, width, height);
+        compress_10bit_to_8bit(bData, bData_8, width, height);
+
+        dstBuffer->metadata_.status = srcBuffer->metadata().status;
+        dstBuffer->metadata_.sequence = srcBuffer->metadata().sequence;
+        dstBuffer->metadata_.timestamp = srcBuffer->metadata().timestamp;
+
+        dstBuffer->metadata_.planes.clear();
+        dstBuffer->metadata_.planes.push_back({rgbPlane.length});
+
+        delete[] rgbData;
+
+        ispCompleted.emit(dstBuffer);
+}
+
+int CPU_ISP::exportBuffers(std::vector<std::unique_ptr<FrameBuffer>> *buffers,
+                       unsigned int count, int width, int height)
+{
+        int bufferByte = width * height * 3;
+
+        for (unsigned int i = 0; i < count; i++) {
+                std::string name = "frame-" + std::to_string(i);
+
+                const int isp_fd = memfd_create(name.c_str(), 0);
+                int ret = ftruncate(isp_fd, bufferByte);
+                if (ret < 0) {
+                        LOG(ISP, Error) << "Failed to resize memfd" << strerror(-ret);
+                        return ret;
+                }
+
+                FrameBuffer::Plane rgbPlane;
+                rgbPlane.fd = FileDescriptor(std::move(isp_fd));
+                rgbPlane.length = bufferByte;
+
+                std::vector<FrameBuffer::Plane> planes{rgbPlane};
+                buffers->emplace_back(std::make_unique<FrameBuffer>(std::move(planes)));
+        }
+
+        return count;
+}
+
+void CPU_ISP::startThreadISP()
+{
+        moveToThread(&thread_);
+        thread_.start();
+}
+
+void CPU_ISP::stopThreadISP()
+{
+        thread_.exit();
+        thread_.wait();
+}
+
+} /* namespace libcamera */
\ No newline at end of file
diff --git a/src/libcamera/swisp/isp.h b/src/libcamera/swisp/isp.h
new file mode 100644
index 00000000..4099e2c6
--- /dev/null
+++ b/src/libcamera/swisp/isp.h
@@ -0,0 +1,92 @@ 
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2021, Siyuan Fan <siyuan.fan@foxmail.com>
+ *
+ * isp.h - The software ISP class
+ */
+#ifndef __LIBCAMERA_SWISP_ISP_H__
+#define __LIBCAMERA_SWISP_ISP_H__
+
+#include <libcamera/framebuffer.h>
+
+#include "libcamera/base/object.h"
+#include "libcamera/base/signal.h"
+#include "libcamera/base/thread.h"
+
+namespace libcamera{
+
+using std::uint16_t;
+using std::uint8_t;
+
+class ISP : public Object
+{
+public:
+        ISP() {}
+
+        virtual void blackLevelCorrect(uint16_t *data, uint16_t offset, int width, int height) = 0;
+
+        virtual void demosaic(uint16_t *data, uint16_t *R, uint16_t *G, uint16_t *B,
+                              int width, int height) = 0;
+
+        virtual void autoWhiteBalance(uint16_t *R, uint16_t *G, uint16_t *B, int width, int height) = 0;
+
+        virtual void gammaCorrect(uint16_t *R, uint16_t *G, uint16_t *B, float val, int width, int height)  = 0;
+
+        virtual void noiseReduction(uint16_t *R, uint16_t *G, uint16_t *B, int width, int height) = 0;
+
+        virtual ~ISP() {}
+};
+
+class CPU_ISP : public ISP
+{
+public:
+        void autoContrast(uint16_t *data, float lowCut, float highCut, int width, int height);
+
+        void blackLevelCorrect(uint16_t *data, uint16_t offset, int width, int height) override;
+
+        void readChannels(uint16_t *data, uint16_t *R, uint16_t *G, uint16_t *B,
+                          int width, int height);
+
+        void firstPixelInsert(uint16_t *src, uint16_t *dst, int width, int height);
+
+        void twoPixelInsert(uint16_t *src, uint16_t *dst, int width, int height);
+
+        void lastPixelInsert(uint16_t *src, uint16_t *dst, int width, int height);
+
+        void demosaic(uint16_t *data, uint16_t *R, uint16_t *G, uint16_t *B,
+                      int width, int height) override;
+
+        void autoWhiteBalance(uint16_t *R, uint16_t *G, uint16_t *B, int width, int height) override;
+
+        void gammaCorrect(uint16_t *R, uint16_t *G, uint16_t *B, float val, int width, int height) override;
+
+        float distance(int x, int y, int i, int j);
+
+        double gaussian(float x, double sigma);
+
+        void bilateralFilter(uint16_t *R, uint16_t *G, uint16_t *B,
+                             int diameter, double sigmaI, double sigmaS,
+                             int width, int height);
+
+        void noiseReduction(uint16_t *R, uint16_t *G, uint16_t *B, int width, int height) override;
+
+        void compress_10bit_to_8bit(uint16_t *src, uint8_t *dst, int width, int height);    
+
+        void processing(FrameBuffer *srcBuffer, FrameBuffer *dstBuffer, int width, int height);
+
+        int exportBuffers(std::vector<std::unique_ptr<FrameBuffer>> *buffers,
+                           unsigned int count, int width, int height);
+
+        void startThreadISP();
+
+        void stopThreadISP();
+
+        Signal<FrameBuffer *> ispCompleted;
+
+private:
+        Thread thread_;
+};
+
+} /* namespace libcamera */
+
+#endif /* __LIBCAMERA_SWISP_ISP_H__ */
\ No newline at end of file