/*
 * SPDX-License-Identifier: BSD-2-Clause
 *
 * Copyright (c) 2024 Dell Inc.
 *
 *      Alvin Chen <weike_chen@dell.com, vico.chern@qq.com>
 *      Zhichao Li <Zhichao1.Li@Dell.com>
 *      Pillar Liang <Pillar.Liang@Dellteam.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

/*
 * UVC spec:https://www.usb.org/sites/default/files/USB_Video_Class_1_5.zip
 */

#include <sys/cdefs.h>
#include <sys/param.h>
#include <sys/kernel.h>
#include <sys/systm.h>
#include <sys/sysctl.h>
#include <sys/proc.h>

#include <sys/types.h>
#include <sys/bus.h>
#include <sys/lock.h>
#include <sys/condvar.h>
#include <sys/malloc.h>
#include <sys/conf.h>
#include <sys/fcntl.h>

#include <bus/u4b/usb.h>
#define USB_DEBUG_VAR uvc_debug
#include <bus/u4b/usb_debug.h>
#include <bus/u4b/usbdi.h>

#include <contrib/v4l/videodev.h>
#include <contrib/v4l/videodev2.h>

#include "uvc_drv.h"
#include "uvc_buf.h"
#include "uvc_v4l2.h"

#define UVC_LOCK(lkp)   lockmgr(lkp, LK_EXCLUSIVE)
#define UVC_UNLOCK(lkp) lockmgr(lkp, LK_RELEASE)

#define STAILQ_FOREACH_SAFE     STAILQ_FOREACH_MUTABLE

#define UVC_CTRL_DATA_CURRENT   0
#define UVC_CTRL_DATA_BACKUP    1
#define UVC_CTRL_DATA_MIN       2
#define UVC_CTRL_DATA_MAX       3
#define UVC_CTRL_DATA_RES       4
#define UVC_CTRL_DATA_DEF       5
#define UVC_CTRL_DATA_LAST      6

#define ARRAY_SIZE(x)   (sizeof(x) / sizeof((x)[0]))

#define UVC_BITMASK           0x7
#define UVC_BITSHIFT          0x3
#define UVC_VALMASK           0x1

static unsigned int
uvc_test_bit(const uint8_t *buf, int b)
{
        return (buf[b >> UVC_BITSHIFT] >> (b & UVC_BITMASK)) & UVC_VALMASK;
}


static int
uvc_ctrl_init_sub_info(struct uvc_ctrl_info *info,
    struct uvc_ctrl_sub_info *sub_info)
{
        int ret = 0;

        enum v4l2_ctrl_type type = V4L2_CTRL_TYPE_INTEGER;
        uint8_t bit_offset = 0;
        uint8_t bit_size = 0;
        uint8_t name[32] = { 0 };
        uint32_t superior_id = 0;
        uint32_t superior_manual = 0;
        uint32_t inferior_ids[2] = { 0 };

        uint32_t id = sub_info->v4l2_id;

