#include "lip_phy_mdio.h"
#include "lip_phy_mdio_regs.h"
#include "lip_phy_mdio_regs_ext.h"
#include "lip_phy_mdio_regs_expage.h"
#include "ltimer.h"

#ifndef LIP_PHY_MDIO_OP_TOUT
    #define LIP_PHY_MDIO_OP_TOUT  5
#endif




t_lip_errs lip_phy_mdio_std_reg_read(const t_lip_phy_mdio_acc *acc, uint8_t reg_addr, uint16_t *pvalue) {
    t_lip_errs err = LIP_ERR_SUCCESS;
    t_ltimer tmr;
    bool rdy = false;
    ltimer_set_ms(&tmr, LIP_PHY_MDIO_OP_TOUT);

    err = acc->iface->reg_rd_start(acc->phy_addr, reg_addr);
    while (!rdy && (err == LIP_ERR_SUCCESS)) {
        rdy = acc->iface->iface_is_rdy();
        if (!rdy && ltimer_expired(&tmr)) {
            err = LIP_ERR_PHY_MDIO_OP_TOUT;
        }
    }
    if ((err == LIP_ERR_SUCCESS) && rdy) {
        err = acc->iface->reg_get_rd_data(pvalue);
    }
    return err;
}

t_lip_errs lip_phy_mdio_std_reg_write(const t_lip_phy_mdio_acc *acc, uint8_t reg_addr, uint16_t value) {
    t_lip_errs err = LIP_ERR_SUCCESS;
    t_ltimer tmr;
    bool rdy = false;
    ltimer_set_ms(&tmr, LIP_PHY_MDIO_OP_TOUT);

    err = acc->iface->reg_wr_start(acc->phy_addr, reg_addr, value);
    while (!rdy && (err == LIP_ERR_SUCCESS)) {
        rdy = acc->iface->iface_is_rdy();
        if (!rdy && ltimer_expired(&tmr)) {
            err = LIP_ERR_PHY_MDIO_OP_TOUT;
        }
    }
    return err;
}

t_lip_errs lip_phy_mdio_std_reg_write_if_modified(const t_lip_phy_mdio_acc *acc, uint8_t reg_addr, uint16_t value, uint16_t mask, bool *pmodified) {
    uint16_t rd_val;
    t_lip_errs err = lip_phy_mdio_std_reg_read(acc, reg_addr, &rd_val);
    if (err == LIP_ERR_SUCCESS) {
        uint16_t wr_val = (rd_val & ~mask) | (value & mask);
        if (wr_val != rd_val) {
            err = lip_phy_mdio_std_reg_write(acc, reg_addr, wr_val);
            if ((err == LIP_ERR_SUCCESS) && (pmodified != NULL)) {
                *pmodified = true;
            }
        }
    }
    return err;
}


t_lip_errs lip_phy_mdio_ext_reg_read(const t_lip_phy_mdio_acc *acc, uint32_t ext_reg, uint16_t *pvalue) {
    t_lip_errs err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXT_REG_ADDR, ext_reg);
    if (err == LIP_ERR_SUCCESS) {
        err = lip_phy_mdio_std_reg_read(acc, LIP_PHY_REGADDR_EXT_REG_DATA, pvalue);
    }
    return err;
}

t_lip_errs lip_phy_mdio_ext_reg_write(const t_lip_phy_mdio_acc *acc, uint32_t ext_reg, uint16_t value) {
    t_lip_errs err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXT_REG_ADDR, ext_reg);
    if (err == LIP_ERR_SUCCESS) {
        err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXT_REG_DATA, value);
    }
    return err;
}
t_lip_errs lip_phy_mdio_ext_reg_write_if_modified(const t_lip_phy_mdio_acc *acc, uint32_t ext_reg,  uint16_t value, uint16_t mask, bool *modified) {
    t_lip_errs err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXT_REG_ADDR, ext_reg);
    if (err == LIP_ERR_SUCCESS) {
        err = lip_phy_mdio_std_reg_write_if_modified(acc, LIP_PHY_REGADDR_EXT_REG_DATA, value, mask, modified);
    }
    return err;
}



t_lip_errs lip_phy_mdio_expage_reg_read(const t_lip_phy_mdio_acc *acc, uint32_t ext_reg, uint16_t *pvalue) {
    t_lip_errs err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXPAGE_SELECT, LIP_PHY_REGADDR_GET_EXPAGE_PAGENUM(ext_reg));
    if (err == LIP_ERR_SUCCESS) {
        err = lip_phy_mdio_std_reg_read(acc, LIP_PHY_REGADDR_GET_EXPAGE_REGADDR(ext_reg), pvalue);


        t_lip_errs dis_err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXPAGE_SELECT, 0);
        if (err == LIP_ERR_SUCCESS) {
            err = dis_err;
        }
    }
    return err;
}

