deps/libretro-common/libco/psp2.c

   1 /*
   2 libco.arm (2015-06-18)
   3 license: public domain
   4 */
   5
   6 #define LIBCO_C
   7 #include "libco.h"
   8
   9 #include <assert.h>
  10 #include <stdlib.h>
  11 #include <unistd.h>
  12 #include <psp2/kernel/sysmem.h>
  13 #include <stdio.h>
  14 #include <string.h>
  15
  16 #define FOUR_KB_ALIGN(x) align(x, 12)
  17 #define MB_ALIGN(x)      align(x, 20)
  18
  19 #ifdef __cplusplus
  20 extern "C" {
  21 #endif
  22
  23    static inline int align(int x, int n)
  24    {
  25       return (((x >> n) + 1) << n);
  26    }
  27
  28    static thread_local unsigned long co_active_buffer[64];
  29    static thread_local cothread_t co_active_handle = 0;
  30    static void(*co_swap)(cothread_t, cothread_t) = 0;
  31    static int block;
  32    static uint32_t co_swap_function[] = {
  33       0xe8a16ff0,  /* stmia r1!, {r4-r11,sp,lr} */
  34       0xe8b0aff0,  /* ldmia r0!, {r4-r11,sp,pc} */
  35       0xe12fff1e,  /* bx lr                     */
  36    };
  37
  38    static void co_init(void)
  39    {
  40       int ret;
  41       void *base;
  42
  43       block = sceKernelAllocMemBlockForVM("libco",
  44             MB_ALIGN(FOUR_KB_ALIGN(sizeof co_swap_function)));
  45       if (block < 0)
  46          return;
  47
  48       /* Get base address */
  49       if ((ret = sceKernelGetMemBlockBase(block, &base)) < 0)
  50          return;
  51
  52       /* Set domain to be writable by user */
  53       if ((ret = sceKernelOpenVMDomain()) < 0)
  54          return;
  55
  56       memcpy(base, co_swap_function, sizeof co_swap_function);
  57
  58       /* Set domain back to read-only */
  59       if ((ret = sceKernelCloseVMDomain()) < 0)
  60          return;
  61
  62       /* Flush icache */
  63       ret = sceKernelSyncVMDomain(block, base,
  64             MB_ALIGN(FOUR_KB_ALIGN(sizeof co_swap_function)));
  65       if (ret < 0)
  66          return;
  67
  68       co_swap = (void(*)(cothread_t, cothread_t))base;
  69    }
  70
  71    cothread_t co_active(void)
  72    {
  73       if (!co_active_handle)
  74          co_active_handle = &co_active_buffer;
  75       return co_active_handle;
  76    }
  77
  78    cothread_t co_create(unsigned int size, void(*entrypoint)(void))
  79    {
  80       unsigned long* handle = 0;
  81       if (!co_swap)
  82          co_init();
  83       if (!co_active_handle)
  84          co_active_handle   = &co_active_buffer;
  85       size                 += 256;
  86       size                 &= ~15;
  87
  88       if ((handle = (unsigned long*)malloc(size)))
  89       {
  90          unsigned long *p   = (unsigned long*)((unsigned char*)handle + size);
  91          handle[8]          = (unsigned long)p;
  92          handle[9]          = (unsigned long)entrypoint;
  93       }
  94
  95       return handle;
  96    }
  97
  98    void co_delete(cothread_t handle)
  99    {
 100       free(handle);
 101       sceKernelFreeMemBlock(block);
 102    }
 103
 104    void co_switch(cothread_t handle)
 105    {
 106       cothread_t co_previous_handle = co_active_handle;
 107       co_swap(co_active_handle = handle, co_previous_handle);
 108    }
 109
 110 #ifdef __cplusplus
 111 }
 112 #endif