Orange Pi5 kernel

/*
 * Copyright (c) 2015 South Silicon Valley Microelectronics Inc.
 * Copyright (c) 2015 iComm Corporation
 *
 * This program is free software: you can redistribute it and/or modify 
 * it under the terms of the GNU General Public License as published by 
 * the Free Software Foundation, either version 3 of the License, or 
 * (at your option) any later version.
 * This program is distributed in the hope that it will be useful, but 
 * WITHOUT ANY WARRANTY; without even the implied warranty of 
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
 * See the GNU General Public License for more details.
 * You should have received a copy of the GNU General Public License 
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include <ssv6200.h>
#include <linux/types.h>
#include <linux/nl80211.h>
#include <net/mac80211.h>
#include <linux/nl80211.h>
#include <linux/etherdevice.h>
#include <linux/delay.h>
#include <linux/version.h>
#include <linux/time.h>
#include <linux/kthread.h>
#include <net/mac80211.h>
#include <ssv6200.h>
#include "p2p.h"
#include "dev.h"
#include "lib.h"
#ifdef CONFIG_P2P_NOA
#define P2P_IE_VENDOR_TYPE 0x506f9a09
#define P2P_NOA_DETECT_INTERVAL (5 * HZ)
#ifndef MAC2STR
#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
#define COMPACT_MACSTR "%02x%02x%02x%02x%02x%02x"
#endif
void ssv6xxx_send_noa_cmd(struct ssv_softc *sc, struct ssv6xxx_p2p_noa_param *p2p_noa_param);
static inline u32 WPA_GET_BE32(const u8 *a)
{
 return (a[0] << 24) | (a[1] << 16) | (a[2] << 8) | a[3];
}
static inline u16 WPA_GET_LE16(const u8 *a)
{
 return (a[1] << 8) | a[0];
}
static inline u32 WPA_GET_LE32(const u8 *a)
{
 return (a[3] << 24) |(a[2] << 16) |(a[1] << 8) | a[0];
}
#define IEEE80211_HDRLEN 24
enum p2p_attr_id {
 P2P_ATTR_STATUS = 0,
 P2P_ATTR_MINOR_REASON_CODE = 1,
 P2P_ATTR_CAPABILITY = 2,
 P2P_ATTR_DEVICE_ID = 3,
 P2P_ATTR_GROUP_OWNER_INTENT = 4,
 P2P_ATTR_CONFIGURATION_TIMEOUT = 5,
 P2P_ATTR_LISTEN_CHANNEL = 6,
 P2P_ATTR_GROUP_BSSID = 7,
 P2P_ATTR_EXT_LISTEN_TIMING = 8,
 P2P_ATTR_INTENDED_INTERFACE_ADDR = 9,
 P2P_ATTR_MANAGEABILITY = 10,
 P2P_ATTR_CHANNEL_LIST = 11,
 P2P_ATTR_NOTICE_OF_ABSENCE = 12,
 P2P_ATTR_DEVICE_INFO = 13,
 P2P_ATTR_GROUP_INFO = 14,
 P2P_ATTR_GROUP_ID = 15,
 P2P_ATTR_INTERFACE = 16,
 P2P_ATTR_OPERATING_CHANNEL = 17,
 P2P_ATTR_INVITATION_FLAGS = 18,
 P2P_ATTR_OOB_GO_NEG_CHANNEL = 19,
 P2P_ATTR_VENDOR_SPECIFIC = 221
};
struct ssv6xxx_p2p_noa_attribute {
    u8 index;
    u16 ctwindows_oppps;
    struct ssv6xxx_p2p_noa_param noa_param;
};
extern void _ssv6xxx_hexdump(const char *title, const u8 *buf,
                             size_t len);
bool p2p_find_noa(const u8 *ies, struct ssv6xxx_p2p_noa_attribute *noa_attr)
{
    const u8 *end, *pos, *ie;
    u32 len;
 len = ie[1] - 4;
 pos = ie + 6;
 end = pos+len;
 while (pos < end) {
  u16 attr_len;
  if (pos + 2 >= end) {
   return false;
  }
  attr_len = WPA_GET_LE16(pos + 1);
  if (pos + 3 + attr_len > end) {
   return false;
  }
        if(pos[0] != P2P_ATTR_NOTICE_OF_ABSENCE){
            pos += 3 + attr_len;
            continue;
        }
        if(attr_len<15){
            printk("*********************NOA descriptor does not exist len[%d]\n", attr_len);
            break;
        }
        if(attr_len>15)
            printk("More than one NOA descriptor\n");
        noa_attr->index = pos[3];
        noa_attr->ctwindows_oppps = pos[4];
        noa_attr->noa_param.count = pos[5];
        noa_attr->noa_param.duration = WPA_GET_LE32(&pos[6]);
        noa_attr->noa_param.interval = WPA_GET_LE32(&pos[10]);
        noa_attr->noa_param.start_time = WPA_GET_LE32(&pos[14]);
        return true;
 }
 return false;
}
bool p2p_get_attribute_noa(const u8 *ies, u32 oui_type, struct ssv6xxx_p2p_noa_attribute *noa_attr)
{
 const u8 *end, *pos, *ie;
    u32 len;
 pos = ies;
 end = ies + ies_len;
 ie = NULL;
 while (pos + 1 < end) {
  if (pos + 2 + pos[1] > end)
   return false;
  if (pos[0] == WLAN_EID_VENDOR_SPECIFIC && pos[1] >= 4 &&
      WPA_GET_BE32(&pos[2]) == oui_type) {
   ie = pos;
            if(p2p_find_noa(ie, 0, noa_attr) == true)
                return true;
  }
  pos += 2 + pos[1];
 }
 return false;
}
void ssv6xxx_process_noa_event(struct ssv_softc *sc, struct sk_buff *skb)
{
    struct cfg_host_event *host_event;
    struct ssv62xx_noa_evt *noa_evt;
    host_event = (struct cfg_host_event *)skb->data;
    noa_evt= (struct ssv62xx_noa_evt *)&host_event->dat[0];
    switch(noa_evt->evt_id){
        case SSV6XXX_NOA_START:
            sc->p2p_noa.active_noa_vif |= (1<<noa_evt->vif);
            printk("SSV6XXX_NOA_START===>[%08x]\n", sc->p2p_noa.active_noa_vif);
            break;
        case SSV6XXX_NOA_STOP:
            sc->p2p_noa.active_noa_vif &= ~(1<<noa_evt->vif);
            printk("SSV6XXX_NOA_STOP===>[%08x]\n", sc->p2p_noa.active_noa_vif);
            break;
        default:
            printk("--------->NOA wrong command<---------\n");
            break;
    }
}
void ssv6xxx_noa_reset(struct ssv_softc *sc){
    unsigned long flags;
    printk("Reset NOA param...\n");
    spin_lock_irqsave(&sc->p2p_noa.p2p_config_lock, flags);
    memset(&sc->p2p_noa.noa_detect, 0, sizeof(struct ssv_p2p_noa_detect)*SSV_NUM_VIF);
    sc->p2p_noa.active_noa_vif = 0;
    sc->p2p_noa.monitor_noa_vif = 0;
    spin_unlock_irqrestore(&sc->p2p_noa.p2p_config_lock, flags);
}
void ssv6xxx_noa_host_stop_noa(struct ssv_softc *sc, u8 vif_id){
    struct ssv6xxx_p2p_noa_attribute noa_attr;
    if(sc->p2p_noa.noa_detect[vif_id].p2p_noa_index>=0){
        sc->p2p_noa.noa_detect[vif_id].p2p_noa_index = -1;
        sc->p2p_noa.active_noa_vif &= ~(1<<vif_id);
        memset(&sc->p2p_noa.noa_detect[vif_id].noa_param_cmd, 0, sizeof(struct ssv6xxx_p2p_noa_param));
        printk("->remove NOA operating vif[%d]\n", vif_id);
        noa_attr.noa_param.enable = 0;
        noa_attr.noa_param.vif_id = vif_id;
        ssv6xxx_send_noa_cmd(sc, &noa_attr.noa_param);
    }
}
void ssv6xxx_noa_detect(struct ssv_softc *sc, struct ieee80211_hdr * hdr, u32 len){
    int i;
    unsigned long flags;
    struct ieee80211_mgmt * mgmt =(struct ieee80211_mgmt *)hdr;
    struct ssv6xxx_p2p_noa_attribute noa_attr;
    spin_lock_irqsave(&sc->p2p_noa.p2p_config_lock, flags);
    if(sc->p2p_noa.monitor_noa_vif == 0)
        goto out;
    for(i=0;i<SSV_NUM_VIF;i++){
        if(sc->p2p_noa.noa_detect[i].noa_addr == NULL)
            continue;
        if(memcmp(mgmt->bssid, sc->p2p_noa.noa_detect[i].noa_addr, 6) != 0)
            continue;
        if(sc->p2p_noa.active_noa_vif &&
            ((sc->p2p_noa.active_noa_vif & 1<<i) == 0))
            continue;
  sc->p2p_noa.noa_detect[i].last_rx = jiffies;
        if(p2p_get_attribute_noa((const u8*)mgmt->u.beacon.variable,
                len - (IEEE80211_HDRLEN + sizeof(mgmt->u.beacon)),
                P2P_IE_VENDOR_TYPE,
                &noa_attr)== false){
            continue;
        }
        if(sc->p2p_noa.noa_detect[i].p2p_noa_index == noa_attr.index){
            goto out;
        }
        printk(MACSTR"->set NOA element\n", MAC2STR(mgmt->bssid));
        sc->p2p_noa.active_noa_vif |= (1<<i);
        sc->p2p_noa.noa_detect[i].p2p_noa_index = noa_attr.index;
        memcpy(&sc->p2p_noa.noa_detect[i].noa_param_cmd, &noa_attr.noa_param, sizeof(struct ssv6xxx_p2p_noa_param));
        noa_attr.noa_param.enable = 1;
        noa_attr.noa_param.vif_id = i;
        memcpy(noa_attr.noa_param.addr, hdr->addr2, 6);
        ssv6xxx_send_noa_cmd(sc, &noa_attr.noa_param);
    }
out:
    spin_unlock_irqrestore(&sc->p2p_noa.p2p_config_lock, flags);
}
void ssv6xxx_noa_hdl_bss_change(struct ssv_softc *sc, enum ssv6xxx_noa_conf conf, u8 vif_idx){
    unsigned long flags;
    if(sc->vif_info[vif_idx].vif->type != NL80211_IFTYPE_STATION ||
        sc->vif_info[vif_idx].vif->p2p != true)
        return;
    spin_lock_irqsave(&sc->p2p_noa.p2p_config_lock, flags);
    printk("====>[NOA]ssv6xxx_noa_hdl_bss_change conf[%d] vif_idx[%d]\n", conf, vif_idx);
    switch(conf)
    {
        case MONITOR_NOA_CONF_ADD:
            memset(&sc->p2p_noa.noa_detect[vif_idx], 0, sizeof(struct ssv_p2p_noa_detect));
            sc->p2p_noa.noa_detect[vif_idx].noa_addr = sc->vif_info[vif_idx].vif->bss_conf.bssid;
            sc->p2p_noa.noa_detect[vif_idx].p2p_noa_index = -1;
            sc->p2p_noa.noa_detect[vif_idx].last_rx = jiffies;
            sc->p2p_noa.monitor_noa_vif |= 1<< vif_idx;
            break;
        case MONITOR_NOA_CONF_REMOVE:
            sc->p2p_noa.monitor_noa_vif &= ~(1<< vif_idx);
            sc->p2p_noa.noa_detect[vif_idx].noa_addr = NULL;
            ssv6xxx_noa_host_stop_noa(sc, vif_idx);
            break;
        default:
            break;
    }
    spin_unlock_irqrestore(&sc->p2p_noa.p2p_config_lock, flags);
}
void ssv6xxx_send_noa_cmd(struct ssv_softc *sc, struct ssv6xxx_p2p_noa_param *p2p_noa_param)
{
    struct sk_buff *skb;
    struct cfg_host_cmd *host_cmd;
    int retry_cnt = 5;
    skb = ssv_skb_alloc(HOST_CMD_HDR_LEN + sizeof(struct ssv6xxx_p2p_noa_param));
    skb->data_len = HOST_CMD_HDR_LEN + sizeof(struct ssv6xxx_p2p_noa_param);
    skb->len = skb->data_len;
    host_cmd = (struct cfg_host_cmd *)skb->data;
    host_cmd->c_type = HOST_CMD;
    host_cmd->h_cmd = (u8)SSV6XXX_HOST_CMD_SET_NOA;
    host_cmd->len = skb->data_len;
    memcpy(host_cmd->dat32, p2p_noa_param, sizeof(struct ssv6xxx_p2p_noa_param));
    printk("Noa cmd NOA Parameter:\nEnable=%d\nInterval=%d\nDuration=%d\nStart_time=0x%08x\nCount=%d\nAddr=[%02x:%02x:%02x:%02x:%02x:%02x]vif[%d]\n\n",
                        p2p_noa_param->enable,
                        p2p_noa_param->interval,
                        p2p_noa_param->duration,
                        p2p_noa_param->start_time,
                        p2p_noa_param->count,
                        p2p_noa_param->addr[0],
                        p2p_noa_param->addr[1],
                        p2p_noa_param->addr[2],
                        p2p_noa_param->addr[3],
                        p2p_noa_param->addr[4],
                        p2p_noa_param->addr[5],
                        p2p_noa_param->vif_id);
    while((HCI_SEND_CMD(sc->sh, skb)!=0)&&(retry_cnt)){
        printk(KERN_INFO "NOA cmd retry=%d!!\n",retry_cnt);
        retry_cnt--;
    }
    ssv_skb_free(skb);
}
#if 0
void ssv6200_cmd_work(struct work_struct *work)
{
    struct ssv_softc *sc =
            container_of(work, struct ssv_softc, cmd_work);
    struct sk_buff *skb;
    bool cmd_mode;
    int retry_cnt = 5;
    do{
        if(sc->cmd_info.state == SSC_CMD_STATE_IDLE){
            struct cfg_host_cmd *host_cmd;
            skb = skb_peek(&sc->cmd_info.cmd_que);
            if(skb == NULL)
                break;
            host_cmd = (struct cfg_host_cmd *)skb->data;
            while((HCI_SEND_CMD(sc->sh, skb)!=0)&&(retry_cnt)){
                    printk(KERN_INFO "cmd retry=%d!!\n",retry_cnt);
                    retry_cnt--;
            }
            if(retry_cnt)
                sc->cmd_info.state = SSC_CMD_STATE_WAIT_RSP;
        }else{
            skb = skb_dequeue(&sc->cmd_info.evt_que);
            if(skb == NULL)
                break;
        }
    }while(1);
}
#endif
#endif