m2_g2_fs/Modules/eq_process/main.c

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <pthread.h>
#include <stdbool.h>

#include <alsa/asoundlib.h>
//#include <sys/time.h>
//#include <sys/socket.h>
//#include <sys/un.h>
#include <hiredis/hiredis.h>
#include <cjson/cJSON.h>
#include <iniparser/iniparser.h>

#define SAMPLE_RATE 48000
#define CHANNEL 2
#define REC_DEVICE_NAME "fake_record"
#define WRITE_DEVICE_NAME "fake_play"
//#define WRITE_DEVICE_NAME "default"
#define JACK_DEVICE_NAME "fake_jack"
#define READ_FRAME  1024    //(768)
#define PERIOD_SIZE (1024)  //(SAMPLE_RATE/8)
#define PERIOD_counts (2) //double of delay 200ms
#define BUFFER_SIZE (PERIOD_SIZE * PERIOD_counts)
// #define MUTE_TIME_THRESHOD (4)//seconds
// #define MUTE_FRAME_THRESHOD (SAMPLE_RATE * MUTE_TIME_THRESHOD / READ_FRAME)//30 seconds
//#define ALSA_READ_FORMAT SND_PCM_FORMAT_S32_LE
#define ALSA_READ_FORMAT SND_PCM_FORMAT_S16_LE
#define ALSA_WRITE_FORMAT SND_PCM_FORMAT_S16_LE
#define paging_channel "volume-event-channel"
// #define volume_channel "volume-value-channel"
// #define volume_temp_channel "volume-temp-channel"
#define VOL_CONF    "/oem/etc/volctrl.conf"
#define unix_socket_path "/tmp/redis.sock"

/*
 * Select different alsa pathways based on device type.
 *  LINE_OUT: LR-Mix(fake_play)->EqDrcProcess(ladspa)->Speaker(real_playback)
 *  HEAD_SET: fake_jack -> Headset(real_playback)
 *  BLUETOOTH: device as bluetooth source.
 */
// #define DEVICE_FLAG_LINE_OUT  0x01
// #define DEVICE_FLAG_HEAD_SET  0x02
// #define DEVICE_FLAG_BLUETOOTH 0x04
#define AULEVEL_MIN    -96.0
#define AULEVEL_MAX      0.0

//current session
#define IDLE        0
#define SIP         1
#define BROADCAST   2
#define MULTICAST   3
#define ONVIF       4

typedef struct _vumeter_play {
	int16_t *ch1_sampv;
    int16_t *ch2_sampv;
    size_t au_sampc;
	double ch1_avg_play;
	double ch2_avg_play;
} vumeter_play;

typedef struct {
    redisContext *context;
    const char *socket_path;
    time_t last_activity;
} redis_client_t;

static bool set_volume = false;
static int volume = 100;
static int en_threshold = false;
static int vol_threshold;
static int current_sess = IDLE;
static int ctrl_sess = IDLE;
static int sip_active = false;
static int mc_active = false;
static int onvif_active = false;
static int bc_active = false;

static bool DEBUG = false;

//struct timeval tv_begin, tv_end;
//gettimeofday(&tv_begin, NULL);

extern int set_sw_params(snd_pcm_t *pcm, snd_pcm_uframes_t buffer_size,
                         snd_pcm_uframes_t period_size, char **msg);

redis_client_t* create_redis_client(const char *socket_path) {
    redis_client_t *client = (redis_client_t *) malloc(sizeof(redis_client_t));
    client->socket_path = socket_path;
    client->context = redisConnectUnix(socket_path);
    client->last_activity = time(NULL);
    return client;
}

int ensure_connected(redis_client_t *client) {
    if (client->context == NULL || client->context->err) {
        if (client->context) {
            redisFree(client->context);
        }
        client->context = redisConnectUnix(client->socket_path);
        if (client->context == NULL || client->context->err) {
            return -1; // 连接失败
        }
    }
    client->last_activity = time(NULL);
    return 0;
}

void redis_command_safe(redis_client_t *client, const char *command) {
    if (ensure_connected(client) == 0) {
        redisReply *reply = (redisReply *) redisCommand(client->context, command);
        if (reply) {
            // 处理回复
            freeReplyObject(reply);
        }
    }
}

