sched.c

嵌入式操作系统内核

  return SOS_OK;
}

/**
 * @brief de-register a task (module)
 * @param pid task id to be removed
 * Note that this function cannot be used inside interrupt handler
 */
int8_t ker_deregister_module(sos_pid_t pid)
{
  HAS_CRITICAL_SECTION;
  uint8_t bins = hash_pid(pid);
  sos_module_t *handle;
  sos_module_t *prev_handle = NULL;
  msg_handler_t handler;

  /**
   * Search the bins while save previous node
   * Once found the module, connect next module to previous one
   * put module back to freelist
   */
  handle = mod_bin[bins];
  while(handle != NULL) {
		if(handle->pid == pid) {
			break;
		} else {
			prev_handle = handle;
			handle = handle->next;
		}
	}
	if(handle == NULL) {
		// unable to find the module
		return -EINVAL;
	}
	handler = (msg_handler_t)sos_read_header_ptr(handle->header,
			offsetof(mod_header_t,
				module_handler));

	if(handler != NULL) {
		void *handler_state = handle->handler_state;
		Message msg;
		sos_pid_t prev_pid = curr_pid;

		curr_pid = handle->pid;
		msg.did = handle->pid;
		msg.sid = KER_SCHED_PID;
		msg.type = MSG_FINAL;
		msg.len = 0;
		msg.data = NULL;
		msg.flag = 0;
		// Ram - If the handler does not write to the message, all is fine
#ifdef SOS_SFI
		ker_cross_domain_call_mod_handler(handler_state, &msg, handler);
#else
		handler(handler_state, &msg);
#endif
		curr_pid = prev_pid;
	}

	// First remove handler from the list.
	// link the bin back
	ENTER_CRITICAL_SECTION();
	if(prev_handle == NULL) {
		mod_bin[bins] = handle->next;
	} else {
		prev_handle->next = handle->next;
	}
	LEAVE_CRITICAL_SECTION();

	// remove the thread pid allocation
	if(handle->pid >= SCHED_MIN_THREAD_PID) {
		uint8_t i = handle->pid - SCHED_MIN_THREAD_PID;
		pid_pool[i/8] &= ~(1 << (i % 8));
  }


  // remove system services
  timer_remove_all(pid);
  sensor_remove_all(pid);
  ker_timestamp_deregister(pid);
  fntable_remove_all(handle);

  // free up memory
  // NOTE: we can only free up memory at the last step
  // because fntable is using the state
  if((SOS_KER_STATIC_MODULE & (handle->flag)) == 0) {
		ker_free(handle);
  }
  mem_remove_all(pid);

  return 0;
}


#ifdef FAULT_TOLERANT_SOS
static int8_t ker_micro_reboot_module(sos_module_t* handle)
{
  sos_pid_t pid;
  uint8_t st_size;
  uint8_t micro_reboot_fail = 0;
  mod_header_ptr hdr;
  Message msg_init;
  msg_handler_t handler;

  pid = handle->pid;
  DEBUG("!!!-----> INITIATING MICRO REBOOT OF MODULE %d\n", pid);

  // Micro-reboot can fail if we are unable to pre-allocate requested timers
  if (timer_micro_reboot(handle) != SOS_OK){
	DEBUG("Timers...FAIL...\n");
	micro_reboot_fail = 1;
  }
  else
	DEBUG("Timers ...OK...\n");


  // Deal with any pending clients
  sensor_micro_reboot(pid);



  // Purge all state owned by the module
  mem_remove_all(pid);


  // Read the state size and allocate a separate memory block for it
  hdr = handle->header;
  st_size = sos_read_header_byte(hdr, offsetof(mod_header_t, state_size));
  if (st_size){
		//handle->handler_state = (uint8_t*)ker_malloc(st_size, pid);
		handle->handler_state = (uint8_t*)malloc_longterm(st_size, pid);
	// This check is not required as we just freed all memory
		if (handle->handler_state == NULL)
			micro_reboot_fail = 1;
	}
	else
		handle->handler_state = NULL;

	// Micro-reboot the dynamic linking
	fntable_link_subscribed_functions(handle);

	if ((((handle->flag) & SOS_KER_STATIC_MODULE) == 0) &&
	  (handle->handler_state != NULL))
	mem_block_set_checksum(handle->handler_state);

  // Send INIT message to the recovered module
  handler = (msg_handler_t)sos_read_header_ptr(handle->header,
											   offsetof(mod_header_t,
														module_handler));
  msg_init.did = handle->pid;
  msg_init.sid = KER_SCHED_PID;
  msg_init.type = MSG_INIT;
  msg_init.len = 0;
  msg_init.data = NULL;
  msg_init.flag = 0;
  handler(handle->handler_state, &msg_init);

  return SOS_OK;
}
#endif//FAULT_TOLERANT_SOS

