/*
  <:copyright-BRCM:2016:DUAL/GPL:standard

  Broadcom Proprietary and Confidential.(c) 2016 Broadcom
  All Rights Reserved

  Unless you and Broadcom execute a separate written software license
  agreement governing use of this software, this software is licensed
  to you under the terms of the GNU General Public License version 2
  (the "GPL"), available at http://www.broadcom.com/licenses/GPLv2.php,
  with the following added to such license:

  As a special exception, the copyright holders of this software give
  you permission to link this software with independent modules, and
  to copy and distribute the resulting executable under terms of your
  choice, provided that you also meet, for each linked independent
  module, the terms and conditions of the license of that module.
  An independent module is a module which is not derived from this
  software.  The special exception does not apply to any modifications
  of the software.

  Not withstanding the above, under no circumstances may you combine
  this software in any way with any other Broadcom software provided
  under a license other than the GPL, without Broadcom's express prior
  written consent.

  :>
*/

#include "dpoe_sec_fsm.h"
#include "dpoe_sec_util.h"
#include "bcmos_hash_table.h"
#include "bcmolt_epon_oam_types.h"
#include "bcmolt_utils.h"
#include "bcmolt_user_appl_epon_oam.h"
#include "bcmolt_api.h"

#define DPOE_SEC_FSM_MAX_LINKS 64
#define DPOE_SEC_FSM_TASK_MSG_Q_SIZE 64

static const uint16_t DPOE_SEC_FSM_DEFAULT_OAM_KEY_EXCHANGE_PERIOD = 600; /* 10 minutes */
/**
 * @brief  Maximum ONU Authentication timeout value
 * DPoE specification indicates that this value is 5 minutes.
 */
static const uint32_t DPOE_SEC_AUTH_TIMEOUT = 5 * 60 * 1000 * 1000;

typedef struct
{
    bcmos_task task;
    char task_name[MAX_TASK_NAME_SIZE];
    char msg_queue_name[MAX_MSG_QUEUE_NAME_SIZE];
    hash_table *link_map;
} dpoe_sec_fsm_ctxt;

dpoe_sec_fsm_ctxt ctxt[BCMTR_MAX_OLTS];

typedef void (*f_dpoe_sec_fsm)(dpoe_sec_link_rec*, dpoe_sec_fsm_event_data*);

/* DPoE Security FSM Event type to string definitions used for debug output */
static const char *dpoe_sec_fsm_event_name[DPOE_SEC_FSM_EVENT__COUNT] =
{
    [DPOE_SEC_FSM_EVENT_ENCRYPT_START] = "Start Encryption",
    [DPOE_SEC_FSM_EVENT_AUTH_START] = "Start Authentication",
    [DPOE_SEC_FSM_EVENT_RX_OAM] = "Receive OAM",
    [DPOE_SEC_FSM_EVENT_RX_EAPOL] = "Receive EAPOL",
    [DPOE_SEC_FSM_EVENT_AUTH_COMPLETE] = "Authentication Complete",
    [DPOE_SEC_FSM_EVENT_MKA_COMPLETE] = "MKA Complete",
    [DPOE_SEC_FSM_EVENT_TIMEOUT] = "Timeout"
};


/* DPoE Security FSM State to string definitions used for debug output */
static const char *dpoe_sec_fsm_state_name[DPOE_SEC_FSM_STATE__COUNT] =
{
    [DPOE_SEC_FSM_STATE_IDLE] = "Idle",
    [DPOE_SEC_FSM_STATE_AUTH_WAIT] = "Authentication in progress",
    [DPOE_SEC_FSM_STATE_ENCRYPT_DOWN_WAIT] = "OAM Key Exchange in progress",
    [DPOE_SEC_FSM_STATE_ENCRYPT_BI_WAIT] = "MKA in progress",
    [DPOE_SEC_FSM_STATE_COMPLETE] = "Authentication/Encryption Complete"
};

static f_dpoe_sec_fsm_cb _fsm_complete;

static bcmos_bool _dpoe_sec_fsm_event_type_is_valid(dpoe_sec_fsm_event type)
{
    return type < DPOE_SEC_FSM_EVENT__COUNT;
}

static bcmos_bool _dpoe_sec_fsm_state_is_valid(dpoe_sec_fsm_state state)
{
    return state < DPOE_SEC_FSM_STATE__COUNT;
}

