/*
 * c37_94.c
 *
 *  Created on: 16-07-2019
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

#include "tdefs.h"
#include "misc.h"
#include "config.h"

#include "c37_94.h"
#include "helper.h"
#include "mkstream.h"

#include <ti/ipc/Notify.h>

#include "comm.h"
#include <ti/sysbios/hal/Hwi.h>

volatile struct stream_frame c37_str[C37_MAX_STREAMS][C37_STREAM_BUF_SIZE] __attribute__((aligned(4)));
volatile u8 c37_str_bits[C37_MAX_STREAMS][C37_STREAM_BUF_SIZE] __attribute__((aligned(4)));
volatile u32 c37_str_pos[C37_MAX_STREAMS] __attribute__((aligned(4)));

int c37_94_initlog(void *arguments, void *logic)
{
	struct c37_94_args *args = (struct c37_94_args *)arguments;
	struct c37_94_logic *log = (struct c37_94_logic *)logic;

	if(set_pointer_in_ptr(args->io.stream_ptr_in,(u32 *)&log->stream_in))
		return -1;

	if(set_bit_ptr_struct(args->io.stream_en_in,&log->stream_en))
		return -1;

	if(set_bit_ptr_struct(args->io.no_sync_in,&log->no_sync))
		return -1;

	if(set_bit_ptr_struct(args->io.clr_in,&log->clr))
		return -1;

	if(set_bit_ptr_struct(args->io.ping_in,&log->ping))
		return -1;

	if(set_pointer_out_ptr(args->io.stream0_ptr_out,(u32 *)&log->stream_out[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream0_act_out,&log->stream_act[0]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream1_ptr_out,(u32 *)&log->stream_out[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream1_act_out,&log->stream_act[1]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream2_ptr_out,(u32 *)&log->stream_out[2]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream2_act_out,&log->stream_act[2]))
		return -1;

	if(set_bit_ptr_struct(args->io.err_out,&log->err))
		return -1;

	if(set_bit_ptr_struct(args->io.y_bit_out,&log->y_bit))
		return -1;

	if(set_bit_ptr_struct(args->io.err2_out,&log->err2))
		return -1;

	if(set_bit_ptr_struct(args->io.y2_bit_out,&log->y_bit2))
		return -1;

	if(set_float_ptr(args->io.stream0_errs_float_out,&log->stream_errs[0]))
		return -1;
	if(set_float_ptr(args->io.stream1_errs_float_out,&log->stream_errs[1]))
		return -1;
	if(set_float_ptr(args->io.stream2_errs_float_out,&log->stream_errs[2]))
		return -1;

	if(set_bit_ptr_struct(args->io.link0_out,&log->link[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.link1_out,&log->link[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.link2_out,&log->link[2]))
		return -1;

	if(set_float_ptr(args->io.cur_shift_float_out,&log->cur_shift))
		return -1;

	if(set_float_ptr(args->io.cur_shift2_float_out,&log->cur_shift2))
		return -1;

	if(set_float_ptr(args->io.stream0_synt_errs_float_out,&log->stream_synt_errs[0]))
		return -1;
	if(set_float_ptr(args->io.stream1_synt_errs_float_out,&log->stream_synt_errs[1]))
		return -1;
	if(set_float_ptr(args->io.stream2_synt_errs_float_out,&log->stream_synt_errs[2]))
		return -1;

	if(set_float_ptr(args->io.ping_float_out,&log->ping_time))
		return -1;

	log->wr_bits=((args->params.bits & 0x00000001)?MUX_REG_EXT_CLK:0)|((args->params.mux_chans<=12?args->params.mux_chans:12)<<MUX_REG_CHAN_NUM_OFFSET);
	ic->mux_reg=log->wr_bits|MUX_REG_SKIP_FF;
	log->prev_addr=ic->mux_state & MUX_STATE_RX_BYTES_MASK;

	//
	log->s_delay=args->params.sample_delay/LOOP_CYCLE_MS;
	if(args->params.sample_delay%LOOP_CYCLE_MS)
		log->s_delay++;

	log->s_delay*=(SAMPLES_PER_MS*LOOP_CYCLE_MS);

	if(log->s_delay>=((C37_STREAM_BUF_SIZE-1)*(SAMPLES_PER_MS*LOOP_CYCLE_MS)))
		log->s_delay=(C37_STREAM_BUF_SIZE-1)*(SAMPLES_PER_MS*LOOP_CYCLE_MS);
	//

	log->wr_addr=1;

	ic->mux_shift=0;

	// mux2
	log->wr_bits2=((args->params.bits & MUX2_EXT_CLK)?MUX_REG_EXT_CLK:0)|((args->params.mux_chans<=12?args->params.mux_chans:12)<<MUX_REG_CHAN_NUM_OFFSET);
	ic->mux2_reg=log->wr_bits2|MUX_REG_SKIP_FF;
	log->prev_addr2=ic->mux2_state & MUX_STATE_RX_BYTES_MASK;
	log->wr_addr2=1;
	ic->mux2_shift=0;

	log->my_mask = (args->params.serial_num<<2)&C37_SERIAL_MASK;
	return 0;
}


void c37_94(void *arguments, void *logic)
{
	struct c37_94_logic *log = (struct c37_94_logic *)logic;
	struct c37_94_args *args = (struct c37_94_args *)arguments;
	int i,rx_bytes;
	u8 j,k;
	u8 cksum;
	u16 find_stamp;
	u16 cur_addr;
	u16 mux_offset;
	u8 already_incremented[C37_MAX_STREAMS]={0,0,0};
	u8 need_pong=0;

	static struct stream_frame tmp_frame;


	if(check_struct(&log->clr))
	{
		for(i=0;i<C37_MAX_STREAMS;i++)
		{
			*log->stream_errs[i]=0;
			*log->stream_synt_errs[i]=0;
		}
	}

	for(i=0;i<C37_MAX_STREAMS;i++)
	{
		if(log->prev_pos[i]!=c37_str_pos[i])
		{
			if(log->link_up_cnt[i]>LINK_UP_TIME)
			{
				if(!check_struct(&log->link[i]))
					*log->stream_errs[i]=0;
				set_struct(&log->link[i]);
			}
			else
				log->link_up_cnt[i]+=LOOP_CYCLE_MS;

			while(log->prev_pos[i]!=c37_str_pos[i])
			{
				u16 prev_tstamp=(!log->prev_pos[i]?c37_str[i][C37_STREAM_BUF_SIZE-1].sample_stamp:c37_str[i][log->prev_pos[i]-1].sample_stamp) & 0x7FFF;
				u16 next_tstamp=(c37_str[i][log->prev_pos[i]].sample_stamp)&0x7FFF;

				if((prev_tstamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)!=next_tstamp)
				{
					if((log->prev_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)==bus_an_cur_timestamp)
						*log->stream_errs[i]+=1.0f;
					already_incremented[i]=1;
				}
				log->prev_pos[i]++;
				log->prev_pos[i]%=C37_STREAM_BUF_SIZE;
			}

			log->link_timeout_cnt[i]=0;
			log->prev_pos[i]=c37_str_pos[i];
		}
		else
		{
			if(log->link_timeout_cnt[i]>LINK_TIMEOUT_TIME)
			{
				clear_struct(&log->link[i]);
				log->link_up_cnt[i]=0;
			}
			else
			{
				if(log->link_timeout_cnt[i]>(1*LOOP_CYCLE_MS))
				{
					if((log->prev_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)==bus_an_cur_timestamp)
						*log->stream_errs[i]+=1.0f;
					already_incremented[i]=1;
				}
				log->link_timeout_cnt[i]+=LOOP_CYCLE_MS;
			}
		}
	}


	check_and_set_struct(ic->mux_state & MUX_STATE_ERR,&log->err);
	check_and_set_struct(ic->mux2_state & MUX_STATE_ERR,&log->err2);

	cur_addr = ic->mux_state & MUX_STATE_RX_BYTES_MASK;
	rx_bytes = cur_addr>=log->prev_addr ? cur_addr-log->prev_addr : C37_MUX_BUFSIZE + cur_addr - log->prev_addr;
	mux_offset = log->prev_addr;
/*
	if(check_struct(&log->shift_out))
	{
		log->out_shift_cnt++;
		log->out_shift_cnt&=0x07;
		ic->mux_shift=(ic->mux_shift & 0x0F) | (log->out_shift_cnt<<4);
	}
*/

