kernel/message_net.c

   1 /* -*- Mode: C; tab-width:2 -*- */
   2 /* ex: set ts=2 shiftwidth=2 softtabstop=2 cindent: */
   3 /*
   4  * Copyright (c) 2003 The Regents of the University of California.
   5  * All rights reserved.
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions
   9  * are met:
  10  * 1. Redistributions of source code must retain the above copyright
  11  *    notice, this list of conditions and the following disclaimer.
  12  * 2. Redistributions in binary form must reproduce the above
  13  *    copyright notice, this list of conditions and the following
  14  *    disclaimer in the documentation and/or other materials provided
  15  *    with the distribution.
  16  * 3. All advertising materials mentioning features or use of this
  17  *    software must display the following acknowledgement:
  18  *       This product includes software developed by Networked &
  19  *       Embedded Systems Lab at UCLA
  20  * 4. Neither the name of the University nor that of the Laboratory
  21  *    may be used to endorse or promote products derived from this
  22  *    software without specific prior written permission.
  23  *
  24  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
  25  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
  26  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
  27  * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS
  28  * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  29  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  30  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
  31  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  32  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  33  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
  34  * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  35  * SUCH DAMAGE.
  36  *
  37  * $Id: message_net.c,v 1.36 2006/12/17 00:51:01 simonhan Exp $
  38  */
  39 /**
  40  * @brief Message handling routines for network
  41  * @author Simon Han (simonhan@cs.ucla.edu)
  42  * @brief General Message Dispatcher (Handles diverse links)
  43  * @author Ram Kumar {ram@ee.ucla.edu}
  44  */
  45
  46 #include <message_queue.h>
  47 #include <monitor.h>
  48 #include <sos_info.h>
  49 #include <message_types.h>
  50 #include <malloc.h>
  51 #include <message.h>
  52 #include <sos_sched.h>
  53 #include <hardware.h>
  54 #include <sos_logging.h>
  55
  56 #ifdef SOS_USE_PREEMPTION
  57 #include <priority.h>
  58 #endif
  59
  60 #if defined (SOS_UART_CHANNEL)
  61 #include <sos_uart.h>
  62 #include <sos_uart_mgr.h>
  63 #endif
  64
  65 #if defined (SOS_I2C_CHANNEL)
  66 #include <sos_i2c.h>
  67 #include <sos_i2c_mgr.h>
  68 #endif
  69
  70 typedef int8_t (*routing_func_t)(func_cb_ptr cb, Message *m);
  71 static int8_t no_router(func_cb_ptr p, Message *msg);
  72 //-------------------------------------------------------------------------------
  73 // STATIC FUNCTION DECLARATIONS
  74 //-------------------------------------------------------------------------------
  75 static int8_t sos_msg_dispatch(Message* m);
  76 static inline void msg_change_endian(Message* e);
  77 static bool sos_msg_find_right_link(Message *m);
  78 static int8_t routing_handler(void *state, Message *msg);
  79
  80 #ifndef SOS_USE_PREEMPTION
  81 static sos_module_t routing_module;
  82 #endif
  83
  84 #ifdef SOS_USE_PREEMPTION
  85 static func_cb_ptr *routing_func_ptr;
  86 #else
  87 static func_cb_ptr routing_func_ptr[1];
  88 #endif
  89
  90 static mod_header_t mod_header SOS_MODULE_HEADER =
  91 {
  92   mod_id : KER_ROUTING_PID,
  93 #ifdef SOS_USE_PREEMPTION
  94         state_size : sizeof(func_cb_ptr),
  95 #else
  96   state_size : 0,
  97 #endif
  98   num_prov_func : 0,
  99   num_sub_func : 1,
 100   module_handler: routing_handler,
 101   funct : {
 102     // routing function
 103     {no_router, "czv1", RUNTIME_PID, RUNTIME_FID},
 104   },
 105 };
 106
 107 static int8_t no_router(func_cb_ptr p, Message *msg)
 108 {
 109         return -1;
 110 }
 111 static int8_t routing_handler(void *state, Message *msg)
 112 {
 113         return -EINVAL;
 114 }
 115
 116 //-------------------------------------------------------------------------------
 117 // FUNCTION IMPLEMENTATIONS
 118 //-------------------------------------------------------------------------------
 119 // Fix the endian-ness of the message header
 120 static inline void msg_change_endian(Message* e){
 121   e->daddr = ehtons(e->daddr);
 122   e->saddr = ehtons(e->saddr);
 123 }
 124
 125 // Create a deep copy of the message
 126
 127 #if defined(SOS_UART_CHANNEL) || defined(SOS_I2C_CHANNEL) || defined(SOS_SPI_CHANNEL)
 128 static Message* msg_duplicate(Message* m){
 129   Message* mcopy;
 130         uint8_t* d;
 131   mcopy = msg_create();
 132   if (NULL == mcopy) return NULL;
 133   d = (uint8_t*)ker_malloc(m->len, KER_SCHED_PID);
 134   if ((NULL == d) && (0 != m->len)){
 135     msg_dispose(mcopy);
 136     return NULL;
 137   }
 138         memcpy(mcopy, m, sizeof(Message));
 139         mcopy->data = d;
 140         mcopy->flag |= SOS_MSG_RELEASE;
 141   memcpy(mcopy->data, m->data, m->len);
 142   return mcopy;
 143 }
 144 #endif
 145
 146 // NULL Link - Simply free the message
 147 static void null_link_msg_alloc(Message* m){
 148   msg_dispose(m);
 149 }
 150
 151
 152
 153 //-------------------------------------------------------------------------------
 154 // POST
 155 //-------------------------------------------------------------------------------
 156 // Copies message header and sends out the message
 157 int8_t post(Message *e){
 158   Message *m = msg_create();
 159   if(m == NULL){
 160         if(flag_msg_release(e->flag)) {
 161           ker_free(e->data);
 162                 e->data = NULL;
 163                 e->len = 0;
 164         }
 165         return -ENOMEM;
 166   }
 167   // deep copy the header
 168   *m = *e;
 169
 170         // Transfer the memory
 171         e->len = 0;
 172         e->data = NULL;
 173         e->flag &= ~SOS_MSG_RELEASE;
 174
 175
 176   // Dispatch Message
 177   return sos_msg_dispatch(m);
 178 }
 179
 180 //-------------------------------------------------------------------------------
 181 // POST LINK
 182 //-------------------------------------------------------------------------------
 183 // Post a message over any link as specified by the flag
 184 int8_t post_link(sos_pid_t did, sos_pid_t sid,
 185                 uint8_t type, uint8_t len,
 186                 void *data, uint16_t flag,
 187                 uint16_t daddr)
 188 {
 189   // Create a message
 190   Message *m = msg_create();
 191   if (NULL == m){
 192     if (flag_msg_release(flag)){
 193       ker_free(data);
 194     }
 195     return -ENOMEM;
 196   }
 197
 198   // Fill out message header
 199   m->daddr = daddr;
 200   m->did = did;
 201   m->type = type;
 202   m->saddr = node_address;
 203   m->sid = sid;
 204   m->len = len;
 205   m->data = (uint8_t*)data;
 206   m->flag = flag;
 207 #ifdef SOS_USE_PREEMPTION
 208   // assign priority based on did
 209   m->priority = get_module_priority(did);
 210 #endif
 211
 212   // Dispatch Message
 213   return sos_msg_dispatch(m);
 214 }
 215
 216
 217 //-----------------------------------------------------------------------------
 218 // SOS Find Right link
 219 //-----------------------------------------------------------------------------
 220 static bool sos_msg_find_right_link(Message *m)
 221 {
 222                 bool link_found = false;
 223
 224                 // try to figure out the right link
 225 #ifdef SOS_UART_CHANNEL
 226                 if (check_uart_address(m->daddr) == SOS_OK) {
 227                         m->flag |= SOS_MSG_UART_IO;
 228                         link_found = true;
 229                 } else {
 230                         m->flag &= ~SOS_MSG_UART_IO;
 231                 }
 232 #else
 233                 m->flag &= ~SOS_MSG_UART_IO;
 234 #endif
 235
 236 #ifdef SOS_I2C_CHANNEL
 237                 if (check_i2c_address(m->daddr) == SOS_OK) {
 238                         m->flag |= SOS_MSG_I2C_IO;
 239                         link_found = true;
 240                 } else {
 241                         m->flag &= ~SOS_MSG_I2C_IO;
 242                 }
 243 #else
 244                 m->flag &= ~SOS_MSG_I2C_IO;
 245 #endif
 246
 247                 if(link_found == false) {
 248                         m->flag |= SOS_MSG_RADIO_IO;
 249                 } else {
 250                         m->flag &= ~SOS_MSG_RADIO_IO;
 251                 }
 252                 return link_found;
 253 }
 254
 255 //--------------------------------------------------------------------------------
 256 // SOS Message Dispatcher
 257 //--------------------------------------------------------------------------------
 258 static int8_t sos_msg_dispatch(Message* m)
 259 {
 260 #if defined(SOS_RADIO_CHANNEL) || defined(SOS_UART_CHANNEL) || defined(SOS_I2C_CHANNEL) || defined(SOS_SPI_CHANNEL)
 261         Message* mcopy[NUM_IO_LINKS] = {NULL};
 262         uint8_t msg_count = 0;
 263 #endif
 264
 265   // Local Dispatch
 266   if (node_address == m->daddr){
 267     sched_msg_alloc(m);
 268                 ker_log( SOS_LOG_POST_NET, m->sid, m->daddr );
 269     return SOS_OK;
 270   }
 271
 272 #if !defined(SOS_UART_CHANNEL) && !defined(SOS_I2C_CHANNEL) && !defined(SOS_RADIO_CHANNEL) && !defined(SOS_SPI_CHANNEL)
 273         null_link_msg_alloc(m);
 274         return -EINVAL;
 275 #endif
 276
 277         if( m->daddr != BCAST_ADDRESS && flag_msg_raw(m->flag) == 0 ) {
 278                 int8_t ret;
 279                 // Not using raw message, send to the routing layer first
 280                 if( sos_msg_find_right_link(m) == false ) {
 281                         ret = SOS_CALL(routing_func_ptr[0], routing_func_t, m);
 282                         if( ret == ( SOS_OK + 1 )) {
 283                                 // Forward to the link layer (don't generate senddone)
 284                                 msg_dispose(m);
 285                                 return SOS_OK;
 286                         } else if( ret == SOS_OK ) {
 287                                 // generate senddone
 288                                 msg_send_senddone( m, true, KER_ROUTING_PID );
 289                                 return SOS_OK;
 290                         }
 291                 }
 292         }
 293
 294         if (flag_msg_link_auto(m->flag) && m->daddr != BCAST_ADDRESS) {
 295                 sos_msg_find_right_link(m);
 296         }
 297
 298         if ((m->flag & SOS_MSG_ALL_LINK_IO) == 0) {
 299                 null_link_msg_alloc(m);
 300                 return SOS_OK;
 301         }
 302
 303         // Pre-allocate the message copies to allow for
 304         // an atomic NOMEM failure
 305 #ifdef SOS_RADIO_CHANNEL
 306         if (flag_msg_from_radio(m->flag)){
 307                 mcopy[SOS_RADIO_LINK_ID] = m;
 308                 msg_count ++;
 309         }
 310 #endif
 311
 312 #ifdef SOS_UART_CHANNEL
 313         if( flag_msg_from_uart(m->flag) ) {
 314                 if (msg_count == 0){
 315                         mcopy[SOS_UART_LINK_ID] = m;
 316                 } else {
 317                         mcopy[SOS_UART_LINK_ID] = msg_duplicate(m);
 318                         if (NULL == mcopy[SOS_UART_LINK_ID]) goto dispatch_cleanup;
 319                         mcopy[SOS_UART_LINK_ID]->flag &= ~SOS_MSG_RELIABLE;
 320                 }
 321                 msg_count++;
 322         }
 323 #endif
 324
 325 #ifdef SOS_I2C_CHANNEL
 326         if (flag_msg_from_i2c(m->flag)) {
 327                 if (msg_count == 0){
 328                         mcopy[SOS_I2C_LINK_ID] = m;
 329                 } else {
 330                         mcopy[SOS_I2C_LINK_ID] = msg_duplicate(m);
 331                         if (NULL == mcopy[SOS_I2C_LINK_ID]) goto dispatch_cleanup;
 332                         mcopy[SOS_I2C_LINK_ID]->flag &= ~SOS_MSG_RELIABLE;
 333                 }
 334                 msg_count++;
 335         }
 336 #endif
 337
 338 #ifdef SOS_SPI_CHANNEL
 339         if (flag_msg_from_spi(m->flag)) {
 340                 if (msg_count == 0){
 341                         mcopy[SOS_SPI_LINK_ID] = m;
 342                 } else {
 343                         mcopy[SOS_SPI_LINK_ID] = msg_duplicate(m);
 344                         if (NULL == mcopy[SOS_SPI_LINK_ID]) goto dispatch_cleanup;
 345                         mcopy[SOS_SPI_LINK_ID]->flag &= ~SOS_MSG_RELIABLE;
 346                 }
 347                 msg_count++;
 348         }
 349 #endif
 350
 351         // Deliver to monitor only once
 352         monitor_deliver_outgoing_msg_to_monitor(m);
 353
 354   // Radio Dispatch
 355 #ifdef SOS_RADIO_CHANNEL
 356         if (NULL != mcopy[SOS_RADIO_LINK_ID]){
 357                 msg_change_endian(mcopy[SOS_RADIO_LINK_ID]);
 358                 SOS_RADIO_LINK_DISPATCH(mcopy[SOS_RADIO_LINK_ID]);
 359                 ker_log( SOS_LOG_POST_NET, mcopy[SOS_RADIO_LINK_ID]->sid, mcopy[SOS_RADIO_LINK_ID]->daddr );
 360         }
 361 #endif
 362
 363   // UART Dispatch
 364 #ifdef SOS_UART_CHANNEL
 365         if (NULL != mcopy[SOS_UART_LINK_ID]){
 366                 msg_change_endian(mcopy[SOS_UART_LINK_ID]);
 367                 SOS_UART_LINK_DISPATCH(mcopy[SOS_UART_LINK_ID]);
 368         }
 369 #endif
 370
 371   // I2C Dispatch
 372 #ifdef SOS_I2C_CHANNEL
 373         if (NULL != mcopy[SOS_I2C_LINK_ID]){
 374                 msg_change_endian(mcopy[SOS_I2C_LINK_ID]);
 375                 SOS_I2C_LINK_DISPATCH(mcopy[SOS_I2C_LINK_ID]);
 376         }
 377 #endif
 378
 379         // SPI Dispatch
 380 #ifdef SOS_SPI_CHANNEL
 381         if (NULL != mcopy[SOS_SPI_LINK_ID]){
 382                 msg_change_endian(mcopy[SOS_SPI_LINK_ID]);
 383                 SOS_SPI_LINK_DISPATCH(mcopy[SOS_SPI_LINK_ID]);
 384         }
 385 #endif
 386
 387         return SOS_OK;
 388
 389 #if defined(SOS_UART_CHANNEL) || defined(SOS_I2C_CHANNEL) || defined(SOS_SPI_CHANNEL)
 390 dispatch_cleanup:
 391         {
 392                 uint8_t i;
 393                 for (i = 0 ; i < NUM_IO_LINKS; i++){
 394                         if (NULL != mcopy[i]){
 395                                 msg_dispose(mcopy[i]);
 396                         }
 397                 }
 398                 DEBUG("----------------Cleaning up-----------\n");
 399         }
 400         return -ENOMEM;
 401 #endif
 402 }
 403
 404 //============================================================================
 405 // sys APIs
 406 //============================================================================
 407
 408 int8_t ker_sys_post_link(sos_pid_t dst_mod_id, uint8_t type,
 409                 uint8_t size, void *data, uint16_t flag, uint16_t dst_node_addr)
 410 {
 411         sos_pid_t my_id = ker_get_current_pid();
 412
 413         if( post_link(dst_mod_id, my_id, type, size, data, flag, dst_node_addr) != SOS_OK) {
 414                 return ker_mod_panic(my_id);
 415         }
 416         return SOS_OK;
 417 }
 418
 419 int8_t ker_sys_routing_register( uint8_t fid )
 420 {
 421         sos_pid_t sub_id = ker_get_current_pid();
 422
 423         if(ker_fntable_subscribe(KER_ROUTING_PID, sub_id, fid, 0) != SOS_OK) {
 424                 return -EINVAL;
 425         }
 426         return SOS_OK;
 427 }
 428
 429 void routing_init( void )
 430 {
 431 #ifdef SOS_USE_PREEMPTION
 432         ker_register_module(sos_get_header_address(mod_header));
 433         routing_func_ptr = ker_get_module_state(KER_SENSOR_PID);
 434 #else
 435         sched_register_kernel_module( &routing_module, sos_get_header_address(mod_header), routing_func_ptr);
 436 #endif
 437 }
 438
 439