/*
 *  Copyright (C) James R. Leu 2000
 *  jleu@mindspring.com
 *
 *  This software is covered under the LGPL, for more
 *  info check out http://www.gnu.org/copyleft/lgpl.html
 */

#include <stdio.h>
#include <netinet/in.h>
#include "ldp_struct.h"
#include "ldp_nortel.h"
#include "ldp_pdu.h"
#include "ldp_msg.h"
#include "ldp_mm_impl.h"
#include "ldp_trace_impl.h"
#include "ldp_msg_alloc.h"
#include <errno.h>

int debug = 0;

ldp_pdu *ldp_pdu_create() {
  ldp_pdu *p = (ldp_pdu *)ldp_malloc(sizeof(ldp_pdu));

  if(p != NULL) {
    LDP_LIST_INIT(&p->msg,ldp_msg);
  }
  return p;
}

void ldp_pdu_delete(ldp_pdu *p) {
  ldp_msg *msg = NULL;

  LDP_LIST_REMOVE_HEAD(&p->msg,msg,_pdu);
  while(msg != NULL) {
    ldp_msg_delete(msg);
    LDP_LIST_REMOVE_HEAD(&p->msg,msg,_pdu);
  }
  ldp_free(p);
}

void ldp_pdu_dump(ldp_instance_handle handle,uint8_t *buf,int buf_size,
   int data_size) {
   int i;
   int j = 0;

   for(i = 0;i < data_size; i++) {
     LDP_TRACE_OUT(handle,"%02x ",buf[i]);
     j++;
     if(j == 16) {
       LDP_TRACE_OUT(handle,"\n");
       j = 0;
     }
   }
   LDP_TRACE_OUT(handle,"\n");
}

ldp_pdu *ldp_pdu_create_decode(ldp_global* g,uint8_t *buf,int buf_size,
  int data_size) {
  ldp_pdu *p = ldp_pdu_create();
  int result;

  if(!p)
    return NULL;

  result = Mpls_decodeLdpPDU(g,p,buf,buf_size,data_size);
  if(result > 0)
    return p;

  ldp_pdu_delete(p);
  return NULL;
}

