ms/dist_tester
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
6af2ca712f6e95623eff27a373b41aac488f31c9
dist_tester/src/goose_out.c

282 lines
6.7 KiB
C
Raw Blame History

/*
* goose_out.c
*
* Created on: 08-02-2020
* Author: Krzysztof Jakubczyk
*/
#include <math.h>
#include "tdefs.h"
#include "misc.h"
#include "helper.h"
#include "eth.h"
#include "goose.h"
#include "goose_out.h"
#include <ti/ipc/Notify.h>
#include "comm.h"
#include "../ethernet/ports/am1808/include/lwiplib.h"
#include "../ethernet/emac.h"
#include "../ethernet/hw_types.h"
#include "ports/am1808/include/netif/sitaraif.h"
#include <ti/sysbios/hal/Hwi.h>
#include <string.h>
#include <stdio.h>
int goose_out_initlog(void *arguments, void *logic)
{
struct goose_out_args *args = (struct goose_out_args *)arguments;
struct goose_out_logic *log = (struct goose_out_logic *)logic;
u8 i;
int values[6];
u8 dstaddr[6];
struct goose_frame_normal *gfr = (struct goose_frame_normal *)log->goose_out_frame;
struct goose_frame_vlan *gfr_vlan = (struct goose_frame_vlan *)log->goose_out_frame;
struct goose_frame2 *g;
for(i=0;i<16;i++)
if(set_bit_ptr_struct(args->io.state_in[i],&log->state_in[i]))
return -1;
if(set_bit_ptr_struct(args->io.test_in,&log->test))
return -1;
if(set_bit_ptr_struct(args->io.ndscom_in,&log->ndscom_in))
return -1;
log->goose_id=(char*)&args->params.strings_start;
u8 len = strlen(log->goose_id) + 1;
if(len%4)
len+=4-(len%4);
log->goose_dataset=log->goose_id+len;
len = strlen(log->goose_dataset) + 1;
if(len%4)
len+=4-(len%4);
log->goose_cbref=log->goose_dataset+len;
len = strlen(log->goose_cbref) + 1;
if(len%4)
len+=4-(len%4);
log->goose_dstaddr=log->goose_cbref+len;
len = strlen(log->goose_dstaddr) + 1;
if(len%4)
len+=4-(len%4);
if( 6 == sscanf( log->goose_dstaddr, "%x-%x-%x-%x-%x-%x%*c",
&values[0], &values[1], &values[2],
&values[3], &values[4], &values[5] ) )
{
for( i = 0; i < 6; ++i )
dstaddr[i] = (u8) values[i];
}
else
return -1;
log->stnum_cur=1;
log->sqnum_cur=1;
// prepare frame headers
memset(log->goose_out_frame,0,sizeof(log->goose_out_frame));
memcpy(gfr->dst_mac,dstaddr,6);
memcpy(gfr->src_mac,eth.hwaddr,6);
if(args->params.bits & GOOSE_OUT_VLAN_EN)
{
gfr_vlan->ethertype=GOOSE_ETHERTYPE_LE;
gfr_vlan->tpid=0x0081; // 0x8100 LE coded
gfr_vlan->tci=((args->params.priority & 0x07)<<5)|((args->params.vlan_no & 0x0F00)>>8)|((args->params.vlan_no & 0x00FF)<<8);
g=(struct goose_frame2 *)&gfr_vlan->goose;
}
else
{
gfr->ethertype=GOOSE_ETHERTYPE_LE;
g=(struct goose_frame2 *)&gfr->goose;
}
g->appid=htons(args->params.appid);
g->pdu_tag=GOOSE_TAG_PDU;
g->pdu_taglen=0x81;
log->len=11;
u32 dcnt = 0;
g->pdu_data[dcnt++]=GOOSE_TAG_GOCBREF;
g->pdu_data[dcnt++]=strlen(log->goose_cbref);
memcpy(&g->pdu_data[dcnt],log->goose_cbref,strlen(log->goose_cbref));
dcnt+=strlen(log->goose_cbref);
g->pdu_data[dcnt++]=GOOSE_TAG_TTL;
g->pdu_data[dcnt++]=2;
log->ttl=&g->pdu_data[dcnt];
*(u16 *)&g->pdu_data[dcnt] = htons(args->params.ttl);
dcnt+=2;
g->pdu_data[dcnt++]=GOOSE_TAG_DATSET;
g->pdu_data[dcnt++]=strlen(log->goose_dataset);
memcpy(&g->pdu_data[dcnt],log->goose_dataset,strlen(log->goose_dataset));
dcnt+=strlen(log->goose_dataset);
g->pdu_data[dcnt++]=GOOSE_TAG_ID;
g->pdu_data[dcnt++]=strlen(log->goose_id);
memcpy(&g->pdu_data[dcnt],log->goose_id,strlen(log->goose_id));
dcnt+=strlen(log->goose_id);
g->pdu_data[dcnt++]=GOOSE_TAG_T;
g->pdu_data[dcnt++]=8;
log->tstamp_secs=&g->pdu_data[dcnt];
for(i=0;i<4;i++)
g->pdu_data[dcnt++]=(((timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_sec:cur_time.tv_sec)>>((3-i)*8));
log->tstamp_frac=&g->pdu_data[dcnt];
u32 stamp = ((timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_usec:cur_time.tv_usec)*GOOSE_TIMESTAMP_MS_MULTIPLIER;
for(i=0;i<4;i++)
g->pdu_data[dcnt++]=stamp>>((3-i)*8);
g->pdu_data[dcnt++]=GOOSE_TAG_STNUM;
g->pdu_data[dcnt++]=4;
log->stnum=&g->pdu_data[dcnt];
for(i=0;i<4;i++)
g->pdu_data[dcnt++]=(log->stnum_cur>>((3-i)*8));
g->pdu_data[dcnt++]=GOOSE_TAG_SQNUM;
g->pdu_data[dcnt++]=4;
log->sqnum=&g->pdu_data[dcnt];
for(i=0;i<4;i++)
g->pdu_data[dcnt++]=(log->sqnum_cur>>((3-i)*8));
g->pdu_data[dcnt++]=GOOSE_TAG_SIMULATION;
g->pdu_data[dcnt++]=1;
log->simulation=&g->pdu_data[dcnt];
g->pdu_data[dcnt++]=0;
g->pdu_data[dcnt++]=GOOSE_TAG_CONFREV;
g->pdu_data[dcnt++]=4;
for(i=0;i<4;i++)
g->pdu_data[dcnt++]=(args->params.cfg_rev>>((3-i)*8));
g->pdu_data[dcnt++]=GOOSE_TAG_NDSCOM;
g->pdu_data[dcnt++]=1;
log->ndscom=&g->pdu_data[dcnt];
g->pdu_data[dcnt++]=0;
g->pdu_data[dcnt++]=GOOSE_TAG_NUMDATSETENTRIES;
g->pdu_data[dcnt++]=1;
g->pdu_data[dcnt++]=16;
g->pdu_data[dcnt++]=GOOSE_TAG_ALLDATA;
g->pdu_data[dcnt++]=16*3; // 16 states * 3 bytes each
for(i=0;i<16;i++)
{
g->pdu_data[dcnt++]=GOOSE_DATA_BOOLEAN;
g->pdu_data[dcnt++]=1;
log->state[i]=&g->pdu_data[dcnt];
g->pdu_data[dcnt++]=0;
}
g->pdu_len+=dcnt;
log->len+=dcnt;
g->len=htons(log->len);
if(log->len>=(GOOSE_MAX_FR_OUT_SIZE-18))
return -1;
log->first_packet=1;
return 0;
}
void goose_out(void *arguments, void *logic)
{
struct goose_out_logic *log = (struct goose_out_logic *)logic;
struct goose_out_args *args = (struct goose_out_args *)arguments;
u8 i;
u32 stamp;
u8 st_changed;
st_changed=0;
for(i=0;i<16;i++)
{
*log->state[i]=check_struct(&log->state_in[i])?1:0;
if(*log->state[i]!=log->state_prev[i])
{
st_changed=1;
log->state_prev[i]=*log->state[i];
}
}
if(st_changed||log->first_packet)
{
log->retrans_cnt=0;
log->retrans_cycle=0;
stamp=((timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_sec:cur_time.tv_sec);
for(i=0;i<4;i++)
*(log->tstamp_secs+i)=(stamp>>((3-i)*8));
stamp = ((timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_usec:cur_time.tv_usec)*GOOSE_TIMESTAMP_MS_MULTIPLIER;
for(i=0;i<4;i++)
*(log->tstamp_frac+i)=stamp>>((3-i)*8);
}
if(!log->retrans_cnt)
{
*log->simulation=check_struct(&log->test)?1:0;
*log->ndscom=check_struct(&log->ndscom_in)?1:0;
if(st_changed && !log->first_packet)
{
log->stnum_cur++;
log->sqnum_cur=0;
}
for(i=0;i<4;i++)
*(log->stnum+i)=log->stnum_cur>>((3-i)*8);
for(i=0;i<4;i++)
*(log->sqnum+i)=log->sqnum_cur>>((3-i)*8);
log->sqnum_cur++;
log->retrans_cnt = (2<<log->retrans_cycle);
if(log->retrans_cnt>=(args->params.ttl>>1))
{
log->retrans_cnt=(args->params.ttl>>1);
*log->ttl = (args->params.ttl)>>8;
*(log->ttl+1) = (args->params.ttl);
}
else
{
*log->ttl = (log->retrans_cnt<<1)>>8;
*(log->ttl+1) = (log->retrans_cnt<<1);
log->retrans_cycle++;
}
if(args->params.bits & GOOSE_OUT_VLAN_EN)
omapl138EthSendPacket(log->goose_out_frame,(log->len+18)>64?(log->len+18):64);
else
omapl138EthSendPacket(log->goose_out_frame,(log->len+14)>64?(log->len+14):64);
log->first_packet=0;
}
if(log->retrans_cnt>=LOOP_CYCLE_MS)
log->retrans_cnt-=LOOP_CYCLE_MS;
else
log->retrans_cnt=0;
}
Reference in New Issue View Git Blame Copy Permalink