pinebuds/services/multimedia/audio/process/anc/cfg/anc_cfg.c

/***************************************************************************
 *
 * Copyright 2015-2019 BES.
 * All rights reserved. All unpublished rights reserved.
 *
 * No part of this work may be used or reproduced in any form or by any
 * means, or stored in a database or retrieval system, without prior written
 * permission of BES.
 *
 * Use of this work is governed by a license granted by BES.
 * This work contains confidential and proprietary information of
 * BES. which is protected by copyright, trade secret,
 * trademark and other intellectual property rights.
 *
 ****************************************************************************/
#include "anc_process.h"
#include "aud_section.h"
#include "aud_section_inc.h"
#include "hal_location.h"
#include "hal_sysfreq.h"
#include "hal_trace.h"
#include "plat_types.h"
#include "tgt_hardware.h"

#ifdef USB_ANC_MC_EQ_TUNING
#include "analog.h"
#include "audioflinger.h"
#include "crc32.h"
#include "hal_aud.h"
#include "hal_cmu.h"
#include "hal_codec.h"
#include "hal_dma.h"
#include "hal_iomux.h"
#include "hal_norflash.h"
#include "hal_timer.h"
#include "hw_codec_iir_process.h"
#include "hw_iir_process.h"
#include "pmu.h"
#include "stdint.h"
#include "string.h"
#endif

#ifdef AUDIO_ANC_FB_MC
#include "cmsis.h"
#endif

#ifdef ANC_COEF_LIST_NUM
#if (ANC_COEF_LIST_NUM < 1)
#error "Invalid ANC_COEF_LIST_NUM configuration"
#endif
#else
#define ANC_COEF_LIST_NUM (1)
#endif

extern const struct_anc_cfg *anc_coef_list_50p7k[ANC_COEF_LIST_NUM];
extern const struct_anc_cfg *anc_coef_list_48k[ANC_COEF_LIST_NUM];
extern const struct_anc_cfg *anc_coef_list_44p1k[ANC_COEF_LIST_NUM];

const struct_anc_cfg *WEAK anc_coef_list_50p7k[ANC_COEF_LIST_NUM] = {};
const struct_anc_cfg *WEAK anc_coef_list_48k[ANC_COEF_LIST_NUM] = {};
const struct_anc_cfg *WEAK anc_coef_list_44p1k[ANC_COEF_LIST_NUM] = {};

static enum ANC_INDEX cur_coef_idx = ANC_INDEX_0;
static enum AUD_SAMPRATE_T cur_coef_samprate;

#ifdef AUDIO_ANC_FB_MC
#define AUD_IIR_NUM (8)

typedef struct {
  aud_item anc_cfg_mc_l;
  aud_item anc_cfg_mc_r;
  float mc_history_l[AUD_IIR_NUM][4];
  float mc_history_r[AUD_IIR_NUM][4];
} IIR_MC_CFG_T;

static IIR_MC_CFG_T mc_iir_cfg;

#endif

#ifndef CODEC_OUTPUT_DEV
#define CODEC_OUTPUT_DEV CFG_HW_AUD_OUTPUT_PATH_SPEAKER_DEV
#endif

int anc_load_cfg(void) {
  int res = 0;
  const struct_anc_cfg **list;

  anc_set_ch_map(CODEC_OUTPUT_DEV);

#ifdef __AUDIO_RESAMPLE__
  res = anccfg_loadfrom_audsec(anc_coef_list_50p7k, anc_coef_list_48k,
                               ANC_COEF_LIST_NUM);
  list = anc_coef_list_50p7k;
  TRACE(0, "50.7k!!!!");

  if (res) {
    TRACE(
        2,
        "[%s] WARNING(%d): Can not load anc coefficient from audio section!!!",
        __func__, res);
  } else {
    TRACE(1, "[%s] Load anc coefficient from audio section.", __func__);
#if (AUD_SECTION_STRUCT_VERSION == 1)
    TRACE(5, "[%s] L: gain = %d, len = %d, dac = %d, adc = %d", __func__,
          list[0]->anc_cfg_ff_l.total_gain, list[0]->anc_cfg_ff_l.fir_len,
          list[0]->anc_cfg_ff_l.dac_gain_offset,
          list[0]->anc_cfg_ff_l.adc_gain_offset);
    TRACE(5, "[%s] R: gain = %d, len = %d, dac = %d, adc = %d", __func__,
          list[0]->anc_cfg_ff_r.total_gain, list[0]->anc_cfg_ff_r.fir_len,
          list[0]->anc_cfg_ff_r.dac_gain_offset,
          list[0]->anc_cfg_ff_r.adc_gain_offset);
#elif (AUD_SECTION_STRUCT_VERSION == 2)
    for (int i = 0; i < ANC_COEF_LIST_NUM; i++) {
      TRACE(3, "appmode%d,FEEDFORWARD,L:gain %d,R:gain %d", i,
            list[i]->anc_cfg_ff_l.total_gain, list[i]->anc_cfg_ff_r.total_gain);
      for (int j = 0; j < AUD_IIR_NUM; j++) {
        TRACE(7,
              "appmode%d,iir coef ff 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, "
              "0x%08x",
              i, list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[0],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[1],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[2],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[0],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[1],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[2]);
      }

      TRACE(3, "appmode%d,FEEDBACK,L:gain %d,R:gain %d", i,
            list[i]->anc_cfg_fb_l.total_gain, list[i]->anc_cfg_fb_r.total_gain);
      for (int j = 0; j < AUD_IIR_NUM; j++) {
        TRACE(7,
              "appmode%d,iir coef fb 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, "
              "0x%08x",
              i, list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[0],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[1],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[2],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[0],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[1],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[2]);
      }
    }
#elif (AUD_SECTION_STRUCT_VERSION == 3)
    for (int i = 0; i < ANC_COEF_LIST_NUM; i++) {
      TRACE(2, "appmode%d,FEEDFORWARD,L:gain %d", i,
            list[i]->anc_cfg_ff_l.total_gain);
      for (int j = 0; j < AUD_IIR_NUM; j++) {
        TRACE(7,
              "appmode%d,iir coef ff 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, "
              "0x%08x",
              i, list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[0],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[1],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[2],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[0],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[1],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[2]);
      }

      TRACE(2, "appmode%d,FEEDBACK,L:gain %d", i,
            list[i]->anc_cfg_fb_l.total_gain);
      for (int j = 0; j < AUD_IIR_NUM; j++) {
        TRACE(7,
              "appmode%d,iir coef fb 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, "
              "0x%08x",
              i, list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[0],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[1],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[2],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[0],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[1],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[2]);
      }
    }
#endif
  }

#else
  res = anccfg_loadfrom_audsec(anc_coef_list_48k, anc_coef_list_44p1k,
                               ANC_COEF_LIST_NUM);
  list = anc_coef_list_44p1k;
  TRACE(0, "44.1k!!!!");

  if (res) {
    TRACE(
        2,
        "[%s] WARNING(%d): Can not load anc coefficient from audio section!!!",
        __func__, res);
  } else {
    TRACE(1, "[%s] Load anc coefficient from audio section.", __func__);
#if (AUD_SECTION_STRUCT_VERSION == 1)
    TRACE(5, "[%s] L: gain = %d, len = %d, dac = %d, adc = %d", __func__,
          list[0]->anc_cfg_ff_l.total_gain, list[0]->anc_cfg_ff_l.fir_len,
          list[0]->anc_cfg_ff_l.dac_gain_offset,
          list[0]->anc_cfg_ff_l.adc_gain_offset);
    TRACE(5, "[%s] R: gain = %d, len = %d, dac = %d, adc = %d", __func__,
          list[0]->anc_cfg_ff_r.total_gain, list[0]->anc_cfg_ff_r.fir_len,
          list[0]->anc_cfg_ff_r.dac_gain_offset,
          list[0]->anc_cfg_ff_r.adc_gain_offset);
#elif (AUD_SECTION_STRUCT_VERSION == 2)
    for (int i = 0; i < ANC_COEF_LIST_NUM; i++) {
      TRACE(3, "appmode%d,FEEDFORWARD,L:gain %d,R:gain %d", i,
            list[i]->anc_cfg_ff_l.total_gain, list[i]->anc_cfg_ff_r.total_gain);
      for (int j = 0; j < AUD_IIR_NUM; j++) {
        TRACE(7,
              "appmode%d,iir coef ff 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, "
              "0x%08x",
              i, list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[0],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[1],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[2],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[0],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[1],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[2]);
      }

      TRACE(3, "appmode%d,FEEDBACK,L:gain %d,R:gain %d", i,
            list[i]->anc_cfg_fb_l.total_gain, list[i]->anc_cfg_fb_r.total_gain);
      for (int j = 0; j < AUD_IIR_NUM; j++) {
        TRACE(7,
              "appmode%d,iir coef fb 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, "
              "0x%08x",
              i, list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[0],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[1],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[2],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[0],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[1],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[2]);
      }
    }
#elif (AUD_SECTION_STRUCT_VERSION == 3)
    for (int i = 0; i < ANC_COEF_LIST_NUM; i++) {
      TRACE(2, "appmode%d,FEEDFORWARD,L:gain %d", i,
            list[i]->anc_cfg_ff_l.total_gain);
      for (int j = 0; j < AUD_IIR_NUM; j++) {
        TRACE(7,
              "appmode%d,iir coef ff 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, "
              "0x%08x",
              i, list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[0],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[1],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[2],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[0],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[1],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[2]);
      }

      TRACE(2, "appmode%d,FEEDBACK,L:gain %d", i,
            list[i]->anc_cfg_fb_l.total_gain);
      for (int j = 0; j < AUD_IIR_NUM; j++) {
        TRACE(7,
              "appmode%d,iir coef fb 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, "
              "0x%08x",
              i, list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[0],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[1],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[2],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[0],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[1],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[2]);
      }
    }

#endif
  }

  res = anccfg_loadfrom_audsec(anc_coef_list_48k, anc_coef_list_44p1k,
                               ANC_COEF_LIST_NUM);
  list = anc_coef_list_48k;
  TRACE(0, "48k!!!!");

  if (res) {
    TRACE(
        2,
        "[%s] WARNING(%d): Can not load anc coefficient from audio section!!!",
        __func__, res);
  } else {
    TRACE(1, "[%s] Load anc coefficient from audio section.", __func__);
#if (AUD_SECTION_STRUCT_VERSION == 1)
    TRACE(5, "[%s] L: gain = %d, len = %d, dac = %d, adc = %d", __func__,
          list[0]->anc_cfg_ff_l.total_gain, list[0]->anc_cfg_ff_l.fir_len,
          list[0]->anc_cfg_ff_l.dac_gain_offset,
          list[0]->anc_cfg_ff_l.adc_gain_offset);
    TRACE(5, "[%s] R: gain = %d, len = %d, dac = %d, adc = %d", __func__,
          list[0]->anc_cfg_ff_r.total_gain, list[0]->anc_cfg_ff_r.fir_len,
          list[0]->anc_cfg_ff_r.dac_gain_offset,
          list[0]->anc_cfg_ff_r.adc_gain_offset);
#elif (AUD_SECTION_STRUCT_VERSION == 2)
    for (int i = 0; i < ANC_COEF_LIST_NUM; i++) {
      TRACE(3, "appmode%d,FEEDFORWARD,L:gain %d,R:gain %d", i,
            list[i]->anc_cfg_ff_l.total_gain, list[i]->anc_cfg_ff_r.total_gain);
      for (int j = 0; j < AUD_IIR_NUM; j++) {
        TRACE(7,
              "appmode%d,iir coef ff 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, "
              "0x%08x",
              i, list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[0],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[1],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[2],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[0],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[1],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[2]);
      }

      TRACE(3, "appmode%d,FEEDBACK,L:gain %d,R:gain %d", i,
            list[i]->anc_cfg_fb_l.total_gain, list[i]->anc_cfg_fb_r.total_gain);
      for (int j = 0; j < AUD_IIR_NUM; j++) {
        TRACE(7,
              "appmode%d,iir coef fb 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, "
              "0x%08x",
              i, list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[0],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[1],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[2],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[0],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[1],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[2]);
      }
    }
#elif (AUD_SECTION_STRUCT_VERSION == 3)
    for (int i = 0; i < ANC_COEF_LIST_NUM; i++) {
      TRACE(2, "appmode%d,FEEDFORWARD,L:gain %d", i,
            list[i]->anc_cfg_ff_l.total_gain);
      for (int j = 0; j < AUD_IIR_NUM; j++) {
        TRACE(7,
              "appmode%d,iir coef ff 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, "
              "0x%08x",
              i, list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[0],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[1],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_b[2],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[0],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[1],
              list[i]->anc_cfg_ff_l.iir_coef[j].coef_a[2]);
      }

      TRACE(2, "appmode%d,FEEDBACK,L:gain %d", i,
            list[i]->anc_cfg_fb_l.total_gain);
      for (int j = 0; j < AUD_IIR_NUM; j++) {
        TRACE(7,
              "appmode%d,iir coef fb 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, "
              "0x%08x",
              i, list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[0],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[1],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_b[2],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[0],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[1],
              list[i]->anc_cfg_fb_l.iir_coef[j].coef_a[2]);
      }
    }
#endif
  }
#endif
  return res;
}

