int count_frames(unsigned char *packet, int len)
{
int sz = packet[0]&0x7;
if (sz == 0)
return 1;
else if (sz == 1 || sz == 4)
return 2;
else if (sz == 2 || sz == 5)
return 3;
else if (sz == 3)
{
/* Many packets, same size */
int count, payload;
int flen = decode_length(packet+1, len-1);
if (flen<=0)
return -1;
payload = len - 2;
if (flen>=252)
payload--;
count = payload/flen;
if (count*flen==payload)
return count;
else
return -1;
} else /* if (sz == 6 || sz == 7) */
{
/* Many packets, different sizes */
int count = 0;
int pos = 1;
int bytes = 1;
int extra = 0;
if (sz==7)
extra = 1;
while (bytes < len)
{
int tmp=extra+1;
int flen = decode_length(packet+pos, len-bytes);
if (flen==-1)
return -1;
if (flen >= 252)
tmp = 2;
pos += tmp;
bytes += tmp+flen;
count++;
}
if (bytes != len)
return -1;
else
return count;
}
}
static int decode_open_type(uint8_t *buf, int limit, int *len, const uint8_t **p_object, int *p_num_octets)
{
int octet_cnt;
int octet_idx;
int stat;
int i;
const uint8_t **pbuf;
for (octet_idx = 0, *p_num_octets = 0; ; octet_idx += octet_cnt) {
if ((stat = decode_length(buf, limit, len, &octet_cnt)) < 0)
return -1;
if (octet_cnt > 0) {
*p_num_octets += octet_cnt;
pbuf = &p_object[octet_idx];
i = 0;
/* Make sure the buffer contains at least the number of bits requested */
if ((*len + octet_cnt) > limit)
return -1;
*pbuf = &buf[*len];
*len += octet_cnt;
}
if (stat == 0)
break;
}
return 0;
}
static inline grub_uint32_t
skip_data_ref_object (const grub_uint8_t *ptr, const grub_uint8_t *end)
{
grub_dprintf ("acpi", "data type = 0x%x\n", *ptr);
switch (*ptr)
{
case GRUB_ACPI_OPCODE_PACKAGE:
case GRUB_ACPI_OPCODE_BUFFER:
return 1 + decode_length (ptr + 1, 0);
case GRUB_ACPI_OPCODE_ZERO:
case GRUB_ACPI_OPCODE_ONES:
case GRUB_ACPI_OPCODE_ONE:
return 1;
case GRUB_ACPI_OPCODE_BYTE_CONST:
return 2;
case GRUB_ACPI_OPCODE_WORD_CONST:
return 3;
case GRUB_ACPI_OPCODE_DWORD_CONST:
return 5;
case GRUB_ACPI_OPCODE_STRING_CONST:
{
const grub_uint8_t *ptr0 = ptr;
for (ptr++; ptr < end && *ptr; ptr++);
if (ptr == end)
return 0;
return ptr - ptr0 + 1;
}
default:
if (*ptr == '^' || *ptr == '\\' || *ptr == '_'
|| (*ptr >= 'A' && *ptr <= 'Z'))
return skip_name_string (ptr, end);
grub_printf ("Unknown opcode 0x%x\n", *ptr);
return 0;
}
}
static inline grub_uint32_t
skip_ext_op (const grub_uint8_t *ptr, const grub_uint8_t *end)
{
const grub_uint8_t *ptr0 = ptr;
int add;
grub_dprintf ("acpi", "Extended opcode: 0x%x\n", *ptr);
switch (*ptr)
{
case GRUB_ACPI_EXTOPCODE_MUTEX:
ptr++;
ptr += skip_name_string (ptr, end);
ptr++;
break;
case GRUB_ACPI_EXTOPCODE_OPERATION_REGION:
ptr++;
ptr += skip_name_string (ptr, end);
ptr++;
ptr += add = skip_data_ref_object (ptr, end);
if (!add)
return 0;
ptr += add = skip_data_ref_object (ptr, end);
if (!add)
return 0;
break;
case GRUB_ACPI_EXTOPCODE_FIELD_OP:
case GRUB_ACPI_EXTOPCODE_INDEX_FIELD_OP:
ptr++;
ptr += decode_length (ptr, 0);
break;
default:
grub_printf ("Unexpected extended opcode: 0x%x\n", *ptr);
return 0;
}
return ptr - ptr0;
}
static int decode_open_type(const uint8_t *buf, int limit, int *len, const uint8_t ** p_object, int *p_num_octets)
{
int octet_cnt;
#if 0
int octet_idx;
int stat;
const uint8_t **pbuf;
*p_num_octets = 0;
for (octet_idx = 0;; octet_idx += octet_cnt) {
if ((stat = decode_length(buf, limit, len, &octet_cnt)) < 0)
return -1;
if (octet_cnt > 0) {
*p_num_octets += octet_cnt;
pbuf = &p_object[octet_idx];
/* Make sure the buffer contains at least the number of bits requested */
if ((*len + octet_cnt) > limit)
return -1;
/* Was told the buffer was large enough, but in reality it didn't exist. FS-5202 */
if ( buf[*len] == 0 )
return -1;
*pbuf = &buf[*len];
*len += octet_cnt;
}
if (stat == 0)
break;
}
#else
/* We do not deal with fragments, so there is no point in looping through them. Just say that something
fragmented is bad. */
if (decode_length(buf, limit, len, &octet_cnt) != 0)
return -1;
*p_num_octets = octet_cnt;
if (octet_cnt > 0) {
/* Make sure the buffer contains at least the number of bits requested */
if ((*len + octet_cnt) > limit)
return -1;
*p_object = &buf[*len];
*len += octet_cnt;
}
#endif
return 0;
}
void dynamic_inflate(reader_t *reader)
{
size_t hlit = read_HLIT(reader) + 257;
size_t hdist = read_HDIST(reader) + 1;
size_t hclen = read_HCLEN(reader) + 4;
/*printf("hclen = %d, hlit = %d, hdist = %d\n", hclen, hlit, hdist);*/
huffman_code litlen_codes[hlit];
huffman_code offset_codes[hdist];
read_alphabets(reader,
litlen_codes, hlit,
offset_codes, hdist,
hclen);
io *io_s;
init_io(&io_s);
byte bytes[LEN_MAX];
size_t litlen;
bool is_end_of_block = false;
size_t block_size = 0;
while (!is_end_of_block) {
litlen = read_next_huffman_code(reader, litlen_codes, hlit);
block_size++;
if (litlen < END_OF_BLOCK)
write_byte(io_s, litlen);
else if (litlen == END_OF_BLOCK)
is_end_of_block = true;
else {
two_bytes length = decode_length(reader, litlen);
two_bytes offcode = read_next_huffman_code(reader,
offset_codes,
hdist);
two_bytes distance = decode_distance(reader, offcode);
get_cyclic_queue(io_s->output, bytes, length, distance);
write_bytes(io_s, bytes, length);
block_size = block_size - 1 + length;
}
}
/*printf("block_size = %d\n", block_size);*/
write_to_output(io_s, reader->output);
delete_io(&io_s);
}
static int decode_open_type(uint8_t *buf, unsigned int limit, unsigned int *len, const uint8_t **p_object, unsigned int *p_num_octets)
{
unsigned int octet_cnt = 0;
if (decode_length(buf, limit, len, &octet_cnt) != 0)
return -1;
if (octet_cnt > 0) {
/* Make sure the buffer contains at least the number of bits requested */
if ((*len + octet_cnt) > limit)
return -1;
*p_num_octets = octet_cnt;
*p_object = &buf[*len];
*len += octet_cnt;
}
return 0;
}
static int
get_sleep_type (grub_uint8_t *table, grub_uint8_t *end)
{
grub_uint8_t *ptr, *prev = table;
int sleep_type = -1;
ptr = table + sizeof (struct grub_acpi_table_header);
while (ptr < end && prev < ptr)
{
int add;
prev = ptr;
grub_dprintf ("acpi", "Opcode 0x%x\n", *ptr);
grub_dprintf ("acpi", "Tell %x\n", (unsigned) (ptr - table));
switch (*ptr)
{
case GRUB_ACPI_OPCODE_EXTOP:
ptr++;
ptr += add = skip_ext_op (ptr, end);
if (!add)
return -1;
break;
case GRUB_ACPI_OPCODE_CREATE_WORD_FIELD:
case GRUB_ACPI_OPCODE_CREATE_BYTE_FIELD:
{
ptr += 5;
ptr += add = skip_data_ref_object (ptr, end);
if (!add)
return -1;
ptr += 4;
break;
}
case GRUB_ACPI_OPCODE_NAME:
ptr++;
if (memcmp (ptr, "_S5_", 4) == 0 || memcmp (ptr, "\\_S5_", 4) == 0)
{
int ll;
grub_uint8_t *ptr2 = ptr;
grub_dprintf ("acpi", "S5 found\n");
ptr2 += skip_name_string (ptr, end);
if (*ptr2 != 0x12)
{
grub_printf ("Unknown opcode in _S5: 0x%x\n", *ptr2);
return -1;
}
ptr2++;
decode_length (ptr2, &ll);
ptr2 += ll;
ptr2++;
switch (*ptr2)
{
case GRUB_ACPI_OPCODE_ZERO:
sleep_type = 0;
break;
case GRUB_ACPI_OPCODE_ONE:
sleep_type = 1;
break;
case GRUB_ACPI_OPCODE_BYTE_CONST:
sleep_type = ptr2[1];
break;
default:
grub_printf ("Unknown data type in _S5: 0x%x\n", *ptr2);
return -1;
}
}
ptr += add = skip_name_string (ptr, end);
if (!add)
return -1;
ptr += add = skip_data_ref_object (ptr, end);
if (!add)
return -1;
break;
case GRUB_ACPI_OPCODE_SCOPE:
case GRUB_ACPI_OPCODE_IF:
case GRUB_ACPI_OPCODE_METHOD:
{
ptr++;
ptr += decode_length (ptr, 0);
break;
}
default:
grub_printf ("Unknown opcode 0x%x\n", *ptr);
return -1;
}
}
grub_dprintf ("acpi", "TYP = %d\n", sleep_type);
return sleep_type;
}
/* Section 21.4.3.1 MAC-RESOURCE */
int macpdu_decode_resource(struct tetra_resrc_decoded *rsd, const uint8_t *bits)
{
const uint8_t *cur = bits + 4;
rsd->encryption_mode = bits_to_uint(cur, 2); cur += 2;
rsd->rand_acc_flag = *cur++;
rsd->macpdu_length = decode_length(bits_to_uint(cur, 6)); cur += 6;
rsd->addr.type = bits_to_uint(cur, 3); cur += 3;
switch (rsd->addr.type) {
case ADDR_TYPE_NULL:
return 0;
break;
case ADDR_TYPE_SSI:
case ADDR_TYPE_USSI:
case ADDR_TYPE_SMI:
rsd->addr.ssi = bits_to_uint(cur, 24);
break;
case ADDR_TYPE_EVENT_LABEL:
rsd->addr.event_label = bits_to_uint(cur, 10);
break;
case ADDR_TYPE_SSI_EVENT:
case ADDR_TYPE_SMI_EVENT:
rsd->addr.ssi = bits_to_uint(cur, 24);
rsd->addr.event_label = bits_to_uint(cur+24, 10);
break;
case ADDR_TYPE_SSI_USAGE:
rsd->addr.ssi = bits_to_uint(cur, 24);
rsd->addr.usage_marker = bits_to_uint(cur+24, 6);
break;
default:
return -EINVAL;
break;
}
cur += addr_len_by_type[rsd->addr.type];
/* no intermediate napping in pi/4 */
rsd->power_control_pres = *cur++;
if (rsd->power_control_pres)
cur += 4;
rsd->slot_granting.pres = *cur++;
if (rsd->slot_granting.pres) {
#if 0
/* check for multiple slot granting flag (can only exist in QAM) */
if (*cur++) {
cur += 0; //FIXME;
} else {
#endif
rsd->slot_granting.nr_slots =
decode_nr_slots(bits_to_uint(cur, 4));
cur += 4;
rsd->slot_granting.delay = bits_to_uint(cur, 4);
cur += 4;
#if 0
}
#endif
}
rsd->chan_alloc_pres = *cur++;
/* FIXME: If encryption is enabled, Channel Allocation is encrypted !!! */
if (rsd->chan_alloc_pres)
cur += decode_chan_alloc(&rsd->cad, cur);
/* FIXME: TM-SDU */
return cur - bits;
}
static int
get_sleep_type (grub_uint8_t *table, grub_uint8_t *ptr, grub_uint8_t *end,
grub_uint8_t *scope, int scope_len)
{
grub_uint8_t *prev = table;
if (!ptr)
ptr = table + sizeof (struct grub_acpi_table_header);
while (ptr < end && prev < ptr)
{
int add;
prev = ptr;
grub_dprintf ("acpi", "Opcode 0x%x\n", *ptr);
grub_dprintf ("acpi", "Tell %x\n", (unsigned) (ptr - table));
switch (*ptr)
{
case GRUB_ACPI_OPCODE_EXTOP:
ptr++;
ptr += add = skip_ext_op (ptr, end);
if (!add)
return -1;
break;
case GRUB_ACPI_OPCODE_CREATE_WORD_FIELD:
case GRUB_ACPI_OPCODE_CREATE_BYTE_FIELD:
{
ptr += 5;
ptr += add = skip_data_ref_object (ptr, end);
if (!add)
return -1;
ptr += 4;
break;
}
case GRUB_ACPI_OPCODE_NAME:
ptr++;
if ((!scope || grub_memcmp (scope, "\\", scope_len) == 0) &&
(grub_memcmp (ptr, "_S5_", 4) == 0 || grub_memcmp (ptr, "\\_S5_", 4) == 0))
{
int ll;
grub_uint8_t *ptr2 = ptr;
grub_dprintf ("acpi", "S5 found\n");
ptr2 += skip_name_string (ptr, end);
if (*ptr2 != 0x12)
{
grub_printf ("Unknown opcode in _S5: 0x%x\n", *ptr2);
return -1;
}
ptr2++;
decode_length (ptr2, &ll);
ptr2 += ll;
ptr2++;
switch (*ptr2)
{
case GRUB_ACPI_OPCODE_ZERO:
return 0;
case GRUB_ACPI_OPCODE_ONE:
return 1;
case GRUB_ACPI_OPCODE_BYTE_CONST:
return ptr2[1];
default:
grub_printf ("Unknown data type in _S5: 0x%x\n", *ptr2);
return -1;
}
}
ptr += add = skip_name_string (ptr, end);
if (!add)
return -1;
ptr += add = skip_data_ref_object (ptr, end);
if (!add)
return -1;
break;
case GRUB_ACPI_OPCODE_SCOPE:
{
int scope_sleep_type;
int ll;
grub_uint8_t *name;
int name_len;
ptr++;
add = decode_length (ptr, &ll);
name = ptr + ll;
name_len = skip_name_string (name, ptr + add);
if (!name_len)
return -1;
scope_sleep_type = get_sleep_type (table, name + name_len,
ptr + add, name, name_len);
if (scope_sleep_type != -2)
return scope_sleep_type;
ptr += add;
break;
}
case GRUB_ACPI_OPCODE_IF:
case GRUB_ACPI_OPCODE_METHOD:
{
ptr++;
ptr += decode_length (ptr, 0);
break;
}
default:
grub_printf ("Unknown opcode 0x%x\n", *ptr);
return -1;
}
}
return -2;
}
