/*
 <: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 <bcmos_system.h>
#include <bcmolt_api.h>
#include <bcmolt_model_types.h>
#include <bcmcli_session.h>
#include <bcmolt_msg_pack.h>
#include <bcm_dev_log.h>
#include <encrypt_oam.h>
#include <oam_defs.h>
#include "bcmolt_epon_hde.h"

#define HDE_TASK_MSG_Q_SIZE 64
#define ENC_KEY_LENGTH_KEY 16
#define HDE_MAX_NUM_LINKS 512
#define KEY_EXCHANGE_PERIOD 15

typedef enum
{
    HDE_EVENT_ENABLE_ENCRYPTION,
    HDE_EVENT_DISABLE_ENCRYPTION,
    HDE_EVENT_FRAME_RX,
    HDE_EVENT_OAM_TIMEOUT,
    HDE_EVENT__NUM_OF
} hde_event;

typedef enum
{
    HDE_STATE_NOT_ENCRYPTED,
    HDE_STATE_ENCRYPTED,
    HDE_STATE_WAITING_FOR_OAM_ACK,
    HDE_STATE__NUM_OF
} hde_state;

typedef struct
{
    bcmos_msg os_msg;
    bcmolt_msg *rx;
    hde_key key;
    hde_event event;
} hde_task_msg;

typedef struct
{
    hde_key hde_link;
    hde_state state;
    bcmos_timer oam_timeout_timer;
    bcmolt_epon_link_rate link_rate;
    bcmolt_epon_llid llid;
    bcmos_mac_address corresponding_pon_port_mac;
} hde_link_data;

static struct
{
    bcmos_task task;
    dev_log_id log_id[BCMTR_MAX_OLTS];
    hde_link_data *links;
    uint16_t num_links;
} hde_data;

static bcmos_bool is_running = BCMOS_FALSE;

typedef void (*link_state_handler)(hde_link_data *hde_link, const bcmolt_msg *ind);

static const char *get_state_string(hde_state state)
{
    switch (state)
    {
    case HDE_STATE_NOT_ENCRYPTED:
        return "not encrypted";
    case HDE_STATE_ENCRYPTED:
        return "encrypted";
    case HDE_STATE_WAITING_FOR_OAM_ACK:
        return "waiting for oam ack";
    default:
        return "UNKNOWN STATE!!!";
    }
}

static bcmos_errno set_new_key(const hde_key *hde_link, bcmolt_encryption_information_container *new_key, bcmolt_epon_key_choice key_choice, bcmolt_epon_encryption_mode mode)
{
    bcmolt_epon_link_cfg link_cfg;
    bcmolt_epon_encryption_config encryption_config = { };

    // Up/down format must match even when doing DS only
    encryption_config.upstream_encryption_information.format = new_key->format;

    encryption_config.downstream_encryption_information = *new_key;
    encryption_config.downstream_key_choice = key_choice;
    encryption_config.downstream_mode = mode;
    bcmolt_epon_link_key link_key =
    {
        .epon_ni = hde_link->epon_ni,
        .mac_address = hde_link->mac_addr
    };
    BCMOLT_CFG_INIT(&link_cfg, epon_link, link_key);
    BCMOLT_CFG_PROP_SET(&link_cfg, epon_link, epon_encryption, encryption_config);
    return bcmolt_cfg_set(hde_link->device_id, &link_cfg.hdr);
}

static bcmos_errno set_pon_encryption_if_needed(const hde_key *hde_link)
{
    // First check the current state of the PON's encryption mode.
    bcmos_errno rc;
    bcmolt_epon_ni_cfg pon_cfg;
    bcmolt_epon_ni_key pon_key = { .epon_ni = hde_link->epon_ni };
    BCMOLT_CFG_INIT(&pon_cfg, epon_ni, pon_key);
    BCMOLT_CFG_PROP_GET(&pon_cfg, epon_ni, encryption_cfg);
    rc = bcmolt_cfg_get(hde_link->device_id, &pon_cfg.hdr);
    if (rc != BCM_ERR_OK)
    {
        BCM_LOG(ERROR, hde_data.log_id[hde_link->device_id], "Failure - Cannot query PON encryption mode!.\n");
        return rc;
    }

    if (pon_cfg.data.encryption_cfg.downstream_encryption_mode != BCMOLT_EPON_ENCRYPTION_MODE_EPON_ZERO_OVERHEAD_AES)
    {
        // Encryption mode is not correctly set on the PON, so set it now.
        pon_cfg.data.encryption_cfg.downstream_encryption_mode = BCMOLT_EPON_ENCRYPTION_MODE_EPON_ZERO_OVERHEAD_AES;
        pon_cfg.data.encryption_cfg.upstream_encryption_mode = BCMOLT_EPON_ENCRYPTION_MODE_NO_ENCRYPTION;
        rc = bcmolt_cfg_set(hde_link->device_id, &pon_cfg.hdr);
        if (rc != BCM_ERR_OK)
        {
            BCM_LOG(ERROR, hde_data.log_id[hde_link->device_id], "Failure - Cannot set PON encryption mode!.\n");
            return rc;
        }
    }

    return BCM_ERR_OK;
}

static bcmolt_epon_link_rate get_link_rate(const hde_key *hde_link)
{
    bcmos_errno rc;
    bcmolt_epon_link_cfg link_cfg;
    bcmolt_epon_link_key link_key =
    {
        .epon_ni = hde_link->epon_ni,
        .mac_address = hde_link->mac_addr
    };
    BCMOLT_CFG_INIT(&link_cfg, epon_link, link_key);
    BCMOLT_CFG_PROP_GET(&link_cfg, epon_link, link_rate);
    rc = bcmolt_cfg_get(hde_link->device_id, &link_cfg.hdr);
    if (rc != BCM_ERR_OK)
    {
        BCM_LOG(
            ERROR,
            hde_data.log_id[hde_link->device_id],
            "Failure - cannot query link rate for link %02x:%02x:%02x:%02x:%02x:%02x!.\n",
            hde_link->mac_addr.u8[0],
            hde_link->mac_addr.u8[1],
            hde_link->mac_addr.u8[2],
            hde_link->mac_addr.u8[3],
            hde_link->mac_addr.u8[4],
            hde_link->mac_addr.u8[5]);
    }
    return link_cfg.data.link_rate;
}

static bcmos_mac_address get_pon_mac(const hde_key *hde_link)
{
    bcmos_errno rc;
    bcmolt_epon_ni_cfg pon_cfg;
    bcmolt_epon_ni_key pon_key = { .epon_ni = hde_link->epon_ni };
    BCMOLT_CFG_INIT(&pon_cfg, epon_ni, pon_key);
    BCMOLT_CFG_PROP_GET(&pon_cfg, epon_ni, mac_address);
    rc = bcmolt_cfg_get(hde_link->device_id, &pon_cfg.hdr);
    if (rc != BCM_ERR_OK)
    {
        BCM_LOG(ERROR, hde_data.log_id[hde_link->device_id], "Failure - Cannot query PON mac address!.\n");
    }
    return pon_cfg.data.mac_address;
}

static bcmolt_epon_llid get_llid(const hde_key *hde_link)
{
    bcmos_errno rc;
    bcmolt_epon_link_cfg link_cfg;
    bcmolt_epon_link_key link_key =
    {
        .epon_ni = hde_link->epon_ni,
        .mac_address = hde_link->mac_addr
    };
    BCMOLT_CFG_INIT(&link_cfg, epon_link, link_key);
    BCMOLT_CFG_PROP_GET(&link_cfg, epon_link, llid);
    rc = bcmolt_cfg_get(hde_link->device_id, &link_cfg.hdr);
    if (rc != BCM_ERR_OK)
    {
        BCM_LOG(
            ERROR,
            hde_data.log_id[hde_link->device_id],
            "Failure - cannot query LLID for link %02x:%02x:%02x:%02x:%02x:%02x!.\n",
            hde_link->mac_addr.u8[0],
            hde_link->mac_addr.u8[1],
            hde_link->mac_addr.u8[2],
            hde_link->mac_addr.u8[3],
            hde_link->mac_addr.u8[4],
            hde_link->mac_addr.u8[5]);
    }
    return link_cfg.data.llid;
}

/* By spec the SCI is the LLID of the link appended to the corresponding epon port mac address */
static void create_sci(const bcmos_mac_address *epon_port_mac, bcmolt_epon_llid llid, uint8_t *sci)
{
    sci[7] = (uint8_t) llid;
    sci[6] = (uint8_t) (llid >> 8);
    sci[5] = epon_port_mac->u8[5];
    sci[4] = epon_port_mac->u8[4];
    sci[3] = epon_port_mac->u8[3];
    sci[2] = epon_port_mac->u8[2];
    sci[1] = epon_port_mac->u8[1];
    sci[0] = epon_port_mac->u8[0];
}