int anc_select_coef(enum AUD_SAMPRATE_T rate, enum ANC_INDEX index,
                    enum ANC_TYPE_T anc_type, ANC_GAIN_TIME anc_gain_delay) {
  const struct_anc_cfg **list = NULL;

  if (index >= ANC_COEF_LIST_NUM) {
    return 1;
  }

#ifdef CHIP_BEST1000
  switch (rate) {
  case AUD_SAMPRATE_96000:
    list = anc_coef_list_48k;
    break;

  case AUD_SAMPRATE_88200:
    list = anc_coef_list_44p1k;
    break;

  default:
    break;
  }

#else
  switch (rate) {
  case AUD_SAMPRATE_48000:
    list = anc_coef_list_48k;
    break;

  case AUD_SAMPRATE_44100:
    list = anc_coef_list_44p1k;
    break;

#ifdef __AUDIO_RESAMPLE__
  case AUD_SAMPRATE_50781:
    list = anc_coef_list_50p7k;
    break;
#endif

  default:
    break;
  }
#endif

  ASSERT(list != NULL && list[index] != NULL,
         "The coefs of Samprate %d is NULL", rate);

  if (anc_opened(anc_type)) {
    hal_sysfreq_req(HAL_SYSFREQ_USER_ANC, HAL_CMU_FREQ_52M);
    anc_set_cfg(list[index], anc_type, anc_gain_delay);
    hal_sysfreq_req(HAL_SYSFREQ_USER_ANC, HAL_CMU_FREQ_32K);
#ifdef AUDIO_ANC_FB_MC
    mc_iir_cfg.anc_cfg_mc_l = (*list[index]).anc_cfg_mc_l;
    mc_iir_cfg.anc_cfg_mc_r = (*list[index]).anc_cfg_mc_r;
#endif
  }

  cur_coef_idx = index;
  cur_coef_samprate = rate;

  return 0;
}

enum ANC_INDEX anc_get_current_coef_index(void) { return cur_coef_idx; }

enum AUD_SAMPRATE_T anc_get_current_coef_samplerate(void) {
  return cur_coef_samprate;
}

#ifdef AUDIO_ANC_FB_MC

void anc_mc_run_init(enum AUD_SAMPRATE_T rate) {
  const struct_anc_cfg **list = NULL;

  switch (rate) {
  case AUD_SAMPRATE_48000:
    list = anc_coef_list_48k;
    break;

  case AUD_SAMPRATE_44100:
    list = anc_coef_list_44p1k;
    break;

#ifdef __AUDIO_RESAMPLE__
  case AUD_SAMPRATE_50781:
    list = anc_coef_list_50p7k;
    break;
#endif
  default:
    break;
  }

  ASSERT(list != NULL && list[cur_coef_idx] != NULL,
         "The coefs of Samprate %d is NULL", rate);

  mc_iir_cfg.anc_cfg_mc_l = (*list[cur_coef_idx]).anc_cfg_mc_l;
  mc_iir_cfg.anc_cfg_mc_r = (*list[cur_coef_idx]).anc_cfg_mc_r;

  for (int j = 0; j < AUD_IIR_NUM; j++) {
    for (int i = 0; i < 4; i++) {
      mc_iir_cfg.mc_history_l[j][i] = 0.0f;
      mc_iir_cfg.mc_history_r[j][i] = 0.0f;
    }
  }
  return;
}

void anc_mc_run_setup(enum AUD_SAMPRATE_T rate) {
  const struct_anc_cfg **list = NULL;

  switch (rate) {
  case AUD_SAMPRATE_48000:
    list = anc_coef_list_48k;
    break;

  case AUD_SAMPRATE_44100:
    list = anc_coef_list_44p1k;
    break;

#ifdef __AUDIO_RESAMPLE__
  case AUD_SAMPRATE_50781:
    list = anc_coef_list_50p7k;
    break;
#endif
  default:
    break;
  }

  ASSERT(list != NULL && list[cur_coef_idx] != NULL,
         "The coefs of Samprate %d is NULL", rate);

  mc_iir_cfg.anc_cfg_mc_l = (*list[cur_coef_idx]).anc_cfg_mc_l;
  mc_iir_cfg.anc_cfg_mc_r = (*list[cur_coef_idx]).anc_cfg_mc_r;

  return;
}

static inline int32_t iir_ssat_24bits(float in) {
  int res = 0;
  int32_t out;

  res = (int)in;
  out = __SSAT(res, 24);

  return out;
}

static inline int32_t iir_ssat_16bits(float in) {
  int res = 0;
  int32_t out;

  res = (int)in;
  out = __SSAT(res, 16);

  return out;
}

