fntable.c

嵌入式操作系统内核

			uint8_t num_sub = sos_read_header_byte(
					h->header, offsetof(mod_header_t, num_sub_func));
			uint8_t i;
			for(i = 0; i < num_sub; i++) {
				uint8_t sub_pid =
					sos_read_header_byte(
							h->header, offsetof(mod_header_t, funct[i].pid));
				if(sub_pid == pub_pid) {
					uint8_t sub_fid =
						sos_read_header_byte(
								h->header, offsetof(mod_header_t, funct[i].fid));
					func_cb_ptr pub_cb =
						fntable_get_prov_cb(funct, sub_fid, start, end);
					if(pub_cb != 0) {
						//! found function to be linked
						uint8_t proto_pub[4];
						uint8_t proto_sub[4];
						uint8_t j;
						func_cb_ptr *cb_in_ram =
							(func_cb_ptr*)(h->handler_state);
						for(j = 0; j < 4; j++) {
							proto_sub[j] =
								sos_read_header_byte(
										h->header,
										offsetof(mod_header_t, funct[i].proto[j]));
							proto_pub[j] = sos_read_header_byte(
									pub_cb, offsetof(func_cb_t, proto[j]));
						}
						if(link) {
							if(check_proto(proto_pub, proto_sub)) {
								cb_in_ram[i] = pub_cb;
							}
						} else {
							//! remove link
							cb_in_ram[i] =
								sos_get_header_member( h->header,
										offsetof(mod_header_t, funct[i]));
						}
					}
				} else if((sub_pid == RUNTIME_PID) && (link == false)) {
					func_cb_ptr *cb_in_ram = (func_cb_ptr *)(h->handler_state);
					func_cb_ptr sub_cb_in_ram = cb_in_ram[i];
					uint8_t k;
					//! compare to see whether there is a match
					for(k = start; k < end; k++) {
						if((funct + (k * sizeof(func_cb_t))) == sub_cb_in_ram) {
							//! found dynamic function
							cb_in_ram[i] =
								sos_get_header_member(h->header,
										offsetof(mod_header_t, funct[i]));
							//! send message back to subscriber
							post_short(h->pid, FNTABLE_PID, MSG_DFUNC_REMOVED,
									i, 0, 0);
							break;
						}
					}
				}
			}
			h = h->next;
		}
	}
}

int8_t fntable_remove_all(sos_module_t *m)
{
	uint8_t num_sub_func;
	uint8_t num_prov_func;
	uint8_t i;
	func_cb_ptr *cb_in_ram = (func_cb_ptr *)(m->handler_state);

	num_prov_func = sos_read_header_byte(m->header,
			offsetof(mod_header_t, num_prov_func));
	//! check whether there is any provided functions
	if(num_prov_func == 0) return SOS_OK;

	num_sub_func = sos_read_header_byte(
			m->header, offsetof(mod_header_t, num_sub_func));

	//! unlink all provided functions
	fntable_link_provided_functions(
			sos_get_header_member(m->header, offsetof(mod_header_t, funct)),
			num_sub_func,
			num_sub_func + num_prov_func, false);

	//! search all used functions and send a message to the providers as well
	//! send message to the function provider based on function pointer table.
	//! we can ignore RUNTIME_PID as provider cannot have it.
	for(i = 0; i < num_sub_func; i++) {
		func_cb_ptr pub_cb = cb_in_ram[i];
		uint8_t pub_pid = sos_read_header_byte(
				pub_cb, offsetof(func_cb_t, pid));
		if(pub_pid == m->pid || pub_pid == RUNTIME_PID) {
			//! if function pointer is pointing to self,
			//! we won't send remove message
			//! similarly if function pointer is pointing to RUNTIME_PID,
			//! this means that it is pointing to self
			continue;
		}
		post_short(pub_pid, FNTABLE_PID, MSG_DFUNC_REMOVED,
				m->pid, 0, 0);

	}
	return SOS_OK;
}


// Check that two function prototypes match
// NOTE: both prototypes are in read-only memory now
static bool check_proto(uint8_t *proto1, uint8_t *proto2)
{

	if ((proto1[0] == proto2[0]) &&
		(proto1[1] == proto2[1]) &&
		(proto1[2] == proto2[2]) &&
		(proto1[3] == proto2[3])) {
	   return true;
	}

	return false;
}

void error_v(func_cb_ptr p) 
{
	DEBUG("error_v is called\n");
}

int8_t error_8(func_cb_ptr p) 
{
	DEBUG("error_8 is called\n");
	return -1;
}	