        if (info->topo_type == UVC_TOPO_TYPE_PROCESSING_UNIT) {
                switch (id) {
                case V4L2_CID_BRIGHTNESS:
                        ksnprintf(name, sizeof(name), "Brightness");
                        bit_size = 16;
                        break;
                case V4L2_CID_CONTRAST:
                        ksnprintf(name, sizeof(name), "Contrast");
                        bit_size = 16;
                        break;
                case V4L2_CID_HUE:
                        ksnprintf(name, sizeof(name), "Hue");
                        bit_size = 16;
                        superior_id = V4L2_CID_HUE_AUTO;
                        break;
                case V4L2_CID_SATURATION:
                        ksnprintf(name, sizeof(name), "Saturation");
                        bit_size = 16;
                        break;
                case V4L2_CID_SHARPNESS:
                        ksnprintf(name, sizeof(name), "Sharpness");
                        bit_size = 16;
                        break;
                case V4L2_CID_GAMMA:
                        ksnprintf(name, sizeof(name), "Gamma");
                        bit_size = 16;
                        break;
                case V4L2_CID_BACKLIGHT_COMPENSATION:
                        ksnprintf(name, sizeof(name), "Backlight Compensation");
                        bit_size = 16;
                        break;
                case V4L2_CID_GAIN:
                        ksnprintf(name, sizeof(name), "Gain");
                        bit_size = 16;
                        break;
                case V4L2_CID_POWER_LINE_FREQUENCY:
                        ksnprintf(name, sizeof(name), "Power Line Frequency");
                        type = V4L2_CTRL_TYPE_MENU;
                        bit_size = 2;
                        break;
                case V4L2_CID_HUE_AUTO:
                        ksnprintf(name, sizeof(name), "Hue, Auto");
                        type = V4L2_CTRL_TYPE_BOOLEAN;
                        bit_size = 1;
                        sub_info->inferior_v4l2_ids[0] = V4L2_CID_HUE;
                        break;
                case V4L2_CID_AUTO_WHITE_BALANCE:
                        if (info->selector ==
                            UVC_PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL) {
                                ksnprintf(name, sizeof(name), "White Balance Temperature, Auto");
                                type = V4L2_CTRL_TYPE_BOOLEAN;
                                bit_size = 1;
                                inferior_ids[0] =
                                    V4L2_CID_WHITE_BALANCE_TEMPERATURE;
                        } else if (info->selector ==
                            UVC_PU_WHITE_BALANCE_COMPONENT_AUTO_CONTROL) {
                                ksnprintf(name, sizeof(name), "White Balance Component, Auto");
                                type = V4L2_CTRL_TYPE_BOOLEAN;
                                bit_size = 1;
                                inferior_ids[0] = V4L2_CID_BLUE_BALANCE;
                                inferior_ids[1] = V4L2_CID_RED_BALANCE;
                        } else {
                                ret = EINVAL;
                        }
                        break;
                case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
                        ksnprintf(name, sizeof(name), "White Balance Temperature");
                        bit_size = 16;
                        superior_id = V4L2_CID_AUTO_WHITE_BALANCE;
                        break;
                case V4L2_CID_BLUE_BALANCE:
                        ksnprintf(name, sizeof(name), "White Balance Blue Component");
                        bit_size = 16;
                        superior_id = V4L2_CID_AUTO_WHITE_BALANCE;
                        break;
                case V4L2_CID_RED_BALANCE:
                        ksnprintf(name, sizeof(name), "White Balance Red Component");
                        bit_offset = 16;
                        bit_size = 16;
                        superior_id = V4L2_CID_AUTO_WHITE_BALANCE;
                        break;
                default:
                        ret = EINVAL;
                        goto err_done;
                        break;
                }
        } else if (info->topo_type == UVC_TOPO_TYPE_CAMERA_TERMINAL) {
                switch (id) {
                case V4L2_CID_EXPOSURE_AUTO:
                        ksnprintf(name, sizeof(name), "Exposure, Auto");
                        type = V4L2_CTRL_TYPE_MENU;
                        bit_size = 4;
                        inferior_ids[0] = V4L2_CID_EXPOSURE_ABSOLUTE;
                        break;
                case V4L2_CID_EXPOSURE_AUTO_PRIORITY:
                        ksnprintf(name, sizeof(name), "Exposure, Auto Priority");
                        type = V4L2_CTRL_TYPE_BOOLEAN;
                        bit_size = 1;
                        break;
                case V4L2_CID_EXPOSURE_ABSOLUTE:
                        ksnprintf(name, sizeof(name), "Exposure (Absolute)");
                        bit_size = 32;
                        superior_id = V4L2_CID_EXPOSURE_AUTO;
                        superior_manual = V4L2_EXPOSURE_MANUAL;
                        break;
                case V4L2_CID_FOCUS_ABSOLUTE:
                        ksnprintf(name, sizeof(name), "Focus (absolute)");
                        bit_size = 16;
                        superior_id = V4L2_CID_FOCUS_AUTO;
                        break;
                case V4L2_CID_FOCUS_AUTO:
                        ksnprintf(name, sizeof(name), "Focus, Auto");
                        type = V4L2_CTRL_TYPE_BOOLEAN;
                        bit_size = 1;
                        inferior_ids[0] = V4L2_CID_FOCUS_ABSOLUTE;
                        break;
                case V4L2_CID_IRIS_ABSOLUTE:
                        ksnprintf(name, sizeof(name), "Iris, Absolute");
                        bit_size = 16;
                        break;
                case V4L2_CID_IRIS_RELATIVE:
                        ksnprintf(name, sizeof(name), "Iris, Relative");
                        bit_size = 8;
                        break;
                case V4L2_CID_ZOOM_ABSOLUTE:
                        ksnprintf(name, sizeof(name), "Zoom, Absolute");
                        bit_size = 16;
                        break;
                case V4L2_CID_ZOOM_CONTINUOUS:
                        ksnprintf(name, sizeof(name), "Zoom, Continuous");
                        bit_size = 0; // should use get/set
                        break;
                case V4L2_CID_PAN_ABSOLUTE:
                        ksnprintf(name, sizeof(name), "Pan (Absolute)");
                        bit_size = 32;
                        break;
                case V4L2_CID_TILT_ABSOLUTE:
                        ksnprintf(name, sizeof(name), "Tilt (Absolute)");
                        bit_offset = 32;
                        bit_size = 32;
                        break;
                case V4L2_CID_PAN_SPEED:
                        ksnprintf(name, sizeof(name), "Pan (Speed)");
                        bit_size = 16;
                        break;
                case V4L2_CID_TILT_SPEED:
                        ksnprintf(name, sizeof(name), "Tilt (Speed)");
                        bit_offset = 16;
                        bit_size = 16;
                        break;
                case V4L2_CID_PRIVACY:
                        ksnprintf(name, sizeof(name), "Privacy");
                        type = V4L2_CTRL_TYPE_BOOLEAN;
                        bit_size = 1;
                        break;
                default:
                        ret = EINVAL;
                        goto err_done;
                        break;
                }
        }

