union weight {
struct {
int16_t diff;
uint16_t fract;
} val;
uint32_t raw;
};

struct scale {

/* Main
* */
int src_width;
int src_height;
int dst_width;
int dst_height;
void *src;
void *dst;
union weight *weights;

/* Blocks
* */
int src_blk_w;
int src_blk_h;
int dst_blk_w;
int dst_blk_h;
int dst_blksz_nb;

/* Helper
* */
int pt_x;
int pt_y;
int scan_w;
int scan_h;
union weight *wp;
};

#define FX_ZERO 0x00000000
#define FX_ONE 0x00010000
#define FX_ONEW 0x0000ffff
#define FX_HALF 0x00008000

#define FX_ILD(i) (((int32_t)(i)) << 16)
#define FX_FLD(i) ((int32_t)(i))
#define FX_STI(fx) ((int)((fx) >> 16))
#define FX_STF(fx) ((uint16_t)(fx & 0x0000ffff))
#define FX_FLOOR(fx) ((int32_t)((fx) & 0xffff0000))
#define FX_FRACT(fx) ((int32_t)((fx) & 0x0000ffff))
#define FX_CEILI(fx) ((int)(((fx) + FX_ONEW) >> 16))
#define FX_MUL(v,m) ((int32_t)(((int64_t)(v) * (int64_t)(m)) >> 16))
#define FX_MUL32(v,m) ((int32_t)(((int32_t)(v) * (int32_t)(m)) >> 16))
#define FX_DIV(n,d) ((int32_t)(((int64_t)(n) << 16) / ((int64_t)(d))))

static inline uint16_t
rgb_pack(int *rgb)
{
uint16_t col;

col = (rgb[0] & 0xf8) << 8;
col |= (rgb[1] & 0xfc) << 3;
col |= (rgb[2] & 0xf8) >> 3;

return col;
}

static inline void
rgb_unpack(uint16_t col, int *rgb)
{
rgb[0] = (col & 0xf800) >> 8;
rgb[1] = (col & 0x07e0) >> 3;
rgb[2] = (col & 0x001f) << 3;
}

static inline void
rgb_zero(int *dst)
{
dst[0] = 0;
dst[1] = 0;
dst[2] = 0;
}

static inline void
rgb_unpack_add_w(uint16_t col, int *rgb, int32_t w)
{
rgb[0] += FX_MUL32((col & 0xf800) >> 8, w);
rgb[1] += FX_MUL32((col & 0x07e0) >> 3, w);
rgb[2] += FX_MUL32((col & 0x001f) << 3, w);
}

static void
fb_calc_weights_for_pixel(struct scale *sc, int x, int y)
{
int32_t us, vs;
int32_t ue, ve;
int32_t ps, qs;
int32_t pe, qe;
int32_t iw, jw, fw;
int32_t wsum, wtmp;
int dx, dy;
int i, j;
int ilim, jlim;
union weight *wbeg;

us = FX_MUL(
FX_DIV(
FX_ILD(sc->src_width),
FX_ILD(sc->dst_width)),
FX_ILD(x));

vs = FX_MUL(
FX_DIV(
FX_ILD(sc->src_height),
FX_ILD(sc->dst_height)),
FX_ILD(y));

ue = FX_MUL(
FX_DIV(
FX_ILD(sc->src_width),
FX_ILD(sc->dst_width)),
FX_ILD(x + 1));

ve = FX_MUL(
FX_DIV(
FX_ILD(sc->src_height),
FX_ILD(sc->dst_height)),
FX_ILD(y + 1));

i = FX_STI(us);
j = FX_STI(vs);

ilim = i + sc->scan_w;
jlim = j + sc->scan_h;

wsum = 0;
wbeg = sc->wp;

for (; j < jlim; ++j)
for (i = FX_STI(us); i < ilim; ++i) {

ps = FX_ILD(i);
qs = FX_ILD(j);

pe = FX_ILD(i + 1);
qe = FX_ILD(j + 1);

/* Intersection pixels on axis X
* */
if (ps > ue)
iw = FX_ZERO;
else if (ps < us)
iw = FX_ONE - (us - ps);
else if (ue < pe)
iw = FX_ONE - (pe - ue);
else
iw = FX_ONEW;

/* Intersection pixels on axis Y
* */
if (qs > ve)
jw = FX_ZERO;
else if (qs < vs)
jw = FX_ONE - (vs - qs);
else if (ve < qe)
jw = FX_ONE - (qe - ve);
else
jw = FX_ONEW;

/* This case give more blured result
* */
//fw = (iw + jw) / 2;

/* Calculate weight as square
* */
fw = FX_MUL(iw, jw);

/* Add if it have influence only
* */
if (fw > FX_ZERO) {

wsum += fw;

dx = i - sc->pt_x;
dy = j - sc->pt_y;

sc->wp->val.diff = sc->src_width * dy + dx;

sc->pt_x = i;
sc->pt_y = j;

sc->wp->val.fract = FX_STF(fw);
sc->wp++;
}
}

/* Add end-pixel marker
* */
sc->wp->raw = 0;
sc->wp++;

/* Normalize weights
* */
for (; wbeg->raw; ++wbeg) {
wtmp = FX_DIV(
FX_FLD(wbeg->val.fract),
wsum);
if (wtmp < FX_ONE)
wbeg->val.fract = FX_STF(wtmp);
else
wbeg->val.fract = FX_ONEW;
}
}