// MUX1
	if(!(ic->mux_state & MUX_STATE_ERR))
	{
		log->no_data_cnt+=LOOP_CYCLE_MS;

		if(log->no_data_cnt > (C37_NO_DATA_SHIFT_INTERVAL))
		{
			log->no_data_cnt=0;
			log->shift_cnt++;
			log->shift_cnt&=0x07;
			ic->mux_shift=(ic->mux_shift & 0xF0)|log->shift_cnt;
			*log->cur_shift=log->shift_cnt;
		}
	}
	else
		log->no_data_cnt=0;

	while(rx_bytes >= C37_FRAME_SIZE)
	{
		if(ic->mux_indata[mux_offset]!='T' || ic->mux_indata[(mux_offset+1)%C37_MUX_BUFSIZE]!='Z')
		{
			rx_bytes--;
			mux_offset++;
			mux_offset%=C37_MUX_BUFSIZE;
			continue;
		}

		if(mux_offset + C37_FRAME_SIZE > C37_MUX_BUFSIZE)
		{
			memcpy((u8*)&tmp_frame,(u8*)&ic->mux_indata[mux_offset],C37_MUX_BUFSIZE - mux_offset);
			memcpy((u8*)&tmp_frame+(C37_MUX_BUFSIZE-mux_offset),(u8*)&ic->mux_indata[0],C37_FRAME_SIZE-(C37_MUX_BUFSIZE-mux_offset));
		}
		else
		{
			memcpy((u8*)&tmp_frame,(u8*)&ic->mux_indata[mux_offset],C37_FRAME_SIZE);
		}

		cksum=CKSUM_IV;
		for(j=0;j<C37_FRAME_SIZE-1;j++)
			cksum+=*((u8*)&tmp_frame+j);

		if(cksum == tmp_frame.cksum && ((tmp_frame.addr & C37_SERIAL_MASK) == log->my_mask))
		{
			if(tmp_frame.addr & 0x80)
				need_pong=1;
			if(tmp_frame.addr & 0x40 && log->ping_sent)
			{
				*log->ping_time=log->ping_cnt;
				log->ping_sent=0;
			}
			tmp_frame.addr&=0x03;
			log->no_data_cnt=0;

			if(tmp_frame.addr <C37_MAX_STREAMS)
			{
				memcpy((u8 *)&c37_str[tmp_frame.addr][c37_str_pos[tmp_frame.addr]],(u8*)&tmp_frame,C37_FRAME_SIZE);
				c37_str_bits[tmp_frame.addr][c37_str_pos[tmp_frame.addr]++]=0;
				c37_str_pos[tmp_frame.addr]%=C37_STREAM_BUF_SIZE;
			}
		}
//		else
//			*log->cksum_err+=1.0;
		//else odjac tylko 2 bajty TZ i szukac kolejnego?

		rx_bytes-=C37_FRAME_SIZE;
		mux_offset+=C37_FRAME_SIZE;
		mux_offset%=C37_MUX_BUFSIZE;
	}

	log->prev_addr=mux_offset;

