hwc2: White point compensation for Adaptive color mode
am: 6239b02b0c

Change-Id: If2aa90ccddebc202c98cc2299bd86dc6517ff16e
diff --git a/include/display_properties.h b/include/display_properties.h
index 3944dc6..06f4e64 100644
--- a/include/display_properties.h
+++ b/include/display_properties.h
@@ -94,6 +94,7 @@
 #define ENABLE_DEFAULT_COLOR_MODE            DISPLAY_PROP("enable_default_color_mode")
 #define DISABLE_HDR                          DISPLAY_PROP("disable_hdr")
 #define DATASPACE_SATURATION_MATRIX_PROP     DISPLAY_PROP("dataspace_saturation_matrix")
+#define WHITE_POINT_COMPENSATED_COEFFICIENT_PROP     DISPLAY_PROP("white_point_compensated_coefficient")
 
 #define HDR_CONFIG_PROP                      RO_DISPLAY_PROP("hdr.config")
 #define QDCM_PCC_TRANS_PROP                  DISPLAY_PROP("qdcm.pcc_for_trans")
diff --git a/libqservice/IQService.h b/libqservice/IQService.h
index b201256..e9af5a1 100644
--- a/libqservice/IQService.h
+++ b/libqservice/IQService.h
@@ -79,6 +79,7 @@
         GET_COMPOSER_STATUS = 37, // Get composer init status-true if primary display init is done
         SET_COLOR_MODE_WITH_RENDER_INTENT = 38,
         SET_IDLE_PC = 39, // Enable/disable Idle power collapse
+        SET_WHITE_POINT_COMPENSATION = 399, // Enable/disable white point compensation
         COMMAND_LIST_END = 400,
     };
 
diff --git a/sdm/libs/hwc2/hwc_display.cpp b/sdm/libs/hwc2/hwc_display.cpp
index 4d29263..03e4cc3 100644
--- a/sdm/libs/hwc2/hwc_display.cpp
+++ b/sdm/libs/hwc2/hwc_display.cpp
@@ -54,6 +54,8 @@
 
 HWC2::Error HWCColorMode::Init() {
   PopulateColorModes();
+  if (GetWhitePointCompensatedCoefficients())
+    white_point_compensated_ = PaserWhitePointCompensatedData();
   return ApplyDefaultColorMode();
 }
 
@@ -121,11 +123,13 @@
     DLOGE("failed for mode = %d intent = %d name = %s", mode, intent, mode_string.c_str());
     return HWC2::Error::Unsupported;
   }
-  // The mode does not have the PCC configured, restore the transform
-  RestoreColorTransform();
 
   current_color_mode_ = mode;
   current_render_intent_ = intent;
+
+  // The mode does not have the PCC configured, restore the transform
+  RestoreColorTransform();
+
   DLOGV_IF(kTagClient, "Successfully applied mode = %d intent = %d name = %s", mode, intent,
            mode_string.c_str());
   return HWC2::Error::None;
@@ -141,8 +145,23 @@
   return HWC2::Error::None;
 }
 
