libspisound/spisound.c

#include "spisound.h"

#include <errno.h>
#include <fcntl.h>
#include <getopt.h>
#include <linux/spi/spidev.h>
#include <linux/types.h>
#include <pthread.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>  //for strerror()
#include <sys/ioctl.h>
#include <unistd.h>

#define MAX_ONE_FRAME_VOICE_SIZE 2000

#define IOCTL(vfd, vkey, vvalue, errorcode) \
  {                                         \
    int ret = ioctl(vfd, vkey, &vvalue);    \
    if (ret != 0) {                         \
      close(vfd);                           \
      vfd          = -1;                    \
      m_sys_error  = errno;                 \
      m_last_error = errorcode;             \
      return errorcode;                     \
    }                                       \
  }

// static const char              *m_deviceName = NULL;
static spisound_cb_t m_callback   = NULL;
static int           m_fd         = 0;
static int           m_sys_error  = 0;
static error_code_t  m_last_error = ksp_success;
static int           m_cs_gpio_id = 0;

static int m_gpiofd = 0;

static pthread_t sound_read_thread;
static bool      inited  = false;
static bool      started = false;

static void *sound_read_thread_func(void *v);
static void  gpio_set_value(int value) { write(m_gpiofd, value ? "1\n" : "0\n", 2); }
static void  readvoice();

typedef struct {
  int32_t *voice;
  int32_t  size;
} audio_t;

static audio_t multiplierX2(int32_t *voice, int32_t size) {
  static int32_t voicebuf[MAX_ONE_FRAME_VOICE_SIZE * 2];  // 50ms
  for (size_t i = 0; i < size; i++) {
    voicebuf[i * 2] = voice[i];
    if (i + 1 < size) {
      voicebuf[i * 2 + 1] = (voice[i] + voice[i + 1]) / 2;
    } else {
      voicebuf[i * 2 + 1] = voice[i] + (voice[i] - voice[i - 1]);
    }
  }
  audio_t ret;
  ret.size  = size * 2;
  ret.voice = voicebuf;
  return ret;
}

static audio_t multiplierX2_2(int32_t *voice, int32_t size) {
  static int32_t voicebuf[MAX_ONE_FRAME_VOICE_SIZE * 4];  // 50ms
  if (size > MAX_ONE_FRAME_VOICE_SIZE) {
    size = MAX_ONE_FRAME_VOICE_SIZE;
  }

  for (size_t i = 0; i < size; i++) {
    voicebuf[i * 2] = voice[i];
    if (i + 1 < size) {
      voicebuf[i * 2 + 1] = (voice[i] + voice[i + 1]) / 2;
    } else {
      voicebuf[i * 2 + 1] = voice[i] + (voice[i] - voice[i - 1]);
    }
  }
  audio_t ret;
  ret.size  = size * 2;
  ret.voice = voicebuf;
  return ret;
}

static int32_t filter(int32_t input, int32_t v1, int32_t v2, int32_t v3, int32_t v4, int32_t v5, int32_t v6) {
  int32_t v[6];
  v[0] = v1;
  v[1] = v2;
  v[2] = v3;
  v[3] = v4;
  v[4] = v5;
  v[5] = v6;

  // 对v进行排序
  for (size_t i = 0; i < 6; i++) {
    for (size_t j = i + 1; j < 6; j++) {
      if (v[i] > v[j]) {
        int32_t tmp = v[i];
        v[i]        = v[j];
        v[j]        = tmp;
      }
    }
  }

  int expect = (v[2] + v[3]) / 2;
  if (abs(input) > abs(expect * 2) || abs(input) < abs(expect / 2)) {
    return expect;
  } else {
    return input;
  }
}

static bool filterVoice(int32_t *voice, int32_t size) {
#if 1
  static int32_t vbuf[MAX_ONE_FRAME_VOICE_SIZE * 3];  // 50ms
  static int32_t vbufnum = 0;
  vbufnum++;

  memmove(vbuf, vbuf + size, 2 * size * sizeof(int32_t));
  memcpy(vbuf + 2 * size, voice, size * sizeof(int32_t));
  // printf("size:%d\n", size);

  if (vbufnum < 3) {
    return false;
  }

  int32_t *cachebegin = &vbuf[size];
  for (int i = 0; i < size; i++) {
    voice[i] = filter(*(cachebegin + i),      //
                      *(cachebegin + i - 1),  //
                      *(cachebegin + i - 2),  //
                      *(cachebegin + i - 3),  //
                      *(cachebegin + i + 1),  //
                      *(cachebegin + i + 2),  //
                      *(cachebegin + i + 3));
    // voice[i] = *(cachebegin + i);
  }
  return true;
#endif
#if 0
  static int32_t vbuf[MAX_ONE_FRAME_VOICE_SIZE];  // 50ms
  memcpy(vbuf, voice, size * sizeof(int32_t));

  int32_t *cachebegin = vbuf;

  for (int32_t i = 3; i < size - 3; i++) {
    voice[i] = filter(voice[i], *(cachebegin + i - 1), *(cachebegin + i - 2), *(cachebegin + i - 3), *(cachebegin + i + 1), *(cachebegin + i + 2),
                      *(cachebegin + i + 3));
    // voice[i] = cachebegin[i];
  }
#endif
  return true;
}

