summaryrefslogtreecommitdiff
path: root/cesar/tools
diff options
context:
space:
mode:
Diffstat (limited to 'cesar/tools')
-rw-r--r--cesar/tools/sniffer_phy/src/mme.c78
1 files changed, 78 insertions, 0 deletions
diff --git a/cesar/tools/sniffer_phy/src/mme.c b/cesar/tools/sniffer_phy/src/mme.c
index 4a84c05b53..5efe5346b4 100644
--- a/cesar/tools/sniffer_phy/src/mme.c
+++ b/cesar/tools/sniffer_phy/src/mme.c
@@ -18,6 +18,7 @@
#include "lib/swap.h"
#include "common/defs/homeplugAV.h"
#include "common/defs/ethernet.h"
+#include "common/defs/spidcom.h"
#include <setjmp.h>
@@ -25,6 +26,12 @@
#include "cp/mme.h"
+/** Sniffer mode: frame control RX. */
+#define MME_SNIFFER_MODE_RX_FC 0x20
+/** All supported sniffer mode. */
+#define MME_SNIFFER_MODE_SUPPORTED \
+ (MME_SNIFFER_MODE_RX_FC)
+
/** Data about a MME buffer. */
struct mme_buffer_t
{
@@ -80,6 +87,8 @@ mme_send_prepare (sniffer_phy_t *ctx, mme_buffer_t *mme, mac_t da,
bitstream_write (&mme->bitstream, HPAV_MMV1, 8);
bitstream_write (&mme->bitstream, mmtype, 16);
bitstream_write (&mme->bitstream, 0, 16);
+ if (mmtype >= VS_MIN && mmtype <= VS_MAX)
+ bitstream_write (&mme->bitstream, SPC_OUI, OUI_SIZE_BITS);
return true;
}
else
@@ -142,6 +151,66 @@ mme_handle_drv_sta_set_mac_addr_req (sniffer_phy_t *ctx, mme_buffer_t *mme)
}
/**
+ * Send a VS_SNIFFER.CNF.
+ * \param ctx sniffer context
+ * \param mme mme data
+ * \param result result byte
+ */
+static void
+mme_send_vs_sniffer_cnf (sniffer_phy_t *ctx, mme_buffer_t *mme, u8 result)
+{
+ mme_buffer_t cnf;
+ dbg_assert (ctx);
+ dbg_assert (mme);
+ /* Prepare header. */
+ if (mme_send_prepare (ctx, &cnf, mme->sa, VS_SNIFFER + CP_MME_CNF))
+ {
+ /* Write MME. */
+ bitstream_write (&cnf.bitstream, result, 8);
+ bitstream_write (&cnf.bitstream,
+ ctx->mme.mac_sniffer ? MME_SNIFFER_MODE_RX_FC : 0,
+ 8);
+ bitstream_write_large (&cnf.bitstream, ctx->mme.mac_sniffer, 48);
+ /* Send MME. */
+ mme_send (ctx, &cnf);
+ }
+}
+
+/**
+ * Handle a VS_SNIFFER.REQ.
+ * \param ctx sniffer context
+ * \param mme mme data
+ */
+static void
+mme_handle_vs_sniffer_req (sniffer_phy_t *ctx, mme_buffer_t *mme)
+{
+ dbg_assert (ctx);
+ dbg_assert (mme);
+ u8 result = 1;
+ do
+ {
+ /* Read command. */
+ u8 command = bitstream_read (&mme->bitstream, 8);
+ /* Check command is valid. */
+ if (command & ~MME_SNIFFER_MODE_SUPPORTED)
+ break;
+ /* If sniffer activated, accept MME only from current sniffing
+ * host. */
+ if (ctx->mme.mac_sniffer && ctx->mme.mac_sniffer != mme->sa)
+ break;
+ /* OK, accept command. */
+ result = 0;
+ bool was_activated = ctx->mme.mac_sniffer != 0;
+ bool is_activated = command != 0;
+ if (was_activated != is_activated)
+ lowlevel_activate (ctx, is_activated);
+ ctx->mme.mac_sniffer = is_activated ? mme->sa : 0;
+ } while (0);
+ /* Send feedback. */
+ mme_send_vs_sniffer_cnf (ctx, mme, result);
+}
+
+/**
* Decode and dispatch an MME.
* \param ctx sniffer context
* \param mme mme data
@@ -168,6 +237,12 @@ mme_handle_decode (sniffer_phy_t *ctx, mme_buffer_t *mme)
u16 fmi = bitstream_read (&mme->bitstream, 16);
if (fmi != 0)
return;
+ if (mmtype >= VS_MIN && mmtype <= VS_MAX)
+ {
+ u32 oui = bitstream_read (&mme->bitstream, OUI_SIZE_BITS);
+ if (oui != SPC_OUI)
+ return;
+ }
/* Dispatch. */
switch (mmtype)
{
@@ -187,6 +262,9 @@ mme_handle_decode (sniffer_phy_t *ctx, mme_buffer_t *mme)
/* Acknowledge, but ignore. */
mme_send_drv_any_cnf (ctx, mme, mmtype, 0);
break;
+ case VS_SNIFFER + CP_MME_REQ:
+ mme_handle_vs_sniffer_req (ctx, mme);
+ break;
}
}
}