/**
 * @brief dispatch short message
 * This is used by the callback that was register by interrupt handler
 */
void sched_dispatch_short_message(sos_pid_t dst, sos_pid_t src,
		uint8_t type, uint8_t byte,
		uint16_t word, uint16_t flag)
{
	sos_module_t *handle;
	msg_handler_t handler;
	void *handler_state;

	MsgParam *p;

	handle = ker_get_module(dst);
	if( handle == NULL ) { return; }

	handler = (msg_handler_t)sos_read_header_ptr(handle->header,
			offsetof(mod_header_t,
				module_handler));
	handler_state = handle->handler_state;

	p = (MsgParam*)(short_msg.data);	

	short_msg.did = dst;
	short_msg.sid = src;
	short_msg.type = type;
	p->byte = byte;
	p->word = word;
	short_msg.flag = flag;

	/*
	 * Update current pid
	 */
	curr_pid = dst;
	ker_log( SOS_LOG_HANDLE_MSG, curr_pid, type );
#ifdef SOS_SFI
		ker_cross_domain_call_mod_handler(handler_state, &short_msg, handler);
#else
		handler(handler_state, &short_msg);
#endif
		ker_log( SOS_LOG_HANDLE_MSG_END, curr_pid, type );
}


/**
 * @brief    real dispatch function
 * We have to handle MSG_PKT_SENDDONE specially
 * In SENDDONE message, msg->data is pointing to the message just sent.
 */

