uint8_t* fp_reallocate (fp_pool_t p, uint8_t* bp, fp_size_t min_size, fp_size_t max_size, uint8_t** fragment_endp) { fp_fragment_t f = get_fragment(p, bp); fp_fragment_t frs; fp_fragment_t fre; fp_size_t original_min_size; fp_size_t frlen; fp_fragment_t bf; fp_size_t bflen; const fp_fragment_t fe = p->fragment + p->fragment_count; fp_size_t copy_len; /* Validate arguments */ if ((NULL == f) || (! FRAGMENT_IS_ALLOCATED(f)) || (0 >= min_size) || (min_size > max_size) || (NULL == fragment_endp)) { return NULL; } original_min_size = min_size; min_size = align_size_up(p, min_size); if (FP_MAX_FRAGMENT_SIZE != max_size) { max_size = align_size_up(p, max_size); } /* Create hooks for a pseudo-slot at f0 for flen octets, * representing what would happen if this fragment were released. */ frs = fre = f; frlen = -f->length; if ((frs > p->fragment) && FRAGMENT_IS_AVAILABLE(frs-1)) { frs = f-1; frlen += frs->length; } if (((fre+1) < fe) && FRAGMENT_IS_AVAILABLE(fre+1)) { fre = f+1; frlen += fre->length; } bf = NULL; bflen = 0; { fp_fragment_t xf = p->fragment; /* Same logic as find_best_fragment, but treat the sequence around * the current fragment as a single fragment */ do { fp_ssize_t flen = xf->length; if (xf == frs) { flen = frlen; } if (min_size <= flen) { if ((NULL == bf) || PREFER_NEW_SIZE(flen, bflen, max_size)) { bf = xf; bflen = flen; } } if (xf == frs) { xf = fre; } } while (++xf < fe); } /* If nothing can satisfy the minimum, fail. */ if (NULL == bf) { return NULL; } /* Save the minimum of the current fragment length and the desired * new size */ copy_len = -f->length; if (copy_len > original_min_size) { copy_len = original_min_size; } /* If best is same fragment, just resize */ if (bf == f) { /* == frs */ return fp_resize(p, bp, max_size, fragment_endp); } /* If best is available fragment preceding this fragment, shift the * data. */ if (bf == frs) { fp_size_t ffrs_len; fp_size_t new_len; if (f < fre) { merge_adjacent_available(f, fe); } memmove(frs->start, f->start, copy_len); ffrs_len = frs->length - f->length; new_len = ffrs_len; if (new_len > max_size) { new_len = max_size; } frs->length = -new_len; *fragment_endp = frs->start + new_len; if (ffrs_len == new_len) { while ((++f < fe) && (! FRAGMENT_IS_INACTIVE(f))) { f[-1] = f[0]; } f[-1].length = 0; } else { f->start = *fragment_endp; f->length = ffrs_len - new_len; } return frs->start; } bp = complete_allocation(p, bf, max_size, fragment_endp); memmove(bp, f->start, copy_len); fp_release(p, f->start); return bp; }
static int nmea_rx_isr_ni (const struct sBSP430halISRVoidChainNode * cb, void * context) { sRxState * sp = (sRxState *)cb; sBSP430halSERIAL * hal = (sBSP430halSERIAL *) context; sNMEAmessage * cp = &sp->curmsg; int rv = 0; switch (sp->state) { case SRX_unsync: if (NULL != cp->message) { if (NULL != serial_cb) { rv |= serial_cb(NULL, sp->message_idx, cp->timestamp_utt); } (void)fp_release(sp->pool, cp->message); cp->message = NULL; } if ('$' == hal->rx_byte) { sp->state = SRX_store_NMEA; } else if (0xA0 == hal->rx_byte) { sp->state = SRX_start_Binary; } else { break; } cp->timestamp_utt = ulBSP430uptime_ni(); sp->message_idx = 0; sp->csum_calc = 0; cp->message = fp_request(sp->pool, 4, FP_MAX_FRAGMENT_SIZE, &cp->message_endp); if (NULL == cp->message) { sp->state = SRX_unsync; if (NULL != serial_cb) { rv |= serial_cb(NULL, 0, cp->timestamp_utt); } } break; case SRX_store_NMEA: if ('*' == hal->rx_byte) { if ((cp->message + sp->message_idx) == cp->message_endp) { sp->state = SRX_unsync; break; } cp->message[sp->message_idx] = 0; sp->message_idx += 1; sp->state = SRX_read_csum_NMEA; break; } /*FALLTHRU*/ case SRX_store_Binary: /* If attempt to store would go beyond end, abort the message. */ if ((cp->message + sp->message_idx) == cp->message_endp) { sp->state = SRX_unsync; break; } /* Add character to checksum and accumulated buffer */ sp->csum_calc ^= hal->rx_byte; cp->message[sp->message_idx] = hal->rx_byte; sp->message_idx += 1; if ((SRX_store_Binary == sp->state) && (sp->message_idx == sp->length_rx)) { sp->state = SRX_read_csum_Binary; } break; case SRX_start_Binary: sp->length_rx = 0; sp->state = (0xA1 == hal->rx_byte) ? SRX_read_len_Binary : SRX_unsync; break; case SRX_read_csum_NMEA: case SRX_read_csum_NMEA_2: if (! IS_HEXDIGIT(hal->rx_byte)) { sp->state = SRX_unsync; break; } sp->csum_rx = (sp->csum_rx << 4) | HEXDIGIT_VALUE(hal->rx_byte); if (SRX_read_csum_NMEA == sp->state) { sp->state = SRX_read_csum_NMEA_2; break; } /*FALLTHRU*/ validate: if (sp->csum_rx == sp->csum_calc) { if (NULL != serial_cb) { uint8_t * msg = fp_resize(sp->pool, cp->message, sp->message_idx, &cp->message_endp); cp->message = NULL; rv |= serial_cb(msg, sp->message_idx, cp->timestamp_utt); } } sp->state = SRX_unsync; break; case SRX_read_csum_Binary: sp->csum_rx = hal->rx_byte; goto validate; case SRX_read_len_Binary: sp->length_rx = hal->rx_byte; sp->state = SRX_read_len_Binary_2; break; case SRX_read_len_Binary_2: sp->length_rx = (sp->length_rx << 8) | hal->rx_byte; sp->state = SRX_store_Binary; break; } return rv; }