Enable 4-channel input

This requires adding channel remap functionality to input/output streams.
Number of channels provided by hardware might be different from what framework expects.
For example in case of Pixel C there is I2S bus to rt5677 codec shared by both capture and
playback. If we want to record 4-channel input from mics and playback stereo at the same time
then HW format has to be 4-channel (I2S cannot have different formats for capture/playback).

In such case we need to adjust number of audio channels.

Change-Id: I6917423379999bb98841db9e42373136c747a82a
diff --git a/audio/hal/audio_hw.c b/audio/hal/audio_hw.c
index 83a623d..13bad7c 100644
--- a/audio/hal/audio_hw.c
+++ b/audio/hal/audio_hw.c
@@ -43,6 +43,7 @@
 #include <system/thread_defs.h>
 #include <audio_effects/effect_aec.h>
 #include <audio_effects/effect_ns.h>
+#include <audio_utils/channels.h>
 #include "audio_hw.h"
 #include "cras_dsp.h"
 
@@ -866,80 +867,45 @@
  * - process if pre-processors are attached
  * - discard unwanted channels
  */
-static ssize_t read_and_process_frames(struct stream_in *in, void* buffer, ssize_t frames)
+static ssize_t read_and_process_frames(struct audio_stream_in *stream, void* buffer, ssize_t frames_num)
 {
-    ssize_t frames_wr = 0;
-    audio_buffer_t in_buf;
-    audio_buffer_t out_buf;
-    size_t src_channels = in->config.channels;
-    size_t dst_channels = audio_channel_count_from_in_mask(in->main_channels);
-    int i;
-    void *proc_buf_out;
-    struct pcm_device *pcm_device;
-    bool has_additional_channels = (dst_channels != src_channels) ? true : false;
+    struct stream_in *in = (struct stream_in *)stream;
+    ssize_t frames_rd = 0; /* Number of frames actually read */
+    size_t bytes_per_sample = audio_bytes_per_sample(stream->common.get_format(&stream->common));
+    void *proc_buf_in = buffer;
 
     /* Additional channels might be added on top of main_channels:
     * - aux_channels (by processing effects)
     * - extra channels due to HW limitations
     * In case of additional channels, we cannot work inplace
     */
-    if (has_additional_channels)
-        proc_buf_out = in->proc_buf_out;
-    else
-        proc_buf_out = buffer;
+    size_t src_channels = in->config.channels;
+    size_t dst_channels = audio_channel_count_from_in_mask(in->main_channels);
+    bool channel_remapping_needed = (dst_channels != src_channels);
 
-    if (list_empty(&in->pcm_dev_list)) {
-        ALOGE("%s: pcm device list empty", __func__);
-        return -EINVAL;
-    }
+    /* No processing effects attached */
+    if (channel_remapping_needed) {
+        size_t src_buffer_size = frames_num * src_channels * bytes_per_sample;
 
-    pcm_device = node_to_item(list_head(&in->pcm_dev_list),
-                              struct pcm_device, stream_list_node);
-
-    {
-        /* No processing effects attached */
-        if (has_additional_channels) {
-            /* With additional channels, we cannot use original buffer */
-            if (in->proc_buf_size < (size_t)frames) {
-                size_t size_in_bytes = pcm_frames_to_bytes(pcm_device->pcm, frames);
-                in->proc_buf_size = (size_t)frames;
-                in->proc_buf_out = (int16_t *)realloc(in->proc_buf_out, size_in_bytes);
-                ALOG_ASSERT((in->proc_buf_out != NULL),
-                            "process_frames() failed to reallocate proc_buf_out");
-                proc_buf_out = in->proc_buf_out;
-            }
+        /* With additional channels, we cannot use original buffer */
+        if (in->proc_buf_size < src_buffer_size) {
+            in->proc_buf_size = src_buffer_size;
+            in->proc_buf_in = realloc(in->proc_buf_in, src_buffer_size);
+            ALOG_ASSERT((in->proc_buf_in != NULL),
+                        "process_frames() failed to reallocate proc_buf_in");
         }
-        frames_wr = read_frames(in, proc_buf_out, frames);
+        proc_buf_in = in->proc_buf_in;
+    }
+    frames_rd = read_frames(in, proc_buf_in, frames_num);
+    ALOG_ASSERT(frames_rd <= frames_num, "read more frames than requested");
+
+    if (channel_remapping_needed) {
+        size_t ret = adjust_channels(proc_buf_in, src_channels, buffer, dst_channels,
+            bytes_per_sample, frames_rd * src_channels * bytes_per_sample);
+        ALOG_ASSERT(ret == (frames_rd * dst_channels * bytes_per_sample));
     }
 
-    /* Remove all additional channels that have been added on top of main_channels:
-     * - aux_channels
-     * - extra channels from HW due to HW limitations
-     * Assumption is made that the channels are interleaved and that the main
-     * channels are first. */
-
-    if (has_additional_channels)
-    {
-        int16_t* src_buffer = (int16_t *)proc_buf_out;
-        int16_t* dst_buffer = (int16_t *)buffer;
-
-        if (dst_channels == 1) {
-            for (i = frames_wr; i > 0; i--)
-            {
-                *dst_buffer++ = *src_buffer;
-                src_buffer += src_channels;
-            }
-        } else {
-            for (i = frames_wr; i > 0; i--)
-            {
-                memcpy(dst_buffer, src_buffer, dst_channels*sizeof(int16_t));
-                dst_buffer += dst_channels;
-                src_buffer += src_channels;
-            }
-        }
-    }
-
-    return frames_wr;
+    return frames_rd;
 }
 
 static int get_next_buffer(struct resampler_buffer_provider *buffer_provider,
@@ -1219,7 +1185,6 @@
 
     /* force read and proc buffer reallocation in case of frame size or
      * channel count change */
-    in->proc_buf_frames = 0;
     in->proc_buf_size = 0;
     in->read_buf_size = 0;
     in->read_buf_frames = 0;
@@ -1533,7 +1498,7 @@
 {
     if (format != AUDIO_FORMAT_PCM_16_BIT) return -EINVAL;
 
-    if ((channel_count < 1) || (channel_count > 2)) return -EINVAL;
+    if ((channel_count < 1) || (channel_count > 4)) return -EINVAL;
 
     switch (sample_rate) {
     case 8000:
@@ -1859,17 +1824,20 @@
             }
             frames_rq = bytes / frame_size;
             frames_wr = pcm_device->res_byte_count / frame_size;
-            ALOGVV("%s: resampler request frames = %d frame_size = %d",
+            ALOGVV("%s: resampler request frames = %zu frame_size = %zu",
                 __func__, frames_rq, frame_size);
             pcm_device->resampler->resample_from_input(pcm_device->resampler,
                 (int16_t *)buffer, &frames_rq, (int16_t *)pcm_device->res_buffer, &frames_wr);
-            ALOGVV("%s: resampler output frames_= %d", __func__, frames_wr);
+            ALOGVV("%s: resampler output frames_= %zu", __func__, frames_wr);
         }
         if (pcm_device->pcm) {
+            size_t src_channels = audio_channel_count_from_out_mask(out->channel_mask);
+            size_t dst_channels = pcm_device->pcm_profile->config.channels;
+            bool channel_remapping_needed = (dst_channels != src_channels);
             unsigned audio_bytes;
             const void *audio_data;
 
-            ALOGVV("%s: writing buffer (%d bytes) to pcm device", __func__, bytes);
+            ALOGVV("%s: writing buffer (%zd bytes) to pcm device", __func__, bytes);
             if (pcm_device->resampler && pcm_device->res_buffer) {
                 audio_data = pcm_device->res_buffer;
                 audio_bytes = frames_wr * frame_size;
@@ -1884,13 +1852,33 @@
              */
             apply_dsp(pcm_device, audio_data, audio_bytes/4);
 
+            if (channel_remapping_needed) {
+                const void *remapped_audio_data;
+                size_t dest_buffer_size = audio_bytes * dst_channels / src_channels;
+                size_t new_size;
+                size_t bytes_per_sample = audio_bytes_per_sample(stream->common.get_format(&stream->common));
+
+                /* With additional channels, we cannot use original buffer */
+                if (out->proc_buf_size < dest_buffer_size) {
+                    out->proc_buf_size = dest_buffer_size;
+                    out->proc_buf_out = realloc(out->proc_buf_out, dest_buffer_size);
+                    ALOG_ASSERT((out->proc_buf_out != NULL),
+                                "out_write() failed to reallocate proc_buf_out");
+                }
+                new_size = adjust_channels(audio_data, src_channels, out->proc_buf_out, dst_channels,
+                    bytes_per_sample, audio_bytes);
+                ALOG_ASSERT(new_size == dest_buffer_size);
+                audio_data = out->proc_buf_out;
+                audio_bytes = dest_buffer_size;
+            }
+
             pcm_device->status = pcm_write(pcm_device->pcm, audio_data, audio_bytes);
             if (pcm_device->status != 0)
                 ret = pcm_device->status;
         }
     }
     if (ret == 0)
-        out->written += bytes / (out->config.channels * sizeof(short));
+        out->written += bytes / frame_size;
 
 exit:
     pthread_mutex_unlock(&out->lock);
@@ -2248,7 +2236,7 @@
              * - process if pre-processors are attached
              * - discard unwanted channels
              */
-            frames = read_and_process_frames(in, buffer, frames_rq);
+            frames = read_and_process_frames(stream, buffer, frames_rq);
             if (frames >= 0)
                 read_and_process_successful = true;
         }
@@ -2401,6 +2389,7 @@
     out_standby(&stream->common);
     pthread_cond_destroy(&out->cond);
     pthread_mutex_destroy(&out->lock);
+    free(out->proc_buf_out);
     free(stream);
     ALOGV("%s: exit", __func__);
 }
@@ -2696,6 +2685,7 @@
     pthread_mutex_lock(&adev->lock_inputs);
 
     in_standby_l(in);
+    free(in->proc_buf_in);
     free(stream);
 
     pthread_mutex_unlock(&adev->lock_inputs);
diff --git a/audio/hal/audio_hw.h b/audio/hal/audio_hw.h
index 92aff51..4aa361e 100644
--- a/audio/hal/audio_hw.h
+++ b/audio/hal/audio_hw.h
@@ -109,7 +109,7 @@
  */
 #define PLAYBACK_PERIOD_SIZE 512
 #define PLAYBACK_PERIOD_COUNT 2
-#define PLAYBACK_DEFAULT_CHANNEL_COUNT 2
+#define PLAYBACK_DEFAULT_CHANNEL_COUNT 4
 #define PLAYBACK_DEFAULT_SAMPLING_RATE 48000
 #define PLAYBACK_START_THRESHOLD ((PLAYBACK_PERIOD_SIZE * PLAYBACK_PERIOD_COUNT) - 1)
 #define PLAYBACK_STOP_THRESHOLD (PLAYBACK_PERIOD_SIZE * PLAYBACK_PERIOD_COUNT)
@@ -129,7 +129,7 @@
 #define CAPTURE_PERIOD_SIZE 1024
 #define CAPTURE_PERIOD_SIZE_LOW_LATENCY 512
 #define CAPTURE_PERIOD_COUNT 2
-#define CAPTURE_DEFAULT_CHANNEL_COUNT 2
+#define CAPTURE_DEFAULT_CHANNEL_COUNT 4
 #define CAPTURE_DEFAULT_SAMPLING_RATE 48000
 #define CAPTURE_START_THRESHOLD 1
 
@@ -229,7 +229,8 @@
     int                         send_new_metadata;
 
     struct audio_device*        dev;
-
+    void *proc_buf_out;
+    size_t proc_buf_size;
 };
 
 struct stream_in {
@@ -257,10 +258,8 @@
     size_t                              read_buf_size;
     size_t                              read_buf_frames;
 
-    int16_t *proc_buf_in;
-    int16_t *proc_buf_out;
+    void *proc_buf_in;
     size_t proc_buf_size;
-    size_t proc_buf_frames;
 
     struct audio_device*                dev;
 };