#include "lboot_mb_shared.h"
#include "modbus.h"


/************************ адреса регистров на шине Modbus *********************/
#define LBOOT_MB_REG_ADDR_OFFSET    0x0001
#define LBOOT_MB_REGADDR_WR_START   0x8000
#define LBOOT_MB_REGADDR_WR_FIRM    0x8100
#define LBOOT_MB_REGADDR_WR_SIGN    0x8101
#define LBOOT_MB_REGADDR_WR_SPECCMD 0x8200


#define LBOOT_MB_REG_DEVINFO_SIZE (sizeof(t_lboot_devinfo)/2)

#define LBOOT_MB_REG_DEVINFO      LBOOT_MB_REG_ADDR_OFFSET
#define LBOOT_MB_REG_MODE         (LBOOT_MB_REG_ADDR_OFFSET + LBOOT_MB_REG_DEVINFO_SIZE)
#define LBOOT_MB_REG_BOOTVER      (LBOOT_MB_REG_MODE + 1)
#define LBOOT_MB_REG_ERR_L        (LBOOT_MB_REG_BOOTVER + 1)
#define LBOOT_MB_REG_ERR_H        (LBOOT_MB_REG_ERR_L + 1)
#define LBOOT_MB_REG_FLAGS_L      (LBOOT_MB_REG_ERR_H + 1)
#define LBOOT_MB_REG_FLAGS_H      (LBOOT_MB_REG_FLAGS_L + 1)
#define LBOOT_MB_REG_FEATURES_L   (LBOOT_MB_REG_FLAGS_H + 1)
#define LBOOT_MB_REG_FEATURES_H   (LBOOT_MB_REG_FEATURES_L + 1)


/** Коды ошибок для интерфейса Modbus RTU */
typedef enum {
    LBOOT_ERR_MB_FIRST              = -100,
    /** ошибка функции modbus_set_slave() */
    LBOOT_ERR_MB_SET_ADDR           = -100,
    /** ошибка чтения регистров modbus */
    LBOOT_ERR_MB_READ_REGS          = -101,
    /** ошибка записи регистров modbus */
    LBOOT_ERR_MB_WRITE_REGS         = -102
} t_lboot_mb_errs;

static const char* f_err_str[] = {
    "Can't set modbus address",
    "Can't read modbus registers",
    "Can't write modbus registers"
};

static int f_mb_tout = 1000;
static int f_mb_sign_tout = 4000;
static int f_mb_max_retry_cnt = 0;
static int f_mb_interframe_delay = 0;
static modbus_t *f_mb_st = NULL;

static unsigned f_mb_cur_resp_tout = (unsigned)-1;


static int32_t f_mb_get_deverr(int32_t* deverr);


const char *lboot_mb_get_err_str(int err) {
    return ((err > LBOOT_ERR_MB_FIRST) ||
            (err <  (LBOOT_ERR_MB_FIRST -
                    (int32_t)(sizeof(f_err_str)/sizeof(f_err_str[0]))+1))) ?
               NULL : f_err_str[LBOOT_ERR_MB_FIRST-err];
}


void lboot_mb_set_resp_tout(unsigned response_tout) {
    if (f_mb_cur_resp_tout != response_tout) {
#if LIBMODBUS_VERSION_CHECK(3, 1, 0)
        modbus_set_response_timeout(f_mb_st, response_tout/1000,  (response_tout % 1000) *1000);
#else
        struct timeval tv;
        tv.tv_sec = response_tout/1000;
        tv.tv_usec = (response_tout % 1000) *1000;
        modbus_set_response_timeout(f_mb_st, &tv);
#endif
        f_mb_cur_resp_tout = response_tout;
    }
}

void lboot_mb_restore_resp_tout(void) {
    lboot_mb_set_resp_tout(f_mb_tout);
}

static void f_sleep_interframe() {    
    SLEEP_MS(f_mb_interframe_delay);
}

static int f_read_input_regs(modbus_t *ctx, int addr, int nb, uint16_t *dest) {
    int err = modbus_read_input_registers(ctx, addr-1, nb, dest);
    f_sleep_interframe();
    return err;
}

static int f_write_regs(modbus_t *ctx, int addr, int nb, const uint16_t *data) {
    int err = modbus_write_registers(ctx, addr-1, nb, data);
    f_sleep_interframe();
    return err;
}


static int f_mb_write_regs_wcheck(int addr, const uint16_t *buf, int size) {
    int err;
    int retry_cnt = 0;
    int out = 0;
    do {
        if (f_write_regs(f_mb_st, addr, size, buf)!=size) {
            int32_t deverr=0;
            if (f_mb_get_deverr(&deverr) !=0) {
                err = LBOOT_ERR_MB_WRITE_REGS;
                /* выходим из цикла, т.к. уже были повторные попытки при чтении ошибки */
                out = 1;
            } else if (deverr == 0) {
                err = LBOOT_ERR_MB_WRITE_REGS;
            } else {
                err = deverr;
                /* если устройство вернуло не 0-ую ошибку, то это не ошибка интерфейса,
                 * соответственно нет необходимости делать повтор */
                out = 1;
            }
        } else {
            err = 0;
            out = 1;
        }
    } while (!out  && (retry_cnt++ < f_mb_max_retry_cnt));
    return err;
}