t_lip_errs lip_phy_mdio_expage_reg_write(const t_lip_phy_mdio_acc *acc, uint32_t ext_reg, uint16_t value) {
    t_lip_errs err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXPAGE_SELECT, LIP_PHY_REGADDR_GET_EXPAGE_PAGENUM(ext_reg));
    if (err == LIP_ERR_SUCCESS) {
        err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_GET_EXPAGE_REGADDR(ext_reg), value);

        t_lip_errs dis_err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXPAGE_SELECT, 0);
        if (err == LIP_ERR_SUCCESS) {
            err = dis_err;
        }
    }
    return err;
}
t_lip_errs lip_phy_mdio_expage_reg_write_if_modified(const t_lip_phy_mdio_acc *acc, uint32_t ext_reg,  uint16_t value, uint16_t mask, bool *modified) {
    t_lip_errs err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXPAGE_SELECT, LIP_PHY_REGADDR_GET_EXPAGE_PAGENUM(ext_reg));
    if (err == LIP_ERR_SUCCESS) {
        err = lip_phy_mdio_std_reg_write_if_modified(acc, LIP_PHY_REGADDR_GET_EXPAGE_REGADDR(ext_reg), value, mask, modified);

        t_lip_errs dis_err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXPAGE_SELECT, 0);
        if (err == LIP_ERR_SUCCESS) {
            err = dis_err;
        }
    }
    return err;
}




t_lip_errs lip_phy_mdio_get_phy_id(const t_lip_phy_mdio_acc *acc, uint32_t *pid) {
    t_lip_errs err = LIP_ERR_SUCCESS;
    uint16_t id_h, id_l;
    err = lip_phy_mdio_std_reg_read(acc, LIP_PHY_REGADDR_PHYID_H, &id_h);
    if (err == LIP_ERR_SUCCESS)
        err = lip_phy_mdio_std_reg_read(acc, LIP_PHY_REGADDR_PHYID_L, &id_l);
    if ((err == LIP_ERR_SUCCESS) && (pid != NULL)) {
        *pid = ((uint32_t)id_h << 16) | id_l;
    }
    return err;
}


static t_lip_errs lip_phy_mdio_mmd_reg_set_addr(const t_lip_phy_mdio_acc *acc, uint32_t mmd_reg, uint8_t op) {
    t_lip_errs err;
    err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_MMD_CTL,
                           LBITFIELD_SET(LIP_PHY_REGFLD_MMD_CTL_DEVADDR, LIP_PHY_REGADDR_GET_MMD_DEVADDR(mmd_reg))
                           | LBITFIELD_SET(LIP_PHY_REGFLD_MMD_CTL_FUNC,
                                           LIP_PHY_REGFLDVAL_MMD_CTL_FUNC_ADDR));
    if (err == LIP_ERR_SUCCESS) {
        err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_MMD_AD, LIP_PHY_REGADDR_GET_MMD_REGADDR(mmd_reg));
    }
    if (err == LIP_ERR_SUCCESS) {
        err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_MMD_CTL,
                                     LBITFIELD_SET(LIP_PHY_REGFLD_MMD_CTL_DEVADDR, LIP_PHY_REGADDR_GET_MMD_DEVADDR(mmd_reg))
                                     | LBITFIELD_SET(LIP_PHY_REGFLD_MMD_CTL_FUNC, op));

    }
    return err;
}


t_lip_errs lip_phy_mdio_mmd_reg_read(const t_lip_phy_mdio_acc *acc, uint32_t mmd_reg, uint16_t *pvalue) {
    t_lip_errs err = lip_phy_mdio_mmd_reg_set_addr(acc, mmd_reg, LIP_PHY_REGFLDVAL_MMD_CTL_FUNC_DATA);
    if (err == LIP_ERR_SUCCESS) {
        err = lip_phy_mdio_std_reg_read(acc, LIP_PHY_REGADDR_MMD_AD, pvalue);
    }
    return err;
}

t_lip_errs lip_phy_mdio_mmd_reg_write(const t_lip_phy_mdio_acc *acc, uint32_t mmd_reg, uint16_t value) {
    t_lip_errs err = lip_phy_mdio_mmd_reg_set_addr(acc, mmd_reg, LIP_PHY_REGFLDVAL_MMD_CTL_FUNC_DATA);
    if (err == LIP_ERR_SUCCESS) {
        err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_MMD_AD, value);
    }
    return err;
}

