From 0fe91480bca31711e5edffd195eeeddc860857bc Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 13 Jul 2011 16:18:50 +0200 Subject: cesar/lib: add library to check sequence number in data packets, refs #2644 By default, this library is disabled (at compilation) for performance reasons. --- cesar/lib/src/seq_check.c | 154 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 154 insertions(+) create mode 100644 cesar/lib/src/seq_check.c (limited to 'cesar/lib/src') diff --git a/cesar/lib/src/seq_check.c b/cesar/lib/src/seq_check.c new file mode 100644 index 0000000000..5b7d0a1e01 --- /dev/null +++ b/cesar/lib/src/seq_check.c @@ -0,0 +1,154 @@ +/* Cesar project {{{ + * + * Copyright (C) 2011 Spidcom + * + * <<>> + * + * }}} */ +/** + * \file lib/src/seq_check.c + * \brief Check sequence number in data packets + * \ingroup lib + * + * + */ +#include "common/std.h" +#include "lib/bitstream.h" +#include "lib/swap.h" +#include "lib/stats.h" + +#include "config/seq/check/vlan/max.h" + +#include "lib/seq_check.h" + +/** + * Ethernet type VLAN ID. + */ +#define SEQ_CHECK_ET_VLAN 0x8100 + +lib_seq_check_config_t lib_seq_check_config; + +void +lib_seq_check_config_set (lib_seq_check_config_t *config) +{ + dbg_assert (config); + /* Set configuration. */ + lib_seq_check_config = *config; +} + +void +lib_seq_check_config_get (lib_seq_check_config_t *config) +{ + dbg_assert (config); + /* Get configuration. */ + *config = lib_seq_check_config; +} + +void +lib_seq_check_config_init (void) +{ + /* Set default configuration. */ + lib_seq_check_config_t config = + { + .seq_size = 16, + .seq_pos = 44, /* For IPerf. */ + .ethertype = SEQ_CHECK_ET_IP, /* No check for Ethernet type. */ + }; + lib_seq_check_config_set (&config); + + /* Register our configuration item. */ + lib_stats_set_stat_value_notype ("SEQ_CHECK_SEQ_POS", + &lib_seq_check_config.seq_pos, + LIB_STATS_ACCESS_READ_WRITE, + LIB_STATS_DEBUG); + lib_stats_set_stat_value_notype ("SEQ_CHECK_ETHERTYPE", + &lib_seq_check_config.ethertype, + LIB_STATS_ACCESS_READ_WRITE, + LIB_STATS_DEBUG); +} + +void +lib_seq_check_init (lib_seq_check_t *ctx, lib_seq_check_error_cb_t cb, + void *user) +{ + /* Check parameters. */ + dbg_assert (ctx); + dbg_assert (cb); + + uint i; + /* Initialize sequence number to 0. */ + for (i = 0; i < CONFIG_SEQ_CHECK_VLAN_MAX; i++) + { + ctx->seq[i] = 0; + } + ctx->cb = cb; + ctx->cb_user = user; +} + +bool +lib_seq_check_packet (lib_seq_check_t *ctx, u8 *buffer, uint len) +{ + /* Check parameter. */ + dbg_assert (ctx); + + bool result = false; + + /* Get Ethernet type (after SRC & DST MAC (6 bytes each)). */ + uint ethertype = swap16 (bitstream_direct_read (buffer, 6 * 8 * 2, 2 * + 8)); + + /* Position of sequence counter index in the table. */ + uint seq_index = 0; + + /* VLAN in used? */ + if (ethertype == SEQ_CHECK_ET_VLAN) + { + /* VLAN id on 12 bits. VLAN id is located after SRC & DST (6 bytes + * each) & VLAN/IEEE_802.1Q fields (TPID + PCP + CFI = 20 bits). But + * we can only get on 2 bytes. We need to get TCI field (PCP, CFI & + * VID). */ + seq_index = swap16 (bitstream_direct_read (buffer, (6 * 2 + 2) * 8, + 16)); + /* Remove PCP & CFI. */ + seq_index &= (0xFFFF >> 4); + dbg_assert (seq_index < CONFIG_SEQ_CHECK_VLAN_MAX); + + /* Get real Ethernet type, located after SRC & DST (6 bytes each) and + * VLAN/IEEE_802.1Q fields (4 bytes). */ + ethertype = swap16 (bitstream_direct_read (buffer, (6 + 6 + 4) * 8, + 2 * 8)); + } + + /* If this is the expected ethertype. */ + if (lib_seq_check_config.ethertype + && ethertype == lib_seq_check_config.ethertype) + { + + /* Get sequence value. */ + dbg_assert (lib_seq_check_config.seq_size == 16); + uint received_seq = swap16 ( + bitstream_direct_read (buffer, lib_seq_check_config.seq_pos * 8, + lib_seq_check_config.seq_size)); + + /* Handle overflow. */ + ctx->seq[seq_index] &= BITS_ONES (lib_seq_check_config.seq_size); + + /* Check. */ + if (ctx->seq[seq_index] != received_seq) + { + /* Call callback. */ + dbg_assert (ctx->cb); + (*ctx->cb) (ctx->cb_user, seq_index, ctx->seq[seq_index], + received_seq); + /* Reset. */ + ctx->seq[seq_index] = received_seq; + result = true; + } + + /* Increment. */ + ctx->seq[seq_index]++; + } + + /* Return result. */ + return result; +} -- cgit v1.2.3