diff --git a/src/drivers/nvs/nvs.c b/src/drivers/nvs/nvs.c index efa49ac5..a4a06ccf 100644 --- a/src/drivers/nvs/nvs.c +++ b/src/drivers/nvs/nvs.c @@ -30,6 +30,34 @@ FILE_LICENCE ( GPL2_OR_LATER ); * */ +/** + * Calculate length up to next block boundary + * + * @v nvs NVS device + * @v address Starting address + * @v max_len Maximum length + * @ret len Length to use, stopping at block boundaries + */ +static size_t nvs_frag_len ( struct nvs_device *nvs, unsigned int address, + size_t max_len ) { + size_t frag_len; + + /* If there are no block boundaries, return the maximum length */ + if ( ! nvs->block_size ) + return max_len; + + /* Calculate space remaining up to next block boundary */ + frag_len = ( ( nvs->block_size - + ( address & ( nvs->block_size - 1 ) ) ) + << nvs->word_len_log2 ); + + /* Limit to maximum length */ + if ( max_len < frag_len ) + return max_len; + + return frag_len; +} + /** * Read from non-volatile storage device * @@ -51,14 +79,8 @@ int nvs_read ( struct nvs_device *nvs, unsigned int address, while ( len ) { - /* Calculate space remaining up to next block boundary */ - frag_len = ( ( nvs->block_size - - ( address & ( nvs->block_size - 1 ) ) ) - << nvs->word_len_log2 ); - - /* Limit to space remaining in buffer */ - if ( frag_len > len ) - frag_len = len; + /* Calculate length to read, stopping at block boundaries */ + frag_len = nvs_frag_len ( nvs, address, len ); /* Read this portion of the buffer from the device */ if ( ( rc = nvs->read ( nvs, address, data, frag_len ) ) != 0 ) @@ -122,14 +144,8 @@ int nvs_write ( struct nvs_device *nvs, unsigned int address, while ( len ) { - /* Calculate space remaining up to next block boundary */ - frag_len = ( ( nvs->block_size - - ( address & ( nvs->block_size - 1 ) ) ) - << nvs->word_len_log2 ); - - /* Limit to space remaining in buffer */ - if ( frag_len > len ) - frag_len = len; + /* Calculate length to write, stopping at block boundaries */ + frag_len = nvs_frag_len ( nvs, address, len ); /* Write this portion of the buffer to the device */ if ( ( rc = nvs->write ( nvs, address, data, frag_len ) ) != 0) diff --git a/src/drivers/nvs/nvsvpd.c b/src/drivers/nvs/nvsvpd.c index 1f61a55d..b53829e8 100644 --- a/src/drivers/nvs/nvsvpd.c +++ b/src/drivers/nvs/nvsvpd.c @@ -111,7 +111,6 @@ int nvs_vpd_init ( struct nvs_vpd_device *nvsvpd, struct pci_device *pci, } /* Initialise NVS device */ - nvsvpd->nvs.block_size = 1; nvsvpd->nvs.size = len; nvsvpd->nvs.read = nvs_vpd_read; nvsvpd->nvs.write = nvs_vpd_write;