+HWC2::Error HWCColorMode::SetWhitePointCompensation(bool enabled) {
+  white_point_compensated_ = enabled;
+  if (white_point_compensated_) {
+    PaserWhitePointCompensatedData();
+    ApplyWhitePointCompensationToMatrix(white_point_compensated_color_matrix_, color_matrix_);
+  }
+
+  RestoreColorTransform();
+
+  DLOGI("Set White Point Compensation: %d", enabled);
+  return HWC2::Error::None;
+}
+
 HWC2::Error HWCColorMode::RestoreColorTransform() {
-  DisplayError error = display_intf_->SetColorTransform(kColorTransformMatrixCount, color_matrix_);
+
+  DisplayError error = display_intf_->SetColorTransform(kColorTransformMatrixCount,
+                                                        GetTransferMatrix());
   if (error != kErrorNone) {
     DLOGE("Failed to set Color Transform");
     return HWC2::Error::BadParameter;
@@ -151,19 +170,106 @@
   return HWC2::Error::None;
 }
 
+bool HWCColorMode::PaserWhitePointCompensatedData() {
+    static constexpr char kWhitePointCalibrationDataPath[] = "/persist/display/calibrated_rgb";
+    FILE *fp = fopen(kWhitePointCalibrationDataPath, "r");
+    int compensated_red = 0;
+    int compensated_green = 0;
+    int compensated_blue = 0;
+
+    if (!fp) {
+      compensated_red_ratio_ = 1.0;
+      compensated_green_ratio_ = 1.0;
+      compensated_blue_ratio_ = 1.0;
+      return false;
+    }
+
+    fscanf(fp, "%d %d %d", &compensated_red, &compensated_green, &compensated_blue);
+
+    fclose(fp);
+
+    // r = r_coeffient2 * R^2 + r_coeffient1 * R + r_coeffient0
+    // g = g_coeffient2 * G^2 + g_coeffient1 * G + g_coeffient0
+    // b = b_coeffient2 * B^2 + b_coeffient1 * B + b_coeffient0
+    // r_ratio = r/kCompensatedMaxRGB
+    // g_ratio = g/kCompensatedMaxRGB
+    // b_ratio = b/kCompensatedMaxRGB
+    auto rgb_ratio = [=](int rgb, float c2, float c1, float c0) {
+                              return ((c2 * rgb * rgb + c1 * rgb + c0) / kCompensatedMaxRGB);};
+
+    compensated_red_ratio_ = rgb_ratio(CheckCompensatedRGB(compensated_red), white_point_compensated_Coefficients_[0],
+                                           white_point_compensated_Coefficients_[1], white_point_compensated_Coefficients_[2]);
+    compensated_green_ratio_ = rgb_ratio(CheckCompensatedRGB(compensated_green), white_point_compensated_Coefficients_[3],
+                                           white_point_compensated_Coefficients_[4], white_point_compensated_Coefficients_[5]);
+    compensated_blue_ratio_ = rgb_ratio(CheckCompensatedRGB(compensated_blue), white_point_compensated_Coefficients_[6],
+                                           white_point_compensated_Coefficients_[7], white_point_compensated_Coefficients_[8]);
+    return true;
+}
+
+bool HWCColorMode::GetWhitePointCompensatedCoefficients() {
+  char value[kPropertyMax] = {};
+  if (Debug::Get()->GetProperty(WHITE_POINT_COMPENSATED_COEFFICIENT_PROP, value) != kErrorNone) {
+    DLOGW("Undefined Compensated Coefficients");
+    return false;
+  }
+
+  std::string value_string(value);
+  std::size_t start = 0, end = 0;
+  int index = 0;
+  // We need 9 coneffients, 3 for red, 3 for green, 3 for blue.
+  float CompensatedCoefficients[kCompensatedCoefficientElements] = { 1.0, 1.0, 1.0, \
+                                                                     1.0, 1.0, 1.0, \
+                                                                     1.0, 1.0, 1.0 };
+
+  // The property must have kCompensatedCoefficientElements delimited by commas
+  while ((end = value_string.find(",", start)) != std::string::npos) {
+    CompensatedCoefficients[index] = std::stof(value_string.substr(start, end - start));
+    start = end + 1;
+    index++;
+    if (index > kCompensatedCoefficientElements - 1) {
+      DLOGW("Over Compensated Coefficients defined");
+      return false;
+    }
+  }
+
+  if (index < kCompensatedCoefficientElements - 1) {
+    DLOGW("Less Compensated Coefficients defined");
+    return false;
+  }
+
+  CompensatedCoefficients[index] = std::stof(value_string.substr(start, end - start));
+
+  for (int32_t i = 0; i < kCompensatedCoefficientElements; i++) {
+    white_point_compensated_Coefficients_[i] = CompensatedCoefficients[i];
+    DLOGD("CompensatedCoefficients[%d]=%f",i, CompensatedCoefficients[i]);
+  }
+  return true;
+}
+
 HWC2::Error HWCColorMode::SetColorTransform(const float *matrix,
                                             android_color_transform_t /*hint*/) {
   DTRACE_SCOPED();
   auto status = HWC2::Error::None;
   double color_matrix[kColorTransformMatrixCount] = {0};
+  double white_point_compensated_color_matrix[kColorTransformMatrixCount] = {0};
   CopyColorTransformMatrix(matrix, color_matrix);
 
-  DisplayError error = display_intf_->SetColorTransform(kColorTransformMatrixCount, color_matrix);
+  if (white_point_compensated_)
+    ApplyWhitePointCompensationToMatrix(white_point_compensated_color_matrix, color_matrix);
+
+  DisplayError error = display_intf_->SetColorTransform(kColorTransformMatrixCount,
+                                                        NeedWhitePointCompensated() ?
+                                                        white_point_compensated_color_matrix :
+                                                        color_matrix);
   if (error != kErrorNone) {
     DLOGE("Failed to set Color Transform Matrix");
     status = HWC2::Error::Unsupported;
   }
   CopyColorTransformMatrix(matrix, color_matrix_);
+
+  if (white_point_compensated_)
+    CopyColorTransformMatrix(white_point_compensated_color_matrix, white_point_compensated_color_matrix_);
+
   return status;
 }
 
@@ -312,13 +418,23 @@
   }
   *os << "current mode: " << static_cast<uint32_t>(current_color_mode_) << std::endl;
   *os << "current render_intent: " << static_cast<uint32_t>(current_render_intent_) << std::endl;