static const char *_dpoe_sec_fsm_event_name(dpoe_sec_fsm_event type)
{
    if (_dpoe_sec_fsm_event_type_is_valid(type))
    {
        return dpoe_sec_fsm_event_name[type];
    }
    else
    {
        return "Unknown Event";
    }
}

static const char *_dpoe_sec_fsm_state_name(dpoe_sec_fsm_state state)
{
    if (_dpoe_sec_fsm_state_is_valid(state))
    {
        return dpoe_sec_fsm_state_name[state];
    }
    else
    {
        return "Unknown State";
    }
}

static void _dpoe_sec_fsm_complete(dpoe_sec_link_rec *link_rec)
{
    /* Parameter checks */
    BUG_ON(link_rec == NULL);

    if (_fsm_complete != NULL)
    {
        _fsm_complete(link_rec->device, link_rec->key, link_rec->status);
    }
}

static void _dpoe_sec_fsm_success(dpoe_sec_link_rec *link_rec)
{
    /* Parameter checks */
    BUG_ON(link_rec == NULL);

    link_rec->fsm_state = DPOE_SEC_FSM_STATE_COMPLETE;
    link_rec->status = BCM_ERR_OK;

    switch (link_rec->cfg.enc_mode)
    {
        case BCMOLT_EPON_OAM_DPOE_ENCRYPTION_MODE_NONE:
        case BCMOLT_EPON_OAM_DPOE_ENCRYPTION_MODE_TEN_BI:
            break;
        case BCMOLT_EPON_OAM_DPOE_ENCRYPTION_MODE_ONE_DOWN:
        case BCMOLT_EPON_OAM_DPOE_ENCRYPTION_MODE_TEN_DOWN:
            bcmos_timer_start(
                &link_rec->timeout,
                (DPOE_SEC_FSM_DEFAULT_OAM_KEY_EXCHANGE_PERIOD * 1000 * 1000) + DPOE_SEC_OAM_TIMEOUT);
            break;
        default:
            BUG();
    }

    _dpoe_sec_fsm_complete(link_rec);
}

static void _dpoe_sec_fsm_stop(dpoe_sec_link_rec *link_rec, bcmos_errno reason)
{
    /* Parameter checks */
    BUG_ON(link_rec == NULL);

    link_rec->fsm_state = DPOE_SEC_FSM_STATE_IDLE;
    link_rec->status = reason;

    bcmos_timer_stop(&link_rec->timeout);

    /* Cleanup any EAP-TLS state. */
    dpoe_eap_tls_cleanup(link_rec);

    _dpoe_sec_fsm_complete(link_rec);
}

static void _dpoe_sec_fsm_auth_start(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    bcmos_errno rc;

    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    /* Start ONU Authentication */
    rc = dpoe_eap_tls_send_start(link_rec);
    if (BCM_ERR_OK != rc)
    {
        DPOE_SEC_LINK_LOG(ERROR, link_rec, "Failed to start authentication: %s (%d)\n", bcmos_strerror(rc), rc);
        _dpoe_sec_fsm_stop(link_rec, rc);
    }
    else
    {
        link_rec->fsm_state = DPOE_SEC_FSM_STATE_AUTH_WAIT;
        bcmos_timer_start(&link_rec->timeout, DPOE_SEC_AUTH_TIMEOUT);
        DPOE_SEC_LINK_LOG(DEBUG, link_rec, "Starting authentication\n");
    }
}

static void _dpoe_sec_fsm_mka_start(dpoe_sec_link_rec *link_rec)
{
    bcmos_errno rc = dpoe_sec_mka_fsm_start(link_rec);
    if (BCM_ERR_OK != rc)
    {
        DPOE_SEC_LINK_LOG(ERROR, link_rec, "Failed to start MKA: %s (%d)\n", bcmos_strerror(rc), rc);
        _dpoe_sec_fsm_stop(link_rec, rc);
    }
    else
    {
        link_rec->fsm_state = DPOE_SEC_FSM_STATE_ENCRYPT_BI_WAIT;
        DPOE_SEC_LINK_LOG(DEBUG, link_rec, "Starting MKA\n");
    }
}

