void
test_classify_packet_MPLSMC(void) {
struct port port;
struct lagopus_packet pkt;
OS_MBUF *m;
m = calloc(1, sizeof(*m));
TEST_ASSERT_NOT_EQUAL_MESSAGE(m, NULL, "calloc error.");
m->data = &m->dat[128];
OS_M_PKTLEN(m) = 128;
m->data[12] = 0x88;
m->data[13] = 0x48;
pkt.mbuf = (OS_MBUF *)m;
lagopus_set_in_port(&pkt, &port);
classify_packet(&pkt);
TEST_ASSERT_EQUAL_MESSAGE(pkt.ether_type, ETHERTYPE_MPLS_MCAST,
"ether_type error.");
/* with VLAN */
m->data[12] = 0x81;
m->data[13] = 0x00;
m->data[16] = 0x88;
m->data[17] = 0x48;
pkt.mbuf = (OS_MBUF *)m;
classify_packet(&pkt);
TEST_ASSERT_EQUAL_MESSAGE(pkt.ether_type, ETHERTYPE_MPLS_MCAST,
"ether_type(vlan) error.");
}
void
test_classify_packet_PBB(void) {
struct port port;
struct lagopus_packet pkt;
OS_MBUF *m;
m = calloc(1, sizeof(*m));
TEST_ASSERT_NOT_EQUAL_MESSAGE(m, NULL, "calloc error.");
m->data = &m->dat[128];
OS_M_PKTLEN(m) = 128;
#ifdef PBB_IS_VLAN
m->data[12] = 0x88;
m->data[13] = 0xe7;
m->data[30] = 0x08;
m->data[31] = 0x00;
pkt.mbuf = (OS_MBUF *)m;
lagopus_set_in_port(&pkt, &port);
classify_packet(&pkt);
TEST_ASSERT_EQUAL_MESSAGE(pkt.ether_type, ETHERTYPE_IP,
"ether_type error.");
/* with VLAN */
m->data[12] = 0x81;
m->data[13] = 0x00;
m->data[16] = 0x88;
m->data[17] = 0xe7;
m->data[34] = 0x08;
m->data[35] = 0x00;
pkt.mbuf = (OS_MBUF *)m;
classify_packet(&pkt);
TEST_ASSERT_EQUAL_MESSAGE(pkt.ether_type, ETHERTYPE_IP,
"ether_type(vlan) error.");
#else
m->data[12] = 0x88;
m->data[13] = 0xe7;
pkt.mbuf = (OS_MBUF *)m;
lagopus_set_in_port(&pkt, &port);
classify_packet(&pkt);
TEST_ASSERT_EQUAL_MESSAGE(pkt.ether_type, ETHERTYPE_PBB,
"ether_type error.");
/* with VLAN */
m->data[12] = 0x81;
m->data[13] = 0x00;
m->data[16] = 0x88;
m->data[17] = 0xe7;
pkt.mbuf = (OS_MBUF *)m;
classify_packet(&pkt);
TEST_ASSERT_EQUAL_MESSAGE(pkt.ether_type, ETHERTYPE_PBB,
"ether_type(vlan) error.");
#endif
}
void
test_classify_packet_IPV6_other(void) {
struct port port;
struct lagopus_packet pkt;
OS_MBUF *m;
m = calloc(1, sizeof(*m));
TEST_ASSERT_NOT_EQUAL_MESSAGE(m, NULL, "calloc error.");
m->data = &m->dat[128];
OS_M_PKTLEN(m) = 64;
m->data[12] = 0x86;
m->data[13] = 0xdd;
m->data[20] = IPPROTO_RSVP;
pkt.mbuf = (OS_MBUF *)m;
lagopus_set_in_port(&pkt, &port);
classify_packet(&pkt);
TEST_ASSERT_EQUAL_MESSAGE(pkt.ether_type, ETHERTYPE_IPV6,
"ether_type error.");
/* with exthdr */
m->data[20] = IPPROTO_DSTOPTS;
m->data[54] = IPPROTO_RSVP;
m->data[55] = 0;
classify_packet(&pkt);
TEST_ASSERT_EQUAL_MESSAGE(pkt.ether_type, ETHERTYPE_IPV6,
"ether_type(exthdr) error.");
/* with VLAN */
m->data[12] = 0x81;
m->data[13] = 0x00;
m->data[16] = 0x86;
m->data[17] = 0xdd;
m->data[24] = IPPROTO_RSVP;
classify_packet(&pkt);
TEST_ASSERT_EQUAL_MESSAGE(pkt.ether_type, ETHERTYPE_IPV6,
"ether_type(vlan) error.");
/* with VLAN and exthdr */
m->data[24] = IPPROTO_DSTOPTS;
m->data[58] = IPPROTO_RSVP;
m->data[59] = 0;
classify_packet(&pkt);
TEST_ASSERT_EQUAL_MESSAGE(pkt.ether_type, ETHERTYPE_IPV6,
"ether_type(vlan+exthdr) error.");
}
void
test_classify_packet_IPV4_TCP(void) {
struct port port;
struct lagopus_packet pkt;
OS_MBUF *m;
/* prepare packet */
m = calloc(1, sizeof(*m));
TEST_ASSERT_NOT_EQUAL_MESSAGE(m, NULL, "calloc error.");
m->data = &m->dat[128];
OS_M_PKTLEN(m) = 128;
m->data[12] = 0x08;
m->data[13] = 0x00;
m->data[14] = 0x45;
m->data[23] = IPPROTO_TCP;
pkt.mbuf = (OS_MBUF *)m;
lagopus_set_in_port(&pkt, &port);
classify_packet(&pkt);
TEST_ASSERT_EQUAL_MESSAGE(pkt.ether_type, ETHERTYPE_IP,
"ether_type error.");
TEST_ASSERT_EQUAL_MESSAGE(pkt.l3_hdr, &m->data[14],
"l3_hdr error.");
TEST_ASSERT_EQUAL_MESSAGE(pkt.l4_hdr, &m->data[34],
"l4_hdr error.");
/* with VLAN */
m->data[12] = 0x81;
m->data[13] = 0x00;
m->data[16] = 0x08;
m->data[17] = 0x00;
m->data[18] = 0x45;
m->data[27] = IPPROTO_TCP;
pkt.mbuf = (OS_MBUF *)m;
lagopus_set_in_port(&pkt, &port);
classify_packet(&pkt);
TEST_ASSERT_EQUAL_MESSAGE(pkt.ether_type, ETHERTYPE_IP,
"ether_type(vlan) error.");
TEST_ASSERT_EQUAL_MESSAGE(pkt.l3_hdr, &m->data[18],
"l3_hdr(vlan) error.");
TEST_ASSERT_EQUAL_MESSAGE(pkt.l4_hdr, &m->data[38],
"l4_hdr(vlan) error.");
}
int
main(int argc, char** argv)
{
const char* inFile = "-"; // default to stdin
const char* hostIP = NULL; // default to no filter on IP
const char* port = NULL; // default to not filter on port
bool doRX = true; // default to showing pkts RX by ip and port
bool doParse = false; // default to no parsing
bool doExecute = false; // default to no execute
bool doClassify = false; // default to no classify
bool verbose = false; // default to not verbose
bool doAllRtcp = false; // default to only MUX/TIP
LibTip::MediaType mType = LibTip::VIDEO; // default to video
char* filterString = NULL; // default to no user filter string
uint8_t profile = 1; // default to CTS-1000 profile
const char* usageString =
"\n"
"[--file file]\n"
"[--ip IP]\n"
"[--port port]\n"
"[--xmit]\n"
"[--parse]\n"
"[--exec]\n"
"[--classify]\n"
"[--verbose]\n"
"[--allrtcp]\n"
"[--audio]\n"
"[--filter pcap_filter_string]\n"
"[--profile 1 (CTS1000) | 2 (T1-CTS1000) | 3 (CTS3000)]\n";
char* progName = argv[0];
while (true) {
int c = -1;
static struct option long_options[] = {
{ "file", 1, 0, 'a' },
{ "ip", 1, 0, 'b' },
{ "port", 1, 0, 'c' },
{ "parse", 0, 0, 'd' },
{ "exec", 0, 0, 'e' },
{ "verbose", 0, 0, 'f' },
{ "audio", 0, 0, 'g' },
{ "xmit", 0, 0, 'h' },
{ "filter", 1, 0, 'i' },
{ "profile", 1, 0, 'j' },
{ "classify",0, 0, 'k' },
{ "allrtcp", 0, 0, 'l' },
{ NULL, 0, 0, 0 }
};
c = getopt_long_only(argc, argv, "", long_options, NULL);
if (c == -1) {
break;
}
switch (c) {
case 'a':
inFile = optarg;
break;
case 'b':
hostIP = optarg;
break;
case 'c':
port = optarg;
break;
case 'd':
doParse = true;
break;
case 'e':
doExecute = true;
break;
case 'f':
verbose = true;
break;
case 'g':
mType = LibTip::AUDIO;
break;
case 'h':
doRX = false;
break;
case 'i':
filterString = optarg;
break;
case 'j':
if (sscanf(optarg, "%hhu", &profile) != 1) {
printf("ERROR: invalid profile '%s'\n", optarg);
}
break;
case 'k':
doClassify = true;
break;
case 'l':
doAllRtcp = true;
break;
case '?':
printf("usage: %s %s", progName, usageString);
return 0;
}
}
// must give us something to do
if (!doParse && !doExecute && !doClassify) {
printf("ERROR: must specify either --parse or --execute or --classify\n");
printf("usage: %s %s", progName, usageString);
return 1;
}
// create an tip instance for executing the packet trace
PcapExecPacketTransmit xmit;
LibTip::CTip tip(xmit);
// configure with requested profile
if (profile == 1) {
LibTip::CSingleScreenProfile::Configure(tip.GetTipSystem(), false, false);
} else if (profile == 2) {
LibTip::CSingleScreenExtendedReachProfile::Configure(tip.GetTipSystem(), false);
} else {
LibTip::CTripleScreenProfile::Configure(tip.GetTipSystem(), false, false);
}
char errbuf[PCAP_ERRBUF_SIZE];
pcap_t* pcap = pcap_open_offline(inFile, errbuf);
if (pcap == NULL) {
printf("ERROR opening pcap file: '%s'\n", errbuf);
return 1;
}
char filter_exp[2048] = { '\0' };
if (filterString == NULL) {
// default filter, only look at UDP
sprintf(filter_exp, "ip proto \\udp");
if (hostIP != NULL) {
if (doRX) {
sprintf(filter_exp, "%s && dst host %s", filter_exp, hostIP);
} else {
sprintf(filter_exp, "%s && src host %s", filter_exp, hostIP);
}
}
if (port != NULL) {
if (doRX) {
sprintf(filter_exp, "%s && dst port %s", filter_exp, port);
} else {
sprintf(filter_exp, "%s && src port %s", filter_exp, port);
}
}
} else {
strncpy(filter_exp, filterString, sizeof(filter_exp));
}
struct bpf_program fp;
if (pcap_compile(pcap, &fp, filter_exp, 0, 0) == -1) {
printf("ERROR: couldn't compile filter '%s': '%s'\n",
filter_exp, pcap_geterr(pcap));
return 1;
}
if (pcap_setfilter(pcap, &fp) == -1) {
printf("ERROR: Couldn't set filter '%s': '%s'\n",
filter_exp, pcap_geterr(pcap));
return 1;
}
if (doExecute) {
tip.StartTipNegotiate(mType);
}
uint32_t pid = 0;
while (1) {
struct pcap_pkthdr header;
uint8_t* packet = (uint8_t*) pcap_next(pcap, &header);
if (packet == NULL) {
break;
}
pid++;
if (header.len > header.caplen) {
printf("WARNING: ignoring packet # %u b/c it is truncated\n", pid);
continue;
}
if (doParse) {
parse_packet(packet, header, verbose, doAllRtcp, mType);
}
if (doExecute) {
exec_packet(packet, header, mType, tip);
}
if (doClassify) {
classify_packet(packet, header);
}
}
pcap_close(pcap);
return 0;
}
void synopt_per_packet(struct libtrace_packet_t *packet)
{
struct libtrace_tcp *tcp = trace_get_tcp(packet);
unsigned char *opt_ptr;
libtrace_direction_t dir = trace_get_direction(packet);
int len;
unsigned char type, optlen, *data;
struct tcp_opts opts_seen = {false, false, false, false, false, false};
if(!tcp)
return;
if (!tcp->syn)
return;
if (dir != TRACE_DIR_INCOMING && dir != TRACE_DIR_OUTGOING)
dir = TRACE_DIR_OTHER;
len = tcp->doff * 4 - sizeof(libtrace_tcp_t);
if(len == 0)
return;
opt_ptr = (unsigned char *)tcp + sizeof (libtrace_tcp_t);
while(trace_get_next_option(&opt_ptr,&len,&type,&optlen,&data)){
/* I don't think we need to count NO-OPs */
if (type == 1)
continue;
switch(type) {
case 2:
opts_seen.mss = true;
break;
case 3:
opts_seen.winscale = true;
break;
case 4:
opts_seen.sack = true;
break;
case 5:
opts_seen.sack = true;
break;
case 8:
opts_seen.ts = true;
break;
case 11:
case 12:
case 13:
opts_seen.ttcp = true;
break;
default:
opts_seen.other = true;
}
}
if (tcp->ack) {
total_synacks ++;
classify_packet(opts_seen, &synack_counts);
} else {
total_syns ++;
classify_packet(opts_seen, &syn_counts);
}
}
/* Code to actually dissect the packets */
static int
dissect_mbtcp(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree)
{
/* Set up structures needed to add the protocol subtree and manage it */
mbtcp_hdr mh;
proto_item *mi, *mf;
proto_tree *mbtcp_tree, *modbus_tree, *group_tree;
int offset, group_offset, packet_type;
guint i;
gint packet_len, payload_start, payload_len;
const char *func_string = "";
const char *pkt_type_str = "";
const char *err_str = "";
guint32 byte_cnt, group_byte_cnt, group_word_cnt;
guint32 packet_num; /* num to uniquely identify different mbtcp
* packets in one packet */
guint8 exception_code;
gboolean exception_returned;
guint8 fc;
mh.transaction_id = tvb_get_ntohs(tvb, 0);
mh.protocol_id = tvb_get_ntohs(tvb, 2);
mh.len = tvb_get_ntohs(tvb, 4);
mh.mdbs_hdr.unit_id = tvb_get_guint8(tvb, 6);
mh.mdbs_hdr.function_code = tvb_get_guint8(tvb, 7);
/* check that it actually looks like Modbus/TCP */
/* protocol id == 0 */
if( mh.protocol_id != 0 ){
return 0;
}
/* length is at least 2 (unit_id + function_code) */
if( mh.len < 2 ){
return 0;
}
/* function code is in the set 1-24, 40, 125-127.
* Note that function code is only 7 bits.
*/
fc=mh.mdbs_hdr.function_code&0x7f;
if(!match_strval(fc, function_code_vals))
return 0;
/* Make entries in Protocol column on summary display */
col_set_str(pinfo->cinfo, COL_PROTOCOL, "Modbus/TCP");
col_clear(pinfo->cinfo, COL_INFO);
/* Make entries in Info column on summary display */
offset = 0;
if ( mh.mdbs_hdr.function_code & 0x80 ) {
exception_code = tvb_get_guint8(tvb, offset + sizeof(mbtcp_hdr));
mh.mdbs_hdr.function_code ^= 0x80;
exception_returned = TRUE;
}
else {
exception_code = 0;
exception_returned = FALSE;
}
func_string = val_to_str(mh.mdbs_hdr.function_code, function_code_vals,
"Unknown function (%u)");
if (check_col(pinfo->cinfo, COL_INFO))
{
packet_type = classify_packet(pinfo);
switch ( packet_type ) {
case QUERY_PACKET : pkt_type_str="query";
break;
case RESPONSE_PACKET : pkt_type_str="response";
break;
case CANNOT_CLASSIFY : err_str="Unable to classify as query or response.";
pkt_type_str="unknown";
break;
default :
break;
}
if ( exception_returned )
err_str="Exception returned ";
col_add_fstr(pinfo->cinfo, COL_INFO,
"%8s [%2u pkt(s)]: trans: %5u; unit: %3u, func: %3u: %s. %s",
pkt_type_str, 1, mh.transaction_id, (unsigned char) mh.mdbs_hdr.unit_id,
(unsigned char) mh.mdbs_hdr.function_code, func_string, err_str);
}
/* build up protocol tree and iterate over multiple packets */
packet_num = 0;
while (1) {
packet_type = classify_packet(pinfo);
packet_len = sizeof(mbtcp_hdr) - sizeof(modbus_hdr) + mh.len;
/* if a tree exists, perform operations to add fields to it */
if (tree) {
mi = proto_tree_add_protocol_format(tree, proto_mbtcp, tvb, offset,
packet_len, "Modbus/TCP");
mbtcp_tree = proto_item_add_subtree(mi, ett_mbtcp);
/* Add items to protocol tree specific to Modbus/TCP */
proto_tree_add_uint(mbtcp_tree, hf_mbtcp_transid, tvb, offset, 2,
mh.transaction_id);
proto_tree_add_uint(mbtcp_tree, hf_mbtcp_protid, tvb, offset + 2, 2,
mh.protocol_id);
proto_tree_add_uint(mbtcp_tree, hf_mbtcp_len, tvb, offset + 4, 2,
mh.len);
proto_tree_add_uint(mbtcp_tree, hf_mbtcp_unitid, tvb, offset + 6, 1,
mh.mdbs_hdr.unit_id);
/* Add items to protocol tree specific to Modbus generic */
mf = proto_tree_add_text(mbtcp_tree, tvb, offset + 7, mh.len - 1,
"Modbus");
modbus_tree = proto_item_add_subtree(mf, ett_modbus_hdr);
mi = proto_tree_add_uint(modbus_tree, hf_mbtcp_functioncode, tvb, offset + 7, 1,
mh.mdbs_hdr.function_code);
/** detail payload as a function of exception/function code */
func_string = val_to_str(mh.mdbs_hdr.function_code,
function_code_vals, "Unknown function");
payload_start = offset + 8;
payload_len = mh.len - sizeof(modbus_hdr);
if (exception_returned) {
proto_item_set_text(mi, "function %u: %s. Exception: %s",
mh.mdbs_hdr.function_code,
func_string,
val_to_str(exception_code,
exception_code_vals,
"Unknown exception code (%u)"));
proto_tree_add_uint(modbus_tree, hf_modbus_exceptioncode, tvb, payload_start, 1,
exception_code);
}
else {
proto_item_set_text(mi, "function %u: %s", mh.mdbs_hdr.function_code,
func_string);
switch (mh.mdbs_hdr.function_code) {
case READ_COILS:
case READ_INPUT_DISCRETES:
if (packet_type == QUERY_PACKET) {
proto_tree_add_item(modbus_tree, hf_modbus_reference, tvb, payload_start, 2, FALSE);
proto_tree_add_item(modbus_tree, hf_modbus_bitcnt, tvb, payload_start + 2, 2, FALSE);
}
else if (packet_type == RESPONSE_PACKET) {
byte_cnt = (guint32)tvb_get_guint8(tvb, payload_start);
proto_tree_add_uint(modbus_tree, hf_modbus_bytecnt, tvb, payload_start, 1, byte_cnt);
proto_tree_add_text(modbus_tree, tvb, payload_start + 1, byte_cnt, "Data");
}
break;
case READ_MULT_REGS:
case READ_INPUT_REGS:
if (packet_type == QUERY_PACKET) {
proto_tree_add_item(modbus_tree, hf_modbus_reference, tvb, payload_start, 2, FALSE);
proto_tree_add_item(modbus_tree, hf_modbus_wordcnt, tvb, payload_start + 2, 2, FALSE);
}
else if (packet_type == RESPONSE_PACKET) {
byte_cnt = (guint32)tvb_get_guint8(tvb, payload_start);
proto_tree_add_uint(modbus_tree, hf_modbus_bytecnt, tvb, payload_start, 1, byte_cnt);
proto_tree_add_text(modbus_tree, tvb, payload_start + 1, byte_cnt, "Data");
}
break;
case WRITE_COIL:
if (packet_type == QUERY_PACKET) {
proto_tree_add_item(modbus_tree, hf_modbus_reference, tvb, payload_start, 2, FALSE);
proto_tree_add_text(modbus_tree, tvb, payload_start + 2, 1, "Data");
proto_tree_add_text(modbus_tree, tvb, payload_start + 3, 1, "Padding");
}
else if (packet_type == RESPONSE_PACKET) {
proto_tree_add_item(modbus_tree, hf_modbus_reference, tvb, payload_start, 2, FALSE);
proto_tree_add_text(modbus_tree, tvb, payload_start + 2, 1, "Data");
proto_tree_add_text(modbus_tree, tvb, payload_start + 3, 1, "Padding");
}
break;
case WRITE_SINGLE_REG:
if (packet_type == QUERY_PACKET) {
proto_tree_add_item(modbus_tree, hf_modbus_reference, tvb, payload_start, 2, FALSE);
proto_tree_add_text(modbus_tree, tvb, payload_start + 2, 2, "Data");
}
else if (packet_type == RESPONSE_PACKET) {
proto_tree_add_item(modbus_tree, hf_modbus_reference, tvb, payload_start, 2, FALSE);
proto_tree_add_text(modbus_tree, tvb, payload_start + 2, 2, "Data");
}
break;
case READ_EXCEPT_STAT:
if (packet_type == RESPONSE_PACKET)
proto_tree_add_text(modbus_tree, tvb, payload_start, 1, "Data");
break;
case FORCE_MULT_COILS:
if (packet_type == QUERY_PACKET) {
proto_tree_add_item(modbus_tree, hf_modbus_reference, tvb, payload_start, 2, FALSE);
proto_tree_add_item(modbus_tree, hf_modbus_bitcnt, tvb, payload_start + 2, 2, FALSE);
byte_cnt = (guint32)tvb_get_guint8(tvb, payload_start + 4);
proto_tree_add_uint(modbus_tree, hf_modbus_bytecnt, tvb, payload_start + 4, 1,
byte_cnt);
proto_tree_add_text(modbus_tree, tvb, payload_start + 5, byte_cnt, "Data");
}
else if (packet_type == RESPONSE_PACKET) {
proto_tree_add_item(modbus_tree, hf_modbus_reference, tvb, payload_start, 2, FALSE);
proto_tree_add_item(modbus_tree, hf_modbus_bitcnt, tvb, payload_start + 2, 2, FALSE);
}
break;
case WRITE_MULT_REGS:
if (packet_type == QUERY_PACKET) {
proto_tree_add_item(modbus_tree, hf_modbus_reference, tvb, payload_start, 2, FALSE);
proto_tree_add_item(modbus_tree, hf_modbus_wordcnt, tvb, payload_start + 2, 2, FALSE);
byte_cnt = (guint32)tvb_get_guint8(tvb, payload_start + 4);
proto_tree_add_uint(modbus_tree, hf_modbus_bytecnt, tvb, payload_start + 4, 1,
byte_cnt);
proto_tree_add_text(modbus_tree, tvb, payload_start + 5, byte_cnt, "Data");
}
else if (packet_type == RESPONSE_PACKET) {
proto_tree_add_item(modbus_tree, hf_modbus_reference, tvb, payload_start, 2, FALSE);
proto_tree_add_item(modbus_tree, hf_modbus_wordcnt, tvb, payload_start + 2, 2, FALSE);
}
break;
case READ_GENL_REF:
if (packet_type == QUERY_PACKET) {
byte_cnt = (guint32)tvb_get_guint8(tvb, payload_start);
proto_tree_add_uint(modbus_tree, hf_modbus_bytecnt, tvb, payload_start, 1,
byte_cnt);
/* add subtrees to describe each group of packet */
group_offset = payload_start + 1;
for (i = 0; i < byte_cnt / 7; i++) {
mi = proto_tree_add_text( modbus_tree, tvb, group_offset, 7,
"Group %u", i);
group_tree = proto_item_add_subtree(mi, ett_group_hdr);
proto_tree_add_item(group_tree, hf_modbus_reftype, tvb, group_offset, 1, FALSE);
proto_tree_add_item(group_tree, hf_modbus_lreference, tvb, group_offset + 1, 4, FALSE);
proto_tree_add_item(group_tree, hf_modbus_wordcnt, tvb, group_offset + 5, 2, FALSE);
group_offset += 7;
}
}
else if (packet_type == RESPONSE_PACKET) {
byte_cnt = (guint32)tvb_get_guint8(tvb, payload_start);
proto_tree_add_uint(modbus_tree, hf_modbus_bytecnt, tvb, payload_start, 1,
byte_cnt);
/* add subtrees to describe each group of packet */
group_offset = payload_start + 1;
i = 0;
while (byte_cnt > 0) {
group_byte_cnt = (guint32)tvb_get_guint8(tvb, group_offset);
mi = proto_tree_add_text( modbus_tree, tvb, group_offset, group_byte_cnt + 1,
"Group %u", i);
group_tree = proto_item_add_subtree(mi, ett_group_hdr);
proto_tree_add_uint(group_tree, hf_modbus_bytecnt, tvb, group_offset, 1,
group_byte_cnt);
proto_tree_add_item(group_tree, hf_modbus_reftype, tvb, group_offset + 1, 1, FALSE);
proto_tree_add_text(group_tree, tvb, group_offset + 2, group_byte_cnt - 1, "Data");
group_offset += (group_byte_cnt + 1);
byte_cnt -= (group_byte_cnt + 1);
i++;
}
}
break;
case WRITE_GENL_REF:
if ((packet_type == QUERY_PACKET) || (packet_type == RESPONSE_PACKET)) {
byte_cnt = (guint32)tvb_get_guint8(tvb, payload_start);
proto_tree_add_uint(modbus_tree, hf_modbus_bytecnt, tvb, payload_start, 1,
byte_cnt);
/* add subtrees to describe each group of packet */
group_offset = payload_start + 1;
i = 0;
while (byte_cnt > 0) {
group_word_cnt = tvb_get_ntohs(tvb, group_offset + 5);
group_byte_cnt = (2 * group_word_cnt) + 7;
mi = proto_tree_add_text( modbus_tree, tvb, group_offset,
group_byte_cnt, "Group %u", i);
group_tree = proto_item_add_subtree(mi, ett_group_hdr);
proto_tree_add_item(group_tree, hf_modbus_reftype, tvb, group_offset, 1, FALSE);
proto_tree_add_item(group_tree, hf_modbus_lreference, tvb, group_offset + 1, 4, FALSE);
proto_tree_add_uint(group_tree, hf_modbus_wordcnt, tvb, group_offset + 5, 2,
group_word_cnt);
proto_tree_add_text(group_tree, tvb, group_offset + 7, group_byte_cnt - 7, "Data");
group_offset += group_byte_cnt;
byte_cnt -= group_byte_cnt;
i++;
}
}
break;
case MASK_WRITE_REG:
if ((packet_type == QUERY_PACKET) || (packet_type == RESPONSE_PACKET)) {
proto_tree_add_item(modbus_tree, hf_modbus_reference, tvb, payload_start, 2, FALSE);
proto_tree_add_item(modbus_tree, hf_modbus_andmask, tvb, payload_start + 2, 2, FALSE);
proto_tree_add_item(modbus_tree, hf_modbus_ormask, tvb, payload_start + 4, 2, FALSE);
}
break;
case READ_WRITE_REG:
if (packet_type == QUERY_PACKET) {
proto_tree_add_item(modbus_tree, hf_modbus_readref, tvb, payload_start, 2, FALSE);
proto_tree_add_item(modbus_tree, hf_modbus_readwordcnt, tvb, payload_start + 2, 2, FALSE);
proto_tree_add_item(modbus_tree, hf_modbus_writeref, tvb, payload_start + 4, 2, FALSE);
proto_tree_add_item(modbus_tree, hf_modbus_writewordcnt, tvb, payload_start + 6, 2, FALSE);
byte_cnt = (guint32)tvb_get_guint8(tvb, payload_start + 8);
proto_tree_add_uint(modbus_tree, hf_modbus_bytecnt, tvb, payload_start + 8, 1,
byte_cnt);
proto_tree_add_text(modbus_tree, tvb, payload_start + 9, byte_cnt, "Data");
}
else if (packet_type == RESPONSE_PACKET) {
byte_cnt = (guint32)tvb_get_guint8(tvb, payload_start);
proto_tree_add_uint(modbus_tree, hf_modbus_bytecnt, tvb, payload_start, 1,
byte_cnt);
proto_tree_add_text(modbus_tree, tvb, payload_start + 1, byte_cnt, "Data");
}
break;
case READ_FIFO_QUEUE:
if (packet_type == QUERY_PACKET)
proto_tree_add_item(modbus_tree, hf_modbus_reference, tvb, payload_start, 2, FALSE);
else if (packet_type == RESPONSE_PACKET) {
byte_cnt = (guint32)tvb_get_ntohs(tvb, payload_start);
proto_tree_add_uint(modbus_tree, hf_modbus_lbytecnt, tvb, payload_start, 2,
byte_cnt);
proto_tree_add_item(modbus_tree, hf_modbus_wordcnt, tvb, payload_start + 2, 2, FALSE);
proto_tree_add_text(modbus_tree, tvb, payload_start + 4, byte_cnt - 2, "Data");
}
break;
case DIAGNOSTICS:
case PROGRAM_484:
case POLL_484:
case GET_COMM_EVENT_CTRS:
case GET_COMM_EVENT_LOG:
case PROGRAM_584_984:
case POLL_584_984:
case REPORT_SLAVE_ID:
case PROGRAM_884_U84:
case RESET_COMM_LINK:
case PROGRAM_CONCEPT:
case FIRMWARE_REPLACE:
case PROGRAM_584_984_2:
case REPORT_LOCAL_ADDR_MB:
/* these function codes are not part of the Modbus/TCP specification */
default:
if (payload_len > 0)
proto_tree_add_text(modbus_tree, tvb, payload_start, payload_len, "Data");
break;
}
}
}
/* move onto next packet (if there) */
offset += packet_len;
packet_num++;
if (tvb_reported_length_remaining(tvb, offset) > 0) {
/* load header structure for next packet */
mh.transaction_id = tvb_get_ntohs(tvb, offset+0);
mh.protocol_id = tvb_get_ntohs(tvb, offset+2);
mh.len = tvb_get_ntohs(tvb, offset+4);
mh.mdbs_hdr.unit_id = tvb_get_guint8(tvb, offset+6);
mh.mdbs_hdr.function_code = tvb_get_guint8(tvb, offset+7);
if ( mh.mdbs_hdr.function_code & 0x80 ) {
exception_code = tvb_get_guint8(tvb, offset + sizeof(mbtcp_hdr));
mh.mdbs_hdr.function_code ^= 0x80;
exception_returned = TRUE;
} else
exception_returned = FALSE;
}
else
break;
}
return tvb_length(tvb);
}