+  *os << "Need white point compensated: " <<  NeedWhitePointCompensated() << ",RGB:["
+      << std::fixed << std::setprecision(4)
+      << compensated_red_ratio_ << ","
+      << compensated_green_ratio_ << ","
+      << compensated_blue_ratio_ << "] "<< std::endl;
+
   *os << "current transform: ";
+  double color_matrix[kColorTransformMatrixCount] = {0};
+
+  CopyColorTransformMatrix(GetTransferMatrix(), color_matrix);
+
   for (uint32_t i = 0; i < kColorTransformMatrixCount; i++) {
     if (i % 4 == 0) {
      *os << std::endl;
     }
-    *os << std::fixed << std::setprecision(2) << std::setw(6) << std::setfill(' ')
-        << color_matrix_[i] << " ";
+    *os << std::fixed << std::setprecision(4) << std::setw(8) << std::setfill(' ')
+        << color_matrix[i] << " ";
   }
   *os << std::endl;
 }
diff --git a/sdm/libs/hwc2/hwc_display.h b/sdm/libs/hwc2/hwc_display.h
index 4940678..ff07217 100644
--- a/sdm/libs/hwc2/hwc_display.h
+++ b/sdm/libs/hwc2/hwc_display.h
@@ -73,6 +73,7 @@
   HWC2::Error RestoreColorTransform();
   PrimariesTransfer  GetWorkingColorSpace();
   ColorMode GetCurrentColorMode() { return current_color_mode_; }
+  HWC2::Error SetWhitePointCompensation(bool enabled);
 
  private:
   static const uint32_t kColorTransformMatrixCount = 16;
@@ -97,6 +98,65 @@
                                                        0.0, 1.0, 0.0, 0.0, \
                                                        0.0, 0.0, 1.0, 0.0, \
                                                        0.0, 0.0, 0.0, 1.0 };
