SHARE
TWEET

Untitled

a guest Oct 21st, 2019 93 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "packet_interface.h"
  2. /* Extra #includes */
  3. /* Don 't forget to include - lz flag ! */
  4. #include <stdio.h>
  5. #include <stdlib.h>
  6. #include <zlib.h>
  7. #include <arpa/inet.h>
  8. #include <string.h>
  9.  
  10. struct __attribute__((__packed__)) pkt {
  11.   uint8_t pwindow:5;
  12.   uint8_t ptr:1;
  13.   ptypes_t ptype:2;
  14.   uint16_t plength;
  15.   uint8_t pseqnum;
  16.   uint32_t ptimestamp;
  17.   uint32_t pcrc1;
  18.   char* ppayload;
  19.   uint32_t pcrc2;
  20. };
  21.  
  22. pkt_t* pkt_new()
  23. {
  24.   return (pkt_t *) calloc(1,sizeof(pkt_t));
  25. }
  26.  
  27. void pkt_del(pkt_t *pkt)
  28. {
  29.   if (pkt==NULL) {
  30.     return;
  31.   }else if (pkt->ppayload != NULL) {
  32.     free(pkt-> ppayload);
  33.   }
  34.   free(pkt);
  35. }
  36.  
  37. pkt_status_code pkt_decode(const char *data, const size_t len, pkt_t *pkt)
  38. {
  39.   if (len == 0 || data == NULL) {
  40.     fprintf(stderr, "Erreur decode E_UNCONSISTENT\n");
  41.     return E_UNCONSISTENT;
  42.   }
  43.   if (len < sizeof(uint8_t)*11) {
  44.     fprintf(stderr, "Erreur decode E_NOHEADER1\n");
  45.     return E_NOHEADER;
  46.   }
  47.   /* Read first bytes */
  48.   memcpy(pkt, data, sizeof(uint8_t));
  49.  
  50.   //Read length /!\ TODO : Clear this stuff ! not clear what "data" means when "data" in varuint_decode != "data" in pkt_decode !
  51.   uint16_t tmplength;
  52.   ssize_t sizeoflength = varuint_decode((uint8_t*)data+sizeof(uint8_t),varuint_len((uint8_t *) data+sizeof(uint8_t)),&tmplength);
  53.   if (sizeoflength == -1) {
  54.     fprintf(stderr, "Erreur decode sizeoflength\n");
  55.     return E_LENGTH;
  56.   }
  57.   if(pkt_set_length(pkt,tmplength) != PKT_OK){
  58.     fprintf(stderr, "Erreur decode set length\n");
  59.     return E_LENGTH;
  60.   }
  61.   size_t sizeAfterLength = sizeof(uint8_t)+sizeoflength;
  62.  
  63.   /*Read Seqnum*/
  64.   memcpy(&(pkt->pseqnum),data+sizeAfterLength,sizeof(uint8_t));
  65.  
  66.   /*Read timestamp*/
  67.   memcpy(&(pkt->ptimestamp),data+sizeAfterLength+sizeof(uint8_t),sizeof(uint32_t));
  68.  
  69.   ssize_t headersize = predict_header_length(pkt);
  70.   if ((size_t) headersize != sizeAfterLength+sizeof(uint8_t)+sizeof(uint32_t)) {
  71.     fprintf(stderr, "Erreur headersize\n");
  72.     return E_UNCONSISTENT;
  73.   }
  74.   // if (len < (size_t)headersize) {
  75.   //   fprintf(stderr, "Erreur decode E_NOHEADER2\n");
  76.   //   return E_NOHEADER;
  77.   // }
  78.  
  79.   /*Read CRC1*/
  80.   memcpy(&(pkt->pcrc1),data+headersize,sizeof(uint32_t));
  81.  
  82.   //Attention network byte order
  83.   //pkt->pseqnum = ntohs(pkt->pseqnum);
  84.   pkt->pcrc1 = ntohl(pkt->pcrc1);
  85.  
  86.   /*Verify header*/
  87.   //Type
  88.   if (pkt->ptype != PTYPE_ACK && pkt->ptype != PTYPE_DATA && pkt->ptype != PTYPE_NACK) {
  89.     fprintf(stderr, "Erreur liee au champ Type\n");
  90.     return E_TYPE;
  91.   }
  92.   //TR
  93.   if (pkt->ptr == 1 && pkt->ptype != PTYPE_DATA) {
  94.     fprintf(stderr, "Erreur liee au champ TR\n");
  95.     return E_TR;
  96.   }
  97.   //Window
  98.   if (pkt->pwindow > MAX_WINDOW_SIZE) {
  99.     fprintf(stderr, "Erreur liee au champ Window\n");
  100.     return E_WINDOW;
  101.   }
  102.  
  103.   //Lenght
  104.   if (pkt->plength > MAX_PAYLOAD_SIZE) {
  105.     fprintf(stderr, "Erreur Max Payload\n");
  106.     return E_LENGTH;
  107.   }
  108.  
  109.   //crc1
  110.   if (pkt->ptr == 1) {
  111.     uint8_t tr = pkt->ptr;
  112.     pkt->ptr = 0;//TR a 0 lors du crc
  113.     char* dataTR0 = malloc(headersize);
  114.     if (!dataTR0) {
  115.       fprintf(stderr, "Erreur malloc dataTR0\n");
  116.       return E_NOMEM;
  117.     }
  118.  
  119.     memcpy(dataTR0,pkt,sizeof(uint8_t));
  120.     size_t sizeoflentr0 = varuint_encode(pkt_get_length(pkt), (uint8_t*) dataTR0+sizeof(uint8_t),sizeof(uint16_t));
  121.     //uint8_t tmpseqTR0 = htons(pkt_get_seqnum(pkt));
  122.     memcpy(dataTR0+sizeof(uint8_t)+sizeoflentr0,&(pkt->pseqnum),sizeof(uint8_t));
  123.     memcpy(dataTR0+sizeof(uint8_t)+sizeoflentr0+sizeof(uint8_t), &(pkt->ptimestamp), sizeof(uint32_t));
  124.  
  125.     uint32_t crc1decode = (uint32_t) crc32(0L,Z_NULL,0);
  126.     if((uint32_t) crc32(crc1decode,(Bytef *)dataTR0,headersize) != pkt->pcrc1){
  127.       free(dataTR0);
  128.       fprintf(stderr, "Erreur calcul crc1\n");
  129.       pkt->ptr = tr;
  130.       return E_CRC;
  131.     }
  132.     free(dataTR0);
  133.     pkt->ptr=tr;
  134.   }else{
  135.     uint32_t crc1decode0 = (uint32_t) crc32(0L,Z_NULL,0);
  136.     crc1decode0 = (uint32_t) crc32(crc1decode0,(Bytef *)data,headersize);
  137.     if (crc1decode0 != pkt_get_crc1(pkt)) {
  138.       fprintf(stderr, "Erreur calcul crc1\n");
  139.       return E_CRC;
  140.     }
  141.   }
  142.  
  143.   /*Read Payload*/
  144.   if (pkt_get_type(pkt) == PTYPE_DATA && pkt_get_length(pkt) !=0 ) {
  145.     pkt_set_payload(pkt,data+headersize+sizeof(uint32_t),pkt_get_length(pkt));
  146.     memcpy(&(pkt->pcrc2),data+headersize+sizeof(uint32_t)+pkt_get_length(pkt),sizeof(uint32_t));
  147.     //Attention ntoh
  148.     pkt->pcrc2 = ntohl(pkt->pcrc2);
  149.  
  150.     //CRC2
  151.     uint32_t crc2decode = (uint32_t) crc32(0L, Z_NULL, 0);
  152.     if ((uint32_t) crc32(crc2decode,(Bytef *) data+headersize+sizeof(uint32_t), pkt_get_length(pkt)) != pkt->pcrc2) {
  153.       fprintf(stderr, "Erreur crc2\n");
  154.       return E_CRC;
  155.     }
  156.     return PKT_OK;
  157.   }
  158.   pkt->pcrc2 = 0;
  159.  
  160.   //All OK
  161.   return PKT_OK;
  162.  
  163. }
  164.  
  165. pkt_status_code pkt_encode(const pkt_t* pkt, char *buf, size_t *len)
  166. {
  167.   if (pkt == NULL) {
  168.     fprintf(stderr, "Erreur encode pkt null\n");
  169.     return E_UNCONSISTENT;
  170.   }
  171.   if (buf == NULL) {
  172.     fprintf(stderr, "Erreur encode buf null\n");
  173.     return E_NOMEM;
  174.   }
  175.   if (len == NULL) {
  176.     fprintf(stderr, "Erreur encode len\n");
  177.     return E_LENGTH;
  178.   }
  179.   if (pkt_get_tr(pkt) == 1 && (pkt_get_type(pkt) != PTYPE_DATA)) {
  180.     fprintf(stderr, "Erreur encode type and tr\n");
  181.     return E_UNCONSISTENT;
  182.   }
  183.   if (*len < predict_header_length(pkt)+sizeof(uint32_t)) {
  184.     fprintf(stderr, "Erreur encode len too small\n");
  185.     return E_NOMEM;
  186.   }
  187.   if (*len < predict_header_length(pkt)+sizeof(uint64_t)+pkt_get_length(pkt) && pkt->ptype != PTYPE_DATA) {
  188.     fprintf(stderr, "Erreur encode len too small\n");
  189.     return E_NOMEM;
  190.   }
  191.  
  192.   //Put first byte
  193.   memcpy(buf,pkt,sizeof(uint8_t));
  194.  
  195.   //Put Lenght
  196.   size_t afterlen = 0;
  197.  
  198.   ssize_t lenlenght = varuint_encode(pkt->plength, (uint8_t *) buf+sizeof(uint8_t) ,sizeof(uint16_t));
  199.   if (lenlenght == -1) {
  200.     fprintf(stderr, "Erreur lenlenght\n");
  201.     return E_LENGTH;
  202.   }
  203.   afterlen = lenlenght + sizeof(uint8_t);
  204.  
  205.   //Put Seqnum
  206.   //uint8_t tmpseq = htons(pkt->pseqnum);
  207.   memcpy(buf+afterlen, &(pkt->pseqnum), sizeof(uint8_t));
  208.   //Put timestamp
  209.   memcpy(buf+afterlen+sizeof(uint8_t),&(pkt->ptimestamp), sizeof(uint32_t));
  210.  
  211.   size_t headersizeencode = predict_header_length(pkt);
  212.   if (headersizeencode != afterlen+sizeof(uint8_t)+sizeof(uint32_t)) {
  213.     fprintf(stderr, "Erreur headersize\n");
  214.     return E_UNCONSISTENT;
  215.   }
  216.  
  217.   //Put crc1
  218.   uint32_t crc1calc = crc32(0L, Z_NULL, 0);
  219.   crc1calc = crc32(crc1calc, (Bytef *) buf, headersizeencode);
  220.   crc1calc = htonl(crc1calc);
  221.   memcpy(buf+headersizeencode, &(crc1calc), sizeof(uint32_t));
  222.  
  223.   //Put payload & crc2
  224.   *len = (size_t) headersizeencode+sizeof(uint32_t);
  225.   if(pkt->ptype == PTYPE_DATA && pkt_get_length(pkt) != 0){
  226.     memcpy(buf+headersizeencode+sizeof(uint32_t), pkt->ppayload,pkt_get_length(pkt));
  227.     uint32_t crc2calc = crc32(0L, Z_NULL, 0);
  228.     crc2calc = crc32(crc2calc,(Bytef *)buf+headersizeencode+sizeof(uint32_t),pkt_get_length(pkt));
  229.     crc2calc = htonl(crc2calc);
  230.     memcpy(buf+headersizeencode+sizeof(uint32_t)+pkt_get_length(pkt), &(crc2calc),sizeof(uint32_t));
  231.     *len = (size_t) headersizeencode + sizeof(uint64_t)+pkt_get_length(pkt);
  232.   }
  233.   // fprintf(stderr, "Encode len final %ld\n", *len);
  234.   return PKT_OK;
  235. }
  236.  
  237. ptypes_t pkt_get_type  (const pkt_t* pkt)
  238. {
  239.   return pkt->ptype;
  240. }
  241.  
  242. uint8_t  pkt_get_tr(const pkt_t* pkt)
  243. {
  244.   return pkt->ptr;
  245. }
  246.  
  247. uint8_t  pkt_get_window(const pkt_t* pkt)
  248. {
  249.   return pkt->pwindow;
  250. }
  251.  
  252. uint8_t  pkt_get_seqnum(const pkt_t* pkt)
  253. {
  254.   return pkt->pseqnum;
  255. }
  256.  
  257. uint16_t pkt_get_length(const pkt_t* pkt)
  258. {
  259.   return pkt->plength;
  260. }
  261.  
  262. uint32_t pkt_get_timestamp   (const pkt_t* pkt)
  263. {
  264.   return pkt->ptimestamp;
  265. }
  266.  
  267. uint32_t pkt_get_crc1   (const pkt_t* pkt)
  268. {
  269.   return pkt->pcrc1;
  270. }
  271.  
  272. uint32_t pkt_get_crc2   (const pkt_t* pkt)
  273. {
  274.   return pkt->pcrc2;
  275. }
  276.  
  277. const char* pkt_get_payload(const pkt_t* pkt)
  278. {
  279.   if (pkt->ptype == PTYPE_DATA && pkt->plength != 0 && pkt->ptr != 1) {
  280.     return pkt->ppayload;
  281.   }
  282.   return NULL;
  283. }
  284.  
  285.  
  286. pkt_status_code pkt_set_type(pkt_t *pkt, const ptypes_t type)
  287. {
  288.   if (pkt == NULL) {
  289.     return E_UNCONSISTENT;
  290.   }
  291.   if(type!= PTYPE_DATA && type != PTYPE_ACK && type != PTYPE_NACK) {
  292.     fprintf(stderr, "Erreur de type\n");
  293.     return E_TYPE;
  294.   }
  295.   pkt->ptype = type;
  296.   return PKT_OK;
  297. }
  298.  
  299. pkt_status_code pkt_set_tr(pkt_t *pkt, const uint8_t tr)
  300. {
  301.   if (pkt == NULL) {
  302.     return E_UNCONSISTENT;
  303.   }
  304.   if(pkt_get_type(pkt)!=PTYPE_DATA && tr!=0) {
  305.     fprintf(stderr, "Erreur set_tr\n");
  306.     return E_TR;
  307.   }
  308.   pkt->ptr=tr;
  309.   return PKT_OK;
  310. }
  311.  
  312. pkt_status_code pkt_set_window(pkt_t *pkt, const uint8_t window)
  313. {
  314.   if (pkt == NULL) {
  315.     return E_UNCONSISTENT;
  316.   }
  317.   if(window > MAX_WINDOW_SIZE) {
  318.     fprintf(stderr, "Erreur : set_window\n");
  319.     return E_WINDOW;
  320.   }
  321.   pkt->pwindow=window;
  322.   return PKT_OK;
  323. }
  324.  
  325. pkt_status_code pkt_set_seqnum(pkt_t *pkt, const uint8_t seqnum)
  326. {
  327.   if (pkt == NULL) {
  328.     return E_UNCONSISTENT;
  329.   }
  330.   pkt->pseqnum = seqnum;
  331.   return PKT_OK;
  332. }
  333.  
  334. pkt_status_code pkt_set_length(pkt_t *pkt, const uint16_t length)
  335. {
  336.   if (pkt == NULL) {
  337.     return E_UNCONSISTENT;
  338.   }
  339.   if(length>MAX_PAYLOAD_SIZE) {
  340.     fprintf(stderr, "Erreur pkt_set_length1\n");
  341.     return E_LENGTH;
  342.   }
  343.   pkt->plength = length;
  344.   return PKT_OK;
  345. }
  346.  
  347. pkt_status_code pkt_set_timestamp(pkt_t *pkt, const uint32_t timestamp)
  348. {
  349.   if (pkt == NULL) {
  350.     return E_UNCONSISTENT;
  351.   }
  352.   pkt->ptimestamp=timestamp;
  353.   return PKT_OK;
  354. }
  355.  
  356. pkt_status_code pkt_set_crc1(pkt_t *pkt, const uint32_t crc1)
  357. {
  358.   if (pkt == NULL) {
  359.     return E_UNCONSISTENT;
  360.   }
  361.   pkt->pcrc1=crc1;
  362.   return PKT_OK;
  363. }
  364.  
  365. pkt_status_code pkt_set_crc2(pkt_t *pkt, const uint32_t crc2)
  366. {
  367.   if (pkt == NULL) {
  368.     return E_UNCONSISTENT;
  369.   }
  370.   pkt->pcrc2=crc2;
  371.   return PKT_OK;
  372. }
  373.  
  374. pkt_status_code pkt_set_payload(pkt_t *pkt,const char *data,const uint16_t length)
  375. {
  376.   if (pkt == NULL || data == NULL) {
  377.     return E_UNCONSISTENT;
  378.   }
  379.   if(pkt_get_tr(pkt)!=0){
  380.     fprintf(stderr, "Erreur set payload E_UNCONSISTENT\n");
  381.     return E_UNCONSISTENT;
  382.   }
  383.   if (length > MAX_PAYLOAD_SIZE) {
  384.     fprintf(stderr, "Erreur set payload length too large\n");
  385.     return E_LENGTH;
  386.   }
  387.  
  388.   if(pkt->ppayload!=NULL){
  389.     free(pkt->ppayload);
  390.   }
  391.  
  392.   pkt->ppayload = malloc(length);
  393.   if(pkt->ppayload==NULL)
  394.   {
  395.     fprintf(stderr, "Erreur set payload E_NOMEM\n");
  396.     return E_NOMEM;
  397.   }
  398.   memcpy(pkt->ppayload, data, length);
  399.  
  400.   if(pkt_set_tr(pkt, 0)!=PKT_OK) {
  401.     fprintf(stderr, "Erreur set payload tr\n");
  402.     return E_TR;
  403.   }
  404.   if(pkt_set_type(pkt, PTYPE_DATA)!=PKT_OK) {
  405.     fprintf(stderr, "Erreur set payload type\n");
  406.     return E_TYPE;
  407.   }
  408.   if (pkt_set_length(pkt,length) != PKT_OK) {
  409.     fprintf(stderr, "Erreur set payload le\n");
  410.     return E_LENGTH;
  411.   }
  412.   return PKT_OK;
  413. }
  414.  
  415.  
  416. ssize_t varuint_decode(const uint8_t *data, const size_t len, uint16_t *retval)
  417. {
  418.   size_t sizeofdata = varuint_len(data);
  419.   if (sizeofdata > len || (int) sizeofdata == -1) {
  420.     return -1;
  421.   }
  422.   if (sizeofdata == sizeof(uint8_t)) {
  423.     *retval = *data & 127;
  424.   }else{
  425.     *retval = ntohs(*(uint16_t *) data) & 32767;
  426.   }
  427.   return sizeofdata;
  428. }
  429.  
  430.  
  431. ssize_t varuint_encode(uint16_t val, uint8_t *data, const size_t len)
  432. {
  433.   size_t sizeofvaruint = varuint_predict_len(val);
  434.   if (sizeofvaruint > len || (int)sizeofvaruint == -1) {
  435.     return -1;
  436.   }
  437.   if (sizeofvaruint == sizeof(uint8_t)) {
  438.     uint8_t nval = (uint8_t) val;
  439.     memcpy(data,&nval,sizeofvaruint);
  440.   }else{
  441.     uint16_t nval = htons(1<<15| val);
  442.     memcpy(data,&nval,sizeofvaruint);
  443.   }
  444.   return sizeofvaruint;
  445. }
  446.  
  447. size_t varuint_len(const uint8_t *data)
  448. {
  449.   if((*data & 1<<7) != 0){
  450.     return sizeof(uint16_t);
  451.   }else {
  452.     return sizeof(uint8_t);
  453.   }
  454. }
  455.  
  456.  
  457. ssize_t varuint_predict_len(uint16_t val)
  458. {
  459.   if(val >= 0x8000){
  460.     return -1;
  461.   }
  462.   if (val <= 127) {
  463.     return sizeof(uint8_t);
  464.   }
  465.   return sizeof(uint16_t);
  466. }
  467.  
  468.  
  469. ssize_t predict_header_length(const pkt_t *pkt)
  470. {
  471.   if(pkt->plength >= 0x8000){
  472.     return -1;
  473.   }
  474.   if(varuint_predict_len(pkt->plength) == sizeof(uint8_t)){
  475.     return (sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t));
  476.   }else if (varuint_predict_len(pkt->plength) == sizeof(uint16_t)) {
  477.     return (sizeof(uint64_t));
  478.   }else{
  479.     return -1;
  480.   }
  481. }
  482. //
  483. //
  484. // int main(int argc, char const *argv[]) {
  485. //   pkt_t* pkt = pkt_new();
  486. //   pkt_set_type(pkt,PTYPE_DATA);
  487. //   pkt_set_tr(pkt,(uint8_t)0);
  488. //   pkt_set_window(pkt,(uint8_t)5);
  489. //   pkt_set_seqnum(pkt,(uint8_t)42);
  490. //   pkt_set_length(pkt,(uint16_t)513);
  491. //   pkt_set_timestamp(pkt,(uint32_t)11111111);
  492. //   pkt_set_crc1(pkt,(uint32_t)0);
  493. //   pkt_set_payload(pkt,(char *) 512,sizeof(uint16_t));
  494. //   pkt_set_crc2(pkt,(uint32_t)0);
  495. //   char* buf = malloc(8*sizeof(uint64_t));
  496. //   size_t leng = 8*sizeof(uint64_t);
  497. //   pkt_encode(pkt,buf,&leng);
  498. //   pkt_del(pkt);
  499. //
  500. //   pkt_t* pktout = pkt_new();
  501. //   pkt_decode(buf,8*sizeof(uint64_t),pktout);
  502. //
  503. //   free(buf);
  504. //
  505. //   fprintf(stdout, "%u\n",pkt_get_type(pktout));
  506. //   fprintf(stdout, "%u\n",pkt_get_tr(pktout));
  507. //   fprintf(stdout, "%u\n",pkt_get_window(pktout));
  508. //   fprintf(stdout, "%u\n",pkt_get_bigl(pktout));
  509. //   fprintf(stdout, "%u\n",pkt_get_length(pktout));
  510. //   fprintf(stdout, "%u\n",pkt_get_seqnum(pktout));
  511. //   fprintf(stdout, "%u\n",pkt_get_timestamp(pktout));
  512. //   fprintf(stdout, "%u\n",pkt_get_crc1(pktout));
  513. //   //fprintf(stdout, "%u\n",pkt_get_payload(pktout));
  514. //   fprintf(stdout, "%u\n",pkt_get_crc2(pktout));
  515. //
  516. //   return 0;
  517. // }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top