common

/* Copyright (c) 2021 RISC OS Developments Ltd
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include "swis.h"

#include "Global/UpCall.h"
#include "Interface/RTSupport.h"
#include "Interface/SDIO.h"

#include "SyncLib/synclib.h"

#include "common.h"

/* Bit definitions for CMD52 and CMD53 */

#define IO_RW_WRITE                  (1u<<31)
#define IO_RW_FUNCTION_SHIFT        (28)
#define IO_RW_FUNCTION_MASK          (7u << IO_RW_FUNCTION_SHIFT)
#define IO_RW_RAW                    (1u<<27) /* CMD52 */
#define IO_RW_BLOCK_MODE             (1u<<27) /* CMD53 */
#define IO_RW_INCREMENTING           (1u<<26) /* CMD53 */
#define IO_RW_REGISTER_SHIFT         (9)
#define IO_RW_REGISTER_MASK    (0x1FFFFu << IO_RW_REGISTER_SHIFT)
#define IO_RW_WRITE_DATA_SHIFT       (0) /* CMD52 */
#define IO_RW_WRITE_DATA_MASK     (0xFFu << IO_RW_WRITE_DATA_SHIFT)
#define IO_RW_COUNT_SHIFT            (0) /* CMD53 */
#define IO_RW_COUNT_MASK         (0x1FFu << IO_RW_COUNT_SHIFT)

/** Size of root stack chunk for interrupt handler thread. Chosen arbitrarily. */
#define CHUNK_SIZE (4*1024)
/** Magic marker for stack chunks */
#define STACK_CHUNK_MAGIC 0xF60690FF

/** Our internal state data for each card that contains at least one IO function */
typedef struct
{
  uint8_t bus;           /**< bus index (for use with SDIODriver SWIs) */
  uint8_t slot;          /**< slot index (for use with SDIODriver SWIs) */
  uint16_t rca;          /**< RCA of card */
  uint8_t os_specific[]; /**< OS-specific storage for card */
} int_card_t;

/** Our internal state data for each IO function */
typedef struct int_function
{
  struct int_function *next;             /**< link to next function (a global list, not per-slot) */
  uint8_t number;                        /**< function index, 1..7 */
  uint8_t log2_blklen;                   /**< log2 of block length configured into FBR I/O block size */
  int_card_t *card;                      /**< corresponding card data */
  void (*interrupt_handler)(void *);     /**< client interrupt handler routine */
  void *interrupt_private;               /**< client interrupt handler argument */
  bool interrupt_call_with_lock;         /**< client interrupt handler expects slot lock to be already acquired */
  volatile uint32_t interrupt_semaphore; /**< used to wake the interrupt thread */
  _kernel_stack_chunk *thread_stack;     /**< stack for interrupt thread */
  void *thread_handle;                   /**< RTSupport handle for interrupt thread, or 0 if none */
  uint8_t os_specific[];                 /**< OS-specific storage for function */
} int_function_t;

/** Data block updated by sdiolib_op_callback */
typedef struct
{
  _kernel_oserror *e;
  // cppcheck-suppress unusedStructMember - there's currently nowhere to send this information in OpenBSD or Linux APIs
  size_t not_transferred;
  volatile uint32_t pollword;
} callback_state_t;

/* Declare entries to/from assembly */
void sdiolib_op_callback(void);
void sdiolib_interrupt_trampoline(void);
void sdiolib_interrupt_handler(int_function_t *f);

/* Linked list of known functions */
static int_function_t *functions_head = NULL;
static int_function_t *functions_tail = NULL;

/** Pseudo-intrinsic for CLZ instruction. */
static inline uint32_t clz(uint32_t x)
{
  uint32_t result;
  __asm {
    CLZ result, x
  }
  return result;
}

/** Fetch the currently-active client relocation offset from the base of the current stack chunk. */
static inline void *reloc_offset(void)
{
  return ((void **)(_kernel_current_stack_chunk() + 1))[1];
}

