|
| 1 | +/************************************************************************************************************* |
| 2 | +Copyright (c) 2012-2016, Harman International Industries, Incorporated |
| 3 | +All rights reserved. |
| 4 | +
|
| 5 | +Redistribution and use in source and binary forms, with or without |
| 6 | +modification, are permitted provided that the following conditions are met: |
| 7 | +
|
| 8 | +1. Redistributions of source code must retain the above copyright notice, this |
| 9 | + list of conditions and the following disclaimer. |
| 10 | +2. Redistributions in binary form must reproduce the above copyright notice, |
| 11 | + this list of conditions and the following disclaimer in the documentation |
| 12 | + and/or other materials provided with the distribution. |
| 13 | +
|
| 14 | +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS LISTED "AS IS" AND |
| 15 | +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED |
| 16 | +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
| 17 | +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS LISTED BE LIABLE FOR |
| 18 | +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES |
| 19 | +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| 20 | +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND |
| 21 | +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
| 22 | +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
| 23 | +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
| 24 | +*************************************************************************************************************/ |
| 25 | + |
| 26 | +#include <ieee1588.hpp> |
| 27 | +#include <avbts_clock.hpp> |
| 28 | +#include <avbap_message.hpp> |
| 29 | +#include <avbts_port.hpp> |
| 30 | +#include <avbts_ostimer.hpp> |
| 31 | +#include <gptp_log.hpp> |
| 32 | + |
| 33 | +#include <stdio.h> |
| 34 | +#include <string.h> |
| 35 | +#include <math.h> |
| 36 | + |
| 37 | + |
| 38 | +#define ntohll(x) __bswap_64 (x) |
| 39 | +#define htonll(x) __bswap_64 (x) |
| 40 | + |
| 41 | + |
| 42 | +// Octet based data 2 buffer macros |
| 43 | +#define OCT_D2BMEMCP(d, s) memcpy(d, s, sizeof(s)); d += sizeof(s); |
| 44 | +#define OCT_D2BBUFCP(d, s, len) memcpy(d, s, len); d += len; |
| 45 | +#define OCT_D2BHTONB(d, s) *(U8 *)(d) = s; d += sizeof(s); |
| 46 | +#define OCT_D2BHTONS(d, s) *(U16 *)(d) = PLAT_htons(s); d += sizeof(s); |
| 47 | +#define OCT_D2BHTONL(d, s) *(U32 *)(d) = PLAT_htonl(s); d += sizeof(s); |
| 48 | + |
| 49 | +// Bit based data 2 buffer macros |
| 50 | +#define BIT_D2BHTONB(d, s, shf) *(uint8_t *)(d) |= s << shf; |
| 51 | +#define BIT_D2BHTONS(d, s, shf) *(uint16_t *)(d) |= PLAT_htons((uint16_t)(s << shf)); |
| 52 | +#define BIT_D2BHTONL(d, s, shf) *(uint32_t *)(d) |= PLAT_htonl((uint32_t)(s << shf)); |
| 53 | + |
| 54 | + |
| 55 | +APMessageTestStatus::APMessageTestStatus() |
| 56 | +{ |
| 57 | +} |
| 58 | + |
| 59 | +APMessageTestStatus::~APMessageTestStatus() |
| 60 | +{ |
| 61 | +} |
| 62 | + |
| 63 | +APMessageTestStatus::APMessageTestStatus(IEEE1588Port * port) |
| 64 | +{ |
| 65 | +} |
| 66 | + |
| 67 | +void APMessageTestStatus::sendPort(IEEE1588Port * port) |
| 68 | +{ |
| 69 | + static uint16_t sequenceId = 0; |
| 70 | + |
| 71 | + uint8_t buf_t[256]; |
| 72 | + uint8_t *buf_ptr = buf_t + port->getPayloadOffset(); |
| 73 | + uint16_t tmp16; |
| 74 | + uint32_t tmp32; |
| 75 | + uint64_t tmp64; |
| 76 | + |
| 77 | + memset(buf_t, 0, 256); |
| 78 | + |
| 79 | + // Create packet in buf |
| 80 | + messageLength = AP_TEST_STATUS_LENGTH; |
| 81 | + |
| 82 | + BIT_D2BHTONB(buf_ptr + AP_TEST_STATUS_AVTP_SUBTYPE(AP_TEST_STATUS_OFFSET), 0xfb, 0); |
| 83 | + BIT_D2BHTONB(buf_ptr + AP_TEST_STATUS_AVTP_VERSION_CONTROL(AP_TEST_STATUS_OFFSET), 0x1, 0); |
| 84 | + BIT_D2BHTONS(buf_ptr + AP_TEST_STATUS_AVTP_STATUS_LENGTH(AP_TEST_STATUS_OFFSET), 148, 0); |
| 85 | + |
| 86 | + port->getLocalAddr()->toOctetArray(buf_ptr + AP_TEST_STATUS_TARGET_ENTITY_ID(AP_TEST_STATUS_OFFSET)); |
| 87 | + |
| 88 | + tmp16 = PLAT_htons(sequenceId++); |
| 89 | + memcpy(buf_ptr + AP_TEST_STATUS_SEQUENCE_ID(AP_TEST_STATUS_OFFSET), &tmp16, sizeof(tmp16)); |
| 90 | + |
| 91 | + BIT_D2BHTONS(buf_ptr + AP_TEST_STATUS_COMMAND_TYPE(AP_TEST_STATUS_OFFSET), 1, 15); |
| 92 | + BIT_D2BHTONS(buf_ptr + AP_TEST_STATUS_COMMAND_TYPE(AP_TEST_STATUS_OFFSET), 0x29, 0); |
| 93 | + |
| 94 | + tmp16 = PLAT_htons(0x09); |
| 95 | + memcpy(buf_ptr + AP_TEST_STATUS_DESCRIPTOR_TYPE(AP_TEST_STATUS_OFFSET), &tmp16, sizeof(tmp16)); |
| 96 | + tmp16 = PLAT_htons(0x00); |
| 97 | + memcpy(buf_ptr + AP_TEST_STATUS_DESCRIPTOR_INDEX(AP_TEST_STATUS_OFFSET), &tmp16, sizeof(tmp16)); |
| 98 | + |
| 99 | + tmp32 = PLAT_htonl(0x07000023); |
| 100 | + memcpy(buf_ptr + AP_TEST_STATUS_COUNTERS_VALID(AP_TEST_STATUS_OFFSET), &tmp32, sizeof(tmp32)); |
| 101 | + |
| 102 | + tmp32 = PLAT_htonl(port->getLinkUpCount()); |
| 103 | + memcpy(buf_ptr + AP_TEST_STATUS_COUNTER_LINKUP(AP_TEST_STATUS_OFFSET), &tmp32, sizeof(tmp32)); |
| 104 | + |
| 105 | + tmp32 = PLAT_htonl(port->getLinkDownCount()); |
| 106 | + memcpy(buf_ptr + AP_TEST_STATUS_COUNTER_LINKDOWN(AP_TEST_STATUS_OFFSET), &tmp32, sizeof(tmp32)); |
| 107 | + |
| 108 | + Timestamp system_time; |
| 109 | + Timestamp device_time; |
| 110 | + uint32_t local_clock, nominal_clock_rate; |
| 111 | + port->getDeviceTime(system_time, device_time, local_clock, nominal_clock_rate); |
| 112 | + tmp64 = htonll(TIMESTAMP_TO_NS(system_time)); |
| 113 | + memcpy(buf_ptr + AP_TEST_STATUS_MESSAGE_TIMESTAMP(AP_TEST_STATUS_OFFSET), &tmp64, sizeof(tmp64)); |
| 114 | + |
| 115 | + BIT_D2BHTONB(buf_ptr + AP_TEST_STATUS_STATION_STATE(AP_TEST_STATUS_OFFSET), (uint8_t)port->getStationState(), 0); |
| 116 | + |
| 117 | + if (port->getStationState() == STATION_STATE_ETHERNET_READY) { |
| 118 | + GPTP_LOG_INFO("AVnu AP Status : STATION_STATE_ETHERNET_READY"); |
| 119 | + } |
| 120 | + else if (port->getStationState() == STATION_STATE_AVB_SYNC) { |
| 121 | + GPTP_LOG_INFO("AVnu AP Status : STATION_STATE_AVB_SYNC"); |
| 122 | + } |
| 123 | + else if (port->getStationState() == STATION_STATE_AVB_MEDIA_READY) { |
| 124 | + GPTP_LOG_INFO("AVnu AP Status : STATION_STATE_AVB_MEDIA_READY"); |
| 125 | + } |
| 126 | + |
| 127 | + port->sendGeneralPort(AVTP_ETHERTYPE, buf_t, messageLength, MCAST_TEST_STATUS, NULL); |
| 128 | + |
| 129 | + return; |
| 130 | +} |
| 131 | + |
0 commit comments