SRAM_TEXT_LOC int anc_mc_run_stereo(uint8_t *buf, int len, float left_gain,
                                    float right_gain,
                                    enum AUD_BITS_T sample_bit) {
  int len_mono;
  float gain_l = 0, gain_r = 0;
  int32_t *coefs = NULL;
  float *history = NULL;

  float x0, x1, x2;
  float y0, y1, y2;

  // Coefs
  float a0, a1, a2;
  float b0, b1, b2;

  // ASSERT(mc_iir_cfg.anc_cfg_mc_l.iir_counter==mc_iir_cfg.anc_cfg_mc_r.iir_counter,
  // "mc need the same counter in left and right ch L:%d,R:%d",
  //         mc_iir_cfg.anc_cfg_mc_l.iir_counter,mc_iir_cfg.anc_cfg_mc_r.iir_counter);

  if (sample_bit == AUD_BITS_16) {
    int16_t *iir_buf;

    len_mono = len >> 2;

    gain_l = (mc_iir_cfg.anc_cfg_mc_l.total_gain / 512.0f) * left_gain;

    if (mc_iir_cfg.anc_cfg_mc_l.iir_counter == 0 ||
        mc_iir_cfg.anc_cfg_mc_l.iir_bypass_flag) {
      iir_buf = (int16_t *)buf;

      for (int j = 0; j < len_mono; j++) {
        x0 = *iir_buf * gain_l;
        *iir_buf++ = iir_ssat_16bits(x0);
        iir_buf++;
      }
    } else {
      for (int i = 0; i < mc_iir_cfg.anc_cfg_mc_l.iir_counter; i++) {
        // Coef
        coefs = mc_iir_cfg.anc_cfg_mc_l.iir_coef[i].coef_a;
        a0 = *coefs++;
        a1 = *coefs++;
        a2 = *coefs;
        coefs = mc_iir_cfg.anc_cfg_mc_l.iir_coef[i].coef_b;
        b0 = *coefs++;
        b1 = *coefs++;
        b2 = *coefs;

        a1 = a1 / a0;
        a2 = a2 / a0;
        b0 = b0 / a0;
        b1 = b1 / a0;
        b2 = b2 / a0;

        // TRACE(7,"[%d] %f, %f, %f, %f, %f, %f", i, a0, a1, a2, b0, b1, b2);

        // Left
        history = mc_iir_cfg.mc_history_l[i];
        x1 = *history++;
        x2 = *history++;
        y1 = *history++;
        y2 = *history;

        iir_buf = (int16_t *)buf;
        if (i == 0) {
          for (int j = 0; j < len_mono; j++) {
            // Left channel
            x0 = *iir_buf * gain_l;
            y0 = x0 * b0 + x1 * b1 + x2 * b2 - y1 * a1 - y2 * a2;
            y2 = y1;
            y1 = y0;
            x2 = x1;
            x1 = x0;
            *iir_buf++ = iir_ssat_16bits(y0);
            iir_buf++;
          }
        } else {
          iir_buf++;
          for (int j = 0; j < len_mono; j++) {
            // Left channel
            x0 = *iir_buf;
            y0 = x0 * b0 + x1 * b1 + x2 * b2 - y1 * a1 - y2 * a2;
            y2 = y1;
            y1 = y0;
            x2 = x1;
            x1 = x0;
            *iir_buf++ = iir_ssat_16bits(y0);
            iir_buf++;
          }
        }
        // Left
        history = mc_iir_cfg.mc_history_l[i];
        *history++ = x1;
        *history++ = x2;
        *history++ = y1;
        *history = y2;
      }
    }

    gain_r = (mc_iir_cfg.anc_cfg_mc_r.total_gain / 512.0f) * right_gain;

    if (mc_iir_cfg.anc_cfg_mc_r.iir_counter == 0 ||
        mc_iir_cfg.anc_cfg_mc_r.iir_bypass_flag) {
      iir_buf = (int16_t *)buf;
      iir_buf++;
      for (int j = 0; j < len_mono; j++) {
        x0 = *iir_buf * gain_r;
        *iir_buf++ = iir_ssat_16bits(x0);
        iir_buf++;
      }
    } else {
      for (int i = 0; i < mc_iir_cfg.anc_cfg_mc_r.iir_counter; i++) {
        // Coef
        coefs = mc_iir_cfg.anc_cfg_mc_r.iir_coef[i].coef_a;
        a0 = *coefs++;
        a1 = *coefs++;
        a2 = *coefs;
        coefs = mc_iir_cfg.anc_cfg_mc_r.iir_coef[i].coef_b;
        b0 = *coefs++;
        b1 = *coefs++;
        b2 = *coefs;

        a1 = a1 / a0;
        a2 = a2 / a0;
        b0 = b0 / a0;
        b1 = b1 / a0;
        b2 = b2 / a0;

        // TRACE(7,"[%d] %f, %f, %f, %f, %f, %f", i, a0, a1, a2, b0, b1, b2);

        // right
        history = mc_iir_cfg.mc_history_r[i];
        x1 = *history++;
        x2 = *history++;
        y1 = *history++;
        y2 = *history;

        iir_buf = (int16_t *)buf;
        iir_buf++;
        if (i == 0) {
          for (int j = 0; j < len_mono; j++) {
            // right channel
            x0 = *iir_buf * gain_r;
            y0 = x0 * b0 + x1 * b1 + x2 * b2 - y1 * a1 - y2 * a2;
            y2 = y1;
            y1 = y0;
            x2 = x1;
            x1 = x0;
            *iir_buf++ = iir_ssat_16bits(y0);
            iir_buf++;
          }
        } else {
          for (int j = 0; j < len_mono; j++) {
            // right channel
            x0 = *iir_buf;
            y0 = x0 * b0 + x1 * b1 + x2 * b2 - y1 * a1 - y2 * a2;
            y2 = y1;
            y1 = y0;
            x2 = x1;
            x1 = x0;
            *iir_buf++ = iir_ssat_16bits(y0);
            iir_buf++;
          }
        }
        // right
        history = mc_iir_cfg.mc_history_r[i];
        *history++ = x1;
        *history++ = x2;
        *history++ = y1;
        *history = y2;
      }
    }

  } else if (sample_bit == AUD_BITS_24) {
    int32_t *iir_buf;

    len_mono = len >> 3;

    gain_l = (mc_iir_cfg.anc_cfg_mc_l.total_gain / 512.0f) * left_gain;

    if (mc_iir_cfg.anc_cfg_mc_l.iir_counter == 0 ||
        mc_iir_cfg.anc_cfg_mc_l.iir_bypass_flag) {
      iir_buf = (int32_t *)buf;
      for (int j = 0; j < len_mono; j++) {
        x0 = *iir_buf * gain_l;
        *iir_buf++ = iir_ssat_24bits(x0);
        iir_buf++;
      }
    } else {
      for (int i = 0; i < mc_iir_cfg.anc_cfg_mc_l.iir_counter; i++) {
        // Coef
        coefs = mc_iir_cfg.anc_cfg_mc_l.iir_coef[i].coef_a;
        a0 = *coefs++;
        a1 = *coefs++;
        a2 = *coefs;
        coefs = mc_iir_cfg.anc_cfg_mc_l.iir_coef[i].coef_b;
        b0 = *coefs++;
        b1 = *coefs++;
        b2 = *coefs;

        a1 = a1 / a0;
        a2 = a2 / a0;
        b0 = b0 / a0;
        b1 = b1 / a0;
        b2 = b2 / a0;

        // TRACE(7,"[%d] %f, %f, %f, %f, %f, %f", i, a0, a1, a2, b0, b1, b2);

        // Left
        history = mc_iir_cfg.mc_history_l[i];
        x1 = *history++;
        x2 = *history++;
        y1 = *history++;
        y2 = *history;

        iir_buf = (int32_t *)buf;
        if (i == 0) {
          for (int j = 0; j < len_mono; j++) {
            // Left channel
            x0 = *iir_buf * gain_l;
            y0 = x0 * b0 + x1 * b1 + x2 * b2 - y1 * a1 - y2 * a2;
            y2 = y1;
            y1 = y0;
            x2 = x1;
            x1 = x0;
            *iir_buf++ = iir_ssat_24bits(y0);
            iir_buf++;
          }
        } else {
          for (int j = 0; j < len_mono; j++) {
            // Left channel
            x0 = *iir_buf;
            y0 = x0 * b0 + x1 * b1 + x2 * b2 - y1 * a1 - y2 * a2;
            y2 = y1;
            y1 = y0;
            x2 = x1;
            x1 = x0;
            *iir_buf++ = iir_ssat_24bits(y0);
            iir_buf++;
          }
        }
        // Left
        history = mc_iir_cfg.mc_history_l[i];
        *history++ = x1;
        *history++ = x2;
        *history++ = y1;
        *history = y2;
      }
    }

    gain_r = (mc_iir_cfg.anc_cfg_mc_r.total_gain / 512.0f) * right_gain;
    if (mc_iir_cfg.anc_cfg_mc_r.iir_counter == 0 ||
        mc_iir_cfg.anc_cfg_mc_r.iir_bypass_flag) {
      iir_buf = (int32_t *)buf;
      iir_buf++;
      for (int j = 0; j < len_mono; j++) {
        x0 = *iir_buf * gain_r;
        *iir_buf++ = iir_ssat_24bits(x0);
        iir_buf++;
      }

    } else {
      for (int i = 0; i < mc_iir_cfg.anc_cfg_mc_r.iir_counter; i++) {
        // Coef
        coefs = mc_iir_cfg.anc_cfg_mc_r.iir_coef[i].coef_a;
        a0 = *coefs++;
        a1 = *coefs++;
        a2 = *coefs;
        coefs = mc_iir_cfg.anc_cfg_mc_r.iir_coef[i].coef_b;
        b0 = *coefs++;
        b1 = *coefs++;
        b2 = *coefs;

        a1 = a1 / a0;
        a2 = a2 / a0;
        b0 = b0 / a0;
        b1 = b1 / a0;
        b2 = b2 / a0;

        // TRACE(7,"[%d] %f, %f, %f, %f, %f, %f", i, a0, a1, a2, b0, b1, b2);

        // right
        history = mc_iir_cfg.mc_history_r[i];
        x1 = *history++;
        x2 = *history++;
        y1 = *history++;
        y2 = *history;

        iir_buf = (int32_t *)buf;
        iir_buf++;
        if (i == 0) {
          for (int j = 0; j < len_mono; j++) {
            // right channel
            x0 = *iir_buf * gain_r;
            y0 = x0 * b0 + x1 * b1 + x2 * b2 - y1 * a1 - y2 * a2;
            y2 = y1;
            y1 = y0;
            x2 = x1;
            x1 = x0;
            *iir_buf++ = iir_ssat_24bits(y0);
            iir_buf++;
          }
        } else {
          for (int j = 0; j < len_mono; j++) {
            // right channel
            x0 = *iir_buf;
            y0 = x0 * b0 + x1 * b1 + x2 * b2 - y1 * a1 - y2 * a2;
            y2 = y1;
            y1 = y0;
            x2 = x1;
            x1 = x0;
            *iir_buf++ = iir_ssat_24bits(y0);
            iir_buf++;
          }
        }
        // right
        history = mc_iir_cfg.mc_history_r[i];
        *history++ = x1;
        *history++ = x2;
        *history++ = y1;
        *history = y2;
      }
    }

  } else {
    ASSERT(false, "Can't support sample bit mode:%d", sample_bit);
  }

  return 0;
}