error_code_t spisound_init(const char *deviceName, int csgpionameId, spisound_cb_t callback) {
  m_callback   = callback;
  m_cs_gpio_id = csgpionameId;
  //
  m_fd = open(deviceName, O_RDWR);
  if (m_fd < 0) {
    m_sys_error  = errno;
    m_last_error = ksp_init_spi_error_open_fail;
    return ksp_init_spi_error_open_fail;
  }
  // 设置模式
  int mode  = SPI_MODE_3;
  int bits  = 8;
  int speed = 3000000;

  // SPI初始化
  IOCTL(m_fd, SPI_IOC_WR_MODE, mode, ksp_init_spi_error_set_mode_fail);
  IOCTL(m_fd, SPI_IOC_WR_BITS_PER_WORD, bits, ksp_init_spi_error_set_mode_fail);
  IOCTL(m_fd, SPI_IOC_WR_MAX_SPEED_HZ, speed, ksp_init_spi_error_set_mode_fail);

  char cmd[256] = {0};
  sprintf(cmd, "echo %d 1> /sys/class/gpio/export 2>/dev/null", m_cs_gpio_id);
  system(cmd);

  sprintf(cmd, "echo out > /sys/class/gpio/gpio%d/direction", m_cs_gpio_id);
  system(cmd);

  sprintf(cmd, "/sys/class/gpio/gpio%d/value", m_cs_gpio_id);
  m_gpiofd = open(cmd, O_RDWR);
  if (m_gpiofd < 0) {
    m_sys_error  = errno;
    m_last_error = ksp_init_spi_cs_pin_open_fail;
    return ksp_init_spi_cs_pin_open_fail;
  }
  inited = true;
  return ksp_success;
}

static void *sound_read_thread_func(void *v) {
  while (true) {
    // 一次系统调用时间大概500us
    readvoice();
    usleep(10000);
  }
  return NULL;
}

error_code_t spisound_start_capture(void) {
  //
  if (!inited) {
    return ksp_module_not_inited;
  }

  if (started) {
    return ksp_module_has_started;
  }

  int ret = pthread_create(&sound_read_thread, NULL, sound_read_thread_func, NULL);
  if (ret != 0) {
    m_sys_error = errno;
    return ksp_create_thread_fail;
  }

  started = true;
  return ksp_success;
}

static void readvoice() {
  static int32_t voicebuf[MAX_ONE_FRAME_VOICE_SIZE];  // 50ms
  static int32_t rxdatabytesize;                      //

  rxdatabytesize = 0;
  memset(voicebuf, 0, sizeof(voicebuf));

  gpio_set_value(0);
  // usleep(1500);  // CS信号触发后，给单片机足够的时间准备音频数据
  int ret = read(m_fd, &rxdatabytesize, 4);
  // printf("%d\n",rxdatabytesize);
  // printf("read rxdatabytesize:%d\n", rxdatabytesize);
  if (rxdatabytesize <= 0) {
    gpio_set_value(1);
    return;
  }
  if (rxdatabytesize > MAX_ONE_FRAME_VOICE_SIZE * 4) {
    printf("WARNING: rxdatabytesize(%d) > voiceBufSize \n", rxdatabytesize);
    gpio_set_value(1);
    return;
  }

  ret = read(m_fd, voicebuf, rxdatabytesize);
  gpio_set_value(1);

  static int count = 0;
  count++;
  if (count < 10) {
    return;
  }

  int32_t voiceframesize = 0;
  bool    ready          = filterVoice(voicebuf, rxdatabytesize / 4);
  if (ready) {
    audio_t v1 = multiplierX2(voicebuf, rxdatabytesize / 4);
    audio_t v2 = multiplierX2_2(v1.voice, v1.size);
    if (m_callback) m_callback(v2.voice, v2.size);
    // if (m_callback) m_callback(voicebuf, rxdatabytesize / 4);
  }

  return;
}

void spisound_dump_last_error() {
  printf("----------------spisound_dump_last_error-----------------\n");
  printf("- module error: %s(%d)\n", error_code_to_string(m_last_error), m_last_error);
  printf("- sys_ error  : %s(%d)\n", strerror(m_sys_error), m_sys_error);
  printf("-\n");
}

const char *spisound_libversion() { return "1.0.0"; }