static void do_dispatch()
{
  Message *e;                                // Current message being dispatched
  sos_module_t *handle;                      // Pointer to the control block of the destination module
  Message *inner_msg = NULL;                 // Message sent as a payload in MSG_PKT_SENDDONE
  sos_pid_t senddone_dst_pid = NULL_PID;     // Destination module ID for the MSG_PKT_SENDDONE
  uint8_t senddone_flag = SOS_MSG_SEND_FAIL; // Status information for the MSG_PKT_SENDDONE

  SOS_MEASUREMENT_DEQUEUE_START();
  e = mq_dequeue(&schedpq);
  SOS_MEASUREMENT_DEQUEUE_END();
  handle = ker_get_module(e->did);
  // Destination module might muck around with the
  // type field. So we check type before dispatch
	if(e->type == MSG_PKT_SENDDONE) {
		inner_msg = (Message*)(e->data);
	}
	// Check for reliable message delivery
	if(flag_msg_reliable(e->flag)) {
		senddone_dst_pid = e->sid;	
	}
	// Deliver message to the monitor
	// Ram - Modules might access kernel domain here
	monitor_deliver_incoming_msg_to_monitor(e);

	if(handle != NULL) {
		if(sched_message_filtered(handle, e) == false) {
			int8_t ret;
			msg_handler_t handler;
			void *handler_state;

			DEBUG("###################################################################\n");
			DEBUG("MESSAGE FROM %d TO %d OF TYPE %d\n", e->sid, e->did, e->type);
			DEBUG("###################################################################\n");


			// Get the function pointer to the message handler
			handler = (msg_handler_t)sos_read_header_ptr(handle->header,
					offsetof(mod_header_t,
						module_handler));
			// Get the pointer to the module state
			handler_state = handle->handler_state;
			// Change ownership if the release flag is set
			// Ram - How to deal with memory blocks that are not released ?
			if(flag_msg_release(e->flag)){
				ker_change_own(e->data, e->did);
			}

#ifdef FAULT_TOLERANT_SOS		
			// Check for memory corruption in dynamic modules
			if (((handle->flag) & SOS_KER_STATIC_MODULE) == 0){
				int8_t mem_verify_status;
				// Check the integrity of all memory owned by the module
				mem_verify_status = mod_mem_verify_checksum(handle);
				// Check the integrity of the message payload being delivered to the module
				if (!flag_msg_release(e->flag)){
					if (mem_check_module_domain(e->data) == true){
						DEBUG("Checking CRC of the incoming message payload\n");
						if (mem_block_verify_checksum(e->data) != SOS_OK){
							// We cannot deliver corrupt message
							DEBUG("Message payload is corrupted\n");
							// We simply skip handling of this message for the time being
							ret = -EINVAL;
							goto skip_handler;
						}
					}
				}
				DEBUG("Message payload is verified corrent\n");
				// Micro-reboot the module if the memory integrity check failed
				if (mem_verify_status != SOS_OK){
					// XXX - Not checking if micro-reboot was a success or a failure
					LED_DBG(LED_RED_ON);
					ker_micro_reboot_module(handle);
					LED_DBG(LED_GREEN_ON);
					ret = -EINVAL;
					goto skip_handler;
				}
			}
			DEBUG("Memory integrity for the destination module is verified\n");
			// Everything is fine !! Ready to rock n roll
#endif

	  DEBUG("RUNNING HANDLER OF MODULE %d \n", handle->pid);
		curr_pid = handle->pid;
		ker_log( SOS_LOG_HANDLE_MSG, curr_pid, e->type );
#ifdef SOS_SFI
		ret = ker_cross_domain_call_mod_handler(handler_state, e, handler);
#else
	  ret = handler(handler_state, e);
#endif
		ker_log( SOS_LOG_HANDLE_MSG_END, curr_pid, e->type );
	  DEBUG("FINISHED HANDLER OF MODULE %d \n", handle->pid);

#ifdef FAULT_TOLERANT_SOS
	  //! Set new checksum value for dynamic modules
	  if (((handle->flag) & SOS_KER_STATIC_MODULE) == 0)
		mod_mem_set_checksum(handle);
skip_handler:
#endif
		if (ret == SOS_OK) senddone_flag = 0;
		}
	} else {
#if 0
		// TODO...
		//! take care MSG_FETCHER_DONE
		//! need to make sure that fetcher has completed its request
		if(e->type == MSG_FETCHER_DONE) {
			fetcher_state_t *fstat = (fetcher_state_t*)e->data;
			fetcher_commit(fstat, false);
		}
#endif
		//XXX no error notification for now.
		DEBUG("Scheduler: Unable to find module\n");
	}
	if(inner_msg != NULL) {
		//! this is SENDDONE message
		msg_dispose(inner_msg);
		msg_dispose(e);
	} else {
		if(senddone_dst_pid != NULL_PID) {
			if(post_long(senddone_dst_pid,
						KER_SCHED_PID,
						MSG_PKT_SENDDONE,
						sizeof(Message), e,
						senddone_flag) < 0) {
				msg_dispose(e);
			}
		} else {
			//! return message back to the pool
			msg_dispose(e);
		}
	}
}

/**
 * @brief query the existence of task
 * @param pid module id
 * @return 0 for exist, -EINVAL otherwise
 *
 */
int8_t ker_query_task(uint8_t pid)
{
  sos_module_t *handle = ker_get_module(pid);
  if(handle == NULL){
	return -EINVAL;
  }
  return 0;
}


