| 9 | #include "crc.h" |
| 10 | |
| 11 | int8_t HDLCParser::parse(uint8_t *d, DataParserContext &ctx) { |
| 12 | int len; |
| 13 | |
| 14 | uint8_t* ptr; |
| 15 | if(ctx.length < 3) |
| 16 | return DATA_PARSE_INCOMPLETE; |
| 17 | |
| 18 | HDLCHeader* h = (HDLCHeader*) d; |
| 19 | ptr = (uint8_t*) &h[1]; |
| 20 | |
| 21 | // Frame format type 3 |
| 22 | if((h->format & 0xF0) == 0xA0) { |
| 23 | // Length field (11 lsb of format) |
| 24 | len = (ntohs(h->format) & 0x7FF) + 2; |
| 25 | if(len > ctx.length) |
| 26 | return DATA_PARSE_INCOMPLETE; |
| 27 | |
| 28 | HDLCFooter* f = (HDLCFooter*) (d + len - sizeof *f); |
| 29 | |
| 30 | // First and last byte should be HDLC_FLAG |
| 31 | if(h->flag != HDLC_FLAG || f->flag != HDLC_FLAG) |
| 32 | return DATA_PARSE_BOUNDARY_FLAG_MISSING; |
| 33 | |
| 34 | // Verify FCS |
| 35 | if(f->fcs > 0 && ntohs(f->fcs) != crc16_x25(d + 1, len - sizeof *f - 1)) |
| 36 | return DATA_PARSE_FOOTER_CHECKSUM_ERROR; |
| 37 | |
| 38 | // Skip destination address, LSB marks last byte |
| 39 | while(((*ptr) & 0x01) == 0x00) { |
| 40 | ptr++; |
| 41 | } |
| 42 | ptr++; |
| 43 | |
| 44 | // Skip source address, LSB marks last byte |
| 45 | while(((*ptr) & 0x01) == 0x00) { |
| 46 | ptr++; |
| 47 | } |
| 48 | ptr++; |
| 49 | |
| 50 | HDLC3CtrlHcs* t3 = (HDLC3CtrlHcs*) (ptr); |
| 51 | |
| 52 | // Verify HCS |
| 53 | if(t3->hcs > 0 && ntohs(t3->hcs) != crc16_x25(d + 1, ptr-d)) |
| 54 | return DATA_PARSE_HEADER_CHECKSUM_ERROR; |
| 55 | ptr += 3; |
| 56 | |
| 57 | // Exclude all of header and 3 byte footer |
| 58 | ctx.length -= ptr-d; |
| 59 | if(ctx.length > 1) { |
| 60 | ctx.length -= 3; |
| 61 | } |
| 62 | |
| 63 | // Payload incomplete |
| 64 | if((h->format & 0x08) == 0x08) { |
| 65 | if(lastSequenceNumber == 0) { |
| 66 | if(buf == NULL) buf = (uint8_t *)malloc((size_t)1024); |
| 67 | pos = 0; |
| 68 | } |
nothing calls this directly
no test coverage detected