1/*
2 * SBUS and OpenPROM access functions.
3 *
4 * Copyright (C) 2000 Jakub Jelinek (jakub@redhat.com)
5 *
6 * Permission is hereby granted, free of charge, to any person obtaining a copy
7 * of this software and associated documentation files (the "Software"), to deal
8 * in the Software without restriction, including without limitation the rights
9 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10 * copies of the Software, and to permit persons to whom the Software is
11 * furnished to do so, subject to the following conditions:
12 *
13 * The above copyright notice and this permission notice shall be included in
14 * all copies or substantial portions of the Software.
15 *
16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
19 * JAKUB JELINEK BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
20 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
21 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22 */
23
24#ifdef HAVE_XORG_CONFIG_H
25#include <xorg-config.h>
26#endif
27
28#include <fcntl.h>
29#include <stdio.h>
30#include <unistd.h>
31#include <stdlib.h>
32#include <sys/ioctl.h>
33#include <sys/mman.h>
34#ifdef __sun
35#include <sys/utsname.h>
36#endif
37#include "xf86.h"
38#include "xf86Priv.h"
39#include "xf86_OSlib.h"
40
41#include "xf86sbusBus.h"
42#include "xf86Sbus.h"
43
44int promRootNode;
45
46static int promFd = -1;
47static int promCurrentNode;
48static int promOpenCount = 0;
49static int promP1275 = -1;
50
51#define MAX_PROP	128
52#define MAX_VAL		(4096-128-4)
53static struct openpromio *promOpio;
54
55sbusDevicePtr *xf86SbusInfo = NULL;
56
57struct sbus_devtable sbusDeviceTable[] = {
58    {SBUS_DEVICE_BW2, FBTYPE_SUN2BW, "bwtwo", "sunbw2",
59     "Sun Monochrome (bwtwo)"},
60    {SBUS_DEVICE_CG2, FBTYPE_SUN2COLOR, "cgtwo", NULL, "Sun Color2 (cgtwo)"},
61    {SBUS_DEVICE_CG3, FBTYPE_SUN3COLOR, "cgthree", "suncg3",
62     "Sun Color3 (cgthree)"},
63    {SBUS_DEVICE_CG4, FBTYPE_SUN4COLOR, "cgfour", NULL, "Sun Color4 (cgfour)"},
64    {SBUS_DEVICE_CG6, FBTYPE_SUNFAST_COLOR, "cgsix", "suncg6", "Sun GX"},
65    {SBUS_DEVICE_CG8, FBTYPE_MEMCOLOR, "cgeight", NULL, "Sun CG8/RasterOps"},
66    {SBUS_DEVICE_CG12, FBTYPE_SUNGP3, "cgtwelve", NULL, "Sun GS (cgtwelve)"},
67    {SBUS_DEVICE_CG14, FBTYPE_MDICOLOR, "cgfourteen", "suncg14", "Sun SX"},
68    {SBUS_DEVICE_GT, FBTYPE_SUNGT, "gt", NULL, "Sun Graphics Tower"},
69    {SBUS_DEVICE_MGX, -1, "mgx", NULL, "Quantum 3D MGXplus"},
70    {SBUS_DEVICE_LEO, FBTYPE_SUNLEO, "leo", "sunleo", "Sun ZX or Turbo ZX"},
71    {SBUS_DEVICE_TCX, FBTYPE_TCXCOLOR, "tcx", "suntcx", "Sun TCX"},
72    {SBUS_DEVICE_FFB, FBTYPE_CREATOR, "ffb", "sunffb", "Sun FFB"},
73    {SBUS_DEVICE_FFB, FBTYPE_CREATOR, "afb", "sunffb", "Sun Elite3D"},
74    {0, 0, NULL}
75};
76
77int
78promGetSibling(int node)
79{
80    promOpio->oprom_size = sizeof(int);
81
82    if (node == -1)
83        return 0;
84    *(int *) promOpio->oprom_array = node;
85    if (ioctl(promFd, OPROMNEXT, promOpio) < 0)
86        return 0;
87    promCurrentNode = *(int *) promOpio->oprom_array;
88    return *(int *) promOpio->oprom_array;
89}
90
91int
92promGetChild(int node)
93{
94    promOpio->oprom_size = sizeof(int);
95
96    if (!node || node == -1)
97        return 0;
98    *(int *) promOpio->oprom_array = node;
99    if (ioctl(promFd, OPROMCHILD, promOpio) < 0)
100        return 0;
101    promCurrentNode = *(int *) promOpio->oprom_array;
102    return *(int *) promOpio->oprom_array;
103}
104
105char *
106promGetProperty(const char *prop, int *lenp)
107{
108    promOpio->oprom_size = MAX_VAL;
109
110    strcpy(promOpio->oprom_array, prop);
111    if (ioctl(promFd, OPROMGETPROP, promOpio) < 0)
112        return 0;
113    if (lenp)
114        *lenp = promOpio->oprom_size;
115    return promOpio->oprom_array;
116}
117
118int
119promGetBool(const char *prop)
120{
121    promOpio->oprom_size = 0;
122
123    *(int *) promOpio->oprom_array = 0;
124    for (;;) {
125        promOpio->oprom_size = MAX_PROP;
126        if (ioctl(promFd, OPROMNXTPROP, promOpio) < 0)
127            return 0;
128        if (!promOpio->oprom_size)
129            return 0;
130        if (!strcmp(promOpio->oprom_array, prop))
131            return 1;
132    }
133}
134
135#define PROM_NODE_SIBLING 0x01
136#define PROM_NODE_PREF    0x02
137#define PROM_NODE_SBUS    0x04
138#define PROM_NODE_EBUS    0x08
139#define PROM_NODE_PCI     0x10
140
141static int
142promSetNode(sbusPromNodePtr pnode)
143{
144    int node;
145
146    if (!pnode->node || pnode->node == -1)
147        return -1;
148    if (pnode->cookie[0] & PROM_NODE_SIBLING)
149        node = promGetSibling(pnode->cookie[1]);
150    else
151        node = promGetChild(pnode->cookie[1]);
152    if (pnode->node != node)
153        return -1;
154    return 0;
155}
156
157static void
158promIsP1275(void)
159{
160#ifdef __linux__
161    FILE *f;
162    char buffer[1024];
163
164    if (promP1275 != -1)
165        return;
166    promP1275 = 0;
167    f = fopen("/proc/cpuinfo", "r");
168    if (!f)
169        return;
170    while (fgets(buffer, 1024, f) != NULL)
171        if (!strncmp(buffer, "type", 4) && strstr(buffer, "sun4u")) {
172            promP1275 = 1;
173            break;
174        }
175    fclose(f);
176#elif defined(__sun)
177    struct utsname buffer;
178
179    if ((uname(&buffer) >= 0) && !strcmp(buffer.machine, "sun4u"))
180        promP1275 = TRUE;
181    else
182        promP1275 = FALSE;
183#elif defined(__FreeBSD__)
184    promP1275 = TRUE;
185#else
186#error Missing promIsP1275() function for this OS
187#endif
188}
189
190void
191sparcPromClose(void)
192{
193    if (promOpenCount > 1) {
194        promOpenCount--;
195        return;
196    }
197    if (promFd != -1) {
198        close(promFd);
199        promFd = -1;
200    }
201    free(promOpio);
202    promOpio = NULL;
203    promOpenCount = 0;
204}
205
206int
207sparcPromInit(void)
208{
209    if (promOpenCount) {
210        promOpenCount++;
211        return 0;
212    }
213    promFd = open("/dev/openprom", O_RDONLY, 0);
214    if (promFd == -1)
215        return -1;
216    promOpio = (struct openpromio *) malloc(4096);
217    if (!promOpio) {
218        sparcPromClose();
219        return -1;
220    }
221    promRootNode = promGetSibling(0);
222    if (!promRootNode) {
223        sparcPromClose();
224        return -1;
225    }
226    promIsP1275();
227    promOpenCount++;
228
229    return 0;
230}
231
232char *
233sparcPromGetProperty(sbusPromNodePtr pnode, const char *prop, int *lenp)
234{
235    if (promSetNode(pnode))
236        return NULL;
237    return promGetProperty(prop, lenp);
238}
239
240int
241sparcPromGetBool(sbusPromNodePtr pnode, const char *prop)
242{
243    if (promSetNode(pnode))
244        return 0;
245    return promGetBool(prop);
246}
247
248static const char *
249promWalkGetDriverName(int node, int oldnode)
250{
251    int nextnode;
252    int len;
253    char *prop;
254    int devId, i;
255
256    prop = promGetProperty("device_type", &len);
257    if (prop && (len > 0))
258        do {
259            if (!strcmp(prop, "display")) {
260                prop = promGetProperty("name", &len);
261                if (!prop || len <= 0)
262                    break;
263                while ((*prop >= 'A' && *prop <= 'Z') || *prop == ',')
264                    prop++;
265                for (i = 0; sbusDeviceTable[i].devId; i++)
266                    if (!strcmp(prop, sbusDeviceTable[i].promName))
267                        break;
268                devId = sbusDeviceTable[i].devId;
269                if (!devId)
270                    break;
271                if (sbusDeviceTable[i].driverName)
272                    return sbusDeviceTable[i].driverName;
273            }
274        } while (0);
275
276    nextnode = promGetChild(node);
277    if (nextnode) {
278        const char *name;
279
280        name = promWalkGetDriverName(nextnode, node);
281        if (name)
282            return name;
283    }
284
285    nextnode = promGetSibling(node);
286    if (nextnode)
287        return promWalkGetDriverName(nextnode, node);
288    return NULL;
289}
290
291const char *
292sparcDriverName(void)
293{
294    const char *name;
295
296    if (sparcPromInit() < 0)
297        return NULL;
298    promGetSibling(0);
299    name = promWalkGetDriverName(promRootNode, 0);
300    sparcPromClose();
301    return name;
302}
303
304static void
305promWalkAssignNodes(int node, int oldnode, int flags,
306                    sbusDevicePtr * devicePtrs)
307{
308    int nextnode;
309    int len, sbus = flags & PROM_NODE_SBUS;
310    char *prop;
311    int devId, i, j;
312    sbusPromNode pNode, pNode2;
313
314    prop = promGetProperty("device_type", &len);
315    if (prop && (len > 0))
316        do {
317            if (!strcmp(prop, "display")) {
318                prop = promGetProperty("name", &len);
319                if (!prop || len <= 0)
320                    break;
321                while ((*prop >= 'A' && *prop <= 'Z') || *prop == ',')
322                    prop++;
323                for (i = 0; sbusDeviceTable[i].devId; i++)
324                    if (!strcmp(prop, sbusDeviceTable[i].promName))
325                        break;
326                devId = sbusDeviceTable[i].devId;
327                if (!devId)
328                    break;
329                if (!sbus) {
330                    if (devId == SBUS_DEVICE_FFB) {
331                        /*
332                         * All /SUNW,ffb outside of SBUS tree come before all
333                         * /SUNW,afb outside of SBUS tree in Linux.
334                         */
335                        if (!strcmp(prop, "afb"))
336                            flags |= PROM_NODE_PREF;
337                    }
338                    else if (devId != SBUS_DEVICE_CG14)
339                        break;
340                }
341                for (i = 0; i < 32; i++) {
342                    if (!devicePtrs[i] || devicePtrs[i]->devId != devId)
343                        continue;
344                    if (devicePtrs[i]->node.node) {
345                        if ((devicePtrs[i]->node.
346                             cookie[0] & ~PROM_NODE_SIBLING) <=
347                            (flags & ~PROM_NODE_SIBLING))
348                            continue;
349                        for (j = i + 1, pNode = devicePtrs[i]->node; j < 32;
350                             j++) {
351                            if (!devicePtrs[j] || devicePtrs[j]->devId != devId)
352                                continue;
353                            pNode2 = devicePtrs[j]->node;
354                            devicePtrs[j]->node = pNode;
355                            pNode = pNode2;
356                        }
357                    }
358                    devicePtrs[i]->node.node = node;
359                    devicePtrs[i]->node.cookie[0] = flags;
360                    devicePtrs[i]->node.cookie[1] = oldnode;
361                    break;
362                }
363                break;
364            }
365        } while (0);
366
367    prop = promGetProperty("name", &len);
368    if (prop && len > 0) {
369        if (!strcmp(prop, "sbus") || !strcmp(prop, "sbi"))
370            sbus = PROM_NODE_SBUS;
371    }
372
373    nextnode = promGetChild(node);
374    if (nextnode)
375        promWalkAssignNodes(nextnode, node, sbus, devicePtrs);
376
377    nextnode = promGetSibling(node);
378    if (nextnode)
379        promWalkAssignNodes(nextnode, node, PROM_NODE_SIBLING | sbus,
380                            devicePtrs);
381}
382
383void
384sparcPromAssignNodes(void)
385{
386    sbusDevicePtr psdp, *psdpp;
387    int n, holes = 0, i, j;
388    FILE *f;
389    sbusDevicePtr devicePtrs[32];
390
391    memset(devicePtrs, 0, sizeof(devicePtrs));
392    for (psdpp = xf86SbusInfo, n = 0; (psdp = *psdpp); psdpp++, n++) {
393        if (psdp->fbNum != n)
394            holes = 1;
395        devicePtrs[psdp->fbNum] = psdp;
396    }
397    if (holes && (f = fopen("/proc/fb", "r")) != NULL) {
398        /* We could not open one of fb devices, check /proc/fb to see what
399         * were the types of the cards missed. */
400        char buffer[64];
401        int fbNum, devId;
402        static struct {
403            int devId;
404            char *prefix;
405        } procFbPrefixes[] = {
406            {SBUS_DEVICE_BW2, "BWtwo"},
407            {SBUS_DEVICE_CG14, "CGfourteen"},
408            {SBUS_DEVICE_CG6, "CGsix"},
409            {SBUS_DEVICE_CG3, "CGthree"},
410            {SBUS_DEVICE_FFB, "Creator"},
411            {SBUS_DEVICE_FFB, "Elite 3D"},
412            {SBUS_DEVICE_LEO, "Leo"},
413            {SBUS_DEVICE_TCX, "TCX"},
414            {0, NULL},
415        };
416
417        while (fscanf(f, "%d %63s\n", &fbNum, buffer) == 2) {
418            for (i = 0; procFbPrefixes[i].devId; i++)
419                if (!strncmp(procFbPrefixes[i].prefix, buffer,
420                             strlen(procFbPrefixes[i].prefix)))
421                    break;
422            devId = procFbPrefixes[i].devId;
423            if (!devId)
424                continue;
425            if (devicePtrs[fbNum]) {
426                if (devicePtrs[fbNum]->devId != devId)
427                    xf86ErrorF("Inconsistent /proc/fb with FBIOGATTR\n");
428            }
429            else if (!devicePtrs[fbNum]) {
430                devicePtrs[fbNum] = psdp = xnfcalloc(sizeof(sbusDevice), 1);
431                psdp->devId = devId;
432                psdp->fbNum = fbNum;
433                psdp->fd = -2;
434            }
435        }
436        fclose(f);
437    }
438    promGetSibling(0);
439    promWalkAssignNodes(promRootNode, 0, PROM_NODE_PREF, devicePtrs);
440    for (i = 0, j = 0; i < 32; i++)
441        if (devicePtrs[i] && devicePtrs[i]->fbNum == -1)
442            j++;
443    xf86SbusInfo = xnfreallocarray(xf86SbusInfo, n + j + 1, sizeof(psdp));
444    for (i = 0, psdpp = xf86SbusInfo; i < 32; i++)
445        if (devicePtrs[i]) {
446            if (devicePtrs[i]->fbNum == -1) {
447                memmove(psdpp + 1, psdpp, sizeof(psdpp) * (n + 1));
448                *psdpp = devicePtrs[i];
449            }
450            else
451                n--;
452        }
453}
454
455static char *
456promGetReg(int type)
457{
458    char *prop;
459    int len;
460    static char regstr[40];
461
462    regstr[0] = 0;
463    prop = promGetProperty("reg", &len);
464    if (prop && len >= 4) {
465        unsigned int *reg = (unsigned int *) prop;
466
467        if (!promP1275 || (type == PROM_NODE_SBUS) || (type == PROM_NODE_EBUS))
468            snprintf(regstr, sizeof(regstr), "@%x,%x", reg[0], reg[1]);
469        else if (type == PROM_NODE_PCI) {
470            if ((reg[0] >> 8) & 7)
471                snprintf(regstr, sizeof(regstr), "@%x,%x",
472                         (reg[0] >> 11) & 0x1f, (reg[0] >> 8) & 7);
473            else
474                snprintf(regstr, sizeof(regstr), "@%x", (reg[0] >> 11) & 0x1f);
475        }
476        else if (len == 4)
477            snprintf(regstr, sizeof(regstr), "@%x", reg[0]);
478        else {
479            unsigned int regs[2];
480
481            /* Things get more complicated on UPA. If upa-portid exists,
482               then address is @upa-portid,second-int-in-reg, otherwise
483               it is @first-int-in-reg/16,second-int-in-reg (well, probably
484               upa-portid always exists, but just to be safe). */
485            memcpy(regs, reg, sizeof(regs));
486            prop = promGetProperty("upa-portid", &len);
487            if (prop && len == 4) {
488                reg = (unsigned int *) prop;
489                snprintf(regstr, sizeof(regstr), "@%x,%x", reg[0], regs[1]);
490            }
491            else
492                snprintf(regstr, sizeof(regstr), "@%x,%x", regs[0] >> 4,
493                         regs[1]);
494        }
495    }
496    return regstr;
497}
498
499static int
500promWalkNode2Pathname(char *path, int parent, int node, int searchNode,
501                      int type)
502{
503    int nextnode;
504    int len, ntype = type;
505    char *prop, *p;
506
507    prop = promGetProperty("name", &len);
508    *path = '/';
509    if (!prop || len <= 0)
510        return 0;
511    if ((!strcmp(prop, "sbus") || !strcmp(prop, "sbi")) && !type)
512        ntype = PROM_NODE_SBUS;
513    else if (!strcmp(prop, "ebus") && type == PROM_NODE_PCI)
514        ntype = PROM_NODE_EBUS;
515    else if (!strcmp(prop, "pci") && !type)
516        ntype = PROM_NODE_PCI;
517    strcpy(path + 1, prop);
518    p = promGetReg(type);
519    if (*p)
520        strcat(path, p);
521    if (node == searchNode)
522        return 1;
523    nextnode = promGetChild(node);
524    if (nextnode &&
525        promWalkNode2Pathname(strchr(path, 0), node, nextnode, searchNode,
526                              ntype))
527        return 1;
528    nextnode = promGetSibling(node);
529    if (nextnode &&
530        promWalkNode2Pathname(path, parent, nextnode, searchNode, type))
531        return 1;
532    return 0;
533}
534
535char *
536sparcPromNode2Pathname(sbusPromNodePtr pnode)
537{
538    char *ret;
539
540    if (!pnode->node)
541        return NULL;
542    ret = malloc(4096);
543    if (!ret)
544        return NULL;
545    if (promWalkNode2Pathname
546        (ret, promRootNode, promGetChild(promRootNode), pnode->node, 0))
547        return ret;
548    free(ret);
549    return NULL;
550}
551
552static int
553promWalkPathname2Node(char *name, char *regstr, int parent, int type)
554{
555    int len, node, ret;
556    char *prop, *p;
557
558    for (;;) {
559        prop = promGetProperty("name", &len);
560        if (!prop || len <= 0)
561            return 0;
562        if ((!strcmp(prop, "sbus") || !strcmp(prop, "sbi")) && !type)
563            type = PROM_NODE_SBUS;
564        else if (!strcmp(prop, "ebus") && type == PROM_NODE_PCI)
565            type = PROM_NODE_EBUS;
566        else if (!strcmp(prop, "pci") && !type)
567            type = PROM_NODE_PCI;
568        for (node = promGetChild(parent); node; node = promGetSibling(node)) {
569            prop = promGetProperty("name", &len);
570            if (!prop || len <= 0)
571                continue;
572            if (*name && strcmp(name, prop))
573                continue;
574            if (*regstr) {
575                p = promGetReg(type);
576                if (!*p || strcmp(p + 1, regstr))
577                    continue;
578            }
579            break;
580        }
581        if (!node) {
582            for (node = promGetChild(parent); node; node = promGetSibling(node)) {
583                ret = promWalkPathname2Node(name, regstr, node, type);
584                if (ret)
585                    return ret;
586            }
587            return 0;
588        }
589        name = strchr(regstr, 0) + 1;
590        if (!*name)
591            return node;
592        p = strchr(name, '/');
593        if (p)
594            *p = 0;
595        else
596            p = strchr(name, 0);
597        regstr = strchr(name, '@');
598        if (regstr)
599            *regstr++ = 0;
600        else
601            regstr = p;
602        if (name == regstr)
603            return 0;
604        parent = node;
605    }
606}
607
608int
609sparcPromPathname2Node(const char *pathName)
610{
611    int i;
612    char *name, *regstr, *p;
613
614    i = strlen(pathName);
615    name = malloc(i + 2);
616    if (!name)
617        return 0;
618    strcpy(name, pathName);
619    name[i + 1] = 0;
620    if (name[0] != '/') {
621        free(name);
622        return 0;
623    }
624    p = strchr(name + 1, '/');
625    if (p)
626        *p = 0;
627    else
628        p = strchr(name, 0);
629    regstr = strchr(name, '@');
630    if (regstr)
631        *regstr++ = 0;
632    else
633        regstr = p;
634    if (name + 1 == regstr) {
635        free(name);
636        return 0;
637    }
638    promGetSibling(0);
639    i = promWalkPathname2Node(name + 1, regstr, promRootNode, 0);
640    free(name);
641    return i;
642}
643
644void *
645xf86MapSbusMem(sbusDevicePtr psdp, unsigned long offset, unsigned long size)
646{
647    void *ret;
648    unsigned long pagemask = getpagesize() - 1;
649    unsigned long off = offset & ~pagemask;
650    unsigned long len = ((offset + size + pagemask) & ~pagemask) - off;
651
652    if (psdp->fd == -1) {
653        psdp->fd = open(psdp->device, O_RDWR);
654        if (psdp->fd == -1)
655            return NULL;
656    }
657    else if (psdp->fd < 0)
658        return NULL;
659
660    ret = (void *) mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_PRIVATE,
661                         psdp->fd, off);
662    if (ret == (void *) -1) {
663        ret = (void *) mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED,
664                             psdp->fd, off);
665    }
666    if (ret == (void *) -1)
667        return NULL;
668
669    return (char *) ret + (offset - off);
670}
671
672void
673xf86UnmapSbusMem(sbusDevicePtr psdp, void *addr, unsigned long size)
674{
675    unsigned long mask = getpagesize() - 1;
676    unsigned long base = (unsigned long) addr & ~mask;
677    unsigned long len = (((unsigned long) addr + size + mask) & ~mask) - base;
678
679    munmap((void *) base, len);
680}
681
682/* Tell OS that we are driving the HW cursor ourselves. */
683void
684xf86SbusHideOsHwCursor(sbusDevicePtr psdp)
685{
686    struct fbcursor fbcursor;
687    unsigned char zeros[8];
688
689    memset(&fbcursor, 0, sizeof(fbcursor));
690    memset(&zeros, 0, sizeof(zeros));
691    fbcursor.cmap.count = 2;
692    fbcursor.cmap.red = zeros;
693    fbcursor.cmap.green = zeros;
694    fbcursor.cmap.blue = zeros;
695    fbcursor.image = (char *) zeros;
696    fbcursor.mask = (char *) zeros;
697    fbcursor.size.x = 32;
698    fbcursor.size.y = 1;
699    fbcursor.set = FB_CUR_SETALL;
700    ioctl(psdp->fd, FBIOSCURSOR, &fbcursor);
701}
702
703/* Set HW cursor colormap. */
704void
705xf86SbusSetOsHwCursorCmap(sbusDevicePtr psdp, int bg, int fg)
706{
707    struct fbcursor fbcursor;
708    unsigned char red[2], green[2], blue[2];
709
710    memset(&fbcursor, 0, sizeof(fbcursor));
711    red[0] = bg >> 16;
712    green[0] = bg >> 8;
713    blue[0] = bg;
714    red[1] = fg >> 16;
715    green[1] = fg >> 8;
716    blue[1] = fg;
717    fbcursor.cmap.count = 2;
718    fbcursor.cmap.red = red;
719    fbcursor.cmap.green = green;
720    fbcursor.cmap.blue = blue;
721    fbcursor.set = FB_CUR_SETCMAP;
722    ioctl(psdp->fd, FBIOSCURSOR, &fbcursor);
723}
724