        strcpy(sub_info->v4l2_name, name);
        sub_info->v4l2_type = type;

        sub_info->bit_offset = bit_offset;
        sub_info->bit_size = bit_size;

        sub_info->superior_v4l2_id = superior_id;
        sub_info->superior_manual = superior_manual;
        sub_info->inferior_v4l2_ids[0] = inferior_ids[0];
        sub_info->inferior_v4l2_ids[1] = inferior_ids[1];

        return 0;

err_done:
        kprintf("%s:%d Unknown topo 0x%x, selector 0x%x, v4l2 id 0x%x\n",
                         __func__, __LINE__, info->topo_type, info->selector, id);

        return ret;
}

static int
uvc_ctrl_get_info_ct(uint8_t bit_idx, struct uvc_ctrl_info *info)
{
        int ret = 0;

        memset(info, 0, sizeof(*info));

        info->topo_type = UVC_TOPO_TYPE_CAMERA_TERMINAL;
        info->index = bit_idx;

        // Table 3-6 Camera Terminal Descriptor, bitmap bmControls
        switch (bit_idx) {
        case 0: // Table 4-9
                info->selector = UVC_CT_SCANNING_MODE_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_CUR |
                    UVC_CTRL_RESTORE;
                break;
        case 1: // Table 4-10
                info->selector = UVC_CT_AE_MODE_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_CUR |
                    UVC_CTRL_GET_DEF | UVC_CTRL_GET_RES | UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_BITMASK;
                break;
        case 2: // Table 4-11
                info->selector = UVC_CT_AE_PRIORITY_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_CUR |
                    UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_BOOLEAN;
                break;
        case 3: // Table 4-12
                info->selector = UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL;
                info->byte_size = 4;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE | UVC_CTRL_AUTO_UPDATE;
                info->data_type = UVC_CTRL_DATA_UNSIGNED;
                break;
        case 4: // Table 4-13
                info->selector = UVC_CT_EXPOSURE_TIME_RELATIVE_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_RESTORE;
                break;
        case 5: // Table 4-14
                info->selector = UVC_CT_FOCUS_ABSOLUTE_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE | UVC_CTRL_AUTO_UPDATE;
                info->data_type = UVC_CTRL_DATA_UNSIGNED;
                break;
        case 6: // Table 4-15
                info->selector = UVC_CT_FOCUS_RELATIVE_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_MIN |
                    UVC_CTRL_GET_MAX | UVC_CTRL_GET_RES | UVC_CTRL_GET_DEF |
                    UVC_CTRL_AUTO_UPDATE;
                break;
        case 7: // Table 4-18
                info->selector = UVC_CT_IRIS_ABSOLUTE_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE | UVC_CTRL_AUTO_UPDATE;
                info->data_type = UVC_CTRL_DATA_UNSIGNED;
                break;
        case 8: // Table 4-19
                info->selector = UVC_CT_IRIS_RELATIVE_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_AUTO_UPDATE;
                info->data_type = UVC_CTRL_DATA_SIGNED;
                break;
        case 9: // Table 4-20
                info->selector = UVC_CT_ZOOM_ABSOLUTE_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE | UVC_CTRL_AUTO_UPDATE;
                info->data_type = UVC_CTRL_DATA_UNSIGNED;
                break;
        case 10: // Table 4-21
                info->selector = UVC_CT_ZOOM_RELATIVE_CONTROL;
                info->byte_size = 3;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_MIN |
                    UVC_CTRL_GET_MAX | UVC_CTRL_GET_RES | UVC_CTRL_GET_DEF |
                    UVC_CTRL_AUTO_UPDATE;
                info->data_type = UVC_CTRL_DATA_SIGNED;
                break;
        case 11: // Table 4-22
                info->selector = UVC_CT_PANTILT_ABSOLUTE_CONTROL;
                info->byte_size = 8;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE | UVC_CTRL_AUTO_UPDATE;
                info->data_type = UVC_CTRL_DATA_SIGNED;
                break;
        case 12: // Table 4-23
                info->selector = UVC_CT_PANTILT_RELATIVE_CONTROL;
                info->byte_size = 4;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_AUTO_UPDATE;
                info->data_type = UVC_CTRL_DATA_SIGNED;
                break;
        case 13: // Table 4-24
                info->selector = UVC_CT_ROLL_ABSOLUTE_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE | UVC_CTRL_AUTO_UPDATE;
                break;
        case 14: // Table 4-25
                info->selector = UVC_CT_ROLL_RELATIVE_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_MIN |
                    UVC_CTRL_GET_MAX | UVC_CTRL_GET_RES | UVC_CTRL_GET_DEF |
                    UVC_CTRL_AUTO_UPDATE;
                break;
        case 17: // Table 4-17
                info->selector = UVC_CT_FOCUS_AUTO_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_CUR |
                    UVC_CTRL_GET_DEF | UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_BOOLEAN;
                break;
        case 18: // Table 4-26
                info->selector = UVC_CT_PRIVACY_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_CUR |
                    UVC_CTRL_RESTORE | UVC_CTRL_AUTO_UPDATE;
                info->data_type = UVC_CTRL_DATA_BOOLEAN;
                break;
        default: // Others to be implemented.
                ret = EINVAL;
                break;
        }

        if (ret != 0) {
                memset(info, 0, sizeof(*info));
        }
        return ret;
}

