427 lines
14 KiB
C
427 lines
14 KiB
C
/**
|
|
* @file sdrv_asw_lut_hvk.c
|
|
* @brief sdrv asw driver source.
|
|
*
|
|
* @copyright Copyright (c) 2022 Semidrive Semiconductor.
|
|
* All rights reserved.
|
|
*/
|
|
|
|
#include <armv7-r/cache.h>
|
|
#include <asw/asw_log.h>
|
|
#include <asw/asw_lut_hvk.h>
|
|
#include <debug.h>
|
|
#include <math.h>
|
|
#include <reg.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#define LUT_SIZE_MAX 1024
|
|
float TEMPAA[LUT_SIZE_MAX] = {0};
|
|
float TEMPAB[LUT_SIZE_MAX] = {0};
|
|
|
|
static int m_parameter_calc_alpha(int lut_size, float *alpha)
|
|
{
|
|
for (int i = 0; i < lut_size; i++) {
|
|
if ((i < 2) || (i >= (lut_size - 2)))
|
|
alpha[i] = 1.0;
|
|
else if (i == 2)
|
|
alpha[i] = 0.25;
|
|
else
|
|
alpha[i] = 1.0 / (4.0 - alpha[i - 1]);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int m_parameter_calc_1d(float *lut_ld, float *m_param_ld, float *alpha,
|
|
int lut_size, bool is_rt1)
|
|
{
|
|
float *delta = TEMPAA;
|
|
float *beta = TEMPAB;
|
|
float c_one_third = 1.0 / 3.0;
|
|
float c_one_sixth = 1.0 / 6.0;
|
|
|
|
for (int i = 0; i < lut_size; i++) {
|
|
if ((i == 0) || (i == (lut_size - 1))) {
|
|
delta[i] = 0.0;
|
|
} else {
|
|
if (!is_rt1) {
|
|
delta[i] =
|
|
6.0 * (lut_ld[i + 1] + lut_ld[i - 1] - 2.0 * lut_ld[i]);
|
|
} else {
|
|
delta[i] = lut_ld[i + 1] + lut_ld[i - 1];
|
|
delta[i] = fma(-2.0, lut_ld[i], delta[i]);
|
|
delta[i] = 6.0 * delta[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < lut_size; i++) {
|
|
if (i == 0) {
|
|
beta[i] = delta[1] * c_one_third;
|
|
} else if (i == 1) {
|
|
beta[i] = delta[1] * c_one_sixth;
|
|
} else if (i == (lut_size - 2)) {
|
|
beta[i] = delta[lut_size - 2] * c_one_sixth;
|
|
} else if (i == (lut_size - 1)) {
|
|
beta[i] = delta[lut_size - 2] * c_one_third;
|
|
} else {
|
|
if (!is_rt1) {
|
|
beta[i] = (delta[i] - beta[i - 1]) * alpha[i];
|
|
} else {
|
|
beta[i] = delta[i] * alpha[i];
|
|
beta[i] = fma(-beta[i - 1], alpha[i], beta[i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
for (int i = lut_size - 2; i >= 0; i--) {
|
|
if (i == 0) {
|
|
m_param_ld[i] = beta[i] - m_param_ld[i + 2];
|
|
} else if (i == 1) {
|
|
m_param_ld[i] = beta[i];
|
|
} else if (i == (lut_size - 2)) {
|
|
m_param_ld[i] = beta[i];
|
|
} else {
|
|
if (!is_rt1) {
|
|
m_param_ld[i] = beta[i] - m_param_ld[i + 1] * alpha[i];
|
|
} else {
|
|
m_param_ld[i] = fma(-m_param_ld[i + 1], alpha[i], beta[i]);
|
|
}
|
|
}
|
|
}
|
|
m_param_ld[lut_size - 1] = beta[lut_size - 1] - m_param_ld[lut_size - 3];
|
|
|
|
return 0;
|
|
}
|
|
static float hvk_pos_calc(float x, float y, struct hvkt_param *hvkt,
|
|
bool calc_x, int c_sel)
|
|
{
|
|
float pa, pb, pc, pd, pe, pf;
|
|
float x_mag, y_mag;
|
|
|
|
struct hvkt_fabc *fabc = hvkt->fabc;
|
|
|
|
pa = fabc->fc0[c_sel] * x - fabc->fa0[c_sel];
|
|
pb = fabc->fc1[c_sel] * x - fabc->fa1[c_sel];
|
|
pc = fabc->fa2[c_sel] - fabc->fc2[c_sel] * x;
|
|
|
|
pd = fabc->fc0[c_sel] * y - fabc->fb0[c_sel];
|
|
pe = fabc->fc1[c_sel] * y - fabc->fb1[c_sel];
|
|
pf = fabc->fb2[c_sel] - fabc->fc2[c_sel] * y;
|
|
|
|
if (calc_x) {
|
|
if (hvkt->fw_map)
|
|
x_mag = (fabc->fa0[c_sel] * x + fabc->fa1[c_sel] * y +
|
|
fabc->fa2[c_sel]) /
|
|
(fabc->fc0[c_sel] * x + fabc->fc1[c_sel] * y +
|
|
fabc->fc2[c_sel]);
|
|
else
|
|
x_mag = (pc * pe - pb * pf) / (pa * pe - pb * pd);
|
|
|
|
return x_mag * (float)(hvkt->hsize_in - 1) / (float)(hvkt->hsize - 1);
|
|
} else {
|
|
if (hvkt->fw_map)
|
|
y_mag = (fabc->fb0[c_sel] * x + fabc->fb1[c_sel] * y +
|
|
fabc->fb2[c_sel]) /
|
|
(fabc->fc0[c_sel] * x + fabc->fc1[c_sel] * y +
|
|
fabc->fc2[c_sel]);
|
|
else
|
|
y_mag = (pa * pf - pc * pd) / (pa * pe - pb * pd);
|
|
|
|
return y_mag * (float)(hvkt->vsize_in - 1) / (float)(hvkt->vsize - 1);
|
|
}
|
|
}
|
|
|
|
static int hvk_table_gen(struct hvkt_param *hvkt)
|
|
{
|
|
float delta_x0, delta_y0;
|
|
float delta_x1, delta_y1;
|
|
float sum_x, sum_y;
|
|
float poly0, poly1;
|
|
|
|
float a0, a1, a2;
|
|
float b0, b1, b2;
|
|
float c0, c1, c2;
|
|
|
|
int lut_hsize, lut_vsize;
|
|
float n, m;
|
|
int extend;
|
|
int chn;
|
|
float *lut;
|
|
int *lut_32[6];
|
|
short *lut_16[6];
|
|
|
|
struct hvkt_fabc fabc = {0};
|
|
|
|
hvkt->fabc = &fabc;
|
|
lut_hsize = hvkt->lut_h_size;
|
|
lut_vsize = hvkt->lut_v_size;
|
|
|
|
lut = (float *)hvkt->lut_addr;
|
|
|
|
chn = hvkt->chn;
|
|
for (int i = 0; i < chn; i++) {
|
|
delta_x0 = hvkt->top_right_x[i] - hvkt->bottom_right_x[i];
|
|
delta_y0 = hvkt->top_right_y[i] - hvkt->bottom_right_y[i];
|
|
delta_x1 = hvkt->bottom_left_x[i] - hvkt->bottom_right_x[i];
|
|
delta_y1 = hvkt->bottom_left_y[i] - hvkt->bottom_right_y[i];
|
|
|
|
sum_x = hvkt->top_left_x[i] - hvkt->top_right_x[i] +
|
|
hvkt->bottom_right_x[i] - hvkt->bottom_left_x[i];
|
|
sum_y = hvkt->top_left_y[i] - hvkt->top_right_y[i] +
|
|
hvkt->bottom_right_y[i] - hvkt->bottom_left_y[i];
|
|
|
|
poly0 = (sum_x * delta_y1 - sum_y * delta_x1);
|
|
poly0 = poly0 / (delta_x0 * delta_y1 - delta_x1 * delta_y0);
|
|
poly1 = (sum_y * delta_x0 - sum_x * delta_y0) /
|
|
(delta_x0 * delta_y1 - delta_x1 * delta_y0);
|
|
|
|
a0 = ((hvkt->top_right_x[i] - hvkt->top_left_x[i]) +
|
|
poly0 * hvkt->top_right_x[i]) /
|
|
(hvkt->hsize - 1);
|
|
a1 = ((hvkt->bottom_left_x[i] - hvkt->top_left_x[i]) +
|
|
poly1 * hvkt->bottom_left_x[i]) /
|
|
(hvkt->vsize - 1);
|
|
a2 = hvkt->top_left_x[i];
|
|
|
|
b0 = ((hvkt->top_right_y[i] - hvkt->top_left_y[i]) +
|
|
poly0 * hvkt->top_right_y[i]) /
|
|
(hvkt->hsize - 1);
|
|
b1 = ((hvkt->bottom_left_y[i] - hvkt->top_left_y[i]) +
|
|
poly1 * hvkt->bottom_left_y[i]) /
|
|
(hvkt->vsize - 1);
|
|
b2 = hvkt->top_left_y[i];
|
|
|
|
c0 = poly0 / (hvkt->hsize - 1);
|
|
c1 = poly1 / (hvkt->vsize - 1);
|
|
c2 = 1;
|
|
|
|
fabc.fa0[i] = a0;
|
|
fabc.fa1[i] = a1;
|
|
fabc.fa2[i] = a2;
|
|
|
|
fabc.fb0[i] = b0;
|
|
fabc.fb1[i] = b1;
|
|
fabc.fb2[i] = b2;
|
|
|
|
fabc.fc0[i] = c0;
|
|
fabc.fc1[i] = c1;
|
|
fabc.fc2[i] = c2;
|
|
}
|
|
|
|
m = (float)hvkt->lut_h_step;
|
|
n = (float)hvkt->lut_v_step;
|
|
|
|
if (!hvkt->is_external_lut) {
|
|
extend = hvkt->extend;
|
|
for (int v = -extend; v < lut_vsize - extend; v++) {
|
|
for (int i = 0; i < chn; i++) {
|
|
for (int h = -extend; h < lut_hsize - extend; h++) {
|
|
float x = h * m;
|
|
float y = v * n;
|
|
int c_sel = i;
|
|
|
|
/*lut[v][i][0][h]*/
|
|
int pos_x = (v + extend) * (lut_hsize * chn * 2) +
|
|
i * (lut_hsize * 2) + (h + extend);
|
|
/*lut[v][i][1][h]*/
|
|
int pos_y = (v + extend) * (lut_hsize * chn * 2) +
|
|
i * (lut_hsize * 2) + lut_hsize + (h + extend);
|
|
// printf("pos_x=%d, pos_y=%d\n", pos_x, pos_y);
|
|
lut[pos_x] = hvk_pos_calc(x, y, hvkt, true, c_sel); /*x*/
|
|
lut[pos_y] = hvk_pos_calc(x, y, hvkt, false, c_sel); /*y*/
|
|
}
|
|
}
|
|
}
|
|
} else { /*bilinear*/
|
|
for (int i = 0; i < 6; i++) {
|
|
lut_32[i] = (int *)hvkt->lut_real_addr[i];
|
|
lut_16[i] = (short *)hvkt->lut_real_addr[i];
|
|
}
|
|
for (int i = 0; i < chn; i++) {
|
|
for (int v = 0; v < lut_vsize; v++) {
|
|
for (int h = 0; h < lut_hsize; h++) {
|
|
float x = h * m;
|
|
float y = v * n;
|
|
int c_sel = i;
|
|
|
|
lut[h] = hvk_pos_calc(x, y, hvkt, true, c_sel); /*x*/
|
|
lut[lut_hsize + h] =
|
|
hvk_pos_calc(x, y, hvkt, false, c_sel); /*y*/
|
|
|
|
if (hvkt->external_lut_format == 1) { /*32bits*/
|
|
int tmp = (int)(lut[h] * 256);
|
|
tmp = (tmp > 524287) ? 524287
|
|
: (tmp < -524288) ? -524288 : tmp;
|
|
lut_32[i * 2][v * lut_hsize + h] = tmp;
|
|
|
|
tmp = (int)(lut[lut_hsize + h] * 256);
|
|
tmp = (tmp > 524287) ? 524287
|
|
: (tmp < -524288) ? -524288 : tmp;
|
|
lut_32[i * 2 + 1][v * lut_hsize + h] = tmp;
|
|
} else {
|
|
int tmp16 = (int)(lut[h] * 16);
|
|
tmp16 = (tmp16 > 32767)
|
|
? 32767
|
|
: (tmp16 < -32768) ? -32768 : tmp16;
|
|
lut_16[i * 2][v * lut_hsize + h] = (short)tmp16;
|
|
|
|
tmp16 = (int)(lut[lut_hsize + h] * 16);
|
|
tmp16 = (tmp16 > 32767)
|
|
? 32767
|
|
: (tmp16 < -32768) ? -32768 : tmp16;
|
|
lut_16[i * 2 + 1][v * lut_hsize + h] = (short)tmp16;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < 6; i++) {
|
|
if (hvkt->external_lut_format == 1) /*32bits*/
|
|
arch_clean_cache_range((int)lut_32[i],
|
|
lut_hsize * lut_vsize * 4);
|
|
else
|
|
arch_clean_cache_range((int)lut_16[i],
|
|
lut_hsize * lut_vsize * 2);
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int m_v_parameter_calc(struct hvkt_param *hvkt)
|
|
{
|
|
int lut_hsize, lut_vsize;
|
|
float *lut_ld = TEMPAA;
|
|
float *m_param_ld = TEMPAB;
|
|
float *lut = (float *)hvkt->lut_addr;
|
|
float *m_v = (float *)hvkt->m_v_addr;
|
|
float *alpha_addr = (float *)hvkt->alpha_addr;
|
|
int chn = hvkt->chn;
|
|
|
|
lut_hsize = hvkt->lut_h_size;
|
|
lut_vsize = hvkt->lut_v_size;
|
|
|
|
m_parameter_calc_alpha(lut_vsize, alpha_addr);
|
|
|
|
for (int h = 0; h < lut_hsize; h++) {
|
|
for (int i = 0; i < chn; i++) {
|
|
/*x*/
|
|
for (int v = 0; v < lut_vsize; v++) {
|
|
lut_ld[v] = lut[v * lut_hsize * chn * 2 + i * lut_hsize * 2 +
|
|
h]; /*lut[v][i][0][h]*/
|
|
}
|
|
m_parameter_calc_1d(lut_ld, m_param_ld, alpha_addr, lut_vsize,
|
|
false);
|
|
|
|
for (int v = 0; v < lut_vsize; v++) {
|
|
/*m_v[i][v][0][h]*/
|
|
m_v[v * lut_hsize * chn * 2 + i * lut_hsize * 2 + h] =
|
|
m_param_ld[i];
|
|
}
|
|
|
|
/*y*/
|
|
for (int v = 0; v < lut_vsize; v++) {
|
|
lut_ld[v] = lut[v * lut_hsize * chn * 2 + i * lut_hsize * 2 +
|
|
lut_hsize + h]; /*lut[v][i][1][h]*/
|
|
}
|
|
m_parameter_calc_1d(lut_ld, m_param_ld, alpha_addr, lut_vsize,
|
|
false);
|
|
|
|
for (int v = 0; v < lut_vsize; v++) {
|
|
/*m_v[i][v][1][h]*/
|
|
m_v[v * lut_hsize * chn * 2 + i * lut_hsize * 2 + lut_hsize +
|
|
h] = m_param_ld[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
double sinc(double phase)
|
|
{
|
|
if (fabs(phase) < 0.0001)
|
|
return 1.0;
|
|
else
|
|
return sin(phase) / phase;
|
|
}
|
|
|
|
static int rotation(double angle, struct hvkt_param *hvkt)
|
|
{
|
|
double r, theta, rot;
|
|
double x, y;
|
|
int transpose_en;
|
|
double rotate_angle;
|
|
|
|
double PI = acos(-1);
|
|
|
|
rotate_angle = (angle * PI) / 180.0;
|
|
x = (hvkt->hsize_in - 1.0) / 2.0;
|
|
y = (hvkt->vsize_in - 1.0) / 2.0;
|
|
theta = atan(y / x);
|
|
r = sqrt(x * x + y * y);
|
|
|
|
if ((rotate_angle >= 0.0 && rotate_angle <= (PI / 4.0)) ||
|
|
(rotate_angle > (PI * 7.0 / 4.0) && rotate_angle < (PI * 8.0 / 4.0))) {
|
|
rot = rotate_angle;
|
|
hvkt->flip_type = 0;
|
|
transpose_en = 0;
|
|
} else if (rotate_angle >= (3.0 * PI / 4.0) &&
|
|
rotate_angle <= (5.0 * PI / 4.0)) {
|
|
rot = rotate_angle - PI;
|
|
hvkt->flip_type = 3;
|
|
transpose_en = 0;
|
|
} else if (rotate_angle > (PI / 4.0) && rotate_angle < (PI * 3.0 / 4.0)) {
|
|
rot = PI / 2.0 - rotate_angle;
|
|
hvkt->flip_type = 1;
|
|
transpose_en = 1;
|
|
} else if (rotate_angle > (PI * 5.0 / 4.0) &&
|
|
rotate_angle < (PI * 7.0 / 4.0)) {
|
|
rot = 6.0 * PI / 4.0 - rotate_angle;
|
|
hvkt->flip_type = 2;
|
|
transpose_en = 1;
|
|
} else { /*illigal*/
|
|
rot = rotate_angle;
|
|
hvkt->flip_type = 0;
|
|
transpose_en = 0;
|
|
}
|
|
|
|
rot = -rot;
|
|
|
|
hvkt->top_left_x[0] = cos(1.0 * PI + theta - rot) * r + x;
|
|
hvkt->top_left_y[0] = sin(1.0 * PI + theta - rot) * r + y;
|
|
|
|
hvkt->top_right_x[0] = cos(-theta - rot) * r + x;
|
|
hvkt->top_right_y[0] = sin(-theta - rot) * r + y;
|
|
|
|
hvkt->bottom_left_x[0] = cos(PI - theta - rot) * r + x;
|
|
hvkt->bottom_left_y[0] = sin(PI - theta - rot) * r + y;
|
|
|
|
hvkt->bottom_right_x[0] = cos(theta - rot) * r + x;
|
|
hvkt->bottom_right_y[0] = sin(theta - rot) * r + y;
|
|
ASW_LOG_DEBUG("top-left(%f, %f), top-right(%f, %f), bottom-left(%f, %f), "
|
|
"bottom-right(%f, %f)",
|
|
hvkt->top_left_x[0], hvkt->top_left_y[0],
|
|
hvkt->top_right_x[0], hvkt->top_right_y[0],
|
|
hvkt->bottom_left_x[0], hvkt->bottom_left_y[0],
|
|
hvkt->bottom_right_x[0], hvkt->bottom_right_y[0]);
|
|
|
|
return transpose_en;
|
|
}
|
|
|
|
static int rotate_lut_table_gen(double angle, struct hvkt_param *hvkt)
|
|
{
|
|
rotation(angle, hvkt);
|
|
hvk_table_gen(hvkt);
|
|
|
|
return 0;
|
|
}
|
|
|
|
struct sdrv_lut_hvk_ops lut_hvk_ops = {
|
|
.lut_table_gen = hvk_table_gen,
|
|
.lut_m_v_gen = m_v_parameter_calc,
|
|
.lut_rotate_table_gen = rotate_lut_table_gen,
|
|
};
|