static void handle_not_encrypted_enable_encryption(hde_link_data *hde_link, const bcmolt_msg *ind)
{
    dpoe_encrypt_mode mode = DPOE_ENCRYPT_MODE_NONE;
    // Retreive useful link info and store it to save messages later.
    hde_link->link_rate = get_link_rate(&hde_link->hde_link);
    hde_link->corresponding_pon_port_mac = get_pon_mac(&hde_link->hde_link);
    hde_link->llid = get_llid(&hde_link->hde_link);

    if (hde_link->link_rate == BCMOLT_EPON_LINK_RATE_TEN_TEN || hde_link->link_rate == BCMOLT_EPON_LINK_RATE_TEN_ONE)
    {
        mode = DPOE_ENCRYPT_MODE_10DOWN;
    }
    else if (hde_link->link_rate == BCMOLT_EPON_LINK_RATE_ONE_ONE)
    {
        mode = DPOE_ENCRYPT_MODE_1DOWN;
    }
    else
    {
        BCM_LOG(ERROR, hde_data.log_id[hde_link->hde_link.device_id], "Failure - UNKNOWN LINK RATE!!!.\n");
    }

    dpoe_encrypt_oam_set_request_send(
        &hde_link->hde_link,
        KEY_EXCHANGE_PERIOD,
        mode,
        hde_link->corresponding_pon_port_mac);

    bcmos_timer_start(&hde_link->oam_timeout_timer, HDE_ENABLE_OAM_WAIT_US + OAM_TIMEOUT_GRACE_PERIOD_US);

    hde_link->state = HDE_STATE_WAITING_FOR_OAM_ACK;
}

