beacon_stuffing.c

Go to the documentation of this file.

/*
 * Copyright 2024 Morse Micro
 *
 * SPDX-License-Identifier: Apache-2.0
 */
 
#include <string.h>
#include <endian.h>
#include "mmhal.h"
#include "mmosal.h"
#include "mmwlan.h"
#include "mmconfig.h"
#include "mmutils.h"
 
#include "mm_app_common.h"
 
/* Application default configurations. */
#define DEFAULT_OUI {0x4D, 0x4D, 0x42};
#define DEFAULT_LOG_LEN_BYTES 30
#define DEFAULT_STATS_UPDATE_PERIOD_MS 1000
 
 
#define FILTER_INDEX 0
#define RGB_DATA_LENGTH_BYTES 3
 
struct mmwlan_beacon_vendor_ie_filter filter;
 
struct beacon_vendor_ie_stat
{
    uint32_t occurrences;
    uint8_t *ie_data;
    uint32_t ie_len;
};
 
static void beacon_vendor_ie_callback(const uint8_t *ies, uint32_t ies_len, void *arg)
{
    int offset;
    uint8_t length;
    struct beacon_vendor_ie_stat *stat = (struct beacon_vendor_ie_stat *)arg;
 
    offset = mm_find_vendor_specific_ie(ies, ies_len, filter.ouis[FILTER_INDEX], MMWLAN_OUI_SIZE);
    if (offset > 0)
    {
        stat->occurrences++;
        /* Note that we rely on mm_find_vendor_specific_ie() to validate that the IE does not
         * extend past the end of the given buffer. */
        length = ies[offset + 1];
        offset += 2;
 
        if (stat->ie_data && (stat->ie_len != length))
        {
            mmosal_free(stat->ie_data);
            stat->ie_data = NULL;
            stat->ie_len = 0;
        }
 
        if (!stat->ie_data)
        {
            stat->ie_data = (uint8_t *)mmosal_malloc(length);
            if (!stat->ie_data)
            {
                /* Generally we would want to avoid operations that might take an extended amount of
                 * time (i.e printf()). This has just been put in to try and make it as clear as
                 * possible what the error might be for the purposes of the example application. */
                printf("Failed to alloc memory for ie data.");
                return;
            }
            stat->ie_len = length;
        }
 
        memcpy(stat->ie_data, (ies + offset), length);
 
        if (stat->ie_len >= (RGB_DATA_LENGTH_BYTES + MMWLAN_OUI_SIZE))
        {
            const uint8_t *rgb_data = stat->ie_data + MMWLAN_OUI_SIZE;
            mmhal_set_led(LED_RED, rgb_data[0]);
            mmhal_set_led(LED_GREEN, rgb_data[1]);
            mmhal_set_led(LED_BLUE, rgb_data[2]);
        }
    }
}
 
static void beacon_vendor_ie_stat_log(struct beacon_vendor_ie_stat *stat, uint32_t max_data_log_len)
{
    uint32_t ii;
    printf("%11lu | %14lu | ", stat->occurrences, stat->ie_len);
    if (stat->ie_data)
    {
        for (ii = 0; (ii < stat->ie_len) && (ii < max_data_log_len); ii++)
        {
            printf("%02x", stat->ie_data[ii]);
        }
 
        printf("%s\n", (stat->ie_len >= max_data_log_len) ? "..." : "");
    }
    else
    {
        printf("N/A \n");
    }
}
 
void app_init(void)
{
    enum mmwlan_status status;
    struct beacon_vendor_ie_stat stat = {0};
    uint8_t mmconfig_oui[MMWLAN_OUI_SIZE];
 
    uint8_t oui[MMWLAN_OUI_SIZE] = DEFAULT_OUI;
    uint32_t log_length_bytes = DEFAULT_LOG_LEN_BYTES;
    uint32_t update_period_ms = DEFAULT_STATS_UPDATE_PERIOD_MS;
 
    /* Read out any params from configstore if they exist. If they don't the variables will retain
     * their current values. */
    mmconfig_read_uint32("beacon_stuffing.log_len_bytes", &log_length_bytes);
    mmconfig_read_uint32("beacon_stuffing.log_period_ms", &update_period_ms);
 
    int mmconfig_oui_len = mmconfig_read_bytes("beacon_stuffing.oui", &mmconfig_oui,
                                               MMWLAN_OUI_SIZE, 0);
    if (mmconfig_oui_len >= MMWLAN_OUI_SIZE)
    {
        memcpy(&oui, &mmconfig_oui, MMWLAN_OUI_SIZE);
    }
 
    printf("\n\nMorse Beacon Stuffing Demo (Built " __DATE__ " " __TIME__ ")\n\n");
 
    /* Initialize and connect to Wi-Fi, blocks till connected */
    app_wlan_init();
    app_wlan_start();
 
    memcpy(filter.ouis[FILTER_INDEX], &oui, MMWLAN_OUI_SIZE);
    filter.n_ouis = 1;
    filter.cb = beacon_vendor_ie_callback;
    filter.cb_arg = (void *)&stat;
 
    status = mmwlan_update_beacon_vendor_ie_filter(&filter);
    if (status != MMWLAN_SUCCESS)
    {
        printf("Failed to register beacon vendor ie filter.\n");
        return;
    }
 
    printf("\nFiltering for Vendor Specific IE with OUI: 0x%02x%02x%02x\n", oui[0], oui[1], oui[2]);
 
    printf("Occurrences | Length (bytes) | Data (hex)\n");
    while (1)
    {
        mmosal_task_sleep(update_period_ms);
        beacon_vendor_ie_stat_log(&stat, log_length_bytes);
    }
}