openwrt/target/linux/ramips/files/drivers/usb/host/xhci-mtk-scheduler.c

#include "xhci-mtk-scheduler.h"
#include <linux/kernel.h>       /* printk() */

static struct sch_ep **ss_out_eps[MAX_EP_NUM];
static struct sch_ep **ss_in_eps[MAX_EP_NUM];
static struct sch_ep **hs_eps[MAX_EP_NUM];	//including tt isoc
static struct sch_ep **tt_intr_eps[MAX_EP_NUM];


int mtk_xhci_scheduler_init(void){
	int i;

	for(i=0; i<MAX_EP_NUM; i++){
		ss_out_eps[i] = NULL;
	}
	for(i=0; i<MAX_EP_NUM; i++){
		ss_in_eps[i] = NULL;
	}
	for(i=0; i<MAX_EP_NUM; i++){
		hs_eps[i] = NULL;
	}
	for(i=0; i<MAX_EP_NUM; i++){
		tt_intr_eps[i] = NULL;
	}
	return 0;
}

int add_sch_ep(int dev_speed, int is_in, int isTT, int ep_type, int maxp, int interval, int burst
	, int mult, int offset, int repeat, int pkts, int cs_count, int burst_mode
	, int bw_cost, mtk_u32 *ep, struct sch_ep *tmp_ep){

	struct sch_ep **ep_array;
	int i;

	if(is_in && dev_speed == USB_SPEED_SUPER ){
		ep_array = (struct sch_ep **)ss_in_eps;
	}
	else if(dev_speed == USB_SPEED_SUPER){
		ep_array = (struct sch_ep **)ss_out_eps;
	}
	else if(dev_speed == USB_SPEED_HIGH || (isTT && ep_type == USB_EP_ISOC)){
		ep_array = (struct sch_ep **)hs_eps;
	}
	else{
		ep_array = (struct sch_ep **)tt_intr_eps;
	}
	for(i=0; i<MAX_EP_NUM; i++){
		if(ep_array[i] == NULL){
			tmp_ep->dev_speed = dev_speed;
			tmp_ep->isTT = isTT;
			tmp_ep->is_in = is_in;
			tmp_ep->ep_type = ep_type;
			tmp_ep->maxp = maxp;
			tmp_ep->interval = interval;
			tmp_ep->burst = burst;
			tmp_ep->mult = mult;
			tmp_ep->offset = offset;
			tmp_ep->repeat = repeat;
			tmp_ep->pkts = pkts;
			tmp_ep->cs_count = cs_count;
			tmp_ep->burst_mode = burst_mode;
			tmp_ep->bw_cost = bw_cost;
			tmp_ep->ep = ep;
			ep_array[i] = tmp_ep;
			return SCH_SUCCESS;
		}
	}
	return SCH_FAIL;
}