static void _dpoe_sec_fsm_encrypt_start(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    bcmos_errno rc;

    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    if (link_rec->cfg.enc_mode == BCMOLT_EPON_OAM_DPOE_ENCRYPTION_MODE_TEN_BI)
    {
        _dpoe_sec_fsm_mka_start(link_rec);
    }
    else
    {
        rc = epon_oam_dpoe_set_encryption(
            link_rec->device,
            link_rec->key.epon_ni,
            &link_rec->key.mac_address,
            link_rec->cfg.enc_mode,
            DPOE_SEC_FSM_DEFAULT_OAM_KEY_EXCHANGE_PERIOD);
        if (BCM_ERR_OK != rc)
        {
            DPOE_SEC_LINK_LOG(ERROR, link_rec, "Failed to set encryption: %s (%d)\n", bcmos_strerror(rc), rc);
            _dpoe_sec_fsm_stop(link_rec, rc);
        }
        else
        {
            link_rec->fsm_state = DPOE_SEC_FSM_STATE_ENCRYPT_DOWN_WAIT;
            bcmos_timer_start(&link_rec->timeout, DPOE_SEC_OAM_TIMEOUT);
        }
    }
}

static void _dpoe_sec_fsm_auth_rx_eapol(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    bcmos_errno rc;

    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    rc = dpoe_eap_tls_process_eapol_pkt(link_rec, evt->data.rx_frame.val, evt->data.rx_frame.len);
    if ((BCM_ERR_OK != rc) && (BCM_ERR_IN_PROGRESS != rc))
    {
        DPOE_SEC_LINK_LOG(ERROR, link_rec, "Error processing EAPOL packet: %s (%d)\n", bcmos_strerror(rc), rc);
        _dpoe_sec_fsm_stop(link_rec, rc);
    }
}

static void _dpoe_sec_fsm_auth_complete(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    bcmos_timer_stop(&link_rec->timeout);
    if (BCM_ERR_OK == evt->data.sub_status)
    {
        if (link_rec->cfg.enc_mode == BCMOLT_EPON_OAM_DPOE_ENCRYPTION_MODE_TEN_BI)
        {
            _dpoe_sec_fsm_mka_start(link_rec);
        }
        else
        {
            _dpoe_sec_fsm_success(link_rec);
        }
    }
    else
    {
        _dpoe_sec_fsm_stop(link_rec, evt->data.sub_status);
    }
}

static bcmos_errno _dpoe_sec_fsm_install_dn_key(
    dpoe_sec_link_rec *link_rec,
    bcmolt_epon_oam_dpoe_vendor_extended_base *dpoe)
{
    bcmolt_encryption_information_container key_info;

    if (BCMOLT_EPON_OAM_DPOE_ENCRYPTION_MODE_ONE_DOWN == link_rec->cfg.enc_mode)
    {
        key_info.format = BCMOLT_EPON_ENCRYPTION_INFORMATION_FORMAT_CFB;
        memcpy(key_info.u.cfb.key, dpoe->u.key_exchange.key_data, sizeof(key_info.u.cfb.key));
    }
    else
    {
        key_info.format = BCMOLT_EPON_ENCRYPTION_INFORMATION_FORMAT_CTR;
        memcpy(key_info.u.ctr.key, dpoe->u.key_exchange.key_data, sizeof(key_info.u.ctr.key));
        memcpy(key_info.u.ctr.sci, link_rec->ni_mac.u8, BCMOS_ETH_ALEN);
        key_info.u.ctr.sci[6] = (link_rec->llid >> 8) & 0xff;
        key_info.u.ctr.sci[7] = (link_rec->llid >> 0) & 0xff;
    }

    return bcmolt_epon_link_encryption_key_set(
        link_rec->device,
        &link_rec->key,
        BCMOS_FALSE,
        BCMOLT_EPON_ENCRYPTION_MODE_EPON_ZERO_OVERHEAD_AES,
        (bcmolt_epon_key_choice)dpoe->u.key_exchange.key_number,
        &key_info);
}