static void
fb_scale_open(struct scale *sc)
{
int prime[] = {
2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31,
37, 41, 43, 47, 53, 59, 61, 67, 71, 73,
79, 83, 89, 97, 101
};

int x, y, i;
int mdw, mdh;
int bsz;
int yet;

/* Calculate scan area size
* */
sc->scan_w = FX_CEILI(FX_DIV(
FX_ILD(sc->src_width),
FX_ILD(sc->dst_width))) + 1;

sc->scan_h = FX_CEILI(FX_DIV(
FX_ILD(sc->src_height),
FX_ILD(sc->dst_height))) + 1;

/* Block fraction
* */
mdw = 1;
mdh = 1;

sc->src_blk_w = sc->src_width / mdw;
sc->src_blk_h = sc->src_height / mdh;
sc->dst_blk_w = sc->dst_width / mdw;
sc->dst_blk_h = sc->dst_height / mdh;

/* Minimize block size on width
* */
do {
yet = 0;

for (i = 0; i < 26; ++i) {
if (
!(sc->src_blk_w % prime[i]) &&
!(sc->dst_blk_w % prime[i])) {
mdw *= prime[i];
sc->src_blk_w = sc->src_width / mdw;
sc->dst_blk_w = sc->dst_width / mdw;
yet = 1;
break;
}
}
}
while (yet);

/* Minimize block size on height
* */
do {
yet = 0;

for (i = 0; i < 26; ++i) {
if (
!(sc->src_blk_h % prime[i]) &&
!(sc->dst_blk_h % prime[i])) {
mdh *= prime[i];
sc->src_blk_h = sc->src_height / mdh;
sc->dst_blk_h = sc->dst_height / mdh;
yet = 1;
break;
}
}
}
while (yet);

/* Near boundary for block size
* */
if (sc->dst_blksz_nb) {

bsz = sc->dst_blk_w * sc->dst_blk_h;

while (sc->dst_blksz_nb < bsz) {

if (sc->dst_blk_w < sc->dst_blk_h)
sc->dst_blk_w *= 2;
else
sc->dst_blk_h *= 2;

bsz = sc->dst_blk_w * sc->dst_blk_h;
}
}

/* Weights allocation
* */
sc->weights = malloc(
sc->dst_blk_h *
sc->dst_blk_w *
sizeof(union weight) *
(sc->scan_w * sc->scan_h + 1));

/* Calculate weights
* */
sc->pt_x = 0;
sc->pt_y = 0;
sc->wp = sc->weights;

for (y = 0; y < sc->dst_blk_h; ++y) {
for (x = 0; x < sc->dst_blk_w; ++x)
fb_calc_weights_for_pixel(sc, x, y);
}
}

static void
fb_scale_close(struct scale *sc)
{
free(sc->weights);
}

static void
fb_scale_block_r5g6b5(
struct scale *sc,
const uint16_t *src,
uint16_t *dst)
{
uint16_t *fin;
int c16[3];
int y, dw, adw, dh;
union weight *wp, w;

dw = sc->dst_blk_w;
dh = sc->dst_blk_h;
wp = sc->weights;

adw = sc->dst_width - dw;

/* For all lines in this block
* */
for (y = 0; y < dh; ++y) {

fin = dst + dw;

/* For all pixels on line
* */
do {
/* c = 0
* */
rgb_zero(c16);

/* Calculate color, c = c1*w1 + c2*w2 + ... + cn*wn
* */
do {
/* If is not end-pixel marker
* */
if ((w = *wp++).raw) {

/* Go to new position in source
* */
src += w.val.diff;

/* Read color, c = c + ci*wi
* */
rgb_unpack_add_w(*src, c16,
FX_FLD(w.val.fract));
}
else
break;
}
while (1);

/* Write result
* */
*dst++ = rgb_pack(c16);
}
while (dst != fin);

/* Go to next line
* */
dst += adw;
}
}

static void
fb_scale_r5g6b5(struct scale *sc)
{
int y, lines;
int inc_d, inc_s;
int dw, bdw_d, bdw_s;
uint16_t *src;
uint16_t *dst, *fin;

dw = sc->dst_width;
bdw_s = sc->src_blk_w;
bdw_d = sc->dst_blk_w;
inc_s = sc->src_width * (sc->src_blk_h - 1);
inc_d = sc->dst_width * (sc->dst_blk_h - 1);
lines = sc->dst_height / sc->dst_blk_h;

src = sc->src;
dst = sc->dst;

/* For all lines of blocks
* */
for (y = 0; y < lines; ++y) {

fin = dst + dw;

/* For all block on line
* */
while (dst != fin) {

/* Scale block
* */
fb_scale_block_r5g6b5(sc, src, dst);

/* Go to next block in this line
* */
src += bdw_s;
dst += bdw_d;
}

src += inc_s;
dst += inc_d;
}
}