static int
uvc_ctrl_get_info_pu(uint8_t bit_idx, struct uvc_ctrl_info *info)
{
        int ret = 0;

        memset(info, 0, sizeof(*info));

        info->topo_type = UVC_TOPO_TYPE_PROCESSING_UNIT;
        info->index = bit_idx;

        // Table 3-8 Processing Unit Descriptor, bitmap bmControls
        switch (bit_idx) {
        case 0: // Table 4-31
                info->selector = UVC_PU_BRIGHTNESS_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_SIGNED;
                break;
        case 1: // Table 4-32
                info->selector = UVC_PU_CONTRAST_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_UNSIGNED;
                break;
        case 2: // Table 4-36
                info->selector = UVC_PU_HUE_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE | UVC_CTRL_AUTO_UPDATE;
                info->data_type = UVC_CTRL_DATA_SIGNED;
                break;
        case 3: // Table 4-38
                info->selector = UVC_PU_SATURATION_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_UNSIGNED;
                break;
        case 4: // Table 4-39
                info->selector = UVC_PU_SHARPNESS_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_UNSIGNED;
                break;
        case 5: // Table 4-40
                info->selector = UVC_PU_GAMMA_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_UNSIGNED;
                break;
        case 6: // Table 4-41
                info->selector = UVC_PU_WHITE_BALANCE_TEMPERATURE_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE | UVC_CTRL_AUTO_UPDATE;
                info->data_type = UVC_CTRL_DATA_UNSIGNED;
                break;
        case 7: // Table 4-43
                info->selector = UVC_PU_WHITE_BALANCE_COMPONENT_CONTROL;
                info->byte_size = 4;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE | UVC_CTRL_AUTO_UPDATE;
                info->data_type = UVC_CTRL_DATA_SIGNED;

                break;
        case 8: // Table 4-30
                info->selector = UVC_PU_BACKLIGHT_COMPENSATION_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_UNSIGNED;
                break;
        case 9: // Table 4-34
                info->selector = UVC_PU_GAIN_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_UNSIGNED;
                break;
        case 10: // Table 4-35
                info->selector = UVC_PU_POWER_LINE_FREQUENCY_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_CUR |
                    UVC_CTRL_GET_DEF | UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_ENUM;
                break;
        case 11: // Table 4-37
                info->selector = UVC_PU_HUE_AUTO_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_CUR |
                    UVC_CTRL_GET_DEF | UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_BOOLEAN;
                break;
        case 12: // Table 4-42
                info->selector = UVC_PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_CUR |
                    UVC_CTRL_GET_DEF | UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_BOOLEAN;
                break;
        case 13: // Table 4-44
                info->selector = UVC_PU_WHITE_BALANCE_COMPONENT_AUTO_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_CUR |
                    UVC_CTRL_GET_DEF | UVC_CTRL_RESTORE;
                info->data_type = UVC_CTRL_DATA_BOOLEAN;
                break;
        case 14: // Table 4-45
                info->selector = UVC_PU_DIGITAL_MULTIPLIER_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE;
                break;
        case 15: // Table 4-46
                info->selector = UVC_PU_DIGITAL_MULTIPLIER_LIMIT_CONTROL;
                info->byte_size = 2;
                info->flags = UVC_CTRL_SET_CUR | UVC_CTRL_GET_RANGE |
                    UVC_CTRL_RESTORE;
                break;
        case 16: // Table 4-47
                info->selector = UVC_PU_ANALOG_VIDEO_STANDARD_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_GET_CUR;
                break;
        case 17: // Table 4-48
                info->selector = UVC_PU_ANALOG_LOCK_STATUS_CONTROL;
                info->byte_size = 1;
                info->flags = UVC_CTRL_GET_CUR;
                break;
        default:
                ret = EINVAL;
                break;
        }

        if (ret != 0) {
                memset(info, 0, sizeof(*info));
        }
        return ret;
}