static void _dpoe_sec_fsm_enc_dn_rx_oam(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    bcmolt_epon_oam_ethernet_frame *oam_frame;
    bcmolt_epon_oam_dpoe_vendor_extended_base *dpoe;

    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    oam_frame = epon_oam_unpack(link_rec->device, evt->data.rx_frame.len, evt->data.rx_frame.val);
    if (NULL == oam_frame)
    {
        DPOE_SEC_LINK_LOG(WARNING, link_rec, "Failed to parse OAM frame\n");
        return;
    }

    if (!epon_oam_is_dpoe(oam_frame))
    {
        bcmos_free(oam_frame);
        return;
    }

    dpoe = &oam_frame->protocols[0].u.slow_protocol.value.u.oam.content.u.organization_specific.value.u.dpoe.value;

    if (epon_oam_is_dpoe_encrypt_response(dpoe))
    {
        if ((dpoe->u.set_response.vars[0].u.extended_attribute.attribute.width == 0x80) &&
            (dpoe->u.set_response.vars[1].u.extended_attribute.attribute.width == 0x80))
        {
            bcmos_timer_start(&link_rec->timeout, DPOE_SEC_OAM_TIMEOUT); /* restart timer to wait for key */
        }
        else
        {
            DPOE_SEC_LINK_LOG(ERROR, link_rec, "ONU rejected encryption: [0] = 0x%x, [1] = 0x%x\n",
                              dpoe->u.set_response.vars[0].u.extended_attribute.attribute.width,
                              dpoe->u.set_response.vars[1].u.extended_attribute.attribute.width);
            _dpoe_sec_fsm_stop(link_rec, BCM_ERR_ONU_ERR_RESP);
        }
    }
    else if (BCMOLT_EPON_OAM_DPOE_OPCODE_KEY_EXCHANGE == dpoe->op)
    {
        _dpoe_sec_fsm_install_dn_key(link_rec, dpoe);
        if (link_rec->cfg.auth)
        {
            _dpoe_sec_fsm_auth_start(link_rec, evt);
        }
        else
        {
            _dpoe_sec_fsm_success(link_rec);
        }
    }
    else
    {
        /* ignore other OAM */
    }

    bcmos_free(oam_frame);
}

static void _dpoe_sec_fsm_enc_bi_rx_oam(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    dpoe_sec_mka_fsm_rx_oam(link_rec, evt->data.rx_frame.val, evt->data.rx_frame.len);
}

static void _dpoe_sec_fsm_enc_bi_rx_eapol(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    dpoe_sec_mka_fsm_rx_eapol(link_rec, evt->data.rx_frame.val, evt->data.rx_frame.len);
}

static void _dpoe_sec_fsm_mka_complete(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    if (BCM_ERR_OK == evt->data.sub_status)
    {
        _dpoe_sec_fsm_success(link_rec);
    }
    else
    {
        _dpoe_sec_fsm_stop(link_rec, evt->data.sub_status);
    }
}

static void _dpoe_sec_fsm_comp_rx_oam(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    if ((link_rec->cfg.enc_mode == BCMOLT_EPON_OAM_DPOE_ENCRYPTION_MODE_ONE_DOWN) ||
        (link_rec->cfg.enc_mode == BCMOLT_EPON_OAM_DPOE_ENCRYPTION_MODE_TEN_DOWN))
    {
        bcmolt_epon_oam_ethernet_frame *oam_frame;
        bcmolt_epon_oam_dpoe_vendor_extended_base *dpoe;

        oam_frame = epon_oam_unpack(link_rec->device, evt->data.rx_frame.len, evt->data.rx_frame.val);
        if (NULL == oam_frame)
        {
            DPOE_SEC_LINK_LOG(WARNING, link_rec, "Failed to parse OAM frame\n");
            return;
        }

        if (!epon_oam_is_dpoe(oam_frame))
        {
            bcmos_free(oam_frame);
            return;
        }

        dpoe = &oam_frame->protocols[0].u.slow_protocol.value.u.oam.content.u.organization_specific.value.u.dpoe.value;

        if (BCMOLT_EPON_OAM_DPOE_OPCODE_KEY_EXCHANGE == dpoe->op)
        {
            _dpoe_sec_fsm_install_dn_key(link_rec, dpoe);
        }
    }
}

static void _dpoe_sec_fsm_comp_rx_eapol(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    if (link_rec->cfg.enc_mode == BCMOLT_EPON_OAM_DPOE_ENCRYPTION_MODE_TEN_BI)
    {
        dpoe_sec_mka_fsm_rx_eapol(link_rec, evt->data.rx_frame.val, evt->data.rx_frame.len);
    }
}

