usbdrv Implement write transfers

kernel/device/storage/usbdrv.c

@@ -20,6 +20,11 @@
  * https://www.seagate.com/files/staticfiles/support/docs/manual/Interface%20manuals/100293068j.pdf
  */
 
+#define SCSI_TEST_UNIT_READY 0x00
+#define SCSI_READ_CAPACITY10 0x25
+#define SCSI_READ10          0x28
+#define SCSI_WRITE10         0x2A
+
 static int usb_ms_send_cbw (struct xhci* xhci, struct xhci_usb_device* usb_device,
                             uint8_t bulk_out_endpoint, uintptr_t cbw_phys, uint64_t* lockflags) {
   return xhci_bulk_transfer (xhci, usb_device, bulk_out_endpoint, cbw_phys,
@@ -33,6 +38,13 @@ static int usb_ms_read_data (struct xhci* xhci, struct xhci_usb_device* usb_devi
                              lockflags);
 }
 
+static int usb_ms_write_data (struct xhci* xhci, struct xhci_usb_device* usb_device,
+                              uint8_t bulk_out_endpoint, uintptr_t buffer_phys, size_t buffer_size,
+                              uint64_t* lockflags) {
+  return xhci_bulk_transfer (xhci, usb_device, bulk_out_endpoint, buffer_phys, buffer_size,
+                             lockflags);
+}
+
 static int usb_ms_read_csw (struct xhci* xhci, struct xhci_usb_device* usb_device,
                             uint8_t bulk_in_endpoint, uintptr_t csw_phys, uint64_t* lockflags) {
   return xhci_bulk_transfer (xhci, usb_device, bulk_in_endpoint, csw_phys,
@@ -59,7 +71,7 @@ static int usb_ms_scsi_read_capacity (struct xhci* xhci, struct xhci_usb_device*
   memset (csw, 0, PAGE_SIZE);
 
   uint8_t cdb[10] = {
-      0x25, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+      SCSI_READ_CAPACITY10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
   };
 
   cbw->signature = USB_CBW_SIGNATURE;
@@ -119,7 +131,7 @@ static int usb_ms_scsi_test_unit_ready (struct xhci* xhci, struct xhci_usb_devic
   memset (csw, 0, PAGE_SIZE);
 
   uint8_t cdb[10] = {
-      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+      SCSI_TEST_UNIT_READY, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
   };
 
   cbw->signature = USB_CBW_SIGNATURE;
@@ -172,7 +184,7 @@ static int usb_ms_scsi_read (struct xhci* xhci, struct xhci_usb_device* usb_devi
   memset (csw, 0, PAGE_SIZE);
 
   uint8_t cdb[10] = {
-      0x28,
+      SCSI_READ10,
       0x00,
       /* LBA */
       (uint8_t)((lba & 0xFF000000) >> 24),
@@ -224,6 +236,73 @@ done:
   return ret;
 }
 
+static int usb_ms_scsi_write (struct xhci* xhci, struct xhci_usb_device* usb_device,
+                              uint8_t bulk_in_endpoint, uint8_t bulk_out_endpoint,
+                              uint8_t* in_buffer, uint32_t lba, uint16_t sector_count,
+                              size_t sector_size, uint64_t* lockflags) {
+  struct limine_hhdm_response* hhdm = limine_hhdm_request.response;
+  int ret;
+
+  uintptr_t cbw_phys = pmm_alloc (1);
+  struct usb_ms_cbw* cbw = (struct usb_ms_cbw*)(cbw_phys + (uintptr_t)hhdm->offset);
+  memset (cbw, 0, PAGE_SIZE);
+
+  uintptr_t buffer_phys = (uintptr_t)in_buffer - (uintptr_t)hhdm->offset;
+
+  uintptr_t csw_phys = pmm_alloc (1);
+  struct usb_ms_csw* csw = (struct usb_ms_csw*)(csw_phys + (uintptr_t)hhdm->offset);
+  memset (csw, 0, PAGE_SIZE);
+
+  uint8_t cdb[10] = {
+      SCSI_WRITE10,
+      0x00,
+      /* LBA */
+      (uint8_t)((lba & 0xFF000000) >> 24),
+      (uint8_t)((lba & 0x00FF0000) >> 16),
+      (uint8_t)((lba & 0x0000FF00) >> 8),
+      (uint8_t)((lba & 0xFF)),
+      0x00,
+      /* Transfer Length */
+      (uint8_t)((sector_count & 0xFF00) >> 8),
+      (uint8_t)((sector_count & 0x00FF)),
+      0x00,
+  };
+
+  cbw->signature = USB_CBW_SIGNATURE;
+  cbw->tag = 0x12345678;
+  cbw->dir = 0x00;
+  cbw->lun = 0;
+  cbw->cmd_len = sizeof (cdb);
+  cbw->length = sector_count * sector_size;
+  memcpy (cbw->data, cdb, sizeof (cdb));
+
+  if ((ret = usb_ms_send_cbw (xhci, usb_device, bulk_out_endpoint, cbw_phys, lockflags)) < 0) {
+    goto done;
+  }
+
+  if ((ret = usb_ms_write_data (xhci, usb_device, bulk_out_endpoint, buffer_phys,
+                                sector_count * sector_size, lockflags)) < 0) {
+    goto done;
+  }
+
+  if ((ret = usb_ms_read_csw (xhci, usb_device, bulk_in_endpoint, csw_phys, lockflags)) < 0) {
+    goto done;
+  }
+
+  if (csw->status != 0 || csw->signature != USB_CSW_SIGNATURE) {
+    DEBUG ("Failed to read sectors. CSW status=%u residue=%u\n", csw->status, csw->residue);
+    ret = -ST_USB_CTRL_ERROR;
+    goto done;
+  }
+
+  ret = ST_OK;
+
+done:
+  pmm_free (cbw_phys, 1);
+  pmm_free (csw_phys, 1);
+  return ret;
+}
+
 static void usbdrv_setup_endpoints (struct usbdrv* usbdrv) {
   for (size_t ep = 0; ep < usbdrv->usb_device->endpoints_count; ep++) {
     struct xhci_usb_device_endpoint* endpoint = &usbdrv->usb_device->endpoints[ep];
@@ -279,7 +358,32 @@ DEFINE_DEVICE_OP (usbdrv_read) {
   return ret;
 }
 
-DEFINE_DEVICE_OP (usbdrv_write) { return -ST_XDRV_WRITE_ERROR; }
+DEFINE_DEVICE_OP (usbdrv_write) {
+  uint64_t fd;
+
+  if (a1 == NULL || a2 == NULL || a3 == NULL)
+    return -ST_BAD_ADDRESS_SPACE;
+
+  size_t sector = *(size_t*)a1;
+  size_t sector_count = *(size_t*)a2;
+  uint8_t* buffer = a3;
+
+  struct usbdrv* usbdrv = device->udata;
+
+  spin_unlock (&device->lock, *lockflags);
+  spin_lock (&usbdrv->xhci->device->lock, &fd);
+  spin_lock (&device->lock, lockflags);
+
+  int ret = usb_ms_scsi_write (usbdrv->xhci, usbdrv->usb_device, usbdrv->bulk_in.endpoint_addr,
+                               usbdrv->bulk_out.endpoint_addr, buffer, sector, sector_count,
+                               usbdrv->sector_size, &fd);
+
+  spin_unlock (&device->lock, *lockflags);
+  spin_unlock (&usbdrv->xhci->device->lock, fd);
+  spin_lock (&device->lock, lockflags);
+
+  return ret;
+}
 
 DEFINE_DEVICE_OP (usbdrv_get_device_type) {
   if (a1 == NULL)