epan/dissectors/packet-canopen.c

   1 /* packet-canopen.c
   2  * Routines for CANopen dissection
   3  * Copyright 2011, Yegor Yefremov <yegorslists@googlemail.com>
   4  *
   5  * $Id$
   6  *
   7  * Wireshark - Network traffic analyzer
   8  * By Gerald Combs <gerald@wireshark.org>
   9  * Copyright 1998 Gerald Combs
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License as published by
  13  * the Free Software Foundation; either version 2 of the License, or
  14  * (at your option) any later version.
  15  *
  16  * This program is distributed in the hope that it will be useful,
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19  * GNU General Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License along
  22  * with this program; if not, write to the Free Software Foundation, Inc.,
  23  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  24  */
  25
  26 #include "config.h"
  27
  28 #include <glib.h>
  29
  30 #include <epan/packet.h>
  31
  32 void proto_register_canopen(void);
  33
  34 /* Initialize the protocol and registered fields */
  35 static int proto_canopen = -1;
  36 static int hf_canopen_cob_id = -1;
  37 static int hf_canopen_function_code = -1;
  38 static int hf_canopen_node_id = -1;
  39 static int hf_canopen_pdo_data = -1;
  40 static int hf_canopen_pdo_data_string = -1;
  41 static int hf_canopen_sdo_cmd = -1;
  42 static int hf_canopen_sdo_main_idx = -1;
  43 static int hf_canopen_sdo_sub_idx = -1;
  44 static int hf_canopen_sdo_data = -1;
  45 static int hf_canopen_em_err_code = -1;
  46 static int hf_canopen_em_err_reg = -1;
  47 static int hf_canopen_em_err_field = -1;
  48 static int hf_canopen_nmt_ctrl_cs = -1;
  49 static int hf_canopen_nmt_ctrl_node_id = -1;
  50 static int hf_canopen_nmt_guard_state = -1;
  51 static int hf_canopen_time_stamp = -1;
  52 static int hf_canopen_time_stamp_ms = -1;
  53 static int hf_canopen_time_stamp_days = -1;
  54
  55 /* Initialize the subtree pointers */
  56 static gint ett_canopen = -1;
  57
  58 /* broadcast messages */
  59 #define FC_NMT                  0x0
  60 #define FC_SYNC                 0x1
  61 #define FC_TIME_STAMP           0x2
  62
  63 /* point-to-point messages */
  64 #define FC_EMERGENCY            0x1
  65 #define FC_PDO1_TX              0x3
  66 #define FC_PDO1_RX              0x4
  67 #define FC_PDO2_TX              0x5
  68 #define FC_PDO2_RX              0x6
  69 #define FC_PDO3_TX              0x7
  70 #define FC_PDO3_RX              0x8
  71 #define FC_PDO4_TX              0x9
  72 #define FC_PDO4_RX              0xA
  73 #define FC_DEFAULT_SDO_TX       0xB
  74 #define FC_DEFAULT_SDO_RX       0xC
  75 #define FC_NMT_ERR_CONTROL      0xE
  76
  77 static const value_string CAN_open_bcast_msg_type_vals[] = {
  78     { FC_NMT,              "EMERGENCY"},
  79     { FC_SYNC,             "Sync"},
  80     { FC_TIME_STAMP,       "TIME STAMP"},
  81     { 0, NULL}
  82 };
  83
  84 static const value_string CAN_open_p2p_msg_type_vals[] = {
  85     { FC_EMERGENCY,        "EMERGENCY"},
  86     { FC_PDO1_TX,          "PDO1 (tx)"},
  87     { FC_PDO1_RX,          "PDO1 (rx)"},
  88     { FC_PDO2_TX,          "PDO2 (tx)"},
  89     { FC_PDO2_RX,          "PDO2 (rx)"},
  90     { FC_PDO3_TX,          "PDO3 (tx)"},
  91     { FC_PDO3_RX,          "PDO3 (rx)"},
  92     { FC_PDO4_TX,          "PDO4 (tx)"},
  93     { FC_PDO4_RX,          "PDO4 (rx)"},
  94     { FC_DEFAULT_SDO_TX,   "Default-SDO (tx)"},
  95     { FC_DEFAULT_SDO_RX,   "Default-SDO (rx)"},
  96     { FC_NMT_ERR_CONTROL,  "NMT Error Control"},
  97     { 0, NULL}
  98 };
  99
 100 /* message types */
 101 #define MT_UNKNOWN                       0
 102 #define MT_NMT_CTRL                      1
 103 #define MT_SYNC                          2
 104 #define MT_TIME_STAMP                    3
 105 #define MT_EMERGENCY                     4
 106 #define MT_PDO                           5
 107 #define MT_SDO                           6
 108 #define MT_NMT_GUARD                     7
 109
 110 /* PDO offsets */
 111 #define CO_PDO_DATA_OFFSET               8
 112
 113 /* SDO offsets */
 114 #define CO_SDO_CMD_OFFSET                8
 115 #define CO_SDO_MAIN_IDX_OFFSET           9
 116 #define CO_SDO_MAIN_SUB_OFFSET          11
 117 #define CO_SDO_DATA_OFFSET              12
 118
 119 /* EMERGENCY offsets */
 120 #define CO_EM_ERR_CODE_OFFSET            8
 121 #define CO_EM_ERR_REG_OFFSET            10
 122 #define CO_EM_ERR_FIELD_OFFSET          11
 123
 124 /* NMT offsets */
 125 #define CO_NMT_CTRL_CS_OFFSET            8
 126 #define CO_NMT_CTRL_NODE_ID_OFFSET       9
 127 #define CO_NMT_GUARD_STATE_OFFSET        8
 128
 129 /* TIME STAMP offsets */
 130 #define CO_TIME_STAMP_MS_OFFSET          8
 131 #define CO_TIME_STAMP_DAYS_OFFSET       12
 132
 133 /* TIME STAMP conversion defines */
 134 #define TS_DAYS_BETWEEN_1970_AND_1984   5113
 135 #define TS_SECONDS_IN_PER_DAY           86400
 136 #define TS_NANOSEC_PER_MSEC             1000000
 137
 138 /* NMT command specifiers */
 139 static const value_string nmt_ctrl_cs[] = {
 140     { 0x01, "Start remote node"},
 141     { 0x02, "Stop remote node"},
 142     { 0x80, "Enter pre-operational state"},
 143     { 0x81, "Reset node"},
 144     { 0x82, "Reset communication"},
 145     { 0, NULL}
 146 };
 147
 148 /* NMT states */
 149 static const value_string nmt_guard_state[] = {
 150     { 0x00, "Initialising"},
 151     { 0x01, "Disconnected"},
 152     { 0x02, "Connecting"},
 153     { 0x03, "Preparing"},
 154     { 0x04, "Stopped"},
 155     { 0x05, "Operational"},
 156     { 0x7F, "Pre-operational"},
 157     { 0, NULL}
 158 };
 159
 160 static guint
 161 canopen_detect_msg_type(guint function_code, guint node_id)
 162 {
 163     switch (function_code) {
 164         case FC_NMT:
 165             return MT_NMT_CTRL;
 166             break;
 167         case FC_SYNC:
 168             if (node_id == 0) {
 169                 return MT_SYNC;
 170             } else {
 171                 return MT_EMERGENCY;
 172             }
 173             break;
 174         case FC_TIME_STAMP:
 175             return MT_TIME_STAMP;
 176             break;
 177         case FC_PDO1_TX:
 178             return MT_PDO;
 179             break;
 180         case FC_PDO1_RX:
 181             return MT_PDO;
 182             break;
 183         case FC_PDO2_TX:
 184             return MT_PDO;
 185             break;
 186         case FC_PDO2_RX:
 187             return MT_PDO;
 188             break;
 189         case FC_PDO3_TX:
 190             return MT_PDO;
 191             break;
 192         case FC_PDO3_RX:
 193             return MT_PDO;
 194             break;
 195         case FC_PDO4_TX:
 196             return MT_PDO;
 197             break;
 198         case FC_PDO4_RX:
 199             return MT_PDO;
 200             break;
 201         case FC_DEFAULT_SDO_TX:
 202             return MT_SDO;
 203             break;
 204         case FC_DEFAULT_SDO_RX:
 205             return MT_SDO;
 206             break;
 207         case FC_NMT_ERR_CONTROL:
 208             return MT_NMT_GUARD;
 209             break;
 210         default:
 211             return MT_UNKNOWN;
 212             break;
 213     }
 214 }
 215
 216 /* Code to actually dissect the packets */
 217 static void
 218 dissect_canopen(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree)
 219 {
 220     guint        function_code;
 221     guint        node_id;
 222     guint32      id;
 223     guint32      time_stamp_msec;
 224     guint32      time_stamp_days;
 225     guint        msg_type_id;
 226     nstime_t     time_stamp;
 227     gint         can_data_len;
 228     const gchar *function_code_str;
 229
 230     col_set_str(pinfo->cinfo, COL_PROTOCOL, "CANopen");
 231     col_clear(pinfo->cinfo, COL_INFO);
 232
 233     can_data_len  = tvb_get_guint8(tvb, 4);
 234     id            = tvb_get_ntohl(tvb, 0);
 235
 236     node_id       = id & 0x7F;
 237     function_code = (id >> 7) & 0xF;
 238
 239     msg_type_id = canopen_detect_msg_type(function_code, node_id);
 240
 241     if (node_id == 0 ) {
 242         /* broadcast */
 243         function_code_str = val_to_str(function_code, CAN_open_bcast_msg_type_vals, "Unknown (%u)");
 244         col_add_fstr(pinfo->cinfo, COL_INFO, "%s", function_code_str);
 245     } else {
 246         /* point-to-point */
 247         function_code_str = val_to_str(function_code, CAN_open_p2p_msg_type_vals, "Unknown (%u)");
 248         col_add_fstr(pinfo->cinfo, COL_INFO, "p2p %s", function_code_str);
 249     }
 250     col_append_fstr(pinfo->cinfo, COL_INFO, "   %s",
 251                     tvb_bytes_to_str_punct(tvb, CO_PDO_DATA_OFFSET, can_data_len, ' '));
 252
 253     if (tree) {
 254         proto_item *ti, *cob_ti, *type_ti;
 255         proto_tree *canopen_tree;
 256         proto_tree *canopen_cob_tree;
 257         proto_tree *canopen_type_tree;
 258
 259         ti = proto_tree_add_item(tree, proto_canopen, tvb, 0, -1, ENC_NA);
 260
 261         canopen_tree = proto_item_add_subtree(ti, ett_canopen);
 262
 263         /* add COB-ID with function code and node id */
 264         cob_ti = proto_tree_add_item(canopen_tree, hf_canopen_cob_id, tvb, 0, 4, ENC_BIG_ENDIAN);
 265         canopen_cob_tree = proto_item_add_subtree(cob_ti, ett_canopen);
 266
 267         /* add function code */
 268         proto_tree_add_item(canopen_cob_tree, hf_canopen_function_code, tvb, 0, 4, ENC_BIG_ENDIAN);
 269
 270         /* add node id */
 271         proto_tree_add_item(canopen_cob_tree, hf_canopen_node_id, tvb, 0, 4, ENC_BIG_ENDIAN);
 272
 273         /* add CANopen frame type */
 274
 275         type_ti = proto_tree_add_text(canopen_tree, tvb,
 276                                       (msg_type_id != MT_SYNC) ?  8 : 0,
 277                                       (msg_type_id != MT_SYNC) ? -1 : 0,
 278                                       "Type: %s", function_code_str);
 279         canopen_type_tree = proto_item_add_subtree(type_ti, ett_canopen);
 280
 281         switch(msg_type_id)
 282         {
 283         case MT_NMT_CTRL:
 284             proto_tree_add_item(canopen_type_tree,
 285                 hf_canopen_nmt_ctrl_cs, tvb, CO_NMT_CTRL_CS_OFFSET, 1, ENC_BIG_ENDIAN);
 286
 287             proto_tree_add_item(canopen_type_tree,
 288                 hf_canopen_nmt_ctrl_node_id, tvb, CO_NMT_CTRL_NODE_ID_OFFSET, 1, ENC_BIG_ENDIAN);
 289             break;
 290         case MT_NMT_GUARD:
 291             proto_tree_add_item(canopen_type_tree,
 292                 hf_canopen_nmt_guard_state, tvb, CO_NMT_GUARD_STATE_OFFSET, 1, ENC_BIG_ENDIAN);
 293             break;
 294         case MT_SYNC:
 295             break;
 296         case MT_TIME_STAMP:
 297             /* calculate the real time stamp */
 298             time_stamp_msec = tvb_get_letohl(tvb, CO_PDO_DATA_OFFSET);
 299             time_stamp_days = tvb_get_ntohs(tvb, CO_PDO_DATA_OFFSET + 4);
 300             time_stamp.secs = (time_stamp_days + TS_DAYS_BETWEEN_1970_AND_1984)
 301                 * TS_SECONDS_IN_PER_DAY + (time_stamp_msec / 1000);
 302             time_stamp.nsecs = (time_stamp_msec % 1000) * TS_NANOSEC_PER_MSEC;
 303
 304             proto_tree_add_time(canopen_type_tree,
 305                 hf_canopen_time_stamp, tvb, CO_TIME_STAMP_MS_OFFSET, 6, &time_stamp);
 306
 307             proto_tree_add_uint(canopen_type_tree,
 308                 hf_canopen_time_stamp_ms, tvb, CO_TIME_STAMP_MS_OFFSET, 4, time_stamp_msec);
 309
 310             proto_tree_add_uint(canopen_type_tree,
 311                 hf_canopen_time_stamp_days, tvb, CO_TIME_STAMP_DAYS_OFFSET, 2, time_stamp_days);
 312
 313             break;
 314         case MT_EMERGENCY:
 315             proto_tree_add_item(canopen_type_tree,
 316                 hf_canopen_em_err_code, tvb, CO_EM_ERR_CODE_OFFSET, 2, ENC_BIG_ENDIAN);
 317
 318             proto_tree_add_item(canopen_type_tree,
 319                 hf_canopen_em_err_reg, tvb, CO_EM_ERR_REG_OFFSET, 1, ENC_BIG_ENDIAN);
 320
 321             proto_tree_add_item(canopen_type_tree,
 322                 hf_canopen_em_err_field, tvb, CO_EM_ERR_FIELD_OFFSET, 4, ENC_NA);
 323             break;
 324         case MT_PDO:
 325             if (can_data_len != 0) {
 326                 proto_tree_add_item(canopen_type_tree,
 327                     hf_canopen_pdo_data, tvb, CO_PDO_DATA_OFFSET, can_data_len, ENC_NA);
 328             }
 329             else {
 330                 proto_tree_add_string(canopen_type_tree,
 331                     hf_canopen_pdo_data_string, tvb, CO_PDO_DATA_OFFSET, 0, "empty");
 332             }
 333             break;
 334         case MT_SDO:
 335             proto_tree_add_item(canopen_type_tree,
 336                 hf_canopen_sdo_cmd, tvb, CO_SDO_CMD_OFFSET, 1, ENC_BIG_ENDIAN);
 337
 338             proto_tree_add_item(canopen_type_tree,
 339                 hf_canopen_sdo_main_idx, tvb, CO_SDO_MAIN_IDX_OFFSET, 2, ENC_BIG_ENDIAN);
 340
 341             proto_tree_add_item(canopen_type_tree,
 342                 hf_canopen_sdo_sub_idx, tvb, CO_SDO_MAIN_SUB_OFFSET, 1, ENC_BIG_ENDIAN);
 343
 344             proto_tree_add_item(canopen_type_tree,
 345                 hf_canopen_sdo_data, tvb, CO_SDO_DATA_OFFSET, 4, ENC_NA);
 346             break;
 347         }
 348     }
 349 }
 350
 351
 352 /* Register the protocol with Wireshark */
 353 void
 354 proto_register_canopen(void)
 355 {
 356     static hf_register_info hf[] = {
 357         { &hf_canopen_cob_id,
 358           { "COB-ID",           "canopen.cob_id",
 359             FT_UINT32, BASE_HEX, NULL, 0x0,
 360             NULL, HFILL }
 361         },
 362         { &hf_canopen_function_code,
 363           { "Function code", "canopen.function_code",
 364             FT_UINT32, BASE_HEX, NULL, 0x780,
 365             NULL, HFILL }
 366         },
 367         { &hf_canopen_node_id,
 368           { "Node-ID", "canopen.node_id",
 369             FT_UINT32, BASE_HEX, NULL, 0x7F,
 370             NULL, HFILL }
 371         },
 372         { &hf_canopen_pdo_data,
 373           { "Data", "canopen.pdo.data",
 374             FT_BYTES, BASE_NONE, NULL, 0x0,
 375             NULL, HFILL }
 376         },
 377         { &hf_canopen_pdo_data_string,
 378           { "Data", "canopen.pdo.data",
 379             FT_STRINGZ, BASE_NONE, NULL, 0x0,
 380             NULL, HFILL }
 381         },
 382         { &hf_canopen_sdo_cmd,
 383           { "SDO command byte", "canopen.sdo.cmd",
 384             FT_UINT8, BASE_HEX, NULL, 0x0,
 385             NULL, HFILL }
 386         },
 387         { &hf_canopen_sdo_main_idx,
 388           { "OD main-index", "canopen.sdo.main_idx",
 389             FT_UINT16, BASE_HEX, NULL, 0x0,
 390             NULL, HFILL }
 391         },
 392         { &hf_canopen_sdo_sub_idx,
 393           { "OD sub-index", "canopen.sdo.sub_idx",
 394             FT_UINT8, BASE_HEX, NULL, 0x0,
 395             NULL, HFILL }
 396         },
 397         { &hf_canopen_sdo_data,
 398           { "Data", "canopen.sdo.data",
 399             FT_BYTES, BASE_NONE, NULL, 0x0,
 400             NULL, HFILL }
 401         },
 402         { &hf_canopen_em_err_code,
 403           { "Error code", "canopen.em.err_code",
 404             FT_UINT16, BASE_HEX, NULL, 0x0,
 405             NULL, HFILL }
 406         },
 407         { &hf_canopen_em_err_reg,
 408           { "Error register", "canopen.em.err_reg",
 409             FT_UINT8, BASE_HEX, NULL, 0x0,
 410             NULL, HFILL }
 411         },
 412         { &hf_canopen_em_err_field,
 413           { "Manufacture specific error field", "canopen.em.err_field",
 414             FT_BYTES, BASE_NONE, NULL, 0x0,
 415             NULL, HFILL }
 416         },
 417         { &hf_canopen_nmt_ctrl_cs,
 418           { "Command specifier", "canopen.nmt_ctrl.cd",
 419             FT_UINT8, BASE_HEX, VALS(nmt_ctrl_cs), 0xFF,
 420             NULL, HFILL }
 421         },
 422         { &hf_canopen_nmt_ctrl_node_id,
 423           { "Node-ID", "canopen.nmt_ctrl.node_id",
 424             FT_UINT8, BASE_HEX, NULL, 0x0,
 425             NULL, HFILL }
 426         },
 427         { &hf_canopen_nmt_guard_state,
 428           { "Node-ID", "canopen.nmt_guard.state",
 429             FT_UINT8, BASE_HEX, VALS(nmt_guard_state), 0x7F,
 430             NULL, HFILL }
 431         },
 432         { &hf_canopen_time_stamp,
 433           { "Time stamp",           "canopen.time_stamp",
 434             FT_ABSOLUTE_TIME, ABSOLUTE_TIME_UTC, NULL, 0x0,
 435             NULL, HFILL }
 436         },
 437         { &hf_canopen_time_stamp_ms,
 438           { "Time, after Midnight in Milliseconds", "canopen.time_stamp_ms",
 439             FT_UINT32, BASE_DEC, NULL, 0x0,
 440             NULL, HFILL }
 441         },
 442         { &hf_canopen_time_stamp_days,
 443           { "Current day since 1 Jan 1984", "canopen.time_stamp_days",
 444             FT_UINT16, BASE_DEC, NULL, 0x0,
 445             NULL, HFILL }
 446         },
 447     };
 448
 449     static gint *ett[] = {
 450         &ett_canopen
 451     };
 452
 453     proto_canopen = proto_register_protocol("CANopen",
 454                                             "CANOPEN",
 455                                             "canopen");
 456
 457     register_dissector("canopen", dissect_canopen, proto_canopen);
 458
 459     proto_register_field_array(proto_canopen, hf, array_length(hf));
 460     proto_register_subtree_array(ett, array_length(ett));
 461 }
 462