void sched_msg_alloc(Message *m)
{
  if(flag_msg_release(m->flag)){
#ifdef FAULT_TOLERANT_SOS
	// Set the checksum before ownership transfer
	// Checksum will be checked only if the
	// destination module is dynamic
	if (m->did >= APP_MOD_MIN_PID){
	  DEBUG("Setting the CRC prior to posting message\n");
	  mem_block_set_checksum(m->data);
	}
#endif
	ker_change_own(m->data, KER_SCHED_PID);
  }	
	DEBUG("sched_msg_alloc\n");
  mq_enqueue(&schedpq, m);	
}

void sched_msg_remove(Message *m)
{
  Message *tmp;
  while(1) {
	tmp = mq_get(&schedpq, m);
	if(tmp) {
	  msg_dispose(tmp);
	} else {
	  break;
	}
  }
}


/**
 * @brief Message filtering rules interface
 * @param rules_in  new rule
 */
int8_t ker_msg_change_rules(sos_pid_t sid, uint8_t rules_in)
{
  sos_module_t *handle = ker_get_module(sid);
  if(handle == NULL) return -EINVAL;
  //! keep kernel state
  handle->flag &= 0x0F;

  handle->flag |= (rules_in & 0xF0);
  return 0;
}

/**
 * @brief get message rules
 */
int8_t sched_get_msg_rule(sos_pid_t pid, sos_ker_flag_t *rules)
{
  sos_module_t *handle = ker_get_module(pid);
  if(handle == NULL) return -EINVAL;
  *rules = handle->flag & 0xF0;
  return 0;
}

/**
 * @brief post crash check up
 */
#if 0
void sched_post_crash_checkup()
{
  sos_pid_t failed_pid;
  mod_handle_t h;

  while((failed_pid = mem_check_memory()) != NULL_PID) {
	// we probably need to report failure here
	h = sched_get_mod_handle(failed_pid);
	if(h >= 0) {
	  module_list[h].flag |= SOS_KER_MEM_FAILED;

	}
  }
  // Other crash testing goes here
}
#endif

#if 0
static void sched_send_crash_report()
{
  if(crash_report != NULL) {
	post_net(KER_SCHED_PID, KER_SCHED_PID, MSG_SCHED_CRASH_REPORT,
			 crash_report_len, crash_report, SOS_MSG_RELEASE, BCAST_ADDRESS);
  }
}
#endif

/**
 * @brief Message filter.
 * Check for promiscuous mode request in the destination module
 * @return true for message shoud be filtered out, false for message is valid
 */
static inline bool sched_message_filtered(sos_module_t *h, Message *m)
{
  sos_ker_flag_t rules;
  // check if it is from network
  if(flag_msg_from_network(m->flag) == 0) return false;
  rules = h->flag;

  // check for promiscuous mode
  if((rules & SOS_MSG_RULES_PROMISCUOUS) == 0){
	// module request to have no promiscuous message
	if(m->daddr != node_address && m->daddr != BCAST_ADDRESS){
	  DEBUG("filtered\n");
	  return true;
	}
  }
  return false;
}

void sched(void)
{
	ENABLE_GLOBAL_INTERRUPTS();

	ker_log_start();
	for(;;){
		SOS_MEASUREMENT_IDLE_END();
		DISABLE_GLOBAL_INTERRUPTS();

		if (int_ready != 0) {
			ENABLE_GLOBAL_INTERRUPTS();
			if (true == sched_stalled) continue;
			handle_callback();
		} else if( schedpq.msg_cnt != 0 ) {
			ENABLE_GLOBAL_INTERRUPTS();
			if (true == sched_stalled) continue;
			do_dispatch();
		} else {
			SOS_MEASUREMENT_IDLE_START();
			/**
			 * ENABLE_INTERRUPT() is done inside atomic_hardware_sleep()
			 */
			ker_log_flush();
			atomic_hardware_sleep();
		}
		watchdog_reset();
	}
}