static const struct uvc_ctrl_menu_item exposure_mode_menu[] = {
        { 2, "Auto Mode" },
        { 1, "Manual Mode" },
        { 4, "Shutter Priority Mode" },
        { 8, "Aperture Priority Mode" },
};

static const struct uvc_ctrl_menu_item power_line_freq_menu[] = {
        { 0, "Disabled" },
        { 1, "50 Hz" },
        { 2, "60 Hz" },
};

static int
uvc_ctrl_init_menu(struct uvc_ctrl_info *info)
{
        const struct uvc_ctrl_menu_item *src_menu_data = NULL;
        struct uvc_ctrl_menu_item *dst_menu_data = NULL;
        uint32_t item_num = 0;
        uint32_t len = 0;

        if (info->topo_type == UVC_TOPO_TYPE_CAMERA_TERMINAL &&
            info->selector == UVC_CT_AE_MODE_CONTROL) {
                src_menu_data = exposure_mode_menu;
                item_num = ARRAY_SIZE(exposure_mode_menu);

        } else if (info->topo_type == UVC_TOPO_TYPE_PROCESSING_UNIT &&
            info->selector == UVC_PU_POWER_LINE_FREQUENCY_CONTROL) {
                src_menu_data = power_line_freq_menu;
                item_num = ARRAY_SIZE(power_line_freq_menu);
        } else {
                return 0;
        }

        len = item_num * sizeof(struct uvc_ctrl_menu_item);
        dst_menu_data = kmalloc(len, M_UVC, M_ZERO | M_WAITOK);

        if (dst_menu_data == NULL) {
                kprintf("%s:%d fail to alloc mem\n", __func__, __LINE__);
                return ENOMEM;
        }

        memcpy(dst_menu_data, src_menu_data, len);

        info->menus[0].item_num = item_num;
        info->menus[0].menu_data = dst_menu_data;

        return 0;
}

static void
uvc_ctrl_init_sub_info_num(struct uvc_ctrl_info *info)
{
        info->sub_info_num = 1;

        if (info->topo_type == UVC_TOPO_TYPE_CAMERA_TERMINAL) {
                switch (info->selector) {
                case UVC_CT_PANTILT_ABSOLUTE_CONTROL:
                case UVC_CT_PANTILT_RELATIVE_CONTROL:
                        info->sub_info_num = 2;
                        break;
                }
        } else if (info->topo_type == UVC_TOPO_TYPE_PROCESSING_UNIT) {
                switch (info->selector) {
                case UVC_PU_WHITE_BALANCE_COMPONENT_CONTROL:
                        info->sub_info_num = 2;
                        break;
                }
        }
}

static int
uvc_ctrl_build_mapping_ct(struct uvc_ctrl_info *info)
{
        int ret = 0;

        switch (info->selector) {
        case UVC_CT_AE_MODE_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_EXPOSURE_AUTO;
                break;
        case UVC_CT_AE_PRIORITY_CONTROL:
                info->sub_infos[0]->v4l2_id =
                    V4L2_CID_EXPOSURE_AUTO_PRIORITY;
                break;
        case UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_EXPOSURE_ABSOLUTE;
                break;
        case UVC_CT_FOCUS_ABSOLUTE_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_FOCUS_ABSOLUTE;
                break;
        case UVC_CT_FOCUS_AUTO_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_FOCUS_AUTO;
                break;
        case UVC_CT_IRIS_ABSOLUTE_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_IRIS_ABSOLUTE;
                break;
        case UVC_CT_IRIS_RELATIVE_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_IRIS_RELATIVE;
                break;
        case UVC_CT_ZOOM_ABSOLUTE_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_ZOOM_ABSOLUTE;
                break;
        case UVC_CT_ZOOM_RELATIVE_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_ZOOM_CONTINUOUS;
                break;
        case UVC_CT_PANTILT_ABSOLUTE_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_PAN_ABSOLUTE;
                info->sub_infos[1]->v4l2_id = V4L2_CID_TILT_ABSOLUTE;
                break;
        case UVC_CT_PANTILT_RELATIVE_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_PAN_SPEED;
                info->sub_infos[1]->v4l2_id = V4L2_CID_TILT_SPEED;
                break;
        case UVC_CT_PRIVACY_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_PRIVACY;
                break;
        default:
                ret = EINVAL;
                break;
        }

        return ret;
}