SRAM_TEXT_LOC int anc_mc_run_mono(uint8_t *buf, int len, float left_gain,
                                  enum AUD_BITS_T sample_bit) {
  int len_mono;
  int num;
  float gain_l = 0;
  int32_t *coefs = NULL;
  float *history = NULL;

  // Left
  float x0, x1, x2;
  float y0, y1, y2;

  // Coefs
  float POSSIBLY_UNUSED a0, a1, a2;
  float b0, b1, b2;

  if (sample_bit == AUD_BITS_16) {
    int16_t *iir_buf;

    len_mono = len >> 1;

    gain_l = (mc_iir_cfg.anc_cfg_mc_l.total_gain / 512.0f) * left_gain;
    num = mc_iir_cfg.anc_cfg_mc_l.iir_counter;

    if (num == 0 || mc_iir_cfg.anc_cfg_mc_l.iir_bypass_flag) {
      iir_buf = (int16_t *)buf;

      for (int j = 0; j < len_mono; j++) {
        x0 = *iir_buf * gain_l;
        *iir_buf++ = iir_ssat_16bits(x0);
      }

      return 0;
    }

    for (int i = 0; i < num; i++) {
      // Coef
      coefs = mc_iir_cfg.anc_cfg_mc_l.iir_coef[i].coef_a;
      a0 = *coefs++;
      a1 = *coefs++;
      a2 = *coefs;
      coefs = mc_iir_cfg.anc_cfg_mc_l.iir_coef[i].coef_b;
      b0 = *coefs++;
      b1 = *coefs++;
      b2 = *coefs;

      a1 = a1 / a0;
      a2 = a2 / a0;
      b0 = b0 / a0;
      b1 = b1 / a0;
      b2 = b2 / a0;

      //      TRACE(7,"[%d] %f, %f, %f, %f, %f, %f", i, a0, a1, a2, b0, b1, b2);

      // Left
      history = mc_iir_cfg.mc_history_l[i];
      x1 = *history++;
      x2 = *history++;
      y1 = *history++;
      y2 = *history;

      iir_buf = (int16_t *)buf;
      if (i == 0) {
        for (int j = 0; j < len_mono; j++) {
          // Left channel
          x0 = *iir_buf * gain_l;
          y0 = x0 * b0 + x1 * b1 + x2 * b2 - y1 * a1 - y2 * a2;
          y2 = y1;
          y1 = y0;
          x2 = x1;
          x1 = x0;
          *iir_buf++ = iir_ssat_16bits(y0);
        }
      } else {
        for (int j = 0; j < len_mono; j++) {
          // Left channel
          x0 = *iir_buf;
          y0 = x0 * b0 + x1 * b1 + x2 * b2 - y1 * a1 - y2 * a2;
          y2 = y1;
          y1 = y0;
          x2 = x1;
          x1 = x0;
          *iir_buf++ = iir_ssat_16bits(y0);
        }
      }
      // Left
      history = mc_iir_cfg.mc_history_l[i];
      *history++ = x1;
      *history++ = x2;
      *history++ = y1;
      *history = y2;
    }

  } else if (sample_bit == AUD_BITS_24) {
    int32_t *iir_buf;

    len_mono = len >> 2;

    gain_l = (mc_iir_cfg.anc_cfg_mc_l.total_gain / 512.0f) * left_gain;
    num = mc_iir_cfg.anc_cfg_mc_l.iir_counter;

    if (num == 0) {
      iir_buf = (int32_t *)buf;

      for (int j = 0; j < len_mono; j++) {
        x0 = *iir_buf * gain_l;
        *iir_buf++ = iir_ssat_24bits(x0);
      }

      return 0;
    }

    for (int i = 0; i < num; i++) {
      // Coef
      coefs = mc_iir_cfg.anc_cfg_mc_l.iir_coef[i].coef_a;
      a0 = *coefs++;
      a1 = *coefs++;
      a2 = *coefs;
      coefs = mc_iir_cfg.anc_cfg_mc_l.iir_coef[i].coef_b;
      b0 = *coefs++;
      b1 = *coefs++;
      b2 = *coefs;

      a1 = a1 / a0;
      a2 = a2 / a0;
      b0 = b0 / a0;
      b1 = b1 / a0;
      b2 = b2 / a0;

      //        TRACE(7,"[%d] %f, %f, %f, %f, %f, %f", i, a0, a1, a2, b0, b1,
      //        b2);

      // Left
      history = mc_iir_cfg.mc_history_l[i];
      x1 = *history++;
      x2 = *history++;
      y1 = *history++;
      y2 = *history;

      iir_buf = (int32_t *)buf;
      if (i == 0) {
        for (int j = 0; j < len_mono; j++) {
          // Left channel
          x0 = *iir_buf * gain_l;
          y0 = x0 * b0 + x1 * b1 + x2 * b2 - y1 * a1 - y2 * a2;
          y2 = y1;
          y1 = y0;
          x2 = x1;
          x1 = x0;
          *iir_buf++ = iir_ssat_24bits(y0);
        }
      } else {
        for (int j = 0; j < len_mono; j++) {
          // Left channel
          x0 = *iir_buf;
          y0 = x0 * b0 + x1 * b1 + x2 * b2 - y1 * a1 - y2 * a2;
          y2 = y1;
          y1 = y0;
          x2 = x1;
          x1 = x0;
          *iir_buf++ = iir_ssat_24bits(y0);
        }
      }
      // Left
      history = mc_iir_cfg.mc_history_l[i];
      *history++ = x1;
      *history++ = x2;
      *history++ = y1;
      *history = y2;
    }

  } else {
    ASSERT(false, "Can't support sample bit mode:%d", sample_bit);
  }

  return 0;
}
#endif
#ifdef USB_ANC_MC_EQ_TUNING
struct message_t {
  struct msg_hdr_t {
    unsigned char prefix;
    unsigned char type;
    unsigned char seq;
    unsigned char len;
  } hdr;
  unsigned char data[255];
};

#define PREFIX_CHAR 0xBE

enum MSG_TYPE {
  TYPE_SYS = 0x00,
  TYPE_READ = 0x01,
  TYPE_WRITE = 0x02,
  TYPE_BULK_READ = 0x03,
  TYPE_SYNC = 0x50,
  TYPE_SIG_INFO = 0x51,
  TYPE_SIG = 0x52,
  TYPE_CODE_INFO = 0x53,
  TYPE_CODE = 0x54,
  TYPE_RUN = 0x55,
  TYPE_SECTOR_SIZE = 0x60,
  TYPE_ERASE_BURN_START = 0x61,
  TYPE_ERASE_BURN_DATA = 0x62,
  TYPE_OBSOLETED_63 = 0x63,
  TYPE_OBSOLETED_64 = 0x64,
  TYPE_FLASH_CMD = 0x65,
  TYPE_GET_SECTOR_INFO = 0x66,
  TYPE_SEC_REG_ERASE_BURN_START = 0x67,
  TYPE_SEC_REG_ERASE_BURN_DATA = 0x68,

  // Extended types
  TYPE_PROD_TEST = 0x81,
  TYPE_RUNTIME_CMD = 0x82,
  TYPE_BT_CALIB_CMD = 0x83,
  TYPE_PROTO_EL = 0xA0,

  TYPE_INVALID = 0xFF,
};

enum ERR_CODE {
  ERR_NONE = 0x00,
  ERR_LEN = 0x01,
  ERR_CHECKSUM = 0x02,
  ERR_NOT_SYNC = 0x03,
  ERR_NOT_SEC = 0x04,
  ERR_SYNC_WORD = 0x05,
  ERR_SYS_CMD = 0x06,
  ERR_DATA_ADDR = 0x07,
  ERR_DATA_LEN = 0x08,
  ERR_ACCESS_RIGHT = 0x09,

  ERR_TYPE_INVALID = 0x0F,

  // ERR_BOOT_OK = 0x10,
  ERR_BOOT_MAGIC = 0x11,
  ERR_BOOT_SEC = 0x12,
  ERR_BOOT_HASH_TYPE = 0x13,
  ERR_BOOT_KEY_TYPE = 0x14,
  ERR_BOOT_KEY_LEN = 0x15,
  ERR_BOOT_SIG_LEN = 0x16,
  ERR_BOOT_SIG = 0x17,
  ERR_BOOT_CRC = 0x18,
  ERR_BOOT_LEN = 0x19,
  ERR_SIG_CODE_SIZE = 0x1A,
  ERR_SIG_SIG_LEN = 0x1B,
  ERR_SIG_INFO_MISSING = 0x1C,
  ERR_BOOT_KEY_ID = 0x1D,
  ERR_BOOT_HASH = 0x1E,

  ERR_CODE_OK = 0x20,
  ERR_BOOT_MISSING = 0x21,
  ERR_CODE_SIZE_SIG = 0x22,
  ERR_CODE_ADDR_SIZE = 0x23,
  ERR_CODE_INFO_MISSING = 0x24,
  ERR_CODE_CRC = 0x25,
  ERR_CODE_SIG = 0x26,

  ERR_CODE_MISSING = 0x31,
  ERR_VERSION = 0x32,

  ERR_BURN_OK = 0x60,
  ERR_SECTOR_SIZE = 0x61,
  ERR_SECTOR_SEQ_OVERFLOW = 0x62,
  ERR_BURN_INFO_MISSING = 0x63,
  ERR_SECTOR_DATA_LEN = 0x64,
  ERR_SECTOR_DATA_CRC = 0x65,
  ERR_SECTOR_SEQ = 0x66,
  ERR_ERASE_FLSH = 0x67,
  ERR_BURN_FLSH = 0x68,
  ERR_VERIFY_FLSH = 0x69,
  ERR_FLASH_CMD = 0x6A,

  ERR_TYPE_MISMATCHED = 0xE1,
  ERR_SEQ_MISMATCHED = 0xE2,
  ERR_BUF_TOO_SMALL = 0xE3,

  ERR_INTERNAL = 0xFF,
};

//#define PROGRAMMER_ANC_DEBUG
enum ANC_CMD_T {
  ANC_CMD_CLOSE = 0,
  ANC_CMD_OPEN = 1,
  ANC_CMD_GET_CFG = 2,
  ANC_CMD_APPLY_CFG = 3,
  ANC_CMD_CFG_SETUP = 4,
  ANC_CMD_CHANNEL_SETUP = 5,
  ANC_CMD_SET_SAMP_RATE = 6,
};

enum FLASH_CMD_TYPE {
  FLASH_CMD_ERASE_SECTOR = 0x21,
  FLASH_CMD_BURN_DATA = 0x22,
  FLASH_CMD_ERASE_CHIP = 0x31,
  FLASH_CMD_SEC_REG_ERASE = 0x41,
  FLASH_CMD_SEC_REG_BURN = 0x42,
  FLASH_CMD_SEC_REG_LOCK = 0x43,
  FLASH_CMD_SEC_REG_READ = 0x44,
  FLASH_CMD_ENABLE_REMAP = 0x51,
  FLASH_CMD_DISABLE_REMAP = 0x52,
};

enum PROD_TEST_CMD_T {
  PROD_TEST_CMD_ANC = 0x00000001,
};

static struct_anc_cfg g_anc_config;

#define MAX_READ_DATA_LEN 255
#define MAX_WRITE_DATA_LEN 255

static struct message_t recv_msg;
static struct message_t send_msg = {
    {
        PREFIX_CHAR,
    },
};

#define MAX_SNED_MSG_QUEUE (10)
static unsigned int send_msg_push_seq = 0;
static unsigned int send_msg_pop_seq = 0;
static struct message_t send_msg_queue[MAX_SNED_MSG_QUEUE];

static uint8_t pcsuppt_anc_type = ANC_NOTYPE;
static enum AUD_SAMPRATE_T anc_sample_rate;

static unsigned int burn_addr;
static unsigned int burn_total_len;
static unsigned int sector_size;
static unsigned int sector_cnt;
static unsigned int last_sector_len;
static unsigned int cur_sector_seq;
static unsigned int burn_len;

