static VALUE stream_write(VALUE self, VALUE buffer)
{
PaError err = Pa_WriteStream(stream, get_samples(buffer), get_buffer_size(buffer));
raise_if_error(err, ERROR_WRITING_STREAM);
return Qtrue;
}
inline void perform(output_buffer_type & out, uint n, wavetable_type const & wavetable)
{
sample_type * out_sample = get_samples(out);
const table_type * table = get_samples(wavetable);
internal_type phase = phasor_type::phase_;
do
{
internal_type index = phasor_type::compute_sample(phase, phasor_type::phase_inc_);
*out_sample = interpolation_wrapper<interpolation_precision>(table, index);
++out_sample;
--n;
}
while (n);
phasor_type::phase_ = phase;
}
void PeriodicDataRecord::get_data_samples(int ch, std::vector<DataSample<double> > &data_samples) const
{
std::vector<double> samples;
get_samples(ch, samples);
data_samples.resize(samples.size());
for (unsigned i = 0; i < samples.size(); i++) {
double frac = (double)i / samples.size();
data_samples[i].time = (1-frac)*first_sample_time + frac*last_sample_plus_one_time;
data_samples[i].value = samples[i];
}
}
// get data from /dev/dsp, apply a FFT on it and do HPS.
// return the freq with max amplitude (hopefully ;-)
void
get_max_amp(void)
{
float max;
double temp;
max = temp = 0.0;
int i=0,maxi=0;
double prefft[N];
fftw_complex postfft[N];
fftw_plan plan;
float amp[N], amp_down2[N], amp_down4[N];
while (1) {
get_samples(sample, N);
// add function to check for volume level
// do more calculations if volume is above a threshold else return
for(i=0;i<N;i++) {
prefft[i]=((float)sample[i] * mycosine[i]);
}
// do FFT
plan = fftw_plan_dft_r2c_1d(N, prefft, postfft, 0);
fftw_execute(plan);
// calculate amplitude of the freq's
for (i = 0; i < 9000; i++ ) {
amp[i] = postfft[i][0] * postfft[i][0] + postfft[i][1] * postfft[i][1];
}
//downsample by 2
downsample(amp, amp_down2, 4500);
//downsample by 2 more
downsample(amp_down2, amp_down4, 2250);
// downsample by a factor of three also
max_amp = 0;
max = 0.0;
// multiply the amplitudes
for (i = 20; i < 1000; i++) {
temp = amp[i] * amp_down2[i] * amp_down4[i];
if (temp > max) {
max = temp;
maxi = i;
}
}
max_amp = maxi;
new_freq = 1;
}
}
static int
tta_read (DB_fileinfo_t *_info, char *bytes, int size) {
tta_info_t *info = (tta_info_t *)_info;
int samplesize = _info->fmt.channels * _info->fmt.bps / 8;
if (info->currentsample + size / samplesize > info->endsample) {
size = (info->endsample - info->currentsample + 1) * samplesize;
if (size <= 0) {
return 0;
}
}
int initsize = size;
while (size > 0) {
if (info->samples_to_skip > 0 && info->remaining > 0) {
int skip = min (info->remaining, info->samples_to_skip);
if (skip < info->remaining) {
memmove (info->buffer, info->buffer + skip * samplesize, (info->remaining - skip) * samplesize);
}
info->remaining -= skip;
info->samples_to_skip -= skip;
}
if (info->remaining > 0) {
int n = size / samplesize;
n = min (n, info->remaining);
int nn = n;
char *p = info->buffer;
memcpy (bytes, p, n * samplesize);
bytes += n * samplesize;
size -= n * samplesize;
p += n * samplesize;
if (info->remaining > nn) {
memmove (info->buffer, p, (info->remaining - nn) * samplesize);
}
info->remaining -= nn;
}
if (size > 0 && !info->remaining) {
info->remaining = get_samples (&info->tta, info->buffer);
if (info->remaining <= 0) {
break;
}
}
}
info->currentsample += (initsize-size) / samplesize;
deadbeef->streamer_set_bitrate (info->tta.BITRATE);
return initsize-size;
}
inline void perform(input_buffer_type const & in, output_buffer_type & out, uint n, wavetable_type const & wavetable,
uint table_size)
{
const sample_type * in_sample = get_samples(in);
sample_type * out_sample = get_samples(out);
const table_type * table = get_samples(wavetable);
internal_type phase = phasor_type::phase_;
do
{
internal_type phase_inc = phasor_type::freq_factor * *in_sample;
internal_type index = phasor_type::compute_sample(phase, phase_inc) * table_size;
*out_sample = interpolation_wrapper<interpolation_precision>(table, index);
++out_sample;
++in_sample;
--n;
}
while (n);
phasor_type::phase_ = phase;
}
double ransac_plane_fitting (sPoint *data, int no_data, sPlane &model, double distance_threshold)
{
const int no_samples = 3;
if (no_data < no_samples) {
return 0.;
}
sPoint *samples = new sPoint[no_samples];
int no_inliers = 0;
sPoint *inliers = new sPoint[no_data];
sPlane estimated_model;
double max_cost = 0.;
int max_iteration = (int)(1 + log(1. - 0.99)/log(1. - pow(0.5, no_samples)));
for (int i = 0; i<max_iteration; i++) {
// 1. hypothesis
// 원본 데이터에서 임의로 N개의 셈플 데이터를 고른다.
get_samples (samples, no_samples, data, no_data);
// 이 데이터를 정상적인 데이터로 보고 모델 파라메터를 예측한다.
compute_model_parameter (samples, no_samples, estimated_model);
// if (!estimated_model.convert_std_form ()) { --i; continue; }
// 2. Verification
// 원본 데이터가 예측된 모델에 잘 맞는지 검사한다.
double cost = model_verification (inliers, &no_inliers, estimated_model, data, no_data, distance_threshold);
// 만일 예측된 모델이 잘 맞는다면, 이 모델에 대한 유효한 데이터로 새로운 모델을 구한다.
if (max_cost < cost) {
max_cost = cost;
compute_model_parameter (inliers, no_inliers, model);
// model.convert_std_form ();
}
}
delete [] samples;
delete [] inliers;
return max_cost;
}
static int Process(tta* p,format_stream* Stream)
{
int Result = ERR_NONE;
int No,Burst;
if (Stream->Pending)
{
Result = Format_Send(&p->Format,Stream);
if (Result == ERR_BUFFER_FULL || Result == ERR_SYNCED)
return Result;
}
Burst = Stream->PacketBurst;
for (No=0;No<Burst;++No)
{
if (p->Format.Reader[0].BufferAvailable < (MINBUFFER/2) &&
!p->Format.Reader[0].NoMoreInput)
return ERR_NEED_MORE_DATA;
p->BufferFilled = get_samples(p->Buffer);
if (p->BufferFilled == -1)
return ERR_INVALID_DATA;
if (p->BufferFilled == 0)
return Format_CheckEof(&p->Format,Stream);
Stream->Packet.RefTime = (tick_t)((p->Samples * TICKSPERSEC) / p->SampleRate);
Stream->Packet.Data[0] = p->Buffer;
Stream->Packet.Length = p->BufferFilled * p->SampleSize;
Stream->Pending = 1;
p->Samples += p->BufferFilled;
Result = Format_Send(&p->Format,Stream);
if (Result == ERR_BUFFER_FULL || Result == ERR_SYNCED)
break;
}
if (Result == ERR_BUFFER_FULL || Result == ERR_NEED_MORE_DATA)
Result = ERR_NONE;
return Result;
}
/**
* This function calculates the fourier transform of the given samples with the provided k
* value. It is returned as a 2-element array in which the first element is the real part
* and the second is the imaginary part.
*/
JNIEXPORT jintArray JNICALL Java_SoundInfo_get_1samples(JNIEnv *env, jclass cls,
jint number, jint start, jstring fileName)
{
jintArray samples;
const char *file_path;
int *result;
samples = (*env)->NewIntArray(env, number+1);
file_path = (*env)->GetStringUTFChars(env, fileName, 0);
/*Call the c function that does all the real work*/
result = get_samples(number, start, file_path);
(*env)->ReleaseStringUTFChars(env, fileName, file_path);
(*env)->SetIntArrayRegion(env, samples, 0, number+1, result);
return samples;
}
void BladeRFSource::run()
{
IQSampleVector iqsamples;
void *msgBuf = 0;
while (!m_this->m_stop_flag->load() && get_samples(&iqsamples))
{
m_this->m_buf->push(move(iqsamples));
int len = nn_recv(m_this->m_nnReceiver, &msgBuf, NN_MSG, NN_DONTWAIT);
if ((len > 0) && msgBuf)
{
std::string msg((char *) msgBuf, len);
std::cerr << "BladeRFSource::run: received: " << msg << std::endl;
m_this->Source::configure(msg);
nn_freemsg(msgBuf);
msgBuf = 0;
}
}
}
void PCA9685::process()
{
QLOG_TOPIC("PCA9685::process");
auto i2c = m_i2c.lock();
if (!i2c)
{
return;
}
for (size_t i = 0; i < m_pwm_channels.size(); i++)
{
auto& ch = m_pwm_channels[i];
auto stream = ch.stream.lock();
if (stream)
{
auto const& samples = stream->get_samples();
if (!samples.empty())
{
set_pwm_value(*i2c, i, samples.back().value);
}
}
}
}
inline void UNSAFE_set_channel(size_t channel_index, const void* buffer, size_t size)
{
LB_ASSERT(size<=get_samples(), "Sample number mismatch during assignment.");
memcpy(reinterpret_cast<ScalarType*>(&(_payload[channel_index][0])),
reinterpret_cast<const ScalarType*>(buffer), size*sizeof(ScalarType));
}
MESQUITE_EXPORT
NodeSet get_samples( size_t element ) const
{ return get_samples( element, non_slave_node_set( element ) ); }
/* this is called for each file to process */
enum codec_status codec_run(void)
{
tta_info info;
unsigned int decodedsamples;
int endofstream;
int new_pos = 0;
int sample_count;
intptr_t param;
if (codec_init())
{
DEBUGF("codec_init() error\n");
return CODEC_ERROR;
}
ci->seek_buffer(0);
if (set_tta_info(&info) < 0 || player_init(&info) < 0)
return CODEC_ERROR;
codec_set_replaygain(ci->id3);
ci->configure(DSP_SWITCH_FREQUENCY, ci->id3->frequency);
if (info.NCH == 2) {
ci->configure(DSP_SET_STEREO_MODE, STEREO_INTERLEAVED);
} else if (info.NCH == 1) {
ci->configure(DSP_SET_STEREO_MODE, STEREO_MONO);
} else {
DEBUGF("CODEC_ERROR: more than 2 channels\n");
player_stop();
return CODEC_ERROR;
}
/* The main decoder loop */
decodedsamples = 0;
endofstream = 0;
if (ci->id3->offset > 0)
{
/* Need to save offset for later use (cleared indirectly by advance_buffer) */
new_pos = set_position(ci->id3->offset, TTA_SEEK_POS);
if (new_pos >= 0)
decodedsamples = new_pos;
}
ci->set_elapsed((uint64_t)info.LENGTH * 1000 * decodedsamples / info.DATALENGTH);
while (!endofstream)
{
enum codec_command_action action = ci->get_command(¶m);
if (action == CODEC_ACTION_HALT)
break;
if (action == CODEC_ACTION_SEEK_TIME)
{
new_pos = set_position(param / SEEK_STEP, TTA_SEEK_TIME);
if (new_pos >= 0)
{
decodedsamples = new_pos;
}
ci->set_elapsed((uint64_t)info.LENGTH * 1000 * decodedsamples / info.DATALENGTH);
ci->seek_complete();
}
sample_count = get_samples(samples);
if (sample_count < 0)
break;
ci->pcmbuf_insert(samples, NULL, sample_count);
decodedsamples += sample_count;
if (decodedsamples >= info.DATALENGTH)
endofstream = 1;
ci->set_elapsed((uint64_t)info.LENGTH * 1000 * decodedsamples / info.DATALENGTH);
}
player_stop();
return CODEC_OK;
}
inline void UNSAFE_retrieve_channel(size_t channel_index, const ScalarType*& channel_ptr, size_t& size) const
{
LB_ASSERT(channel_index<get_channels(), "Tried to retrieve a non existing channel (index exceeding array dimensions).");
channel_ptr = reinterpret_cast<const ScalarType*>(&(_payload[channel_index][0]));
size = get_samples();
}
/**
* TTA音乐播放回调函数，
* 负责将解码数据填充声音缓存区
*
* @note 声音缓存区的格式为双声道，16位低字序
*
* @param buf 声音缓冲区指针
* @param reqn 缓冲区帧大小
* @param pdata 用户数据，无用
*/
static int tta_audiocallback(void *buf, unsigned int reqn, void *pdata)
{
int avail_frame;
int snd_buf_frame_size = (int) reqn;
int ret;
double incr;
signed short *audio_buf = buf;
UNUSED(pdata);
if (g_status != ST_PLAYING) {
if (g_status == ST_FFORWARD) {
g_play_time += g_seek_seconds;
if (g_play_time >= g_info.duration) {
__end();
return -1;
}
generic_lock();
g_status = ST_PLAYING;
generic_set_playback(true);
generic_unlock();
tta_seek_seconds(g_play_time);
} else if (g_status == ST_FBACKWARD) {
g_play_time -= g_seek_seconds;
if (g_play_time < 0.) {
g_play_time = 0.;
}
generic_lock();
g_status = ST_PLAYING;
generic_set_playback(true);
generic_unlock();
tta_seek_seconds(g_play_time);
}
xAudioClearSndBuf(buf, snd_buf_frame_size);
xrKernelDelayThread(100000);
return 0;
}
while (snd_buf_frame_size > 0) {
avail_frame = g_buff_frame_size - g_buff_frame_start;
if (avail_frame >= snd_buf_frame_size) {
send_to_sndbuf(audio_buf,
&g_buff[g_buff_frame_start * g_info.channels],
snd_buf_frame_size, g_info.channels);
g_buff_frame_start += snd_buf_frame_size;
audio_buf += snd_buf_frame_size * 2;
snd_buf_frame_size = 0;
} else {
send_to_sndbuf(audio_buf,
&g_buff[g_buff_frame_start * g_info.channels],
avail_frame, g_info.channels);
snd_buf_frame_size -= avail_frame;
audio_buf += avail_frame * 2;
if (g_samples_decoded >= g_info.samples) {
__end();
return -1;
}
ret = get_samples((byte *) g_buff);
if (ret <= 0) {
__end();
return -1;
}
g_buff_frame_size = ret;
g_buff_frame_start = 0;
incr = (double) (g_buff_frame_size) / g_info.sample_freq;
g_play_time += incr;
g_samples_decoded += g_buff_frame_size;
}
}
return 0;
}
void skip_samples(unsigned int quantity)
{
int16_t throwaway_samples[quantity];
get_samples(quantity, throwaway_samples);
}
/* this is the codec entry point */
enum codec_status codec_main(void)
{
tta_info info;
int status;
unsigned int decodedsamples;
int endofstream;
int new_pos = 0;
int sample_count;
/* Generic codec initialisation */
ci->configure(DSP_SET_SAMPLE_DEPTH, TTA_OUTPUT_DEPTH - 1);
next_track:
status = CODEC_OK;
if (codec_init())
{
DEBUGF("codec_init() error\n");
status = CODEC_ERROR;
goto exit;
}
if (codec_wait_taginfo() != 0)
goto done;
if (set_tta_info(&info) < 0 || player_init(&info) < 0)
{
status = CODEC_ERROR;
goto exit;
}
codec_set_replaygain(ci->id3);
ci->configure(DSP_SWITCH_FREQUENCY, ci->id3->frequency);
if (info.NCH == 2) {
ci->configure(DSP_SET_STEREO_MODE, STEREO_INTERLEAVED);
} else if (info.NCH == 1) {
ci->configure(DSP_SET_STEREO_MODE, STEREO_MONO);
} else {
DEBUGF("CODEC_ERROR: more than 2 channels\n");
status = CODEC_ERROR;
goto done;
}
/* The main decoder loop */
decodedsamples = 0;
endofstream = 0;
if (ci->id3->offset > 0)
{
/* Need to save offset for later use (cleared indirectly by advance_buffer) */
new_pos = set_position(ci->id3->offset, TTA_SEEK_POS);
if (new_pos >= 0)
decodedsamples = new_pos;
ci->seek_complete();
}
while (!endofstream)
{
ci->yield();
if (ci->stop_codec || ci->new_track)
break;
if (ci->seek_time)
{
new_pos = set_position(ci->seek_time / SEEK_STEP, TTA_SEEK_TIME);
if (new_pos >= 0)
{
decodedsamples = new_pos;
ci->seek_complete();
}
}
sample_count = get_samples(samples);
if (sample_count < 0)
{
status = CODEC_ERROR;
break;
}
ci->pcmbuf_insert(samples, NULL, sample_count);
decodedsamples += sample_count;
if (decodedsamples >= info.DATALENGTH)
endofstream = 1;
ci->set_elapsed((uint64_t)info.LENGTH * 1000 * decodedsamples / info.DATALENGTH);
}
done:
player_stop();
if (ci->request_next_track())
goto next_track;
exit:
return status;
}
int main(void) {
volatile int16_t* samples;
unsigned int i;
DISABLE_GLOBAL_INT();
/* stop watchdog timer */
WDTCTL = WDTPW +WDTHOLD;
/* SET CPU to 5MHz */
/* max DCO
MCLK = DCOCLK
SMCLK = DCOCLK
ACLK = 8KHz
*/
DCOCTL = DCO0 + DCO1 + DCO2;
BCSCTL1 = RSEL0 + RSEL1 + RSEL2 + XT2OFF;
BCSCTL2 = 0x00;
delay_us(10000);
/* activate Active Mode */
__bic_SR_register(LPM4_bits);
/* set LEDs when loaded */
P5SEL = 0x00;
P5DIR = 0x70;
LED_RED_ON();
LED_GREEN_OFF();
LED_BLUE_OFF();
check_for_clock();
init_usb_serial();
#ifdef USE_DMA
init_dma(&g_sample_flag);
#endif
#ifdef TX
init_adc(&g_sample_flag);
#else
init_dac();
#endif
init_rf(RF_CHANNEL, PAN_ID, NODE_ADDR, &g_sample_flag);
debug_print("Successfully booted.\n");
/* set LEDS to signalize finished initilizing */
LED_RED_OFF();
ENABLE_GLOBAL_INT();
#ifdef TX
/* TX */
while(1) {
if(g_sample_flag == 1) {
g_sample_flag = 0;
#ifdef USE_DMA
/* get samples */
samples = get_samples_dma();
#else
/* get samples */
samples = get_samples();
#endif
/* send oder radio, 2*num_words */
send_rf_data(RF_RX_ADDR, (uint8_t*) samples, NUM_SAMPLES*2);
}
/* reset WDT */
WDTCTL = WDTPW + WDTCNTCL;
}
#else
/* RX */
while(1) {
if(g_sample_flag == 1) {
g_sample_flag = 0;
samples = get_samples_rf();
#if 0
uint8_t err = 0;
for(i = 0; i < NUM_SAMPLES; ++i) {
//samples[i] = 4095-7*i;
usb_printf("%d\n", samples[i]);
//if( ((uint16_t) samples[i]) > 4095) {
// usb_printf("i=%u\n", i);
// ++err;
//}
}
usb_printf("#error: %u\n", err);
usb_printf("\n\n");
#endif
set_dma_data(samples, NUM_SAMPLES);
}
/* reset WDT */
WDTCTL = WDTPW + WDTCNTCL;
}
#endif
return 0;
}
int main(int argc, char *argv[])
{
unsigned i;
int err;
struct igb_dma_alloc a_page;
struct igb_packet a_packet;
struct igb_packet *tmp_packet;
struct igb_packet *cleaned_packets;
struct igb_packet *free_packets;
int c;
u_int64_t last_time;
int rc = 0;
char *interface = NULL;
int class_a_id = 0;
int a_priority = 0;
u_int16_t a_vid = 0;
#ifdef DOMAIN_QUERY
int class_b_id = 0;
int b_priority = 0;
u_int16_t b_vid = 0;
#endif
int seqnum;
int time_stamp;
unsigned total_samples = 0;
gPtpTimeData td;
int32_t sample_buffer[SAMPLES_PER_FRAME * SRC_CHANNELS];
seventeen22_header *header0;
six1883_header *header1;
six1883_sample *sample;
uint64_t now_local, now_8021as;
uint64_t update_8021as;
unsigned delta_8021as, delta_local;
long double ml_ratio;
for (;;) {
c = getopt(argc, argv, "hi:");
if (c < 0)
break;
switch (c) {
case 'h':
usage();
break;
case 'i':
if (interface) {
printf
("only one interface per daemon is supported\n");
usage();
}
interface = strdup(optarg);
break;
}
}
if (optind < argc)
usage();
if (NULL == interface) {
usage();
}
rc = mrp_connect();
if (rc) {
printf("socket creation failed\n");
return (errno);
}
err = pci_connect();
if (err) {
printf("connect failed (%s) - are you running as root?\n",
strerror(errno));
return (errno);
}
err = igb_init(&igb_dev);
if (err) {
printf("init failed (%s) - is the driver really loaded?\n",
strerror(errno));
return (errno);
}
err = igb_dma_malloc_page(&igb_dev, &a_page);
if (err) {
printf("malloc failed (%s) - out of memory?\n",
strerror(errno));
return (errno);
}
signal(SIGINT, sigint_handler);
rc = get_mac_address(interface);
if (rc) {
printf("failed to open interface\n");
usage();
}
mrp_monitor();
#ifdef DOMAIN_QUERY
/*
* should use mrp_get_domain() above but this is a simplification
*/
#endif
domain_a_valid = 1;
class_a_id = MSRP_SR_CLASS_A;
a_priority = MSRP_SR_CLASS_A_PRIO;
a_vid = 2;
printf("detected domain Class A PRIO=%d VID=%04x...\n", a_priority,
a_vid);
#define PKT_SZ 100
mrp_register_domain(&class_a_id, &a_priority, &a_vid);
igb_set_class_bandwidth(&igb_dev, PACKET_IPG / 125000, 0, PKT_SZ - 22,
0);
memset(STREAM_ID, 0, sizeof(STREAM_ID));
memcpy(STREAM_ID, STATION_ADDR, sizeof(STATION_ADDR));
a_packet.dmatime = a_packet.attime = a_packet.flags = 0;
a_packet.map.paddr = a_page.dma_paddr;
a_packet.map.mmap_size = a_page.mmap_size;
a_packet.offset = 0;
a_packet.vaddr = a_page.dma_vaddr + a_packet.offset;
a_packet.len = PKT_SZ;
free_packets = NULL;
seqnum = 0;
/* divide the dma page into buffers for packets */
for (i = 1; i < ((a_page.mmap_size) / PKT_SZ); i++) {
tmp_packet = malloc(sizeof(struct igb_packet));
if (NULL == tmp_packet) {
printf("failed to allocate igb_packet memory!\n");
return (errno);
}
*tmp_packet = a_packet;
tmp_packet->offset = (i * PKT_SZ);
tmp_packet->vaddr += tmp_packet->offset;
tmp_packet->next = free_packets;
memset(tmp_packet->vaddr, 0, PKT_SZ); /* MAC header at least */
memcpy(tmp_packet->vaddr, DEST_ADDR, sizeof(DEST_ADDR));
memcpy(tmp_packet->vaddr + 6, STATION_ADDR,
sizeof(STATION_ADDR));
/* Q-tag */
((char *)tmp_packet->vaddr)[12] = 0x81;
((char *)tmp_packet->vaddr)[13] = 0x00;
((char *)tmp_packet->vaddr)[14] =
((a_priority << 13 | a_vid)) >> 8;
((char *)tmp_packet->vaddr)[15] =
((a_priority << 13 | a_vid)) & 0xFF;
((char *)tmp_packet->vaddr)[16] = 0x22; /* 1722 eth type */
((char *)tmp_packet->vaddr)[17] = 0xF0;
/* 1722 header update + payload */
header0 =
(seventeen22_header *) (((char *)tmp_packet->vaddr) + 18);
header0->cd_indicator = 0;
header0->subtype = 0;
header0->sid_valid = 1;
header0->version = 0;
header0->reset = 0;
header0->reserved0 = 0;
header0->gateway_valid = 0;
header0->reserved1 = 0;
header0->timestamp_uncertain = 0;
memset(&(header0->stream_id), 0, sizeof(header0->stream_id));
memcpy(&(header0->stream_id), STATION_ADDR,
sizeof(STATION_ADDR));
header0->length = htons(32);
header1 = (six1883_header *) (header0 + 1);
header1->format_tag = 1;
header1->packet_channel = 0x1F;
header1->packet_tcode = 0xA;
header1->app_control = 0x0;
header1->reserved0 = 0;
header1->source_id = 0x3F;
header1->data_block_size = 1;
header1->fraction_number = 0;
header1->quadlet_padding_count = 0;
header1->source_packet_header = 0;
header1->reserved1 = 0;
header1->eoh = 0x2;
header1->format_id = 0x10;
header1->format_dependent_field = 0x02;
header1->syt = 0xFFFF;
tmp_packet->len =
18 + sizeof(seventeen22_header) + sizeof(six1883_header) +
(SAMPLES_PER_FRAME * CHANNELS * sizeof(six1883_sample));
free_packets = tmp_packet;
}
/*
* subtract 16 bytes for the MAC header/Q-tag - pktsz is limited to the
* data payload of the ethernet frame .
*
* IPG is scaled to the Class (A) observation interval of packets per 125 usec
*/
fprintf(stderr, "advertising stream ...\n");
mrp_advertise_stream(STREAM_ID, DEST_ADDR, a_vid, PKT_SZ - 16,
PACKET_IPG / 125000, a_priority, 3900);
fprintf(stderr, "awaiting a listener ...\n");
mrp_await_listener(STREAM_ID);
printf("got a listener ...\n");
halt_tx = 0;
gptpinit();
gptpscaling(&td);
if( igb_get_wallclock( &igb_dev, &now_local, NULL ) != 0 ) {
fprintf( stderr, "Failed to get wallclock time\n" );
return -1;
}
update_8021as = td.local_time - td.ml_phoffset;
delta_local = (unsigned)(now_local - td.local_time);
ml_ratio = -1 * (((long double)td.ml_freqoffset) / 1000000000000) + 1;
delta_8021as = (unsigned)(ml_ratio * delta_local);
now_8021as = update_8021as + delta_8021as;
last_time = now_local + XMIT_DELAY;
time_stamp = now_8021as + RENDER_DELAY;
rc = nice(-20);
while (listeners && !halt_tx) {
tmp_packet = free_packets;
if (NULL == tmp_packet)
goto cleanup;
header0 =
(seventeen22_header *) (((char *)tmp_packet->vaddr) + 18);
header1 = (six1883_header *) (header0 + 1);
free_packets = tmp_packet->next;
/* unfortuntely unless this thread is at rtprio
* you get pre-empted between fetching the time
* and programming the packet and get a late packet
*/
tmp_packet->attime = last_time + PACKET_IPG;
last_time += PACKET_IPG;
get_samples(SAMPLES_PER_FRAME, sample_buffer);
header0->seq_number = seqnum++;
if (seqnum % 4 == 0)
header0->timestamp_valid = 0;
else
header0->timestamp_valid = 1;
time_stamp = htonl(time_stamp);
header0->timestamp = time_stamp;
time_stamp = ntohl(time_stamp);
time_stamp += PACKET_IPG;
header1->data_block_continuity = total_samples;
total_samples += SAMPLES_PER_FRAME*CHANNELS;
sample =
(six1883_sample *) (((char *)tmp_packet->vaddr) +
(18 + sizeof(seventeen22_header) +
sizeof(six1883_header)));
for (i = 0; i < SAMPLES_PER_FRAME * CHANNELS; ++i) {
uint32_t tmp = htonl(sample_buffer[i]);
sample[i].label = 0x40;
memcpy(&(sample[i].value), &(tmp),
sizeof(sample[i].value));
}
err = igb_xmit(&igb_dev, 0, tmp_packet);
if (!err) {
continue;
}
if (ENOSPC == err) {
/* put back for now */
tmp_packet->next = free_packets;
free_packets = tmp_packet;
}
cleanup: igb_clean(&igb_dev, &cleaned_packets);
i = 0;
while (cleaned_packets) {
i++;
tmp_packet = cleaned_packets;
cleaned_packets = cleaned_packets->next;
tmp_packet->next = free_packets;
free_packets = tmp_packet;
}
}
rc = nice(0);
if (halt_tx == 0)
printf("listener left ...\n");
halt_tx = 1;
mrp_unadvertise_stream(STREAM_ID, DEST_ADDR, a_vid, PKT_SZ - 16,
PACKET_IPG / 125000, a_priority, 3900);
igb_set_class_bandwidth(&igb_dev, 0, 0, 0, 0); /* disable Qav */
rc = mrp_disconnect();
igb_dma_free_page(&igb_dev, &a_page);
err = igb_detach(&igb_dev);
pthread_exit(NULL);
return (0);
}