void alsa_fake_device_record_open(snd_pcm_t** capture_handle,int channels,uint32_t rate)
{
    snd_pcm_hw_params_t *hw_params;
    snd_pcm_uframes_t periodSize = PERIOD_SIZE;
    snd_pcm_uframes_t bufferSize = BUFFER_SIZE;
    int dir = 0;
    int err;

    err = snd_pcm_open(capture_handle, REC_DEVICE_NAME, SND_PCM_STREAM_CAPTURE, 0);
    if (err)
    {
        printf( "Unable to open capture PCM device: \n");
        exit(1);
    }
    printf("snd_pcm_open\n");
    //err = snd_pcm_hw_params_alloca(&hw_params);

    err = snd_pcm_hw_params_malloc(&hw_params);
    if(err)
    {
        fprintf(stderr, "cannot allocate hardware parameter structure (%s)\n",snd_strerror(err));
        exit(1);
    }
    printf("snd_pcm_hw_params_malloc\n");

    err = snd_pcm_hw_params_any(*capture_handle, hw_params);
    if(err)
    {
        fprintf(stderr, "cannot initialize hardware parameter structure (%s)\n",snd_strerror(err));
        exit(1);
    }
    printf("snd_pcm_hw_params_any!\n");

    err = snd_pcm_hw_params_set_access(*capture_handle, hw_params, SND_PCM_ACCESS_RW_INTERLEAVED);
    // err = snd_pcm_hw_params_set_access(*capture_handle, hw_params, SND_PCM_ACCESS_MMAP_NONINTERLEAVED);
    if (err)
    {
        printf("Error setting interleaved mode\n");
        exit(1);
    }
    printf("snd_pcm_hw_params_set_access!\n");

    err = snd_pcm_hw_params_set_format(*capture_handle, hw_params, ALSA_READ_FORMAT);
    if (err)
    {
        printf("Error setting format: %s\n", snd_strerror(err));
        exit(1);
    }
    printf("snd_pcm_hw_params_set_format\n");

    err = snd_pcm_hw_params_set_channels(*capture_handle, hw_params, channels);
    if (err)
    {
        printf("channels = %d\n",channels);
        printf( "Error setting channels: %s\n", snd_strerror(err));
        exit(1);
    }
    printf("channels = %d\n",channels);

    err = snd_pcm_hw_params_set_buffer_size_near(*capture_handle, hw_params, &bufferSize);
    if (err)
    {
        printf("Error setting buffer size (%ld): %s\n", bufferSize, snd_strerror(err));
        exit(1);
    }
    printf("bufferSize = %ld\n",bufferSize);

    err = snd_pcm_hw_params_set_period_size_near(*capture_handle, hw_params, &periodSize, 0);
    if (err)
    {
        printf("Error setting period time (%ld): %s\n", periodSize, snd_strerror(err));
        exit(1);
    }
    printf("periodSize = %ld\n",periodSize);

    err = snd_pcm_hw_params_set_rate_near(*capture_handle, hw_params, &rate, 0/*&dir*/);
    if (err)
    {
        printf("Error setting sampling rate (%d): %s\n", rate, snd_strerror(err));
        //goto error;
        exit(1);
    }
    printf("Rate = %d\n", rate);

    /* Write the parameters to the driver */
    err = snd_pcm_hw_params(*capture_handle, hw_params);
    if (err < 0)
    {
        printf( "Unable to set HW parameters: %s\n", snd_strerror(err));
        //goto error;
        exit(1);
    }

    err = snd_pcm_prepare(*capture_handle);

    printf("Open record device done \n");
    //set_sw_params(*capture_handle,bufferSize,periodSize,NULL);
    if(hw_params)
        snd_pcm_hw_params_free(hw_params);
}

