pico_device.c

00001 /*********************************************************************
00002 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
00003 See LICENSE and COPYING for usage.
00004 
00005 .
00006 
00007 Authors: Daniele Lacamera
00008 *********************************************************************/
00009 
00010 
00011 #include "pico_config.h"
00012 #include "pico_device.h"
00013 #include "pico_stack.h"
00014 #include "pico_protocol.h"
00015 #include "pico_tree.h"
00016 
00017 
00018 static int pico_dev_cmp(void *ka, void *kb)
00019 {
00020     struct pico_device *a = ka, *b = kb;
00021   if (a->hash < b->hash)
00022     return -1;
00023   if (a->hash > b->hash)
00024     return 1;
00025   return 0;
00026 }
00027 
00028 PICO_TREE_DECLARE(Device_tree,pico_dev_cmp);
00029 
00030 int pico_device_init(struct pico_device *dev, char *name, uint8_t *mac)
00031 {
00032     int len = strlen(name);
00033     if(len>MAX_DEVICE_NAME)
00034         len = MAX_DEVICE_NAME;
00035   memcpy(dev->name, name, len);
00036   dev->hash = pico_hash(dev->name);
00037 
00038   pico_tree_insert(&Device_tree,dev);
00039   dev->q_in = pico_zalloc(sizeof(struct pico_queue));
00040   dev->q_out = pico_zalloc(sizeof(struct pico_queue));
00041 
00042   if (mac) {
00043     dev->eth = pico_zalloc(sizeof(struct pico_ethdev));
00044     memcpy(dev->eth->mac.addr, mac, PICO_SIZE_ETH);
00045   } else {
00046     dev->eth = NULL;
00047   }
00048 
00049   if (!dev->q_in || !dev->q_out || (mac && !dev->eth))
00050     return -1;
00051   return 0;
00052 }
00053 
00054 void pico_device_destroy(struct pico_device *dev)
00055 {
00056   if (dev->destroy)
00057     dev->destroy(dev);
00058 
00059   if (dev->q_in) {
00060     pico_queue_empty(dev->q_in);
00061     pico_free(dev->q_in);
00062   }
00063   if (dev->q_out) {
00064     pico_queue_empty(dev->q_out);
00065     pico_free(dev->q_out);
00066   }
00067 
00068   if (dev->eth)
00069     pico_free(dev->eth);
00070 
00071   pico_tree_delete(&Device_tree,dev);
00072   pico_free(dev);
00073 }
00074 
00075 static int devloop(struct pico_device *dev, int loop_score, int direction)
00076 {
00077   struct pico_frame *f;
00078 
00079   /* If device supports interrupts, read the value of the condition and trigger the dsr */
00080   if ((dev->__serving_interrupt) && (dev->dsr)) {
00081     /* call dsr routine */
00082     loop_score = dev->dsr(dev, loop_score);
00083   }
00084 
00085   /* If device supports polling, give control. Loop score is managed internally, 
00086    * remaining loop points are returned. */
00087   if (dev->poll) {
00088     loop_score = dev->poll(dev, loop_score);
00089   }
00090 
00091   if (direction == PICO_LOOP_DIR_OUT) {
00092 
00093     while(loop_score > 0) {
00094       if (dev->q_out->frames <= 0)
00095         break;
00096 
00097       /* Device dequeue + send */
00098       f = pico_dequeue(dev->q_out);
00099       if (f) {
00100         if (dev->eth) {
00101           int ret = pico_ethernet_send(f);
00102           if (0 == ret) {
00103             loop_score--;
00104             continue;
00105           } if (ret < 0) {
00106             if (!pico_source_is_local(f)) { 
00107               dbg("Destination unreachable -------> SEND ICMP\n");
00108               pico_notify_dest_unreachable(f);
00109             } else {
00110               dbg("Destination unreachable -------> LOCAL\n");
00111             }
00112             pico_frame_discard(f);
00113             continue;
00114           }
00115         } else {
00116           dev->send(dev, f->start, f->len);
00117         }
00118         pico_frame_discard(f);
00119         loop_score--;
00120       }
00121     }
00122 
00123   } else if (direction == PICO_LOOP_DIR_IN) {
00124 
00125     while(loop_score > 0) {
00126       if (dev->q_in->frames <= 0)
00127         break;
00128 
00129       /* Receive */
00130       f = pico_dequeue(dev->q_in);
00131       if (f) {
00132         if (dev->eth) {
00133           f->datalink_hdr = f->buffer;
00134           pico_ethernet_receive(f);
00135         } else {
00136           f->net_hdr = f->buffer;
00137           pico_network_receive(f);
00138         }
00139         loop_score--;
00140       }
00141     }
00142   }
00143 
00144   return loop_score;
00145 }
00146 
00147 
00148 #define DEV_LOOP_MIN  16
00149 
00150 int pico_devices_loop(int loop_score, int direction)
00151 {
00152   struct pico_device *start;
00153   static struct pico_device *next = NULL, *next_in = NULL, *next_out = NULL;
00154   static struct pico_tree_node * next_node, * in_node, * out_node;
00155 
00156   if (next_in == NULL) {
00157     in_node = pico_tree_firstNode(Device_tree.root);
00158     next_in = in_node->keyValue;
00159   }
00160   if (next_out == NULL) {
00161       out_node = pico_tree_firstNode(Device_tree.root);
00162     next_out = out_node->keyValue;
00163   }
00164   
00165   if (direction == PICO_LOOP_DIR_IN)
00166   {
00167       next_node = in_node;
00168     next = next_in;
00169   }
00170   else if (direction == PICO_LOOP_DIR_OUT)
00171   {
00172       next_node = out_node;
00173     next = next_out;
00174   }
00175 
00176   /* init start node */
00177   start = next;
00178 
00179   /* round-robin all devices, break if traversed all devices */
00180   while (loop_score > DEV_LOOP_MIN && next != NULL) {
00181     loop_score = devloop(next, loop_score, direction);
00182 
00183     next_node = pico_tree_next(next_node);
00184     next = next_node->keyValue;
00185 
00186     if (next == NULL)
00187     {
00188         next_node = pico_tree_firstNode(Device_tree.root);
00189       next = next_node->keyValue;
00190     }
00191     if (next == start)
00192       break;
00193   }
00194 
00195   if (direction == PICO_LOOP_DIR_IN)
00196   {
00197       in_node = next_node;
00198     next_in = next;
00199   }
00200   else if (direction == PICO_LOOP_DIR_OUT)
00201   {
00202       out_node = next_node;
00203     next_out = next;
00204   }
00205 
00206   return loop_score;
00207 }
00208 
00209 struct pico_device* pico_get_device(char* name)
00210 {
00211   struct pico_device *dev;
00212   struct pico_tree_node * index;
00213   pico_tree_foreach(index, &Device_tree){
00214       dev = index->keyValue;
00215     if(strcmp(name, dev->name) == 0)
00216       return dev;
00217   }
00218   return NULL;
00219 }
00220 
00221 int pico_device_broadcast(struct pico_frame * f)
00222 {
00223     struct pico_tree_node * index;
00224     int ret = -1;
00225 
00226     pico_tree_foreach(index,&Device_tree)
00227     {
00228         struct pico_device * dev = index->keyValue;
00229         if(dev != f->dev)
00230         {
00231             struct pico_frame * copy = pico_frame_copy(f);
00232 
00233             if(!copy)
00234                 return -1;
00235             copy->dev = dev;
00236             copy->dev->send(copy->dev, copy->start, copy->len);
00237             pico_frame_discard(copy);
00238         }
00239         else
00240         {
00241             ret = f->dev->send(f->dev, f->start, f->len);
00242         }
00243     }
00244 
00245     return ret;
00246 }