dodany kod c od Krzyska po wielu modyfikacjach przez Gemini zeby usunac zalezność od TI-RTOS
This commit is contained in:
281
src/goose_out.c
Normal file
281
src/goose_out.c
Normal file
@@ -0,0 +1,281 @@
|
||||
/*
|
||||
* 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
Block a user