C++ PRINT_DEBUG函数代码示例

cl_int clEnqueueNDRangeKernel (cl_command_queue command_queue,
                               cl_kernel kernel,
                               cl_uint work_dim,
                               const size_t *global_work_offset,
                               const size_t *global_work_size,
                               const size_t *local_work_size,
                               cl_uint num_events_in_wait_list,
                               const cl_event *event_wait_list,
                               cl_event *event)
{

  if(command_queue == NULL || command_queue == (cl_command_queue)0)
    {
      return CL_INVALID_COMMAND_QUEUE;
    }

  if(kernel == NULL || kernel == (cl_kernel)0)
    {
      return CL_INVALID_KERNEL;
    }

  PRINT_DEBUG("\n====\tCreating Event  \t====\n");

  cl_event new_event = createNewEvent(command_queue, CL_COMMAND_NDRANGE_KERNEL,
				      num_events_in_wait_list, event_wait_list);
  
  setNDRangeEvent(new_event, kernel, work_dim, global_work_offset,
		  global_work_size, local_work_size);

  
  //  clReleaseEvent(new_event);
  //  free(new_event);


  PRINT_DEBUG("\n====\tEnqueuing kernel  \t====\n");
  
  //Will only do one dimension
  if(work_dim != 1)
    {
      return CL_INVALID_WORK_DIMENSION;
    }

  cl_program program = kernel->kernel_program;


  PRINT_DEBUG("NDRange: Program location %p\n", program);
  PRINT_DEBUG("NDRange: Program elefs location %p\n",
	      program->program_elfs);

  addEventToCommandQueue(new_event, command_queue);

  if(num_events_in_wait_list > 0)
    {

      // Do appropriate stuff here for event wait list
    }

  PRINT_DEBUG("\n====\tKernel Enqueued \t====\n");

  return CL_SUCCESS;

}

/**
 * Prepares the stack for a 64-bit guest and places the arguments in the correct
 * register / stack locations.
 *
 * Arguments on 64-bit systems:
 * 		1st ARG: %RDI
 * 		2nd ARG: %RSI
 * 		3rd ARG: %RDX
 * 		4th ARG: %RCX
 * 		5th ARG: %R8
 * 		6th ARG: %R9
 * 		7th ARG - nth ARG: on stack from right to left
 *
 * @param inject The injection structure of the module that is injected.
 * @param virt_stack A pointer to the virtual address of the memory area
 *                   that was reserved for the stack of the module.
 */
