95 lines
2.4 KiB
C
95 lines
2.4 KiB
C
#include <flexptp_options.h>
|
|
|
|
#include "FreeRTOS.h"
|
|
#include "queue.h"
|
|
|
|
#include "ptp_msg_tx.h"
|
|
#include "ptp_defs.h"
|
|
#include "ptp_core.h"
|
|
#include "ptp_raw_msg_circbuf.h"
|
|
|
|
#include "settings_interface.h"
|
|
|
|
static struct {
|
|
struct udp_pcb *pPri_Ev;
|
|
struct udp_pcb *pPri_Gen;
|
|
|
|
} sPcbLut = { 0 };
|
|
|
|
static const uint16_t sPortLut[2] = { PTP_PORT_EVENT, PTP_PORT_GENERAL };
|
|
static ip4_addr_t sIpLut[2] = { 0 };
|
|
static const uint8_t *sEthLut[2] = { PTP_ETHERNET_PRIMARY };
|
|
|
|
void ptp_transmit_init(struct udp_pcb *pPriE, struct udp_pcb *pPriG)
|
|
{
|
|
sPcbLut.pPri_Ev = pPriE;
|
|
sPcbLut.pPri_Gen = pPriG;
|
|
|
|
sIpLut[0] = PTP_IGMP_PRIMARY;
|
|
|
|
}
|
|
|
|
// release buffer
|
|
void ptp_transmit_free(struct pbuf *pPBuf)
|
|
{
|
|
pbuf_free(pPBuf);
|
|
}
|
|
|
|
void ptp_transmit_cb_handler(struct pbuf *pPBuf)
|
|
{
|
|
RawPtpMessage *pMsg = (RawPtpMessage *) pPBuf->tag;
|
|
pMsg->ts.sec = pPBuf->time_s;
|
|
pMsg->ts.nanosec = pPBuf->time_ns;
|
|
if (pMsg->pTxCb) {
|
|
pMsg->pTxCb(pMsg);
|
|
}
|
|
}
|
|
|
|
bool ptp_transmit_enqueue(const RawPtpMessage * pMsg)
|
|
{
|
|
extern PtpCircBuf gRawTxMsgBuf;
|
|
extern QueueHandle_t gTxPacketFIFO;
|
|
RawPtpMessage *pMsgAlloc = ptp_circ_buf_alloc(&gRawTxMsgBuf);
|
|
if (pMsgAlloc) {
|
|
*pMsgAlloc = *pMsg;
|
|
uint8_t idx = ptp_circ_buf_commit(&gRawTxMsgBuf);
|
|
bool hptWoken = false;
|
|
if (xPortIsInsideInterrupt()) {
|
|
xQueueSendFromISR(gTxPacketFIFO, &idx, &hptWoken);
|
|
} else {
|
|
xQueueSend(gTxPacketFIFO, &idx, portMAX_DELAY);
|
|
}
|
|
return true;
|
|
} else {
|
|
MSG("enqueue failed!");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void ptp_transmit_msg(RawPtpMessage * pMsg)
|
|
{
|
|
PtpTransportType tp = ptp_get_transport_type();
|
|
PtpDelayMechanism dm = pMsg->tx_dm;
|
|
PtpMessageClass mc = pMsg->tx_mc;
|
|
|
|
// allocate buffer
|
|
struct pbuf *txbuf = NULL;
|
|
txbuf = pbuf_alloc(PBUF_TRANSPORT, pMsg->size, PBUF_RAM);
|
|
|
|
// fill buffer
|
|
memcpy(txbuf->payload, pMsg->data, pMsg->size);
|
|
txbuf->ts_writeback_addr[0] = (uint32_t *) & (pMsg->pTs->sec);
|
|
txbuf->ts_writeback_addr[1] = (uint32_t *) & (pMsg->pTs->nanosec);
|
|
txbuf->tag = pMsg;
|
|
txbuf->tx_cb = ptp_transmit_cb_handler;
|
|
|
|
if (tp == PTP_TP_IPv4) {
|
|
struct udp_pcb *pPcb = ((struct udp_pcb **)&sPcbLut)[2 * ((int)dm) + (int)mc];
|
|
uint16_t port = sPortLut[(int)mc];
|
|
ip_addr_t ipaddr = sIpLut[(int)dm];
|
|
udp_sendto(pPcb, txbuf, &ipaddr, port);
|
|
}
|
|
|
|
pbuf_free(txbuf); // release buffer
|
|
}
|