static void _dpoe_sec_fsm_timeout(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    _dpoe_sec_fsm_stop(link_rec, BCM_ERR_TIMEOUT);
}

static f_dpoe_sec_fsm dpoe_sec_fsm[DPOE_SEC_FSM_STATE__COUNT][DPOE_SEC_FSM_EVENT__COUNT] =
{
    [DPOE_SEC_FSM_STATE_IDLE] =
    {
        [DPOE_SEC_FSM_EVENT_AUTH_START] = _dpoe_sec_fsm_auth_start,
        [DPOE_SEC_FSM_EVENT_ENCRYPT_START] = _dpoe_sec_fsm_encrypt_start
    },
    [DPOE_SEC_FSM_STATE_AUTH_WAIT] =
    {
        [DPOE_SEC_FSM_EVENT_RX_EAPOL] = _dpoe_sec_fsm_auth_rx_eapol,
        [DPOE_SEC_FSM_EVENT_AUTH_COMPLETE] = _dpoe_sec_fsm_auth_complete,
        [DPOE_SEC_FSM_EVENT_TIMEOUT] = _dpoe_sec_fsm_timeout
    },
    [DPOE_SEC_FSM_STATE_ENCRYPT_DOWN_WAIT] =
    {
        [DPOE_SEC_FSM_EVENT_RX_OAM] = _dpoe_sec_fsm_enc_dn_rx_oam,
        [DPOE_SEC_FSM_EVENT_TIMEOUT] = _dpoe_sec_fsm_timeout
    },
    [DPOE_SEC_FSM_STATE_ENCRYPT_BI_WAIT] =
    {
        [DPOE_SEC_FSM_EVENT_RX_OAM] = _dpoe_sec_fsm_enc_bi_rx_oam,
        [DPOE_SEC_FSM_EVENT_RX_EAPOL] = _dpoe_sec_fsm_enc_bi_rx_eapol,
        [DPOE_SEC_FSM_EVENT_MKA_COMPLETE] = _dpoe_sec_fsm_mka_complete,
        [DPOE_SEC_FSM_EVENT_TIMEOUT] = _dpoe_sec_fsm_timeout
    },
    [DPOE_SEC_FSM_STATE_COMPLETE] =
    {
        [DPOE_SEC_FSM_EVENT_RX_OAM] = _dpoe_sec_fsm_comp_rx_oam,
        [DPOE_SEC_FSM_EVENT_RX_EAPOL] = _dpoe_sec_fsm_comp_rx_eapol,
        [DPOE_SEC_FSM_EVENT_TIMEOUT] = _dpoe_sec_fsm_timeout
    }
};

static void _dpoe_sec_fsm_unexpected(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    DPOE_SEC_LINK_LOG(WARNING, link_rec, "Unexpected event %s (%u) in state %s (%u)\n",
                      _dpoe_sec_fsm_event_name(evt->type), evt->type, _dpoe_sec_fsm_state_name(link_rec->fsm_state), link_rec->fsm_state);
}

static void _dpoe_sec_fsm_exec(dpoe_sec_link_rec *link_rec, dpoe_sec_fsm_event_data *evt)
{
    /* Parameter checks */
    BUG_ON(link_rec == NULL);
    BUG_ON(evt == NULL);

    if (evt->type < DPOE_SEC_FSM_EVENT__COUNT)
    {
        dpoe_sec_fsm_state pre_state = link_rec->fsm_state;

        if (dpoe_sec_fsm[link_rec->fsm_state][evt->type] != NULL)
        {
            DPOE_SEC_LINK_LOG(DEBUG, link_rec, "processing event %s in state %s\n",
                              _dpoe_sec_fsm_event_name(evt->type), _dpoe_sec_fsm_state_name(link_rec->fsm_state));
            dpoe_sec_fsm[link_rec->fsm_state][evt->type](link_rec, evt);
        }
        else
        {
            _dpoe_sec_fsm_unexpected(link_rec, evt);
        }

        if (link_rec->fsm_state != pre_state)
        {
            DPOE_SEC_LINK_LOG(DEBUG, link_rec, "transition from %s to %s\n",
                              _dpoe_sec_fsm_state_name(pre_state), _dpoe_sec_fsm_state_name(link_rec->fsm_state));
        }
    }
    else
    {
        DPOE_SEC_LINK_LOG(ERROR, link_rec, "Unknown event: %u", evt->type);
    }
}