static void handle_encrypted_disable_encryption(hde_link_data *hde_link, const bcmolt_msg *ind)
{
    bcmolt_encryption_information_container empty_key = { };
    bcmos_errno rc;

    // Turn off encryption on the embedded side.
    rc = set_new_key(&hde_link->hde_link, &empty_key, BCMOLT_EPON_KEY_CHOICE_KEY_0, BCMOLT_EPON_ENCRYPTION_MODE_NO_ENCRYPTION);
    if (rc != BCM_ERR_OK)
    {
        BCM_LOG(ERROR, hde_data.log_id[hde_link->hde_link.device_id],
            "Failure - Cannot communicate with the embedded!.\n");
    }

    // Send the disable OAM to the link. Don't bother waiting for a response.
    dpoe_encrypt_oam_set_request_send(&hde_link->hde_link, 0, DPOE_ENCRYPT_MODE_NONE, get_pon_mac(&hde_link->hde_link));

    hde_link->state = HDE_STATE_NOT_ENCRYPTED;
}

static void handle_encrypted_frame_rx(hde_link_data *hde_link, const bcmolt_msg *ind)
{
    const bcmolt_epon_link_frame_captured *frame_captured = (const bcmolt_epon_link_frame_captured*) ind;
    uint8_t new_key[16];
    bcmolt_epon_key_choice key_choice;
    bcmos_errno rc;
    bcmolt_encryption_information_container key_for_embedded;
    rc = dpoe_encrypt_oam_parse_new_key(&frame_captured->data.frame, new_key, &key_choice);
    if (rc != BCM_ERR_OK)
    {
        // Not a DPoE new key, ignore it
        return;
    }

    if (hde_link->link_rate == BCMOLT_EPON_LINK_RATE_TEN_TEN || hde_link->link_rate == BCMOLT_EPON_LINK_RATE_TEN_ONE)
    {
        uint8_t sci[8];
        create_sci(&hde_link->corresponding_pon_port_mac, hde_link->llid, sci);
        key_for_embedded.format = BCMOLT_EPON_ENCRYPTION_INFORMATION_FORMAT_CTR;
        memcpy(key_for_embedded.u.ctr.key, new_key, sizeof(new_key));
        memcpy(key_for_embedded.u.ctr.sci, sci, sizeof(sci));
    }
    else if (hde_link->link_rate == BCMOLT_EPON_LINK_RATE_ONE_ONE)
    {
        key_for_embedded.format = BCMOLT_EPON_ENCRYPTION_INFORMATION_FORMAT_CFB;
        memcpy(key_for_embedded.u.cfb.key, new_key, sizeof(new_key));
    }
    else
    {
        BCM_LOG(ERROR, hde_data.log_id[hde_link->hde_link.device_id], "Failure - UNKNOWN LINK RATE!!!.\n");
    }

    rc = set_new_key(&hde_link->hde_link, &key_for_embedded, key_choice, BCMOLT_EPON_ENCRYPTION_MODE_EPON_ZERO_OVERHEAD_AES);
    if (rc != BCM_ERR_OK)
    {
        BCM_LOG(ERROR, hde_data.log_id[hde_link->hde_link.device_id],
            "Failure - Cannot communicate with the embedded!.\n");
    }
}

