[FFmpeg-devel,2/6] opus_rc: add entropy encoding functions

diff --git a/libavcodec/opus_rc.c b/libavcodec/opus_rc.c
index b0e72f6ffe..85268bd284 100644
--- a/libavcodec/opus_rc.c
+++ b/libavcodec/opus_rc.c
@@ -1,7 +1,7 @@ 
 /*
  * Copyright (c) 2012 Andrew D'Addesio
  * Copyright (c) 2013-2014 Mozilla Corporation
- * Copyright (c) 2016 Rostislav Pehlivanov <atomnuker@gmail.com>
+ * Copyright (c) 2017 Rostislav Pehlivanov <atomnuker@gmail.com>
  *
  * This file is part of FFmpeg.
  *
@@ -29,6 +29,21 @@ 
 #define OPUS_RC_BOT (OPUS_RC_TOP >> OPUS_RC_SYM)
 #define OPUS_RC_SHIFT (OPUS_RC_BITS - OPUS_RC_SYM - 1)
 
+static av_always_inline void opus_rc_enc_carryout(OpusRangeCoder *rc, int cbuf)
+{
+    const int cb = cbuf >> OPUS_RC_SYM, mb = (OPUS_RC_CEIL + cb) & OPUS_RC_CEIL;
+    if (cbuf == OPUS_RC_CEIL) {
+        rc->ext++;
+        return;
+    }
+    rc->rng_cur[0] = rc->rem + cb;
+    rc->rng_cur += (rc->rem >= 0);
+    for (; rc->ext > 0; rc->ext--)
+        *rc->rng_cur++ = mb;
+    av_assert0(rc->rng_cur < rc->rb.position);
+    rc->rem = cbuf & OPUS_RC_CEIL; /* Propagate */
+}
+
 static av_always_inline void opus_rc_dec_normalize(OpusRangeCoder *rc)
 {
     while (rc->range <= OPUS_RC_BOT) {
@@ -38,6 +53,16 @@  static av_always_inline void opus_rc_dec_normalize(OpusRangeCoder *rc)
     }
 }
 
+static av_always_inline void opus_rc_enc_normalize(OpusRangeCoder *rc)
+{
+    while (rc->range <= OPUS_RC_BOT) {
+        opus_rc_enc_carryout(rc, rc->value >> OPUS_RC_SHIFT);
+        rc->value = (rc->value << OPUS_RC_SYM) & (OPUS_RC_TOP - 1);
+        rc->range     <<= OPUS_RC_SYM;
+        rc->total_bits += OPUS_RC_SYM;
+    }
+}
+
 static av_always_inline void opus_rc_dec_update(OpusRangeCoder *rc, uint32_t scale,
                                                 uint32_t low, uint32_t high,
                                                 uint32_t total)
@@ -48,6 +73,20 @@  static av_always_inline void opus_rc_dec_update(OpusRangeCoder *rc, uint32_t sca
     opus_rc_dec_normalize(rc);
 }
 
+/* Main encoding function, this needs to go fast */
+static av_always_inline void opus_rc_enc_update(OpusRangeCoder *rc, uint32_t b, uint32_t p,
+                                                uint32_t p_tot, const int ptwo)
+{
+    uint32_t rscaled, cnd = !!b;
+    if (ptwo) /* Whole function is inlined so hopefully branch is optimized out */
+        rscaled = rc->range >> ff_log2(p_tot);
+    else
+        rscaled = rc->range/p_tot;
+    rc->value +=    cnd*(rc->range - rscaled*(p_tot - b));
+    rc->range  = (!cnd)*(rc->range - rscaled*(p_tot - p)) + cnd*rscaled*(p - b);
+    opus_rc_enc_normalize(rc);
+}
+
 uint32_t ff_opus_rc_dec_cdf(OpusRangeCoder *rc, const uint16_t *cdf)
 {
     unsigned int k, scale, total, symbol, low, high;
@@ -67,6 +106,11 @@  uint32_t ff_opus_rc_dec_cdf(OpusRangeCoder *rc, const uint16_t *cdf)
     return k;
 }
 
