1 /*
2 * Copyright (C) 2018 Knowles Electronics
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <stdio.h>
18 #include <stdint.h>
19 #include <stdlib.h>
20 #include <stdbool.h>
21 #include <string.h>
22 #include <math.h>
23 #include <errno.h>
24
25 #define LOG_TAG "SoundTriggerHALAdnc"
26 //#define LOG_NDEBUG 0
27 //#define LOG_NDDEBUG 0
28
29 #include <log/log.h>
30 #include <linux/mfd/adnc/iaxxx-system-identifiers.h>
31 #include "adnc_strm.h"
32 #include "tunnel.h"
33
34 #define MAX_TUNNELS (32)
35 #define BUF_SIZE (8192)
36
37 #define CVQ_TUNNEL_ID (1)
38 #define TNL_Q15 (0xF)
39
40 #define UNPARSED_OUTPUT_FILE "/data/data/unparsed_output"
41 // By defining this macros, dumps will be enabled at key points to help in debugging
42 //#define ENABLE_DEBUG_DUMPS
43 #define HOTWORD_MODEL (0)
44 #define AMBIENT_MODEL (1)
45
46 struct raf_format_type {
47 uint16_t frameSizeInBytes; // Frame length in bytes
48 uint8_t encoding; // Encoding
49 uint8_t sampleRate; // Sample rate
50 };
51
52 struct raf_frame_type {
53 uint64_t timeStamp; // Timestamp of the frame
54 uint32_t seqNo; // Optional sequence number of the frame
55 struct raf_format_type format; // Format information for the frame
56 uint32_t data[0]; /* Start of the variable size payload.
57 It must start at 128 bit aligned
58 address for all the frames */
59 };
60
61 struct adnc_strm_device
62 {
63 struct ia_tunneling_hal *tun_hdl;
64 int end_point;
65 int idx;
66 int mode;
67 int encode;
68
69 bool enable_stripping;
70 unsigned int kw_start_frame;
71
72 void *pcm_buf;
73 size_t pcm_buf_size;
74 size_t pcm_avail_size;
75 size_t pcm_read_offset;
76
77 void *unparsed_buf;
78 size_t unparsed_buf_size;
79 size_t unparsed_avail_size;
80
81 #ifdef DUMP_UNPARSED_OUTPUT
82 FILE *dump_file;
83 #endif
84
85 pthread_mutex_t lock;
86 };
87
kst_split_aft(uint32_t * pAfloat,int32_t * exp,int64_t * mant,int32_t * sign)88 static void kst_split_aft(uint32_t *pAfloat, int32_t *exp,
89 int64_t *mant, int32_t *sign)
90 {
91 uint32_t uAft = *pAfloat;
92
93 *exp = (uAft >> 25) & 0x3F;
94 *mant = uAft & 0x1FFFFFF;
95 *sign = uAft >> 31;
96 if (*exp || *mant) {
97 *mant |= 1 << 25;
98 }
99 }
100
kst_aft_to_dbl(void * pDouble,void * pAfloat)101 static void kst_aft_to_dbl(void *pDouble, void *pAfloat)
102 {
103 uint64_t uDbl;
104 int32_t exp;
105 int32_t sign;
106 int64_t mant;
107
108 kst_split_aft((uint32_t *)pAfloat, &exp, &mant, &sign);
109 if (exp || mant) {
110 uDbl = ((uint64_t)sign << 63) |
111 ((uint64_t)(exp + (1023 - (1 << 5))) << 52) |
112 ((uint64_t)(mant & ((1 << 25) - 1)) << (52 - 25));
113 } else {
114 uDbl = (uint64_t)sign << 63;
115 }
116 *((uint64_t *)pDouble) = uDbl;
117 }
118
kst_float_to_q15_vector(void * pDst,void * pSrc,uint32_t elCnt)119 void kst_float_to_q15_vector(void *pDst, void *pSrc, uint32_t elCnt)
120 {
121 uint32_t *pSrcT;
122 int16_t *pDstT;
123 uint32_t idx;
124 double smp;
125
126 pSrcT = (uint32_t *)pSrc;
127 pDstT = (int16_t *)pDst;
128 for (idx = 0; idx < elCnt; idx++) {
129 kst_aft_to_dbl(&smp, &(pSrcT[idx]));
130 smp = smp * 32768.0;
131 pDstT[idx] = ((smp < 32767.0) ?
132 ((smp > -32768.0) ?
133 ((int16_t)smp) : -32768) : 32767);
134 }
135 }
136
parse_audio_tunnel_data(unsigned char * buf_itr,unsigned char * pcm_buf_itr,int frame_sz_in_bytes,bool is_q15_conversion_required)137 void parse_audio_tunnel_data(unsigned char *buf_itr,
138 unsigned char *pcm_buf_itr,
139 int frame_sz_in_bytes,
140 bool is_q15_conversion_required)
141 {
142 //char q16_buf[BUF_SIZE]; // This can be smaller but by how much?
143 int frameSizeInWords = (frame_sz_in_bytes + 3) >> 2;
144
145 if (buf_itr == NULL || pcm_buf_itr == NULL) {
146 ALOGE("%s: Buffer is NULL", __func__);
147 return;
148 }
149
150 if (is_q15_conversion_required == true) {
151 kst_float_to_q15_vector(pcm_buf_itr, buf_itr, frameSizeInWords);
152 } else {
153 memcpy(pcm_buf_itr, buf_itr, frame_sz_in_bytes);
154 }
155 #ifdef ENABLE_DEBUG_DUMPS
156 out_fp = fopen("/data/data/pcm_dump", "ab");
157 if (out_fp) {
158 ALOGE("Dumping to pcm_dump");
159 fwrite(pcm_buf_itr, (frameSizeInWords * 2), 1, out_fp);
160 fflush(out_fp);
161 fclose(out_fp);
162 } else {
163 ALOGE("Failed to open the out_fp file %s", strerror(errno));
164 ALOGE("out_fp is NULL");
165 }
166 #endif
167 }
168
parse_tunnel_buf(struct adnc_strm_device * adnc_strm_dev)169 static int parse_tunnel_buf(struct adnc_strm_device *adnc_strm_dev)
170 {
171 /*
172 * The magic number is ROME in ASCII reversed.
173 * So we are looking for EMOR in the byte stream
174 */
175 const unsigned char magic_num[4] = {0x45, 0x4D, 0x4F, 0x52};
176 unsigned short int tunnel_id;
177 unsigned char *start_frame = NULL;
178 bool valid_frame = true;
179 unsigned char *buf_itr = adnc_strm_dev->unparsed_buf;
180 /*
181 * Minimum bytes required is
182 * magic number + tunnel id + reserved and crc + raf struct
183 */
184 int min_bytes_req = 4 + 2 + 6 + sizeof(struct raf_frame_type);
185 int bytes_avail = adnc_strm_dev->unparsed_avail_size;
186 unsigned char *pcm_buf_itr = NULL;
187 int curr_pcm_frame_size;
188 bool is_q15_conversion_required = false;
189
190 if (buf_itr == NULL) {
191 ALOGE("Invalid input sent to parse_tunnel_buf");
192 return 0;
193 }
194
195 do {
196 // Check for MagicNumber 0x454D4F52
197 while (buf_itr[0] != magic_num[0] || buf_itr[1] != magic_num[1] ||
198 buf_itr[2] != magic_num[2] || buf_itr[3] != magic_num[3]) {
199 buf_itr++;
200 bytes_avail--;
201 if (bytes_avail <= 0) {
202 ALOGE("Could not find the magic number, reading again");
203 ALOGE("buf_itr[0] %x buf_itr[1] %x buf_itr[2] %x buf_itr[3] %x",
204 buf_itr[0], buf_itr[1], buf_itr[2], buf_itr[3]);
205 return 0;
206 }
207 }
208
209 start_frame = buf_itr;
210
211 // Skip the magic number
212 buf_itr += 4;
213 bytes_avail -= 4;
214
215 // Read the tunnelID
216 tunnel_id = ((unsigned char) (buf_itr[0]) |
217 (unsigned char) (buf_itr[1]) << 8);
218
219 // Skip tunnelID
220 buf_itr += 2;
221 bytes_avail -= 2;
222
223 // Skip Reserved field and CRC - 6 bytes in total
224 buf_itr += 6;
225 bytes_avail -= 6;
226
227 valid_frame = true;
228 // There is only one tunnel data we are looking
229 if (tunnel_id > MAX_TUNNELS) {
230 ALOGE("Invalid tunnel id %d\n", tunnel_id);
231 valid_frame = false;
232 }
233
234 struct raf_frame_type rft;
235 memcpy(&rft, buf_itr, sizeof(struct raf_frame_type));
236
237 bool skip_extra_data = false;
238 if ((adnc_strm_dev->enable_stripping == true) &&
239 (rft.seqNo < adnc_strm_dev->kw_start_frame)) {
240 skip_extra_data = true;
241 }
242
243 /*
244 * 1 indicates that it is afloat encoding and
245 * F indicates it is in q15 encoding
246 */
247 if (rft.format.encoding == 1) {
248 is_q15_conversion_required = true;
249 curr_pcm_frame_size = rft.format.frameSizeInBytes / 2;
250 } else {
251 is_q15_conversion_required = false;
252 curr_pcm_frame_size = rft.format.frameSizeInBytes;
253 }
254
255 // Skip the raf_frame_type
256 buf_itr += sizeof(struct raf_frame_type);
257 bytes_avail -= sizeof(struct raf_frame_type);
258
259 if (bytes_avail < rft.format.frameSizeInBytes) {
260 ALOGD("Incomplete frame received bytes_avail %d framesize %d",
261 bytes_avail, rft.format.frameSizeInBytes);
262 bytes_avail += min_bytes_req;
263 break;
264 }
265
266 if (valid_frame == true && skip_extra_data == false) {
267 if ((adnc_strm_dev->pcm_avail_size + curr_pcm_frame_size) <
268 adnc_strm_dev->pcm_buf_size) {
269 pcm_buf_itr = (unsigned char *)adnc_strm_dev->pcm_buf +
270 adnc_strm_dev->pcm_avail_size;
271 parse_audio_tunnel_data(buf_itr, pcm_buf_itr,
272 rft.format.frameSizeInBytes,
273 is_q15_conversion_required);
274 adnc_strm_dev->pcm_avail_size += curr_pcm_frame_size;
275 } else {
276 ALOGD("Not enough PCM buffer available break now");
277 bytes_avail += min_bytes_req;
278 break;
279 }
280 }
281
282 // Skip the data
283 buf_itr += rft.format.frameSizeInBytes;
284 bytes_avail -= rft.format.frameSizeInBytes;
285 } while (bytes_avail > min_bytes_req);
286
287 return bytes_avail;
288 }
289
290
291 __attribute__ ((visibility ("default")))
adnc_strm_read(long handle,void * buffer,size_t bytes)292 size_t adnc_strm_read(long handle, void *buffer, size_t bytes)
293 {
294 int ret = 0;
295 struct adnc_strm_device *adnc_strm_dev = (struct adnc_strm_device *) handle;
296 int bytes_read, bytes_rem;
297
298 if (adnc_strm_dev == NULL) {
299 ALOGE("Invalid handle");
300 ret = 0;
301 goto exit;
302 }
303
304 pthread_mutex_lock(&adnc_strm_dev->lock);
305
306 if (bytes > adnc_strm_dev->pcm_avail_size) {
307 /*
308 * We don't have enough PCM data, read more from the device.
309 * First copy the remainder of the PCM buffer to the front
310 * of the PCM buffer
311 */
312 if (adnc_strm_dev->pcm_avail_size != 0) {
313 ALOGD("Copying to the front of the buffer pcm_avail_size %zu"
314 " pcm_read_offset %zu", adnc_strm_dev->pcm_avail_size,
315 adnc_strm_dev->pcm_read_offset);
316 memcpy(adnc_strm_dev->pcm_buf,
317 ((unsigned char *)adnc_strm_dev->pcm_buf +
318 adnc_strm_dev->pcm_read_offset),
319 adnc_strm_dev->pcm_avail_size);
320 }
321 // Always read from the start of the PCM buffer at this point of time
322 adnc_strm_dev->pcm_read_offset = 0;
323
324 read_again:
325 // Read data from the kernel, account for the leftover
326 // data from previous run
327 bytes_read = ia_read_tunnel_data(adnc_strm_dev->tun_hdl,
328 (void *)((unsigned char *)
329 adnc_strm_dev->unparsed_buf +
330 adnc_strm_dev->unparsed_avail_size),
331 BUF_SIZE);
332 if (bytes_read <= 0) {
333 ALOGE("Failed to read data from tunnel");
334 ret = 0; // TODO should we try to read a couple of times?
335 pthread_mutex_unlock(&adnc_strm_dev->lock);
336 goto exit;
337 }
338
339 // Parse the data to get PCM data
340 adnc_strm_dev->unparsed_avail_size += bytes_read;
341 bytes_rem = parse_tunnel_buf(adnc_strm_dev);
342
343 #ifdef ENABLE_DEBUG_DUMPS
344 if (adnc_strm_dev->pcm_avail_size != 0) {
345 FILE *out_fp = fopen("/data/data/pcm_dump2", "ab");
346 if (out_fp) {
347 ALOGE("Dumping to pcm_dump2");
348 fwrite(((unsigned char *)adnc_strm_dev->pcm_buf +
349 adnc_strm_dev->pcm_avail_size),
350 adnc_strm_dev->pcm_avail_size, 1, out_fp);
351 fflush(out_fp);
352 fclose(out_fp);
353 } else {
354 ALOGE("Failed to open the pcm_dump2 file %s", strerror(errno));
355 }
356 }
357 #endif
358
359 // Copy the left over unparsed data to the front of the buffer
360 if (bytes_rem != 0) {
361 int offset = adnc_strm_dev->unparsed_avail_size - bytes_rem;
362 memcpy(adnc_strm_dev->unparsed_buf,
363 ((unsigned char *)adnc_strm_dev->unparsed_buf + offset),
364 bytes_rem);
365 }
366 adnc_strm_dev->unparsed_avail_size = bytes_rem;
367
368 /*
369 * If stripping is enabled then we didn't read anything to the pcm
370 * bufferso read again or if we still don't have enough bytes then
371 * read data again.
372 */
373 if (adnc_strm_dev->pcm_avail_size == 0 ||
374 adnc_strm_dev->pcm_avail_size < bytes) {
375 goto read_again;
376 }
377 }
378
379 // Copy the PCM data to output buffer and return
380 memcpy(buffer,
381 ((unsigned char *)adnc_strm_dev->pcm_buf +
382 adnc_strm_dev->pcm_read_offset),
383 bytes);
384
385 #ifdef ENABLE_DEBUG_DUMPS
386 char l_buffer[64];
387 int cx;
388 FILE *out_fp = NULL;
389 cx = snprintf(l_buffer, sizeof(l_buffer), "/data/data/adnc_dump_%x",
390 adnc_strm_dev->end_point);
391 if (cx >= 0 && cx < 64)
392 out_fp = fopen(l_buffer, "ab");
393 if (out_fp) {
394 ALOGD("Dumping to adnc_dump:%s", l_buffer);
395 fwrite(buffer, bytes, 1, out_fp);
396 fflush(out_fp);
397 fclose(out_fp);
398 } else {
399 ALOGE("Failed to open the adnc_dump file %s", strerror(errno));
400 }
401 #endif
402
403 adnc_strm_dev->pcm_avail_size -= bytes;
404 adnc_strm_dev->pcm_read_offset += bytes;
405
406 pthread_mutex_unlock(&adnc_strm_dev->lock);
407
408 exit:
409
410 return bytes;
411 }
412
413
414 __attribute__ ((visibility ("default")))
adnc_strm_open(bool enable_stripping,unsigned int kw_start_frame,int stream_end_point)415 long adnc_strm_open(bool enable_stripping,
416 unsigned int kw_start_frame,
417 int stream_end_point)
418 {
419 int ret = 0, err;
420 struct adnc_strm_device *adnc_strm_dev = NULL;
421
422 adnc_strm_dev = (struct adnc_strm_device *)
423 calloc(1, sizeof(struct adnc_strm_device));
424 if (adnc_strm_dev == NULL) {
425 ALOGE("Failed to allocate memory for adnc_strm_dev");
426 ret = 0;
427 goto exit_no_memory;
428 }
429
430 pthread_mutex_init(&adnc_strm_dev->lock, (const pthread_mutexattr_t *) NULL);
431
432 pthread_mutex_lock(&adnc_strm_dev->lock);
433
434 adnc_strm_dev->end_point = stream_end_point;
435 adnc_strm_dev->idx = 0;
436 adnc_strm_dev->mode = 0;
437 adnc_strm_dev->encode = TNL_Q15;
438 adnc_strm_dev->enable_stripping = enable_stripping;
439 adnc_strm_dev->kw_start_frame = kw_start_frame;
440 adnc_strm_dev->tun_hdl = NULL;
441 adnc_strm_dev->pcm_buf = NULL;
442 adnc_strm_dev->unparsed_buf = NULL;
443
444 adnc_strm_dev->tun_hdl = ia_start_tunneling(640);
445 if (adnc_strm_dev->tun_hdl == NULL) {
446 ALOGE("Failed to start tunneling");
447 ret = 0;
448 goto exit_on_error;
449 }
450
451 ret = ia_enable_tunneling_source(adnc_strm_dev->tun_hdl,
452 adnc_strm_dev->end_point,
453 adnc_strm_dev->mode,
454 adnc_strm_dev->encode);
455 if (ret != 0) {
456 ALOGE("Failed to enable tunneling for CVQ tunl_id %u src_id %u mode %u",
457 adnc_strm_dev->idx, adnc_strm_dev->end_point, adnc_strm_dev->mode);
458 ret = 0;
459 goto exit_on_error;
460 }
461
462 adnc_strm_dev->unparsed_buf_size = BUF_SIZE * 2;
463 adnc_strm_dev->unparsed_avail_size = 0;
464 adnc_strm_dev->unparsed_buf = malloc(adnc_strm_dev->unparsed_buf_size);
465 if (adnc_strm_dev->unparsed_buf == NULL) {
466 ret = 0;
467 ALOGE("Failed to allocate memory for unparsed buffer");
468 goto exit_on_error;
469 }
470
471 adnc_strm_dev->pcm_buf_size = BUF_SIZE * 2;
472 adnc_strm_dev->pcm_avail_size = 0;
473 adnc_strm_dev->pcm_read_offset = 0;
474 adnc_strm_dev->pcm_buf = malloc(adnc_strm_dev->pcm_buf_size);
475 if (adnc_strm_dev->pcm_buf == NULL) {
476 ret = 0;
477 ALOGE("Failed to allocate memory for pcm buffer");
478 goto exit_on_error;
479 }
480
481 pthread_mutex_unlock(&adnc_strm_dev->lock);
482
483 return (long)adnc_strm_dev;
484
485 exit_on_error:
486 if (adnc_strm_dev->pcm_buf) {
487 free(adnc_strm_dev->pcm_buf);
488 }
489
490 if (adnc_strm_dev->unparsed_buf) {
491 free(adnc_strm_dev->unparsed_buf);
492 }
493
494 err = ia_disable_tunneling_source(adnc_strm_dev->tun_hdl,
495 adnc_strm_dev->end_point,
496 adnc_strm_dev->mode,
497 adnc_strm_dev->encode);
498 if (err != 0) {
499 ALOGE("Failed to disable the tunneling source");
500 }
501
502 err = ia_stop_tunneling(adnc_strm_dev->tun_hdl);
503 if (err != 0) {
504 ALOGE("Failed to stop tunneling");
505 }
506
507 pthread_mutex_unlock(&adnc_strm_dev->lock);
508
509 if (adnc_strm_dev) {
510 free(adnc_strm_dev);
511 }
512
513 exit_no_memory:
514
515 return ret;
516 }
517
518 __attribute__ ((visibility ("default")))
adnc_strm_close(long handle)519 int adnc_strm_close(long handle)
520 {
521 int ret = 0;
522 struct adnc_strm_device *adnc_strm_dev = (struct adnc_strm_device *) handle;
523
524 if (adnc_strm_dev == NULL) {
525 ALOGE("Invalid handle");
526 ret = -1;
527 goto exit;
528 }
529
530 pthread_mutex_lock(&adnc_strm_dev->lock);
531
532 if (adnc_strm_dev->pcm_buf) {
533 free(adnc_strm_dev->pcm_buf);
534 }
535
536 if (adnc_strm_dev->unparsed_buf) {
537 free(adnc_strm_dev->unparsed_buf);
538 }
539
540 ret = ia_disable_tunneling_source(adnc_strm_dev->tun_hdl,
541 adnc_strm_dev->end_point,
542 adnc_strm_dev->mode,
543 adnc_strm_dev->encode);
544 if (ret != 0) {
545 ALOGE("Failed to disable the tunneling source");
546 }
547
548 ret = ia_stop_tunneling(adnc_strm_dev->tun_hdl);
549 if (ret != 0) {
550 ALOGE("Failed to stop tunneling");
551 }
552
553 pthread_mutex_unlock(&adnc_strm_dev->lock);
554
555 if (adnc_strm_dev) {
556 free(adnc_strm_dev);
557 }
558
559 exit:
560 return ret;
561 }
562
563