static bcmos_timer_rc _dpoe_sec_fsm_link_timer_expired(bcmos_timer *timer, long data)
{
    dpoe_sec_link_rec *link_rec = (dpoe_sec_link_rec*)data;
    dpoe_sec_fsm_event_data evt;

    evt.type = DPOE_SEC_FSM_EVENT_TIMEOUT;

    _dpoe_sec_fsm_exec(link_rec, &evt);

    return BCMOS_TIMER_OK;
}

static void _dpoe_sec_fsm_link_start_handle(bcmos_module_id module_id, bcmos_msg *os_msg)
{
    dpoe_sec_fsm_msg *msg = (dpoe_sec_fsm_msg*)os_msg;
    bcmolt_devid device = os_msg->instance;
    dpoe_sec_link_rec *link_rec;
    dpoe_sec_fsm_event_data evt;
    bcmos_errno err;

    link_rec = hash_table_get(ctxt[device].link_map, (const uint8_t*)&msg->link);
    if (link_rec == NULL)
    {
        dpoe_sec_link_rec new_link;
        bcmolt_epon_ni_cfg pon_cfg;
        bcmolt_epon_link_cfg link_cfg;
        bcmolt_epon_ni_key pon_key = { .epon_ni = msg->link.epon_ni };
        bcmos_timer_parm tp =
        {
            .name = "dpoe_sec_fsm_timer",
            .owner = BCMOS_MODULE_ID_USER_APPL_DPOE_SEC + device,
            .periodic = BCMOS_FALSE,
            .handler = _dpoe_sec_fsm_link_timer_expired,
            .flags = BCMOS_TIMER_PARM_FLAGS_NON_URGENT
        };

        /* retrieve the EPON NI MAC address */
        BCMOLT_CFG_INIT(&pon_cfg, epon_ni, pon_key);
        BCMOLT_CFG_PROP_GET(&pon_cfg, epon_ni, mac_address);
        err = bcmolt_cfg_get(device, &pon_cfg.hdr);
        BUG_ON(BCM_ERR_OK != err);

        BCMOLT_CFG_INIT(&link_cfg, epon_link, msg->link);
        BCMOLT_CFG_PROP_GET(&link_cfg, epon_link, llid);
        err = bcmolt_cfg_get(device, &link_cfg.hdr);
        if (BCM_ERR_OK != err)
        {
            BCM_LOG(ERROR, dpoe_sec_log_id[device], "Failed to get LLID\n");
        }

        new_link.device = device;
        new_link.key = msg->link;
        new_link.ni_mac = pon_cfg.data.mac_address;
        new_link.llid = link_cfg.data.llid;
        new_link.fsm_state = DPOE_SEC_FSM_STATE_IDLE;
        new_link.status = BCM_ERR_IN_PROGRESS;

        /* add new rec to hash table */
        link_rec = hash_table_put(ctxt[device].link_map, (uint8_t*)&msg->link, &new_link);
        if (link_rec == NULL)
        {
            DPOE_SEC_LINK_LOG(ERROR, &new_link, "Failed to store link record\n");
        }

        tp.data = (long)link_rec;
        err = bcmos_timer_create(&link_rec->timeout, &tp);
        BUG_ON(BCM_ERR_OK != err);
    }

    link_rec->cfg = msg->data.start;

    if (!link_rec->cfg.auth || (link_rec->cfg.enc_mode != BCMOLT_EPON_OAM_DPOE_ENCRYPTION_MODE_TEN_BI))
    {
        evt.type = DPOE_SEC_FSM_EVENT_ENCRYPT_START;
    }
    else
    {
        evt.type = DPOE_SEC_FSM_EVENT_AUTH_START;
    }

    _dpoe_sec_fsm_exec(link_rec, &evt);

    bcmos_free(os_msg);
}

