drivers/flash: nios2_qspi byte read access

Correct nios2_qspi to allow byte size read access

Signed-off-by: Laczen JMS <[email protected]>

Author: Laczen

drivers/flash/soc_flash_nios2_qspi.c

@@ -330,18 +330,40 @@ static int flash_nios2_qspi_read(struct device *dev, off_t offset,
 	u32_t word_to_read, bytes_to_copy;
 	s32_t rc = 0;
 
-	k_sem_take(&flash_cfg->sem_lock, K_FOREVER);
 	/*
-	 * check if offset is word aligned and
-	 * length is with in the range
+	 * check if offset and length are within the range
 	 */
-	if ((data == NULL) || ((offset + len) > qspi_dev->data_end) ||
-			(0 != (read_offset & (NIOS2_WRITE_BLOCK_SIZE - 1)))) {
+	if ((data == NULL) || (offset < qspi_dev->data_base) ||
+	    ((offset + len) > qspi_dev->data_end)) {
 		LOG_ERR("read failed at offset 0x%lx", (long)offset);
-		rc = -EINVAL;
-		goto qspi_read_err;
+		return -EINVAL;
 	}
 
+	if (!len) {
+		return 0;
+	}
+
+	k_sem_take(&flash_cfg->sem_lock, K_FOREVER);
+
+	/* first unaligned start */
+	read_offset &= ~(NIOS2_WRITE_BLOCK_SIZE - 1U);
+	if (offset > read_offset) {
+		/* number of bytes from source to copy */
+		bytes_to_copy = NIOS2_WRITE_BLOCK_SIZE - (offset - read_offset);
+		if (bytes_to_copy > remaining_length) {
+			bytes_to_copy = remaining_length;
+		}
+		/* read from flash 32 bits at a time */
+		word_to_read = IORD_32DIRECT(qspi_dev->data_base, read_offset);
+		memcpy((u8_t *)data, (u8_t *)&word_to_read + offset -
+		       read_offset, bytes_to_copy);
+		/* update offset and length variables */
+		read_offset += NIOS2_WRITE_BLOCK_SIZE;
+		buffer_offset += bytes_to_copy;
+		remaining_length -= bytes_to_copy;
+	}
+
+	/* aligned part, including unaligned end */
 	while (remaining_length > 0) {
 		/* number of bytes from source to copy */
 		bytes_to_copy = NIOS2_WRITE_BLOCK_SIZE;
@@ -354,14 +376,12 @@ static int flash_nios2_qspi_read(struct device *dev, off_t offset,
 		word_to_read = IORD_32DIRECT(qspi_dev->data_base, read_offset);
 		memcpy((u8_t *)data + buffer_offset, &word_to_read,
 		       bytes_to_copy);
-
 		/* update offset and length variables */
 		read_offset += bytes_to_copy;
 		buffer_offset += bytes_to_copy;
 		remaining_length -= bytes_to_copy;
 	}
 
-qspi_read_err:
 	k_sem_give(&flash_cfg->sem_lock);
 	return rc;
 }