void anc_set_gpio(enum HAL_GPIO_PIN_T io_pin, bool set_flag) {
  // #define ANC_SET_GPIO_PIN   HAL_IOMUX_PIN_P2_4
  struct HAL_IOMUX_PIN_FUNCTION_MAP pinmux_anc_set_gpio[1] = {
      {io_pin, HAL_IOMUX_FUNC_AS_GPIO, HAL_IOMUX_PIN_VOLTAGE_VIO,
       HAL_IOMUX_PIN_NOPULL},
  };

  hal_iomux_init(pinmux_anc_set_gpio, ARRAY_SIZE(pinmux_anc_set_gpio));
  hal_gpio_pin_set_dir(io_pin, HAL_GPIO_DIR_OUT, 0);
  if (set_flag)
    hal_gpio_pin_set(io_pin);
  else
    hal_gpio_pin_clr(io_pin);
}

#define MSG_TOTAL_LEN(msg) (sizeof((msg)->hdr) + (msg)->hdr.len + 1)

#define TRACE_TIME(num, str, ...)                                              \
  TRACE(1 + num, "[%05u] " str, TICKS_TO_MS(hal_sys_timer_get()), ##__VA_ARGS__)

static unsigned char check_sum(const unsigned char *buf, unsigned char len) {
  int i;
  unsigned char sum = 0;

  for (i = 0; i < len; i++) {
    sum += buf[i];
  }

  return sum;
}
static void trace_stage_info(const char *name) {
  TRACE_TIME(1, "------ %s ------", name);
}
static void trace_rw_len_err(const char *name, unsigned int len) {
  TRACE(2, "[%s] Length error: %u", name, len);
}
static void trace_rw_info(const char *name, unsigned int addr,
                          unsigned int len) {
  // TRACE(3,"[%s] addr=0x%08X len=%u", name, addr, len);
}

static void trace_flash_cmd_info(const char *name) {
  TRACE_TIME(1, "- %s -", name);
}

static void trace_flash_cmd_len_err(const char *name, unsigned int len) {
  TRACE(2, "Invalid %s cmd param len: %u", name, len);
}

static void trace_flash_cmd_err(const char *name) {
  TRACE_TIME(1, "%s failed", name);
}

static void trace_flash_cmd_done(const char *name) {
  TRACE_TIME(1, "%s done", name);
}

int send_reply(const unsigned char *payload, unsigned int len) {
  int ret = 0;

  if (len + 1 > sizeof(send_msg.data)) {
    TRACE(1, "Packet length too long: %u", len);
    return -1;
  }

  send_msg.hdr.type = recv_msg.hdr.type;
  send_msg.hdr.seq = recv_msg.hdr.seq;
  send_msg.hdr.len = len;
  memcpy(&send_msg.data[0], payload, len);
  send_msg.data[len] =
      ~check_sum((unsigned char *)&send_msg, MSG_TOTAL_LEN(&send_msg) - 1);

  send_msg_push_seq = send_msg_push_seq % MAX_SNED_MSG_QUEUE;
  send_msg_queue[send_msg_push_seq] = send_msg;
  send_msg_push_seq++;

  return ret;
}
static enum ERR_CODE handle_read_cmd(unsigned int type, unsigned int addr,
                                     unsigned int len) {
  union {
    unsigned int data[1 + (MAX_READ_DATA_LEN + 3) / 4];
    unsigned char buf[(1 + (MAX_READ_DATA_LEN + 3) / 4) * 4];
  } d;
  int i;

  int cnt;
  unsigned int *p32;
  unsigned short *p16;

  const char *name = NULL;

  if (type == TYPE_READ) {
    name = "READ";
  } else {
    return ERR_INTERNAL;
  }

  if (len > MAX_READ_DATA_LEN) {
    trace_rw_len_err(name, len);
    return ERR_DATA_LEN;
  }

  if (type == TYPE_READ) {
    // Handle half-word and word register reading
    if ((len & 0x03) == 0 && (addr & 0x03) == 0) {
      cnt = len / 4;
      p32 = (unsigned int *)&d.data[1];
      for (i = 0; i < cnt; i++) {
        p32[i] = *((unsigned int *)addr + i);
      }
    } else if ((len & 0x01) == 0 && (addr & 0x01) == 0) {
      cnt = len / 2;
      p16 = (unsigned short *)&d.data[1];
      for (i = 0; i < cnt; i++) {
        p16[i] = *((unsigned short *)addr + i);
      }
    } else {
      memcpy(&d.data[1], (unsigned char *)addr, len);
    }
  }

  d.buf[3] = ERR_NONE;
  send_reply((unsigned char *)&d.buf[3], 1 + len);

  return ERR_NONE;
}
static enum ERR_CODE handle_write_cmd(unsigned int addr, unsigned int len,
                                      unsigned char *wdata) {
  unsigned int data;
  int i;
  int cnt;
  const char *name = "WRITE";

  trace_rw_info(name, addr, len);

  if (len > MAX_WRITE_DATA_LEN) {
    trace_rw_len_err(name, len);
    return ERR_DATA_LEN;
  }
  // Handle half-word and word register writing
  if ((len & 0x03) == 0 && (addr & 0x03) == 0) {
    cnt = len / 4;
    for (i = 0; i < cnt; i++) {
      data = wdata[4 * i] | (wdata[4 * i + 1] << 8) | (wdata[4 * i + 2] << 16) |
             (wdata[4 * i + 3] << 24);
      *((unsigned int *)addr + i) = data;
    }
  } else if ((len & 0x01) == 0 && (addr & 0x01) == 0) {
    cnt = len / 2;
    for (i = 0; i < cnt; i++) {
      data = wdata[2 * i] | (wdata[2 * i + 1] << 8);
      *((unsigned short *)addr + i) = (unsigned short)data;
    }
  } else {
    memcpy((unsigned char *)addr, wdata, len);
  }

  data = ERR_NONE;
  send_reply((unsigned char *)&data, 1);

  return ERR_NONE;
}
static struct_anc_cfg *get_anc_config(void) { return &g_anc_config; }

static void tool_anc_close(void) {
  if (pcsuppt_anc_type & ANC_FEEDFORWARD) {
    anc_close(ANC_FEEDFORWARD);
    af_anc_close(ANC_FEEDFORWARD);
  }

  if (pcsuppt_anc_type & ANC_FEEDBACK) {
    anc_close(ANC_FEEDBACK);
    af_anc_close(ANC_FEEDBACK);
  }
}
static void tool_anc_open(void) {
  if (pcsuppt_anc_type & ANC_FEEDFORWARD) {
    af_anc_open(ANC_FEEDFORWARD, anc_sample_rate, anc_sample_rate, NULL);
    anc_open(ANC_FEEDFORWARD);
    anc_set_gain(512, 512, ANC_FEEDFORWARD);

#ifdef AUDIO_ANC_TT_HW
    af_anc_open(ANC_TALKTHRU, anc_sample_rate, anc_sample_rate, NULL);
    anc_open(ANC_TALKTHRU);
    anc_set_gain(512, 512, ANC_TALKTHRU);
#endif
  }

  if (pcsuppt_anc_type & ANC_FEEDBACK) {
    af_anc_open(ANC_FEEDBACK, anc_sample_rate, anc_sample_rate, NULL);
    anc_open(ANC_FEEDBACK);
    anc_set_gain(512, 512, ANC_FEEDBACK);

#ifdef AUDIO_ANC_FB_MC_HW
    anc_open(ANC_MUSICCANCLE);
    anc_set_gain(512, 512, ANC_MUSICCANCLE);
#endif
  }
}
#if !defined(AUDIO_ANC_TT_HW)
#if defined(__HW_IIR_EQ_PROCESS__)
static HW_IIR_CFG_T hw_iir_cfg;
#endif

#if defined(__HW_DAC_IIR_EQ_PROCESS__)
static HW_CODEC_IIR_CFG_T hw_codec_iir_cfg;
#endif
#endif

static int handle_anc_cmd(enum ANC_CMD_T cmd, const uint8_t *data,
                          uint32_t len) {

  unsigned char cret = ERR_NONE;

  switch (cmd) {
  case ANC_CMD_CLOSE: {
    TRACE(0, "ANC_CMD_CLOSE ------");
    if (len != 0) {
      return ERR_LEN;
    }
    anc_disable();
    tool_anc_close();
    send_reply(&cret, 1);
    break;
  }
  case ANC_CMD_OPEN: {
    TRACE(0, "ANC_CMD_OPEN ------");
    if (len != 0) {
      return ERR_LEN;
    }
    hal_sysfreq_req(HAL_SYSFREQ_USER_ANC, HAL_CMU_FREQ_52M);
    tool_anc_open();
    anc_enable();
    send_reply(&cret, 1);
    break;
  }
  case ANC_CMD_GET_CFG: {
    TRACE(0, "ANC_CMD_GET_CFG ------");

    struct_anc_cfg *anccfg_addr = get_anc_config();
    uint32_t addr = (uint32_t)anccfg_addr;

    if (len != 0) {
      return ERR_LEN;
    }

    TRACE(1, "send anccfg address 0x%x ------", addr);
    send_reply((unsigned char *)&addr, sizeof(addr));
    break;
  }
  case ANC_CMD_APPLY_CFG: {
    TRACE(0, "ANC_CMD_APPLY_CFG ------");

    if (len != 0) {
      return ERR_LEN;
    }
    TRACE(0, "apply anccfg ------");
    // best2000_prod_test_anccfg_apply();
    struct_anc_cfg *anccfg = get_anc_config();

    // process ANC
    if (pcsuppt_anc_type & ANC_FEEDFORWARD) {
      anc_set_cfg(anccfg, ANC_FEEDFORWARD, ANC_GAIN_NO_DELAY);
#ifdef AUDIO_ANC_TT_HW
      // anc_set_cfg(anccfg,ANC_TALKTHRU,ANC_GAIN_NO_DELAY);
#endif
    }

    if (pcsuppt_anc_type & ANC_FEEDBACK) {
      anc_set_cfg(anccfg, ANC_FEEDBACK, ANC_GAIN_NO_DELAY);
#ifdef AUDIO_ANC_FB_MC_HW
      anc_set_cfg(anccfg, ANC_MUSICCANCLE, ANC_GAIN_NO_DELAY);
#endif
    }
#ifdef AUDIO_ANC_FB_MC
    // process MC
    mc_iir_cfg.anc_cfg_mc_l = anccfg->anc_cfg_mc_l;
    mc_iir_cfg.anc_cfg_mc_r = anccfg->anc_cfg_mc_r;
#endif

    if (anccfg->anc_cfg_mc_l.adc_gain_offset == 0) {
      hal_codec_anc_adc_enable(ANC_FEEDFORWARD);
      analog_aud_codec_anc_enable(ANC_FEEDFORWARD, true);
      hal_codec_anc_adc_enable(ANC_FEEDBACK);
      analog_aud_codec_anc_enable(ANC_FEEDBACK, true);
      TRACE(0, "ADC UNMUTE........");
    } else {
      hal_codec_anc_adc_disable(ANC_FEEDFORWARD);
      analog_aud_codec_anc_enable(ANC_FEEDFORWARD, false);
      hal_codec_anc_adc_disable(ANC_FEEDBACK);
      analog_aud_codec_anc_enable(ANC_FEEDBACK, false);
      TRACE(0, "ADC MUTE........");
    }

    if (anccfg->anc_cfg_mc_l.dac_gain_offset == 0) {
      analog_aud_codec_nomute();
      TRACE(0, "DAC UNMUTE........");
    } else {
      analog_aud_codec_mute();
      TRACE(0, "DAC MUTE........");
    }

#if !defined(AUDIO_ANC_TT_HW)

#if defined(__HW_IIR_EQ_PROCESS__)
    // process EQ
    if (anccfg->anc_cfg_tt_l.total_gain == 0) {
      hw_iir_cfg.iir_filtes_l.iir_counter = 0;
    } else {
      hw_iir_cfg.iir_filtes_l.iir_counter = anccfg->anc_cfg_tt_l.iir_counter;
    }

    if (anccfg->anc_cfg_tt_r.total_gain == 0) {
      hw_iir_cfg.iir_filtes_r.iir_counter = 0;
    } else {
      hw_iir_cfg.iir_filtes_r.iir_counter = anccfg->anc_cfg_tt_r.iir_counter;
    }

    for (int i = 0; i < AUD_IIR_NUM_EQ; i++) {
      hw_iir_cfg.iir_filtes_l.iir_coef[i].coef_b[0] =
          (anccfg->anc_cfg_tt_l.iir_coef[i].coef_b[0]);
      hw_iir_cfg.iir_filtes_l.iir_coef[i].coef_b[1] =
          (anccfg->anc_cfg_tt_l.iir_coef[i].coef_b[1]);
      hw_iir_cfg.iir_filtes_l.iir_coef[i].coef_b[2] =
          (anccfg->anc_cfg_tt_l.iir_coef[i].coef_b[2]);
      hw_iir_cfg.iir_filtes_l.iir_coef[i].coef_a[0] =
          (anccfg->anc_cfg_tt_l.iir_coef[i].coef_a[0]);
      hw_iir_cfg.iir_filtes_l.iir_coef[i].coef_a[1] =
          (anccfg->anc_cfg_tt_l.iir_coef[i].coef_a[1]);
      hw_iir_cfg.iir_filtes_l.iir_coef[i].coef_a[2] =
          (anccfg->anc_cfg_tt_l.iir_coef[i].coef_a[2]);

      hw_iir_cfg.iir_filtes_r.iir_coef[i].coef_b[0] =
          (anccfg->anc_cfg_tt_r.iir_coef[i].coef_b[0]);
      hw_iir_cfg.iir_filtes_r.iir_coef[i].coef_b[1] =
          (anccfg->anc_cfg_tt_r.iir_coef[i].coef_b[1]);
      hw_iir_cfg.iir_filtes_r.iir_coef[i].coef_b[2] =
          (anccfg->anc_cfg_tt_r.iir_coef[i].coef_b[2]);
      hw_iir_cfg.iir_filtes_r.iir_coef[i].coef_a[0] =
          (anccfg->anc_cfg_tt_r.iir_coef[i].coef_a[0]);
      hw_iir_cfg.iir_filtes_r.iir_coef[i].coef_a[1] =
          (anccfg->anc_cfg_tt_r.iir_coef[i].coef_a[1]);
      hw_iir_cfg.iir_filtes_r.iir_coef[i].coef_a[2] =
          (anccfg->anc_cfg_tt_r.iir_coef[i].coef_a[2]);
    }

    hw_iir_set_cfg(&hw_iir_cfg);
#endif

#if defined(__HW_DAC_IIR_EQ_PROCESS__)

#if 0
                 TRACE(1,"__HW_DAC_IIR_EQ_PROCESS__ ........");

                TRACE(1,"eq gain: %d, counter: %d iir_bypass_flag: %d ",anccfg->anc_cfg_tt_l.total_gain, anccfg->anc_cfg_tt_l.iir_counter,anccfg->anc_cfg_tt_l.iir_bypass_flag);
                TRACE(1,"eq dac_gain_offset %d, adc_gain_offset %d",anccfg->anc_cfg_tt_l.dac_gain_offset, anccfg->anc_cfg_tt_l.adc_gain_offset);

                for(int j = 0; j <AUD_IIR_NUM_EQ; j++)
                {
                    //TRACE(1,"iir coef ff l %10d, %10d, %10d, %10d, %10d, %10d",
                    TRACE(1,"iir coef eq l 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x",\
                            anccfg->anc_cfg_tt_l.iir_coef[j].coef_b[0], \
                            anccfg->anc_cfg_tt_l.iir_coef[j].coef_b[1], \
                            anccfg->anc_cfg_tt_l.iir_coef[j].coef_b[2], \
                            anccfg->anc_cfg_tt_l.iir_coef[j].coef_a[0], \
                            anccfg->anc_cfg_tt_l.iir_coef[j].coef_a[1], \
                            anccfg->anc_cfg_tt_l.iir_coef[j].coef_a[2]);
                }
#endif

    // process EQ
    if (anccfg->anc_cfg_tt_l.total_gain == 0) {
      hw_codec_iir_cfg.iir_filtes_l.iir_counter = 0;
    } else {
      hw_codec_iir_cfg.iir_filtes_l.iir_counter =
          anccfg->anc_cfg_tt_l.iir_counter;
    }

    if (anccfg->anc_cfg_tt_r.total_gain == 0) {
      hw_codec_iir_cfg.iir_filtes_r.iir_counter = 0;
    } else {
      hw_codec_iir_cfg.iir_filtes_r.iir_counter =
          anccfg->anc_cfg_tt_r.iir_counter;
    }

    for (int i = 0; i < AUD_IIR_NUM_EQ; i++) {
      hw_codec_iir_cfg.iir_filtes_l.iir_coef[i].coef_b[0] =
          (anccfg->anc_cfg_tt_l.iir_coef[i].coef_b[0]);
      hw_codec_iir_cfg.iir_filtes_l.iir_coef[i].coef_b[1] =
          (anccfg->anc_cfg_tt_l.iir_coef[i].coef_b[1]);
      hw_codec_iir_cfg.iir_filtes_l.iir_coef[i].coef_b[2] =
          (anccfg->anc_cfg_tt_l.iir_coef[i].coef_b[2]);
      hw_codec_iir_cfg.iir_filtes_l.iir_coef[i].coef_a[0] =
          (anccfg->anc_cfg_tt_l.iir_coef[i].coef_a[0]);
      hw_codec_iir_cfg.iir_filtes_l.iir_coef[i].coef_a[1] =
          (anccfg->anc_cfg_tt_l.iir_coef[i].coef_a[1]);
      hw_codec_iir_cfg.iir_filtes_l.iir_coef[i].coef_a[2] =
          (anccfg->anc_cfg_tt_l.iir_coef[i].coef_a[2]);

      hw_codec_iir_cfg.iir_filtes_r.iir_coef[i].coef_b[0] =
          (anccfg->anc_cfg_tt_r.iir_coef[i].coef_b[0]);
      hw_codec_iir_cfg.iir_filtes_r.iir_coef[i].coef_b[1] =
          (anccfg->anc_cfg_tt_r.iir_coef[i].coef_b[1]);
      hw_codec_iir_cfg.iir_filtes_r.iir_coef[i].coef_b[2] =
          (anccfg->anc_cfg_tt_r.iir_coef[i].coef_b[2]);
      hw_codec_iir_cfg.iir_filtes_r.iir_coef[i].coef_a[0] =
          (anccfg->anc_cfg_tt_r.iir_coef[i].coef_a[0]);
      hw_codec_iir_cfg.iir_filtes_r.iir_coef[i].coef_a[1] =
          (anccfg->anc_cfg_tt_r.iir_coef[i].coef_a[1]);
      hw_codec_iir_cfg.iir_filtes_r.iir_coef[i].coef_a[2] =
          (anccfg->anc_cfg_tt_r.iir_coef[i].coef_a[2]);
    }

    if (anc_sample_rate == AUD_SAMPRATE_50781) {
      hw_codec_iir_set_cfg(&hw_codec_iir_cfg, AUD_SAMPRATE_50781,
                           HW_CODEC_IIR_DAC);
    } else {
      hw_codec_iir_set_cfg(&hw_codec_iir_cfg, AUD_SAMPRATE_48000,
                           HW_CODEC_IIR_DAC);
    }
#endif
#endif
    send_reply(&cret, 1);
    break;
  }
  case ANC_CMD_CFG_SETUP: {
    TRACE(0, "ANC_CMD_CFG_SETUP ------");

    int ret = 0;
    bool diff;
    bool high_performance_adc;
    bool vcrystal_on;
    uint16_t vcodec;
    pctool_iocfg *iocfg1, *iocfg2;

    if (data[0] == 1) {
      pcsuppt_anc_type = ANC_FEEDFORWARD;
    } else if (data[0] == 2) {
      pcsuppt_anc_type = ANC_FEEDBACK;
    } else if (data[0] == 3) {
      pcsuppt_anc_type = ANC_FEEDFORWARD | ANC_FEEDBACK;
    }

    vcodec = (data[1] | (data[2] << 8));
    diff = (bool)data[3];
    high_performance_adc = false; //(bool)data[4]; // default 0
    anc_sample_rate =
        (data[5] | (data[6] << 8) | (data[7] << 16) | (data[8] << 24));
    vcrystal_on = data[9]; //(bool)data[?]; // default 0
    TRACE(4, "vcodec:%d,diff:%d,anc_sample_rate:%d,vcrystal_on:%d.", vcodec,
          diff, anc_sample_rate, vcrystal_on);
    ret |= pmu_debug_config_vcrystal(vcrystal_on);
    ret |= analog_debug_config_audio_output(diff);
    ret |= analog_debug_config_codec(vcodec);
    ret |= analog_debug_config_low_power_adc(!high_performance_adc);
    if (anc_sample_rate == AUD_SAMPRATE_50781) {
      hal_cmu_audio_resample_enable();
    } else {
      hal_cmu_audio_resample_disable();
    }
    iocfg1 = (pctool_iocfg *)(&data[10]);
    iocfg2 = (pctool_iocfg *)(&data[12]);
    TRACE(4, "io cfg:%d    %d    %d    %d", iocfg1->io_pin, iocfg1->set_flag,
          iocfg2->io_pin, iocfg2->set_flag);
    if (iocfg1->io_pin >= 0)
      anc_set_gpio(iocfg1->io_pin, iocfg1->set_flag);
    if (iocfg2->io_pin >= 0)
      anc_set_gpio(iocfg2->io_pin, iocfg2->set_flag);
    cret = ret ? ERR_INTERNAL : ERR_NONE;

    send_reply((unsigned char *)&cret, 1);
    break;
  }
  case ANC_CMD_CHANNEL_SETUP: {

    const enum AUD_CHANNEL_MAP_T channel_map_arr[16] = {
        AUD_CHANNEL_MAP_CH0,        AUD_CHANNEL_MAP_CH1,
        AUD_CHANNEL_MAP_CH2,        AUD_CHANNEL_MAP_CH3,
        AUD_CHANNEL_MAP_CH4,        AUD_CHANNEL_MAP_CH5,
        AUD_CHANNEL_MAP_CH6,        AUD_CHANNEL_MAP_CH7,
        AUD_CHANNEL_MAP_DIGMIC_CH0, AUD_CHANNEL_MAP_DIGMIC_CH1,
        AUD_CHANNEL_MAP_DIGMIC_CH2, AUD_CHANNEL_MAP_DIGMIC_CH3,
        AUD_CHANNEL_MAP_DIGMIC_CH4, AUD_CHANNEL_MAP_DIGMIC_CH5,
        AUD_CHANNEL_MAP_DIGMIC_CH6, AUD_CHANNEL_MAP_DIGMIC_CH7,
    };
    anc_ff_mic_ch_l = channel_map_arr[data[0]];
    anc_ff_mic_ch_r = channel_map_arr[data[1]];
    anc_fb_mic_ch_l = channel_map_arr[data[2]];
    anc_fb_mic_ch_r = channel_map_arr[data[3]];
    TRACE(4,
          "anc_ff_mic_ch_l 0x%x,anc_ff_mic_ch_r 0x%x,anc_fb_mic_ch_l "
          "0x%x,anc_fb_mic_ch_r 0x%x",
          anc_ff_mic_ch_l, anc_ff_mic_ch_r, anc_fb_mic_ch_l, anc_fb_mic_ch_r);

    hal_iomux_set_dig_mic_clock_pin(data[4]);
    hal_iomux_set_dig_mic_data0_pin(data[5]);
    hal_iomux_set_dig_mic_data1_pin(data[6]);
    hal_iomux_set_dig_mic_data2_pin(data[7]);
    uint8_t phase = data[8];
#if defined(CHIP_BEST2300) || defined(CHIP_BEST2300P) || defined(CHIP_BEST2300A)
    analog_debug_config_vad_mic(!!(phase & (1 << 7)));
    phase &= ~(1 << 7);
#endif
    hal_codec_config_digmic_phase(phase);

    send_reply((unsigned char *)&cret, 1);
    break;
  }
  case ANC_CMD_SET_SAMP_RATE: {
    bool opened;

    opened = (anc_opened(ANC_FEEDFORWARD) || anc_opened(ANC_FEEDBACK));
    if (opened) {
      anc_disable();
      tool_anc_close();
    }
    anc_sample_rate =
        data[0] | (data[1] << 8) | (data[2] << 16) | (data[3] << 24);
    if (anc_sample_rate == AUD_SAMPRATE_50781) {
      hal_cmu_audio_resample_enable();
    } else {
      hal_cmu_audio_resample_disable();
    }
    if (opened) {
      tool_anc_open();
      anc_enable();
    }
    send_reply((unsigned char *)&cret, 1);
    break;
  }
  }

  return ERR_NONE;
}
static int get_sector_info(unsigned int addr, unsigned int *sector_addr,
                           unsigned int *sector_len) {
  int ret;

  ret = hal_norflash_get_boundary(HAL_NORFLASH_ID_0, addr, NULL,
                                  (uint32_t *)sector_addr);
  if (ret) {
    return ret;
  }

  ret = hal_norflash_get_size(HAL_NORFLASH_ID_0, NULL, NULL,
                              (uint32_t *)sector_len, NULL);

  return ret;
}
static int erase_sector(unsigned int sector_addr, unsigned int sector_len) {
  return hal_norflash_erase(HAL_NORFLASH_ID_0, sector_addr, sector_len);
}

static int erase_chip(void) {
  return hal_norflash_erase_chip(HAL_NORFLASH_ID_0);
}

static int burn_data(unsigned int addr, const unsigned char *data,
                     unsigned int len) {
  int ret;

  ret = hal_norflash_write(HAL_NORFLASH_ID_0, addr, data, len);
  return ret;
}

static int verify_flash_data(unsigned int addr, const unsigned char *data,
                             unsigned int len) {
  const unsigned char *fdata;
  const unsigned char *mdata;
  int i;

  fdata = (unsigned char *)addr;
  mdata = data;
  for (i = 0; i < len; i++) {
    if (*fdata++ != *mdata++) {
      --fdata;
      --mdata;
      TRACE(4, "*** Verify flash data failed: 0x%02X @ %p != 0x%02X @ %p",
            *fdata, fdata, *mdata, mdata);
      return *fdata - *mdata;
    }
  }
  return 0;
}
static enum ERR_CODE handle_sector_info_cmd(unsigned int addr) {
  unsigned int sector_addr;
  unsigned int sector_len;
  unsigned char buf[9];
  int ret;

  ret = get_sector_info(addr, &sector_addr, &sector_len);
  if (ret) {
    return ERR_DATA_ADDR;
  }

  TRACE(3, "addr=0x%08X sector_addr=0x%08X sector_len=%u", addr, sector_addr,
        sector_len);

  buf[0] = ERR_NONE;
  memcpy(&buf[1], &sector_addr, 4);
  memcpy(&buf[5], &sector_len, 4);

  send_reply(buf, 9);

  return ERR_NONE;
}
static enum ERR_CODE handle_flash_cmd(enum FLASH_CMD_TYPE cmd,
                                      unsigned char *param, unsigned int len) {
  int ret = 0;
  unsigned char cret = ERR_NONE;
  const char *name = NULL;

  switch (cmd) {
  case FLASH_CMD_ERASE_SECTOR: {
    unsigned int addr;
    unsigned int size;

    if (cmd == FLASH_CMD_ERASE_SECTOR) {
      name = "ERASE_SECTOR";
    }

    trace_flash_cmd_info(name);

    if (len != 8) {
      trace_flash_cmd_len_err(name, len);
      return ERR_LEN;
    }

    addr = param[0] | (param[1] << 8) | (param[2] << 16) | (param[3] << 24);
    size = param[4] | (param[5] << 8) | (param[6] << 16) | (param[7] << 24);
    TRACE(2, "addr=0x%08X size=%u", addr, size);

    if (cmd == FLASH_CMD_ERASE_SECTOR) {
      ret = erase_sector(addr, size);
    }

    if (ret) {
      trace_flash_cmd_err(name);
      return ERR_ERASE_FLSH;
    }

    trace_flash_cmd_done(name);

    send_reply(&cret, 1);
    break;
  }
  case FLASH_CMD_BURN_DATA: {
    unsigned int addr;

    if (cmd == FLASH_CMD_BURN_DATA) {
      name = "BURN_DATA";
    }

    trace_flash_cmd_info(name);

    if (len <= 4 || len > 20) {
      trace_flash_cmd_len_err(name, len);
      return ERR_LEN;
    }

    addr = param[0] | (param[1] << 8) | (param[2] << 16) | (param[3] << 24);
    TRACE(2, "addr=0x%08X len=%u", addr, len - 4);

    if (cmd == FLASH_CMD_BURN_DATA) {
      ret = burn_data(addr, &param[4], len - 4);
    }

    if (ret) {
      trace_flash_cmd_err(name);
      return ERR_BURN_FLSH;
    }

    TRACE_TIME(1, "%s verifying", name);

    if (cmd == FLASH_CMD_BURN_DATA) {
      ret = verify_flash_data(addr, &param[4], len - 4);
    }
    if (ret) {
      TRACE(1, "%s verify failed", name);
      return ERR_VERIFY_FLSH;
    }

    trace_flash_cmd_done(name);

    send_reply(&cret, 1);
    break;
  }
  case FLASH_CMD_ERASE_CHIP: {
    name = "CMD_ERASE_CHIP";

    trace_flash_cmd_info(name);

    if (len != 0) {
      trace_flash_cmd_len_err(name, len);
      return ERR_LEN;
    }

    ret = erase_chip();
    if (ret) {
      trace_flash_cmd_err(name);
      return ERR_ERASE_FLSH;
    }

    trace_flash_cmd_done(name);

    send_reply(&cret, 1);
    break;
  }
  default:
    TRACE(1, "Unsupported flash cmd: 0x%x", cmd);
    return ERR_FLASH_CMD;
  }

  return ERR_NONE;
}
/*
#define SECTOR_SIZE_64K                     (1 << 16)
#define SECTOR_SIZE_32K                     (1 << 15)
#define SECTOR_SIZE_16K                     (1 << 14)
#define SECTOR_SIZE_4K                      (1 << 12)
*/
#define BURN_DATA_MSG_OVERHEAD 16

// static const unsigned int size_mask = SECTOR_SIZE_32K | SECTOR_SIZE_4K;

enum PROGRAMMER_STATE {
  PROGRAMMER_NONE,
  PROGRAMMER_ERASE_BURN_START,
  PROGRAMMER_SEC_REG_ERASE_BURN_START,
};

#define BURN_BUFFER_LOC __attribute__((section(".burn_buffer")))

static unsigned char BURN_BUFFER_LOC data_buf[65536 + 2048];
static enum PROGRAMMER_STATE programmer_state = PROGRAMMER_NONE;

/*
static unsigned int count_set_bits(unsigned int i)
{
    i = i - ((i >> 1) & 0x55555555);
    i = (i & 0x33333333) + ((i >> 2) & 0x33333333);
    return (((i + (i >> 4)) & 0x0F0F0F0F) * 0x01010101) >> 24;
}
*/

static int send_burn_data_reply(enum ERR_CODE code, unsigned short sec_seq,
                                unsigned char seq) {
  int ret = 0;
  enum MSG_TYPE type = TYPE_INVALID;

  if (programmer_state == PROGRAMMER_ERASE_BURN_START) {
    type = TYPE_ERASE_BURN_DATA;
  }

  send_msg.hdr.type = type;
  send_msg.hdr.seq = recv_msg.hdr.seq;
  send_msg.hdr.len = 3;
  send_msg.data[0] = code;
  send_msg.data[1] = sec_seq & 0xFF;
  send_msg.data[2] = (sec_seq >> 8) & 0xFF;
  send_msg.data[3] =
      ~check_sum((unsigned char *)&send_msg, MSG_TOTAL_LEN(&send_msg) - 1);

  send_msg_push_seq = send_msg_push_seq % MAX_SNED_MSG_QUEUE;
  send_msg_queue[send_msg_push_seq] = send_msg;
  send_msg_push_seq++;

  // ret = send_data((unsigned char *)&send_msg, MSG_TOTAL_LEN(&send_msg));
  return ret;
}

int anc_cmd_receve_process(uint8_t *buf, uint32_t len) {
  enum MSG_TYPE type;
  enum ERR_CODE errcode = ERR_NONE;
  unsigned char cret = ERR_NONE;

  if (len > sizeof(recv_msg)) {
    memcpy(&recv_msg, buf, sizeof(recv_msg));
  } else {
    memcpy(&recv_msg, buf, len);
  }
  // Checksum
  if (check_sum((unsigned char *)&recv_msg, MSG_TOTAL_LEN(&recv_msg)) != 0xFF) {
    trace_stage_info("Checksum error");
    return ERR_CHECKSUM;
  }
  type = recv_msg.hdr.type;

  // TRACE(1,"COMMAND:0x%x",type);

  switch (type) {
  case TYPE_SYS: {
    TRACE(0, "SYS CMD");
    break;
  }
  case TYPE_READ: {
    unsigned int addr;
    unsigned int len;

    trace_stage_info("READ CMD");

    addr = recv_msg.data[0] | (recv_msg.data[1] << 8) |
           (recv_msg.data[2] << 16) | (recv_msg.data[3] << 24);
    len = recv_msg.data[4];

    TRACE(2, "addr:0x%x,len:%d", addr, len);

    errcode = handle_read_cmd(type, addr, len);
    if (errcode != ERR_NONE) {
      return errcode;
    }
    break;
  }
  case TYPE_WRITE: {
    unsigned int addr;
    unsigned int len;
    unsigned char *wdata;

    //  trace_stage_info("WRITE CMD");

    addr = recv_msg.data[0] | (recv_msg.data[1] << 8) |
           (recv_msg.data[2] << 16) | (recv_msg.data[3] << 24);
    len = recv_msg.hdr.len - 4;
    wdata = &recv_msg.data[4];

    errcode = handle_write_cmd(addr, len, wdata);
    if (errcode != ERR_NONE) {
      return errcode;
    }
    break;
  }
  case TYPE_ERASE_BURN_START: {
    TRACE(0, "TYPE_ERASE_BURN_START CMD");

    if (programmer_state == PROGRAMMER_NONE) {
      trace_stage_info("ERASE_BURN_START");

      burn_addr = recv_msg.data[0] | (recv_msg.data[1] << 8) |
                  (recv_msg.data[2] << 16) | (recv_msg.data[3] << 24);
      burn_total_len = recv_msg.data[4] | (recv_msg.data[5] << 8) |
                       (recv_msg.data[6] << 16) | (recv_msg.data[7] << 24);
      sector_size = recv_msg.data[8] | (recv_msg.data[9] << 8) |
                    (recv_msg.data[10] << 16) | (recv_msg.data[11] << 24);

      TRACE(3, "burn_addr=0x%08X burn_total_len=%u sector_size=%u", burn_addr,
            burn_total_len, sector_size);

      /*if ((size_mask & sector_size) == 0 || count_set_bits(sector_size) != 1)
      {
          TRACE(2,"Unsupported sector_size=0x%08X mask=0x%08X", sector_size,
      size_mask); return ERR_SECTOR_SIZE;
      }*/

      sector_cnt = burn_total_len / sector_size;
      last_sector_len = burn_total_len % sector_size;

      if (last_sector_len) {
        sector_cnt++;
      } else {
        last_sector_len = sector_size;
      }

      if (sector_cnt > 0xFFFF) {
        TRACE(1, "Sector seq overflow: %u", sector_cnt);
        return ERR_SECTOR_SEQ_OVERFLOW;
      }

      send_reply(&cret, 1);

      if (burn_total_len == 0) {
        TRACE(0, "Burn length = 0");
        break;
      }

      programmer_state = PROGRAMMER_ERASE_BURN_START;

      burn_len = 0;

      trace_stage_info("ERASE_BURN_START end");
    } else {
      TRACE(1, "ERROR programmer_state status:%d", programmer_state);
    }
    break;
  }
  case TYPE_ERASE_BURN_DATA: {

    if (programmer_state == PROGRAMMER_ERASE_BURN_START) {
      unsigned int dlen;
      unsigned int mcrc;
      unsigned int crc;

      dlen = recv_msg.data[0] | (recv_msg.data[1] << 8) |
             (recv_msg.data[2] << 16) | (recv_msg.data[3] << 24);
      mcrc = recv_msg.data[4] | (recv_msg.data[5] << 8) |
             (recv_msg.data[6] << 16) | (recv_msg.data[7] << 24);

      cur_sector_seq = recv_msg.data[8] | (recv_msg.data[9] << 8);

      TRACE(2, " sec_seq=%u dlen=%u", cur_sector_seq, dlen);

      if (cur_sector_seq >= sector_cnt) {
        TRACE(2, "Bad sector seq: sec_seq=%u sector_cnt=%u", cur_sector_seq,
              sector_cnt);
        send_burn_data_reply(ERR_SECTOR_SEQ, cur_sector_seq, recv_msg.hdr.seq);
        return ERR_NONE;
      }

      if (((cur_sector_seq + 1) == sector_cnt && dlen != last_sector_len) ||
          ((cur_sector_seq + 1) != sector_cnt && dlen != sector_size)) {
        TRACE(2, " Bad data len: sec_seq=%u dlen=%u", cur_sector_seq, dlen);
        send_burn_data_reply(ERR_SECTOR_DATA_LEN, cur_sector_seq,
                             recv_msg.hdr.seq);
        return ERR_NONE;
      }

      crc = crc32(0, (unsigned char *)&buf[BURN_DATA_MSG_OVERHEAD], dlen);
      if (crc != mcrc) {
        TRACE(0, "Bad CRC");
        send_burn_data_reply(ERR_SECTOR_DATA_CRC, cur_sector_seq,
                             recv_msg.hdr.seq);
        return ERR_NONE;
      }

      if (burn_len + dlen <= burn_total_len) {
        memcpy(&data_buf[burn_len],
               (unsigned char *)&buf[BURN_DATA_MSG_OVERHEAD], dlen);
        burn_len = burn_len + dlen;
      } else {
        TRACE(2, "Error burn_len=%d,burn_total_len=%d", burn_len,
              burn_total_len);
      }

      if (burn_len == burn_total_len) {
        TRACE(2, "BURN_DATA addr=0x%08X len=%u .........................",
              burn_addr, burn_total_len);
        int ret = burn_data(burn_addr, data_buf, burn_total_len);
        if (ret) {
          TRACE(2, "### FLASH_TASK: BURN_DATA failed: addr=0x%08X len=%u ###",
                burn_addr, burn_total_len);
          return 0;
        } else {
          TRACE(0, "burn sucessful");
        }
        ret = verify_flash_data(burn_addr, data_buf, burn_total_len);
        if (ret) {
          TRACE(0, "verify failed");
          return ERR_VERIFY_FLSH;
        } else {
          TRACE(0, "verify sucessful");
        }
      }

      send_burn_data_reply(ERR_BURN_OK, cur_sector_seq, recv_msg.hdr.seq);

    } else {
      TRACE(1, "ERROR programmer_state status:%d", programmer_state);
    }
    break;
  }
  case TYPE_FLASH_CMD: {
    trace_stage_info("FLASH CMD");

    errcode = handle_flash_cmd((enum FLASH_CMD_TYPE)recv_msg.data[0],
                               &recv_msg.data[1], recv_msg.hdr.len - 1);

    TRACE_TIME(1, "FLASH CMD ret=%d", errcode);

    if (errcode != ERR_NONE) {
      return errcode;
    }
    break;
  }
  case TYPE_GET_SECTOR_INFO: {
    unsigned int addr;

    trace_stage_info("GET SECTOR INFO");

    addr = recv_msg.data[0] | (recv_msg.data[1] << 8) |
           (recv_msg.data[2] << 16) | (recv_msg.data[3] << 24);

    errcode = handle_sector_info_cmd(addr);
    if (errcode != ERR_NONE) {
      return errcode;
    }
    break;
  }
  case TYPE_PROD_TEST: {
    uint32_t cmd;

    trace_stage_info("ANC CMD");

    cmd = recv_msg.data[0] | (recv_msg.data[1] << 8) |
          (recv_msg.data[2] << 16) | (recv_msg.data[3] << 24);
    if (cmd != PROD_TEST_CMD_ANC) {
      return ERR_TYPE_INVALID;
    }

    if (recv_msg.hdr.len < 5) {
      TRACE(1, "PROD_TEST/ANC msg length error: %d", recv_msg.hdr.len);
      return ERR_LEN;
    }

    errcode = handle_anc_cmd(recv_msg.data[4], &recv_msg.data[5],
                             recv_msg.hdr.len - 5);
    if (errcode != ERR_NONE) {
      return errcode;
    }

    break;
  }
  default:
    TRACE(1, "COMMAND:0x%x", type);

    cret = ERR_TYPE_INVALID;
    send_reply((unsigned char *)&cret, 1);
    break;
  }

  return 0;
}
int anc_cmd_send_process(uint8_t **pbuf, uint16_t *len) {

  send_msg_pop_seq = send_msg_pop_seq % MAX_SNED_MSG_QUEUE;
  *pbuf = (uint8_t *)&send_msg_queue[send_msg_pop_seq];
  *len = MSG_TOTAL_LEN(&send_msg);
  send_msg_pop_seq++;

  return 0;
}
#endif