+void ff_opus_rc_enc_cdf(OpusRangeCoder *rc, int val, const uint16_t *cdf)
+{
+    opus_rc_enc_update(rc, cdf[val], cdf[val + 1], cdf[0], 1);
+}
+
 uint32_t ff_opus_rc_dec_log(OpusRangeCoder *rc, uint32_t bits)
 {
     uint32_t k, scale;
@@ -84,6 +128,12 @@  uint32_t ff_opus_rc_dec_log(OpusRangeCoder *rc, uint32_t bits)
     return k;
 }
 
+void ff_opus_rc_enc_log(OpusRangeCoder *rc, int val, uint32_t bits)
+{
+    bits = (1 << bits) - 1;
+    opus_rc_enc_update(rc, (!!val)*bits, bits + !!val, bits + 1, 1);
+}
+
 /**
  * CELT: read 1-25 raw bits at the end of the frame, backwards byte-wise
  */
@@ -106,6 +156,27 @@  uint32_t ff_opus_rc_get_raw(OpusRangeCoder *rc, uint32_t count)
 }
 
 /**
+ * CELT: write 0 - 31 bits to the rawbits buffer
+ */
+void ff_opus_rc_put_raw(OpusRangeCoder *rc, uint32_t val, uint32_t count)
+{
+    const int to_write = FFMIN(32 - rc->rb.cachelen, count);
+
+    rc->total_bits += count;
+    rc->rb.cacheval |= av_mod_uintp2(val, to_write) << rc->rb.cachelen;
+    rc->rb.cachelen = (rc->rb.cachelen + to_write) % 32;
+
+    if (!rc->rb.cachelen && count) {
+        AV_WB32(rc->rb.position, rc->rb.cacheval);
+        rc->rb.bytes    += 4;
+        rc->rb.position -= 4;
+        rc->rb.cachelen = count - to_write;
+        rc->rb.cacheval = av_mod_uintp2(val >> to_write, rc->rb.cachelen);
+        av_assert0(rc->rng_cur < rc->rb.position);
+    }
+}
+
+/**
  * CELT: read a uniform distribution
  */
 uint32_t ff_opus_rc_dec_uint(OpusRangeCoder *rc, uint32_t size)
@@ -127,6 +198,16 @@  uint32_t ff_opus_rc_dec_uint(OpusRangeCoder *rc, uint32_t size)
         return k;
 }
 
+/**
+ * CELT: write a uniformly distributed integer
+ */
+void ff_opus_rc_enc_uint(OpusRangeCoder *rc, uint32_t val, uint32_t size)
+{
+    const int ps = FFMAX(opus_ilog(size - 1) - 8, 0);
+    opus_rc_enc_update(rc, val >> ps, (val >> ps) + 1, ((size - 1) >> ps) + 1, 0);
+    ff_opus_rc_put_raw(rc, val, ps);
+}
+
 uint32_t ff_opus_rc_dec_uint_step(OpusRangeCoder *rc, int k0)
 {
     /* Use a probability of 3 up to itheta=8192 and then use 1 after */
@@ -142,6 +223,14 @@  uint32_t ff_opus_rc_dec_uint_step(OpusRangeCoder *rc, int k0)
     return k;
 }
 
+void ff_opus_rc_enc_uint_step(OpusRangeCoder *rc, uint32_t val, int k0)
+{
+    const uint32_t a = val <= k0, b = 2*a + 1;
+    k0 = (k0 + 1) << 1;
+    val = b*(val + k0) - 3*a*k0;
+    opus_rc_enc_update(rc, val, val + b, (k0 << 1) - 1, 0);
+}
+
 uint32_t ff_opus_rc_dec_uint_tri(OpusRangeCoder *rc, int qn)
 {
     uint32_t k, scale, symbol, total, low, center;
@@ -166,6 +255,23 @@  uint32_t ff_opus_rc_dec_uint_tri(OpusRangeCoder *rc, int qn)
     return k;
 }
 
+void ff_opus_rc_enc_uint_tri(OpusRangeCoder *rc, uint32_t k, int qn)
+{
+    uint32_t symbol, low, total;
+
+    total = ((qn>>1) + 1) * ((qn>>1) + 1);
+
+    if (k <= qn >> 1) {
+        low    = k * (k + 1) >> 1;
+        symbol = k + 1;
+    } else {
+        low    = total - ((qn + 1 - k) * (qn + 2 - k) >> 1);
+        symbol = qn + 1 - k;
+    }
+
+    opus_rc_enc_update(rc, low, low + symbol, total, 0);
+}
+
 int ff_opus_rc_dec_laplace(OpusRangeCoder *rc, uint32_t symbol, int decay)
 {
     /* extends the range coder to model a Laplace distribution */
@@ -205,6 +311,30 @@  int ff_opus_rc_dec_laplace(OpusRangeCoder *rc, uint32_t symbol, int decay)
     return value;
 }
 
