payloads/libpayload/drivers/storage/nvme.c

   1 // SPDX-License-Identifier: BSD-3-Clause
   2 /*
   3  * Libpayload NVMe device driver
   4  * Copyright (C) 2019 secunet Security Networks AG
   5  */
   6
   7 #include <stdlib.h>
   8 #include <stdint.h>
   9 #include <stdio.h>
  10 #include <pci.h>
  11 #include <pci/pci.h>
  12 #include <libpayload.h>
  13 #include <storage/storage.h>
  14 #include <storage/nvme.h>
  15
  16 #define NVME_CC_EN      (1 <<  0)
  17 #define NVME_CC_CSS     (0 <<  4)
  18 #define NVME_CC_MPS     (0 <<  7)
  19 #define NVME_CC_AMS     (0 << 11)
  20 #define NVME_CC_SHN     (0 << 14)
  21 #define NVME_CC_IOSQES  (6 << 16)
  22 #define NVME_CC_IOCQES  (4 << 20)
  23
  24 #define NVME_QUEUE_SIZE 2
  25 #define NVME_SQ_ENTRY_SIZE 64
  26 #define NVME_CQ_ENTRY_SIZE 16
  27
  28 struct nvme_dev {
  29         storage_dev_t storage_dev;
  30
  31         pcidev_t pci_dev;
  32         void *config;
  33         struct {
  34                 void *base;
  35                 uint32_t *bell;
  36                 uint16_t idx; // bool pos 0 or 1
  37                 uint16_t round; // bool round 0 or 1+0xd
  38         } queue[4];
  39
  40         uint64_t *prp_list;
  41 };
  42
  43
  44 struct nvme_s_queue_entry {
  45         uint32_t dw[16];
  46 };
  47
  48 struct nvme_c_queue_entry {
  49         uint32_t dw[4];
  50 };
  51
  52 enum nvme_queue {
  53         NVME_ADMIN_QUEUE = 0,
  54         ads = 0,
  55         adc = 1,
  56         NVME_IO_QUEUE = 2,
  57         ios = 2,
  58         ioc = 3,
  59 };
  60
  61 static storage_poll_t nvme_poll(struct storage_dev *dev)
  62 {
  63         return POLL_MEDIUM_PRESENT;
  64 }
  65
  66 static int nvme_cmd(
  67                 struct nvme_dev *nvme, enum nvme_queue q, const struct nvme_s_queue_entry *cmd)
  68 {
  69         int sq = q, cq = q+1;
  70
  71         void *s_entry = nvme->queue[sq].base + (nvme->queue[sq].idx * NVME_SQ_ENTRY_SIZE);
  72         memcpy(s_entry, cmd, NVME_SQ_ENTRY_SIZE);
  73         nvme->queue[sq].idx = (nvme->queue[sq].idx + 1) & (NVME_QUEUE_SIZE - 1);
  74         write32(nvme->queue[sq].bell, nvme->queue[sq].idx);
  75
  76         struct nvme_c_queue_entry *c_entry = nvme->queue[cq].base +
  77                 (nvme->queue[cq].idx * NVME_CQ_ENTRY_SIZE);
  78         while (((read32(&c_entry->dw[3]) >> 16) & 0x1) == nvme->queue[cq].round)
  79                 ;
  80         nvme->queue[cq].idx = (nvme->queue[cq].idx + 1) & (NVME_QUEUE_SIZE - 1);
  81         write32(nvme->queue[cq].bell, nvme->queue[cq].idx);
  82         if (nvme->queue[cq].idx == 0)
  83                 nvme->queue[cq].round = (nvme->queue[cq].round + 1) & 1;
  84         return c_entry->dw[3] >> 17;
  85 }
  86
  87 static int delete_io_submission_queue(struct nvme_dev *nvme)
  88 {
  89         const struct nvme_s_queue_entry e = {
  90                 .dw[0]  = 0,
  91                 .dw[10] = ios,
  92         };
  93
  94         int res = nvme_cmd(nvme, NVME_ADMIN_QUEUE, &e);
  95
  96         free(nvme->queue[ios].base);
  97         nvme->queue[ios].base = NULL;
  98         nvme->queue[ios].bell = NULL;
  99         nvme->queue[ios].idx  = 0;
 100         return res;
 101 }
 102
 103 static int delete_io_completion_queue(struct nvme_dev *nvme)
 104 {
 105         const struct nvme_s_queue_entry e = {
 106                 .dw[0]  = 1,
 107                 .dw[10] = ioc,
 108         };
 109
 110         int res = nvme_cmd(nvme, NVME_ADMIN_QUEUE, &e);
 111         free(nvme->queue[ioc].base);
 112
 113         nvme->queue[ioc].base  = NULL;
 114         nvme->queue[ioc].bell  = NULL;
 115         nvme->queue[ioc].idx   = 0;
 116         nvme->queue[ioc].round = 0;
 117         return res;
 118 }
 119
 120 static int delete_admin_queues(struct nvme_dev *nvme)
 121 {
 122         if (nvme->queue[ios].base || nvme->queue[ioc].base)
 123                 printf("NVMe ERROR: IO queues still active.\n");
 124
 125         free(nvme->queue[ads].base);
 126         nvme->queue[ads].base = NULL;
 127         nvme->queue[ads].bell = NULL;
 128         nvme->queue[ads].idx  = 0;
 129
 130         free(nvme->queue[adc].base);
 131         nvme->queue[adc].base = NULL;
 132         nvme->queue[adc].bell = NULL;
 133         nvme->queue[adc].idx  = 0;
 134         nvme->queue[adc].round = 0;
 135
 136         return 0;
 137 }
 138
 139 static void nvme_detach_device(struct storage_dev *dev)
 140 {
 141         struct nvme_dev *nvme = (struct nvme_dev *)dev;
 142
 143         if (delete_io_submission_queue(nvme))
 144                 printf("NVMe ERROR: Failed to delete io submission queue\n");
 145         if (delete_io_completion_queue(nvme))
 146                 printf("NVME ERROR: Failed to delete io completion queue\n");
 147         if (delete_admin_queues(nvme))
 148                 printf("NVME ERROR: Failed to delete admin queues\n");
 149
 150         write32(nvme->config + 0x14, 0);
 151
 152         int status, timeout = (read64(nvme->config) >> 24 & 0xff) * 500;
 153         do {
 154                 status = read32(nvme->config + 0x1c) & 0x3;
 155                 if (status == 0x2) {
 156                         printf("NVMe ERROR: Failed to disable controller. FATAL ERROR\n");
 157                         break;
 158                 }
 159                 if (timeout < 0) {
 160                         printf("NVMe ERROR: Failed to disable controller. Timeout.\n");
 161                         break;
 162                 }
 163                 timeout -= 10;
 164                 mdelay(10);
 165         } while (status != 0x0);
 166
 167         uint16_t command = pci_read_config16(nvme->pci_dev, PCI_COMMAND);
 168         pci_write_config16(nvme->pci_dev, PCI_COMMAND, command & ~PCI_COMMAND_MASTER);
 169
 170         free(nvme->prp_list);
 171 }
 172
 173 static int nvme_read(struct nvme_dev *nvme, unsigned char *buffer, uint64_t base, uint16_t count)
 174 {
 175         if (count == 0 || count > 512)
 176                 return -1;
 177
 178         struct nvme_s_queue_entry e = {
 179                 .dw[0] = 0x02,
 180                 .dw[1] = 0x1,
 181                 .dw[6] = virt_to_phys(buffer),
 182                 .dw[10] = base,
 183                 .dw[11] = base >> 32,
 184                 .dw[12] = count - 1,
 185         };
 186
 187         const unsigned int start_page = (uintptr_t)buffer >> 12;
 188         const unsigned int end_page = ((uintptr_t)buffer + count * 512 - 1) >> 12;
 189         if (end_page == start_page) {
 190                 /* No page crossing, PRP2 is reserved */
 191         } else if (end_page == start_page + 1) {
 192                 /* Crossing exactly one page boundary, PRP2 is second page */
 193                 e.dw[8] = virt_to_phys(buffer + 0x1000) & ~0xfff;
 194         } else {
 195                 /* Use a single page as PRP list, PRP2 points to the list */
 196                 unsigned int i;
 197                 for (i = 0; i < end_page - start_page; ++i) {
 198                         buffer += 0x1000;
 199                         nvme->prp_list[i] = virt_to_phys(buffer) & ~0xfff;
 200                 }
 201                 e.dw[8] = virt_to_phys(nvme->prp_list);
 202         }
 203
 204         return nvme_cmd(nvme, ios, &e);
 205 }
 206
 207 static ssize_t nvme_read_blocks512(
 208                 struct storage_dev *const dev,
 209                 const lba_t start, const size_t count, unsigned char *const buf)
 210 {
 211         unsigned int off = 0;
 212         while (off < count) {
 213                 const unsigned int blocks = MIN(count - off, 512);
 214                 if (nvme_read((struct nvme_dev *)dev, buf + (off * 512), start + off, blocks))
 215                         return off;
 216                 off += blocks;
 217         }
 218         return count;
 219 }
 220
 221 static int create_io_submission_queue(struct nvme_dev *nvme)
 222 {
 223         void *sq_buffer = memalign(0x1000, NVME_SQ_ENTRY_SIZE * NVME_QUEUE_SIZE);
 224         if (!sq_buffer) {
 225                 printf("NVMe ERROR: Failed to allocate memory for io submission queue.\n");
 226                 return -1;
 227         }
 228         memset(sq_buffer, 0, NVME_SQ_ENTRY_SIZE * NVME_QUEUE_SIZE);
 229
 230         struct nvme_s_queue_entry e = {
 231                 .dw[0]  = 0x01,
 232                 .dw[6]  = virt_to_phys(sq_buffer),
 233                 .dw[10] = ((NVME_QUEUE_SIZE - 1) << 16) | ios >> 1,
 234                 .dw[11] = (1 << 16) | 1,
 235         };
 236
 237         int res = nvme_cmd(nvme, NVME_ADMIN_QUEUE, &e);
 238         if (res) {
 239                 printf("NVMe ERROR: nvme_cmd returned with %i.\n", res);
 240                 free(sq_buffer);
 241                 return res;
 242         }
 243
 244         uint8_t cap_dstrd = (read64(nvme->config) >> 32) & 0xf;
 245         nvme->queue[ios].base = sq_buffer;
 246         nvme->queue[ios].bell = nvme->config + 0x1000 + (ios * (4 << cap_dstrd));
 247         nvme->queue[ios].idx = 0;
 248         return 0;
 249 }
 250
 251 static int create_io_completion_queue(struct nvme_dev *nvme)
 252 {
 253         void *const cq_buffer = memalign(0x1000, NVME_CQ_ENTRY_SIZE * NVME_QUEUE_SIZE);
 254         if (!cq_buffer) {
 255                 printf("NVMe ERROR: Failed to allocate memory for io completion queue.\n");
 256                 return -1;
 257         }
 258         memset(cq_buffer, 0, NVME_CQ_ENTRY_SIZE * NVME_QUEUE_SIZE);
 259
 260         const struct nvme_s_queue_entry e = {
 261                 .dw[0]  = 0x05,
 262                 .dw[6]  = virt_to_phys(cq_buffer),
 263                 .dw[10] = ((NVME_QUEUE_SIZE - 1) << 16) | ioc >> 1,
 264                 .dw[11] = 1,
 265         };
 266
 267         int res = nvme_cmd(nvme, NVME_ADMIN_QUEUE, &e);
 268         if (res) {
 269                 printf("NVMe ERROR: nvme_cmd returned with %i.\n", res);
 270                 free(cq_buffer);
 271                 return res;
 272         }
 273
 274         uint8_t cap_dstrd = (read64(nvme->config) >> 32) & 0xf;
 275         nvme->queue[ioc].base  = cq_buffer;
 276         nvme->queue[ioc].bell  = nvme->config + 0x1000 + (ioc * (4 << cap_dstrd));
 277         nvme->queue[ioc].idx   = 0;
 278         nvme->queue[ioc].round = 0;
 279
 280         return 0;
 281 }
 282
 283 static int create_admin_queues(struct nvme_dev *nvme)
 284 {
 285         uint8_t cap_dstrd = (read64(nvme->config) >> 32) & 0xf;
 286         write32(nvme->config + 0x24, (NVME_QUEUE_SIZE - 1) << 16 | (NVME_QUEUE_SIZE - 1));
 287
 288         void *sq_buffer = memalign(0x1000, NVME_SQ_ENTRY_SIZE * NVME_QUEUE_SIZE);
 289         if (!sq_buffer) {
 290                 printf("NVMe ERROR: Failed to allocated memory for admin submission queue\n");
 291                 return -1;
 292         }
 293         memset(sq_buffer, 0, NVME_SQ_ENTRY_SIZE * NVME_QUEUE_SIZE);
 294         write64(nvme->config + 0x28, virt_to_phys(sq_buffer));
 295
 296         nvme->queue[ads].base = sq_buffer;
 297         nvme->queue[ads].bell = nvme->config + 0x1000 + (ads * (4 << cap_dstrd));
 298         nvme->queue[ads].idx = 0;
 299
 300         void *cq_buffer = memalign(0x1000, NVME_CQ_ENTRY_SIZE * NVME_QUEUE_SIZE);
 301         if (!cq_buffer) {
 302                 printf("NVMe ERROR: Failed to allocate memory for admin completion queue\n");
 303                 free(cq_buffer);
 304                 return -1;
 305         }
 306         memset(cq_buffer, 0, NVME_CQ_ENTRY_SIZE * NVME_QUEUE_SIZE);
 307         write64(nvme->config + 0x30, virt_to_phys(cq_buffer));
 308
 309         nvme->queue[adc].base = cq_buffer;
 310         nvme->queue[adc].bell = nvme->config + 0x1000 + (adc * (4 << cap_dstrd));
 311         nvme->queue[adc].idx = 0;
 312         nvme->queue[adc].round = 0;
 313
 314         return 0;
 315 }
 316
 317 static void nvme_init(pcidev_t dev)
 318 {
 319         printf("NVMe init (Device %02x:%02x.%02x)\n",
 320                         PCI_BUS(dev), PCI_SLOT(dev), PCI_FUNC(dev));
 321
 322         void *pci_bar0 = phys_to_virt(pci_read_config32(dev, 0x10) & ~0x3ff);
 323
 324         if (!(read64(pci_bar0) >> 37 & 0x01)) {
 325                 printf("NVMe ERROR: PCIe device does not support the NVMe command set\n");
 326                 return;
 327         }
 328         struct nvme_dev *nvme = malloc(sizeof(*nvme));
 329         if (!nvme) {
 330                 printf("NVMe ERROR: Failed to allocate buffer for nvme driver struct\n");
 331                 return;
 332         }
 333         nvme->storage_dev.port_type             = PORT_TYPE_NVME;
 334         nvme->storage_dev.poll                  = nvme_poll;
 335         nvme->storage_dev.read_blocks512        = nvme_read_blocks512;
 336         nvme->storage_dev.write_blocks512       = NULL;
 337         nvme->storage_dev.detach_device         = nvme_detach_device;
 338         nvme->pci_dev                           = dev;
 339         nvme->config                            = pci_bar0;
 340         nvme->prp_list                          = memalign(0x1000, 0x1000);
 341
 342         if (!nvme->prp_list) {
 343                 printf("NVMe ERROR: Failed to allocate buffer for PRP list\n");
 344                 goto _free_abort;
 345         }
 346
 347         const uint32_t cc = NVME_CC_EN | NVME_CC_CSS | NVME_CC_MPS | NVME_CC_AMS | NVME_CC_SHN
 348                         | NVME_CC_IOSQES | NVME_CC_IOCQES;
 349
 350         write32(nvme->config + 0x14, 0);
 351
 352         int status, timeout = (read64(nvme->config) >> 24 & 0xff) * 500;
 353         do {
 354                 status = read32(nvme->config + 0x1c) & 0x3;
 355                 if (status == 0x2) {
 356                         printf("NVMe ERROR: Failed to disable controller. FATAL ERROR\n");
 357                         goto _free_abort;
 358                 }
 359                 if (timeout < 0) {
 360                         printf("NVMe ERROR: Failed to disable controller. Timeout.\n");
 361                         goto _free_abort;
 362                 }
 363                 timeout -= 10;
 364                 mdelay(10);
 365         } while (status != 0x0);
 366         if (create_admin_queues(nvme))
 367                 goto _free_abort;
 368         write32(nvme->config + 0x14, cc);
 369
 370         timeout = (read64(nvme->config) >> 24 & 0xff) * 500;
 371         do {
 372                 status = read32(nvme->config + 0x1c) & 0x3;
 373                 if (status == 0x2)
 374                         goto _delete_admin_abort;
 375                 if (timeout < 0)
 376                         goto _delete_admin_abort;
 377                 timeout -= 10;
 378                 mdelay(10);
 379         } while (status != 0x1);
 380
 381         uint16_t command = pci_read_config16(dev, PCI_COMMAND);
 382         pci_write_config16(dev, PCI_COMMAND, command | PCI_COMMAND_MASTER);
 383         if (create_io_completion_queue(nvme))
 384                 goto _delete_admin_abort;
 385         if (create_io_submission_queue(nvme))
 386                 goto _delete_completion_abort;
 387         storage_attach_device((storage_dev_t *)nvme);
 388         printf("NVMe init done.\n");
 389         return;
 390
 391 _delete_completion_abort:
 392         delete_io_completion_queue(nvme);
 393 _delete_admin_abort:
 394         delete_admin_queues(nvme);
 395 _free_abort:
 396         free(nvme->prp_list);
 397         free(nvme);
 398         printf("NVMe init failed.\n");
 399 }
 400
 401 void nvme_initialize(struct pci_dev *dev)
 402 {
 403         nvme_init(PCI_DEV(dev->bus, dev->dev, dev->func));
 404 }