static int
uvc_ctrl_build_mapping_pu(struct uvc_ctrl_info *info)
{
        int ret = 0;

        switch (info->selector) {
        case UVC_PU_BRIGHTNESS_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_BRIGHTNESS;
                break;
        case UVC_PU_CONTRAST_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_CONTRAST;
                break;
        case UVC_PU_HUE_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_HUE;
                break;
        case UVC_PU_SATURATION_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_SATURATION;
                break;
        case UVC_PU_SHARPNESS_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_SHARPNESS;
                break;
        case UVC_PU_GAMMA_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_GAMMA;
                break;
        case UVC_PU_BACKLIGHT_COMPENSATION_CONTROL:
                info->sub_infos[0]->v4l2_id =
                    V4L2_CID_BACKLIGHT_COMPENSATION;
                break;
        case UVC_PU_GAIN_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_GAIN;
                break;
        case UVC_PU_POWER_LINE_FREQUENCY_CONTROL:
                info->sub_infos[0]->v4l2_id =
                    V4L2_CID_POWER_LINE_FREQUENCY;
                break;
        case UVC_PU_HUE_AUTO_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_HUE_AUTO;
                break;
        case UVC_PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_AUTO_WHITE_BALANCE;
                break;
        case UVC_PU_WHITE_BALANCE_TEMPERATURE_CONTROL:
                info->sub_infos[0]->v4l2_id =
                    V4L2_CID_WHITE_BALANCE_TEMPERATURE;
                break;
        case UVC_PU_WHITE_BALANCE_COMPONENT_AUTO_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_AUTO_WHITE_BALANCE;
                break;
        case UVC_PU_WHITE_BALANCE_COMPONENT_CONTROL:
                info->sub_infos[0]->v4l2_id = V4L2_CID_BLUE_BALANCE;
                info->sub_infos[1]->v4l2_id = V4L2_CID_RED_BALANCE;
                break;
        default:
                ret = EINVAL;
        }

        return ret;
}

static int
uvc_ctrl_build_mapping(struct uvc_ctrl_info *info)
{
        int ret = 0;
        uint8_t i = 0;

        enum uvc_topo_type topo_type = info->topo_type;

        uint8_t sub_info_num = info->sub_info_num;
        struct uvc_ctrl_sub_info *sub_info = NULL;

        if (sub_info_num == 0) {
                return 0;
        }

        for (i = 0; i < sub_info_num; i++) {
                sub_info = kmalloc(sizeof(*sub_info), M_UVC, M_ZERO | M_WAITOK);

                if (sub_info == NULL) {
                        kprintf("%s:%d fail to alloc mem\n", __func__, __LINE__);
                        ret = ENOMEM;
                        goto err_done;
                }

                sub_info->uvc_info = info;
                info->sub_infos[i] = sub_info;
        }

        if (topo_type == UVC_TOPO_TYPE_CAMERA_TERMINAL) {
                ret = uvc_ctrl_build_mapping_ct(info);
        } else if (topo_type == UVC_TOPO_TYPE_PROCESSING_UNIT) {
                ret = uvc_ctrl_build_mapping_pu(info);
        } else {
                ret = EINVAL;
        }

        if (ret != 0) {
                goto err_done;
        }

        for (int i = 0; i < sub_info_num; i++) {
                uvc_ctrl_init_sub_info(info, info->sub_infos[i]);
        }

        return 0;

err_done:
        for (i = 0; i < MAX_MAPPING_NUM; i++) {
                if (info->sub_infos[i] != NULL) {
                        kfree(info->sub_infos[i], M_UVC);
                        info->sub_infos[i] = NULL;
                }
        }

        return ret;
}

static int
uvc_ctrl_initialize_control(struct uvc_control *ctrl)
{
        int ret = 0;

        uint8_t bit_idx = ctrl->index;
        uint16_t node_type = UVC_ENT_TYPE(ctrl->topo_node);

        struct uvc_ctrl_info tmp_info;

        switch (node_type) {
        case UVC_ITT_CAMERA:
                ret = uvc_ctrl_get_info_ct(bit_idx, &tmp_info);
                if (ret != 0) {
                        return EINVAL;
                }
                break;
        case UDESCSUB_VC_PROCESSING_UNIT:
                ret = uvc_ctrl_get_info_pu(bit_idx, &tmp_info);
                if (ret != 0) {
                        return EINVAL;
                }
                break;
        default:
                return 0;
        }

        ctrl->uvc_data = kmalloc(tmp_info.byte_size * UVC_CTRL_DATA_LAST + 1, M_UVC,
            M_ZERO | M_WAITOK);

        if (ctrl->uvc_data == NULL) {
                kprintf("%s:%d fail to allocate memory\n", __func__, __LINE__);
                ret = ENOMEM;
                goto err;
        }

        uvc_ctrl_init_sub_info_num(&tmp_info);

        ret = uvc_ctrl_build_mapping(&tmp_info);
        if (0 != ret) {
                goto err;
        }

        ret = uvc_ctrl_init_menu(&tmp_info);
        if (0 != ret) {
                goto err;
        }

        ctrl->info = tmp_info;
        ctrl->initialized = 1;

        return 0;

err:
        if (ctrl->uvc_data != NULL) {
                kfree(ctrl->uvc_data, M_UVC);
                ctrl->uvc_data = NULL;
        }

        return ret;
}