void alsa_fake_device_write_open(snd_pcm_t** write_handle, int channels,
                                 uint32_t write_sampleRate)
{
    snd_pcm_hw_params_t *write_params;
    snd_pcm_uframes_t write_periodSize = PERIOD_SIZE;
    snd_pcm_uframes_t write_bufferSize = BUFFER_SIZE;
    int write_err;
    int write_dir;
    int i = 10;

retry:

    write_err = snd_pcm_open(write_handle, WRITE_DEVICE_NAME,
                                 SND_PCM_STREAM_PLAYBACK, 0);
    // }

    if (write_err) {
        usleep(100*1000);
        i --;
        if(i > 0)
            goto retry;
        printf( "Unable to open playback PCM device: \n");
        exit(1);
    }
    //printf( "interleaved mode\n");

    // snd_pcm_hw_params_alloca(&write_params);
    snd_pcm_hw_params_malloc(&write_params);
    //printf("snd_pcm_hw_params_alloca\n");

    snd_pcm_hw_params_any(*write_handle, write_params);

    write_err = snd_pcm_hw_params_set_access(*write_handle, write_params, SND_PCM_ACCESS_RW_INTERLEAVED);
    //write_err = snd_pcm_hw_params_set_access(*write_handle,  write_params, SND_PCM_ACCESS_MMAP_NONINTERLEAVED);
    if (write_err)
    {
        printf("Error setting interleaved mode\n");
        exit(1);
    }
    //printf( "interleaved mode\n");

    write_err = snd_pcm_hw_params_set_format(*write_handle, write_params, ALSA_WRITE_FORMAT);
    if (write_err)
    {
        printf("Error setting format: %s\n", snd_strerror(write_err));
        exit(1);
    }
    //printf( "format successed\n");

    write_err = snd_pcm_hw_params_set_channels(*write_handle, write_params, channels);
    if (write_err)
    {
        printf( "Error setting channels: %s\n", snd_strerror(write_err));
        exit(1);
    }
    //printf("channels = %d\n",channels);

    write_err = snd_pcm_hw_params_set_rate_near(*write_handle, write_params, &write_sampleRate, 0/*&write_dir*/);
    if (write_err)
    {
        printf("Error setting sampling rate (%d): %s\n", write_sampleRate, snd_strerror(write_err));
        exit(1);
    }
    //printf("setting sampling rate (%d)\n", write_sampleRate);

    write_err = snd_pcm_hw_params_set_buffer_size_near(*write_handle, write_params, &write_bufferSize);
    if (write_err)
    {
        printf("Error setting buffer size (%ld): %s\n", write_bufferSize, snd_strerror(write_err));
        exit(1);
    }
    //printf("write_bufferSize = %ld\n",write_bufferSize);

    write_err = snd_pcm_hw_params_set_period_size_near(*write_handle, write_params, &write_periodSize, 0);
    if (write_err)
    {
        printf("Error setting period time (%ld): %s\n", write_periodSize, snd_strerror(write_err));
        exit(1);
    }
    //printf("write_periodSize = %ld\n",write_periodSize);

#if 0
    snd_pcm_uframes_t write_final_buffer;
    write_err = snd_pcm_hw_params_get_buffer_size(write_params, &write_final_buffer);
    printf(" final buffer size %ld \n" , write_final_buffer);

    snd_pcm_uframes_t write_final_period;
    write_err = snd_pcm_hw_params_get_period_size(write_params, &write_final_period, &write_dir);
    printf(" final period size %ld \n" , write_final_period);
#endif

    /* Write the parameters to the driver */
    write_err = snd_pcm_hw_params(*write_handle, write_params);
    if (write_err < 0)
    {
        printf( "Unable to set HW parameters: %s\n", snd_strerror(write_err));
        exit(1);
    }

    write_err = snd_pcm_prepare(*write_handle);

    if(DEBUG) printf("open write device is successful\n");
    set_sw_params(*write_handle, write_bufferSize, write_periodSize, NULL);
    if(write_params)
        snd_pcm_hw_params_free(write_params);
}