static void _dpoe_sec_fsm_link_rx_oam_handle(bcmos_module_id module_id, bcmos_msg *os_msg)
{
    dpoe_sec_fsm_msg *msg = (dpoe_sec_fsm_msg*)os_msg;
    bcmolt_devid device = os_msg->instance;
    dpoe_sec_link_rec *link_rec;
    dpoe_sec_fsm_event_data evt;

    link_rec = hash_table_get(ctxt[device].link_map, (uint8_t*)&msg->link);
    if (link_rec == NULL)
    {
        BCM_LOG(DEBUG, dpoe_sec_log_id[device], "Failed to find link: PON %u, "BCMOS_MACADDR_FMT_STR"\n",
                msg->link.epon_ni, BCMOS_MACADDR_PARAMS(&msg->link.mac_address));
    }
    else
    {
        evt.type = DPOE_SEC_FSM_EVENT_RX_OAM;
        evt.data.rx_frame.val = msg->data.rx_frame.frame;
        evt.data.rx_frame.len = msg->data.rx_frame.length;
        _dpoe_sec_fsm_exec(link_rec, &evt);
    }

    bcmos_free(msg->data.rx_frame.frame);
    bcmos_free(os_msg);
}

static void _dpoe_sec_fsm_link_rx_eapol_handle(bcmos_module_id module_id, bcmos_msg *os_msg)
{
    dpoe_sec_fsm_msg *msg = (dpoe_sec_fsm_msg*)os_msg;
    bcmolt_devid device = os_msg->instance;
    dpoe_sec_link_rec *link_rec;
    dpoe_sec_fsm_event_data evt;

    link_rec = hash_table_get(ctxt[device].link_map, (uint8_t*)&msg->link);
    if (link_rec == NULL)
    {
        BCM_LOG(INFO, dpoe_sec_log_id[device], "Failed to find link: PON %u, "BCMOS_MACADDR_FMT_STR"\n",
                msg->link.epon_ni, BCMOS_MACADDR_PARAMS(&msg->link.mac_address));
    }
    else
    {
        evt.type = DPOE_SEC_FSM_EVENT_RX_EAPOL;
        evt.data.rx_frame.val = msg->data.rx_frame.frame;
        evt.data.rx_frame.len = msg->data.rx_frame.length;
        _dpoe_sec_fsm_exec(link_rec, &evt);
    }

    bcmos_free(os_msg);
}

static void _dpoe_sec_fsm_link_auth_complete(dpoe_sec_link_rec* link_rec, bcmos_errno status)
{
    dpoe_sec_fsm_event_data evt;

    evt.type = DPOE_SEC_FSM_EVENT_AUTH_COMPLETE;
    evt.data.sub_status = status;

    _dpoe_sec_fsm_exec(link_rec, &evt);
}

static void _dpoe_sec_fsm_link_mka_complete(dpoe_sec_link_rec* link_rec, bcmos_errno status)
{
    dpoe_sec_fsm_event_data evt;

    evt.type = DPOE_SEC_FSM_EVENT_MKA_COMPLETE;
    evt.data.sub_status = status;

    _dpoe_sec_fsm_exec(link_rec, &evt);
}

static bcmos_errno _dpoe_sec_fsm_init_task(bcmolt_devid device)
{
    bcmos_task_parm task_params =
    {
        .name         = ctxt[device].task_name,
        .priority     = TASK_PRIORITY_USER_APPL_DPOE_SEC,
        .core         = BCMOS_CPU_CORE_ANY, /* No CPU affinity */
        .init_handler = NULL
    };
    snprintf(ctxt[device].task_name, sizeof(ctxt[device].task_name), "user_appl_dpoe_sec%u", device);
    bcmos_errno rc = bcmos_task_create(&ctxt[device].task, &task_params);

    return rc;
}


static bcmos_errno _dpoe_sec_fsm_init_module(bcmolt_devid device)
{
    bcmos_module_parm module_params =
    {
        .qparm = { .name = ctxt[device].msg_queue_name, .size = DPOE_SEC_FSM_TASK_MSG_Q_SIZE },
        .init = NULL
    };
    snprintf(ctxt[device].msg_queue_name, sizeof(ctxt[device].msg_queue_name), "dpoe_sec_msg_q%u", device);
    return bcmos_module_create(BCMOS_MODULE_ID_USER_APPL_DPOE_SEC + device, &ctxt[device].task, &module_params);
}