+  /*
+   * Transform matrix is 4 x 4
+   *  |r.r   r.g   r.b  0|
+   *  |g.r   g.g   g.b  0|
+   *  |b.r   b.g   b.b  0|
+   *  |T.r   T.g   T.b  1|
+   *   R_out = R_in * r.r + G_in * g.r + B_in * b.r + Tr
+   *   G_out = R_in * r.g + G_in * g.g + B_in * b.g + Tg
+   *   B_out = R_in * r.b + G_in * g.b + B_in * b.b + Tb
+   *
+   * Cr, Cg, Cb for white point compensation
+   *  |r.r*Cr   r.g*Cg   r.b*Cb  0|
+   *  |g.r*Cr   g.g*Cg   g.b*Cb  0|
+   *  |b.r*Cr   b.g*Cg   b.b*Cb  0|
+   *  |T.r*Cr   T.g*Cg   T.b*Cb  1|
+   *   R_out = R_in * r.r * Cr + G_in * g.r * Cr + B_in * b.r * Cr + Tr * Cr
+   *   G_out = R_in * r.g * Cg + G_in * g.g * Cg + B_in * b.g * Cg + Tg * Cg
+   *   B_out = R_in * r.b * Cb + G_in * g.b * Cb + B_in * b.b * Cb + Tb * Cb
+   */
+  static constexpr int kCompensatedMaxRGB = 255;
+  static constexpr int kCompensatedMinRGB = 230;
+  static constexpr int kCompensatedCoefficientElements = 9;
+  double white_point_compensated_color_matrix_[kColorTransformMatrixCount] = {
+                                                       1.0, 0.0, 0.0, 0.0, \
+                                                       0.0, 1.0, 0.0, 0.0, \
+                                                       0.0, 0.0, 1.0, 0.0, \
+                                                       0.0, 0.0, 0.0, 1.0 };
+  bool PaserWhitePointCompensatedData();
+  float white_point_compensated_Coefficients_[kCompensatedCoefficientElements] = {
+                                                               1.0, 1.0, 1.0, \
+                                                               1.0, 1.0, 1.0, \
+                                                               1.0, 1.0, 1.0 };
+
+  bool GetWhitePointCompensatedCoefficients();
+  bool NeedWhitePointCompensated() { return (current_render_intent_ == RenderIntent::ENHANCE) &&
+                                                                       white_point_compensated_; }
+  double * GetTransferMatrix() { return NeedWhitePointCompensated() ? white_point_compensated_color_matrix_
+                                                                      : color_matrix_;}
+  bool   white_point_compensated_ = false;
+  double compensated_red_ratio_ = 1.0;
+  double compensated_green_ratio_ = 1.0;
+  double compensated_blue_ratio_ = 1.0;
+  inline static constexpr int CheckCompensatedRGB (int value) {
+      return ((value < kCompensatedMinRGB) ? kCompensatedMinRGB :
+              (value > kCompensatedMaxRGB) ? kCompensatedMaxRGB : value);
+  }
+
+  void ApplyWhitePointCompensationToMatrix(double *out, double *in) {
+    for (uint32_t i = 0; i < kColorTransformMatrixCount; i++) {
+       if ((i % 4) == 0)
+         out[i] = compensated_red_ratio_ * in[i];
+       else if ((i % 4) == 1)
+         out[i] = compensated_green_ratio_ * in[i];
+       else if ((i % 4) == 2)
+         out[i] = compensated_blue_ratio_ * in[i];
+       else if ((i % 4) == 3)
+         out[i] = in[i];
+    }
+  }
 };
 
 class HWCDisplay : public DisplayEventHandler {
@@ -202,6 +262,7 @@
   virtual HWC2::Error SetClientTarget(buffer_handle_t target, int32_t acquire_fence,
                                       int32_t dataspace, hwc_region_t damage);
   virtual HWC2::Error SetColorMode(ColorMode mode) { return HWC2::Error::Unsupported; }
+  virtual HWC2::Error SetWhitePointCompensation(bool enabled) { return HWC2::Error::Unsupported; }
   virtual HWC2::Error SetColorModeWithRenderIntent(ColorMode mode, RenderIntent intent) {
     return HWC2::Error::Unsupported;
   }
diff --git a/sdm/libs/hwc2/hwc_display_primary.cpp b/sdm/libs/hwc2/hwc_display_primary.cpp
index f5970cd..b1eeecd 100644
--- a/sdm/libs/hwc2/hwc_display_primary.cpp
+++ b/sdm/libs/hwc2/hwc_display_primary.cpp
@@ -339,6 +339,18 @@
   return SetColorModeWithRenderIntent(mode, RenderIntent::COLORIMETRIC);
 }
 
