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;
}
