int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz) { int nparsed = 0, crc, sen_sz, ptype; nmeaParserNODE *node = 0; NMEA_ASSERT(parser && parser->buffer); /* clear unuse buffer (for debug) */ /* memset( parser->buffer + parser->buff_use, 0, parser->buff_size - parser->buff_use ); */ /* add */ if(parser->buff_use + buff_sz >= parser->buff_size) nmea_parser_buff_clear(parser); memcpy(parser->buffer + parser->buff_use, buff, buff_sz); parser->buff_use += buff_sz; /* parse */ for(;;node = 0) { sen_sz = nmea_find_tail( (const char *)parser->buffer + nparsed, (int)parser->buff_use - nparsed, &crc); if(!sen_sz) { if(nparsed) memcpy( parser->buffer, parser->buffer + nparsed, parser->buff_use -= nparsed); break; } else if(crc >= 0) { ptype = nmea_pack_type( (const char *)parser->buffer + nparsed + 1, parser->buff_use - nparsed - 1); if(0 == (node = malloc(sizeof(nmeaParserNODE)))) goto mem_fail; node->pack = 0; switch(ptype) { case GPGGA: if(0 == (node->pack = malloc(sizeof(nmeaGPGGA)))) goto mem_fail; node->packType = GPGGA; if(!nmea_parse_GPGGA( (const char *)parser->buffer + nparsed, sen_sz, (nmeaGPGGA *)node->pack)) { free(node->pack); free(node); node = 0; } break; case GPGSA: if(0 == (node->pack = malloc(sizeof(nmeaGPGSA)))) goto mem_fail; node->packType = GPGSA; if(!nmea_parse_GPGSA( (const char *)parser->buffer + nparsed, sen_sz, (nmeaGPGSA *)node->pack)) { free(node->pack); free(node); node = 0; } break; case GPGSV: if(0 == (node->pack = malloc(sizeof(nmeaGPGSV)))) goto mem_fail; node->packType = GPGSV; if(!nmea_parse_GPGSV( (const char *)parser->buffer + nparsed, sen_sz, (nmeaGPGSV *)node->pack)) { free(node->pack); free(node); node = 0; } break; case GPRMC: if(0 == (node->pack = malloc(sizeof(nmeaGPRMC)))) goto mem_fail; node->packType = GPRMC; if(!nmea_parse_GPRMC( (const char *)parser->buffer + nparsed, sen_sz, (nmeaGPRMC *)node->pack)) { free(node->pack); free(node); node = 0; } break; case GPVTG: if(0 == (node->pack = malloc(sizeof(nmeaGPVTG)))) goto mem_fail; node->packType = GPVTG; if(!nmea_parse_GPVTG( (const char *)parser->buffer + nparsed, sen_sz, (nmeaGPVTG *)node->pack)) { free(node->pack); free(node); node = 0; } break; default: free(node); node = 0; break; }; if(node) { if(parser->end_node) ((nmeaParserNODE *)parser->end_node)->next_node = node; parser->end_node = node; if(!parser->top_node) parser->top_node = node; node->next_node = 0; } } nparsed += sen_sz; } return nparsed; mem_fail: if(node) free(node); nmea_error("Insufficient memory!"); return -1; }
int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz) { int nparsed = 0, crc, sen_sz, ptype; nmeaParserNODE *node = 0; NMEA_ASSERT(parser && parser->buffer); /* clear unuse buffer (for debug) */ /* memset( parser->buffer + parser->buff_use, 0, parser->buff_size - parser->buff_use ); */ /* add */ if(parser->buff_use + buff_sz >= parser->buff_size) nmea_parser_buff_clear(parser); memcpy(parser->buffer + parser->buff_use, buff, buff_sz); parser->buff_use += buff_sz; /* parse */ for(;;node = 0) { // return number of byte to packet tail. If find an invalid trailer or and invalid crc return 0. // if it is a valid message the function return > 0 with a crc >= 0 sen_sz = nmea_find_tail( (const char *)parser->buffer + nparsed, (int)parser->buff_use - nparsed, &crc); //printf("sen_sz: %i\n", sen_sz); if(!sen_sz) { if(nparsed) memcpy( parser->buffer, parser->buffer + nparsed, parser->buff_use -= nparsed); break; } else if(crc >= 0) { ptype = nmea_pack_type( (const char *)parser->buffer + nparsed + 1, parser->buff_use - nparsed - 1); if(0 == (node = malloc(sizeof(nmeaParserNODE)))) goto mem_fail; node->pack = 0; switch(ptype) // ADD: add packet type case here { case GPGGA: if(0 == (node->pack = malloc(sizeof(nmeaGPGGA)))) goto mem_fail; node->packType = GPGGA; if(!nmea_parse_GPGGA( (const char *)parser->buffer + nparsed, sen_sz, (nmeaGPGGA *)node->pack)) { free(node); node = 0; } break; case GPGSA: if(0 == (node->pack = malloc(sizeof(nmeaGPGSA)))) goto mem_fail; node->packType = GPGSA; if(!nmea_parse_GPGSA( (const char *)parser->buffer + nparsed, sen_sz, (nmeaGPGSA *)node->pack)) { free(node); node = 0; } break; case GPGSV: if(0 == (node->pack = malloc(sizeof(nmeaGPGSV)))) goto mem_fail; node->packType = GPGSV; if(!nmea_parse_GPGSV( (const char *)parser->buffer + nparsed, sen_sz, (nmeaGPGSV *)node->pack)) { free(node); node = 0; } break; case GPRMC: if(0 == (node->pack = malloc(sizeof(nmeaGPRMC)))) goto mem_fail; node->packType = GPRMC; if(!nmea_parse_GPRMC( (const char *)parser->buffer + nparsed, sen_sz, (nmeaGPRMC *)node->pack)) { free(node); node = 0; } break; case GPVTG: if(0 == (node->pack = malloc(sizeof(nmeaGPVTG)))) goto mem_fail; node->packType = GPVTG; if(!nmea_parse_GPVTG( (const char *)parser->buffer + nparsed, sen_sz, (nmeaGPVTG *)node->pack)) { free(node); node = 0; } break; case HCHDG: if(0 == (node->pack = malloc(sizeof(nmeaHCHDG)))) goto mem_fail; node->packType = HCHDG; if(!nmea_parse_HCHDG( (const char *)parser->buffer + nparsed, sen_sz, (nmeaHCHDG *)node->pack)) { free(node); node = 0; } break; case HCHDT: if(0 == (node->pack = malloc(sizeof(nmeaHCHDT)))) goto mem_fail; node->packType = HCHDT; if(!nmea_parse_HCHDT( (const char *)parser->buffer + nparsed, sen_sz, (nmeaHCHDT *)node->pack)) { free(node); node = 0; } break; case TIROT: if(0 == (node->pack = malloc(sizeof(nmeaTIROT)))) goto mem_fail; node->packType = TIROT; if(!nmea_parse_TIROT( (const char *)parser->buffer + nparsed, sen_sz, (nmeaTIROT *)node->pack)) { free(node); node = 0; } break; case YXXDR: if(0 == (node->pack = malloc(sizeof(nmeaYXXDR)))) goto mem_fail; node->packType = YXXDR; if(!nmea_parse_YXXDR( (const char *)parser->buffer + nparsed, sen_sz, (nmeaYXXDR *)node->pack)) { free(node); node = 0; } break; default: free(node); node = 0; break; }; if(node) { if(parser->end_node) ((nmeaParserNODE *)parser->end_node)->next_node = node; parser->end_node = node; if(!parser->top_node) parser->top_node = node; node->next_node = 0; } } nparsed += sen_sz; } return nparsed; mem_fail: if(node) free(node); nmea_error("Insufficient memory!"); return -1; }