コード例 #1
0
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));        
    }
}
コード例 #2
0
ファイル: shsecd.c プロジェクト: arvjus/shsec
/* 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 {