553 lines
16 KiB
C
553 lines
16 KiB
C
/* DSP functions
|
|
*
|
|
* (C) 2020 by Andreas Eversberg <jolly@eversberg.eu>
|
|
* All Rights Reserved
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#define CHAN ((ss5_t *)(dsp->priv))->name
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <math.h>
|
|
#include <errno.h>
|
|
#include <sys/types.h>
|
|
#include <arpa/inet.h>
|
|
#include "../libdebug/debug.h"
|
|
#include "ss5.h"
|
|
|
|
//#define DEBUG_DEMODULATOR
|
|
|
|
#define NUM_TONES 8
|
|
#define db2level(db) pow(10, (double)(db) / 20.0)
|
|
#define level2db(level) (20 * log10(level))
|
|
|
|
static double tone_dbm[NUM_TONES] = { -7, -7, -7, -7, -7, -7, -9, -9 };
|
|
static double tone_freq[NUM_TONES] = { 700, 900, 1100, 1300, 1500, 1700, 2400, 2600 };
|
|
static double tone_width[NUM_TONES] = { 25, 25, 25, 25, 25, 25, 25, 25 };
|
|
|
|
static double tone_min_dbm[NUM_TONES] = { -14, -14, -14, -14, -14, -14, -16, -16 };
|
|
static double tone_min_ampl_sq[NUM_TONES];
|
|
static double tone_diff_db[NUM_TONES] = { 4, 4, 4, 4, 4, 4, 5, 5 };
|
|
static double tone_diff_ampl_sq[NUM_TONES];
|
|
|
|
#define INTERRUPT_DURATION 0.015
|
|
#define SPLIT_DURATION 0.030
|
|
#define MF_RECOGNITION 0.025
|
|
|
|
int dsp_init_inst(dsp_t *dsp, void *priv, double samplerate, double sense_db)
|
|
{
|
|
double tone_amplitude[NUM_TONES];
|
|
int t;
|
|
|
|
PDEBUG(DDSP, DEBUG_DEBUG, "Init DSP for SS5 instance.\n");
|
|
|
|
memset(dsp, 0, sizeof(*dsp));
|
|
dsp->priv = priv;
|
|
dsp->samplerate = samplerate;
|
|
dsp->interrupt_duration = (int)(1000.0 * INTERRUPT_DURATION);
|
|
dsp->split_duration = (int)(1000.0 * SPLIT_DURATION);
|
|
dsp->mf_detect_duration = (int)(1000.0 * MF_RECOGNITION);
|
|
dsp->ms_per_sample = 1000.0 / samplerate;
|
|
dsp->detect_tone = ' ';
|
|
|
|
/* convert dbm of tones to speech level */
|
|
for (t = 0; t < NUM_TONES; t++) {
|
|
tone_amplitude[t] = db2level(tone_dbm[t]) / SPEECH_LEVEL;
|
|
tone_min_ampl_sq[t] = pow(db2level(tone_min_dbm[t] - sense_db) / SPEECH_LEVEL, 2);
|
|
tone_diff_ampl_sq[t] = pow(db2level(tone_diff_db[t]), 2);
|
|
}
|
|
|
|
/* init MF modulator */
|
|
dsp->mf_mod = mf_mod_init(samplerate, NUM_TONES, tone_freq, tone_amplitude);
|
|
if (!dsp->mf_mod)
|
|
return -EINVAL;
|
|
|
|
/* init MF demodulator */
|
|
dsp->mf_demod = mf_demod_init(samplerate, NUM_TONES, tone_freq, tone_width);
|
|
if (!dsp->mf_mod)
|
|
return -EINVAL;
|
|
|
|
return 0;
|
|
}
|
|
|
|
void dsp_cleanup_inst(dsp_t *dsp)
|
|
{
|
|
PDEBUG(DDSP, DEBUG_DEBUG, "Cleanup DSP of SS5 instance.\n");
|
|
|
|
if (dsp->mf_mod) {
|
|
mf_mod_exit(dsp->mf_mod);
|
|
dsp->mf_mod = NULL;
|
|
}
|
|
|
|
if (dsp->mf_demod) {
|
|
mf_demod_exit(dsp->mf_demod);
|
|
dsp->mf_demod = NULL;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* tone encoder
|
|
*/
|
|
|
|
static struct dsp_digits {
|
|
char tone;
|
|
uint32_t mask;
|
|
} dsp_digits[] = {
|
|
{ '1', 0x01 + 0x02 },
|
|
{ '2', 0x01 + 0x04 },
|
|
{ '3', 0x02 + 0x04 },
|
|
{ '4', 0x01 + 0x08 },
|
|
{ '5', 0x02 + 0x08 },
|
|
{ '6', 0x04 + 0x08 },
|
|
{ '7', 0x01 + 0x10 },
|
|
{ '8', 0x02 + 0x10 },
|
|
{ '9', 0x04 + 0x10 },
|
|
{ '0', 0x08 + 0x10 },
|
|
{ '*', 0x01 + 0x20 }, /* code 11 */
|
|
{ '#', 0x02 + 0x20 }, /* code 12 */
|
|
{ 'a', 0x04 + 0x20 }, /* KP1 */
|
|
{ 'b', 0x08 + 0x20 }, /* KP2 */
|
|
{ 'c', 0x10 + 0x20 }, /* ST */
|
|
{ 'A', 0x40 }, /* 2400 answer, acknowledge */
|
|
{ 'B', 0x80 }, /* 2600 busy */
|
|
{ 'C', 0x40 + 0x80 }, /* 2600+2400 clear forward */
|
|
{ ' ', 0 }, /* silence */
|
|
{ 0 , 0 },
|
|
};
|
|
|
|
#define KP_DIGIT_DURATION 0.100
|
|
#define OTHER_DIGIT_DURATION 0.055
|
|
#define DIGIT_PAUSE 0.055
|
|
|
|
/* set signaling tone duration threshold */
|
|
void set_sig_detect_duration(dsp_t *dsp, double duration_AB, double duration_C)
|
|
{
|
|
dsp->detect_count = 0;
|
|
dsp->sig_detect_duration_AB = (int)(1000.0 * duration_AB);
|
|
dsp->sig_detect_duration_C = (int)(1000.0 * duration_C);
|
|
}
|
|
|
|
/* set given tone with duration (ms) or continuously (0) */
|
|
void set_tone(dsp_t *dsp, char tone, double duration)
|
|
{
|
|
int i;
|
|
|
|
dsp->tone = 0;
|
|
|
|
if (!tone) {
|
|
PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Remove tone\n");
|
|
return;
|
|
}
|
|
|
|
for (i = 0; dsp_digits[i].tone; i++) {
|
|
if (dsp_digits[i].tone == tone) {
|
|
dsp->tone_mask = dsp_digits[i].mask;
|
|
dsp->tone = tone;
|
|
dsp->tone_duration = (int)(dsp->samplerate * duration);
|
|
PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Set tone=\'%c\' duration=%.0fms (mask = 0x%02x)\n", dsp->tone, 1000.0 * duration, dsp->tone_mask);
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* get next tone from dial string, if any */
|
|
static void get_tone_from_dial_string(dsp_t *dsp)
|
|
{
|
|
char tone;
|
|
double duration;
|
|
|
|
dsp->tone = 0;
|
|
|
|
if (dsp->dial_index == dsp->dial_length) {
|
|
dsp->dial_length = 0;
|
|
dialing_complete(dsp->priv);
|
|
return;
|
|
}
|
|
|
|
/* get alternating tone/pause from dial string */
|
|
if (!dsp->digit_pause) {
|
|
/* digit on */
|
|
tone = dsp->dial_string[dsp->dial_index++];
|
|
dsp->digit_pause = 1;
|
|
if (tone == 'a' || tone == 'b')
|
|
duration = KP_DIGIT_DURATION;
|
|
else
|
|
duration = OTHER_DIGIT_DURATION;
|
|
PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Send digit \'%c\' from dial string\n", tone);
|
|
} else {
|
|
/* digit pause */
|
|
tone = ' ';
|
|
dsp->digit_pause = 0;
|
|
duration = DIGIT_PAUSE;
|
|
PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Send pause after digit from dial string\n");
|
|
}
|
|
|
|
set_tone(dsp, tone, duration);
|
|
}
|
|
|
|
/* set given dial string */
|
|
void set_dial_string(dsp_t *dsp, const char *dial)
|
|
{
|
|
dsp->digit_pause = 0;
|
|
strncpy(dsp->dial_string, dial, sizeof(dsp->dial_string) - 1);
|
|
dsp->dial_index = 0;
|
|
dsp->dial_length = strlen(dsp->dial_string);
|
|
}
|
|
|
|
/* determine which tones to be modulated, get next tone, if elapsed */
|
|
static int assemble_tones(dsp_t *dsp, uint32_t *mask, int length)
|
|
{
|
|
int i;
|
|
|
|
for (i = 0; i < length; i++) {
|
|
/* if tone was done, try to get next digit */
|
|
if (!dsp->tone) {
|
|
if (!dsp->dial_length)
|
|
return i;
|
|
get_tone_from_dial_string(dsp);
|
|
if (!dsp->tone)
|
|
return i;
|
|
}
|
|
*mask++ = dsp->tone_mask;
|
|
if (dsp->tone_duration) {
|
|
/* count down duration, if tones is not continuous */
|
|
if (!(--dsp->tone_duration))
|
|
dsp->tone = 0;
|
|
}
|
|
}
|
|
|
|
return i;
|
|
}
|
|
|
|
/*
|
|
* tone deencoder
|
|
*/
|
|
|
|
/* detection array for one frequency */
|
|
static char decode_one[8] =
|
|
{ ' ', ' ', ' ', ' ', ' ', ' ', 'A', 'B' }; /* A = 2400, B = 2600 */
|
|
|
|
/* detection matrix for two frequencies */
|
|
static char decode_two[8][8] =
|
|
{
|
|
{ ' ', '1', '2', '4', '7', '*', ' ', ' ' }, /* * = code 11 */
|
|
{ '1', ' ', '3', '5', '8', '#', ' ', ' ' }, /* # = code 12 */
|
|
{ '2', '3', ' ', '6', '9', 'a', ' ', ' ' }, /* a = KP1 */
|
|
{ '4', '5', '6', ' ', '0', 'b', ' ', ' ' }, /* b = KP2 */
|
|
{ '7', '8', '9', '0', ' ', 'c', ' ', ' ' }, /* c = ST */
|
|
{ '*', '#', 'a', 'b', 'c', ' ', ' ', ' ' },
|
|
{ ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'C' }, /* C = 2600+2400 */
|
|
{ ' ', ' ', ' ', ' ', ' ', ' ', 'C', ' ' }
|
|
};
|
|
|
|
#define NONE_MIN_LEVEL_SQUARED
|
|
|
|
/* determine which tone is played */
|
|
static void detect_tones(dsp_t *dsp, sample_t *samples, sample_t **levels_squared, int length, int incoming)
|
|
{
|
|
int f1, f2;
|
|
double f1_level_squared, f2_level_squared;
|
|
char tone;
|
|
int s, t;
|
|
|
|
for (s = 0; s < length; s++) {
|
|
/* mute if split duration reached */
|
|
if (dsp->split_duration && dsp->split_count == dsp->split_duration)
|
|
samples[s] = 0.0;
|
|
/* only perform tone detection every millisecond */
|
|
dsp->detect_interval += dsp->ms_per_sample;
|
|
if (dsp->detect_interval < 1.0)
|
|
continue;
|
|
dsp->detect_interval -= 1.0;
|
|
|
|
if (incoming) {
|
|
#ifdef DEBUG_DEMODULATOR
|
|
for (t = 0; t < dsp->mf_demod->tones; t++) {
|
|
char level[20];
|
|
int db;
|
|
memset(level, 32, sizeof(level));
|
|
db = roundf(level2db(sqrt(levels_squared[t][s]) * SPEECH_LEVEL) + 25);
|
|
if (db >= 0 && db < (int)sizeof(level))
|
|
level[db] = '*';
|
|
level[sizeof(level)-1]=0;
|
|
printf("%s|", level);
|
|
}
|
|
printf("\n");
|
|
#endif
|
|
}
|
|
|
|
/* find the tone which is the loudest */
|
|
f1 = -1;
|
|
f1_level_squared = -1.0;
|
|
for (t = 0; t < dsp->mf_demod->tones; t++) {
|
|
if (levels_squared[t][s] > f1_level_squared) {
|
|
f1_level_squared = levels_squared[t][s];
|
|
f1 = t;
|
|
}
|
|
}
|
|
/* find the tone which is the second loudest */
|
|
f2 = -1;
|
|
f2_level_squared = -1.0;
|
|
for (t = 0; t < dsp->mf_demod->tones; t++) {
|
|
if (t == f1)
|
|
continue;
|
|
if (levels_squared[t][s] > f2_level_squared) {
|
|
f2_level_squared = levels_squared[t][s];
|
|
f2 = t;
|
|
}
|
|
}
|
|
/* now check if the minimum level is reached */
|
|
if (f1 >= 0 && f1_level_squared < tone_min_ampl_sq[f1])
|
|
f1 = -1;
|
|
if (f2 >= 0 && f2_level_squared < tone_min_ampl_sq[f2])
|
|
f2 = -1;
|
|
|
|
// printf("%s f1=%.0f (%.1f dBm) f2=%.0f (%.1f dBm)\n", CHAN, (f1 >= 0) ? tone_freq[f1] : 0, level2db(sqrt(f1_level_squared) * SPEECH_LEVEL), (f2 >= 0) ? tone_freq[f2] : 0, level2db(sqrt(f2_level_squared) * SPEECH_LEVEL));
|
|
/* check if no, one or two tones are detected */
|
|
if (f1 < 0)
|
|
tone = ' ';
|
|
else if (f2 < 0)
|
|
tone = decode_one[f1];
|
|
else {
|
|
if (f2_level_squared * tone_diff_ampl_sq[f2] < f1_level_squared)
|
|
tone = ' ';
|
|
else
|
|
tone = decode_two[f1][f2];
|
|
}
|
|
//printf("tone=%c\n", tone);
|
|
|
|
/* process interrupt counting, keep tone until interrupt counter expires */
|
|
if (dsp->detect_tone != ' ' && tone != dsp->detect_tone) {
|
|
if (dsp->interrupt_count < dsp->interrupt_duration) {
|
|
dsp->interrupt_count++;
|
|
tone = dsp->detect_tone;
|
|
}
|
|
} else
|
|
dsp->interrupt_count = 0;
|
|
|
|
/* split audio, after minimum duration of detecting a tone */
|
|
if (tone >= 'A' && tone <= 'C') {
|
|
if (dsp->split_count < dsp->split_duration)
|
|
dsp->split_count++;
|
|
} else
|
|
dsp->split_count = 0;
|
|
|
|
|
|
/* some change in tone */
|
|
if (dsp->detect_tone != tone) {
|
|
if (dsp->detect_count == 0)
|
|
PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Detected new tone '%c' (%.1f dBm)\n", tone, level2db(sqrt(f1_level_squared) * SPEECH_LEVEL));
|
|
switch (tone) {
|
|
case 'A':
|
|
case 'B':
|
|
/* tone appears, wait some time */
|
|
if (dsp->detect_count < dsp->sig_detect_duration_AB)
|
|
dsp->detect_count++;
|
|
else {
|
|
/* sign tone detected */
|
|
dsp->detect_count = 0;
|
|
dsp->detect_tone = tone;
|
|
receive_digit(dsp->priv, tone, level2db(sqrt(f1_level_squared) * SPEECH_LEVEL));
|
|
}
|
|
break;
|
|
case 'C':
|
|
/* tone appears, wait some time */
|
|
if (dsp->detect_count < dsp->sig_detect_duration_C)
|
|
dsp->detect_count++;
|
|
else {
|
|
/* sign tone detected */
|
|
dsp->detect_count = 0;
|
|
dsp->detect_tone = tone;
|
|
receive_digit(dsp->priv, tone, level2db(sqrt(f1_level_squared) * SPEECH_LEVEL));
|
|
}
|
|
break;
|
|
case ' ':
|
|
/* tone appears or ceases */
|
|
dsp->detect_count = 0;
|
|
dsp->detect_tone = tone;
|
|
receive_digit(dsp->priv, tone, 0.0);
|
|
break;
|
|
default:
|
|
/* tone appears, wait some time */
|
|
if (dsp->detect_count < dsp->mf_detect_duration)
|
|
dsp->detect_count++;
|
|
else {
|
|
/* sign tone detected */
|
|
dsp->detect_count = 0;
|
|
dsp->detect_tone = tone;
|
|
receive_digit(dsp->priv, tone, level2db(sqrt(f1_level_squared) * SPEECH_LEVEL));
|
|
}
|
|
}
|
|
} else
|
|
dsp->detect_count = 0;
|
|
}
|
|
}
|
|
|
|
/* process audio from one link (source) to another (destination) */
|
|
static void process_audio(ss5_t *ss5_a, ss5_t *ss5_b, int length)
|
|
{
|
|
sample_t samples[2][length], s;
|
|
sample_t b1[length], b2[length], b3[length], b4[length], b5[length], b6[length], b7[length], b8[length];
|
|
sample_t *levels_squared[NUM_TONES] = { b1, b2, b3, b4, b5, b6, b7, b8 };
|
|
uint32_t mask[length];
|
|
int16_t data[160];
|
|
int count1, count2;
|
|
int i;
|
|
|
|
/* trigger reception of RTP stuff */
|
|
if (ss5_a->cc_session)
|
|
osmo_cc_session_handle(ss5_a->cc_session);
|
|
if (ss5_b->cc_session)
|
|
osmo_cc_session_handle(ss5_b->cc_session);
|
|
|
|
/* get audio from jitter buffer */
|
|
jitter_load(&ss5_a->dejitter, samples[0], length);
|
|
jitter_load(&ss5_b->dejitter, samples[1], length);
|
|
|
|
/* optionally add comfort noise */
|
|
if (!ss5_a->cc_callref && ss5_a->ss5_ep->comfort_noise) {
|
|
for (i = 0; i < length; i++)
|
|
samples[0][i] += (double)((int8_t)random()) / 2000.0;
|
|
}
|
|
if (!ss5_b->cc_callref && ss5_b->ss5_ep->comfort_noise) {
|
|
for (i = 0; i < length; i++)
|
|
samples[1][i] += (double)((int8_t)random()) / 2000.0;
|
|
}
|
|
|
|
/* modulate tone/digit. if no tone has to be played (or it stopped), count is less than length */
|
|
count1 = assemble_tones(&ss5_a->dsp, mask, length);
|
|
mf_mod(ss5_a->dsp.mf_mod, mask, samples[0], count1);
|
|
count2 = assemble_tones(&ss5_b->dsp, mask, length);
|
|
mf_mod(ss5_b->dsp.mf_mod, mask, samples[1], count2);
|
|
|
|
/* optionally add some crosstalk */
|
|
if (ss5_a->ss5_ep->crosstalk) {
|
|
/* use count, since it carries number of samples with signalling */
|
|
for (i = 0; i < count1; i++)
|
|
samples[1][i] += samples[0][i] / 70.0;
|
|
}
|
|
if (ss5_b->ss5_ep->crosstalk) {
|
|
/* use count, since it carries number of samples with signalling */
|
|
for (i = 0; i < count2; i++)
|
|
samples[0][i] += samples[1][i] / 70.0;
|
|
}
|
|
|
|
/* ! here is the bridge from a to b and from b to a ! */
|
|
|
|
/* optionally add one way delay */
|
|
if (ss5_b->delay_buffer) {
|
|
for (i = 0; i < length; i++) {
|
|
s = ss5_b->delay_buffer[ss5_b->delay_index];
|
|
ss5_b->delay_buffer[ss5_b->delay_index] = samples[0][i];
|
|
if (++(ss5_b->delay_index) == ss5_b->delay_length)
|
|
ss5_b->delay_index = 0;
|
|
samples[0][i] = s;
|
|
}
|
|
}
|
|
if (ss5_a->delay_buffer) {
|
|
for (i = 0; i < length; i++) {
|
|
s = ss5_a->delay_buffer[ss5_a->delay_index];
|
|
ss5_a->delay_buffer[ss5_a->delay_index] = samples[1][i];
|
|
if (++(ss5_a->delay_index) == ss5_a->delay_length)
|
|
ss5_a->delay_index = 0;
|
|
samples[1][i] = s;
|
|
}
|
|
}
|
|
|
|
/* demodulate and call tone detector */
|
|
mf_demod(ss5_b->dsp.mf_demod, samples[0], length, levels_squared);
|
|
detect_tones(&ss5_b->dsp, samples[0], levels_squared, length, 1);
|
|
mf_demod(ss5_a->dsp.mf_demod, samples[1], length, levels_squared);
|
|
detect_tones(&ss5_a->dsp, samples[1], levels_squared, length, 0);
|
|
|
|
/* forward audio to CC if call exists */
|
|
if (ss5_b->cc_callref && ss5_b->codec) {
|
|
samples_to_int16(data, samples[0], length);
|
|
osmo_cc_rtp_send(ss5_b->codec, (uint8_t *)data, length * 2, 1, length);
|
|
}
|
|
if (ss5_a->cc_callref && ss5_a->codec) {
|
|
samples_to_int16(data, samples[1], length);
|
|
osmo_cc_rtp_send(ss5_a->codec, (uint8_t *)data, length * 2, 1, length);
|
|
}
|
|
}
|
|
|
|
/* clock is called every given number of samples (20ms) */
|
|
void audio_clock(ss5_endpoint_t *ss5_ep_sunset, ss5_endpoint_t *ss5_ep_sunrise, int len)
|
|
{
|
|
ss5_t *ss5_a, *ss5_b;
|
|
|
|
if (!ss5_ep_sunset)
|
|
return;
|
|
|
|
if (!ss5_ep_sunrise) {
|
|
/* each pair of links on the same endpoint are bridged */
|
|
for (ss5_b = ss5_ep_sunset->link_list; ss5_b; ss5_b = ss5_b->next) {
|
|
ss5_a = ss5_b;
|
|
ss5_b = ss5_b->next;
|
|
if (!ss5_b)
|
|
break;
|
|
process_audio(ss5_a, ss5_b, len);
|
|
}
|
|
} else {
|
|
/* each link on two endpoints are bridged */
|
|
for (ss5_a = ss5_ep_sunset->link_list, ss5_b = ss5_ep_sunrise->link_list; ss5_a && ss5_b; ss5_a = ss5_a->next, ss5_b = ss5_b->next) {
|
|
process_audio(ss5_a, ss5_b, len);
|
|
}
|
|
}
|
|
}
|
|
|
|
/* take audio from CC and store in jitter buffer */
|
|
void down_audio(struct osmo_cc_session_codec *codec, uint16_t __attribute__((unused)) sequence_number, uint32_t __attribute__((unused)) timestamp, uint8_t *data, int len)
|
|
{
|
|
ss5_t *ss5 = codec->media->session->priv;
|
|
sample_t samples[len / 2];
|
|
|
|
int16_to_samples(samples, (int16_t *)data, len / 2);
|
|
jitter_save(&ss5->dejitter, samples, len / 2);
|
|
}
|
|
|
|
void encode_l16(uint8_t *src_data, int src_len, uint8_t **dst_data, int *dst_len)
|
|
{
|
|
uint16_t *src = (uint16_t *)src_data, *dst;
|
|
int len = src_len / 2, i;
|
|
|
|
dst = malloc(len * 2);
|
|
if (!dst)
|
|
return;
|
|
for (i = 0; i < len; i++)
|
|
dst[i] = htons(src[i]);
|
|
*dst_data = (uint8_t *)dst;
|
|
*dst_len = len * 2;
|
|
}
|
|
|
|
void decode_l16(uint8_t *src_data, int src_len, uint8_t **dst_data, int *dst_len)
|
|
{
|
|
uint16_t *src = (uint16_t *)src_data, *dst;
|
|
int len = src_len / 2, i;
|
|
|
|
dst = malloc(len * 2);
|
|
if (!dst)
|
|
return;
|
|
for (i = 0; i < len; i++)
|
|
dst[i] = ntohs(src[i]);
|
|
*dst_data = (uint8_t *)dst;
|
|
*dst_len = len * 2;
|
|
}
|
|
|