API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Oct 21st, 2019
138
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 13.44 KB | None | 0 0
raw download clone embed print report
  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. // }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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!