int set_sw_params(snd_pcm_t *pcm, snd_pcm_uframes_t buffer_size,
                  snd_pcm_uframes_t period_size, char **msg) {

    snd_pcm_sw_params_t *params;
    char buf[256];
    int err;

    //snd_pcm_sw_params_alloca(&params);
    snd_pcm_sw_params_malloc(&params);
    if ((err = snd_pcm_sw_params_current(pcm, params)) != 0) {
        snprintf(buf, sizeof(buf), "Get current params: %s", snd_strerror(err));
        //goto fail;
        exit(1);
    }

    /* start the transfer when the buffer is full (or almost full) */
    snd_pcm_uframes_t threshold = (buffer_size / period_size) * period_size;
    if ((err = snd_pcm_sw_params_set_start_threshold(pcm, params, threshold)) != 0) {
        snprintf(buf, sizeof(buf), "Set start threshold: %s: %lu", snd_strerror(err), threshold);
        exit(1);
    }

    /* allow the transfer when at least period_size samples can be processed */
    if ((err = snd_pcm_sw_params_set_avail_min(pcm, params, period_size)) != 0) {
        snprintf(buf, sizeof(buf), "Set avail min: %s: %lu", snd_strerror(err), period_size);
        exit(1);
    }

    if ((err = snd_pcm_sw_params(pcm, params)) != 0) {
        snprintf(buf, sizeof(buf), "%s", snd_strerror(err));
        exit(1);
    }
    if(params)
        snd_pcm_sw_params_free(params);

    return 0;
}

//修改当前音量
void *control_volume(void *)
{
    char cmd[64];
    set_volume = true;
    volume -= 10;
    usleep(1000*1000);
    set_volume = false;
    return NULL;
}

void get_volume_threshold()
{
    dictionary  *   ini ;
	ini = iniparser_load(VOL_CONF);
	
    if (ini==NULL) {
        fprintf(stderr, "cannot parse file: %s\n", VOL_CONF);
        return;
    }
    en_threshold = iniparser_getboolean(ini, "volume:enable_threshold", false);
	vol_threshold = iniparser_getint(ini, "volume:volume_threshold", -15);
	iniparser_freedict(ini);
}

/**
 * Generic routine to calculate RMS (Root-Mean-Square) from
 * a set of signed 16-bit values
 *
 * \verbatim

	     .---------------
	     |   N-1
	     |  ----.
	     |  \
	     |   \        2
	     |    |   s[n]
	     |   /
	     |  /
	 _   |  ----'
	  \  |   n=0
	   \ |  ------------
	    \|       N

   \endverbatim
 *
 * @param data Array of signed 16-bit values
 * @param len  Number of values
 *
 * @return RMS value from 0 to 32768
 */
static double calc_rms_s16(const int16_t *data, size_t len)
{
	int64_t sum = 0;
	size_t i;

	if (!data || !len)
		return .0;

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

	return sqrt(sum / (double)len);
}

double aout_Aulevel_dbov(const int16_t *sampv, size_t sampc)
{
    static const double peak_s16 = 32767.0;
	double rms, dbov;

	if (!sampv || !sampc)
		return 0;

	rms = calc_rms_s16(sampv, sampc) / peak_s16;

	dbov = 20 * log10(rms);

	if (dbov < AULEVEL_MIN)
		dbov = AULEVEL_MIN;
	else if (dbov > AULEVEL_MAX)
		dbov = AULEVEL_MAX;

	return dbov;
}

static void *vumeter_decode(void *arg)
{
    char redisCmd[128];
    int set_idle = false;
    vumeter_play *vu_p = (vumeter_play *) arg;
    redis_client_t *redisClient = create_redis_client(unix_socket_path);

    usleep(100*1000);
    while(1)
    {
        usleep(500*1000);
        if(sip_active || mc_active || onvif_active || bc_active)
        {
            vu_p->ch1_avg_play = aout_Aulevel_dbov(vu_p->ch1_sampv, vu_p->au_sampc);
            sprintf(redisCmd, "HSet volume audio_out_ch1 %d", (int) vu_p->ch1_avg_play);
            redis_command_safe(redisClient, redisCmd);
            vu_p->ch2_avg_play = aout_Aulevel_dbov(vu_p->ch2_sampv, vu_p->au_sampc);
            sprintf(redisCmd, "HSet volume audio_out_ch2 %d", (int) vu_p->ch2_avg_play);
            redis_command_safe(redisClient, redisCmd);
            set_idle = false;

            if(DEBUG) printf("vol1: %f, vol2: %f\n", vu_p->ch1_avg_play, vu_p->ch2_avg_play);
        }
        else if(!set_idle)
        {
            sprintf(redisCmd, "HSet volume audio_out_ch1 %d", (int) AULEVEL_MIN);
            redis_command_safe(redisClient, redisCmd);
            sprintf(redisCmd, "HSet volume audio_out_ch2 %d", (int) AULEVEL_MIN);
            redis_command_safe(redisClient, redisCmd);
            set_idle = true;
        }
    }

    return NULL;
}

