fntable.c

嵌入式操作系统内核

/* -*- Mode: C; tab-width:4 -*- */
/* ex: set ts=4 shiftwidth=4 softtabstop=4 cindent: */
/*
 * Copyright (c) 2003 The Regents of the University of California.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above
 *    copyright notice, this list of conditions and the following
 *    disclaimer in the documentation and/or other materials provided
 *    with the distribution.
 * 3. All advertising materials mentioning features or use of this
 *    software must display the following acknowledgement:
 *       This product includes software developed by Networked &
 *       Embedded Systems Lab at UCLA
 * 4. Neither the name of the University nor that of the Laboratory
 *    may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 */

/**
 * @brief    Function Pointer Table
 * @author   Ram Kumar ([e-mail])
 */

#include <sos_types.h>
#include <sos_sched.h>
#include <malloc.h>
#include <message.h>
#include <sos_module_types.h>
#include <sos_linker_conf.h>
#include <fntable_types.h>
#include <fntable.h>
#ifdef SOS_SFI
#include <sfi_jumptable.h>
#endif

/*
#ifndef SOS_DEBUG_FN_TABLE
#undef DEBUG
#define DEBUG(...)
#endif
*/
//----------------------------------------------------------------------------
// Typedefs
//----------------------------------------------------------------------------


//----------------------------------------------------------------------------
// Global Variables
//----------------------------------------------------------------------------


#ifndef QUALNET_PLATFORM
//! Local Functions
static bool check_proto(uint8_t *proto1, uint8_t *proto2);
static func_cb_ptr fntable_get_prov_cb(func_cb_ptr funct, uint8_t fid,
		uint8_t start, uint8_t end);
static func_cb_ptr fntable_real_subscribe(mod_header_ptr sub_h,
		sos_pid_t pub_pid, uint8_t fid, uint8_t table_index);
static void fntable_link_provided_functions(func_cb_ptr funct,
		uint8_t start, uint8_t end, bool link);
#endif


/**
 * @brief Initializes the function pointer list to NULL
 */
int8_t fntable_init()
{
    return SOS_OK;
}

/**
 * @brief subscribe to function pointer
 * @param sos_pid_t sub_pid Module requesting function. (User) *
 * @param sos_pid_t pub_pid Module implementing the requested function. (Provider)
 * @param uint8_t fid function id the module is going to subscribe
 * @param uint8_t table_index the index to the function record, starting zero, this is used for gettting function prototypes
 * @return errno
 *
 */
int8_t ker_fntable_subscribe(sos_pid_t sub_pid, sos_pid_t pub_pid, uint8_t fid, uint8_t table_index)
{
	sos_module_t *mod;
	mod_header_ptr sub_h;
	func_cb_ptr cb;
	func_cb_ptr *cb_in_ram;

	mod = ker_get_module(sub_pid);
	if(mod == NULL) {	
		ker_panic();
		return -EINVAL;
	}

	sub_h = mod->header;
	cb_in_ram = (func_cb_ptr*)(mod->handler_state);

	cb = fntable_real_subscribe(sub_h, pub_pid, fid, table_index);
	if(cb != 0) {
		cb_in_ram[table_index] = cb;
	} else {
		cb_in_ram[table_index] = sos_get_header_member(sub_h,
				offsetof(mod_header_t, funct[table_index]));
	}
	return SOS_OK;
}

void* ker_fntable_get_dfunc_addr(sos_pid_t pub_pid, uint8_t fid)
{
	sos_module_t *mod;
	uint8_t num_sub_func;
	uint8_t num_prov_func;
	mod_header_ptr pub_h;
	func_cb_ptr pub_cb = 0;
	
	mod = ker_get_module(pub_pid);
	if(mod == NULL) {	
		return NULL;
	}
	
	pub_h = mod->header;
	num_sub_func = sos_read_header_byte(pub_h,
			offsetof(mod_header_t, num_sub_func));
	num_prov_func = sos_read_header_byte(pub_h,
			offsetof(mod_header_t, num_prov_func));
			
	pub_cb = fntable_get_prov_cb(
			sos_get_header_member(pub_h, offsetof(mod_header_t, funct)),
			fid, num_sub_func, num_sub_func + num_prov_func);
	if(pub_cb == 0) {
		return NULL;
	}
	// TODO: typechecking
	
	return (void*)sos_read_header_ptr(pub_cb, offsetof(func_cb_t, ptr));
}

