/************************************************************************
* NAME: fnet_netbuf_trim
*
* DESCRIPTION: Trims len bytes from the begin of the net_buf data if len
* is positive. Otherwise len bytes should be trimmed from the
* end of net_buf buffer. If len=0 - do nothing
*************************************************************************/
void fnet_netbuf_trim( fnet_netbuf_t **nb_ptr, fnet_int32_t len )
{
fnet_netbuf_t *head_nb;
fnet_netbuf_t *nb;
fnet_size_t tot_len;
fnet_size_t total_rem;
fnet_netbuf_t *tmp_nb;
if(len == 0)
{
return;
}
tmp_nb = (fnet_netbuf_t *) *nb_ptr;
nb = (fnet_netbuf_t *) *nb_ptr;
head_nb = nb;
/* If the quantity of trimmed bytes is greater than net_buf size - do nothing.*/
if((nb->total_length < (fnet_size_t)(len > 0 ? len : -len)) || (nb == 0))
{
return;
}
tot_len = nb->length;
total_rem = nb->total_length;
if(len > 0) /* Trim len bytes from the begin of the buffer.*/
{
while((nb != 0) && ((fnet_size_t)len >= tot_len))
{
*nb_ptr = nb->next;
nb = fnet_netbuf_free(nb); /* In some cases we delete some net_bufs.*/
if(nb != 0)
{
tot_len += nb->length;
}
}
if(nb != 0)
{
nb->data_ptr = (fnet_uint8_t *)nb->data_ptr + /* Or change pointer. */
nb->length - (tot_len - (fnet_size_t)len);
nb->length = tot_len - (fnet_size_t)len;
nb->flags = tmp_nb->flags;
}
}
else /* Trim len bytes from the end of the buffer. */
{
while((nb != 0) && ((fnet_int32_t)total_rem + len > (fnet_int32_t)tot_len))
{
nb = nb->next; /* Run up to the first net_buf, which points */
/* to the data, which should be erased.*/
if(nb != 0)
{
tot_len += nb->length;
}
}
/* Cut the part of the first net_buf, which should be modified.*/
if(nb != 0)
{
nb->length += ((fnet_size_t)len + total_rem - tot_len);
while(nb->next != 0) /* Cut the redundant net_bufs. */
{
nb->next = fnet_netbuf_free(nb->next);
}
if(nb->length == 0u) /* if |len| == total_length */
{
head_nb = fnet_netbuf_free(nb);
}
nb = head_nb;
}
}
if(nb != 0)
{
nb->total_length = (total_rem - (fnet_size_t)(len > 0 ? len : -len));
}
*nb_ptr = nb;
}
/************************************************************************
* NAME: fnet_netbuf_copy
*
* DESCRIPTION: Creates a new net_buf using data buffer,
* which was created before for another net_buf.
*************************************************************************/
fnet_netbuf_t *fnet_netbuf_copy( fnet_netbuf_t *nb, fnet_size_t offset, fnet_size_t len, fnet_bool_t drain )
{
fnet_netbuf_t *loc_nb, *loc_nb_head, *tmp_nb;
fnet_int32_t tot_len = 0;
fnet_size_t tot_offset = 0u;
tmp_nb = nb;
do /* Calculate the total length of the buf for current net_buf chain.*/
{
tot_len += (fnet_int32_t)tmp_nb->length;
tmp_nb = tmp_nb->next;
} while (tmp_nb);
if(len == FNET_NETBUF_COPYALL)
{
tot_len -= (fnet_int32_t)offset;
len = (fnet_size_t)tot_len;
}
else
{
if(((len + offset) > (fnet_size_t)tot_len ) || ((fnet_size_t)tot_len < offset))
{
return (fnet_netbuf_t *)0;
}
}
/* In tot_len finally - the size of required net_buf data*/
tmp_nb = nb;
loc_nb = (fnet_netbuf_t *)fnet_malloc_netbuf(sizeof(fnet_netbuf_t));
if((loc_nb == 0) && drain)
{
fnet_prot_drain();
loc_nb = (fnet_netbuf_t *)fnet_malloc_netbuf(sizeof(fnet_netbuf_t));
}
if(loc_nb == 0)
{
return (fnet_netbuf_t *)0;
}
loc_nb_head = loc_nb; /* Save the head of net_buf chain.*/
loc_nb->next_chain = (fnet_netbuf_t *)0;
loc_nb->total_length = (fnet_size_t)len;
loc_nb->flags = nb->flags;
if(tmp_nb->length > offset) /* If offset less than size of 1st net_buf.*/
{
tot_offset = offset;
}
else /* Find corresponding net_buf and calculate the offset in it.*/
{
while((tot_offset += tmp_nb->length) <= offset)
{
tmp_nb = tmp_nb->next;
}
tot_offset = (tmp_nb->length + offset - tot_offset);
}
loc_nb->data = tmp_nb->data;
loc_nb->data_ptr = (fnet_uint8_t *)tmp_nb->data_ptr + tot_offset;
((fnet_uint32_t *)loc_nb->data)[0] = ((fnet_uint32_t *)loc_nb->data)[0] + 1u; /* Increment the the reference_counter.*/
tot_len = (fnet_int32_t)(len - (tmp_nb->length - tot_offset));
if(tot_len <= 0) /* If only one net_buf required.*/
{
loc_nb->length = (fnet_size_t)len;
}
else
{
loc_nb->length = tmp_nb->length - tot_offset;
do
{
loc_nb->next = (fnet_netbuf_t *)fnet_malloc_netbuf(sizeof(fnet_netbuf_t));
if((loc_nb->next == 0) && drain )
{
fnet_prot_drain();
loc_nb->next = (fnet_netbuf_t *)fnet_malloc_netbuf(sizeof(fnet_netbuf_t));
}
if(loc_nb->next == 0) /* there is a need to erase all buffers,*/
{ /* which were created earlier.*/
loc_nb_head = fnet_netbuf_free(loc_nb_head);
while(loc_nb_head != loc_nb->next)
{
tmp_nb = loc_nb_head->next;
fnet_free_netbuf(loc_nb_head);
loc_nb_head = tmp_nb;
}
return (fnet_netbuf_t *)0;
}
loc_nb = loc_nb->next;
loc_nb->next_chain = (fnet_netbuf_t *)0;
tmp_nb = tmp_nb->next;
loc_nb->data = tmp_nb->data;
loc_nb->flags = tmp_nb->flags;
((fnet_uint32_t *)loc_nb->data)[0] = ((fnet_uint32_t *)loc_nb->data)[0] + 1u; /* Increment the the reference_counter.*/
loc_nb->data_ptr = tmp_nb->data_ptr;
tot_len -= (fnet_int32_t)tmp_nb->length;
if(tot_len < 0) /* for correct calculation of length */
{
loc_nb->length = (fnet_size_t)(tot_len + (fnet_int32_t)tmp_nb->length);
}
else
{
loc_nb->length = tmp_nb->length;
}
}
while (tot_len > 0);
}
loc_nb->next = (fnet_netbuf_t *)0;
return (loc_nb_head);
}
/************************************************************************
* NAME: fnet_netbuf_pullup
*
* DESCRIPTION: Create a data buffer for the first net_buf with the
* length len
*************************************************************************/
fnet_return_t fnet_netbuf_pullup( fnet_netbuf_t **nb_ptr, fnet_size_t len)
{
fnet_netbuf_t *nb = *nb_ptr;
fnet_size_t tot_len = 0u;
fnet_size_t offset;
fnet_netbuf_t *tmp_nb;
fnet_netbuf_t *nb_run;
void *new_buf;
/* Check length*/
if(nb->total_length < len)
{
return FNET_OK;
}
if((nb->length >= len) || (len == 0u) || (nb == 0))
{
/* if function shouldn't do anything*/
return FNET_OK;
}
tmp_nb = nb;
tot_len += tmp_nb->length;
/* search of the last buffer, from which the data have to be copied*/
while((tot_len < len) && tmp_nb)
{
tmp_nb = tmp_nb->next;
tot_len += tmp_nb->length;
}
new_buf = (struct net_buf_data *)fnet_malloc_netbuf((fnet_size_t)len + sizeof(fnet_uint32_t)/* For reference_counter */);
if(new_buf == 0)
{
return FNET_ERR;
}
((fnet_uint32_t *)new_buf)[0] = 1u; /* First element is used by the reference_counter.*/
/* Copy into it the contents of first data buffer. Skip the reference counter (placed in the first bytes). */
fnet_memcpy(&((fnet_uint32_t *)new_buf)[1], nb->data_ptr, nb->length);
offset = nb->length;
/* Free old data buffer (for the first net_buf) */
if(((fnet_uint32_t *)nb->data)[0] == 1u) /* If nobody uses this data buffer. */
{
fnet_free_netbuf(nb->data);
}
else /* Else decrement reference counter */
{
((fnet_uint32_t *)nb->data)[0] = ((fnet_uint32_t *)nb->data)[0] - 1u;
}
/* Currently data buffer contains the contents of the first buffer */
nb->data = &((fnet_uint32_t *)new_buf)[0];
nb->data_ptr = &((fnet_uint32_t *)new_buf)[1];
nb_run = nb->next; /* Let's start from the next buffer */
while(nb_run != tmp_nb) /* Copy full data buffers */
{
fnet_memcpy((fnet_uint8_t *)nb->data_ptr + offset, nb_run->data_ptr, nb_run->length);
if(nb_run != tmp_nb)
{
offset += nb_run->length;
nb_run = fnet_netbuf_free(nb_run);
}
}
tot_len = len - offset;
/* Copy the remaining part and change data pointer and length of the
* last net_buf, which is the source for the first net_buf */
fnet_memcpy((fnet_uint8_t *)nb->data_ptr + offset, nb_run->data_ptr, tot_len);
nb_run->length -= tot_len;
if(nb_run->length == 0u)
{
nb_run = fnet_netbuf_free(nb_run);
}
else
{
nb_run->data_ptr = (fnet_uint8_t *)nb_run->data_ptr + tot_len;
}
/* Setting up the params of the first net_buf.*/
nb->next = nb_run;
nb->length = (fnet_size_t)len;
*nb_ptr = nb;
return FNET_OK;
}
/************************************************************************
* NAME: fnet_netbuf_trim
*
* DESCRIPTION: Trims len bytes from the begin of the net_buf data if len
* is positive. Otherwise len bytes should be trimmed from the
* end of net_buf buffer. If len=0 - do nothing
*************************************************************************/
void fnet_netbuf_trim( fnet_netbuf_t **nb_ptr, int len )
{
fnet_netbuf_t *head_nb;
fnet_netbuf_t *nb;
long tot_len;
long total_rem;
fnet_netbuf_t *tmp_nb;
if(len == 0)
return;
tmp_nb = (fnet_netbuf_t *) *nb_ptr;
nb = (fnet_netbuf_t *) *nb_ptr;
head_nb = nb;
/* If the quantity of trimmed bytes is greater than net_buf size - do nothing.*/
if((nb->total_length < (len > 0 ? len : -len)) || nb == 0)
return;
tot_len = (long)nb->length;
total_rem = (long)nb->total_length;
if(len > 0) /* Trim len bytes from the begin of the buffer.*/
{
while(nb != 0 && len >= tot_len)
{
*nb_ptr = nb->next;
nb = fnet_netbuf_free(nb); /* In some cases we delete some net_bufs.*/
tot_len += nb->length;
}
if(nb != 0)
{
nb->data_ptr = (unsigned char *)nb->data_ptr + /* Or change pointer. */
nb->length - (tot_len - len);
nb->length = (unsigned long)(tot_len - len);
nb->flags = tmp_nb->flags;
}
}
else /* Trim len bytes from the end of the buffer. */
{
while(nb != 0 && (total_rem + len > tot_len))
{
nb = nb->next; /* Run up th the first net_buf, which points */
tot_len += nb->length; /* to the data, which should be erased.*/
}
/* Cut the part of the first net_buf, which should be modified.*/
if(nb != 0)
{
nb->length += (len + total_rem - tot_len);
while(nb->next != 0) /* Cut the redundant net_bufs. */
nb->next = fnet_netbuf_free(nb->next);
if(nb->length == 0) /* if |len| == total_length */
head_nb = fnet_netbuf_free(nb);
nb = head_nb;
}
}
if(nb != 0)
{
nb->total_length = (unsigned long)(total_rem - (len > 0 ? len : -len));
}
*nb_ptr = nb;
}
