static void gyro_cnf_handler(GYRO_CNF *cnf)
{
printf("-------------------------------------------------------------------\n");
if (cnf->op == GYRO_OP_CALI) {
printf("OP_CALI: %d %d %s\n", cnf->gyro_err, cnf->status, GET_ERR(cnf->status));
printf("(%+6d, %+6d, %+6d) => (%+9.4f, %+9.4f, %+9.4f)\n",
cnf->ack.cali.x, cnf->ack.cali.y, cnf->ack.cali.z,
LSB_TO_GRA(cnf->ack.cali.x), LSB_TO_GRA(cnf->ack.cali.y), LSB_TO_GRA(cnf->ack.cali.z));
} else if (cnf->op == GYRO_OP_READ_RAW) {
printf("OP_READ_RAW: %d %d %s\n", cnf->gyro_err, cnf->status, GET_ERR(cnf->status));
printf("(%+6d, %+6d, %+6d) => (%+9.4f, %+9.4f, %+9.4f)\n",
cnf->ack.readraw.x, cnf->ack.cali.y, cnf->ack.cali.z,
LSB_TO_GRA(cnf->ack.readraw.x), LSB_TO_GRA(cnf->ack.cali.y), LSB_TO_GRA(cnf->ack.cali.z));
} else if (cnf->op == GYRO_OP_WRITE_NVRAM) {
printf("GYRO_OP_WRITE_NVRAM: %d %d %s\n", cnf->gyro_err, cnf->status, GET_ERR(cnf->status));
} else if (cnf->op == GYRO_OP_READ_NVRAM) {
printf("OP_READ_NVRAM: %d %d %s\n", cnf->gyro_err, cnf->status, GET_ERR(cnf->status));
printf("(%+6d, %+6d, %+6d) => (%+9.4f, %+9.4f, %+9.4f)\n",
cnf->ack.readnv.x, cnf->ack.readnv.y, cnf->ack.readnv.z,
LSB_TO_GRA(cnf->ack.readnv.x), LSB_TO_GRA(cnf->ack.readnv.y), LSB_TO_GRA(cnf->ack.readnv.z));
}
}
/* accept and process request form local client */
void process_client_request(int fd_un2)
{
int len, len_toreceive, len_received, len_tosend, len_sent;
int rc, fd_in = 0;
CLI_REQUEST creq;
CLI_RESPONSE cresp;
SRV_REQUEST sreq;
SRV_RESPONSE sresp;
SRV_MESSAGE smsg;
char buf[2048];
char keyid[MAX_TAG_LEN + MAX_HOST_LEN + 10];
BIGNUM *bn_base = NULL, *bn_prime = NULL, *bn_private = NULL;
unsigned char* public = NULL;
int public_len;
unsigned char* secret_key = NULL;
int secret_key_len;
memset(&cresp, 0, sizeof(cresp));
memset(&sreq, 0, sizeof(sreq));
memset(&smsg, 0, sizeof(smsg));
/* receive request - we can expect at least 7 first bytes to receive */
len = recv(fd_un2, buf, 7, 0);
if (len < 0) {
output(LOG_ERR, "recv failed. %s", strerror(errno));
cresp.status = MK_STATUS(SCOPE_HOST, ERR_COMM);
goto send;
}
else if (!len) return;
len_received = len;
len_toreceive = get_der_seq_size(buf);
if (len_toreceive > sizeof(buf)) {
output(LOG_ERR, "packet sent by client is too big");
cresp.status = MK_STATUS(SCOPE_CLIENT, ERR_MALFORMED);
goto send;
}
/* get the rest */
while (len_toreceive > len_received) {
len = len_toreceive - len_received;
len = recv(fd_un2, &buf[len_received], len, 0);
if (len < 0) {
output(LOG_ERR, "recv failed. %s", strerror(errno));
cresp.status = MK_STATUS(SCOPE_HOST, ERR_COMM);
goto send;
}
else if (!len) return;
len_received += len;
}
/* decode request message */
rc = decode_client_request(buf, len_received, &creq);
if (rc) {
cresp.status = MK_STATUS(SCOPE_CLIENT, rc);
goto send;
}
/* check if parameters, flags are valid */
if (creq.version != CURRENT_PROTOCOL_VERSION) {
output(LOG_ERR, "unsupported protocol version %d", creq.version);
cresp.status = MK_STATUS(SCOPE_CLIENT, ERR_PROTOCOL);
goto send;
}
if (check_flag_validity(creq.flags)) {
output(LOG_ERR, "not allowed option combination");
cresp.status = MK_STATUS(SCOPE_CLIENT, ERR_INVALID_PARAM);
goto send;
}
if (creq.reqType == CRT_ENUM_KEYS) {
cresp.status = STATUS_SUCCESS;
goto send;
}
else
if (creq.reqType == CRT_REQUEST_KEY &&
(!creq.keyLen || creq.keyLen > MAX_KEY_LEN)) {
output(LOG_ERR, "invalid size of requested key: %d", creq.keyLen);
cresp.status = MK_STATUS(SCOPE_CLIENT, ERR_INVALID_PARAM);
goto send;
}
/* make keyid */
sprintf(keyid, "%s@%s", creq.tag, creq.peer);
/* process request */
if (creq.reqType == CRT_DELETE_KEY) {
/* check for a key in database and delete it */
if (keydb_exists(keyid)) {
rc = keydb_delete(keyid);
if (rc) {
output(LOG_ERR, "keydb_delete failed. %s", strerror(errno));
cresp.status = MK_STATUS(SCOPE_HOST, ERR_INTERNAL);
}
}
else {
output(LOG_ERR, "requested key does not exist: '%s'", keyid);
cresp.status = MK_STATUS(SCOPE_HOST, ERR_KEY_NOT_EXISTS);
}
}
/* here we block and get requested key */
else if (creq.flags & FLAG_INIT_KEY) {
struct sockaddr_in peer_addr;
char addr_accept[] = "0123456789.";
PEER_ENTRY* pe;
/* locate and check the peer */
pe = find_peer(creq.peer);
if (!pe) {
output(LOG_ERR, "peer '%s' is not available", creq.peer);
cresp.status = MK_STATUS(SCOPE_HOST, ERR_NOT_AVAILABLE);
goto send;
}
else
if (!pe->allow) {
output(LOG_ERR, "access denied for peer '%s'", creq.peer);
cresp.status = MK_STATUS(SCOPE_HOST, ERR_ACCESS_DENIED);
goto send;
}
/* generate secret, calculate public value */
rc = dh_init_group_params(conf_dh_group, &bn_prime, &bn_base);
if (rc) {
output(LOG_ERR, "dh_init_group_params failed");
cresp.status = MK_STATUS(SCOPE_HOST, rc);
goto send;
}
DUMP_BN(bn_prime, "prime ");
DUMP_BN(bn_base, "base ");
rc = dh_gen_private_public(conf_dh_group, bn_prime, bn_base,
&bn_private, &public, &public_len);
if (rc) {
output(LOG_ERR, "dh_gen_private_public failed");
cresp.status = MK_STATUS(SCOPE_HOST, rc);
goto send;
}
DUMP_BN(bn_private, "priv-i");
DUMP_BIN(public, public_len, "publ-i");
/* fill-in request */
sreq.cookie = generate_unique_cookie();
sreq.keyLen = creq.keyLen;
strncpy(sreq.tag, creq.tag, sizeof(sreq.tag));
strncpy(sreq.host, conf_host, sizeof(sreq.host));
strncpy(sreq.expires, creq.expires, sizeof(sreq.expires));
sreq.dhGroup = conf_dh_group;
sreq.dhPublic_len = public_len;
memcpy(sreq.dhPublic, public, public_len);
smsg.tbsMsg_len = sizeof(smsg.tbsMsg);
rc = encode_server_request(&sreq, smsg.tbsMsg, &smsg.tbsMsg_len);
if (rc) {
cresp.status = MK_STATUS(SCOPE_HOST, rc);
goto send;
}
/* fill-in request message */
smsg.version = CURRENT_PROTOCOL_VERSION;
/* sign request if required */
if (pe->auth != AUTH_NONE) {
rc = sign_message(pe, smsg.tbsMsg, smsg.tbsMsg_len,
smsg.sigAlg, &smsg.sigAlg_len, smsg.sig, &smsg.sig_len);
if (rc) {
cresp.status = MK_STATUS(SCOPE_HOST, rc);
goto send;
}
}
/* encode message */
len = sizeof(buf);
rc = encode_server_message(&smsg, buf, &len);
if (rc) {
cresp.status = MK_STATUS(SCOPE_HOST, rc);
goto send;
}
/* get peer id & address, connect to socket and send the message */
/* is it ip address? */
if (strspn(creq.peer, addr_accept) == strlen(creq.peer)) {
if (!inet_aton(creq.peer, &(peer_addr.sin_addr))) {
output(LOG_ERR, "inet_aton failed");
cresp.status = MK_STATUS(SCOPE_HOST, ERR_COMM);
goto send;
}
}
/* resolve dns */
else {
struct hostent* h = gethostbyname(creq.peer);
if (!h) {
output(LOG_ERR, "gethostbyname failed");
cresp.status = MK_STATUS(SCOPE_HOST, ERR_COMM);
goto send;
}
peer_addr.sin_addr = *((struct in_addr*)h->h_addr);
}
peer_addr.sin_family = AF_INET;
peer_addr.sin_port = htons(creq.port ? creq.port : DEFAULT_PORT);
memset(peer_addr.sin_zero, 0, sizeof(peer_addr.sin_zero));
fd_in = socket(AF_INET, SOCK_STREAM, 0);
if (!fd_in) {
output(LOG_ERR, "socket failed. %s", strerror(errno));
cresp.status = MK_STATUS(SCOPE_HOST, ERR_COMM);
goto send;
}
rc = connect(fd_in, (struct sockaddr*)&peer_addr,
sizeof(struct sockaddr));
if (rc == -1) {
output(LOG_ERR, "connect failed. %s", strerror(errno));
cresp.status = MK_STATUS(SCOPE_HOST, ERR_COMM);
goto send;
}
/* send request */
len_sent = 0;
len_tosend = len;
while (len_tosend > len_sent) {
len = len_tosend - len_sent;
len = send(fd_in, &buf[len_sent], len, 0);
if (len < 0) {
output(LOG_ERR, "send failed/connection closed by peer");
cresp.status = MK_STATUS(SCOPE_HOST, ERR_COMM);
goto send;
}
len_sent += len;
}
memset(&smsg, 0, sizeof(smsg));
/* receive response - we can expect at least 14 bytes to receive */
len = recv(fd_in, buf, 14, 0);
if (len <= 0) {
output(LOG_ERR, "recv failed/connection closed by peer");
cresp.status = MK_STATUS(SCOPE_HOST, ERR_COMM);
goto send;
}
len_received = len;
len_toreceive = get_der_seq_size(buf);
assert(len_toreceive <= sizeof(buf));
if (len_toreceive > sizeof(buf)) {
output(LOG_ERR, "packet sent by peer is too big");
cresp.status = MK_STATUS(SCOPE_PEER, ERR_MALFORMED);
goto send;
}
/* get the rest */
while (len_toreceive > len_received) {
len = len_toreceive - len_received;
len = recv(fd_in, &buf[len_received], len, 0);
if (len < 0) {
output(LOG_ERR, "recv failed/connection closed by peer");
cresp.status = MK_STATUS(SCOPE_HOST, ERR_COMM);
goto send;
}
len_received += len;
}
close(fd_in);
fd_in = 0;
/* decode response message */
rc = decode_server_message(buf, len_received, &smsg);
if (rc) {
cresp.status = MK_STATUS(SCOPE_HOST, rc);
goto send;
}
/* sanity check */
if (smsg.version != CURRENT_PROTOCOL_VERSION) {
output(LOG_ERR, "unsupported protocol version %d", smsg.version);
cresp.status = MK_STATUS(SCOPE_PEER, ERR_PROTOCOL);
goto send;
}
/* verify signature if required */
if (smsg.sigAlg_len && smsg.sig_len) {
rc = verify_signature(pe, smsg.tbsMsg, smsg.tbsMsg_len,
smsg.sigAlg, smsg.sigAlg_len, smsg.sig, smsg.sig_len);
if (rc) {
cresp.status = MK_STATUS(SCOPE_HOST, rc);
goto send;
}
}
else
if (pe->auth != AUTH_NONE) {
output(LOG_ERR, "response is not signed");
cresp.status = MK_STATUS(SCOPE_PEER, ERR_SIG_VERIFY);
goto send;
}
/* decode response */
rc = decode_server_response(smsg.tbsMsg, smsg.tbsMsg_len, &sresp);
if (rc) {
cresp.status = MK_STATUS(SCOPE_HOST, rc);
goto send;
}
if (GET_ERR(sresp.status)) {
output(LOG_ERR, "peer returned status 0x%0.2hx", sresp.status);
cresp.status = MK_STATUS(SCOPE_PEER, GET_ERR(sresp.status));
goto send;
}
/* calculate shared secret, make a requested key */
rc = dh_get_secret(sresp.dhPublic, sresp.dhPublic_len,
bn_private, bn_prime, &secret_key, &secret_key_len);
if (rc) {
output(LOG_ERR, "dh_get_secret failed");
cresp.status = MK_STATUS(SCOPE_HOST, rc);
goto send;
}
DUMP_BIN(sresp.dhPublic, sresp.dhPublic_len, "publ-r");
DUMP_BIN(secret_key, secret_key_len, "secr-i");
assert(secret_key_len >= BITS_TO_BYTES(creq.keyLen));
/* store a key to database */
if (creq.flags & FLAG_STORE_KEY) {
char kval_buf[KEY_VAL_SIZE + MAX_KEY_LEN];
PKEY_VAL pkey_val = (PKEY_VAL)kval_buf;
pkey_val->expires = gentime_to_time_t(creq.expires);
if (pkey_val->expires == -1) {
output(LOG_ERR, "cannot convert time value %s", creq.expires);
cresp.status = MK_STATUS(SCOPE_HOST, ERR_INVALID_PARAM);
goto send;
}
pkey_val->key_len = creq.keyLen;
memcpy(pkey_val->key, secret_key,
BITS_TO_BYTES(pkey_val->key_len));
rc = keydb_store(keyid, kval_buf, sizeof(KEY_VAL) +
BITS_TO_BYTES(pkey_val->key_len));
if (rc) {
output(LOG_ERR, "keydb_store failed. %s", strerror(errno));
cresp.status = MK_STATUS(SCOPE_HOST, ERR_INTERNAL);
goto send;
}
output(LOG_INFO, "new key '%s' (%dbits) added, validity: %s",
keyid, pkey_val->key_len, ctime(&pkey_val->expires));
}
cresp.keyLen = creq.keyLen;
memcpy(cresp.key, secret_key, BITS_TO_BYTES(cresp.keyLen));
cresp.status = STATUS_SUCCESS;
}
else {