--- /dev/null
+/*\r
+ * Copyright (c) 2005-2017 Lev Walkin <vlm@lionet.info>.\r
+ * All rights reserved.\r
+ * Redistribution and modifications are permitted subject to BSD license.\r
+ */\r
+#include <asn_system.h>\r
+#include <asn_internal.h>\r
+#include <asn_bit_data.h>\r
+\r
+/*\r
+ * Create a contiguous non-refillable bit data structure.\r
+ * Can be freed by FREEMEM().\r
+ */\r
+asn_bit_data_t *\r
+asn_bit_data_new_contiguous(const void *data, size_t size_bits) {\r
+ size_t size_bytes = (size_bits + 7) / 8;\r
+ asn_bit_data_t *pd;\r
+ uint8_t *bytes;\r
+\r
+ /* Get the extensions map */\r
+ pd = CALLOC(1, sizeof(*pd) + size_bytes + 1);\r
+ if(!pd) {\r
+ return NULL;\r
+ }\r
+ bytes = (void *)(((char *)pd) + sizeof(*pd));\r
+ memcpy(bytes, data, size_bytes);\r
+ bytes[size_bytes] = 0;\r
+ pd->buffer = bytes;\r
+ pd->nboff = 0;\r
+ pd->nbits = size_bits;\r
+\r
+ return pd;\r
+}\r
+\r
+\r
+char *\r
+asn_bit_data_string(asn_bit_data_t *pd) {\r
+ static char buf[2][32];\r
+ static int n;\r
+ n = (n+1) % 2;\r
+ snprintf(buf[n], sizeof(buf[n]),\r
+ "{m=%" ASN_PRI_SIZE " span %" ASN_PRI_SIZE "[%" ASN_PRI_SIZE\r
+ "..%" ASN_PRI_SIZE "] (%" ASN_PRI_SIZE ")}",\r
+ pd->moved, ((uintptr_t)(pd->buffer) & 0xf), pd->nboff, pd->nbits,\r
+ pd->nbits - pd->nboff);\r
+ return buf[n];\r
+}\r
+\r
+void\r
+asn_get_undo(asn_bit_data_t *pd, int nbits) {\r
+ if((ssize_t)pd->nboff < nbits) {\r
+ assert((ssize_t)pd->nboff < nbits);\r
+ } else {\r
+ pd->nboff -= nbits;\r
+ pd->moved -= nbits;\r
+ }\r
+}\r
+\r
+/*\r
+ * Extract a small number of bits (<= 31) from the specified PER data pointer.\r
+ */\r
+int32_t\r
+asn_get_few_bits(asn_bit_data_t *pd, int nbits) {\r
+ size_t off; /* Next after last bit offset */\r
+ ssize_t nleft; /* Number of bits left in this stream */\r
+ uint32_t accum;\r
+ const uint8_t *buf;\r
+\r
+ if(nbits < 0)\r
+ return -1;\r
+\r
+ nleft = pd->nbits - pd->nboff;\r
+ if(nbits > nleft) {\r
+ int32_t tailv, vhead;\r
+ if(!pd->refill || nbits > 31) return -1;\r
+ /* Accumulate unused bytes before refill */\r
+ ASN_DEBUG("Obtain the rest %d bits (want %d)",\r
+ (int)nleft, (int)nbits);\r
+ tailv = asn_get_few_bits(pd, nleft);\r
+ if(tailv < 0) return -1;\r
+ /* Refill (replace pd contents with new data) */\r
+ if(pd->refill(pd))\r
+ return -1;\r
+ nbits -= nleft;\r
+ vhead = asn_get_few_bits(pd, nbits);\r
+ /* Combine the rest of previous pd with the head of new one */\r
+ tailv = (tailv << nbits) | vhead; /* Could == -1 */\r
+ return tailv;\r
+ }\r
+\r
+ /*\r
+ * Normalize position indicator.\r
+ */\r
+ if(pd->nboff >= 8) {\r
+ pd->buffer += (pd->nboff >> 3);\r
+ pd->nbits -= (pd->nboff & ~0x07);\r
+ pd->nboff &= 0x07;\r
+ }\r
+ pd->moved += nbits;\r
+ pd->nboff += nbits;\r
+ off = pd->nboff;\r
+ buf = pd->buffer;\r
+\r
+ /*\r
+ * Extract specified number of bits.\r
+ */\r
+ if(off <= 8)\r
+ accum = nbits ? (buf[0]) >> (8 - off) : 0;\r
+ else if(off <= 16)\r
+ accum = ((buf[0] << 8) + buf[1]) >> (16 - off);\r
+ else if(off <= 24)\r
+ accum = ((buf[0] << 16) + (buf[1] << 8) + buf[2]) >> (24 - off);\r
+ else if(off <= 31)\r
+ accum = (((uint32_t)buf[0] << 24) + (buf[1] << 16)\r
+ + (buf[2] << 8) + (buf[3])) >> (32 - off);\r
+ else if(nbits <= 31) {\r
+ asn_bit_data_t tpd = *pd;\r
+ /* Here are we with our 31-bits limit plus 1..7 bits offset. */\r
+ asn_get_undo(&tpd, nbits);\r
+ /* The number of available bits in the stream allow\r
+ * for the following operations to take place without\r
+ * invoking the ->refill() function */\r
+ accum = asn_get_few_bits(&tpd, nbits - 24) << 24;\r
+ accum |= asn_get_few_bits(&tpd, 24);\r
+ } else {\r
+ asn_get_undo(pd, nbits);\r
+ return -1;\r
+ }\r
+\r
+ accum &= (((uint32_t)1 << nbits) - 1);\r
+\r
+ ASN_DEBUG(" [PER got %2d<=%2d bits => span %d %+ld[%d..%d]:%02x (%d) => 0x%x]",\r
+ (int)nbits, (int)nleft,\r
+ (int)pd->moved,\r
+ (((long)pd->buffer) & 0xf),\r
+ (int)pd->nboff, (int)pd->nbits,\r
+ ((pd->buffer != NULL)?pd->buffer[0]:0),\r
+ (int)(pd->nbits - pd->nboff),\r
+ (int)accum);\r
+\r
+ return accum;\r
+}\r
+\r
+/*\r
+ * Extract a large number of bits from the specified PER data pointer.\r
+ */\r
+int\r
+asn_get_many_bits(asn_bit_data_t *pd, uint8_t *dst, int alright, int nbits) {\r
+ int32_t value;\r
+\r
+ if(alright && (nbits & 7)) {\r
+ /* Perform right alignment of a first few bits */\r
+ value = asn_get_few_bits(pd, nbits & 0x07);\r
+ if(value < 0) return -1;\r
+ *dst++ = value; /* value is already right-aligned */\r
+ nbits &= ~7;\r
+ }\r
+\r
+ while(nbits) {\r
+ if(nbits >= 24) {\r
+ value = asn_get_few_bits(pd, 24);\r
+ if(value < 0) return -1;\r
+ *(dst++) = value >> 16;\r
+ *(dst++) = value >> 8;\r
+ *(dst++) = value;\r
+ nbits -= 24;\r
+ } else {\r
+ value = asn_get_few_bits(pd, nbits);\r
+ if(value < 0) return -1;\r
+ if(nbits & 7) { /* implies left alignment */\r
+ value <<= 8 - (nbits & 7),\r
+ nbits += 8 - (nbits & 7);\r
+ if(nbits > 24)\r
+ *dst++ = value >> 24;\r
+ }\r
+ if(nbits > 16)\r
+ *dst++ = value >> 16;\r
+ if(nbits > 8)\r
+ *dst++ = value >> 8;\r
+ *dst++ = value;\r
+ break;\r
+ }\r
+ }\r
+\r
+ return 0;\r
+}\r
+\r
+/*\r
+ * Put a small number of bits (<= 31).\r
+ */\r
+int\r
+asn_put_few_bits(asn_bit_outp_t *po, uint32_t bits, int obits) {\r
+ size_t off; /* Next after last bit offset */\r
+ size_t omsk; /* Existing last byte meaningful bits mask */\r
+ uint8_t *buf;\r
+\r
+ if(obits <= 0 || obits >= 32) return obits ? -1 : 0;\r
+\r
+ ASN_DEBUG("[PER put %d bits %x to %p+%d bits]",\r
+ obits, (int)bits, (void *)po->buffer, (int)po->nboff);\r
+\r
+ /*\r
+ * Normalize position indicator.\r
+ */\r
+ if(po->nboff >= 8) {\r
+ po->buffer += (po->nboff >> 3);\r
+ po->nbits -= (po->nboff & ~0x07);\r
+ po->nboff &= 0x07;\r
+ }\r
+\r
+ /*\r
+ * Flush whole-bytes output, if necessary.\r
+ */\r
+ if(po->nboff + obits > po->nbits) {\r
+ size_t complete_bytes;\r
+ if(!po->buffer) po->buffer = po->tmpspace;\r
+ complete_bytes = (po->buffer - po->tmpspace);\r
+ ASN_DEBUG("[PER output %ld complete + %ld]",\r
+ (long)complete_bytes, (long)po->flushed_bytes);\r
+ if(po->output(po->tmpspace, complete_bytes, po->op_key) < 0)\r
+ return -1;\r
+ if(po->nboff)\r
+ po->tmpspace[0] = po->buffer[0];\r
+ po->buffer = po->tmpspace;\r
+ po->nbits = 8 * sizeof(po->tmpspace);\r
+ po->flushed_bytes += complete_bytes;\r
+ }\r
+\r
+ /*\r
+ * Now, due to sizeof(tmpspace), we are guaranteed large enough space.\r
+ */\r
+ buf = po->buffer;\r
+ omsk = ~((1 << (8 - po->nboff)) - 1);\r
+ off = (po->nboff + obits);\r
+\r
+ /* Clear data of debris before meaningful bits */\r
+ bits &= (((uint32_t)1 << obits) - 1);\r
+\r
+ ASN_DEBUG("[PER out %d %u/%x (t=%d,o=%d) %x&%x=%x]", obits,\r
+ (int)bits, (int)bits,\r
+ (int)po->nboff, (int)off,\r
+ buf[0], (int)(omsk&0xff),\r
+ (int)(buf[0] & omsk));\r
+\r
+ if(off <= 8) /* Completely within 1 byte */\r
+ po->nboff = off,\r
+ bits <<= (8 - off),\r
+ buf[0] = (buf[0] & omsk) | bits;\r
+ else if(off <= 16)\r
+ po->nboff = off,\r
+ bits <<= (16 - off),\r
+ buf[0] = (buf[0] & omsk) | (bits >> 8),\r
+ buf[1] = bits;\r
+ else if(off <= 24)\r
+ po->nboff = off,\r
+ bits <<= (24 - off),\r
+ buf[0] = (buf[0] & omsk) | (bits >> 16),\r
+ buf[1] = bits >> 8,\r
+ buf[2] = bits;\r
+ else if(off <= 31)\r
+ po->nboff = off,\r
+ bits <<= (32 - off),\r
+ buf[0] = (buf[0] & omsk) | (bits >> 24),\r
+ buf[1] = bits >> 16,\r
+ buf[2] = bits >> 8,\r
+ buf[3] = bits;\r
+ else {\r
+ if(asn_put_few_bits(po, bits >> (obits - 24), 24)) return -1;\r
+ if(asn_put_few_bits(po, bits, obits - 24)) return -1;\r
+ }\r
+\r
+ ASN_DEBUG("[PER out %u/%x => %02x buf+%ld]",\r
+ (int)bits, (int)bits, buf[0],\r
+ (long)(po->buffer - po->tmpspace));\r
+\r
+ return 0;\r
+}\r
+\r
+\r
+/*\r
+ * Output a large number of bits.\r
+ */\r
+int\r
+asn_put_many_bits(asn_bit_outp_t *po, const uint8_t *src, int nbits) {\r
+\r
+ while(nbits) {\r
+ uint32_t value;\r
+\r
+ if(nbits >= 24) {\r
+ value = (src[0] << 16) | (src[1] << 8) | src[2];\r
+ src += 3;\r
+ nbits -= 24;\r
+ if(asn_put_few_bits(po, value, 24))\r
+ return -1;\r
+ } else {\r
+ value = src[0];\r
+ if(nbits > 8)\r
+ value = (value << 8) | src[1];\r
+ if(nbits > 16)\r
+ value = (value << 8) | src[2];\r
+ if(nbits & 0x07)\r
+ value >>= (8 - (nbits & 0x07));\r
+ if(asn_put_few_bits(po, value, nbits))\r
+ return -1;\r
+ break;\r
+ }\r
+ }\r
+\r
+ return 0;\r
+}\r
+\r
+\r
+int\r
+asn_put_aligned_flush(asn_bit_outp_t *po) {\r
+ uint32_t unused_bits = (0x7 & (8 - (po->nboff & 0x07)));\r
+ size_t complete_bytes =\r
+ (po->buffer ? po->buffer - po->tmpspace : 0) + ((po->nboff + 7) >> 3);\r
+\r
+ if(unused_bits) {\r
+ po->buffer[po->nboff >> 3] &= ~0u << unused_bits;\r
+ }\r
+\r
+ if(po->output(po->tmpspace, complete_bytes, po->op_key) < 0) {\r
+ return -1;\r
+ } else {\r
+ po->buffer = po->tmpspace;\r
+ po->nboff = 0;\r
+ po->nbits = 8 * sizeof(po->tmpspace);\r
+ po->flushed_bytes += complete_bytes;\r
+ return 0;\r
+ }\r
+}\r
+\r
Code Review / ric-app / kpimon.git / blobdiff