+HWC2::Error HWCDisplayPrimary::SetWhitePointCompensation(bool enabled) {
+  auto status = color_mode_->SetWhitePointCompensation(enabled);
+  if (status != HWC2::Error::None) {
+    DLOGE("failed for SetWhitePointCompensation to %d", enabled);
+    return status;
+  }
+
+  callbacks_->Refresh(HWC_DISPLAY_PRIMARY);
+
+  return status;
+}
+
 HWC2::Error HWCDisplayPrimary::SetColorModeWithRenderIntent(ColorMode mode, RenderIntent intent) {
   auto status = color_mode_->SetColorModeWithRenderIntent(mode, intent);
   if (status != HWC2::Error::None) {
diff --git a/sdm/libs/hwc2/hwc_display_primary.h b/sdm/libs/hwc2/hwc_display_primary.h
index 40f0f48..d6dcc53 100644
--- a/sdm/libs/hwc2/hwc_display_primary.h
+++ b/sdm/libs/hwc2/hwc_display_primary.h
@@ -58,6 +58,7 @@
   virtual HWC2::Error Present(int32_t *out_retire_fence);
   virtual HWC2::Error GetColorModes(uint32_t *out_num_modes, ColorMode *out_modes);
   virtual HWC2::Error SetColorMode(ColorMode mode);
+  virtual HWC2::Error SetWhitePointCompensation(bool enabled);
   virtual HWC2::Error GetRenderIntents(ColorMode mode, uint32_t *out_num_intents,
                                        RenderIntent *out_intents);
   virtual HWC2::Error SetColorModeWithRenderIntent(ColorMode mode, RenderIntent intent);
diff --git a/sdm/libs/hwc2/hwc_session.cpp b/sdm/libs/hwc2/hwc_session.cpp
index e440464..2691a42 100644
--- a/sdm/libs/hwc2/hwc_session.cpp
+++ b/sdm/libs/hwc2/hwc_session.cpp
@@ -1355,6 +1355,14 @@
       status = SetIdlePC(input_parcel);
       break;
 
+    case qService::IQService::SET_WHITE_POINT_COMPENSATION:
+      if (!input_parcel) {
+        DLOGE("QService command = %d: input_parcel needed.", command);
+        break;
+      }
+      status = SetWhitePointCompensation(input_parcel);
+      break;
+
     default:
       DLOGW("QService command = %d is not supported.", command);
       break;
@@ -1551,6 +1559,18 @@
   return 0;
 }
 
+android::status_t HWCSession::SetWhitePointCompensation(const android::Parcel *input_parcel) {
+  auto display = static_cast<hwc2_display_t >(input_parcel->readInt32());
+  auto enabled = static_cast<bool >(input_parcel->readInt32());
+  auto device = static_cast<hwc2_device_t *>(this);
+
+  auto err = CallDisplayFunction(device, display, &HWCDisplay::SetWhitePointCompensation, enabled);
+  if (err != HWC2_ERROR_NONE)
+    return -EINVAL;
+
+  return 0;
+}
+
 android::status_t HWCSession::SetColorModeWithRenderIntentOverride(
     const android::Parcel *input_parcel) {
   auto display = static_cast<hwc2_display_t>(input_parcel->readInt32());
diff --git a/sdm/libs/hwc2/hwc_session.h b/sdm/libs/hwc2/hwc_session.h
index 5b94b57..8eb7016 100644
--- a/sdm/libs/hwc2/hwc_session.h
+++ b/sdm/libs/hwc2/hwc_session.h
@@ -271,6 +271,7 @@
   android::status_t SetMixerResolution(const android::Parcel *input_parcel);
   android::status_t SetColorModeOverride(const android::Parcel *input_parcel);
   android::status_t SetColorModeWithRenderIntentOverride(const android::Parcel *input_parcel);
+  android::status_t SetWhitePointCompensation(const android::Parcel *input_parcel);
 
   android::status_t SetColorModeById(const android::Parcel *input_parcel);
   android::status_t getComposerStatus();