ab.c

#include "ab.h"
/*
  Implementation of the alternating bit protocol on top of LNP for 
  LegOS/BrickOS
  Copyright (C) 2004  Bram De Wachter

  This program is free software; you can redistribute it and/or
  modify it under the terms of the GNU General Public License
  as published by the Free Software Foundation; either version 2
  of the License, or (at your option) any later version.
  
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.
  
  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
*/


#if DEBUG
#define DEBUG_AB(debug_instr)  debug_instr
#else
#define DEBUG_AB(debug_instr)
#endif

#define TRUE                        1
#define FALSE                       0

#define INCOMING                    0
#define OUTGOING                    1

#define ERR_AB_WRITE               10
#define MSG_HEAD(msg)          msg[0]
#define MSG_SOURCE(msg)        msg[1]
#define MSG_DEST(msg)          msg[2]

sem_t sem_ab_write,sem_ab_portHandler,sem_todo_i,sem_todo_o;

site_s * s_head;     

msg_s *  m_head, * m_tail;     

unsigned char _id;

void (*user_portHandler_8)  (const unsigned char *buf, unsigned char len);



/*
   Message :

      0          1           2      3 ... payload ... N
   MSG_HEAD  MSG_SOURCE  MSG_DEST  

   MSG_HEAD : 
   7 6 5 4 3 2 1 0  (8 bits)
  MSB           LSB
   
   0 : bit for abp
   1 : is_ack

   => HEAD = 0 : message with bit = 0
             1 : message with bit = 1
             2 : ack with bit     = 0 
             3 : ack with bit     = 1
*/   
#define set_ack(msg,val)   MSG_HEAD(msg) = (val ? (MSG_HEAD(msg)|0x2) : (MSG_HEAD(msg)&0xFD))
#define set_ack_1(msg)     MSG_HEAD(msg) |= 0x2;
#define set_ack_0(msg)     MSG_HEAD(msg) &= 0xFD;
#define is_ack(msg)        (MSG_HEAD(msg)&0x2)

#define set_bit(msg,val)   MSG_HEAD(msg) = (val ? (MSG_HEAD(msg)|0x1) : (MSG_HEAD(msg)&0xFE))
#define set_bit_1(msg)     MSG_HEAD(msg) |= 0x1;
#define set_bit_0(msg)     MSG_HEAD(msg) &= 0xFE;
#define get_bit(msg)       (MSG_HEAD(msg)&0x1)


#define alloc_site()       ((site_s*) malloc(sizeof(site_s)))
#define alloc_mess(length) ((msg_s*)  malloc(sizeof(msg_s)+HEADER_SZ+length))
#define free_mess(mess)    free (mess)

/*****************************************************************************/
/*****************************************************************************/

#define flip_sb(rs)    rs^=0x1
#define flip_rb(rs)    rs^=0x2
#define get_sb(rs)     (rs&0x1)
#define get_rb(rs)     ((rs&0x2)>>1)


static void erase(site_s * site ) {
  msg_s * msg;
  msg = site->head;
  if (msg) {
    site->head=msg->next;
  }
  sem_trywait(&sem_todo_o);
}

static msg_s * new_msg(const unsigned char * header, const unsigned char* msg, 
                       unsigned char length){
  msg_s * m;

  m = alloc_mess(length);
  if (!m) return NULL;

  // Combine the two mallocs to only one, 
  // make m->txt point to the area after the msg_s structure
  m->txt = ((char*)m)+sizeof(msg_s);
  m->len=length + HEADER_SZ;
  sem_init(&m->sem_ack, 1, 0);
  memcpy(m->txt, header, HEADER_SZ);
  memcpy(m->txt+HEADER_SZ, msg, length);

  m->next=NULL;

  return m;
}

static site_s * new_site(unsigned char id){
  site_s *s;
  if ((s = alloc_site()) == NULL ) return NULL;
  s->id=id;
  s->head=0;
  s->status=0;
  s->timeout_wait=0;
  return s;
}

static void add( site_s *site, msg_s *m ) {
  msg_s *i=site->head;
  if (m) {
    if(!i) site->head=m;
    else {
      while(i->next) i=i->next;
      i->next=m;
    }
  }
}

static site_s * search_and_add(unsigned char id) {
  site_s* i;
  for (i=s_head; i; i=i->next) if (i->id == id) break;
  if (!i) {
    i = s_head;
    s_head = new_site(id);
    if (s_head) s_head->next = i;        
    i = s_head;
  }
  return i;
}

static void AB_portHandler(const unsigned char *message, unsigned char length){
  msg_s *curr;

  if(length>=HEADER_SZ){
          if(MSG_DEST(message) == _id) {
                  if(sem_trywait(&sem_ab_portHandler)==0) {/*list not in use in thread AB*/
                          if ((curr = new_msg(message,
                                              message+HEADER_SZ,
                                              length-HEADER_SZ)) != NULL) {
                                  if (m_tail) m_tail->next = curr;
                                  else m_head = curr;
                                  m_tail = curr;
                                  sem_post(&sem_todo_i);
                          } 
                          sem_post(&sem_ab_portHandler);
                  }
          }
  }
}

