diff --git a/include/mtp_level3.h b/include/mtp_level3.h index bc2d186..f311ad5 100644 --- a/include/mtp_level3.h +++ b/include/mtp_level3.h @@ -65,7 +65,7 @@ #define SCCP_SSA 0x01 #define MTP_LINK_MASK 0x0F -#define MTP_ADDR_MASK 0x0FFF +#define MTP_ADDR_MASK 0x3FFF #define MTP_APOC_MASK 0x3f @@ -77,6 +77,10 @@ ((link) & MTP_LINK_MASK) << 28) #define MTP_MAKE_APOC(apoc) \ (apoc & 0x3fff) +#define MTP_ADDR_DPC(addr) \ + (addr & MTP_ADDR_MASK) +#define MTP_ADDR_OPC(addr) \ + ((addr >> 14) & MTP_ADDR_MASK) #elif __BYTE_ORDER == __BIG_ENDIAN static inline uint32_t c_swap_32(uint32_t in) { @@ -97,6 +101,10 @@ static inline uint16_t c_swap_16(uint16_t in) ((link) & MTP_LINK_MASK) << 28) #define MTP_MAKE_APOC(apoc) \ c_swap_16((apoc & 0x3fff)) +#define MTP_ADDR_DPC(addr) \ + (c_swap_32(addr) & MTP_ADDR_MASK) +#define MTP_ADDR_OPC(addr) \ + ((c_swap_32(addr) >> 14) & MTP_ADDR_MASK) #endif diff --git a/tests/mtp/mtp_parse_test.c b/tests/mtp/mtp_parse_test.c index 2ef726b..cd5d603 100644 --- a/tests/mtp/mtp_parse_test.c +++ b/tests/mtp/mtp_parse_test.c @@ -589,6 +589,18 @@ int main(int argc, char **argv) tests[2].hdr.addr = MTP_ADDR(0x00, 136, 91); tests[2].prohib.apoc = MTP_MAKE_APOC(86); + addr = MTP_ADDR(0x00, 0x2AAA, 0x1555); + if (MTP_ADDR_OPC(addr) != 0x1555) { + fprintf(stderr, "Failed to extract OPC: %d\n", + MTP_ADDR_OPC(addr)); + abort(); + } + if (MTP_ADDR_DPC(addr) != 0x2AAA) { + fprintf(stderr, "Failed to extract DPC: %d\n", + MTP_ADDR_DPC(addr)); + abort(); + } + for (i = 0; i < ARRAY_SIZE(tests); ++i) { check_hdr(tests[i].input, &tests[i].hdr); if (tests[i].has_mng)