static void handle_waiting_for_oam_ack_oam_timeout(hde_link_data *hde_link, const bcmolt_msg *ind)
{
    BCM_LOG(ERROR, hde_data.log_id[hde_link->hde_link.device_id], "Failure - No response to initial OAM.\n");
    hde_link->state = HDE_STATE_NOT_ENCRYPTED;
}

static void handle_waiting_for_oam_ack_frame_rx(hde_link_data *hde_link, const bcmolt_msg *ind)
{
    const bcmolt_epon_link_frame_captured *frame_captured = (const bcmolt_epon_link_frame_captured*) ind;
    uint8_t encrypt_mode_response;
    uint8_t key_expiry_response;
    bcmos_errno rc;

    rc = dpoe_encrypt_oam_parse_set_response(&frame_captured->data.frame, &encrypt_mode_response, &key_expiry_response);
    if (rc != BCM_ERR_OK)
    {
        // Not a encryption set reponse, ignore it
        return;
    }

    /* 0 represents 'no error'. */
    if ((encrypt_mode_response != 0) || (key_expiry_response != 0))
    {
        BCM_LOG(ERROR, hde_data.log_id[hde_link->hde_link.device_id],
            "Failure - The link refused one of the requested TLVs.\n");
        hde_link->state = HDE_STATE_NOT_ENCRYPTED;
        return;
    }

    // We have an appropriate response, so move on normally.
    bcmos_timer_stop(&hde_link->oam_timeout_timer);
    hde_link->state = HDE_STATE_ENCRYPTED;
}

static void handle_waiting_for_oam_ack_disable_encryption(hde_link_data *hde_link, const bcmolt_msg *ind)
{
    hde_link->state = HDE_STATE_NOT_ENCRYPTED;
}

static void handle_ignore(hde_link_data *hde_link, const bcmolt_msg *ind)
{
    // Literally do nothing
}

static void handle_error(hde_link_data *hde_link, const bcmolt_msg *ind)
{
    BCM_LOG(ERROR, hde_data.log_id[hde_link->hde_link.device_id], "Unexpected event in state %s\n",
        get_state_string(hde_link->state));
}

