epan/dissectors/packet-canopen.c

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