/** Perform a blocking SD operation with an R1 response and no data transfer. */
static _kernel_oserror *r1command_nodata(const int_card_t *restrict c, uint8_t command, uint32_t argument, uint32_t *restrict response)
{
  callback_state_t callback_state = { 0 };
  uint32_t cmdres[3] = { command, argument };
  _kernel_oserror *e = _swix(SDIO_Op, _INR(0,2)|_INR(4,7),
      (c->bus << SDIOOp_BusShift) |
      (c->slot) << SDIOOp_SlotShift |
      SDIOOp_R1 | SDIOOp_Background,
      sizeof cmdres,
      cmdres,
      0,
      &callback_state,
      sdiolib_op_callback,
      reloc_offset());
  while (!e && callback_state.pollword == 0)
    e = _swix(OS_UpCall, _INR(0,1), UpCall_Sleep, &callback_state.pollword);
  if (!e)
  {
    barrier();
    e = callback_state.e;
  }
  if (response)
    *response = cmdres[2];
  return e;
}

/** Perform a blocking SD operation with an R5 response and no data transfer. */
static _kernel_oserror *r5command_nodata(const int_card_t *restrict c, uint8_t command, uint32_t argument, uint32_t *restrict response)
{
  callback_state_t callback_state = { 0 };
  uint32_t cmdres[3] = { command, argument };
  _kernel_oserror *e = _swix(SDIO_Op, _INR(0,2)|_INR(4,7),
      (c->bus << SDIOOp_BusShift) |
      (c->slot) << SDIOOp_SlotShift |
      SDIOOp_R5sdio | SDIOOp_Background,
      sizeof cmdres,
      cmdres,
      0,
      &callback_state,
      sdiolib_op_callback,
      reloc_offset());
  while (!e && callback_state.pollword == 0)
    e = _swix(OS_UpCall, _INR(0,1), UpCall_Sleep, &callback_state.pollword);
  if (!e)
  {
    barrier();
    e = callback_state.e;
  }
  if (response)
    *response = cmdres[2];
  return e;
}

/** Perform a blocking SD operation with an R5 response and data transfer from the card. */
static _kernel_oserror *r5command_readdata(const int_card_t *restrict c, uint8_t command, uint32_t argument, uint32_t *restrict response, void *restrict data, size_t length, uint8_t log2_blklen)
{
  callback_state_t callback_state = { 0 };
  uint32_t cmdres[3] = { command | log2_blklen<<SDIOOp_CmdFlg_BlockSplitShift, argument };
  _kernel_oserror *e = _swix(SDIO_Op, _INR(0,7),
      (c->bus << SDIOOp_BusShift) |
      (c->slot) << SDIOOp_SlotShift |
      SDIOOp_R5sdio | SDIOOp_TransRead | SDIOOp_Background,
      sizeof cmdres,
      cmdres,
      data,
      length,
      &callback_state,
      sdiolib_op_callback,
      reloc_offset());
  while (!e && callback_state.pollword == 0)
    e = _swix(OS_UpCall, _INR(0,1), UpCall_Sleep, &callback_state.pollword);
  if (!e)
  {
    barrier();
    e = callback_state.e;
  }
  if (response)
    *response = cmdres[2];
  return e;
}

/** Perform a blocking SD operation with an R5 response and data transfer to the card. */
static _kernel_oserror *r5command_writedata(const int_card_t *restrict c, uint8_t command, uint32_t argument, uint32_t *restrict response, const void *restrict data, size_t length, uint8_t log2_blklen)
{
  callback_state_t callback_state = { 0 };
  uint32_t cmdres[3] = { command | log2_blklen<<SDIOOp_CmdFlg_BlockSplitShift, argument };
  _kernel_oserror *e = _swix(SDIO_Op, _INR(0,7),
      (c->bus << SDIOOp_BusShift) |
      (c->slot) << SDIOOp_SlotShift |
      SDIOOp_R5sdio | SDIOOp_TransWrite | SDIOOp_Background,
      sizeof cmdres,
      cmdres,
      data,
      length,
      &callback_state,
      sdiolib_op_callback,
      reloc_offset());
  while (!e && callback_state.pollword == 0)
    e = _swix(OS_UpCall, _INR(0,1), UpCall_Sleep, &callback_state.pollword);
  if (!e)
  {
    barrier();
    e = callback_state.e;
  }
  if (response)
    *response = cmdres[2];
  return e;
}

/** Ensure the card is in cmd state if it isn't already. */
static _kernel_oserror *ensure_selected(const int_card_t *c)
{
  uint32_t selected_rca;
  _kernel_oserror *e = _swix(SDIO_Status, _INR(0,1)|_OUT(0),
      SDIOStatus_CurrentRCA,
      (c->bus << SDIOStatus_BusShift) | (c->slot << SDIOStatus_SlotShift),
      &selected_rca);
  if (!e && selected_rca != c->rca)
    e = r1command_nodata(c, CMD7_SELECT_DESELECT_CARD, c->rca << 16, NULL);
  return e;
}