static link_state_handler link_state_machine[HDE_STATE__NUM_OF][HDE_EVENT__NUM_OF] =
{
    [HDE_STATE_NOT_ENCRYPTED] =
    {
        [HDE_EVENT_ENABLE_ENCRYPTION] = handle_not_encrypted_enable_encryption,
        [HDE_EVENT_DISABLE_ENCRYPTION] = handle_error,
        [HDE_EVENT_FRAME_RX] = handle_ignore,
        [HDE_EVENT_OAM_TIMEOUT] = handle_ignore,
    },
    [HDE_STATE_ENCRYPTED] =
    {
        [HDE_EVENT_ENABLE_ENCRYPTION] = handle_error,
        [HDE_EVENT_DISABLE_ENCRYPTION] = handle_encrypted_disable_encryption,
        [HDE_EVENT_FRAME_RX] = handle_encrypted_frame_rx,
        [HDE_EVENT_OAM_TIMEOUT] = handle_ignore,
    },
    [HDE_STATE_WAITING_FOR_OAM_ACK] =
    {
        [HDE_EVENT_ENABLE_ENCRYPTION] = handle_error,
        [HDE_EVENT_DISABLE_ENCRYPTION] = handle_waiting_for_oam_ack_disable_encryption,
        [HDE_EVENT_FRAME_RX] = handle_waiting_for_oam_ack_frame_rx,
        [HDE_EVENT_OAM_TIMEOUT] = handle_waiting_for_oam_ack_oam_timeout,
    }
};

static int32_t find_table_index(const hde_key *hde_link)
{
    uint16_t i;
    for (i = 0; i < hde_data.num_links; ++i)
    {
        if (memcmp(hde_link, &hde_data.links[i], sizeof(*hde_link)) == 0)
        {
            return i;
        }
    }
    return -1;
}

static void remove_link_from_table(int32_t link_index)
{
    int32_t i;
    for (i = link_index; i < hde_data.num_links - 1; ++i)
    {
        /* Backwards copy all data expect for the timer handle */
        hde_data.links[i].hde_link = hde_data.links[i + 1].hde_link;
        hde_data.links[i].link_rate = hde_data.links[i + 1].link_rate;
        hde_data.links[i].llid = hde_data.links[i + 1].llid;
        hde_data.links[i].state = hde_data.links[i + 1].state;
    }
    /* also clear the state of the now empty link */
    hde_data.links[hde_data.num_links - 1].state = HDE_STATE_NOT_ENCRYPTED;
    hde_data.num_links--;
}

static void run_state_machine(int32_t link_index, const hde_task_msg *task_msg)
{
    if (link_index < 0) // We don't know of this link yet.
    {
        if (hde_data.num_links == HDE_MAX_NUM_LINKS)
        {
            BCM_LOG(ERROR, hde_data.log_id[task_msg->key.device_id], "Failure - Cannot add any more links!.\n");
            return;
        }

        link_index = hde_data.num_links;
        hde_data.links[link_index].hde_link = task_msg->key;
        link_state_machine[hde_data.links[link_index].state][task_msg->event](&hde_data.links[link_index], task_msg->rx);

        if (hde_data.links[link_index].state != HDE_STATE_NOT_ENCRYPTED)
        {
            // This link did not end in the state 'not encrypted', so we need to start tracking him.
            ++hde_data.num_links;
        }
    }
    else // We already know of this link.
    {
        link_state_machine[hde_data.links[link_index].state][task_msg->event](&hde_data.links[link_index], task_msg->rx);

        if (hde_data.links[link_index].state == HDE_STATE_NOT_ENCRYPTED)
        {
            // The link has is no longer encrypted so lets forget about him.
            remove_link_from_table(link_index);
        }
    }
}

static void hde_handle_event(bcmos_module_id module_id, bcmos_msg *os_msg)
{
    hde_task_msg *task_msg = (hde_task_msg *) os_msg;

    int32_t link_index;

    link_index = find_table_index(&task_msg->key);
    run_state_machine(link_index, task_msg);

    if (task_msg->rx != NULL)
    {
        bcmolt_msg_free(task_msg->rx);
    }
    bcmos_free(task_msg);
}