void prepareStack64(struct kvm_vcpu *vcpu, struct injection *inject, u64 *virt_stack)
{
	u64 phys_stack = 0;
	struct x86_exception error;
	struct injection_arg *arg = NULL;
	unsigned int i = 0;
	int ret = 0;
	enum kvm_reg reg;

	// Do we actually have arguments?
	if (inject->args)
	{
		// Move all data to the stack that cannot be directly passed as an argument
		// such as strings and structures.
		for (i = 0; i < inject->args->argc; ++i)
		{
			arg = get_next_arg(inject, arg);

			if (!is_immediate(arg))
			{
				// Copy the data to the stack
				PRINT_DEBUG("Writing data of argument %d with type %d and size %d to 0x%llx\n",
							i, arg->type, arg->size, *virt_stack - arg->size);

				// Update address
				(*virt_stack) -= arg->size;
				arg->data_on_stack = (void *)(*virt_stack);

				// Write
				phys_stack = vcpu->arch.mmu.gva_to_gpa(vcpu, (*virt_stack), 0, &error);
				ret = kvm_write_guest(vcpu->kvm, phys_stack, get_arg_data(inject, arg), arg->size);

				if(ret < 0)
				{
					PRINT_ERROR("An error (code: %d) occurred while writing the argument %d to memory!\n",
								ret, i);
					return;
				}
			}
		}

		// Place arguments into the correct register / stack locations
		arg = NULL;
		for (i = inject->args->argc; i > 0 ; --i)
		{
			arg = get_prev_arg(inject, arg);

			if (i >= 7)
			{
				// Arg goes on the stack
				// ToDo: We just fix this to 8 byte here, but the size of the arg
				// may actually be shorter
				(*virt_stack) -= 8;
				phys_stack = vcpu->arch.mmu.gva_to_gpa(vcpu, (*virt_stack), 0, &error);

				if (is_immediate(arg))
				{
					PRINT_DEBUG("Writing argument %d with type %d and size %d to the stack 0x%llx\n",
								 i, arg->type, arg->size, *virt_stack);

					ret = kvm_write_guest(vcpu->kvm, phys_stack, get_arg_data(inject, arg), arg->size);
				}
				else
				{
					PRINT_DEBUG("Writing pointer 0x%lx to argument %d with type %d and size %d to the stack 0x%llx\n",
								(unsigned long)arg->data_on_stack, i, arg->type, arg->size, *virt_stack);
					ret = kvm_write_guest(vcpu->kvm, phys_stack, &arg->data_on_stack, 8);
				}

				if(ret < 0)
				{
					PRINT_ERROR("An error (code: %d) occurred while writing the argument %d "
								"to the stack!\n",
								ret, i);
					return;
				}
			}
			else
			{
				// Arg goes into a register
				switch (i)
				{
					case 1:
//.........这里部分代码省略.........

/*
 * Handle a halt instruction that can indicate that the executing code just finished.
 */
int XTIER_inject_handle_hlt(struct kvm_vcpu *vcpu)
{
	// Stop execution time
	XTIER_inject_end_time_measurement(&_starttime, &_XTIER_performance.total_module_exec_time);

	// Get Time
	XTIER_inject_begin_time_measurement(&_starttime);

	PRINT_INFO("Handling HLT Exit...\n");

	//PRINT_DEBUG("RAX: 0x%llx\n", kvm_register_read(vcpu, VCPU_REGS_RAX));

	// Remove Mappings
	XTIER_memory_remove_mappings_pid(vcpu, XTIER_INJECTION_PID);

	// Disable Exit
	XTIER_disable_hlt_exiting();

	// Disable Exception Exiting
	XTIER_disable_interrupt_exiting(vcpu);

	// Restore state
	restoreVMState(vcpu);

	// Reset Reinjection
	_XTIER_inject.reinject = 0;

	// Reset faults
	if(!_XTIER_inject.injection_fault)
		_injection_faults = 0;

	// Set Mode
	if(!_XTIER_inject.event_based)
	{
		_XTIER.mode &= ~((u64)XTIER_CODE_INJECTION);
	}
	else
	{
		// Reenable hook if the was no injection fault
		if(!_XTIER_inject.injection_fault)
			XTIER_inject_enable_hook(vcpu, &_XTIER_inject_current_injection);
	}

	// Get Removal Time
	XTIER_inject_end_time_measurement(&_starttime, &_XTIER_performance.total_module_unload_time);

	// Pause execution ?
	if(_XTIER_inject.exit_after_injection &&
	   !_XTIER_inject_current_injection.auto_inject &&
	   !_XTIER_inject_current_injection.time_inject &&
	   !_XTIER_inject_current_injection.event_based)
	{
		PRINT_DEBUG("Exit after injection is set! Returning to userspace...\n");
		vcpu->run->exit_reason = XTIER_EXIT_REASON_INJECT_FINISHED;
		return 0;
	}
	else
	{
		//vcpu->run->exit_reason = XTIER_EXIT_REASON_INJECT_FINISHED;
		//return 0;
		PRINT_DEBUG("Exit after injection is _NOT_ set! Resuming...\n");
		return 1;
	}
}

rp_frame *
frame_read (char *str, rp_screen *screen)
{
    Window w = 0L;
    rp_window *win;
    rp_frame *f;
    char *tmp, *d;
    int s_width = -1;
    int s_height = -1;

    /* Create a blank frame. */
    f = xmalloc (sizeof (rp_frame));
    init_frame(f);

    PRINT_DEBUG(("parsing '%s'\n", str));

    d = xstrdup(str);
    tmp = strtok_ws (d);

    /* Verify it starts with '(frame ' */
    if (strcmp(tmp, "(frame"))
    {
        PRINT_DEBUG(("Doesn't start with '(frame '\n"));
        free (d);
        free (f);
        return NULL;
    }
    /* NOTE: there is no check to make sure each field was filled in. */
    tmp = strtok_ws(NULL);
    while (tmp)
    {
        if (!strcmp(tmp, ":number"))
            read_slot(f->number);
        else if (!strcmp(tmp, ":x"))
            read_slot(f->x);
        else if (!strcmp(tmp, ":y"))
            read_slot(f->y);
        else if (!strcmp(tmp, ":width"))
            read_slot(f->width);
        else if (!strcmp(tmp, ":height"))
            read_slot(f->height);
        else if (!strcmp(tmp, ":screenw"))
            read_slot(s_width);
        else if (!strcmp(tmp, ":screenh"))
            read_slot(s_height);
        else if (!strcmp(tmp, ":window"))
            read_slot(w);
        else if (!strcmp(tmp, ":last-access"))
            read_slot(f->last_access);
        else if (!strcmp(tmp, ":dedicated")) {
            /* f->dedicated is unsigned, so read into local variable. */
            long dedicated;

            read_slot(dedicated);
            if (dedicated <= 0)
                f->dedicated = 0;
            else
                f->dedicated = 1;
        }
        else if (!strcmp(tmp, ")"))
            break;
        else
            PRINT_ERROR(("Unknown slot %s\n", tmp));
        /* Read the next token. */
        tmp = strtok_ws(NULL);
    }
    if (tmp)
        PRINT_ERROR(("Frame has trailing garbage\n"));
    free (d);

    /* adjust x, y, width and height to a possible screen size change */
    if (s_width > 0)
    {
        f->x = (f->x*screen->width)/s_width;
        f->width = (f->width*screen->width)/s_width;
    }
    if (s_height > 0)
    {
        f->y = (f->y*screen->height)/s_height;
        f->height = (f->height*screen->height)/s_height;
    }

    /* Perform some integrity checks on what we got and fix any
       problems. */
    if (f->number <= 0)
        f->number = 0;
    if (f->x <= 0)
        f->x = 0;
    if (f->y <= 0)
        f->y = 0;
    if (f->width <= defaults.window_border_width*2)
        f->width = defaults.window_border_width*2 + 1;
    if (f->height <= defaults.window_border_width*2)
        f->height = defaults.window_border_width*2 + 1;
    if (f->last_access < 0)
        f->last_access = 0;

    /* Find the window with the X11 window ID. */
    win = find_window_in_list (w, &rp_mapped_window);
    if (win)
//.........这里部分代码省略.........

void core_tests(void) {
	int i = 0;
	while (1) {
		PRINT_IMPORTANT("waiting...");
		//sleep(10);
		//char recv_data[4000];
		//gets(recv_data);
		fgetc(stdin); //wait until user enters
		PRINT_IMPORTANT("active");

		i++;
		if (i == 1) {
			metadata *meta = (metadata *) secure_malloc(sizeof(metadata));
			metadata_create(meta);

			uint32_t host_ip = IP4_ADR_P2H(192,168,1,8);
			uint32_t host_port = 55454;
			uint32_t dst_ip = IP4_ADR_P2H(192,168,1,3);
			uint32_t dst_port = 44444;
			uint32_t ttl = 64;
			uint32_t tos = 64;

			secure_metadata_writeToElement(meta, "send_src_ip", &host_ip, META_TYPE_INT32);
			secure_metadata_writeToElement(meta, "send_src_port", &host_port, META_TYPE_INT32);
			secure_metadata_writeToElement(meta, "send_dst_ip", &dst_ip, META_TYPE_INT32);
			secure_metadata_writeToElement(meta, "send_dst_port", &dst_port, META_TYPE_INT32);
			secure_metadata_writeToElement(meta, "send_ttl", &ttl, META_TYPE_INT32);
			secure_metadata_writeToElement(meta, "send_tos", &tos, META_TYPE_INT32);

			uint32_t src_index = 1;
			uint32_t dst_index = 2;
			struct finsFrame *ff = (struct finsFrame *) secure_malloc(sizeof(struct finsFrame));
			ff->dataOrCtrl = FF_DATA;
			ff->destinationID = dst_index;
			ff->metaData = meta;

			ff->dataFrame.directionFlag = DIR_UP;
			ff->dataFrame.pduLength = 10;
			ff->dataFrame.pdu = (uint8_t *) secure_malloc(10);

			PRINT_IMPORTANT("sending: ff=%p, meta=%p, src='%s' to dst='%s'", ff, meta, overall->modules[src_index]->name, overall->modules[dst_index]->name);
			module_to_switch(overall->modules[src_index], ff);
		}

		if (0) {
			PRINT_DEBUG("Sending ARP req");

			metadata *meta_req = (metadata *) secure_malloc(sizeof(metadata));
			metadata_create(meta_req);

			uint32_t dst_ip = IP4_ADR_P2H(192, 168, 1, 1);
			//uint32_t dst_ip = IP4_ADR_P2H(172, 31, 54, 169);
			uint32_t src_ip = IP4_ADR_P2H(192, 168, 1, 20);
			//uint32_t src_ip = IP4_ADR_P2H(172, 31, 50, 160);

			secure_metadata_writeToElement(meta_req, "dst_ip", &dst_ip, META_TYPE_INT32);
			secure_metadata_writeToElement(meta_req, "src_ip", &src_ip, META_TYPE_INT32);

			struct finsFrame *ff_req = (struct finsFrame*) secure_malloc(sizeof(struct finsFrame));
			ff_req->dataOrCtrl = FF_CONTROL;
			ff_req->destinationID = 1; //arp
			ff_req->metaData = meta_req;

			ff_req->ctrlFrame.sender_id = 4; //ipv4
			ff_req->ctrlFrame.serial_num = gen_control_serial_num();
			ff_req->ctrlFrame.opcode = CTRL_EXEC;
			ff_req->ctrlFrame.param_id = 0; //EXEC_ARP_GET_ADDR;

			ff_req->ctrlFrame.data_len = 0;
			ff_req->ctrlFrame.data = NULL;

			PRINT_IMPORTANT("sending: ff=%p, meta=%p, src='%s' to dst='%s'", ff_req, meta_req, overall->modules[0]->name, overall->modules[1]->name);
			module_to_switch(overall->modules[0], ff_req);
		}

		if (i == 2) {
			PRINT_DEBUG("Sending data");

			metadata *meta_req = (metadata *) secure_malloc(sizeof(metadata));
			metadata_create(meta_req);

			uint32_t ether_type = 0x0800; //ipv4
			int32_t if_index = 3; //wlan0
			uint32_t src_ip = IP4_ADR_P2H(192, 168, 1, 5); //wlan0
			uint32_t dst_ip = IP4_ADR_P2H(192, 168, 1, 1); //gw

			uint32_t src_index = 2;
			uint32_t dst_index = 1;
			secure_metadata_writeToElement(meta_req, "send_ether_type", &ether_type, META_TYPE_INT32);
			secure_metadata_writeToElement(meta_req, "send_if_index", &if_index, META_TYPE_INT32);
			secure_metadata_writeToElement(meta_req, "send_src_ipv4", &src_ip, META_TYPE_INT32);
			secure_metadata_writeToElement(meta_req, "send_dst_ipv4", &dst_ip, META_TYPE_INT32);

			struct finsFrame *ff = (struct finsFrame*) secure_malloc(sizeof(struct finsFrame));
			ff->dataOrCtrl = FF_DATA;
			ff->destinationID = dst_index; //arp
			ff->metaData = meta_req;

			ff->dataFrame.directionFlag = DIR_DOWN;
			ff->dataFrame.pduLength = 100;
//.........这里部分代码省略.........

// Request a checkpoint of the local process
// The return value is
// - negative in case of error
// - zero when successfully resuming after the checkpoint
// - positive when restarting from the checkpoint
static int request_checkpoint( const char* filename ) 
{
    cr_checkpoint_args_t cr_file_args;
    cr_checkpoint_handle_t cr_handle;
    int cr_fd = -1;
    int return_code = 0;

    // Check current state
    CR_state_lock();
    if ( cr_state != CR_READY ) {
        switch( cr_state ) {
            case CR_REQUEST_CHECKPOINT:
            case CR_CHECKPOINT:
            {
                PRINT_ERROR("Error: Already checkpointing... (cr_state=%d)\n", cr_state);
                return_code = -10;
                break;
            }
            default:
            {
                PRINT_ERROR("Error: Not ready to checkpoint... (cr_state=%d)\n", cr_state);
                return_code = -11;
                break;
            }
        }
        CR_state_unlock();
        goto error;
    } else {
        // All is ok, proceed to checkpoint request
        CR_state_transition_nolock( CR_REQUEST_CHECKPOINT );
    }
    CR_state_unlock();


    cr_fd = open(filename, O_CREAT | O_WRONLY | O_TRUNC, 0600);
    if ( cr_fd < 0 ) {
        PRINT_ERROR_ERRNO("Failed to open checkpoint file '%s'", errno, filename);
        return_code = -1;
        goto error;
    }

    int ret = cr_initialize_checkpoint_args_t(&cr_file_args);
    if (ret < 0) {
        PRINT_ERROR("BLCR call cr_initialize_checkpoint_args_t() failed\n");
        return_code = -2;
        goto error;
    }

    cr_file_args.cr_scope = CR_SCOPE_PROC;
    cr_file_args.cr_target = getpid();
    cr_file_args.cr_fd = cr_fd;
    cr_file_args.cr_signal = 0;
    cr_file_args.cr_timeout = 0;
    cr_file_args.cr_flags &= ~CR_CHKPT_DUMP_ALL;    // Save None

    // Request a checkpoint
    PRINT_DEBUG( DEBUG_FT_verbose, "cr_request_checkpoint() with file '%s'\n", filename );
    ret = cr_request_checkpoint(&cr_file_args, &cr_handle);
    PRINT_DEBUG( DEBUG_FT_verbose>1, "cr_request_checkpoint() returned %d\n", ret );
    if (ret < 0) {
        PRINT_ERROR("BLCR call cr_request_checkpoint() failed with error %d: %s\n", errno, cr_strerror(errno));
        return_code = -3;
        goto error;
    }

    // Wait for the end of the checkpoint, and retry while interrupted
    PRINT_DEBUG( DEBUG_FT_verbose, "cr_poll_checkpoint()\n" );
    do {
        ret = cr_poll_checkpoint(&cr_handle, NULL);
    } while (ret == CR_POLL_CHKPT_ERR_PRE && errno == EINTR);
    PRINT_DEBUG( DEBUG_FT_verbose>1, "cr_poll_checkpoint() returned %d\n", ret );

    // Check the result of the checkpoint
    if (ret == CR_POLL_CHKPT_ERR_POST && errno == CR_ERESTARTED) { 
        // We are restarting, ignore this error code

        // The checkpoint file is not opened at restart
        cr_fd = -1;

        // Positive value means restart
        return_code = 1;
        return return_code;
    } else if (ret < 0) {
        // Checkpoint failed
        PRINT_ERROR("BLCR call cr_poll_checkpoint() failed with error %d: %s\n", errno, cr_strerror(errno));

        // Negative value for failure
        return_code = -4;
        goto error;
    } else if (ret == 0) {
        // 0 means that the checkpoint is in progress
        // It should never happen because we don't specify any timeout when calling cr_poll_checkpoint()
        ASSERT_MSG( 0==1, "Internal error\n");
    }

//.........这里部分代码省略.........

struct ip4_routing_table * IP4_get_routing_table()
{
	int nlmsg_len;
	struct nlmsghdr* msg;
	char receive_buffer[IP4_NETLINK_BUFF_SIZE];
	char * receive_ptr;
	unsigned int sock;
	struct ip4_route_request route_req;
	struct ip4_routing_table * routing_table;
	struct ip4_routing_table * current_table_entry;

	unsigned int pid = (uint32_t) getpid();
	unsigned int seq = (uint32_t) getppid();

	if ((sock = socket(PF_NETLINK, SOCK_RAW, NETLINK_ROUTE)) == -1)
	{
		PRINT_DEBUG("couldn't open NETLINK_ROUTE socket");
		return NULL;
	}

	/* prepare netlink message header*/
	route_req.msg.nlmsg_len = NLMSG_LENGTH(sizeof(struct rtmsg));
	route_req.msg.nlmsg_type = RTM_GETROUTE;
	route_req.msg.nlmsg_flags = NLM_F_REQUEST | NLM_F_DUMP;
	route_req.msg.nlmsg_seq = seq;
	route_req.msg.nlmsg_pid = pid;

	route_req.rt.rtm_family = AF_INET;
	route_req.rt.rtm_dst_len = IP4_ALEN * 8;// must be supplied in bits
	route_req.rt.rtm_src_len = 0;
	route_req.rt.rtm_table = RT_TABLE_MAIN;
	route_req.rt.rtm_protocol = RTPROT_UNSPEC;
	route_req.rt.rtm_scope = RT_SCOPE_UNIVERSE;
	route_req.rt.rtm_type = RTN_UNSPEC;
	route_req.rt.rtm_flags = 0;

	// write the message to our netlink socket
	int result = send(sock, &route_req, sizeof(route_req), 0);
	if (result < 0)
	{
		PRINT_ERROR("Routing table request send error.");
		return NULL;
	}

	memset(receive_buffer, 0, IP4_NETLINK_BUFF_SIZE);
	receive_ptr = receive_buffer;
	nlmsg_len = 0;
	while (1)
	{
		int msg_len = recv(sock, receive_ptr, IP4_NETLINK_BUFF_SIZE, 0);
		if (msg_len < 0)
		{
			PRINT_ERROR("recv() error.");
			return NULL; //ERROR
		}
		msg = (struct nlmsghdr *) receive_ptr;
		if (msg->nlmsg_type == NLMSG_DONE)
		{
			break;
		}
		for (; 0 != NLMSG_OK(msg, msg_len); msg = NLMSG_NEXT(msg, msg_len))
		{
			if (msg->nlmsg_seq == seq)
			{
				if (routing_table == NULL)
				{
					routing_table = current_table_entry = parse_nlmsg(msg);
				} else
				{
					current_table_entry->next_entry = parse_nlmsg(msg);
					if (current_table_entry->next_entry != NULL)
					{
						current_table_entry = current_table_entry->next_entry;
					}
				}
			}
			receive_ptr = receive_ptr + msg_len;
			nlmsg_len = nlmsg_len + msg_len;
		}
	}
	return routing_table;
}

/**
 * Funkcja pobiera certyfikat z formularza wczytanego do struktury xmlDocPtr i
 * zapisuje go do bufora generycznego.
 * \param document Sparsowany formularz XML.
 * \param certyfikat Wskaźnik na bufor generyczny. Pod wskazanym adresem zapisany
 * zostanie zdekodowany certyfikat. Potrzebna pamięc zostanie zaalokowana.
 *
 * \retval 0 Wszystko OK.
 * \retval -1 Nie można pobrać z formularza certyfikatu. Może podano zły formularz?
 * \retval -2 Nie można zdekodować certyfikatu.
 * */
long _getCertificateFromXML(xmlDocPtr *document, GenBuf_t ** certyfikat)
{
	long status			= 0;
	int si_temp			= 0;
	char *xpath 			= NULL;	/*ściezka xpath do certyfikatu*/
	char *node_value 		= NULL;	/*zawartość taga ze zbejzowanym certem*/
	size_t dlugosc_bufora=0;		/*długość rozbejzowanego certyfikatu*/

	/*sprawdzamy wywolanie*/
	if (document == NULL)
	{
		PRINT_DEBUG("Wrong argument 1\n");
		return ERR_arg+1;
	}

	if (certyfikat == NULL)
	{
		PRINT_DEBUG("Wrong argument 2\n");
		return ERR_arg+2;
	}

	if (*certyfikat != NULL)
	{
		PRINT_DEBUG("Wrong argument 2\n");
		return ERR_arg;
	}

	/*pobieramy certyfikat*/
#ifdef CASA_XADES
	asprintf(&xpath, "//ds:X509Certificate");
#else
	asprintf(&xpath, "//ds:Signature/ds:KeyInfo/ds:X509Data/ds:X509Certificate");
#endif
	status = bmdxml_get_node_value_by_xpath (*document, (const char *) xpath, &node_value);
	if (status < 0)
	{
		PRINT_DEBUG("Unable to get certificate from the form!\n");
		return -1;
	}

	/*dodajemy go do genbufa*/
	(*certyfikat) = (GenBuf_t*) malloc (sizeof(GenBuf_t));
	(*certyfikat)->buf = (char *)spc_base64_decode((unsigned char *)node_value, &dlugosc_bufora, 0, &si_temp);
	if (si_temp != 0)
	{
		PRINT_DEBUG("Error while decoding certificate!\n");
		return -2;
	}
	if (dlugosc_bufora <= 5)
	{
		PRINT_DEBUG("Decoded buffer has %li length!\n", (long)dlugosc_bufora);
		return -2;
	}
	(*certyfikat)->size = (long)dlugosc_bufora;

	/*sprzatamy*/
	free(xpath);
	free(node_value);

	return 0;
}

/**
 * Funkcja porównuje numery seryjne z certyfikatu i z formularza
 * */
long _compareSerials(char **cert, char **form)
{
	BIGNUM *certGigant	= NULL;	/*serial z certyfikatu*/
	BIGNUM *formGigant 	= NULL;	/*serial z formularza*/
	BN_CTX *ctx				= NULL;	/*kontekst dla numerków*/
	char *castrate			= NULL;	/*serial cert. pozbawiony spacji*/
	long equal				= -13;
	long i, j;
	long len		= 0;		/*długość numeru seryjnego*/

	/*wywolanie funkcji*/
	if (cert == NULL)
	{
		PRINT_DEBUG("Wrong argument 1\n");
		return ERR_arg+1;
	}

	if (*cert == NULL)
	{
		PRINT_DEBUG("Wrong argument 1\n");
		return ERR_arg+1;
	}

	if (form == NULL)
	{
		PRINT_DEBUG("Wrong argument 2\n");
		return ERR_arg+2;
	}

	if (*form == NULL)
	{
		PRINT_DEBUG("Wrong argument 2\n");
		return ERR_arg+2;
	}

	/*kastracja cert*/
	len = (long)strlen(*cert);
	castrate = (char*) malloc (sizeof(char) * len);
	if (castrate == NULL)
	{
		PRINT_ERROR("No memory!\n");
		return -13;
	}
	for (i = 0, j = 0; i < len; i++)
	{
		if ((*cert)[i] != ' ')
		{
			castrate[j] = (*cert)[i];
			j++;
		}
	}
	castrate[j] = '\0';

	/*inicjaliacja numerków*/
	certGigant = BN_new();
	formGigant = BN_new();

	/*ładujemy numerki do BN*/
	BN_hex2bn(&certGigant, castrate);
	BN_dec2bn(&formGigant, *form);

	/*porównujemy seriale*/
	equal = BN_cmp(certGigant, formGigant);

	/*inicjalizujemy kontekst do BN*/
	ctx = BN_CTX_new();

	/*sprzatamy*/
	free(castrate);
	BN_CTX_free(ctx);
	BN_free(certGigant);
	BN_free(formGigant);
	return equal;
}

/**
 * Funkcja pobiera numer seryjny i wystawcę certyfikatu z formularza.
 * \param document Wskaźnik na sparsowany dokument xml.
 * \param FormSerialNumber Wskaźnik na numer seryjny. String pod ten numer
 * zostanie zaalokowany, należy go potem zwolnić.
 * \param FormIssuerName Wskaźnik na nazwę wystawcy. String pod tę nazwę
 * zostanie zaalokowany, nalezy go potem zwolnić.
 *
 * \retval 0 Wszystko OK.
 * \retval -1 Nie można pobrać numeru seryjnego z formularza.
 * \retval -2 Nie można pobrać imienia wystawcy z formularza.
 * */
long _getInfoFromXML(const xmlDocPtr *document, char **FormSerialNumber, char **FormIssuerName)
{
	long status;
	char *xpath 			= NULL;	/*ściezka xpath do certyfikatu*/
	char *node_value 		= NULL;	/*zawartość taga ze zbejzowanym certem*/

	if (document == NULL)
	{
		PRINT_DEBUG("Wrong argument 1\n");
		return ERR_arg+1;
	}

	if (FormSerialNumber == NULL)
	{
		PRINT_DEBUG("Wrong argument 2\n");
		return ERR_arg+2;
	}

	if (*FormSerialNumber != NULL)
	{
		PRINT_DEBUG("Wrong argument 2\n");
		return ERR_arg+2;
	}

	if (FormIssuerName == NULL)
	{
		PRINT_DEBUG("Wrong argument 3\n");
		return ERR_arg+3;
	}

	if (*FormIssuerName != NULL)
	{
		PRINT_DEBUG("Wrong argument 3\n");
		return ERR_arg+3;
	}

	/*pobieramy serial*/
#ifdef CASA_XADES
	asprintf(&xpath, "//X509SerialNumber");
#else
	asprintf(&xpath, "//ds:Signature/ds:Object/xades:QualifyingProperties/xades:SignedProperties/xades:SignedSignatureProperties/xades:SigningCertificate/xades:Cert/xades:IssuerSerial/ds:X509SerialNumber");
#endif
	status = bmdxml_get_node_value_by_xpath (*document, xpath, &node_value);
	if (status < 0)
	{
		PRINT_DEBUG("Unable to get serial number from the form!\n");
		return -1;
	}
	asprintf(FormSerialNumber, "%s", node_value);
	free(node_value);
	node_value = NULL;
	free(xpath);
	xpath = NULL;

	/*pobieramy IssuerName*/
#ifdef CASA_XADES
	asprintf(&xpath, "//X509IssuerName");
#else
	asprintf(&xpath, "//ds:Signature/ds:Object/xades:QualifyingProperties/xades:SignedProperties/xades:SignedSignatureProperties/xades:SigningCertificate/xades:Cert/xades:IssuerSerial/ds:X509IssuerName");
#endif
	status = bmdxml_get_node_value_by_xpath (*document, (const char *) xpath, &node_value);
	if (status < 0)
	{
		PRINT_DEBUG("Unable to get issuer name from the form!\n");
		return -2;
	}
	asprintf(FormIssuerName, "%s", node_value);
	free(node_value);
	node_value = NULL;
	free(xpath);
	xpath = NULL;

	return 0;
}

/**
 * Funkcja dokonuje weryfikacji poprawności złożonego podpisu pod formularzem.
 * \param[in] mngr Zainicjalizowany menedżer kluczy.
 * \param[in] buffer Formularz do zweryfikowania.
 * \param[in] buffer_len Długość weryfikowanego formularza.
 *
 * \retval 0 Podpis poprawny.
 * \retval 1 Podpis niepoprawny.
 * \retval -1 Nie można przeparsować dokumentu.
 * \retval -2 Nie znaleziono startowego węzła.
 * \retval -3 Nie dało się utworzyć kontekstu podpisu.
 * \retval -4 Nie dało rady zweryfikować podpisu.
 * */
static long verify_memory(xmlSecKeysMngrPtr mngr, const char* buffer, const long buffer_len) {
	xmlDocPtr doc = NULL;
	xmlNodePtr node = NULL;
	xmlSecDSigCtxPtr dsigCtx = NULL;
	long ret = -99;

	assert(mngr);

	/* load file */
	doc = xmlParseMemory(buffer,buffer_len);
	if ((doc == NULL) || (xmlDocGetRootElement(doc) == NULL)){
		PRINT_DEBUG("UNABLE TO PARSE DOCUMENT\n");
		ret = -1;
		goto done;
	}

	/* find start node */
	node = xmlSecFindNode(xmlDocGetRootElement(doc), xmlSecNodeSignature, xmlSecDSigNs);
	if(node == NULL) {
		PRINT_DEBUG("Start node %s not found\n",xmlSecNodeSignature);
		ret = -2;
		goto done;
	}

	/* create signature context */
	dsigCtx = xmlSecDSigCtxCreate(mngr);
	if(dsigCtx == NULL) {
		PRINT_DEBUG("Failed to create signature context\n");
		ret = -3;
		goto done;
	}

	/* Verify signature */
	if(xmlSecDSigCtxVerify(dsigCtx, node) < 0) {
		PRINT_DEBUG("Error: signature verify failed\n");
		ret = -4;
		goto done;
	}


	/* print verification result to stdout */
	if(dsigCtx->status == xmlSecDSigStatusSucceeded) {
		ret = 0;
		PRINT_DEBUG("XAdES: Signature is OK\n");
	} else {
		ret = 1;
		PRINT_DEBUG("XAdES: Signature is INVALID\n");
	}


done:
			/* cleanup */
			if(dsigCtx != NULL) {
	xmlSecDSigCtxDestroy(dsigCtx);
			}

			if(doc != NULL) {
				xmlFreeDoc(doc);
			}
			return(ret);
}

/**Funkcja sprawdza, czy certyfikat jest ważny w danym momencie oznaczonym przez timestamp.
 * \param certyfikat Certyfikat do sprawdzenia.
 * \param timestamp Znacznik czasu.
 * */
long isCertificateValid(GenBuf_t **certyfikat, GenBuf_t *timestamp)
{
	long status		= -1;
	char *validNB	= NULL;/*początek okresu wazności*/
	char *validNA	= NULL;/*koniec okresu ważności*/
	char *today		= NULL;/*dzisiejsza data*/
	GenBuf_t *PlainData = NULL;
	time_t tNB, tNA, tToday;

	if (certyfikat == NULL || *certyfikat == NULL)
	{
		PRINT_DEBUG("Wrong argument 1\n");
		return ERR_arg+1;
	}
	if (timestamp == NULL)
	{
		PRINT_DEBUG("Wrong argument 2\n");
		return ERR_arg+2;
	}

	/*poczatek waznosci*/
	status = GetValidityNBFromX509Certificate_time(*certyfikat, &tNB);
	if (status < 0)
	{
		PRINT_DEBUG("Couldn't get validity not before\n");
		return -1;
	}

	/*koniec waznosci*/
	status = GetValidityNAFromX509Certificate_time(*certyfikat, &tNA);
	if (status < 0)
	{
		PRINT_DEBUG("Couldn't get validity not after\n");
		return -2;
	}

	PRINT_VDEBUG("Getting current date from timestamp...\n");

	status = GetGenerationTimeFromTimestamp_time(timestamp, &tToday);
	if (status < 0)
	{
		PRINT_DEBUG("Couldn't get current date and time\n");
		return -3;
	}

	/*ewentualne wydruki*/
	/*PRINT_VDEBUG("Valid not before: %s\n", validNB);
	PRINT_VDEBUG("Valid not after:  %s\n", validNA);
	PRINT_VDEBUG("Today is:         %s\n", today);*/

	/*konieczne porównania*/
	/*tNB = _to_seconds(validNB);
	tNA = _to_seconds(validNA);
	tToday = _to_seconds(today);*/

	if (tToday < tNB)
	{
		PRINT_DEBUG("Certificate not valid before %s!\n", validNB);
		return -4;
	}
	else if (tToday > tNA)
	{
		PRINT_DEBUG("Certificate not valid after %s!\n", validNA);
		return -5;
	}

	free(validNB);
	free(validNA);
	free(today);
	free_gen_buf(&PlainData);

	return 0;
}

/**
 * Funkcja porównuje zawartość pól X509IssuerName, X509SerialNumber i CertDigest
 * w certyfikacie i w formularzu.
 * \retval -1 Nie można przeparsować dokumentu.
 * \retval -2 Nie można pobrać info certyfikatu z formularza.
 * \retval -3 Nie można pobrać info z formularza.
 * \retval -4 Niezgodne numery seryjne.
 * \retval -5 Niezgodne nazwy wystawców.
 * \retval -6 Nie można utworzyć skrótu z certyfikatu.
 * \retval -7 Nie można pobrać skrótu certyfikatu z formularza.
 * \retval -8 Niezgodne skróty z certyfikatów.
 * \retval -9 Certyfikat przeterminowany.
 * */
long verify_fields(const char *buffer, const long buffer_len, GenBuf_t *timestamp)
{
	xmlDocPtr document 		= NULL;
	char *CertSerialNumber	= NULL;	/*serial sczytany z certyfikatu*/
	char *FormSerialNumber	= NULL;	/*serial sczytany z formularza*/
	char *CertIssuerName		= NULL; 	/*wystawca sczytany z certyfikatu*/
	char *FormIssuerName		= NULL; 	/*wystawca sczytany z formularza*/
	char *CertDigest			= NULL;	/*digest w base64 do porównania*/
	char *FormDigest			= NULL;	/*digest z formularza*/
	GenBuf_t *certyfikat		= NULL;	/*genbuf z certyfikatem*/
	LIBBMDXADES_DIGEST_METHOD_t metoda;
	long status;

	/*kontrola poprawnosci parametrow*/
	if (buffer == NULL)
	{
		PRINT_DEBUG("Wrong argument 1\n");
		return ERR_arg+1;
	}

	if (buffer_len == 0)
	{
		PRINT_DEBUG("Wrong argument 2 (too short!)\n");
		return ERR_arg+2;
	}

	/* load file */
	document = xmlParseMemory(buffer,buffer_len);
	if ((document == NULL) || (xmlDocGetRootElement(document) == NULL))
	{
		PRINT_DEBUG("UNABLE TO PARSE DOCUMENT\n");
		return -1;
	}

	/* pobieramy certyfikat*/
	status = _getCertificateFromXML(&document, &certyfikat);
	if (status < 0)
	{
		PRINT_DEBUG("Error while getting certificate.\n");
		return -2;
	}

	/*pobieramy date waznosci*/
	status = isCertificateValid(&certyfikat, timestamp);
	if (status < 0)
	{
		PRINT_DEBUG("Error - certificate not valid!\n");
		return -9;
	}

	/* get Serial and IssuerName from certificate in the form*/
	status = _getInfoFromCertificate(&certyfikat, &CertSerialNumber, &CertIssuerName);
	if (status < 0)
	{
		PRINT_DEBUG("Error while getting info from X509 Certificate.\n");
		return -2;
	}
	PRINT_VDEBUG("Form signed by certificate issued by %s, serial: %s\n",
					 CertIssuerName, CertSerialNumber);

	/* get Serial and IssuerName from the form*/
	_getInfoFromXML(&document, &FormSerialNumber, &FormIssuerName);
	if (status < 0)
	{
		PRINT_DEBUG("Error while getting info from the form.\n");
		return -3;
	}

	/*porównujemy seriale*/
	status = _compareSerials(&CertSerialNumber, &FormSerialNumber);
	if (status != 0)
	{
		PRINT_DEBUG("Bad serial number.\n");
		return -4;
	}

	/*porównujemy wystawcow*/
	status = _compareIssuerNames(&CertIssuerName, &FormIssuerName);
	if (status != 0)
	{
		PRINT_DEBUG("Bad issuer name.\n");
		return -5;
	}

	/*sprawdzamy digest*/
	status = _getDigestAndMethod(&document, &FormDigest, &metoda);
	if (status < 0)
//.........这里部分代码省略.........

void *switch_loop(void *local) {
	struct fins_module *module = (struct fins_module *) local;
	PRINT_DEBUG("Entered: module=%p, index=%u, id=%u, name='%s'", module, module->index, module->id, module->name);
	struct switch_data *md = (struct switch_data *) module->data;

	uint32_t i;
	int ret;
	//int32_t val;
	struct finsFrame *ff;
	//uint8_t index;

	int counter = 0;

	while (module->state == FMS_RUNNING) {
		secure_sem_wait(module->event_sem); //TODO uncomment, for testing
		//secure_sem_wait(module->input_sem);
		secure_sem_wait(&md->overall->sem);
		for (i = 0; i < MAX_MODULES; i++) {
			if (md->overall->modules[i] != NULL) {
				//helgrind says is race condition, though there will always be FF when post to event_sem
				if (!IsEmpty(md->overall->modules[i]->output_queue)) { //added as optimization
					/*
					 //can possibly cause switch to be "behind"
					 ret = sem_getvalue(md->overall->modules[i]->output_sem, &val);
					 if (ret) {
					 PRINT_ERROR("sem get value prob: src module_index=%u, ret=%d", i, ret);
					 exit(-1);
					 } //*/

					//if (val != 0) {
					while ((ret = sem_wait(md->overall->modules[i]->output_sem)) && errno == EINTR)
						;
					if (ret != 0) {
						PRINT_ERROR("sem wait prob: src module_index=%u, ret=%d", i, ret);
						exit(-1);
					}
					ff = read_queue(md->overall->modules[i]->output_queue);
					sem_post(md->overall->modules[i]->output_sem);

					//if (ff != NULL) { //shouldn't occur
					counter++;

					//index = ff->destinationID;
					if (ff->destinationID < 0 || ff->destinationID > MAX_MODULES) {
						PRINT_ERROR("dropping ff: illegal destination: src module_index=%u, dst module_index=%u, ff=%p, meta=%p",
								i, ff->destinationID, ff, ff->metaData);
						//TODO if FCF set ret_val=0 & return? or free or just exit(-1)?
						freeFinsFrame(ff);
					} else { //if (i != id) //TODO add this?
						if (md->overall->modules[ff->destinationID] != NULL) {
							PRINT_DEBUG("Counter=%d, from='%s', to='%s', ff=%p, meta=%p",
									counter, md->overall->modules[i]->name, md->overall->modules[ff->destinationID]->name, ff, ff->metaData);
							//TODO decide if should drop all traffic to switch input queues, or use that as linking table requests
							if (ff->destinationID == module->index) {
								switch_process_ff(module, ff);
							} else {
								while ((ret = sem_wait(md->overall->modules[ff->destinationID]->input_sem)) && errno == EINTR)
									;
								if (ret != 0) {
									PRINT_ERROR("sem wait prob: dst index=%u, ff=%p, meta=%p, ret=%d", ff->destinationID, ff, ff->metaData, ret);
									exit(-1);
								}
								if (write_queue(ff, md->overall->modules[ff->destinationID]->input_queue)) {
									sem_post(md->overall->modules[ff->destinationID]->event_sem);
									sem_post(md->overall->modules[ff->destinationID]->input_sem);
								} else {
									sem_post(md->overall->modules[ff->destinationID]->input_sem);
									PRINT_ERROR("Write queue error: dst index=%u, ff=%p, meta=%p", ff->destinationID, ff, ff->metaData);
									freeFinsFrame(ff);
								}
							}
						} else {
							PRINT_ERROR("dropping ff: destination not registered: src index=%u, dst index=%u, ff=%p, meta=%p",
									i, ff->destinationID, ff, ff->metaData);
							print_finsFrame(ff);
							//TODO if FCF set ret_val=0 & return? or free or just exit(-1)?
							freeFinsFrame(ff);
						}
						//}
						//}
					}
				}
			}
		}
		//sem_post(module->input_sem);
		sem_post(&md->overall->sem);
	}

	PRINT_DEBUG("Exited: module=%p, index=%u, id=%u, name='%s'", module, module->index, module->id, module->name);
	return NULL;
}

//RTM's main function
//Gets information from RTM_IN pipe
//Is started as a thread in core.c
void rtm_init(pthread_attr_t *fins_pthread_attr) {

    PRINT_IMPORTANT("RTM has started");

    /*
     //added to include code from fins_daemon.sh -- mrd015 !!!!! //TODO move this to RTM module
     if (mkfifo(RTM_PIPE_IN, 0777) != 0) {
     if (errno == EEXIST) {
     PRINT_DEBUG("mkfifo(" RTM_PIPE_IN ", 0777) already exists.");
     } else {
     PRINT_ERROR("mkfifo(" RTM_PIPE_IN ", 0777) failed.");
     exit(-1);
     }
     }
     if (mkfifo(RTM_PIPE_OUT, 0777) != 0) {
     if (errno == EEXIST) {
     PRINT_DEBUG("mkfifo(" RTM_PIPE_OUT ", 0777) already exists.");
     } else {
     PRINT_ERROR("mkfifo(" RTM_PIPE_OUT ", 0777) failed.");
     exit(-1);
     }
     }
     */

    //int datalen;
    int numBytes;
    //int val_len;
    int temp_serial_cntr = 0;
    unsigned char* serialized_FCF = NULL;
    int length_serialized_FCF;

    //create a finsframe to be sent tover the queue
    struct finsFrame *fins_frame = (struct finsFrame *) secure_malloc(sizeof(struct finsFrame));
    fins_frame->dataOrCtrl = CONTROL;

    //opens the pipe from clicomm (or wherever)
    rtm_in_fd = open(RTM_PIPE_IN, O_RDWR);

    if (rtm_in_fd == -1) {
        PRINT_DEBUG("rtm_in_fd Pipe failure ");
        exit(EXIT_FAILURE);
    }

    fflush(stdout);

    while (1) {
        temp_serial_cntr++; //used as a temporary serial_number generator

        //READ FROM PIPE RTM_IN
        numBytes = 0;
        numBytes += read(rtm_in_fd, &length_serialized_FCF, sizeof(int)); //length of incoming serialized FCF
        numBytes += read(rtm_in_fd, serialized_FCF, length_serialized_FCF); //incoming serialized FCF

        fins_frame = unserializeCtrlFrame(serialized_FCF, length_serialized_FCF);

        //value, Assumption was made, notice the size
        PRINT_DEBUG("received data");
        numBytes = 0;

        //ERROR Message
        fflush(stdout);
        if (numBytes >= 0) {
            PRINT_DEBUG("numBytes written %d", numBytes);
        }

        //CHANGE SenderID and SerialNum
        fins_frame->ctrlFrame.senderID = RTM_ID;
        fins_frame->ctrlFrame.serial_num = temp_serial_cntr;

        //SEND TO QUEUE
        secure_sem_wait(&RTM_to_Switch_Qsem);
        write_queue(fins_frame, RTM_to_Switch_Queue);
        sem_post(&RTM_to_Switch_Qsem);
        PRINT_DEBUG("sent data ");

        //READ FROM QUEUE
        rtm_get_ff();
    }
}