Add the av1_cost_tokens_from_cdf() function.

Change-Id: I148f8c7045d179c0a1ba7f1fe33b859f66bfc7f3
diff --git a/av1/encoder/cost.c b/av1/encoder/cost.c
index e3151a5..e33df53 100644
--- a/av1/encoder/cost.c
+++ b/av1/encoder/cost.c
@@ -65,3 +65,21 @@
   costs[-tree[0]] = av1_cost_bit(probs[0], 0);
   cost(costs, tree, probs, 2, 0);
 }
+
+void av1_cost_tokens_from_cdf(int *costs, const aom_cdf_prob *cdf,
+                              const int *inv_map) {
+  int i;
+  aom_cdf_prob prev_cdf = 0;
+  for (i = 0;; ++i) {
+    const aom_cdf_prob p15 = AOM_ICDF(cdf[i]) - prev_cdf;
+    prev_cdf = AOM_ICDF(cdf[i]);
+
+    if (inv_map)
+      costs[inv_map[i]] = av1_cost_symbol(p15);
+    else
+      costs[i] = av1_cost_symbol(p15);
+
+    // Stop once we reach the end of the CDF
+    if (cdf[i] == AOM_ICDF(CDF_PROB_TOP)) break;
+  }
+}
diff --git a/av1/encoder/cost.h b/av1/encoder/cost.h
index d8fb357..e606320 100644
--- a/av1/encoder/cost.h
+++ b/av1/encoder/cost.h
@@ -34,6 +34,14 @@
 // for each bit.
 #define av1_cost_literal(n) ((n) * (1 << AV1_PROB_COST_SHIFT))
 
+// Calculate the cost of a symbol with probability p15 / 2^15
+static INLINE int av1_cost_symbol(aom_cdf_prob p15) {
+  assert(0 < p15 && p15 < CDF_PROB_TOP);
+  const int shift = CDF_PROB_BITS - 1 - get_msb(p15);
+  return av1_cost_zero(get_prob(p15 << shift, CDF_PROB_TOP)) +
+         av1_cost_literal(shift);
+}
+
 static INLINE unsigned int cost_branch256(const unsigned int ct[2],
                                           aom_prob p) {
   return ct[0] * av1_cost_zero(p) + ct[1] * av1_cost_one(p);
@@ -55,6 +63,8 @@
 
 void av1_cost_tokens(int *costs, const aom_prob *probs, aom_tree tree);
 void av1_cost_tokens_skip(int *costs, const aom_prob *probs, aom_tree tree);
+void av1_cost_tokens_from_cdf(int *costs, const aom_cdf_prob *cdf,
+                              const int *inv_map);
 
 #ifdef __cplusplus
 }  // extern "C"