int16_t error_16(func_cb_ptr p)
{
	DEBUG("error_16 is called\n");
	return -1;
}

int32_t error_32(func_cb_ptr p)
{
	DEBUG("error_32 is called\n");
	return -1;
}	

void* error_ptr(func_cb_ptr p)
{
	DEBUG("error_ptr is called\n");
	return NULL;
}

dummy_func ker_get_func_ptr(func_cb_ptr p, sos_pid_t *prev)
{
	if( prev != NULL ) {
		*prev = 
			ker_set_current_pid(sos_read_header_byte(p, offsetof(func_cb_t, pid)));
	}
	
	return (dummy_func)sos_read_header_ptr( p, offsetof(func_cb_t, ptr));
}

/**
 * Call to get the function pointer in the control block
 * It also sets the current pid to the function destination
 *
 */
dummy_func ker_sys_enter_func( func_cb_ptr p )
{
	HAS_CRITICAL_SECTION;

	ENTER_CRITICAL_SECTION();

	*pid_sp =
		ker_set_current_pid(sos_read_header_byte(p, offsetof(func_cb_t, pid)));

	pid_sp++;
	LEAVE_CRITICAL_SECTION();

	return (dummy_func)sos_read_header_ptr( p, offsetof(func_cb_t, ptr));
}                                                               

/**                                                             
 * Pop current_pid from the stack when func has finished execution
 */                                                             
void ker_sys_leave_func( void )                                 
{                                                               
	HAS_CRITICAL_SECTION;                                       

	ENTER_CRITICAL_SECTION();                                   
	pid_sp--;                                                   
	ker_set_current_pid( *pid_sp );                             
	LEAVE_CRITICAL_SECTION();                                   
}                    

int8_t ker_sys_fntable_subscribe( sos_pid_t pub_pid, uint8_t fid, uint8_t table_index ) 
{
	sos_pid_t my_id = ker_get_current_pid();

	return ker_fntable_subscribe( my_id, pub_pid, fid, table_index );
}

//! Error Stub Function
/*
   int8_t error_stub(func_cb_ptr proto_in, ...)
   {
   uint8_t j, num_args;
   va_list ap;
   int i;
   unsigned int ui;
   long l;
   unsigned long ul;
   double d;
   void* ptr;
  char proto[4];
  uint8_t itr;

  //! XXX
  DEBUG("received error_stub call\n");
  return -ENOSYS;
  va_start(ap, proto_in);
  for(itr = 0; itr < 4; itr++) {
	proto[itr] = sos_read_header_byte(
			proto_in, offsetof(func_cb_t, proto[itr]));
	DEBUG("proto[%d] = %c\n", itr, proto[itr]);
  }
  num_args = proto[3] - 47; //! 47 corresponds to character '0' in the ASCII table

  for (j = 1; j < num_args && j < 3; j++){
	switch (proto[j+1]){
	case 'c':
	case 's':
	  {
		// The above two would be promoted to a signed int of 16 bits
		i = va_arg(ap, int);
		break;
	  }
	case 'C':
	case 'S':
	  {
		ui = va_arg(ap, unsigned int);
		break;
	  }
	case 'l':
	  {
		l = va_arg(ap, long);
		break;
	  }
	case 'L':
	  {
		ul = va_arg(ap, unsigned long);
		break;
	  }

	case 'f':
	case 'o':
	  {
		d = va_arg(ap, double);
		break;
	  }
	case 'd': //! char*
	case 'D': //! unsigned char*
	case 'g': //! float*
	case 'm': //! long*
	case 'M': //! unsigned long*
	case 'p': //! double*
	case 't': //! short*
	case 'T': //! unsigned short*
	case 'w': //! void*
	case 'z': //! struct*
	  {
		// The pointer has the same size
		ptr = va_arg(ap, void*);
		break;
	  }
	case 'e': //! char* (dynamic)
	case 'E': //! unsigned char* (dynamic)
	case 'h': //! float* (dynamic)
	case 'n': //! long* (dynamic)
	case 'N': //! unsigned long (dynamic)
	case 'q': //! double* (dynamic)
	case 'u': //! short* (dynamic)
	case 'U': //! unsigned short* (dynamic)
	case 'x': //! void* (dynamic)
	case 'a': //! struct* (dynamic)
	  {
		// The pointer has the same size
		ptr = va_arg(ap, void*);
		// Now what ? Do we free the memory always ?
		ker_free(ptr);
		break;
	  }
	default:
	  break;
	}
  }
  return -ENOSYS;
}
*/