static func_cb_ptr fntable_real_subscribe(mod_header_ptr sub_h, sos_pid_t pub_pid, uint8_t fid, uint8_t table_index)
{
	mod_header_ptr pub_h;
	uint8_t proto_pub[4];
	uint8_t proto_sub[4];
	func_cb_ptr pub_cb = 0;
	uint8_t i = 0;
	uint8_t num_sub_func;
	uint8_t num_prov_func;
	sos_module_t *mod;

	mod = ker_get_module(pub_pid);
	if(mod == NULL) return 0;
	pub_h = mod->header;
	num_sub_func = sos_read_header_byte(pub_h,
			offsetof(mod_header_t, num_sub_func));
	num_prov_func = sos_read_header_byte(pub_h,
			offsetof(mod_header_t, num_prov_func));
	pub_cb = fntable_get_prov_cb(
			sos_get_header_member(pub_h, offsetof(mod_header_t, funct)),
			fid, num_sub_func, num_sub_func + num_prov_func);
	if(pub_cb == 0) {
		return 0;
	}
	for(i = 0; i < 4; i++) {
		proto_pub[i] = sos_read_header_byte(pub_cb,
				offsetof(func_cb_t, proto[i]));
		proto_sub[i] = sos_read_header_byte(sub_h,
				offsetof(mod_header_t, funct[table_index].proto[i]));
	}
	if(check_proto(proto_pub, proto_sub)) {
		return pub_cb;
	}
	return 0;
}

static func_cb_ptr fntable_get_prov_cb(func_cb_ptr funct, uint8_t fid, uint8_t start, uint8_t end)
{
	uint8_t i;

	for(i = start; i < end; i++) {
		if(sos_read_header_byte(funct,
					i * sizeof(func_cb_t) + offsetof(func_cb_t, fid))
			   	== fid) {
			return sos_get_header_member(funct,
					i * sizeof(func_cb_t));
		}
	}
	return 0;
}


/**
 * @brief link the module into correct address
 * NOTE: this rountine assumes the header is in RAM
 * fix offsetof(mod_header_t, module_handler)
 * fix offsetof(mod_header_t, funct) + (n * sizeof(func_cb_t)) + offsetof(func_cb_t, ptr) for n = [0 ... number_of_funcs) 
 */
static inline func_addr_t mod_header_size(uint8_t num_funcs)
{
	return offsetof(mod_header_t, funct) + 
		(num_funcs * sizeof(func_cb_t)) + offsetof(func_cb_t, ptr);
}


/**
 * @brief link the functions
 */
int8_t fntable_link(sos_module_t *m)
{
   uint8_t num_prov_func;
   uint8_t num_sub_func;
   
   fntable_link_subscribed_functions(m);
   num_prov_func = sos_read_header_byte(m->header,
                                        offsetof(mod_header_t, num_prov_func));
   num_sub_func = sos_read_header_byte(m->header,
                                       offsetof(mod_header_t, num_sub_func));
   if(num_prov_func > 0) {
      fntable_link_provided_functions(
         sos_get_header_member(m->header, offsetof(mod_header_t, funct)),
         num_sub_func,
         num_sub_func + num_prov_func, true);
   }
   return SOS_OK;
}


void fntable_link_subscribed_functions(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_sub_func = sos_read_header_byte(m->header,
                                       offsetof(mod_header_t, num_sub_func));
   num_prov_func = sos_read_header_byte(m->header,
                                        offsetof(mod_header_t, num_prov_func));
   
   for(i = 0; i < num_sub_func; i++) {
      uint8_t pub_pid = sos_read_header_byte(m->header,
                                             offsetof(mod_header_t, funct[i].pid));
      uint8_t pub_fid = sos_read_header_byte(m->header,
                                             offsetof(mod_header_t, funct[i].fid));
      func_cb_ptr pub_cb = 0;
      if(pub_pid == m->pid) {
         pub_cb = fntable_get_prov_cb(
            sos_get_header_member(m->header,
                                  offsetof(mod_header_t, funct)),
            pub_fid,
            num_sub_func, (num_sub_func + num_prov_func));
      } else if(pub_pid != RUNTIME_PID) {
         pub_cb = fntable_real_subscribe(m->header, pub_pid, pub_fid, i);
      }
      if(pub_cb != 0) {
			cb_in_ram[i] = pub_cb;
		} else {
			cb_in_ram[i] = sos_get_header_member(m->header,
					offsetof(mod_header_t, funct[i]));
		}
	}
}


/**
 * @brief given provided function table, search ALL OTHER modules to find the match
 *
 * PROFILE NEEDED
 * NOTE: it will be good idea to profile this...
 */
static void fntable_link_provided_functions(func_cb_ptr funct, uint8_t start, uint8_t end, bool link)
{
	sos_module_t **all_modules = sched_get_all_module();
	uint8_t bin_itr;
	uint8_t pub_pid = sos_read_header_byte(
			//funct[start].pid);
			funct, start * sizeof(func_cb_t) + offsetof(func_cb_t, pid));

	//! search both function users and dyanmic functions
	for(bin_itr = 0; bin_itr < SCHED_NUMBER_BINS; bin_itr++) {
		sos_module_t *h = all_modules[bin_itr];
		while(h != NULL) {