From 7f14d97e72d96b4cb23a666b4ea70737f23a1773 Mon Sep 17 00:00:00 2001 From: Toomas Soome Date: Tue, 15 Oct 2019 06:19:33 +0000 Subject: [PATCH] loader.efi: efipart needs to use ioalign UEFI specification 2.7A, EFI_BLOCK_IO_PROTOCOL, page 566. The ioalign property does define the alignment of data buffer. If the alignment is required and our buffer is not aligned, or if the data buffer is not multiple of Blocksize, we need to use bounce buffer to perform the block IO. This is much like with BIOS version, except there the INT13 needs buffer to be located in low memory. MFS: r353501 MFC: r347195,350654-350656,351274,351630,351637,352421,352439,352443-352446,352451 Approved by: re (gjb) --- stand/efi/boot1/boot1.c | 15 +- stand/efi/include/efilib.h | 2 + stand/efi/libefi/devpath.c | 29 +- stand/efi/libefi/efinet.c | 2 +- stand/efi/libefi/efipart.c | 791 +++++++++++++++-------------- stand/efi/libefi/libefi.c | 7 + stand/efi/loader/arch/i386/efimd.c | 2 +- stand/efi/loader/efi_main.c | 2 +- stand/efi/loader/framebuffer.c | 3 +- stand/efi/loader/main.c | 8 +- stand/libsa/stand.h | 10 +- stand/libsa/zalloc.c | 336 ++++++------ stand/libsa/zalloc_defs.h | 29 +- stand/libsa/zalloc_malloc.c | 179 ++++--- stand/libsa/zalloc_mem.h | 23 +- stand/libsa/zalloc_protos.h | 9 +- 16 files changed, 779 insertions(+), 668 deletions(-) diff --git a/stand/efi/boot1/boot1.c b/stand/efi/boot1/boot1.c index 93592a536b5..5d977850b51 100644 --- a/stand/efi/boot1/boot1.c +++ b/stand/efi/boot1/boot1.c @@ -246,8 +246,9 @@ try_boot(void) goto errout; } - if ((status = BS->HandleProtocol(loaderhandle, &LoadedImageGUID, - (VOID**)&loaded_image)) != EFI_SUCCESS) { + status = OpenProtocolByHandle(loaderhandle, &LoadedImageGUID, + (void **)&loaded_image); + if (status != EFI_SUCCESS) { printf("Failed to query LoadedImage provided by %s (%lu)\n", mod->name, EFI_ERROR_CODE(status)); goto errout; @@ -306,7 +307,7 @@ probe_handle(EFI_HANDLE h, EFI_DEVICE_PATH *imgpath, BOOLEAN *preferred) UINTN i; /* Figure out if we're dealing with an actual partition. */ - status = BS->HandleProtocol(h, &DevicePathGUID, (void **)&devpath); + status = OpenProtocolByHandle(h, &DevicePathGUID, (void **)&devpath); if (status == EFI_UNSUPPORTED) return (status); @@ -322,7 +323,7 @@ probe_handle(EFI_HANDLE h, EFI_DEVICE_PATH *imgpath, BOOLEAN *preferred) efi_free_devpath_name(text); } #endif - status = BS->HandleProtocol(h, &BlockIoProtocolGUID, (void **)&blkio); + status = OpenProtocolByHandle(h, &BlockIoProtocolGUID, (void **)&blkio); if (status == EFI_UNSUPPORTED) return (status); @@ -445,7 +446,7 @@ efi_main(EFI_HANDLE Ximage, EFI_SYSTEM_TABLE *Xsystab) putchar('\n'); /* Determine the devpath of our image so we can prefer it. */ - status = BS->HandleProtocol(IH, &LoadedImageGUID, (VOID**)&img); + status = OpenProtocolByHandle(IH, &LoadedImageGUID, (void **)&img); imgpath = NULL; if (status == EFI_SUCCESS) { text = efi_devpath_name(img->FilePath); @@ -455,8 +456,8 @@ efi_main(EFI_HANDLE Ximage, EFI_SYSTEM_TABLE *Xsystab) efi_free_devpath_name(text); } - status = BS->HandleProtocol(img->DeviceHandle, &DevicePathGUID, - (void **)&imgpath); + status = OpenProtocolByHandle(img->DeviceHandle, + &DevicePathGUID, (void **)&imgpath); if (status != EFI_SUCCESS) { DPRINTF("Failed to get image DevicePath (%lu)\n", EFI_ERROR_CODE(status)); diff --git a/stand/efi/include/efilib.h b/stand/efi/include/efilib.h index 215728af2d2..766b98d9f0c 100644 --- a/stand/efi/include/efilib.h +++ b/stand/efi/include/efilib.h @@ -69,6 +69,7 @@ pdinfo_t *efiblk_get_pdinfo_by_handle(EFI_HANDLE h); pdinfo_t *efiblk_get_pdinfo_by_device_path(EFI_DEVICE_PATH *path); void *efi_get_table(EFI_GUID *tbl); +EFI_STATUS OpenProtocolByHandle(EFI_HANDLE, EFI_GUID *, void **); int efi_getdev(void **vdev, const char *devspec, const char **path); char *efi_fmtdev(void *vdev); @@ -92,6 +93,7 @@ CHAR16 *efi_devpath_name(EFI_DEVICE_PATH *); void efi_free_devpath_name(CHAR16 *); EFI_DEVICE_PATH *efi_devpath_to_media_path(EFI_DEVICE_PATH *); UINTN efi_devpath_length(EFI_DEVICE_PATH *); +EFI_HANDLE efi_devpath_to_handle(EFI_DEVICE_PATH *path, EFI_HANDLE *handles, unsigned nhandles); int efi_status_to_errno(EFI_STATUS); EFI_STATUS errno_to_efi_status(int errno); diff --git a/stand/efi/libefi/devpath.c b/stand/efi/libefi/devpath.c index 7bd49c775b6..07e93379f0d 100644 --- a/stand/efi/libefi/devpath.c +++ b/stand/efi/libefi/devpath.c @@ -42,8 +42,8 @@ efi_lookup_image_devpath(EFI_HANDLE handle) EFI_DEVICE_PATH *devpath; EFI_STATUS status; - status = BS->HandleProtocol(handle, &ImageDevicePathGUID, - (VOID **)&devpath); + status = OpenProtocolByHandle(handle, &ImageDevicePathGUID, + (void **)&devpath); if (EFI_ERROR(status)) devpath = NULL; return (devpath); @@ -55,7 +55,8 @@ efi_lookup_devpath(EFI_HANDLE handle) EFI_DEVICE_PATH *devpath; EFI_STATUS status; - status = BS->HandleProtocol(handle, &DevicePathGUID, (VOID **)&devpath); + status = OpenProtocolByHandle(handle, &DevicePathGUID, + (void **)&devpath); if (EFI_ERROR(status)) devpath = NULL; return (devpath); @@ -229,3 +230,25 @@ efi_devpath_length(EFI_DEVICE_PATH *path) path = NextDevicePathNode(path); return ((UINTN)path - (UINTN)start) + DevicePathNodeLength(path); } + +EFI_HANDLE +efi_devpath_to_handle(EFI_DEVICE_PATH *path, EFI_HANDLE *handles, unsigned nhandles) +{ + unsigned i; + EFI_DEVICE_PATH *media, *devpath; + EFI_HANDLE h; + + media = efi_devpath_to_media_path(path); + if (media == NULL) + return (NULL); + for (i = 0; i < nhandles; i++) { + h = handles[i]; + devpath = efi_lookup_devpath(h); + if (devpath == NULL) + continue; + if (!efi_devpath_match_node(media, efi_devpath_to_media_path(devpath))) + continue; + return (h); + } + return (NULL); +} diff --git a/stand/efi/libefi/efinet.c b/stand/efi/libefi/efinet.c index 8a3e418ef28..d9402875124 100644 --- a/stand/efi/libefi/efinet.c +++ b/stand/efi/libefi/efinet.c @@ -195,7 +195,7 @@ efinet_init(struct iodesc *desc, void *machdep_hint) } h = nif->nif_driver->netif_ifs[nif->nif_unit].dif_private; - status = BS->HandleProtocol(h, &sn_guid, (VOID **)&nif->nif_devdata); + status = OpenProtocolByHandle(h, &sn_guid, (void **)&nif->nif_devdata); if (status != EFI_SUCCESS) { printf("net%d: cannot fetch interface data (status=%lu)\n", nif->nif_unit, EFI_ERROR_CODE(status)); diff --git a/stand/efi/libefi/efipart.c b/stand/efi/libefi/efipart.c index 6f7f621b70c..e53637a389f 100644 --- a/stand/efi/libefi/efipart.c +++ b/stand/efi/libefi/efipart.c @@ -64,6 +64,9 @@ static int efipart_printhd(int); #define PNP0700 0x700 #define PNP0701 0x701 +/* Bounce buffer max size */ +#define BIO_BUFFER_SIZE 0x4000 + struct devsw efipart_fddev = { .dv_name = "fd", .dv_type = DEVT_FD, @@ -100,12 +103,18 @@ struct devsw efipart_hddev = { .dv_cleanup = NULL }; -static pdinfo_list_t fdinfo; -static pdinfo_list_t cdinfo; -static pdinfo_list_t hdinfo; +static pdinfo_list_t fdinfo = STAILQ_HEAD_INITIALIZER(fdinfo); +static pdinfo_list_t cdinfo = STAILQ_HEAD_INITIALIZER(cdinfo); +static pdinfo_list_t hdinfo = STAILQ_HEAD_INITIALIZER(hdinfo); -static EFI_HANDLE *efipart_handles = NULL; -static UINTN efipart_nhandles = 0; +/* + * efipart_inithandles() is used to build up the pdinfo list from + * block device handles. Then each devsw init callback is used to + * pick items from pdinfo and move to proper device list. + * In ideal world, we should end up with empty pdinfo once all + * devsw initializers are called. + */ +static pdinfo_list_t pdinfo = STAILQ_HEAD_INITIALIZER(pdinfo); pdinfo_list_t * efiblk_get_pdinfo_list(struct devsw *dev) @@ -140,23 +149,14 @@ efiblk_get_pdinfo(struct devdesc *dev) pdinfo_t * efiblk_get_pdinfo_by_device_path(EFI_DEVICE_PATH *path) { - unsigned i; - EFI_DEVICE_PATH *media, *devpath; EFI_HANDLE h; + EFI_STATUS status; + EFI_DEVICE_PATH *devp = path; - media = efi_devpath_to_media_path(path); - if (media == NULL) + status = BS->LocateDevicePath(&blkio_guid, &devp, &h); + if (EFI_ERROR(status)) return (NULL); - for (i = 0; i < efipart_nhandles; i++) { - h = efipart_handles[i]; - devpath = efi_lookup_devpath(h); - if (devpath == NULL) - continue; - if (!efi_devpath_match_node(media, efi_devpath_to_media_path(devpath))) - continue; - return (efiblk_get_pdinfo_by_handle(h)); - } - return (NULL); + return (efiblk_get_pdinfo_by_handle(h)); } static bool @@ -185,6 +185,10 @@ efiblk_get_pdinfo_by_handle(EFI_HANDLE h) STAILQ_FOREACH(dp, &cdinfo, pd_link) { if (same_handle(dp, h)) return (dp); + STAILQ_FOREACH(pp, &dp->pd_part, pd_link) { + if (same_handle(pp, h)) + return (pp); + } } STAILQ_FOREACH(dp, &fdinfo, pd_link) { if (same_handle(dp, h)) @@ -208,15 +212,16 @@ efiblk_pdinfo_count(pdinfo_list_t *pdi) int efipart_inithandles(void) { + unsigned i, nin; UINTN sz; EFI_HANDLE *hin; + EFI_DEVICE_PATH *devpath; + EFI_BLOCK_IO *blkio; EFI_STATUS status; + pdinfo_t *pd; - if (efipart_nhandles != 0) { - free(efipart_handles); - efipart_handles = NULL; - efipart_nhandles = 0; - } + if (!STAILQ_EMPTY(&pdinfo)) + return (0); sz = 0; hin = NULL; @@ -231,12 +236,60 @@ efipart_inithandles(void) if (EFI_ERROR(status)) return (efi_status_to_errno(status)); - efipart_handles = hin; - efipart_nhandles = sz / sizeof(*hin); + nin = sz / sizeof(*hin); #ifdef EFIPART_DEBUG - printf("%s: Got %d BLOCK IO MEDIA handle(s)\n", __func__, - efipart_nhandles); + printf("%s: Got %d BLOCK IO MEDIA handle(s)\n", __func__, nin); #endif + + for (i = 0; i < nin; i++) { + /* + * Get devpath and open protocol. + * We should not get errors here + */ + if ((devpath = efi_lookup_devpath(hin[i])) == NULL) + continue; + + status = OpenProtocolByHandle(hin[i], &blkio_guid, + (void **)&blkio); + if (EFI_ERROR(status)) { + printf("error %lu\n", EFI_ERROR_CODE(status)); + continue; + } + + /* + * We assume the block size 512 or greater power of 2. + * Also skip devices with block size > 64k (16 is max + * ashift supported by zfs). + * iPXE is known to insert stub BLOCK IO device with + * BlockSize 1. + */ + if (blkio->Media->BlockSize < 512 || + blkio->Media->BlockSize > (1 << 16) || + !powerof2(blkio->Media->BlockSize)) { + continue; + } + + /* Allowed values are 0, 1 and power of 2. */ + if (blkio->Media->IoAlign > 1 && + !powerof2(blkio->Media->IoAlign)) { + continue; + } + + /* This is bad. */ + if ((pd = calloc(1, sizeof(*pd))) == NULL) { + printf("efipart_inithandles: Out of memory.\n"); + free(hin); + return (ENOMEM); + } + STAILQ_INIT(&pd->pd_part); + + pd->pd_handle = hin[i]; + pd->pd_devpath = devpath; + pd->pd_blkio = blkio; + STAILQ_INSERT_TAIL(&pdinfo, pd, pd_link); + } + + free(hin); return (0); } @@ -257,133 +310,48 @@ efipart_floppy(EFI_DEVICE_PATH *node) return (NULL); } -/* - * Determine if the provided device path is hdd. - * - * There really is no simple fool proof way to classify the devices. - * Since we do build three lists of devices - floppy, cd and hdd, we - * will try to see if the device is floppy or cd, and list anything else - * as hdd. - */ -static bool -efipart_hdd(EFI_DEVICE_PATH *dp) +static pdinfo_t * +efipart_find_parent(pdinfo_list_t *pdi, EFI_DEVICE_PATH *devpath) { - unsigned i; - EFI_DEVICE_PATH *devpath, *node; - EFI_BLOCK_IO *blkio; - EFI_STATUS status; - - if (dp == NULL) - return (false); - - if ((node = efi_devpath_last_node(dp)) == NULL) - return (false); - - if (efipart_floppy(node) != NULL) - return (false); - - /* - * Test every EFI BLOCK IO handle to make sure dp is not device path - * for CD/DVD. - */ - for (i = 0; i < efipart_nhandles; i++) { - devpath = efi_lookup_devpath(efipart_handles[i]); - if (devpath == NULL) - return (false); - - /* Only continue testing when dp is prefix in devpath. */ - if (!efi_devpath_is_prefix(dp, devpath)) - continue; - - /* - * The device path has to have last node describing the - * device, or we can not test the type. - */ - if ((node = efi_devpath_last_node(devpath)) == NULL) - return (false); - - if (DevicePathType(node) == MEDIA_DEVICE_PATH && - DevicePathSubType(node) == MEDIA_CDROM_DP) { - return (false); - } - - /* Make sure we do have the media. */ - status = BS->HandleProtocol(efipart_handles[i], - &blkio_guid, (void **)&blkio); - if (EFI_ERROR(status)) - return (false); - - /* USB or SATA cd without the media. */ - if (blkio->Media->RemovableMedia && - !blkio->Media->MediaPresent) { - return (false); - } + pdinfo_t *pd; - /* - * We assume the block size 512 or greater power of 2. - * iPXE is known to insert stub BLOCK IO device with - * BlockSize 1. - */ - if (blkio->Media->BlockSize < 512 || - !powerof2(blkio->Media->BlockSize)) { - return (false); - } + STAILQ_FOREACH(pd, pdi, pd_link) { + if (efi_devpath_is_prefix(pd->pd_devpath, devpath)) + return (pd); } - return (true); + return (NULL); } -/* - * Add or update entries with new handle data. - */ static int -efipart_fdinfo_add(EFI_HANDLE handle, uint32_t uid, EFI_DEVICE_PATH *devpath) -{ - pdinfo_t *fd; - - fd = calloc(1, sizeof(pdinfo_t)); - if (fd == NULL) { - printf("Failed to register floppy %d, out of memory\n", uid); - return (ENOMEM); - } - STAILQ_INIT(&fd->pd_part); - - fd->pd_unit = uid; - fd->pd_handle = handle; - fd->pd_devpath = devpath; - fd->pd_parent = NULL; - fd->pd_devsw = &efipart_fddev; - STAILQ_INSERT_TAIL(&fdinfo, fd, pd_link); - return (0); -} - -static void -efipart_updatefd(void) +efipart_initfd(void) { - EFI_DEVICE_PATH *devpath, *node; + EFI_DEVICE_PATH *node; ACPI_HID_DEVICE_PATH *acpi; - int i; + pdinfo_t *parent, *fd; - for (i = 0; i < efipart_nhandles; i++) { - devpath = efi_lookup_devpath(efipart_handles[i]); - if (devpath == NULL) +restart: + STAILQ_FOREACH(fd, &pdinfo, pd_link) { + if ((node = efi_devpath_last_node(fd->pd_devpath)) == NULL) continue; - if ((node = efi_devpath_last_node(devpath)) == NULL) + if ((acpi = efipart_floppy(node)) == NULL) continue; - if ((acpi = efipart_floppy(node)) != NULL) { - efipart_fdinfo_add(efipart_handles[i], acpi->UID, - devpath); + + STAILQ_REMOVE(&pdinfo, fd, pdinfo, pd_link); + parent = efipart_find_parent(&pdinfo, fd->pd_devpath); + if (parent != NULL) { + STAILQ_REMOVE(&pdinfo, parent, pdinfo, pd_link); + parent->pd_alias = fd->pd_handle; + parent->pd_unit = acpi->UID; + free(fd); + fd = parent; + } else { + fd->pd_unit = acpi->UID; } + fd->pd_devsw = &efipart_fddev; + STAILQ_INSERT_TAIL(&fdinfo, fd, pd_link); + goto restart; } -} - -static int -efipart_initfd(void) -{ - - STAILQ_INIT(&fdinfo); - - efipart_updatefd(); bcache_add_dev(efiblk_pdinfo_count(&fdinfo)); return (0); @@ -392,68 +360,90 @@ efipart_initfd(void) /* * Add or update entries with new handle data. */ -static int -efipart_cdinfo_add(EFI_HANDLE handle, EFI_HANDLE alias, - EFI_DEVICE_PATH *devpath) +static void +efipart_cdinfo_add(pdinfo_t *cd) { - int unit; - pdinfo_t *cd; - pdinfo_t *pd; + pdinfo_t *pd, *last; - unit = 0; STAILQ_FOREACH(pd, &cdinfo, pd_link) { - if (efi_devpath_match(pd->pd_devpath, devpath) == true) { - pd->pd_handle = handle; - pd->pd_alias = alias; - return (0); + if (efi_devpath_is_prefix(pd->pd_devpath, cd->pd_devpath)) { + last = STAILQ_LAST(&pd->pd_part, pdinfo, pd_link); + if (last != NULL) + cd->pd_unit = last->pd_unit + 1; + else + cd->pd_unit = 0; + cd->pd_parent = pd; + cd->pd_devsw = &efipart_cddev; + STAILQ_INSERT_TAIL(&pd->pd_part, cd, pd_link); + return; } - unit++; } - cd = calloc(1, sizeof(pdinfo_t)); - if (cd == NULL) { - printf("Failed to add cd %d, out of memory\n", unit); - return (ENOMEM); - } - STAILQ_INIT(&cd->pd_part); + last = STAILQ_LAST(&cdinfo, pdinfo, pd_link); + if (last != NULL) + cd->pd_unit = last->pd_unit + 1; + else + cd->pd_unit = 0; - cd->pd_handle = handle; - cd->pd_unit = unit; - cd->pd_alias = alias; - cd->pd_devpath = devpath; cd->pd_parent = NULL; cd->pd_devsw = &efipart_cddev; STAILQ_INSERT_TAIL(&cdinfo, cd, pd_link); - return (0); +} + +static bool +efipart_testcd(EFI_DEVICE_PATH *node, EFI_BLOCK_IO *blkio) +{ + if (DevicePathType(node) == MEDIA_DEVICE_PATH && + DevicePathSubType(node) == MEDIA_CDROM_DP) { + return (true); + } + + /* cd drive without the media. */ + if (blkio->Media->RemovableMedia && + !blkio->Media->MediaPresent) { + return (true); + } + + return (false); } static void efipart_updatecd(void) { - int i; - EFI_DEVICE_PATH *devpath, *devpathcpy, *tmpdevpath, *node; - EFI_HANDLE handle; - EFI_BLOCK_IO *blkio; + EFI_DEVICE_PATH *devpath, *node; EFI_STATUS status; + pdinfo_t *parent, *cd; - for (i = 0; i < efipart_nhandles; i++) { - devpath = efi_lookup_devpath(efipart_handles[i]); - if (devpath == NULL) - continue; - - if ((node = efi_devpath_last_node(devpath)) == NULL) +restart: + STAILQ_FOREACH(cd, &pdinfo, pd_link) { + if ((node = efi_devpath_last_node(cd->pd_devpath)) == NULL) continue; if (efipart_floppy(node) != NULL) continue; - if (efipart_hdd(devpath)) - continue; + /* Is parent of this device already registered? */ + parent = efipart_find_parent(&cdinfo, cd->pd_devpath); + if (parent != NULL) { + STAILQ_REMOVE(&pdinfo, cd, pdinfo, pd_link); + efipart_cdinfo_add(cd); + goto restart; + } - status = BS->HandleProtocol(efipart_handles[i], - &blkio_guid, (void **)&blkio); - if (EFI_ERROR(status)) + if (!efipart_testcd(node, cd->pd_blkio)) continue; + + /* Find parent and unlink both parent and cd from pdinfo */ + STAILQ_REMOVE(&pdinfo, cd, pdinfo, pd_link); + parent = efipart_find_parent(&pdinfo, cd->pd_devpath); + if (parent != NULL) { + STAILQ_REMOVE(&pdinfo, parent, pdinfo, pd_link); + efipart_cdinfo_add(parent); + } + + if (parent == NULL) + parent = efipart_find_parent(&cdinfo, cd->pd_devpath); + /* * If we come across a logical partition of subtype CDROM * it doesn't refer to the CD filesystem itself, but rather @@ -462,132 +452,79 @@ efipart_updatecd(void) * that will be the CD filesystem. */ if (DevicePathType(node) == MEDIA_DEVICE_PATH && - DevicePathSubType(node) == MEDIA_CDROM_DP) { - devpathcpy = efi_devpath_trim(devpath); - if (devpathcpy == NULL) - continue; - tmpdevpath = devpathcpy; - status = BS->LocateDevicePath(&blkio_guid, &tmpdevpath, - &handle); - free(devpathcpy); - if (EFI_ERROR(status)) - continue; - devpath = efi_lookup_devpath(handle); - efipart_cdinfo_add(handle, efipart_handles[i], - devpath); - continue; - } + DevicePathSubType(node) == MEDIA_CDROM_DP && + parent == NULL) { + parent = calloc(1, sizeof(*parent)); + if (parent == NULL) { + printf("efipart_updatecd: out of memory\n"); + /* this device is lost but try again. */ + free(cd); + goto restart; + } - if (DevicePathType(node) == MESSAGING_DEVICE_PATH && - DevicePathSubType(node) == MSG_ATAPI_DP) { - efipart_cdinfo_add(efipart_handles[i], NULL, - devpath); - continue; + devpath = efi_devpath_trim(cd->pd_devpath); + if (devpath == NULL) { + printf("efipart_updatecd: out of memory\n"); + /* this device is lost but try again. */ + free(parent); + free(cd); + goto restart; + } + parent->pd_devpath = devpath; + status = BS->LocateDevicePath(&blkio_guid, + &parent->pd_devpath, &parent->pd_handle); + free(devpath); + if (EFI_ERROR(status)) { + printf("efipart_updatecd: error %lu\n", + EFI_ERROR_CODE(status)); + free(parent); + free(cd); + goto restart; + } + parent->pd_devpath = + efi_lookup_devpath(parent->pd_handle); + efipart_cdinfo_add(parent); } - /* USB or SATA cd without the media. */ - if (blkio->Media->RemovableMedia && - !blkio->Media->MediaPresent) { - efipart_cdinfo_add(efipart_handles[i], NULL, - devpath); - } + efipart_cdinfo_add(cd); + goto restart; } } static int efipart_initcd(void) { - - STAILQ_INIT(&cdinfo); - efipart_updatecd(); bcache_add_dev(efiblk_pdinfo_count(&cdinfo)); return (0); } -static int -efipart_hdinfo_add(EFI_HANDLE disk_handle, EFI_HANDLE part_handle) +static void +efipart_hdinfo_add(pdinfo_t *hd, HARDDRIVE_DEVICE_PATH *node) { - EFI_DEVICE_PATH *disk_devpath, *part_devpath; - HARDDRIVE_DEVICE_PATH *node; - int unit; - pdinfo_t *hd, *pd, *last; - - disk_devpath = efi_lookup_devpath(disk_handle); - if (disk_devpath == NULL) - return (ENOENT); - - if (part_handle != NULL) { - part_devpath = efi_lookup_devpath(part_handle); - if (part_devpath == NULL) - return (ENOENT); - node = (HARDDRIVE_DEVICE_PATH *) - efi_devpath_last_node(part_devpath); - if (node == NULL) - return (ENOENT); /* This should not happen. */ - } else { - part_devpath = NULL; - node = NULL; - } - - pd = calloc(1, sizeof(pdinfo_t)); - if (pd == NULL) { - printf("Failed to add disk, out of memory\n"); - return (ENOMEM); - } - STAILQ_INIT(&pd->pd_part); - - STAILQ_FOREACH(hd, &hdinfo, pd_link) { - if (efi_devpath_match(hd->pd_devpath, disk_devpath) == true) { - if (part_devpath == NULL) - return (0); + pdinfo_t *pd, *last; + STAILQ_FOREACH(pd, &hdinfo, pd_link) { + if (efi_devpath_is_prefix(pd->pd_devpath, hd->pd_devpath)) { /* Add the partition. */ - pd->pd_handle = part_handle; - pd->pd_unit = node->PartitionNumber; - pd->pd_devpath = part_devpath; - pd->pd_parent = hd; - pd->pd_devsw = &efipart_hddev; - STAILQ_INSERT_TAIL(&hd->pd_part, pd, pd_link); - return (0); + hd->pd_unit = node->PartitionNumber; + hd->pd_parent = pd; + hd->pd_devsw = &efipart_hddev; + STAILQ_INSERT_TAIL(&pd->pd_part, hd, pd_link); + return; } } last = STAILQ_LAST(&hdinfo, pdinfo, pd_link); if (last != NULL) - unit = last->pd_unit + 1; + hd->pd_unit = last->pd_unit + 1; else - unit = 0; + hd->pd_unit = 0; /* Add the disk. */ - hd = pd; - hd->pd_handle = disk_handle; - hd->pd_unit = unit; - hd->pd_devpath = disk_devpath; - hd->pd_parent = NULL; hd->pd_devsw = &efipart_hddev; STAILQ_INSERT_TAIL(&hdinfo, hd, pd_link); - - if (part_devpath == NULL) - return (0); - - pd = calloc(1, sizeof(pdinfo_t)); - if (pd == NULL) { - printf("Failed to add partition, out of memory\n"); - return (ENOMEM); - } - STAILQ_INIT(&pd->pd_part); - - /* Add the partition. */ - pd->pd_handle = part_handle; - pd->pd_unit = node->PartitionNumber; - pd->pd_devpath = part_devpath; - pd->pd_parent = hd; - pd->pd_devsw = &efipart_hddev; - STAILQ_INSERT_TAIL(&hd->pd_part, pd, pd_link); - - return (0); } /* @@ -596,40 +533,25 @@ efipart_hdinfo_add(EFI_HANDLE disk_handle, EFI_HANDLE part_handle) * of typeN:M, where type is interface type, N is disk id * and M is partition id. */ -static int -efipart_hdinfo_add_filepath(EFI_HANDLE disk_handle) +static void +efipart_hdinfo_add_filepath(pdinfo_t *hd, FILEPATH_DEVICE_PATH *node) { - EFI_DEVICE_PATH *devpath; - FILEPATH_DEVICE_PATH *node; char *pathname, *p; - int unit, len; - pdinfo_t *pd, *last; + int len; + pdinfo_t *last; - /* First collect and verify all the data */ - if ((devpath = efi_lookup_devpath(disk_handle)) == NULL) - return (ENOENT); - node = (FILEPATH_DEVICE_PATH *)efi_devpath_last_node(devpath); - if (node == NULL) - return (ENOENT); /* This should not happen. */ - - pd = calloc(1, sizeof(pdinfo_t)); - if (pd == NULL) { - printf("Failed to add disk, out of memory\n"); - return (ENOMEM); - } - STAILQ_INIT(&pd->pd_part); last = STAILQ_LAST(&hdinfo, pdinfo, pd_link); if (last != NULL) - unit = last->pd_unit + 1; + hd->pd_unit = last->pd_unit + 1; else - unit = 0; + hd->pd_unit = 0; /* FILEPATH_DEVICE_PATH has 0 terminated string */ len = ucs2len(node->PathName); if ((pathname = malloc(len + 1)) == NULL) { printf("Failed to add disk, out of memory\n"); - free(pd); - return (ENOMEM); + free(hd); + return; } cpy16to8(node->PathName, pathname, len + 1); p = strchr(pathname, ':'); @@ -640,23 +562,19 @@ efipart_hdinfo_add_filepath(EFI_HANDLE disk_handle) * false, this code would need update. */ if (p == NULL) { /* no colon, add the disk */ - pd->pd_handle = disk_handle; - pd->pd_unit = unit; - pd->pd_devpath = devpath; - pd->pd_parent = NULL; - pd->pd_devsw = &efipart_hddev; - STAILQ_INSERT_TAIL(&hdinfo, pd, pd_link); + hd->pd_devsw = &efipart_hddev; + STAILQ_INSERT_TAIL(&hdinfo, hd, pd_link); free(pathname); - return (0); + return; } p++; /* skip the colon */ errno = 0; - unit = (int)strtol(p, NULL, 0); + hd->pd_unit = (int)strtol(p, NULL, 0); if (errno != 0) { printf("Bad unit number for partition \"%s\"\n", pathname); free(pathname); - free(pd); - return (EUNIT); + free(hd); + return; } /* @@ -668,80 +586,99 @@ efipart_hdinfo_add_filepath(EFI_HANDLE disk_handle) if (last == NULL) { printf("BUG: No disk for partition \"%s\"\n", pathname); free(pathname); - free(pd); - return (EINVAL); + free(hd); + return; } /* Add the partition. */ - pd->pd_handle = disk_handle; - pd->pd_unit = unit; - pd->pd_devpath = devpath; - pd->pd_parent = last; - pd->pd_devsw = &efipart_hddev; - STAILQ_INSERT_TAIL(&last->pd_part, pd, pd_link); + hd->pd_parent = last; + hd->pd_devsw = &efipart_hddev; + STAILQ_INSERT_TAIL(&last->pd_part, hd, pd_link); free(pathname); - return (0); } static void efipart_updatehd(void) { - int i; - EFI_DEVICE_PATH *devpath, *devpathcpy, *tmpdevpath, *node; - EFI_HANDLE handle; - EFI_BLOCK_IO *blkio; + EFI_DEVICE_PATH *devpath, *node; EFI_STATUS status; + pdinfo_t *parent, *hd; - for (i = 0; i < efipart_nhandles; i++) { - devpath = efi_lookup_devpath(efipart_handles[i]); - if (devpath == NULL) +restart: + STAILQ_FOREACH(hd, &pdinfo, pd_link) { + if ((node = efi_devpath_last_node(hd->pd_devpath)) == NULL) continue; - if ((node = efi_devpath_last_node(devpath)) == NULL) + if (efipart_floppy(node) != NULL) continue; - if (!efipart_hdd(devpath)) + if (efipart_testcd(node, hd->pd_blkio)) continue; - status = BS->HandleProtocol(efipart_handles[i], - &blkio_guid, (void **)&blkio); - if (EFI_ERROR(status)) - continue; + if (DevicePathType(node) == HARDWARE_DEVICE_PATH && + (DevicePathSubType(node) == HW_PCI_DP || + DevicePathSubType(node) == HW_VENDOR_DP)) { + STAILQ_REMOVE(&pdinfo, hd, pdinfo, pd_link); + efipart_hdinfo_add(hd, NULL); + goto restart; + } if (DevicePathType(node) == MEDIA_DEVICE_PATH && DevicePathSubType(node) == MEDIA_FILEPATH_DP) { - efipart_hdinfo_add_filepath(efipart_handles[i]); - continue; + STAILQ_REMOVE(&pdinfo, hd, pdinfo, pd_link); + efipart_hdinfo_add_filepath(hd, + (FILEPATH_DEVICE_PATH *)node); + goto restart; + } + + STAILQ_REMOVE(&pdinfo, hd, pdinfo, pd_link); + parent = efipart_find_parent(&pdinfo, hd->pd_devpath); + if (parent != NULL) { + STAILQ_REMOVE(&pdinfo, parent, pdinfo, pd_link); + efipart_hdinfo_add(parent, NULL); + } else { + parent = efipart_find_parent(&hdinfo, hd->pd_devpath); } if (DevicePathType(node) == MEDIA_DEVICE_PATH && - DevicePathSubType(node) == MEDIA_HARDDRIVE_DP) { - devpathcpy = efi_devpath_trim(devpath); - if (devpathcpy == NULL) - continue; - tmpdevpath = devpathcpy; - status = BS->LocateDevicePath(&blkio_guid, &tmpdevpath, - &handle); - free(devpathcpy); - if (EFI_ERROR(status)) - continue; - /* - * We do not support nested partitions. - */ - devpathcpy = efi_lookup_devpath(handle); - if (devpathcpy == NULL) - continue; - if ((node = efi_devpath_last_node(devpathcpy)) == NULL) - continue; + DevicePathSubType(node) == MEDIA_HARDDRIVE_DP && + parent == NULL) { + parent = calloc(1, sizeof(*parent)); + if (parent == NULL) { + printf("efipart_updatehd: out of memory\n"); + /* this device is lost but try again. */ + free(hd); + goto restart; + } - if (DevicePathType(node) == MEDIA_DEVICE_PATH && - DevicePathSubType(node) == MEDIA_HARDDRIVE_DP) - continue; + devpath = efi_devpath_trim(hd->pd_devpath); + if (devpath == NULL) { + printf("efipart_updatehd: out of memory\n"); + /* this device is lost but try again. */ + free(parent); + free(hd); + goto restart; + } - efipart_hdinfo_add(handle, efipart_handles[i]); - continue; + parent->pd_devpath = devpath; + status = BS->LocateDevicePath(&blkio_guid, + &parent->pd_devpath, &parent->pd_handle); + free(devpath); + if (EFI_ERROR(status)) { + printf("efipart_updatehd: error %lu\n", + EFI_ERROR_CODE(status)); + free(parent); + free(hd); + goto restart; + } + + parent->pd_devpath = + efi_lookup_devpath(&parent->pd_handle); + + efipart_hdinfo_add(parent, NULL); } - efipart_hdinfo_add(efipart_handles[i], NULL); + efipart_hdinfo_add(hd, (HARDDRIVE_DEVICE_PATH *)node); + goto restart; } } @@ -749,8 +686,6 @@ static int efipart_inithd(void) { - STAILQ_INIT(&hdinfo); - efipart_updatehd(); bcache_add_dev(efiblk_pdinfo_count(&hdinfo)); @@ -790,7 +725,7 @@ efipart_print_common(struct devsw *dev, pdinfo_list_t *pdlist, int verbose) snprintf(line, sizeof(line), " %s%d", dev->dv_name, pd->pd_unit); printf("%s:", line); - status = BS->HandleProtocol(h, &blkio_guid, (void **)&blkio); + status = OpenProtocolByHandle(h, &blkio_guid, (void **)&blkio); if (!EFI_ERROR(status)) { printf(" %llu", blkio->Media->LastBlock == 0? 0: @@ -863,7 +798,7 @@ efipart_open(struct open_file *f, ...) EFI_STATUS status; va_start(args, f); - dev = va_arg(args, struct disk_devdesc*); + dev = va_arg(args, struct disk_devdesc *); va_end(args); if (dev == NULL) return (EINVAL); @@ -873,7 +808,7 @@ efipart_open(struct open_file *f, ...) return (EIO); if (pd->pd_blkio == NULL) { - status = BS->HandleProtocol(pd->pd_handle, &blkio_guid, + status = OpenProtocolByHandle(pd->pd_handle, &blkio_guid, (void **)&pd->pd_blkio); if (EFI_ERROR(status)) return (efi_status_to_errno(status)); @@ -1053,9 +988,11 @@ efipart_realstrategy(void *devdata, int rw, daddr_t blk, size_t size, EFI_BLOCK_IO *blkio; uint64_t off, disk_blocks, d_offset = 0; char *blkbuf; - size_t blkoff, blksz; - int error; - size_t diskend, readstart; + size_t blkoff, blksz, bio_size; + unsigned ioalign; + bool need_buf; + int rc; + uint64_t diskend, readstart; if (dev == NULL || blk < 0) return (EINVAL); @@ -1102,40 +1039,118 @@ efipart_realstrategy(void *devdata, int rw, daddr_t blk, size_t size, size = size * blkio->Media->BlockSize; } - if (rsize != NULL) - *rsize = size; - + need_buf = true; + /* Do we need bounce buffer? */ if ((size % blkio->Media->BlockSize == 0) && (off % blkio->Media->BlockSize == 0)) - return (efipart_readwrite(blkio, rw, - off / blkio->Media->BlockSize, - size / blkio->Media->BlockSize, buf)); + need_buf = false; + + /* Do we have IO alignment requirement? */ + ioalign = blkio->Media->IoAlign; + if (ioalign == 0) + ioalign++; + + if (ioalign > 1 && (uintptr_t)buf != roundup2((uintptr_t)buf, ioalign)) + need_buf = true; + + if (need_buf) { + for (bio_size = BIO_BUFFER_SIZE; bio_size > 0; + bio_size -= blkio->Media->BlockSize) { + blkbuf = memalign(ioalign, bio_size); + if (blkbuf != NULL) + break; + } + } else { + blkbuf = buf; + bio_size = size; + } - /* - * The block size of the media is not a multiple of I/O. - */ - blkbuf = malloc(blkio->Media->BlockSize); if (blkbuf == NULL) return (ENOMEM); - error = 0; + if (rsize != NULL) + *rsize = size; + + rc = 0; blk = off / blkio->Media->BlockSize; blkoff = off % blkio->Media->BlockSize; - blksz = blkio->Media->BlockSize - blkoff; + while (size > 0) { - error = efipart_readwrite(blkio, rw, blk, 1, blkbuf); - if (error) + size_t x = min(size, bio_size); + + if (x < blkio->Media->BlockSize) + x = 1; + else + x /= blkio->Media->BlockSize; + + switch (rw & F_MASK) { + case F_READ: + blksz = blkio->Media->BlockSize * x - blkoff; + if (size < blksz) + blksz = size; + + rc = efipart_readwrite(blkio, rw, blk, x, blkbuf); + if (rc != 0) + goto error; + + if (need_buf) + bcopy(blkbuf + blkoff, buf, blksz); + break; + case F_WRITE: + rc = 0; + if (blkoff != 0) { + /* + * We got offset to sector, read 1 sector to + * blkbuf. + */ + x = 1; + blksz = blkio->Media->BlockSize - blkoff; + blksz = min(blksz, size); + rc = efipart_readwrite(blkio, F_READ, blk, x, + blkbuf); + } else if (size < blkio->Media->BlockSize) { + /* + * The remaining block is not full + * sector. Read 1 sector to blkbuf. + */ + x = 1; + blksz = size; + rc = efipart_readwrite(blkio, F_READ, blk, x, + blkbuf); + } else { + /* We can write full sector(s). */ + blksz = blkio->Media->BlockSize * x; + } + + if (rc != 0) + goto error; + /* + * Put your Data In, Put your Data out, + * Put your Data In, and shake it all about + */ + if (need_buf) + bcopy(buf, blkbuf + blkoff, blksz); + rc = efipart_readwrite(blkio, F_WRITE, blk, x, blkbuf); + if (rc != 0) + goto error; break; - if (size < blksz) - blksz = size; - bcopy(blkbuf + blkoff, buf, blksz); + default: + /* DO NOTHING */ + rc = EROFS; + goto error; + } + + blkoff = 0; buf += blksz; size -= blksz; - blk++; - blkoff = 0; - blksz = blkio->Media->BlockSize; + blk += x; } - free(blkbuf); - return (error); +error: + if (rsize != NULL) + *rsize -= size; + + if (need_buf) + free(blkbuf); + return (rc); } diff --git a/stand/efi/libefi/libefi.c b/stand/efi/libefi/libefi.c index e0a721f58b3..e7b1406839f 100644 --- a/stand/efi/libefi/libefi.c +++ b/stand/efi/libefi/libefi.c @@ -50,3 +50,10 @@ efi_get_table(EFI_GUID *tbl) } return (NULL); } + +EFI_STATUS +OpenProtocolByHandle(EFI_HANDLE handle, EFI_GUID *protocol, void **interface) +{ + return (BS->OpenProtocol(handle, protocol, interface, IH, NULL, + EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL)); +} diff --git a/stand/efi/loader/arch/i386/efimd.c b/stand/efi/loader/arch/i386/efimd.c index e3d8b1c6482..a72f21bab7d 100644 --- a/stand/efi/loader/arch/i386/efimd.c +++ b/stand/efi/loader/arch/i386/efimd.c @@ -76,7 +76,7 @@ ldr_bootinfo(struct bootinfo *bi, uint64_t *bi_addr) sz = sizeof(EFI_HANDLE); status = BS->LocateHandle(ByProtocol, &fpswa_guid, 0, &sz, &handle); if (status == 0) - status = BS->HandleProtocol(handle, &fpswa_guid, &fpswa); + status = OpenProtocolByHandle(handle, &fpswa_guid, &fpswa); bi->bi_fpswa = (status == 0) ? (uintptr_t)fpswa : 0; bisz = (sizeof(struct bootinfo) + 0x0f) & ~0x0f; diff --git a/stand/efi/loader/efi_main.c b/stand/efi/loader/efi_main.c index e424d89666e..45b62e71198 100644 --- a/stand/efi/loader/efi_main.c +++ b/stand/efi/loader/efi_main.c @@ -101,7 +101,7 @@ efi_main(EFI_HANDLE image_handle, EFI_SYSTEM_TABLE *system_table) /* Use efi_exit() from here on... */ - status = BS->HandleProtocol(IH, &image_protocol, (VOID**)&img); + status = OpenProtocolByHandle(IH, &image_protocol, (void**)&img); if (status != EFI_SUCCESS) efi_exit(status); diff --git a/stand/efi/loader/framebuffer.c b/stand/efi/loader/framebuffer.c index c012be92731..3f7e5fcbabe 100644 --- a/stand/efi/loader/framebuffer.c +++ b/stand/efi/loader/framebuffer.c @@ -244,7 +244,8 @@ efifb_uga_get_pciio(void) /* Get the PCI I/O interface of the first handle that supports it. */ pciio = NULL; for (hp = buf; hp < buf + bufsz; hp++) { - status = BS->HandleProtocol(*hp, &pciio_guid, (void **)&pciio); + status = OpenProtocolByHandle(*hp, &pciio_guid, + (void **)&pciio); if (status == EFI_SUCCESS) { free(buf); return (pciio); diff --git a/stand/efi/loader/main.c b/stand/efi/loader/main.c index 1cc4e5b6a59..bfbffefde99 100644 --- a/stand/efi/loader/main.c +++ b/stand/efi/loader/main.c @@ -125,7 +125,7 @@ has_keyboard(void) */ hin_end = &hin[sz / sizeof(*hin)]; for (walker = hin; walker < hin_end; walker++) { - status = BS->HandleProtocol(*walker, &devid, (VOID **)&path); + status = OpenProtocolByHandle(*walker, &devid, (void **)&path); if (EFI_ERROR(status)) continue; @@ -769,7 +769,7 @@ main(int argc, CHAR16 *argv[]) #endif /* Get our loaded image protocol interface structure. */ - BS->HandleProtocol(IH, &imgid, (VOID**)&img); + (void) OpenProtocolByHandle(IH, &imgid, (void **)&img); #ifdef EFI_ZFS_BOOT /* Tell ZFS probe code where we booted from */ @@ -894,7 +894,7 @@ main(int argc, CHAR16 *argv[]) efi_free_devpath_name(text); } - rv = BS->HandleProtocol(img->DeviceHandle, &devid, (void **)&imgpath); + rv = OpenProtocolByHandle(img->DeviceHandle, &devid, (void **)&imgpath); if (rv == EFI_SUCCESS) { text = efi_devpath_name(imgpath); if (text != NULL) { @@ -1322,7 +1322,7 @@ command_chain(int argc, char *argv[]) command_errmsg = "LoadImage failed"; return (CMD_ERROR); } - status = BS->HandleProtocol(loaderhandle, &LoadedImageGUID, + status = OpenProtocolByHandle(loaderhandle, &LoadedImageGUID, (void **)&loaded_image); if (argc > 2) { diff --git a/stand/libsa/stand.h b/stand/libsa/stand.h index 5e44e31ff2d..4e1a93fb34d 100644 --- a/stand/libsa/stand.h +++ b/stand/libsa/stand.h @@ -264,9 +264,6 @@ static __inline int tolower(int c) extern void setheap(void *base, void *top); extern char *sbrk(int incr); -extern void *reallocf(void *ptr, size_t size); -extern void mallocstats(void); - extern int printf(const char *fmt, ...) __printflike(1, 2); extern int asprintf(char **buf, const char *cfmt, ...) __printflike(2, 3); extern int sprintf(char *buf, const char *cfmt, ...) __printflike(2, 3); @@ -430,20 +427,27 @@ extern uint16_t ntohs(uint16_t); #endif void *Malloc(size_t, const char *, int); +void *Memalign(size_t, size_t, const char *, int); void *Calloc(size_t, size_t, const char *, int); void *Realloc(void *, size_t, const char *, int); +void *Reallocf(void *, size_t, const char *, int); void Free(void *, const char *, int); +extern void mallocstats(void); #ifdef DEBUG_MALLOC #define malloc(x) Malloc(x, __FILE__, __LINE__) +#define memalign(x, y) Memalign(x, y, __FILE__, __LINE__) #define calloc(x, y) Calloc(x, y, __FILE__, __LINE__) #define free(x) Free(x, __FILE__, __LINE__) #define realloc(x, y) Realloc(x, y, __FILE__, __LINE__) +#define reallocf(x, y) Reallocf(x, y, __FILE__, __LINE__) #else #define malloc(x) Malloc(x, NULL, 0) +#define memalign(x, y) Memalign(x, y, NULL, 0) #define calloc(x, y) Calloc(x, y, NULL, 0) #define free(x) Free(x, NULL, 0) #define realloc(x, y) Realloc(x, y, NULL, 0) +#define reallocf(x, y) Reallocf(x, y, NULL, 0) #endif #endif /* STAND_H */ diff --git a/stand/libsa/zalloc.c b/stand/libsa/zalloc.c index 4d1ec629121..371a1449409 100644 --- a/stand/libsa/zalloc.c +++ b/stand/libsa/zalloc.c @@ -1,5 +1,5 @@ /* - * This module derived from code donated to the FreeBSD Project by + * This module derived from code donated to the FreeBSD Project by * Matthew Dillon * * Copyright (c) 1998 The FreeBSD Project @@ -30,11 +30,13 @@ #include __FBSDID("$FreeBSD$"); +#include + /* - * LIB/MEMORY/ZALLOC.C - self contained low-overhead memory pool/allocation + * LIB/MEMORY/ZALLOC.C - self contained low-overhead memory pool/allocation * subsystem * - * This subsystem implements memory pools and memory allocation + * This subsystem implements memory pools and memory allocation * routines. * * Pools are managed via a linked list of 'free' areas. Allocating @@ -43,7 +45,7 @@ __FBSDID("$FreeBSD$"); * to allocate the entire pool without incuring any structural overhead. * * The system works best when allocating similarly-sized chunks of - * memory. Care must be taken to avoid fragmentation when + * memory. Care must be taken to avoid fragmentation when * allocating/deallocating dissimilar chunks. * * When a memory pool is first allocated, the entire pool is marked as @@ -53,7 +55,7 @@ __FBSDID("$FreeBSD$"); * available. * * z[n]xalloc() works like z[n]alloc() but the allocation is made from - * within the specified address range. If the segment could not be + * within the specified address range. If the segment could not be * allocated, NULL is returned. WARNING! The address range will be * aligned to an 8 or 16 byte boundry depending on the cpu so if you * give an unaligned address range, unexpected results may occur. @@ -86,58 +88,82 @@ typedef char assert_align[(sizeof(struct MemNode) <= MALLOCALIGN) ? 1 : -1]; */ void * -znalloc(MemPool *mp, uintptr_t bytes) +znalloc(MemPool *mp, uintptr_t bytes, size_t align) { - /* - * align according to pool object size (can be 0). This is - * inclusive of the MEMNODE_SIZE_MASK minimum alignment. - * - */ - bytes = (bytes + MEMNODE_SIZE_MASK) & ~MEMNODE_SIZE_MASK; - - if (bytes == 0) - return((void *)-1); - - /* - * locate freelist entry big enough to hold the object. If all objects - * are the same size, this is a constant-time function. - */ - - if (bytes <= mp->mp_Size - mp->mp_Used) { MemNode **pmn; MemNode *mn; - for (pmn = &mp->mp_First; (mn=*pmn) != NULL; pmn = &mn->mr_Next) { - if (bytes > mn->mr_Bytes) - continue; + /* + * align according to pool object size (can be 0). This is + * inclusive of the MEMNODE_SIZE_MASK minimum alignment. + * + */ + bytes = (bytes + MEMNODE_SIZE_MASK) & ~MEMNODE_SIZE_MASK; - /* - * Cut a chunk of memory out of the beginning of this - * block and fixup the link appropriately. - */ + if (bytes == 0) + return ((void *)-1); - { + /* + * locate freelist entry big enough to hold the object. If all objects + * are the same size, this is a constant-time function. + */ + + if (bytes > mp->mp_Size - mp->mp_Used) + return (NULL); + + for (pmn = &mp->mp_First; (mn = *pmn) != NULL; pmn = &mn->mr_Next) { char *ptr = (char *)mn; + uintptr_t dptr; + char *aligned; + size_t extra; + + dptr = (uintptr_t)(ptr + MALLOCALIGN); /* pointer to data */ + aligned = (char *)(roundup2(dptr, align) - MALLOCALIGN); + extra = aligned - ptr; + + if (bytes + extra > mn->mr_Bytes) + continue; + + /* + * Cut extra from head and create new memory node from reminder. + */ + + if (extra != 0) { + MemNode *new; + + new = (MemNode *)aligned; + new->mr_Next = mn->mr_Next; + new->mr_Bytes = mn->mr_Bytes - extra; + + /* And update current memory node */ + mn->mr_Bytes = extra; + mn->mr_Next = new; + /* In next iteration, we will get our aligned address */ + continue; + } + + /* + * Cut a chunk of memory out of the beginning of this + * block and fixup the link appropriately. + */ if (mn->mr_Bytes == bytes) { - *pmn = mn->mr_Next; + *pmn = mn->mr_Next; } else { - mn = (MemNode *)((char *)mn + bytes); - mn->mr_Next = ((MemNode *)ptr)->mr_Next; - mn->mr_Bytes = ((MemNode *)ptr)->mr_Bytes - bytes; - *pmn = mn; + mn = (MemNode *)((char *)mn + bytes); + mn->mr_Next = ((MemNode *)ptr)->mr_Next; + mn->mr_Bytes = ((MemNode *)ptr)->mr_Bytes - bytes; + *pmn = mn; } mp->mp_Used += bytes; return(ptr); - } } - } - /* - * Memory pool is full, return NULL. - */ + /* + * Memory pool is full, return NULL. + */ - return(NULL); + return (NULL); } /* @@ -147,99 +173,97 @@ znalloc(MemPool *mp, uintptr_t bytes) void zfree(MemPool *mp, void *ptr, uintptr_t bytes) { - /* - * align according to pool object size (can be 0). This is - * inclusive of the MEMNODE_SIZE_MASK minimum alignment. - */ - bytes = (bytes + MEMNODE_SIZE_MASK) & ~MEMNODE_SIZE_MASK; - - if (bytes == 0) - return; + MemNode **pmn; + MemNode *mn; - /* - * panic if illegal pointer - */ + /* + * align according to pool object size (can be 0). This is + * inclusive of the MEMNODE_SIZE_MASK minimum alignment. + */ + bytes = (bytes + MEMNODE_SIZE_MASK) & ~MEMNODE_SIZE_MASK; - if ((char *)ptr < (char *)mp->mp_Base || - (char *)ptr + bytes > (char *)mp->mp_End || - ((uintptr_t)ptr & MEMNODE_SIZE_MASK) != 0) - panic("zfree(%p,%ju): wild pointer", ptr, (uintmax_t)bytes); + if (bytes == 0) + return; - /* - * free the segment - */ + /* + * panic if illegal pointer + */ - { - MemNode **pmn; - MemNode *mn; + if ((char *)ptr < (char *)mp->mp_Base || + (char *)ptr + bytes > (char *)mp->mp_End || + ((uintptr_t)ptr & MEMNODE_SIZE_MASK) != 0) + panic("zfree(%p,%ju): wild pointer", ptr, (uintmax_t)bytes); + /* + * free the segment + */ mp->mp_Used -= bytes; for (pmn = &mp->mp_First; (mn = *pmn) != NULL; pmn = &mn->mr_Next) { - /* - * If area between last node and current node - * - check range - * - check merge with next area - * - check merge with previous area - */ - if ((char *)ptr <= (char *)mn) { /* - * range check + * If area between last node and current node + * - check range + * - check merge with next area + * - check merge with previous area */ - if ((char *)ptr + bytes > (char *)mn) { - panic("zfree(%p,%ju): corrupt memlist1", ptr, - (uintmax_t)bytes); + if ((char *)ptr <= (char *)mn) { + /* + * range check + */ + if ((char *)ptr + bytes > (char *)mn) { + panic("zfree(%p,%ju): corrupt memlist1", ptr, + (uintmax_t)bytes); + } + + /* + * merge against next area or create independant area + */ + + if ((char *)ptr + bytes == (char *)mn) { + ((MemNode *)ptr)->mr_Next = mn->mr_Next; + ((MemNode *)ptr)->mr_Bytes = + bytes + mn->mr_Bytes; + } else { + ((MemNode *)ptr)->mr_Next = mn; + ((MemNode *)ptr)->mr_Bytes = bytes; + } + *pmn = mn = (MemNode *)ptr; + + /* + * merge against previous area (if there is a previous + * area). + */ + + if (pmn != &mp->mp_First) { + if ((char *)pmn + ((MemNode*)pmn)->mr_Bytes == + (char *)ptr) { + ((MemNode *)pmn)->mr_Next = mn->mr_Next; + ((MemNode *)pmn)->mr_Bytes += + mn->mr_Bytes; + mn = (MemNode *)pmn; + } + } + return; } - - /* - * merge against next area or create independant area - */ - - if ((char *)ptr + bytes == (char *)mn) { - ((MemNode *)ptr)->mr_Next = mn->mr_Next; - ((MemNode *)ptr)->mr_Bytes= bytes + mn->mr_Bytes; - } else { - ((MemNode *)ptr)->mr_Next = mn; - ((MemNode *)ptr)->mr_Bytes= bytes; + if ((char *)ptr < (char *)mn + mn->mr_Bytes) { + panic("zfree(%p,%ju): corrupt memlist2", ptr, + (uintmax_t)bytes); } - *pmn = mn = (MemNode *)ptr; - - /* - * merge against previous area (if there is a previous - * area). - */ - - if (pmn != &mp->mp_First) { - if ((char*)pmn + ((MemNode*)pmn)->mr_Bytes == (char*)ptr) { - ((MemNode *)pmn)->mr_Next = mn->mr_Next; - ((MemNode *)pmn)->mr_Bytes += mn->mr_Bytes; - mn = (MemNode *)pmn; - } - } - return; - /* NOT REACHED */ - } - if ((char *)ptr < (char *)mn + mn->mr_Bytes) { - panic("zfree(%p,%ju): corrupt memlist2", ptr, - (uintmax_t)bytes); - } } /* * We are beyond the last MemNode, append new MemNode. Merge against * previous area if possible. */ - if (pmn == &mp->mp_First || - (char *)pmn + ((MemNode *)pmn)->mr_Bytes != (char *)ptr - ) { - ((MemNode *)ptr)->mr_Next = NULL; - ((MemNode *)ptr)->mr_Bytes = bytes; - *pmn = (MemNode *)ptr; - mn = (MemNode *)ptr; + if (pmn == &mp->mp_First || + (char *)pmn + ((MemNode *)pmn)->mr_Bytes != (char *)ptr) { + ((MemNode *)ptr)->mr_Next = NULL; + ((MemNode *)ptr)->mr_Bytes = bytes; + *pmn = (MemNode *)ptr; + mn = (MemNode *)ptr; } else { - ((MemNode *)pmn)->mr_Bytes += bytes; - mn = (MemNode *)pmn; + ((MemNode *)pmn)->mr_Bytes += bytes; + mn = (MemNode *)pmn; } - } } /* @@ -256,26 +280,26 @@ zfree(MemPool *mp, void *ptr, uintptr_t bytes) void zextendPool(MemPool *mp, void *base, uintptr_t bytes) { - if (mp->mp_Size == 0) { - mp->mp_Base = base; - mp->mp_Used = bytes; - mp->mp_End = (char *)base + bytes; - mp->mp_Size = bytes; - } else { - void *pend = (char *)mp->mp_Base + mp->mp_Size; - - if (base < mp->mp_Base) { - mp->mp_Size += (char *)mp->mp_Base - (char *)base; - mp->mp_Used += (char *)mp->mp_Base - (char *)base; - mp->mp_Base = base; - } - base = (char *)base + bytes; - if (base > pend) { - mp->mp_Size += (char *)base - (char *)pend; - mp->mp_Used += (char *)base - (char *)pend; - mp->mp_End = (char *)base; + if (mp->mp_Size == 0) { + mp->mp_Base = base; + mp->mp_Used = bytes; + mp->mp_End = (char *)base + bytes; + mp->mp_Size = bytes; + } else { + void *pend = (char *)mp->mp_Base + mp->mp_Size; + + if (base < mp->mp_Base) { + mp->mp_Size += (char *)mp->mp_Base - (char *)base; + mp->mp_Used += (char *)mp->mp_Base - (char *)base; + mp->mp_Base = base; + } + base = (char *)base + bytes; + if (base > pend) { + mp->mp_Size += (char *)base - (char *)pend; + mp->mp_Used += (char *)base - (char *)pend; + mp->mp_End = (char *)base; + } } - } } #ifdef ZALLOCDEBUG @@ -283,34 +307,32 @@ zextendPool(MemPool *mp, void *base, uintptr_t bytes) void zallocstats(MemPool *mp) { - int abytes = 0; - int hbytes = 0; - int fcount = 0; - MemNode *mn; + int abytes = 0; + int hbytes = 0; + int fcount = 0; + MemNode *mn; - printf("%d bytes reserved", (int) mp->mp_Size); + printf("%d bytes reserved", (int)mp->mp_Size); - mn = mp->mp_First; + mn = mp->mp_First; - if ((void *)mn != (void *)mp->mp_Base) { - abytes += (char *)mn - (char *)mp->mp_Base; - } + if ((void *)mn != (void *)mp->mp_Base) { + abytes += (char *)mn - (char *)mp->mp_Base; + } - while (mn) { - if ((char *)mn + mn->mr_Bytes != mp->mp_End) { - hbytes += mn->mr_Bytes; - ++fcount; + while (mn != NULL) { + if ((char *)mn + mn->mr_Bytes != mp->mp_End) { + hbytes += mn->mr_Bytes; + ++fcount; + } + if (mn->mr_Next != NULL) { + abytes += (char *)mn->mr_Next - + ((char *)mn + mn->mr_Bytes); + } + mn = mn->mr_Next; } - if (mn->mr_Next) - abytes += (char *)mn->mr_Next - ((char *)mn + mn->mr_Bytes); - mn = mn->mr_Next; - } - printf(" %d bytes allocated\n%d fragments (%d bytes fragmented)\n", - abytes, - fcount, - hbytes - ); + printf(" %d bytes allocated\n%d fragments (%d bytes fragmented)\n", + abytes, fcount, hbytes); } #endif - diff --git a/stand/libsa/zalloc_defs.h b/stand/libsa/zalloc_defs.h index 7f2cc1202c9..bb7c593ba82 100644 --- a/stand/libsa/zalloc_defs.h +++ b/stand/libsa/zalloc_defs.h @@ -1,5 +1,5 @@ /* - * This module derived from code donated to the FreeBSD Project by + * This module derived from code donated to the FreeBSD Project by * Matthew Dillon * * Copyright (c) 1998 The FreeBSD Project @@ -33,23 +33,26 @@ * DEFS.H */ -#define USEGUARD /* use stard/end guard bytes */ -#define USEENDGUARD -#define DMALLOCDEBUG /* add debugging code to gather stats */ -#define ZALLOCDEBUG +#ifndef _ZALLOC_DEFS_H +#define _ZALLOC_DEFS_H + +#define USEGUARD /* use stard/end guard bytes */ +#define USEENDGUARD +#define DMALLOCDEBUG /* add debugging code to gather stats */ +#define ZALLOCDEBUG #include #include "stand.h" #include "zalloc_mem.h" -#define Library extern +#define Library extern /* * block extension for sbrk() */ -#define BLKEXTEND (4 * 1024) -#define BLKEXTENDMASK (BLKEXTEND - 1) +#define BLKEXTEND (4 * 1024) +#define BLKEXTENDMASK (BLKEXTEND - 1) /* * Required malloc alignment. @@ -68,11 +71,13 @@ #define MALLOCALIGN_MASK (MALLOCALIGN - 1) typedef struct Guard { - size_t ga_Bytes; - size_t ga_Magic; /* must be at least 32 bits */ + size_t ga_Bytes; + size_t ga_Magic; /* must be at least 32 bits */ } Guard; -#define GAMAGIC 0x55FF44FD -#define GAFREE 0x5F54F4DF +#define GAMAGIC 0x55FF44FD +#define GAFREE 0x5F54F4DF #include "zalloc_protos.h" + +#endif /* _ZALLOC_DEFS_H */ diff --git a/stand/libsa/zalloc_malloc.c b/stand/libsa/zalloc_malloc.c index b9a295fb385..98e28b8ef7f 100644 --- a/stand/libsa/zalloc_malloc.c +++ b/stand/libsa/zalloc_malloc.c @@ -1,5 +1,5 @@ /* - * This module derived from code donated to the FreeBSD Project by + * This module derived from code donated to the FreeBSD Project by * Matthew Dillon * * Copyright (c) 1998 The FreeBSD Project @@ -50,139 +50,163 @@ void mallocstats(void); #undef free #endif +static void *Malloc_align(size_t, size_t); + +void * +Malloc(size_t bytes, const char *file __unused, int line __unused) +{ + return (Malloc_align(bytes, 1)); +} + void * -Malloc(size_t bytes, const char *file, int line) +Memalign(size_t alignment, size_t bytes, const char *file __unused, + int line __unused) { - Guard *res; + if (alignment == 0) + alignment = 1; + + return (Malloc_align(bytes, alignment)); +} + +static void * +Malloc_align(size_t bytes, size_t alignment) +{ + Guard *res; #ifdef USEENDGUARD - bytes += MALLOCALIGN + 1; + bytes += MALLOCALIGN + 1; #else - bytes += MALLOCALIGN; + bytes += MALLOCALIGN; #endif - while ((res = znalloc(&MallocPool, bytes)) == NULL) { - int incr = (bytes + BLKEXTENDMASK) & ~BLKEXTENDMASK; - char *base; + while ((res = znalloc(&MallocPool, bytes, alignment)) == NULL) { + int incr = (bytes + BLKEXTENDMASK) & ~BLKEXTENDMASK; + char *base; - if ((base = sbrk(incr)) == (char *)-1) - return(NULL); - zextendPool(&MallocPool, base, incr); - zfree(&MallocPool, base, incr); - } + if ((base = sbrk(incr)) == (char *)-1) + return (NULL); + zextendPool(&MallocPool, base, incr); + zfree(&MallocPool, base, incr); + } #ifdef DMALLOCDEBUG - if (++MallocCount > MallocMax) - MallocMax = MallocCount; + if (++MallocCount > MallocMax) + MallocMax = MallocCount; #endif #ifdef USEGUARD - res->ga_Magic = GAMAGIC; + res->ga_Magic = GAMAGIC; #endif - res->ga_Bytes = bytes; + res->ga_Bytes = bytes; #ifdef USEENDGUARD - *((signed char *)res + bytes - 1) = -2; + *((signed char *)res + bytes - 1) = -2; #endif - return((char *)res + MALLOCALIGN); + return ((char *)res + MALLOCALIGN); } void Free(void *ptr, const char *file, int line) { - size_t bytes; + size_t bytes; - if (ptr != NULL) { - Guard *res = (void *)((char *)ptr - MALLOCALIGN); + if (ptr != NULL) { + Guard *res = (void *)((char *)ptr - MALLOCALIGN); - if (file == NULL) - file = "unknown"; + if (file == NULL) + file = "unknown"; #ifdef USEGUARD - if (res->ga_Magic == GAFREE) { - printf("free: duplicate free @ %p from %s:%d\n", ptr, file, line); - return; - } - if (res->ga_Magic != GAMAGIC) - panic("free: guard1 fail @ %p from %s:%d", ptr, file, line); - res->ga_Magic = GAFREE; + if (res->ga_Magic == GAFREE) { + printf("free: duplicate free @ %p from %s:%d\n", + ptr, file, line); + return; + } + if (res->ga_Magic != GAMAGIC) + panic("free: guard1 fail @ %p from %s:%d", + ptr, file, line); + res->ga_Magic = GAFREE; #endif #ifdef USEENDGUARD - if (*((signed char *)res + res->ga_Bytes - 1) == -1) { - printf("free: duplicate2 free @ %p from %s:%d\n", ptr, file, line); - return; - } - if (*((signed char *)res + res->ga_Bytes - 1) != -2) - panic("free: guard2 fail @ %p + %zu from %s:%d", ptr, res->ga_Bytes - MALLOCALIGN, file, line); - *((signed char *)res + res->ga_Bytes - 1) = -1; + if (*((signed char *)res + res->ga_Bytes - 1) == -1) { + printf("free: duplicate2 free @ %p from %s:%d\n", + ptr, file, line); + return; + } + if (*((signed char *)res + res->ga_Bytes - 1) != -2) + panic("free: guard2 fail @ %p + %zu from %s:%d", + ptr, res->ga_Bytes - MALLOCALIGN, file, line); + *((signed char *)res + res->ga_Bytes - 1) = -1; #endif - bytes = res->ga_Bytes; - zfree(&MallocPool, res, bytes); + bytes = res->ga_Bytes; + zfree(&MallocPool, res, bytes); #ifdef DMALLOCDEBUG - --MallocCount; + --MallocCount; #endif - } + } } void * Calloc(size_t n1, size_t n2, const char *file, int line) { - uintptr_t bytes = (uintptr_t)n1 * (uintptr_t)n2; - void *res; + uintptr_t bytes = (uintptr_t)n1 * (uintptr_t)n2; + void *res; - if ((res = Malloc(bytes, file, line)) != NULL) { - bzero(res, bytes); + if ((res = Malloc(bytes, file, line)) != NULL) { + bzero(res, bytes); #ifdef DMALLOCDEBUG - if (++MallocCount > MallocMax) - MallocMax = MallocCount; + if (++MallocCount > MallocMax) + MallocMax = MallocCount; #endif - } - return(res); + } + return (res); } /* * realloc() - I could be fancier here and free the old buffer before - * allocating the new one (saving potential fragmentation + * allocating the new one (saving potential fragmentation * and potential buffer copies). But I don't bother. */ void * Realloc(void *ptr, size_t size, const char *file, int line) { - void *res; - size_t old; - - if ((res = Malloc(size, file, line)) != NULL) { - if (ptr) { - old = *(size_t *)((char *)ptr - MALLOCALIGN) - MALLOCALIGN; - if (old < size) - bcopy(ptr, res, old); - else - bcopy(ptr, res, size); - Free(ptr, file, line); - } else { + void *res; + size_t old; + + if ((res = Malloc(size, file, line)) != NULL) { + if (ptr != NULL) { + Guard *g = (Guard *)((char *)ptr - MALLOCALIGN); + + old = g->ga_Bytes - MALLOCALIGN; + if (old < size) + bcopy(ptr, res, old); + else + bcopy(ptr, res, size); + Free(ptr, file, line); + } else { #ifdef DMALLOCDEBUG - if (++MallocCount > MallocMax) - MallocMax = MallocCount; + if (++MallocCount > MallocMax) + MallocMax = MallocCount; #ifdef EXITSTATS - if (DidAtExit == 0) { - DidAtExit = 1; - atexit(mallocstats); - } + if (DidAtExit == 0) { + DidAtExit = 1; + atexit(mallocstats); + } #endif #endif + } } - } - return(res); + return (res); } void * Reallocf(void *ptr, size_t size, const char *file, int line) { - void *res; + void *res; - if ((res = Realloc(ptr, size, file, line)) == NULL) - Free(ptr, file, line); - return(res); + if ((res = Realloc(ptr, size, file, line)) == NULL) + Free(ptr, file, line); + return (res); } #ifdef DMALLOCDEBUG @@ -190,11 +214,10 @@ Reallocf(void *ptr, size_t size, const char *file, int line) void mallocstats(void) { - printf("Active Allocations: %d/%d\n", MallocCount, MallocMax); + printf("Active Allocations: %d/%d\n", MallocCount, MallocMax); #ifdef ZALLOCDEBUG - zallocstats(&MallocPool); + zallocstats(&MallocPool); #endif } #endif - diff --git a/stand/libsa/zalloc_mem.h b/stand/libsa/zalloc_mem.h index 26d388dfc75..5a685410597 100644 --- a/stand/libsa/zalloc_mem.h +++ b/stand/libsa/zalloc_mem.h @@ -1,5 +1,5 @@ /* - * This module derived from code donated to the FreeBSD Project by + * This module derived from code donated to the FreeBSD Project by * Matthew Dillon * * Copyright (c) 1998 The FreeBSD Project @@ -34,20 +34,23 @@ * * Basic memory pool / memory node structures. */ +#ifndef _ZALLOC_MEM_H +#define _ZALLOC_MEM_H typedef struct MemNode { - struct MemNode *mr_Next; - uintptr_t mr_Bytes; + struct MemNode *mr_Next; + uintptr_t mr_Bytes; } MemNode; typedef struct MemPool { - void *mp_Base; - void *mp_End; - MemNode *mp_First; - uintptr_t mp_Size; - uintptr_t mp_Used; + void *mp_Base; + void *mp_End; + MemNode *mp_First; + uintptr_t mp_Size; + uintptr_t mp_Used; } MemPool; -#define ZNOTE_FREE 0 -#define ZNOTE_REUSE 1 +#define ZNOTE_FREE 0 +#define ZNOTE_REUSE 1 +#endif /* _ZALLOC_MEM_H */ diff --git a/stand/libsa/zalloc_protos.h b/stand/libsa/zalloc_protos.h index 53a40e400dc..d129a64f299 100644 --- a/stand/libsa/zalloc_protos.h +++ b/stand/libsa/zalloc_protos.h @@ -1,5 +1,5 @@ /* - * This module derived from code donated to the FreeBSD Project by + * This module derived from code donated to the FreeBSD Project by * Matthew Dillon * * Copyright (c) 1998 The FreeBSD Project @@ -29,7 +29,12 @@ * $FreeBSD$ */ -Library void *znalloc(struct MemPool *mpool, uintptr_t bytes); +#ifndef _ZALLOC_PROTOS_H +#define _ZALLOC_PROTOS_H + +Library void *znalloc(struct MemPool *mpool, uintptr_t bytes, size_t align); Library void zfree(struct MemPool *mpool, void *ptr, uintptr_t bytes); Library void zextendPool(MemPool *mp, void *base, uintptr_t bytes); Library void zallocstats(struct MemPool *mp); + +#endif /* _ZALLOC_PROTOS_H */ -- 2.45.0