int net_recv(void * buf, int len)
{
void * pkt;
pkt = pktbuf_alloc();
if (pkt == NULL) {
DCC_LOG(LOG_ERROR, "pktbuf_alloc() failed!");
DBG("pktbuf_alloc() failed!\n");
return -1;
}
len = rs485_pkt_receive(&net.link, &pkt, pktbuf_len);
// DBG("len=%d\n", len);
DCC_LOG1(LOG_TRACE, "%d", len);
DCC_LOG2(LOG_TRACE, "pkt=%p len=%d", pkt, len);
if (pkt != NULL) {
memcpy(buf, pkt, len);
pktbuf_free(pkt);
}
return len;
}
static ssize_t _read_pktbuf ( timeshift_file_t *tsf, int fd, pktbuf_t **pktbuf )
{
ssize_t r, cnt = 0;
size_t sz;
/* Size */
r = _read_buf(tsf, fd, &sz, sizeof(sz));
if (r < 0) return -1;
if (r != sizeof(sz)) return 0;
cnt += r;
/* Empty And Sanity Check */
if (!sz || sz > 1024 * 1024) {
*pktbuf = NULL;
return cnt;
}
/* Data */
*pktbuf = pktbuf_alloc(NULL, sz);
r = _read_buf(tsf, fd, (*pktbuf)->pb_data, sz);
if (r != sz) {
free((*pktbuf)->pb_data);
free(*pktbuf);
return r < 0 ? -1 : 0;
}
cnt += r;
return cnt;
}
static ssize_t _read_pktbuf ( int fd, pktbuf_t **pktbuf )
{
ssize_t r, cnt = 0;
size_t sz;
/* Size */
r = read(fd, &sz, sizeof(sz));
if (r < 0) return -1;
if (r != sizeof(sz)) return 0;
cnt += r;
/* Empty */
if (!sz) {
*pktbuf = NULL;
return cnt;
}
/* Data */
*pktbuf = pktbuf_alloc(NULL, sz);
r = read(fd, (*pktbuf)->pb_data, sz);
if (r != sz) {
free((*pktbuf)->pb_data);
free(*pktbuf);
return r < 0 ? -1 : 0;
}
cnt += r;
return cnt;
}
int net_send(const void * buf, int len)
{
void * pkt;
pkt = pktbuf_alloc();
if (pkt == NULL) {
DCC_LOG(LOG_ERROR, "pktbuf_alloc() failed!");
return -1;
}
DCC_LOG2(LOG_TRACE, "pkt=%p len=%d", pkt, len);
len = MIN(len, pktbuf_len);
memcpy(pkt, buf, len);
pkt = rs485_pkt_enqueue(&net.link, pkt, len);
net.stat.tx.pkt_cnt++;
net.stat.tx.octet_cnt += len;
if (pkt != NULL)
pktbuf_free(pkt);
return 0;
}
/**
* Allocate a new packet and give it a refcount of one (which caller is
* suppoed to take care of)
*/
th_pkt_t *
pkt_alloc(streaming_component_type_t type, const uint8_t *data, size_t datalen,
int64_t pts, int64_t dts, int64_t pcr)
{
th_pkt_t *pkt;
pktbuf_t *payload;
if (datalen > 0) {
payload = pktbuf_alloc(data, datalen);
if (payload == NULL)
return NULL;
} else {
payload = NULL;
}
pkt = calloc(1, sizeof(th_pkt_t));
if (pkt) {
pkt->pkt_type = type;
pkt->pkt_payload = payload;
pkt->pkt_dts = dts;
pkt->pkt_pts = pts;
pkt->pkt_pcr = pcr;
pkt->pkt_refcount = 1;
memoryinfo_alloc(&pkt_memoryinfo, sizeof(*pkt));
} else {
if (payload)
pktbuf_ref_dec(payload);
}
return pkt;
}
int net_recv(void * buf, int len)
{
void * pkt;
pkt = pktbuf_alloc();
if (pkt == NULL) {
DCC_LOG(LOG_ERROR, "pktbuf_alloc() failed!");
printf("%s(): pktbuf_alloc() failed!\n", __func__);
return -1;
}
len = rs485_pkt_receive(&link, &pkt, len);
// printf("%s(): len=%d\n", __func__, len);
DCC_LOG1(LOG_TRACE, "%d", len);
DCC_LOG2(LOG_TRACE, "pkt=%p len=%d", pkt, len);
if (pkt != NULL) {
memcpy(buf, pkt, len);
pktbuf_free(pkt);
}
return len;
}
int net_probe(void)
{
void * pkt;
int ret;
/* drain the transmmit queue */
pkt = rs485_pkt_drain(&net.link);
if (pkt != NULL)
pktbuf_free(pkt);
/* set the probe pin low */
net.probe_mode = true;
stm32_gpio_clr(RS485_MODE);
DCC_LOG(LOG_TRACE, "Probe mode.");
thinkos_flag_clr(net.probe_flag);
if ((pkt = pktbuf_alloc()) != NULL) {
uint32_t seq;
while ((seq = rand()) == 0);
/* send a probe packet */
sprintf((char *)pkt, "PRB=%08x", seq);
DCC_LOG1(LOG_TRACE, "seq=0x%08x", seq);
rs485_pkt_enqueue(&net.link, pkt, 12);
net.stat.tx.pkt_cnt++;
net.stat.tx.octet_cnt += 12;
/* wait for the end of transmission */
pkt = rs485_pkt_drain(&net.link);
pktbuf_free(pkt);
if ((ret = thinkos_flag_timedwait(net.probe_flag, 10)) == 0) {
if (seq != net.probe_seq) {
DCC_LOG(LOG_WARNING, "probe sequence mismatch!");
ret = -1;
}
} else if (ret == THINKOS_ETIMEDOUT) {
DCC_LOG(LOG_WARNING, "probe sequence timedout!");
}
DCC_LOG1(LOG_TRACE, "seq=0x%08x", seq);
} else {
DCC_LOG(LOG_ERROR, "pktbuf_alloc() failed!");
ret = -1;
}
/* set the probe pin high */
net.probe_mode = false;
stm32_gpio_set(RS485_MODE);
DCC_LOG(LOG_TRACE, "Probe mode.");
return ret;
}
/**
* Allocate a new packet and give it a refcount of one (which caller is
* suppoed to take care of)
*/
th_pkt_t *
pkt_alloc(const void *data, size_t datalen, int64_t pts, int64_t dts)
{
th_pkt_t *pkt;
pkt = calloc(1, sizeof(th_pkt_t));
if(datalen)
pkt->pkt_payload = pktbuf_alloc(data, datalen);
pkt->pkt_dts = dts;
pkt->pkt_pts = pts;
pkt->pkt_refcount = 1;
return pkt;
}
pktbuf_t *
pktbuf_append(pktbuf_t *pb, const void *data, size_t size)
{
void *ndata;
if (pb == NULL)
return pktbuf_alloc(data, size);
ndata = realloc(pb->pb_data, pb->pb_size + size);
if (ndata) {
pb->pb_data = ndata;
memcpy(ndata + pb->pb_size, data, size);
pb->pb_size += size;
memoryinfo_append(&pktbuf_memoryinfo, size);
}
return pb;
}
void __attribute__((noreturn)) net_recv_task(void)
{
void * pkt;
int len;
DCC_LOG1(LOG_TRACE, "thread=%d", thinkos_thread_self());
DBG("<%d> started...", thinkos_thread_self());
for (;;) {
pkt = pktbuf_alloc();
if (pkt == NULL) {
DCC_LOG(LOG_ERROR, "pktbuf_alloc() failed!");
DBG("pktbuf_alloc() failed!");
thinkos_sleep(1000);
continue;
}
len = rs485_pkt_receive(&net.link, &pkt, pktbuf_len);
if (len < 0) {
DBG("rs485_pkt_receive() failed!");
thinkos_sleep(1000);
continue;
}
if (len == 0) {
DBG("rs485_pkt_receive() == 0!");
thinkos_sleep(1000);
continue;
}
if (pkt != NULL) {
if (net.probe_mode)
net_probe_recv((char *)pkt, len);
else if (net.pkt_mode)
net_pkt_recv((struct net_pkt *)pkt, len);
else
net_recv((char *)pkt, len);
pktbuf_free(pkt);
}
}
}
int net_pkt_send(const void * buf, int len)
{
struct net_pkt * pkt;
unsigned int data_len;
uint8_t * dst;
uint8_t * src;
int i;
pkt = (struct net_pkt *)pktbuf_alloc();
if (pkt == NULL) {
DCC_LOG(LOG_ERROR, "pktbuf_alloc() failed!");
DBG("pktbuf_alloc() failed!\n");
net.stat.tx.err_cnt++;
return -1;
}
data_len = MIN(len, pktbuf_len - sizeof(struct net_pkt));
pkt->crc = 0;
pkt->data_len = data_len;
pkt->seq = net.tx_seq++;
dst = (uint8_t *)pkt->data;
src = (uint8_t *)buf;
for (i = 0; i < data_len; ++i)
dst[i] = src[i];
pkt->crc = crc16ccitt(0, pkt, data_len + sizeof(struct net_pkt));
pkt = rs485_pkt_enqueue(&net.link, pkt,
data_len + sizeof(struct net_pkt));
net.stat.tx.pkt_cnt++;
net.stat.tx.octet_cnt += data_len + sizeof(struct net_pkt);
if (pkt != NULL)
pktbuf_free(pkt);
return 0;
}