//播放状态监控
void ProcessReply( char * pReplyStr )
{
    cJSON *pJson = NULL;

    if(DEBUG) printf( "Msg [%s]\n", pReplyStr );
    pJson =  cJSON_Parse(pReplyStr);
    if ( pJson ) {
        if(strcmp(cJSON_GetObjectItem(pJson, "name")->valuestring, "SIP") == 0)
        {
            if(strcmp(cJSON_GetObjectItem(pJson, "action")->valuestring, "on") == 0)
            {
                get_volume_threshold();
                current_sess = SIP;
                sip_active = true;
            }
            else if(strcmp(cJSON_GetObjectItem(pJson, "action")->valuestring, "off") == 0)
            {
                if(ctrl_sess == SIP)
                {
                    volume = 100;
                    ctrl_sess = IDLE;
                }
                sip_active = false;
            }
        }
        else if(strcmp(cJSON_GetObjectItem(pJson, "name")->valuestring, "BROADCAST") == 0)
        {
            if(strcmp(cJSON_GetObjectItem(pJson, "action")->valuestring, "on") == 0)
            {
                get_volume_threshold();
                current_sess = BROADCAST;
                bc_active = true;
            }
            else if(strcmp(cJSON_GetObjectItem(pJson, "action")->valuestring, "off") == 0)
            {
                if(ctrl_sess == BROADCAST)
                {
                    volume = 100;
                    ctrl_sess = IDLE;
                }
                bc_active = false;
            }
        }
        else if(strcmp(cJSON_GetObjectItem(pJson, "name")->valuestring, "MULTICAST") == 0)
        {
            if(strcmp(cJSON_GetObjectItem(pJson, "action")->valuestring, "on") == 0)
            {
                get_volume_threshold();
                current_sess = MULTICAST;
                mc_active = true;
            }
            else if(strcmp(cJSON_GetObjectItem(pJson, "action")->valuestring, "off") == 0)
            {
                if(ctrl_sess == MULTICAST)
                {
                    volume = 100;
                    ctrl_sess = IDLE;
                }
                mc_active = false;
            }
        }
        else if(strcmp(cJSON_GetObjectItem(pJson, "name")->valuestring, "ONVIF") == 0)
        {
            if(strcmp(cJSON_GetObjectItem(pJson, "action")->valuestring, "on") == 0)
            {
                get_volume_threshold();
                current_sess = ONVIF;
                onvif_active = true;
            }
            else if(strcmp(cJSON_GetObjectItem(pJson, "action")->valuestring, "off") == 0)
            {
                if(ctrl_sess == ONVIF)
                {
                    volume = 100;
                    ctrl_sess = IDLE;
                }
                onvif_active = false;
            }
        }
    }
    if(pJson != NULL) cJSON_Delete(pJson);
}

void *service_status_listen(void *arg)
{
    // redisContext * pContext = redisConnect( "127.0.0.1", 6379 );
    redisContext * pContext = redisConnectUnix(unix_socket_path);

    if ( NULL == pContext || pContext->err == 1 )
    {
        printf( "%s\n", pContext->errstr );
        exit( -1 );
    }
    void* replyPtr = redisCommand( pContext, "SUBSCRIBE %s", paging_channel );
    redisReply* pReply = static_cast<redisReply*>(replyPtr);
    freeReplyObject( pReply );
    while ( redisGetReply( pContext, (void **)&pReply ) == REDIS_OK )
    {
        if ( pReply->elements == 3 )
        {
            redisReply * pSubReply = pReply->element[2];
            ProcessReply( pSubReply->str );
        }
        if(pReply != NULL) freeReplyObject( pReply );
    }

    redisFree( pContext );
    return NULL;
}

