mirror of https://github.com/rusefi/ChibiOS.git
549 lines
17 KiB
C
549 lines
17 KiB
C
/*
|
|
ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio
|
|
|
|
Licensed under the Apache License, Version 2.0 (the "License");
|
|
you may not use this file except in compliance with the License.
|
|
You may obtain a copy of the License at
|
|
|
|
http://www.apache.org/licenses/LICENSE-2.0
|
|
|
|
Unless required by applicable law or agreed to in writing, software
|
|
distributed under the License is distributed on an "AS IS" BASIS,
|
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
See the License for the specific language governing permissions and
|
|
limitations under the License.
|
|
*/
|
|
|
|
/**
|
|
* @file tssockstub.c
|
|
* @brief Sockets stub for trusted services.
|
|
*
|
|
*/
|
|
|
|
#include "ch.h"
|
|
#include "chobjfifos.h"
|
|
#include "chtssi.h"
|
|
#include "tssockstub.h"
|
|
#include <string.h>
|
|
#include <ctype.h>
|
|
|
|
/*===========================================================================*/
|
|
/* Module local definitions. */
|
|
/*===========================================================================*/
|
|
|
|
#define METHOD_MAX_PARAMS 6
|
|
#define STUB_MAX_OPS 32
|
|
|
|
#define OP_PRMDIR_NONE 0
|
|
#define OP_PRMDIR_IN 1
|
|
#define OP_PRMDIR_OUT 2
|
|
|
|
/*===========================================================================*/
|
|
/* Module exported variables. */
|
|
/*===========================================================================*/
|
|
|
|
/*===========================================================================*/
|
|
/* Module local types. */
|
|
/*===========================================================================*/
|
|
typedef struct stub_op stub_op_t;
|
|
typedef enum {FREE=0, CALLING, PENDING} op_state_t;
|
|
|
|
typedef struct stub_param {
|
|
uint32_t dir;
|
|
uint32_t val;
|
|
uint32_t size;
|
|
} stub_parm_t;
|
|
|
|
typedef struct stub_op {
|
|
uint32_t op_code; /* e.g. connect, recv, sendv, close, etc.*/
|
|
op_state_t op_state; /* calling, pending, free.*/
|
|
stub_parm_t op_p[METHOD_MAX_PARAMS];
|
|
thread_reference_t op_wthdp; /* TS internal client thread (the caller).*/
|
|
} stub_op_t;
|
|
|
|
/*===========================================================================*/
|
|
/* Module local variables. */
|
|
/*===========================================================================*/
|
|
|
|
static objects_fifo_t ops_fifo;
|
|
static msg_t ops_msgs[STUB_MAX_OPS];
|
|
static struct stub_op ops[STUB_MAX_OPS] = {0};
|
|
static bool tsSkelIsReady = false;
|
|
|
|
/*===========================================================================*/
|
|
/* Module local functions. */
|
|
/*===========================================================================*/
|
|
|
|
static bool isOpValid(stub_op_t *op)
|
|
{
|
|
if ((op < &ops[0]) || (op >= &ops[STUB_MAX_OPS]))
|
|
return FALSE;
|
|
if (((char *)op - (char *)&ops[0]) % sizeof ops[0])
|
|
return FALSE;
|
|
return TRUE;
|
|
}
|
|
|
|
/**
|
|
* @brief Implement an a call to a NSEC function.
|
|
* @details It activates the channel between the stubs service and
|
|
* the skels daemon running in the nsec world.
|
|
* To do it, it uses an event to signal the skels
|
|
* daemon that a new op request is ready to be executed.
|
|
* Behind the scenes, the skels daemon will then gets the op, calling
|
|
* the stubs service via smc. The daemon executes it and then calls
|
|
* the stubs service again to post the result and to wake up the
|
|
* calling thread of this function.
|
|
*
|
|
* @param[in] op the 'remote' method description.
|
|
*
|
|
* @return the return value of 'remote' method.
|
|
*/
|
|
static uint32_t callRemote(stub_op_t *op) {
|
|
uint32_t r;
|
|
|
|
chSysLock();
|
|
chFifoSendObjectI(&ops_fifo, op);
|
|
chEvtBroadcastFlagsI(&tsEventSource, EVT_F_SOCK_NEW_OP);
|
|
chThdSuspendS(&op->op_wthdp);
|
|
chSysUnlock();
|
|
r = op->op_code;
|
|
chFifoReturnObject(&ops_fifo, op);
|
|
return r;
|
|
}
|
|
|
|
static stub_op_t *getNewOp(void) {
|
|
stub_op_t *op = chFifoTakeObjectTimeout(&ops_fifo, TIME_INFINITE);
|
|
memset(op, 0, sizeof *op);
|
|
op->op_state = CALLING;
|
|
return op;
|
|
}
|
|
|
|
/*===========================================================================*/
|
|
/* Module exported functions. */
|
|
/*===========================================================================*/
|
|
|
|
/**
|
|
* @brief The stubs service.
|
|
* @details And this is where the magic happens.
|
|
*/
|
|
THD_WORKING_AREA(waTsStubsService, 1024);
|
|
THD_FUNCTION(TsStubsService, tsstate) {
|
|
ts_state_t *svcp = tsstate;
|
|
skel_req_t *skrp;
|
|
stub_op_t *op;
|
|
msg_t r;
|
|
int i;
|
|
|
|
chFifoObjectInit(&ops_fifo, sizeof (stub_op_t), STUB_MAX_OPS,
|
|
sizeof (uint8_t), ops, ops_msgs);
|
|
for (;/* ever */;) {
|
|
|
|
/* Wait a service request.*/
|
|
(void)tssiWaitRequest(svcp);
|
|
skrp = (skel_req_t *)TS_GET_DATA(svcp);
|
|
r = SMC_SVC_OK;
|
|
|
|
/* Process the request.*/
|
|
if (TS_GET_DATALEN(svcp) != sizeof (skel_req_t)) {
|
|
TS_SET_STATUS(svcp, SMC_SVC_INVALID);
|
|
continue;
|
|
}
|
|
|
|
switch (skrp->req) {
|
|
case SKEL_REQ_READY:
|
|
tsSkelIsReady = true;
|
|
break;
|
|
|
|
case SKEL_REQ_GETOP:
|
|
|
|
/* The nsec skeleton calls us to get a new op ready to be executed.*/
|
|
if (chFifoReceiveObjectTimeout(&ops_fifo, (void **)&op, TIME_IMMEDIATE) ==
|
|
MSG_TIMEOUT) {
|
|
|
|
/* no op ready to be executed.*/
|
|
r = SMC_SVC_NHND;
|
|
break;
|
|
}
|
|
skrp->stub_op = (uint32_t)op;
|
|
skrp->stub_op_code = op->op_code;
|
|
|
|
/* Pass all the 'by value' arguments from stub to skel.*/
|
|
for (i = 0; i < METHOD_MAX_PARAMS; ++i) {
|
|
if (op->op_p[i].dir == OP_PRMDIR_NONE)
|
|
skrp->stub_op_p[i] = op->op_p[i].val;
|
|
}
|
|
op->op_state = PENDING;
|
|
break;
|
|
|
|
case SKEL_REQ_CPYPRMS:
|
|
|
|
/* The nsec skel calls us to get a copy of the 'in' parameters of
|
|
the specified op.
|
|
An 'in' parameter is an indirect argument, that is an argument
|
|
the value of which is a pointer to a memory buffer, that
|
|
must be copied in a non secure memory buffer.
|
|
It represents data to be consumed by the callee.*/
|
|
op = (stub_op_t *)skrp->stub_op;
|
|
if (!isOpValid(op) || op->op_state != PENDING ||
|
|
op->op_code != skrp->stub_op_code) {
|
|
r = SMC_SVC_INVALID;
|
|
break;
|
|
}
|
|
|
|
/* Copy all 'in' parameters.
|
|
For each parameter check that the destination memory area
|
|
is in the non secure memory arena.*/
|
|
for (i = 0; i < METHOD_MAX_PARAMS; ++i) {
|
|
if ((op->op_p[i].dir & OP_PRMDIR_IN) == 0)
|
|
continue;
|
|
if (!tsIsAddrSpaceValid((void *)skrp->stub_op_p[i], op->op_p[i].size)) {
|
|
r = SMC_SVC_INVALID;
|
|
break;
|
|
}
|
|
memcpy((void *)skrp->stub_op_p[i], (void *)op->op_p[i].val,
|
|
op->op_p[i].size);
|
|
}
|
|
break;
|
|
|
|
case SKEL_REQ_PUTRES:
|
|
|
|
/* The nsec skel calls us to put a copy of the 'out' parameters of
|
|
the specified op.
|
|
An 'out' parameter is an indirect argument, that is an argument
|
|
the value of which is a pointer to a memory buffer, that
|
|
must be copied in a secure memory buffer.
|
|
It represents data produced by the callee.*/
|
|
op = (stub_op_t *)skrp->stub_op;
|
|
if (!isOpValid(op) || op->op_state != PENDING ||
|
|
op->op_code != skrp->stub_op_code) {
|
|
r = SMC_SVC_INVALID;
|
|
break;
|
|
}
|
|
|
|
/* Copy all 'out' parameters.
|
|
For each parameter check that the source memory area
|
|
is in the non secure memory arena, and that the size returned
|
|
fits in the caller buffer size.*/
|
|
for (i = 0; i < METHOD_MAX_PARAMS; ++i) {
|
|
if ((op->op_p[i].dir & OP_PRMDIR_OUT) == 0)
|
|
continue;
|
|
if (!tsIsAddrSpaceValid((void *)skrp->stub_op_p[i], skrp->stub_op_p_sz[i])
|
|
|| (skrp->stub_op_p_sz[i] > op->op_p[i].size)) {
|
|
r = SMC_SVC_INVALID;
|
|
break;
|
|
}
|
|
memcpy((void *)op->op_p[i].val, (void *)skrp->stub_op_p[i],
|
|
skrp->stub_op_p_sz[i]);
|
|
}
|
|
if (r != SMC_SVC_OK)
|
|
break;
|
|
|
|
/* Set the return value of the 'remote' callee method,
|
|
and wake up the caller.*/
|
|
op->op_code = skrp->stub_op_result;
|
|
chThdResume(&op->op_wthdp, MSG_OK);
|
|
break;
|
|
|
|
default:
|
|
r = SMC_SVC_INVALID;
|
|
break;
|
|
}
|
|
|
|
/* Set the response.*/
|
|
TS_SET_STATUS(svcp, r);
|
|
}
|
|
}
|
|
/**
|
|
* @brief Is the skeletons daemon ready to operate?
|
|
* @details It is used at the startup to synchronize the
|
|
* stub service with the skeleton daemon.
|
|
*/
|
|
void tsWaitStubSkelReady(void) {
|
|
while (!tsSkelIsReady) {
|
|
chThdSleepMilliseconds(100);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief The sockets API.
|
|
*/
|
|
int socket(int domain, int type, int protocol) {
|
|
stub_op_t *op = getNewOp();
|
|
op->op_code = STUB_OP_SOCKET;
|
|
op->op_p[0].dir = OP_PRMDIR_NONE;
|
|
op->op_p[0].val = (uint32_t)domain;
|
|
op->op_p[1].dir = OP_PRMDIR_NONE;
|
|
op->op_p[1].val = (uint32_t)type;
|
|
op->op_p[2].dir = OP_PRMDIR_NONE;
|
|
op->op_p[2].val = (uint32_t)protocol;
|
|
return (int)callRemote(op);
|
|
}
|
|
|
|
int connect(int s, const struct sockaddr *name, socklen_t namelen) {
|
|
stub_op_t *op = getNewOp();
|
|
op->op_code = STUB_OP_CONNECT;
|
|
op->op_p[0].dir = OP_PRMDIR_NONE;
|
|
op->op_p[0].val = (uint32_t)s;
|
|
op->op_p[1].dir = OP_PRMDIR_IN;
|
|
op->op_p[1].val = (uint32_t)name;
|
|
op->op_p[1].size = (uint32_t)namelen;
|
|
op->op_p[2].dir = OP_PRMDIR_NONE;
|
|
op->op_p[2].val = (uint32_t)namelen;
|
|
return (int)callRemote(op);
|
|
}
|
|
|
|
int close(int s) {
|
|
stub_op_t *op = getNewOp();
|
|
op->op_code = STUB_OP_CLOSE;
|
|
op->op_p[0].dir = OP_PRMDIR_NONE;
|
|
op->op_p[0].val = (uint32_t)s;
|
|
return (int)callRemote(op);
|
|
}
|
|
|
|
int recv(int s, void *mem, size_t len, int flags) {
|
|
stub_op_t *op = getNewOp();
|
|
op->op_code = STUB_OP_RECV;
|
|
op->op_p[0].dir = OP_PRMDIR_NONE;
|
|
op->op_p[0].val = (uint32_t)s;
|
|
op->op_p[1].dir = OP_PRMDIR_OUT;
|
|
op->op_p[1].val = (uint32_t)mem;
|
|
op->op_p[1].size = (uint32_t)len;
|
|
op->op_p[2].dir = OP_PRMDIR_NONE;
|
|
op->op_p[2].val = (uint32_t)len;
|
|
op->op_p[3].dir = OP_PRMDIR_NONE;
|
|
op->op_p[3].val = (uint32_t)flags;
|
|
return (int)callRemote(op);
|
|
}
|
|
|
|
int send(int s, const void *dataptr, size_t size, int flags) {
|
|
stub_op_t *op = getNewOp();
|
|
op->op_code = STUB_OP_SEND;
|
|
op->op_p[0].dir = OP_PRMDIR_NONE;
|
|
op->op_p[0].val = (uint32_t)s;
|
|
op->op_p[1].dir = OP_PRMDIR_IN;
|
|
op->op_p[1].val = (uint32_t)dataptr;
|
|
op->op_p[1].size = (uint32_t)size;
|
|
op->op_p[2].dir = OP_PRMDIR_NONE;
|
|
op->op_p[2].val = (uint32_t)size;
|
|
op->op_p[3].dir = OP_PRMDIR_NONE;
|
|
op->op_p[3].val = (uint32_t)flags;
|
|
return (int)callRemote(op);
|
|
}
|
|
|
|
#if 0
|
|
int select(int maxfdp1, fd_set *readset, fd_set *writeset, fd_set *exceptset,
|
|
struct timeval *timeout) {
|
|
stub_op_t *op = getNewOp();
|
|
op->op_code = STUB_OP_SELECT;
|
|
op->op_p[0].dir = OP_PRMDIR_NONE;
|
|
op->op_p[0].val = (uint32_t)maxfdp1;
|
|
op->op_p[1].dir = OP_PRMDIR_IN|OP_PRMDIR_OUT;
|
|
op->op_p[1].val = (uint32_t)readset;
|
|
op->op_p[1].size = sizeof (fd_set);
|
|
op->op_p[2].dir = OP_PRMDIR_IN|OP_PRMDIR_OUT;
|
|
op->op_p[2].val = (uint32_t)writeset;
|
|
op->op_p[2].size = sizeof (fd_set);
|
|
op->op_p[3].dir = OP_PRMDIR_IN|OP_PRMDIR_OUT;
|
|
op->op_p[3].val = (uint32_t)exceptset;
|
|
op->op_p[3].size = sizeof (fd_set);
|
|
op->op_p[4].dir = OP_PRMDIR_IN;
|
|
op->op_p[4].val = (uint32_t)timeout;
|
|
op->op_p[4].size = sizeof (struct timeval);
|
|
return (int)callRemote(op);
|
|
}
|
|
#endif
|
|
|
|
int bind(int s, const struct sockaddr *name, socklen_t namelen) {
|
|
stub_op_t *op = getNewOp();
|
|
op->op_code = STUB_OP_BIND;
|
|
op->op_p[0].dir = OP_PRMDIR_NONE;
|
|
op->op_p[0].val = (uint32_t)s;
|
|
op->op_p[1].dir = OP_PRMDIR_IN;
|
|
op->op_p[1].val = (uint32_t)name;
|
|
op->op_p[1].size = (uint32_t)namelen;
|
|
op->op_p[2].dir = OP_PRMDIR_NONE;
|
|
op->op_p[2].val = (uint32_t)namelen;
|
|
return (int)callRemote(op);
|
|
}
|
|
|
|
int listen(int s, int backlog) {
|
|
stub_op_t *op = getNewOp();
|
|
op->op_code = STUB_OP_LISTEN;
|
|
op->op_p[0].dir = OP_PRMDIR_NONE;
|
|
op->op_p[0].val = (uint32_t)s;
|
|
op->op_p[1].dir = OP_PRMDIR_NONE;
|
|
op->op_p[1].val = (uint32_t)backlog;
|
|
return (int)callRemote(op);
|
|
}
|
|
|
|
int write(int s, const void *dataptr, size_t size) {
|
|
stub_op_t *op = getNewOp();
|
|
op->op_code = STUB_OP_WRITE;
|
|
op->op_p[0].dir = OP_PRMDIR_NONE;
|
|
op->op_p[0].val = (uint32_t)s;
|
|
op->op_p[1].dir = OP_PRMDIR_IN;
|
|
op->op_p[1].val = (uint32_t)dataptr;
|
|
op->op_p[2].dir = OP_PRMDIR_NONE;
|
|
op->op_p[2].val = (uint32_t)size;
|
|
return (int)callRemote(op);
|
|
}
|
|
|
|
int read(int s, void *mem, size_t len) {
|
|
stub_op_t *op = getNewOp();
|
|
op->op_code = STUB_OP_READ;
|
|
op->op_p[0].dir = OP_PRMDIR_NONE;
|
|
op->op_p[0].val = (uint32_t)s;
|
|
op->op_p[1].dir = OP_PRMDIR_OUT;
|
|
op->op_p[1].val = (uint32_t)mem;
|
|
op->op_p[2].dir = OP_PRMDIR_NONE;
|
|
op->op_p[2].val = (uint32_t)len;
|
|
return (int)callRemote(op);
|
|
}
|
|
|
|
/*
|
|
* TODO verify parameters
|
|
*/
|
|
int getaddrinfo(const char *nodename, const char *servname,
|
|
const struct addrinfo *hints, struct addrinfo **res) {
|
|
stub_op_t *op = getNewOp();
|
|
op->op_code = STUB_OP_GETADDRINFO;
|
|
op->op_p[0].dir = OP_PRMDIR_IN;
|
|
op->op_p[0].val = (uint32_t)nodename;
|
|
op->op_p[1].dir = OP_PRMDIR_IN;
|
|
op->op_p[1].val = (uint32_t)servname;
|
|
op->op_p[2].dir = OP_PRMDIR_IN;
|
|
op->op_p[2].val = (uint32_t)hints;
|
|
op->op_p[3].dir = OP_PRMDIR_OUT;
|
|
op->op_p[3].val = (uint32_t)res;
|
|
return (int)callRemote(op);
|
|
}
|
|
|
|
int freeaddrinfo(struct addrinfo *ai) {
|
|
stub_op_t *op = getNewOp();
|
|
op->op_code = STUB_OP_FREEADDRINFO;
|
|
op->op_p[0].dir = OP_PRMDIR_IN;
|
|
op->op_p[0].val = (uint32_t)ai;
|
|
return (int)callRemote(op);
|
|
}
|
|
|
|
|
|
#if 0
|
|
int accept(int s, struct sockaddr *addr, socklen_t *addrlen);
|
|
int shutdown(int s, int how);
|
|
int getpeername (int s, struct sockaddr *name, socklen_t *namelen);
|
|
int getsockname (int s, struct sockaddr *name, socklen_t *namelen);
|
|
int getsockopt (int s, int level, int optname, void *optval, socklen_t *optlen);
|
|
int setsockopt (int s, int level, int optname, const void *optval, socklen_t optlen);
|
|
int recvfrom(int s, void *mem, size_t len, int flags,
|
|
struct sockaddr *from, socklen_t *fromlen);
|
|
int sendmsg(int s, const struct msghdr *message, int flags);
|
|
int sendto(int s, const void *dataptr, size_t size, int flags,
|
|
const struct sockaddr *to, socklen_t tolen);
|
|
int writev(int s, const struct iovec *iov, int iovcnt);
|
|
int ioctl(int s, long cmd, void *argp);
|
|
int fcntl(int s, int cmd, int val);
|
|
#endif
|
|
|
|
/*
|
|
* Ascii internet address interpretation routine.
|
|
* The value returned is in network order.
|
|
*/
|
|
in_addr_t inet_addr(const char *cp) {
|
|
struct in_addr val;
|
|
|
|
if (inet_aton(cp, &val))
|
|
return val.s_addr;
|
|
return INADDR_NONE;
|
|
}
|
|
|
|
/*
|
|
* Check whether "cp" is a valid ascii representation
|
|
* of an Internet address and convert to a binary address.
|
|
* Returns 1 if the address is valid, 0 if not.
|
|
* This replaces inet_addr, the return value from which
|
|
* cannot distinguish between failure and a local broadcast address.
|
|
*/
|
|
|
|
int inet_aton(const char *cp, struct in_addr *addr) {
|
|
uint32_t val, base, n;
|
|
char c;
|
|
uint32_t parts[4], *pp = parts;
|
|
|
|
for (;;) {
|
|
/*
|
|
* Collect number up to '.'.
|
|
* Values are specified as for C:
|
|
* 0x=hex, 0=octal, other=decimal.
|
|
*/
|
|
val = 0; base = 10;
|
|
if (*cp == '0') {
|
|
if (*++cp == 'x' || *cp == 'X')
|
|
base = 16, cp++;
|
|
else
|
|
base = 8;
|
|
}
|
|
while ((c = *cp) != '\0') {
|
|
if (isascii(c) && isdigit(c)) {
|
|
val = (val * base) + (c - '0');
|
|
cp++;
|
|
continue;
|
|
}
|
|
if (base == 16 && isascii(c) && isxdigit(c)) {
|
|
val = (val << 4) +
|
|
(c + 10 - (islower(c) ? 'a' : 'A'));
|
|
cp++;
|
|
continue;
|
|
}
|
|
break;
|
|
}
|
|
if (*cp == '.') {
|
|
/*
|
|
* Internet format:
|
|
* a.b.c.d
|
|
* a.b.c (with c treated as 16-bits)
|
|
* a.b (with b treated as 24 bits)
|
|
*/
|
|
if (pp >= parts + 3 || val > 0xff)
|
|
return 0;
|
|
*pp++ = val, cp++;
|
|
} else
|
|
break;
|
|
}
|
|
/*
|
|
* Check for trailing characters.
|
|
*/
|
|
if (*cp && (!isascii(*cp) || !isspace(*cp)))
|
|
return 0;
|
|
/*
|
|
* Make the address according to
|
|
* the number of parts specified.
|
|
*/
|
|
n = pp - parts + 1;
|
|
switch (n) {
|
|
|
|
case 1: /* a -- 32 bits */
|
|
break;
|
|
|
|
case 2: /* a.b -- 8.24 bits */
|
|
if (val > 0xffffff)
|
|
return 0;
|
|
val |= parts[0] << 24;
|
|
break;
|
|
|
|
case 3: /* a.b.c -- 8.8.16 bits */
|
|
if (val > 0xffff)
|
|
return 0;
|
|
val |= (parts[0] << 24) | (parts[1] << 16);
|
|
break;
|
|
|
|
case 4: /* a.b.c.d -- 8.8.8.8 bits */
|
|
if (val > 0xff)
|
|
return 0;
|
|
val |= (parts[0] << 24) | (parts[1] << 16) | (parts[2] << 8);
|
|
break;
|
|
}
|
|
if (addr)
|
|
addr->s_addr = htonl(val);
|
|
return 1;
|
|
}
|