| User | Revision | Line number | New contents of line |
|---|
| sblair |
0:230c10b228ea
|
1
|
#include "svPacketData.h"
|
| sblair |
0:230c10b228ea
|
2
|
#include "svEncode.h"
|
| sblair |
0:230c10b228ea
|
3
|
#include "gseEncode.h"
|
| sblair |
0:230c10b228ea
|
4
|
#include "encodePacket.h"
|
| sblair |
0:230c10b228ea
|
5
|
|
| sblair |
0:230c10b228ea
|
6
|
|
| sblair |
0:230c10b228ea
|
7
|
int svASDULength(struct svData *svData) {
|
| sblair |
0:230c10b228ea
|
8
|
int len = 0;
|
| sblair |
0:230c10b228ea
|
9
|
|
| sblair |
0:230c10b228ea
|
10
|
len += strlen((const char *) svData->ASDU[0].svID) + 2;
|
| sblair |
0:230c10b228ea
|
11
|
//printf("%i, %s\n", strlen(svData->ASDU[0].svID), svData->ASDU[0].svID);
|
| sblair |
0:230c10b228ea
|
12
|
//len += strlen((const char *) svData->ASDU[0].datset) + 2;
|
| sblair |
0:230c10b228ea
|
13
|
len += BER_GET_LENGTH_CTYPE_INT16U(&svData->ASDU[0].smpCnt) + 2;
|
| sblair |
0:230c10b228ea
|
14
|
len += BER_GET_LENGTH_CTYPE_INT32U(&svData->ASDU[0].confRev) + 2;
|
| sblair |
0:230c10b228ea
|
15
|
len += SV_GET_LENGTH_BOOLEAN + 2;
|
| sblair |
0:230c10b228ea
|
16
|
//len += BER_GET_LENGTH_CTYPE_INT16U(&svData->ASDU[0].smpRate) + 2;
|
| sblair |
0:230c10b228ea
|
17
|
len += svData->ASDU[0].data.size + getLengthBytes(svData->ASDU[0].data.size);
|
| sblair |
0:230c10b228ea
|
18
|
len++;
|
| sblair |
0:230c10b228ea
|
19
|
|
| sblair |
0:230c10b228ea
|
20
|
return len;
|
| sblair |
0:230c10b228ea
|
21
|
}
|
| sblair |
0:230c10b228ea
|
22
|
|
| sblair |
0:230c10b228ea
|
23
|
int svSeqLength(struct svData *svData) {
|
| sblair |
0:230c10b228ea
|
24
|
int len = svASDULength(svData);
|
| sblair |
0:230c10b228ea
|
25
|
len += getLengthBytes(len);
|
| sblair |
0:230c10b228ea
|
26
|
len++;
|
| sblair |
0:230c10b228ea
|
27
|
len = len * svData->noASDU; // assume all ASDUs are the same size
|
| sblair |
0:230c10b228ea
|
28
|
|
| sblair |
0:230c10b228ea
|
29
|
return len;
|
| sblair |
0:230c10b228ea
|
30
|
}
|
| sblair |
0:230c10b228ea
|
31
|
|
| sblair |
0:230c10b228ea
|
32
|
int svAPDULength(struct svData *svData) {
|
| sblair |
0:230c10b228ea
|
33
|
int len = svSeqLength(svData);
|
| sblair |
0:230c10b228ea
|
34
|
len += getLengthBytes(len);
|
| sblair |
0:230c10b228ea
|
35
|
len++;
|
| sblair |
0:230c10b228ea
|
36
|
|
| sblair |
0:230c10b228ea
|
37
|
len += 3;
|
| sblair |
0:230c10b228ea
|
38
|
|
| sblair |
0:230c10b228ea
|
39
|
return len;
|
| sblair |
0:230c10b228ea
|
40
|
}
|
| sblair |
0:230c10b228ea
|
41
|
|
| sblair |
0:230c10b228ea
|
42
|
|
| sblair |
0:230c10b228ea
|
43
|
//TODO: convert to proper BER sizes
|
| sblair |
0:230c10b228ea
|
44
|
// creates an SV packet, including frame header. returns 0 on fail; number of bytes on success
|
| sblair |
0:230c10b228ea
|
45
|
int svEncodePacket(struct svData *svData, unsigned char *buf) {
|
| sblair |
0:230c10b228ea
|
46
|
int offset = 0;
|
| sblair |
0:230c10b228ea
|
47
|
int len = svAPDULength(svData);
|
| sblair |
0:230c10b228ea
|
48
|
len += getLengthBytes(len);
|
| sblair |
0:230c10b228ea
|
49
|
len += 9; // savPdu tag size (1 byte), plus 8 "header" bytes
|
| sblair |
0:230c10b228ea
|
50
|
|
| sblair |
0:230c10b228ea
|
51
|
// frame header
|
| sblair |
0:230c10b228ea
|
52
|
memcpy(&buf[offset], svData->ethHeaderData.destMACAddress, 6); // destination MAC addresses
|
| sblair |
0:230c10b228ea
|
53
|
offset += 6;
|
| sblair |
0:230c10b228ea
|
54
|
memcpy(&buf[offset], LOCAL_MAC_ADDRESS, 6); // source MAC addresses
|
| sblair |
0:230c10b228ea
|
55
|
offset += 6;
|
| sblair |
0:230c10b228ea
|
56
|
|
| sblair |
0:230c10b228ea
|
57
|
buf[offset++] = 0x81; // TPID
|
| sblair |
0:230c10b228ea
|
58
|
buf[offset++] = 0x00;
|
| sblair |
0:230c10b228ea
|
59
|
|
| sblair |
0:230c10b228ea
|
60
|
netmemcpy(&buf[offset], &svData->ethHeaderData.VLAN_ID, 2); // TCI
|
| sblair |
0:230c10b228ea
|
61
|
buf[offset] |= (svData->ethHeaderData.VLAN_PRIORITY << 5);
|
| sblair |
0:230c10b228ea
|
62
|
offset += 2;
|
| sblair |
0:230c10b228ea
|
63
|
|
| sblair |
0:230c10b228ea
|
64
|
buf[offset++] = 0x88; // EtherType
|
| sblair |
0:230c10b228ea
|
65
|
buf[offset++] = 0xBA;
|
| sblair |
0:230c10b228ea
|
66
|
|
| sblair |
0:230c10b228ea
|
67
|
netmemcpy(&buf[offset], &svData->ethHeaderData.APPID, 2); // APPID
|
| sblair |
0:230c10b228ea
|
68
|
offset += 2;
|
| sblair |
0:230c10b228ea
|
69
|
|
| sblair |
0:230c10b228ea
|
70
|
netmemcpy(&buf[offset], &len, 2); // length
|
| sblair |
0:230c10b228ea
|
71
|
offset += 2;
|
| sblair |
0:230c10b228ea
|
72
|
|
| sblair |
0:230c10b228ea
|
73
|
buf[offset++] = 0x00; // reserved 1
|
| sblair |
0:230c10b228ea
|
74
|
buf[offset++] = 0x00;
|
| sblair |
0:230c10b228ea
|
75
|
buf[offset++] = 0x00; // reserved 2
|
| sblair |
0:230c10b228ea
|
76
|
buf[offset++] = 0x00;
|
| sblair |
0:230c10b228ea
|
77
|
|
| sblair |
0:230c10b228ea
|
78
|
buf[offset++] = 0x60;
|
| sblair |
0:230c10b228ea
|
79
|
offset += encodeLength(&buf[offset], svAPDULength(svData));
|
| sblair |
0:230c10b228ea
|
80
|
|
| sblair |
0:230c10b228ea
|
81
|
//TODO noASDU may be > 126?
|
| sblair |
0:230c10b228ea
|
82
|
buf[offset++] = 0x80;
|
| sblair |
0:230c10b228ea
|
83
|
buf[offset++] = 1;
|
| sblair |
0:230c10b228ea
|
84
|
buf[offset++] = (unsigned char) svData->noASDU;
|
| sblair |
0:230c10b228ea
|
85
|
|
| sblair |
0:230c10b228ea
|
86
|
buf[offset++] = 0xA2;
|
| sblair |
0:230c10b228ea
|
87
|
offset += encodeLength(&buf[offset], svSeqLength(svData));
|
| sblair |
0:230c10b228ea
|
88
|
|
| sblair |
0:230c10b228ea
|
89
|
int i = 0;
|
| sblair |
0:230c10b228ea
|
90
|
int size = 0;
|
| sblair |
0:230c10b228ea
|
91
|
for (i = 0; i < svData->noASDU; i++) {
|
| sblair |
0:230c10b228ea
|
92
|
buf[offset++] = 0x30;
|
| sblair |
0:230c10b228ea
|
93
|
offset += encodeLength(&buf[offset], svASDULength(svData));
|
| sblair |
0:230c10b228ea
|
94
|
|
| sblair |
0:230c10b228ea
|
95
|
size = strlen((const char *) svData->ASDU[i].svID);
|
| sblair |
0:230c10b228ea
|
96
|
buf[offset++] = 0x80;
|
| sblair |
0:230c10b228ea
|
97
|
buf[offset++] = size;
|
| sblair |
0:230c10b228ea
|
98
|
memcpy(&buf[offset], svData->ASDU[i].svID, size);
|
| sblair |
0:230c10b228ea
|
99
|
offset += size;
|
| sblair |
0:230c10b228ea
|
100
|
|
| sblair |
0:230c10b228ea
|
101
|
/*size = strlen(svData->ASDU[i].datset);
|
| sblair |
0:230c10b228ea
|
102
|
buf[offset++] = 0x81;
|
| sblair |
0:230c10b228ea
|
103
|
buf[offset++] = size;
|
| sblair |
0:230c10b228ea
|
104
|
memcpy(&buf[offset], svData->ASDU[i].datset, size);
|
| sblair |
0:230c10b228ea
|
105
|
offset += size;*/
|
| sblair |
0:230c10b228ea
|
106
|
|
| sblair |
0:230c10b228ea
|
107
|
buf[offset++] = 0x82;
|
| sblair |
0:230c10b228ea
|
108
|
offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT16U(&svData->ASDU[i].smpCnt));
|
| sblair |
0:230c10b228ea
|
109
|
offset += ber_encode_integer(&buf[offset], &svData->ASDU[i].smpCnt, SV_GET_LENGTH_INT16U);
|
| sblair |
0:230c10b228ea
|
110
|
//buf[offset++] = BER_GET_LENGTH_CTYPE_INT16U(&svData->ASDU[i].smpCnt);
|
| sblair |
0:230c10b228ea
|
111
|
//offset += BER_ENCODE_CTYPE_INT16U(&buf[offset], &svData->ASDU[i].smpCnt);
|
| sblair |
0:230c10b228ea
|
112
|
|
| sblair |
0:230c10b228ea
|
113
|
buf[offset++] = 0x83;
|
| sblair |
0:230c10b228ea
|
114
|
offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&svData->ASDU[i].confRev));
|
| sblair |
0:230c10b228ea
|
115
|
offset += ber_encode_integer(&buf[offset], &svData->ASDU[i].confRev, SV_GET_LENGTH_INT32U);
|
| sblair |
0:230c10b228ea
|
116
|
//buf[offset++] = BER_GET_LENGTH_CTYPE_INT32U(&svData->ASDU[i].confRev);
|
| sblair |
0:230c10b228ea
|
117
|
//offset += BER_ENCODE_CTYPE_INT32U(&buf[offset], &svData->ASDU[i].confRev);
|
| sblair |
0:230c10b228ea
|
118
|
|
| sblair |
0:230c10b228ea
|
119
|
buf[offset++] = 0x85;
|
| sblair |
0:230c10b228ea
|
120
|
buf[offset++] = SV_GET_LENGTH_BOOLEAN;
|
| sblair |
0:230c10b228ea
|
121
|
offset += ENCODE_CTYPE_BOOLEAN(&buf[offset], &svData->ASDU[i].smpSynch);
|
| sblair |
0:230c10b228ea
|
122
|
|
| sblair |
0:230c10b228ea
|
123
|
/*buf[offset++] = 0x86;
|
| sblair |
0:230c10b228ea
|
124
|
buf[offset++] = SV_GET_LENGTH_INT16U;
|
| sblair |
0:230c10b228ea
|
125
|
offset += ENCODE_CTYPE_INT16U(&buf[offset], &svData->ASDU[i].smpRate);*/
|
| sblair |
0:230c10b228ea
|
126
|
|
| sblair |
0:230c10b228ea
|
127
|
buf[offset++] = 0x87;
|
| sblair |
0:230c10b228ea
|
128
|
offset += encodeLength(&buf[offset], svData->ASDU[i].data.size);
|
| sblair |
0:230c10b228ea
|
129
|
memcpy(&buf[offset], svData->ASDU[i].data.data, svData->ASDU[i].data.size);
|
| sblair |
0:230c10b228ea
|
130
|
offset += svData->ASDU[i].data.size;
|
| sblair |
0:230c10b228ea
|
131
|
}
|
| sblair |
0:230c10b228ea
|
132
|
|
| sblair |
0:230c10b228ea
|
133
|
// assume network interface, such as WinPcap, generates CRC bytes
|
| sblair |
0:230c10b228ea
|
134
|
|
| sblair |
0:230c10b228ea
|
135
|
return offset;
|
| sblair |
0:230c10b228ea
|
136
|
}
|