void check_volume(int16_t *buffer, int count, vumeter_play *vu)
{
    int i;
    pthread_t thread;
    if(vu->ch1_avg_play > vol_threshold || vu->ch2_avg_play > vol_threshold)
    {
        for(i=0; i<count; i++)
        {
            buffer[i*2] = 0x00; //ch1[i]*0.35;
            buffer[i*2+1] = 0x00; //ch2[i]*0.35;
        }
        if(!set_volume)
        {
            ctrl_sess = current_sess;
            pthread_create(&thread, NULL, control_volume, NULL);
            pthread_detach(thread);
        }
    }
}

static float get_volume(int vol)
{
	float v = vol / 100.f;
	if (v < 0.f)
		v = 0.f;
	else if (v > 1.f)
		v = 1.f;

	return v;
}

void vol_uint16(int16_t *data, size_t n)
{
	size_t i;
	for (i = 0; i < n; i++)
    {
        data[i*2] = (int16_t) (data[i*2] * get_volume(volume));
        data[i*2+1] = (int16_t) (data[i*2+1] * get_volume(volume));
    }
}


int main(int argc, char *argv[])
{
    snd_pcm_t *capture_handle;
    snd_pcm_t *write_handle;
    vumeter_play *vu = ( vumeter_play *) malloc (sizeof (*vu));
    int err, i;
    int16_t buffer[READ_FRAME * 2];
    unsigned int sampleRate;
    unsigned int channels;
    pthread_t service_status_listen_thread, thread;
    // pthread_t volume_temp_value_listen_thread;

    if(argc == 2 && strcmp(argv[1], "debug") == 0)
        DEBUG = true;

repeat:
    //capture_handle = NULL;
    write_handle = NULL;
    err = 0;
    memset(buffer, 0, sizeof(buffer));
    sampleRate = SAMPLE_RATE;
    channels = CHANNEL;

    printf("\n==========EQ/DRC process release version 1.23===============\n");
    alsa_fake_device_record_open(&capture_handle, channels, sampleRate);
    alsa_fake_device_write_open(&write_handle, channels, sampleRate);
    vu->ch1_sampv = (int16_t *) malloc(READ_FRAME*2*sizeof(int16_t));
    vu->ch2_sampv = (int16_t *) malloc(READ_FRAME*2*sizeof(int16_t));
    vu->au_sampc = READ_FRAME;

    //RK_acquire_wake_lock(wake_lock);
    pthread_create(&thread, NULL, vumeter_decode, vu);
    pthread_detach(thread);

    pthread_create(&service_status_listen_thread, NULL, service_status_listen, NULL);
    pthread_detach(service_status_listen_thread);

    while (1) {
        err = snd_pcm_readi(capture_handle, buffer , READ_FRAME);
        if (err != READ_FRAME)
            printf("====read frame error = %d===\n",err);

        if (err < 0) {
            if (err == -EPIPE)
                printf( "Overrun occurred: %d\n", err);

            err = snd_pcm_recover(capture_handle, err, 0);
            // Still an error, need to exit.
            if (err < 0) {
                printf( "Error occured while recording: %s\n", snd_strerror(err));
                usleep(200 * 1000);
                if (capture_handle)
                    snd_pcm_close(capture_handle);
                if (write_handle)
                    snd_pcm_close(write_handle);
                goto repeat;
            }
        }

        if(en_threshold)
        {
            vol_uint16(buffer, READ_FRAME);
            check_volume(buffer, READ_FRAME, vu);
        }

        for(i=0; i<READ_FRAME; i++)
        {
            vu->ch1_sampv[i] = buffer[i*2];
            vu->ch2_sampv[i] = buffer[i*2+1];
        }

        err = snd_pcm_writei(write_handle, buffer, READ_FRAME);
        if (-EPIPE == err) {
			snd_pcm_prepare(write_handle);

			err = snd_pcm_writei(write_handle, buffer, READ_FRAME);
			if (err < 0) {
				printf("alsa: write error: %s\n",
					snd_strerror((int) err));
			}
		}
		else if (err < 0) {
            printf("alsa: write error: %s\n",
                snd_strerror((int) err));
		}
		else if (err != READ_FRAME) {
			printf("alsa: write: wrote %d of %d samples\n",
				(int) err, READ_FRAME);
		}
    }

error:
    if (capture_handle)
        snd_pcm_close(capture_handle);
    if (write_handle)
        snd_pcm_close(write_handle);

    return 0;
}