avdtp_util.c (ea6072aff7664340978c5dd4457bb44e23303e5b) avdtp_util.c (f6f3c9037bb5af01c27f4ed83398f8a68d35a69b)
1/*
2 * Copyright (C) 2016 BlueKitchen GmbH
3 *
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions
6 * are met:
7 *
8 * 1. Redistributions of source code must retain the above copyright

--- 321 unchanged lines hidden (view full) ---

330 if (cap_len != 0){
331 log_info(" ERROR: REJECT CATEGORY, BAD_LENGTH\n");
332 connection->reject_service_category = category;
333 connection->error_code = BAD_LENGTH;
334 return 1;
335 }
336 break;
337 case AVDTP_RECOVERY:
1/*
2 * Copyright (C) 2016 BlueKitchen GmbH
3 *
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions
6 * are met:
7 *
8 * 1. Redistributions of source code must retain the above copyright

--- 321 unchanged lines hidden (view full) ---

330 if (cap_len != 0){
331 log_info(" ERROR: REJECT CATEGORY, BAD_LENGTH\n");
332 connection->reject_service_category = category;
333 connection->error_code = BAD_LENGTH;
334 return 1;
335 }
336 break;
337 case AVDTP_RECOVERY:
338 if (cap_len < 3){
338 if (cap_len != 3){
339 log_info(" ERROR: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n");
340 connection->reject_service_category = category;
341 connection->error_code = BAD_RECOVERY_FORMAT;
342 return 1;
343 }
344 break;
345 case AVDTP_CONTENT_PROTECTION:
346 if (cap_len < 2){
347 log_info(" ERROR: REJECT CATEGORY, BAD_CP_FORMAT\n");
348 connection->reject_service_category = category;
349 connection->error_code = BAD_CP_FORMAT;
350 return 1;
351 }
352 break;
353 case AVDTP_HEADER_COMPRESSION:
339 log_info(" ERROR: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n");
340 connection->reject_service_category = category;
341 connection->error_code = BAD_RECOVERY_FORMAT;
342 return 1;
343 }
344 break;
345 case AVDTP_CONTENT_PROTECTION:
346 if (cap_len < 2){
347 log_info(" ERROR: REJECT CATEGORY, BAD_CP_FORMAT\n");
348 connection->reject_service_category = category;
349 connection->error_code = BAD_CP_FORMAT;
350 return 1;
351 }
352 break;
353 case AVDTP_HEADER_COMPRESSION:
354 // TODO: find error code for bad header compression
355 if (cap_len != 1){
356 log_info(" ERROR: REJECT CATEGORY, BAD_HEADER_COMPRESSION\n");
357 connection->reject_service_category = category;
358 connection->error_code = BAD_RECOVERY_FORMAT;
359 return 1;
360 }
354 break;
355 case AVDTP_MULTIPLEXING:
356 break;
357 case AVDTP_MEDIA_CODEC:
358 break;
359 default:
360 break;
361 }
362 return 0;
363}
364
365uint16_t avdtp_unpack_service_capabilities(avdtp_connection_t * connection, avdtp_capabilities_t * caps, uint8_t * packet, uint16_t size){
361 break;
362 case AVDTP_MULTIPLEXING:
363 break;
364 case AVDTP_MEDIA_CODEC:
365 break;
366 default:
367 break;
368 }
369 return 0;
370}
371
372uint16_t avdtp_unpack_service_capabilities(avdtp_connection_t * connection, avdtp_capabilities_t * caps, uint8_t * packet, uint16_t size){
366 if (size == 0) return 0;
367
373
368 uint16_t registered_service_categories = 0;
369 int pos = 0;
370 int i;
374 int i;
371 avdtp_service_category_t category = (avdtp_service_category_t)packet[pos++];
372 uint8_t cap_len = packet[pos++];
373 if (cap_len > size - pos){
374 connection->reject_service_category = category;
375 connection->error_code = BAD_LENGTH;
376 return 0;
377 }
378
379 if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)) return 0;
380
381 int processed_cap_len = 0;
382 int rfa = 0;
383
384 while (pos < size){
385 rfa = 0;
386 processed_cap_len = pos;
375
376 uint16_t registered_service_categories = 0;
377 uint16_t to_process = size;
378
379 while (to_process >= 2){
380
381 avdtp_service_category_t category = (avdtp_service_category_t) packet[0];
382 uint8_t cap_len = packet[1];
383 packet += 2;
384 to_process -= 2;
385
386 if (cap_len > to_process){
387 connection->reject_service_category = category;
388 connection->error_code = BAD_LENGTH;
389 return 0;
390 }
391
392 if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)) return 0;
393
394 int category_valid = 1;
395
396 uint8_t * data = packet;
397 uint16_t pos = 0;
398
387 switch(category){
388 case AVDTP_RECOVERY:
399 switch(category){
400 case AVDTP_RECOVERY:
389 caps->recovery.recovery_type = packet[pos++];
390 caps->recovery.maximum_recovery_window_size = packet[pos++];
391 caps->recovery.maximum_number_media_packets = packet[pos++];
401 caps->recovery.recovery_type = data[pos++];
402 caps->recovery.maximum_recovery_window_size = data[pos++];
403 caps->recovery.maximum_number_media_packets = data[pos++];
392 break;
393 case AVDTP_CONTENT_PROTECTION:
404 break;
405 case AVDTP_CONTENT_PROTECTION:
394 caps->content_protection.cp_type = big_endian_read_16(packet, pos);
395 pos+=2;
396
406 caps->content_protection.cp_type = big_endian_read_16(data, 0);
397 caps->content_protection.cp_type_value_len = cap_len - 2;
407 caps->content_protection.cp_type_value_len = cap_len - 2;
398 pos += caps->content_protection.cp_type_value_len;
399
400 // connection->reject_service_category = category;
401 // connection->error_code = UNSUPPORTED_CONFIGURATION;
402 // support for content protection goes here
403 break;
408 // connection->reject_service_category = category;
409 // connection->error_code = UNSUPPORTED_CONFIGURATION;
410 // support for content protection goes here
411 break;
404
405 case AVDTP_HEADER_COMPRESSION:
412 case AVDTP_HEADER_COMPRESSION:
406 caps->header_compression.back_ch = packet[pos] >> 7;
407 caps->header_compression.media = packet[pos] >> 6;
408 caps->header_compression.recovery = packet[pos] >> 5;
409 pos++;
413 caps->header_compression.back_ch = (data[0] >> 7) & 1;
414 caps->header_compression.media = (data[0] >> 6) & 1;
415 caps->header_compression.recovery = (data[0] >> 5) & 1;
410 break;
411 case AVDTP_MULTIPLEXING:
416 break;
417 case AVDTP_MULTIPLEXING:
412 caps->multiplexing_mode.fragmentation = packet[pos++] >> 7;
418 caps->multiplexing_mode.fragmentation = (data[pos++] >> 7) & 1;
413 // read [tsid, tcid] for media, reporting. recovery respectively
414 caps->multiplexing_mode.transport_identifiers_num = 3;
415 for (i=0; i<caps->multiplexing_mode.transport_identifiers_num; i++){
419 // read [tsid, tcid] for media, reporting. recovery respectively
420 caps->multiplexing_mode.transport_identifiers_num = 3;
421 for (i=0; i<caps->multiplexing_mode.transport_identifiers_num; i++){
416 caps->multiplexing_mode.transport_session_identifiers[i] = packet[pos++] >> 7;
417 caps->multiplexing_mode.tcid[i] = packet[pos++] >> 7;
422 caps->multiplexing_mode.transport_session_identifiers[i] = (data[pos++] >> 7) & 1;
423 caps->multiplexing_mode.tcid[i] = (data[pos++] >> 7) & 1;
418 }
419 break;
420 case AVDTP_MEDIA_CODEC:
424 }
425 break;
426 case AVDTP_MEDIA_CODEC:
421 caps->media_codec.media_type = (avdtp_media_type_t)(packet[pos++] >> 4);
422 caps->media_codec.media_codec_type = (avdtp_media_codec_type_t)(packet[pos++]);
427 caps->media_codec.media_type = (avdtp_media_type_t)(data[pos++] >> 4);
428 caps->media_codec.media_codec_type = (avdtp_media_codec_type_t)(data[pos++]);
423 caps->media_codec.media_codec_information_len = cap_len - 2;
429 caps->media_codec.media_codec_information_len = cap_len - 2;
424 caps->media_codec.media_codec_information = &packet[pos];
425 pos += caps->media_codec.media_codec_information_len;
430 caps->media_codec.media_codec_information = &data[pos++];
426 break;
427 case AVDTP_MEDIA_TRANSPORT:
428 case AVDTP_REPORTING:
431 break;
432 case AVDTP_MEDIA_TRANSPORT:
433 case AVDTP_REPORTING:
429 case AVDTP_DELAY_REPORTING:
430 pos += cap_len;
434 case AVDTP_DELAY_REPORTING:
431 break;
432 default:
435 break;
436 default:
433 pos += cap_len;
434 rfa = 1;
437 category_valid = 0;
435 break;
436 }
438 break;
439 }
437 processed_cap_len = pos - processed_cap_len;
438 // printf("processed category %d, cap_len %d, processed_cap_len %d, rfa %d\n", category, cap_len, processed_cap_len, rfa);
439
440
440 if (cap_len == processed_cap_len){
441 // printf("pos %d, size %d \n", rfa, size - 2);
441 if (category_valid) {
442 registered_service_categories = store_bit16(registered_service_categories, category, 1);
443 }
442
444
443 if (!rfa) {
444 registered_service_categories = store_bit16(registered_service_categories, category, 1);
445 }
446 if (pos < size-2){
447 //int old_pos = pos;
448 category = (avdtp_service_category_t)packet[pos++];
449 cap_len = packet[pos++];
450 if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)){
451 log_error("avdtp_unpack_service_capabilities_has_errors");
452 return 0;
453 }
454 }
455 }
445 packet += cap_len;
446 to_process -= cap_len;
456 }
447 }
448
457 return registered_service_categories;
458}
459
460void avdtp_prepare_capabilities(avdtp_signaling_packet_t * signaling_packet, uint8_t transaction_label, uint16_t registered_service_categories, avdtp_capabilities_t capabilities, uint8_t identifier){
461 if (signaling_packet->offset) return;
462 uint8_t pack_all_capabilities = 1;
463 signaling_packet->message_type = AVDTP_RESPONSE_ACCEPT_MSG;
464 int i;

--- 174 unchanged lines hidden (view full) ---

639 event[pos++] = HCI_EVENT_AVDTP_META;
640 event[pos++] = sizeof(event) - 2;
641 event[pos++] = AVDTP_SUBEVENT_SIGNALING_SEP_DICOVERY_DONE;
642 little_endian_store_16(event, pos, avdtp_cid);
643 pos += 2;
644 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
645}
646
449 return registered_service_categories;
450}
451
452void avdtp_prepare_capabilities(avdtp_signaling_packet_t * signaling_packet, uint8_t transaction_label, uint16_t registered_service_categories, avdtp_capabilities_t capabilities, uint8_t identifier){
453 if (signaling_packet->offset) return;
454 uint8_t pack_all_capabilities = 1;
455 signaling_packet->message_type = AVDTP_RESPONSE_ACCEPT_MSG;
456 int i;

--- 174 unchanged lines hidden (view full) ---

631 event[pos++] = HCI_EVENT_AVDTP_META;
632 event[pos++] = sizeof(event) - 2;
633 event[pos++] = AVDTP_SUBEVENT_SIGNALING_SEP_DICOVERY_DONE;
634 little_endian_store_16(event, pos, avdtp_cid);
635 pos += 2;
636 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
637}
638
647
648void avdtp_signaling_emit_accept(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t local_seid, avdtp_signal_identifier_t identifier){
649 if (!callback) return;
650 uint8_t event[7];
651 int pos = 0;
652 event[pos++] = HCI_EVENT_AVDTP_META;
653 event[pos++] = sizeof(event) - 2;
654 event[pos++] = AVDTP_SUBEVENT_SIGNALING_ACCEPT;
655 little_endian_store_16(event, pos, avdtp_cid);

--- 424 unchanged lines hidden --- 		639void avdtp_signaling_emit_accept(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t local_seid, avdtp_signal_identifier_t identifier){
640 if (!callback) return;
641 uint8_t event[7];
642 int pos = 0;
643 event[pos++] = HCI_EVENT_AVDTP_META;
644 event[pos++] = sizeof(event) - 2;
645 event[pos++] = AVDTP_SUBEVENT_SIGNALING_ACCEPT;
646 little_endian_store_16(event, pos, avdtp_cid);

--- 424 unchanged lines hidden ---