diff --git a/dvb/sim.h b/dvb/sim.h index 87ba15f..1930ae9 100644 --- a/dvb/sim.h +++ b/dvb/sim.h @@ -31,8 +31,8 @@ extern "C" /***************************************************************************** * Generic "Type, Length, Value" message *****************************************************************************/ -#define TLV_HEADER_SIZE 5 -#define TLV_PARAM_SIZE 4 +#define TLVH_HEADER_SIZE 1 +#define TLV_HEADER_SIZE 4 #define TLV_PARAM_EMPTY 0 #define TLV_PARAM_EMPTY_SIZE 2 @@ -43,72 +43,55 @@ typedef struct tlv_param_count_t { uint16_t i_max; } tlv_param_count_t; -static inline void tlv_append_empty(uint8_t *p_tlv) +static inline void tlvh_set_version(uint8_t *p_tlv, uint8_t i_version) { - /* prepare a dummy parameter for later appending */ - p_tlv[TLV_HEADER_SIZE] = TLV_PARAM_EMPTY >> 8; - p_tlv[TLV_HEADER_SIZE + 1] = TLV_PARAM_EMPTY & 0xff; + p_tlv[0] = i_version; } -static inline void tlv_set_version(uint8_t *p_tlv, uint8_t i_version) +static inline uint8_t tlvh_get_version(uint8_t *p_tlv) { - p_tlv[0] = i_version; + return p_tlv[0]; } -static inline uint8_t tlv_get_version(uint8_t *p_tlv) +static inline uint8_t *tlvh_get_tlv(uint8_t *p_tlv) { - return p_tlv[0]; + return p_tlv + 1; } static inline void tlv_set_type(uint8_t *p_tlv, uint16_t i_type) { - p_tlv[1] = i_type >> 8; - p_tlv[2] = i_type & 0xff; + p_tlv[0] = i_type >> 8; + p_tlv[1] = i_type & 0xff; } static inline uint16_t tlv_get_type(uint8_t *p_tlv) { - return (p_tlv[1] << 8) | p_tlv[2]; + return (p_tlv[0] << 8) | p_tlv[1]; } static inline void tlv_set_length(uint8_t *p_tlv, uint16_t i_length) { - p_tlv[3] = i_length >> 8; - p_tlv[4] = i_length & 0xff; + p_tlv[2] = i_length >> 8; + p_tlv[3] = i_length & 0xff; } static inline uint16_t tlv_get_length(uint8_t *p_tlv) { - return (p_tlv[3] << 8) | p_tlv[4]; -} - -static inline void tlvn_set_type(uint8_t *p_tlv_n, uint16_t i_type) -{ - p_tlv_n[0] = i_type >> 8; - p_tlv_n[1] = i_type & 0xff; + return (p_tlv[2] << 8) | p_tlv[3]; } -static inline uint16_t tlvn_get_type(uint8_t *p_tlv_n) +static inline void tlv_empty(uint8_t *p_tlv) { - return (p_tlv_n[0] << 8) | p_tlv_n[1]; -} - -static inline void tlvn_set_length(uint8_t *p_tlv_n, uint16_t i_length) -{ - p_tlv_n[2] = i_length >> 8; - p_tlv_n[3] = i_length & 0xff; -} - -static inline uint16_t tlvn_get_length(uint8_t *p_tlv_n) -{ - return (p_tlv_n[2] << 8) | p_tlv_n[3]; + /* prepare a dummy parameter for later appending */ + p_tlv[TLV_HEADER_SIZE] = TLV_PARAM_EMPTY >> 8; + p_tlv[TLV_HEADER_SIZE + 1] = TLV_PARAM_EMPTY & 0xff; } -static inline void tlvn_append_empty(uint8_t *p_tlv_n) +static inline void tlv_append_empty(uint8_t *p_tlv) { - uint16_t i_length = tlvn_get_length(p_tlv_n); - p_tlv_n[TLV_PARAM_SIZE + i_length] = TLV_PARAM_EMPTY >> 8; - p_tlv_n[TLV_PARAM_SIZE + i_length + 1] = TLV_PARAM_EMPTY & 0xff; + uint16_t i_length = tlv_get_length(p_tlv); + p_tlv[TLV_HEADER_SIZE + i_length] = TLV_PARAM_EMPTY >> 8; + p_tlv[TLV_HEADER_SIZE + i_length + 1] = TLV_PARAM_EMPTY & 0xff; } static inline uint8_t *tlv_get_param(uint8_t *p_tlv, uint16_t n) @@ -118,9 +101,9 @@ static inline uint8_t *tlv_get_param(uint8_t *p_tlv, uint16_t n) if (p_tlv_n - p_tlv - TLV_HEADER_SIZE > i_tlv_size) return NULL; while (n) { - if (p_tlv_n + TLV_PARAM_SIZE - p_tlv - TLV_HEADER_SIZE > i_tlv_size) + if (p_tlv_n - p_tlv > i_tlv_size) return NULL; - p_tlv_n += TLV_PARAM_SIZE + tlvn_get_length(p_tlv_n); + p_tlv_n += TLV_HEADER_SIZE + tlv_get_length(p_tlv_n); n--; } if (p_tlv_n - p_tlv - TLV_HEADER_SIZE >= i_tlv_size) return NULL; @@ -131,8 +114,7 @@ static inline bool tlv_validate_param(const uint8_t *p_tlv, const uint8_t *p_tlv_n, uint16_t i_length) { uint16_t i_tlv_size = tlv_get_length(p_tlv); - return (p_tlv_n + TLV_PARAM_SIZE + i_length - p_tlv - TLV_HEADER_SIZE - <= i_tlv_size); + return (p_tlv_n + i_length - p_tlv <= i_tlv_size); } static inline bool tlv_validate(const uint8_t *p_tlv) @@ -140,10 +122,9 @@ static inline bool tlv_validate(const uint8_t *p_tlv) uint16_t i_tlv_size = tlv_get_length(p_tlv); const uint8_t *p_tlv_n = p_tlv + TLV_HEADER_SIZE; - while (p_tlv_n + TLV_PARAM_SIZE - p_tlv - TLV_HEADER_SIZE <= i_tlv_size - && p_tlv_n + TLV_PARAM_SIZE + tlvn_get_length(p_tlv_n) - - p_tlv - TLV_HEADER_SIZE <= i_tlv_size) - p_tlv_n += TLV_PARAM_SIZE + tlvn_get_length(p_tlv_n); + while (p_tlv_n - p_tlv <= i_tlv_size + && p_tlv_n + tlv_get_length(p_tlv_n) - p_tlv <= i_tlv_size) + p_tlv_n += TLV_HEADER_SIZE + tlv_get_length(p_tlv_n); return (p_tlv_n - p_tlv - TLV_HEADER_SIZE == i_tlv_size); } @@ -156,7 +137,7 @@ static inline uint8_t *tlv_find_param(uint8_t *p_tlv, uint16_t i_type, while ((p_param = tlv_get_param(p_tlv, j)) != NULL) { j++; - if (tlvn_get_type(p_param) == i_type) { + if (tlv_get_type(p_param) == i_type) { if (!n) return p_param; n--; } @@ -173,7 +154,7 @@ static inline uint16_t tlv_count_param(uint8_t *p_tlv, uint16_t i_type) while ((p_tlv_n = tlv_get_param(p_tlv, j)) != NULL) { j++; - if (tlvn_get_type(p_tlv_n) == i_type) + if (tlv_get_type(p_tlv_n) == i_type) i_count++; } return i_count; @@ -194,12 +175,12 @@ static inline bool intf##_append_##name(uint8_t *p_tlv, type i_##name) \ if (!tlv_validate_param(p_tlv, p_tlv_n, \ sizeof(type) + TLV_PARAM_EMPTY_SIZE)) \ return false; \ - tlvn_set_type(p_tlv_n, param); \ - tlvn_set_length(p_tlv_n, sizeof(type)); \ + tlv_set_type(p_tlv_n, param); \ + tlv_set_length(p_tlv_n, sizeof(type)); \ for (i = 0; i < sizeof(type); i++) \ p_tlv_n[4 + i] = ((utype)(i_##name) >> 8 * (sizeof(type) - i - 1)) \ & 0xff; \ - tlvn_append_empty(p_tlv_n); \ + tlv_append_empty(p_tlv_n); \ return true; \ } \ \ @@ -221,10 +202,10 @@ static inline bool tlv_append_data(uint8_t *p_tlv, uint16_t i_type, if (!tlv_validate_param(p_tlv, p_tlv_n, i_length + TLV_PARAM_EMPTY_SIZE)) return false; - tlvn_set_type(p_tlv_n, i_type); - tlvn_set_length(p_tlv_n, i_length); + tlv_set_type(p_tlv_n, i_type); + tlv_set_length(p_tlv_n, i_length); memcpy(p_tlv_n + 4, p_data, i_length); - tlvn_append_empty(p_tlv_n); + tlv_append_empty(p_tlv_n); return true; } @@ -232,7 +213,7 @@ static inline uint8_t *tlv_find_data(uint8_t *p_tlv, uint16_t i_type, uint16_t n, uint16_t *pi_length) { const uint8_t *p_tlv_n = tlv_find_param(p_tlv, i_type, n); - *pi_length = tlvn_get_length(p_tlv_n); + *pi_length = tlv_get_length(p_tlv_n); return p_tlv_n + 4; } @@ -285,7 +266,7 @@ static inline uint8_t *tlv_find_data(uint8_t *p_tlv, uint16_t i_type, static inline void ecmg_init(uint8_t *p_tlv) { - tlv_append_empty(p_tlv); + tlv_empty(p_tlv); } TLV_DECLARE_PARAM(ecmg, supercasid, ECMG_PARAM_SUPERCASID, uint32_t, uint32_t) @@ -339,8 +320,8 @@ static inline bool ecmg_validate_param(const uint8_t *p_tlv_n) /* 0x10 */ 2, 1, 2, 2, 0, 0, 2, 2, /* 0x18 */ 0, 2 }; - uint16_t i_type = tlvn_get_type(p_tlv_n); - uint16_t i_length = tlvn_get_length(p_tlv_n); + uint16_t i_type = tlv_get_type(p_tlv_n); + uint16_t i_length = tlv_get_length(p_tlv_n); if (i_type <= ECMG_PARAM_ECMID) { if (i_length < pi_ecmg_params_minlength[i_type]) return false; diff --git a/examples/dvb_ecmg.c b/examples/dvb_ecmg.c index 0f2d0d4..0e95067 100644 --- a/examples/dvb_ecmg.c +++ b/examples/dvb_ecmg.c @@ -131,14 +131,15 @@ ssize_t write_wrapper(const void *p_buf, size_t i_count) *****************************************************************************/ static void send_channel_status(void) { - uint8_t p_tlv[MAX_TLV_SIZE]; + uint8_t p_tlv_h[MAX_TLV_SIZE]; + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint8_t *p_tlv_n; + tlvh_set_version(p_tlv_h, i_version); ecmg_init(p_tlv); - tlv_set_version(p_tlv, i_version); tlv_set_type(p_tlv, ECMG_TYPE_CHANNEL_STATUS); /* length will be written at the end */ - tlv_set_length(p_tlv, MAX_TLV_SIZE); + tlv_set_length(p_tlv, MAX_TLV_SIZE - TLV_HEADER_SIZE - TLVH_HEADER_SIZE); ecmg_append_channelid(p_tlv, i_channelid); ecmg_append_sectiontspkt(p_tlv, 0x0); /* sections */ @@ -153,7 +154,7 @@ static void send_channel_status(void) p_tlv_n = tlv_find_param(p_tlv, TLV_PARAM_EMPTY, 0); tlv_set_length(p_tlv, p_tlv_n - p_tlv - TLV_HEADER_SIZE); - write_wrapper(p_tlv, p_tlv_n - p_tlv); + write_wrapper(p_tlv_h, p_tlv_n - p_tlv_h); } /***************************************************************************** @@ -161,22 +162,23 @@ static void send_channel_status(void) *****************************************************************************/ static void send_channel_test(void) { - uint8_t p_tlv[MAX_TLV_SIZE]; + uint8_t p_tlv_h[MAX_TLV_SIZE]; + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint8_t *p_tlv_n; fprintf(stderr, "sending test on channel ID=0x%hx\n", i_channelid); + tlvh_set_version(p_tlv_h, i_version); ecmg_init(p_tlv); - tlv_set_version(p_tlv, i_version); tlv_set_type(p_tlv, ECMG_TYPE_CHANNEL_TEST); /* length will be written at the end */ - tlv_set_length(p_tlv, MAX_TLV_SIZE); + tlv_set_length(p_tlv, MAX_TLV_SIZE - TLV_HEADER_SIZE - TLVH_HEADER_SIZE); ecmg_append_channelid(p_tlv, i_channelid); p_tlv_n = tlv_find_param(p_tlv, TLV_PARAM_EMPTY, 0); tlv_set_length(p_tlv, p_tlv_n - p_tlv - TLV_HEADER_SIZE); - write_wrapper(p_tlv, p_tlv_n - p_tlv); + write_wrapper(p_tlv_h, p_tlv_n - p_tlv_h); } /***************************************************************************** @@ -184,24 +186,25 @@ static void send_channel_test(void) *****************************************************************************/ static void send_channel_error(uint16_t i_wanted_channelid, uint16_t i_error) { - uint8_t p_tlv[MAX_TLV_SIZE]; + uint8_t p_tlv_h[MAX_TLV_SIZE]; + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint8_t *p_tlv_n; fprintf(stderr, "sending error on channel ID=0x%hx error=0x%hx\n", i_wanted_channelid, i_error); + tlvh_set_version(p_tlv_h, i_version); ecmg_init(p_tlv); - tlv_set_version(p_tlv, i_version); tlv_set_type(p_tlv, ECMG_TYPE_CHANNEL_ERROR); /* length will be written at the end */ - tlv_set_length(p_tlv, MAX_TLV_SIZE); + tlv_set_length(p_tlv, MAX_TLV_SIZE - TLV_HEADER_SIZE - TLVH_HEADER_SIZE); ecmg_append_channelid(p_tlv, i_wanted_channelid); ecmg_append_errorstatus(p_tlv, i_error); p_tlv_n = tlv_find_param(p_tlv, TLV_PARAM_EMPTY, 0); tlv_set_length(p_tlv, p_tlv_n - p_tlv - TLV_HEADER_SIZE); - write_wrapper(p_tlv, p_tlv_n - p_tlv); + write_wrapper(p_tlv_h, p_tlv_n - p_tlv_h); } /***************************************************************************** @@ -209,14 +212,15 @@ static void send_channel_error(uint16_t i_wanted_channelid, uint16_t i_error) *****************************************************************************/ static void send_stream_status(stream_t *p_stream) { - uint8_t p_tlv[MAX_TLV_SIZE]; + uint8_t p_tlv_h[MAX_TLV_SIZE]; + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint8_t *p_tlv_n; + tlvh_set_version(p_tlv_h, i_version); ecmg_init(p_tlv); - tlv_set_version(p_tlv, i_version); tlv_set_type(p_tlv, ECMG_TYPE_STREAM_STATUS); /* length will be written at the end */ - tlv_set_length(p_tlv, MAX_TLV_SIZE); + tlv_set_length(p_tlv, MAX_TLV_SIZE - TLV_HEADER_SIZE - TLVH_HEADER_SIZE); ecmg_append_channelid(p_tlv, i_channelid); ecmg_append_streamid(p_tlv, p_stream->i_streamid); @@ -226,7 +230,7 @@ static void send_stream_status(stream_t *p_stream) p_tlv_n = tlv_find_param(p_tlv, TLV_PARAM_EMPTY, 0); tlv_set_length(p_tlv, p_tlv_n - p_tlv - TLV_HEADER_SIZE); - write_wrapper(p_tlv, p_tlv_n - p_tlv); + write_wrapper(p_tlv_h, p_tlv_n - p_tlv_h); } /***************************************************************************** @@ -234,21 +238,22 @@ static void send_stream_status(stream_t *p_stream) *****************************************************************************/ static void send_stream_close(stream_t *p_stream) { - uint8_t p_tlv[MAX_TLV_SIZE]; + uint8_t p_tlv_h[MAX_TLV_SIZE]; + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint8_t *p_tlv_n; + tlvh_set_version(p_tlv_h, i_version); ecmg_init(p_tlv); - tlv_set_version(p_tlv, i_version); tlv_set_type(p_tlv, ECMG_TYPE_STREAM_CLOSERESP); /* length will be written at the end */ - tlv_set_length(p_tlv, MAX_TLV_SIZE); + tlv_set_length(p_tlv, MAX_TLV_SIZE - TLV_HEADER_SIZE - TLVH_HEADER_SIZE); ecmg_append_channelid(p_tlv, i_channelid); ecmg_append_streamid(p_tlv, p_stream->i_streamid); p_tlv_n = tlv_find_param(p_tlv, TLV_PARAM_EMPTY, 0); tlv_set_length(p_tlv, p_tlv_n - p_tlv - TLV_HEADER_SIZE); - write_wrapper(p_tlv, p_tlv_n - p_tlv); + write_wrapper(p_tlv_h, p_tlv_n - p_tlv_h); } /***************************************************************************** @@ -256,17 +261,18 @@ static void send_stream_close(stream_t *p_stream) *****************************************************************************/ static void send_stream_error(uint16_t i_wanted_streamid, uint16_t i_error) { - uint8_t p_tlv[MAX_TLV_SIZE]; + uint8_t p_tlv_h[MAX_TLV_SIZE]; + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint8_t *p_tlv_n; fprintf(stderr, "sending error on stream ID=0x%hx error=0x%hx\n", i_wanted_streamid, i_error); + tlvh_set_version(p_tlv_h, i_version); ecmg_init(p_tlv); - tlv_set_version(p_tlv, i_version); tlv_set_type(p_tlv, ECMG_TYPE_STREAM_ERROR); /* length will be written at the end */ - tlv_set_length(p_tlv, MAX_TLV_SIZE); + tlv_set_length(p_tlv, MAX_TLV_SIZE - TLV_HEADER_SIZE - TLVH_HEADER_SIZE); ecmg_append_channelid(p_tlv, i_channelid); ecmg_append_streamid(p_tlv, i_wanted_streamid); @@ -274,7 +280,7 @@ static void send_stream_error(uint16_t i_wanted_streamid, uint16_t i_error) p_tlv_n = tlv_find_param(p_tlv, TLV_PARAM_EMPTY, 0); tlv_set_length(p_tlv, p_tlv_n - p_tlv - TLV_HEADER_SIZE); - write_wrapper(p_tlv, p_tlv_n - p_tlv); + write_wrapper(p_tlv_h, p_tlv_n - p_tlv_h); } /***************************************************************************** @@ -283,14 +289,16 @@ static void send_stream_error(uint16_t i_wanted_streamid, uint16_t i_error) static void send_ecm(stream_t *p_stream, uint16_t i_cp_number, const uint8_t *p_ecm, uint16_t i_length) { - uint8_t p_tlv[MAX_TLV_SIZE + i_length]; /* this is oversized */ + uint8_t p_tlv_h[MAX_TLV_SIZE + i_length]; /* this is oversized */ + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint8_t *p_tlv_n; + tlvh_set_version(p_tlv_h, i_version); ecmg_init(p_tlv); - tlv_set_version(p_tlv, i_version); tlv_set_type(p_tlv, ECMG_TYPE_ECM); /* length will be written at the end */ - tlv_set_length(p_tlv, MAX_TLV_SIZE + i_length); + tlv_set_length(p_tlv, MAX_TLV_SIZE - TLV_HEADER_SIZE - TLVH_HEADER_SIZE + + i_length); ecmg_append_channelid(p_tlv, i_channelid); ecmg_append_streamid(p_tlv, p_stream->i_streamid); @@ -299,7 +307,7 @@ static void send_ecm(stream_t *p_stream, uint16_t i_cp_number, p_tlv_n = tlv_find_param(p_tlv, TLV_PARAM_EMPTY, 0); tlv_set_length(p_tlv, p_tlv_n - p_tlv - TLV_HEADER_SIZE); - write_wrapper(p_tlv, p_tlv_n - p_tlv); + write_wrapper(p_tlv_h, p_tlv_n - p_tlv_h); } /***************************************************************************** @@ -356,7 +364,6 @@ static void handle_channel_setup(uint8_t *p_tlv) return; } - i_version = tlv_get_version(p_tlv); i_channelid = i_wanted_channelid; i_supercasid = ecmg_find_supercasid(p_tlv, 0); @@ -610,7 +617,8 @@ int main(int i_argc, char **ppsz_argv) } for ( ; ; ) { - uint8_t *p_tlv = malloc(TLV_HEADER_SIZE); + uint8_t *p_tlv_h = malloc(TLVH_HEADER_SIZE + TLV_HEADER_SIZE); + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint16_t i_type; fd_set rset; struct timeval timeout; @@ -631,41 +639,46 @@ int main(int i_argc, char **ppsz_argv) continue; } - if (read_wrapper(p_tlv, TLV_HEADER_SIZE) <= 0) + if (read_wrapper(p_tlv_h, TLVH_HEADER_SIZE + TLV_HEADER_SIZE) <= 0) return EXIT_FAILURE; - p_tlv = realloc(p_tlv, TLV_HEADER_SIZE + tlv_get_length(p_tlv)); + p_tlv_h = realloc(p_tlv_h, TLVH_HEADER_SIZE + TLV_HEADER_SIZE + + tlv_get_length(p_tlv)); + p_tlv = tlvh_get_tlv(p_tlv_h); if (read_wrapper(p_tlv + TLV_HEADER_SIZE, tlv_get_length(p_tlv)) <= 0) return EXIT_FAILURE; + if ((b_init && tlvh_get_version(p_tlv_h) > 3) || + (!b_init && tlvh_get_version(p_tlv_h) != i_version)) { + send_channel_error(i_channelid, 0x2); + free(p_tlv_h); + continue; + } + i_type = tlv_get_type(p_tlv); if (!tlv_validate(p_tlv)) { send_channel_error(i_channelid, 0x1); - free(p_tlv); + free(p_tlv_h); continue; } if (!ecmg_validate(p_tlv)) { send_channel_error(i_channelid, 0xf); - free(p_tlv); + free(p_tlv_h); continue; } if (b_init && i_type != ECMG_TYPE_CHANNEL_SETUP) { send_channel_error(i_channelid, 0x6); - free(p_tlv); - continue; - } - - if (tlv_get_version(p_tlv) > 3) { - send_channel_error(i_channelid, 0x2); - free(p_tlv); + free(p_tlv_h); continue; } switch (i_type) { case ECMG_TYPE_CHANNEL_SETUP: + if (b_init) + i_version = tlvh_get_version(p_tlv_h); handle_channel_setup(p_tlv); break; @@ -710,7 +723,7 @@ int main(int i_argc, char **ppsz_argv) break; } - free(p_tlv); + free(p_tlv_h); } return EXIT_SUCCESS; diff --git a/examples/dvb_ecmg_test.c b/examples/dvb_ecmg_test.c index 2584c16..966214b 100644 --- a/examples/dvb_ecmg_test.c +++ b/examples/dvb_ecmg_test.c @@ -115,11 +115,12 @@ ssize_t write_wrapper(const void *p_buf, size_t i_count) *****************************************************************************/ static void send_channel_setup(void) { - uint8_t p_tlv[MAX_TLV_SIZE]; + uint8_t p_tlv_h[MAX_TLV_SIZE]; + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint8_t *p_tlv_n; + tlvh_set_version(p_tlv_h, i_version); ecmg_init(p_tlv); - tlv_set_version(p_tlv, i_version); tlv_set_type(p_tlv, ECMG_TYPE_CHANNEL_SETUP); /* length will be written at the end */ tlv_set_length(p_tlv, MAX_TLV_SIZE); @@ -129,7 +130,7 @@ static void send_channel_setup(void) p_tlv_n = tlv_find_param(p_tlv, TLV_PARAM_EMPTY, 0); tlv_set_length(p_tlv, p_tlv_n - p_tlv - TLV_HEADER_SIZE); - write_wrapper(p_tlv, p_tlv_n - p_tlv); + write_wrapper(p_tlv_h, p_tlv_n - p_tlv_h); } /***************************************************************************** @@ -137,24 +138,25 @@ static void send_channel_setup(void) *****************************************************************************/ static void send_channel_error(uint16_t i_wanted_channelid, uint16_t i_error) { - uint8_t p_tlv[MAX_TLV_SIZE]; + uint8_t p_tlv_h[MAX_TLV_SIZE]; + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint8_t *p_tlv_n; fprintf(stderr, "sending error on channel ID=0x%hx error=0x%hx\n", i_wanted_channelid, i_error); + tlvh_set_version(p_tlv_h, i_version); ecmg_init(p_tlv); - tlv_set_version(p_tlv, i_version); tlv_set_type(p_tlv, ECMG_TYPE_CHANNEL_ERROR); /* length will be written at the end */ - tlv_set_length(p_tlv, MAX_TLV_SIZE); + tlv_set_length(p_tlv, MAX_TLV_SIZE - TLV_HEADER_SIZE - TLVH_HEADER_SIZE); ecmg_append_channelid(p_tlv, i_wanted_channelid); ecmg_append_errorstatus(p_tlv, i_error); p_tlv_n = tlv_find_param(p_tlv, TLV_PARAM_EMPTY, 0); tlv_set_length(p_tlv, p_tlv_n - p_tlv - TLV_HEADER_SIZE); - write_wrapper(p_tlv, p_tlv_n - p_tlv); + write_wrapper(p_tlv_h, p_tlv_n - p_tlv_h); } /***************************************************************************** @@ -162,14 +164,15 @@ static void send_channel_error(uint16_t i_wanted_channelid, uint16_t i_error) *****************************************************************************/ static void send_stream_setup(stream_t *p_stream) { - uint8_t p_tlv[MAX_TLV_SIZE]; + uint8_t p_tlv_h[MAX_TLV_SIZE]; + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint8_t *p_tlv_n; + tlvh_set_version(p_tlv_h, i_version); ecmg_init(p_tlv); - tlv_set_version(p_tlv, i_version); tlv_set_type(p_tlv, ECMG_TYPE_STREAM_SETUP); /* length will be written at the end */ - tlv_set_length(p_tlv, MAX_TLV_SIZE); + tlv_set_length(p_tlv, MAX_TLV_SIZE - TLV_HEADER_SIZE - TLVH_HEADER_SIZE); ecmg_append_channelid(p_tlv, i_channelid); ecmg_append_streamid(p_tlv, p_stream->i_streamid); @@ -179,7 +182,7 @@ static void send_stream_setup(stream_t *p_stream) p_tlv_n = tlv_find_param(p_tlv, TLV_PARAM_EMPTY, 0); tlv_set_length(p_tlv, p_tlv_n - p_tlv - TLV_HEADER_SIZE); - write_wrapper(p_tlv, p_tlv_n - p_tlv); + write_wrapper(p_tlv_h, p_tlv_n - p_tlv_h); } /***************************************************************************** @@ -187,17 +190,18 @@ static void send_stream_setup(stream_t *p_stream) *****************************************************************************/ static void send_stream_error(uint16_t i_wanted_streamid, uint16_t i_error) { - uint8_t p_tlv[MAX_TLV_SIZE]; + uint8_t p_tlv_h[MAX_TLV_SIZE]; + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint8_t *p_tlv_n; fprintf(stderr, "sending error on stream ID=0x%hx error=0x%hx\n", i_wanted_streamid, i_error); + tlvh_set_version(p_tlv_h, i_version); ecmg_init(p_tlv); - tlv_set_version(p_tlv, i_version); tlv_set_type(p_tlv, ECMG_TYPE_STREAM_ERROR); /* length will be written at the end */ - tlv_set_length(p_tlv, MAX_TLV_SIZE); + tlv_set_length(p_tlv, MAX_TLV_SIZE - TLV_HEADER_SIZE - TLVH_HEADER_SIZE); ecmg_append_channelid(p_tlv, i_channelid); ecmg_append_streamid(p_tlv, i_wanted_streamid); @@ -205,7 +209,7 @@ static void send_stream_error(uint16_t i_wanted_streamid, uint16_t i_error) p_tlv_n = tlv_find_param(p_tlv, TLV_PARAM_EMPTY, 0); tlv_set_length(p_tlv, p_tlv_n - p_tlv - TLV_HEADER_SIZE); - write_wrapper(p_tlv, p_tlv_n - p_tlv); + write_wrapper(p_tlv_h, p_tlv_n - p_tlv_h); } /***************************************************************************** @@ -213,12 +217,13 @@ static void send_stream_error(uint16_t i_wanted_streamid, uint16_t i_error) *****************************************************************************/ static void send_cw(stream_t *p_stream) { - uint8_t p_tlv[MAX_TLV_SIZE]; + uint8_t p_tlv_h[MAX_TLV_SIZE]; + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint8_t *p_tlv_n; int i; + tlvh_set_version(p_tlv_h, i_version); ecmg_init(p_tlv); - tlv_set_version(p_tlv, i_version); tlv_set_type(p_tlv, ECMG_TYPE_CW); /* length will be written at the end */ tlv_set_length(p_tlv, MAX_TLV_SIZE); @@ -245,7 +250,7 @@ static void send_cw(stream_t *p_stream) p_tlv_n = tlv_find_param(p_tlv, TLV_PARAM_EMPTY, 0); tlv_set_length(p_tlv, p_tlv_n - p_tlv - TLV_HEADER_SIZE); - write_wrapper(p_tlv, p_tlv_n - p_tlv); + write_wrapper(p_tlv_h, p_tlv_n - p_tlv_h); } /***************************************************************************** @@ -483,7 +488,8 @@ int main(int i_argc, char **ppsz_argv) send_channel_setup(); for ( ; ; ) { - uint8_t *p_tlv = malloc(TLV_HEADER_SIZE); + uint8_t *p_tlv_h = malloc(TLVH_HEADER_SIZE + TLV_HEADER_SIZE); + uint8_t *p_tlv = tlvh_get_tlv(p_tlv_h); uint16_t i_type; fd_set rset; struct timeval timeout; @@ -500,36 +506,38 @@ int main(int i_argc, char **ppsz_argv) goto no_packet; } - if (read_wrapper(p_tlv, TLV_HEADER_SIZE) <= 0) + if (read_wrapper(p_tlv_h, TLVH_HEADER_SIZE + TLV_HEADER_SIZE) <= 0) return EXIT_FAILURE; - p_tlv = realloc(p_tlv, TLV_HEADER_SIZE + tlv_get_length(p_tlv)); + p_tlv_h = realloc(p_tlv_h, TLVH_HEADER_SIZE + TLV_HEADER_SIZE + + tlv_get_length(p_tlv)); + p_tlv = tlvh_get_tlv(p_tlv_h); if (read_wrapper(p_tlv + TLV_HEADER_SIZE, tlv_get_length(p_tlv)) <= 0) return EXIT_FAILURE; + if (tlvh_get_version(p_tlv_h) != i_version) { + send_channel_error(i_channelid, 0x2); + free(p_tlv_h); + exit(EXIT_FAILURE); + } + i_type = tlv_get_type(p_tlv); if (!tlv_validate(p_tlv)) { send_channel_error(i_channelid, 0x1); - free(p_tlv); + free(p_tlv_h); exit(EXIT_FAILURE); } if (!ecmg_validate(p_tlv)) { send_channel_error(i_channelid, 0xf); - free(p_tlv); + free(p_tlv_h); exit(EXIT_FAILURE); } if (b_init && i_type != ECMG_TYPE_CHANNEL_STATUS) { send_channel_error(i_channelid, 0x6); - free(p_tlv); - exit(EXIT_FAILURE); - } - - if (tlv_get_version(p_tlv) > 3) { - send_channel_error(i_channelid, 0x2); - free(p_tlv); + free(p_tlv_h); exit(EXIT_FAILURE); } @@ -567,9 +575,9 @@ int main(int i_argc, char **ppsz_argv) break; } - free(p_tlv); - no_packet: + free(p_tlv_h); + if (!b_init) { time_t i_time = time(NULL);