add correct handle to sensorpropget requests

[rsbs2] / rsb-lz.c

diff --git a/rsb-lz.c b/rsb-lz.c
index 4dddd9b37997d778f316472bad6cb7e736ab0d59..bb735d6cebaf040b318eae11fa88ec4c9f67a2fb 100644 (file)
--- a/rsb-lz.c
+++ b/rsb-lz.c
@@ -143,6 +143,104 @@ void lz_expand(struct data_in_s *data_in, struct data_out_s *data_out)
        }
 }
 
+void set_next_bit(unsigned char *buf, unsigned int set, unsigned int *currbit) {
+       unsigned char *pos;
+       unsigned char bitpos;
+
+       if (set) {
+               pos = buf + ((*currbit) / 8);
+               bitpos = 0x80 >> ((*currbit) % 8);
+               *pos |= bitpos;
+       }
+
+       *currbit = *currbit + 1;
+}
+
+void write_bits(unsigned char *buf, unsigned int data, unsigned int bits, unsigned int *currbit) {
+       int i;
+       unsigned int bitpos;
+
+       bitpos = 1 << (bits - 1);
+
+       for (i = 0; i < bits; i++) {
+               set_next_bit(buf, data & bitpos, currbit);
+               bitpos >>= 1;
+       }
+}
+
+unsigned char *compress_lz(unsigned char *inbuf, int inlen, int *outlen)
+{
+       unsigned char *end = inbuf + inlen;
+       unsigned char *outbuf;
+       unsigned char window[1024];
+       int pos = 0;
+       int fill = 0;
+       unsigned int currbit = 0;
+       int offset;
+       int wordlen;
+       int found;
+       int i;
+
+       if ((outbuf = malloc((inlen * 2) + 4)) == NULL) {
+               perror("malloc");
+       }
+
+       *((unsigned int*)outbuf) = LZ_MAGIC;
+       currbit = 8 * 8;
+
+       while(inbuf < end) {
+               found = 0;
+               for (wordlen = 17; wordlen > 1; wordlen--) {
+                       for (offset = 1; offset < ((fill < 1023) ? fill : 1023); offset++) {
+                               if ((fill < 1023) &&
+                                   (wordlen + offset > fill))
+                                       break;
+
+                               for (i = 0; i < wordlen; i++) {
+                                       if (inbuf[i] != window[(offset + i) & 0x3ff]) {
+                                               break;
+                                       }
+                               }
+                               if (i == wordlen)
+                                       found = 1;
+                       }
+                       if (found)
+                               break;
+               }
+
+               if (found) {
+                       write_bits(outbuf, 0x00, 0x01, &currbit);
+                       write_bits(outbuf, offset, 0x0a, &currbit);
+                       write_bits(outbuf, wordlen - 1, 0x04, &currbit);
+                       for (i = 0; i < wordlen; i++) {
+                               window[pos] = *(inbuf + i);
+                               pos = (pos + 1) & 0x3ff;
+                       }
+                       inbuf += wordlen;
+
+                       if (fill < sizeof(window))
+                               fill += wordlen;
+               } else {
+                       write_bits(outbuf, 0x01, 0x01, &currbit);
+                       write_bits(outbuf, *inbuf, 0x08, &currbit);
+                       window[pos] = *inbuf;
+                       pos = (pos + 1) & 0x3ff;
+                       inbuf++;
+                       if (fill < sizeof(window))
+                               fill++;
+               }
+       }
+
+       write_bits(outbuf, 0x00, 0x01, &currbit);
+       write_bits(outbuf, 0x00, 0x0a, &currbit);
+
+       *outlen = (currbit / 8) + 1;
+
+       *((unsigned int*)(outbuf + 4)) = *outlen;
+
+       return outbuf;
+}
+
 /* Checksum is only used for the compressed firmware in 'firmware' */
 unsigned int crc_check(unsigned char *buf, unsigned int len, unsigned int magic)
 {
@@ -173,9 +271,8 @@ unsigned int crc_check(unsigned char *buf, unsigned int len, unsigned int magic)
        return 0;
 }
 
-void extract_lz_file(unsigned char *inbuf, unsigned char *name, unsigned char check_crc)
+unsigned char *extract_lz_file(unsigned char *inbuf, unsigned int *outlen , unsigned char check_crc)
 {
-       unsigned int len;
        unsigned char *outbuf;
        struct data_in_s data_in;
        struct data_out_s data_out;
@@ -183,23 +280,23 @@ void extract_lz_file(unsigned char *inbuf, unsigned char *name, unsigned char ch
        if (*((unsigned int*)inbuf) != LZ_MAGIC)
                err_exit(__func__);
 
-       len = *((unsigned int*)(inbuf + 4));
-       printf(", length: %d", len);
+       *outlen = *((unsigned int*)(inbuf + 4));
+       printf(", length: %d", *outlen);
 
-       if ((outbuf = malloc(len)) == NULL) {
+       if ((outbuf = malloc(*outlen)) == NULL) {
                perror("malloc");
                exit(1);
        }
 
-       bzero(outbuf, len);
+       bzero(outbuf, *outlen);
 
        data_in.start = inbuf + 8;
-       data_in.stop = inbuf + len;
+       data_in.stop = inbuf + *outlen;
        data_in.byte = 0x00;
        data_in.bitpos = 0x80;
 
        data_out.pos = outbuf;
-       data_out.end = outbuf + len;
+       data_out.end = outbuf + *outlen;
 
        lz_expand(&data_in, &data_out);
 
@@ -215,7 +312,5 @@ void extract_lz_file(unsigned char *inbuf, unsigned char *name, unsigned char ch
                }
        }
 
-       write_file((char*)name, outbuf, len);
-       
-       free(outbuf);
+       return outbuf;
 }