static int thread_AB_I () {
        site_s * site;
        msg_s  * msg, * msg_temp;
        unsigned char* _txt;
        char ack[HEADER_SZ];

        while(1) {

                sem_wait(&sem_todo_i); 
                sem_post(&sem_todo_i); 

                //treatment of received messages
                sem_wait(&sem_ab_portHandler);
                msg=m_head;
                m_tail = NULL;
                sem_post(&sem_ab_portHandler);

                while(msg != NULL) {
                        _txt = msg->txt;
                        sem_trywait(&sem_todo_i);

                        if(is_ack(_txt)) { // ACK RECEIVED
                                DEBUG_AB(cputs("ack"));

                                sem_wait(&sem_ab_write);
                                site=search_and_add(MSG_SOURCE(_txt));
                                if(site && site->head) {
                                        if(get_sb(site->status)==get_bit(_txt) ) {
                                                sem_t *t = &site->head->sem_ack;
                                                erase(site);
                                                sem_post(t);
                                                site->timeout_wait = 0; // Reset timeout
                                                flip_sb(site->status);
                                        }
                                }
                                sem_post(&sem_ab_write);
                        } else {        // Message received,
                                // answer with an ack containing same bit
                                MSG_SOURCE(ack) = MSG_DEST  (_txt);
                                ab_source       = MSG_DEST  (ack) = MSG_SOURCE(_txt);
                                MSG_HEAD  (ack) = 0;
                                set_ack_1 (ack);
                                set_bit   (ack,get_bit(_txt));  

                                sem_wait(&sem_ab_write);        
                                // Send ack
                                lnp_integrity_write(ack, HEADER_SZ);
                                msleep(0);

                                // Find the site that sent this message
                                site=search_and_add(MSG_SOURCE(_txt));

                                // Site found, if bit corresponds, call user
                                if(site) {
                                        if(get_rb(site->status) == get_bit(_txt)) {
                                                user_portHandler_8(_txt+HEADER_SZ,
                                                                msg->len-HEADER_SZ);
                                                flip_rb(site->status); //switch BIT of site
                                        }
                                }
                                sem_post(&sem_ab_write);
                        }

                        // Message treated, destroy it
                        msg_temp=msg;
                        msg=msg->next;
                        free_mess(msg_temp);
                } // Loop on messages

        } // Loop while(1)
}

static int thread_AB_O () {
        site_s * site;            
        unsigned char* _txt;

        while(1) {
                msleep (WRITE_WAIT_TIME);
                sem_wait(&sem_todo_o);
                sem_post(&sem_todo_o);

                sem_wait(&sem_ab_write);
                site=s_head;
                while(site) {

                        // Check if there is a site whose timeout expired
                        if(site->head!=NULL && site->timeout_wait < get_system_up_time()) {
                                // This site is ready for sending !
                                // Take out the message to send
                                _txt=site->head->txt;

                                if (MSG_DEST(_txt) == _id) {
                                        // Sending to ourselfs ... hmmm ... call the handler immediately
                                        // Of course, there will be no ack, and delivery is certain,
                                        // so remove the message from the queue
                                        user_portHandler_8(_txt+HEADER_SZ,
                                                        site->head->len-HEADER_SZ);
                                        erase (site);
                                } else {
                                        // Sending to a distant site ...

                                        // Clear the status. Make it a message status
                                        // With the corresponding bit
                                        MSG_HEAD(_txt)=0;
                                        set_bit(_txt,get_sb(site->status));

                                        // Send the message, and start the timeout timer
                                        lnp_integrity_write(_txt,site->head->len);
                                        site->timeout_wait = get_system_up_time() + WRITE_TIMEOUT + 
                                                             get_system_up_time()%WRITE_TIMEOUT + site->head->len;
                                        msleep(0);
                                }
                        } 
                        site=site->next;
                }
                sem_post(&sem_ab_write);
        }
        return 0;
}



int init_ab_protocol_8(void (*u_portHandler) (const unsigned char *message, unsigned char length),  
                unsigned char id) {
        unsigned char * msg;
        s_head=NULL;
        m_head=NULL;
        m_tail=NULL;
        _id = id;
        sem_init(&sem_ab_portHandler,1,1);
        sem_init(&sem_ab_write,1,1);
        sem_init(&sem_todo_i,1,0);
        sem_init(&sem_todo_o,1,0);
        user_portHandler_8 = u_portHandler; 
        lnp_integrity_set_handler(AB_portHandler);
        
        if (execi(thread_AB_I, 0,0,PRIO_NORMAL, DEFAULT_STACK_SIZE*2) == -1) return 0xfe ;
        if (execi(thread_AB_O, 0,0,PRIO_NORMAL, DEFAULT_STACK_SIZE) == -1) return 0xff;

        return 0;
}

static msg_s * ab_write(const unsigned char* message, unsigned char length, unsigned char destination) {
        char msg_header[HEADER_SZ];
        site_s* site;
        msg_s* mess = NULL;

        MSG_HEAD(msg_header)   = 0;
        MSG_SOURCE(msg_header) = _id;
        MSG_DEST(msg_header)   = destination;

        sem_wait(&sem_ab_write);
        site=search_and_add(destination);
        sem_post(&sem_ab_write);

        if (site) {
                mess = new_msg(msg_header, message, length);
                if (mess)  {
                        sem_wait(&sem_ab_write);
                        add(site, mess);
                        sem_post(&sem_ab_write);
                        sem_post(&sem_todo_o);
                }
        }

        return mess;
}



int ab_awrite(const unsigned char* message, unsigned char length, unsigned char destination) {
        msg_s * msg = ab_write(message, length, destination);   
        if (msg) free_mess(msg);
        return msg == NULL;
}

int ab_swrite(const unsigned char* message, unsigned char length,
                 unsigned char destination) {
  msg_s * msg = ab_write(message, length, destination);
  if (msg) {
      sem_wait(&msg->sem_ack);
      free_mess(msg);
  }
  return msg == NULL;
}

---