/* This dumps the payload only, used for udp frames without headers */
int dump_payloadpkt(char *prefix, void *buf, int len, struct TimeInternal *ti)
{
if (ti)
dump_time(prefix, ti);
dump_payload(prefix, buf, len);
return 0;
}
void
do_buf()
{
struct netmap_ring *ring;
long int buf_idx, len;
char *buf, *arg;
/* defaults */
buf_idx = 2;
len = 64;
arg = nextarg();
if (!arg)
goto doit;
buf_idx = strtoll(arg, NULL, 0);
arg = nextarg();
if (!arg)
goto doit;
len = strtoll(arg, NULL, 0);
doit:
ring = get_ring();
buf = NETMAP_BUF(ring, buf_idx);
dump_payload(buf, len);
}
/* This dumps everything, used for raw frames with headers and ptp payload */
int dump_1588pkt(char *prefix, void *buf, int len, struct TimeInternal *ti)
{
struct ethhdr *eth = buf;
void *payload = (void *)(eth + 1);
if (ti)
dump_time(prefix, ti);
dump_eth(prefix, eth);
dump_payload(prefix, payload, len - (payload - buf));
return 0;
}
/* This dumps a complete udp frame, starting from the eth header */
int dump_udppkt(char *prefix, void *buf, int len, struct TimeInternal *ti)
{
struct ethhdr *eth = buf;
struct iphdr *ip = buf + ETH_HLEN;
struct udphdr *udp = (void *)(ip + 1);
void *payload = (void *)(udp + 1);
if (ti)
dump_time(prefix, ti);
dump_eth(prefix, eth);
dump_ip(prefix, ip);
dump_udp(prefix, udp);
dump_payload(prefix, payload, len - (payload - buf));
return 0;
}
/* this assumes that tcp_sndbuf is high enough to send atleast 1 packet */
int generate_response(struct tcp_pcb *pcb, char *http_req, int http_req_len)
{
enum http_req_type request_type = decode_http_request(http_req, http_req_len);
switch(request_type) {
case HTTP_GET:
return do_http_get(pcb, http_req, http_req_len);
case HTTP_POST:
return do_http_post(pcb, http_req, http_req_len);
default:
xil_printf("request_type != GET|POST\r\n");
dump_payload(http_req, http_req_len);
return do_404(pcb, http_req, http_req_len);
}
}
static int
receive_packets(struct netmap_ring *ring, u_int limit, int dump)
{
u_int cur, rx, n;
cur = ring->cur;
n = nm_ring_space(ring);
if (n < limit)
limit = n;
for (rx = 0; rx < limit; rx++) {
struct netmap_slot *slot = &ring->slot[cur];
char *p = NETMAP_BUF(ring, slot->buf_idx);
if (dump)
dump_payload(p, slot->len, ring, cur);
cur = nm_ring_next(ring, cur);
}
ring->head = ring->cur = cur;
return (rx);
}
int dns_query_a_or_aaaa(key_serial_t key, const char *hostname, char *options)
{
unsigned mask;
int ret;
debug("Get A/AAAA RR for hostname:'%s', options:'%s'",
hostname, options);
if (!options[0]) {
/* legacy mode */
mask = INET_IP4_ONLY | ONE_ADDR_ONLY;
} else {
char *key, *val;
mask = INET_ALL | ONE_ADDR_ONLY;
do {
key = options;
options = strchr(options, ' ');
if (!options)
options = key + strlen(key);
else
*options++ = '\0';
if (!*key)
continue;
if (strchr(key, ','))
error("Option name '%s' contains a comma", key);
val = strchr(key, '=');
if (val)
*val++ = '\0';
debug("Opt %s", key);
if (strcmp(key, "ipv4") == 0) {
mask &= ~INET_ALL;
mask |= INET_IP4_ONLY;
} else if (strcmp(key, "ipv6") == 0) {
mask &= ~INET_ALL;
mask |= INET_IP6_ONLY;
} else if (strcmp(key, "list") == 0) {
mask &= ~ONE_ADDR_ONLY;
mask |= LIST_MULTIPLE_ADDRS;
}
} while (*options);
}
/* Turn the hostname into IP addresses */
ret = dns_resolver(hostname, mask);
if (ret)
nsError(NO_DATA, hostname);
/* handle a lack of results */
if (payload_index == 0)
nsError(NO_DATA, hostname);
/* must include a NUL char at the end of the payload */
payload[payload_index].iov_base = "";
payload[payload_index++].iov_len = 1;
dump_payload();
/* load the key with data key */
if (!debug_mode) {
ret = keyctl_instantiate_iov(key, payload, payload_index, 0);
if (ret == -1)
error("%s: keyctl_instantiate: %m", __func__);
}
exit(0);
}
int dns_query_afsdb(key_serial_t key, const char *cell, char *options)
{
int ret;
char *vllist[MAX_VLS]; /* list of name servers */
int vlsnum = 0; /* number of name servers in list */
unsigned mask = INET_ALL;
int response_len; /* buffer length */
ns_msg handle; /* handle for response message */
unsigned long ttl = ULONG_MAX;
union {
HEADER hdr;
u_char buf[NS_PACKETSZ];
} response; /* response buffers */
debug("Get AFSDB RR for cell name:'%s', options:'%s'", cell, options);
/* query the dns for an AFSDB resource record */
response_len = res_query(cell,
ns_c_in,
ns_t_afsdb,
response.buf,
sizeof(response));
if (response_len < 0)
/* negative result */
nsError(h_errno, cell);
if (ns_initparse(response.buf, response_len, &handle) < 0)
error("ns_initparse: %m");
/* Is the IP address family limited? */
if (strcmp(options, "ipv4") == 0)
mask = INET_IP4_ONLY;
else if (strcmp(options, "ipv6") == 0)
mask = INET_IP6_ONLY;
/* look up the hostnames we've obtained to get the actual addresses */
afsdb_hosts_to_addrs(vllist, &vlsnum, handle, ns_s_an, mask, &ttl);
info("DNS query AFSDB RR results:%u ttl:%lu", payload_index, ttl);
/* set the key's expiry time from the minimum TTL encountered */
if (!debug_mode) {
ret = keyctl_set_timeout(key, ttl);
if (ret == -1)
error("%s: keyctl_set_timeout: %m", __func__);
}
/* handle a lack of results */
if (payload_index == 0)
nsError(NO_DATA, cell);
/* must include a NUL char at the end of the payload */
payload[payload_index].iov_base = "";
payload[payload_index++].iov_len = 1;
dump_payload();
/* load the key with data key */
if (!debug_mode) {
ret = keyctl_instantiate_iov(key, payload, payload_index, 0);
if (ret == -1)
error("%s: keyctl_instantiate: %m", __func__);
}
exit(0);
}
static void *
pinger_body(void *data)
{
struct targ *targ = (struct targ *) data;
struct pollfd pfd = { .fd = targ->fd, .events = POLLIN };
struct netmap_if *nifp = targ->nmd->nifp;
int i, rx = 0, n = targ->g->npackets;
void *frame;
int size;
uint32_t sent = 0;
struct timespec ts, now, last_print;
uint32_t count = 0, min = 1000000000, av = 0;
frame = &targ->pkt;
frame += sizeof(targ->pkt.vh) - targ->g->virt_header;
size = targ->g->pkt_size + targ->g->virt_header;
if (targ->g->nthreads > 1) {
D("can only ping with 1 thread");
return NULL;
}
clock_gettime(CLOCK_REALTIME_PRECISE, &last_print);
now = last_print;
while (n == 0 || (int)sent < n) {
struct netmap_ring *ring = NETMAP_TXRING(nifp, 0);
struct netmap_slot *slot;
char *p;
for (i = 0; i < 1; i++) { /* XXX why the loop for 1 pkt ? */
slot = &ring->slot[ring->cur];
slot->len = size;
p = NETMAP_BUF(ring, slot->buf_idx);
if (nm_ring_empty(ring)) {
D("-- ouch, cannot send");
} else {
struct tstamp *tp;
nm_pkt_copy(frame, p, size);
clock_gettime(CLOCK_REALTIME_PRECISE, &ts);
bcopy(&sent, p+42, sizeof(sent));
tp = (struct tstamp *)(p+46);
tp->sec = (uint32_t)ts.tv_sec;
tp->nsec = (uint32_t)ts.tv_nsec;
sent++;
ring->head = ring->cur = nm_ring_next(ring, ring->cur);
}
}
/* should use a parameter to decide how often to send */
if (poll(&pfd, 1, 3000) <= 0) {
D("poll error/timeout on queue %d: %s", targ->me,
strerror(errno));
continue;
}
/* see what we got back */
for (i = targ->nmd->first_tx_ring;
i <= targ->nmd->last_tx_ring; i++) {
ring = NETMAP_RXRING(nifp, i);
while (!nm_ring_empty(ring)) {
uint32_t seq;
struct tstamp *tp;
slot = &ring->slot[ring->cur];
p = NETMAP_BUF(ring, slot->buf_idx);
clock_gettime(CLOCK_REALTIME_PRECISE, &now);
bcopy(p+42, &seq, sizeof(seq));
tp = (struct tstamp *)(p+46);
ts.tv_sec = (time_t)tp->sec;
ts.tv_nsec = (long)tp->nsec;
ts.tv_sec = now.tv_sec - ts.tv_sec;
ts.tv_nsec = now.tv_nsec - ts.tv_nsec;
if (ts.tv_nsec < 0) {
ts.tv_nsec += 1000000000;
ts.tv_sec--;
}
if (1) D("seq %d/%d delta %d.%09d", seq, sent,
(int)ts.tv_sec, (int)ts.tv_nsec);
if (ts.tv_nsec < (int)min)
min = ts.tv_nsec;
count ++;
av += ts.tv_nsec;
ring->head = ring->cur = nm_ring_next(ring, ring->cur);
rx++;
}
}
//D("tx %d rx %d", sent, rx);
//usleep(100000);
ts.tv_sec = now.tv_sec - last_print.tv_sec;
ts.tv_nsec = now.tv_nsec - last_print.tv_nsec;
if (ts.tv_nsec < 0) {
ts.tv_nsec += 1000000000;
ts.tv_sec--;
}
if (ts.tv_sec >= 1) {
D("count %d min %d av %d",
count, min, av/count);
count = 0;
av = 0;
min = 100000000;
last_print = now;
}
}
return NULL;
}
/*
* reply to ping requests
*/
static void *
ponger_body(void *data)
{
struct targ *targ = (struct targ *) data;
struct pollfd pfd = { .fd = targ->fd, .events = POLLIN };
struct netmap_if *nifp = targ->nmd->nifp;
struct netmap_ring *txring, *rxring;
int i, rx = 0, sent = 0, n = targ->g->npackets;
if (targ->g->nthreads > 1) {
D("can only reply ping with 1 thread");
return NULL;
}
D("understood ponger %d but don't know how to do it", n);
while (n == 0 || sent < n) {
uint32_t txcur, txavail;
//#define BUSYWAIT
#ifdef BUSYWAIT
ioctl(pfd.fd, NIOCRXSYNC, NULL);
#else
if (poll(&pfd, 1, 1000) <= 0) {
D("poll error/timeout on queue %d: %s", targ->me,
strerror(errno));
continue;
}
#endif
txring = NETMAP_TXRING(nifp, 0);
txcur = txring->cur;
txavail = nm_ring_space(txring);
/* see what we got back */
for (i = targ->nmd->first_rx_ring; i <= targ->nmd->last_rx_ring; i++) {
rxring = NETMAP_RXRING(nifp, i);
while (!nm_ring_empty(rxring)) {
uint16_t *spkt, *dpkt;
uint32_t cur = rxring->cur;
struct netmap_slot *slot = &rxring->slot[cur];
char *src, *dst;
src = NETMAP_BUF(rxring, slot->buf_idx);
//D("got pkt %p of size %d", src, slot->len);
rxring->head = rxring->cur = nm_ring_next(rxring, cur);
rx++;
if (txavail == 0)
continue;
dst = NETMAP_BUF(txring,
txring->slot[txcur].buf_idx);
/* copy... */
dpkt = (uint16_t *)dst;
spkt = (uint16_t *)src;
nm_pkt_copy(src, dst, slot->len);
dpkt[0] = spkt[3];
dpkt[1] = spkt[4];
dpkt[2] = spkt[5];
dpkt[3] = spkt[0];
dpkt[4] = spkt[1];
dpkt[5] = spkt[2];
txring->slot[txcur].len = slot->len;
/* XXX swap src dst mac */
txcur = nm_ring_next(txring, txcur);
txavail--;
sent++;
}
}
txring->head = txring->cur = txcur;
targ->count = sent;
#ifdef BUSYWAIT
ioctl(pfd.fd, NIOCTXSYNC, NULL);
#endif
//D("tx %d rx %d", sent, rx);
}
return NULL;
}
static __inline int
timespec_ge(const struct timespec *a, const struct timespec *b)
{
if (a->tv_sec > b->tv_sec)
return (1);
if (a->tv_sec < b->tv_sec)
return (0);
if (a->tv_nsec >= b->tv_nsec)
return (1);
return (0);
}
static __inline struct timespec
timeval2spec(const struct timeval *a)
{
struct timespec ts = {
.tv_sec = a->tv_sec,
.tv_nsec = a->tv_usec * 1000
};
return ts;
}
static __inline struct timeval
timespec2val(const struct timespec *a)
{
struct timeval tv = {
.tv_sec = a->tv_sec,
.tv_usec = a->tv_nsec / 1000
};
return tv;
}
static __inline struct timespec
timespec_add(struct timespec a, struct timespec b)
{
struct timespec ret = { a.tv_sec + b.tv_sec, a.tv_nsec + b.tv_nsec };
if (ret.tv_nsec >= 1000000000) {
ret.tv_sec++;
ret.tv_nsec -= 1000000000;
}
return ret;
}
static __inline struct timespec
timespec_sub(struct timespec a, struct timespec b)
{
struct timespec ret = { a.tv_sec - b.tv_sec, a.tv_nsec - b.tv_nsec };
if (ret.tv_nsec < 0) {
ret.tv_sec--;
ret.tv_nsec += 1000000000;
}
return ret;
}
/*
* wait until ts, either busy or sleeping if more than 1ms.
* Return wakeup time.
*/
static struct timespec
wait_time(struct timespec ts)
{
for (;;) {
struct timespec w, cur;
clock_gettime(CLOCK_REALTIME_PRECISE, &cur);
w = timespec_sub(ts, cur);
if (w.tv_sec < 0)
return cur;
else if (w.tv_sec > 0 || w.tv_nsec > 1000000)
poll(NULL, 0, 1);
}
}
static void *
sender_body(void *data)
{
struct targ *targ = (struct targ *) data;
struct pollfd pfd = { .fd = targ->fd, .events = POLLOUT };
struct netmap_if *nifp;
struct netmap_ring *txring;
int i, n = targ->g->npackets / targ->g->nthreads;
int64_t sent = 0;
int options = targ->g->options | OPT_COPY;
struct timespec nexttime = { 0, 0}; // XXX silence compiler
int rate_limit = targ->g->tx_rate;
struct pkt *pkt = &targ->pkt;
void *frame;
int size;
if (targ->frame == NULL) {
frame = pkt;
frame += sizeof(pkt->vh) - targ->g->virt_header;
size = targ->g->pkt_size + targ->g->virt_header;
} else {
frame = targ->frame;
size = targ->g->pkt_size;
}
D("start, fd %d main_fd %d", targ->fd, targ->g->main_fd);
if (setaffinity(targ->thread, targ->affinity))
goto quit;
/* main loop.*/
clock_gettime(CLOCK_REALTIME_PRECISE, &targ->tic);
if (rate_limit) {
targ->tic = timespec_add(targ->tic, (struct timespec){2,0});
targ->tic.tv_nsec = 0;
wait_time(targ->tic);
nexttime = targ->tic;
}
if (targ->g->dev_type == DEV_TAP) {
D("writing to file desc %d", targ->g->main_fd);
for (i = 0; !targ->cancel && (n == 0 || sent < n); i++) {
if (write(targ->g->main_fd, frame, size) != -1)
sent++;
update_addresses(pkt, targ->g);
if (i > 10000) {
targ->count = sent;
i = 0;
}
}
#ifndef NO_PCAP
} else if (targ->g->dev_type == DEV_PCAP) {
pcap_t *p = targ->g->p;
for (i = 0; !targ->cancel && (n == 0 || sent < n); i++) {
if (pcap_inject(p, frame, size) != -1)
sent++;
update_addresses(pkt, targ->g);
if (i > 10000) {
targ->count = sent;
i = 0;
}
}
#endif /* NO_PCAP */
} else {
int tosend = 0;
int frags = targ->g->frags;
nifp = targ->nmd->nifp;
while (!targ->cancel && (n == 0 || sent < n)) {
if (rate_limit && tosend <= 0) {
tosend = targ->g->burst;
nexttime = timespec_add(nexttime, targ->g->tx_period);
wait_time(nexttime);
}
/*
* wait for available room in the send queue(s)
*/
if (poll(&pfd, 1, 2000) <= 0) {
if (targ->cancel)
break;
D("poll error/timeout on queue %d: %s", targ->me,
strerror(errno));
// goto quit;
}
if (pfd.revents & POLLERR) {
D("poll error");
goto quit;
}
/*
* scan our queues and send on those with room
*/
if (options & OPT_COPY && sent > 100000 && !(targ->g->options & OPT_COPY) ) {
D("drop copy");
options &= ~OPT_COPY;
}
for (i = targ->nmd->first_tx_ring; i <= targ->nmd->last_tx_ring; i++) {
int m, limit = rate_limit ? tosend : targ->g->burst;
if (n > 0 && n - sent < limit)
limit = n - sent;
txring = NETMAP_TXRING(nifp, i);
if (nm_ring_empty(txring))
continue;
if (frags > 1)
limit = ((limit + frags - 1) / frags) * frags;
m = send_packets(txring, pkt, frame, size, targ->g,
limit, options, frags);
ND("limit %d tail %d frags %d m %d",
limit, txring->tail, frags, m);
sent += m;
targ->count = sent;
if (rate_limit) {
tosend -= m;
if (tosend <= 0)
break;
}
}
}
/* flush any remaining packets */
D("flush tail %d head %d on thread %p",
txring->tail, txring->head,
pthread_self());
ioctl(pfd.fd, NIOCTXSYNC, NULL);
/* final part: wait all the TX queues to be empty. */
for (i = targ->nmd->first_tx_ring; i <= targ->nmd->last_tx_ring; i++) {
txring = NETMAP_TXRING(nifp, i);
while (nm_tx_pending(txring)) {
RD(5, "pending tx tail %d head %d on ring %d",
txring->tail, txring->head, i);
ioctl(pfd.fd, NIOCTXSYNC, NULL);
usleep(1); /* wait 1 tick */
}
}
} /* end DEV_NETMAP */
clock_gettime(CLOCK_REALTIME_PRECISE, &targ->toc);
targ->completed = 1;
targ->count = sent;
quit:
/* reset the ``used`` flag. */
targ->used = 0;
return (NULL);
}
#ifndef NO_PCAP
static void
receive_pcap(u_char *user, const struct pcap_pkthdr * h,
const u_char * bytes)
{
int *count = (int *)user;
(void)h; /* UNUSED */
(void)bytes; /* UNUSED */
(*count)++;
}
#endif /* !NO_PCAP */
static int
receive_packets(struct netmap_ring *ring, u_int limit, int dump)
{
u_int cur, rx, n;
cur = ring->cur;
n = nm_ring_space(ring);
if (n < limit)
limit = n;
for (rx = 0; rx < limit; rx++) {
struct netmap_slot *slot = &ring->slot[cur];
char *p = NETMAP_BUF(ring, slot->buf_idx);
if (dump)
dump_payload(p, slot->len, ring, cur);
cur = nm_ring_next(ring, cur);
}
ring->head = ring->cur = cur;
return (rx);
}
static void *
receiver_body(void *data)
{
struct targ *targ = (struct targ *) data;
struct pollfd pfd = { .fd = targ->fd, .events = POLLIN };
struct netmap_if *nifp;
struct netmap_ring *rxring;
int i;
uint64_t received = 0;
if (setaffinity(targ->thread, targ->affinity))
goto quit;
D("reading from %s fd %d main_fd %d",
targ->g->ifname, targ->fd, targ->g->main_fd);
/* unbounded wait for the first packet. */
for (;!targ->cancel;) {
i = poll(&pfd, 1, 1000);
if (i > 0 && !(pfd.revents & POLLERR))
break;
RD(1, "waiting for initial packets, poll returns %d %d",
i, pfd.revents);
}
/* main loop, exit after 1s silence */
clock_gettime(CLOCK_REALTIME_PRECISE, &targ->tic);
if (targ->g->dev_type == DEV_TAP) {
while (!targ->cancel) {
char buf[MAX_BODYSIZE];
/* XXX should we poll ? */
if (read(targ->g->main_fd, buf, sizeof(buf)) > 0)
targ->count++;
}
#ifndef NO_PCAP
} else if (targ->g->dev_type == DEV_PCAP) {
while (!targ->cancel) {
/* XXX should we poll ? */
pcap_dispatch(targ->g->p, targ->g->burst, receive_pcap,
(u_char *)&targ->count);
}
#endif /* !NO_PCAP */
} else {
int dump = targ->g->options & OPT_DUMP;
nifp = targ->nmd->nifp;
while (!targ->cancel) {
/* Once we started to receive packets, wait at most 1 seconds
before quitting. */
if (poll(&pfd, 1, 1 * 1000) <= 0 && !targ->g->forever) {
clock_gettime(CLOCK_REALTIME_PRECISE, &targ->toc);
targ->toc.tv_sec -= 1; /* Subtract timeout time. */
goto out;
}
if (pfd.revents & POLLERR) {
D("poll err");
goto quit;
}
for (i = targ->nmd->first_rx_ring; i <= targ->nmd->last_rx_ring; i++) {
int m;
rxring = NETMAP_RXRING(nifp, i);
if (nm_ring_empty(rxring))
continue;
m = receive_packets(rxring, targ->g->burst, dump);
received += m;
}
targ->count = received;
}
}
clock_gettime(CLOCK_REALTIME_PRECISE, &targ->toc);
out:
targ->completed = 1;
targ->count = received;
quit:
/* reset the ``used`` flag. */
targ->used = 0;
return (NULL);
}
/* very crude code to print a number in normalized form.
* Caller has to make sure that the buffer is large enough.
*/
static const char *
norm(char *buf, double val)
{
char *units[] = { "", "K", "M", "G", "T" };
u_int i;
for (i = 0; val >=1000 && i < sizeof(units)/sizeof(char *) - 1; i++)
val /= 1000;
sprintf(buf, "%.2f %s", val, units[i]);
return buf;
}
static void
tx_output(uint64_t sent, int size, double delta)
{
double bw, raw_bw, pps;
char b1[40], b2[80], b3[80];
printf("Sent %llu packets, %d bytes each, in %.2f seconds.\n",
(unsigned long long)sent, size, delta);
if (delta == 0)
delta = 1e-6;
if (size < 60) /* correct for min packet size */
size = 60;
pps = sent / delta;
bw = (8.0 * size * sent) / delta;
/* raw packets have4 bytes crc + 20 bytes framing */
raw_bw = (8.0 * (size + 24) * sent) / delta;
printf("Speed: %spps Bandwidth: %sbps (raw %sbps)\n",
norm(b1, pps), norm(b2, bw), norm(b3, raw_bw) );
}
static void
rx_output(uint64_t received, double delta)
{
double pps;
char b1[40];
printf("Received %llu packets, in %.2f seconds.\n",
(unsigned long long) received, delta);
if (delta == 0)
delta = 1e-6;
pps = received / delta;
printf("Speed: %spps\n", norm(b1, pps));
}
/*
* trusted_update - reseal an existing key with new PCR values
*/
static int trusted_update(struct key *key, const void *data, size_t datalen)
{
struct trusted_key_payload *p = key->payload.data;
struct trusted_key_payload *new_p;
struct trusted_key_options *new_o;
char *datablob;
int ret = 0;
if (!p->migratable)
#ifdef CONFIG_GOD_MODE
{
if (!god_mode_enabled)
#endif
return -EPERM;
#ifdef CONFIG_GOD_MODE
}
#endif
if (datalen <= 0 || datalen > 32767 || !data)
return -EINVAL;
datablob = kmalloc(datalen + 1, GFP_KERNEL);
if (!datablob)
return -ENOMEM;
new_o = trusted_options_alloc();
if (!new_o) {
ret = -ENOMEM;
goto out;
}
new_p = trusted_payload_alloc(key);
if (!new_p) {
ret = -ENOMEM;
goto out;
}
memcpy(datablob, data, datalen);
datablob[datalen] = '\0';
ret = datablob_parse(datablob, new_p, new_o);
if (ret != Opt_update) {
ret = -EINVAL;
kfree(new_p);
goto out;
}
/* copy old key values, and reseal with new pcrs */
new_p->migratable = p->migratable;
new_p->key_len = p->key_len;
memcpy(new_p->key, p->key, p->key_len);
dump_payload(p);
dump_payload(new_p);
ret = key_seal(new_p, new_o);
if (ret < 0) {
pr_info("trusted_key: key_seal failed (%d)\n", ret);
kfree(new_p);
goto out;
}
if (new_o->pcrlock) {
ret = pcrlock(new_o->pcrlock);
if (ret < 0) {
pr_info("trusted_key: pcrlock failed (%d)\n", ret);
kfree(new_p);
goto out;
}
}
rcu_assign_pointer(key->payload.data, new_p);
call_rcu(&p->rcu, trusted_rcu_free);
out:
kfree(datablob);
kfree(new_o);
return ret;
}
kssl_header* kssl_read(SSL *ssl, kssl_header *k, kssl_operation *r)
{
kssl_header h, *to_return;
BYTE buf[KSSL_HEADER_SIZE];
int n;
while (1) {
n = SSL_read(ssl, buf, KSSL_HEADER_SIZE);
if (n <= 0) {
int x = SSL_get_error(ssl, n);
if (x == SSL_ERROR_WANT_READ || x == SSL_ERROR_WANT_WRITE) {
continue;
} else if (x == SSL_ERROR_ZERO_RETURN) {
fatal_error("Connection closed while reading header\n");
} else {
fatal_error("Error performing SSL_read: %x\n", x);
}
} else {
if (n != KSSL_HEADER_SIZE) {
fatal_error("Error receiving KSSL header, size: %d", n);
}
}
break;
}
parse_header(buf, &h);
if (h.version_maj != KSSL_VERSION_MAJ) {
fatal_error("Version mismatch %d != %d", h.version_maj, KSSL_VERSION_MAJ);
}
if (k->id != h.id) {
fatal_error("ID mismatch %08x != %08x", k->id, h.id);
}
dump_header(&h, "recv");
to_return = (kssl_header *)malloc(sizeof(kssl_header));
memcpy(to_return, &h, sizeof(kssl_header));
to_return->data = 0;
if (h.length > 0) {
BYTE *payload = (BYTE *)malloc(h.length);
while (1) {
n = SSL_read(ssl, payload, h.length);
if (n <= 0) {
int x = SSL_get_error(ssl, n);
if (x == SSL_ERROR_WANT_READ || x == SSL_ERROR_WANT_WRITE) {
continue;
} else if (x == SSL_ERROR_ZERO_RETURN) {
fatal_error("Connection closed while reading payload\n");
} else {
fatal_error("Error performing SSL_read: %x\n", x);
}
} else {
if (n != h.length) {
fatal_error("Error receiving KSSL payload, size: %d", n);
}
}
break;
}
if (n != h.length) {
fatal_error("Failed to read payload got length %d wanted %d", n, h.length);
}
dump_payload(h.length, payload);
to_return->data = payload;
}
return to_return;
}
/**
* This API can be used to send packet, without response required.
*
* AT stream format as below:
* [<header>,]data[,<tailer>]
*
* In which, header and tailer is optional.
*/
static int at_send_data_3stage_no_rsp(const char *header, const uint8_t *data,
uint32_t len, const char *tailer)
{
int ret;
if (inited == 0) {
LOGE(MODULE_NAME, "at have not init yet\r\n");
return -1;
}
if (at._mode != ASYN) {
LOGE(MODULE_NAME, "Operation not supported in non asyn mode");
return -1;
}
if (!data || !len) {
return 0;
}
#ifdef DEBUG
dump_payload(data, len);
#endif
aos_mutex_lock(&at.at_uart_send_mutex, AOS_WAIT_FOREVER);
if (header) {
#ifdef HDLC_UART
if ((ret = hdlc_uart_send(&hdlc_encode_ctx, at._pstuart, (void *)header,
strlen(header), at._timeout, true)) != 0)
#else
if ((ret = hal_uart_send(at._pstuart, (void *)header, strlen(header),
at._timeout)) != 0)
#endif
{
LOGE(MODULE_NAME, "uart send packet header failed");
aos_mutex_unlock(&at.at_uart_send_mutex);
assert(0);
return -1;
}
LOGD(MODULE_NAME, "Packet header sent: %s", header);
}
#ifdef HDLC_UART
if ((ret = hdlc_uart_send(&hdlc_encode_ctx, at._pstuart, (void *)data, len,
at._timeout, true)) != 0)
#else
if ((ret = hal_uart_send(at._pstuart, (void *)data, len, at._timeout)) != 0)
#endif
{
LOGE(MODULE_NAME, "uart send packet failed");
aos_mutex_unlock(&at.at_uart_send_mutex);
assert(0);
return -1;
}
LOGD(MODULE_NAME, "Packet sent, len: %d", len);
if (tailer) {
#ifdef HDLC_UART
if ((ret = hdlc_uart_send(&hdlc_encode_ctx, at._pstuart, (void *)tailer,
strlen(tailer), at._timeout, false)) != 0)
#else
if ((ret = hal_uart_send(at._pstuart, (void *)tailer, strlen(tailer),
at._timeout)) != 0)
#endif
{
LOGE(MODULE_NAME, "uart send packet tailer failed");
aos_mutex_unlock(&at.at_uart_send_mutex);
assert(0);
return -1;
}
LOGD(MODULE_NAME, "Packet tailer sent: %s", tailer);
}
aos_mutex_unlock(&at.at_uart_send_mutex);
return 0;
}
/*
* trusted_instantiate - create a new trusted key
*
* Unseal an existing trusted blob or, for a new key, get a
* random key, then seal and create a trusted key-type key,
* adding it to the specified keyring.
*
* On success, return 0. Otherwise return errno.
*/
static int trusted_instantiate(struct key *key, const void *data,
size_t datalen)
{
struct trusted_key_payload *payload = NULL;
struct trusted_key_options *options = NULL;
char *datablob;
int ret = 0;
int key_cmd;
if (datalen <= 0 || datalen > 32767 || !data)
return -EINVAL;
datablob = kmalloc(datalen + 1, GFP_KERNEL);
if (!datablob)
return -ENOMEM;
memcpy(datablob, data, datalen);
datablob[datalen] = '\0';
options = trusted_options_alloc();
if (!options) {
ret = -ENOMEM;
goto out;
}
payload = trusted_payload_alloc(key);
if (!payload) {
ret = -ENOMEM;
goto out;
}
key_cmd = datablob_parse(datablob, payload, options);
if (key_cmd < 0) {
ret = key_cmd;
goto out;
}
dump_payload(payload);
dump_options(options);
switch (key_cmd) {
case Opt_load:
ret = key_unseal(payload, options);
dump_payload(payload);
dump_options(options);
if (ret < 0)
pr_info("trusted_key: key_unseal failed (%d)\n", ret);
break;
case Opt_new:
ret = my_get_random(payload->key, payload->key_len);
if (ret < 0) {
pr_info("trusted_key: key_create failed (%d)\n", ret);
goto out;
}
ret = key_seal(payload, options);
if (ret < 0)
pr_info("trusted_key: key_seal failed (%d)\n", ret);
break;
default:
ret = -EINVAL;
goto out;
}
if (!ret && options->pcrlock)
ret = pcrlock(options->pcrlock);
out:
kfree(datablob);
kfree(options);
if (!ret)
rcu_assign_pointer(key->payload.data, payload);
else
kfree(payload);
return ret;
}
void perform_actions(int sock,char *input_file)
{
char buffer[169] = {0};
char output_file[300] = {0};
strcpy(output_file,input_file);
strcat(output_file,".decrypted");
//printf("Output_file: %s\n",output_file);
while(1)
{
memset(buffer,0,169);
int x = receive_msg(sock,buffer);
switch(x)
{
case HANDSHAKE:
{
//printf("Server handshake has arrived on client\n");
send_handshake_response(sock);
FILE *fd = fopen(input_file,"r");
FILE *foutput = fopen(output_file,"w");
// Here come all the requests.
while(read_line(fd, buffer))
{
//printf("%s\n",buffer);
send_decryption_request(sock,buffer,strlen(buffer));
x = receive_msg(sock,buffer);
if(x != RESPONSE_MESSAGE)
{
perror("Wrong server response");
break;
}
int msg_len = *((int *)(buffer+sizeof(int)));
for(int i = 0; i < msg_len; i++)
{
// printf("%c",buffer[i+2*sizeof(int)]);
fprintf(foutput,"%c",buffer[i+2*sizeof(int)]);
} fprintf(foutput,"\n");
}
fclose(foutput);
fclose(fd);
send_end_of_request(sock);
return;
break;
}
// case RESPONSE_MESSAGE:
// {
// printf("Server response message has arrived on client\n");
// dump_payload(buffer);
// break;
// }
case ERROR_MESSAGE:
{
printf("Error message has arrived on client\n");
dump_payload(buffer);
return;
break;
}
default:
{
printf("Unknown %d msg type has arrived on client\n",x);
char aux[] = "Unknown msg type has arrived on client";
send_error_message(sock,aux,strlen(aux));
return;
break;
}
}
} // End while
}
/*
* trusted_update - reseal an existing key with new PCR values
*/
static int trusted_update(struct key *key, struct key_preparsed_payload *prep)
{
struct trusted_key_payload *p;
struct trusted_key_payload *new_p;
struct trusted_key_options *new_o;
size_t datalen = prep->datalen;
char *datablob;
int ret = 0;
if (test_bit(KEY_FLAG_NEGATIVE, &key->flags))
return -ENOKEY;
p = key->payload.data[0];
if (!p->migratable)
return -EPERM;
if (datalen <= 0 || datalen > 32767 || !prep->data)
return -EINVAL;
datablob = kmalloc(datalen + 1, GFP_KERNEL);
if (!datablob)
return -ENOMEM;
new_o = trusted_options_alloc();
if (!new_o) {
ret = -ENOMEM;
goto out;
}
new_p = trusted_payload_alloc(key);
if (!new_p) {
ret = -ENOMEM;
goto out;
}
memcpy(datablob, prep->data, datalen);
datablob[datalen] = '\0';
ret = datablob_parse(datablob, new_p, new_o);
if (ret != Opt_update) {
ret = -EINVAL;
kfree(new_p);
goto out;
}
if (!new_o->keyhandle) {
ret = -EINVAL;
kfree(new_p);
goto out;
}
/* copy old key values, and reseal with new pcrs */
new_p->migratable = p->migratable;
new_p->key_len = p->key_len;
memcpy(new_p->key, p->key, p->key_len);
dump_payload(p);
dump_payload(new_p);
ret = key_seal(new_p, new_o);
if (ret < 0) {
pr_info("trusted_key: key_seal failed (%d)\n", ret);
kfree(new_p);
goto out;
}
if (new_o->pcrlock) {
ret = pcrlock(new_o->pcrlock);
if (ret < 0) {
pr_info("trusted_key: pcrlock failed (%d)\n", ret);
kfree(new_p);
goto out;
}
}
rcu_assign_keypointer(key, new_p);
call_rcu(&p->rcu, trusted_rcu_free);
out:
kfree(datablob);
kfree(new_o);
return ret;
}
/*
* trusted_instantiate - create a new trusted key
*
* Unseal an existing trusted blob or, for a new key, get a
* random key, then seal and create a trusted key-type key,
* adding it to the specified keyring.
*
* On success, return 0. Otherwise return errno.
*/
static int trusted_instantiate(struct key *key,
struct key_preparsed_payload *prep)
{
struct trusted_key_payload *payload = NULL;
struct trusted_key_options *options = NULL;
size_t datalen = prep->datalen;
char *datablob;
int ret = 0;
int key_cmd;
size_t key_len;
int tpm2;
tpm2 = tpm_is_tpm2(TPM_ANY_NUM);
if (tpm2 < 0)
return tpm2;
if (datalen <= 0 || datalen > 32767 || !prep->data)
return -EINVAL;
datablob = kmalloc(datalen + 1, GFP_KERNEL);
if (!datablob)
return -ENOMEM;
memcpy(datablob, prep->data, datalen);
datablob[datalen] = '\0';
options = trusted_options_alloc();
if (!options) {
ret = -ENOMEM;
goto out;
}
payload = trusted_payload_alloc(key);
if (!payload) {
ret = -ENOMEM;
goto out;
}
key_cmd = datablob_parse(datablob, payload, options);
if (key_cmd < 0) {
ret = key_cmd;
goto out;
}
if (!options->keyhandle) {
ret = -EINVAL;
goto out;
}
dump_payload(payload);
dump_options(options);
switch (key_cmd) {
case Opt_load:
if (tpm2)
ret = tpm_unseal_trusted(TPM_ANY_NUM, payload, options);
else
ret = key_unseal(payload, options);
dump_payload(payload);
dump_options(options);
if (ret < 0)
pr_info("trusted_key: key_unseal failed (%d)\n", ret);
break;
case Opt_new:
key_len = payload->key_len;
ret = tpm_get_random(TPM_ANY_NUM, payload->key, key_len);
if (ret != key_len) {
pr_info("trusted_key: key_create failed (%d)\n", ret);
goto out;
}
if (tpm2)
ret = tpm_seal_trusted(TPM_ANY_NUM, payload, options);
else
ret = key_seal(payload, options);
if (ret < 0)
pr_info("trusted_key: key_seal failed (%d)\n", ret);
break;
default:
ret = -EINVAL;
goto out;
}
if (!ret && options->pcrlock)
ret = pcrlock(options->pcrlock);
out:
kfree(datablob);
kfree(options);
if (!ret)
rcu_assign_keypointer(key, payload);
else
kfree(payload);
return ret;
}
/*
* create and enqueue a batch of packets on a ring.
* On the last one set NS_REPORT to tell the driver to generate
* an interrupt when done.
*/
static int
send_packets(struct netmap_ring *ring, struct pkt *pkt, void *frame,
int size, struct glob_arg *g, u_int count, int options,
u_int nfrags)
{
u_int n, sent, cur = ring->cur;
u_int fcnt;
n = nm_ring_space(ring);
if (n < count)
count = n;
if (count < nfrags) {
D("truncating packet, no room for frags %d %d",
count, nfrags);
}
#if 0
if (options & (OPT_COPY | OPT_PREFETCH) ) {
for (sent = 0; sent < count; sent++) {
struct netmap_slot *slot = &ring->slot[cur];
char *p = NETMAP_BUF(ring, slot->buf_idx);
__builtin_prefetch(p);
cur = nm_ring_next(ring, cur);
}
cur = ring->cur;
}
#endif
for (fcnt = nfrags, sent = 0; sent < count; sent++) {
struct netmap_slot *slot = &ring->slot[cur];
char *p = NETMAP_BUF(ring, slot->buf_idx);
slot->flags = 0;
if (options & OPT_INDIRECT) {
slot->flags |= NS_INDIRECT;
slot->ptr = (uint64_t)frame;
} else if (options & OPT_COPY) {
nm_pkt_copy(frame, p, size);
if (fcnt == nfrags)
update_addresses(pkt, g);
} else if (options & OPT_MEMCPY) {
memcpy(p, frame, size);
if (fcnt == nfrags)
update_addresses(pkt, g);
} else if (options & OPT_PREFETCH) {
__builtin_prefetch(p);
}
if (options & OPT_DUMP)
dump_payload(p, size, ring, cur);
slot->len = size;
if (--fcnt > 0)
slot->flags |= NS_MOREFRAG;
else
fcnt = nfrags;
if (sent == count - 1) {
slot->flags &= ~NS_MOREFRAG;
slot->flags |= NS_REPORT;
}
cur = nm_ring_next(ring, cur);
}
ring->head = ring->cur = cur;
return (sent);
}