static bcmos_errno _dpoe_sec_fsm_send_os_msg(
    bcmolt_devid device,
    bcmolt_epon_link_key link_key,
    bcmos_msg_id type,
    dpoe_sec_fsm_msg_data *data)
{
    bcmos_errno rc;
    dpoe_sec_fsm_msg *msg = bcmos_calloc(sizeof(*msg));
    if (msg == NULL)
    {
        BCM_LOG(ERROR, dpoe_sec_log_id[device], "OS Message calloc failed\n");
        return BCM_ERR_NOMEM;
    }

    msg->os_msg.instance = device;
    msg->os_msg.type = type;
    msg->link = link_key;
    msg->data = *data;
    rc = bcmos_msg_dispatch(&msg->os_msg, BCMOS_MSG_SEND_AUTO_FREE);
    if (BCM_ERR_OK != rc)
    {
        BCM_LOG(ERROR, dpoe_sec_log_id[device], "OS Message send failed (%s)\n", bcmos_strerror(rc));
    }

    return rc;
}

bcmos_errno dpoe_sec_fsm_link_start(bcmolt_devid device, bcmolt_epon_link_key link_key, dpoe_sec_fsm_start_data *cfg)
{
    dpoe_sec_fsm_msg_data data;

    data.start = *cfg;

    return _dpoe_sec_fsm_send_os_msg(device, link_key, BCMOS_MSG_ID_DPOE_SEC_START, &data);
}

bcmos_errno dpoe_sec_fsm_link_rx_oam(bcmolt_devid device, bcmolt_epon_link_key link_key, uint8_t *oam, uint16_t length)
{
    dpoe_sec_fsm_msg_data data;

    data.rx_frame.frame = oam;
    data.rx_frame.length = length;

    return _dpoe_sec_fsm_send_os_msg(device, link_key, BCMOS_MSG_ID_DPOE_SEC_RX_OAM, &data);
}

bcmos_errno dpoe_sec_fsm_link_rx_eapol(
    bcmolt_devid device,
    bcmolt_epon_link_key link_key,
    uint8_t *eapol,
    uint16_t length)
{
    dpoe_sec_fsm_msg_data data;

    data.rx_frame.frame = eapol;
    data.rx_frame.length = length;

    return _dpoe_sec_fsm_send_os_msg(device, link_key, BCMOS_MSG_ID_DPOE_SEC_RX_EAPOL, &data);
}

bcmos_errno dpoe_sec_fsm_init(
    f_dpoe_sec_cert_trust_cb cert_trust_cb,
    f_dpoe_sec_fsm_cb fsm_complete_cb)
{
    bcmos_errno rc;

    for (uint32_t device = 0; device < BCMTR_MAX_OLTS; device++)
    {
        rc = _dpoe_sec_fsm_init_task(device);
        BUG_ON(rc != BCM_ERR_OK);

        rc = _dpoe_sec_fsm_init_module(device);
        BUG_ON(rc != BCM_ERR_OK);

        char log_name[MAX_DEV_LOG_ID_NAME];
        snprintf(log_name, sizeof(log_name), "user_appl_dpoe_sec%u", device);
        dpoe_sec_log_id[device] = bcm_dev_log_id_register(log_name, DEV_LOG_LEVEL_INFO, DEV_LOG_ID_TYPE_BOTH);

        bcmos_msg_register(
            BCMOS_MSG_ID_DPOE_SEC_START,
            device,
            BCMOS_MODULE_ID_USER_APPL_DPOE_SEC + device,
            _dpoe_sec_fsm_link_start_handle);
        bcmos_msg_register(
            BCMOS_MSG_ID_DPOE_SEC_RX_OAM,
            device,
            BCMOS_MODULE_ID_USER_APPL_DPOE_SEC + device,
            _dpoe_sec_fsm_link_rx_oam_handle);
        bcmos_msg_register(
            BCMOS_MSG_ID_DPOE_SEC_RX_EAPOL,
            device,
            BCMOS_MODULE_ID_USER_APPL_DPOE_SEC + device,
            _dpoe_sec_fsm_link_rx_eapol_handle);

        ctxt[device].link_map = hash_table_create(
            DPOE_SEC_FSM_MAX_LINKS,
            sizeof(dpoe_sec_link_rec),
            sizeof(bcmolt_epon_link_key),
            "dpoe_sec_link_state");
        BUG_ON(ctxt[device].link_map == NULL);
    }

    _fsm_complete = fsm_complete_cb;

    dpoe_eap_tls_init(_dpoe_sec_fsm_link_auth_complete, cert_trust_cb);
    dpoe_sec_mka_fsm_init(_dpoe_sec_fsm_link_mka_complete);

    return rc;
}