void
file_open(struct file *file, char path[])
{
assert_fatal((file->fd = open(path, O_RDWR, 0)) != -1,
"error opening '%s': %s", path, strerror(errno));
assert_fatal(!fstat(file->fd, &file->st), "stat failed: %s",
strerror(errno));
file->mem = 0;
}
static void
capture()
{
for (;;) {
fd_set fds;
struct timeval tv;
int r;
FD_ZERO (&fds);
FD_SET (video.fd, &fds);
/* Timeout. */
tv.tv_sec = 2;
tv.tv_usec = 0;
r = select (video.fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
if (EINTR == errno)
continue;
assert(0, "select == -1");
}
assert_fatal(0 != r, "select timeout");
assert(read_frame() == 1, "error when reading frame");
}
}
static void
open_device(void)
{
struct stat st;
assert_fatal(-1 != stat (video.dev_name, &st), "Cannot identify '%s': %d, %s\n",
video.dev_name, errno, strerror (errno));
assert_fatal(S_ISCHR (st.st_mode), "%s is no device\n",
video.dev_name);
video.fd = open (video.dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);
assert_fatal(-1 != video.fd, "Cannot open '%s': %d, %s\n",
video.dev_name, errno, strerror(errno));
}
static int
read_frame(void)
{
struct v4l2_buffer buf;
CLEAR (buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
if (-1 == _ioctl (video.fd, VIDIOC_DQBUF, &buf)) {
switch (errno) {
case EAGAIN:
return 0;
case EIO:
/* Could ignore EIO, see spec. */
/* fall through */
default:
assert(0, "failure on ioctl.VIDIOC_DQBUF");
}
}
assert(buf.index < n_buffers, "non-fatal assert");
thread_lock(video.array);
video.array.val = buffers[buf.index].start;
thread_cond_broadcast(&video.array_new);
assert_fatal(-1 != _ioctl (video.fd, VIDIOC_QBUF, &buf),
"failure on ioctl.VIDIOC_QBUF");
return 1;
}
void*
file_mmap(struct file *file)
{
file->mem = mmap(0, file->st.st_size, PROT_READ | PROT_WRITE,
MAP_SHARED, file->fd, 0);
assert_fatal(file->mem != MAP_FAILED, "mmap failed: %s", strerror(errno));
return file->mem;
}
static void
start_capturing(void)
{
unsigned int i;
enum v4l2_buf_type type;
for (i = 0; i < n_buffers; ++i) {
struct v4l2_buffer buf;
CLEAR (buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
assert_fatal(-1 != _ioctl (video.fd, VIDIOC_QBUF, &buf),
"failure on ioctl.VIDIOC_QBUF");
}
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
assert_fatal(-1 != _ioctl (video.fd, VIDIOC_STREAMON, &type),
"failure on ioctl.VIDIOC_STREAMON");
}
static void
init_mmap(void)
{
struct v4l2_requestbuffers req;
CLEAR (req);
req.count = 4;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_MMAP;
if (-1 == _ioctl (video.fd, VIDIOC_REQBUFS, &req)) {
if (EINVAL == errno) {
assert_fatal(0, "%s does not support "
"memory mapping", video.dev_name);
} else {
assert_fatal(0, "failure on ioctl.VIDIOC_REQBUFS");
}
}
assert_fatal(req.count >= 2, "Insufficient buffer memory on %s", video.dev_name);
buffers = calloc (req.count, sizeof (*buffers));
assert_fatal(buffers, "Out of memory");
for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
struct v4l2_buffer buf;
CLEAR (buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = n_buffers;
assert_fatal(-1 != _ioctl (video.fd, VIDIOC_QUERYBUF, &buf),
"failure on ioctl.VIDIOC_QUERYBUF");
buffers[n_buffers].length = buf.length;
buffers[n_buffers].start =
mmap (NULL /* start anywhere */,
buf.length,
PROT_READ | PROT_WRITE /* required */,
MAP_SHARED /* recommended */,
video.fd, buf.m.offset);
assert_fatal(MAP_FAILED != buffers[n_buffers].start, "mmap failed");
}
}
void
file_write(const struct file *file)
{
assert_fatal(write(file->fd, file->mem, file->st.st_size) != -1,
"write failed: %s", strerror(errno));
}
static void
close_device(void)
{
assert_fatal(-1 != close (video.fd), "cannot close video device's fd");
video.fd = -1;
}
static void
init_device(void)
{
struct v4l2_capability cap;
struct v4l2_cropcap cropcap;
struct v4l2_crop crop;
struct v4l2_format fmt;
unsigned int min;
struct v4l2_queryctrl queryctrl;
struct v4l2_control control;
memset (&queryctrl, 0, sizeof (queryctrl));
queryctrl.id = V4L2_CID_BRIGHTNESS;
if (-1 == ioctl (video.fd, VIDIOC_QUERYCTRL, &queryctrl)) {
if (errno != EINVAL)
log("failure on ioctl.VIDIOC_QUERYCTRL");
else
log("V4L2_CID_BRIGHTNESS is not supported");
} else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
log("V4L2_CID_BRIGHTNESS is not supported");
} else {
memset (&control, 0, sizeof (control));
control.id = V4L2_CID_BRIGHTNESS;
control.value = queryctrl.default_value;
log("Brightness set to %i.", queryctrl.default_value);
}
if (-1 == _ioctl (video.fd, VIDIOC_QUERYCAP, &cap)) {
if (EINVAL == errno) {
assert_fatal(0, "%s is no V4L2 device\n",
video.dev_name);
} else {
assert_fatal(0, "VIDIOC_QUERYCAP");
}
}
assert_fatal(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE,
"%s is no video capture device", video.dev_name);
/* Select video input, video standard and tune here. */
CLEAR (cropcap);
cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (0 == _ioctl (video.fd, VIDIOC_CROPCAP, &cropcap)) {
crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
crop.c = cropcap.defrect; /* reset to default */
if (-1 == _ioctl (video.fd, VIDIOC_S_CROP, &crop)) {
switch (errno) {
case EINVAL:
/* Cropping not supported. */
break;
default:
/* Errors ignored. */
break;
}
}
} else {
/* Errors ignored. */
}
CLEAR (fmt);
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.fmt.pix.width = video.width;
fmt.fmt.pix.height = video.height;
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
assert_fatal(-1 != _ioctl (video.fd, VIDIOC_S_FMT, &fmt),
"failure on ioctl.VIDIOC_S_FMT");
/* Note VIDIOC_S_FMT may change width and height. */
/* Buggy driver paranoia. */
min = fmt.fmt.pix.width * 2;
if (fmt.fmt.pix.bytesperline < min)
fmt.fmt.pix.bytesperline = min;
min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
if (fmt.fmt.pix.sizeimage < min)
fmt.fmt.pix.sizeimage = min;
init_mmap ();
}
error_t ZkcmCpuDetectionMod::setSmpMode(void)
{
error_t ret;
ubit8 *lowmem;
void *srcAddr, *destAddr;
uarch_t copySize, cpuFlags;
x86Mp::sFloatingPtr *mpFp;
ubit8 i8254WasEnabled=0, i8254WasSoftEnabled=0;
/** EXPLANATION:
* This function will enable Symmetric I/O mode on the IBM-PC chipset.
* By extension, it also sets the CMOS warm reset mode and the BIOS
* reset vector.
*
* Everything done here is taken from the MP specification and Intel
* manuals, with cross-examination from Linux source just in case.
*
* Naturally, to access lowmem, we use the chipset's memoryAreas
* manager.
*
* NOTE:
* * I don't think you need to enable Symm. I/O mode to use the LAPICs.
**/
ret = chipsetMemAreas::mapArea(CHIPSET_MEMAREA_LOWMEM);
if (ret != ERROR_SUCCESS)
{
printf(ERROR CPUMOD"setSmpMode(): Failed to map lowmem.\n");
return ret;
};
lowmem = static_cast<ubit8 *>(
chipsetMemAreas::getArea(CHIPSET_MEMAREA_LOWMEM) );
// Change the warm reset vector.
*reinterpret_cast<uarch_t **>( &lowmem[(0x40 << 4) + 0x67] ) =
&__kcpuPowerOnTextStart;
// TODO: Set CMOS reset operation to "warm reset".
cpuControl::safeDisableInterrupts(&cpuFlags);
io::write8(0x70, 0x0F);
io::write8(0x71, 0x0A);
cpuControl::safeEnableInterrupts(cpuFlags);
/* Next, we need to copy the kernel's .__kcpuPowerOn[Text/Data] stuff to
* lowmem. Memcpy() ftw...
**/
srcAddr = (void *)(((uarch_t)&__kcpuPowerOnTextStart
- x8632_IBMPC_POWERON_PADDR_BASE)
+ ARCH_MEMORY___KLOAD_VADDR_BASE);
destAddr = &lowmem[(uarch_t)&__kcpuPowerOnTextStart];
copySize = (uarch_t)&__kcpuPowerOnTextEnd
- (uarch_t)&__kcpuPowerOnTextStart;
printf(NOTICE CPUMOD"setSmpMode: Copy CPU wakeup code: %p "
"to %p; %d B.\n",
srcAddr, destAddr, copySize);
memcpy(destAddr, srcAddr, copySize);
srcAddr = (void *)(((uarch_t)&__kcpuPowerOnDataStart
- x8632_IBMPC_POWERON_PADDR_BASE)
+ ARCH_MEMORY___KLOAD_VADDR_BASE);
destAddr = &lowmem[(uarch_t)&__kcpuPowerOnDataStart];
copySize = (uarch_t)&__kcpuPowerOnDataEnd
- (uarch_t)&__kcpuPowerOnDataStart;
printf(NOTICE CPUMOD"setSmpMode: Copy CPU wakeup data: %p "
"to %p; %d B.\n",
srcAddr, destAddr, copySize);
memcpy(destAddr, srcAddr, copySize);
x86IoApic::initializeCache();
if (!x86IoApic::ioApicsAreDetected())
{
ret = x86IoApic::detectIoApics();
if (ret != ERROR_SUCCESS) { return ret; };
};
/* The i8254 is the only device which should be operational right
* now. Disable it to force it to forget its __kpin assignment, then
* mask all of the i8259 PIC pins.
**/
if (i8254Pit.isEnabled()) { i8254WasEnabled = 1; };
if (i8254Pit.isSoftEnabled()) { i8254WasSoftEnabled = 1; };
printf(NOTICE CPUMOD"setSmpMode: i8254: wasEnabled=%d, "
"wasSoftEnabled=%d.\n",
i8254WasEnabled, i8254WasSoftEnabled);
i8254Pit.setSmpModeSwitchFlag(
cpuTrib.getCurrentCpuStream()
->taskStream.getCurrentThread()->getFullId());
printf(NOTICE CPUMOD"setSmpMode: Waiting for devices to disable.\n");
if (i8254WasEnabled)
{
i8254Pit.disable();
taskTrib.block();
};
i8254Pit.unsetSmpModeSwitchFlag();
/** EXPLANATION
* Next, we parse the MP tables to see if the chipset has the IMCR
* implemented. If so, we turn on Symm. I/O mode.
*
* If there are no MP tables, the chipset is assumed to be in virtual
* wire mode.
**/
mpFp = x86Mp::findMpFp();
if (mpFp != NULL
&& FLAG_TEST(mpFp->features[1], x86_MPFP_FEAT1_FLAG_PICMODE))
{
ibmPcState.smpInfo.chipsetOriginalState = SMPSTATE_UNIPROCESSOR;
// Enable Symm. I/O mode using IMCR.
cpuControl::safeDisableInterrupts(&cpuFlags);
io::write8(0x22, 0x70);
io::write8(0x23, 0x1);
cpuControl::safeEnableInterrupts(cpuFlags);
printf(NOTICE CPUMOD"setSmpMode: chipset was in PIC "
"mode.\n");
}
else
{
ibmPcState.smpInfo.chipsetOriginalState = SMPSTATE_SMP;
printf(NOTICE CPUMOD"setSmpMode: chipset was in Virtual "
"wire mode.\n");
};
ibmPcState.smpInfo.chipsetState = SMPSTATE_SMP;
assert_fatal(
zkcmCore.irqControl.bpm.loadBusPinMappings(CC"isa")
== ERROR_SUCCESS);
printf(NOTICE CPUMOD"setSmpMode: Re-enabling devices.\n");
if (i8254WasEnabled)
{
ret = i8254Pit.enable();
if (ret != ERROR_SUCCESS)
{
printf(ERROR CPUMOD"setSmpMode: i8254 enable() "
"failed.\n");
return ret;
};
if (!i8254WasSoftEnabled) { i8254Pit.softDisable(); };
};
zkcmCore.irqControl.chipsetEventNotification(
IRQCTL_EVENT_SMP_MODE_SWITCH, 0);
return ERROR_SUCCESS;
}