/** Probe for IO functions. Should only be called once (hot-plug not currently supported).
 * @arg card_size      Size of data to allocate for OS-specific storage per card
 * @arg init_card      Callback function to initialise OS-specific storage for a card
 * @arg function_size  Size of data to allocate for OS-specific storage per function
 * @arg init_function  Callback function to initialise OS-specific storage for a function
 * */
void sdiolib_init(size_t card_size, void (*init_card)(void *), size_t function_size, void (*init_function)(void *, void *, uint8_t, uint16_t, uint16_t, uint16_t))
{
  /* Initialise SyncLib in case our client didn't */
  synclib_init();

  /* Loop over all slots */
  for (uint32_t slot_key = 0;;)
  {
    uint32_t slot_details;
    if (_swix(SDIO_Enumerate, _INR(0,1)|_OUTR(1,2),
        SDIOEnumerate_Slots, slot_key,
        &slot_key, &slot_details) != NULL ||
        slot_key == 0)
      break;

    /* We don't yet know if this card has any IO functions, so delay
     * allocating a heap block until we know for sure */
    int_card_t *card = NULL;

    /* Loop over all units for this slot */
    for (uint32_t unit_key = 0;;)
    {
      uint32_t unit_details[2], manf_id, card_id;
      if (_swix(SDIO_Enumerate, _INR(0,2)|_OUTR(1,5),
          SDIOEnumerate_Units, unit_key, slot_details,
          &unit_key, &unit_details[0], &unit_details[1], &manf_id, &card_id) != NULL ||
          unit_key == 0)
        break;
      if (unit_details[1] & SDIOEnumerate_UnitMask)
      {
        /* Found an IO unit */
        if (card == NULL)
        {
          /* Allocate and initialise card data */
          card = calloc(1, sizeof (int_card_t) + card_size);
          if (card == NULL)
           break;
          card->bus = (unit_details[0] & SDIOEnumerate_BusMask) >> SDIOEnumerate_BusShift;
          card->slot = (unit_details[0] & SDIOEnumerate_SlotMask) >> SDIOEnumerate_SlotShift;
          card->rca = (unit_details[0] & SDIOEnumerate_RCAMask) >> SDIOEnumerate_RCAShift;
          init_card(card->os_specific);
        }
        /* Allocate and initialise function data */
        int_function_t *f = calloc(1, sizeof (int_function_t) + function_size);
        if (f == NULL)
          break;
        if (functions_head == NULL)
          functions_head = f;
        else
          functions_tail->next = f;
        functions_tail = f;
        f->number = (unit_details[1] & SDIOEnumerate_UnitMask) >> SDIOEnumerate_UnitShift;
        f->card = card;
        uint32_t maxblklen = 0;
        _swix(SDIO_Status, _INR(0,1)|_OUT(0),
            SDIOStatus_FunctionMaxBlockLen,
            (f->number << SDIOStatus_UnitShift) |
            (card->slot << SDIOStatus_SlotShift) |
            (card->bus << SDIOStatus_BusShift),
            &maxblklen);
        init_function(f->os_specific,
            card->os_specific,
            (unit_details[1] & SDIOEnumerate_UnitMask) >> SDIOEnumerate_UnitShift,
            manf_id,
            card_id,
            maxblklen);
      }
    }
  }
}

/** Release all resources used by the library */
void sdiolib_final(void)
{
  int_card_t *card = NULL;
  int_function_t *next;
  for (int_function_t *f = functions_head; f != NULL; f = next)
  {
    /* Functions belonging to the same card will always be grouped together, so
     * each time we see the card pointer change, we know it's a unique value
     * that can be destructed. */
    if (f->card != card)
    {
      card = f->card;
      free(card);
    }

    /* Free any resources associated with the function */
    /* Note that the client will require its own mechanism for releasing
     * anything whose handles are stored on the interrupt thread stack prior
     * to this point. */
    sdiolib_remove_interrupt_handler(f->os_specific);

    next = f->next;
    free(f);
  }
}

/** Discover the next available function.
 *  @arg previous  NULL to start a new enumeration, else the value returned from previous call
 *  @return        pointer to OS-specific function data, or NULL if no more functions */

void *sdiolib_enumerate_functions(void *previous)
{
  int_function_t *f;
  if (previous == NULL)
    f = functions_head;
  else
    f = ((int_function_t *) previous - 1)->next;
  if (f == NULL)
    return NULL;
  else
    return f->os_specific;
}

