MBM_FC16_HoldingRegWrN - overwriting memory after data buffer

[luknaw0x1B]

In Function:

CPU_INT16U  MBM_FC16_HoldingRegWrN (MODBUS_CH   *pch,
                                    CPU_INT08U   slave_node,
                                    CPU_INT16U   slave_addr,
                                    CPU_INT16U  *p_reg_tbl,
                                    CPU_INT16U   nbr_regs)
{
    CPU_INT16U   err;
    CPU_BOOLEAN  ok;
    CPU_INT08U   nbr_bytes;
    CPU_INT08U   i;
    CPU_INT08U  *p_data;



    MBM_TX_FRAME_NBYTES             = (CPU_INT16U)(nbr_regs * sizeof(CPU_INT16U) + 5);
    MBM_TX_FRAME_SLAVE_ADDR         = slave_node;                               /* Setup command                     */
    MBM_TX_FRAME_FC                 = 16;
    MBM_TX_FRAME_FC16_ADDR_HI       = (CPU_INT08U)((slave_addr >> 8) & 0x00FF);
    MBM_TX_FRAME_FC16_ADDR_LO       = (CPU_INT08U) (slave_addr       & 0x00FF);
    MBM_TX_FRAME_FC16_NBR_REGS_HI   = (CPU_INT08U)((nbr_regs   >> 8) & 0x00FF);
    MBM_TX_FRAME_FC16_NBR_REGS_LO   = (CPU_INT08U) (nbr_regs         & 0x00FF);
    nbr_bytes                       = (CPU_INT08U) (nbr_regs * sizeof(CPU_INT16U));
    MBM_TX_FRAME_FC16_BYTE_CNT      = nbr_bytes;
    p_data                          = MBM_TX_FRAME_FC16_DATA;

    for (i = 0; i < nbr_bytes; i++) {
        *p_data++ = (CPU_INT08U)((*p_reg_tbl >> 8) & 0x00FF);                   /* Write HIGH data byte              */
        *p_data++ = (CPU_INT08U) (*p_reg_tbl       & 0x00FF);                   /* Write LOW  data byte              */
        p_reg_tbl++;
    }

Two times more data is rewritten than it should be because we increment the number of bytes (nbr_bytes) but rewrite the whole register.

*p_data++ = (CPU_INT08U)((*p_reg_tbl >> 8) & 0x00FF);                   /* Write HIGH data byte              */
*p_data++ = (CPU_INT08U) (*p_reg_tbl       & 0x00FF);                   /* Write LOW  data byte              */