system/modules/cdi/storage.c

   1 /*
   2 Copyright (C) 2008  Mathias Gottschlag
   3
   4 Permission is hereby granted, free of charge, to any person obtaining a copy of
   5 this software and associated documentation files (the "Software"), to deal in the
   6 Software without restriction, including without limitation the rights to use,
   7 copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
   8 Software, and to permit persons to whom the Software is furnished to do so,
   9 subject to the following conditions:
  10
  11 The above copyright notice and this permission notice shall be included in all
  12 copies or substantial portions of the Software.
  13
  14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
  15 INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
  16 PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
  17 HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
  18 OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
  19 SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  20 */
  21
  22 #include <cdi/storage.h>
  23 #include <fs/devfs.h>
  24 #include <fs/request.h>
  25 #include <stdlib.h>
  26 #include <string.h>
  27 #include <ke/spinlock.h>
  28
  29 struct storage_device
  30 {
  31         FsDeviceFile file;
  32         struct cdi_storage_device *device;
  33         KeThread *thread;
  34         KeSpinlock request_lock;
  35         cdi_list_t requests;
  36 };
  37
  38 static cdi_list_t msdevices = 0;
  39
  40 static uint32_t cdi_storage_read(struct storage_device *msd, uint32_t offset, uint32_t length, void *buffer)
  41 {
  42         struct cdi_storage_device *device = msd->device;
  43         struct cdi_storage_driver *driver = (struct cdi_storage_driver*)device->dev.driver;
  44
  45         size_t block_size = device->block_size;
  46         // Blocks to be read
  47         uint64_t block_read_start = offset / block_size;
  48         uint64_t block_read_end = (offset + length) / block_size;
  49         uint64_t block_read_count = block_read_end - block_read_start;
  50
  51         if (((offset % block_size) == 0) && (((offset + length) %  block_size) == 0))
  52         {
  53                 // Only read whole blocks
  54                 kePrint("Reading blocks.\n");
  55                 if (driver->read_blocks(device, block_read_start, block_read_count, buffer) != 0)
  56                 {
  57                         kePrint("Could not read blocks.\n");
  58                         return -1;
  59                 }
  60                 kePrint("Read blocks.\n");
  61                 return length;
  62         }
  63         else
  64         {
  65                 // TODO: Optimize this
  66                 block_read_count++;
  67                 uint8_t buffer[block_read_count * block_size];
  68
  69                 // Buffer data
  70                 if (driver->read_blocks(device, block_read_start, block_read_count, buffer) != 0)
  71                 {
  72                         return -1;
  73                 }
  74
  75                 // Copy data from buffer
  76                 memcpy(buffer, buffer + (offset % block_size), length);
  77         }
  78         return length;
  79 }
  80 static uint32_t cdi_storage_write(struct storage_device *msd, uint32_t offset, uint32_t length, void *data)
  81 {
  82         struct cdi_storage_device *device = msd->device;
  83         struct cdi_storage_driver *driver = (struct cdi_storage_driver*)device->dev.driver;
  84
  85         size_t block_size = device->block_size;
  86         uint64_t block_write_start = offset / block_size;
  87         uint8_t buffer[block_size];
  88         size_t blockoffset;
  89         size_t tmp_size;
  90
  91         // Wenn die Startposition nicht auf einer Blockgrenze liegt, muessen wir
  92         // hier zuerst den ersten Block laden, die gewuenschten Aenderungen machen,
  93         // und den Block wieder Speichern.
  94         blockoffset = (offset % block_size);
  95         if (blockoffset != 0)
  96         {
  97                 tmp_size = block_size - blockoffset;
  98                 tmp_size = (tmp_size > length ? length : tmp_size);
  99
 100                 if (driver->read_blocks(device, block_write_start, 1, buffer) != 0) {
 101                         return -1;
 102                 }
 103                 memcpy(data + blockoffset, data, tmp_size);
 104
 105                 // Buffer abspeichern
 106                 if (driver->write_blocks(device, block_write_start, 1, buffer) != 0)
 107                 {
 108                         return -1;
 109                 }
 110
 111                 length -= tmp_size;
 112                 data += tmp_size;
 113                 block_write_start++;
 114         }
 115
 116         // Jetzt wird die Menge der ganzen Blocks auf einmal geschrieben, falls
 117         // welche existieren
 118         tmp_size = length / block_size;
 119         if (tmp_size != 0) {
 120                 // Buffer abspeichern
 121                 if (driver->write_blocks(device, block_write_start, tmp_size, data) != 0)
 122                 {
 123                         return -1;
 124                 }
 125                 length -= tmp_size * block_size;
 126                 data += tmp_size * block_size;
 127                 block_write_start += block_size;
 128         }
 129
 130         // Wenn der letzte Block nur teilweise beschrieben wird, geschieht das hier
 131         if (length != 0) {
 132                 // Hier geschieht fast das Selbe wie oben beim ersten Block
 133                 if (driver->read_blocks(device, block_write_start, 1, buffer) != 0) {
 134                         return -1;
 135                 }
 136                 memcpy(data, buffer, length);
 137
 138                 // Buffer abspeichern
 139                 if (driver->write_blocks(device, block_write_start, 1, buffer) != 0) {
 140                         return -1;
 141                 }
 142         }
 143         return 0;
 144 }
 145 static void cdi_storage_perform_request(struct storage_device *device,
 146         FsRequest *request)
 147 {
 148         switch (request->type)
 149         {
 150                 case FS_REQUEST_READ:
 151                         request->return_value = cdi_storage_read(device, request->offset, request->bufferlength, request->buffer);
 152                         fsFinishRequest(request);
 153                         break;
 154                 case FS_REQUEST_WRITE:
 155                         request->return_value = cdi_storage_write(device, request->offset, request->bufferlength, request->buffer);
 156                         fsFinishRequest(request);
 157                         break;
 158                 case FS_REQUEST_IOCTL:
 159                         kePrint("IOCTL.\n");
 160                         fsFinishRequest(request);
 161                         break;
 162                 default:
 163                         fsFinishRequest(request);
 164         }
 165 }
 166
 167 static void cdi_storage_thread(struct storage_device *device)
 168 {
 169         //kePrint("Worker thread: %s\n", device->file.path);
 170         while (1)
 171         {
 172                 // Process requests
 173                 keLockSpinlock(&device->request_lock);
 174                 while (cdi_list_size(device->requests) > 0)
 175                 {
 176                         FsRequest *request = cdi_list_pop(device->requests);
 177                         keUnlockSpinlock(&device->request_lock);
 178
 179                         kePrint("Performing request on %s\n", device->file.path);
 180                         cdi_storage_perform_request(device, request);
 181
 182                         keLockSpinlock(&device->request_lock);
 183                 }
 184                 // Wait until next request
 185                 //kePrint("Pausing worker thread.\n");
 186                 device->thread->status = KE_THREAD_FSREQUEST;
 187                 keUnlockSpinlock(&device->request_lock);
 188                 // Schedule out of the thread
 189                 asm volatile("int $0x32");
 190                 //kePrint("Thread woke up.\n");
 191         }
 192 }
 193
 194 static int cdi_storage_request(struct FsDeviceFile *file, FsRequest *request)
 195 {
 196         struct storage_device *device = (struct storage_device*)file;
 197         switch (request->type)
 198         {
 199                 case FS_REQUEST_READ:
 200                 case FS_REQUEST_WRITE:
 201                 case FS_REQUEST_IOCTL:
 202                         keLockSpinlock(&device->request_lock);
 203                         device->requests = cdi_list_push(device->requests, request);
 204                         if (device->thread->status == KE_THREAD_FSREQUEST)
 205                         {
 206                                 kePrint("Waking up thread.\n");
 207                                 device->thread->status = KE_THREAD_RUNNING;
 208                         }
 209                         keUnlockSpinlock(&device->request_lock);
 210                         return 0;
 211                 case FS_REQUEST_SEEK:
 212                         // TODO
 213                         break;
 214                 default:
 215                         return -1;
 216         }
 217         return -1;
 218 }
 219
 220 void cdi_storage_driver_init(struct cdi_storage_driver *driver)
 221 {
 222         cdi_driver_init(&driver->drv);
 223 }
 224
 225 void cdi_storage_driver_destroy(struct cdi_storage_driver *driver)
 226 {
 227         cdi_driver_destroy(&driver->drv);
 228 }
 229
 230 void cdi_storage_driver_register(struct cdi_storage_driver *driver)
 231 {
 232         uint32_t i;
 233         for (i = 0; i < cdi_list_size(driver->drv.devices); i++)
 234         {
 235                 // Create device file
 236                 struct cdi_storage_device *device = cdi_list_get(driver->drv.devices, i);
 237                 struct storage_device *sd = malloc(sizeof(struct storage_device));
 238                 sd->device = device;
 239                 memset(&sd->file, 0, sizeof(sd->file));
 240                 sd->file.path = device->dev.name;
 241                 sd->file.query_request = cdi_storage_request;
 242                 keInitSpinlock(&sd->request_lock);
 243                 sd->requests = cdi_list_create();
 244                 sd->thread = keCreateKernelThread((uintptr_t)cdi_storage_thread, 1, sd);
 245                 fsCreateDeviceFile(&sd->file);
 246                 if (!msdevices) msdevices = cdi_list_create();
 247                 msdevices = cdi_list_push(msdevices, sd);
 248         }
 249
 250         cdi_driver_register(&driver->drv);
 251 }
 252