+void ff_opus_rc_enc_laplace(OpusRangeCoder *rc, int *value, uint32_t symbol, int decay)
+{
+    uint32_t low = symbol;
+    int i = 1, val = FFABS(*value), pos = *value > 0;
+    if (!val) {
+        opus_rc_enc_update(rc, 0, symbol, 1 << 15, 1);
+        return;
+    }
+    symbol = ((32768 - 32 - symbol)*(16384 - decay)) >> 15;
+    for (; i < val && symbol; i++) {
+        low   += (symbol << 1) + 2;
+        symbol = (symbol*decay) >> 14;
+    }
+    if (symbol) {
+        low += (++symbol)*pos;
+    } else {
+        const int distance = FFMIN(val - i, (((32768 - low) - !pos) >> 1) - 1);
+        low   += pos + (distance << 1);
+        symbol = FFMIN(1, 32768 - low);
+        *value = FFSIGN(*value)*(distance + i);
+    }
+    opus_rc_enc_update(rc, low, low + symbol, 1 << 15, 1);
+}
+
 int ff_opus_rc_dec_init(OpusRangeCoder *rc, const uint8_t *data, int size)
 {
     int ret = init_get_bits8(&rc->gb, data, size);
@@ -226,3 +356,53 @@  void ff_opus_rc_dec_raw_init(OpusRangeCoder *rc, const uint8_t *rightend, uint32
     rc->rb.cachelen = 0;
     rc->rb.cacheval = 0;
 }
+
+void ff_opus_rc_enc_end(OpusRangeCoder *rc, uint8_t *dst, int size)
+{
+    int rng_bytes, bits = OPUS_RC_BITS - opus_ilog(rc->range);
+    uint32_t mask = (OPUS_RC_TOP - 1) >> bits;
+    uint32_t end = (rc->value + mask) & ~mask;
+
+    if ((end | mask) >= rc->value + rc->range) {
+        bits++;
+        mask >>= 1;
+        end = (rc->value + mask) & ~mask;
+    }
+
+    /* Finish what's left */
+    while (bits > 0) {
+        opus_rc_enc_carryout(rc, end >> OPUS_RC_SHIFT);
+        end = (end << OPUS_RC_SYM) & (OPUS_RC_TOP - 1);
+        bits -= OPUS_RC_SYM;
+    }
+
+    /* Flush out anything left or marked */
+    if (rc->rem >= 0 || rc->ext > 0)
+        opus_rc_enc_carryout(rc, 0);
+
+    rng_bytes = rc->rng_cur - rc->buf;
+    rc->waste = (size - (rc->rb.bytes + rng_bytes)) << 3;
+    memcpy(dst, rc->buf, rng_bytes);
+    memset(dst + rng_bytes, 0, FFMAX(rc->waste >> 3, 0) + 1);
+
+    /* Put the rawbits part, if any */
+    if (rc->rb.bytes || rc->rb.cachelen) {
+        int rawbytes = FFALIGN(rc->rb.bytes*8 + rc->rb.cachelen, 8) >> 3;
+        int dst_loc = FFMAX(size - rawbytes, 0);
+        uint8_t *src = rc->buf + OPUS_MAX_PACKET_SIZE + 12 - rawbytes;
+        ff_opus_rc_put_raw(rc, 0, 32 - rc->rb.cachelen);
+        dst[dst_loc] |= *src++;
+        memcpy(&dst[dst_loc + 1], src, rawbytes - 1);
+    }
+}
+
+void ff_opus_rc_enc_init(OpusRangeCoder *rc)
+{
+    rc->value = 0;
+    rc->range = OPUS_RC_TOP;
+    rc->total_bits = OPUS_RC_BITS + 1;
+    rc->rem = -1;
+    rc->ext =  0;
+    rc->rng_cur = rc->buf;
+    ff_opus_rc_dec_raw_init(rc, rc->buf + OPUS_MAX_PACKET_SIZE + 8, 0);
+}
diff --git a/libavcodec/opus_rc.h b/libavcodec/opus_rc.h
index 9f5253b51d..821eeadd6e 100644
--- a/libavcodec/opus_rc.h
+++ b/libavcodec/opus_rc.h
@@ -1,7 +1,7 @@ 
 /*
  * Copyright (c) 2012 Andrew D'Addesio
  * Copyright (c) 2013-2014 Mozilla Corporation
- * Copyright (c) 2016 Rostislav Pehlivanov <atomnuker@gmail.com>
+ * Copyright (c) 2017 Rostislav Pehlivanov <atomnuker@gmail.com>
  *
  * This file is part of FFmpeg.
  *
@@ -26,6 +26,8 @@ 
 #include <stdint.h>
 #include "get_bits.h"
 
+#define OPUS_MAX_PACKET_SIZE 1275
+
 #define opus_ilog(i) (av_log2(i) + !!(i))
 
 typedef struct RawBitsContext {
@@ -41,6 +43,15 @@  typedef struct OpusRangeCoder {
     uint32_t range;
     uint32_t value;
     uint32_t total_bits;
+
+    /* Encoder */
+    uint8_t buf[OPUS_MAX_PACKET_SIZE + 12]; /* memcpy vs (memmove + overreading) */
+    uint8_t *rng_cur;                      /* Current range coded byte */
+    int ext;                               /* Awaiting propagation */
+    int rem;                               /* Carryout flag */
+
+    /* Encoding stats */
+    int waste;
 } OpusRangeCoder;
 
 /**
@@ -72,14 +83,30 @@  static av_always_inline uint32_t opus_rc_tell_frac(const OpusRangeCoder *rc)
 }
 
 uint32_t ff_opus_rc_dec_cdf(OpusRangeCoder *rc, const uint16_t *cdf);
+void     ff_opus_rc_enc_cdf(OpusRangeCoder *rc, int val, const uint16_t *cdf);
+
 uint32_t ff_opus_rc_dec_log(OpusRangeCoder *rc, uint32_t bits);
-uint32_t ff_opus_rc_dec_uint(OpusRangeCoder *rc, uint32_t size);
+void     ff_opus_rc_enc_log(OpusRangeCoder *rc, int val, uint32_t bits);
+
 uint32_t ff_opus_rc_dec_uint_step(OpusRangeCoder *rc, int k0);
+void     ff_opus_rc_enc_uint_step(OpusRangeCoder *rc, uint32_t val, int k0);
+
 uint32_t ff_opus_rc_dec_uint_tri(OpusRangeCoder *rc, int qn);
+void     ff_opus_rc_enc_uint_tri(OpusRangeCoder *rc, uint32_t k, int qn);
+
+uint32_t ff_opus_rc_dec_uint(OpusRangeCoder *rc, uint32_t size);
+void     ff_opus_rc_enc_uint(OpusRangeCoder *rc, uint32_t val, uint32_t size);
+
 uint32_t ff_opus_rc_get_raw(OpusRangeCoder *rc, uint32_t count);
+void     ff_opus_rc_put_raw(OpusRangeCoder *rc, uint32_t val, uint32_t count);
+
 int      ff_opus_rc_dec_laplace(OpusRangeCoder *rc, uint32_t symbol, int decay);
+void     ff_opus_rc_enc_laplace(OpusRangeCoder *rc, int *value, uint32_t symbol, int decay);
+
+int      ff_opus_rc_dec_init(OpusRangeCoder *rc, const uint8_t *data, int size);
+void     ff_opus_rc_dec_raw_init(OpusRangeCoder *rc, const uint8_t *rightend, uint32_t bytes);
 
-int  ff_opus_rc_dec_init(OpusRangeCoder *rc, const uint8_t *data, int size);
-void ff_opus_rc_dec_raw_init(OpusRangeCoder *rc, const uint8_t *rightend, uint32_t bytes);
+void     ff_opus_rc_enc_end(OpusRangeCoder *rc, uint8_t *dst, int size);
+void     ff_opus_rc_enc_init(OpusRangeCoder *rc);
 
 #endif /* AVCODEC_OPUS_RC_H */