static void bnep_eval_extension(int level, struct frame *frm)
{
uint8_t type = p_get_u8(frm);
uint8_t length = p_get_u8(frm);
int extension = type & 0x80;
p_indent(level, frm);
switch (type & 0x7f) {
case BNEP_EXTENSION_CONTROL:
printf("Ext Control(0x%02x|%s) len 0x%02x\n",
type & 0x7f, extension ? "1" : "0", length);
bnep_control(level, frm, length);
break;
default:
printf("Ext Unknown(0x%02x|%s) len 0x%02x\n",
type & 0x7f, extension ? "1" : "0", length);
raw_ndump(level + 1, frm, length);
frm->ptr += length;
frm->len -= length;
}
if (extension)
bnep_eval_extension(level, frm);
}
static void parse_parameters(int level, struct frame *frm)
{
uint8_t param;
uint16_t len;
uint8_t pv8;
while (frm->len > 3) {
p_indent(level, frm);
param = get_u8(frm);
get_u8(frm);
len = get_u16(frm);
printf("%s (0x%02x) len %d = ", param2str(param), param, len);
switch (param) {
case SAP_PARAM_ID_MAX_MSG_SIZE:
printf("%d\n", get_u16(frm));
break;
case SAP_PARAM_ID_CONN_STATUS:
pv8 = get_u8(frm);
printf("0x%02x (%s)\n", pv8, status2str(pv8));
break;
case SAP_PARAM_ID_RESULT_CODE:
case SAP_PARAM_ID_CARD_READER_STATUS:
pv8 = get_u8(frm);
printf("0x%02x (%s)\n", pv8, result2str(pv8));
break;
case SAP_PARAM_ID_DISCONNECT_IND:
pv8 = get_u8(frm);
printf("0x%02x (%s)\n", pv8, disctype2str(pv8));
break;
case SAP_PARAM_ID_STATUS_CHANGE:
pv8 = get_u8(frm);
printf("0x%02x (%s)\n", pv8, statuschg2str(pv8));
break;
case SAP_PARAM_ID_TRANSPORT_PROTOCOL:
pv8 = get_u8(frm);
printf("0x%02x (%s)\n", pv8, prot2str(pv8));
break;
default:
printf("\n");
raw_ndump(level + 1, frm, len);
frm->ptr += len;
frm->len -= len;
}
/* Skip padding */
frm->ptr += PADDING4(len);
frm->len -= PADDING4(len);
}
}
void raw_dump(int level, struct frame *frm)
{
raw_ndump(level, frm, -1);
}
static void bnep_control(int level, struct frame *frm, int header_length)
{
uint8_t uuid_size;
int i, length;
char *s;
uint32_t uuid = 0;
uint8_t type = p_get_u8(frm);
p_indent(++level, frm);
switch (type) {
case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
printf("Not Understood(0x%02x) type 0x%02x\n", type, p_get_u8(frm));
break;
case BNEP_SETUP_CONNECTION_REQUEST_MSG:
uuid_size = p_get_u8(frm);
printf("Setup Req(0x%02x) size 0x%02x ", type, uuid_size);
switch (uuid_size) {
case 2:
uuid = p_get_u16(frm);
printf("dst 0x%x", uuid);
if ((s = get_uuid_name(uuid)) != 0)
printf("(%s)", s);
uuid = p_get_u16(frm);
printf(" src 0x%x", uuid);
if ((s = get_uuid_name(uuid)) != 0)
printf("(%s)", s);
printf("\n");
break;
case 4:
uuid = p_get_u32(frm);
printf("dst 0x%x", uuid);
if ((s = get_uuid_name(uuid)) != 0)
printf("(%s)", s);
uuid = p_get_u32(frm);
printf(" src 0x%x", uuid);
if ((s = get_uuid_name(uuid)) != 0)
printf("(%s)", s);
printf("\n");
break;
case 16:
uuid = p_get_u32(frm);
printf("dst 0x%x", uuid);
if ((s = get_uuid_name(uuid)) != 0)
printf("(%s)", s);
frm->ptr += 12;
frm->len -= 12;
uuid = p_get_u32(frm);
printf(" src 0x%x", uuid);
if ((s = get_uuid_name(uuid)) != 0)
printf("(%s)", s);
printf("\n");
frm->ptr += 12;
frm->len -= 12;
break;
default:
frm->ptr += (uuid_size * 2);
frm->len -= (uuid_size * 2);
break;
}
break;
case BNEP_SETUP_CONNECTION_RESPONSE_MSG:
printf("Setup Rsp(0x%02x) res 0x%04x\n",
type, p_get_u16(frm));
break;
case BNEP_FILTER_NET_TYPE_SET_MSG:
length = p_get_u16(frm);
printf("Filter NetType Set(0x%02x) len 0x%04x\n",
type, length);
for (i = 0; i < length / 4; i++) {
p_indent(level + 1, frm);
printf("0x%04x - ", p_get_u16(frm));
printf("0x%04x\n", p_get_u16(frm));
}
break;
case BNEP_FILTER_NET_TYPE_RESPONSE_MSG:
printf("Filter NetType Rsp(0x%02x) res 0x%04x\n",
type, p_get_u16(frm));
break;
case BNEP_FILTER_MULT_ADDR_SET_MSG:
length = p_get_u16(frm);
printf("Filter MultAddr Set(0x%02x) len 0x%04x\n",
type, length);
for (i = 0; i < length / 12; i++) {
p_indent(level + 1, frm);
printf("%s - ", get_macaddr(frm));
printf("%s\n", get_macaddr(frm));
}
break;
case BNEP_FILTER_MULT_ADDR_RESPONSE_MSG:
printf("Filter MultAddr Rsp(0x%02x) res 0x%04x\n",
type, p_get_u16(frm));
break;
default:
printf("Unknown control type(0x%02x)\n", type);
raw_ndump(level + 1, frm, header_length - 1);
frm->ptr += header_length - 1;
frm->len -= header_length - 1;
return;
}
}
static void parse_headers(int level, struct frame *frm)
{
uint8_t hi, hv8;
uint16_t len;
uint32_t hv32;
while (frm->len > 0) {
hi = get_u8(frm);
p_indent(level, frm);
printf("%s (0x%02x)", hi2str(hi), hi);
switch (hi & 0xc0) {
case 0x00: /* Unicode */
if (frm->len < 2) {
printf("\n");
return;
}
len = get_u16(frm) - 3;
printf(" = Unicode length %d\n", len);
if (frm->len < len)
return;
raw_ndump(level, frm, len);
frm->ptr += len;
frm->len -= len;
break;
case 0x40: /* Byte sequence */
if (frm->len < 2) {
printf("\n");
return;
}
len = get_u16(frm) - 3;
printf(" = Sequence length %d\n", len);
if (frm->len < len)
return;
raw_ndump(level, frm, len);
frm->ptr += len;
frm->len -= len;
break;
case 0x80: /* One byte */
if (frm->len < 1) {
printf("\n");
return;
}
hv8 = get_u8(frm);
printf(" = %d\n", hv8);
break;
case 0xc0: /* Four bytes */
if (frm->len < 4) {
printf("\n");
return;
}
hv32 = get_u32(frm);
printf(" = %u\n", hv32);
break;
}
}
}
