ctrl_reg = cdns_i2c_readreg(CDNS_I2C_CR_OFFSET);
        ctrl_reg |= CDNS_I2C_CR_RW | CDNS_I2C_CR_CLR_FIFO;
 
+       /*
+        * Receive up to I2C_SMBUS_BLOCK_MAX data bytes, plus one message length
+        * byte, plus one checksum byte if PEC is enabled. p_msg->len will be 2 if
+        * PEC is enabled, otherwise 1.
+        */
        if (id->p_msg->flags & I2C_M_RECV_LEN)
-               id->recv_count = I2C_SMBUS_BLOCK_MAX + 1;
+               id->recv_count = I2C_SMBUS_BLOCK_MAX + id->p_msg->len;
 
        id->curr_recv_count = id->recv_count;
 
        if (id->err_status & CDNS_I2C_IXR_ARB_LOST)
                return -EAGAIN;
 
+       if (msg->flags & I2C_M_RECV_LEN)
+               msg->len += min_t(unsigned int, msg->buf[0], I2C_SMBUS_BLOCK_MAX);
+
        return 0;
 }