int count_ss_bw(int is_in, int ep_type, int maxp, int interval, int burst, int mult, int offset, int repeat
	, int td_size){
	int i, j, k;
	int bw_required[3];
	int final_bw_required;
	int bw_required_per_repeat;
	int tmp_bw_required;
	struct sch_ep *cur_sch_ep;
	struct sch_ep **ep_array;
	int cur_offset;
	int cur_ep_offset;
	int tmp_offset;
	int tmp_interval;
	int ep_offset;
	int ep_interval;
	int ep_repeat;
	int ep_mult;
	
	if(is_in){
		ep_array = (struct sch_ep **)ss_in_eps;
	}
	else{
		ep_array = (struct sch_ep **)ss_out_eps;
	}
	
	bw_required[0] = 0;
	bw_required[1] = 0;
	bw_required[2] = 0;
	
	if(repeat == 0){
		final_bw_required = 0;
		for(i=0; i<MAX_EP_NUM; i++){
			cur_sch_ep = ep_array[i];
			if(cur_sch_ep == NULL){
				continue;
			}
			ep_interval = cur_sch_ep->interval;
			ep_offset = cur_sch_ep->offset;
			if(cur_sch_ep->repeat == 0){
				if(ep_interval >= interval){
					tmp_offset = ep_offset + ep_interval - offset;
					tmp_interval = interval;
				}
				else{
					tmp_offset = offset + interval - ep_offset;
					tmp_interval = ep_interval;
				}
				if(tmp_offset % tmp_interval == 0){
					final_bw_required += cur_sch_ep->bw_cost;
				}
			}
			else{
				ep_repeat = cur_sch_ep->repeat;
				ep_mult = cur_sch_ep->mult;
				for(k=0; k<=ep_mult; k++){
					cur_ep_offset = ep_offset+(k*ep_mult);
					if(ep_interval >= interval){
						tmp_offset = cur_ep_offset + ep_interval - offset;
						tmp_interval = interval;
					}
					else{
						tmp_offset = offset + interval - cur_ep_offset;
						tmp_interval = ep_interval;
					}
					if(tmp_offset % tmp_interval == 0){
						final_bw_required += cur_sch_ep->bw_cost;
						break;
					}
				}
			}
		}
		final_bw_required += td_size;
	}
	else{
		bw_required_per_repeat = maxp * (burst+1);
		for(j=0; j<=mult; j++){
			tmp_bw_required = 0;
			cur_offset = offset+(j*repeat);
			for(i=0; i<MAX_EP_NUM; i++){
				cur_sch_ep = ep_array[i];
				if(cur_sch_ep == NULL){
					continue;
				}
				ep_interval = cur_sch_ep->interval;
				ep_offset = cur_sch_ep->offset;
				if(cur_sch_ep->repeat == 0){
					if(ep_interval >= interval){
						tmp_offset = ep_offset + ep_interval - cur_offset;
						tmp_interval = interval;
					}
					else{
						tmp_offset = cur_offset + interval - ep_offset;
						tmp_interval = ep_interval;
					}
					if(tmp_offset % tmp_interval == 0){
						tmp_bw_required += cur_sch_ep->bw_cost;
					}
				}
				else{
					ep_repeat = cur_sch_ep->repeat;
					ep_mult = cur_sch_ep->mult;
					for(k=0; k<=ep_mult; k++){
						cur_ep_offset = ep_offset+(k*ep_repeat);
						if(ep_interval >= interval){
							tmp_offset = cur_ep_offset + ep_interval - cur_offset;
							tmp_interval = interval;
						}
						else{
							tmp_offset = cur_offset + interval - cur_ep_offset;
							tmp_interval = ep_interval;
						}
						if(tmp_offset % tmp_interval == 0){
							tmp_bw_required += cur_sch_ep->bw_cost;
							break;
						}
					}
				}
			}
			bw_required[j] = tmp_bw_required;
		}
		final_bw_required = SS_BW_BOUND;
		for(j=0; j<=mult; j++){
			if(bw_required[j] < final_bw_required){
				final_bw_required = bw_required[j];
			}
		}
		final_bw_required += bw_required_per_repeat;
	}
	return final_bw_required;
}

int count_hs_bw(int ep_type, int maxp, int interval, int offset, int td_size){
	int i;
	int bw_required;
	struct sch_ep *cur_sch_ep;
	int tmp_offset;
	int tmp_interval;
	int ep_offset;
	int ep_interval;
	int cur_tt_isoc_interval;	//for isoc tt check
	
	bw_required = 0;
	for(i=0; i<MAX_EP_NUM; i++){
		
		cur_sch_ep = (struct sch_ep *)hs_eps[i];
		if(cur_sch_ep == NULL){
				continue;
		}
		ep_offset = cur_sch_ep->offset;
		ep_interval = cur_sch_ep->interval;
		
		if(cur_sch_ep->isTT && cur_sch_ep->ep_type == USB_EP_ISOC){
			cur_tt_isoc_interval = ep_interval<<3;
			if(ep_interval >= interval){
				tmp_offset = ep_offset + cur_tt_isoc_interval - offset;
				tmp_interval = interval;
			}
			else{
				tmp_offset = offset + interval - ep_offset;
				tmp_interval = cur_tt_isoc_interval;
			}
			if(cur_sch_ep->is_in){
				if((tmp_offset%tmp_interval >=2) && (tmp_offset%tmp_interval <= cur_sch_ep->cs_count)){
					bw_required += 188;
				}
			}
			else{
				if(tmp_offset%tmp_interval <= cur_sch_ep->cs_count){
					bw_required += 188;
				}
			}
		}
		else{
			if(ep_interval >= interval){
				tmp_offset = ep_offset + ep_interval - offset;
				tmp_interval = interval;
			}
			else{
				tmp_offset = offset + interval - ep_offset;
				tmp_interval = ep_interval;
			}
			if(tmp_offset%tmp_interval == 0){
				bw_required += cur_sch_ep->bw_cost;
			}
		}
	}
	bw_required += td_size;
	return bw_required;
}

