stage2/netrpc.c

   1 /*  netrpc.c - server to perform lv1 and device ops via network requests
   2
   3 Copyright (C) 2010-2011  Hector Martin "marcan" <hector@marcansoft.com>
   4
   5 This code is licensed to you under the terms of the GNU GPL, version 2;
   6 see file COPYING or http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
   7 */
   8
   9 #include <stddef.h>
  10
  11 #include "config.h"
  12 #include "netrpc.h"
  13
  14 #include "types.h"
  15 #include "lwip/udp.h"
  16 #include "debug.h"
  17 #include "lv1call.h"
  18 #include "mm.h"
  19 #include "network.h"
  20
  21 #ifdef NETRPC_ENABLE
  22
  23 extern volatile u64 _thread1_release;
  24 extern volatile u64 _thread1_vector;
  25
  26 enum {
  27         RPC_PING = 0,
  28
  29         RPC_READMEM,
  30         RPC_WRITEMEM,
  31         RPC_HVCALL,
  32         RPC_ADDMMIO,
  33         RPC_DELMMIO,
  34         RPC_CLRMMIO,
  35         RPC_MEMSET,
  36         RPC_VECTOR,
  37         RPC_SYNC,
  38         RPC_CALL,
  39 };
  40
  41 struct rpc_vec {
  42         void *vec0;
  43         void *vec1;
  44         void *copy_dst;
  45         void *copy_src;
  46         u32 copy_size;
  47 };
  48
  49 typedef s64 (*rpc_callfn) (u64,u64,u64,u64,u64,u64,u64,u64);
  50
  51 struct rpc_header {
  52         u32 cmd;
  53         u32 tag;
  54         union {
  55                 struct {
  56                         void *addr;
  57                         u32 size;
  58                         u8 data[];
  59                 } memop;
  60                 struct {
  61                         u32 numin, numout;
  62                         u64 code;
  63                         u64 regs[];
  64                 } hvcall;
  65                 struct {
  66                         u64 start;
  67                         u32 size;
  68                 } addmmio;
  69                 struct {
  70                         u64 start;
  71                 } delmmio;
  72                 struct {
  73                         s64 retcode;
  74                         u8 retdata[];
  75                 } reply;
  76                 struct rpc_vec vector;
  77                 struct {
  78                         void *addr;
  79                         u32 size;
  80                 } sync;
  81                 struct {
  82                         rpc_callfn addr;
  83                         u64 args[8];
  84                 } call;
  85         };
  86 } __attribute__((packed));
  87
  88 static struct udp_pcb *pcb;
  89
  90 #define BUFSIZE 1500
  91 #define XFERSIZE 1024
  92 #define PORT 1337
  93
  94 static struct pbuf *outbuf;
  95
  96 #define REPLY_SIZE 16
  97
  98 static void sendbuf(int len, struct ip_addr *addr, u16_t port)
  99 {
 100         outbuf->tot_len = len;
 101         outbuf->len = len;
 102
 103         err_t err = udp_sendto(pcb, outbuf, addr, port);
 104         // reclaim the header space used by udp_send
 105         pbuf_header(outbuf, -(outbuf->len - len));
 106         outbuf->tot_len = BUFSIZE;
 107         outbuf->len = BUFSIZE;
 108         if (err)
 109                 printf("netrpc: udp_sendto() returned %d\n", err);
 110 }
 111
 112 void memcpy64(u64 *dst, u64 *src, s32 size)
 113 {
 114         while (size > 0) {
 115                 *dst++ = *src++;
 116                 size -= 8;
 117         }
 118 }
 119
 120 void memcpy32(u32 *dst, u32 *src, s32 size)
 121 {
 122         while (size > 0) {
 123                 *dst++ = *src++;
 124                 size -= 4;
 125         }
 126 }
 127
 128 void memcpy16(u16 *dst, u16 *src, s32 size)
 129 {
 130         while (size > 0) {
 131                 *dst++ = *src++;
 132                 size -= 2;
 133         }
 134 }
 135
 136 void memset64(u64 *dst, u64 val, s32 size)
 137 {
 138         while (size > 0) {
 139                 *dst++ = val;
 140                 size -= 8;
 141         }
 142 }
 143
 144 void memset32(u32 *dst, u32 val, s32 size)
 145 {
 146         while (size > 0) {
 147                 *dst++ = val;
 148                 size -= 4;
 149         }
 150 }
 151
 152 void memset16(u16 *dst, u16 val, s32 size)
 153 {
 154         while (size > 0) {
 155                 *dst++ = val;
 156                 size -= 2;
 157         }
 158 }
 159
 160 void memcpy_align(void *dst, void *src, u32 size)
 161 {
 162         u64 t = ((u64)dst)|((u64)src)|size;
 163
 164         if ((t&7) == 0) {
 165                 memcpy64(dst, src, size);
 166         } else if ((t&3) == 0) {
 167                 memcpy32(dst, src, size);
 168         } else if ((t&1) == 0) {
 169                 memcpy16(dst, src, size);
 170         } else {
 171                 memcpy(dst, src, size);
 172         }
 173 }
 174
 175 void memset_align(void *dst, u64 val, u32 size)
 176 {
 177         u64 t = ((u64)dst)|size;
 178
 179         if ((t&7) == 0) {
 180                 memset64(dst, val, size);
 181         } else if ((t&3) == 0) {
 182                 memset32(dst, val, size);
 183         } else if ((t&1) == 0) {
 184                 memset16(dst, val, size);
 185         } else {
 186                 memset(dst, val, size);
 187         }
 188 }
 189
 190 static u8 tmpbuf[XFERSIZE] __attribute__((aligned(64)));
 191
 192 static u64 *vector[3] = {NULL,NULL,NULL};
 193
 194 static void netrpc_vector(void *vec0, void *vec1, void *copy_dst, void *copy_src, u32 copy_size)
 195 {
 196         netrpc_shutdown();
 197         net_shutdown();
 198         mm_shutdown_highmem();
 199         mm_shutdown();
 200         if (copy_size) {
 201                 printf("netrpc: Relocating vectors...\n");
 202                 memcpy(copy_dst, copy_src, copy_size);
 203                 sync_before_exec(copy_dst, copy_size);
 204         }
 205         printf("netrpc: Letting thread1 run loose...\n");
 206         _thread1_vector = (u64)vec1;
 207         _thread1_release = 1;
 208         vector[0] = vec0;
 209         printf("netrpc: Taking the plunge...\n");
 210         debug_shutdown();
 211         ((void (*)(void))vector)();
 212         lv1_panic(0);
 213 }
 214
 215 static void netrpc_recv(void *arg, struct udp_pcb *upcb, struct pbuf *p, struct ip_addr *addr, u16_t port)
 216 {
 217         if (p->tot_len > BUFSIZE) {
 218                 printf("netrpc: max buffer size exceeded (%d)\n", p->tot_len);
 219                 return;
 220         }
 221
 222         u8 *buf = outbuf->payload;
 223         pbuf_copy_partial(p, buf, BUFSIZE, 0);
 224
 225         struct rpc_header *hdr = (void*)buf;
 226
 227         u64 regs[9];
 228         u32 numout;
 229         u32 size;
 230         u64 val;
 231
 232         struct rpc_vec vector;
 233
 234         switch (hdr->cmd) {
 235                 case RPC_PING:
 236                         hdr->reply.retcode = 0;
 237                         sendbuf(REPLY_SIZE, addr, port);
 238                         break;
 239                 case RPC_READMEM:
 240                         if (hdr->memop.size > XFERSIZE) {
 241                                 printf("netrpc: READMEM size > %d (%d)\n", XFERSIZE, hdr->memop.size);
 242                                 hdr->reply.retcode = -1;
 243                                 sendbuf(REPLY_SIZE, addr, port);
 244                                 break;
 245                         }
 246                         size = hdr->memop.size;
 247                         memcpy_align(tmpbuf, hdr->memop.addr, size);
 248                         memcpy(hdr->reply.retdata, tmpbuf, size);
 249                         hdr->reply.retcode = 0;
 250                         sendbuf(REPLY_SIZE + size, addr, port);
 251                         break;
 252                 case RPC_WRITEMEM:
 253                         if (hdr->memop.size > XFERSIZE) {
 254                                 printf("netrpc: WRITEMEM size > %d (%d)\n", XFERSIZE, hdr->memop.size);
 255                                 hdr->reply.retcode = -1;
 256                                 sendbuf(REPLY_SIZE, addr, port);
 257                                 break;
 258                         }
 259                         memcpy(tmpbuf, hdr->memop.data, hdr->memop.size);
 260                         memcpy_align(hdr->memop.addr, tmpbuf, hdr->memop.size);
 261                         hdr->reply.retcode = 0;
 262                         sendbuf(REPLY_SIZE, addr, port);
 263                         break;
 264                 case RPC_MEMSET:
 265                         memcpy(&val, hdr->memop.data, sizeof(u64));
 266                         memset_align(hdr->memop.addr, val, hdr->memop.size);
 267                         hdr->reply.retcode = 0;
 268                         sendbuf(REPLY_SIZE, addr, port);
 269                         break;
 270                 case RPC_HVCALL:
 271                         if (hdr->hvcall.numout > 7) {
 272                                 printf("netrpc: HVCALL numout > 7 (%d)\n", hdr->hvcall.numout);
 273                                 hdr->reply.retcode = -100;
 274                                 sendbuf(REPLY_SIZE, addr, port);
 275                                 break;
 276                         }
 277                         if (hdr->hvcall.numin > 8) {
 278                                 printf("netrpc: HVCALL numin > 8 (%d)\n", hdr->hvcall.numin);
 279                                 hdr->reply.retcode = -100;
 280                                 sendbuf(REPLY_SIZE, addr, port);
 281                                 break;
 282                         }
 283                         numout = hdr->hvcall.numout;
 284                         memcpy(regs, hdr->hvcall.regs, hdr->hvcall.numin*8);
 285                         regs[8] = hdr->hvcall.code;
 286                         hvcallv(regs);
 287                         hdr->reply.retcode = regs[0];
 288                         memcpy(hdr->reply.retdata, &regs[1], numout*8);
 289                         sendbuf(REPLY_SIZE+numout*8, addr, port);
 290                         break;
 291                 case RPC_ADDMMIO:
 292                         hdr->reply.retcode = mm_addmmio(hdr->addmmio.start, hdr->addmmio.size);
 293                         sendbuf(REPLY_SIZE, addr, port);
 294                         break;
 295                 case RPC_DELMMIO:
 296                         hdr->reply.retcode = mm_delmmio(hdr->addmmio.start);
 297                         sendbuf(REPLY_SIZE, addr, port);
 298                         break;
 299                 case RPC_CLRMMIO:
 300                         hdr->reply.retcode = mm_clrmmio();
 301                         sendbuf(REPLY_SIZE, addr, port);
 302                         break;
 303                 case RPC_VECTOR:
 304                         vector = hdr->vector;
 305                         printf("netrpc: vector called (%p,%p %p,%p,0x%x)...\n", vector.vec0, vector.vec1, vector.copy_dst, vector.copy_src, vector.copy_size);
 306                         hdr->reply.retcode = 0;
 307                         sendbuf(REPLY_SIZE, addr, port);
 308                         netrpc_vector(vector.vec0, vector.vec1, vector.copy_dst, vector.copy_src, vector.copy_size);
 309                         break;
 310                 case RPC_SYNC:
 311                         sync_before_exec(hdr->sync.addr, hdr->sync.size);
 312                         hdr->reply.retcode = 0;
 313                         sendbuf(REPLY_SIZE, addr, port);
 314                         break;
 315                 case RPC_CALL:
 316                         hdr->reply.retcode = hdr->call.addr(hdr->call.args[0], hdr->call.args[1],
 317                                                                                                 hdr->call.args[2], hdr->call.args[3],
 318                                                                                                 hdr->call.args[4], hdr->call.args[5],
 319                                                                                                 hdr->call.args[6], hdr->call.args[7]);
 320                         sendbuf(REPLY_SIZE, addr, port);
 321                         break;
 322                 default:
 323                         printf("netrpc: Unknown RPC command 0x%x\n", hdr->cmd);
 324                         hdr->reply.retcode = -1;
 325                         sendbuf(REPLY_SIZE, addr, port);
 326                         break;
 327         }
 328         pbuf_free(p);
 329 }
 330
 331 void netrpc_init(void)
 332 {
 333         pcb = udp_new();
 334         if (!pcb)
 335                 fatal("netrpc: Could not allocate PCB for netrpc");
 336         outbuf = pbuf_alloc(PBUF_TRANSPORT, BUFSIZE, PBUF_RAM);
 337         if (!outbuf)
 338                 fatal("netrpc: Could not allocate buffer");
 339
 340         udp_bind(pcb, IP_ADDR_ANY, PORT);
 341         udp_recv(pcb, netrpc_recv, NULL);
 342         printf("netrpc: initialized\n");
 343 }
 344 void netrpc_shutdown(void)
 345 {
 346         printf("netrpc: shutting down\n");
 347         if (pcb)
 348                 udp_remove(pcb);
 349         pcb = NULL;
 350         if (outbuf)
 351                 pbuf_free(outbuf);
 352         outbuf = NULL;
 353         printf("netrpc: shutdown complete\n");
 354 }
 355
 356 #endif