318 lines
9.0 KiB
C
318 lines
9.0 KiB
C
#include "kmdw_usbh.h"
|
|
#include "kmdw_uvc.h"
|
|
#include "kmdw_memory.h"
|
|
#include "kdrv_gdma.h"
|
|
#include "kmdw_camera.h"
|
|
#include "kmdw_console.h"
|
|
#ifdef USBH_UVC_ERR
|
|
#include "kmdw_console.h"
|
|
#endif
|
|
|
|
#define UVC_MAX_FRAME 10
|
|
|
|
typedef struct _UVC_FRAME_LINK
|
|
{
|
|
uint32_t *user_frame;
|
|
uint32_t size;
|
|
uint8_t in_use; // 1: in use, 0: not in use
|
|
struct _UVC_FRAME_LINK *next;
|
|
} UVC_FRAME_LINK;
|
|
|
|
static UVC_FRAME_LINK uvc_block_list[UVC_MAX_FRAME] = {0};
|
|
static UVC_FRAME_LINK *frame_head = NULL;
|
|
static UVC_FRAME_LINK *frame_last = NULL;
|
|
#ifdef KDP_UVC
|
|
typedef struct write_frame
|
|
{
|
|
uint32_t buffer;
|
|
uint32_t size;
|
|
int index;
|
|
|
|
} write_frame_t;
|
|
|
|
static write_frame_t curr_write_frame = {0, 0, -1};
|
|
|
|
extern uint32_t ping_pong_buf_addr[2];
|
|
#endif
|
|
|
|
// GDMA handle
|
|
static kdrv_gdma_handle_t dma_handle;
|
|
|
|
__weak uint8_t USBH_UVC_Configure(uint8_t device, const USB_DEVICE_DESCRIPTOR *ptr_dev_desc, const USB_CONFIGURATION_DESCRIPTOR *ptr_cfg_desc)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
__weak usbStatus USBH_UVC_Initialize(uint8_t instance)
|
|
{
|
|
return usbUnknownError;
|
|
}
|
|
|
|
|
|
__weak usbStatus USBH_UVC_Disconnected(uint8_t instance)
|
|
{
|
|
return usbUnknownError;
|
|
}
|
|
|
|
#ifdef KDP_UVC
|
|
__weak void USBH_UVC_Get_Frame(uint32_t *frame_ptr, uint32_t *frame_size, int *index)
|
|
{
|
|
return;
|
|
}
|
|
#else
|
|
__weak void USBH_UVC_Get_Frame(uint32_t *frame_ptr, uint32_t frame_size)
|
|
{
|
|
return;
|
|
}
|
|
#endif
|
|
// callback from usbh middleware
|
|
uint8_t USBH_CustomClass_Configure(uint8_t device, const USB_DEVICE_DESCRIPTOR *ptr_dev_desc, const USB_CONFIGURATION_DESCRIPTOR *ptr_cfg_desc)
|
|
{
|
|
// can do something for UVC
|
|
return USBH_UVC_Configure(device, ptr_dev_desc, ptr_cfg_desc);
|
|
}
|
|
|
|
// callback from usbh middleware
|
|
usbStatus USBH_CustomClass_Initialize(uint8_t instance)
|
|
{
|
|
// UVC internal initialization
|
|
|
|
// can do something for UVC
|
|
return USBH_UVC_Initialize(instance);
|
|
}
|
|
|
|
usbStatus USBH_CustomClass_Disconnected(uint8_t instance)
|
|
{
|
|
// UVC internal initialization
|
|
|
|
// can do something for UVC
|
|
return USBH_UVC_Disconnected(instance);
|
|
}
|
|
|
|
usbStatus USBH_UVC_VS_Control(uint8_t device, UVC_VS_Request_t vs_req, UVC_VS_ControlSelector_t cs, UVC_PROBE_COMMIT_CONTROL *upc_ctrl)
|
|
{
|
|
USB_SETUP_PACKET setup;
|
|
|
|
setup.bmRequestType.Recipient = 0x1; // interface
|
|
setup.bmRequestType.Type = 0x1; // class
|
|
setup.bmRequestType.Dir = (vs_req == SET_CUR) ? 0 : 1;
|
|
setup.bRequest = vs_req;
|
|
setup.wValue = cs;
|
|
setup.wIndex = 1; // zero and interface
|
|
setup.wLength = 26;
|
|
|
|
return USBH_ControlTransfer(device, &setup, (uint8_t *)upc_ctrl, 26);
|
|
}
|
|
|
|
USBH_PIPE_HANDLE USBH_UVC_PipeCreate_Isoch(uint8_t device, uint8_t ep_addr, uint32_t wMaxPacketSize, uint8_t bInterval)
|
|
{
|
|
#define ITD_BUF_SIZE (4 * 1024 * 1024)
|
|
static uint8_t *buf = NULL;
|
|
if (NULL == buf)
|
|
buf = (uint8_t *)kmdw_ddr_reserve(ITD_BUF_SIZE);
|
|
return USBH_Pipe_ISOCH_PipeCreate(device, ep_addr, wMaxPacketSize, bInterval, buf, ITD_BUF_SIZE);
|
|
}
|
|
|
|
#define UVC_HEADER_SIZE 12
|
|
#define USBH_UVC_Send_Frame USBH_UVC_Get_Frame // uvc middleware send frames and uvc user get frames
|
|
|
|
#ifdef KDP_UVC
|
|
#define ty_msg(fmt, ...) info_msg(fmt, ##__VA_ARGS__)
|
|
|
|
void uvc_isoch_cb(uint32_t *payload, uint32_t length)
|
|
{
|
|
static uint32_t frame_cur_offset = 0;
|
|
static uint32_t pay_load_num =0;
|
|
static uint32_t pay_load_size =0;
|
|
if (curr_write_frame.index == -1)
|
|
USBH_UVC_Get_Frame(&curr_write_frame.buffer, &curr_write_frame.size, &curr_write_frame.index);
|
|
|
|
if ((2 >= length ) || (0 == payload ))
|
|
return;
|
|
|
|
uint32_t *frame_buf_offset = (uint32_t *)((uint32_t)(curr_write_frame.buffer) + frame_cur_offset);
|
|
uint8_t *uv_header = (uint8_t *)payload;
|
|
uint32_t *img_body = (uint32_t *)((uint32_t)payload + UVC_HEADER_SIZE);
|
|
int32_t payload_size = (length - UVC_HEADER_SIZE);
|
|
uint16_t *p = (uint16_t *)img_body;
|
|
|
|
if (payload_size > 0) {
|
|
pay_load_num++;
|
|
pay_load_size +=payload_size;
|
|
|
|
for (int i = 0; i < (payload_size >> 1) + (payload_size & 0x1); i++)
|
|
p[i] = p[i] >> 8 | p[i] << 8;
|
|
|
|
if ((frame_cur_offset + payload_size) <= ITD_BUF_SIZE)
|
|
{
|
|
kdrv_status_t dma_sts = kdrv_gdma_transfer(dma_handle, (uint32_t)frame_buf_offset, (uint32_t)img_body, payload_size);
|
|
|
|
#ifdef USBH_UVC_ERR
|
|
if (dma_sts != KDRV_STATUS_OK)
|
|
kmdw_printf("UVC: DMA failed, dst 0x%x src 0x%x size %d\n", (uint32_t)frame_buf_offset, (uint32_t)img_body, payload_size);
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
#ifdef USBH_UVC_ERR
|
|
kmdw_printf("UVC: detect frame size overrun\n");
|
|
#endif
|
|
frame_cur_offset = 0;
|
|
kdrv_status_t dma_sts = kdrv_gdma_transfer(dma_handle, (uint32_t)frame_buf_offset, (uint32_t)img_body, payload_size);
|
|
}
|
|
}
|
|
if (uv_header[1] & 0x2) // check bit D1:End of Frame, UVC SPEC 1.0 Table 2-5
|
|
{
|
|
pay_load_num = 0;
|
|
pay_load_size = 0;
|
|
|
|
// FIXME: temp solution (ping pong buffer) to remove kdrv_fb_mgr, need to test if 520 uvc device is ready
|
|
if (curr_write_frame.buffer == ping_pong_buf_addr[0])
|
|
curr_write_frame.buffer = ping_pong_buf_addr[1];
|
|
else
|
|
curr_write_frame.buffer = ping_pong_buf_addr[0];
|
|
|
|
frame_cur_offset = 0;
|
|
}
|
|
else {
|
|
frame_cur_offset += payload_size;
|
|
|
|
}
|
|
|
|
}
|
|
#else
|
|
void uvc_isoch_cb(uint32_t *payload, uint32_t length)
|
|
{
|
|
static uint32_t frame_cur_offset = 0;
|
|
|
|
if (frame_head == NULL || frame_head->user_frame == NULL)
|
|
{
|
|
#ifdef USBH_UVC_ERR
|
|
kmdw_printf("UVC: no available frame buffer to write\n");
|
|
#endif
|
|
return;
|
|
}
|
|
if ((0 == length ) || (0 == payload ))
|
|
return;
|
|
|
|
uint32_t *frame_buf_offset = (uint32_t *)((uint32_t)(frame_head->user_frame) + frame_cur_offset);
|
|
uint8_t *uv_header = (uint8_t *)payload;
|
|
uint32_t *img_body = (uint32_t *)((uint32_t)payload + UVC_HEADER_SIZE);
|
|
uint32_t payload_size = (length - UVC_HEADER_SIZE);
|
|
uint16_t *p = (uint16_t *)img_body;
|
|
|
|
for (int i = 0; i < (payload_size >> 1) + (payload_size & 0x1); i++)
|
|
p[i] = p[i] >> 8 | p[i] << 8;
|
|
|
|
if ((frame_cur_offset + payload_size) <= frame_head->size)
|
|
{
|
|
kdrv_status_t dma_sts = kdrv_gdma_transfer(dma_handle, (uint32_t)frame_buf_offset, (uint32_t)img_body, payload_size);
|
|
|
|
#ifdef USBH_UVC_ERR
|
|
if (dma_sts != KDRV_STATUS_OK)
|
|
kmdw_printf("UVC: DMA failed, dst 0x%x src 0x%x size %d\n", (uint32_t)frame_buf_offset, (uint32_t)img_body, payload_size);
|
|
#endif
|
|
}
|
|
#ifdef USBH_UVC_ERR
|
|
else
|
|
{
|
|
frame_cur_offset = 0;
|
|
kdrv_status_t dma_sts = kdrv_gdma_transfer(dma_handle, (uint32_t)frame_buf_offset, (uint32_t)img_body, payload_size);
|
|
}
|
|
#endif
|
|
if (uv_header[1] & 0x2) // check bit D1:End of Frame, UVC SPEC 1.0 Table 2-5
|
|
{
|
|
// send completed frame to user and advacne to the next frame
|
|
USBH_UVC_Send_Frame(frame_head->user_frame, frame_head->size);
|
|
|
|
frame_head->in_use = 0;
|
|
frame_head = frame_head->next;
|
|
if (frame_head == NULL)
|
|
frame_last = NULL;
|
|
|
|
frame_cur_offset = 0;
|
|
}
|
|
else {
|
|
frame_cur_offset += payload_size;
|
|
|
|
}
|
|
}
|
|
#endif
|
|
usbStatus USBH_UVC_PipeStart_Isoch(USBH_PIPE_HANDLE pipe_hndl)
|
|
{
|
|
// acquire a DMA channel here and configure it in advance
|
|
|
|
// this is to make sure GDMA initialization
|
|
kdrv_gdma_initialize();
|
|
|
|
kdrv_status_t sts = kdrv_gdma_acquire_handle(&dma_handle);
|
|
#ifdef USBH_UVC_ERR
|
|
if (sts != KDRV_STATUS_OK)
|
|
{
|
|
kmdw_printf("UVC: acquire GDMA handle failed\n");
|
|
return usbUnknownError;
|
|
}
|
|
#endif
|
|
|
|
gdma_setting_t dma_setting;
|
|
dma_setting.dst_width = GDMA_TXFER_WIDTH_32_BITS;
|
|
dma_setting.src_width = GDMA_TXFER_WIDTH_32_BITS;
|
|
dma_setting.burst_size = GDMA_BURST_SIZE_16;
|
|
dma_setting.dst_addr_ctrl = GDMA_INCREMENT_ADDRESS;
|
|
dma_setting.src_addr_ctrl = GDMA_INCREMENT_ADDRESS;
|
|
dma_setting.dma_mode = GDMA_NORMAL_MODE;
|
|
dma_setting.dma_dst_req = 0;
|
|
dma_setting.dma_src_req = 0;
|
|
|
|
kdrv_gdma_configure_setting(dma_handle, &dma_setting);
|
|
|
|
// then start ISOCH transfer
|
|
return USBH_Pipe_ISOCH_Start(pipe_hndl, uvc_isoch_cb);
|
|
}
|
|
|
|
usbStatus USBH_UVC_PipeStop_Isoch(USBH_PIPE_HANDLE pipe_hndl)
|
|
{
|
|
kdrv_gdma_release_handle(dma_handle);
|
|
return USBH_Pipe_ISOCH_Stop(pipe_hndl);
|
|
}
|
|
|
|
usbStatus USBH_UVC_Queue_Frame(USBH_PIPE_HANDLE pipe, uint32_t *frame_ptr, uint32_t size)
|
|
{
|
|
// FIXME : pipe ?
|
|
|
|
UVC_FRAME_LINK *new_frame;
|
|
|
|
// find a free one
|
|
int i;
|
|
for (i = 0; i < UVC_MAX_FRAME; i++)
|
|
if (uvc_block_list[i].in_use == 0)
|
|
{
|
|
new_frame = &uvc_block_list[i];
|
|
new_frame->in_use = 1;
|
|
break;
|
|
};
|
|
|
|
#ifdef USBH_UVC_ERR
|
|
if (i >= UVC_MAX_FRAME)
|
|
{
|
|
kmdw_printf("UVC: no available UVC block to queue\n");
|
|
}
|
|
#endif
|
|
|
|
new_frame->user_frame = frame_ptr;
|
|
new_frame->size = size; // it should match image size
|
|
new_frame->next = NULL;
|
|
|
|
if (frame_head)
|
|
{
|
|
frame_last->next = new_frame;
|
|
frame_last = new_frame;
|
|
}
|
|
else
|
|
{
|
|
frame_head = frame_last = new_frame;
|
|
}
|
|
|
|
return usbOK;
|
|
}
|