/* -*- 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 (ram@ee.ucla.edu) */#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 Functionsstatic 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) {