1 // SPDX-License-Identifier: GPL-2.0
2 #include <linux/module.h>
3 #include <linux/kernel.h>
4 #include <linux/errno.h>
5 #include <linux/string.h>
6 #include <linux/mm.h>
7 #include <linux/slab.h>
8 #include <linux/delay.h>
9 #include <linux/fb.h>
10 #include <linux/ioport.h>
11 #include <linux/init.h>
12 #include <linux/pci.h>
13 #include <linux/vmalloc.h>
14 #include <linux/pagemap.h>
15 #include <linux/console.h>
16 #ifdef CONFIG_MTRR
17 #include <asm/mtrr.h>
18 #endif
19 #include <linux/platform_device.h>
20 #include <linux/sizes.h>
21 
22 #include "sm750.h"
23 #include "ddk750.h"
24 #include "sm750_accel.h"
25 
26 void __iomem *mmio750;
27 
hw_sm750_map(struct sm750_dev * sm750_dev,struct pci_dev * pdev)28 int hw_sm750_map(struct sm750_dev *sm750_dev, struct pci_dev *pdev)
29 {
30 	int ret;
31 
32 	ret = 0;
33 
34 	sm750_dev->vidreg_start = pci_resource_start(pdev, 1);
35 	sm750_dev->vidreg_size = SZ_2M;
36 
37 	pr_info("mmio phyAddr = %lx\n", sm750_dev->vidreg_start);
38 
39 	/*
40 	 * reserve the vidreg space of smi adaptor
41 	 * if you do this, you need to add release region code
42 	 * in lynxfb_remove, or memory will not be mapped again
43 	 * successfully
44 	 */
45 	ret = pci_request_region(pdev, 1, "sm750fb");
46 	if (ret) {
47 		pr_err("Can not request PCI regions.\n");
48 		goto exit;
49 	}
50 
51 	/* now map mmio and vidmem */
52 	sm750_dev->pvReg =
53 		ioremap(sm750_dev->vidreg_start, sm750_dev->vidreg_size);
54 	if (!sm750_dev->pvReg) {
55 		pr_err("mmio failed\n");
56 		ret = -EFAULT;
57 		goto exit;
58 	} else {
59 		pr_info("mmio virtual addr = %p\n", sm750_dev->pvReg);
60 	}
61 
62 	sm750_dev->accel.dprBase = sm750_dev->pvReg + DE_BASE_ADDR_TYPE1;
63 	sm750_dev->accel.dpPortBase = sm750_dev->pvReg + DE_PORT_ADDR_TYPE1;
64 
65 	mmio750 = sm750_dev->pvReg;
66 	sm750_set_chip_type(sm750_dev->devid, sm750_dev->revid);
67 
68 	sm750_dev->vidmem_start = pci_resource_start(pdev, 0);
69 	/*
70 	 * don't use pdev_resource[x].end - resource[x].start to
71 	 * calculate the resource size, it's only the maximum available
72 	 * size but not the actual size, using
73 	 * @ddk750_get_vm_size function can be safe.
74 	 */
75 	sm750_dev->vidmem_size = ddk750_get_vm_size();
76 	pr_info("video memory phyAddr = %lx, size = %u bytes\n",
77 		sm750_dev->vidmem_start, sm750_dev->vidmem_size);
78 
79 	/* reserve the vidmem space of smi adaptor */
80 	sm750_dev->pvMem =
81 		ioremap_wc(sm750_dev->vidmem_start, sm750_dev->vidmem_size);
82 	if (!sm750_dev->pvMem) {
83 		iounmap(sm750_dev->pvReg);
84 		pr_err("Map video memory failed\n");
85 		ret = -EFAULT;
86 		goto exit;
87 	} else {
88 		pr_info("video memory vaddr = %p\n", sm750_dev->pvMem);
89 	}
90 exit:
91 	return ret;
92 }
93 
hw_sm750_inithw(struct sm750_dev * sm750_dev,struct pci_dev * pdev)94 int hw_sm750_inithw(struct sm750_dev *sm750_dev, struct pci_dev *pdev)
95 {
96 	struct init_status *parm;
97 
98 	parm = &sm750_dev->initParm;
99 	if (parm->chip_clk == 0)
100 		parm->chip_clk = (sm750_get_chip_type() == SM750LE) ?
101 					       DEFAULT_SM750LE_CHIP_CLOCK :
102 					       DEFAULT_SM750_CHIP_CLOCK;
103 
104 	if (parm->mem_clk == 0)
105 		parm->mem_clk = parm->chip_clk;
106 	if (parm->master_clk == 0)
107 		parm->master_clk = parm->chip_clk / 3;
108 
109 	ddk750_init_hw((struct initchip_param *)&sm750_dev->initParm);
110 	/* for sm718, open pci burst */
111 	if (sm750_dev->devid == 0x718) {
112 		poke32(SYSTEM_CTRL,
113 		       peek32(SYSTEM_CTRL) | SYSTEM_CTRL_PCI_BURST);
114 	}
115 
116 	if (sm750_get_chip_type() != SM750LE) {
117 		unsigned int val;
118 		/* does user need CRT? */
119 		if (sm750_dev->nocrt) {
120 			poke32(MISC_CTRL,
121 			       peek32(MISC_CTRL) | MISC_CTRL_DAC_POWER_OFF);
122 			/* shut off dpms */
123 			val = peek32(SYSTEM_CTRL) & ~SYSTEM_CTRL_DPMS_MASK;
124 			val |= SYSTEM_CTRL_DPMS_VPHN;
125 			poke32(SYSTEM_CTRL, val);
126 		} else {
127 			poke32(MISC_CTRL,
128 			       peek32(MISC_CTRL) & ~MISC_CTRL_DAC_POWER_OFF);
129 			/* turn on dpms */
130 			val = peek32(SYSTEM_CTRL) & ~SYSTEM_CTRL_DPMS_MASK;
131 			val |= SYSTEM_CTRL_DPMS_VPHP;
132 			poke32(SYSTEM_CTRL, val);
133 		}
134 
135 		val = peek32(PANEL_DISPLAY_CTRL) &
136 		      ~(PANEL_DISPLAY_CTRL_DUAL_DISPLAY |
137 			PANEL_DISPLAY_CTRL_DOUBLE_PIXEL);
138 		switch (sm750_dev->pnltype) {
139 		case sm750_24TFT:
140 			break;
141 		case sm750_doubleTFT:
142 			val |= PANEL_DISPLAY_CTRL_DOUBLE_PIXEL;
143 			break;
144 		case sm750_dualTFT:
145 			val |= PANEL_DISPLAY_CTRL_DUAL_DISPLAY;
146 			break;
147 		}
148 		poke32(PANEL_DISPLAY_CTRL, val);
149 	} else {
150 		/*
151 		 * for 750LE, no DVI chip initialization
152 		 * makes Monitor no signal
153 		 *
154 		 * Set up GPIO for software I2C to program DVI chip in the
155 		 * Xilinx SP605 board, in order to have video signal.
156 		 */
157 		sm750_sw_i2c_init(0, 1);
158 
159 		/*
160 		 * Customer may NOT use CH7301 DVI chip, which has to be
161 		 * initialized differently.
162 		 */
163 		if (sm750_sw_i2c_read_reg(0xec, 0x4a) == 0x95) {
164 			/*
165 			 * The following register values for CH7301 are from
166 			 * Chrontel app note and our experiment.
167 			 */
168 			pr_info("yes,CH7301 DVI chip found\n");
169 			sm750_sw_i2c_write_reg(0xec, 0x1d, 0x16);
170 			sm750_sw_i2c_write_reg(0xec, 0x21, 0x9);
171 			sm750_sw_i2c_write_reg(0xec, 0x49, 0xC0);
172 			pr_info("okay,CH7301 DVI chip setup done\n");
173 		}
174 	}
175 
176 	/* init 2d engine */
177 	if (!sm750_dev->accel_off)
178 		hw_sm750_initAccel(sm750_dev);
179 
180 	return 0;
181 }
182 
hw_sm750_output_setMode(struct lynxfb_output * output,struct fb_var_screeninfo * var,struct fb_fix_screeninfo * fix)183 int hw_sm750_output_setMode(struct lynxfb_output *output,
184 			    struct fb_var_screeninfo *var,
185 			    struct fb_fix_screeninfo *fix)
186 {
187 	int ret;
188 	enum disp_output disp_set;
189 	int channel;
190 
191 	ret = 0;
192 	disp_set = 0;
193 	channel = *output->channel;
194 
195 	if (sm750_get_chip_type() != SM750LE) {
196 		if (channel == sm750_primary) {
197 			pr_info("primary channel\n");
198 			if (output->paths & sm750_panel)
199 				disp_set |= do_LCD1_PRI;
200 			if (output->paths & sm750_crt)
201 				disp_set |= do_CRT_PRI;
202 
203 		} else {
204 			pr_info("secondary channel\n");
205 			if (output->paths & sm750_panel)
206 				disp_set |= do_LCD1_SEC;
207 			if (output->paths & sm750_crt)
208 				disp_set |= do_CRT_SEC;
209 		}
210 		ddk750_set_logical_disp_out(disp_set);
211 	} else {
212 		/* just open DISPLAY_CONTROL_750LE register bit 3:0 */
213 		u32 reg;
214 
215 		reg = peek32(DISPLAY_CONTROL_750LE);
216 		reg |= 0xf;
217 		poke32(DISPLAY_CONTROL_750LE, reg);
218 	}
219 
220 	pr_info("ddk setlogicdispout done\n");
221 	return ret;
222 }
223 
hw_sm750_crtc_checkMode(struct lynxfb_crtc * crtc,struct fb_var_screeninfo * var)224 int hw_sm750_crtc_checkMode(struct lynxfb_crtc *crtc,
225 			    struct fb_var_screeninfo *var)
226 {
227 	struct sm750_dev *sm750_dev;
228 	struct lynxfb_par *par = container_of(crtc, struct lynxfb_par, crtc);
229 
230 	sm750_dev = par->dev;
231 
232 	switch (var->bits_per_pixel) {
233 	case 8:
234 	case 16:
235 		break;
236 	case 32:
237 		if (sm750_dev->revid == SM750LE_REVISION_ID) {
238 			pr_debug("750le do not support 32bpp\n");
239 			return -EINVAL;
240 		}
241 		break;
242 	default:
243 		return -EINVAL;
244 	}
245 
246 	return 0;
247 }
248 
249 /* set the controller's mode for @crtc charged with @var and @fix parameters */
hw_sm750_crtc_setMode(struct lynxfb_crtc * crtc,struct fb_var_screeninfo * var,struct fb_fix_screeninfo * fix)250 int hw_sm750_crtc_setMode(struct lynxfb_crtc *crtc,
251 			  struct fb_var_screeninfo *var,
252 			  struct fb_fix_screeninfo *fix)
253 {
254 	int ret, fmt;
255 	u32 reg;
256 	struct mode_parameter modparm;
257 	enum clock_type clock;
258 	struct sm750_dev *sm750_dev;
259 	struct lynxfb_par *par;
260 
261 	ret = 0;
262 	par = container_of(crtc, struct lynxfb_par, crtc);
263 	sm750_dev = par->dev;
264 
265 	if (!sm750_dev->accel_off) {
266 		/* set 2d engine pixel format according to mode bpp */
267 		switch (var->bits_per_pixel) {
268 		case 8:
269 			fmt = 0;
270 			break;
271 		case 16:
272 			fmt = 1;
273 			break;
274 		case 32:
275 		default:
276 			fmt = 2;
277 			break;
278 		}
279 		sm750_hw_set2dformat(&sm750_dev->accel, fmt);
280 	}
281 
282 	/* set timing */
283 	modparm.pixel_clock = ps_to_hz(var->pixclock);
284 	modparm.vertical_sync_polarity =
285 		(var->sync & FB_SYNC_HOR_HIGH_ACT) ? POS : NEG;
286 	modparm.horizontal_sync_polarity =
287 		(var->sync & FB_SYNC_VERT_HIGH_ACT) ? POS : NEG;
288 	modparm.clock_phase_polarity =
289 		(var->sync & FB_SYNC_COMP_HIGH_ACT) ? POS : NEG;
290 	modparm.horizontal_display_end = var->xres;
291 	modparm.horizontal_sync_width = var->hsync_len;
292 	modparm.horizontal_sync_start = var->xres + var->right_margin;
293 	modparm.horizontal_total = var->xres + var->left_margin +
294 				   var->right_margin + var->hsync_len;
295 	modparm.vertical_display_end = var->yres;
296 	modparm.vertical_sync_height = var->vsync_len;
297 	modparm.vertical_sync_start = var->yres + var->lower_margin;
298 	modparm.vertical_total = var->yres + var->upper_margin +
299 				 var->lower_margin + var->vsync_len;
300 
301 	/* choose pll */
302 	if (crtc->channel != sm750_secondary)
303 		clock = PRIMARY_PLL;
304 	else
305 		clock = SECONDARY_PLL;
306 
307 	pr_debug("Request pixel clock = %lu\n", modparm.pixel_clock);
308 	ret = ddk750_setModeTiming(&modparm, clock);
309 	if (ret) {
310 		pr_err("Set mode timing failed\n");
311 		goto exit;
312 	}
313 
314 	if (crtc->channel != sm750_secondary) {
315 		/* set pitch, offset, width, start address, etc... */
316 		poke32(PANEL_FB_ADDRESS,
317 		       crtc->o_screen & PANEL_FB_ADDRESS_ADDRESS_MASK);
318 
319 		reg = var->xres * (var->bits_per_pixel >> 3);
320 		/*
321 		 * crtc->channel is not equal to par->index on numeric,
322 		 * be aware of that
323 		 */
324 		reg = ALIGN(reg, crtc->line_pad);
325 		reg = (reg << PANEL_FB_WIDTH_WIDTH_SHIFT) &
326 		      PANEL_FB_WIDTH_WIDTH_MASK;
327 		reg |= (fix->line_length & PANEL_FB_WIDTH_OFFSET_MASK);
328 		poke32(PANEL_FB_WIDTH, reg);
329 
330 		reg = ((var->xres - 1) << PANEL_WINDOW_WIDTH_WIDTH_SHIFT) &
331 		      PANEL_WINDOW_WIDTH_WIDTH_MASK;
332 		reg |= (var->xoffset & PANEL_WINDOW_WIDTH_X_MASK);
333 		poke32(PANEL_WINDOW_WIDTH, reg);
334 
335 		reg = (var->yres_virtual - 1)
336 		      << PANEL_WINDOW_HEIGHT_HEIGHT_SHIFT;
337 		reg &= PANEL_WINDOW_HEIGHT_HEIGHT_MASK;
338 		reg |= (var->yoffset & PANEL_WINDOW_HEIGHT_Y_MASK);
339 		poke32(PANEL_WINDOW_HEIGHT, reg);
340 
341 		poke32(PANEL_PLANE_TL, 0);
342 
343 		reg = ((var->yres - 1) << PANEL_PLANE_BR_BOTTOM_SHIFT) &
344 		      PANEL_PLANE_BR_BOTTOM_MASK;
345 		reg |= ((var->xres - 1) & PANEL_PLANE_BR_RIGHT_MASK);
346 		poke32(PANEL_PLANE_BR, reg);
347 
348 		/* set pixel format */
349 		reg = peek32(PANEL_DISPLAY_CTRL);
350 		poke32(PANEL_DISPLAY_CTRL, reg | (var->bits_per_pixel >> 4));
351 	} else {
352 		/* not implemented now */
353 		poke32(CRT_FB_ADDRESS, crtc->o_screen);
354 		reg = var->xres * (var->bits_per_pixel >> 3);
355 		/*
356 		 * crtc->channel is not equal to par->index on numeric,
357 		 * be aware of that
358 		 */
359 		reg = ALIGN(reg, crtc->line_pad) << CRT_FB_WIDTH_WIDTH_SHIFT;
360 		reg &= CRT_FB_WIDTH_WIDTH_MASK;
361 		reg |= (fix->line_length & CRT_FB_WIDTH_OFFSET_MASK);
362 		poke32(CRT_FB_WIDTH, reg);
363 
364 		/* SET PIXEL FORMAT */
365 		reg = peek32(CRT_DISPLAY_CTRL);
366 		reg |= ((var->bits_per_pixel >> 4) &
367 			CRT_DISPLAY_CTRL_FORMAT_MASK);
368 		poke32(CRT_DISPLAY_CTRL, reg);
369 	}
370 
371 exit:
372 	return ret;
373 }
374 
hw_sm750_setColReg(struct lynxfb_crtc * crtc,ushort index,ushort red,ushort green,ushort blue)375 int hw_sm750_setColReg(struct lynxfb_crtc *crtc, ushort index, ushort red,
376 		       ushort green, ushort blue)
377 {
378 	static unsigned int add[] = { PANEL_PALETTE_RAM, CRT_PALETTE_RAM };
379 
380 	poke32(add[crtc->channel] + index * 4,
381 	       (red << 16) | (green << 8) | blue);
382 	return 0;
383 }
384 
hw_sm750le_setBLANK(struct lynxfb_output * output,int blank)385 int hw_sm750le_setBLANK(struct lynxfb_output *output, int blank)
386 {
387 	int dpms, crtdb;
388 
389 	switch (blank) {
390 	case FB_BLANK_UNBLANK:
391 		dpms = CRT_DISPLAY_CTRL_DPMS_0;
392 		crtdb = 0;
393 		break;
394 	case FB_BLANK_NORMAL:
395 		dpms = CRT_DISPLAY_CTRL_DPMS_0;
396 		crtdb = CRT_DISPLAY_CTRL_BLANK;
397 		break;
398 	case FB_BLANK_VSYNC_SUSPEND:
399 		dpms = CRT_DISPLAY_CTRL_DPMS_2;
400 		crtdb = CRT_DISPLAY_CTRL_BLANK;
401 		break;
402 	case FB_BLANK_HSYNC_SUSPEND:
403 		dpms = CRT_DISPLAY_CTRL_DPMS_1;
404 		crtdb = CRT_DISPLAY_CTRL_BLANK;
405 		break;
406 	case FB_BLANK_POWERDOWN:
407 		dpms = CRT_DISPLAY_CTRL_DPMS_3;
408 		crtdb = CRT_DISPLAY_CTRL_BLANK;
409 		break;
410 	default:
411 		return -EINVAL;
412 	}
413 
414 	if (output->paths & sm750_crt) {
415 		unsigned int val;
416 
417 		val = peek32(CRT_DISPLAY_CTRL) & ~CRT_DISPLAY_CTRL_DPMS_MASK;
418 		poke32(CRT_DISPLAY_CTRL, val | dpms);
419 
420 		val = peek32(CRT_DISPLAY_CTRL) & ~CRT_DISPLAY_CTRL_BLANK;
421 		poke32(CRT_DISPLAY_CTRL, val | crtdb);
422 	}
423 	return 0;
424 }
425 
hw_sm750_setBLANK(struct lynxfb_output * output,int blank)426 int hw_sm750_setBLANK(struct lynxfb_output *output, int blank)
427 {
428 	unsigned int dpms, pps, crtdb;
429 
430 	dpms = 0;
431 	pps = 0;
432 	crtdb = 0;
433 
434 	switch (blank) {
435 	case FB_BLANK_UNBLANK:
436 		pr_debug("flag = FB_BLANK_UNBLANK\n");
437 		dpms = SYSTEM_CTRL_DPMS_VPHP;
438 		pps = PANEL_DISPLAY_CTRL_DATA;
439 		break;
440 	case FB_BLANK_NORMAL:
441 		pr_debug("flag = FB_BLANK_NORMAL\n");
442 		dpms = SYSTEM_CTRL_DPMS_VPHP;
443 		crtdb = CRT_DISPLAY_CTRL_BLANK;
444 		break;
445 	case FB_BLANK_VSYNC_SUSPEND:
446 		dpms = SYSTEM_CTRL_DPMS_VNHP;
447 		crtdb = CRT_DISPLAY_CTRL_BLANK;
448 		break;
449 	case FB_BLANK_HSYNC_SUSPEND:
450 		dpms = SYSTEM_CTRL_DPMS_VPHN;
451 		crtdb = CRT_DISPLAY_CTRL_BLANK;
452 		break;
453 	case FB_BLANK_POWERDOWN:
454 		dpms = SYSTEM_CTRL_DPMS_VNHN;
455 		crtdb = CRT_DISPLAY_CTRL_BLANK;
456 		break;
457 	}
458 
459 	if (output->paths & sm750_crt) {
460 		unsigned int val = peek32(SYSTEM_CTRL) & ~SYSTEM_CTRL_DPMS_MASK;
461 
462 		poke32(SYSTEM_CTRL, val | dpms);
463 
464 		val = peek32(CRT_DISPLAY_CTRL) & ~CRT_DISPLAY_CTRL_BLANK;
465 		poke32(CRT_DISPLAY_CTRL, val | crtdb);
466 	}
467 
468 	if (output->paths & sm750_panel) {
469 		unsigned int val = peek32(PANEL_DISPLAY_CTRL);
470 
471 		val &= ~PANEL_DISPLAY_CTRL_DATA;
472 		val |= pps;
473 		poke32(PANEL_DISPLAY_CTRL, val);
474 	}
475 
476 	return 0;
477 }
478 
hw_sm750_initAccel(struct sm750_dev * sm750_dev)479 void hw_sm750_initAccel(struct sm750_dev *sm750_dev)
480 {
481 	u32 reg;
482 
483 	sm750_enable_2d_engine(1);
484 
485 	if (sm750_get_chip_type() == SM750LE) {
486 		reg = peek32(DE_STATE1);
487 		reg |= DE_STATE1_DE_ABORT;
488 		poke32(DE_STATE1, reg);
489 
490 		reg = peek32(DE_STATE1);
491 		reg &= ~DE_STATE1_DE_ABORT;
492 		poke32(DE_STATE1, reg);
493 
494 	} else {
495 		/* engine reset */
496 		reg = peek32(SYSTEM_CTRL);
497 		reg |= SYSTEM_CTRL_DE_ABORT;
498 		poke32(SYSTEM_CTRL, reg);
499 
500 		reg = peek32(SYSTEM_CTRL);
501 		reg &= ~SYSTEM_CTRL_DE_ABORT;
502 		poke32(SYSTEM_CTRL, reg);
503 	}
504 
505 	/* call 2d init */
506 	sm750_dev->accel.de_init(&sm750_dev->accel);
507 }
508 
hw_sm750le_deWait(void)509 int hw_sm750le_deWait(void)
510 {
511 	int i = 0x10000000;
512 	unsigned int mask = DE_STATE2_DE_STATUS_BUSY | DE_STATE2_DE_FIFO_EMPTY |
513 			    DE_STATE2_DE_MEM_FIFO_EMPTY;
514 
515 	while (i--) {
516 		unsigned int val = peek32(DE_STATE2);
517 
518 		if ((val & mask) ==
519 		    (DE_STATE2_DE_FIFO_EMPTY | DE_STATE2_DE_MEM_FIFO_EMPTY))
520 			return 0;
521 	}
522 	/* timeout error */
523 	return -1;
524 }
525 
hw_sm750_deWait(void)526 int hw_sm750_deWait(void)
527 {
528 	int i = 0x10000000;
529 	unsigned int mask = SYSTEM_CTRL_DE_STATUS_BUSY |
530 			    SYSTEM_CTRL_DE_FIFO_EMPTY |
531 			    SYSTEM_CTRL_DE_MEM_FIFO_EMPTY;
532 
533 	while (i--) {
534 		unsigned int val = peek32(SYSTEM_CTRL);
535 
536 		if ((val & mask) ==
537 		    (SYSTEM_CTRL_DE_FIFO_EMPTY | SYSTEM_CTRL_DE_MEM_FIFO_EMPTY))
538 			return 0;
539 	}
540 	/* timeout error */
541 	return -1;
542 }
543 
hw_sm750_pan_display(struct lynxfb_crtc * crtc,const struct fb_var_screeninfo * var,const struct fb_info * info)544 int hw_sm750_pan_display(struct lynxfb_crtc *crtc,
545 			 const struct fb_var_screeninfo *var,
546 			 const struct fb_info *info)
547 {
548 	u32 total;
549 	/* check params */
550 	if ((var->xoffset + var->xres > var->xres_virtual) ||
551 	    (var->yoffset + var->yres > var->yres_virtual)) {
552 		return -EINVAL;
553 	}
554 
555 	total = var->yoffset * info->fix.line_length +
556 		((var->xoffset * var->bits_per_pixel) >> 3);
557 	total += crtc->o_screen;
558 	if (crtc->channel == sm750_primary) {
559 		poke32(PANEL_FB_ADDRESS,
560 		       peek32(PANEL_FB_ADDRESS) |
561 			       (total & PANEL_FB_ADDRESS_ADDRESS_MASK));
562 	} else {
563 		poke32(CRT_FB_ADDRESS,
564 		       peek32(CRT_FB_ADDRESS) |
565 			       (total & CRT_FB_ADDRESS_ADDRESS_MASK));
566 	}
567 	return 0;
568 }
569