/*
* V4L2 video capture example
*/
#include "FreeRTOS.h"
#include "task.h"
#include "example_uvc.h"
#include "videodev2.h"
#include "uvcvideo.h"
#include "v4l2_driver.h"
#include "mjpeg/mjpeg.h"
#include "rtsp/rtsp_api.h"
#include "sockets.h"
#include "lwip/netif.h"
#include "uvc_intf.h"
#include "section_config.h"
SDRAM_DATA_SECTION struct rtp_object rtp_payload[VIDEO_MAX_FRAME];
struct rtp_payload_queue payload_queue;
void example_uvc(void)
{
/*init payload queue*/
INIT_LIST_HEAD(&payload_queue.wait_queue);
INIT_LIST_HEAD(&payload_queue.done_queue);
RtlMutexInit(&payload_queue.wait_mutex);
RtlMutexInit(&payload_queue.done_mutex);
RtlInitSema(&payload_queue.wait_sema, 0);
RtlInitSema(&payload_queue.done_sema, 0);
payload_queue.flush_err = 0;
uvc_stream_init();
}
void uvc_entry_handle(void *param)
{
int i, ret, cnt;
struct stream_context *stream_ctx = (struct stream_context *)param;
struct uvc_buf_context buf;
struct rtp_object *payload;
/*initialize rtp payload*/
for(i = 0; i < VIDEO_MAX_FRAME; i++)
{
if(rtp_init_payload(stream_ctx, &rtp_payload[i]) < 0)
{
for(; i>=0; i--)
{
rtp_uninit_payload(stream_ctx, &rtp_payload[i]);
}
goto exit;
}
}
if(uvc_set_param(stream_ctx, UVC_FORMAT_MJPEG, 640, 480, 30)<0)
goto exit;
if(uvc_stream_on(stream_ctx)<0)
goto exit;
/*do buffer queue & dequeue inside the loop*/
payload_queue.flush_err = 0;
while(!(payload_queue.flush_err))
{
memset(&buf, 0, sizeof(struct uvc_buf_context));
ret = uvc_dqbuf(stream_ctx, &buf);
if(buf.index < 0)
continue;//empty buffer retrieved
if((uvc_buf_check(&buf)<0)||(ret < 0)){
RTSP_ERROR("\n\rbuffer error!");
ret = -ENOENT;
goto exit;
}
rtp_payload[buf.index].index = buf.index;
if(rtp_fill_payload(stream_ctx, &rtp_payload[buf.index], buf.data, buf.len) < 0)
goto exit;
/*add rtp_payload into payload queue*/
RtlDownMutex(&payload_queue.wait_mutex);
list_add_tail(&rtp_payload[buf.index].rtp_list, &payload_queue.wait_queue);
RtlUpMutex(&payload_queue.wait_mutex);
RtlUpSema(&payload_queue.wait_sema);
//check if any rtp payload is queued in done_queue
while(RtlDownSemaWithTimeout(&payload_queue.done_sema, 5)==0)
{
if(payload_queue.flush_err)
goto exit;
}
if(!list_empty(&payload_queue.done_queue))
{
RtlDownMutex(&payload_queue.done_mutex);
payload = list_first_entry(&payload_queue.done_queue, struct rtp_object, rtp_list);
if(payload == NULL)
{
RtlUpMutex(&payload_queue.done_mutex);
continue;
}
list_del_init(&payload->rtp_list);
RtlUpMutex(&payload_queue.done_mutex);
buf.index = payload->index;
buf.data = payload->data;
buf.len = payload->len;
ret = uvc_qbuf(stream_ctx, &buf);
if (ret < 0){
RTSP_ERROR("\n\rread_frame mmap method enqueue buffer failed");
ret = -ENOENT;
goto exit;
}
}
}
exit:
uvc_stream_off(stream_ctx);
uvc_stream_free(stream_ctx);
for(i = 0; i < VIDEO_MAX_FRAME; i++)
{
rtp_uninit_payload(stream_ctx, &rtp_payload[i]);
}
//free payload_queue memory
INIT_LIST_HEAD(&payload_queue.wait_queue);
INIT_LIST_HEAD(&payload_queue.done_queue);
RtlMutexFree(&payload_queue.wait_mutex);
RtlMutexFree(&payload_queue.done_mutex);
RtlFreeSema(&payload_queue.wait_sema);
RtlFreeSema(&payload_queue.done_sema);
printf("\n\rstream free success, delete task...");
vTaskDelete(NULL);
}
int uvc_rtp_init(struct rtsp_context *rtsp_ctx);
void uvc_rtsp_handle(void *param)
{
struct stream_context *stream_ctx = (struct stream_context *)param;
struct rtsp_context *rtsp_ctx;
u8 *request_header; //buffer to hold rtsp request
struct sockaddr_in server_addr, client_addr;
int client_socket;
socklen_t client_addr_len = sizeof(struct sockaddr_in);
fd_set read_fds;
struct timeval timeout;
int ok;
rtsp_ctx = malloc(sizeof(struct rtsp_context));
if(rtsp_ctx == NULL)
{
RTSP_ERROR("\n\rrtsp context is NULL");
goto exit;
}
request_header = malloc(512);
if(request_header == NULL)
{
RTSP_ERROR("\n\rallocate request header buffer failed");
goto exit;
}
// Delay to wait for IP by DHCP
vTaskDelay(500);
/*init rtsp context to unicast udp mode*/
if(rtsp_context_init(rtsp_ctx) < 0)
goto exit;
server_addr.sin_family = AF_INET;
server_addr.sin_addr.s_addr = *(uint32_t *)(rtsp_ctx->connect_ctx.server_ip)/*htonl(INADDR_ANY)*/;
server_addr.sin_port = htons(rtsp_ctx->connect_ctx.port);
if(bind(rtsp_ctx->connect_ctx.socket_id, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {
RTSP_ERROR("\n\rCannot bind stream socket");
goto exit;
}
listen(rtsp_ctx->connect_ctx.socket_id, 1);
printf("\n\rrtsp context initialized!");
stream_ctx->protoCtx = (void *)rtsp_ctx;
rtsp_ctx->stream_ctx = (void *)stream_ctx;
/*start rtp task*/
uvc_rtp_init(rtsp_ctx);
while(stream_ctx->allowStream)
{
FD_ZERO(&read_fds);
timeout.tv_sec = 1;
timeout.tv_usec = 0;
FD_SET(rtsp_ctx->connect_ctx.socket_id, &read_fds);
if(select(1, &read_fds, NULL, NULL, &timeout))
{
client_socket = accept(rtsp_ctx->connect_ctx.socket_id,(struct sockaddr *)&client_addr, &client_addr_len);
if(client_socket < 0)
{
RTSP_ERROR("client_socket error:%d\r\n", client_socket);
close(client_socket);
continue;
}
*(rtsp_ctx->connect_ctx.remote_ip + 3) = (unsigned char) (client_addr.sin_addr.s_addr >> 24);
*(rtsp_ctx->connect_ctx.remote_ip + 2) = (unsigned char) (client_addr.sin_addr.s_addr >> 16);
*(rtsp_ctx->connect_ctx.remote_ip + 1) = (unsigned char) (client_addr.sin_addr.s_addr >> 8);
*(rtsp_ctx->connect_ctx.remote_ip) = (unsigned char) (client_addr.sin_addr.s_addr );
while(stream_ctx->allowStream)
{
read(client_socket, request_header, 512);
rtsp_readheader(request_header);
if(*request_header == 0)
{
//Do I need to send error response to client?
continue;
}
rtsp_getparam(rtsp_ctx, request_header);
switch(rtsp_ctx->rtsp_cmd)
{
case(CMD_OPTIONS):
RTSP_PRINTF("\n\rReceive options command!");
if(rtsp_ctx->state == RTSP_PLAYING)
break;
rtsp_cmd_options(rtsp_ctx);
ok = write(client_socket, rtsp_ctx->response, strlen(rtsp_ctx->response));
if (ok <= 0)
{
RTSP_ERROR("\n\rsend OPTIONS response failed!");
goto exit;
}
break;
case(CMD_DESCRIBE):
RTSP_PRINTF("\n\rReceive describe command!");
if(rtsp_ctx->state == RTSP_PLAYING)
break;
rtsp_cmd_describe(rtsp_ctx);
ok = write(client_socket, rtsp_ctx->response, strlen(rtsp_ctx->response));
if (ok <= 0)
{
RTSP_ERROR("\n\rsend DESCRIBE response failed!");
goto exit;
}
break;
case(CMD_SETUP):
RTSP_PRINTF("\n\rReceive setup command!");
if(rtsp_ctx->state == RTSP_PLAYING)
break;
//fill transport parameter
rtsp_cmd_setup(rtsp_ctx);
ok = write(client_socket, rtsp_ctx->response, strlen(rtsp_ctx->response));
if (ok <= 0)
{
RTSP_ERROR("\n\rsend SETUP response failed!");
goto exit;
}
if(rtsp_ctx->state == RTSP_INIT)
{
rtsp_ctx->state = RTSP_READY;
RTSP_PRINTF("\n\rstate changed from RTSP_INIT to RTSP_READY");
};
break;
case(CMD_TEARDOWN):
RTSP_PRINTF("\n\rReceive teardown command!");
rtsp_ctx->state = RTSP_INIT;
rtsp_cmd_teardown(rtsp_ctx);
ok = write(client_socket, rtsp_ctx->response, strlen(rtsp_ctx->response));
if (ok <= 0)
{
RTSP_ERROR("\n\rsend TEARDOWN response failed!");
goto exit;
}
RTSP_PRINTF("\n\rstreaming teardown, state changed back to RTSP_INIT");
/*have to wait until rtp server reinit*/
vTaskDelay(1000);
goto out;
break;
case(CMD_PLAY):
RTSP_PRINTF("\n\rReceive play command!");
if(rtsp_ctx->state == RTSP_PLAYING)
break;
rtsp_cmd_play(rtsp_ctx);
ok = write(client_socket, rtsp_ctx->response, strlen(rtsp_ctx->response));
if (ok <= 0)
{
RTSP_ERROR("\n\rsend PLAY response failed!");
goto exit;
}
if(rtsp_ctx->state == RTSP_READY)
{
rtsp_ctx->state = RTSP_PLAYING;
RTSP_PRINTF("\n\rstate changed from RTSP_READY to RTSP_PLAYING");
rtsp_ctx->is_rtp_start = 1;
RtlUpSema(&rtsp_ctx->start_rtp_sema);
}
break;
case(CMD_PAUSE):
RTSP_PRINTF("\n\rReceive pause command!");
rtsp_cmd_pause(rtsp_ctx);
ok = write(client_socket, rtsp_ctx->response, strlen(rtsp_ctx->response));
if (ok <= 0)
{
RTSP_ERROR("\n\rsend PAUSE response failed!");
goto exit;
}
if(rtsp_ctx->state == RTSP_PLAYING)
{
rtsp_ctx->state = RTSP_READY;
RTSP_PRINTF("\n\rstate changed from RTSP_PLAYING to RTSP_READY");
}
break;
default:
RTSP_ERROR("\n\rReceive unrecognized command!");
rtsp_cmd_error(rtsp_ctx);
ok = write(client_socket, rtsp_ctx->response, strlen(rtsp_ctx->response));
if (ok <= 0)
{
RTSP_ERROR("\n\rsend ERROR response failed!");
goto exit;
}
rtsp_ctx->state = RTSP_INIT;
}
if((rtsp_ctx->is_rtp_start == 0) && (rtsp_ctx->state == RTSP_PLAYING))
{
rtsp_ctx->state = RTSP_INIT;
RtlUpSema(&rtsp_ctx->start_rtp_sema);
}
}
out:
rtsp_ctx->state = RTSP_INIT;
close(client_socket);
}
}
exit:
if((rtsp_ctx->is_rtp_start) == 1){
RtlUpSema(&rtsp_ctx->start_rtp_sema);
}
printf("\n\rrtsp -> Available heap 0x%x\n", xPortGetFreeHeapSize());
close(client_socket);
close(rtsp_ctx->connect_ctx.socket_id);
if(request_header != NULL)
free(request_header);
/*wait until rtp task being destroyed*/
while((rtsp_ctx->is_rtp_start))
{
vTaskDelay(100);
}
rtsp_context_free(rtsp_ctx);
if(rtsp_ctx != NULL)
free(rtsp_ctx);
RTSP_ERROR("\n\rkill rtsp server thread!");
//printf("Available heap 0x%x\n", xPortGetFreeHeapSize());
//thread must be killed after server socket is terminated
vTaskDelete(NULL);
}
void uvc_rtp_udp_init(struct stream_context *stream_ctx)
{
struct rtsp_context *rtsp_ctx = (struct rtsp_context *)stream_ctx->protoCtx;
struct uvc_buf_context buf;
struct rtp_object *payload;
struct sockaddr_in rtp_addr;
int rtp_socket;
int i, ret;
socklen_t addrlen = sizeof(struct sockaddr_in);
int rtp_port;
/* varibles for recording statistic use*/
unsigned int cnt, total, total_time, time1, time2, time3;
cnt = total = total_time = time1 = time2 = time3 = 0;
/*init rtp socket*/
rtp_socket = socket(AF_INET, SOCK_DGRAM, 0);
rtp_port = rtsp_ctx->transport.serverport_min;
memset(&rtp_addr, 0, addrlen);
rtp_addr.sin_family = AF_INET;
rtp_addr.sin_addr.s_addr = *(uint32_t *)(rtsp_ctx->connect_ctx.server_ip);
rtp_addr.sin_port = htons((u16)rtp_port);
if (bind(rtp_socket,(struct sockaddr *)&rtp_addr, addrlen)<0) {
RTSP_ERROR("bind failed\r\n");
goto exit;
}
restart:
while((stream_ctx->isProcess)&&(rtsp_ctx->state == RTSP_PLAYING))
{
if(RtlDownSemaWithTimeout(&payload_queue.wait_sema, 5)==0)
continue;
time1 = time3;
time2 = xTaskGetTickCount();
/*send rtp payload*/
if(!list_empty(&payload_queue.wait_queue))
{
RtlDownMutex(&payload_queue.wait_mutex);
payload = list_first_entry(&payload_queue.wait_queue, struct rtp_object, rtp_list);
if(payload == NULL)
{
RtlUpMutex(&payload_queue.wait_mutex);
continue;
}
list_del_init(&payload->rtp_list);
RtlUpMutex(&payload_queue.wait_mutex);
if(rtsp_ctx->state == RTSP_PLAYING)
{
payload->connect_ctx.socket_id = rtp_socket;
payload->connect_ctx.port = (u16)rtsp_ctx->transport.clientport_min;
payload->connect_ctx.server_ip = rtsp_ctx->connect_ctx.server_ip;
payload->connect_ctx.remote_ip = rtsp_ctx->connect_ctx.remote_ip;
ret = rtp_udp_send(stream_ctx, payload);
}
//dequeue the this buffer from payload_queue
RtlDownMutex(&payload_queue.done_mutex);
list_add_tail(&payload->rtp_list, &payload_queue.done_queue);
RtlUpMutex(&payload_queue.done_mutex);
RtlUpSema(&payload_queue.done_sema);
time3 = xTaskGetTickCount();
cnt ++;
total += payload->len;
total_time += (time3-time1);
if(cnt == 100)
{
/* print statistics info */
/*1.average frame size(kB) T:2.time waited for next frame sending start(ms)-3.udp sending time(ms) 4.frame rate(fps)*/
printf("\n\r%dkB T:%d-%d %dfps", (total/102400), (time2 - time1), (time3 - time2), (100000/total_time));
cnt = 0;
total = 0;
total_time = 0;
}
}
}
mdelay(1000);
if(rtsp_ctx->state == RTSP_READY)
{
goto restart;
}
exit:
close(rtp_socket);
}
void uvc_rtp_tcp_init(struct stream_context *stream_ctx)
{
}
void uvc_rtp_multi_init(struct stream_context *stream_ctx)
{
}
void uvc_rtp_handle(void *param)
{
struct stream_context *stream_ctx = (struct stream_context *)param;
struct rtsp_context *rtsp_ctx = (struct rtsp_context *)stream_ctx->protoCtx;
/*go down when rtsp state change to playing*/
while(1)
{
RtlDownSema(&rtsp_ctx->start_rtp_sema);
/*check rtp cast mode*/
if(rtsp_ctx->state == RTSP_PLAYING)
{
printf("\n\rrtp start...");
switch(rtsp_ctx->transport.castMode)
{
case(UNICAST_UDP_MODE):
uvc_rtp_udp_init(stream_ctx);
break;
case(MULTICAST_MODE):
uvc_rtp_tcp_init(stream_ctx);
break;
case(UNICAST_TCP_MODE):
uvc_rtp_multi_init(stream_ctx);
break;
default:
RTSP_ERROR("\r\n unknown streaming mode! Go back to RTSP_INIT state\n");
rtsp_ctx->is_rtp_start = 0;
break;
}
}else{
break;
}
printf("\n\rrtp stop...");
}
rtsp_ctx->is_rtp_start = 0;
RTSP_ERROR("\n\rkill rtp server thread!");
//printf("Available heap 0x%x\n", xPortGetFreeHeapSize());
vTaskDelete(NULL);
}
int uvc_rtp_init(struct rtsp_context *rtsp_ctx)
{
struct stream_context *stream_ctx = (struct stream_context *)rtsp_ctx->stream_ctx;
if(xTaskCreate(uvc_rtp_handle, ((const signed char*)"uvc_rtp_handle"), 2048, (void *)stream_ctx, tskIDLE_PRIORITY + 2, NULL) != pdPASS) {
RTSP_ERROR("\r\n uvc_rtp_handle: Create Task Error\n");
return -1;
}
return 0;
}
void uvc_task_init(void * param)
{
/*entry to start uvc streaming -- dequeue uvc buffer*/
if(xTaskCreate(uvc_entry_handle, ((const signed char*)"uvc_entry_handle"), 1024, param, tskIDLE_PRIORITY + 2, NULL) != pdPASS) {
UVC_ERROR("\r\n uvc_entry_handle: Create Task Error\n");
}
/*entry to start rtsp server*/
#if UVC_RTSP_EN
if(xTaskCreate(uvc_rtsp_handle, ((const signed char*)"uvc_rtsp_handle"), 4096, param, tskIDLE_PRIORITY + 2, NULL) != pdPASS) {
RTSP_ERROR("\r\n uvc_rtsp_handle: Create Task Error\n");
}
#endif
}
/************************************************************end of rtsp/rtp with motion-jpeg************************************************/