/** Acquire exclusive use of a slot (sleeping thread if possible). */
void sdiolib_lock(const void *function)
{
  const int_function_t *f = (const int_function_t *) function - 1;
  _swix(SDIO_Control, _INR(0,1),
      SDIOControl_SleepLock,
      (f->card->bus << SDIOControl_BusShift) |
      (f->card->slot << SDIOControl_SlotShift));
}

/** Release exclusive use of a slot. */
void sdiolib_unlock(const void *function)
{
  const int_function_t *f = (const int_function_t *) function - 1;
  _swix(SDIO_Control, _INR(0,1),
      SDIOControl_Unlock,
      (f->card->bus << SDIOControl_BusShift) |
      (f->card->slot << SDIOControl_SlotShift));
}

/** Perform an IO_RW_DIRECT command to read a register. */
_kernel_oserror *sdiolib_io_rw_direct_read(const void *restrict function, bool cia, uint32_t reg, uint8_t *restrict data)
{
  const int_function_t *f = (int_function_t *) function - 1;
  uint32_t response;
  _kernel_oserror *e = ensure_selected(f->card);
  if (!e)
  {
    uint32_t argument = (reg << IO_RW_REGISTER_SHIFT) & IO_RW_REGISTER_MASK;
    if (!cia)
      argument |= (f->number << IO_RW_FUNCTION_SHIFT) & IO_RW_FUNCTION_MASK;
    e = r5command_nodata(f->card, CMD52_IO_RW_DIRECT, argument, &response);
  }
  if (!e)
    *data = response;
  return e;
}

/** Perform an IO_RW_DIRECT command to write a register. */
_kernel_oserror *sdiolib_io_rw_direct_write(const void *function, bool cia, uint32_t reg, uint8_t data)
{
  const int_function_t *f = (int_function_t *) function - 1;
  _kernel_oserror *e = ensure_selected(f->card);
  if (!e)
  {
    uint32_t argument = IO_RW_WRITE | (data << IO_RW_WRITE_DATA_SHIFT);
    argument |= (reg << IO_RW_REGISTER_SHIFT) & IO_RW_REGISTER_MASK;
    if (!cia)
      argument |= (f->number << IO_RW_FUNCTION_SHIFT) & IO_RW_FUNCTION_MASK;
    e = r5command_nodata(f->card, CMD52_IO_RW_DIRECT, argument, NULL);
  }
  return e;
}

/** Perform an IO_RW_EXTENDED command to perform a sequence of register reads. */
_kernel_oserror *sdiolib_io_rw_extended_read(const void *restrict function, bool cia, uint32_t reg, bool incrementing, void *restrict data, size_t bytes, bool block_mode)
{
  const int_function_t *f = (int_function_t *) function - 1;
  if (block_mode && (bytes & ((1<<f->log2_blklen)-1)) != 0)
    return (_kernel_oserror *) "\0\0\0\0Transfer must be a whole number of blocks in block mode";
  _kernel_oserror *e = ensure_selected(f->card);
  if (!e)
  {
    uint32_t argument = (reg << IO_RW_REGISTER_SHIFT) & IO_RW_REGISTER_MASK;
    if (!cia)
      argument |= (f->number << IO_RW_FUNCTION_SHIFT) & IO_RW_FUNCTION_MASK;
    if (incrementing)
      argument |= IO_RW_INCREMENTING;
    if (block_mode)
      argument |= (((bytes >> f->log2_blklen) << IO_RW_COUNT_SHIFT) & IO_RW_COUNT_MASK) | IO_RW_BLOCK_MODE;
    else
      argument |= (bytes << IO_RW_COUNT_SHIFT) & IO_RW_COUNT_MASK;
    e = r5command_readdata(f->card, CMD53_IO_RW_EXTENDED, argument, NULL, data, bytes, f->log2_blklen);
  }
  return e;
}