int Mpls_encodeLdpPDU(ldp_global* g,uint32_t lsraddr,int label_space,
  ldp_msg *msg,
  uint8_t* buf,int buf_size) {
  unsigned char *hdrBuf = buf;
  int hdrBuf_size = MPLS_LDP_HDRSIZE;
  int hdr_size;
  ldp_trace_flags type = LDP_TRACE_FLAG_INIT;

  unsigned char *bodyBuf = buf + MPLS_LDP_HDRSIZE;
  int bodyBuf_size = buf_size - MPLS_LDP_HDRSIZE;

  int body_size = 0;

  switch(((mplsLdpMsg_t*)msg->body)->flags.flags.msgType) {
    case MPLS_INIT_MSGTYPE:
      body_size = Mpls_encodeLdpInitMsg((mplsLdpInitMsg_t*)msg->body,bodyBuf,
        bodyBuf_size);
      break;
    case MPLS_NOT_MSGTYPE:
      body_size = Mpls_encodeLdpNotMsg((mplsLdpNotifMsg_t*)msg->body,bodyBuf,
        bodyBuf_size);
      break;
    case MPLS_KEEPAL_MSGTYPE:
      body_size = Mpls_encodeLdpKeepAliveMsg((mplsLdpKeepAlMsg_t*)msg->body,
        bodyBuf,bodyBuf_size);
      break;
    case MPLS_HELLO_MSGTYPE:
      body_size = Mpls_encodeLdpHelloMsg((mplsLdpHelloMsg_t*)msg->body,bodyBuf,
        bodyBuf_size);
      break;
    case MPLS_LBLREQ_MSGTYPE:
      body_size = Mpls_encodeLdpLblReqMsg((mplsLdpLblReqMsg_t*)msg->body,
        bodyBuf,bodyBuf_size);
      break;
    case MPLS_LBLMAP_MSGTYPE:
      body_size = Mpls_encodeLdpLblMapMsg((mplsLdpLblMapMsg_t*)msg->body,
        bodyBuf,bodyBuf_size);
      break;
    case MPLS_ADDR_MSGTYPE:
    case MPLS_ADDRWITH_MSGTYPE:
      body_size = Mpls_encodeLdpAdrMsg((mplsLdpAdrMsg_t*)msg->body,bodyBuf,
        bodyBuf_size);
      break;
    case MPLS_LBLWITH_MSGTYPE:
    case MPLS_LBLREL_MSGTYPE:
      body_size = Mpls_encodeLdpLbl_W_R_Msg((mplsLdpLbl_W_R_Msg_t*)msg->body,
        bodyBuf,bodyBuf_size);
      break;
    case MPLS_LBLABORT_MSGTYPE:
      body_size = Mpls_encodeLdpLblAbortMsg((mplsLdpLblAbortMsg_t*)msg->body,
        bodyBuf,bodyBuf_size);
      break;
    default:
      LDP_ASSERT(0);
  }

  if(body_size < 0)
    return body_size;

  if(msg->hdr != NULL) {
    ldp_free(msg->hdr);
  }
  msg->hdr = allocLdpHdr(lsraddr,label_space,body_size);
  if(msg->hdr == NULL) {
    return 0;
  }

  switch(((mplsLdpMsg_t*)msg->body)->flags.flags.msgType) {
    case MPLS_INIT_MSGTYPE:
      type = LDP_TRACE_FLAG_INIT;
      LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,LDP_TRACE_FLAG_INIT,
         printHeader(g->user_data,msg->hdr),
         printInitMsg(g->user_data,(mplsLdpInitMsg_t*)msg->body));
      break;
    case MPLS_NOT_MSGTYPE:
      type = LDP_TRACE_FLAG_NOTIF;
      LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,LDP_TRACE_FLAG_NOTIF,
         printHeader(g->user_data,msg->hdr),
         printNotMsg(g->user_data,(mplsLdpNotifMsg_t*)msg->body));
      break;
    case MPLS_KEEPAL_MSGTYPE:
      type = LDP_TRACE_FLAG_PERIODIC;
      LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,LDP_TRACE_FLAG_PERIODIC,
         printHeader(g->user_data,msg->hdr),
         printKeepAliveMsg(g->user_data,(mplsLdpKeepAlMsg_t*)msg->body));
      break;
    case MPLS_HELLO_MSGTYPE:
      type = LDP_TRACE_FLAG_PERIODIC;
      LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,LDP_TRACE_FLAG_PERIODIC,
         printHeader(g->user_data,msg->hdr),
         printHelloMsg(g->user_data,(mplsLdpHelloMsg_t*)msg->body));
      break;
    case MPLS_LBLREQ_MSGTYPE:
      type = LDP_TRACE_FLAG_LABEL;
      LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,LDP_TRACE_FLAG_LABEL,
         printHeader(g->user_data,msg->hdr),
         printLlbReqMsg(g->user_data,(mplsLdpLblReqMsg_t*)msg->body));
      break;
    case MPLS_LBLMAP_MSGTYPE:
      type = LDP_TRACE_FLAG_LABEL;
      LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,LDP_TRACE_FLAG_LABEL,
         printHeader(g->user_data,msg->hdr),
         printLlbMapMsg(g->user_data,(mplsLdpLblMapMsg_t*)msg->body));
      break;
    case MPLS_ADDR_MSGTYPE:
    case MPLS_ADDRWITH_MSGTYPE:
      type = LDP_TRACE_FLAG_ADDRESS;
      LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,LDP_TRACE_FLAG_ADDRESS,
         printHeader(g->user_data,msg->hdr),
         printAddressMsg(g->user_data,(mplsLdpAdrMsg_t*)msg->body));
      break;
    case MPLS_LBLWITH_MSGTYPE:
    case MPLS_LBLREL_MSGTYPE:
      type = LDP_TRACE_FLAG_LABEL;
      LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,LDP_TRACE_FLAG_LABEL,
         printHeader(g->user_data,msg->hdr),
         printLbl_W_R_Msg(g->user_data,(mplsLdpLbl_W_R_Msg_t*)msg->body));
      break;
    case MPLS_LBLABORT_MSGTYPE:
      type = LDP_TRACE_FLAG_LABEL;
      LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,LDP_TRACE_FLAG_LABEL,
         printHeader(g->user_data,msg->hdr),
         printLlbAbortMsg(g->user_data,(mplsLdpLblAbortMsg_t*)msg->body));
      break;
  }

  hdr_size = Mpls_encodeLdpMsgHeader(msg->hdr,hdrBuf,hdrBuf_size);
  // ldp_free(msg->hdr);

  if(hdr_size < 0)
    return hdr_size;

  LDP_DUMP_PKT(g->user_data,type,LDP_TRACE_STATE_SEND,
    ldp_pdu_dump(g->user_data,buf,buf_size,hdr_size + body_size));

  return hdr_size + body_size;
}

