Add F32/I16x2 conversions to support TDM

Some dialogue on the Teensy forum raised the issue of
support for 24- or 32-bit output; for example see
https://forum.pjrc.com/index.php?threads/updated-8x8-and-16x16-audio.75569/post-357275
Obviously not supported by the 16-bit Audio library,
but partially supported in this library for I²S and S/PDIF.

Since the TDM support usually emits 32-bit samples, forcing use
of even-numbered ports only, there is an opportunity
to adopt the I/O objects directly by converting two
16-bit TDM port values (containing high and low words) to
and from a single F32 stream, giving 24-bit I/O.

This commit implements the required objects, plus a
minor change to AudioStream_F32 needed for the
AudioConvert_I16x2toF32 class.

AudioConvert_F32.h

@@ -46,6 +46,70 @@ class AudioConvert_I16toF32 : public AudioStream_F32 //receive Int and transmits
 
 };
 
+class AudioConvert_I16x2toF32 : public AudioStream_F32 //receive Int and transmits Float
+{
+  //GUI: inputs:2, outputs:1  //this line used for automatic generation of GUI node    
+    audio_block_t *inputQueueArray[2]; 
+
+  public:
+    AudioConvert_I16x2toF32(void) 
+      : AudioStream_F32(0, nullptr, 2, inputQueueArray) { };
+    AudioConvert_I16x2toF32(const AudioSettings_F32 &settings) 
+      : AudioStream_F32(0, nullptr, 2, inputQueueArray) { };
+	
+    void update(void) {	
+      //get the Int16 blocks
+      audio_block_t *int_blockH, *int_blockL;
+      rxInt16block(int_blockH);
+      rxInt16block(int_blockL, 1);
+
+      //allocate a float block
+      audio_block_f32_t *float_block = AudioStream_F32::allocate_f32(); 
+
+      // process, as long as we have all blocks
+      if (nullptr != int_blockH && nullptr != int_blockH && nullptr != float_block) 
+      {
+        //convert to float
+        convertAudio_I16x2toF32(int_blockH, int_blockL, float_block, float_block->length);
+
+        //transmit the audio and return it to the system
+        AudioStream_F32::transmit(float_block,0);
+      }
+      if( nullptr != float_block) AudioStream_F32::release(float_block);
+      releaseInt16block(int_blockH);
+      releaseInt16block(int_blockL);
+    };
+    
+    static void convertAudio_I16x2toF32(audio_block_t *inH, audio_block_t *inL, audio_block_f32_t *out, int len) 
+    {
+      const float MAX_INT = 32768.0f*65536.0f;
+      for (int i = 0; i < len; i++)
+      {
+        // reassemble a 32-bit signed value from two 16-bit values
+        int32_t sample = ((int32_t) inH->data[i] << 16) | (((int32_t) inL->data[i]) & 0xFFFF);
+        out->data[i] = (float)(sample);
+      }
+      arm_scale_f32(out->data, 1.0/MAX_INT, out->data, out->length); //divide by 32678*64k to get -1.0 to +1.0
+    }
+    
+    // Receive an I16 block, or create a silent one if it's NULL
+    void rxInt16block(audio_block_t*& blk, unsigned int index = 0)
+    {
+      blk = AudioStream::receiveReadOnly(index);
+      if (nullptr == blk)
+      {
+        blk = AudioStream::allocate();
+        if (nullptr != blk) 
+          memset(blk->data, 0, sizeof blk->data);
+      }
+    }
+
+    static void releaseInt16block(audio_block_t*& blk)
+    {
+      if (nullptr != blk )
+        AudioStream::release(blk);
+    }
+};
 
 class AudioConvert_F32toI16 : public AudioStream_F32 //receive Float and transmits Int
 {
@@ -88,4 +152,57 @@ class AudioConvert_F32toI16 : public AudioStream_F32 //receive Float and transmi
  
 };
 
+class AudioConvert_F32toI16x2 : public AudioStream_F32 //receive Float and transmits Int
+{
+  //GUI: inputs:1, outputs:2  //this line used for automatic generation of GUI node
+    audio_block_f32_t *inputQueueArray_Float[1]; 
+  public:
+    AudioConvert_F32toI16x2(void) : AudioStream_F32(1, inputQueueArray_Float) {};
+    void update(void) {
+      //get the float block
+      audio_block_f32_t *float_block;
+      float_block = AudioStream_F32::receiveReadOnly_f32(); //float data block
+      if (!float_block) return;
+
+      //allocate a Int16 block
+      audio_block_t *int_blockH, *int_blockL;
+      int_blockH = AudioStream::allocate(); 
+      if (int_blockH == NULL) 
+      {
+      	  AudioStream_F32::release(float_block);
+      	  return;
+      }
+      else
+      {
+        int_blockL = AudioStream::allocate(); 
+        if (int_blockL == NULL) 
+        {
+          AudioStream::release(int_blockH);
+          AudioStream_F32::release(float_block);
+          return;
+        }
+      }
+      
+      //convert back to int16
+      convertAudio_F32toI16x2(float_block, int_blockH, int_blockL, float_block->length);
+
+      //return audio to the system
+      AudioStream::transmit(int_blockH);
+      AudioStream::transmit(int_blockL,1);
+      AudioStream::release(int_blockH);
+      AudioStream::release(int_blockL);
+      AudioStream_F32::release(float_block);
+    };
+
+   static void convertAudio_F32toI16x2(audio_block_f32_t *in, audio_block_t *outH, audio_block_t *outL, int len) {
+      //WEA Method.  Should look at CMSIS arm_float_to_q15 instead: https://www.keil.com/pack/doc/CMSIS/DSP/html/group__float__to__x.html#ga215456e35a18db86882e1d3f0d24e1f2	
+      const float MAX_INT = 32678.0f * 65536.0f;
+      for (int i = 0; i < len; i++) {
+        int32_t intValue = (int32_t)(max(min( (in->data[i] * MAX_INT), MAX_INT), -MAX_INT));
+        outH->data[i] = (int16_t) ((intValue & 0xFFFF0000)>>16);
+        outL->data[i] = (int16_t) ((intValue & 0x0000FFFF));
+      }
+    }    
+};
+
 #endif

AudioStream_F32.h

@@ -108,14 +108,21 @@ class AudioConnection_F32
 
 class AudioStream_F32 : public AudioStream {
   public:
-    AudioStream_F32(unsigned char n_input_f32, audio_block_f32_t **iqueue) : AudioStream(1, inputQueueArray_i16),
+    AudioStream_F32(unsigned char n_input_f32, audio_block_f32_t **iqueue, 
-        num_inputs_f32(n_input_f32), inputQueue_f32(iqueue) {
+        unsigned char ninput_i16, audio_block_t** iqueues_i16)
+      : AudioStream(ninput_i16, iqueues_i16),
+        num_inputs_f32(n_input_f32), inputQueue_f32(iqueue) 
+    {
       //active_f32 = false;
       destination_list_f32 = NULL;
       for (int i=0; i < n_input_f32; i++) {
         inputQueue_f32[i] = NULL;
       }
     };
+    AudioStream_F32(unsigned char n_input_f32, audio_block_f32_t **iqueue)
+      : AudioStream_F32(n_input_f32, iqueue, 1, inputQueueArray_i16)
+      {}
+
     static void initialize_f32_memory(audio_block_f32_t *data, unsigned int num);
     static void initialize_f32_memory(audio_block_f32_t *data, unsigned int num, const AudioSettings_F32 &settings);
     //virtual void update(audio_block_f32_t *) = 0;