diff --git a/tools/probes/CMakeLists.txt b/tools/probes/CMakeLists.txt index 86d03812b685..1b39e6d192db 100644 --- a/tools/probes/CMakeLists.txt +++ b/tools/probes/CMakeLists.txt @@ -3,8 +3,8 @@ cmake_minimum_required(VERSION 3.13) add_executable(sof-probes + probes_demux.c probes_main.c - ../../src/math/numbers.c ) target_compile_options(sof-probes PRIVATE diff --git a/tools/probes/probes_demux.c b/tools/probes/probes_demux.c new file mode 100644 index 000000000000..3db021e8a6e3 --- /dev/null +++ b/tools/probes/probes_demux.c @@ -0,0 +1,337 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2022 Intel Corporation. All rights reserved. +// +// Author: Adrian Bonislawski +// Jyri Sarha (restructured and moved to this file) + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#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; +} diff --git a/tools/probes/probes_demux.h b/tools/probes/probes_demux.h new file mode 100644 index 000000000000..a656d4c7af47 --- /dev/null +++ b/tools/probes/probes_demux.h @@ -0,0 +1,27 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2022 Intel Corporation. All rights reserved. +// +// Author: Jyri Sarha +// + +#ifndef _PROBES_DEMUX_H_ +#define _PROBES_DEMUX_H_ + +#include +#include +#include + +struct dma_frame_parser; + +struct dma_frame_parser *parser_init(void); + +void parser_log_to_stdout(struct dma_frame_parser *p); + +void parser_free(struct dma_frame_parser *p); + +void parser_fetch_free_buffer(struct dma_frame_parser *p, uint8_t **d, size_t *len); + +int parser_parse_data(struct dma_frame_parser *p, size_t d_len); + +#endif diff --git a/tools/probes/probes_main.c b/tools/probes/probes_main.c index db8ab73a3a21..7b553709a446 100644 --- a/tools/probes/probes_main.c +++ b/tools/probes/probes_main.c @@ -13,345 +13,82 @@ * */ -#include #include -#include #include #include #include #include #include -#include #include -#include -#include - -#include "wave.h" +#include "probes_demux.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 */ -}; - -static uint32_t sample_rate[] = { - 8000, 11025, 12000, 16000, 22050, 24000, 32000, 44100, - 48000, 64000, 88200, 96000, 128000, 176400, 192000 -}; - static void usage(void) { fprintf(stdout, "Usage %s \n\n", APP_NAME); fprintf(stdout, "%s:\t -p file\tParse extracted file\n\n", APP_NAME); + fprintf(stdout, "%s:\t -l \t\tLog to stdout\n\n", APP_NAME); fprintf(stdout, "%s:\t -h \t\tHelp, usage info\n", APP_NAME); exit(0); } -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 wave_files *files, uint32_t buffer_id, uint32_t format) +void parse_data(const char *file_in, bool log_to_stdout) { - bool audio = is_audio_format(format); - char path[FILE_PATH_LIMIT]; - int i; - - i = get_buffer_file_free(files); - if (i == -1) { - fprintf(stderr, "error: too many buffers\n"); - exit(0); - } - - sprintf(path, "buffer_%d.%s", buffer_id, audio ? "wav" : "bin"); - - fprintf(stdout, "%s:\t Creating file %s\n", APP_NAME, path); + struct dma_frame_parser *p = parser_init(); + FILE *fd_in; + uint8_t *data; + size_t len; + int ret; - files[i].fd = fopen(path, "wb"); - if (!files[i].fd) { - fprintf(stderr, "error: unable to create file %s, error %d\n", - path, errno); - exit(0); + if (!p) { + fprintf(stderr, "parser_init() failed\n"); + exit(1); } - files[i].buffer_id = buffer_id; - files[i].fmt = format; + if (log_to_stdout) + parser_log_to_stdout(p); - if (!audio) - return i; - - files[i].header.riff.chunk_id = HEADER_RIFF; - files[i].header.riff.format = HEADER_WAVE; - files[i].header.fmt.subchunk_id = HEADER_FMT; - files[i].header.fmt.subchunk_size = 16; - files[i].header.fmt.audio_format = 1; - files[i].header.fmt.num_channels = ((format & PROBE_MASK_NB_CHANNELS) >> PROBE_SHIFT_NB_CHANNELS) + 1; - files[i].header.fmt.sample_rate = sample_rate[(format & PROBE_MASK_SAMPLE_RATE) >> PROBE_SHIFT_SAMPLE_RATE]; - files[i].header.fmt.bits_per_sample = (((format & PROBE_MASK_CONTAINER_SIZE) >> PROBE_SHIFT_CONTAINER_SIZE) + 1) * 8; - files[i].header.fmt.byte_rate = files[i].header.fmt.sample_rate * - files[i].header.fmt.num_channels * - files[i].header.fmt.bits_per_sample / 8; - files[i].header.fmt.block_align = files[i].header.fmt.num_channels * - files[i].header.fmt.bits_per_sample / 8; - files[i].header.data.subchunk_id = HEADER_DATA; - - fwrite(&files[i].header, sizeof(struct wave), 1, 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); + if (file_in) { + fd_in = fopen(file_in, "rb"); + if (!fd_in) { + fprintf(stderr, "error: unable to open file %s, error %d\n", + file_in, errno); + exit(0); } + } else { + fd_in = stdin; } -} - -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%016lx != 0x%016lx\n", sum, *checksump); - return -EINVAL; - } - - return 0; -} - -int process_sync(struct probe_data_packet **packet, uint8_t **w_ptr, uint32_t *total_data_to_copy) -{ - struct probe_data_packet *temp_packet; - - /* request to copy data_size from probe packet and 64-bit checksum */ - *total_data_to_copy = (*packet)->data_size_bytes + sizeof(uint64_t); - - if (sizeof(struct probe_data_packet) + *total_data_to_copy > PACKET_MAX_SIZE) { - temp_packet = realloc(packet, - sizeof(struct probe_data_packet) + *total_data_to_copy); - if (!temp_packet) - return -ENOMEM; - - *packet = temp_packet; - } - - *w_ptr = (uint8_t *)&(*packet)->data; - - return 0; -} -void parse_data(char *file_in) -{ - FILE *fd_in; - struct wave_files files[FILES_LIMIT]; - struct probe_data_packet *packet; - uint8_t data[DATA_READ_LIMIT]; - uint32_t total_data_to_copy = 0; - uint8_t *w_ptr; - int start, i, j, file; - - enum p_state state = READY; - - fprintf(stdout, "%s:\t Parsing file: %s\n", APP_NAME, file_in); - - fd_in = fopen(file_in, "rb"); - if (!fd_in) { - fprintf(stderr, "error: unable to open file %s, error %d\n", - file_in, errno); - exit(0); - } - - packet = malloc(PACKET_MAX_SIZE); - if (!packet) { - fprintf(stderr, "error: allocation failed, err %d\n", - errno); - fclose(fd_in); - exit(0); - } - memset(&data, 0, DATA_READ_LIMIT); - memset(&files, 0, sizeof(struct wave_files) * FILES_LIMIT); - - start = 0; - /* Data read loop to process DATA_READ_LIMIT bytes at each - * iteration. If there is under sizeof(sync_word) bytes left - * in the buffer when a new frame is searched for, the remaining - * bytes are moved to the beginning of the buffer for the next - * iteration. - */ do { - i = fread(&data[start], 1, DATA_READ_LIMIT - start, fd_in); - i += start; - j = 0; - start = 0; - - /* processing all loaded bytes */ - while (j < i) { - if (total_data_to_copy == 0) { - switch (state) { - case READY: - /* check for SYNC */ - if (i - j < sizeof(packet->sync_word)) { - start = i - j; - memmove(&data[0], &data[j], start); - j += start; - } else if (*((uint32_t *)&data[j]) == - PROBE_EXTRACT_SYNC_WORD) { - memset(packet, 0, PACKET_MAX_SIZE); - /* request to copy full data packet */ - total_data_to_copy = - sizeof(struct probe_data_packet); - w_ptr = (uint8_t *)packet; - state = SYNC; - } else { - j++; - } - break; - case SYNC: - /* SYNC -> CHECK */ - if (process_sync(&packet, &w_ptr, &total_data_to_copy) < 0) { - fprintf(stderr, "OOM, quitting\n"); - goto err; - } - state = CHECK; - break; - case CHECK: - /* CHECK -> READY */ - /* find corresponding file and save data if valid */ - if (validate_data_packet(packet) == 0) { - file = get_buffer_file(files, - packet->buffer_id); - - if (file < 0) - file = init_wave(files, - packet->buffer_id, - packet->format); - - if (file < 0) { - fprintf(stderr, - "unable to open file for %u\n", - packet->buffer_id); - goto err; - } - - fwrite(packet->data, 1, - packet->data_size_bytes, files[file].fd); - - files[file].size += packet->data_size_bytes; - } - state = READY; - break; - } - } - /* data copying section */ - if (total_data_to_copy > 0) { - uint32_t data_to_copy; - /* check if there is enough bytes loaded */ - /* or copy partially if not */ - if (j + total_data_to_copy > i) { - data_to_copy = i - j; - total_data_to_copy -= data_to_copy; - } else { - data_to_copy = total_data_to_copy; - total_data_to_copy = 0; - } - memcpy(w_ptr, data + j, data_to_copy); - w_ptr += data_to_copy; - j += data_to_copy; - } - } - } while (i > 0); - -err: - /* all done, can close files */ - finalize_wave_files(files); - free(packet); - fclose(fd_in); - fprintf(stdout, "%s:\t done\n", APP_NAME); + parser_fetch_free_buffer(p, &data, &len); + len = fread(data, 1, len, fd_in); + ret = parser_parse_data(p, len); + } while (!ret && !feof(fd_in)); } int main(int argc, char *argv[]) { + const char *fname = NULL; + bool log_to_stdout = false; int opt; - while ((opt = getopt(argc, argv, "hp:")) != -1) { + while ((opt = getopt(argc, argv, "lhp:")) != -1) { switch (opt) { case 'p': - parse_data(optarg); + fname = optarg; + break; + case 'l': + log_to_stdout = true; break; case 'h': default: usage(); + return 0; } } + parse_data(fname, log_to_stdout); return 0; }