void
uvc_ctrl_destroy_mappings(struct uvc_control *ctrl)
{
        uint8_t i = 0;

        if (!ctrl)
                return;

        for (i = 0; i < MAX_MAPPING_NUM; i++) {
                if (ctrl->info.sub_infos[i] != NULL) {
                        DPRINTF("removing v4l2 mapping '%s' \n",
                                ctrl->info.sub_infos[i]->v4l2_name);

                        kfree(ctrl->info.sub_infos[i], M_UVC);
                        ctrl->info.sub_infos[i] = NULL;
                }

                if (ctrl->info.menus[i].menu_data != NULL) {
                        kfree(ctrl->info.menus[i].menu_data, M_UVC);
                        ctrl->info.menus[i].menu_data = NULL;
                }
        }

        ctrl->info.sub_info_num = 0;

        return;
}

static unsigned int
uvc_ctrl_count_control(const uint8_t *bmCtrls, uint8_t bCtrlSize)
{
        int i = 0;
        unsigned int count = 0;
        const uint8_t *data = bmCtrls;

        if (data == NULL || bCtrlSize == 0)
                return 0;

        for (i = 0; i < bCtrlSize * 8; i++) {
                if ((data[i >> UVC_BITSHIFT] >>
                     (i & UVC_BITMASK)) & UVC_VALMASK)
                        count++;
        }
        return count;
}

int
uvc_ctrl_init_dev(struct uvc_softc *sc, struct uvc_drv_ctrl *ctrls)
{
        struct uvc_topo_node *topo_node, *tmp;
        struct uvc_control *ctrl = NULL;
        uint8_t bCtrlSize = 0;
        uint32_t nctrls = 0;
        uint8_t *bmCtrls = NULL;
        uint8_t i = 0;

        struct uvc_xu_node_info *node_info_xu = NULL;
        struct uvc_pu_node_info *node_info_pu = NULL;
        struct uvc_ct_node_info *node_info_ct = NULL;

        if (!ctrls) {
                return EINVAL;
        }

        STAILQ_FOREACH_SAFE(topo_node, &ctrls->topo_nodes, link, tmp) {
                if (UVC_ENT_TYPE(topo_node) == UDESCSUB_VC_EXTENSION_UNIT) {
                        node_info_xu = (struct uvc_xu_node_info*)topo_node->node_info;

                        bmCtrls = node_info_xu->bmControls;
                        bCtrlSize = node_info_xu->bControlSize;
                } else if (UVC_ENT_TYPE(topo_node) == UDESCSUB_VC_PROCESSING_UNIT) {
                        node_info_pu = (struct uvc_pu_node_info*)topo_node->node_info;

                        bmCtrls = node_info_pu->bmControls;
                        bCtrlSize = node_info_pu->bControlSize;
                } else if (UVC_ENT_TYPE(topo_node) == UVC_ITT_CAMERA) {
                        node_info_ct = (struct uvc_ct_node_info*)topo_node->node_info;

                        bmCtrls = node_info_ct->bmControls;
                        bCtrlSize = node_info_ct->bControlSize;
                } else
                        continue;

                nctrls = uvc_ctrl_count_control(bmCtrls, bCtrlSize);
                if (nctrls == 0)
                        continue;
                topo_node->controls = kmalloc(nctrls * sizeof(*ctrl), M_UVC,
                    M_ZERO | M_WAITOK);
                if (!topo_node->controls)
                        return ENOMEM;
                topo_node->controls_num = nctrls;

                ctrl = topo_node->controls;
                for (i = 0; i < bCtrlSize * 8; i++) {
                        if (uvc_test_bit(bmCtrls, i) == 0)
                                continue;

                        ctrl->topo_node = topo_node;
                        ctrl->index = i;

                        uvc_ctrl_initialize_control(ctrl);

                        ctrl++;
                }
        }

        return 0;
}

