61 lines
1.5 KiB
C
61 lines
1.5 KiB
C
#include <flexptp/clock_utils.h>
|
|
#include <stdint.h>
|
|
|
|
#include <flexptp_options.h>
|
|
|
|
#include "ptp_core.h"
|
|
|
|
#define S (gPtpCoreState)
|
|
|
|
// print clock identity
|
|
void ptp_print_clock_identity(uint64_t clockID)
|
|
{
|
|
uint8_t *p = (uint8_t *) & clockID;
|
|
uint8_t i;
|
|
for (i = 0; i < 8; i++) { // reverse byte order due to Network->Host byte order conversion
|
|
MSG("%02x", p[7 - i]);
|
|
}
|
|
}
|
|
|
|
// create clock identity based on MAC address
|
|
void ptp_create_clock_identity()
|
|
{
|
|
uint8_t *p = (uint8_t *) & S.hwoptions.clockIdentity;
|
|
// construct clockIdentity
|
|
memcpy(p, netif_default->hwaddr, 3); // first 3 octets of MAC address
|
|
p[3] = 0xff;
|
|
p[4] = 0xfe;
|
|
memcpy(&p[5], &netif_default->hwaddr[3], 3); // last 3 octets of MAC address
|
|
|
|
// display ID
|
|
MSG("Own clock ID: ");
|
|
ptp_print_clock_identity(S.hwoptions.clockIdentity);
|
|
MSG("\n");
|
|
}
|
|
|
|
// convert string clock id to 64-bit number
|
|
uint64_t hextoclkid(const char *str)
|
|
{
|
|
size_t len = strlen(str);
|
|
uint64_t clkid = 0;
|
|
for (size_t i = 0; i < len; i++) {
|
|
char digit = tolower(str[i]);
|
|
if (digit >= '0' && digit <= '9') {
|
|
digit = digit - '0';
|
|
} else if (digit >= 'a' && digit <= 'f') {
|
|
digit = digit - 'a' + 10;
|
|
} else {
|
|
break;
|
|
}
|
|
|
|
clkid += (uint64_t) digit *((uint64_t) 1 << (4 * i));
|
|
}
|
|
uint8_t *p = NULL;
|
|
for (size_t i = 0; i < 8; i++) {
|
|
p = ((uint8_t *) & clkid) + i;
|
|
*p = ((*p & 0x0F) << 4) | ((*p & 0xF0) >> 4);
|
|
}
|
|
|
|
return clkid;
|
|
}
|