static bcmos_timer_rc oam_response_timeout(bcmos_timer *timer, long data)
{
    hde_task_msg *task_msg = bcmos_calloc(sizeof(*task_msg));

    BUG_UNLESS(task_msg);

    task_msg->key = hde_data.links[data].hde_link;
    task_msg->event = HDE_EVENT_OAM_TIMEOUT;
    task_msg->os_msg.handler = hde_handle_event;

    bcmos_msg_send_to_module(BCMOS_MODULE_ID_USER_APPL_EPON_HDE, &task_msg->os_msg, BCMOS_MSG_SEND_AUTO_FREE);
    return BCMOS_TIMER_OK;
}

static bcmos_errno hde_set_encryption(hde_key* key, bcmos_bool enabled)
{
    bcmos_errno rc = BCM_ERR_OK;

    hde_task_msg *task_msg = bcmos_calloc(sizeof(*task_msg));

    BUG_UNLESS(task_msg);

    task_msg->key = *key;
    task_msg->event = enabled ? HDE_EVENT_ENABLE_ENCRYPTION : HDE_EVENT_DISABLE_ENCRYPTION;
    task_msg->os_msg.handler = hde_handle_event;

    rc = bcmos_msg_send_to_module(BCMOS_MODULE_ID_USER_APPL_EPON_HDE, &task_msg->os_msg, BCMOS_MSG_SEND_AUTO_FREE);
    BUG_ON(rc != BCM_ERR_OK);
    return rc;
}

static bcmos_errno hde_cli_enable(bcmcli_session *session, const bcmcli_cmd_parm parm[], uint16_t n_parms)
{
    bcmos_errno rc = BCM_ERR_OK;
    hde_key key =
    {
        .device_id = current_device,
        .epon_ni = (bcmolt_epon_ni)(bcmcli_find_named_parm(session, "epon_ni")->value.number),
        .mac_addr = (bcmos_mac_address)(bcmcli_find_named_parm(session, "mac_address")->value.mac)
    };

    rc = set_pon_encryption_if_needed(&key);
    if (rc != BCM_ERR_OK)
    {
        return rc;
    }
    return hde_set_encryption(&key, BCMOS_TRUE);
}

static bcmos_errno hde_cli_disable(bcmcli_session *session, const bcmcli_cmd_parm parm[], uint16_t n_parms)
{
    hde_key key =
    {
        .device_id = current_device,
        .epon_ni = (bcmolt_epon_ni)(bcmcli_find_named_parm(session, "epon_ni")->value.number),
        .mac_addr = (bcmos_mac_address)(bcmcli_find_named_parm(session, "mac_address")->value.mac)
    };

    return hde_set_encryption(&key, BCMOS_FALSE);
}

// public indication handler interface -- called in transport layer context
bcmos_errno bcmolt_epon_hde_process_rx(bcmolt_devid device_id, uint8_t instance, bcmolt_proxy_rx *rx)
{
    bcmos_errno rc = BCM_ERR_OK;
    bcmolt_epon_link_frame_captured* actual_rx;
    hde_task_msg *task_msg;
    hde_key key;

    if (!is_running)
    {
        return BCM_ERR_OK;
    }

    // We only look at message targetting epon links.
    if (rx->hdr.obj_type != BCMOLT_OBJ_ID_EPON_LINK)
    {
        // Not an error, we just don't care about this object.
        return BCM_ERR_OK;
    }

    // This is the only kind of proxy RX currently supported.
    if (rx->hdr.subgroup != BCMOLT_EPON_LINK_PROXY_RX_ID_FRAME_CAPTURED)
    {
        return BCM_ERR_OK;
    }

    // This is safe due to the above subgroup check.
    actual_rx = (bcmolt_epon_link_frame_captured*) rx;
    key.device_id = device_id;
    key.epon_ni = instance;
    key.mac_addr = actual_rx->key.mac_address;

    task_msg = bcmos_calloc(sizeof(*task_msg));
    if (task_msg == NULL)
    {
        rc = BCM_ERR_NOMEM;
    }
    else
    {
        bcmolt_msg *rx_clone = NULL;
        rc = bcmolt_msg_clone(&rx_clone, &rx->hdr);
        if (rc != BCM_ERR_OK)
        {
            bcmos_free(task_msg);
            return rc;
        }
        actual_rx = (bcmolt_epon_link_frame_captured*) rx_clone;

        task_msg->rx = rx_clone;
        task_msg->key = key;
        task_msg->event = HDE_EVENT_FRAME_RX;
        task_msg->os_msg.handler = hde_handle_event;
        rc = bcmos_msg_send_to_module(BCMOS_MODULE_ID_USER_APPL_EPON_HDE, &task_msg->os_msg, BCMOS_MSG_SEND_AUTO_FREE);
        if (rc != BCM_ERR_OK)
        {
            BCM_LOG(ERROR, hde_data.log_id[device_id], "Message send failed!\n");
            bcmolt_msg_free(rx_clone);
        }
    }

    return rc;
}