ldp_return_enum Mpls_decodeLdpPDU(ldp_global* g,ldp_pdu *pdu,uint8_t *buf,
   int size,int n) {
   ldp_msg          * msg = NULL;
   int                encodedSize;
   u_char           * startPtr;
   u_short            totalSize;
   u_short            type = 0;
   int                pduSize = 0;

   if(pdu == NULL) {
      return LDP_FAILURE;
   }

   startPtr = buf;

   while(pduSize < n)
   {
      msg = ldp_msg_create(0,0);
      if(msg == NULL) {
         break;
      };

      totalSize = 0;

      msg->hdr = (mplsLdpHeader_t*)ldp_malloc(sizeof(mplsLdpHeader_t));
      encodedSize = Mpls_decodeLdpMsgHeader(msg->hdr,startPtr,n);
      if (encodedSize < 0)
      {
         LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,LDP_TRACE_FLAG_PACKET,
            "Failed while decoding HEADER:%d\n",encodedSize);
         return LDP_FAILURE;
      }
      startPtr += encodedSize;

      if (msg->hdr->pduLength > size)
      {
         LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,
           LDP_TRACE_FLAG_PACKET,"Buffer too small. Decoding failed\n");
         return LDP_FAILURE;
      }

      /* found the message type */
      MEM_COPY((u_char*)&type, startPtr, 2);
      type = ntohs(type) & 0x7fff;   /* ignore the U bit for now */
      LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,LDP_TRACE_FLAG_PACKET,
         "Found type %x\n", type);

      switch(type)
      {
         case MPLS_INIT_MSGTYPE:
         {
            MPLS_MSGMALLOC(Init);
            LDP_DUMP_PKT(g->user_data,LDP_TRACE_FLAG_INIT,LDP_TRACE_STATE_RECV,
               ldp_pdu_dump(g->user_data,buf,size,n));
            encodedSize = Mpls_decodeLdpInitMsg( MPLS_MSGPARAM(Init),
                                                 startPtr,
                                                 n-MPLS_LDP_HDRSIZE-totalSize);
            LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_INIT,
               "decodedSize for Init msg = %d\n", encodedSize);
            if (encodedSize < 0)
            {
               ldp_free(MPLS_MSGPARAM(Init));
               return LDP_FAILURE;
            }
            LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,LDP_TRACE_FLAG_INIT,
               printHeader(g->user_data,msg->hdr),
               printInitMsg(g->user_data,MPLS_MSGPARAM(Init)));
            totalSize += encodedSize;
            startPtr  += encodedSize;
            msg->type = type;
            msg->body = (mplsLdpMsg_t*)MPLS_MSGPARAM(Init);
            break;
         }
         case MPLS_NOT_MSGTYPE:
         {
            MPLS_MSGMALLOC(Notif);
            LDP_DUMP_PKT(g->user_data,LDP_TRACE_FLAG_NOTIF,LDP_TRACE_STATE_RECV,
               ldp_pdu_dump(g->user_data,buf,size,n));
            encodedSize = Mpls_decodeLdpNotMsg( MPLS_MSGPARAM(Notif),
                                                startPtr,
                                                n-MPLS_LDP_HDRSIZE-totalSize);
            LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_NOTIF,
               "decodedSize for Notif msg = %d\n", encodedSize);
            if (encodedSize < 0)
            {
               ldp_free(MPLS_MSGPARAM(Notif));
               return LDP_FAILURE;
            }
            LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_NOTIF,
               printHeader(g->user_data,msg->hdr),
               printNotMsg(g->user_data,MPLS_MSGPARAM(Notif)));
            totalSize += encodedSize;
            startPtr  += encodedSize;
            msg->type = type;
            msg->body = (mplsLdpMsg_t*)MPLS_MSGPARAM(Notif);
            break;
         }
         case MPLS_KEEPAL_MSGTYPE:
         {
            MPLS_MSGMALLOC(KeepAl);
            LDP_DUMP_PKT(g->user_data,LDP_TRACE_FLAG_PERIODIC,
               LDP_TRACE_STATE_RECV,
               ldp_pdu_dump(g->user_data,buf,size,n));
            encodedSize = Mpls_decodeLdpKeepAliveMsg(MPLS_MSGPARAM(KeepAl),
                                                     startPtr,
                                                     n-MPLS_LDP_HDRSIZE-
                                                     totalSize);
            LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_PERIODIC,
               "decodedSize for KeepAlive msg = %d\n", encodedSize);
            if (encodedSize < 0)
            {
               ldp_free(MPLS_MSGPARAM(KeepAl));
               return LDP_FAILURE;
            }
            LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_PERIODIC,
               printHeader(g->user_data,msg->hdr),
               printKeepAliveMsg(g->user_data,MPLS_MSGPARAM(KeepAl)));
            totalSize += encodedSize;
            startPtr  += encodedSize;
            msg->type = type;
            msg->body = (mplsLdpMsg_t*)MPLS_MSGPARAM(KeepAl);
            break;
         }
         case MPLS_HELLO_MSGTYPE:
         {
            MPLS_MSGMALLOC(Hello);
            LDP_DUMP_PKT(g->user_data,LDP_TRACE_FLAG_PERIODIC,
               LDP_TRACE_STATE_RECV,
               ldp_pdu_dump(g->user_data,buf,size,n));
            encodedSize = Mpls_decodeLdpHelloMsg(MPLS_MSGPARAM(Hello),
                                                 startPtr,
                                                 n-MPLS_LDP_HDRSIZE-totalSize);
            LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_PERIODIC,
               "decodedSize for Hello msg = %d\n", encodedSize);
            if (encodedSize < 0)
            {
               ldp_free(MPLS_MSGPARAM(Hello));
               return LDP_FAILURE;
            }
            LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_PERIODIC,
               printHeader(g->user_data,msg->hdr),
               printHelloMsg(g->user_data,MPLS_MSGPARAM(Hello)));
            totalSize += encodedSize;
            startPtr  += encodedSize;
            msg->type = type;
            msg->body = (mplsLdpMsg_t*)MPLS_MSGPARAM(Hello);
            break;
         }
         case MPLS_LBLREQ_MSGTYPE:
         {
            MPLS_MSGMALLOC(LblReq);
            LDP_DUMP_PKT(g->user_data,LDP_TRACE_FLAG_LABEL,LDP_TRACE_STATE_RECV,
               ldp_pdu_dump(g->user_data,buf,size,n));
            encodedSize = Mpls_decodeLdpLblReqMsg(MPLS_MSGPARAM(LblReq),
                                                  startPtr,
                                                  n-MPLS_LDP_HDRSIZE-totalSize);
            LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_LABEL,
               "decodedSize for Req msg = %d\n", encodedSize);
            if (encodedSize < 0)
            {
               ldp_free(MPLS_MSGPARAM(LblReq));
               return LDP_FAILURE;
            }
            LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_LABEL,
               printHeader(g->user_data,msg->hdr),
               printLlbReqMsg(g->user_data,MPLS_MSGPARAM(LblReq)));
            totalSize += encodedSize;
            startPtr  += encodedSize;
            msg->type = type;
            msg->body = (mplsLdpMsg_t*)MPLS_MSGPARAM(LblReq);
            break;
         }
         case MPLS_LBLMAP_MSGTYPE:
         {
            MPLS_MSGMALLOC(LblMap);
            LDP_DUMP_PKT(g->user_data,LDP_TRACE_FLAG_LABEL,LDP_TRACE_STATE_RECV,
               ldp_pdu_dump(g->user_data,buf,size,n));
            encodedSize = Mpls_decodeLdpLblMapMsg(MPLS_MSGPARAM(LblMap),
                                                  startPtr,
                                                  n-MPLS_LDP_HDRSIZE-totalSize);
            LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_LABEL,
               "decodedSize for Map msg = %d\n", encodedSize);
            if (encodedSize < 0)
            {
               ldp_free(MPLS_MSGPARAM(LblMap));
               return LDP_FAILURE;
            }
            LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_LABEL,
               printHeader(g->user_data,msg->hdr),
               printLlbMapMsg(g->user_data,MPLS_MSGPARAM(LblMap)));
            totalSize += encodedSize;
            startPtr  += encodedSize;
            msg->type = type;
            msg->body = (mplsLdpMsg_t*)MPLS_MSGPARAM(LblMap);
            break;
         }
         case MPLS_ADDR_MSGTYPE:
         case MPLS_ADDRWITH_MSGTYPE:
         {
            MPLS_MSGMALLOC(Adr);
            LDP_DUMP_PKT(g->user_data,LDP_TRACE_FLAG_ADDRESS,
               LDP_TRACE_STATE_RECV,
               ldp_pdu_dump(g->user_data,buf,size,n));
            encodedSize = Mpls_decodeLdpAdrMsg(MPLS_MSGPARAM(Adr),
                                               startPtr,
                                               n-MPLS_LDP_HDRSIZE-totalSize);
            LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_ADDRESS,
               "decodedSize for Adr msg = %d\n", encodedSize);
            if (encodedSize < 0)
            {
               ldp_free(MPLS_MSGPARAM(Adr));
               return LDP_FAILURE;
            }
            LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_ADDRESS,
               printHeader(g->user_data,msg->hdr),
               printAddressMsg(g->user_data,MPLS_MSGPARAM(Adr)));
            totalSize += encodedSize;
            startPtr  += encodedSize;
            msg->type = type;
            msg->body = (mplsLdpMsg_t*)MPLS_MSGPARAM(Adr);
            break;
         }
         case MPLS_LBLWITH_MSGTYPE:
         case MPLS_LBLREL_MSGTYPE:
         {
            MPLS_MSGMALLOC(Lbl_W_R_);
            LDP_DUMP_PKT(g->user_data,LDP_TRACE_FLAG_LABEL,LDP_TRACE_STATE_RECV,
               ldp_pdu_dump(g->user_data,buf,size,n));
            encodedSize = Mpls_decodeLdpLbl_W_R_Msg(MPLS_MSGPARAM(Lbl_W_R_),
                                                    startPtr,
                                                    n-MPLS_LDP_HDRSIZE-
                                                    totalSize);
            LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_LABEL,
               "decodedSize for Lbl Release/Mapping msg = %d\n",encodedSize);
            if (encodedSize < 0)
            {
               ldp_free(MPLS_MSGPARAM(Lbl_W_R_));
               return LDP_FAILURE;
            }
            LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_LABEL,
               printHeader(g->user_data,msg->hdr),
               printLbl_W_R_Msg(g->user_data,MPLS_MSGPARAM(Lbl_W_R_)));
            totalSize += encodedSize;
            startPtr  += encodedSize;
            msg->type = type;
            msg->body = (mplsLdpMsg_t*)MPLS_MSGPARAM(Lbl_W_R_);
            break;
         }
         case MPLS_LBLABORT_MSGTYPE:
         {
            MPLS_MSGSTRUCT(LblAbort);
            LDP_DUMP_PKT(g->user_data,LDP_TRACE_FLAG_LABEL,LDP_TRACE_STATE_RECV,
               ldp_pdu_dump(g->user_data,buf,size,n));
            encodedSize = Mpls_decodeLdpLblAbortMsg(&MPLS_MSGPARAM(LblAbort),
                                                    startPtr,
                                                    n-MPLS_LDP_HDRSIZE-
                                                    totalSize);
            LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_LABEL,
               "decodedSize for Abort msg = %d\n", encodedSize);
            if (encodedSize < 0)
            {
               return LDP_FAILURE;
            }
#if 0
            LDP_TRACE_PKT(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_LABEL,
               printHeader(g->user_data,msg->hdr),
               printLlbAbortMsg(g->user_data,MPLS_MSGPARAM(LblAbort)));
#endif
            totalSize += encodedSize;
            startPtr  += encodedSize;
            break;
         }
         default:
         {
            LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,
               LDP_TRACE_FLAG_PACKET,"Unknown message type = %x\n", type);
            ldp_free(msg);
            msg = NULL;
         }
      } /* switch */
      if(msg != NULL) {
        LDP_LIST_ADD_TAIL(&pdu->msg,msg,_pdu,ldp_msg);
      }
      pduSize += totalSize + 10;
      LDP_TRACE_LOG(g->user_data,LDP_TRACE_STATE_RECV,
         LDP_TRACE_FLAG_PACKET,"Mesg size: %d (%d)\n",pduSize,n);
   } /* while */

   return LDP_SUCCESS;
}