Return to pack.c CVS log

Up to [ELWIX - Embedded LightWeight unIX -] / libelwix / src

version 1.9

    1: #include "global.h"
    2: 
    3: 
    4: static inline uint8_t *
    5: rpack_next_boundary(uint8_t * __restrict buf, uint8_t * __restrict p, size_t align)
    6: {
    7: 	size_t misa = (size_t) (p - buf) % align;
    8: 
    9: 	if (!misa)
   10: 		return p;
   11: 
   12: 	return p + (align - misa);
   13: }
   14: 
   15: 
   16: /*
   17:  * rpack_create() - Allocate & init raw packet structure
   18:  *
   19:  * @buf = buffer
   20:  * @buflen = length of buffer
   21:  * return: NULL error or !=NULL raw packet, should be freed by rpack_destroy()
   22:  */
   23: rpack_t *
   24: rpack_create(void * __restrict buf, size_t buflen)
   25: {
   26: 	rpack_t *rp = NULL;
   27: 
   28: 	rp = e_malloc(sizeof(rpack_t));
   29: 	if (!rp) {
   30: 		LOGERR;
   31: 		return NULL;
   32: 	}
   33: 
   34: 	RPACK_INIT(rp, buf, buflen);
   35: 	return rp;
   36: }
   37: 
   38: /*
   39:  * rpack_destroy() - Release & free raw packet structure
   40:  *
   41:  * @rp = raw packet
   42:  * return: none
   43:  */
   44: void
   45: rpack_destroy(rpack_t ** __restrict rp)
   46: {
   47: 	if (!rp)
   48: 		return;
   49: 
   50: 	if (*rp) {
   51: 		RPACK_FREE(*rp);
   52: 		e_free(*rp);
   53: 		*rp = NULL;
   54: 	}
   55: }
   56: 
   57: /*
   58:  * rpack_align_and_reserve() - Align & reserve space
   59:  *
   60:  * @rp = raw buffer
   61:  * @siz = need size
   62:  * return: NULL error or not enough space, !=NULL next position
   63:  */
   64: uint8_t *
   65: rpack_align_and_reserve(rpack_t * __restrict rp, size_t siz)
   66: {
   67: 	uint8_t *n;
   68: 
   69: 	if (!RPACK_SANITY(rp))
   70: 		return NULL;
   71: 
   72: 	n = rpack_next_boundary(rp->r_buf, rp->r_next, siz);
   73: 	/* too little space for siz */
   74: 	if (n - rp->r_buf + siz > rp->r_len)
   75: 		return NULL;
   76: 
   77: 	return n;
   78: }
   79: 
   80: 
   81: /*
   82:  * rpack_uint8() - Pack/Unpack 8bit value
   83:  *
   84:  * @rp = raw buffer
   85:  * @n = set value if !=NULL
   86:  * return: -1 error or get value
   87:  */
   88: uint8_t
   89: rpack_uint8(rpack_t * __restrict rp, uint8_t * __restrict n)
   90: {
   91: 	uint8_t u;
   92: 
   93: 	if (!RPACK_SANITY(rp))
   94: 		return (uint8_t) -1;
   95: 	/* No space left */
   96: 	if ((size_t) (rp->r_next - rp->r_buf) >= rp->r_len)
   97: 		return (uint8_t) -1;
   98: 
   99: 	u = *rp->r_next;
  100: 	if (n)
  101: 		*rp->r_next = *n;
  102: 
  103: 	rp->r_next++;
  104: 	return u;
  105: }
  106: 
  107: /*
  108:  * rpack_uint16() - Pack/Unpack 16bit value
  109:  *
  110:  * @rp = raw buffer
  111:  * @n = set value if !=NULL
  112:  * return: -1 error or get value
  113:  */
  114: uint16_t
  115: rpack_uint16(rpack_t * __restrict rp, uint16_t * __restrict n)
  116: {
  117: 	uint16_t u;
  118: 	uint8_t *next;
  119: 
  120: 	if (!RPACK_SANITY(rp))
  121: 		return (uint16_t) -1;
  122: 	/* No space left */
  123: 	if (!(next = rpack_align_and_reserve(rp, sizeof(uint16_t))))
  124: 		return (uint16_t) -1;
  125: 
  126: 	u = EXTRACT_LE_16(next);
  127: 	if (n)
  128: 		RPACK_SET_16(next, n);
  129: 
  130: 	rp->r_next = next + sizeof(uint16_t);
  131: 	return u;
  132: }
  133: 
  134: /*
  135:  * rpack_uint24() - Pack/Unpack 24bit value
  136:  *
  137:  * @rp = raw buffer
  138:  * @n = set value if !=NULL
  139:  * return: -1 error or get value
  140:  */
  141: uint32_t
  142: rpack_uint24(rpack_t * __restrict rp, uint32_t * __restrict n)
  143: {
  144: 	uint32_t u;
  145: 	uint8_t *next;
  146: 
  147: 	if (!RPACK_SANITY(rp))
  148: 		return (uint32_t) -1;
  149: 	/* No space left */
  150: 	if (!(next = rpack_align_and_reserve(rp, sizeof(uint32_t))))
  151: 		return (uint32_t) -1;
  152: 
  153: 	u = EXTRACT_LE_24(next);
  154: 	if (n)
  155: 		RPACK_SET_24(next, n);
  156: 
  157: 	rp->r_next = next + sizeof(uint32_t);
  158: 	return u;
  159: }
  160: 
  161: /*
  162:  * rpack_uint32() - Pack/Unpack 32bit value
  163:  *
  164:  * @rp = raw buffer
  165:  * @n = set value if !=NULL
  166:  * return: -1 error or get value
  167:  */
  168: uint32_t
  169: rpack_uint32(rpack_t * __restrict rp, uint32_t * __restrict n)
  170: {
  171: 	uint32_t u;
  172: 	uint8_t *next;
  173: 
  174: 	if (!RPACK_SANITY(rp))
  175: 		return (uint32_t) -1;
  176: 	/* No space left */
  177: 	if (!(next = rpack_align_and_reserve(rp, sizeof(uint32_t))))
  178: 		return (uint32_t) -1;
  179: 
  180: 	u = EXTRACT_LE_32(next);
  181: 	if (n)
  182: 		RPACK_SET_32(next, n);
  183: 
  184: 	rp->r_next = next + sizeof(uint32_t);
  185: 	return u;
  186: }
  187: 
  188: /*
  189:  * rpack_uint64() - Pack/Unpack 64bit value
  190:  *
  191:  * @rp = raw buffer
  192:  * @n = set value if !=NULL
  193:  * return: -1 error or get value
  194:  */
  195: uint64_t
  196: rpack_uint64(rpack_t * __restrict rp, uint64_t * __restrict n)
  197: {
  198: 	uint64_t u;
  199: 	uint8_t *next;
  200: 
  201: 	if (!RPACK_SANITY(rp))
  202: 		return (uint64_t) -1;
  203: 	/* No space left */
  204: 	if (!(next = rpack_align_and_reserve(rp, sizeof(uint64_t))))
  205: 		return (uint64_t) -1;
  206: 
  207: 	u = EXTRACT_LE_64(next);
  208: 	if (n)
  209: 		RPACK_SET_64(next, n);
  210: 
  211: 	rp->r_next = next + sizeof(uint64_t);
  212: 	return u;
  213: }