int count_tt_isoc_bw(int is_in, int maxp, int interval, int offset, int td_size){
	char is_cs;
	int mframe_idx, frame_idx, s_frame, s_mframe, cur_mframe;
	int bw_required, max_bw;
	int ss_cs_count;
	int cs_mframe;
	int max_frame;
	int i,j;
	struct sch_ep *cur_sch_ep;
	int ep_offset;
	int ep_interval;
	int ep_cs_count;
	int tt_isoc_interval;	//for isoc tt check
	int cur_tt_isoc_interval;	//for isoc tt check
	int tmp_offset;
	int tmp_interval;
	
	is_cs = 0;
	
	tt_isoc_interval = interval<<3;	//frame to mframe
	if(is_in){
		is_cs = 1;
	}
	s_frame = offset/8;
	s_mframe = offset%8;
	ss_cs_count = (maxp + (188 - 1))/188;
	if(is_cs){
		cs_mframe = offset%8 + 2 + ss_cs_count;
		if (cs_mframe <= 6)
			ss_cs_count += 2;
		else if (cs_mframe == 7)
			ss_cs_count++;
		else if (cs_mframe > 8)
			return -1;
	}
	max_bw = 0;
	if(is_in){
		i=2;
	}
	for(cur_mframe = offset+i; i<ss_cs_count; cur_mframe++, i++){
		bw_required = 0;
		for(j=0; j<MAX_EP_NUM; j++){
			cur_sch_ep = (struct sch_ep *)hs_eps[j];
			if(cur_sch_ep == NULL){
				continue;
			}
			ep_offset = cur_sch_ep->offset;
			ep_interval = cur_sch_ep->interval;
			if(cur_sch_ep->isTT && cur_sch_ep->ep_type == USB_EP_ISOC){
				//isoc tt
				//check if mframe offset overlap
				//if overlap, add 188 to the bw
				cur_tt_isoc_interval = ep_interval<<3;
				if(cur_tt_isoc_interval >= tt_isoc_interval){
					tmp_offset = (ep_offset+cur_tt_isoc_interval)  - cur_mframe;
					tmp_interval = tt_isoc_interval;
				}
				else{
					tmp_offset = (cur_mframe+tt_isoc_interval) - ep_offset;
					tmp_interval = cur_tt_isoc_interval;
				}
				if(cur_sch_ep->is_in){
					if((tmp_offset%tmp_interval >=2) && (tmp_offset%tmp_interval <= cur_sch_ep->cs_count)){
						bw_required += 188;
					}
				}
				else{
					if(tmp_offset%tmp_interval <= cur_sch_ep->cs_count){
						bw_required += 188;
					}
				}
				
			}
			else if(cur_sch_ep->ep_type == USB_EP_INT || cur_sch_ep->ep_type == USB_EP_ISOC){
				//check if mframe
				if(ep_interval >= tt_isoc_interval){
					tmp_offset = (ep_offset+ep_interval) - cur_mframe;
					tmp_interval = tt_isoc_interval;
				}
				else{
					tmp_offset = (cur_mframe+tt_isoc_interval) - ep_offset;
					tmp_interval = ep_interval;
				}
				if(tmp_offset%tmp_interval == 0){
					bw_required += cur_sch_ep->bw_cost;
				}
			}
		}
		bw_required += 188;
		if(bw_required > max_bw){
			max_bw = bw_required;
		}
	}
	return max_bw;
}

int count_tt_intr_bw(int interval, int frame_offset){
	//check all eps in tt_intr_eps
	int ret;
	int i,j;
	int ep_offset;
	int ep_interval;
	int tmp_offset;
	int tmp_interval;
	ret = SCH_SUCCESS;
	struct sch_ep *cur_sch_ep;
	
	for(i=0; i<MAX_EP_NUM; i++){
		cur_sch_ep = (struct sch_ep *)tt_intr_eps[i];
		if(cur_sch_ep == NULL){
			continue;
		}
		ep_offset = cur_sch_ep->offset;
		ep_interval = cur_sch_ep->interval;
		if(ep_interval  >= interval){
			tmp_offset = ep_offset + ep_interval - frame_offset;
			tmp_interval = interval;
		}
		else{
			tmp_offset = frame_offset + interval - ep_offset;
			tmp_interval = ep_interval;
		}
		
		if(tmp_offset%tmp_interval==0){
			return SCH_FAIL;
		}
	}
	return SCH_SUCCESS;
}

struct sch_ep * mtk_xhci_scheduler_remove_ep(int dev_speed, int is_in, int isTT, int ep_type, mtk_u32 *ep){
	int i;
	struct sch_ep **ep_array;
	struct sch_ep *cur_ep;