static int
uvc_query_v4l2_ctrl_sub(struct uvc_control *ctrl, struct v4l2_queryctrl *query)
{
        int i = 0;

        uint8_t sub_info_idx = ctrl->info.cached_sub_info_idx;

        struct uvc_ctrl_menu_item *menu_items =
            ctrl->info.menus[sub_info_idx].menu_data;
        uint8_t menu_item_num = ctrl->info.menus[sub_info_idx].item_num;

        struct uvc_ctrl_sub_info *sub_info = ctrl->info.sub_infos[sub_info_idx];

        memset(query, 0, sizeof(*query));

        if (sub_info == NULL) {
                return EINVAL;
        }

        query->id = sub_info->v4l2_id;
        query->type = sub_info->v4l2_type;
        strlcpy(query->name, sub_info->v4l2_name, sizeof(query->name));

        query->flags = 0;

        switch (sub_info->v4l2_type) {
        case V4L2_CTRL_TYPE_BOOLEAN:
                query->minimum = 0;
                query->maximum = 1;
                query->step = 1;
                goto end;

        case V4L2_CTRL_TYPE_BUTTON:
                query->minimum = 0;
                query->maximum = 0;
                query->step = 0;
                goto end;

        case V4L2_CTRL_TYPE_MENU:
                query->minimum = 0;
                query->maximum = menu_item_num - 1;
                query->step = 1;

                for (i = 0; i < menu_item_num; i++) {
                        if (menu_items[i].value == query->default_value) {
                                query->default_value = i;
                                break;
                        }
                }
                goto end;

        default:
                break;
        }

        /*todo*/
        DPRINTF("WARNING __TO_BE_IMPLEMENT__ other par %s\n", __func__);
end:
        return 0;
}

static struct uvc_control *
uvc_search_control_sub(struct uvc_topo_node *topo_node, uint32_t v4l2_id)
{
        unsigned int i;
        unsigned int j;

        struct uvc_control *ctrl = NULL;

        if (topo_node == NULL) {
                return NULL;
        }

        for (i = 0; i < topo_node->controls_num; ++i) {
                ctrl = &topo_node->controls[i];

                if (!ctrl->initialized)
                        continue;

                for (j = 0; j < MAX_MAPPING_NUM; j++) {
                        if (ctrl->info.sub_infos[j] == NULL) {
                                continue;
                        }

                        if (ctrl->info.sub_infos[j]->v4l2_id == v4l2_id) {
                                ctrl->info.cached_sub_info_idx = j;
                                return ctrl;
                        }
                }
        }

        return NULL;
}

static struct uvc_control *
uvc_search_control(struct uvc_drv_ctrl *ctrls, uint32_t v4l2_id)
{
        struct uvc_control *ret_ctrl = NULL;
        struct uvc_topo_node *topo_node = NULL, *tmp = NULL;

        if (v4l2_id & V4L2_CTRL_FLAG_NEXT_CTRL) {
                DPRINTF("Not support V4L2_CTRL_FLAG_NEXT_CTRL (0x%08x )\n",
                    v4l2_id);
                return NULL;
        }

        /* Mask the query flags. */
        v4l2_id &= V4L2_CTRL_ID_MASK;

        /* Find the control. */
        STAILQ_FOREACH_SAFE(topo_node, &ctrls->topo_nodes, link, tmp) {
                ret_ctrl = uvc_search_control_sub(topo_node, v4l2_id);
                if (ret_ctrl != NULL) {
                        return ret_ctrl;
                }
        }

        DPRINTF("Control 0x%08x not found.\n", v4l2_id);
        return NULL;
}

int
uvc_query_v4l2_ctrl(struct uvc_drv_video *video, struct v4l2_queryctrl *query)
{
        struct uvc_control *ctrl;
        int ret = EINVAL;

        UVC_LOCK(&video->ctrl->mtx);

        ctrl = uvc_search_control(video->ctrl, query->id);
        if (ctrl == NULL) {
                ret = EINVAL;
                goto done;
        }

        ret = uvc_query_v4l2_ctrl_sub(ctrl, query);
done:
        UVC_UNLOCK(&video->ctrl->mtx);
        return ret;
}

int
uvc_query_v4l2_menu(struct uvc_drv_video *video,
                    struct v4l2_querymenu *qm)
{
        struct uvc_control *ctrl;
        uint8_t sub_info_idx = 0;
        int ret = 0;
        int id = qm->id;
        int index = qm->index;

        memset(qm, 0, sizeof(*qm));
        qm->id = id;
        qm->index = index;

        UVC_LOCK(&video->ctrl->mtx);

        ctrl = uvc_search_control(video->ctrl, qm->id);
        if (ctrl == NULL) {
                ret = EINVAL;
                goto done;
        }

        sub_info_idx = ctrl->info.cached_sub_info_idx;

        if (ctrl->info.menus[sub_info_idx].menu_data == NULL) {
                ret = EINVAL;
                goto done;
        }

        strlcpy(qm->name,
                ctrl->info.sub_infos[sub_info_idx]->v4l2_name,
                sizeof(qm->name));

done:
        UVC_UNLOCK(&video->ctrl->mtx);
        return ret;
}