Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 1 | /* |
Douglas Raillard | a8954fc | 2017-01-26 15:54:44 +0000 | [diff] [blame] | 2 | * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved. |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 3 | * |
dp-arm | fa3cf0b | 2017-05-03 09:38:09 +0100 | [diff] [blame] | 4 | * SPDX-License-Identifier: BSD-3-Clause |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 5 | */ |
| 6 | |
| 7 | #include <assert.h> |
| 8 | #include <debug.h> |
| 9 | #include <errno.h> |
| 10 | #include <io_block.h> |
| 11 | #include <io_driver.h> |
| 12 | #include <io_storage.h> |
| 13 | #include <platform_def.h> |
| 14 | #include <string.h> |
Douglas Raillard | a8954fc | 2017-01-26 15:54:44 +0000 | [diff] [blame] | 15 | #include <utils.h> |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 16 | |
| 17 | typedef struct { |
| 18 | io_block_dev_spec_t *dev_spec; |
| 19 | uintptr_t base; |
| 20 | size_t file_pos; |
| 21 | size_t size; |
| 22 | } block_dev_state_t; |
| 23 | |
| 24 | #define is_power_of_2(x) ((x != 0) && ((x & (x - 1)) == 0)) |
| 25 | |
| 26 | io_type_t device_type_block(void); |
| 27 | |
| 28 | static int block_open(io_dev_info_t *dev_info, const uintptr_t spec, |
| 29 | io_entity_t *entity); |
| 30 | static int block_seek(io_entity_t *entity, int mode, ssize_t offset); |
| 31 | static int block_read(io_entity_t *entity, uintptr_t buffer, size_t length, |
| 32 | size_t *length_read); |
| 33 | static int block_write(io_entity_t *entity, const uintptr_t buffer, |
| 34 | size_t length, size_t *length_written); |
| 35 | static int block_close(io_entity_t *entity); |
| 36 | static int block_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info); |
| 37 | static int block_dev_close(io_dev_info_t *dev_info); |
| 38 | |
| 39 | static const io_dev_connector_t block_dev_connector = { |
| 40 | .dev_open = block_dev_open |
| 41 | }; |
| 42 | |
| 43 | static const io_dev_funcs_t block_dev_funcs = { |
| 44 | .type = device_type_block, |
| 45 | .open = block_open, |
| 46 | .seek = block_seek, |
| 47 | .size = NULL, |
| 48 | .read = block_read, |
| 49 | .write = block_write, |
| 50 | .close = block_close, |
| 51 | .dev_init = NULL, |
| 52 | .dev_close = block_dev_close, |
| 53 | }; |
| 54 | |
| 55 | static block_dev_state_t state_pool[MAX_IO_BLOCK_DEVICES]; |
| 56 | static io_dev_info_t dev_info_pool[MAX_IO_BLOCK_DEVICES]; |
| 57 | |
| 58 | /* Track number of allocated block state */ |
| 59 | static unsigned int block_dev_count; |
| 60 | |
| 61 | io_type_t device_type_block(void) |
| 62 | { |
| 63 | return IO_TYPE_BLOCK; |
| 64 | } |
| 65 | |
| 66 | /* Locate a block state in the pool, specified by address */ |
| 67 | static int find_first_block_state(const io_block_dev_spec_t *dev_spec, |
| 68 | unsigned int *index_out) |
| 69 | { |
Yann Gautier | 3796621 | 2018-12-03 13:38:06 +0100 | [diff] [blame] | 70 | unsigned int index; |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 71 | int result = -ENOENT; |
Yann Gautier | 3796621 | 2018-12-03 13:38:06 +0100 | [diff] [blame] | 72 | |
| 73 | for (index = 0U; index < MAX_IO_BLOCK_DEVICES; ++index) { |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 74 | /* dev_spec is used as identifier since it's unique */ |
| 75 | if (state_pool[index].dev_spec == dev_spec) { |
| 76 | result = 0; |
| 77 | *index_out = index; |
| 78 | break; |
| 79 | } |
| 80 | } |
| 81 | return result; |
| 82 | } |
| 83 | |
| 84 | /* Allocate a device info from the pool and return a pointer to it */ |
| 85 | static int allocate_dev_info(io_dev_info_t **dev_info) |
| 86 | { |
| 87 | int result = -ENOMEM; |
| 88 | assert(dev_info != NULL); |
| 89 | |
| 90 | if (block_dev_count < MAX_IO_BLOCK_DEVICES) { |
| 91 | unsigned int index = 0; |
| 92 | result = find_first_block_state(NULL, &index); |
| 93 | assert(result == 0); |
| 94 | /* initialize dev_info */ |
| 95 | dev_info_pool[index].funcs = &block_dev_funcs; |
| 96 | dev_info_pool[index].info = (uintptr_t)&state_pool[index]; |
| 97 | *dev_info = &dev_info_pool[index]; |
| 98 | ++block_dev_count; |
| 99 | } |
| 100 | |
| 101 | return result; |
| 102 | } |
| 103 | |
| 104 | |
| 105 | /* Release a device info to the pool */ |
| 106 | static int free_dev_info(io_dev_info_t *dev_info) |
| 107 | { |
| 108 | int result; |
| 109 | unsigned int index = 0; |
| 110 | block_dev_state_t *state; |
| 111 | assert(dev_info != NULL); |
| 112 | |
| 113 | state = (block_dev_state_t *)dev_info->info; |
| 114 | result = find_first_block_state(state->dev_spec, &index); |
| 115 | if (result == 0) { |
| 116 | /* free if device info is valid */ |
Douglas Raillard | a8954fc | 2017-01-26 15:54:44 +0000 | [diff] [blame] | 117 | zeromem(state, sizeof(block_dev_state_t)); |
| 118 | zeromem(dev_info, sizeof(io_dev_info_t)); |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 119 | --block_dev_count; |
| 120 | } |
| 121 | |
| 122 | return result; |
| 123 | } |
| 124 | |
| 125 | static int block_open(io_dev_info_t *dev_info, const uintptr_t spec, |
| 126 | io_entity_t *entity) |
| 127 | { |
| 128 | block_dev_state_t *cur; |
| 129 | io_block_spec_t *region; |
| 130 | |
| 131 | assert((dev_info->info != (uintptr_t)NULL) && |
| 132 | (spec != (uintptr_t)NULL) && |
| 133 | (entity->info == (uintptr_t)NULL)); |
| 134 | |
| 135 | region = (io_block_spec_t *)spec; |
| 136 | cur = (block_dev_state_t *)dev_info->info; |
| 137 | assert(((region->offset % cur->dev_spec->block_size) == 0) && |
| 138 | ((region->length % cur->dev_spec->block_size) == 0)); |
| 139 | |
| 140 | cur->base = region->offset; |
| 141 | cur->size = region->length; |
| 142 | cur->file_pos = 0; |
| 143 | |
| 144 | entity->info = (uintptr_t)cur; |
| 145 | return 0; |
| 146 | } |
| 147 | |
| 148 | /* parameter offset is relative address at here */ |
| 149 | static int block_seek(io_entity_t *entity, int mode, ssize_t offset) |
| 150 | { |
| 151 | block_dev_state_t *cur; |
| 152 | |
| 153 | assert(entity->info != (uintptr_t)NULL); |
| 154 | |
| 155 | cur = (block_dev_state_t *)entity->info; |
| 156 | assert((offset >= 0) && (offset < cur->size)); |
| 157 | |
| 158 | switch (mode) { |
| 159 | case IO_SEEK_SET: |
| 160 | cur->file_pos = offset; |
| 161 | break; |
| 162 | case IO_SEEK_CUR: |
| 163 | cur->file_pos += offset; |
| 164 | break; |
| 165 | default: |
| 166 | return -EINVAL; |
| 167 | } |
| 168 | assert(cur->file_pos < cur->size); |
| 169 | return 0; |
| 170 | } |
| 171 | |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 172 | /* |
| 173 | * This function allows the caller to read any number of bytes |
| 174 | * from any position. It hides from the caller that the low level |
| 175 | * driver only can read aligned blocks of data. For this reason |
| 176 | * we need to handle the use case where the first byte to be read is not |
| 177 | * aligned to start of the block, the last byte to be read is also not |
| 178 | * aligned to the end of a block, and there are zero or more blocks-worth |
| 179 | * of data in between. |
| 180 | * |
| 181 | * In such a case we need to read more bytes than requested (i.e. full |
| 182 | * blocks) and strip-out the leading bytes (aka skip) and the trailing |
| 183 | * bytes (aka padding). See diagram below |
| 184 | * |
| 185 | * cur->file_pos ------------ |
| 186 | * | |
| 187 | * cur->base | |
| 188 | * | | |
| 189 | * v v<---- length ----> |
| 190 | * -------------------------------------------------------------- |
| 191 | * | | block#1 | | block#n | |
| 192 | * | block#0 | + | ... | + | |
| 193 | * | | <- skip -> + | | + <- padding ->| |
| 194 | * ------------------------+----------------------+-------------- |
| 195 | * ^ ^ |
| 196 | * | | |
| 197 | * v iteration#1 iteration#n v |
| 198 | * -------------------------------------------------- |
| 199 | * | | | | |
| 200 | * |<---- request ---->| ... |<----- request ---->| |
| 201 | * | | | | |
| 202 | * -------------------------------------------------- |
| 203 | * / / | | |
| 204 | * / / | | |
| 205 | * / / | | |
| 206 | * / / | | |
| 207 | * / / | | |
| 208 | * / / | | |
| 209 | * / / | | |
| 210 | * / / | | |
| 211 | * / / | | |
| 212 | * / / | | |
| 213 | * <---- request ------> <------ request -----> |
| 214 | * --------------------- ----------------------- |
| 215 | * | | | | | | |
| 216 | * |<-skip->|<-nbytes->| -------->|<-nbytes->|<-padding->| |
| 217 | * | | | | | | | |
| 218 | * --------------------- | ----------------------- |
| 219 | * ^ \ \ | | | |
| 220 | * | \ \ | | | |
| 221 | * | \ \ | | | |
| 222 | * buf->offset \ \ buf->offset | | |
| 223 | * \ \ | | |
| 224 | * \ \ | | |
| 225 | * \ \ | | |
| 226 | * \ \ | | |
| 227 | * \ \ | | |
| 228 | * \ \ | | |
| 229 | * \ \ | | |
| 230 | * -------------------------------- |
| 231 | * | | | | |
| 232 | * buffer-------------->| | ... | | |
| 233 | * | | | | |
| 234 | * -------------------------------- |
| 235 | * <-count#1->| | |
| 236 | * <---------- count#n --------> |
| 237 | * <---------- length ----------> |
| 238 | * |
| 239 | * Additionally, the IO driver has an underlying buffer that is at least |
| 240 | * one block-size and may be big enough to allow. |
| 241 | */ |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 242 | static int block_read(io_entity_t *entity, uintptr_t buffer, size_t length, |
| 243 | size_t *length_read) |
| 244 | { |
| 245 | block_dev_state_t *cur; |
| 246 | io_block_spec_t *buf; |
| 247 | io_block_ops_t *ops; |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 248 | int lba; |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 249 | size_t block_size, left; |
| 250 | size_t nbytes; /* number of bytes read in one iteration */ |
| 251 | size_t request; /* number of requested bytes in one iteration */ |
| 252 | size_t count; /* number of bytes already read */ |
| 253 | /* |
| 254 | * number of leading bytes from start of the block |
| 255 | * to the first byte to be read |
| 256 | */ |
| 257 | size_t skip; |
| 258 | |
| 259 | /* |
| 260 | * number of trailing bytes between the last byte |
| 261 | * to be read and the end of the block |
| 262 | */ |
| 263 | size_t padding; |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 264 | |
| 265 | assert(entity->info != (uintptr_t)NULL); |
| 266 | cur = (block_dev_state_t *)entity->info; |
| 267 | ops = &(cur->dev_spec->ops); |
| 268 | buf = &(cur->dev_spec->buffer); |
| 269 | block_size = cur->dev_spec->block_size; |
| 270 | assert((length <= cur->size) && |
| 271 | (length > 0) && |
| 272 | (ops->read != 0)); |
| 273 | |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 274 | /* |
| 275 | * We don't know the number of bytes that we are going |
| 276 | * to read in every iteration, because it will depend |
| 277 | * on the low level driver. |
| 278 | */ |
| 279 | count = 0; |
| 280 | for (left = length; left > 0; left -= nbytes) { |
Haojian Zhuang | f800ccf | 2016-07-28 10:15:32 +0800 | [diff] [blame] | 281 | /* |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 282 | * We must only request operations aligned to the block |
| 283 | * size. Therefore if file_pos is not block-aligned, |
| 284 | * we have to request the operation to start at the |
| 285 | * previous block boundary and skip the leading bytes. And |
| 286 | * similarly, the number of bytes requested must be a |
| 287 | * block size multiple |
Haojian Zhuang | f800ccf | 2016-07-28 10:15:32 +0800 | [diff] [blame] | 288 | */ |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 289 | skip = cur->file_pos & (block_size - 1); |
Haojian Zhuang | f800ccf | 2016-07-28 10:15:32 +0800 | [diff] [blame] | 290 | |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 291 | /* |
| 292 | * Calculate the block number containing file_pos |
| 293 | * - e.g. block 3. |
| 294 | */ |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 295 | lba = (cur->file_pos + cur->base) / block_size; |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 296 | |
| 297 | if (skip + left > buf->length) { |
Haojian Zhuang | f800ccf | 2016-07-28 10:15:32 +0800 | [diff] [blame] | 298 | /* |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 299 | * The underlying read buffer is too small to |
| 300 | * read all the required data - limit to just |
| 301 | * fill the buffer, and then read again. |
Haojian Zhuang | f800ccf | 2016-07-28 10:15:32 +0800 | [diff] [blame] | 302 | */ |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 303 | request = buf->length; |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 304 | } else { |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 305 | /* |
| 306 | * The underlying read buffer is big enough to |
| 307 | * read all the required data. Calculate the |
| 308 | * number of bytes to read to align with the |
| 309 | * block size. |
| 310 | */ |
| 311 | request = skip + left; |
| 312 | request = (request + (block_size - 1)) & ~(block_size - 1); |
| 313 | } |
| 314 | request = ops->read(lba, buf->offset, request); |
| 315 | |
| 316 | if (request <= skip) { |
| 317 | /* |
| 318 | * We couldn't read enough bytes to jump over |
| 319 | * the skip bytes, so we should have to read |
| 320 | * again the same block, thus generating |
| 321 | * the same error. |
| 322 | */ |
| 323 | return -EIO; |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 324 | } |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 325 | |
| 326 | /* |
| 327 | * Need to remove skip and padding bytes,if any, from |
| 328 | * the read data when copying to the user buffer. |
| 329 | */ |
| 330 | nbytes = request - skip; |
| 331 | padding = (nbytes > left) ? nbytes - left : 0; |
| 332 | nbytes -= padding; |
| 333 | |
| 334 | memcpy((void *)(buffer + count), |
| 335 | (void *)(buf->offset + skip), |
| 336 | nbytes); |
| 337 | |
| 338 | cur->file_pos += nbytes; |
| 339 | count += nbytes; |
| 340 | } |
| 341 | assert(count == length); |
| 342 | *length_read = count; |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 343 | |
| 344 | return 0; |
| 345 | } |
| 346 | |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 347 | /* |
| 348 | * This function allows the caller to write any number of bytes |
| 349 | * from any position. It hides from the caller that the low level |
| 350 | * driver only can write aligned blocks of data. |
| 351 | * See comments for block_read for more details. |
| 352 | */ |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 353 | static int block_write(io_entity_t *entity, const uintptr_t buffer, |
| 354 | size_t length, size_t *length_written) |
| 355 | { |
| 356 | block_dev_state_t *cur; |
| 357 | io_block_spec_t *buf; |
| 358 | io_block_ops_t *ops; |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 359 | int lba; |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 360 | size_t block_size, left; |
| 361 | size_t nbytes; /* number of bytes read in one iteration */ |
| 362 | size_t request; /* number of requested bytes in one iteration */ |
| 363 | size_t count; /* number of bytes already read */ |
| 364 | /* |
| 365 | * number of leading bytes from start of the block |
| 366 | * to the first byte to be read |
| 367 | */ |
| 368 | size_t skip; |
| 369 | |
| 370 | /* |
| 371 | * number of trailing bytes between the last byte |
| 372 | * to be read and the end of the block |
| 373 | */ |
| 374 | size_t padding; |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 375 | |
| 376 | assert(entity->info != (uintptr_t)NULL); |
| 377 | cur = (block_dev_state_t *)entity->info; |
| 378 | ops = &(cur->dev_spec->ops); |
| 379 | buf = &(cur->dev_spec->buffer); |
| 380 | block_size = cur->dev_spec->block_size; |
| 381 | assert((length <= cur->size) && |
| 382 | (length > 0) && |
| 383 | (ops->read != 0) && |
| 384 | (ops->write != 0)); |
| 385 | |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 386 | /* |
| 387 | * We don't know the number of bytes that we are going |
| 388 | * to write in every iteration, because it will depend |
| 389 | * on the low level driver. |
| 390 | */ |
| 391 | count = 0; |
| 392 | for (left = length; left > 0; left -= nbytes) { |
Haojian Zhuang | f800ccf | 2016-07-28 10:15:32 +0800 | [diff] [blame] | 393 | /* |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 394 | * We must only request operations aligned to the block |
| 395 | * size. Therefore if file_pos is not block-aligned, |
| 396 | * we have to request the operation to start at the |
| 397 | * previous block boundary and skip the leading bytes. And |
| 398 | * similarly, the number of bytes requested must be a |
| 399 | * block size multiple |
Haojian Zhuang | f800ccf | 2016-07-28 10:15:32 +0800 | [diff] [blame] | 400 | */ |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 401 | skip = cur->file_pos & (block_size - 1); |
Haojian Zhuang | f800ccf | 2016-07-28 10:15:32 +0800 | [diff] [blame] | 402 | |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 403 | /* |
| 404 | * Calculate the block number containing file_pos |
| 405 | * - e.g. block 3. |
| 406 | */ |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 407 | lba = (cur->file_pos + cur->base) / block_size; |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 408 | |
| 409 | if (skip + left > buf->length) { |
| 410 | /* |
| 411 | * The underlying read buffer is too small to |
| 412 | * read all the required data - limit to just |
| 413 | * fill the buffer, and then read again. |
| 414 | */ |
| 415 | request = buf->length; |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 416 | } else { |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 417 | /* |
| 418 | * The underlying read buffer is big enough to |
| 419 | * read all the required data. Calculate the |
| 420 | * number of bytes to read to align with the |
| 421 | * block size. |
| 422 | */ |
| 423 | request = skip + left; |
| 424 | request = (request + (block_size - 1)) & ~(block_size - 1); |
| 425 | } |
| 426 | |
| 427 | /* |
| 428 | * The number of bytes that we are going to write |
| 429 | * from the user buffer will depend of the size |
| 430 | * of the current request. |
| 431 | */ |
| 432 | nbytes = request - skip; |
| 433 | padding = (nbytes > left) ? nbytes - left : 0; |
| 434 | nbytes -= padding; |
| 435 | |
| 436 | /* |
| 437 | * If we have skip or padding bytes then we have to preserve |
| 438 | * some content and it means that we have to read before |
| 439 | * writing |
| 440 | */ |
| 441 | if (skip > 0 || padding > 0) { |
| 442 | request = ops->read(lba, buf->offset, request); |
| 443 | /* |
| 444 | * The read may return size less than |
| 445 | * requested. Round down to the nearest block |
| 446 | * boundary |
| 447 | */ |
| 448 | request &= ~(block_size-1); |
| 449 | if (request <= skip) { |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 450 | /* |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 451 | * We couldn't read enough bytes to jump over |
| 452 | * the skip bytes, so we should have to read |
| 453 | * again the same block, thus generating |
| 454 | * the same error. |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 455 | */ |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 456 | return -EIO; |
| 457 | } |
| 458 | nbytes = request - skip; |
| 459 | padding = (nbytes > left) ? nbytes - left : 0; |
| 460 | nbytes -= padding; |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 461 | } |
Roberto Vargas | 034c9f8 | 2017-11-23 12:03:46 +0000 | [diff] [blame] | 462 | |
| 463 | memcpy((void *)(buf->offset + skip), |
| 464 | (void *)(buffer + count), |
| 465 | nbytes); |
| 466 | |
| 467 | request = ops->write(lba, buf->offset, request); |
| 468 | if (request <= skip) |
| 469 | return -EIO; |
| 470 | |
| 471 | /* |
| 472 | * And the previous write operation may modify the size |
| 473 | * of the request, so again, we have to calculate the |
| 474 | * number of bytes that we consumed from the user |
| 475 | * buffer |
| 476 | */ |
| 477 | nbytes = request - skip; |
| 478 | padding = (nbytes > left) ? nbytes - left : 0; |
| 479 | nbytes -= padding; |
| 480 | |
| 481 | cur->file_pos += nbytes; |
| 482 | count += nbytes; |
| 483 | } |
| 484 | assert(count == length); |
| 485 | *length_written = count; |
| 486 | |
Haojian Zhuang | 12ade16 | 2016-03-18 15:14:19 +0800 | [diff] [blame] | 487 | return 0; |
| 488 | } |
| 489 | |
| 490 | static int block_close(io_entity_t *entity) |
| 491 | { |
| 492 | entity->info = (uintptr_t)NULL; |
| 493 | return 0; |
| 494 | } |
| 495 | |
| 496 | static int block_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info) |
| 497 | { |
| 498 | block_dev_state_t *cur; |
| 499 | io_block_spec_t *buffer; |
| 500 | io_dev_info_t *info; |
| 501 | size_t block_size; |
| 502 | int result; |
| 503 | |
| 504 | assert(dev_info != NULL); |
| 505 | result = allocate_dev_info(&info); |
| 506 | if (result) |
| 507 | return -ENOENT; |
| 508 | |
| 509 | cur = (block_dev_state_t *)info->info; |
| 510 | /* dev_spec is type of io_block_dev_spec_t. */ |
| 511 | cur->dev_spec = (io_block_dev_spec_t *)dev_spec; |
| 512 | buffer = &(cur->dev_spec->buffer); |
| 513 | block_size = cur->dev_spec->block_size; |
| 514 | assert((block_size > 0) && |
| 515 | (is_power_of_2(block_size) != 0) && |
| 516 | ((buffer->offset % block_size) == 0) && |
| 517 | ((buffer->length % block_size) == 0)); |
| 518 | |
| 519 | *dev_info = info; /* cast away const */ |
| 520 | (void)block_size; |
| 521 | (void)buffer; |
| 522 | return 0; |
| 523 | } |
| 524 | |
| 525 | static int block_dev_close(io_dev_info_t *dev_info) |
| 526 | { |
| 527 | return free_dev_info(dev_info); |
| 528 | } |
| 529 | |
| 530 | /* Exported functions */ |
| 531 | |
| 532 | /* Register the Block driver with the IO abstraction */ |
| 533 | int register_io_dev_block(const io_dev_connector_t **dev_con) |
| 534 | { |
| 535 | int result; |
| 536 | |
| 537 | assert(dev_con != NULL); |
| 538 | |
| 539 | /* |
| 540 | * Since dev_info isn't really used in io_register_device, always |
| 541 | * use the same device info at here instead. |
| 542 | */ |
| 543 | result = io_register_device(&dev_info_pool[0]); |
| 544 | if (result == 0) |
| 545 | *dev_con = &block_dev_connector; |
| 546 | return result; |
| 547 | } |