Var* iom_iheader2var(struct iom_iheader* h)
{
Var* v;
int i;
v = newVar();
V_TYPE(v) = ID_VAL;
V_FORMAT(v) = ihfmt2vfmt(h->format);
/** Will not be possible now
if (V_FORMAT(v) == VAX_FLOAT) V_FORMAT(v) = DV_FLOAT;
if (V_FORMAT(v) == VAX_INTEGER) V_FORMAT(v) = DV_INT32;
*/
if (h->dim[0] < 0 || h->dim[1] < 0 || h->dim[2] < 0) {
fprintf(stderr, "One of dim[i]'s is not set properly.\n");
fprintf(stderr, "See File: %s Line: %d.\n", __FILE__, __LINE__);
}
V_ORDER(v) = ihorg2vorg(h->org);
V_DSIZE(v) = 1;
for (i = 0; i < 3; i++) {
V_SIZE(v)[i] = (((h->dim)[i] - 1) / h->s_skip[i]) + 1;
V_DSIZE(v) *= V_SIZE(v)[i];
}
return (v);
}
Var* ff_fft(vfuncptr func, Var* arg)
{
Var *real = NULL, *img = NULL;
double* data;
int i, j, n, x, y, z;
COMPLEX *in, *out;
Alist alist[4];
alist[0] = make_alist("real", ID_VAL, NULL, &real);
alist[1] = make_alist("img", ID_VAL, NULL, &img);
alist[2].name = NULL;
if (parse_args(func, arg, alist) == 0) return (NULL);
if (real == NULL && img == NULL) {
parse_error("%s: No real or imaginary objects specified\n", func->name);
return (NULL);
}
x = GetSamples(V_SIZE(real), V_ORG(real));
y = GetLines(V_SIZE(real), V_ORG(real));
z = GetBands(V_SIZE(real), V_ORG(real));
if (img == NULL && x == 2) {
n = y * z;
in = (COMPLEX*)calloc(n, sizeof(COMPLEX));
out = (COMPLEX*)calloc(n, sizeof(COMPLEX));
for (i = 0; i < y; i++) {
for (j = 0; j < z; j++) {
in[i].re = extract_double(real, cpos(0, i, j, real));
in[i].im = extract_double(real, cpos(1, i, j, real));
}
}
} else {
n = V_DSIZE(real);
in = (COMPLEX*)calloc(n, sizeof(COMPLEX));
out = (COMPLEX*)calloc(n, sizeof(COMPLEX));
for (i = 0; i < n; i++) {
in[i].re = extract_double(real, i);
in[i].im = (img == NULL ? 0.0 : extract_double(img, i));
}
}
if (func->fdata == (void*)1) {
fft(in, n, out);
} else {
rft(in, n, out);
}
data = (double*)calloc(n * 2, sizeof(double));
for (i = 0; i < n; i++) {
data[i * 2] = out[i].re;
data[i * 2 + 1] = out[i].im;
}
return (newVal(BSQ, 2, n, 1, DV_DOUBLE, data));
}
int dv_WriteGRD(Var* s, char* filename, int force, char* title, char* task)
{
struct iom_iheader h;
int status;
if (V_TYPE(s) != ID_VAL) {
sprintf(error_buf, "Var is not a value: %s", V_NAME(s));
parse_error(NULL);
return 0;
}
if (GetBands(V_SIZE(s), V_ORG(s)) != 1) {
parse_error("Cannot write GRD files with more than 1 band");
return 0;
}
var2iom_iheader(s, &h);
status = iom_WriteGRD(filename, V_DATA(s), &h, force, title, task);
iom_cleanup_iheader(&h);
if (status == 0) {
parse_error("Writing of GRD file %s failed.\n", filename);
}
return (status == 1);
}
void var2iom_iheader(Var* v, struct iom_iheader* h)
{
int i;
iom_init_iheader(h);
h->org = vorg2ihorg(V_ORDER(v));
for (i = 0; i < 3; i++) {
h->size[i] = V_SIZE(v)[i];
}
h->format = vfmt2ihfmt(V_FORMAT(v));
}
/* does not support updating windows on multiple dcs in one call */
int tegra_dc_update_windows(struct tegra_dc_win *windows[], int n)
{
struct tegra_dc *dc;
unsigned long update_mask = GENERAL_ACT_REQ;
unsigned long win_options;
bool update_blend = false;
int i;
dc = windows[0]->dc;
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE) {
/* Acquire one_shot_lock to avoid race condition between
* cancellation of old delayed work and schedule of new
* delayed work. */
mutex_lock(&dc->one_shot_lock);
cancel_delayed_work_sync(&dc->one_shot_work);
}
mutex_lock(&dc->lock);
if (!dc->enabled) {
mutex_unlock(&dc->lock);
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
mutex_unlock(&dc->one_shot_lock);
return -EFAULT;
}
tegra_dc_hold_dc_out(dc);
if (no_vsync)
tegra_dc_writel(dc, WRITE_MUX_ACTIVE | READ_MUX_ACTIVE,
DC_CMD_STATE_ACCESS);
else
tegra_dc_writel(dc, WRITE_MUX_ASSEMBLY | READ_MUX_ASSEMBLY,
DC_CMD_STATE_ACCESS);
for (i = 0; i < n; i++) {
struct tegra_dc_win *win = windows[i];
bool scan_column = 0;
fixed20_12 h_offset, v_offset;
bool invert_h = (win->flags & TEGRA_WIN_FLAG_INVERT_H) != 0;
bool invert_v = (win->flags & TEGRA_WIN_FLAG_INVERT_V) != 0;
bool yuv = tegra_dc_is_yuv(win->fmt);
bool yuvp = tegra_dc_is_yuv_planar(win->fmt);
unsigned Bpp = tegra_dc_fmt_bpp(win->fmt) / 8;
/* Bytes per pixel of bandwidth, used for dda_inc calculation */
unsigned Bpp_bw = Bpp * (yuvp ? 2 : 1);
const bool filter_h = win_use_h_filter(dc, win);
const bool filter_v = win_use_v_filter(dc, win);
#if defined(CONFIG_TEGRA_DC_SCAN_COLUMN)
scan_column = (win->flags & TEGRA_WIN_FLAG_SCAN_COLUMN);
#endif
if (win->z != dc->blend.z[win->idx]) {
dc->blend.z[win->idx] = win->z;
update_blend = true;
}
if ((win->flags & TEGRA_WIN_BLEND_FLAGS_MASK) !=
dc->blend.flags[win->idx]) {
dc->blend.flags[win->idx] =
win->flags & TEGRA_WIN_BLEND_FLAGS_MASK;
update_blend = true;
}
tegra_dc_writel(dc, WINDOW_A_SELECT << win->idx,
DC_CMD_DISPLAY_WINDOW_HEADER);
if (!no_vsync)
update_mask |= WIN_A_ACT_REQ << win->idx;
if (!WIN_IS_ENABLED(win)) {
/*dc->windows[i].dirty = 1; NV patch, but Now we do not use it */
tegra_dc_writel(dc, 0, DC_WIN_WIN_OPTIONS);
continue;
}
tegra_dc_writel(dc, win->fmt & 0x1f, DC_WIN_COLOR_DEPTH);
tegra_dc_writel(dc, win->fmt >> 6, DC_WIN_BYTE_SWAP);
tegra_dc_writel(dc,
V_POSITION(win->out_y) | H_POSITION(win->out_x),
DC_WIN_POSITION);
tegra_dc_writel(dc,
V_SIZE(win->out_h) | H_SIZE(win->out_w),
DC_WIN_SIZE);
/* Check scan_column flag to set window size and scaling. */
win_options = WIN_ENABLE;
if (scan_column) {
win_options |= WIN_SCAN_COLUMN;
win_options |= H_FILTER_ENABLE(filter_v);
win_options |= V_FILTER_ENABLE(filter_h);
} else {
win_options |= H_FILTER_ENABLE(filter_h);
win_options |= V_FILTER_ENABLE(filter_v);
}
/* Update scaling registers if window supports scaling. */
if (likely(tegra_dc_feature_has_scaling(dc, win->idx)))
tegra_dc_update_scaling(dc, win, Bpp, Bpp_bw,
scan_column);
tegra_dc_writel(dc, 0, DC_WIN_BUF_STRIDE);
tegra_dc_writel(dc, 0, DC_WIN_UV_BUF_STRIDE);
tegra_dc_writel(dc, (unsigned long)win->phys_addr,
DC_WINBUF_START_ADDR);
if (!yuvp) {
tegra_dc_writel(dc, win->stride, DC_WIN_LINE_STRIDE);
} else {
tegra_dc_writel(dc,
(unsigned long)win->phys_addr_u,
DC_WINBUF_START_ADDR_U);
tegra_dc_writel(dc,
(unsigned long)win->phys_addr_v,
DC_WINBUF_START_ADDR_V);
tegra_dc_writel(dc,
LINE_STRIDE(win->stride) |
UV_LINE_STRIDE(win->stride_uv),
DC_WIN_LINE_STRIDE);
}
if (invert_h) {
h_offset.full = win->x.full + win->w.full;
h_offset.full = dfixed_floor(h_offset) * Bpp;
h_offset.full -= dfixed_const(1);
} else {
h_offset.full = dfixed_floor(win->x) * Bpp;
}
v_offset = win->y;
if (invert_v) {
v_offset.full += win->h.full - dfixed_const(1);
}
tegra_dc_writel(dc, dfixed_trunc(h_offset),
DC_WINBUF_ADDR_H_OFFSET);
tegra_dc_writel(dc, dfixed_trunc(v_offset),
DC_WINBUF_ADDR_V_OFFSET);
if (tegra_dc_feature_has_tiling(dc, win->idx)) {
if (WIN_IS_TILED(win))
tegra_dc_writel(dc,
DC_WIN_BUFFER_ADDR_MODE_TILE |
DC_WIN_BUFFER_ADDR_MODE_TILE_UV,
DC_WIN_BUFFER_ADDR_MODE);
else
tegra_dc_writel(dc,
DC_WIN_BUFFER_ADDR_MODE_LINEAR |
DC_WIN_BUFFER_ADDR_MODE_LINEAR_UV,
DC_WIN_BUFFER_ADDR_MODE);
}
if (yuv)
win_options |= CSC_ENABLE;
else if (tegra_dc_fmt_bpp(win->fmt) < 24)
win_options |= COLOR_EXPAND;
#if defined(CONFIG_ARCH_TEGRA_3x_SOC)
if (win->global_alpha == 255) {
tegra_dc_writel(dc, 0, DC_WIN_GLOBAL_ALPHA);
} else {
tegra_dc_writel(dc, GLOBAL_ALPHA_ENABLE |
win->global_alpha, DC_WIN_GLOBAL_ALPHA);
win_options |= CP_ENABLE;
}
#endif
if (win->ppflags & TEGRA_WIN_PPFLAG_CP_ENABLE)
win_options |= CP_ENABLE;
win_options |= H_DIRECTION_DECREMENT(invert_h);
win_options |= V_DIRECTION_DECREMENT(invert_v);
tegra_dc_writel(dc, win_options, DC_WIN_WIN_OPTIONS);
win->dirty = no_vsync ? 0 : 1;
dev_dbg(&dc->ndev->dev, "%s():idx=%d z=%d x=%d y=%d w=%d h=%d "
"out_x=%u out_y=%u out_w=%u out_h=%u "
"fmt=%d yuvp=%d Bpp=%u filter_h=%d filter_v=%d",
__func__, win->idx, win->z,
dfixed_trunc(win->x), dfixed_trunc(win->y),
dfixed_trunc(win->w), dfixed_trunc(win->h),
win->out_x, win->out_y, win->out_w, win->out_h,
win->fmt, yuvp, Bpp, filter_h, filter_v);
trace_printk("%s:win%u in:%ux%u out:%ux%u fmt=%d\n",
dc->ndev->name, win->idx, dfixed_trunc(win->w),
dfixed_trunc(win->h), win->out_w, win->out_h, win->fmt);
}
if (update_blend) {
tegra_dc_set_blending(dc, &dc->blend);
for (i = 0; i < DC_N_WINDOWS; i++) {
if (!no_vsync)
dc->windows[i].dirty = 1;
update_mask |= WIN_A_ACT_REQ << i;
}
}
tegra_dc_set_dynamic_emc(windows, n);
tegra_dc_writel(dc, update_mask << 8, DC_CMD_STATE_CONTROL);
tegra_dc_writel(dc, FRAME_END_INT | V_BLANK_INT, DC_CMD_INT_STATUS);
if (!no_vsync) {
set_bit(V_BLANK_FLIP, &dc->vblank_ref_count);
tegra_dc_unmask_interrupt(dc,
FRAME_END_INT | V_BLANK_INT | ALL_UF_INT);
} else {
clear_bit(V_BLANK_FLIP, &dc->vblank_ref_count);
tegra_dc_mask_interrupt(dc,
FRAME_END_INT | V_BLANK_INT | ALL_UF_INT);
}
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE) {
atomic_set(&update_frame,1);
schedule_delayed_work(&dc->one_shot_work,
msecs_to_jiffies(dc->one_shot_delay_ms));
}
/* update EMC clock if calculated bandwidth has changed */
tegra_dc_program_bandwidth(dc, false);
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
update_mask |= NC_HOST_TRIG;
tegra_dc_writel(dc, update_mask, DC_CMD_STATE_CONTROL);
trace_printk("%s:update_mask=%#lx\n", dc->ndev->name, update_mask);
tegra_dc_release_dc_out(dc);
mutex_unlock(&dc->lock);
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
mutex_unlock(&dc->one_shot_lock);
bool is_yuvp = 0;
for (i = 0; i < n; i++) {
struct tegra_dc_win *win = windows[i];
bool yuvp = tegra_dc_is_yuv_planar(win->fmt);
is_yuvp |= yuvp;
}
if (dc->ndev->id == 0) {
struct tegra_dc_out *out = dc->out;
struct tegra_dsi_out *dsi = out->dsi;
struct tegra_dsi_cmd *cur = NULL;
int n = dsi->n_cabc_cmd;
if (out && dsi && dc->out_ops && dc->out_ops->send_cmd) {
if (is_yuvp && !dc->isyuv_lasttime) {
printk(KERN_INFO "[DISP] YUV\r\n");
cur = dsi->dsi_cabc_still_mode;
dc->isyuv_lasttime = is_yuvp;
}
else if (!is_yuvp && dc->isyuv_lasttime) {
printk(KERN_INFO "[DISP] RGB\r\n");
cur = dsi->dsi_cabc_moving_mode;
dc->isyuv_lasttime = is_yuvp;
}
if (cur) {
dc->out_ops->send_cmd(dc, cur, n);
}
}
}
return 0;
}
/* does not support updating windows on multiple dcs in one call */
int tegra_dc_update_windows(struct tegra_dc_win *windows[], int n)
{
struct tegra_dc *dc;
unsigned long update_mask = GENERAL_ACT_REQ;
unsigned long val;
bool update_blend = false;
int i;
dc = windows[0]->dc;
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE) {
/* Acquire one_shot_lock to avoid race condition between
* cancellation of old delayed work and schedule of new
* delayed work. */
mutex_lock(&dc->one_shot_lock);
cancel_delayed_work_sync(&dc->one_shot_work);
}
mutex_lock(&dc->lock);
if (!dc->enabled) {
mutex_unlock(&dc->lock);
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
mutex_unlock(&dc->one_shot_lock);
return -EFAULT;
}
tegra_dc_hold_dc_out(dc);
if (no_vsync)
tegra_dc_writel(dc, WRITE_MUX_ACTIVE | READ_MUX_ACTIVE,
DC_CMD_STATE_ACCESS);
else
tegra_dc_writel(dc, WRITE_MUX_ASSEMBLY | READ_MUX_ASSEMBLY,
DC_CMD_STATE_ACCESS);
for (i = 0; i < n; i++) {
struct tegra_dc_win *win = windows[i];
unsigned h_dda;
unsigned v_dda;
fixed20_12 h_offset, v_offset;
bool invert_h = (win->flags & TEGRA_WIN_FLAG_INVERT_H) != 0;
bool invert_v = (win->flags & TEGRA_WIN_FLAG_INVERT_V) != 0;
bool yuv = tegra_dc_is_yuv(win->fmt);
bool yuvp = tegra_dc_is_yuv_planar(win->fmt);
unsigned Bpp = tegra_dc_fmt_bpp(win->fmt) / 8;
/* Bytes per pixel of bandwidth, used for dda_inc calculation */
unsigned Bpp_bw = Bpp * (yuvp ? 2 : 1);
const bool filter_h = win_use_h_filter(dc, win);
const bool filter_v = win_use_v_filter(dc, win);
if (win->z != dc->blend.z[win->idx]) {
dc->blend.z[win->idx] = win->z;
update_blend = true;
}
if ((win->flags & TEGRA_WIN_BLEND_FLAGS_MASK) !=
dc->blend.flags[win->idx]) {
dc->blend.flags[win->idx] =
win->flags & TEGRA_WIN_BLEND_FLAGS_MASK;
update_blend = true;
}
tegra_dc_writel(dc, WINDOW_A_SELECT << win->idx,
DC_CMD_DISPLAY_WINDOW_HEADER);
if (!no_vsync)
update_mask |= WIN_A_ACT_REQ << win->idx;
if (!WIN_IS_ENABLED(win)) {
dc->windows[i].dirty = 1;
tegra_dc_writel(dc, 0, DC_WIN_WIN_OPTIONS);
continue;
}
tegra_dc_writel(dc, win->fmt & 0x1f, DC_WIN_COLOR_DEPTH);
tegra_dc_writel(dc, win->fmt >> 6, DC_WIN_BYTE_SWAP);
tegra_dc_writel(dc,
V_POSITION(win->out_y) | H_POSITION(win->out_x),
DC_WIN_POSITION);
tegra_dc_writel(dc,
V_SIZE(win->out_h) | H_SIZE(win->out_w),
DC_WIN_SIZE);
if (tegra_dc_feature_has_scaling(dc, win->idx)) {
tegra_dc_writel(dc,
V_PRESCALED_SIZE(dfixed_trunc(win->h)) |
H_PRESCALED_SIZE(dfixed_trunc(win->w) * Bpp),
DC_WIN_PRESCALED_SIZE);
h_dda = compute_dda_inc(win->w, win->out_w, false,
Bpp_bw);
v_dda = compute_dda_inc(win->h, win->out_h, true,
Bpp_bw);
tegra_dc_writel(dc, V_DDA_INC(v_dda) |
H_DDA_INC(h_dda), DC_WIN_DDA_INCREMENT);
h_dda = compute_initial_dda(win->x);
v_dda = compute_initial_dda(win->y);
tegra_dc_writel(dc, h_dda, DC_WIN_H_INITIAL_DDA);
tegra_dc_writel(dc, v_dda, DC_WIN_V_INITIAL_DDA);
}
tegra_dc_writel(dc, 0, DC_WIN_BUF_STRIDE);
tegra_dc_writel(dc, 0, DC_WIN_UV_BUF_STRIDE);
tegra_dc_writel(dc, (unsigned long)win->phys_addr,
DC_WINBUF_START_ADDR);
if (!yuvp) {
tegra_dc_writel(dc, win->stride, DC_WIN_LINE_STRIDE);
} else {
tegra_dc_writel(dc,
(unsigned long)win->phys_addr_u,
DC_WINBUF_START_ADDR_U);
tegra_dc_writel(dc,
(unsigned long)win->phys_addr_v,
DC_WINBUF_START_ADDR_V);
tegra_dc_writel(dc,
LINE_STRIDE(win->stride) |
UV_LINE_STRIDE(win->stride_uv),
DC_WIN_LINE_STRIDE);
}
h_offset = win->x;
if (invert_h) {
h_offset.full += win->w.full - dfixed_const(1);
}
v_offset = win->y;
if (invert_v) {
v_offset.full += win->h.full - dfixed_const(1);
}
tegra_dc_writel(dc, dfixed_trunc(h_offset) * Bpp,
DC_WINBUF_ADDR_H_OFFSET);
tegra_dc_writel(dc, dfixed_trunc(v_offset),
DC_WINBUF_ADDR_V_OFFSET);
if (tegra_dc_feature_has_tiling(dc, win->idx)) {
if (WIN_IS_TILED(win))
tegra_dc_writel(dc,
DC_WIN_BUFFER_ADDR_MODE_TILE |
DC_WIN_BUFFER_ADDR_MODE_TILE_UV,
DC_WIN_BUFFER_ADDR_MODE);
else
tegra_dc_writel(dc,
DC_WIN_BUFFER_ADDR_MODE_LINEAR |
DC_WIN_BUFFER_ADDR_MODE_LINEAR_UV,
DC_WIN_BUFFER_ADDR_MODE);
}
val = WIN_ENABLE;
if (yuv)
val |= CSC_ENABLE;
else if (tegra_dc_fmt_bpp(win->fmt) < 24)
val |= COLOR_EXPAND;
if (win->ppflags & TEGRA_WIN_PPFLAG_CP_ENABLE)
val |= CP_ENABLE;
if (filter_h)
val |= H_FILTER_ENABLE;
if (filter_v)
val |= V_FILTER_ENABLE;
if (invert_h)
val |= H_DIRECTION_DECREMENT;
if (invert_v)
val |= V_DIRECTION_DECREMENT;
tegra_dc_writel(dc, val, DC_WIN_WIN_OPTIONS);
#ifdef CONFIG_ARCH_TEGRA_3x_SOC
if (win->global_alpha == 255)
tegra_dc_writel(dc, 0, DC_WIN_GLOBAL_ALPHA);
else
tegra_dc_writel(dc, GLOBAL_ALPHA_ENABLE |
win->global_alpha, DC_WIN_GLOBAL_ALPHA);
#endif
win->dirty = no_vsync ? 0 : 1;
dev_dbg(&dc->ndev->dev, "%s():idx=%d z=%d x=%d y=%d w=%d h=%d "
"out_x=%u out_y=%u out_w=%u out_h=%u "
"fmt=%d yuvp=%d Bpp=%u filter_h=%d filter_v=%d",
__func__, win->idx, win->z,
dfixed_trunc(win->x), dfixed_trunc(win->y),
dfixed_trunc(win->w), dfixed_trunc(win->h),
win->out_x, win->out_y, win->out_w, win->out_h,
win->fmt, yuvp, Bpp, filter_h, filter_v);
trace_printk("%s:win%u in:%ux%u out:%ux%u fmt=%d\n",
dc->ndev->name, win->idx, dfixed_trunc(win->w),
dfixed_trunc(win->h), win->out_w, win->out_h, win->fmt);
}
if (update_blend) {
tegra_dc_set_blending(dc, &dc->blend);
for (i = 0; i < DC_N_WINDOWS; i++) {
if (!no_vsync)
dc->windows[i].dirty = 1;
update_mask |= WIN_A_ACT_REQ << i;
}
}
tegra_dc_set_dynamic_emc(windows, n);
tegra_dc_writel(dc, update_mask << 8, DC_CMD_STATE_CONTROL);
tegra_dc_writel(dc, FRAME_END_INT | V_BLANK_INT, DC_CMD_INT_STATUS);
if (!no_vsync) {
set_bit(V_BLANK_FLIP, &dc->vblank_ref_count);
tegra_dc_unmask_interrupt(dc,
FRAME_END_INT | V_BLANK_INT | ALL_UF_INT);
} else {
clear_bit(V_BLANK_FLIP, &dc->vblank_ref_count);
tegra_dc_mask_interrupt(dc, V_BLANK_INT | ALL_UF_INT);
if (!atomic_read(&frame_end_ref))
tegra_dc_mask_interrupt(dc, FRAME_END_INT);
}
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
schedule_delayed_work(&dc->one_shot_work,
msecs_to_jiffies(dc->one_shot_delay_ms));
/* update EMC clock if calculated bandwidth has changed */
tegra_dc_program_bandwidth(dc, false);
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
update_mask |= NC_HOST_TRIG;
tegra_dc_writel(dc, update_mask, DC_CMD_STATE_CONTROL);
trace_printk("%s:update_mask=%#lx\n", dc->ndev->name, update_mask);
tegra_dc_release_dc_out(dc);
mutex_unlock(&dc->lock);
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
mutex_unlock(&dc->one_shot_lock);
return 0;
}
/*
** compute_windowed_mean() - computes the mean and count of the pixels
** within a wxh window, using a running-sum table.
** Returns the mean and count for each pixel.
*/
void init_sums(Var* data, int w, int h, int d, Var** rn, Var** rs, Var** rcount, Var** rmean,
Var** rsigma, double ignore)
{
int x, y, z;
int i, j, k;
size_t p1, p2, p3, p4, p5, p6, p7, p8;
size_t nelements;
int east, south, north, west, front, back;
double *s, *s2;
int* n;
float* mean;
float* sigma;
int* c;
double value;
double sum, sum2;
int count;
x = GetX(data);
y = GetY(data);
z = GetZ(data);
nelements = V_DSIZE(data);
s = calloc(nelements, sizeof(double)); /* running sum */
s2 = calloc(nelements, sizeof(double)); /* running sum */
n = calloc(nelements, sizeof(int)); /* running count */
mean = calloc(nelements, sizeof(float));
sigma = calloc(nelements, sizeof(float));
c = calloc(nelements, sizeof(int));
/*
** compute the running sum and count of V
**
** s(i,j) = f(i,j) +s(i-1,j)+s(i,j-1)-s(i-1,j-1)
*/
for (k = 0; k < z; k++) {
for (j = 0; j < y; j++) {
for (i = 0; i < x; i++) {
p1 = cpos(i, j, k, data);
value = extract_double(data, p1);
if (value != ignore) {
s[p1] = value;
s2[p1] = value * value;
n[p1] = 1;
}
if (i) {
p2 = cpos(i - 1, j, k, data);
s[p1] += s[p2];
s2[p1] += s2[p2];
n[p1] += n[p2];
}
if (j) {
p3 = cpos(i, j - 1, k, data);
s[p1] += s[p3];
s2[p1] += s2[p3];
n[p1] += n[p3];
}
if (i && j) {
p4 = cpos(i - 1, j - 1, k, data);
s[p1] -= s[p4];
s2[p1] -= s2[p4];
n[p1] -= n[p4];
}
if (k) {
p5 = cpos(i, j, k - 1, data);
s[p1] += s[p5];
s2[p1] += s2[p5];
n[p1] += n[p5];
if (i) {
p6 = cpos(i - 1, j, k - 1, data);
s[p1] -= s[p6];
s2[p1] -= s2[p6];
n[p1] -= n[p6];
}
if (j) {
p7 = cpos(i, j - 1, k - 1, data);
s[p1] -= s[p7];
s2[p1] -= s2[p7];
n[p1] -= n[p7];
}
if (i && j) {
p8 = cpos(i - 1, j - 1, k - 1, data);
s[p1] += s[p8];
s2[p1] += s2[p8];
n[p1] += n[p8];
}
}
}
}
}
/* Formula to compute windowed sum from running sum is:
** s(u,v) = s(u+M-1,v+N-1) - s(u-1,v+N-1) - s(u+M-1,v-1) + s(u-1,v-1)
** This is: sum(pt) = s(se) - s(sw) - s(ne) + s(nw).
** Given a 4x3 mask, it's this:
**
** nw -- -- -- ne
** -- pt -- -- --
** -- -- -- -- --
** sw -- -- -- se
**/
for (k = 0; k < z; k++) {
for (j = 0; j < y; j++) {
for (i = 0; i < x; i++) {
east = min(i + (w / 2), x - 1);
south = min(j + (h / 2), y - 1);
front = min(k + (d / 2), z - 1);
west = i - (w / 2) - 1;
north = j - (h / 2) - 1;
back = k - (d / 2) - 1;
p1 = cpos(east, south, front, data);
count = n[p1];
sum = s[p1];
sum2 = s2[p1];
if (west >= 0) {
p2 = cpos(west, south, front, data);
count -= n[p2];
sum -= s[p2];
sum2 -= s2[p2];
}
if (north >= 0) {
p3 = cpos(east, north, front, data);
count -= n[p3];
sum -= s[p3];
sum2 -= s2[p3];
}
if (north >= 0 && west >= 0) {
p4 = cpos(west, north, front, data);
count += n[p4];
sum += s[p4];
sum2 += s2[p4];
}
if (back >= 0) {
p5 = cpos(east, south, back, data);
count -= n[p5];
sum -= s[p5];
sum2 -= s2[p5];
if (west >= 0) {
p6 = cpos(west, south, back, data);
count += n[p6];
sum += s[p6];
sum2 += s2[p6];
}
if (north >= 0) {
p7 = cpos(east, north, back, data);
count += n[p7];
sum += s[p7];
sum2 += s2[p7];
}
if (north >= 0 && west >= 0) {
p8 = cpos(west, north, back, data);
count -= n[p8];
sum -= s[p8];
sum2 -= s2[p8];
}
}
/*
sum = s[cpos(min(i+w-1, x-1), min(j+h-1, y-1), k, data)];
sum -= (i ? s[cpos(i-1, min(j+h-1, y-1), k, data)] : 0.0);
sum -= (j ? s[cpos(min(i+w-1, x-1), j-1, k, data)] : 0.0);
sum += (i == 0 || j == 0 ? 0.0 : s[cpos(i-1,j-1,k,data)]);
*/
if (count) {
p1 = cpos(i, j, k, data);
mean[p1] = sum / count;
if (count > 1) {
sigma[p1] = sqrt((sum2 - (sum * sum / count)) / (count - 1));
} else {
sigma[p1] = ignore;
}
c[p1] = count;
} else {
mean[cpos(i, j, k, data)] = ignore;
sigma[cpos(i, j, k, data)] = ignore;
c[p1] = ignore;
}
}
}
}
free(s2);
*rn = newVal(V_ORG(data), V_SIZE(data)[0], V_SIZE(data)[1], V_SIZE(data)[2], DV_INT32, n);
*rs = newVal(V_ORG(data), V_SIZE(data)[0], V_SIZE(data)[1], V_SIZE(data)[2], DV_DOUBLE, s);
*rcount = newVal(V_ORG(data), V_SIZE(data)[0], V_SIZE(data)[1], V_SIZE(data)[2], DV_INT32, c);
*rmean = newVal(V_ORG(data), V_SIZE(data)[0], V_SIZE(data)[1], V_SIZE(data)[2], DV_FLOAT, mean);
*rsigma = newVal(V_ORG(data), V_SIZE(data)[0], V_SIZE(data)[1], V_SIZE(data)[2], DV_FLOAT, sigma);
}
/* does not support updating windows on multiple dcs in one call */
int tegra_dc_update_windows(struct tegra_dc_win *windows[], int n)
{
struct tegra_dc *dc;
unsigned long update_mask = GENERAL_ACT_REQ;
unsigned long val;
bool update_blend = false;
int i;
dc = windows[0]->dc;
mutex_lock(&dc->lock);
if (!dc->enabled) {
mutex_unlock(&dc->lock);
return -EFAULT;
}
if (no_vsync)
tegra_dc_writel(dc, WRITE_MUX_ACTIVE | READ_MUX_ACTIVE, DC_CMD_STATE_ACCESS);
else
tegra_dc_writel(dc, WRITE_MUX_ASSEMBLY | READ_MUX_ASSEMBLY, DC_CMD_STATE_ACCESS);
for (i = 0; i < n; i++) {
struct tegra_dc_win *win = windows[i];
unsigned h_dda;
unsigned v_dda;
bool yuvp = tegra_dc_is_yuv_planar(win->fmt);
unsigned Bpp = tegra_dc_fmt_bpp(win->fmt) / 8;
static const struct {
bool h;
bool v;
} can_filter[] = {
/* Window A has no filtering */
{ false, false },
/* Window B has both H and V filtering */
{ true, true },
/* Window C has only H filtering */
{ false, true },
};
const bool filter_h = can_filter[win->idx].h &&
(win->w.full != dfixed_const(win->out_w));
const bool filter_v = can_filter[win->idx].v &&
(win->h.full != dfixed_const(win->out_h));
if (win->z != dc->blend.z[win->idx]) {
dc->blend.z[win->idx] = win->z;
update_blend = true;
}
if ((win->flags & TEGRA_WIN_BLEND_FLAGS_MASK) !=
dc->blend.flags[win->idx]) {
dc->blend.flags[win->idx] =
win->flags & TEGRA_WIN_BLEND_FLAGS_MASK;
update_blend = true;
}
tegra_dc_writel(dc, WINDOW_A_SELECT << win->idx,
DC_CMD_DISPLAY_WINDOW_HEADER);
if (!no_vsync)
update_mask |= WIN_A_ACT_REQ << win->idx;
if (!(win->flags & TEGRA_WIN_FLAG_ENABLED)) {
tegra_dc_writel(dc, 0, DC_WIN_WIN_OPTIONS);
continue;
}
tegra_dc_writel(dc, win->fmt, DC_WIN_COLOR_DEPTH);
tegra_dc_writel(dc, 0, DC_WIN_BYTE_SWAP);
tegra_dc_writel(dc,
V_POSITION(win->out_y) | H_POSITION(win->out_x),
DC_WIN_POSITION);
tegra_dc_writel(dc,
V_SIZE(win->out_h) | H_SIZE(win->out_w),
DC_WIN_SIZE);
tegra_dc_writel(dc,
V_PRESCALED_SIZE(dfixed_trunc(win->h)) |
H_PRESCALED_SIZE(dfixed_trunc(win->w) * Bpp),
DC_WIN_PRESCALED_SIZE);
h_dda = compute_dda_inc(win->w, win->out_w, false, Bpp);
v_dda = compute_dda_inc(win->h, win->out_h, true, Bpp);
tegra_dc_writel(dc, V_DDA_INC(v_dda) | H_DDA_INC(h_dda),
DC_WIN_DDA_INCREMENT);
h_dda = compute_initial_dda(win->x);
v_dda = compute_initial_dda(win->y);
tegra_dc_writel(dc, h_dda, DC_WIN_H_INITIAL_DDA);
tegra_dc_writel(dc, v_dda, DC_WIN_V_INITIAL_DDA);
tegra_dc_writel(dc, 0, DC_WIN_BUF_STRIDE);
tegra_dc_writel(dc, 0, DC_WIN_UV_BUF_STRIDE);
tegra_dc_writel(dc, (unsigned long)win->phys_addr,
DC_WINBUF_START_ADDR);
if (!yuvp) {
tegra_dc_writel(dc, win->stride, DC_WIN_LINE_STRIDE);
} else {
tegra_dc_writel(dc,
(unsigned long)win->phys_addr +
(unsigned long)win->offset_u,
DC_WINBUF_START_ADDR_U);
tegra_dc_writel(dc,
(unsigned long)win->phys_addr +
(unsigned long)win->offset_v,
DC_WINBUF_START_ADDR_V);
tegra_dc_writel(dc,
LINE_STRIDE(win->stride) |
UV_LINE_STRIDE(win->stride_uv),
DC_WIN_LINE_STRIDE);
}
tegra_dc_writel(dc, dfixed_trunc(win->x) * Bpp,
DC_WINBUF_ADDR_H_OFFSET);
tegra_dc_writel(dc, dfixed_trunc(win->y),
DC_WINBUF_ADDR_V_OFFSET);
val = WIN_ENABLE;
if (yuvp)
val |= CSC_ENABLE;
else if (tegra_dc_fmt_bpp(win->fmt) < 24)
val |= COLOR_EXPAND;
if (filter_h)
val |= H_FILTER_ENABLE;
if (filter_v)
val |= V_FILTER_ENABLE;
tegra_dc_writel(dc, val, DC_WIN_WIN_OPTIONS);
win->dirty = no_vsync ? 0 : 1;
}
if (update_blend) {
tegra_dc_set_blending(dc, &dc->blend);
for (i = 0; i < DC_N_WINDOWS; i++) {
if (!no_vsync)
dc->windows[i].dirty = 1;
update_mask |= WIN_A_ACT_REQ << i;
}
}
tegra_dc_writel(dc, update_mask << 8, DC_CMD_STATE_CONTROL);
if (!no_vsync) {
val = tegra_dc_readl(dc, DC_CMD_INT_ENABLE);
val |= FRAME_END_INT;
tegra_dc_writel(dc, val, DC_CMD_INT_ENABLE);
val = tegra_dc_readl(dc, DC_CMD_INT_MASK);
val |= FRAME_END_INT;
tegra_dc_writel(dc, val, DC_CMD_INT_MASK);
}
tegra_dc_writel(dc, update_mask, DC_CMD_STATE_CONTROL);
mutex_unlock(&dc->lock);
return 0;
}
/* Does not support updating windows on multiple dcs in one call.
* Requires a matching sync_windows to avoid leaking ref-count on clocks. */
int tegra_dc_update_windows(struct tegra_dc_win *windows[], int n)
{
struct tegra_dc *dc;
unsigned long update_mask = GENERAL_ACT_REQ;
unsigned long win_options;
bool update_blend_par = false;
bool update_blend_seq = false;
int i;
dc = windows[0]->dc;
trace_update_windows(dc);
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE) {
/* Acquire one_shot_lock to avoid race condition between
* cancellation of old delayed work and schedule of new
* delayed work. */
mutex_lock(&dc->one_shot_lock);
cancel_delayed_work_sync(&dc->one_shot_work);
}
mutex_lock(&dc->lock);
if (!dc->enabled) {
mutex_unlock(&dc->lock);
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
mutex_unlock(&dc->one_shot_lock);
return -EFAULT;
}
tegra_dc_io_start(dc);
tegra_dc_hold_dc_out(dc);
if (no_vsync)
tegra_dc_writel(dc, WRITE_MUX_ACTIVE | READ_MUX_ACTIVE,
DC_CMD_STATE_ACCESS);
else
tegra_dc_writel(dc, WRITE_MUX_ASSEMBLY | READ_MUX_ASSEMBLY,
DC_CMD_STATE_ACCESS);
for (i = 0; i < n; i++) {
struct tegra_dc_win *win = windows[i];
struct tegra_dc_win *dc_win = tegra_dc_get_window(dc, win->idx);
bool scan_column = 0;
fixed20_12 h_offset, v_offset;
bool invert_h = (win->flags & TEGRA_WIN_FLAG_INVERT_H) != 0;
bool invert_v = (win->flags & TEGRA_WIN_FLAG_INVERT_V) != 0;
bool yuv = tegra_dc_is_yuv(win->fmt);
bool yuvp = tegra_dc_is_yuv_planar(win->fmt);
unsigned Bpp = tegra_dc_fmt_bpp(win->fmt) / 8;
/* Bytes per pixel of bandwidth, used for dda_inc calculation */
unsigned Bpp_bw = Bpp * (yuvp ? 2 : 1);
const bool filter_h = win_use_h_filter(dc, win);
const bool filter_v = win_use_v_filter(dc, win);
#if defined(CONFIG_TEGRA_DC_SCAN_COLUMN)
scan_column = (win->flags & TEGRA_WIN_FLAG_SCAN_COLUMN);
#endif
/* Update blender */
if ((win->z != dc->blend.z[win->idx]) ||
((win->flags & TEGRA_WIN_BLEND_FLAGS_MASK) !=
dc->blend.flags[win->idx])) {
dc->blend.z[win->idx] = win->z;
dc->blend.flags[win->idx] =
win->flags & TEGRA_WIN_BLEND_FLAGS_MASK;
if (tegra_dc_feature_is_gen2_blender(dc, win->idx))
update_blend_seq = true;
else
update_blend_par = true;
}
tegra_dc_writel(dc, WINDOW_A_SELECT << win->idx,
DC_CMD_DISPLAY_WINDOW_HEADER);
if (!no_vsync)
update_mask |= WIN_A_ACT_REQ << win->idx;
if (!WIN_IS_ENABLED(win)) {
dc_win->dirty = no_vsync ? 0 : 1;
tegra_dc_writel(dc, 0, DC_WIN_WIN_OPTIONS);
continue;
}
tegra_dc_writel(dc, win->fmt & 0x1f, DC_WIN_COLOR_DEPTH);
tegra_dc_writel(dc, win->fmt >> 6, DC_WIN_BYTE_SWAP);
tegra_dc_writel(dc,
V_POSITION(win->out_y) | H_POSITION(win->out_x),
DC_WIN_POSITION);
tegra_dc_writel(dc,
V_SIZE(win->out_h) | H_SIZE(win->out_w),
DC_WIN_SIZE);
/* Check scan_column flag to set window size and scaling. */
win_options = WIN_ENABLE;
if (scan_column) {
win_options |= WIN_SCAN_COLUMN;
win_options |= H_FILTER_ENABLE(filter_v);
win_options |= V_FILTER_ENABLE(filter_h);
} else {
win_options |= H_FILTER_ENABLE(filter_h);
win_options |= V_FILTER_ENABLE(filter_v);
}
/* Update scaling registers if window supports scaling. */
if (likely(tegra_dc_feature_has_scaling(dc, win->idx)))
tegra_dc_update_scaling(dc, win, Bpp, Bpp_bw,
scan_column);
#if defined(CONFIG_ARCH_TEGRA_2x_SOC) || defined(CONFIG_ARCH_TEGRA_3x_SOC)
tegra_dc_writel(dc, 0, DC_WIN_BUF_STRIDE);
tegra_dc_writel(dc, 0, DC_WIN_UV_BUF_STRIDE);
#endif
tegra_dc_writel(dc, (unsigned long)win->phys_addr,
DC_WINBUF_START_ADDR);
if (!yuvp) {
tegra_dc_writel(dc, win->stride, DC_WIN_LINE_STRIDE);
} else {
tegra_dc_writel(dc,
(unsigned long)win->phys_addr_u,
DC_WINBUF_START_ADDR_U);
tegra_dc_writel(dc,
(unsigned long)win->phys_addr_v,
DC_WINBUF_START_ADDR_V);
tegra_dc_writel(dc,
LINE_STRIDE(win->stride) |
UV_LINE_STRIDE(win->stride_uv),
DC_WIN_LINE_STRIDE);
}
if (invert_h) {
h_offset.full = win->x.full + win->w.full;
h_offset.full = dfixed_floor(h_offset) * Bpp;
h_offset.full -= dfixed_const(1);
} else {
h_offset.full = dfixed_floor(win->x) * Bpp;
}
v_offset = win->y;
if (invert_v) {
v_offset.full += win->h.full - dfixed_const(1);
}
tegra_dc_writel(dc, dfixed_trunc(h_offset),
DC_WINBUF_ADDR_H_OFFSET);
tegra_dc_writel(dc, dfixed_trunc(v_offset),
DC_WINBUF_ADDR_V_OFFSET);
if (tegra_dc_feature_has_tiling(dc, win->idx)) {
if (WIN_IS_TILED(win))
tegra_dc_writel(dc,
DC_WIN_BUFFER_ADDR_MODE_TILE |
DC_WIN_BUFFER_ADDR_MODE_TILE_UV,
DC_WIN_BUFFER_ADDR_MODE);
else
tegra_dc_writel(dc,
DC_WIN_BUFFER_ADDR_MODE_LINEAR |
DC_WIN_BUFFER_ADDR_MODE_LINEAR_UV,
DC_WIN_BUFFER_ADDR_MODE);
}
if (yuv)
win_options |= CSC_ENABLE;
else if (tegra_dc_fmt_bpp(win->fmt) < 24)
win_options |= COLOR_EXPAND;
#if defined(CONFIG_ARCH_TEGRA_3x_SOC) || defined(CONFIG_ARCH_TEGRA_11x_SOC)
if (win->global_alpha == 255) {
tegra_dc_writel(dc, 0, DC_WIN_GLOBAL_ALPHA);
} else {
tegra_dc_writel(dc, GLOBAL_ALPHA_ENABLE |
win->global_alpha, DC_WIN_GLOBAL_ALPHA);
win_options |= CP_ENABLE;
}
#endif
if (win->ppflags & TEGRA_WIN_PPFLAG_CP_ENABLE)
win_options |= CP_ENABLE;
win_options |= H_DIRECTION_DECREMENT(invert_h);
win_options |= V_DIRECTION_DECREMENT(invert_v);
tegra_dc_writel(dc, win_options, DC_WIN_WIN_OPTIONS);
dc_win->dirty = no_vsync ? 0 : 1;
trace_window_update(dc, win);
}
if (update_blend_par || update_blend_seq) {
if (update_blend_par)
tegra_dc_blend_parallel(dc, &dc->blend);
if (update_blend_seq)
tegra_dc_blend_sequential(dc, &dc->blend);
for (i = 0; i < DC_N_WINDOWS; i++) {
if (!no_vsync)
dc->windows[i].dirty = 1;
update_mask |= WIN_A_ACT_REQ << i;
}
}
tegra_dc_set_dynamic_emc(windows, n);
tegra_dc_writel(dc, update_mask << 8, DC_CMD_STATE_CONTROL);
tegra_dc_writel(dc, FRAME_END_INT | V_BLANK_INT, DC_CMD_INT_STATUS);
if (!no_vsync) {
set_bit(V_BLANK_FLIP, &dc->vblank_ref_count);
tegra_dc_unmask_interrupt(dc,
FRAME_END_INT | V_BLANK_INT | ALL_UF_INT);
#if !defined(CONFIG_ARCH_TEGRA_2x_SOC) && !defined(CONFIG_ARCH_TEGRA_3x_SOC)
set_bit(V_PULSE2_FLIP, &dc->vpulse2_ref_count);
tegra_dc_unmask_interrupt(dc, V_PULSE2_INT);
#endif
} else {
clear_bit(V_BLANK_FLIP, &dc->vblank_ref_count);
tegra_dc_mask_interrupt(dc, V_BLANK_INT | ALL_UF_INT);
#if !defined(CONFIG_ARCH_TEGRA_2x_SOC) && !defined(CONFIG_ARCH_TEGRA_3x_SOC)
clear_bit(V_PULSE2_FLIP, &dc->vpulse2_ref_count);
tegra_dc_mask_interrupt(dc, V_PULSE2_INT);
#endif
if (!atomic_read(&frame_end_ref))
tegra_dc_mask_interrupt(dc, FRAME_END_INT);
}
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
schedule_delayed_work(&dc->one_shot_work,
msecs_to_jiffies(dc->one_shot_delay_ms));
/* update EMC clock if calculated bandwidth has changed */
tegra_dc_program_bandwidth(dc, false);
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
update_mask |= NC_HOST_TRIG;
tegra_dc_writel(dc, update_mask, DC_CMD_STATE_CONTROL);
tegra_dc_release_dc_out(dc);
/* tegra_dc_io_end() is called in tegra_dc_sync_windows() */
mutex_unlock(&dc->lock);
if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
mutex_unlock(&dc->one_shot_lock);
return 0;
}
Var* ff_warp(vfuncptr func, Var* arg)
{
Var *obj = NULL, *xm = NULL, *oval;
float ignore = FLT_MIN;
int i, j;
float* out;
int x, y, n;
int grow = 0;
float m[9];
float* minverse;
float xmax, xmin, ymax, ymin;
float v[3];
int dsize;
const char* options[] = {"nearest", "bilinear", 0};
char* interp = NULL;
float (*interp_f)(float, float, Var*, float);
Alist alist[6];
alist[0] = make_alist("object", ID_VAL, NULL, &obj);
alist[1] = make_alist("matrix", ID_VAL, NULL, &xm);
alist[2] = make_alist("ignore", DV_FLOAT, NULL, &ignore);
alist[3] = make_alist("grow", DV_INT32, NULL, &grow);
alist[4] = make_alist("interp", ID_ENUM, options, &interp);
alist[5].name = NULL;
if (parse_args(func, arg, alist) == 0) return (NULL);
if (obj == NULL) {
parse_error("%s: No object specified\n", func->name);
return (NULL);
}
if (ignore == FLT_MIN) ignore = -32768;
x = GetX(obj);
y = GetY(obj);
n = V_SIZE(xm)[2];
for (j = 0; j < 3; j++) {
for (i = 0; i < 3; i++) {
m[i + j * 3] = extract_float(xm, cpos(i, j, 0, xm));
}
}
xmin = ymin = 0;
xmax = x;
ymax = y;
if (grow) {
/* figure out the size of the output array */
float* out;
minverse = m_inverse(m);
out = vxm(new_v(0, 0), minverse);
xmin = out[0];
xmax = out[0];
ymin = out[1];
ymax = out[1];
free(out);
out = vxm(new_v(x, 0), minverse);
xmin = min(xmin, out[0]);
xmax = max(xmax, out[0]);
ymin = min(ymin, out[1]);
ymax = max(ymax, out[1]);
free(out);
out = vxm(new_v(0, y), minverse);
xmin = min(xmin, out[0]);
xmax = max(xmax, out[0]);
ymin = min(ymin, out[1]);
ymax = max(ymax, out[1]);
free(out);
out = vxm(new_v(x, y), minverse);
xmin = min(xmin, out[0]);
xmax = max(xmax, out[0]);
ymin = min(ymin, out[1]);
ymax = max(ymax, out[1]);
free(out);
xmax = ceil(xmax);
xmin = floor(xmin);
ymax = ceil(ymax);
ymin = floor(ymin);
printf("new array corners:\n");
printf(" %fx%f , %fx%f\n", xmin, ymin, xmax, ymax);
}
if (interp == NULL || !strcmp(interp, "nearest")) {
interp_f = interp_nn;
} else if (!strcmp(interp, "bilinear")) {
interp_f = interp_bilinear;
} else {
parse_error("Invalid interpolation function\n");
return (NULL);
}
dsize = (xmax - xmin) * (ymax - ymin);
out = calloc(dsize, sizeof(float));
oval = newVal(BSQ, xmax - xmin, ymax - ymin, 1, DV_FLOAT, out);
for (j = ymin; j < ymax; j++) {
for (i = xmin; i < xmax; i++) {
v[0] = i + 0.5;
v[1] = j + 0.5;
v[2] = 1;
vxm(v, m);
out[cpos((int)(i - xmin), (int)(j - ymin), 0, oval)] = interp_f(v[0], v[1], obj, ignore);
}
}
return (oval);
}
Var* linear_interp(Var* v0, Var* v1, Var* v2, float ignore)
{
Var* s = NULL;
float *x = NULL, *y = NULL, *fdata = NULL;
size_t i, count = 0;
float x1, y1, x2, y2, w;
float *m = NULL, *c = NULL; /* slopes and y-intercepts */
size_t fromsz, tosz; /* number of elements in from & to arrays */
fromsz = V_DSIZE(v0);
tosz = V_DSIZE(v2);
x = (float*)calloc(fromsz, sizeof(float));
y = (float*)calloc(fromsz, sizeof(float));
count = 0;
for (i = 0; i < fromsz; i++) {
x[count] = extract_float(v1, i);
y[count] = extract_float(v0, i);
if (is_deleted(x[count]) || is_deleted(y[count]) || x[count] == ignore || y[count] == ignore)
continue;
if (count && x[count] <= x[count - 1]) {
parse_error("Error: data is not monotonically increasing x1[%d] = %f", i, x[count]);
free(fdata);
free(x);
free(y);
return (NULL);
}
count++;
}
fdata = (float*)calloc(tosz, sizeof(float));
m = (float*)calloc(fromsz - 1, sizeof(float));
c = (float*)calloc(fromsz - 1, sizeof(float));
/* evaluate & cache slopes & y-intercepts */
for (i = 1; i < fromsz; i++) {
m[i - 1] = (y[i] - y[i - 1]) / (x[i] - x[i - 1]);
c[i - 1] = y[i - 1] - m[i - 1] * x[i - 1];
}
for (i = 0; i < tosz; i++) {
w = extract_float(v2, i); /* output wavelength */
if (is_deleted(w)) {
fdata[i] = -1.23e34;
} else if (w == ignore) {
fdata[i] = ignore;
} else {
/*
** Locate the segment containing the x-value of "w".
** Assume that x-values are monotonically increasing.
*/
size_t st = 0, ed = fromsz - 1, mid;
while ((ed - st) > 1) {
mid = (st + ed) / 2;
if (w > x[mid]) {
st = mid;
} else if (w < x[mid]) {
ed = mid;
} else {
st = ed = mid;
}
}
x2 = x[ed];
y2 = y[ed];
x1 = x[st];
y1 = y[st];
if (y2 == y1) {
fdata[i] = y1;
} else {
/* m = (y2-y1)/(x2-x1); */
/* fdata[i] = m[st]*w + (y1 - m[st]*x1); */
fdata[i] = m[st] * w + c[st];
}
}
}
s = newVar();
V_TYPE(s) = ID_VAL;
V_DATA(s) = (void*)fdata;
V_DSIZE(s) = V_DSIZE(v2);
V_SIZE(s)[0] = V_SIZE(v2)[0];
V_SIZE(s)[1] = V_SIZE(v2)[1];
V_SIZE(s)[2] = V_SIZE(v2)[2];
V_ORG(s) = V_ORG(v2);
V_FORMAT(s) = DV_FLOAT;
free(x);
free(y);
return (s);
}
Var* cubic_interp(Var* v0, Var* v1, Var* v2, char* type, float ignore)
{
float **yd, *out, *xp, *yp, *arena;
size_t npts, nout;
size_t i, j;
float x0, x1, x, h;
int done;
size_t count = 0;
int error = 0;
npts = V_DSIZE(v0);
nout = V_DSIZE(v2);
/* this is the hard way */
yd = calloc(npts, sizeof(float*));
xp = calloc(npts, sizeof(float));
yp = calloc(npts, sizeof(float));
arena = calloc(npts * 4, sizeof(float));
out = calloc(nout, sizeof(float));
for (i = 0; i < npts; i++) {
xp[count] = extract_float(v1, i);
yp[count] = extract_float(v0, i);
yd[count] = arena + 4 * count;
/* Handle deleted points and non-increasing data */
if (xp[count] == ignore || yp[count] == ignore) {
continue;
}
if (count && xp[count] <= xp[count - 1]) {
parse_error("Error: data is not monotonically increasing x1[%ld] = %f", i, xp[count]);
error = 1;
break;
}
count++;
}
/* this is the case if we're not monotonic increasing */
if (error) {
free(arena);
free(yd);
free(xp);
free(yp);
return (NULL);
}
npts = count;
cakima(npts, xp, yp, yd);
done = i = j = 0;
while (!done) {
if (i >= nout)
break;
else if (j >= npts)
break;
x0 = xp[j];
x1 = xp[j + 1];
x = extract_float(v2, i);
if (x == ignore) {
out[i] = ignore;
i++;
}
if (x < x0)
i++;
else if (x > x1)
j++;
else {
h = x - x0;
out[i] = yd[j][0] + h * (yd[j][1] + h * (yd[j][2] / 2.0 + h * yd[j][3] / 6.0));
i++;
}
}
free(arena);
free(yd);
free(xp);
free(yp);
return (newVal(V_ORG(v2), V_SIZE(v2)[0], V_SIZE(v2)[1], V_SIZE(v2)[2], DV_FLOAT, out));
}
