?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
epan/dissectors/pidl/ C99 drsuapi
[wireshark-sm.git] / epan / dissectors / packet-5co-legacy.c
blobeda6f8bbc63c9002567ce7cb8afa331178010c33
1 /* packet-5co-legacy.c
2 * Routines for FiveCo's Legacy Register Access Protocol dissector
3 * Copyright 2021, Antoine Gardiol <antoine.gardiol@fiveco.ch>
4 *
5 * Wireshark - Network traffic analyzer
6 * By Gerald Combs <gerald@wireshark.org>
7 * Copyright 1998 Gerald Combs
8 *
9 * SPDX-License-Identifier: GPL-2.0-or-later
10 */
11
12 /*
13 * This protocol allows access to FiveCo's Ethernet products registers with old legacy
14 * protocol. Product list can be found under https://www.fiveco.ch/bus-converter-products.html.
15 * Protocol description can be found (by example) in FMod-TCP xx manual that can be dowloaded from
16 * https://www.fiveco.ch/product-fmod-tcp-db.html.
17 * Note that this protocol is a question-answer protocol. It's header is composed of:
18 * - 16 bits type
19 * - 16 bits frame id
20 * - 16 bits length of parameters (n)
21 * - n bytes of parameters (depends upon packet type)
22 * - 16 bits IP like checksum
23 *
24 * This build-in dissector is replacing a plugin dissector available from Wireshark 1.8.
25 */
26
27 #include <config.h>
28 #include <epan/packet.h>
29 #include <epan/proto_data.h>
30 #include <wsutil/array.h>
31 #include "packet-tcp.h"
32 #include <stdio.h>
33 #include <stdbool.h>
34 #include <stdint.h>
35 /* Prototypes */
36 void proto_reg_handoff_FiveCoLegacy(void);
37 void proto_register_FiveCoLegacy(void);
38
39 static dissector_handle_t FiveCoLegacy_handle;
40
41 /****************************************************************************/
42 /* Definition declaration */
43 /****************************************************************************/
44
45 // Protocol header length and frame minimum length
46 #define FIVECO_LEGACY_HEADER_LENGTH 6
47 #define FIVECO_LEGACY_MIN_LENGTH FIVECO_LEGACY_HEADER_LENGTH + 2 // Checksum is 16 bits
48
49 #define PSNAME "5co-legacy"
50
51 /* Global sample ports preferences */
52 #define FIVECO_PORT1 8010 /* TCP port of the FiveCo protocol */
53 #define FIVECO_PORT2 8004 /* TCP port of the FiveCo protocol for web page upload */
54 #define FIVECO_UDP_PORT1 7010 /* UDP port of the FiveCo protocol */
55
56 /* 16 bits type known available functions */
57 enum fiveco_functions
58 {
59 I2C_READ = 0x0001,
60 I2C_WRITE,
61 I2C_READ_ANSWER,
62 I2C_WRITE_ANSWER,
63 I2C_SCAN,
64 I2C_SCAN_ANSWER,
65 I2C_READ_WRITE_ACK,
66 I2C_READ_WRITE_ACK_ANSWER,
67 I2C_READ_WRITE_ACK_ERROR,
68 READ_REGISTER = 0x0021,
69 WRITE_REGISTER,
70 READ_REGISTER_ANSWER,
71 WRITE_REGISTER_ANSWER,
72 WRITE_REGISTER_QUIET,
73 EASY_IP_ADDRESS_CONFIG = 0x002A,
74 EASY_IP_ADDRESS_CONFIG_ANSWER,
75 FLASH_AREA_ERASE = 0x0031,
76 FLASH_AREA_LOAD,
77 FLASH_AREA_ANSWER
78 };
79
80 /* Forward references to functions */
81 static uint16_t
82 checksum_fiveco(tvbuff_t * byte_tab, uint16_t start_offset, uint16_t size);
83 static int fiveco_hash_equal(const void *v, const void *w);
84
85 /* Register decoding functions prototypes */
86 static void dispType( char *result, uint32_t type);
87 static void dispVersion( char *result, uint32_t type);
88 static void dispMAC( char *result, uint64_t type);
89 static void dispIP( char *result, uint32_t type);
90 static void dispMask( char *result, uint32_t type);
91 static void dispTimeout( char *result, uint32_t type);
92
93 /* Initialize the protocol and registered fields */
94 static int proto_FiveCoLegacy; /* Wireshark ID of the FiveCo protocol */
95
96 /* static dissector_handle_t data_handle = NULL; */
97 static int hf_fiveco_header; /* The following hf_* variables are used to hold the Wireshark IDs of */
98 static int hf_fiveco_fct; /* our header fields; they are filled out when we call */
99 static int hf_fiveco_id; /* proto_register_field_array() in proto_register_fiveco() */
100 static int hf_fiveco_length;
101 static int hf_fiveco_data;
102 static int hf_fiveco_cks;
103 static int hf_fiveco_i2cadd;
104 static int hf_fiveco_i2c2write;
105 static int hf_fiveco_i2cwrite;
106 static int hf_fiveco_i2c2read;
107 static int hf_fiveco_i2c2scan;
108 static int hf_fiveco_i2canswer;
109 static int hf_fiveco_i2cwriteanswer;
110 static int hf_fiveco_i2cscaned;
111 static int hf_fiveco_i2cerror;
112 static int hf_fiveco_i2cack;
113 static int hf_fiveco_regread;
114 static int hf_fiveco_regreadunknown;
115 static int hf_fiveco_regreaduk;
116 static int hf_fiveco_EasyIPMAC;
117 static int hf_fiveco_EasyIPIP;
118 static int hf_fiveco_EasyIPSM;
119
120 static int ett_fiveco_header; /* These are the ids of the subtrees that we may be creating */
121 static int ett_fiveco_data; /* for the header fields. */
122 static int ett_fiveco;
123 static int ett_fiveco_checksum;
124
125 /* Constants declaration */
126 static const value_string packettypenames[] = {
127 {I2C_READ, "I2C Read (deprecated)"},
128 {I2C_READ_ANSWER, "I2C Read Answer (deprecated)"},
129 {I2C_WRITE, "I2C Write (deprecated)"},
130 {I2C_WRITE_ANSWER, "I2C Write Answer (deprecated)"},
131 {I2C_SCAN, "I2C Scan"},
132 {I2C_SCAN_ANSWER, "I2C Scan Answer"},
133 {I2C_READ_WRITE_ACK, "I2C Read and write with ack"},
134 {I2C_READ_WRITE_ACK_ANSWER, "I2C Read and write with ack Answer"},
135 {I2C_READ_WRITE_ACK_ERROR, "I2C Read and write error"},
136 {READ_REGISTER, "Read register"},
137 {READ_REGISTER_ANSWER, "Read register Answer"},
138 {WRITE_REGISTER, "Write register"},
139 {WRITE_REGISTER_ANSWER, "Write register Answer"},
140 {WRITE_REGISTER_QUIET, "Write register (no answer wanted)"},
141 {EASY_IP_ADDRESS_CONFIG, "Easy IP address config"},
142 {EASY_IP_ADDRESS_CONFIG_ANSWER, "Easy IP address config Acknowledge"},
143 {FLASH_AREA_ERASE, "Flash area Erase"},
144 {FLASH_AREA_LOAD, "Flash area Upload"},
145 {FLASH_AREA_ANSWER, "Flash area Answer"},
146 {0, NULL}};
147
148 /* Conversation request key structure */
149 typedef struct
150 {
151 uint32_t conversation;
152 uint64_t unInternalID;
153 uint16_t usExpCmd;
154 } FCOSConvRequestKey;
155
156 /* Conversation request value structure */
157 typedef struct
158 {
159 uint16_t usParaLen;
160 uint16_t isReplied;
161 uint8_t *pDataBuffer;
162 } FCOSConvRequestVal;
163
164 /* Conversation hash tables */
165 static wmem_map_t *FiveCo_requests_hash;
166
167 /* Internal unique ID (used to match answer with question
168 since some software set always 0 as packet ID in protocol header)
169 */
170 static uint64_t g_unInternalID;
171
172 /* Register definition structure (used to detect known registers when it is possible) */
173 typedef struct
174 {
175 uint32_t unValue; // Register address
176 uint32_t unSize; // Register size (in bytes)
177 const char *name; // Register name
178 const char *abbrev; // Abbreviation for header fill
179 const enum ftenum ft; // Field type
180 int nsWsHeaderID; // Wireshark ID for header fill
181 const void *pFct; // Conversion function
182 } FCOSRegisterDef;
183
184 /* Known (common on every product) registers */
185 static FCOSRegisterDef aRegisters[] = {
186 {0x00, 4, "Register Type/Model", "5co_legacy.RegTypeModel", FT_UINT32, -1, CF_FUNC(dispType)},
187 {0x01, 4, "Register Version", "5co_legacy.RegVersion", FT_UINT32, -1, CF_FUNC(dispVersion)},
188 {0x02, 0, "Function Reset device", "5co_legacy.RegReset", FT_NONE, -1, NULL},
189 {0x03, 0, "Function Save user parameters", "5co_legacy.RegSave", FT_NONE, -1, NULL},
190 {0x04, 0, "Function Restore user parameters", "5co_legacy.RegRestore", FT_NONE, -1, NULL},
191 {0x05, 0, "Function Restore factory parameters", "5co_legacy.RegRestoreFact", FT_NONE, -1, NULL},
192 {0x06, 0, "Function Save factory parameters", "5co_legacy.SaveFact", FT_NONE, -1, NULL},
193 {0x07, 0, "Register unknown", "5co_legacy.RegUnknown07", FT_NONE, -1, NULL},
194 {0x08, 0, "Register unknown", "5co_legacy.RegUnknown08", FT_NONE, -1, NULL},
195 {0x09, 0, "Register unknown", "5co_legacy.RegUnknown09", FT_NONE, -1, NULL},
196 {0x0A, 0, "Register unknown", "5co_legacy.RegUnknown0A", FT_NONE, -1, NULL},
197 {0x0B, 0, "Register unknown", "5co_legacy.RegUnknown0B", FT_NONE, -1, NULL},
198 {0x0C, 0, "Register unknown", "5co_legacy.RegUnknown0C", FT_NONE, -1, NULL},
199 {0x0D, 0, "Register unknown", "5co_legacy.RegUnknown0D", FT_NONE, -1, NULL},
200 {0x0E, 0, "Register unknown", "5co_legacy.RegUnknown0E", FT_NONE, -1, NULL},
201 {0x0F, 0, "Register unknown", "5co_legacy.RegUnknown0F", FT_NONE, -1, NULL},
202 {0x10, 4, "Register Communication options", "5co_legacy.RegComOption", FT_UINT32, -1, NULL},
203 {0x11, 6, "Register Ethernet MAC Address", "5co_legacy.RegMAC", FT_UINT48, -1, CF_FUNC(dispMAC)},
204 {0x12, 4, "Register IP Address", "5co_legacy.RegIPAdd", FT_UINT32, -1, CF_FUNC(dispIP)},
205 {0x13, 4, "Register IP Mask", "5co_legacy.RegIPMask", FT_UINT32, -1, CF_FUNC(dispMask)},
206 {0x14, 1, "Register TCP Timeout", "5co_legacy.RegTCPTimeout", FT_UINT8, -1, CF_FUNC(dispTimeout)},
207 {0x15, 16, "Register Module name", "5co_legacy.RegName", FT_STRING, -1, NULL}};
208
209 /* List of static header fields */
210 static hf_register_info hf_base[] = {
211 {&hf_fiveco_header, {"Header", "5co_legacy.header", FT_NONE, BASE_NONE, NULL, 0x0, "Header of the packet", HFILL}},
212 {&hf_fiveco_fct, {"Function", "5co_legacy.fct", FT_UINT16, BASE_HEX, VALS(packettypenames), 0x0, "Function type", HFILL}},
213 {&hf_fiveco_id, {"Frame ID", "5co_legacy.id", FT_UINT16, BASE_DEC, NULL, 0x0, "Packet ID", HFILL}},
214 {&hf_fiveco_length, {"Data length", "5co_legacy.length", FT_UINT16, BASE_DEC, NULL, 0x0, "Parameters length of the packet", HFILL}},
215 {&hf_fiveco_data, {"Data", "5co_legacy.data", FT_NONE, BASE_NONE, NULL, 0x0, "Data (parameters)", HFILL}},
216 {&hf_fiveco_cks, {"Checksum", "5co_legacy.checksum", FT_UINT16, BASE_HEX, NULL, 0x0, "Checksum of the packet", HFILL}},
217 {&hf_fiveco_i2cadd, {"I2C Address", "5co_legacy.i2cadd", FT_UINT8, BASE_DEC, NULL, 0x0, NULL, HFILL}},
218 {&hf_fiveco_i2c2write, {"I2C number of bytes to write", "5co_legacy.i2c2write", FT_UINT8, BASE_DEC, NULL, 0x0, NULL, HFILL}},
219 {&hf_fiveco_i2cwrite, {"I2C bytes to write", "5co_legacy.i2cwrite", FT_NONE, BASE_NONE, NULL, 0x0, NULL, HFILL}},
220 {&hf_fiveco_i2c2read, {"I2C number of bytes to read", "5co_legacy.i2c2read", FT_UINT8, BASE_DEC, NULL, 0x0, NULL, HFILL}},
221 {&hf_fiveco_i2canswer, {"I2C bytes read", "5co_legacy.i2cread", FT_NONE, BASE_NONE, NULL, 0x0, NULL, HFILL}},
222 {&hf_fiveco_i2cwriteanswer, {"I2C bytes write", "5co_legacy.i2writeanswer", FT_NONE, BASE_NONE, NULL, 0x0, NULL, HFILL}},
223 {&hf_fiveco_i2cack, {"I2C ack state", "5co_legacy.i2cack", FT_UINT8, BASE_HEX, NULL, 0x0, NULL, HFILL}},
224 {&hf_fiveco_i2c2scan, {"I2C addresses to scan", "5co_legacy.i2c2scan", FT_NONE, BASE_NONE, NULL, 0x0, NULL, HFILL}},
225 {&hf_fiveco_i2cscaned, {"I2C addresses present", "5co_legacy.i2cscaned", FT_NONE, BASE_NONE, NULL, 0x0, NULL, HFILL}},
226 {&hf_fiveco_i2cerror, {"I2C error", "5co_legacy.i2cerror", FT_NONE, BASE_NONE, NULL, 0x0, NULL, HFILL}},
227 {&hf_fiveco_regread, {"Read", "5co_legacy.regread", FT_NONE, BASE_NONE, NULL, 0x0, NULL, HFILL}},
228 {&hf_fiveco_regreadunknown, {"Read Register unknown", "5co_legacy.hf_fiveco_regreadunknown", FT_NONE, BASE_NONE, NULL, 0x0, NULL, HFILL}},
229 {&hf_fiveco_regreaduk, {"Data not decoded", "5co_legacy.regreaduk", FT_NONE, BASE_NONE, NULL, 0x0, "Data not decoded because there are unable to map to a known register", HFILL}},
230 {&hf_fiveco_EasyIPMAC, {"MAC address", "5co_legacy.EasyIPMAC", FT_ETHER, BASE_NONE, NULL, 0x0, NULL, HFILL}},
231 {&hf_fiveco_EasyIPIP, {"New IP address", "5co_legacy.EasyIPIP", FT_IPv4, BASE_NONE, NULL, 0x0, NULL, HFILL}},
232 {&hf_fiveco_EasyIPSM, {"New subnet mask", "5co_legacy.EasyIPSM", FT_IPv4, BASE_NONE, NULL, 0x0, NULL, HFILL}}
233 };
234
235 /*****************************************************************************/
236 /* Code to actually dissect the packets */
237 /* Callback function for reassembled packet */
238 /*****************************************************************************/
239 static int
240 dissect_FiveCoLegacy(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data _U_)
241 {
242 uint16_t checksum_cal, checksum_rx;
243 uint16_t i, j, y;
244 uint16_t tcp_data_offset = 0;
245 uint32_t tcp_data_length = 0;
246 uint16_t header_type = 0;
247 uint16_t header_id = 0;
248 uint16_t header_data_length = 0;
249 uint8_t data_i2c_length = 0;
250 proto_item *fiveco_item = NULL;
251 proto_item *fiveco_header_item = NULL;
252 proto_item *fiveco_data_item = NULL;
253 proto_tree *fiveco_tree = NULL;
254 proto_tree *fiveco_header_tree = NULL;
255 proto_tree *fiveco_data_tree = NULL;
256 conversation_t *conversation;
257 bool isRequest = false;
258 uint64_t *pulInternalID = NULL;
259 FCOSConvRequestKey requestKey, *pNewRequestKey;
260 FCOSConvRequestVal *pRequestVal = NULL;
261 tvbuff_t *pRequestTvb = NULL;
262 uint8_t ucAdd, ucBytesToWrite, ucBytesToRead;
263 uint8_t ucRegAdd, ucRegSize;
264 uint32_t unOffset;
265 uint32_t unSize;
266
267 /* Load protocol payload length (including checksum) */
268 tcp_data_length = tvb_captured_length(tvb);
269 if (tcp_data_length < FIVECO_LEGACY_MIN_LENGTH) // Check checksum presence
270 return 0;
271
272 /* Display fiveco in protocol column */
273 col_set_str(pinfo->cinfo, COL_PROTOCOL, PSNAME);
274 /* Clear out stuff in the info column */
275 col_clear(pinfo->cinfo, COL_INFO);
276
277 /* Look for all future TCP conversations between the
278 * requestiong server and the FiveCo device using the
279 * same src & dest addr and ports.
280 */
281 conversation = find_or_create_conversation(pinfo);
282 requestKey.conversation = conversation->conv_index;
283
284 /* Loop because several fiveco packets can be present in one TCP packet */
285 while (tcp_data_offset < tcp_data_length) {
286
287 /* Check that header type is correct */
288 header_type = tvb_get_ntohs(tvb, tcp_data_offset + 0);
289 if (try_val_to_str(header_type, packettypenames) == NULL)
290 return 0;
291
292 /* Read packet ID */
293 header_id = tvb_get_ntohs(tvb, tcp_data_offset + 2);
294
295 /* Check that there's enough data versus prot data header_data_length */
296 header_data_length = tvb_get_ntohs(tvb, tcp_data_offset + 4);
297 if (header_data_length > tcp_data_length - tcp_data_offset - 8) {
298 return 0;
299 }
300
301 /* Get/Set internal ID for this packet number */
302 pulInternalID = (uint64_t *)p_get_proto_data(wmem_file_scope(), pinfo, proto_FiveCoLegacy, pinfo->num);
303 /* If internal ID is not set (null), create it */
304 if (!pulInternalID)
305 {
306 /* If it is a new request, increment internal ID */
307 if ((header_type == I2C_READ) || (header_type == I2C_WRITE) || (header_type == I2C_SCAN) ||
308 (header_type == I2C_READ_WRITE_ACK) || (header_type == READ_REGISTER) || (header_type == WRITE_REGISTER))
309 {
310 isRequest = true;
311 g_unInternalID++; // Increment unique request ID and record it in the new request
312 /* Note: Since some software do not increment packet id located in frame header
313 we use an internal ID to match answers to request. */
314 }
315 pulInternalID = wmem_new(wmem_file_scope(), uint64_t);
316 *pulInternalID = g_unInternalID;
317 p_add_proto_data(wmem_file_scope(), pinfo, proto_FiveCoLegacy, pinfo->num, pulInternalID);
318 }
319
320 /* Get info about the request */
321 requestKey.usExpCmd = header_type;
322 requestKey.unInternalID = *pulInternalID;
323 pRequestVal = (FCOSConvRequestVal *)wmem_map_lookup(FiveCo_requests_hash, &requestKey);
324 if ((!pinfo->fd->visited) && (!pRequestVal) && (isRequest))
325 {
326 /* If unknown and if it is a request, allocate new hash element that we want to handle later in answer */
327 pNewRequestKey = wmem_new(wmem_file_scope(), FCOSConvRequestKey);
328 *pNewRequestKey = requestKey;
329 pNewRequestKey->unInternalID = g_unInternalID;
330 switch (header_type)
331 {
332 case I2C_READ:
333 pNewRequestKey->usExpCmd = I2C_READ_ANSWER;
334 break;
335 case I2C_WRITE:
336 pNewRequestKey->usExpCmd = I2C_WRITE_ANSWER;
337 break;
338 case I2C_SCAN:
339 pNewRequestKey->usExpCmd = I2C_SCAN_ANSWER;
340 break;
341 case I2C_READ_WRITE_ACK:
342 pNewRequestKey->usExpCmd = I2C_READ_WRITE_ACK_ANSWER;
343 break;
344 case READ_REGISTER:
345 pNewRequestKey->usExpCmd = READ_REGISTER_ANSWER;
346 break;
347 }
348
349 pRequestVal = wmem_new(wmem_file_scope(), FCOSConvRequestVal);
350 pRequestVal->usParaLen = header_data_length;
351 pRequestVal->isReplied = false;
352 pRequestVal->pDataBuffer = (uint8_t *)wmem_alloc(wmem_file_scope(), header_data_length);
353 tvb_memcpy(tvb, pRequestVal->pDataBuffer, tcp_data_offset + 6, header_data_length);
354
355 wmem_map_insert(FiveCo_requests_hash, pNewRequestKey, pRequestVal);
356 }
357
358 if (pRequestVal) {
359 pRequestTvb = tvb_new_child_real_data(tvb, pRequestVal->pDataBuffer, pRequestVal->usParaLen, pRequestVal->usParaLen);
360 }
361
362 /* Compute checksum of the packet and read one received */
363 checksum_cal = checksum_fiveco(tvb, tcp_data_offset, header_data_length + 6);
364 checksum_rx = tvb_get_ntohs(tvb, tcp_data_offset + header_data_length + 6);
365
366 /* Add text to info column */
367 /* If the offset != 0 (not first fiveco frame in tcp packet) add a comma in info column */
368 if (tcp_data_offset != 0)
369 {
370 col_append_fstr(pinfo->cinfo, COL_INFO, ", %s ID=%d Len=%d",
371 val_to_str(header_type, packettypenames, "Unknown Type:0x%02x"), header_id, header_data_length);
372 }
373 else
374 {
375 col_append_fstr(pinfo->cinfo, COL_INFO, "%s ID=%d Len=%d",
376 val_to_str(header_type, packettypenames, "Unknown Type:0x%02x"), header_id, header_data_length);
377 }
378
379 if (checksum_rx != checksum_cal)
380 {
381 col_append_str(pinfo->cinfo, COL_INFO, " [BAD CHECKSUM !!]");
382 }
383
384 /* Add FiveCo protocol in tree (after TCP or UDP entry) */
385 fiveco_item = proto_tree_add_item(tree, proto_FiveCoLegacy, tvb, tcp_data_offset + 0,
386 header_data_length + 8, ENC_NA); /* Add a new entry inside tree display */
387 proto_item_append_text(fiveco_item, " (%s)", val_to_str(header_type, packettypenames, "Unknown Type:0x%02x"));
388
389 /* Add fiveco Protocol tree and sub trees for Header, Data and Checksum */
390 fiveco_tree = proto_item_add_subtree(fiveco_item, ett_fiveco); // FiveCo prot tree
391 fiveco_header_item = proto_tree_add_item(fiveco_tree, hf_fiveco_header,
392 tvb, tcp_data_offset + 0, 6, ENC_NA); // Header tree
393 fiveco_header_tree = proto_item_add_subtree(fiveco_header_item, ett_fiveco_header);
394 proto_tree_add_item(fiveco_header_tree, hf_fiveco_fct,
395 tvb, tcp_data_offset + 0, 2, ENC_BIG_ENDIAN); // Packet type (function) in Header
396 proto_tree_add_item(fiveco_header_tree, hf_fiveco_id,
397 tvb, tcp_data_offset + 2, 2, ENC_BIG_ENDIAN); // Packet ID in Header
398 proto_tree_add_item(fiveco_header_tree, hf_fiveco_length,
399 tvb, tcp_data_offset + 4, 2, ENC_BIG_ENDIAN); // Length of para in Header
400
401 tcp_data_offset += 6; // put offset on start of data (parameters)
402
403 // If there are parameters (data) in packet, display them in data sub tree
404 if (header_data_length > 0)
405 {
406 fiveco_data_item = proto_tree_add_item(fiveco_tree, hf_fiveco_data, tvb, tcp_data_offset,
407 header_data_length, ENC_NA); // Data tree
408 fiveco_data_tree = proto_item_add_subtree(fiveco_data_item, ett_fiveco_data);
409 switch (header_type)
410 {
411 case I2C_READ:
412 case I2C_READ_WRITE_ACK:
413 i = 0;
414 while (i < header_data_length)
415 {
416 proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2cadd, tvb, tcp_data_offset + i, 1, ENC_BIG_ENDIAN);
417 i += 1;
418 data_i2c_length = tvb_get_uint8(tvb, tcp_data_offset + i);
419 proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2c2write, tvb, tcp_data_offset + i, 1, ENC_BIG_ENDIAN);
420 i += 1;
421 fiveco_data_item = proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2cwrite,
422 tvb, tcp_data_offset + i, data_i2c_length, ENC_NA);
423 proto_item_append_text(fiveco_data_item, ": ");
424 for (j = 0; j < data_i2c_length; j++)
425 {
426 proto_item_append_text(fiveco_data_item, "0x%.2X ",
427 tvb_get_uint8(tvb, tcp_data_offset + i));
428 i += 1;
429 }
430 proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2c2read, tvb, tcp_data_offset + i, 1, ENC_BIG_ENDIAN);
431 i += 1;
432 }
433 break;
434 case I2C_WRITE:
435 i = 0;
436 while (i < header_data_length)
437 {
438 proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2cadd, tvb, tcp_data_offset + i, 1, ENC_BIG_ENDIAN);
439 i += 1;
440 data_i2c_length = tvb_get_uint8(tvb, tcp_data_offset + i);
441 proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2c2write, tvb, tcp_data_offset + i, 1, ENC_BIG_ENDIAN);
442 i += 1;
443 fiveco_data_item = proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2cwrite,
444 tvb, tcp_data_offset + i, data_i2c_length, ENC_NA);
445 proto_item_append_text(fiveco_data_item, ": ");
446 for (j = 0; j < data_i2c_length; j++)
447 {
448 proto_item_append_text(fiveco_data_item, "0x%.2X ",
449 tvb_get_uint8(tvb, tcp_data_offset + i));
450 i += 1;
451 }
452 }
453 break;
454 case I2C_SCAN:
455 fiveco_data_item = proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2c2scan,
456 tvb, tcp_data_offset + 0, header_data_length, ENC_NA);
457 proto_item_append_text(fiveco_data_item, ": ");
458 // If specific address exists in packet, display them
459 for (i = 0; i < header_data_length; i++)
460 {
461 proto_item_append_text(fiveco_data_item, "0x%.2X ",
462 tvb_get_uint8(tvb, tcp_data_offset + i));
463 }
464 break;
465 case I2C_SCAN_ANSWER:
466 fiveco_data_item = proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2cscaned,
467 tvb, tcp_data_offset + 0, header_data_length, ENC_NA);
468 proto_item_append_text(fiveco_data_item, ": ");
469 // Display slave address presents in answer
470 for (i = 0; i < header_data_length; i++)
471 {
472 proto_item_append_text(fiveco_data_item, "0x%.2X ",
473 tvb_get_uint8(tvb, tcp_data_offset + i));
474 }
475 break;
476 case I2C_READ_WRITE_ACK_ERROR:
477 fiveco_data_item = proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2cerror,
478 tvb, tcp_data_offset + 0, header_data_length, ENC_NA);
479 proto_item_append_text(fiveco_data_item, ": ");
480 proto_item_append_text(fiveco_data_item, "0x%.2X ",
481 tvb_get_uint8(tvb, tcp_data_offset));
482 break;
483 case READ_REGISTER:
484 // List registers asked for read
485 for (i = 0; i < header_data_length; i++)
486 {
487 ucRegAdd = tvb_get_uint8(tvb, tcp_data_offset + i);
488 if ((ucRegAdd < array_length(aRegisters)) &&
489 (aRegisters[ucRegAdd].unValue == ucRegAdd))
490 {
491 fiveco_data_item = proto_tree_add_item(fiveco_data_tree, hf_fiveco_regread,
492 tvb, tcp_data_offset + i, 0, ENC_NA);
493 proto_item_append_text(fiveco_data_item, " %s", aRegisters[ucRegAdd].name);
494 }
495 else
496 {
497 fiveco_data_item = proto_tree_add_item(fiveco_data_tree, hf_fiveco_regreadunknown,
498 tvb, tcp_data_offset + i, 0, ENC_NA);
499 }
500 proto_item_append_text(fiveco_data_item, " (0x%.2X)", ucRegAdd);
501 }
502 break;
503 case WRITE_REGISTER:
504 case WRITE_REGISTER_QUIET:
505 // List register asked to write with data to fill in until an unknown one is found
506 for (i = tcp_data_offset; i < tcp_data_offset + header_data_length;)
507 {
508 ucRegAdd = tvb_get_uint8(tvb, i++);
509 // If register address is known & found
510 if ((ucRegAdd < array_length(aRegisters)) &&
511 (aRegisters[ucRegAdd].unValue == ucRegAdd))
512 {
513 ucRegSize = aRegisters[ucRegAdd].unSize;
514 // If a display function is defined, call it
515 if (aRegisters[ucRegAdd].pFct != NULL)
516 {
517 proto_tree_add_item(fiveco_data_tree, aRegisters[ucRegAdd].nsWsHeaderID,
518 tvb, i, ucRegSize, ENC_NA);
519 i += ucRegSize;
520 }
521 // else if register type is string, display it as string
522 else if (aRegisters[ucRegAdd].ft == FT_STRING)
523 {
524 fiveco_data_item = proto_tree_add_item(fiveco_data_tree,
525 aRegisters[ucRegAdd].nsWsHeaderID,
526 tvb, i, ucRegSize,
527 ENC_NA);
528 proto_item_append_text(fiveco_data_item, ": %s", tvb_format_text(pinfo->pool, tvb, i, ucRegSize));
529 i += ucRegSize;
530 }
531 // else display raw data in hex
532 else
533 {
534 fiveco_data_item = proto_tree_add_item(fiveco_data_tree, hf_fiveco_regread,
535 tvb, i, ucRegSize, ENC_NA);
536 proto_item_append_text(fiveco_data_item, " %s (Add: 0x%.2X, Size: %d bytes): ",
537 aRegisters[ucRegAdd].name, ucRegAdd, ucRegSize);
538 for (j = 0; j < ucRegSize; j++)
539 {
540 proto_item_append_text(fiveco_data_item, "0x%.2X ", tvb_get_uint8(tvb, i++));
541 }
542 }
543 }
544 // Else tell user that data cannot be interpreted
545 else
546 {
547 fiveco_data_item = proto_tree_add_item(fiveco_data_tree, hf_fiveco_regreaduk,
548 tvb, i, tcp_data_offset + header_data_length - i, ENC_NA);
549 proto_item_append_text(fiveco_data_item, " (Interpretation depends on product type)");
550 break;
551 }
552 }
553 break;
554 case EASY_IP_ADDRESS_CONFIG:
555 proto_tree_add_item(fiveco_data_tree, hf_fiveco_EasyIPMAC, tvb, tcp_data_offset + 0, 6, ENC_NA);
556 proto_tree_add_item(fiveco_data_tree, hf_fiveco_EasyIPIP, tvb, tcp_data_offset + 6, 4, ENC_BIG_ENDIAN);
557 proto_tree_add_item(fiveco_data_tree, hf_fiveco_EasyIPSM, tvb, tcp_data_offset + 10, 4, ENC_BIG_ENDIAN);
558 break;
559 case I2C_READ_ANSWER:
560 case I2C_WRITE_ANSWER:
561 case I2C_READ_WRITE_ACK_ANSWER:
562 if (pRequestVal)
563 {
564 if (pRequestVal->isReplied != 0)
565 {
566 proto_item_append_text(fiveco_data_item,
567 " WARNING : Answer already found ! Maybe packets ID not incremented.");
568 }
569 else
570 {
571 i = tcp_data_offset; // Answer index
572 y = 0; // Request index
573 while ((y < pRequestVal->usParaLen) && (i < tcp_data_offset + header_data_length))
574 {
575 // I2C address in first byte of request
576 ucAdd = tvb_get_uint8(pRequestTvb, y++);
577 // Read number of bytes to write
578 ucBytesToWrite = tvb_get_uint8(pRequestTvb, y);
579 // Skip number of bytes to write and those bytes
580 y += 1 + ucBytesToWrite;
581 // Read number of bytes to read
582 ucBytesToRead = tvb_get_uint8(pRequestTvb, y++);
583 if (ucBytesToRead > 0)
584 {
585 fiveco_data_item = proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2canswer,
586 tvb, i, ucBytesToRead, ENC_NA);
587 proto_item_append_text(fiveco_data_item,
588 " from address %d (%d bytes written) : ",
589 ucAdd, ucBytesToWrite);
590 for (j = 0; j < ucBytesToRead; j++)
591 {
592 proto_item_append_text(fiveco_data_item, "0x%.2X ",
593 tvb_get_uint8(tvb, i++));
594 }
595 if (header_type == 0x08)
596 proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2cack, tvb, i++, 1, ENC_BIG_ENDIAN);
597 }
598 else if (header_type == I2C_READ_WRITE_ACK_ANSWER)
599 {
600 // if it's an answer to a write but with ack, display it
601 fiveco_data_item = proto_tree_add_item(fiveco_data_tree,
602 hf_fiveco_i2cwriteanswer, tvb, i,
603 ucBytesToRead, ENC_NA);
604 proto_item_append_text(fiveco_data_item, " to address %d (%d bytes written)",
605 ucAdd, ucBytesToWrite);
606 proto_tree_add_item(fiveco_data_tree, hf_fiveco_i2cack, tvb, i++, 1, ENC_BIG_ENDIAN);
607 }
608 }
609 }
610 break;
611 }
612 else {
613 proto_item_append_text(fiveco_data_item, " (Interpretation depends on product type)");
614 }
615 break;
616 case READ_REGISTER_ANSWER:
617 if (pRequestVal)
618 {
619 if (pRequestVal->isReplied != 0)
620 {
621 proto_item_append_text(fiveco_data_item,
622 " WARNING : Answer already found ! Maybe packets ID not incremented.");
623 }
624 else
625 {
626 i = tcp_data_offset; // Answer index
627 y = 0; // Request index
628 // For each request stored in the last read request of the conversation
629 while ((y < pRequestVal->usParaLen) && (i < tcp_data_offset + header_data_length))
630 {
631 // Register address in first byte of request
632 ucRegAdd = tvb_get_uint8(pRequestTvb, y++);
633 // If register address is known & found in answer
634 if ((ucRegAdd < array_length(aRegisters)) &&
635 (aRegisters[ucRegAdd].unValue == ucRegAdd) &&
636 (ucRegAdd == tvb_get_uint8(tvb, i++)))
637 {
638 // Retrieve register size and display it with address
639 ucRegSize = aRegisters[ucRegAdd].unSize;
640 // If a display function is defined, call it
641 if (aRegisters[ucRegAdd].pFct != NULL)
642 {
643 proto_tree_add_item(fiveco_data_tree, aRegisters[ucRegAdd].nsWsHeaderID,
644 tvb, i, ucRegSize, ENC_NA);
645 i += ucRegSize;
646 }
647 // else if register type is string, display it as string
648 else if (aRegisters[ucRegAdd].ft == FT_STRING)
649 {
650 fiveco_data_item = proto_tree_add_item(fiveco_data_tree,
651 aRegisters[ucRegAdd].nsWsHeaderID,
652 tvb, i, ucRegSize,
653 ENC_NA);
654 proto_item_append_text(fiveco_data_item, ": %s", tvb_format_text(pinfo->pool, tvb, i, ucRegSize));
655 i += ucRegSize;
656 }
657 // else display raw data in hex
658 else
659 {
660 fiveco_data_item = proto_tree_add_item(fiveco_data_tree,
661 hf_fiveco_regread, tvb, i, ucRegSize, ENC_NA);
662 proto_item_append_text(fiveco_data_item,
663 " %s (Add: 0x%.2X, Size: %d bytes): ",
664 aRegisters[ucRegAdd].name, ucRegAdd, ucRegSize);
665 for (j = 0; j < ucRegSize; j++)
666 {
667 proto_item_append_text(fiveco_data_item,
668 "0x%.2X ", tvb_get_uint8(tvb, i++));
669 }
670 }
671 }
672 // Else tell user that data cannot be interpreted
673 else
674 {
675 fiveco_data_item = proto_tree_add_item(fiveco_data_tree,
676 hf_fiveco_regreaduk, tvb, i,
677 tcp_data_offset + header_data_length - i,
678 ENC_NA);
679 proto_item_append_text(fiveco_data_item,
680 " (Interpretation depends on product type)");
681 break;
682 }
683 }
684 }
685 }
686 break;
687 case FLASH_AREA_LOAD:
688 unOffset = tvb_get_uint24(tvb, tcp_data_offset, ENC_BIG_ENDIAN);
689 unSize = tvb_get_uint24(tvb, tcp_data_offset + 3, ENC_BIG_ENDIAN);
690 proto_item_append_text(fiveco_data_item,
691 " (%d bytes to load into flash at offset %d)", unSize, unOffset);
692 break;
693 case FLASH_AREA_ANSWER:
694 if ( header_data_length > 1 ) {
695 proto_item_append_text(fiveco_data_item, " (%s)", tvb_format_text(pinfo->pool, tvb, tcp_data_offset, header_data_length - 1));
696 }
697 break;
698
699 case WRITE_REGISTER_ANSWER:
700 case FLASH_AREA_ERASE:
701 case EASY_IP_ADDRESS_CONFIG_ANSWER:
702 proto_item_append_text(fiveco_data_item, " (ERROR: No data should be present with that packet type !!)");
703 break;
704
705 default:
706 proto_item_append_text(fiveco_data_item, " (Interpretation depends on product type)");
707 break;
708 }
709 }
710
711 // Checksum validation and sub tree
712 proto_tree_add_checksum(fiveco_tree, tvb, tcp_data_offset + header_data_length, hf_fiveco_cks, -1, NULL, NULL,
713 checksum_cal, ENC_BIG_ENDIAN, PROTO_CHECKSUM_VERIFY);
714
715 tcp_data_offset += header_data_length + 2 ; /* jump to next packet if exists */
716 } /*while (tcp_data_offset < tcp_data_length) */
717
718 return tvb_captured_length(tvb);
719 }
720
721 /*****************************************************************************/
722 /* This function returns the calculated checksum (IP based) */
723 /*****************************************************************************/
724 static uint16_t checksum_fiveco(tvbuff_t *byte_tab, uint16_t start_offset, uint16_t size)
725 {
726 uint32_t Sum = 0;
727 uint8_t AddHighByte = 1;
728 uint32_t ChecksumCalculated;
729 uint16_t i;
730 uint8_t temp;
731
732 for (i = 0; i < size; i++)
733 {
734 tvb_memcpy(byte_tab, (uint8_t *)&temp, start_offset + i, 1);
735 if (AddHighByte)
736 {
737 Sum += (temp << 8) ^ 0xFF00;
738 AddHighByte = 0;
739 }
740 else
741 {
742 Sum += (temp) ^ 0x00FF;
743 AddHighByte = 1;
744 }
745 }
746
747 if (AddHighByte == 0)
748 Sum += 0xFF;
749
750 ChecksumCalculated = ((Sum >> 16) & 0xFFFF) + (Sum & 0xFFFF);
751 ChecksumCalculated = ((ChecksumCalculated >> 16) & 0xFFFF) + (ChecksumCalculated & 0xFFFF);
752 return (uint16_t)ChecksumCalculated;
753 }
754
755 /*****************************************************************************/
756 /* Compute an unique hash value */
757 /*****************************************************************************/
758 static unsigned fiveco_hash(const void *v)
759 {
760 const FCOSConvRequestKey *key = (const FCOSConvRequestKey *)v;
761 unsigned val;
762
763 val = key->conversation + (((key->usExpCmd) & 0xFFFF) << 16) +
764 (key->unInternalID & 0xFFFFFFFF) + ((key->unInternalID >>32) & 0xFFFFFFFF);
765
766 return val;
767 }
768
769 /*****************************************************************************/
770 /* Check hash equal */
771 /*****************************************************************************/
772 static int fiveco_hash_equal(const void *v, const void *w)
773 {
774 const FCOSConvRequestKey *v1 = (const FCOSConvRequestKey *)v;
775 const FCOSConvRequestKey *v2 = (const FCOSConvRequestKey *)w;
776
777 if (v1->conversation == v2->conversation &&
778 v1->usExpCmd == v2->usExpCmd &&
779 v1->unInternalID == v2->unInternalID)
780 {
781 return 1;
782 }
783 return 0;
784 }
785
786 /*****************************************************************************/
787 /* Register the protocol with Wireshark.
788 *
789 * This format is required because a script is used to build the C function that
790 * calls all the protocol registration.
791 */
792 /*****************************************************************************/
793 void proto_register_FiveCoLegacy(void)
794 {
795 /* Setup list of header fields (based on static table and specific table) */
796 static hf_register_info hf[array_length(hf_base) + array_length(aRegisters)];
797 for (uint32_t i = 0; i < array_length(hf_base); i++) {
798 hf[i] = hf_base[i];
799 }
800 for (uint32_t i = 0; i < array_length(aRegisters); i++) {
801 if (aRegisters[i].pFct != NULL){
802 hf_register_info hfx = { &(aRegisters[i].nsWsHeaderID),{aRegisters[i].name, aRegisters[i].abbrev, aRegisters[i].ft, BASE_CUSTOM, aRegisters[i].pFct, 0x0, NULL, HFILL}};
803 hf[array_length(hf_base) + i] = hfx;
804 } else {
805 hf_register_info hfx = { &(aRegisters[i].nsWsHeaderID),{aRegisters[i].name, aRegisters[i].abbrev, FT_NONE, BASE_NONE, NULL, 0x0, NULL, HFILL}};
806 hf[array_length(hf_base) + i] = hfx;
807 }
808 }
809
810 /* Setup protocol subtree array */
811 static int *ett[] = {
812 &ett_fiveco_header,
813 &ett_fiveco_data,
814 &ett_fiveco,
815 &ett_fiveco_checksum};
816
817 /* Register the protocol name and description */
818 proto_FiveCoLegacy = proto_register_protocol("FiveCo's Legacy Register Access Protocol",
819 PSNAME, "5co_legacy");
820
821 /* Required function calls to register the header fields and subtrees */
822 proto_register_field_array(proto_FiveCoLegacy, hf, array_length(hf));
823 proto_register_subtree_array(ett, array_length(ett));
824
825 /* Register the dissector */
826 FiveCoLegacy_handle = register_dissector("5co_legacy", dissect_FiveCoLegacy,
827 proto_FiveCoLegacy);
828
829 FiveCo_requests_hash = wmem_map_new_autoreset(wmem_epan_scope(), wmem_file_scope(), fiveco_hash, fiveco_hash_equal);
830
831 /* Set preference callback to NULL since it is not used */
832 prefs_register_protocol(proto_FiveCoLegacy, NULL);
833 }
834
835 /* If this dissector uses sub-dissector registration add a registration routine.
836 * This exact format is required because a script is used to find these
837 * routines and create the code that calls these routines.
838 *
839 * Simpler form of proto_reg_handoff_FiveCoLegacy which can be used if there are
840 * no prefs-dependent registration function calls. */
841 void proto_reg_handoff_FiveCoLegacy(void)
842 {
843 static bool initialized = false;
844
845 if (!initialized)
846 {
847 dissector_add_uint("tcp.port", FIVECO_PORT1, FiveCoLegacy_handle);
848 dissector_add_uint("tcp.port", FIVECO_PORT2, FiveCoLegacy_handle);
849 dissector_add_uint("udp.port", FIVECO_UDP_PORT1, FiveCoLegacy_handle);
850 initialized = true;
851 }
852 }
853
854 /*****************************************************************************/
855 /* Registers decoding functions */
856 /*****************************************************************************/
857 static void
858 dispType( char *result, uint32_t type)
859 {
860 int nValueH = (type>>16) & 0xFFFF;
861 int nValueL = (type & 0xFFFF);
862 snprintf( result, ITEM_LABEL_LENGTH, "%d.%d (%.4X.%.4X)", nValueH, nValueL, nValueH, nValueL);
863 }
864
865 static void
866 dispVersion( char *result, uint32_t version)
867 {
868 if ((version & 0xFF000000) == 0)
869 {
870 int nValueH = (version>>16) & 0xFFFF;
871 int nValueL = (version & 0xFFFF);
872 snprintf( result, ITEM_LABEL_LENGTH, "FW: %d.%d", nValueH, nValueL);
873 }
874 else
875 {
876 int nHWHigh = (version>>24) & 0xFF;
877 int nHWLow = (version>>16) & 0xFF;
878 int nFWHigh = (version>>8) & 0xFF;
879 int nFWLow = (version>>8) & 0xFF;
880 snprintf( result, ITEM_LABEL_LENGTH, "HW: %d.%d / FW: %d.%d", nHWHigh, nHWLow, nFWHigh, nFWLow);
881 }
882 }
883
884 static void dispMAC( char *result, uint64_t mac)
885 {
886 uint8_t *pData = (uint8_t*)(&mac);
887
888 snprintf( result, ITEM_LABEL_LENGTH, "%.2X-%.2X-%.2X-%.2X-%.2X-%.2X", pData[5], pData[4], pData[3], pData[2],
889 pData[1], pData[0]);
890 }
891
892 static void dispIP( char *result, uint32_t ip)
893 {
894 uint8_t *pData = (uint8_t*)(&ip);
895
896 snprintf( result, ITEM_LABEL_LENGTH, "%d.%d.%d.%d", pData[3], pData[2], pData[1], pData[0]);
897 }
898
899 static void dispMask( char *result, uint32_t mask)
900 {
901 uint8_t *pData = (uint8_t*)(&mask);
902
903 snprintf( result, ITEM_LABEL_LENGTH, "%d.%d.%d.%d", pData[3], pData[2], pData[1], pData[0]);
904 }
905
906 static void dispTimeout( char *result, uint32_t timeout)
907 {
908 if (timeout != 0)
909 snprintf( result, ITEM_LABEL_LENGTH, "%d seconds", timeout);
910 else
911 snprintf( result, ITEM_LABEL_LENGTH, "Disabled");
912 }
913
914 /*
915 * Editor modelines - https://www.wireshark.org/tools/modelines.html
916 *
917 * Local variables:
918 * c-basic-offset: 4
919 * tab-width: 8
920 * indent-tabs-mode: nil
921 * End:
922 *
923 * vi: set shiftwidth=4 tabstop=8 expandtab:
924 * :indentSize=4:tabSize=8:noTabs=true:
925 */
Metzes wireshark repo