sof/tools/probes/probes_demux.c

338 lines
8.4 KiB
C

// SPDX-License-Identifier: BSD-3-Clause
//
// Copyright(c) 2022 Intel Corporation. All rights reserved.
//
// Author: Adrian Bonislawski <adrian.bonislawski@intel.com>
// Jyri Sarha <jyri.sarha@intel.com> (restructured and moved to this file)
#include <ctype.h>
#include <errno.h>
#include <inttypes.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <ipc/probe_dma_frame.h>
#include "wave.h"
#define APP_NAME "sof-probes"
#define PACKET_MAX_SIZE 4096 /**< Size limit for probe data packet */
#define DATA_READ_LIMIT 4096 /**< Data limit for file read */
#define FILES_LIMIT 32 /**< Maximum num of probe output files */
#define FILE_PATH_LIMIT 128 /**< Path limit for probe output files */
struct wave_files {
FILE *fd;
uint32_t buffer_id;
uint32_t fmt;
uint32_t size;
struct wave header;
};
enum p_state {
READY = 0, /**< At this stage app is looking for a SYNC word */
SYNC, /**< SYNC received, copying data */
CHECK /**< Check crc and save packet if valid */
};
struct dma_frame_parser {
bool log_to_stdout;
enum p_state state;
struct probe_data_packet *packet;
size_t packet_size;
uint8_t *w_ptr; /* Write pointer to copy data to */
uint32_t total_data_to_copy; /* Total bytes left to copy */
int start; /* Start of unfilled data */
int len; /* Data buffer fill level */
uint8_t data[DATA_READ_LIMIT];
struct wave_files files[FILES_LIMIT];
};
static uint32_t sample_rate[] = {
8000, 11025, 12000, 16000, 22050, 24000, 32000, 44100,
48000, 64000, 88200, 96000, 128000, 176400, 192000
};
int get_buffer_file(struct wave_files *files, uint32_t buffer_id)
{
int i;
for (i = 0; i < FILES_LIMIT; i++) {
if (files[i].fd != NULL && files[i].buffer_id == buffer_id)
return i;
}
return -1;
}
int get_buffer_file_free(struct wave_files *files)
{
int i;
for (i = 0; i < FILES_LIMIT; i++) {
if (files[i].fd == NULL)
return i;
}
return -1;
}
bool is_audio_format(uint32_t format)
{
return (format & PROBE_MASK_FMT_TYPE) != 0 && (format & PROBE_MASK_AUDIO_FMT) == 0;
}
int init_wave(struct dma_frame_parser *p, uint32_t buffer_id, uint32_t format)
{
bool audio = is_audio_format(format);
char path[FILE_PATH_LIMIT];
int i;
i = get_buffer_file_free(p->files);
if (i == -1) {
fprintf(stderr, "error: too many buffers\n");
exit(0);
}
sprintf(path, "buffer_%d.%s", buffer_id, audio ? "wav" : "bin");
fprintf(stderr, "%s:\t Creating file %s\n", APP_NAME, path);
if (!audio && p->log_to_stdout) {
p->files[i].fd = stdout;
} else {
p->files[i].fd = fopen(path, "wb");
if (!p->files[i].fd) {
fprintf(stderr, "error: unable to create file %s, error %d\n",
path, errno);
exit(0);
}
}
p->files[i].buffer_id = buffer_id;
p->files[i].fmt = format;
if (!audio)
return i;
p->files[i].header.riff.chunk_id = HEADER_RIFF;
p->files[i].header.riff.format = HEADER_WAVE;
p->files[i].header.fmt.subchunk_id = HEADER_FMT;
p->files[i].header.fmt.subchunk_size = 16;
p->files[i].header.fmt.audio_format = 1;
p->files[i].header.fmt.num_channels = ((format & PROBE_MASK_NB_CHANNELS) >> PROBE_SHIFT_NB_CHANNELS) + 1;
p->files[i].header.fmt.sample_rate = sample_rate[(format & PROBE_MASK_SAMPLE_RATE) >> PROBE_SHIFT_SAMPLE_RATE];
p->files[i].header.fmt.bits_per_sample = (((format & PROBE_MASK_CONTAINER_SIZE) >> PROBE_SHIFT_CONTAINER_SIZE) + 1) * 8;
p->files[i].header.fmt.byte_rate = p->files[i].header.fmt.sample_rate *
p->files[i].header.fmt.num_channels *
p->files[i].header.fmt.bits_per_sample / 8;
p->files[i].header.fmt.block_align = p->files[i].header.fmt.num_channels *
p->files[i].header.fmt.bits_per_sample / 8;
p->files[i].header.data.subchunk_id = HEADER_DATA;
fwrite(&p->files[i].header, sizeof(struct wave), 1, p->files[i].fd);
return i;
}
void finalize_wave_files(struct wave_files *files)
{
uint32_t i, chunk_size;
/* fill the header at the beginning of each file */
/* and close all opened files */
/* check wave struct to understand the offsets */
for (i = 0; i < FILES_LIMIT; i++) {
if (!is_audio_format(files[i].fmt))
continue;
if (files[i].fd) {
chunk_size = files[i].size + sizeof(struct wave) -
offsetof(struct riff_chunk, format);
fseek(files[i].fd, sizeof(uint32_t), SEEK_SET);
fwrite(&chunk_size, sizeof(uint32_t), 1, files[i].fd);
fseek(files[i].fd, sizeof(struct wave) -
offsetof(struct data_subchunk, subchunk_size),
SEEK_SET);
fwrite(&files[i].size, sizeof(uint32_t), 1, files[i].fd);
fclose(files[i].fd);
}
}
}
int validate_data_packet(struct probe_data_packet *packet)
{
uint64_t *checksump;
uint64_t sum;
sum = (uint32_t) (packet->sync_word +
packet->buffer_id +
packet->format +
packet->timestamp_high +
packet->timestamp_low +
packet->data_size_bytes);
checksump = (uint64_t *) (packet->data + packet->data_size_bytes);
if (sum != *checksump) {
fprintf(stderr, "Checksum error 0x%016" PRIx64 " != 0x%016" PRIx64 "\n",
sum, *checksump);
return -EINVAL;
}
return 0;
}
int process_sync(struct dma_frame_parser *p)
{
struct probe_data_packet *temp_packet;
/* request to copy data_size from probe packet and 64-bit checksum */
p->total_data_to_copy = p->packet->data_size_bytes + sizeof(uint64_t);
if (sizeof(struct probe_data_packet) + p->total_data_to_copy >
p->packet_size) {
p->packet_size = sizeof(struct probe_data_packet) +
p->total_data_to_copy;
temp_packet = realloc(p->packet, p->packet_size);
if (!temp_packet)
return -ENOMEM;
p->packet = temp_packet;
}
p->w_ptr = (uint8_t *)p->packet->data;
return 0;
}
struct dma_frame_parser *parser_init(void)
{
struct dma_frame_parser *p = malloc(sizeof(*p));
if (!p) {
fprintf(stderr, "error: allocation failed, err %d\n",
errno);
return NULL;
}
memset(p, 0, sizeof(*p));
p->packet = malloc(PACKET_MAX_SIZE);
if (!p) {
fprintf(stderr, "error: allocation failed, err %d\n",
errno);
free(p);
return NULL;
}
memset(p->packet, 0, PACKET_MAX_SIZE);
p->packet_size = PACKET_MAX_SIZE;
return p;
}
void parser_free(struct dma_frame_parser *p)
{
free(p->packet);
free(p);
}
void parser_log_to_stdout(struct dma_frame_parser *p)
{
p->log_to_stdout = true;
}
void parser_fetch_free_buffer(struct dma_frame_parser *p, uint8_t **d, size_t *len)
{
*d = &p->data[p->start];
*len = sizeof(p->data) - p->start;
}
int parser_parse_data(struct dma_frame_parser *p, size_t d_len)
{
uint i = 0;
p->len = p->start + d_len;
/* processing all loaded bytes */
while (i < p->len) {
if (p->total_data_to_copy == 0) {
switch (p->state) {
case READY:
/* check for SYNC */
if (p->len - i < sizeof(p->packet->sync_word)) {
p->start = p->len - i;
memmove(&p->data[0], &p->data[i], p->start);
i += p->start;
} else if (*((uint32_t *)&p->data[i]) ==
PROBE_EXTRACT_SYNC_WORD) {
memset(p->packet, 0, p->packet_size);
/* request to copy full data packet */
p->total_data_to_copy =
sizeof(struct probe_data_packet);
p->w_ptr = (uint8_t *)p->packet;
p->state = SYNC;
p->start = 0;
} else {
i++;
}
break;
case SYNC:
/* SYNC -> CHECK */
if (process_sync(p) < 0) {
fprintf(stderr, "OOM, quitting\n");
return -ENOMEM;
}
p->state = CHECK;
break;
case CHECK:
/* CHECK -> READY */
/* find corresponding file and save data if valid */
if (validate_data_packet(p->packet) == 0) {
int file = get_buffer_file(p->files,
p->packet->buffer_id);
if (file < 0)
file = init_wave(p, p->packet->buffer_id,
p->packet->format);
if (file < 0) {
fprintf(stderr,
"unable to open file for %u\n",
p->packet->buffer_id);
return -EIO;
}
fwrite(p->packet->data, 1,
p->packet->data_size_bytes,
p->files[file].fd);
p->files[file].size += p->packet->data_size_bytes;
}
p->state = READY;
break;
}
}
/* data copying section */
if (p->total_data_to_copy > 0) {
uint data_to_copy;
/* check if there is enough bytes loaded */
/* or copy partially if not */
if (i + p->total_data_to_copy > p->len) {
data_to_copy = p->len - i;
p->total_data_to_copy -= data_to_copy;
} else {
data_to_copy = p->total_data_to_copy;
p->total_data_to_copy = 0;
}
memcpy(p->w_ptr, &p->data[i], data_to_copy);
p->w_ptr += data_to_copy;
i += data_to_copy;
}
}
return 0;
}