/** Perform an IO_RW_EXTENDED command to perform a sequence of register writes. */
_kernel_oserror *sdiolib_io_rw_extended_write(const void *restrict function, bool cia, uint32_t reg, bool incrementing, const void *restrict data, size_t bytes, bool block_mode)
{
  const int_function_t *f = (int_function_t *) function - 1;
  if (block_mode && (bytes & ((1<<f->log2_blklen)-1)) != 0)
    return (_kernel_oserror *) "\0\0\0\0Transfer must be a whole number of blocks in block mode";
  _kernel_oserror *e = ensure_selected(f->card);
  if (!e)
  {
    uint32_t argument = IO_RW_WRITE | ((reg << IO_RW_REGISTER_SHIFT) & IO_RW_REGISTER_MASK);
    if (!cia)
      argument |= (f->number << IO_RW_FUNCTION_SHIFT) & IO_RW_FUNCTION_MASK;
    if (incrementing)
      argument |= IO_RW_INCREMENTING;
    if (block_mode)
      argument |= (((bytes >> f->log2_blklen) << IO_RW_COUNT_SHIFT) & IO_RW_COUNT_MASK) | IO_RW_BLOCK_MODE;
    else
      argument |= (bytes << IO_RW_COUNT_SHIFT) & IO_RW_COUNT_MASK;
    e = r5command_writedata(f->card, CMD53_IO_RW_EXTENDED, argument, NULL, data, bytes, f->log2_blklen);
  }
  return e;
}

/** Perform arbitrarily large data transfer to or from a fixed register or range of registers. */
_kernel_oserror *sdiolib_transfer(const void *restrict function, bool cia, bool write_not_read, uint32_t reg, bool incrementing, uint8_t *restrict data, size_t bytes)
{
  const int_function_t *f = (int_function_t *) function - 1;
  _kernel_oserror *e = NULL;
  _kernel_oserror *(*io_rw_extended)(const void *restrict, bool, uint32_t, bool, const void *restrict, size_t, bool) =
      write_not_read ? sdiolib_io_rw_extended_write : (_kernel_oserror *(*)(const void *restrict, bool, uint32_t, bool, const void *restrict, size_t, bool)) sdiolib_io_rw_extended_read;
  while (!e && bytes >= (IO_RW_COUNT_MASK << f->log2_blklen))
  {
    size_t bytes_this_time = IO_RW_COUNT_MASK << f->log2_blklen;
    e = io_rw_extended(function, cia, reg, incrementing, data, bytes_this_time, true);
    if (incrementing)
      reg += bytes_this_time;
    data += bytes_this_time;
    bytes -= bytes_this_time;
  }
  if (!e && bytes >> f->log2_blklen)
  {
    size_t bytes_this_time = bytes &~ ((1u << f->log2_blklen) - 1);
    e = io_rw_extended(function, cia, reg, incrementing, data, bytes_this_time, true);
    if (incrementing)
      reg += bytes_this_time;
    data += bytes_this_time;
    bytes -= bytes_this_time;
  }
  if (!e && bytes)
    e = io_rw_extended(function, cia, reg, incrementing, data, bytes, false);
  return e;
}

/** Minimalist interrupt handler: defers all work to the RTSupport thread. */
void sdiolib_interrupt_handler(int_function_t *f)
{
  atomic_update(1,&f->interrupt_semaphore);
}

/** Main entry point for RTSupport thread. */
static void interrupt_thread(int_function_t *f)
{
  for (;;)
  {
    /* Sleep thread waiting for semaphore */
    while (f->interrupt_semaphore == 0)
      _swix(RT_Yield, _IN(1), &f->interrupt_semaphore);

    atomic_update(0,&f->interrupt_semaphore);
    /* Lock slot if appropriate */
    if (f->interrupt_call_with_lock)
      sdiolib_lock(f->os_specific);

    /* Dispatch to client */
    f->interrupt_handler(f->interrupt_private);

    /* Unlock slot if appropriate */
    if (f->interrupt_call_with_lock)
      sdiolib_unlock(f->os_specific);

    barrier();
    _swix(SDIO_Control, _INR(0,1),
        SDIOControl_AckSDIOCardInt,
        (f->card->bus << SDIOControl_BusShift) |
        (f->card->slot << SDIOControl_SlotShift));
  }
}

/** Set up interrupt handler (better to do so on demand than to do so during
 *  enumeration, which would potentially be wasteful of resources). */