//	*log->hw_err=(float)((ic->mux_state & MUX_STATE_ERR_CNT_MASK)>>MUX_STATE_ERR_CNT_OFFSET);
	check_and_set_struct(ic->mux_state & MUX_STATE_YELLOW_BIT_IN, &log->y_bit);

//MUX2
	if(args->params.bits & MUX2_ACTIVE)
	{
		cur_addr = ic->mux2_state & MUX_STATE_RX_BYTES_MASK;
		rx_bytes = cur_addr>=log->prev_addr2 ? cur_addr-log->prev_addr2 : C37_MUX_BUFSIZE + cur_addr - log->prev_addr2;
		mux_offset = log->prev_addr2;

		if(!(ic->mux2_state & MUX_STATE_ERR))
		{
			log->no_data_cnt2+=LOOP_CYCLE_MS;

			if(log->no_data_cnt2 > (C37_NO_DATA_SHIFT_INTERVAL))
			{
				log->no_data_cnt2=0;
				log->shift_cnt2++;
				log->shift_cnt2&=0x07;
				ic->mux2_shift=(ic->mux2_shift & 0xF0)|log->shift_cnt2;
				*log->cur_shift2=log->shift_cnt2;
			}
		}
		else
			log->no_data_cnt2=0;

		while(rx_bytes >= C37_FRAME_SIZE)
		{
			if(ic->mux2_indata[mux_offset]!='T' || ic->mux2_indata[(mux_offset+1)%C37_MUX_BUFSIZE]!='Z')
			{
				rx_bytes--;
				mux_offset++;
				mux_offset%=C37_MUX_BUFSIZE;
				continue;
			}

			if(mux_offset + C37_FRAME_SIZE > C37_MUX_BUFSIZE)
			{
				memcpy((u8*)&tmp_frame,(u8*)&ic->mux2_indata[mux_offset],C37_MUX_BUFSIZE - mux_offset);
				memcpy((u8*)&tmp_frame+(C37_MUX_BUFSIZE-mux_offset),(u8*)&ic->mux2_indata[0],C37_FRAME_SIZE-(C37_MUX_BUFSIZE-mux_offset));
			}
			else
			{
				memcpy((u8*)&tmp_frame,(u8*)&ic->mux2_indata[mux_offset],C37_FRAME_SIZE);
			}

			cksum=CKSUM_IV;
			for(j=0;j<C37_FRAME_SIZE-1;j++)
				cksum+=*((u8*)&tmp_frame+j);

			if(cksum == tmp_frame.cksum && ((tmp_frame.addr & C37_SERIAL_MASK) == log->my_mask))
			{
				if(tmp_frame.addr & 0x80)
					need_pong=1;
				if(tmp_frame.addr & 0x40 && log->ping_sent)
				{
					*log->ping_time=log->ping_cnt;
					log->ping_sent=0;
				}
				tmp_frame.addr&=0x03;
				log->no_data_cnt2=0;

				if(tmp_frame.addr <C37_MAX_STREAMS)
				{
					memcpy((u8 *)&c37_str[tmp_frame.addr][c37_str_pos[tmp_frame.addr]],(u8*)&tmp_frame,C37_FRAME_SIZE);
					c37_str_bits[tmp_frame.addr][c37_str_pos[tmp_frame.addr]++]=0;
					c37_str_pos[tmp_frame.addr]%=C37_STREAM_BUF_SIZE;
				}
			}
	//		else
	//			*log->cksum_err+=1.0;
			//else odjac tylko 2 bajty TZ i szukac kolejnego?

			rx_bytes-=C37_FRAME_SIZE;
			mux_offset+=C37_FRAME_SIZE;
			mux_offset%=C37_MUX_BUFSIZE;
		}

		log->prev_addr2=mux_offset;

	//	*log->hw_err=(float)((ic->mux_state & MUX_STATE_ERR_CNT_MASK)>>MUX_STATE_ERR_CNT_OFFSET);
		check_and_set_struct(ic->mux2_state & MUX_STATE_YELLOW_BIT_IN, &log->y_bit2);
	}
	//

	if(bus_an_cur_timestamp>=log->s_delay)
		find_stamp=bus_an_cur_timestamp-log->s_delay;
	else
		find_stamp=(SAMPLES_PER_MS*1000)+bus_an_cur_timestamp-log->s_delay;


	for(i=0;i<C37_MAX_STREAMS;i++)
	{
		if(!check_struct(&log->no_sync))
		{
			if(!check_struct(&log->link[i]))
			{
				clear_struct(&log->stream_act[i]);
				continue;
			}

			for(k=0;k<C37_STREAM_BUF_SIZE;k++)
			{
				if(c37_str[i][k].sample_stamp == find_stamp && !(c37_str_bits[i][k] & STATUS_FRAME_READ))
				{
					c37_str_bits[i][k]|=STATUS_FRAME_READ;
					log->stream_out[i] = c37_str[i][k];
//					*log->stream_snum[i] = c37_str[i][k].sample_stamp;
					break;
				}
			}

			if(k<C37_STREAM_BUF_SIZE)
				set_struct(&log->stream_act[i]);
			else
			{
				if(!already_incremented[i])
					*log->stream_synt_errs[i]+=1.0f;
					//*log->stream_errs[i]+=1.0f;
				clear_struct(&log->stream_act[i]);
			}
		}
		else
		{
			if(args->params.bits & 0x00000002) // no buf
			{
				if(!(c37_str_bits[i][(c37_str_pos[i]+C37_STREAM_BUF_SIZE-1) % C37_STREAM_BUF_SIZE] & STATUS_FRAME_READ))
				{
					c37_str_bits[i][(c37_str_pos[i]+C37_STREAM_BUF_SIZE-1) % C37_STREAM_BUF_SIZE]|=STATUS_FRAME_READ;
					log->stream_out[i] = c37_str[i][(c37_str_pos[i]+C37_STREAM_BUF_SIZE-1) % C37_STREAM_BUF_SIZE];
//					*log->stream_snum[i] = c37_str[i][(c37_str_pos[i]+C37_STREAM_BUF_SIZE-1) % C37_STREAM_BUF_SIZE].sample_stamp;
					break;
				}
			}
			else
			{
				for(k=0;k<C37_STREAM_BUF_SIZE;k++)
				{

					if(!(c37_str_bits[i][(c37_str_pos[i]+k) % C37_STREAM_BUF_SIZE] & STATUS_FRAME_READ))
					{
						c37_str_bits[i][(c37_str_pos[i]+k) % C37_STREAM_BUF_SIZE]|=STATUS_FRAME_READ;
						log->stream_out[i] = c37_str[i][(c37_str_pos[i]+k) % C37_STREAM_BUF_SIZE];
//						*log->stream_snum[i] = c37_str[i][(c37_str_pos[i]+k) % C37_STREAM_BUF_SIZE].sample_stamp;
						break;
					}
				}

				if(k<C37_STREAM_BUF_SIZE)
					set_struct(&log->stream_act[i]);
				else
				{
					if(!already_incremented[i])
						*log->stream_synt_errs[i]+=1.0f;
						//*log->stream_errs[i]+=1.0f;
					clear_struct(&log->stream_act[i]);
				}
			}

		}

	}

	//
	log->prev_timestamp=bus_an_cur_timestamp;
	if(log->stream_in->addr<C37_MAX_STREAMS)
	{
		memcpy((u8 *)&c37_str[log->stream_in->addr][c37_str_pos[log->stream_in->addr]],(u8*)log->stream_in,C37_FRAME_SIZE);
		c37_str_bits[log->stream_in->addr][c37_str_pos[log->stream_in->addr]++]=0;
		c37_str_pos[log->stream_in->addr]%=C37_STREAM_BUF_SIZE;

		if(check_struct(&log->stream_en))
		{
			//
			log->stream_in->addr|=log->my_mask;
			log->stream_in->cksum+=log->my_mask;

			if(check_struct(&log->ping) && log->ping_cnt==0)
			{
				log->stream_in->addr|=0x80; // ping req
				log->stream_in->cksum+=0x80;
				log->ping_sent=1;
			}
			if(need_pong)
			{
				log->stream_in->addr|=0x40;
				log->stream_in->cksum+=0x40;
			}

			if(log->wr_addr + C37_FRAME_SIZE > C37_MUX_BUFSIZE)
			{
				memcpy((u8*)&ic->mux_outdata[log->wr_addr],(u8 *)log->stream_in,C37_MUX_BUFSIZE - log->wr_addr);
				memcpy((u8*)ic->mux_outdata,(u8 *)log->stream_in+(C37_MUX_BUFSIZE - log->wr_addr),C37_FRAME_SIZE - (C37_MUX_BUFSIZE - log->wr_addr));
			}
			else
				memcpy((u8*)&ic->mux_outdata[log->wr_addr],(u8 *)log->stream_in,C37_FRAME_SIZE);

			log->wr_addr+=C37_FRAME_SIZE-1;
			log->wr_addr%=C37_MUX_BUFSIZE;
			ic->mux_reg=log->wr_bits|MUX_REG_WR|log->wr_addr;
			log->wr_addr++;
			log->wr_addr%=C37_MUX_BUFSIZE;
			//
			//mux2
			if(args->params.bits & MUX2_ACTIVE)
			{
				if(log->wr_addr2 + C37_FRAME_SIZE > C37_MUX_BUFSIZE)
				{
					memcpy((u8*)&ic->mux2_outdata[log->wr_addr2],(u8 *)log->stream_in,C37_MUX_BUFSIZE - log->wr_addr2);
					memcpy((u8*)ic->mux2_outdata,(u8 *)log->stream_in+(C37_MUX_BUFSIZE - log->wr_addr2),C37_FRAME_SIZE - (C37_MUX_BUFSIZE - log->wr_addr2));
				}
				else
					memcpy((u8*)&ic->mux2_outdata[log->wr_addr2],(u8 *)log->stream_in,C37_FRAME_SIZE);

				log->wr_addr2+=C37_FRAME_SIZE-1;
				log->wr_addr2%=C37_MUX_BUFSIZE;
				ic->mux2_reg=log->wr_bits2|MUX_REG_WR|log->wr_addr2;
				log->wr_addr2++;
				log->wr_addr2%=C37_MUX_BUFSIZE;
			}
		}
		else
		{
			if(check_struct(&log->ping) && log->ping_cnt==0)
			{
				memcpy((u8 *)&c37_str[log->stream_in->addr][c37_str_pos[log->stream_in->addr]],(u8*)log->stream_in,C37_FRAME_SIZE);
				c37_str_bits[log->stream_in->addr][c37_str_pos[log->stream_in->addr]++]=0;
				c37_str_pos[log->stream_in->addr]%=C37_STREAM_BUF_SIZE;

				log->ping_sent=1;
				log->stream_in->addr|=0x80; // ping req
				log->stream_in->cksum+=0x80;
				log->stream_in->addr|=log->my_mask;
				log->stream_in->cksum+=log->my_mask;

				if(need_pong)
				{
					log->stream_in->addr|=0x40;
					log->stream_in->cksum+=0x40;
				}

				//
				if(log->wr_addr + C37_FRAME_SIZE > C37_MUX_BUFSIZE)
				{
					memcpy((u8*)&ic->mux_outdata[log->wr_addr],(u8 *)log->stream_in,C37_MUX_BUFSIZE - log->wr_addr);
					memcpy((u8*)ic->mux_outdata,(u8 *)log->stream_in+(C37_MUX_BUFSIZE - log->wr_addr),C37_FRAME_SIZE - (C37_MUX_BUFSIZE - log->wr_addr));
				}
				else
					memcpy((u8*)&ic->mux_outdata[log->wr_addr],(u8 *)log->stream_in,C37_FRAME_SIZE);

				log->wr_addr+=C37_FRAME_SIZE-1;
				log->wr_addr%=C37_MUX_BUFSIZE;
				ic->mux_reg=log->wr_bits|MUX_REG_WR|log->wr_addr;
				log->wr_addr++;
				log->wr_addr%=C37_MUX_BUFSIZE;
				//
				//
				if(args->params.bits & MUX2_ACTIVE)
				{
					if(log->wr_addr + C37_FRAME_SIZE > C37_MUX_BUFSIZE)
					{
						memcpy((u8*)&ic->mux2_outdata[log->wr_addr2],(u8 *)log->stream_in,C37_MUX_BUFSIZE - log->wr_addr2);
						memcpy((u8*)ic->mux2_outdata,(u8 *)log->stream_in+(C37_MUX_BUFSIZE - log->wr_addr2),C37_FRAME_SIZE - (C37_MUX_BUFSIZE - log->wr_addr2));
					}
					else
						memcpy((u8*)&ic->mux2_outdata[log->wr_addr2],(u8 *)log->stream_in,C37_FRAME_SIZE);

					log->wr_addr2+=C37_FRAME_SIZE-1;
					log->wr_addr2%=C37_MUX_BUFSIZE;
					ic->mux2_reg=log->wr_bits2|MUX_REG_WR|log->wr_addr2;
					log->wr_addr2++;
					log->wr_addr2%=C37_MUX_BUFSIZE;
					//
				}
			}
		}
	}

	if(log->ping_cnt<5000)
		log->ping_cnt+=LOOP_CYCLE_MS;
	else
	{
		if(log->ping_sent)
		{
			*log->ping_time=999999.0f;
			log->ping_sent=0;
		}

		log->ping_cnt=0;
	}
}