t_lip_errs lip_phy_mdio_mmd_reg_read_array(const t_lip_phy_mdio_acc *acc, uint32_t mmd_reg, uint16_t *pvalue, uint16_t count) {
    t_lip_errs err = lip_phy_mdio_mmd_reg_set_addr(acc, mmd_reg, LIP_PHY_REGFLDVAL_MMD_CTL_FUNC_DATA_INC_RW);
    for (uint16_t i = 0; (i < count) && (err == LIP_ERR_SUCCESS); ++i) {
        err = lip_phy_mdio_std_reg_read(acc, LIP_PHY_REGADDR_MMD_AD, pvalue++);
    }
    return err;
}

t_lip_errs lip_phy_mdio_mmd_reg_write_array(const t_lip_phy_mdio_acc *acc, uint32_t mmd_reg, const uint16_t *pvalue, uint16_t count) {
    t_lip_errs err = lip_phy_mdio_mmd_reg_set_addr(acc, mmd_reg, LIP_PHY_REGFLDVAL_MMD_CTL_FUNC_DATA_INC_RW);
    for (uint16_t i = 0; (i < count) && (err == LIP_ERR_SUCCESS); ++i) {
        err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_MMD_AD, *pvalue++);
    }
    return err;
}


t_lip_errs lip_phy_mdio_mmd_reg_write_if_modified(const t_lip_phy_mdio_acc *acc, uint32_t mmd_reg, uint16_t value, uint16_t mask, bool *modified) {
    t_lip_errs err = lip_phy_mdio_mmd_reg_set_addr(acc, mmd_reg, LIP_PHY_REGFLDVAL_MMD_CTL_FUNC_DATA);
    if (err == LIP_ERR_SUCCESS) {
        err = lip_phy_mdio_std_reg_write_if_modified(acc, LIP_PHY_REGADDR_MMD_AD, value, mask, modified);
    }
    return err;
}

t_lip_errs lip_phy_mdio_reg_read(const t_lip_phy_mdio_acc *acc, uint32_t reg, uint16_t *pvalue) {
    t_lip_errs err;
    if (LIP_PHY_REGADDR_IS_MMD(reg)) {
        err = lip_phy_mdio_mmd_reg_read(acc, reg, pvalue);
    } else if (LIP_PHY_REGADDR_IS_EXT(reg)) {
        err = lip_phy_mdio_ext_reg_read(acc, reg, pvalue);
    } else if (LIP_PHY_REGADDR_IS_EXPAGE(reg)) {
        err = lip_phy_mdio_expage_reg_read(acc, reg, pvalue);
    } else {
        err = lip_phy_mdio_std_reg_read(acc, LIP_PHY_REGADDR_GET_STD_REGADDR(reg), pvalue);
    }
    return err;
}

t_lip_errs lip_phy_mdio_reg_write(const t_lip_phy_mdio_acc *acc, uint32_t reg, uint16_t value) {
    t_lip_errs err;
    if (LIP_PHY_REGADDR_IS_MMD(reg)) {
        err = lip_phy_mdio_mmd_reg_write(acc, reg, value);
    } else if (LIP_PHY_REGADDR_IS_EXT(reg)) {
        err = lip_phy_mdio_ext_reg_write(acc, reg, value);
    } else if (LIP_PHY_REGADDR_IS_EXPAGE(reg)) {
        err = lip_phy_mdio_expage_reg_write(acc, reg, value);
    } else {
        err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_GET_STD_REGADDR(reg), value);
    }
    return err;
}

t_lip_errs lip_phy_mdio_reg_write_if_modified(const t_lip_phy_mdio_acc *acc, uint32_t reg, uint16_t value, uint16_t mask, bool *modified) {
    t_lip_errs err;
    if (LIP_PHY_REGADDR_IS_MMD(reg)) {
        err = lip_phy_mdio_mmd_reg_write_if_modified(acc, reg, value, mask, modified);
    } else if (LIP_PHY_REGADDR_IS_EXT(reg)) {
        err = lip_phy_mdio_ext_reg_write_if_modified(acc, reg, value, mask, modified);
    } else if (LIP_PHY_REGADDR_IS_EXPAGE(reg)) {
        err = lip_phy_mdio_expage_reg_write_if_modified(acc, reg, value, mask, modified);
    } else {
        err = lip_phy_mdio_std_reg_write_if_modified(acc, LIP_PHY_REGADDR_GET_STD_REGADDR(reg), value, mask, modified);
    }
    return err;
}