_kernel_oserror *sdiolib_install_interrupt_handler(void *function, void (*fun)(void *), void *arg, bool call_with_lock)
{
  int_function_t *f = (int_function_t *) function - 1;
  _kernel_oserror *e;

  /* Set up thread stack */
  if ((f->thread_stack = calloc(1, CHUNK_SIZE)) == NULL)
    return (_kernel_oserror *) "\0\0\0\0Memory allocation failure"; /* Error never actually thrown, so could be any arbitrary non-NULL pointer, but just in case someone printfs errmess... */
  f->thread_stack->sc_mark = STACK_CHUNK_MAGIC;
  f->thread_stack->sc_size = CHUNK_SIZE;
  f->thread_stack->sc_deallocate = (int (*)()) free;
  memcpy(f->thread_stack + 1, _kernel_current_stack_chunk() + 1, 28);

  /* Store client routine parameters */
  f->interrupt_handler = fun;
  f->interrupt_private = arg;
  f->interrupt_call_with_lock = call_with_lock;

  /* Create thread */
  const int guaranteed_nonzero = 1;
  if ((e = _swix(RT_Register, _INR(0,7)|_OUT(0),
                 0, interrupt_thread, f, 0, &guaranteed_nonzero,
                 (uintptr_t) f->thread_stack + 560,
                 (uintptr_t) f->thread_stack + CHUNK_SIZE,
                 "Normal", &f->thread_handle)) != NULL)
  {
    free(f->thread_stack);
    f->thread_stack = NULL;
    return e;
  }

  /* Register interrupt handler with SDIODriver */
  if ((e = _swix(SDIO_ClaimDeviceVector, _INR(0,2),
                 (f->number << SDIOCDV_UnitShift) |
                 (f->card->slot << SDIOCDV_SlotShift) |
                 (f->card->bus << SDIOCDV_BusShift),
                 sdiolib_interrupt_trampoline,
                 f)) != NULL)
  {
    _swix(RT_Deregister, _INR(0,1), 0, f->thread_handle);
    f->thread_handle = NULL;
    free(f->thread_stack);
    f->thread_stack = NULL;
    return e;
  }

  return NULL;
}

/** Shut down interrupt handler. */
void sdiolib_remove_interrupt_handler(void *function)
{
  int_function_t *f = (int_function_t *) function - 1;

  if (f->thread_handle != NULL)
  {
    _swix(SDIO_ReleaseDeviceVector, _INR(0,2),
          (f->number << SDIOCDV_UnitShift) |
          (f->card->slot << SDIOCDV_SlotShift) |
          (f->card->bus << SDIOCDV_BusShift),
          sdiolib_interrupt_trampoline,
          f);
    _swix(RT_Deregister, _INR(0,1), 0, f->thread_handle);
    f->thread_handle = NULL;
    _kernel_stack_chunk *next;
    for (_kernel_stack_chunk *chunk = f->thread_stack; chunk != NULL; chunk = next)
    {
      next = chunk->sc_next;
      if (chunk->sc_deallocate)
        chunk->sc_deallocate(chunk);
    }
    f->thread_stack = NULL;
  }
}

/** Query longest block length supported by controller. */
_kernel_oserror *sdiolib_get_controller_max_block_size(const void *restrict function, uint16_t *result)
{
  int_function_t *f = (int_function_t *) function - 1;

  /* Block size must be a power of 2 no larger than the controller supports */
  uint32_t log2_ctrlr_max_blklen;
  _kernel_oserror *e;
  if ((e = _swix(SDIO_ControllerFeatures, _INR(0,1)|_OUT(2),
                 SDIOController_MaxBlockLen,
                 (f->card->slot << SDIOController_SlotShift) |
                 (f->card->bus << SDIOController_BusShift),
                 &log2_ctrlr_max_blklen)) != NULL)
    return e;

  *result = 1u << log2_ctrlr_max_blklen;
  return NULL;
}

/** Set block length in FBR, and remember block length to use in SD controller. */
_kernel_oserror *sdiolib_set_block_size(const void *function, size_t blklen)
{
  int_function_t *f = (int_function_t *) function - 1;
  _kernel_oserror *e;

  /* Block size must be a power of 2 no larger than the controller supports */
  uint16_t ctrlr_max;
  if ((e = sdiolib_get_controller_max_block_size(function, &ctrlr_max)) != NULL)
    return e;

  if (blklen == 0 || (blklen & (blklen-1)) || blklen > ctrlr_max)
    return (_kernel_oserror *)"\0\0\0\0Unsupported block size";

  if ((e = sdiolib_io_rw_direct_write(function, true, FBR_IO_BLOCK_SIZE0(f->number), blklen & 0xFF)) != NULL ||
      (e = sdiolib_io_rw_direct_write(function, true, FBR_IO_BLOCK_SIZE1(f->number), (blklen >> 8) & 0xFF)) != NULL)
    return e;

  f->log2_blklen = 31 - clz(blklen);
  return NULL;
}