static int f_mb_read_regs_wretry(int addr, uint16_t *buf, int size) {
    int err;
    int retry_cnt = 0;
    do {
        if (f_read_input_regs(f_mb_st, addr, size, buf)!=size) {
            err = LBOOT_ERR_MB_READ_REGS;
        } else {
            err = 0;
        }
    } while ((err != 0)  && (retry_cnt++ < f_mb_max_retry_cnt));
    return err;
}


int lboot_mb_set_tout(int tout) {
    f_mb_tout = tout;
    return 0;
}

int lboot_mb_set_sign_tout(int tout) {
    f_mb_sign_tout = tout;
    return 0;
}

int lboot_mb_set_max_retry_cnt(int cnt) {
    f_mb_max_retry_cnt = cnt;
    return 0;
}

int lboot_mb_set_interframe_delay(int delay) {
    f_mb_interframe_delay = 0;
    return 0;
}


int lboot_mb_read_dev_info(t_lboot_devinfo *info) {
    return f_read_input_regs(f_mb_st, LBOOT_MB_REG_DEVINFO,
                          LBOOT_MB_REG_DEVINFO_SIZE, (uint16_t*)info) ==LBOOT_MB_REG_DEVINFO_SIZE ?
               0 : LBOOT_ERR_MB_READ_REGS;
}

int lboot_mb_open(modbus_t *mb, int slave, int open_err) {
    int err = 0;
    lboot_mb_close();
    /* устанавливаем соединение (открываем порт) */
    if (modbus_connect(mb)) {
         err = open_err;
    } else {
    /* устанавливаем адрес устройства */
        if (modbus_set_slave(mb, slave)) {
            err = LBOOT_ERR_MB_SET_ADDR;
            modbus_close(mb);
        }
    }

    if (!err) {
        f_mb_st = mb;
        lboot_mb_set_resp_tout(f_mb_tout);
    }

    return err;
}



int lboot_mb_get_devmode(uint8_t* mode) {
    uint16_t val;
    int err = 0;

    err = f_mb_read_regs_wretry(LBOOT_MB_REG_MODE, &val,1);
    if (!err) {
        *mode = val&0xFF;
    }

    return err;
}

int lboot_mb_get_bootloader_version(uint16_t *ver) {
    return f_mb_read_regs_wretry(LBOOT_MB_REG_BOOTVER, ver, 1);
}

int lboot_mb_get_dev_flags(uint32_t *flags) {
    return f_mb_read_regs_wretry(LBOOT_MB_REG_FLAGS_L, (uint16_t*)flags, 2);
}

int lboot_mb_get_features(uint32_t *flags) {
    return f_mb_read_regs_wretry(LBOOT_MB_REG_FEATURES_L, (uint16_t*)flags, 2);
}


/***************************************************************************//**
  Запись блока данных по Modbus RTU. При нечетном размере последний байт пишется
  отдельно с установленной в ноль старшей частью регистра.
  При неудаче идет попытка считать код ошибки из устройства.
  @param[in] addr  Адрес регистра (с вычитом 1 - как в libmodbus, а не в протоколе)
  @param[in] buf   Буфер для записи
  @param[in] size  Размер буфера в байтах
  @return          Код ошибки
  *****************************************************************************/
static int f_mb_write_data(int addr, const uint8_t *buf, uint32_t size) {
    int err =0;
    err = f_mb_write_regs_wcheck(addr, (const uint16_t*)buf, size/2);
    /* если было нечетное число, то последний байт записываем отдельным словом */
    if (!err && (size%2)) {
        uint16_t val = buf[size-1];
        err = f_mb_write_regs_wcheck(addr, &val, 1);
    }
    return err;
}


static int32_t f_mb_get_deverr(int32_t* deverr) {
    int32_t err;
    unsigned tout = f_mb_cur_resp_tout;
    /* изменяем таймаут на обычный (т.к. можем находится в долгой операции,
     * а чтение ошибки долгого ожидания не требует) и восстанавливем
     * его по выходу */
    lboot_mb_set_resp_tout((unsigned)f_mb_tout);
    err =  f_mb_read_regs_wretry(LBOOT_MB_REG_ERR_L, (uint16_t*)deverr, 2);
    lboot_mb_set_resp_tout(tout);
    return err;
}


int lboot_mb_start_write(t_lboot_startwr_info wr_info) {
    return f_mb_write_data(LBOOT_MB_REGADDR_WR_START, (const uint8_t*)&wr_info,
                           sizeof(t_lboot_startwr_info));
}

int lboot_mb_write(const uint8_t *buf, uint32_t size) {
    return f_mb_write_data(LBOOT_MB_REGADDR_WR_FIRM, buf, size);
}

int lboot_mb_write_sign(const uint8_t *sign, uint32_t size) {
    lboot_mb_set_resp_tout((unsigned)f_mb_sign_tout);
    int err = f_mb_write_data(LBOOT_MB_REGADDR_WR_SIGN, sign, size);
    lboot_mb_set_resp_tout((unsigned)f_mb_tout);
    return  err;
}


int lboot_mb_speccmd(const t_lboot_speccmd_info *cmd_info, uint32_t size) {
    return f_mb_write_data(LBOOT_MB_REGADDR_WR_SPECCMD, (const uint8_t*)cmd_info, size);
}

int lboot_mb_close(void) {
    if (f_mb_st != NULL) {
        /* закрываем соединение,если было открыто */
        modbus_close(f_mb_st);
        /* очищаем контекст */
        modbus_free(f_mb_st);
        f_mb_st = NULL;
    }

    return 0;
}
