--- bitstream.h	2003/02/09 19:32:52	1.16
+++ bitstream.h	2003/07/28 12:39:32	1.16.2.2
@@ -50,13 +50,15 @@
  *  exception also makes it possible to release a modified version which
  *  carries forward this exception.
  *
- * $Id: bitstream.h,v 1.16 2003/02/09 19:32:52 edgomez Exp $
+ * $Id: bitstream.h,v 1.16.2.2 2003/07/28 12:39:32 edgomez Exp $
  *
  ****************************************************************************/
 
 #ifndef _BITSTREAM_H_
 #define _BITSTREAM_H_
 
+#include <stdio.h>
+
 #include "../portab.h"
 #include "../decoder.h"
 #include "../encoder.h"
@@ -165,23 +167,31 @@
 			  uint32_t length)
 {
 	uint32_t tmp;
+	size_t bitpos;
+	ptr_t adjbitstream = (ptr_t)bitstream;
 
-	bs->start = bs->tail = (uint32_t *) bitstream;
+	/*
+	 * Start the stream on a uint32_t boundary, by rounding down to the
+	 * previous uint32_t and skipping the intervening bytes.
+	 */
+	bitpos = ((sizeof(uint32_t)-1) & (size_t)bitstream);
+	adjbitstream = adjbitstream - bitpos;
+	bs->start = bs->tail = (uint32_t *) adjbitstream;
 
-	tmp = *(uint32_t *) bitstream;
+	tmp = *bs->start;
 #ifndef ARCH_IS_BIG_ENDIAN
 	BSWAP(tmp);
 #endif
 	bs->bufa = tmp;
 
-	tmp = *((uint32_t *) bitstream + 1);
+	tmp = *(bs->start + 1);
 #ifndef ARCH_IS_BIG_ENDIAN
 	BSWAP(tmp);
 #endif
 	bs->bufb = tmp;
 
 	bs->buf = 0;
-	bs->pos = 0;
+	bs->pos = bs->initpos = bitpos*8;
 	bs->length = length;
 }
 
@@ -208,7 +218,7 @@
 	bs->bufb = tmp;
 
 	bs->buf = 0;
-	bs->pos = 0;
+	bs->pos = bs->initpos;
 }
 
 
@@ -301,7 +311,7 @@
 static uint32_t __inline
 BitstreamPos(const Bitstream * const bs)
 {
-	return((uint32_t)(8*((ptr_t)bs->tail - (ptr_t)bs->start) + bs->pos));
+	return((uint32_t)(8*((ptr_t)bs->tail - (ptr_t)bs->start) + bs->pos - bs->initpos));
 }
 
 
@@ -325,6 +335,10 @@
 		len += (bs->pos + 7) / 8;
 	}
 
+	/* initpos is always on a byte boundary */
+	if (bs->initpos)
+		len -= bs->initpos/8;
+
 	return len;
 }
 
@@ -349,20 +363,6 @@
 	}
 }
 
-
-/* pad bitstream to the next byte boundary */
-
-static void __inline
-BitstreamPad(Bitstream * const bs)
-{
-	uint32_t remainder = bs->pos % 8;
-
-	if (remainder) {
-		BitstreamForward(bs, 8 - remainder);
-	}
-}
-
-
 /* read n bits from bitstream */
 
 static uint32_t __inline
@@ -425,4 +425,39 @@
 	}
 }
 
+static const int stuffing_codes[8] =
+{
+	        /* nbits     stuffing code */
+	0,		/* 1          0 */
+	1,		/* 2          01 */
+	3,		/* 3          011 */
+	7,		/* 4          0111 */
+	0xf,	/* 5          01111 */
+	0x1f,	/* 6          011111 */
+	0x3f,   /* 7          0111111 */
+	0x7f,	/* 8          01111111 */
+};
+
+/* pad bitstream to the next byte boundary */
+
+static void __inline
+BitstreamPad(Bitstream * const bs)
+{
+	int bits = 8 - (bs->pos % 8);
+	if (bits < 8)
+		BitstreamPutBits(bs, stuffing_codes[bits - 1], bits);
+}
+
+/*
+ * pad bitstream to the next byte boundary 
+ * alway pad: even if currently at the byte boundary
+ */
+
+static void __inline
+BitstreamPadAlways(Bitstream * const bs)
+{
+	int bits = 8 - (bs->pos % 8);
+	BitstreamPutBits(bs, stuffing_codes[bits - 1], bits);
+}
+
 #endif /* _BITSTREAM_H_ */