	if (is_in && dev_speed == USB_SPEED_SUPER) {
		ep_array = (struct sch_ep **)ss_in_eps;
	}
	else if (dev_speed == USB_SPEED_SUPER) {
		ep_array = (struct sch_ep **)ss_out_eps;
	}
	else if (dev_speed == USB_SPEED_HIGH || (isTT && ep_type == USB_EP_ISOC)) {
		ep_array = (struct sch_ep **)hs_eps;
	}
	else {
		ep_array = (struct sch_ep **)tt_intr_eps;
	}
	for (i = 0; i < MAX_EP_NUM; i++) {
		cur_ep = (struct sch_ep *)ep_array[i];
		if(cur_ep != NULL && cur_ep->ep == ep){
			ep_array[i] = NULL;
			return cur_ep;
		}
	}
	return NULL;
}

int mtk_xhci_scheduler_add_ep(int dev_speed, int is_in, int isTT, int ep_type, int maxp, int interval, int burst
	, int mult, mtk_u32 *ep, mtk_u32 *ep_ctx, struct sch_ep *sch_ep){
	mtk_u32 bPkts = 0;
	mtk_u32 bCsCount = 0;
	mtk_u32 bBm = 1;
	mtk_u32 bOffset = 0;
	mtk_u32 bRepeat = 0;
	int ret;
	struct mtk_xhci_ep_ctx *temp_ep_ctx;
	int td_size;
	int mframe_idx, frame_idx;
	int bw_cost;
	int cur_bw, best_bw, best_bw_idx,repeat, max_repeat, best_bw_repeat;
	int cur_offset, cs_mframe;
	int break_out;
	int frame_interval;

	printk(KERN_ERR "add_ep parameters, dev_speed %d, is_in %d, isTT %d, ep_type %d, maxp %d, interval %d, burst %d, mult %d, ep 0x%x, ep_ctx 0x%x, sch_ep 0x%x\n", dev_speed, is_in, isTT, ep_type, maxp
		, interval, burst, mult, ep, ep_ctx, sch_ep);
	if(isTT && ep_type == USB_EP_INT && ((dev_speed == USB_SPEED_LOW) || (dev_speed == USB_SPEED_FULL))){
		frame_interval = interval >> 3;
		for(frame_idx=0; frame_idx<frame_interval; frame_idx++){
			printk(KERN_ERR "check tt_intr_bw interval %d, frame_idx %d\n", frame_interval, frame_idx);
			if(count_tt_intr_bw(frame_interval, frame_idx) == SCH_SUCCESS){
				printk(KERN_ERR "check OK............\n");
				bOffset = frame_idx<<3;
				bPkts = 1;
				bCsCount = 3;
				bw_cost = maxp;
				bRepeat = 0;
				if(add_sch_ep(dev_speed, is_in, isTT, ep_type, maxp, frame_interval, burst, mult
					, bOffset, bRepeat, bPkts, bCsCount, bBm, maxp, ep, sch_ep) == SCH_FAIL){
					return SCH_FAIL;
				}
				ret = SCH_SUCCESS;
				break;
			}
		}
	}
	else if(isTT && ep_type == USB_EP_ISOC){
		best_bw = HS_BW_BOUND;
		best_bw_idx = -1;
		cur_bw = 0;
		td_size = maxp;
		break_out = 0;
		frame_interval = interval>>3;
		for(frame_idx=0; frame_idx<frame_interval && !break_out; frame_idx++){
			for(mframe_idx=0; mframe_idx<8; mframe_idx++){
				cur_offset = (frame_idx*8) + mframe_idx;
				cur_bw = count_tt_isoc_bw(is_in, maxp, frame_interval, cur_offset, td_size);
				if(cur_bw > 0 && cur_bw < best_bw){
					best_bw_idx = cur_offset;
					best_bw = cur_bw;
					if(cur_bw == td_size || cur_bw < (HS_BW_BOUND>>1)){
						break_out = 1;
						break;
					}
				}
			}
		}
		if(best_bw_idx == -1){
			return SCH_FAIL;
		}
		else{
			bOffset = best_bw_idx;
			bPkts = 1;
			bCsCount = (maxp + (188 - 1)) / 188;
			if(is_in){
				cs_mframe = bOffset%8 + 2 + bCsCount;
				if (cs_mframe <= 6)
					bCsCount += 2;
				else if (cs_mframe == 7)
					bCsCount++;
			}
			bw_cost = 188;
			bRepeat = 0;
			if(add_sch_ep( dev_speed, is_in, isTT, ep_type, maxp, interval, burst, mult
				, bOffset, bRepeat, bPkts, bCsCount, bBm, bw_cost, ep, sch_ep) == SCH_FAIL){
				return SCH_FAIL;
			}
			ret = SCH_SUCCESS;
		}
	}
	else if((dev_speed == USB_SPEED_FULL || dev_speed == USB_SPEED_LOW) && ep_type == USB_EP_INT){
		bPkts = 1;
		ret = SCH_SUCCESS;
	}
	else if(dev_speed == USB_SPEED_FULL && ep_type == USB_EP_ISOC){
		bPkts = 1;
		ret = SCH_SUCCESS;
	}
	else if(dev_speed == USB_SPEED_HIGH && (ep_type == USB_EP_INT || ep_type == USB_EP_ISOC)){
		best_bw = HS_BW_BOUND;
		best_bw_idx = -1;
		cur_bw = 0;
		td_size = maxp*(burst+1);
		for(cur_offset = 0; cur_offset<interval; cur_offset++){
			cur_bw = count_hs_bw(ep_type, maxp, interval, cur_offset, td_size);
			if(cur_bw > 0 && cur_bw < best_bw){
				best_bw_idx = cur_offset;
				best_bw = cur_bw;
				if(cur_bw == td_size || cur_bw < (HS_BW_BOUND>>1)){
					break;
				}
			}
		}
		if(best_bw_idx == -1){
			return SCH_FAIL;
		}
		else{
			bOffset = best_bw_idx;
			bPkts = burst + 1;
			bCsCount = 0;
			bw_cost = td_size;
			bRepeat = 0;
			if(add_sch_ep(dev_speed, is_in, isTT, ep_type, maxp, interval, burst, mult
				, bOffset, bRepeat, bPkts, bCsCount, bBm, bw_cost, ep, sch_ep) == SCH_FAIL){
				return SCH_FAIL;
			}
			ret = SCH_SUCCESS;
		}
	}
	else if(dev_speed == USB_SPEED_SUPER && (ep_type == USB_EP_INT || ep_type == USB_EP_ISOC)){
		best_bw = SS_BW_BOUND;
		best_bw_idx = -1;
		cur_bw = 0;
		td_size = maxp * (mult+1) * (burst+1);
		if(mult == 0){
			max_repeat = 0;
		}
		else{
			max_repeat = (interval-1)/(mult+1);
		}
		break_out = 0;
		for(frame_idx = 0; (frame_idx < interval) && !break_out; frame_idx++){
			for(repeat = max_repeat; repeat >= 0; repeat--){
				cur_bw = count_ss_bw(is_in, ep_type, maxp, interval, burst, mult, frame_idx
					, repeat, td_size);
				printk(KERN_ERR "count_ss_bw, frame_idx %d, repeat %d, td_size %d, result bw %d\n"
					, frame_idx, repeat, td_size, cur_bw);
				if(cur_bw > 0 && cur_bw < best_bw){
					best_bw_idx = frame_idx;
					best_bw_repeat = repeat;
					best_bw = cur_bw;
					if(cur_bw <= td_size || cur_bw < (HS_BW_BOUND>>1)){
						break_out = 1;
						break;
					}
				}
			}
		}
		printk(KERN_ERR "final best idx %d, best repeat %d\n", best_bw_idx, best_bw_repeat);
		if(best_bw_idx == -1){
			return SCH_FAIL;
		}
		else{
			bOffset = best_bw_idx;
			bCsCount = 0;
			bRepeat = best_bw_repeat;
			if(bRepeat == 0){
				bw_cost = (burst+1)*(mult+1)*maxp;
				bPkts = (burst+1)*(mult+1);
			}
			else{
				bw_cost = (burst+1)*maxp;
				bPkts = (burst+1);
			}
			if(add_sch_ep(dev_speed, is_in, isTT, ep_type, maxp, interval, burst, mult
				, bOffset, bRepeat, bPkts, bCsCount, bBm, bw_cost, ep, sch_ep) == SCH_FAIL){
				return SCH_FAIL;
			}
			ret = SCH_SUCCESS;
		}
	}
	else{
		bPkts = 1;
		ret = SCH_SUCCESS;
	}
	if(ret == SCH_SUCCESS){
		temp_ep_ctx = (struct mtk_xhci_ep_ctx *)ep_ctx;
		temp_ep_ctx->reserved[0] |= (BPKTS(bPkts) | BCSCOUNT(bCsCount) | BBM(bBm));
		temp_ep_ctx->reserved[1] |= (BOFFSET(bOffset) | BREPEAT(bRepeat));

		printk(KERN_DEBUG "[DBG] BPKTS: %x, BCSCOUNT: %x, BBM: %x\n", bPkts, bCsCount, bBm);
		printk(KERN_DEBUG "[DBG] BOFFSET: %x, BREPEAT: %x\n", bOffset, bRepeat);
		return SCH_SUCCESS;
	}
	else{
		return SCH_FAIL;
	}
}