void bcmolt_epon_hde_appl_cli_init(bcmcli_entry *top_dir)
{
    static const char *dir_name = "hde";

    if (bcmcli_dir_find(top_dir, dir_name))
    {
        return;
    }

    bcmcli_entry *dir = bcmcli_dir_add(top_dir, dir_name, "EPON OAM negotiation commands", BCMCLI_ACCESS_ADMIN, NULL);
    BUG_ON(dir == NULL);

    BCMCLI_MAKE_CMD(
        dir,
        "enable",
        "Enable encryption for an EPON link",
        hde_cli_enable,
        BCMCLI_MAKE_PARM("epon_ni", "EPON NI", BCMCLI_PARM_NUMBER, 0),
        BCMCLI_MAKE_PARM("mac_address", "MAC address", BCMCLI_PARM_MAC, 0));

    BCMCLI_MAKE_CMD(
        dir,
        "disable",
        "Disable encryption for an EPON link",
        hde_cli_disable,
        BCMCLI_MAKE_PARM("epon_ni", "EPON NI", BCMCLI_PARM_NUMBER, 0),
        BCMCLI_MAKE_PARM("mac_address", "MAC address", BCMCLI_PARM_MAC, 0));
}

void bcmolt_epon_hde_appl_init(void)
{
    bcmos_errno rc = BCM_ERR_OK;
    uint32_t i;

    bcmos_task_parm task_params =
    {
        .name = "user_appl_hde",
        .priority = TASK_PRIORITY_USER_APPL_EPON_HDE,
        .core = BCMOS_CPU_CORE_ANY, /* No CPU affinity */
        .init_handler = NULL
    };

    bcmos_timer_parm timer_parm =
    {
        .name = "oam_timeout_timer",
        .owner = BCMOS_MODULE_ID_NONE,
        .periodic = BCMOS_FALSE,
        .handler = oam_response_timeout
    };

    if (is_running)
    {
        return;
    }

    is_running = BCMOS_TRUE;

    for (i=0; i<BCMTR_MAX_OLTS; i++)
    {
        char log_name[MAX_DEV_LOG_ID_NAME];
        snprintf(log_name, sizeof(log_name)-1, "user_appl_hde_%d", i);
        hde_data.log_id[i] = bcm_dev_log_id_register(log_name, DEV_LOG_LEVEL_ERROR, DEV_LOG_ID_TYPE_BOTH);
    }

    hde_data.links = bcmos_calloc(HDE_MAX_NUM_LINKS * sizeof(hde_data.links[0]));
    BUG_ON(hde_data.links == NULL);

    rc = bcmos_task_create(&hde_data.task, &task_params);
    BUG_ON(rc != BCM_ERR_OK);

    bcmos_module_parm module_params =
    {
        .qparm = { .name = "user_appl_hde", .size = HDE_TASK_MSG_Q_SIZE }
    };
    rc = bcmos_module_create(BCMOS_MODULE_ID_USER_APPL_EPON_HDE, &hde_data.task, &module_params);
    BUG_ON(rc != BCM_ERR_OK);

    for (i = 0; i < HDE_MAX_NUM_LINKS; ++i)
    {
        timer_parm.data = i;
        rc = bcmos_timer_create(&hde_data.links[i].oam_timeout_timer, &timer_parm);
        BUG_ON(rc != BCM_ERR_OK);
    }
}