summaryrefslogtreecommitdiff
path: root/cesar/tools/sniffer_phy
diff options
context:
space:
mode:
authorschodet2010-03-24 10:21:22 +0000
committerschodet2010-03-24 10:21:22 +0000
commit6b16302fe922eef7f5526c3f39150170fdaa737d (patch)
treeb7b4a08a98edd7d608d5c975dcd7fd837351473b /cesar/tools/sniffer_phy
parentf68d5aa4dddf67174885f5c3ea0be348f314ba69 (diff)
cesar/tools/sniffer_phy: add DRV MME implementation, refs #1256
git-svn-id: svn+ssh://pessac/svn/cesar/trunk@6841 017c9cb6-072f-447c-8318-d5b54f68fe89
Diffstat (limited to 'cesar/tools/sniffer_phy')
-rw-r--r--cesar/tools/sniffer_phy/src/mme.c195
1 files changed, 195 insertions, 0 deletions
diff --git a/cesar/tools/sniffer_phy/src/mme.c b/cesar/tools/sniffer_phy/src/mme.c
index f465ad2f3c..4a84c05b53 100644
--- a/cesar/tools/sniffer_phy/src/mme.c
+++ b/cesar/tools/sniffer_phy/src/mme.c
@@ -14,16 +14,211 @@
*/
#include "common/std.h"
+#include "lib/bitstream.h"
+#include "lib/swap.h"
+#include "common/defs/homeplugAV.h"
+#include "common/defs/ethernet.h"
+
+#include <setjmp.h>
+
#include "inc/context.h"
+#include "cp/mme.h"
+
+/** Data about a MME buffer. */
+struct mme_buffer_t
+{
+ /** Bitstream context. */
+ bitstream_t bitstream;
+ /** Source MAC address. */
+ mac_t sa;
+ /** Destination MAC address. */
+ mac_t da;
+ /** Longjmp context in case of underflow. */
+ jmp_buf jump_env;
+ /** Buffer address (for TX). */
+ u8 *buffer;
+};
+typedef struct mme_buffer_t mme_buffer_t;
+
void
mme_init (sniffer_phy_t *ctx)
{
+ dbg_assert (ctx);
+ /* Initialise context. */
+ ctx->mme.mac = 0;
+ ctx->mme.mac_sniffer = 0;
+}
+
+/**
+ * Prepare an MME buffer and fill header.
+ * \param ctx sniffer context
+ * \param mme mme data
+ * \param da destination MAC address
+ * \param mmtype MMTYPE
+ * \return true on success
+ */
+static bool
+mme_send_prepare (sniffer_phy_t *ctx, mme_buffer_t *mme, mac_t da,
+ u16 mmtype)
+{
+ dbg_assert (ctx);
+ dbg_assert (mme);
+ /* Initialise MME data. */
+ mme->da = da;
+ mme->sa = ctx->mme.mac;
+ /* Ask for a new buffer. */
+ mme->buffer = lhle_buffer_get (ctx);
+ /* Build header. */
+ if (mme->buffer)
+ {
+ bitstream_write_init (&mme->bitstream, mme->buffer,
+ ETH_PACKET_MAX_SIZE);
+ bitstream_write_large (&mme->bitstream, mme->da, 48);
+ bitstream_write_large (&mme->bitstream, mme->sa, 48);
+ bitstream_write (&mme->bitstream, swap16 (HPAV_MTYPE_MME), 16);
+ bitstream_write (&mme->bitstream, HPAV_MMV1, 8);
+ bitstream_write (&mme->bitstream, mmtype, 16);
+ bitstream_write (&mme->bitstream, 0, 16);
+ return true;
+ }
+ else
+ return false;
+}
+
+/**
+ * Finalise and send an MME.
+ * \param ctx sniffer context
+ * \param mme mme data
+ */
+static void
+mme_send (sniffer_phy_t *ctx, mme_buffer_t *mme)
+{
+ dbg_assert (ctx);
+ dbg_assert (mme);
+ uint bits = bitstream_write_finalise (&mme->bitstream);
+ lhle_send (ctx, mme->buffer, CEIL_DIV (bits, 8));
+}
+
+/**
+ * Send a DRV confirmation.
+ * \param ctx sniffer context
+ * \param mme mme data
+ * \param mmtype request MMTYPE
+ * \param result result byte
+ */
+static void
+mme_send_drv_any_cnf (sniffer_phy_t *ctx, mme_buffer_t *mme, u16 mmtype,
+ u8 result)
+{
+ mme_buffer_t cnf;
+ dbg_assert (ctx);
+ dbg_assert (mme);
+ /* Prepare header. */
+ if (mme_send_prepare (ctx, &cnf, mme->sa,
+ CP_MSG_MMTYPE_BASE (mmtype) + CP_MME_CNF))
+ {
+ /* Write result. */
+ bitstream_write (&cnf.bitstream, result, 8);
+ /* Send MME. */
+ mme_send (ctx, &cnf);
+ }
+}
+
+/**
+ * Handle a DRV_STA_SET_MAC_ADDR.REQ.
+ * \param ctx sniffer context
+ * \param mme mme data
+ */
+static void
+mme_handle_drv_sta_set_mac_addr_req (sniffer_phy_t *ctx, mme_buffer_t *mme)
+{
+ dbg_assert (ctx);
+ dbg_assert (mme);
+ /* Take mac address. */
+ ctx->mme.mac = bitstream_read_large (&mme->bitstream, 48);
+ /* Acknowledge. */
+ mme_send_drv_any_cnf (ctx, mme, DRV_STA_SET_MAC_ADDR_REQ, 0);
+}
+
+/**
+ * Decode and dispatch an MME.
+ * \param ctx sniffer context
+ * \param mme mme data
+ */
+static void
+mme_handle_decode (sniffer_phy_t *ctx, mme_buffer_t *mme)
+{
+ dbg_assert (ctx);
+ dbg_assert (mme);
+ /* Catch underflows. */
+ if (setjmp (mme->jump_env) == 0)
+ {
+ /* Decode MAC header. */
+ mme->da = bitstream_read_large (&mme->bitstream, 48);
+ mme->sa = bitstream_read_large (&mme->bitstream, 48);
+ u16 ethertype = bitstream_read (&mme->bitstream, 16);
+ if (ethertype != swap16 (HPAV_MTYPE_MME))
+ return;
+ /* Decode MME header. */
+ u8 mmv = bitstream_read (&mme->bitstream, 8);
+ if (mmv > HPAV_MMV_MAX)
+ return;
+ u16 mmtype = bitstream_read (&mme->bitstream, 16);
+ u16 fmi = bitstream_read (&mme->bitstream, 16);
+ if (fmi != 0)
+ return;
+ /* Dispatch. */
+ switch (mmtype)
+ {
+ case DRV_STA_SET_MAC_ADDR_REQ:
+ mme_handle_drv_sta_set_mac_addr_req (ctx, mme);
+ break;
+ case DRV_STA_SET_CCO_PREF_REQ:
+ case DRV_STA_SET_WAS_CCO_REQ:
+ case DRV_STA_SET_SL_REQ:
+ case DRV_STA_SET_M_STA_HFID_REQ:
+ case DRV_STA_SET_U_STA_HFID_REQ:
+ case DRV_STA_SET_AVLN_HFID_REQ:
+ case DRV_STA_SET_TONEMASK_REQ:
+ case DRV_STA_MAC_START_REQ:
+ case DRV_STA_SET_KEY_REQ:
+ case DRV_STA_SET_DAK_REQ:
+ /* Acknowledge, but ignore. */
+ mme_send_drv_any_cnf (ctx, mme, mmtype, 0);
+ break;
+ }
+ }
+}
+
+/**
+ * Called on bitstream underflow.
+ * \param bitstream bitstream context
+ * \param user_data mme data
+ */
+static void
+mme_handle_underflow (bitstream_t *bitstream, void *user_data)
+{
+ mme_buffer_t *mme = user_data;
+ dbg_assert (bitstream);
+ dbg_assert (mme);
+ /* Long jump to exit this trap. */
+ longjmp (mme->jump_env, -1);
}
void
mme_handle (sniffer_phy_t *ctx, const u8 *buffer, uint length)
{
+ dbg_assert (ctx);
+ dbg_assert (buffer);
+ dbg_assert (length >= ETH_PACKET_MIN_SIZE_ALLOWED);
+ /* Setup MME buffer. */
+ mme_buffer_t mme;
+ bitstream_read_init (&mme.bitstream, buffer, length);
+ bitstream_init_buffer_cb (&mme.bitstream, mme_handle_underflow, &mme);
+ mme.buffer = NULL;
+ /* Decode. */
+ mme_handle_decode (ctx, &mme);
}
void