#include <errno.h>
#include <netdb.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include "kdp-udp.h"
#include "defs.h"
#define assert CHECK_FATAL
static inline unsigned short read16u (const unsigned char *s, int bigendian)
{
if (bigendian) {
return (s[1] << 8) + s[0];
} else {
return (s[0] << 8) + s[1];
}
}
static inline unsigned long read32u (const unsigned char *s, int bigendian)
{
if (bigendian) {
return (s[3] << 24) + (s[2] << 16) + (s[1] << 8) + s[0];
} else {
return (s[0] << 24) + (s[1] << 16) + (s[2] << 8) + s[3];
}
}
static inline void write16u (unsigned char *s, unsigned short i, int bigendian)
{
if (bigendian) {
s[1] = (i >> 8) & 0xff;
s[0] = (i >> 0) & 0xff;
} else {
s[0] = (i >> 8) & 0xff;
s[1] = (i >> 0) & 0xff;
}
}
static inline void write32u (unsigned char *s, unsigned long i, int bigendian)
{
if (bigendian) {
s[3] = (i >> 24) & 0xff;
s[2] = (i >> 16) & 0xff;
s[1] = (i >> 8) & 0xff;
s[0] = (i >> 0) & 0xff;
} else {
s[0] = (i >> 24) & 0xff;
s[1] = (i >> 16) & 0xff;
s[2] = (i >> 8) & 0xff;
s[3] = (i >> 0) & 0xff;
}
}
const char *kdp_req_string (kdp_req_t req)
{
switch (req) {
case KDP_CONNECT: return "CONNECT";
case KDP_DISCONNECT: return "DISCONNECT";
case KDP_HOSTINFO: return "HOSTINFO";
case KDP_REGIONS: return "REGIONS";
case KDP_MAXBYTES: return "MAXBYTES";
case KDP_READMEM: return "READMEM";
case KDP_WRITEMEM: return "WRITEMEM";
case KDP_READREGS: return "READREGS";
case KDP_WRITEREGS: return "WRITEREGS";
case KDP_LOAD: return "LOAD";
case KDP_IMAGEPATH: return "IMAGEPATH";
case KDP_SUSPEND: return "SUSPEND";
case KDP_RESUMECPUS: return "RESUMECPUS";
case KDP_EXCEPTION: return "EXCEPTION";
case KDP_TERMINATION: return "TERMINATION";
default: return "[UNKNOWN]";
}
}
const char *kdp_error_string (kdp_error_t error)
{
switch (error) {
case KDP_PROTERR_SUCCESS: return "no error detected";
case KDP_PROTERR_ALREADY_CONNECTED: return "target already connected";
case KDP_PROTERR_BAD_NBYTES: return "invalid memory access";
case KDP_PROTERR_BADFLAVOR: return "bad thread state flavor";
default: return "unknown error";
}
}
const char *kdp_return_string (kdp_return_t error)
{
switch (error) {
case RR_SUCCESS: return "no error detected";
case RR_ALREADY_CONNECTED: return "target already connected";
case RR_BAD_NBYTES: return "illegal address or byte count";
case RR_BADFLAVOR: return "bad thread state flavor";
case RR_SEND_TIMEOUT: return "send timeout exceeded";
case RR_RECV_TIMEOUT: return "receive timeout exceeded";
case RR_IP_ERROR: return "RR_IP_ERROR";
case RR_BAD_ACK: return "RR_BAD_ACK";
case RR_BYTE_COUNT: return "RR_BYTE_COUNT";
case RR_BAD_SEQ: return "RR_BAD_SEQ";
case RR_RESOURCE: return "RR_RESOURCE";
case RR_LOOKUP: return "RR_LOOKUP";
case RR_INTERNAL: return "RR_INTERNAL";
case RR_CONNECT: return "RR_CONNECT";
case RR_INVALID_ADDRESS: return "RR_INVALID_ADDRESS";
case RR_EXCEPTION: return "RR_EXCEPTION";
case RR_RECV_INTR: return "RR_RECV_INTR";
default: return "[UNKNOWN]";
}
}
void kdp_log_data
(kdp_log_function *f, kdp_log_level l, const unsigned char *data, unsigned int nbytes)
{
unsigned int i;
for (i = 0; i < (nbytes / 4); i++) {
const unsigned char *s = data + (i * 4);
if (i == 0) {
f (l, " %8s:", "data");
}
f (l, " 0x%02x%02x%02x%02x", s[0], s[1], s[2], s[3]);
if ((i % 6) == 5) {
f (l, "\n ");
}
}
if (i == 0) {
f (l, " %8s:", "data");
}
for (i = i * 4; i < nbytes; i++) {
f (l, " 0x%02x", data[i]);
}
f (l, "\n");
}
void kdp_log_packet
(kdp_log_function *f, kdp_log_level l, const kdp_pkt_t *p)
{
const kdp_hdr_t *h = &p->hdr;
unsigned int i;
f (l,
"{\n"
" %8s: %s\n"
" %8s: %d\n"
" %8s: %d\n"
" %8s: %d\n",
"request", kdp_req_string (h->request),
"is_reply", h->is_reply,
"seq", h->seq,
"key", h->key);
if (h->is_reply) {
switch (h->request) {
case KDP_CONNECT:
f (l,
" %8s: \"%s\" (%d)\n",
"error", kdp_error_string (p->connect_reply.error), p->connect_reply.error);
break;
case KDP_DISCONNECT:
break;
case KDP_HOSTINFO:
f (l,
" %8s: %d\n"
" %8s: %d\n"
" %8s: %d\n",
"cpu_mask", p->hostinfo_reply.cpu_mask,
"type", p->hostinfo_reply.cpu_type,
"subtype", p->hostinfo_reply.cpu_subtype);
break;
case KDP_REGIONS:
f (l,
" %8s: %d\n",
"nregions", p->regions_reply.nregions);
for (i = 0; i < p->regions_reply.nregions; i++) {
f (l,
" r[%d].addr: %d\n"
" r[%d].nbytes: %d\n"
" r[%d].prot: %d\n",
i, p->regions_reply.regions[i].address,
i, p->regions_reply.regions[i].nbytes,
i, p->regions_reply.regions[i].protection);
}
break;
case KDP_MAXBYTES:
f (l,
" %8s: %d\n",
"maxbytes", p->maxbytes_reply.max_bytes);
break;
case KDP_READMEM:
f (l,
" %8s: \"%s\" (%d)\n"
" %8s: %d\n",
"error", kdp_error_string (p->readmem_reply.error), p->readmem_reply.error,
"nbytes", p->readmem_reply.nbytes);
kdp_log_data (f, l, p->readmem_reply.data, p->readmem_reply.nbytes);
break;
case KDP_WRITEMEM:
f (l,
" %8s: \"%s\" (%d)\n",
"error", kdp_error_string (p->writemem_reply.error), p->writemem_reply.error);
break;
case KDP_READREGS:
f (l,
" %8s: \"%s\" (%d)\n"
" %8s: %d\n",
"error", kdp_error_string (p->readregs_reply.error), p->readregs_reply.error,
"nbytes", p->readregs_reply.nbytes);
kdp_log_data (f, l, p->readregs_reply.data, p->readregs_reply.nbytes);
break;
case KDP_WRITEREGS:
f (l,
" %8s: \"%s\" (%d)\n",
"error", kdp_error_string (p->writeregs_reply.error), p->writeregs_reply.error);
break;
case KDP_LOAD:
f (l,
" %8s: \"%s\" (%d)\n",
"error", kdp_error_string (p->load_reply.error), p->load_reply.error);
break;
case KDP_IMAGEPATH:
f (l,
" %8s: \"%s\"\n",
"path", p->imagepath_reply.path);
break;
case KDP_SUSPEND:
case KDP_RESUMECPUS:
break;
case KDP_EXCEPTION:
break;
case KDP_TERMINATION:
break;
}
} else {
switch (h->request) {
case KDP_CONNECT:
f (l,
" %8s: %d\n"
" %8s: %d\n"
" %8s: \"%s\"\n",
"req_port", p->connect_req.req_reply_port,
"exc_port", p->connect_req.exc_note_port,
"greeting", p->connect_req.greeting);
break;
case KDP_DISCONNECT: break;
case KDP_HOSTINFO: break;
case KDP_REGIONS: break;
case KDP_MAXBYTES: break;
case KDP_READMEM:
f (l,
" %8s: 0x%lx\n"
" %8s: %d\n",
"addr", (unsigned long) p->readmem_req.address,
"nbytes", (unsigned long) p->readmem_req.nbytes);
break;
case KDP_WRITEMEM:
f (l,
" %8s: 0x%lx\n"
" %8s: %d\n",
"addr", (unsigned long) p->writemem_req.address,
"nbytes", (unsigned long) p->writemem_req.nbytes);
kdp_log_data (f, l, p->writemem_req.data, p->writemem_req.nbytes);
break;
case KDP_READREGS:
f (l,
" %8s: 0x%lx\n"
" %8s: 0x%lx\n",
"cpu", (unsigned long) p->readregs_req.cpu,
"flavor", (unsigned long) p->readregs_req.flavor);
break;
case KDP_WRITEREGS:
f (l,
" %8s: 0x%lx\n"
" %8s: 0x%lx\n"
" %8s: 0x%lx\n",
"cpu", (unsigned long) p->writeregs_req.cpu,
"flavor", (unsigned long) p->writeregs_req.flavor,
"nbytes", (unsigned long) p->writeregs_req.nbytes);
kdp_log_data (f, l, p->writeregs_req.data, p->writeregs_req.nbytes);
break;
case KDP_LOAD:
f (l,
" %8s: \"%s\"\n",
"file", p->load_req.file_args);
break;
case KDP_IMAGEPATH: break;
case KDP_SUSPEND: break;
case KDP_RESUMECPUS:
f (l,
" %8s: 0x%lx\n",
"mask", p->resumecpus_req.cpu_mask);
break;
case KDP_EXCEPTION: break;
case KDP_TERMINATION: break;
}
}
f (l, "}\n");
}
kdp_return_t kdp_marshal
(kdp_connection *c, kdp_pkt_t *p, unsigned char *s, size_t maxlen, size_t *plen)
{
#define CHECK_LEN_MAX(len, maxlen) \
if (len > maxlen) { \
c->logger (KDP_LOG_ERROR, \
"kdp_marshal: length required for packet (%lu) is greater than length provided (%lu)\n", \
(unsigned long) len, (unsigned long) maxlen); \
return RR_RESOURCE; \
}
size_t len = 0;
*plen = 0;
CHECK_FATAL (c->logger != NULL);
if (maxlen < 32) {
c->logger (KDP_LOG_ERROR, "kdp_marshal: maximum length must be at least 32\n");
return RR_RESOURCE;
}
if (p->hdr.is_reply) {
switch (p->hdr.request) {
case KDP_CONNECT:
len = 12;
write32u (s + 8, p->connect_reply.error, c->bigendian);
break;
case KDP_DISCONNECT:
len = 8;
break;
case KDP_HOSTINFO:
len = 20;
write32u (s + 8, p->hostinfo_reply.cpu_mask, c->bigendian);
write32u (s + 12, p->hostinfo_reply.cpu_type, c->bigendian);
write32u (s + 16, p->hostinfo_reply.cpu_subtype, c->bigendian);
break;
case KDP_REGIONS: {
const unsigned int REGION_SIZE = 12;
unsigned int i = 0;
len = 12 + (p->regions_reply.nregions * REGION_SIZE);
CHECK_LEN_MAX (len, maxlen);
for (i = 0; i < p->regions_reply.nregions; i++) {
unsigned int offset = 12 + (i * REGION_SIZE);
write32u (s + offset, p->regions_reply.regions[i].address, c->bigendian);
write32u (s + offset + 4, p->regions_reply.regions[i].nbytes, c->bigendian);
write32u (s + offset + 8, p->regions_reply.regions[i].protection, c->bigendian);
}
break;
}
case KDP_MAXBYTES:
len = 12;
write32u (s + 8, p->maxbytes_reply.max_bytes, c->bigendian);
break;
case KDP_READMEM:
len = 12 + p->readmem_reply.nbytes;
CHECK_LEN_MAX (len, maxlen);
memcpy (s + 12, p->readmem_reply.data, len - 12);
break;
case KDP_WRITEMEM:
len = 12;
write32u (s + 8, p->writemem_reply.error, c->bigendian);
break;
case KDP_READREGS: {
const unsigned int KDP_REGISTER_SIZE = 4;
unsigned int i;
unsigned int reglen = p->readregs_reply.nbytes;
len = reglen + 12;
CHECK_LEN_MAX (len, maxlen);
if ((reglen % KDP_REGISTER_SIZE) != 0) { return RR_IP_ERROR; }
write32u (s + 8, p->readregs_reply.error, c->bigendian);
for (i = 0; i < (reglen / KDP_REGISTER_SIZE); i++) {
write32u (s + 12 + (i * KDP_REGISTER_SIZE),
((unsigned long *) p->readregs_reply.data)[i],
c->bigendian);
}
break;
}
case KDP_WRITEREGS:
len = 12;
write32u (s + 8, p->writeregs_reply.error, c->bigendian);
break;
case KDP_LOAD:
len = 12;
write32u (s + 8, p->load_reply.error, c->bigendian);
break;
case KDP_IMAGEPATH:
len = 8 + strlen (p->imagepath_reply.path) + 1;
CHECK_LEN_MAX (len, maxlen);
memcpy (s + 8, p->imagepath_reply.path, len - 8);
break;
case KDP_SUSPEND:
case KDP_RESUMECPUS:
case KDP_EXCEPTION:
case KDP_TERMINATION:
len = 8;
break;
default:
c->logger (KDP_LOG_ERROR, "kdp_marshal: unknown packet type for reply 0x%x\n", p->hdr.request);
return RR_IP_ERROR;
}
} else {
switch (p->hdr.request) {
case KDP_CONNECT:
len = 12 + strlen (p->connect_req.greeting) + 1;
CHECK_LEN_MAX (len, maxlen);
write16u (s + 8, p->connect_req.req_reply_port, 0);
write16u (s + 10, p->connect_req.exc_note_port, 0);
memcpy (s + 12, p->connect_req.greeting, len - 12);
break;
case KDP_DISCONNECT:
case KDP_HOSTINFO:
case KDP_REGIONS:
case KDP_MAXBYTES:
len = 8;
break;
case KDP_READMEM:
len = 16;
write32u (s + 8, p->readmem_req.address, c->bigendian);
write32u (s + 12, p->readmem_req.nbytes, c->bigendian);
break;
case KDP_WRITEMEM:
len = 16 + p->writemem_req.nbytes;
CHECK_LEN_MAX (len, maxlen);
write32u (s + 8, p->writemem_req.address, c->bigendian);
write32u (s + 12, p->writemem_req.nbytes, c->bigendian);
memcpy (s + 16, p->writemem_req.data, p->writemem_req.nbytes);
break;
case KDP_READREGS:
len = 16;
write32u (s + 8, p->readregs_req.cpu, c->bigendian);
write32u (s + 12, p->readregs_req.flavor, c->bigendian);
break;
case KDP_WRITEREGS: {
const unsigned int KDP_REGISTER_SIZE = 4;
unsigned int i;
unsigned int reglen = p->writeregs_req.nbytes;
if ((reglen % KDP_REGISTER_SIZE) != 0) {
c->logger (KDP_LOG_ERROR,
"kdp_marshal: length of register data (%u bytes) "
"is not a multiple of register size (%u bytes)\n",
p->writeregs_req.nbytes, KDP_REGISTER_SIZE);
return RR_IP_ERROR;
}
len = reglen + 16;
CHECK_LEN_MAX (len, maxlen);
write32u (s + 8, p->writeregs_req.cpu, c->bigendian);
write32u (s + 12, p->writeregs_req.flavor, c->bigendian);
for (i = 0; i < (reglen / KDP_REGISTER_SIZE); i++) {
write32u (s + 16 + (i * KDP_REGISTER_SIZE),
((unsigned long *) p->writeregs_req.data)[i],
c->bigendian);
}
break;
}
case KDP_LOAD:
len = 8 + strlen (p->load_req.file_args) + 1;
CHECK_LEN_MAX (len, maxlen);
memcpy (s + 8, p->connect_req.greeting, len - 8);
break;
case KDP_IMAGEPATH:
case KDP_SUSPEND:
len = 8;
break;
case KDP_RESUMECPUS:
len = 12;
write32u (s + 8, p->resumecpus_req.cpu_mask, c->bigendian);
break;
case KDP_EXCEPTION: {
const unsigned int EXCEPTION_SIZE = 16;
unsigned int i;
len = 12 + (p->exception.n_exc_info * EXCEPTION_SIZE);
CHECK_LEN_MAX (len, maxlen);
write32u (s + 8, p->exception.n_exc_info, c->bigendian);
for (i = 0; i < p->exception.n_exc_info; i++) {
kdp_exc_info_t *e = &p->exception.exc_info[i];
write32u (s + 12 + (i * EXCEPTION_SIZE), e->cpu, c->bigendian);
write32u (s + 12 + (i * EXCEPTION_SIZE) + 4, e->exception, c->bigendian);
write32u (s + 12 + (i * EXCEPTION_SIZE) + 8, e->code, c->bigendian);
write32u (s + 12 + (i * EXCEPTION_SIZE) + 12, e->subcode, c->bigendian);
}
break;
}
case KDP_TERMINATION:
len = 16;
write32u (s + 8, p->termination.term_code, c->bigendian);
write32u (s + 12, p->termination.exit_code, c->bigendian);
break;
default:
c->logger (KDP_LOG_ERROR, "kdp_marshal: unknown packet type for request 0x%x\n", p->hdr.request);
return RR_IP_ERROR;
}
}
CHECK_LEN_MAX (len, maxlen);
if (len < 8) {
c->logger (KDP_LOG_ERROR, "kdp_marshal: length of packet (%lu) is less than 8 "
"(an error must have occurred)\n");
return RR_RESOURCE;
}
if (c->bigendian) {
s[0] = (p->hdr.request & 0x8f) | (p->hdr.is_reply << 7);
} else {
s[0] = ((p->hdr.request & 0x8f) << 1) | (p->hdr.is_reply & 0x1);
}
s[1] = p->hdr.seq;
write16u (s + 2, len, c->bigendian);
write32u (s + 4, p->hdr.key, c->bigendian);
*plen = len;
return RR_SUCCESS;
}
kdp_return_t kdp_unmarshal
(kdp_connection *c, kdp_pkt_t *p, const unsigned char *s, size_t rlen)
{
#define CHECK_PLEN_RLEN(plen, rlen) \
if (plen != rlen) { \
c->logger (KDP_LOG_ERROR, \
"kdp_unmarshal: length contained in packet (%lu) " \
"does not match length read from socket (%lu)\n", \
(unsigned long) plen, (unsigned long) rlen); \
return RR_IP_ERROR; \
}
#define CHECK_PLEN_LEN(plen, len) \
if (plen != len) { \
c->logger (KDP_LOG_ERROR, \
"kdp_unmarshal: length contained in packet (%lu) " \
"does not match expected length of packet (%lu)\n", \
(unsigned long) plen, (unsigned long) len); \
return RR_IP_ERROR; \
}
#define CHECK_PLEN_MINLEN(plen, len) \
if (plen < len) { \
c->logger (KDP_LOG_ERROR, \
"kdp_unmarshal: length contained in packet (%lu) " \
"is smaller than expected minimum packet length (%lu)\n", \
(unsigned long) plen, (unsigned long) len); \
return RR_IP_ERROR; \
}
size_t plen = 0;
CHECK_FATAL (c->logger != NULL);
if (rlen < 8) {
c->logger (KDP_LOG_ERROR, "data length (%lu) is too small (less than %d)\n",
rlen, 8);
return RR_IP_ERROR;
}
if (rlen > KDP_MAX_PACKET_SIZE) {
c->logger (KDP_LOG_ERROR, "data length (%lu) is too large (greater than %d)\n",
rlen, KDP_MAX_PACKET_SIZE);
return RR_IP_ERROR;
}
if (c->bigendian) {
p->hdr.is_reply = s[0] >> 7;
p->hdr.request = s[0] & 0x7f;
} else {
p->hdr.request = s[0] >> 1;
p->hdr.is_reply = s[0] & 0x1;
}
p->hdr.seq = s[1];
plen = read16u (s + 2, c->bigendian);
p->hdr.key = read32u (s + 4, c->bigendian);
if (p->hdr.is_reply) {
switch (p->hdr.request) {
case KDP_CONNECT:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 12);
p->connect_reply.error = read32u (s + 8, c->bigendian);
break;
case KDP_DISCONNECT:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 8);
break;
case KDP_HOSTINFO:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 20);
p->hostinfo_reply.cpu_mask = read32u (s + 8, c->bigendian);
p->hostinfo_reply.cpu_type = read32u (s + 12, c->bigendian);
p->hostinfo_reply.cpu_subtype = read32u (s + 16, c->bigendian);
break;
case KDP_REGIONS: {
const unsigned int REGION_SIZE = 12;
unsigned int i;
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 12);
p->regions_reply.nregions = read32u (s + 8, c->bigendian);
CHECK_PLEN_LEN (plen, ((p->regions_reply.nregions * REGION_SIZE) + 12));
for (i = 0; i < p->regions_reply.nregions; i++) {
unsigned int offset = 12 + (i * REGION_SIZE);
p->regions_reply.regions[i].address = read32u (s + offset, c->bigendian);
p->regions_reply.regions[i].nbytes = read32u (s + offset + 4, c->bigendian);
p->regions_reply.regions[i].protection = read32u (s + offset + 8, c->bigendian);
}
break;
}
case KDP_MAXBYTES:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 12);
p->maxbytes_reply.max_bytes = read32u (s + 8, c->bigendian);
break;
case KDP_READMEM:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 12);
p->readmem_reply.error = read32u (s + 8, c->bigendian);
p->readmem_reply.nbytes = plen - 12;
memcpy (p->readmem_reply.data, s + 12, plen - 12);
break;
case KDP_WRITEMEM:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 12);
p->writemem_reply.error = read32u (s + 8, c->bigendian);
break;
case KDP_READREGS: {
const unsigned int KDP_REGISTER_SIZE = 4;
unsigned int i;
unsigned int reglen = plen - 12;
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 12);
p->readregs_reply.error = read32u (s + 8, c->bigendian);
if ((reglen % KDP_REGISTER_SIZE) != 0) {
c->logger (KDP_LOG_ERROR,
"length of register data (%u) is not an integer multiple of KDP_REGISTER_SIZE (%u)\n",
reglen, KDP_REGISTER_SIZE);
return RR_IP_ERROR;
}
p->readregs_reply.nbytes = reglen;
for (i = 0; i < (reglen / KDP_REGISTER_SIZE); i++) {
((unsigned long *) p->readregs_reply.data)[i] =
read32u (s + 12 + (i * KDP_REGISTER_SIZE), c->bigendian);
}
break;
}
case KDP_WRITEREGS:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 12);
p->writeregs_reply.error = read32u (s + 8, c->bigendian);
break;
case KDP_LOAD:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 12);
p->load_reply.error = read32u (s + 8, c->bigendian);
break;
case KDP_IMAGEPATH:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 8);
memcpy (p->imagepath_reply.path, s + 8, plen - 8);
break;
case KDP_SUSPEND:
case KDP_RESUMECPUS:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 8);
break;
case KDP_EXCEPTION:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 8);
break;
case KDP_TERMINATION:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 8);
break;
default:
c->logger (KDP_LOG_ERROR, "kdp_unmarshal: unknown packet type 0x%x\n", p->hdr.request);
return RR_IP_ERROR;
}
} else {
switch (p->hdr.request) {
case KDP_CONNECT:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 12 + 1);
p->connect_req.req_reply_port = read16u (s + 8, 0);
p->connect_req.exc_note_port = read16u (s + 10, 0);
memcpy (p->connect_req.greeting, s + 12, plen - 12);
break;
case KDP_DISCONNECT:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 8);
break;
case KDP_HOSTINFO:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 8);
break;
case KDP_REGIONS:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 8);
break;
case KDP_MAXBYTES:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 8);
break;
case KDP_READMEM:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 16);
p->readmem_req.address = read32u (s + 8, c->bigendian);
p->readmem_req.nbytes = read32u (s + 12, c->bigendian);
break;
case KDP_WRITEMEM:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 16);
p->writemem_req.address = read32u (s + 8, c->bigendian);
p->writemem_req.nbytes = read32u (s + 12, c->bigendian);
CHECK_PLEN_LEN (plen, p->writemem_req.nbytes + 16);
memcpy (p->writemem_req.data, s + 16, plen - 16);
break;
case KDP_READREGS:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 16);
p->readregs_req.cpu = read32u (s + 8, c->bigendian);
p->readregs_req.flavor = read32u (s + 12, c->bigendian);
break;
case KDP_WRITEREGS: {
const unsigned int KDP_REGISTER_SIZE = 4;
unsigned int i;
unsigned int reglen = plen - 16;
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 16);
p->writeregs_req.cpu = read32u (s + 8, c->bigendian);
p->writeregs_req.flavor = read32u (s + 12, c->bigendian);
if ((reglen % KDP_REGISTER_SIZE) != 0) {
c->logger (KDP_LOG_ERROR,
"length of register data (%u) is not an integer multiple of KDP_REGISTER_SIZE (%u)\n",
reglen, KDP_REGISTER_SIZE);
return RR_IP_ERROR;
}
p->writeregs_req.nbytes = reglen;
for (i = 0; i < (reglen / KDP_REGISTER_SIZE); i++) {
((unsigned long *) p->writeregs_req.data)[i] =
read32u (s + 16 + (i * KDP_REGISTER_SIZE), c->bigendian);
}
break;
}
case KDP_LOAD:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 8 + 1);
memcpy (p->load_req.file_args, c + 8, plen - 8);
break;
case KDP_IMAGEPATH:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 8);
break;
case KDP_SUSPEND:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 8);
break;
case KDP_RESUMECPUS:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 12);
p->resumecpus_req.cpu_mask = read32u (s + 8, c->bigendian);
break;
case KDP_EXCEPTION: {
const unsigned int EXCEPTION_SIZE = 16;
unsigned int i;
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_MINLEN (plen, 12);
p->exception.n_exc_info = read32u (s + 8, c->bigendian);
if ((plen == 44) && (p->exception.n_exc_info == 1))
{ plen = 28; }
CHECK_PLEN_LEN (plen, ((p->exception.n_exc_info * EXCEPTION_SIZE) + 12));
for (i = 0; i < p->exception.n_exc_info; i++) {
unsigned int offset = 12 + (i * EXCEPTION_SIZE);
p->exception.exc_info[i].cpu = read32u (s + offset, c->bigendian);
p->exception.exc_info[i].exception = read32u (s + offset + 4, c->bigendian);
p->exception.exc_info[i].code = read32u (s + offset + 8, c->bigendian);
p->exception.exc_info[i].subcode = read32u (s + offset + 12, c->bigendian);
}
break;
}
case KDP_TERMINATION:
CHECK_PLEN_RLEN (plen, rlen);
CHECK_PLEN_LEN (plen, 16);
p->termination.term_code = read32u (s + 8, c->bigendian);
p->termination.exit_code = read32u (s + 12, c->bigendian);
break;
default:
c->logger (KDP_LOG_ERROR, "kdp_unmarshal: unknown packet type 0x%x\n", p->hdr.request);
return RR_IP_ERROR;
}
}
return RR_SUCCESS;
}