t_lip_errs lip_phy_mdio_reg_read_array(const t_lip_phy_mdio_acc *acc, uint32_t reg, uint16_t *pvalue, uint16_t count) {
    t_lip_errs err = LIP_ERR_SUCCESS;
    if (LIP_PHY_REGADDR_IS_MMD(reg)) {
        err = lip_phy_mdio_mmd_reg_read_array(acc, reg, pvalue, count);
    } else if (LIP_PHY_REGADDR_IS_EXT(reg)) {
        uint16_t regaddr = LIP_PHY_REGADDR_GET_EXT_REGADDR(reg);
        for (uint16_t i = 0; (i < count) && (err == LIP_ERR_SUCCESS); ++i) {
            err = lip_phy_mdio_ext_reg_read(acc, regaddr, pvalue);
            if (err == LIP_ERR_SUCCESS) {
                ++regaddr;
                ++pvalue;
            }
        }
    } else if (LIP_PHY_REGADDR_IS_EXPAGE(reg)) {
        uint8_t regaddr = LIP_PHY_REGADDR_GET_EXPAGE_REGADDR(reg);
        err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXPAGE_SELECT, LIP_PHY_REGADDR_GET_EXPAGE_PAGENUM(reg));
        for (uint16_t i = 0; (i < count) && (err == LIP_ERR_SUCCESS) && (regaddr < LIP_PHY_STD_REG_MAX_CNT); ++i) {
            err = lip_phy_mdio_std_reg_read(acc, regaddr, pvalue);
            if (err == LIP_ERR_SUCCESS) {
                ++regaddr;
                ++pvalue;
            }
        }
        t_lip_errs dis_err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXPAGE_SELECT, 0);
        if (err == LIP_ERR_SUCCESS) {
            err = dis_err;
        }
    } else {
        uint8_t regaddr = LIP_PHY_REGADDR_GET_STD_REGADDR(reg);
        for (uint16_t i = 0; (i < count) && (err == LIP_ERR_SUCCESS) && (regaddr < LIP_PHY_STD_REG_MAX_CNT); ++i) {
            err = lip_phy_mdio_std_reg_read(acc, regaddr, pvalue);
            if (err == LIP_ERR_SUCCESS) {
                ++regaddr;
                ++pvalue;
            }
        }
    }
    return err;
}

t_lip_errs lip_phy_mdio_reg_write_array(const t_lip_phy_mdio_acc *acc, uint32_t reg, const uint16_t *pvalue, uint16_t count) {
    t_lip_errs err = LIP_ERR_SUCCESS;
    if (LIP_PHY_REGADDR_IS_MMD(reg)) {
        err = lip_phy_mdio_mmd_reg_write_array(acc, reg, pvalue, count);
    } else if (LIP_PHY_REGADDR_IS_EXT(reg)) {
        uint16_t regaddr = LIP_PHY_REGADDR_GET_EXT_REGADDR(reg);
        for (uint16_t i = 0; (i < count) && (err == LIP_ERR_SUCCESS); ++i) {
            err = lip_phy_mdio_ext_reg_write(acc, regaddr, *pvalue);
            if (err == LIP_ERR_SUCCESS) {
                ++regaddr;
                ++pvalue;
            }
        }
    } else if (LIP_PHY_REGADDR_IS_EXPAGE(reg)) {
        uint8_t regaddr = LIP_PHY_REGADDR_GET_EXPAGE_REGADDR(reg);
        err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXPAGE_SELECT, LIP_PHY_REGADDR_GET_EXPAGE_PAGENUM(reg));
        for (uint16_t i = 0; (i < count) && (err == LIP_ERR_SUCCESS) && (regaddr < LIP_PHY_STD_REG_MAX_CNT); ++i) {
            err = lip_phy_mdio_std_reg_write(acc, regaddr, *pvalue);
            if (err == LIP_ERR_SUCCESS) {
                ++regaddr;
                ++pvalue;
            }
        }
        t_lip_errs dis_err = lip_phy_mdio_std_reg_write(acc, LIP_PHY_REGADDR_EXPAGE_SELECT, 0);
        if (err == LIP_ERR_SUCCESS) {
            err = dis_err;
        }
    } else {
        uint8_t regaddr = LIP_PHY_REGADDR_GET_STD_REGADDR(reg);
        for (uint16_t i = 0; (i < count) && (err == LIP_ERR_SUCCESS) && (regaddr < LIP_PHY_STD_REG_MAX_CNT); ++i) {
            err = lip_phy_mdio_std_reg_write(acc, regaddr, *pvalue);
            if (err == LIP_ERR_SUCCESS) {
                ++regaddr;
                ++pvalue;
            }
        }
    }
    return err;
}
