diff options
Diffstat (limited to 'drivers/staging/vc04_services')
42 files changed, 17409 insertions, 0 deletions
diff --git a/drivers/staging/vc04_services/Kconfig b/drivers/staging/vc04_services/Kconfig new file mode 100644 index 000000000..31e58c9d1 --- /dev/null +++ b/drivers/staging/vc04_services/Kconfig @@ -0,0 +1,50 @@ +# SPDX-License-Identifier: GPL-2.0 +menuconfig BCM_VIDEOCORE + tristate "Broadcom VideoCore support" + depends on OF + depends on RASPBERRYPI_FIRMWARE || (COMPILE_TEST && !RASPBERRYPI_FIRMWARE) + default y + help + Support for Broadcom VideoCore services including + the BCM2835 family of products which is used + by the Raspberry PI. + +if BCM_VIDEOCORE + +config BCM2835_VCHIQ + tristate "BCM2835 VCHIQ" + depends on HAS_DMA + imply VCHIQ_CDEV + help + Broadcom BCM2835 and similar SoCs have a VPU called VideoCore. This config + enables the VCHIQ driver, which implements a messaging interface between + the kernel and the firmware running on VideoCore. Other drivers use this + interface to communicate to the VPU. More specifically, the VCHIQ driver is + used by audio/video and camera drivers as well as for implementing MMAL + API, which is in turn used by several multimedia services on the BCM2835 + family of SoCs. + Defaults to Y when the Broadcom Videocore services are included in + the build, N otherwise. + +if BCM2835_VCHIQ + +config VCHIQ_CDEV + bool "VCHIQ Character Driver" + help + Enable the creation of VCHIQ character driver. The cdev exposes ioctls used + by userspace libraries and testing tools to interact with VideoCore, via + the VCHIQ core driver (Check BCM2835_VCHIQ for more info). + This can be set to 'N' if the VideoCore communication is not needed by + userspace but only by other kernel modules (like bcm2835-audio). If not + sure, set this to 'Y'. + +endif + +source "drivers/staging/vc04_services/bcm2835-audio/Kconfig" + +source "drivers/staging/vc04_services/bcm2835-camera/Kconfig" + +source "drivers/staging/vc04_services/vchiq-mmal/Kconfig" + +endif + diff --git a/drivers/staging/vc04_services/Makefile b/drivers/staging/vc04_services/Makefile new file mode 100644 index 000000000..1fd191e2e --- /dev/null +++ b/drivers/staging/vc04_services/Makefile @@ -0,0 +1,19 @@ +# SPDX-License-Identifier: GPL-2.0 +obj-$(CONFIG_BCM2835_VCHIQ) += vchiq.o + +vchiq-objs := \ + interface/vchiq_arm/vchiq_core.o \ + interface/vchiq_arm/vchiq_arm.o \ + interface/vchiq_arm/vchiq_debugfs.o \ + interface/vchiq_arm/vchiq_connected.o \ + +ifdef CONFIG_VCHIQ_CDEV +vchiq-objs += interface/vchiq_arm/vchiq_dev.o +endif + +obj-$(CONFIG_SND_BCM2835) += bcm2835-audio/ +obj-$(CONFIG_VIDEO_BCM2835) += bcm2835-camera/ +obj-$(CONFIG_BCM2835_VCHIQ_MMAL) += vchiq-mmal/ + +ccflags-y += -I $(srctree)/$(src)/include + diff --git a/drivers/staging/vc04_services/bcm2835-audio/Kconfig b/drivers/staging/vc04_services/bcm2835-audio/Kconfig new file mode 100644 index 000000000..7f22f6c85 --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-audio/Kconfig @@ -0,0 +1,11 @@ +# SPDX-License-Identifier: GPL-2.0 +config SND_BCM2835 + tristate "BCM2835 Audio" + depends on (ARCH_BCM2835 || COMPILE_TEST) && SND + select SND_PCM + select BCM2835_VCHIQ if HAS_DMA + help + Say Y or M if you want to support BCM2835 built in audio. + This driver handles both 3.5mm and HDMI audio, by leveraging + the VCHIQ messaging interface between the kernel and the firmware + running on VideoCore.
\ No newline at end of file diff --git a/drivers/staging/vc04_services/bcm2835-audio/Makefile b/drivers/staging/vc04_services/bcm2835-audio/Makefile new file mode 100644 index 000000000..d59fe4dde --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-audio/Makefile @@ -0,0 +1,5 @@ +# SPDX-License-Identifier: GPL-2.0 +obj-$(CONFIG_SND_BCM2835) += snd-bcm2835.o +snd-bcm2835-objs := bcm2835.o bcm2835-ctl.o bcm2835-pcm.o bcm2835-vchiq.o + +ccflags-y += -I $(srctree)/$(src)/../include -D__VCCOREVER__=0x04000000 diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-ctl.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-ctl.c new file mode 100644 index 000000000..1c1f04012 --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-ctl.c @@ -0,0 +1,232 @@ +// SPDX-License-Identifier: GPL-2.0 +/* Copyright 2011 Broadcom Corporation. All rights reserved. */ + +#include <sound/core.h> +#include <sound/control.h> +#include <sound/tlv.h> +#include <sound/asoundef.h> + +#include "bcm2835.h" + +/* volume maximum and minimum in terms of 0.01dB */ +#define CTRL_VOL_MAX 400 +#define CTRL_VOL_MIN -10239 /* originally -10240 */ + +static int bcm2835_audio_set_chip_ctls(struct bcm2835_chip *chip) +{ + int i, err = 0; + + /* change ctls for all substreams */ + for (i = 0; i < MAX_SUBSTREAMS; i++) { + if (chip->alsa_stream[i]) { + err = bcm2835_audio_set_ctls(chip->alsa_stream[i]); + if (err < 0) + break; + } + } + return err; +} + +static int snd_bcm2835_ctl_info(struct snd_kcontrol *kcontrol, + struct snd_ctl_elem_info *uinfo) +{ + if (kcontrol->private_value == PCM_PLAYBACK_VOLUME) { + uinfo->type = SNDRV_CTL_ELEM_TYPE_INTEGER; + uinfo->count = 1; + uinfo->value.integer.min = CTRL_VOL_MIN; + uinfo->value.integer.max = CTRL_VOL_MAX; /* 2303 */ + } else if (kcontrol->private_value == PCM_PLAYBACK_MUTE) { + uinfo->type = SNDRV_CTL_ELEM_TYPE_BOOLEAN; + uinfo->count = 1; + uinfo->value.integer.min = 0; + uinfo->value.integer.max = 1; + } else if (kcontrol->private_value == PCM_PLAYBACK_DEVICE) { + uinfo->type = SNDRV_CTL_ELEM_TYPE_INTEGER; + uinfo->count = 1; + uinfo->value.integer.min = 0; + uinfo->value.integer.max = AUDIO_DEST_MAX - 1; + } + return 0; +} + +static int snd_bcm2835_ctl_get(struct snd_kcontrol *kcontrol, + struct snd_ctl_elem_value *ucontrol) +{ + struct bcm2835_chip *chip = snd_kcontrol_chip(kcontrol); + + mutex_lock(&chip->audio_mutex); + + if (kcontrol->private_value == PCM_PLAYBACK_VOLUME) + ucontrol->value.integer.value[0] = chip->volume; + else if (kcontrol->private_value == PCM_PLAYBACK_MUTE) + ucontrol->value.integer.value[0] = chip->mute; + else if (kcontrol->private_value == PCM_PLAYBACK_DEVICE) + ucontrol->value.integer.value[0] = chip->dest; + + mutex_unlock(&chip->audio_mutex); + return 0; +} + +static int snd_bcm2835_ctl_put(struct snd_kcontrol *kcontrol, + struct snd_ctl_elem_value *ucontrol) +{ + struct bcm2835_chip *chip = snd_kcontrol_chip(kcontrol); + int val, *valp; + int changed = 0; + + if (kcontrol->private_value == PCM_PLAYBACK_VOLUME) + valp = &chip->volume; + else if (kcontrol->private_value == PCM_PLAYBACK_MUTE) + valp = &chip->mute; + else if (kcontrol->private_value == PCM_PLAYBACK_DEVICE) + valp = &chip->dest; + else + return -EINVAL; + + val = ucontrol->value.integer.value[0]; + mutex_lock(&chip->audio_mutex); + if (val != *valp) { + *valp = val; + changed = 1; + if (bcm2835_audio_set_chip_ctls(chip)) + dev_err(chip->card->dev, "Failed to set ALSA controls..\n"); + } + mutex_unlock(&chip->audio_mutex); + return changed; +} + +static DECLARE_TLV_DB_SCALE(snd_bcm2835_db_scale, CTRL_VOL_MIN, 1, 1); + +static const struct snd_kcontrol_new snd_bcm2835_ctl[] = { + { + .iface = SNDRV_CTL_ELEM_IFACE_MIXER, + .name = "PCM Playback Volume", + .access = SNDRV_CTL_ELEM_ACCESS_READWRITE | SNDRV_CTL_ELEM_ACCESS_TLV_READ, + .private_value = PCM_PLAYBACK_VOLUME, + .info = snd_bcm2835_ctl_info, + .get = snd_bcm2835_ctl_get, + .put = snd_bcm2835_ctl_put, + .tlv = {.p = snd_bcm2835_db_scale} + }, + { + .iface = SNDRV_CTL_ELEM_IFACE_MIXER, + .name = "PCM Playback Switch", + .access = SNDRV_CTL_ELEM_ACCESS_READWRITE, + .private_value = PCM_PLAYBACK_MUTE, + .info = snd_bcm2835_ctl_info, + .get = snd_bcm2835_ctl_get, + .put = snd_bcm2835_ctl_put, + }, +}; + +static int snd_bcm2835_spdif_default_info(struct snd_kcontrol *kcontrol, + struct snd_ctl_elem_info *uinfo) +{ + uinfo->type = SNDRV_CTL_ELEM_TYPE_IEC958; + uinfo->count = 1; + return 0; +} + +static int snd_bcm2835_spdif_default_get(struct snd_kcontrol *kcontrol, + struct snd_ctl_elem_value *ucontrol) +{ + struct bcm2835_chip *chip = snd_kcontrol_chip(kcontrol); + int i; + + mutex_lock(&chip->audio_mutex); + + for (i = 0; i < 4; i++) + ucontrol->value.iec958.status[i] = + (chip->spdif_status >> (i * 8)) & 0xff; + + mutex_unlock(&chip->audio_mutex); + return 0; +} + +static int snd_bcm2835_spdif_default_put(struct snd_kcontrol *kcontrol, + struct snd_ctl_elem_value *ucontrol) +{ + struct bcm2835_chip *chip = snd_kcontrol_chip(kcontrol); + unsigned int val = 0; + int i, change; + + mutex_lock(&chip->audio_mutex); + + for (i = 0; i < 4; i++) + val |= (unsigned int)ucontrol->value.iec958.status[i] << (i * 8); + + change = val != chip->spdif_status; + chip->spdif_status = val; + + mutex_unlock(&chip->audio_mutex); + return change; +} + +static int snd_bcm2835_spdif_mask_info(struct snd_kcontrol *kcontrol, + struct snd_ctl_elem_info *uinfo) +{ + uinfo->type = SNDRV_CTL_ELEM_TYPE_IEC958; + uinfo->count = 1; + return 0; +} + +static int snd_bcm2835_spdif_mask_get(struct snd_kcontrol *kcontrol, + struct snd_ctl_elem_value *ucontrol) +{ + /* + * bcm2835 supports only consumer mode and sets all other format flags + * automatically. So the only thing left is signalling non-audio content + */ + ucontrol->value.iec958.status[0] = IEC958_AES0_NONAUDIO; + return 0; +} + +static const struct snd_kcontrol_new snd_bcm2835_spdif[] = { + { + .iface = SNDRV_CTL_ELEM_IFACE_PCM, + .name = SNDRV_CTL_NAME_IEC958("", PLAYBACK, DEFAULT), + .info = snd_bcm2835_spdif_default_info, + .get = snd_bcm2835_spdif_default_get, + .put = snd_bcm2835_spdif_default_put + }, + { + .access = SNDRV_CTL_ELEM_ACCESS_READ, + .iface = SNDRV_CTL_ELEM_IFACE_PCM, + .name = SNDRV_CTL_NAME_IEC958("", PLAYBACK, CON_MASK), + .info = snd_bcm2835_spdif_mask_info, + .get = snd_bcm2835_spdif_mask_get, + }, +}; + +static int create_ctls(struct bcm2835_chip *chip, size_t size, + const struct snd_kcontrol_new *kctls) +{ + int i, err; + + for (i = 0; i < size; i++) { + err = snd_ctl_add(chip->card, snd_ctl_new1(&kctls[i], chip)); + if (err < 0) + return err; + } + return 0; +} + +int snd_bcm2835_new_headphones_ctl(struct bcm2835_chip *chip) +{ + strscpy(chip->card->mixername, "Broadcom Mixer", sizeof(chip->card->mixername)); + return create_ctls(chip, ARRAY_SIZE(snd_bcm2835_ctl), + snd_bcm2835_ctl); +} + +int snd_bcm2835_new_hdmi_ctl(struct bcm2835_chip *chip) +{ + int err; + + strscpy(chip->card->mixername, "Broadcom Mixer", sizeof(chip->card->mixername)); + err = create_ctls(chip, ARRAY_SIZE(snd_bcm2835_ctl), snd_bcm2835_ctl); + if (err < 0) + return err; + return create_ctls(chip, ARRAY_SIZE(snd_bcm2835_spdif), + snd_bcm2835_spdif); +} + diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c new file mode 100644 index 000000000..68e8d491a --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c @@ -0,0 +1,355 @@ +// SPDX-License-Identifier: GPL-2.0 +/* Copyright 2011 Broadcom Corporation. All rights reserved. */ + +#include <linux/interrupt.h> +#include <linux/slab.h> + +#include <sound/asoundef.h> + +#include "bcm2835.h" + +/* hardware definition */ +static const struct snd_pcm_hardware snd_bcm2835_playback_hw = { + .info = (SNDRV_PCM_INFO_INTERLEAVED | SNDRV_PCM_INFO_BLOCK_TRANSFER | + SNDRV_PCM_INFO_MMAP | SNDRV_PCM_INFO_MMAP_VALID | + SNDRV_PCM_INFO_SYNC_APPLPTR | SNDRV_PCM_INFO_BATCH), + .formats = SNDRV_PCM_FMTBIT_U8 | SNDRV_PCM_FMTBIT_S16_LE, + .rates = SNDRV_PCM_RATE_CONTINUOUS | SNDRV_PCM_RATE_8000_192000, + .rate_min = 8000, + .rate_max = 192000, + .channels_min = 1, + .channels_max = 8, + .buffer_bytes_max = 512 * 1024, + .period_bytes_min = 1 * 1024, + .period_bytes_max = 512 * 1024, + .periods_min = 1, + .periods_max = 128, +}; + +static const struct snd_pcm_hardware snd_bcm2835_playback_spdif_hw = { + .info = (SNDRV_PCM_INFO_INTERLEAVED | SNDRV_PCM_INFO_BLOCK_TRANSFER | + SNDRV_PCM_INFO_MMAP | SNDRV_PCM_INFO_MMAP_VALID | + SNDRV_PCM_INFO_SYNC_APPLPTR | SNDRV_PCM_INFO_BATCH), + .formats = SNDRV_PCM_FMTBIT_S16_LE, + .rates = SNDRV_PCM_RATE_CONTINUOUS | SNDRV_PCM_RATE_44100 | + SNDRV_PCM_RATE_48000, + .rate_min = 44100, + .rate_max = 48000, + .channels_min = 2, + .channels_max = 2, + .buffer_bytes_max = 128 * 1024, + .period_bytes_min = 1 * 1024, + .period_bytes_max = 128 * 1024, + .periods_min = 1, + .periods_max = 128, +}; + +static void snd_bcm2835_playback_free(struct snd_pcm_runtime *runtime) +{ + kfree(runtime->private_data); +} + +void bcm2835_playback_fifo(struct bcm2835_alsa_stream *alsa_stream, + unsigned int bytes) +{ + struct snd_pcm_substream *substream = alsa_stream->substream; + unsigned int pos; + + if (!alsa_stream->period_size) + return; + + if (bytes >= alsa_stream->buffer_size) { + snd_pcm_stream_lock(substream); + snd_pcm_stop(substream, + alsa_stream->draining ? + SNDRV_PCM_STATE_SETUP : + SNDRV_PCM_STATE_XRUN); + snd_pcm_stream_unlock(substream); + return; + } + + pos = atomic_read(&alsa_stream->pos); + pos += bytes; + pos %= alsa_stream->buffer_size; + atomic_set(&alsa_stream->pos, pos); + + alsa_stream->period_offset += bytes; + alsa_stream->interpolate_start = ktime_get(); + if (alsa_stream->period_offset >= alsa_stream->period_size) { + alsa_stream->period_offset %= alsa_stream->period_size; + snd_pcm_period_elapsed(substream); + } +} + +/* open callback */ +static int snd_bcm2835_playback_open_generic(struct snd_pcm_substream *substream, int spdif) +{ + struct bcm2835_chip *chip = snd_pcm_substream_chip(substream); + struct snd_pcm_runtime *runtime = substream->runtime; + struct bcm2835_alsa_stream *alsa_stream; + int idx; + int err; + + mutex_lock(&chip->audio_mutex); + idx = substream->number; + + if (spdif && chip->opened) { + err = -EBUSY; + goto out; + } else if (!spdif && (chip->opened & (1 << idx))) { + err = -EBUSY; + goto out; + } + if (idx >= MAX_SUBSTREAMS) { + dev_err(chip->dev, + "substream(%d) device doesn't exist max(%d) substreams allowed\n", + idx, MAX_SUBSTREAMS); + err = -ENODEV; + goto out; + } + + alsa_stream = kzalloc(sizeof(*alsa_stream), GFP_KERNEL); + if (!alsa_stream) { + err = -ENOMEM; + goto out; + } + + /* Initialise alsa_stream */ + alsa_stream->chip = chip; + alsa_stream->substream = substream; + alsa_stream->idx = idx; + + err = bcm2835_audio_open(alsa_stream); + if (err) { + kfree(alsa_stream); + goto out; + } + runtime->private_data = alsa_stream; + runtime->private_free = snd_bcm2835_playback_free; + if (spdif) { + runtime->hw = snd_bcm2835_playback_spdif_hw; + } else { + /* clear spdif status, as we are not in spdif mode */ + chip->spdif_status = 0; + runtime->hw = snd_bcm2835_playback_hw; + } + /* minimum 16 bytes alignment (for vchiq bulk transfers) */ + snd_pcm_hw_constraint_step(runtime, + 0, + SNDRV_PCM_HW_PARAM_PERIOD_BYTES, + 16); + + /* position update is in 10ms order */ + snd_pcm_hw_constraint_minmax(runtime, + SNDRV_PCM_HW_PARAM_PERIOD_TIME, + 10 * 1000, UINT_MAX); + + chip->alsa_stream[idx] = alsa_stream; + + chip->opened |= (1 << idx); + +out: + mutex_unlock(&chip->audio_mutex); + + return err; +} + +static int snd_bcm2835_playback_open(struct snd_pcm_substream *substream) +{ + return snd_bcm2835_playback_open_generic(substream, 0); +} + +static int snd_bcm2835_playback_spdif_open(struct snd_pcm_substream *substream) +{ + return snd_bcm2835_playback_open_generic(substream, 1); +} + +static int snd_bcm2835_playback_close(struct snd_pcm_substream *substream) +{ + struct bcm2835_alsa_stream *alsa_stream; + struct snd_pcm_runtime *runtime; + struct bcm2835_chip *chip; + + chip = snd_pcm_substream_chip(substream); + mutex_lock(&chip->audio_mutex); + runtime = substream->runtime; + alsa_stream = runtime->private_data; + + alsa_stream->period_size = 0; + alsa_stream->buffer_size = 0; + + bcm2835_audio_close(alsa_stream); + alsa_stream->chip->alsa_stream[alsa_stream->idx] = NULL; + /* + * Do not free up alsa_stream here, it will be freed up by + * runtime->private_free callback we registered in *_open above + */ + + chip->opened &= ~(1 << substream->number); + + mutex_unlock(&chip->audio_mutex); + + return 0; +} + +static int snd_bcm2835_pcm_prepare(struct snd_pcm_substream *substream) +{ + struct bcm2835_chip *chip = snd_pcm_substream_chip(substream); + struct snd_pcm_runtime *runtime = substream->runtime; + struct bcm2835_alsa_stream *alsa_stream = runtime->private_data; + int channels; + int err; + + /* notify the vchiq that it should enter spdif passthrough mode by + * setting channels=0 (see + * https://github.com/raspberrypi/linux/issues/528) + */ + if (chip->spdif_status & IEC958_AES0_NONAUDIO) + channels = 0; + else + channels = runtime->channels; + + err = bcm2835_audio_set_params(alsa_stream, channels, + runtime->rate, + snd_pcm_format_width(runtime->format)); + if (err < 0) + return err; + + memset(&alsa_stream->pcm_indirect, 0, sizeof(alsa_stream->pcm_indirect)); + + alsa_stream->pcm_indirect.hw_buffer_size = + alsa_stream->pcm_indirect.sw_buffer_size = + snd_pcm_lib_buffer_bytes(substream); + + alsa_stream->buffer_size = snd_pcm_lib_buffer_bytes(substream); + alsa_stream->period_size = snd_pcm_lib_period_bytes(substream); + atomic_set(&alsa_stream->pos, 0); + alsa_stream->period_offset = 0; + alsa_stream->draining = false; + alsa_stream->interpolate_start = ktime_get(); + + return 0; +} + +static void snd_bcm2835_pcm_transfer(struct snd_pcm_substream *substream, + struct snd_pcm_indirect *rec, size_t bytes) +{ + struct snd_pcm_runtime *runtime = substream->runtime; + struct bcm2835_alsa_stream *alsa_stream = runtime->private_data; + void *src = (void *)(substream->runtime->dma_area + rec->sw_data); + + bcm2835_audio_write(alsa_stream, bytes, src); +} + +static int snd_bcm2835_pcm_ack(struct snd_pcm_substream *substream) +{ + struct snd_pcm_runtime *runtime = substream->runtime; + struct bcm2835_alsa_stream *alsa_stream = runtime->private_data; + struct snd_pcm_indirect *pcm_indirect = &alsa_stream->pcm_indirect; + + return snd_pcm_indirect_playback_transfer(substream, pcm_indirect, + snd_bcm2835_pcm_transfer); +} + +/* trigger callback */ +static int snd_bcm2835_pcm_trigger(struct snd_pcm_substream *substream, int cmd) +{ + struct snd_pcm_runtime *runtime = substream->runtime; + struct bcm2835_alsa_stream *alsa_stream = runtime->private_data; + + switch (cmd) { + case SNDRV_PCM_TRIGGER_START: + return bcm2835_audio_start(alsa_stream); + case SNDRV_PCM_TRIGGER_DRAIN: + alsa_stream->draining = true; + return bcm2835_audio_drain(alsa_stream); + case SNDRV_PCM_TRIGGER_STOP: + return bcm2835_audio_stop(alsa_stream); + default: + return -EINVAL; + } +} + +/* pointer callback */ +static snd_pcm_uframes_t +snd_bcm2835_pcm_pointer(struct snd_pcm_substream *substream) +{ + struct snd_pcm_runtime *runtime = substream->runtime; + struct bcm2835_alsa_stream *alsa_stream = runtime->private_data; + ktime_t now = ktime_get(); + + /* Give userspace better delay reporting by interpolating between GPU + * notifications, assuming audio speed is close enough to the clock + * used for ktime + */ + + if ((ktime_to_ns(alsa_stream->interpolate_start)) && + (ktime_compare(alsa_stream->interpolate_start, now) < 0)) { + u64 interval = + (ktime_to_ns(ktime_sub(now, + alsa_stream->interpolate_start))); + u64 frames_output_in_interval = + div_u64((interval * runtime->rate), 1000000000); + snd_pcm_sframes_t frames_output_in_interval_sized = + -frames_output_in_interval; + runtime->delay = frames_output_in_interval_sized; + } + + return snd_pcm_indirect_playback_pointer(substream, + &alsa_stream->pcm_indirect, + atomic_read(&alsa_stream->pos)); +} + +/* operators */ +static const struct snd_pcm_ops snd_bcm2835_playback_ops = { + .open = snd_bcm2835_playback_open, + .close = snd_bcm2835_playback_close, + .prepare = snd_bcm2835_pcm_prepare, + .trigger = snd_bcm2835_pcm_trigger, + .pointer = snd_bcm2835_pcm_pointer, + .ack = snd_bcm2835_pcm_ack, +}; + +static const struct snd_pcm_ops snd_bcm2835_playback_spdif_ops = { + .open = snd_bcm2835_playback_spdif_open, + .close = snd_bcm2835_playback_close, + .prepare = snd_bcm2835_pcm_prepare, + .trigger = snd_bcm2835_pcm_trigger, + .pointer = snd_bcm2835_pcm_pointer, + .ack = snd_bcm2835_pcm_ack, +}; + +/* create a pcm device */ +int snd_bcm2835_new_pcm(struct bcm2835_chip *chip, const char *name, + int idx, enum snd_bcm2835_route route, + u32 numchannels, bool spdif) +{ + struct snd_pcm *pcm; + int err; + + err = snd_pcm_new(chip->card, name, idx, numchannels, 0, &pcm); + if (err) + return err; + + pcm->private_data = chip; + pcm->nonatomic = true; + strscpy(pcm->name, name, sizeof(pcm->name)); + if (!spdif) { + chip->dest = route; + chip->volume = 0; + chip->mute = CTRL_VOL_UNMUTE; + } + + snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_PLAYBACK, + spdif ? &snd_bcm2835_playback_spdif_ops : + &snd_bcm2835_playback_ops); + + snd_pcm_set_managed_buffer_all(pcm, SNDRV_DMA_TYPE_DEV, + chip->card->dev, 128 * 1024, 128 * 1024); + + if (spdif) + chip->pcm_spdif = pcm; + else + chip->pcm = pcm; + return 0; +} diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c new file mode 100644 index 000000000..f4c2c9506 --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c @@ -0,0 +1,378 @@ +// SPDX-License-Identifier: GPL-2.0 +/* Copyright 2011 Broadcom Corporation. All rights reserved. */ + +#include <linux/slab.h> +#include <linux/module.h> +#include <linux/completion.h> +#include "bcm2835.h" +#include "vc_vchi_audioserv_defs.h" + +struct bcm2835_audio_instance { + struct device *dev; + unsigned int service_handle; + struct completion msg_avail_comp; + struct mutex vchi_mutex; /* Serialize vchiq access */ + struct bcm2835_alsa_stream *alsa_stream; + int result; + unsigned int max_packet; + short peer_version; +}; + +static bool force_bulk; +module_param(force_bulk, bool, 0444); +MODULE_PARM_DESC(force_bulk, "Force use of vchiq bulk for audio"); + +static void bcm2835_audio_lock(struct bcm2835_audio_instance *instance) +{ + mutex_lock(&instance->vchi_mutex); + vchiq_use_service(instance->alsa_stream->chip->vchi_ctx->instance, + instance->service_handle); +} + +static void bcm2835_audio_unlock(struct bcm2835_audio_instance *instance) +{ + vchiq_release_service(instance->alsa_stream->chip->vchi_ctx->instance, + instance->service_handle); + mutex_unlock(&instance->vchi_mutex); +} + +static int bcm2835_audio_send_msg_locked(struct bcm2835_audio_instance *instance, + struct vc_audio_msg *m, bool wait) +{ + int status; + + if (wait) { + instance->result = -1; + init_completion(&instance->msg_avail_comp); + } + + status = vchiq_queue_kernel_message(instance->alsa_stream->chip->vchi_ctx->instance, + instance->service_handle, m, sizeof(*m)); + if (status) { + dev_err(instance->dev, + "vchi message queue failed: %d, msg=%d\n", + status, m->type); + return -EIO; + } + + if (wait) { + if (!wait_for_completion_timeout(&instance->msg_avail_comp, + msecs_to_jiffies(10 * 1000))) { + dev_err(instance->dev, + "vchi message timeout, msg=%d\n", m->type); + return -ETIMEDOUT; + } else if (instance->result) { + dev_err(instance->dev, + "vchi message response error:%d, msg=%d\n", + instance->result, m->type); + return -EIO; + } + } + + return 0; +} + +static int bcm2835_audio_send_msg(struct bcm2835_audio_instance *instance, + struct vc_audio_msg *m, bool wait) +{ + int err; + + bcm2835_audio_lock(instance); + err = bcm2835_audio_send_msg_locked(instance, m, wait); + bcm2835_audio_unlock(instance); + return err; +} + +static int bcm2835_audio_send_simple(struct bcm2835_audio_instance *instance, + int type, bool wait) +{ + struct vc_audio_msg m = { .type = type }; + + return bcm2835_audio_send_msg(instance, &m, wait); +} + +static enum vchiq_status audio_vchi_callback(struct vchiq_instance *vchiq_instance, + enum vchiq_reason reason, + struct vchiq_header *header, + unsigned int handle, void *userdata) +{ + struct bcm2835_audio_instance *instance = vchiq_get_service_userdata(vchiq_instance, + handle); + struct vc_audio_msg *m; + + if (reason != VCHIQ_MESSAGE_AVAILABLE) + return VCHIQ_SUCCESS; + + m = (void *)header->data; + if (m->type == VC_AUDIO_MSG_TYPE_RESULT) { + instance->result = m->result.success; + complete(&instance->msg_avail_comp); + } else if (m->type == VC_AUDIO_MSG_TYPE_COMPLETE) { + if (m->complete.cookie1 != VC_AUDIO_WRITE_COOKIE1 || + m->complete.cookie2 != VC_AUDIO_WRITE_COOKIE2) + dev_err(instance->dev, "invalid cookie\n"); + else + bcm2835_playback_fifo(instance->alsa_stream, + m->complete.count); + } else { + dev_err(instance->dev, "unexpected callback type=%d\n", m->type); + } + + vchiq_release_message(vchiq_instance, instance->service_handle, header); + return VCHIQ_SUCCESS; +} + +static int +vc_vchi_audio_init(struct vchiq_instance *vchiq_instance, + struct bcm2835_audio_instance *instance) +{ + struct vchiq_service_params_kernel params = { + .version = VC_AUDIOSERV_VER, + .version_min = VC_AUDIOSERV_MIN_VER, + .fourcc = VCHIQ_MAKE_FOURCC('A', 'U', 'D', 'S'), + .callback = audio_vchi_callback, + .userdata = instance, + }; + int status; + + /* Open the VCHI service connections */ + status = vchiq_open_service(vchiq_instance, ¶ms, + &instance->service_handle); + + if (status) { + dev_err(instance->dev, + "failed to open VCHI service connection (status=%d)\n", + status); + return -EPERM; + } + + /* Finished with the service for now */ + vchiq_release_service(instance->alsa_stream->chip->vchi_ctx->instance, + instance->service_handle); + + return 0; +} + +static void vc_vchi_audio_deinit(struct bcm2835_audio_instance *instance) +{ + int status; + + mutex_lock(&instance->vchi_mutex); + vchiq_use_service(instance->alsa_stream->chip->vchi_ctx->instance, + instance->service_handle); + + /* Close all VCHI service connections */ + status = vchiq_close_service(instance->alsa_stream->chip->vchi_ctx->instance, + instance->service_handle); + if (status) { + dev_err(instance->dev, + "failed to close VCHI service connection (status=%d)\n", + status); + } + + mutex_unlock(&instance->vchi_mutex); +} + +int bcm2835_new_vchi_ctx(struct device *dev, struct bcm2835_vchi_ctx *vchi_ctx) +{ + int ret; + + /* Initialize and create a VCHI connection */ + ret = vchiq_initialise(&vchi_ctx->instance); + if (ret) { + dev_err(dev, "failed to initialise VCHI instance (ret=%d)\n", + ret); + return -EIO; + } + + ret = vchiq_connect(vchi_ctx->instance); + if (ret) { + dev_dbg(dev, "failed to connect VCHI instance (ret=%d)\n", + ret); + + kfree(vchi_ctx->instance); + vchi_ctx->instance = NULL; + + return -EIO; + } + + return 0; +} + +void bcm2835_free_vchi_ctx(struct bcm2835_vchi_ctx *vchi_ctx) +{ + /* Close the VCHI connection - it will also free vchi_ctx->instance */ + WARN_ON(vchiq_shutdown(vchi_ctx->instance)); + + vchi_ctx->instance = NULL; +} + +int bcm2835_audio_open(struct bcm2835_alsa_stream *alsa_stream) +{ + struct bcm2835_vchi_ctx *vchi_ctx = alsa_stream->chip->vchi_ctx; + struct bcm2835_audio_instance *instance; + int err; + + /* Allocate memory for this instance */ + instance = kzalloc(sizeof(*instance), GFP_KERNEL); + if (!instance) + return -ENOMEM; + mutex_init(&instance->vchi_mutex); + instance->dev = alsa_stream->chip->dev; + instance->alsa_stream = alsa_stream; + alsa_stream->instance = instance; + + err = vc_vchi_audio_init(vchi_ctx->instance, + instance); + if (err < 0) + goto free_instance; + + err = bcm2835_audio_send_simple(instance, VC_AUDIO_MSG_TYPE_OPEN, + false); + if (err < 0) + goto deinit; + + bcm2835_audio_lock(instance); + vchiq_get_peer_version(vchi_ctx->instance, instance->service_handle, + &instance->peer_version); + bcm2835_audio_unlock(instance); + if (instance->peer_version < 2 || force_bulk) + instance->max_packet = 0; /* bulk transfer */ + else + instance->max_packet = 4000; + + return 0; + + deinit: + vc_vchi_audio_deinit(instance); + free_instance: + alsa_stream->instance = NULL; + kfree(instance); + return err; +} + +int bcm2835_audio_set_ctls(struct bcm2835_alsa_stream *alsa_stream) +{ + struct bcm2835_chip *chip = alsa_stream->chip; + struct vc_audio_msg m = {}; + + m.type = VC_AUDIO_MSG_TYPE_CONTROL; + m.control.dest = chip->dest; + if (!chip->mute) + m.control.volume = CHIP_MIN_VOLUME; + else + m.control.volume = alsa2chip(chip->volume); + + return bcm2835_audio_send_msg(alsa_stream->instance, &m, true); +} + +int bcm2835_audio_set_params(struct bcm2835_alsa_stream *alsa_stream, + unsigned int channels, unsigned int samplerate, + unsigned int bps) +{ + struct vc_audio_msg m = { + .type = VC_AUDIO_MSG_TYPE_CONFIG, + .config.channels = channels, + .config.samplerate = samplerate, + .config.bps = bps, + }; + int err; + + /* resend ctls - alsa_stream may not have been open when first send */ + err = bcm2835_audio_set_ctls(alsa_stream); + if (err) + return err; + + return bcm2835_audio_send_msg(alsa_stream->instance, &m, true); +} + +int bcm2835_audio_start(struct bcm2835_alsa_stream *alsa_stream) +{ + return bcm2835_audio_send_simple(alsa_stream->instance, + VC_AUDIO_MSG_TYPE_START, false); +} + +int bcm2835_audio_stop(struct bcm2835_alsa_stream *alsa_stream) +{ + return bcm2835_audio_send_simple(alsa_stream->instance, + VC_AUDIO_MSG_TYPE_STOP, false); +} + +/* FIXME: this doesn't seem working as expected for "draining" */ +int bcm2835_audio_drain(struct bcm2835_alsa_stream *alsa_stream) +{ + struct vc_audio_msg m = { + .type = VC_AUDIO_MSG_TYPE_STOP, + .stop.draining = 1, + }; + + return bcm2835_audio_send_msg(alsa_stream->instance, &m, false); +} + +int bcm2835_audio_close(struct bcm2835_alsa_stream *alsa_stream) +{ + struct bcm2835_audio_instance *instance = alsa_stream->instance; + int err; + + err = bcm2835_audio_send_simple(alsa_stream->instance, + VC_AUDIO_MSG_TYPE_CLOSE, true); + + /* Stop the audio service */ + vc_vchi_audio_deinit(instance); + alsa_stream->instance = NULL; + kfree(instance); + + return err; +} + +int bcm2835_audio_write(struct bcm2835_alsa_stream *alsa_stream, + unsigned int size, void *src) +{ + struct bcm2835_audio_instance *instance = alsa_stream->instance; + struct bcm2835_vchi_ctx *vchi_ctx = alsa_stream->chip->vchi_ctx; + struct vchiq_instance *vchiq_instance = vchi_ctx->instance; + struct vc_audio_msg m = { + .type = VC_AUDIO_MSG_TYPE_WRITE, + .write.count = size, + .write.max_packet = instance->max_packet, + .write.cookie1 = VC_AUDIO_WRITE_COOKIE1, + .write.cookie2 = VC_AUDIO_WRITE_COOKIE2, + }; + unsigned int count; + int err, status; + + if (!size) + return 0; + + bcm2835_audio_lock(instance); + err = bcm2835_audio_send_msg_locked(instance, &m, false); + if (err < 0) + goto unlock; + + count = size; + if (!instance->max_packet) { + /* Send the message to the videocore */ + status = vchiq_bulk_transmit(vchiq_instance, instance->service_handle, src, count, + NULL, VCHIQ_BULK_MODE_BLOCKING); + } else { + while (count > 0) { + int bytes = min(instance->max_packet, count); + + status = vchiq_queue_kernel_message(vchiq_instance, + instance->service_handle, src, bytes); + src += bytes; + count -= bytes; + } + } + + if (status) { + dev_err(instance->dev, + "failed on %d bytes transfer (status=%d)\n", + size, status); + err = -EIO; + } + + unlock: + bcm2835_audio_unlock(instance); + return err; +} diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c new file mode 100644 index 000000000..00bc898b0 --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c @@ -0,0 +1,323 @@ +// SPDX-License-Identifier: GPL-2.0 +/* Copyright 2011 Broadcom Corporation. All rights reserved. */ + +#include <linux/platform_device.h> + +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/module.h> + +#include "bcm2835.h" + +static bool enable_hdmi; +static bool enable_headphones = true; +static int num_channels = MAX_SUBSTREAMS; + +module_param(enable_hdmi, bool, 0444); +MODULE_PARM_DESC(enable_hdmi, "Enables HDMI virtual audio device"); +module_param(enable_headphones, bool, 0444); +MODULE_PARM_DESC(enable_headphones, "Enables Headphones virtual audio device"); +module_param(num_channels, int, 0644); +MODULE_PARM_DESC(num_channels, "Number of audio channels (default: 8)"); + +static void bcm2835_devm_free_vchi_ctx(struct device *dev, void *res) +{ + struct bcm2835_vchi_ctx *vchi_ctx = res; + + bcm2835_free_vchi_ctx(vchi_ctx); +} + +static int bcm2835_devm_add_vchi_ctx(struct device *dev) +{ + struct bcm2835_vchi_ctx *vchi_ctx; + int ret; + + vchi_ctx = devres_alloc(bcm2835_devm_free_vchi_ctx, sizeof(*vchi_ctx), + GFP_KERNEL); + if (!vchi_ctx) + return -ENOMEM; + + ret = bcm2835_new_vchi_ctx(dev, vchi_ctx); + if (ret) { + devres_free(vchi_ctx); + return ret; + } + + devres_add(dev, vchi_ctx); + + return 0; +} + +struct bcm2835_audio_driver { + struct device_driver driver; + const char *shortname; + const char *longname; + int minchannels; + int (*newpcm)(struct bcm2835_chip *chip, const char *name, + enum snd_bcm2835_route route, u32 numchannels); + int (*newctl)(struct bcm2835_chip *chip); + enum snd_bcm2835_route route; +}; + +static int bcm2835_audio_dual_newpcm(struct bcm2835_chip *chip, + const char *name, + enum snd_bcm2835_route route, + u32 numchannels) +{ + int err; + + err = snd_bcm2835_new_pcm(chip, name, 0, route, + numchannels, false); + + if (err) + return err; + + err = snd_bcm2835_new_pcm(chip, "IEC958", 1, route, 1, true); + if (err) + return err; + + return 0; +} + +static int bcm2835_audio_simple_newpcm(struct bcm2835_chip *chip, + const char *name, + enum snd_bcm2835_route route, + u32 numchannels) +{ + return snd_bcm2835_new_pcm(chip, name, 0, route, numchannels, false); +} + +static struct bcm2835_audio_driver bcm2835_audio_hdmi = { + .driver = { + .name = "bcm2835_hdmi", + .owner = THIS_MODULE, + }, + .shortname = "bcm2835 HDMI", + .longname = "bcm2835 HDMI", + .minchannels = 1, + .newpcm = bcm2835_audio_dual_newpcm, + .newctl = snd_bcm2835_new_hdmi_ctl, + .route = AUDIO_DEST_HDMI +}; + +static struct bcm2835_audio_driver bcm2835_audio_headphones = { + .driver = { + .name = "bcm2835_headphones", + .owner = THIS_MODULE, + }, + .shortname = "bcm2835 Headphones", + .longname = "bcm2835 Headphones", + .minchannels = 1, + .newpcm = bcm2835_audio_simple_newpcm, + .newctl = snd_bcm2835_new_headphones_ctl, + .route = AUDIO_DEST_HEADPHONES +}; + +struct bcm2835_audio_drivers { + struct bcm2835_audio_driver *audio_driver; + const bool *is_enabled; +}; + +static struct bcm2835_audio_drivers children_devices[] = { + { + .audio_driver = &bcm2835_audio_hdmi, + .is_enabled = &enable_hdmi, + }, + { + .audio_driver = &bcm2835_audio_headphones, + .is_enabled = &enable_headphones, + }, +}; + +static void bcm2835_card_free(void *data) +{ + snd_card_free(data); +} + +static int snd_add_child_device(struct device *dev, + struct bcm2835_audio_driver *audio_driver, + u32 numchans) +{ + struct bcm2835_chip *chip; + struct snd_card *card; + int err; + + err = snd_card_new(dev, -1, NULL, THIS_MODULE, sizeof(*chip), &card); + if (err < 0) { + dev_err(dev, "Failed to create card"); + return err; + } + + chip = card->private_data; + chip->card = card; + chip->dev = dev; + mutex_init(&chip->audio_mutex); + + chip->vchi_ctx = devres_find(dev, + bcm2835_devm_free_vchi_ctx, NULL, NULL); + if (!chip->vchi_ctx) { + err = -ENODEV; + goto error; + } + + strscpy(card->driver, audio_driver->driver.name, sizeof(card->driver)); + strscpy(card->shortname, audio_driver->shortname, sizeof(card->shortname)); + strscpy(card->longname, audio_driver->longname, sizeof(card->longname)); + + err = audio_driver->newpcm(chip, audio_driver->shortname, + audio_driver->route, + numchans); + if (err) { + dev_err(dev, "Failed to create pcm, error %d\n", err); + goto error; + } + + err = audio_driver->newctl(chip); + if (err) { + dev_err(dev, "Failed to create controls, error %d\n", err); + goto error; + } + + err = snd_card_register(card); + if (err) { + dev_err(dev, "Failed to register card, error %d\n", err); + goto error; + } + + dev_set_drvdata(dev, chip); + + err = devm_add_action(dev, bcm2835_card_free, card); + if (err < 0) { + dev_err(dev, "Failed to add devm action, err %d\n", err); + goto error; + } + + dev_info(dev, "card created with %d channels\n", numchans); + return 0; + + error: + snd_card_free(card); + return err; +} + +static int snd_add_child_devices(struct device *device, u32 numchans) +{ + int extrachannels_per_driver = 0; + int extrachannels_remainder = 0; + int count_devices = 0; + int extrachannels = 0; + int minchannels = 0; + int i; + + for (i = 0; i < ARRAY_SIZE(children_devices); i++) + if (*children_devices[i].is_enabled) + count_devices++; + + if (!count_devices) + return 0; + + for (i = 0; i < ARRAY_SIZE(children_devices); i++) + if (*children_devices[i].is_enabled) + minchannels += + children_devices[i].audio_driver->minchannels; + + if (minchannels < numchans) { + extrachannels = numchans - minchannels; + extrachannels_per_driver = extrachannels / count_devices; + extrachannels_remainder = extrachannels % count_devices; + } + + dev_dbg(device, "minchannels %d\n", minchannels); + dev_dbg(device, "extrachannels %d\n", extrachannels); + dev_dbg(device, "extrachannels_per_driver %d\n", + extrachannels_per_driver); + dev_dbg(device, "extrachannels_remainder %d\n", + extrachannels_remainder); + + for (i = 0; i < ARRAY_SIZE(children_devices); i++) { + struct bcm2835_audio_driver *audio_driver; + int numchannels_this_device; + int err; + + if (!*children_devices[i].is_enabled) + continue; + + audio_driver = children_devices[i].audio_driver; + + if (audio_driver->minchannels > numchans) { + dev_err(device, + "Out of channels, needed %d but only %d left\n", + audio_driver->minchannels, + numchans); + continue; + } + + numchannels_this_device = + audio_driver->minchannels + extrachannels_per_driver + + extrachannels_remainder; + extrachannels_remainder = 0; + + numchans -= numchannels_this_device; + + err = snd_add_child_device(device, audio_driver, + numchannels_this_device); + if (err) + return err; + } + + return 0; +} + +static int snd_bcm2835_alsa_probe(struct platform_device *pdev) +{ + struct device *dev = &pdev->dev; + int err; + + if (num_channels <= 0 || num_channels > MAX_SUBSTREAMS) { + num_channels = MAX_SUBSTREAMS; + dev_warn(dev, "Illegal num_channels value, will use %u\n", + num_channels); + } + + err = bcm2835_devm_add_vchi_ctx(dev); + if (err) + return err; + + err = snd_add_child_devices(dev, num_channels); + if (err) + return err; + + return 0; +} + +#ifdef CONFIG_PM + +static int snd_bcm2835_alsa_suspend(struct platform_device *pdev, + pm_message_t state) +{ + return 0; +} + +static int snd_bcm2835_alsa_resume(struct platform_device *pdev) +{ + return 0; +} + +#endif + +static struct platform_driver bcm2835_alsa_driver = { + .probe = snd_bcm2835_alsa_probe, +#ifdef CONFIG_PM + .suspend = snd_bcm2835_alsa_suspend, + .resume = snd_bcm2835_alsa_resume, +#endif + .driver = { + .name = "bcm2835_audio", + }, +}; +module_platform_driver(bcm2835_alsa_driver); + +MODULE_AUTHOR("Dom Cobley"); +MODULE_DESCRIPTION("Alsa driver for BCM2835 chip"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("platform:bcm2835_audio"); diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h new file mode 100644 index 000000000..38b7451d7 --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h @@ -0,0 +1,113 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* Copyright 2011 Broadcom Corporation. All rights reserved. */ + +#ifndef __SOUND_ARM_BCM2835_H +#define __SOUND_ARM_BCM2835_H + +#include <linux/device.h> +#include <linux/wait.h> +#include <linux/raspberrypi/vchiq.h> +#include <sound/core.h> +#include <sound/pcm.h> +#include <sound/pcm-indirect.h> + +#define MAX_SUBSTREAMS (8) +#define AVAIL_SUBSTREAMS_MASK (0xff) + +enum { + CTRL_VOL_MUTE, + CTRL_VOL_UNMUTE +}; + +/* macros for alsa2chip and chip2alsa, instead of functions */ + +// convert alsa to chip volume (defined as macro rather than function call) +#define alsa2chip(vol) ((uint)(-(((vol) << 8) / 100))) + +// convert chip to alsa volume +#define chip2alsa(vol) -(((vol) * 100) >> 8) + +#define CHIP_MIN_VOLUME 26214 /* minimum level aka mute */ + +/* Some constants for values .. */ +enum snd_bcm2835_route { + AUDIO_DEST_AUTO = 0, + AUDIO_DEST_HEADPHONES = 1, + AUDIO_DEST_HDMI = 2, + AUDIO_DEST_MAX, +}; + +enum snd_bcm2835_ctrl { + PCM_PLAYBACK_VOLUME, + PCM_PLAYBACK_MUTE, + PCM_PLAYBACK_DEVICE, +}; + +struct bcm2835_vchi_ctx { + struct vchiq_instance *instance; +}; + +/* definition of the chip-specific record */ +struct bcm2835_chip { + struct snd_card *card; + struct snd_pcm *pcm; + struct snd_pcm *pcm_spdif; + struct device *dev; + struct bcm2835_alsa_stream *alsa_stream[MAX_SUBSTREAMS]; + + int volume; + int dest; + int mute; + + unsigned int opened; + unsigned int spdif_status; + struct mutex audio_mutex; /* Serialize chip data access */ + + struct bcm2835_vchi_ctx *vchi_ctx; +}; + +struct bcm2835_alsa_stream { + struct bcm2835_chip *chip; + struct snd_pcm_substream *substream; + struct snd_pcm_indirect pcm_indirect; + + int draining; + + atomic_t pos; + unsigned int period_offset; + unsigned int buffer_size; + unsigned int period_size; + ktime_t interpolate_start; + + struct bcm2835_audio_instance *instance; + int idx; +}; + +int snd_bcm2835_new_ctl(struct bcm2835_chip *chip); +int snd_bcm2835_new_pcm(struct bcm2835_chip *chip, const char *name, + int idx, enum snd_bcm2835_route route, + u32 numchannels, bool spdif); + +int snd_bcm2835_new_hdmi_ctl(struct bcm2835_chip *chip); +int snd_bcm2835_new_headphones_ctl(struct bcm2835_chip *chip); + +int bcm2835_new_vchi_ctx(struct device *dev, struct bcm2835_vchi_ctx *vchi_ctx); +void bcm2835_free_vchi_ctx(struct bcm2835_vchi_ctx *vchi_ctx); + +int bcm2835_audio_open(struct bcm2835_alsa_stream *alsa_stream); +int bcm2835_audio_close(struct bcm2835_alsa_stream *alsa_stream); +int bcm2835_audio_set_params(struct bcm2835_alsa_stream *alsa_stream, + unsigned int channels, unsigned int samplerate, + unsigned int bps); +int bcm2835_audio_start(struct bcm2835_alsa_stream *alsa_stream); +int bcm2835_audio_stop(struct bcm2835_alsa_stream *alsa_stream); +int bcm2835_audio_drain(struct bcm2835_alsa_stream *alsa_stream); +int bcm2835_audio_set_ctls(struct bcm2835_alsa_stream *alsa_stream); +int bcm2835_audio_write(struct bcm2835_alsa_stream *alsa_stream, + unsigned int count, + void *src); +void bcm2835_playback_fifo(struct bcm2835_alsa_stream *alsa_stream, + unsigned int size); +unsigned int bcm2835_audio_retrieve_buffers(struct bcm2835_alsa_stream *alsa_stream); + +#endif /* __SOUND_ARM_BCM2835_H */ diff --git a/drivers/staging/vc04_services/bcm2835-audio/vc_vchi_audioserv_defs.h b/drivers/staging/vc04_services/bcm2835-audio/vc_vchi_audioserv_defs.h new file mode 100644 index 000000000..b4fa239c5 --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-audio/vc_vchi_audioserv_defs.h @@ -0,0 +1,98 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* Copyright 2011 Broadcom Corporation. All rights reserved. */ + +#ifndef _VC_AUDIO_DEFS_H_ +#define _VC_AUDIO_DEFS_H_ + +#define VC_AUDIOSERV_MIN_VER 1 +#define VC_AUDIOSERV_VER 2 + +/* FourCC codes used for VCHI communication */ +#define VC_AUDIO_WRITE_COOKIE1 VCHIQ_MAKE_FOURCC('B', 'C', 'M', 'A') +#define VC_AUDIO_WRITE_COOKIE2 VCHIQ_MAKE_FOURCC('D', 'A', 'T', 'A') + +/* + * List of screens that are currently supported + * All message types supported for HOST->VC direction + */ + +enum vc_audio_msg_type { + VC_AUDIO_MSG_TYPE_RESULT, // Generic result + VC_AUDIO_MSG_TYPE_COMPLETE, // Generic result + VC_AUDIO_MSG_TYPE_CONFIG, // Configure audio + VC_AUDIO_MSG_TYPE_CONTROL, // Configure audio + VC_AUDIO_MSG_TYPE_OPEN, // Configure audio + VC_AUDIO_MSG_TYPE_CLOSE, // Configure audio + VC_AUDIO_MSG_TYPE_START, // Configure audio + VC_AUDIO_MSG_TYPE_STOP, // Configure audio + VC_AUDIO_MSG_TYPE_WRITE, // Configure audio + VC_AUDIO_MSG_TYPE_MAX +}; + +/* configure the audio */ + +struct vc_audio_config { + u32 channels; + u32 samplerate; + u32 bps; +}; + +struct vc_audio_control { + u32 volume; + u32 dest; +}; + +struct vc_audio_open { + u32 dummy; +}; + +struct vc_audio_close { + u32 dummy; +}; + +struct vc_audio_start { + u32 dummy; +}; + +struct vc_audio_stop { + u32 draining; +}; + +/* configure the write audio samples */ +struct vc_audio_write { + u32 count; // in bytes + u32 cookie1; + u32 cookie2; + s16 silence; + s16 max_packet; +}; + +/* Generic result for a request (VC->HOST) */ +struct vc_audio_result { + s32 success; // Success value +}; + +/* Generic result for a request (VC->HOST) */ +struct vc_audio_complete { + s32 count; // Success value + u32 cookie1; + u32 cookie2; +}; + +/* Message header for all messages in HOST->VC direction */ +struct vc_audio_msg { + s32 type; /* Message type (VC_AUDIO_MSG_TYPE) */ + union { + struct vc_audio_config config; + struct vc_audio_control control; + struct vc_audio_open open; + struct vc_audio_close close; + struct vc_audio_start start; + struct vc_audio_stop stop; + struct vc_audio_write write; + struct vc_audio_result result; + struct vc_audio_complete complete; + }; +}; + +#endif /* _VC_AUDIO_DEFS_H_ */ diff --git a/drivers/staging/vc04_services/bcm2835-camera/Kconfig b/drivers/staging/vc04_services/bcm2835-camera/Kconfig new file mode 100644 index 000000000..870c9afb2 --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-camera/Kconfig @@ -0,0 +1,13 @@ +# SPDX-License-Identifier: GPL-2.0 +config VIDEO_BCM2835 + tristate "BCM2835 Camera" + depends on MEDIA_SUPPORT + depends on VIDEO_DEV && (ARCH_BCM2835 || COMPILE_TEST) + select BCM2835_VCHIQ if HAS_DMA + select BCM2835_VCHIQ_MMAL if HAS_DMA + select VIDEOBUF2_VMALLOC + select BTREE + help + Say Y here to enable camera host interface devices for + Broadcom BCM2835 SoC. This operates over the VCHIQ interface + to a service running on VideoCore. diff --git a/drivers/staging/vc04_services/bcm2835-camera/Makefile b/drivers/staging/vc04_services/bcm2835-camera/Makefile new file mode 100644 index 000000000..3a76d6ade --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-camera/Makefile @@ -0,0 +1,11 @@ +# SPDX-License-Identifier: GPL-2.0 +bcm2835-v4l2-$(CONFIG_VIDEO_BCM2835) := \ + bcm2835-camera.o \ + controls.o + +obj-$(CONFIG_VIDEO_BCM2835) += bcm2835-v4l2.o + +ccflags-y += \ + -I $(srctree)/$(src)/.. \ + -I $(srctree)/$(src)/../vchiq-mmal/ \ + -D__VCCOREVER__=0x04000000 diff --git a/drivers/staging/vc04_services/bcm2835-camera/TODO b/drivers/staging/vc04_services/bcm2835-camera/TODO new file mode 100644 index 000000000..6c2b4ffe4 --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-camera/TODO @@ -0,0 +1,17 @@ +1) Support dma-buf memory management. + +In order to zero-copy import camera images into the 3D or display +pipelines, we need to export our buffers through dma-buf so that the +vc4 driver can import them. This may involve bringing in the VCSM +driver (which allows long-term management of regions of memory in the +space that the VPU reserved and Linux otherwise doesn't have access +to), or building some new protocol that allows VCSM-style management +of Linux's CMA memory. + +2) Avoid extra copies for padding of images. + +We expose V4L2_PIX_FMT_* formats that have a specified stride/height +padding in the V4L2 spec, but that padding doesn't match what the +hardware can do. If we exposed the native padding requirements +through the V4L2 "multiplanar" formats, the firmware would have one +less copy it needed to do. diff --git a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c new file mode 100644 index 000000000..fd456d1f7 --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c @@ -0,0 +1,2006 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Broadcom BCM2835 V4L2 driver + * + * Copyright © 2013 Raspberry Pi (Trading) Ltd. + * + * Authors: Vincent Sanders @ Collabora + * Dave Stevenson @ Broadcom + * (now dave.stevenson@raspberrypi.org) + * Simon Mellor @ Broadcom + * Luke Diamand @ Broadcom + */ + +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/slab.h> +#include <media/videobuf2-vmalloc.h> +#include <media/videobuf2-dma-contig.h> +#include <media/v4l2-device.h> +#include <media/v4l2-ioctl.h> +#include <media/v4l2-ctrls.h> +#include <media/v4l2-fh.h> +#include <media/v4l2-event.h> +#include <media/v4l2-common.h> +#include <linux/delay.h> +#include <linux/platform_device.h> + +#include "mmal-common.h" +#include "mmal-encodings.h" +#include "mmal-vchiq.h" +#include "mmal-msg.h" +#include "mmal-parameters.h" +#include "bcm2835-camera.h" + +#define MIN_WIDTH 32 +#define MIN_HEIGHT 32 +#define MIN_BUFFER_SIZE (80 * 1024) + +#define MAX_VIDEO_MODE_WIDTH 1280 +#define MAX_VIDEO_MODE_HEIGHT 720 + +#define MAX_BCM2835_CAMERAS 2 + +int bcm2835_v4l2_debug; +module_param_named(debug, bcm2835_v4l2_debug, int, 0644); +MODULE_PARM_DESC(bcm2835_v4l2_debug, "Debug level 0-2"); + +#define UNSET (-1) +static int video_nr[] = {[0 ... (MAX_BCM2835_CAMERAS - 1)] = UNSET }; +module_param_array(video_nr, int, NULL, 0644); +MODULE_PARM_DESC(video_nr, "videoX start numbers, -1 is autodetect"); + +static int max_video_width = MAX_VIDEO_MODE_WIDTH; +static int max_video_height = MAX_VIDEO_MODE_HEIGHT; +module_param(max_video_width, int, 0644); +MODULE_PARM_DESC(max_video_width, "Threshold for video mode"); +module_param(max_video_height, int, 0644); +MODULE_PARM_DESC(max_video_height, "Threshold for video mode"); + +/* camera instance counter */ +static atomic_t camera_instance = ATOMIC_INIT(0); + +/* global device data array */ +static struct bcm2835_mmal_dev *gdev[MAX_BCM2835_CAMERAS]; + +#define FPS_MIN 1 +#define FPS_MAX 90 + +/* timeperframe: min/max and default */ +static const struct v4l2_fract + tpf_min = {.numerator = 1, .denominator = FPS_MAX}, + tpf_max = {.numerator = 1, .denominator = FPS_MIN}, + tpf_default = {.numerator = 1000, .denominator = 30000}; + +/* Container for MMAL and VB2 buffers*/ +struct vb2_mmal_buffer { + struct vb2_v4l2_buffer vb; + struct mmal_buffer mmal; +}; + +/* video formats */ +static struct mmal_fmt formats[] = { + { + .fourcc = V4L2_PIX_FMT_YUV420, + .mmal = MMAL_ENCODING_I420, + .depth = 12, + .mmal_component = COMP_CAMERA, + .ybbp = 1, + .remove_padding = 1, + }, { + .fourcc = V4L2_PIX_FMT_YUYV, + .mmal = MMAL_ENCODING_YUYV, + .depth = 16, + .mmal_component = COMP_CAMERA, + .ybbp = 2, + .remove_padding = 0, + }, { + .fourcc = V4L2_PIX_FMT_RGB24, + .mmal = MMAL_ENCODING_RGB24, + .depth = 24, + .mmal_component = COMP_CAMERA, + .ybbp = 3, + .remove_padding = 0, + }, { + .fourcc = V4L2_PIX_FMT_JPEG, + .flags = V4L2_FMT_FLAG_COMPRESSED, + .mmal = MMAL_ENCODING_JPEG, + .depth = 8, + .mmal_component = COMP_IMAGE_ENCODE, + .ybbp = 0, + .remove_padding = 0, + }, { + .fourcc = V4L2_PIX_FMT_H264, + .flags = V4L2_FMT_FLAG_COMPRESSED, + .mmal = MMAL_ENCODING_H264, + .depth = 8, + .mmal_component = COMP_VIDEO_ENCODE, + .ybbp = 0, + .remove_padding = 0, + }, { + .fourcc = V4L2_PIX_FMT_MJPEG, + .flags = V4L2_FMT_FLAG_COMPRESSED, + .mmal = MMAL_ENCODING_MJPEG, + .depth = 8, + .mmal_component = COMP_VIDEO_ENCODE, + .ybbp = 0, + .remove_padding = 0, + }, { + .fourcc = V4L2_PIX_FMT_YVYU, + .mmal = MMAL_ENCODING_YVYU, + .depth = 16, + .mmal_component = COMP_CAMERA, + .ybbp = 2, + .remove_padding = 0, + }, { + .fourcc = V4L2_PIX_FMT_VYUY, + .mmal = MMAL_ENCODING_VYUY, + .depth = 16, + .mmal_component = COMP_CAMERA, + .ybbp = 2, + .remove_padding = 0, + }, { + .fourcc = V4L2_PIX_FMT_UYVY, + .mmal = MMAL_ENCODING_UYVY, + .depth = 16, + .mmal_component = COMP_CAMERA, + .ybbp = 2, + .remove_padding = 0, + }, { + .fourcc = V4L2_PIX_FMT_NV12, + .mmal = MMAL_ENCODING_NV12, + .depth = 12, + .mmal_component = COMP_CAMERA, + .ybbp = 1, + .remove_padding = 1, + }, { + .fourcc = V4L2_PIX_FMT_BGR24, + .mmal = MMAL_ENCODING_BGR24, + .depth = 24, + .mmal_component = COMP_CAMERA, + .ybbp = 3, + .remove_padding = 0, + }, { + .fourcc = V4L2_PIX_FMT_YVU420, + .mmal = MMAL_ENCODING_YV12, + .depth = 12, + .mmal_component = COMP_CAMERA, + .ybbp = 1, + .remove_padding = 1, + }, { + .fourcc = V4L2_PIX_FMT_NV21, + .mmal = MMAL_ENCODING_NV21, + .depth = 12, + .mmal_component = COMP_CAMERA, + .ybbp = 1, + .remove_padding = 1, + }, { + .fourcc = V4L2_PIX_FMT_BGR32, + .mmal = MMAL_ENCODING_BGRA, + .depth = 32, + .mmal_component = COMP_CAMERA, + .ybbp = 4, + .remove_padding = 0, + }, +}; + +static struct mmal_fmt *get_format(struct v4l2_format *f) +{ + struct mmal_fmt *fmt; + unsigned int k; + + for (k = 0; k < ARRAY_SIZE(formats); k++) { + fmt = &formats[k]; + if (fmt->fourcc == f->fmt.pix.pixelformat) + return fmt; + } + + return NULL; +} + +/* ------------------------------------------------------------------ + * Videobuf queue operations + * ------------------------------------------------------------------ + */ + +static int queue_setup(struct vb2_queue *vq, + unsigned int *nbuffers, unsigned int *nplanes, + unsigned int sizes[], struct device *alloc_ctxs[]) +{ + struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq); + unsigned long size; + + /* refuse queue setup if port is not configured */ + if (!dev->capture.port) { + v4l2_err(&dev->v4l2_dev, + "%s: capture port not configured\n", __func__); + return -EINVAL; + } + + /* Handle CREATE_BUFS situation - *nplanes != 0 */ + if (*nplanes) { + if (*nplanes != 1 || + sizes[0] < dev->capture.port->current_buffer.size) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s: dev:%p Invalid buffer request from CREATE_BUFS, size %u < %u, nplanes %u != 1\n", + __func__, dev, sizes[0], + dev->capture.port->current_buffer.size, + *nplanes); + return -EINVAL; + } else { + return 0; + } + } + + /* Handle REQBUFS situation */ + size = dev->capture.port->current_buffer.size; + if (size == 0) { + v4l2_err(&dev->v4l2_dev, + "%s: capture port buffer size is zero\n", __func__); + return -EINVAL; + } + + if (*nbuffers < dev->capture.port->minimum_buffer.num) + *nbuffers = dev->capture.port->minimum_buffer.num; + + dev->capture.port->current_buffer.num = *nbuffers; + + *nplanes = 1; + + sizes[0] = size; + + /* + * videobuf2-vmalloc allocator is context-less so no need to set + * alloc_ctxs array. + */ + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n", + __func__, dev); + + return 0; +} + +static int buffer_init(struct vb2_buffer *vb) +{ + struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue); + struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb); + struct vb2_mmal_buffer *buf = + container_of(vb2, struct vb2_mmal_buffer, vb); + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n", + __func__, dev, vb); + buf->mmal.buffer = vb2_plane_vaddr(&buf->vb.vb2_buf, 0); + buf->mmal.buffer_size = vb2_plane_size(&buf->vb.vb2_buf, 0); + + return mmal_vchi_buffer_init(dev->instance, &buf->mmal); +} + +static int buffer_prepare(struct vb2_buffer *vb) +{ + struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue); + unsigned long size; + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n", + __func__, dev, vb); + + if (!dev->capture.port || !dev->capture.fmt) + return -ENODEV; + + size = dev->capture.stride * dev->capture.height; + if (vb2_plane_size(vb, 0) < size) { + v4l2_err(&dev->v4l2_dev, + "%s data will not fit into plane (%lu < %lu)\n", + __func__, vb2_plane_size(vb, 0), size); + return -EINVAL; + } + + return 0; +} + +static void buffer_cleanup(struct vb2_buffer *vb) +{ + struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue); + struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb); + struct vb2_mmal_buffer *buf = + container_of(vb2, struct vb2_mmal_buffer, vb); + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n", + __func__, dev, vb); + + mmal_vchi_buffer_cleanup(&buf->mmal); +} + +static inline bool is_capturing(struct bcm2835_mmal_dev *dev) +{ + return dev->capture.camera_port == + &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE]; +} + +static void buffer_cb(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + int status, + struct mmal_buffer *mmal_buf) +{ + struct bcm2835_mmal_dev *dev = port->cb_ctx; + struct vb2_mmal_buffer *buf = + container_of(mmal_buf, struct vb2_mmal_buffer, mmal); + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s: status:%d, buf:%p, length:%lu, flags %u, pts %lld\n", + __func__, status, buf, mmal_buf->length, mmal_buf->mmal_flags, + mmal_buf->pts); + + if (status) { + /* error in transfer */ + if (buf) { + /* there was a buffer with the error so return it */ + vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); + } + return; + } + + if (mmal_buf->length == 0) { + /* stream ended */ + if (dev->capture.frame_count) { + /* empty buffer whilst capturing - expected to be an + * EOS, so grab another frame + */ + if (is_capturing(dev)) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Grab another frame"); + vchiq_mmal_port_parameter_set( + instance, + dev->capture.camera_port, + MMAL_PARAMETER_CAPTURE, + &dev->capture.frame_count, + sizeof(dev->capture.frame_count)); + } + if (vchiq_mmal_submit_buffer(instance, port, + &buf->mmal)) + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Failed to return EOS buffer"); + } else { + /* stopping streaming. + * return buffer, and signal frame completion + */ + vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); + complete(&dev->capture.frame_cmplt); + } + return; + } + + if (!dev->capture.frame_count) { + /* signal frame completion */ + vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); + complete(&dev->capture.frame_cmplt); + return; + } + + if (dev->capture.vc_start_timestamp != -1 && mmal_buf->pts) { + ktime_t timestamp; + s64 runtime_us = mmal_buf->pts - + dev->capture.vc_start_timestamp; + timestamp = ktime_add_us(dev->capture.kernel_start_ts, + runtime_us); + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Convert start time %llu and %llu with offset %llu to %llu\n", + ktime_to_ns(dev->capture.kernel_start_ts), + dev->capture.vc_start_timestamp, mmal_buf->pts, + ktime_to_ns(timestamp)); + buf->vb.vb2_buf.timestamp = ktime_to_ns(timestamp); + } else { + buf->vb.vb2_buf.timestamp = ktime_get_ns(); + } + buf->vb.sequence = dev->capture.sequence++; + buf->vb.field = V4L2_FIELD_NONE; + + vb2_set_plane_payload(&buf->vb.vb2_buf, 0, mmal_buf->length); + if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_KEYFRAME) + buf->vb.flags |= V4L2_BUF_FLAG_KEYFRAME; + + vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE); + + if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS && + is_capturing(dev)) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Grab another frame as buffer has EOS"); + vchiq_mmal_port_parameter_set( + instance, + dev->capture.camera_port, + MMAL_PARAMETER_CAPTURE, + &dev->capture.frame_count, + sizeof(dev->capture.frame_count)); + } +} + +static int enable_camera(struct bcm2835_mmal_dev *dev) +{ + int ret; + + if (!dev->camera_use_count) { + ret = vchiq_mmal_port_parameter_set( + dev->instance, + &dev->component[COMP_CAMERA]->control, + MMAL_PARAMETER_CAMERA_NUM, &dev->camera_num, + sizeof(dev->camera_num)); + if (ret < 0) { + v4l2_err(&dev->v4l2_dev, + "Failed setting camera num, ret %d\n", ret); + return -EINVAL; + } + + ret = vchiq_mmal_component_enable(dev->instance, + dev->component[COMP_CAMERA]); + if (ret < 0) { + v4l2_err(&dev->v4l2_dev, + "Failed enabling camera, ret %d\n", ret); + return -EINVAL; + } + } + dev->camera_use_count++; + v4l2_dbg(1, bcm2835_v4l2_debug, + &dev->v4l2_dev, "enabled camera (refcount %d)\n", + dev->camera_use_count); + return 0; +} + +static int disable_camera(struct bcm2835_mmal_dev *dev) +{ + int ret; + + if (!dev->camera_use_count) { + v4l2_err(&dev->v4l2_dev, + "Disabled the camera when already disabled\n"); + return -EINVAL; + } + dev->camera_use_count--; + if (!dev->camera_use_count) { + unsigned int i = 0xFFFFFFFF; + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Disabling camera\n"); + ret = vchiq_mmal_component_disable(dev->instance, + dev->component[COMP_CAMERA]); + if (ret < 0) { + v4l2_err(&dev->v4l2_dev, + "Failed disabling camera, ret %d\n", ret); + return -EINVAL; + } + vchiq_mmal_port_parameter_set( + dev->instance, + &dev->component[COMP_CAMERA]->control, + MMAL_PARAMETER_CAMERA_NUM, &i, + sizeof(i)); + } + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Camera refcount now %d\n", dev->camera_use_count); + return 0; +} + +static void buffer_queue(struct vb2_buffer *vb) +{ + struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue); + struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb); + struct vb2_mmal_buffer *buf = + container_of(vb2, struct vb2_mmal_buffer, vb); + int ret; + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s: dev:%p buf:%p, idx %u\n", + __func__, dev, buf, vb2->vb2_buf.index); + + ret = vchiq_mmal_submit_buffer(dev->instance, dev->capture.port, + &buf->mmal); + if (ret < 0) + v4l2_err(&dev->v4l2_dev, "%s: error submitting buffer\n", + __func__); +} + +static int start_streaming(struct vb2_queue *vq, unsigned int count) +{ + struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq); + int ret; + u32 parameter_size; + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n", + __func__, dev); + + /* ensure a format has actually been set */ + if (!dev->capture.port) + return -EINVAL; + + if (enable_camera(dev) < 0) { + v4l2_err(&dev->v4l2_dev, "Failed to enable camera\n"); + return -EINVAL; + } + + /*init_completion(&dev->capture.frame_cmplt); */ + + /* enable frame capture */ + dev->capture.frame_count = 1; + + /* reset sequence number */ + dev->capture.sequence = 0; + + /* if the preview is not already running, wait for a few frames for AGC + * to settle down. + */ + if (!dev->component[COMP_PREVIEW]->enabled) + msleep(300); + + /* enable the connection from camera to encoder (if applicable) */ + if (dev->capture.camera_port != dev->capture.port && + dev->capture.camera_port) { + ret = vchiq_mmal_port_enable(dev->instance, + dev->capture.camera_port, NULL); + if (ret) { + v4l2_err(&dev->v4l2_dev, + "Failed to enable encode tunnel - error %d\n", + ret); + return -1; + } + } + + /* Get VC timestamp at this point in time */ + parameter_size = sizeof(dev->capture.vc_start_timestamp); + if (vchiq_mmal_port_parameter_get(dev->instance, + dev->capture.camera_port, + MMAL_PARAMETER_SYSTEM_TIME, + &dev->capture.vc_start_timestamp, + ¶meter_size)) { + v4l2_err(&dev->v4l2_dev, + "Failed to get VC start time - update your VC f/w\n"); + + /* Flag to indicate just to rely on kernel timestamps */ + dev->capture.vc_start_timestamp = -1; + } else { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Start time %lld size %d\n", + dev->capture.vc_start_timestamp, parameter_size); + } + + dev->capture.kernel_start_ts = ktime_get(); + + /* enable the camera port */ + dev->capture.port->cb_ctx = dev; + ret = vchiq_mmal_port_enable(dev->instance, dev->capture.port, + buffer_cb); + if (ret) { + v4l2_err(&dev->v4l2_dev, + "Failed to enable capture port - error %d. Disabling camera port again\n", + ret); + + vchiq_mmal_port_disable(dev->instance, + dev->capture.camera_port); + if (disable_camera(dev) < 0) { + v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n"); + return -EINVAL; + } + return -1; + } + + /* capture the first frame */ + vchiq_mmal_port_parameter_set(dev->instance, + dev->capture.camera_port, + MMAL_PARAMETER_CAPTURE, + &dev->capture.frame_count, + sizeof(dev->capture.frame_count)); + return 0; +} + +/* abort streaming and wait for last buffer */ +static void stop_streaming(struct vb2_queue *vq) +{ + int ret; + unsigned long timeout; + struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq); + struct vchiq_mmal_port *port = dev->capture.port; + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n", + __func__, dev); + + init_completion(&dev->capture.frame_cmplt); + dev->capture.frame_count = 0; + + /* ensure a format has actually been set */ + if (!port) { + v4l2_err(&dev->v4l2_dev, + "no capture port - stream not started?\n"); + return; + } + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "stopping capturing\n"); + + /* stop capturing frames */ + vchiq_mmal_port_parameter_set(dev->instance, + dev->capture.camera_port, + MMAL_PARAMETER_CAPTURE, + &dev->capture.frame_count, + sizeof(dev->capture.frame_count)); + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "disabling connection\n"); + + /* disable the connection from camera to encoder */ + ret = vchiq_mmal_port_disable(dev->instance, dev->capture.camera_port); + if (!ret && dev->capture.camera_port != port) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "disabling port\n"); + ret = vchiq_mmal_port_disable(dev->instance, port); + } else if (dev->capture.camera_port != port) { + v4l2_err(&dev->v4l2_dev, "port_disable failed, error %d\n", + ret); + } + + /* wait for all buffers to be returned */ + while (atomic_read(&port->buffers_with_vpu)) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s: Waiting for buffers to be returned - %d outstanding\n", + __func__, atomic_read(&port->buffers_with_vpu)); + timeout = wait_for_completion_timeout(&dev->capture.frame_cmplt, + HZ); + if (timeout == 0) { + v4l2_err(&dev->v4l2_dev, "%s: Timeout waiting for buffers to be returned - %d outstanding\n", + __func__, + atomic_read(&port->buffers_with_vpu)); + break; + } + } + + if (disable_camera(dev) < 0) + v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n"); +} + +static const struct vb2_ops bcm2835_mmal_video_qops = { + .queue_setup = queue_setup, + .buf_init = buffer_init, + .buf_prepare = buffer_prepare, + .buf_cleanup = buffer_cleanup, + .buf_queue = buffer_queue, + .start_streaming = start_streaming, + .stop_streaming = stop_streaming, + .wait_prepare = vb2_ops_wait_prepare, + .wait_finish = vb2_ops_wait_finish, +}; + +/* ------------------------------------------------------------------ + * IOCTL operations + * ------------------------------------------------------------------ + */ + +static int set_overlay_params(struct bcm2835_mmal_dev *dev, + struct vchiq_mmal_port *port) +{ + struct mmal_parameter_displayregion prev_config = { + .set = MMAL_DISPLAY_SET_LAYER | + MMAL_DISPLAY_SET_ALPHA | + MMAL_DISPLAY_SET_DEST_RECT | + MMAL_DISPLAY_SET_FULLSCREEN, + .layer = 2, + .alpha = dev->overlay.global_alpha, + .fullscreen = 0, + .dest_rect = { + .x = dev->overlay.w.left, + .y = dev->overlay.w.top, + .width = dev->overlay.w.width, + .height = dev->overlay.w.height, + }, + }; + return vchiq_mmal_port_parameter_set(dev->instance, port, + MMAL_PARAMETER_DISPLAYREGION, + &prev_config, sizeof(prev_config)); +} + +/* overlay ioctl */ +static int vidioc_enum_fmt_vid_overlay(struct file *file, void *priv, + struct v4l2_fmtdesc *f) +{ + struct mmal_fmt *fmt; + + if (f->index >= ARRAY_SIZE(formats)) + return -EINVAL; + + fmt = &formats[f->index]; + + f->pixelformat = fmt->fourcc; + + return 0; +} + +static int vidioc_g_fmt_vid_overlay(struct file *file, void *priv, + struct v4l2_format *f) +{ + struct bcm2835_mmal_dev *dev = video_drvdata(file); + + f->fmt.win = dev->overlay; + + return 0; +} + +static int vidioc_try_fmt_vid_overlay(struct file *file, void *priv, + struct v4l2_format *f) +{ + struct bcm2835_mmal_dev *dev = video_drvdata(file); + + f->fmt.win.field = V4L2_FIELD_NONE; + f->fmt.win.chromakey = 0; + f->fmt.win.clips = NULL; + f->fmt.win.clipcount = 0; + f->fmt.win.bitmap = NULL; + + v4l_bound_align_image(&f->fmt.win.w.width, MIN_WIDTH, dev->max_width, 1, + &f->fmt.win.w.height, MIN_HEIGHT, dev->max_height, + 1, 0); + v4l_bound_align_image(&f->fmt.win.w.left, MIN_WIDTH, dev->max_width, 1, + &f->fmt.win.w.top, MIN_HEIGHT, dev->max_height, + 1, 0); + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Overlay: Now w/h %dx%d l/t %dx%d\n", + f->fmt.win.w.width, f->fmt.win.w.height, + f->fmt.win.w.left, f->fmt.win.w.top); + + v4l2_dump_win_format(1, + bcm2835_v4l2_debug, + &dev->v4l2_dev, + &f->fmt.win, + __func__); + return 0; +} + +static int vidioc_s_fmt_vid_overlay(struct file *file, void *priv, + struct v4l2_format *f) +{ + struct bcm2835_mmal_dev *dev = video_drvdata(file); + + vidioc_try_fmt_vid_overlay(file, priv, f); + + dev->overlay = f->fmt.win; + if (dev->component[COMP_PREVIEW]->enabled) { + set_overlay_params(dev, + &dev->component[COMP_PREVIEW]->input[0]); + } + + return 0; +} + +static int vidioc_overlay(struct file *file, void *f, unsigned int on) +{ + int ret; + struct bcm2835_mmal_dev *dev = video_drvdata(file); + struct vchiq_mmal_port *src; + struct vchiq_mmal_port *dst; + + if ((on && dev->component[COMP_PREVIEW]->enabled) || + (!on && !dev->component[COMP_PREVIEW]->enabled)) + return 0; /* already in requested state */ + + src = &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW]; + + if (!on) { + /* disconnect preview ports and disable component */ + ret = vchiq_mmal_port_disable(dev->instance, src); + if (!ret) + ret = vchiq_mmal_port_connect_tunnel(dev->instance, src, + NULL); + if (ret >= 0) + ret = vchiq_mmal_component_disable( + dev->instance, + dev->component[COMP_PREVIEW]); + + disable_camera(dev); + return ret; + } + + /* set preview port format and connect it to output */ + dst = &dev->component[COMP_PREVIEW]->input[0]; + + ret = vchiq_mmal_port_set_format(dev->instance, src); + if (ret < 0) + return ret; + + ret = set_overlay_params(dev, dst); + if (ret < 0) + return ret; + + if (enable_camera(dev) < 0) + return -EINVAL; + + ret = vchiq_mmal_component_enable(dev->instance, + dev->component[COMP_PREVIEW]); + if (ret < 0) + return ret; + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "connecting %p to %p\n", + src, dst); + ret = vchiq_mmal_port_connect_tunnel(dev->instance, src, dst); + if (ret) + return ret; + + return vchiq_mmal_port_enable(dev->instance, src, NULL); +} + +static int vidioc_g_fbuf(struct file *file, void *fh, + struct v4l2_framebuffer *a) +{ + /* The video overlay must stay within the framebuffer and can't be + * positioned independently. + */ + struct bcm2835_mmal_dev *dev = video_drvdata(file); + struct vchiq_mmal_port *preview_port = + &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW]; + + a->capability = V4L2_FBUF_CAP_EXTERNOVERLAY | + V4L2_FBUF_CAP_GLOBAL_ALPHA; + a->flags = V4L2_FBUF_FLAG_OVERLAY; + a->fmt.width = preview_port->es.video.width; + a->fmt.height = preview_port->es.video.height; + a->fmt.pixelformat = V4L2_PIX_FMT_YUV420; + a->fmt.bytesperline = preview_port->es.video.width; + a->fmt.sizeimage = (preview_port->es.video.width * + preview_port->es.video.height * 3) >> 1; + a->fmt.colorspace = V4L2_COLORSPACE_SMPTE170M; + + return 0; +} + +/* input ioctls */ +static int vidioc_enum_input(struct file *file, void *priv, + struct v4l2_input *inp) +{ + /* only a single camera input */ + if (inp->index) + return -EINVAL; + + inp->type = V4L2_INPUT_TYPE_CAMERA; + sprintf((char *)inp->name, "Camera %u", inp->index); + return 0; +} + +static int vidioc_g_input(struct file *file, void *priv, unsigned int *i) +{ + *i = 0; + return 0; +} + +static int vidioc_s_input(struct file *file, void *priv, unsigned int i) +{ + if (i) + return -EINVAL; + + return 0; +} + +/* capture ioctls */ +static int vidioc_querycap(struct file *file, void *priv, + struct v4l2_capability *cap) +{ + struct bcm2835_mmal_dev *dev = video_drvdata(file); + u32 major; + u32 minor; + + vchiq_mmal_version(dev->instance, &major, &minor); + + strscpy(cap->driver, "bcm2835 mmal", sizeof(cap->driver)); + snprintf((char *)cap->card, sizeof(cap->card), "mmal service %d.%d", major, minor); + + snprintf((char *)cap->bus_info, sizeof(cap->bus_info), "platform:%s", dev->v4l2_dev.name); + return 0; +} + +static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv, + struct v4l2_fmtdesc *f) +{ + struct mmal_fmt *fmt; + + if (f->index >= ARRAY_SIZE(formats)) + return -EINVAL; + + fmt = &formats[f->index]; + + f->pixelformat = fmt->fourcc; + + return 0; +} + +static int vidioc_g_fmt_vid_cap(struct file *file, void *priv, + struct v4l2_format *f) +{ + struct bcm2835_mmal_dev *dev = video_drvdata(file); + + f->fmt.pix.width = dev->capture.width; + f->fmt.pix.height = dev->capture.height; + f->fmt.pix.field = V4L2_FIELD_NONE; + f->fmt.pix.pixelformat = dev->capture.fmt->fourcc; + f->fmt.pix.bytesperline = dev->capture.stride; + f->fmt.pix.sizeimage = dev->capture.buffersize; + + if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_RGB24) + f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB; + else if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_JPEG) + f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG; + else + f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; + f->fmt.pix.priv = 0; + + v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix, + __func__); + return 0; +} + +static int vidioc_try_fmt_vid_cap(struct file *file, void *priv, + struct v4l2_format *f) +{ + struct bcm2835_mmal_dev *dev = video_drvdata(file); + struct mmal_fmt *mfmt; + + mfmt = get_format(f); + if (!mfmt) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Fourcc format (0x%08x) unknown.\n", + f->fmt.pix.pixelformat); + f->fmt.pix.pixelformat = formats[0].fourcc; + mfmt = get_format(f); + } + + f->fmt.pix.field = V4L2_FIELD_NONE; + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Clipping/aligning %dx%d format %08X\n", + f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat); + + v4l_bound_align_image(&f->fmt.pix.width, MIN_WIDTH, dev->max_width, 1, + &f->fmt.pix.height, MIN_HEIGHT, dev->max_height, + 1, 0); + f->fmt.pix.bytesperline = f->fmt.pix.width * mfmt->ybbp; + if (!mfmt->remove_padding) { + if (mfmt->depth == 24) { + /* + * 24bpp is a pain as we can't use simple masking. + * Min stride is width aligned to 16, times 24bpp. + */ + f->fmt.pix.bytesperline = + ((f->fmt.pix.width + 15) & ~15) * 3; + } else { + /* + * GPU isn't removing padding, so stride is aligned to + * 32 + */ + int align_mask = ((32 * mfmt->depth) >> 3) - 1; + + f->fmt.pix.bytesperline = + (f->fmt.pix.bytesperline + align_mask) & + ~align_mask; + } + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Not removing padding, so bytes/line = %d\n", + f->fmt.pix.bytesperline); + } + + /* Image buffer has to be padded to allow for alignment, even though + * we sometimes then remove that padding before delivering the buffer. + */ + f->fmt.pix.sizeimage = ((f->fmt.pix.height + 15) & ~15) * + (((f->fmt.pix.width + 31) & ~31) * mfmt->depth) >> 3; + + if ((mfmt->flags & V4L2_FMT_FLAG_COMPRESSED) && + f->fmt.pix.sizeimage < MIN_BUFFER_SIZE) + f->fmt.pix.sizeimage = MIN_BUFFER_SIZE; + + if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24) + f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB; + else if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG) + f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG; + else + f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; + f->fmt.pix.priv = 0; + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Now %dx%d format %08X\n", + f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat); + + v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix, + __func__); + return 0; +} + + +static int mmal_setup_video_component(struct bcm2835_mmal_dev *dev, + struct v4l2_format *f) +{ + bool overlay_enabled = !!dev->component[COMP_PREVIEW]->enabled; + struct vchiq_mmal_port *preview_port; + int ret; + + preview_port = &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW]; + + /* Preview and encode ports need to match on resolution */ + if (overlay_enabled) { + /* Need to disable the overlay before we can update + * the resolution + */ + ret = vchiq_mmal_port_disable(dev->instance, preview_port); + if (!ret) { + ret = vchiq_mmal_port_connect_tunnel(dev->instance, + preview_port, + NULL); + } + } + preview_port->es.video.width = f->fmt.pix.width; + preview_port->es.video.height = f->fmt.pix.height; + preview_port->es.video.crop.x = 0; + preview_port->es.video.crop.y = 0; + preview_port->es.video.crop.width = f->fmt.pix.width; + preview_port->es.video.crop.height = f->fmt.pix.height; + preview_port->es.video.frame_rate.numerator = + dev->capture.timeperframe.denominator; + preview_port->es.video.frame_rate.denominator = + dev->capture.timeperframe.numerator; + ret = vchiq_mmal_port_set_format(dev->instance, preview_port); + + if (overlay_enabled) { + ret = vchiq_mmal_port_connect_tunnel(dev->instance, + preview_port, + &dev->component[COMP_PREVIEW]->input[0]); + if (ret) + return ret; + + ret = vchiq_mmal_port_enable(dev->instance, preview_port, NULL); + } + + return ret; +} + +static int mmal_setup_encode_component(struct bcm2835_mmal_dev *dev, + struct v4l2_format *f, + struct vchiq_mmal_port *port, + struct vchiq_mmal_port *camera_port, + struct vchiq_mmal_component *component) +{ + struct mmal_fmt *mfmt = get_format(f); + int ret; + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "vid_cap - set up encode comp\n"); + + /* configure buffering */ + camera_port->current_buffer.size = camera_port->recommended_buffer.size; + camera_port->current_buffer.num = camera_port->recommended_buffer.num; + + ret = vchiq_mmal_port_connect_tunnel(dev->instance, camera_port, + &component->input[0]); + if (ret) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s failed to create connection\n", __func__); + /* ensure capture is not going to be tried */ + dev->capture.port = NULL; + return ret; + } + + port->es.video.width = f->fmt.pix.width; + port->es.video.height = f->fmt.pix.height; + port->es.video.crop.x = 0; + port->es.video.crop.y = 0; + port->es.video.crop.width = f->fmt.pix.width; + port->es.video.crop.height = f->fmt.pix.height; + port->es.video.frame_rate.numerator = + dev->capture.timeperframe.denominator; + port->es.video.frame_rate.denominator = + dev->capture.timeperframe.numerator; + + port->format.encoding = mfmt->mmal; + port->format.encoding_variant = 0; + /* Set any encoding specific parameters */ + switch (mfmt->mmal_component) { + case COMP_VIDEO_ENCODE: + port->format.bitrate = dev->capture.encode_bitrate; + break; + case COMP_IMAGE_ENCODE: + /* Could set EXIF parameters here */ + break; + default: + break; + } + + ret = vchiq_mmal_port_set_format(dev->instance, port); + if (ret) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s failed to set format %dx%d fmt %08X\n", + __func__, + f->fmt.pix.width, + f->fmt.pix.height, + f->fmt.pix.pixelformat); + return ret; + } + + ret = vchiq_mmal_component_enable(dev->instance, component); + if (ret) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s Failed to enable encode components\n", __func__); + return ret; + } + + /* configure buffering */ + port->current_buffer.num = 1; + port->current_buffer.size = f->fmt.pix.sizeimage; + if (port->format.encoding == MMAL_ENCODING_JPEG) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "JPG - buf size now %d was %d\n", + f->fmt.pix.sizeimage, + port->current_buffer.size); + port->current_buffer.size = + (f->fmt.pix.sizeimage < (100 << 10)) ? + (100 << 10) : f->fmt.pix.sizeimage; + } + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "vid_cap - cur_buf.size set to %d\n", f->fmt.pix.sizeimage); + port->current_buffer.alignment = 0; + + return 0; +} + +static int mmal_setup_components(struct bcm2835_mmal_dev *dev, + struct v4l2_format *f) +{ + int ret; + struct vchiq_mmal_port *port = NULL, *camera_port = NULL; + struct vchiq_mmal_component *encode_component = NULL; + struct mmal_fmt *mfmt = get_format(f); + u32 remove_padding; + + if (!mfmt) + return -EINVAL; + + if (dev->capture.encode_component) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "vid_cap - disconnect previous tunnel\n"); + + /* Disconnect any previous connection */ + vchiq_mmal_port_connect_tunnel(dev->instance, + dev->capture.camera_port, NULL); + dev->capture.camera_port = NULL; + ret = vchiq_mmal_component_disable(dev->instance, + dev->capture.encode_component); + if (ret) + v4l2_err(&dev->v4l2_dev, + "Failed to disable encode component %d\n", + ret); + + dev->capture.encode_component = NULL; + } + /* format dependent port setup */ + switch (mfmt->mmal_component) { + case COMP_CAMERA: + /* Make a further decision on port based on resolution */ + if (f->fmt.pix.width <= max_video_width && + f->fmt.pix.height <= max_video_height) + camera_port = + &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO]; + else + camera_port = + &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE]; + port = camera_port; + break; + case COMP_IMAGE_ENCODE: + encode_component = dev->component[COMP_IMAGE_ENCODE]; + port = &dev->component[COMP_IMAGE_ENCODE]->output[0]; + camera_port = + &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE]; + break; + case COMP_VIDEO_ENCODE: + encode_component = dev->component[COMP_VIDEO_ENCODE]; + port = &dev->component[COMP_VIDEO_ENCODE]->output[0]; + camera_port = + &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO]; + break; + default: + break; + } + + if (!port) + return -EINVAL; + + if (encode_component) + camera_port->format.encoding = MMAL_ENCODING_OPAQUE; + else + camera_port->format.encoding = mfmt->mmal; + + if (dev->rgb_bgr_swapped) { + if (camera_port->format.encoding == MMAL_ENCODING_RGB24) + camera_port->format.encoding = MMAL_ENCODING_BGR24; + else if (camera_port->format.encoding == MMAL_ENCODING_BGR24) + camera_port->format.encoding = MMAL_ENCODING_RGB24; + } + + remove_padding = mfmt->remove_padding; + vchiq_mmal_port_parameter_set(dev->instance, camera_port, + MMAL_PARAMETER_NO_IMAGE_PADDING, + &remove_padding, sizeof(remove_padding)); + + camera_port->format.encoding_variant = 0; + camera_port->es.video.width = f->fmt.pix.width; + camera_port->es.video.height = f->fmt.pix.height; + camera_port->es.video.crop.x = 0; + camera_port->es.video.crop.y = 0; + camera_port->es.video.crop.width = f->fmt.pix.width; + camera_port->es.video.crop.height = f->fmt.pix.height; + camera_port->es.video.frame_rate.numerator = 0; + camera_port->es.video.frame_rate.denominator = 1; + camera_port->es.video.color_space = MMAL_COLOR_SPACE_JPEG_JFIF; + + ret = vchiq_mmal_port_set_format(dev->instance, camera_port); + + if (!ret && + camera_port == + &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO]) { + ret = mmal_setup_video_component(dev, f); + } + + if (ret) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s failed to set format %dx%d %08X\n", __func__, + f->fmt.pix.width, f->fmt.pix.height, + f->fmt.pix.pixelformat); + /* ensure capture is not going to be tried */ + dev->capture.port = NULL; + return ret; + } + + if (encode_component) { + ret = mmal_setup_encode_component(dev, f, port, + camera_port, + encode_component); + + if (ret) + return ret; + } else { + /* configure buffering */ + camera_port->current_buffer.num = 1; + camera_port->current_buffer.size = f->fmt.pix.sizeimage; + camera_port->current_buffer.alignment = 0; + } + + dev->capture.fmt = mfmt; + dev->capture.stride = f->fmt.pix.bytesperline; + dev->capture.width = camera_port->es.video.crop.width; + dev->capture.height = camera_port->es.video.crop.height; + dev->capture.buffersize = port->current_buffer.size; + + /* select port for capture */ + dev->capture.port = port; + dev->capture.camera_port = camera_port; + dev->capture.encode_component = encode_component; + v4l2_dbg(1, bcm2835_v4l2_debug, + &dev->v4l2_dev, + "Set dev->capture.fmt %08X, %dx%d, stride %d, size %d", + port->format.encoding, + dev->capture.width, dev->capture.height, + dev->capture.stride, dev->capture.buffersize); + + /* todo: Need to convert the vchiq/mmal error into a v4l2 error. */ + return ret; +} + +static int vidioc_s_fmt_vid_cap(struct file *file, void *priv, + struct v4l2_format *f) +{ + int ret; + struct bcm2835_mmal_dev *dev = video_drvdata(file); + struct mmal_fmt *mfmt; + + /* try the format to set valid parameters */ + ret = vidioc_try_fmt_vid_cap(file, priv, f); + if (ret) { + v4l2_err(&dev->v4l2_dev, + "vid_cap - vidioc_try_fmt_vid_cap failed\n"); + return ret; + } + + /* if a capture is running refuse to set format */ + if (vb2_is_busy(&dev->capture.vb_vidq)) { + v4l2_info(&dev->v4l2_dev, "%s device busy\n", __func__); + return -EBUSY; + } + + /* If the format is unsupported v4l2 says we should switch to + * a supported one and not return an error. + */ + mfmt = get_format(f); + if (!mfmt) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Fourcc format (0x%08x) unknown.\n", + f->fmt.pix.pixelformat); + f->fmt.pix.pixelformat = formats[0].fourcc; + mfmt = get_format(f); + } + + ret = mmal_setup_components(dev, f); + if (ret) { + v4l2_err(&dev->v4l2_dev, + "%s: failed to setup mmal components: %d\n", + __func__, ret); + ret = -EINVAL; + } + + return ret; +} + +static int vidioc_enum_framesizes(struct file *file, void *fh, + struct v4l2_frmsizeenum *fsize) +{ + struct bcm2835_mmal_dev *dev = video_drvdata(file); + static const struct v4l2_frmsize_stepwise sizes = { + MIN_WIDTH, 0, 2, + MIN_HEIGHT, 0, 2 + }; + int i; + + if (fsize->index) + return -EINVAL; + for (i = 0; i < ARRAY_SIZE(formats); i++) + if (formats[i].fourcc == fsize->pixel_format) + break; + if (i == ARRAY_SIZE(formats)) + return -EINVAL; + fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE; + fsize->stepwise = sizes; + fsize->stepwise.max_width = dev->max_width; + fsize->stepwise.max_height = dev->max_height; + return 0; +} + +/* timeperframe is arbitrary and continuous */ +static int vidioc_enum_frameintervals(struct file *file, void *priv, + struct v4l2_frmivalenum *fival) +{ + struct bcm2835_mmal_dev *dev = video_drvdata(file); + int i; + + if (fival->index) + return -EINVAL; + + for (i = 0; i < ARRAY_SIZE(formats); i++) + if (formats[i].fourcc == fival->pixel_format) + break; + if (i == ARRAY_SIZE(formats)) + return -EINVAL; + + /* regarding width & height - we support any within range */ + if (fival->width < MIN_WIDTH || fival->width > dev->max_width || + fival->height < MIN_HEIGHT || fival->height > dev->max_height) + return -EINVAL; + + fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS; + + /* fill in stepwise (step=1.0 is required by V4L2 spec) */ + fival->stepwise.min = tpf_min; + fival->stepwise.max = tpf_max; + fival->stepwise.step = (struct v4l2_fract) {1, 1}; + + return 0; +} + +static int vidioc_g_parm(struct file *file, void *priv, + struct v4l2_streamparm *parm) +{ + struct bcm2835_mmal_dev *dev = video_drvdata(file); + + if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) + return -EINVAL; + + parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME; + parm->parm.capture.timeperframe = dev->capture.timeperframe; + parm->parm.capture.readbuffers = 1; + return 0; +} + +static int vidioc_s_parm(struct file *file, void *priv, + struct v4l2_streamparm *parm) +{ + struct bcm2835_mmal_dev *dev = video_drvdata(file); + struct v4l2_fract tpf; + + if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) + return -EINVAL; + + tpf = parm->parm.capture.timeperframe; + + /* tpf: {*, 0} resets timing; clip to [min, max]*/ + tpf = tpf.denominator ? tpf : tpf_default; + tpf = V4L2_FRACT_COMPARE(tpf, <, tpf_min) ? tpf_min : tpf; + tpf = V4L2_FRACT_COMPARE(tpf, >, tpf_max) ? tpf_max : tpf; + + dev->capture.timeperframe = tpf; + parm->parm.capture.timeperframe = tpf; + parm->parm.capture.readbuffers = 1; + parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME; + + set_framerate_params(dev); + + return 0; +} + +static const struct v4l2_ioctl_ops camera0_ioctl_ops = { + /* overlay */ + .vidioc_enum_fmt_vid_overlay = vidioc_enum_fmt_vid_overlay, + .vidioc_g_fmt_vid_overlay = vidioc_g_fmt_vid_overlay, + .vidioc_try_fmt_vid_overlay = vidioc_try_fmt_vid_overlay, + .vidioc_s_fmt_vid_overlay = vidioc_s_fmt_vid_overlay, + .vidioc_overlay = vidioc_overlay, + .vidioc_g_fbuf = vidioc_g_fbuf, + + /* inputs */ + .vidioc_enum_input = vidioc_enum_input, + .vidioc_g_input = vidioc_g_input, + .vidioc_s_input = vidioc_s_input, + + /* capture */ + .vidioc_querycap = vidioc_querycap, + .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap, + .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap, + .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap, + .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap, + + /* buffer management */ + .vidioc_reqbufs = vb2_ioctl_reqbufs, + .vidioc_create_bufs = vb2_ioctl_create_bufs, + .vidioc_prepare_buf = vb2_ioctl_prepare_buf, + .vidioc_querybuf = vb2_ioctl_querybuf, + .vidioc_qbuf = vb2_ioctl_qbuf, + .vidioc_dqbuf = vb2_ioctl_dqbuf, + .vidioc_enum_framesizes = vidioc_enum_framesizes, + .vidioc_enum_frameintervals = vidioc_enum_frameintervals, + .vidioc_g_parm = vidioc_g_parm, + .vidioc_s_parm = vidioc_s_parm, + .vidioc_streamon = vb2_ioctl_streamon, + .vidioc_streamoff = vb2_ioctl_streamoff, + + .vidioc_log_status = v4l2_ctrl_log_status, + .vidioc_subscribe_event = v4l2_ctrl_subscribe_event, + .vidioc_unsubscribe_event = v4l2_event_unsubscribe, +}; + +/* ------------------------------------------------------------------ + * Driver init/finalise + * ------------------------------------------------------------------ + */ + +static const struct v4l2_file_operations camera0_fops = { + .owner = THIS_MODULE, + .open = v4l2_fh_open, + .release = vb2_fop_release, + .read = vb2_fop_read, + .poll = vb2_fop_poll, + .unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */ + .mmap = vb2_fop_mmap, +}; + +static const struct video_device vdev_template = { + .name = "camera0", + .fops = &camera0_fops, + .ioctl_ops = &camera0_ioctl_ops, + .release = video_device_release_empty, + .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY | + V4L2_CAP_STREAMING | V4L2_CAP_READWRITE, +}; + +/* Returns the number of cameras, and also the max resolution supported + * by those cameras. + */ +static int get_num_cameras(struct vchiq_mmal_instance *instance, + unsigned int resolutions[][2], int num_resolutions) +{ + int ret; + struct vchiq_mmal_component *cam_info_component; + struct mmal_parameter_camera_info cam_info = {0}; + u32 param_size = sizeof(cam_info); + int i; + + /* create a camera_info component */ + ret = vchiq_mmal_component_init(instance, "camera_info", + &cam_info_component); + if (ret < 0) + /* Unusual failure - let's guess one camera. */ + return 1; + + if (vchiq_mmal_port_parameter_get(instance, + &cam_info_component->control, + MMAL_PARAMETER_CAMERA_INFO, + &cam_info, + ¶m_size)) { + pr_info("Failed to get camera info\n"); + } + for (i = 0; + i < min_t(unsigned int, cam_info.num_cameras, num_resolutions); + i++) { + resolutions[i][0] = cam_info.cameras[i].max_width; + resolutions[i][1] = cam_info.cameras[i].max_height; + } + + vchiq_mmal_component_finalise(instance, + cam_info_component); + + return cam_info.num_cameras; +} + +static int set_camera_parameters(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_component *camera, + struct bcm2835_mmal_dev *dev) +{ + struct mmal_parameter_camera_config cam_config = { + .max_stills_w = dev->max_width, + .max_stills_h = dev->max_height, + .stills_yuv422 = 1, + .one_shot_stills = 1, + .max_preview_video_w = (max_video_width > 1920) ? + max_video_width : 1920, + .max_preview_video_h = (max_video_height > 1088) ? + max_video_height : 1088, + .num_preview_video_frames = 3, + .stills_capture_circular_buffer_height = 0, + .fast_preview_resume = 0, + .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC + }; + + return vchiq_mmal_port_parameter_set(instance, &camera->control, + MMAL_PARAMETER_CAMERA_CONFIG, + &cam_config, sizeof(cam_config)); +} + +#define MAX_SUPPORTED_ENCODINGS 20 + +/* MMAL instance and component init */ +static int mmal_init(struct bcm2835_mmal_dev *dev) +{ + int ret; + struct mmal_es_format_local *format; + u32 supported_encodings[MAX_SUPPORTED_ENCODINGS]; + u32 param_size; + struct vchiq_mmal_component *camera; + + ret = vchiq_mmal_init(&dev->instance); + if (ret < 0) { + v4l2_err(&dev->v4l2_dev, "%s: vchiq mmal init failed %d\n", + __func__, ret); + return ret; + } + + /* get the camera component ready */ + ret = vchiq_mmal_component_init(dev->instance, "ril.camera", + &dev->component[COMP_CAMERA]); + if (ret < 0) + goto unreg_mmal; + + camera = dev->component[COMP_CAMERA]; + if (camera->outputs < CAM_PORT_COUNT) { + v4l2_err(&dev->v4l2_dev, "%s: too few camera outputs %d needed %d\n", + __func__, camera->outputs, CAM_PORT_COUNT); + ret = -EINVAL; + goto unreg_camera; + } + + ret = set_camera_parameters(dev->instance, + camera, + dev); + if (ret < 0) { + v4l2_err(&dev->v4l2_dev, "%s: unable to set camera parameters: %d\n", + __func__, ret); + goto unreg_camera; + } + + /* There was an error in the firmware that meant the camera component + * produced BGR instead of RGB. + * This is now fixed, but in order to support the old firmwares, we + * have to check. + */ + dev->rgb_bgr_swapped = true; + param_size = sizeof(supported_encodings); + ret = vchiq_mmal_port_parameter_get(dev->instance, + &camera->output[CAM_PORT_CAPTURE], + MMAL_PARAMETER_SUPPORTED_ENCODINGS, + &supported_encodings, + ¶m_size); + if (ret == 0) { + int i; + + for (i = 0; i < param_size / sizeof(u32); i++) { + if (supported_encodings[i] == MMAL_ENCODING_BGR24) { + /* Found BGR24 first - old firmware. */ + break; + } + if (supported_encodings[i] == MMAL_ENCODING_RGB24) { + /* Found RGB24 first + * new firmware, so use RGB24. + */ + dev->rgb_bgr_swapped = false; + break; + } + } + } + format = &camera->output[CAM_PORT_PREVIEW].format; + + format->encoding = MMAL_ENCODING_OPAQUE; + format->encoding_variant = MMAL_ENCODING_I420; + + format->es->video.width = 1024; + format->es->video.height = 768; + format->es->video.crop.x = 0; + format->es->video.crop.y = 0; + format->es->video.crop.width = 1024; + format->es->video.crop.height = 768; + format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */ + format->es->video.frame_rate.denominator = 1; + + format = &camera->output[CAM_PORT_VIDEO].format; + + format->encoding = MMAL_ENCODING_OPAQUE; + format->encoding_variant = MMAL_ENCODING_I420; + + format->es->video.width = 1024; + format->es->video.height = 768; + format->es->video.crop.x = 0; + format->es->video.crop.y = 0; + format->es->video.crop.width = 1024; + format->es->video.crop.height = 768; + format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */ + format->es->video.frame_rate.denominator = 1; + + format = &camera->output[CAM_PORT_CAPTURE].format; + + format->encoding = MMAL_ENCODING_OPAQUE; + + format->es->video.width = 2592; + format->es->video.height = 1944; + format->es->video.crop.x = 0; + format->es->video.crop.y = 0; + format->es->video.crop.width = 2592; + format->es->video.crop.height = 1944; + format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */ + format->es->video.frame_rate.denominator = 1; + + dev->capture.width = format->es->video.width; + dev->capture.height = format->es->video.height; + dev->capture.fmt = &formats[0]; + dev->capture.encode_component = NULL; + dev->capture.timeperframe = tpf_default; + dev->capture.enc_profile = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH; + dev->capture.enc_level = V4L2_MPEG_VIDEO_H264_LEVEL_4_0; + + /* get the preview component ready */ + ret = vchiq_mmal_component_init(dev->instance, "ril.video_render", + &dev->component[COMP_PREVIEW]); + if (ret < 0) + goto unreg_camera; + + if (dev->component[COMP_PREVIEW]->inputs < 1) { + ret = -EINVAL; + v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n", + __func__, dev->component[COMP_PREVIEW]->inputs, 1); + goto unreg_preview; + } + + /* get the image encoder component ready */ + ret = vchiq_mmal_component_init(dev->instance, "ril.image_encode", + &dev->component[COMP_IMAGE_ENCODE]); + if (ret < 0) + goto unreg_preview; + + if (dev->component[COMP_IMAGE_ENCODE]->inputs < 1) { + ret = -EINVAL; + v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n", + __func__, dev->component[COMP_IMAGE_ENCODE]->inputs, + 1); + goto unreg_image_encoder; + } + + /* get the video encoder component ready */ + ret = vchiq_mmal_component_init(dev->instance, "ril.video_encode", + &dev->component[COMP_VIDEO_ENCODE]); + if (ret < 0) + goto unreg_image_encoder; + + if (dev->component[COMP_VIDEO_ENCODE]->inputs < 1) { + ret = -EINVAL; + v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n", + __func__, dev->component[COMP_VIDEO_ENCODE]->inputs, + 1); + goto unreg_vid_encoder; + } + + { + struct vchiq_mmal_port *encoder_port = + &dev->component[COMP_VIDEO_ENCODE]->output[0]; + encoder_port->format.encoding = MMAL_ENCODING_H264; + ret = vchiq_mmal_port_set_format(dev->instance, + encoder_port); + } + + { + unsigned int enable = 1; + + vchiq_mmal_port_parameter_set( + dev->instance, + &dev->component[COMP_VIDEO_ENCODE]->control, + MMAL_PARAMETER_VIDEO_IMMUTABLE_INPUT, + &enable, sizeof(enable)); + + vchiq_mmal_port_parameter_set(dev->instance, + &dev->component[COMP_VIDEO_ENCODE]->control, + MMAL_PARAMETER_MINIMISE_FRAGMENTATION, + &enable, + sizeof(enable)); + } + ret = bcm2835_mmal_set_all_camera_controls(dev); + if (ret < 0) { + v4l2_err(&dev->v4l2_dev, "%s: failed to set all camera controls: %d\n", + __func__, ret); + goto unreg_vid_encoder; + } + + return 0; + +unreg_vid_encoder: + pr_err("Cleanup: Destroy video encoder\n"); + vchiq_mmal_component_finalise(dev->instance, + dev->component[COMP_VIDEO_ENCODE]); + +unreg_image_encoder: + pr_err("Cleanup: Destroy image encoder\n"); + vchiq_mmal_component_finalise(dev->instance, + dev->component[COMP_IMAGE_ENCODE]); + +unreg_preview: + pr_err("Cleanup: Destroy video render\n"); + vchiq_mmal_component_finalise(dev->instance, + dev->component[COMP_PREVIEW]); + +unreg_camera: + pr_err("Cleanup: Destroy camera\n"); + vchiq_mmal_component_finalise(dev->instance, + dev->component[COMP_CAMERA]); + +unreg_mmal: + vchiq_mmal_finalise(dev->instance); + return ret; +} + +static int bcm2835_mmal_init_device(struct bcm2835_mmal_dev *dev, struct video_device *vfd) +{ + int ret; + + *vfd = vdev_template; + + vfd->v4l2_dev = &dev->v4l2_dev; + + vfd->lock = &dev->mutex; + + vfd->queue = &dev->capture.vb_vidq; + + /* video device needs to be able to access instance data */ + video_set_drvdata(vfd, dev); + + ret = video_register_device(vfd, VFL_TYPE_VIDEO, + video_nr[dev->camera_num]); + if (ret < 0) + return ret; + + v4l2_info(vfd->v4l2_dev, + "V4L2 device registered as %s - stills mode > %dx%d\n", + video_device_node_name(vfd), + max_video_width, max_video_height); + + return 0; +} + +static void bcm2835_cleanup_instance(struct bcm2835_mmal_dev *dev) +{ + if (!dev) + return; + + v4l2_info(&dev->v4l2_dev, "unregistering %s\n", + video_device_node_name(&dev->vdev)); + + video_unregister_device(&dev->vdev); + + if (dev->capture.encode_component) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "mmal_exit - disconnect tunnel\n"); + vchiq_mmal_port_connect_tunnel(dev->instance, + dev->capture.camera_port, NULL); + vchiq_mmal_component_disable(dev->instance, + dev->capture.encode_component); + } + vchiq_mmal_component_disable(dev->instance, + dev->component[COMP_CAMERA]); + + vchiq_mmal_component_finalise(dev->instance, + dev->component[COMP_VIDEO_ENCODE]); + + vchiq_mmal_component_finalise(dev->instance, + dev->component[COMP_IMAGE_ENCODE]); + + vchiq_mmal_component_finalise(dev->instance, + dev->component[COMP_PREVIEW]); + + vchiq_mmal_component_finalise(dev->instance, + dev->component[COMP_CAMERA]); + + v4l2_ctrl_handler_free(&dev->ctrl_handler); + + v4l2_device_unregister(&dev->v4l2_dev); + + kfree(dev); +} + +static struct v4l2_format default_v4l2_format = { + .fmt.pix.pixelformat = V4L2_PIX_FMT_JPEG, + .fmt.pix.width = 1024, + .fmt.pix.bytesperline = 0, + .fmt.pix.height = 768, + .fmt.pix.sizeimage = 1024 * 768, +}; + +static int bcm2835_mmal_probe(struct platform_device *pdev) +{ + int ret; + struct bcm2835_mmal_dev *dev; + struct vb2_queue *q; + int camera; + unsigned int num_cameras; + struct vchiq_mmal_instance *instance; + unsigned int resolutions[MAX_BCM2835_CAMERAS][2]; + int i; + + ret = vchiq_mmal_init(&instance); + if (ret < 0) + return ret; + + num_cameras = get_num_cameras(instance, + resolutions, + MAX_BCM2835_CAMERAS); + + if (num_cameras < 1) { + ret = -ENODEV; + goto cleanup_mmal; + } + + if (num_cameras > MAX_BCM2835_CAMERAS) + num_cameras = MAX_BCM2835_CAMERAS; + + for (camera = 0; camera < num_cameras; camera++) { + dev = kzalloc(sizeof(*dev), GFP_KERNEL); + if (!dev) { + ret = -ENOMEM; + goto cleanup_gdev; + } + + /* v4l2 core mutex used to protect all fops and v4l2 ioctls. */ + mutex_init(&dev->mutex); + dev->max_width = resolutions[camera][0]; + dev->max_height = resolutions[camera][1]; + + /* setup device defaults */ + dev->overlay.w.left = 150; + dev->overlay.w.top = 50; + dev->overlay.w.width = 1024; + dev->overlay.w.height = 768; + dev->overlay.clipcount = 0; + dev->overlay.field = V4L2_FIELD_NONE; + dev->overlay.global_alpha = 255; + + dev->capture.fmt = &formats[3]; /* JPEG */ + + /* v4l device registration */ + dev->camera_num = v4l2_device_set_name(&dev->v4l2_dev, KBUILD_MODNAME, + &camera_instance); + ret = v4l2_device_register(NULL, &dev->v4l2_dev); + if (ret) { + dev_err(&pdev->dev, "%s: could not register V4L2 device: %d\n", + __func__, ret); + goto free_dev; + } + + /* setup v4l controls */ + ret = bcm2835_mmal_init_controls(dev, &dev->ctrl_handler); + if (ret < 0) { + v4l2_err(&dev->v4l2_dev, "%s: could not init controls: %d\n", + __func__, ret); + goto unreg_dev; + } + dev->v4l2_dev.ctrl_handler = &dev->ctrl_handler; + + /* mmal init */ + dev->instance = instance; + ret = mmal_init(dev); + if (ret < 0) { + v4l2_err(&dev->v4l2_dev, "%s: mmal init failed: %d\n", + __func__, ret); + goto unreg_dev; + } + /* initialize queue */ + q = &dev->capture.vb_vidq; + memset(q, 0, sizeof(*q)); + q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ; + q->drv_priv = dev; + q->buf_struct_size = sizeof(struct vb2_mmal_buffer); + q->ops = &bcm2835_mmal_video_qops; + q->mem_ops = &vb2_vmalloc_memops; + q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; + q->lock = &dev->mutex; + ret = vb2_queue_init(q); + if (ret < 0) + goto unreg_dev; + + /* initialise video devices */ + ret = bcm2835_mmal_init_device(dev, &dev->vdev); + if (ret < 0) { + v4l2_err(&dev->v4l2_dev, "%s: could not init device: %d\n", + __func__, ret); + goto unreg_dev; + } + + /* Really want to call vidioc_s_fmt_vid_cap with the default + * format, but currently the APIs don't join up. + */ + ret = mmal_setup_components(dev, &default_v4l2_format); + if (ret < 0) { + v4l2_err(&dev->v4l2_dev, "%s: could not setup components: %d\n", + __func__, ret); + goto unreg_dev; + } + + v4l2_info(&dev->v4l2_dev, "Broadcom 2835 MMAL video capture loaded.\n"); + + gdev[camera] = dev; + } + return 0; + +unreg_dev: + v4l2_ctrl_handler_free(&dev->ctrl_handler); + v4l2_device_unregister(&dev->v4l2_dev); + +free_dev: + kfree(dev); + +cleanup_gdev: + for (i = 0; i < camera; i++) { + bcm2835_cleanup_instance(gdev[i]); + gdev[i] = NULL; + } + +cleanup_mmal: + vchiq_mmal_finalise(instance); + + return ret; +} + +static int bcm2835_mmal_remove(struct platform_device *pdev) +{ + int camera; + struct vchiq_mmal_instance *instance = gdev[0]->instance; + + for (camera = 0; camera < MAX_BCM2835_CAMERAS; camera++) { + bcm2835_cleanup_instance(gdev[camera]); + gdev[camera] = NULL; + } + vchiq_mmal_finalise(instance); + + return 0; +} + +static struct platform_driver bcm2835_camera_driver = { + .probe = bcm2835_mmal_probe, + .remove = bcm2835_mmal_remove, + .driver = { + .name = "bcm2835-camera", + }, +}; + +module_platform_driver(bcm2835_camera_driver) + +MODULE_DESCRIPTION("Broadcom 2835 MMAL video capture"); +MODULE_AUTHOR("Vincent Sanders"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("platform:bcm2835-camera"); diff --git a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h new file mode 100644 index 000000000..0f0c6f7a3 --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h @@ -0,0 +1,142 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * Broadcom BCM2835 V4L2 driver + * + * Copyright © 2013 Raspberry Pi (Trading) Ltd. + * + * Authors: Vincent Sanders @ Collabora + * Dave Stevenson @ Broadcom + * (now dave.stevenson@raspberrypi.org) + * Simon Mellor @ Broadcom + * Luke Diamand @ Broadcom + * + * core driver device + */ + +#define V4L2_CTRL_COUNT 29 /* number of v4l controls */ + +enum { + COMP_CAMERA = 0, + COMP_PREVIEW, + COMP_IMAGE_ENCODE, + COMP_VIDEO_ENCODE, + COMP_COUNT +}; + +enum { + CAM_PORT_PREVIEW = 0, + CAM_PORT_VIDEO, + CAM_PORT_CAPTURE, + CAM_PORT_COUNT +}; + +extern int bcm2835_v4l2_debug; + +struct bcm2835_mmal_dev { + /* v4l2 devices */ + struct v4l2_device v4l2_dev; + struct video_device vdev; + struct mutex mutex; + + /* controls */ + struct v4l2_ctrl_handler ctrl_handler; + struct v4l2_ctrl *ctrls[V4L2_CTRL_COUNT]; + enum v4l2_scene_mode scene_mode; + struct mmal_colourfx colourfx; + int hflip; + int vflip; + int red_gain; + int blue_gain; + enum mmal_parameter_exposuremode exposure_mode_user; + enum v4l2_exposure_auto_type exposure_mode_v4l2_user; + /* active exposure mode may differ if selected via a scene mode */ + enum mmal_parameter_exposuremode exposure_mode_active; + enum mmal_parameter_exposuremeteringmode metering_mode; + unsigned int manual_shutter_speed; + bool exp_auto_priority; + bool manual_iso_enabled; + u32 iso; + + /* allocated mmal instance and components */ + struct vchiq_mmal_instance *instance; + struct vchiq_mmal_component *component[COMP_COUNT]; + int camera_use_count; + + struct v4l2_window overlay; + + struct { + unsigned int width; /* width */ + unsigned int height; /* height */ + unsigned int stride; /* stride */ + unsigned int buffersize; /* buffer size with padding */ + struct mmal_fmt *fmt; + struct v4l2_fract timeperframe; + + /* H264 encode bitrate */ + int encode_bitrate; + /* H264 bitrate mode. CBR/VBR */ + int encode_bitrate_mode; + /* H264 profile */ + enum v4l2_mpeg_video_h264_profile enc_profile; + /* H264 level */ + enum v4l2_mpeg_video_h264_level enc_level; + /* JPEG Q-factor */ + int q_factor; + + struct vb2_queue vb_vidq; + + /* VC start timestamp for streaming */ + s64 vc_start_timestamp; + /* Kernel start timestamp for streaming */ + ktime_t kernel_start_ts; + /* Sequence number of last buffer */ + u32 sequence; + + struct vchiq_mmal_port *port; /* port being used for capture */ + /* camera port being used for capture */ + struct vchiq_mmal_port *camera_port; + /* component being used for encode */ + struct vchiq_mmal_component *encode_component; + /* number of frames remaining which driver should capture */ + unsigned int frame_count; + /* last frame completion */ + struct completion frame_cmplt; + + } capture; + + unsigned int camera_num; + unsigned int max_width; + unsigned int max_height; + unsigned int rgb_bgr_swapped; +}; + +int bcm2835_mmal_init_controls(struct bcm2835_mmal_dev *dev, struct v4l2_ctrl_handler *hdl); + +int bcm2835_mmal_set_all_camera_controls(struct bcm2835_mmal_dev *dev); +int set_framerate_params(struct bcm2835_mmal_dev *dev); + +/* Debug helpers */ + +#define v4l2_dump_pix_format(level, debug, dev, pix_fmt, desc) \ +{ \ + v4l2_dbg(level, debug, dev, \ +"%s: w %u h %u field %u pfmt 0x%x bpl %u sz_img %u colorspace 0x%x priv %u\n", \ + desc, \ + (pix_fmt)->width, (pix_fmt)->height, (pix_fmt)->field, \ + (pix_fmt)->pixelformat, (pix_fmt)->bytesperline, \ + (pix_fmt)->sizeimage, (pix_fmt)->colorspace, (pix_fmt)->priv); \ +} + +#define v4l2_dump_win_format(level, debug, dev, win_fmt, desc) \ +{ \ + v4l2_dbg(level, debug, dev, \ +"%s: w %u h %u l %u t %u field %u chromakey %06X clip %p " \ +"clipcount %u bitmap %p\n", \ + desc, \ + (win_fmt)->w.width, (win_fmt)->w.height, \ + (win_fmt)->w.left, (win_fmt)->w.top, \ + (win_fmt)->field, \ + (win_fmt)->chromakey, \ + (win_fmt)->clips, (win_fmt)->clipcount, \ + (win_fmt)->bitmap); \ +} diff --git a/drivers/staging/vc04_services/bcm2835-camera/controls.c b/drivers/staging/vc04_services/bcm2835-camera/controls.c new file mode 100644 index 000000000..5644d1d45 --- /dev/null +++ b/drivers/staging/vc04_services/bcm2835-camera/controls.c @@ -0,0 +1,1401 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Broadcom BCM2835 V4L2 driver + * + * Copyright © 2013 Raspberry Pi (Trading) Ltd. + * + * Authors: Vincent Sanders @ Collabora + * Dave Stevenson @ Broadcom + * (now dave.stevenson@raspberrypi.org) + * Simon Mellor @ Broadcom + * Luke Diamand @ Broadcom + */ + +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/slab.h> +#include <media/videobuf2-vmalloc.h> +#include <media/v4l2-device.h> +#include <media/v4l2-ioctl.h> +#include <media/v4l2-ctrls.h> +#include <media/v4l2-fh.h> +#include <media/v4l2-event.h> +#include <media/v4l2-common.h> + +#include "mmal-common.h" +#include "mmal-vchiq.h" +#include "mmal-parameters.h" +#include "bcm2835-camera.h" + +/* The supported V4L2_CID_AUTO_EXPOSURE_BIAS values are from -4.0 to +4.0. + * MMAL values are in 1/6th increments so the MMAL range is -24 to +24. + * V4L2 docs say value "is expressed in terms of EV, drivers should interpret + * the values as 0.001 EV units, where the value 1000 stands for +1 EV." + * V4L2 is limited to a max of 32 values in a menu, so count in 1/3rds from + * -4 to +4 + */ +static const s64 ev_bias_qmenu[] = { + -4000, -3667, -3333, + -3000, -2667, -2333, + -2000, -1667, -1333, + -1000, -667, -333, + 0, 333, 667, + 1000, 1333, 1667, + 2000, 2333, 2667, + 3000, 3333, 3667, + 4000 +}; + +/* Supported ISO values (*1000) + * ISOO = auto ISO + */ +static const s64 iso_qmenu[] = { + 0, 100000, 200000, 400000, 800000, +}; + +static const u32 iso_values[] = { + 0, 100, 200, 400, 800, +}; + +enum bcm2835_mmal_ctrl_type { + MMAL_CONTROL_TYPE_STD, + MMAL_CONTROL_TYPE_STD_MENU, + MMAL_CONTROL_TYPE_INT_MENU, + MMAL_CONTROL_TYPE_CLUSTER, /* special cluster entry */ +}; + +struct bcm2835_mmal_v4l2_ctrl { + u32 id; /* v4l2 control identifier */ + enum bcm2835_mmal_ctrl_type type; + /* control minimum value or + * mask for MMAL_CONTROL_TYPE_STD_MENU + */ + s64 min; + s64 max; /* maximum value of control */ + s64 def; /* default value of control */ + u64 step; /* step size of the control */ + const s64 *imenu; /* integer menu array */ + u32 mmal_id; /* mmal parameter id */ + int (*setter)(struct bcm2835_mmal_dev *dev, struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl); +}; + +struct v4l2_to_mmal_effects_setting { + u32 v4l2_effect; + u32 mmal_effect; + s32 col_fx_enable; + s32 col_fx_fixed_cbcr; + u32 u; + u32 v; + u32 num_effect_params; + u32 effect_params[MMAL_MAX_IMAGEFX_PARAMETERS]; +}; + +static const struct v4l2_to_mmal_effects_setting + v4l2_to_mmal_effects_values[] = { + { V4L2_COLORFX_NONE, MMAL_PARAM_IMAGEFX_NONE, + 0, 0, 0, 0, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_BW, MMAL_PARAM_IMAGEFX_NONE, + 1, 0, 128, 128, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_SEPIA, MMAL_PARAM_IMAGEFX_NONE, + 1, 0, 87, 151, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_NEGATIVE, MMAL_PARAM_IMAGEFX_NEGATIVE, + 0, 0, 0, 0, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_EMBOSS, MMAL_PARAM_IMAGEFX_EMBOSS, + 0, 0, 0, 0, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_SKETCH, MMAL_PARAM_IMAGEFX_SKETCH, + 0, 0, 0, 0, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_SKY_BLUE, MMAL_PARAM_IMAGEFX_PASTEL, + 0, 0, 0, 0, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_GRASS_GREEN, MMAL_PARAM_IMAGEFX_WATERCOLOUR, + 0, 0, 0, 0, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_SKIN_WHITEN, MMAL_PARAM_IMAGEFX_WASHEDOUT, + 0, 0, 0, 0, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_VIVID, MMAL_PARAM_IMAGEFX_SATURATION, + 0, 0, 0, 0, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_AQUA, MMAL_PARAM_IMAGEFX_NONE, + 1, 0, 171, 121, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_ART_FREEZE, MMAL_PARAM_IMAGEFX_HATCH, + 0, 0, 0, 0, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_SILHOUETTE, MMAL_PARAM_IMAGEFX_FILM, + 0, 0, 0, 0, 0, {0, 0, 0, 0, 0} }, + { V4L2_COLORFX_SOLARIZATION, MMAL_PARAM_IMAGEFX_SOLARIZE, + 0, 0, 0, 0, 5, {1, 128, 160, 160, 48} }, + { V4L2_COLORFX_ANTIQUE, MMAL_PARAM_IMAGEFX_COLOURBALANCE, + 0, 0, 0, 0, 3, {108, 274, 238, 0, 0} }, + { V4L2_COLORFX_SET_CBCR, MMAL_PARAM_IMAGEFX_NONE, + 1, 1, 0, 0, 0, {0, 0, 0, 0, 0} } +}; + +struct v4l2_mmal_scene_config { + enum v4l2_scene_mode v4l2_scene; + enum mmal_parameter_exposuremode exposure_mode; + enum mmal_parameter_exposuremeteringmode metering_mode; +}; + +static const struct v4l2_mmal_scene_config scene_configs[] = { + /* V4L2_SCENE_MODE_NONE automatically added */ + { + V4L2_SCENE_MODE_NIGHT, + MMAL_PARAM_EXPOSUREMODE_NIGHT, + MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE + }, + { + V4L2_SCENE_MODE_SPORTS, + MMAL_PARAM_EXPOSUREMODE_SPORTS, + MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE + }, +}; + +/* control handlers*/ + +static int ctrl_set_rational(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + struct s32_fract rational_value; + struct vchiq_mmal_port *control; + + control = &dev->component[COMP_CAMERA]->control; + + rational_value.numerator = ctrl->val; + rational_value.denominator = 100; + + return vchiq_mmal_port_parameter_set(dev->instance, control, + mmal_ctrl->mmal_id, + &rational_value, + sizeof(rational_value)); +} + +static int ctrl_set_value(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + u32 u32_value; + struct vchiq_mmal_port *control; + + control = &dev->component[COMP_CAMERA]->control; + + u32_value = ctrl->val; + + return vchiq_mmal_port_parameter_set(dev->instance, control, + mmal_ctrl->mmal_id, + &u32_value, sizeof(u32_value)); +} + +static int ctrl_set_iso(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + u32 u32_value; + struct vchiq_mmal_port *control; + + if (ctrl->val > mmal_ctrl->max || ctrl->val < mmal_ctrl->min) + return 1; + + if (ctrl->id == V4L2_CID_ISO_SENSITIVITY) + dev->iso = iso_values[ctrl->val]; + else if (ctrl->id == V4L2_CID_ISO_SENSITIVITY_AUTO) + dev->manual_iso_enabled = + (ctrl->val == V4L2_ISO_SENSITIVITY_MANUAL); + + control = &dev->component[COMP_CAMERA]->control; + + if (dev->manual_iso_enabled) + u32_value = dev->iso; + else + u32_value = 0; + + return vchiq_mmal_port_parameter_set(dev->instance, control, + MMAL_PARAMETER_ISO, + &u32_value, sizeof(u32_value)); +} + +static int ctrl_set_value_ev(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + s32 s32_value; + struct vchiq_mmal_port *control; + + control = &dev->component[COMP_CAMERA]->control; + + s32_value = (ctrl->val - 12) * 2; /* Convert from index to 1/6ths */ + + return vchiq_mmal_port_parameter_set(dev->instance, control, + mmal_ctrl->mmal_id, + &s32_value, sizeof(s32_value)); +} + +static int ctrl_set_rotate(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + int ret; + u32 u32_value; + struct vchiq_mmal_component *camera; + + camera = dev->component[COMP_CAMERA]; + + u32_value = ((ctrl->val % 360) / 90) * 90; + + ret = vchiq_mmal_port_parameter_set(dev->instance, &camera->output[0], + mmal_ctrl->mmal_id, + &u32_value, sizeof(u32_value)); + if (ret < 0) + return ret; + + ret = vchiq_mmal_port_parameter_set(dev->instance, &camera->output[1], + mmal_ctrl->mmal_id, + &u32_value, sizeof(u32_value)); + if (ret < 0) + return ret; + + return vchiq_mmal_port_parameter_set(dev->instance, &camera->output[2], + mmal_ctrl->mmal_id, + &u32_value, sizeof(u32_value)); +} + +static int ctrl_set_flip(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + int ret; + u32 u32_value; + struct vchiq_mmal_component *camera; + + if (ctrl->id == V4L2_CID_HFLIP) + dev->hflip = ctrl->val; + else + dev->vflip = ctrl->val; + + camera = dev->component[COMP_CAMERA]; + + if (dev->hflip && dev->vflip) + u32_value = MMAL_PARAM_MIRROR_BOTH; + else if (dev->hflip) + u32_value = MMAL_PARAM_MIRROR_HORIZONTAL; + else if (dev->vflip) + u32_value = MMAL_PARAM_MIRROR_VERTICAL; + else + u32_value = MMAL_PARAM_MIRROR_NONE; + + ret = vchiq_mmal_port_parameter_set(dev->instance, &camera->output[0], + mmal_ctrl->mmal_id, + &u32_value, sizeof(u32_value)); + if (ret < 0) + return ret; + + ret = vchiq_mmal_port_parameter_set(dev->instance, &camera->output[1], + mmal_ctrl->mmal_id, + &u32_value, sizeof(u32_value)); + if (ret < 0) + return ret; + + return vchiq_mmal_port_parameter_set(dev->instance, &camera->output[2], + mmal_ctrl->mmal_id, + &u32_value, sizeof(u32_value)); +} + +static int ctrl_set_exposure(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + enum mmal_parameter_exposuremode exp_mode = dev->exposure_mode_user; + u32 shutter_speed = 0; + struct vchiq_mmal_port *control; + int ret = 0; + + control = &dev->component[COMP_CAMERA]->control; + + if (mmal_ctrl->mmal_id == MMAL_PARAMETER_SHUTTER_SPEED) { + /* V4L2 is in 100usec increments. + * MMAL is 1usec. + */ + dev->manual_shutter_speed = ctrl->val * 100; + } else if (mmal_ctrl->mmal_id == MMAL_PARAMETER_EXPOSURE_MODE) { + switch (ctrl->val) { + case V4L2_EXPOSURE_AUTO: + exp_mode = MMAL_PARAM_EXPOSUREMODE_AUTO; + break; + + case V4L2_EXPOSURE_MANUAL: + exp_mode = MMAL_PARAM_EXPOSUREMODE_OFF; + break; + } + dev->exposure_mode_user = exp_mode; + dev->exposure_mode_v4l2_user = ctrl->val; + } else if (mmal_ctrl->id == V4L2_CID_EXPOSURE_AUTO_PRIORITY) { + dev->exp_auto_priority = ctrl->val; + } + + if (dev->scene_mode == V4L2_SCENE_MODE_NONE) { + if (exp_mode == MMAL_PARAM_EXPOSUREMODE_OFF) + shutter_speed = dev->manual_shutter_speed; + + ret = vchiq_mmal_port_parameter_set(dev->instance, + control, + MMAL_PARAMETER_SHUTTER_SPEED, + &shutter_speed, + sizeof(shutter_speed)); + ret += vchiq_mmal_port_parameter_set(dev->instance, + control, + MMAL_PARAMETER_EXPOSURE_MODE, + &exp_mode, + sizeof(u32)); + dev->exposure_mode_active = exp_mode; + } + /* exposure_dynamic_framerate (V4L2_CID_EXPOSURE_AUTO_PRIORITY) should + * always apply irrespective of scene mode. + */ + ret += set_framerate_params(dev); + + return ret; +} + +static int ctrl_set_metering_mode(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + switch (ctrl->val) { + case V4L2_EXPOSURE_METERING_AVERAGE: + dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE; + break; + + case V4L2_EXPOSURE_METERING_CENTER_WEIGHTED: + dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT; + break; + + case V4L2_EXPOSURE_METERING_SPOT: + dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT; + break; + + case V4L2_EXPOSURE_METERING_MATRIX: + dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX; + break; + } + + if (dev->scene_mode == V4L2_SCENE_MODE_NONE) { + struct vchiq_mmal_port *control; + u32 u32_value = dev->metering_mode; + + control = &dev->component[COMP_CAMERA]->control; + + return vchiq_mmal_port_parameter_set(dev->instance, control, + mmal_ctrl->mmal_id, + &u32_value, sizeof(u32_value)); + } else { + return 0; + } +} + +static int ctrl_set_flicker_avoidance(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + u32 u32_value; + struct vchiq_mmal_port *control; + + control = &dev->component[COMP_CAMERA]->control; + + switch (ctrl->val) { + case V4L2_CID_POWER_LINE_FREQUENCY_DISABLED: + u32_value = MMAL_PARAM_FLICKERAVOID_OFF; + break; + case V4L2_CID_POWER_LINE_FREQUENCY_50HZ: + u32_value = MMAL_PARAM_FLICKERAVOID_50HZ; + break; + case V4L2_CID_POWER_LINE_FREQUENCY_60HZ: + u32_value = MMAL_PARAM_FLICKERAVOID_60HZ; + break; + case V4L2_CID_POWER_LINE_FREQUENCY_AUTO: + u32_value = MMAL_PARAM_FLICKERAVOID_AUTO; + break; + } + + return vchiq_mmal_port_parameter_set(dev->instance, control, + mmal_ctrl->mmal_id, + &u32_value, sizeof(u32_value)); +} + +static int ctrl_set_awb_mode(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + u32 u32_value; + struct vchiq_mmal_port *control; + + control = &dev->component[COMP_CAMERA]->control; + + switch (ctrl->val) { + case V4L2_WHITE_BALANCE_MANUAL: + u32_value = MMAL_PARAM_AWBMODE_OFF; + break; + + case V4L2_WHITE_BALANCE_AUTO: + u32_value = MMAL_PARAM_AWBMODE_AUTO; + break; + + case V4L2_WHITE_BALANCE_INCANDESCENT: + u32_value = MMAL_PARAM_AWBMODE_INCANDESCENT; + break; + + case V4L2_WHITE_BALANCE_FLUORESCENT: + u32_value = MMAL_PARAM_AWBMODE_FLUORESCENT; + break; + + case V4L2_WHITE_BALANCE_FLUORESCENT_H: + u32_value = MMAL_PARAM_AWBMODE_TUNGSTEN; + break; + + case V4L2_WHITE_BALANCE_HORIZON: + u32_value = MMAL_PARAM_AWBMODE_HORIZON; + break; + + case V4L2_WHITE_BALANCE_DAYLIGHT: + u32_value = MMAL_PARAM_AWBMODE_SUNLIGHT; + break; + + case V4L2_WHITE_BALANCE_FLASH: + u32_value = MMAL_PARAM_AWBMODE_FLASH; + break; + + case V4L2_WHITE_BALANCE_CLOUDY: + u32_value = MMAL_PARAM_AWBMODE_CLOUDY; + break; + + case V4L2_WHITE_BALANCE_SHADE: + u32_value = MMAL_PARAM_AWBMODE_SHADE; + break; + } + + return vchiq_mmal_port_parameter_set(dev->instance, control, + mmal_ctrl->mmal_id, + &u32_value, sizeof(u32_value)); +} + +static int ctrl_set_awb_gains(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + struct vchiq_mmal_port *control; + struct mmal_parameter_awbgains gains; + + control = &dev->component[COMP_CAMERA]->control; + + if (ctrl->id == V4L2_CID_RED_BALANCE) + dev->red_gain = ctrl->val; + else if (ctrl->id == V4L2_CID_BLUE_BALANCE) + dev->blue_gain = ctrl->val; + + gains.r_gain.numerator = dev->red_gain; + gains.r_gain.denominator = 1000; + gains.b_gain.numerator = dev->blue_gain; + gains.b_gain.denominator = 1000; + + return vchiq_mmal_port_parameter_set(dev->instance, control, + mmal_ctrl->mmal_id, + &gains, sizeof(gains)); +} + +static int ctrl_set_image_effect(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + int ret = -EINVAL; + int i, j; + struct vchiq_mmal_port *control; + struct mmal_parameter_imagefx_parameters imagefx; + + for (i = 0; i < ARRAY_SIZE(v4l2_to_mmal_effects_values); i++) { + if (ctrl->val != v4l2_to_mmal_effects_values[i].v4l2_effect) + continue; + + imagefx.effect = + v4l2_to_mmal_effects_values[i].mmal_effect; + imagefx.num_effect_params = + v4l2_to_mmal_effects_values[i].num_effect_params; + + if (imagefx.num_effect_params > MMAL_MAX_IMAGEFX_PARAMETERS) + imagefx.num_effect_params = MMAL_MAX_IMAGEFX_PARAMETERS; + + for (j = 0; j < imagefx.num_effect_params; j++) + imagefx.effect_parameter[j] = + v4l2_to_mmal_effects_values[i].effect_params[j]; + + dev->colourfx.enable = + v4l2_to_mmal_effects_values[i].col_fx_enable; + if (!v4l2_to_mmal_effects_values[i].col_fx_fixed_cbcr) { + dev->colourfx.u = v4l2_to_mmal_effects_values[i].u; + dev->colourfx.v = v4l2_to_mmal_effects_values[i].v; + } + + control = &dev->component[COMP_CAMERA]->control; + + ret = vchiq_mmal_port_parameter_set( + dev->instance, control, + MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS, + &imagefx, sizeof(imagefx)); + if (ret) + goto exit; + + ret = vchiq_mmal_port_parameter_set( + dev->instance, control, + MMAL_PARAMETER_COLOUR_EFFECT, + &dev->colourfx, sizeof(dev->colourfx)); + } + +exit: + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "mmal_ctrl:%p ctrl id:0x%x ctrl val:%d imagefx:0x%x color_effect:%s u:%d v:%d ret %d(%d)\n", + mmal_ctrl, ctrl->id, ctrl->val, imagefx.effect, + dev->colourfx.enable ? "true" : "false", + dev->colourfx.u, dev->colourfx.v, + ret, (ret == 0 ? 0 : -EINVAL)); + return (ret == 0 ? 0 : -EINVAL); +} + +static int ctrl_set_colfx(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + int ret; + struct vchiq_mmal_port *control; + + control = &dev->component[COMP_CAMERA]->control; + + dev->colourfx.u = (ctrl->val & 0xff00) >> 8; + dev->colourfx.v = ctrl->val & 0xff; + + ret = vchiq_mmal_port_parameter_set(dev->instance, control, + MMAL_PARAMETER_COLOUR_EFFECT, + &dev->colourfx, + sizeof(dev->colourfx)); + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s: After: mmal_ctrl:%p ctrl id:0x%x ctrl val:%d ret %d(%d)\n", + __func__, mmal_ctrl, ctrl->id, ctrl->val, ret, + (ret == 0 ? 0 : -EINVAL)); + return (ret == 0 ? 0 : -EINVAL); +} + +static int ctrl_set_bitrate(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + int ret; + struct vchiq_mmal_port *encoder_out; + + dev->capture.encode_bitrate = ctrl->val; + + encoder_out = &dev->component[COMP_VIDEO_ENCODE]->output[0]; + + ret = vchiq_mmal_port_parameter_set(dev->instance, encoder_out, + mmal_ctrl->mmal_id, &ctrl->val, + sizeof(ctrl->val)); + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s: After: mmal_ctrl:%p ctrl id:0x%x ctrl val:%d ret %d(%d)\n", + __func__, mmal_ctrl, ctrl->id, ctrl->val, ret, + (ret == 0 ? 0 : -EINVAL)); + + /* + * Older firmware versions (pre July 2019) have a bug in handling + * MMAL_PARAMETER_VIDEO_BIT_RATE that result in the call + * returning -MMAL_MSG_STATUS_EINVAL. So ignore errors from this call. + */ + return 0; +} + +static int ctrl_set_bitrate_mode(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + u32 bitrate_mode; + struct vchiq_mmal_port *encoder_out; + + encoder_out = &dev->component[COMP_VIDEO_ENCODE]->output[0]; + + dev->capture.encode_bitrate_mode = ctrl->val; + switch (ctrl->val) { + default: + case V4L2_MPEG_VIDEO_BITRATE_MODE_VBR: + bitrate_mode = MMAL_VIDEO_RATECONTROL_VARIABLE; + break; + case V4L2_MPEG_VIDEO_BITRATE_MODE_CBR: + bitrate_mode = MMAL_VIDEO_RATECONTROL_CONSTANT; + break; + } + + vchiq_mmal_port_parameter_set(dev->instance, encoder_out, + mmal_ctrl->mmal_id, + &bitrate_mode, + sizeof(bitrate_mode)); + return 0; +} + +static int ctrl_set_image_encode_output(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + u32 u32_value; + struct vchiq_mmal_port *jpeg_out; + + jpeg_out = &dev->component[COMP_IMAGE_ENCODE]->output[0]; + + u32_value = ctrl->val; + + return vchiq_mmal_port_parameter_set(dev->instance, jpeg_out, + mmal_ctrl->mmal_id, + &u32_value, sizeof(u32_value)); +} + +static int ctrl_set_video_encode_param_output(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + u32 u32_value; + struct vchiq_mmal_port *vid_enc_ctl; + + vid_enc_ctl = &dev->component[COMP_VIDEO_ENCODE]->output[0]; + + u32_value = ctrl->val; + + return vchiq_mmal_port_parameter_set(dev->instance, vid_enc_ctl, + mmal_ctrl->mmal_id, + &u32_value, sizeof(u32_value)); +} + +static int ctrl_set_video_encode_profile_level(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + struct mmal_parameter_video_profile param; + int ret = 0; + + if (ctrl->id == V4L2_CID_MPEG_VIDEO_H264_PROFILE) { + switch (ctrl->val) { + case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE: + case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE: + case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN: + case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH: + dev->capture.enc_profile = ctrl->val; + break; + default: + ret = -EINVAL; + break; + } + } else if (ctrl->id == V4L2_CID_MPEG_VIDEO_H264_LEVEL) { + switch (ctrl->val) { + case V4L2_MPEG_VIDEO_H264_LEVEL_1_0: + case V4L2_MPEG_VIDEO_H264_LEVEL_1B: + case V4L2_MPEG_VIDEO_H264_LEVEL_1_1: + case V4L2_MPEG_VIDEO_H264_LEVEL_1_2: + case V4L2_MPEG_VIDEO_H264_LEVEL_1_3: + case V4L2_MPEG_VIDEO_H264_LEVEL_2_0: + case V4L2_MPEG_VIDEO_H264_LEVEL_2_1: + case V4L2_MPEG_VIDEO_H264_LEVEL_2_2: + case V4L2_MPEG_VIDEO_H264_LEVEL_3_0: + case V4L2_MPEG_VIDEO_H264_LEVEL_3_1: + case V4L2_MPEG_VIDEO_H264_LEVEL_3_2: + case V4L2_MPEG_VIDEO_H264_LEVEL_4_0: + dev->capture.enc_level = ctrl->val; + break; + default: + ret = -EINVAL; + break; + } + } + + if (!ret) { + switch (dev->capture.enc_profile) { + case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE: + param.profile = MMAL_VIDEO_PROFILE_H264_BASELINE; + break; + case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE: + param.profile = + MMAL_VIDEO_PROFILE_H264_CONSTRAINED_BASELINE; + break; + case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN: + param.profile = MMAL_VIDEO_PROFILE_H264_MAIN; + break; + case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH: + param.profile = MMAL_VIDEO_PROFILE_H264_HIGH; + break; + default: + /* Should never get here */ + break; + } + + switch (dev->capture.enc_level) { + case V4L2_MPEG_VIDEO_H264_LEVEL_1_0: + param.level = MMAL_VIDEO_LEVEL_H264_1; + break; + case V4L2_MPEG_VIDEO_H264_LEVEL_1B: + param.level = MMAL_VIDEO_LEVEL_H264_1b; + break; + case V4L2_MPEG_VIDEO_H264_LEVEL_1_1: + param.level = MMAL_VIDEO_LEVEL_H264_11; + break; + case V4L2_MPEG_VIDEO_H264_LEVEL_1_2: + param.level = MMAL_VIDEO_LEVEL_H264_12; + break; + case V4L2_MPEG_VIDEO_H264_LEVEL_1_3: + param.level = MMAL_VIDEO_LEVEL_H264_13; + break; + case V4L2_MPEG_VIDEO_H264_LEVEL_2_0: + param.level = MMAL_VIDEO_LEVEL_H264_2; + break; + case V4L2_MPEG_VIDEO_H264_LEVEL_2_1: + param.level = MMAL_VIDEO_LEVEL_H264_21; + break; + case V4L2_MPEG_VIDEO_H264_LEVEL_2_2: + param.level = MMAL_VIDEO_LEVEL_H264_22; + break; + case V4L2_MPEG_VIDEO_H264_LEVEL_3_0: + param.level = MMAL_VIDEO_LEVEL_H264_3; + break; + case V4L2_MPEG_VIDEO_H264_LEVEL_3_1: + param.level = MMAL_VIDEO_LEVEL_H264_31; + break; + case V4L2_MPEG_VIDEO_H264_LEVEL_3_2: + param.level = MMAL_VIDEO_LEVEL_H264_32; + break; + case V4L2_MPEG_VIDEO_H264_LEVEL_4_0: + param.level = MMAL_VIDEO_LEVEL_H264_4; + break; + default: + /* Should never get here */ + break; + } + + ret = vchiq_mmal_port_parameter_set(dev->instance, + &dev->component[COMP_VIDEO_ENCODE]->output[0], + mmal_ctrl->mmal_id, + ¶m, sizeof(param)); + } + return ret; +} + +static int ctrl_set_scene_mode(struct bcm2835_mmal_dev *dev, + struct v4l2_ctrl *ctrl, + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl) +{ + int ret = 0; + int shutter_speed; + struct vchiq_mmal_port *control; + + v4l2_dbg(0, bcm2835_v4l2_debug, &dev->v4l2_dev, + "scene mode selected %d, was %d\n", ctrl->val, + dev->scene_mode); + control = &dev->component[COMP_CAMERA]->control; + + if (ctrl->val == dev->scene_mode) + return 0; + + if (ctrl->val == V4L2_SCENE_MODE_NONE) { + /* Restore all user selections */ + dev->scene_mode = V4L2_SCENE_MODE_NONE; + + if (dev->exposure_mode_user == MMAL_PARAM_EXPOSUREMODE_OFF) + shutter_speed = dev->manual_shutter_speed; + else + shutter_speed = 0; + + v4l2_dbg(0, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s: scene mode none: shut_speed %d, exp_mode %d, metering %d\n", + __func__, shutter_speed, dev->exposure_mode_user, + dev->metering_mode); + ret = vchiq_mmal_port_parameter_set(dev->instance, + control, + MMAL_PARAMETER_SHUTTER_SPEED, + &shutter_speed, + sizeof(shutter_speed)); + ret += vchiq_mmal_port_parameter_set(dev->instance, + control, + MMAL_PARAMETER_EXPOSURE_MODE, + &dev->exposure_mode_user, + sizeof(u32)); + dev->exposure_mode_active = dev->exposure_mode_user; + ret += vchiq_mmal_port_parameter_set(dev->instance, + control, + MMAL_PARAMETER_EXP_METERING_MODE, + &dev->metering_mode, + sizeof(u32)); + ret += set_framerate_params(dev); + } else { + /* Set up scene mode */ + int i; + const struct v4l2_mmal_scene_config *scene = NULL; + int shutter_speed; + enum mmal_parameter_exposuremode exposure_mode; + enum mmal_parameter_exposuremeteringmode metering_mode; + + for (i = 0; i < ARRAY_SIZE(scene_configs); i++) { + if (scene_configs[i].v4l2_scene == ctrl->val) { + scene = &scene_configs[i]; + break; + } + } + if (!scene) + return -EINVAL; + if (i >= ARRAY_SIZE(scene_configs)) + return -EINVAL; + + /* Set all the values */ + dev->scene_mode = ctrl->val; + + if (scene->exposure_mode == MMAL_PARAM_EXPOSUREMODE_OFF) + shutter_speed = dev->manual_shutter_speed; + else + shutter_speed = 0; + exposure_mode = scene->exposure_mode; + metering_mode = scene->metering_mode; + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s: scene mode none: shut_speed %d, exp_mode %d, metering %d\n", + __func__, shutter_speed, exposure_mode, metering_mode); + + ret = vchiq_mmal_port_parameter_set(dev->instance, control, + MMAL_PARAMETER_SHUTTER_SPEED, + &shutter_speed, + sizeof(shutter_speed)); + ret += vchiq_mmal_port_parameter_set(dev->instance, control, + MMAL_PARAMETER_EXPOSURE_MODE, + &exposure_mode, + sizeof(u32)); + dev->exposure_mode_active = exposure_mode; + ret += vchiq_mmal_port_parameter_set(dev->instance, control, + MMAL_PARAMETER_EXPOSURE_MODE, + &exposure_mode, + sizeof(u32)); + ret += vchiq_mmal_port_parameter_set(dev->instance, control, + MMAL_PARAMETER_EXP_METERING_MODE, + &metering_mode, + sizeof(u32)); + ret += set_framerate_params(dev); + } + if (ret) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "%s: Setting scene to %d, ret=%d\n", + __func__, ctrl->val, ret); + ret = -EINVAL; + } + return 0; +} + +static int bcm2835_mmal_s_ctrl(struct v4l2_ctrl *ctrl) +{ + struct bcm2835_mmal_dev *dev = container_of(ctrl->handler, struct bcm2835_mmal_dev, + ctrl_handler); + const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl = ctrl->priv; + int ret; + + if (!mmal_ctrl || mmal_ctrl->id != ctrl->id || !mmal_ctrl->setter) { + pr_warn("mmal_ctrl:%p ctrl id:%d\n", mmal_ctrl, ctrl->id); + return -EINVAL; + } + + ret = mmal_ctrl->setter(dev, ctrl, mmal_ctrl); + if (ret) + pr_warn("ctrl id:%d/MMAL param %08X- returned ret %d\n", + ctrl->id, mmal_ctrl->mmal_id, ret); + return ret; +} + +static const struct v4l2_ctrl_ops bcm2835_mmal_ctrl_ops = { + .s_ctrl = bcm2835_mmal_s_ctrl, +}; + +static const struct bcm2835_mmal_v4l2_ctrl v4l2_ctrls[V4L2_CTRL_COUNT] = { + { + .id = V4L2_CID_SATURATION, + .type = MMAL_CONTROL_TYPE_STD, + .min = -100, + .max = 100, + .def = 0, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_SATURATION, + .setter = ctrl_set_rational, + }, + { + .id = V4L2_CID_SHARPNESS, + .type = MMAL_CONTROL_TYPE_STD, + .min = -100, + .max = 100, + .def = 0, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_SHARPNESS, + .setter = ctrl_set_rational, + }, + { + .id = V4L2_CID_CONTRAST, + .type = MMAL_CONTROL_TYPE_STD, + .min = -100, + .max = 100, + .def = 0, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_CONTRAST, + .setter = ctrl_set_rational, + }, + { + .id = V4L2_CID_BRIGHTNESS, + .type = MMAL_CONTROL_TYPE_STD, + .min = 0, + .max = 100, + .def = 50, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_BRIGHTNESS, + .setter = ctrl_set_rational, + }, + { + .id = V4L2_CID_ISO_SENSITIVITY, + .type = MMAL_CONTROL_TYPE_INT_MENU, + .min = 0, + .max = ARRAY_SIZE(iso_qmenu) - 1, + .def = 0, + .step = 1, + .imenu = iso_qmenu, + .mmal_id = MMAL_PARAMETER_ISO, + .setter = ctrl_set_iso, + }, + { + .id = V4L2_CID_ISO_SENSITIVITY_AUTO, + .type = MMAL_CONTROL_TYPE_STD_MENU, + .min = 0, + .max = V4L2_ISO_SENSITIVITY_AUTO, + .def = V4L2_ISO_SENSITIVITY_AUTO, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_ISO, + .setter = ctrl_set_iso, + }, + { + .id = V4L2_CID_IMAGE_STABILIZATION, + .type = MMAL_CONTROL_TYPE_STD, + .min = 0, + .max = 1, + .def = 0, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_VIDEO_STABILISATION, + .setter = ctrl_set_value, + }, + { + .id = V4L2_CID_EXPOSURE_AUTO, + .type = MMAL_CONTROL_TYPE_STD_MENU, + .min = ~0x03, + .max = V4L2_EXPOSURE_APERTURE_PRIORITY, + .def = V4L2_EXPOSURE_AUTO, + .step = 0, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_EXPOSURE_MODE, + .setter = ctrl_set_exposure, + }, + { + .id = V4L2_CID_EXPOSURE_ABSOLUTE, + .type = MMAL_CONTROL_TYPE_STD, + /* Units of 100usecs */ + .min = 1, + .max = 1 * 1000 * 10, + .def = 100 * 10, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_SHUTTER_SPEED, + .setter = ctrl_set_exposure, + }, + { + .id = V4L2_CID_AUTO_EXPOSURE_BIAS, + .type = MMAL_CONTROL_TYPE_INT_MENU, + .min = 0, + .max = ARRAY_SIZE(ev_bias_qmenu) - 1, + .def = (ARRAY_SIZE(ev_bias_qmenu) + 1) / 2 - 1, + .step = 0, + .imenu = ev_bias_qmenu, + .mmal_id = MMAL_PARAMETER_EXPOSURE_COMP, + .setter = ctrl_set_value_ev, + }, + { + .id = V4L2_CID_EXPOSURE_AUTO_PRIORITY, + .type = MMAL_CONTROL_TYPE_STD, + .min = 0, + .max = 1, + .def = 0, + .step = 1, + .imenu = NULL, + /* Dummy MMAL ID as it gets mapped into FPS range */ + .mmal_id = 0, + .setter = ctrl_set_exposure, + }, + { + .id = V4L2_CID_EXPOSURE_METERING, + .type = MMAL_CONTROL_TYPE_STD_MENU, + .min = ~0xf, + .max = V4L2_EXPOSURE_METERING_MATRIX, + .def = V4L2_EXPOSURE_METERING_AVERAGE, + .step = 0, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_EXP_METERING_MODE, + .setter = ctrl_set_metering_mode, + }, + { + .id = V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE, + .type = MMAL_CONTROL_TYPE_STD_MENU, + .min = ~0x3ff, + .max = V4L2_WHITE_BALANCE_SHADE, + .def = V4L2_WHITE_BALANCE_AUTO, + .step = 0, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_AWB_MODE, + .setter = ctrl_set_awb_mode, + }, + { + .id = V4L2_CID_RED_BALANCE, + .type = MMAL_CONTROL_TYPE_STD, + .min = 1, + .max = 7999, + .def = 1000, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_CUSTOM_AWB_GAINS, + .setter = ctrl_set_awb_gains, + }, + { + .id = V4L2_CID_BLUE_BALANCE, + .type = MMAL_CONTROL_TYPE_STD, + .min = 1, + .max = 7999, + .def = 1000, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_CUSTOM_AWB_GAINS, + .setter = ctrl_set_awb_gains, + }, + { + .id = V4L2_CID_COLORFX, + .type = MMAL_CONTROL_TYPE_STD_MENU, + .min = 0, + .max = V4L2_COLORFX_SET_CBCR, + .def = V4L2_COLORFX_NONE, + .step = 0, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_IMAGE_EFFECT, + .setter = ctrl_set_image_effect, + }, + { + .id = V4L2_CID_COLORFX_CBCR, + .type = MMAL_CONTROL_TYPE_STD, + .min = 0, + .max = 0xffff, + .def = 0x8080, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_COLOUR_EFFECT, + .setter = ctrl_set_colfx, + }, + { + .id = V4L2_CID_ROTATE, + .type = MMAL_CONTROL_TYPE_STD, + .min = 0, + .max = 360, + .def = 0, + .step = 90, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_ROTATION, + .setter = ctrl_set_rotate, + }, + { + .id = V4L2_CID_HFLIP, + .type = MMAL_CONTROL_TYPE_STD, + .min = 0, + .max = 1, + .def = 0, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_MIRROR, + .setter = ctrl_set_flip, + }, + { + .id = V4L2_CID_VFLIP, + .type = MMAL_CONTROL_TYPE_STD, + .min = 0, + .max = 1, + .def = 0, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_MIRROR, + .setter = ctrl_set_flip, + }, + { + .id = V4L2_CID_MPEG_VIDEO_BITRATE_MODE, + .type = MMAL_CONTROL_TYPE_STD_MENU, + .min = 0, + .max = V4L2_MPEG_VIDEO_BITRATE_MODE_CBR, + .def = 0, + .step = 0, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_RATECONTROL, + .setter = ctrl_set_bitrate_mode, + }, + { + .id = V4L2_CID_MPEG_VIDEO_BITRATE, + .type = MMAL_CONTROL_TYPE_STD, + .min = 25 * 1000, + .max = 25 * 1000 * 1000, + .def = 10 * 1000 * 1000, + .step = 25 * 1000, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_VIDEO_BIT_RATE, + .setter = ctrl_set_bitrate, + }, + { + .id = V4L2_CID_JPEG_COMPRESSION_QUALITY, + .type = MMAL_CONTROL_TYPE_STD, + .min = 1, + .max = 100, + .def = 30, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_JPEG_Q_FACTOR, + .setter = ctrl_set_image_encode_output, + }, + { + .id = V4L2_CID_POWER_LINE_FREQUENCY, + .type = MMAL_CONTROL_TYPE_STD_MENU, + .min = 0, + .max = V4L2_CID_POWER_LINE_FREQUENCY_AUTO, + .def = 1, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_FLICKER_AVOID, + .setter = ctrl_set_flicker_avoidance, + }, + { + .id = V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER, + .type = MMAL_CONTROL_TYPE_STD, + .min = 0, + .max = 1, + .def = 0, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_VIDEO_ENCODE_INLINE_HEADER, + .setter = ctrl_set_video_encode_param_output, + }, + { + .id = V4L2_CID_MPEG_VIDEO_H264_PROFILE, + .type = MMAL_CONTROL_TYPE_STD_MENU, + .min = ~(BIT(V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) | + BIT(V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE) | + BIT(V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) | + BIT(V4L2_MPEG_VIDEO_H264_PROFILE_HIGH)), + .max = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH, + .def = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_PROFILE, + .setter = ctrl_set_video_encode_profile_level, + }, + { + .id = V4L2_CID_MPEG_VIDEO_H264_LEVEL, + .type = MMAL_CONTROL_TYPE_STD_MENU, + .min = ~(BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_0) | + BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1B) | + BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_1) | + BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_2) | + BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_3) | + BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_0) | + BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_1) | + BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_2) | + BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_0) | + BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_1) | + BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_2) | + BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_0)), + .max = V4L2_MPEG_VIDEO_H264_LEVEL_4_0, + .def = V4L2_MPEG_VIDEO_H264_LEVEL_4_0, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_PROFILE, + .setter = ctrl_set_video_encode_profile_level, + }, + { + .id = V4L2_CID_SCENE_MODE, + .type = MMAL_CONTROL_TYPE_STD_MENU, + /* mask is computed at runtime */ + .min = -1, + .max = V4L2_SCENE_MODE_TEXT, + .def = V4L2_SCENE_MODE_NONE, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_PROFILE, + .setter = ctrl_set_scene_mode, + }, + { + .id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD, + .type = MMAL_CONTROL_TYPE_STD, + .min = 0, + .max = 0x7FFFFFFF, + .def = 60, + .step = 1, + .imenu = NULL, + .mmal_id = MMAL_PARAMETER_INTRAPERIOD, + .setter = ctrl_set_video_encode_param_output, + }, +}; + +int bcm2835_mmal_set_all_camera_controls(struct bcm2835_mmal_dev *dev) +{ + int c; + int ret = 0; + + for (c = 0; c < V4L2_CTRL_COUNT; c++) { + if ((dev->ctrls[c]) && (v4l2_ctrls[c].setter)) { + ret = v4l2_ctrls[c].setter(dev, dev->ctrls[c], + &v4l2_ctrls[c]); + if (ret) { + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Failed when setting default values for ctrl %d\n", + c); + break; + } + } + } + return ret; +} + +int set_framerate_params(struct bcm2835_mmal_dev *dev) +{ + struct mmal_parameter_fps_range fps_range; + int ret; + + fps_range.fps_high.numerator = dev->capture.timeperframe.denominator; + fps_range.fps_high.denominator = dev->capture.timeperframe.numerator; + + if ((dev->exposure_mode_active != MMAL_PARAM_EXPOSUREMODE_OFF) && + (dev->exp_auto_priority)) { + /* Variable FPS. Define min FPS as 1fps. */ + fps_range.fps_low.numerator = 1; + fps_range.fps_low.denominator = 1; + } else { + /* Fixed FPS - set min and max to be the same */ + fps_range.fps_low.numerator = fps_range.fps_high.numerator; + fps_range.fps_low.denominator = fps_range.fps_high.denominator; + } + + v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Set fps range to %d/%d to %d/%d\n", + fps_range.fps_low.numerator, + fps_range.fps_low.denominator, + fps_range.fps_high.numerator, + fps_range.fps_high.denominator); + + ret = vchiq_mmal_port_parameter_set(dev->instance, + &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW], + MMAL_PARAMETER_FPS_RANGE, + &fps_range, sizeof(fps_range)); + ret += vchiq_mmal_port_parameter_set(dev->instance, + &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO], + MMAL_PARAMETER_FPS_RANGE, + &fps_range, sizeof(fps_range)); + ret += vchiq_mmal_port_parameter_set(dev->instance, + &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE], + MMAL_PARAMETER_FPS_RANGE, + &fps_range, sizeof(fps_range)); + if (ret) + v4l2_dbg(0, bcm2835_v4l2_debug, &dev->v4l2_dev, + "Failed to set fps ret %d\n", ret); + + return ret; +} + +int bcm2835_mmal_init_controls(struct bcm2835_mmal_dev *dev, struct v4l2_ctrl_handler *hdl) +{ + int c; + const struct bcm2835_mmal_v4l2_ctrl *ctrl; + + v4l2_ctrl_handler_init(hdl, V4L2_CTRL_COUNT); + + for (c = 0; c < V4L2_CTRL_COUNT; c++) { + ctrl = &v4l2_ctrls[c]; + + switch (ctrl->type) { + case MMAL_CONTROL_TYPE_STD: + dev->ctrls[c] = v4l2_ctrl_new_std(hdl, &bcm2835_mmal_ctrl_ops, + ctrl->id, ctrl->min, ctrl->max, + ctrl->step, ctrl->def); + break; + + case MMAL_CONTROL_TYPE_STD_MENU: + { + u64 mask = ctrl->min; + + if (ctrl->id == V4L2_CID_SCENE_MODE) { + /* Special handling to work out the mask + * value based on the scene_configs array + * at runtime. Reduces the chance of + * mismatches. + */ + int i; + + mask = BIT(V4L2_SCENE_MODE_NONE); + for (i = 0; + i < ARRAY_SIZE(scene_configs); + i++) { + mask |= BIT(scene_configs[i].v4l2_scene); + } + mask = ~mask; + } + + dev->ctrls[c] = v4l2_ctrl_new_std_menu(hdl, &bcm2835_mmal_ctrl_ops, + ctrl->id, ctrl->max, mask, + ctrl->def); + break; + } + + case MMAL_CONTROL_TYPE_INT_MENU: + dev->ctrls[c] = v4l2_ctrl_new_int_menu(hdl, &bcm2835_mmal_ctrl_ops, + ctrl->id, ctrl->max, + ctrl->def, ctrl->imenu); + break; + + case MMAL_CONTROL_TYPE_CLUSTER: + /* skip this entry when constructing controls */ + continue; + } + + if (hdl->error) + break; + + dev->ctrls[c]->priv = (void *)ctrl; + } + + if (hdl->error) { + pr_err("error adding control %d/%d id 0x%x\n", c, + V4L2_CTRL_COUNT, ctrl->id); + return hdl->error; + } + + for (c = 0; c < V4L2_CTRL_COUNT; c++) { + ctrl = &v4l2_ctrls[c]; + + switch (ctrl->type) { + case MMAL_CONTROL_TYPE_CLUSTER: + v4l2_ctrl_auto_cluster(ctrl->min, + &dev->ctrls[c + 1], + ctrl->max, + ctrl->def); + break; + + case MMAL_CONTROL_TYPE_STD: + case MMAL_CONTROL_TYPE_STD_MENU: + case MMAL_CONTROL_TYPE_INT_MENU: + break; + } + } + + return 0; +} diff --git a/drivers/staging/vc04_services/include/linux/raspberrypi/vchiq.h b/drivers/staging/vc04_services/include/linux/raspberrypi/vchiq.h new file mode 100644 index 000000000..690ab7165 --- /dev/null +++ b/drivers/staging/vc04_services/include/linux/raspberrypi/vchiq.h @@ -0,0 +1,118 @@ +/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */ +/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */ + +#ifndef VCHIQ_H +#define VCHIQ_H + +#define VCHIQ_MAKE_FOURCC(x0, x1, x2, x3) \ + (((x0) << 24) | ((x1) << 16) | ((x2) << 8) | (x3)) + +enum vchiq_reason { + VCHIQ_SERVICE_OPENED, /* service, -, - */ + VCHIQ_SERVICE_CLOSED, /* service, -, - */ + VCHIQ_MESSAGE_AVAILABLE, /* service, header, - */ + VCHIQ_BULK_TRANSMIT_DONE, /* service, -, bulk_userdata */ + VCHIQ_BULK_RECEIVE_DONE, /* service, -, bulk_userdata */ + VCHIQ_BULK_TRANSMIT_ABORTED, /* service, -, bulk_userdata */ + VCHIQ_BULK_RECEIVE_ABORTED /* service, -, bulk_userdata */ +}; + +enum vchiq_status { + VCHIQ_ERROR = -1, + VCHIQ_SUCCESS = 0, + VCHIQ_RETRY = 1 +}; + +enum vchiq_bulk_mode { + VCHIQ_BULK_MODE_CALLBACK, + VCHIQ_BULK_MODE_BLOCKING, + VCHIQ_BULK_MODE_NOCALLBACK, + VCHIQ_BULK_MODE_WAITING /* Reserved for internal use */ +}; + +enum vchiq_service_option { + VCHIQ_SERVICE_OPTION_AUTOCLOSE, + VCHIQ_SERVICE_OPTION_SLOT_QUOTA, + VCHIQ_SERVICE_OPTION_MESSAGE_QUOTA, + VCHIQ_SERVICE_OPTION_SYNCHRONOUS, + VCHIQ_SERVICE_OPTION_TRACE +}; + +struct vchiq_header { + /* The message identifier - opaque to applications. */ + int msgid; + + /* Size of message data. */ + unsigned int size; + + char data[]; /* message */ +}; + +struct vchiq_element { + const void __user *data; + unsigned int size; +}; + +struct vchiq_instance; + +struct vchiq_service_base { + int fourcc; + enum vchiq_status (*callback)(struct vchiq_instance *instance, + enum vchiq_reason reason, + struct vchiq_header *header, + unsigned int handle, + void *bulk_userdata); + void *userdata; +}; + +struct vchiq_completion_data_kernel { + enum vchiq_reason reason; + struct vchiq_header *header; + void *service_userdata; + void *bulk_userdata; +}; + +struct vchiq_service_params_kernel { + int fourcc; + enum vchiq_status (*callback)(struct vchiq_instance *instance, + enum vchiq_reason reason, + struct vchiq_header *header, + unsigned int handle, + void *bulk_userdata); + void *userdata; + short version; /* Increment for non-trivial changes */ + short version_min; /* Update for incompatible changes */ +}; + +struct vchiq_instance; + +extern int vchiq_initialise(struct vchiq_instance **pinstance); +extern enum vchiq_status vchiq_shutdown(struct vchiq_instance *instance); +extern enum vchiq_status vchiq_connect(struct vchiq_instance *instance); +extern enum vchiq_status vchiq_open_service(struct vchiq_instance *instance, + const struct vchiq_service_params_kernel *params, + unsigned int *pservice); +extern enum vchiq_status vchiq_close_service(struct vchiq_instance *instance, + unsigned int service); +extern enum vchiq_status vchiq_use_service(struct vchiq_instance *instance, unsigned int service); +extern enum vchiq_status vchiq_release_service(struct vchiq_instance *instance, + unsigned int service); +extern void vchiq_msg_queue_push(struct vchiq_instance *instance, unsigned int handle, + struct vchiq_header *header); +extern void vchiq_release_message(struct vchiq_instance *instance, unsigned int service, + struct vchiq_header *header); +extern int vchiq_queue_kernel_message(struct vchiq_instance *instance, unsigned int handle, + void *data, unsigned int size); +extern enum vchiq_status vchiq_bulk_transmit(struct vchiq_instance *instance, unsigned int service, + const void *data, unsigned int size, void *userdata, + enum vchiq_bulk_mode mode); +extern enum vchiq_status vchiq_bulk_receive(struct vchiq_instance *instance, unsigned int service, + void *data, unsigned int size, void *userdata, + enum vchiq_bulk_mode mode); +extern void *vchiq_get_service_userdata(struct vchiq_instance *instance, unsigned int service); +extern enum vchiq_status vchiq_get_peer_version(struct vchiq_instance *instance, + unsigned int handle, + short *peer_version); +extern struct vchiq_header *vchiq_msg_hold(struct vchiq_instance *instance, unsigned int handle); + +#endif /* VCHIQ_H */ diff --git a/drivers/staging/vc04_services/interface/TESTING b/drivers/staging/vc04_services/interface/TESTING new file mode 100644 index 000000000..a6d63efcb --- /dev/null +++ b/drivers/staging/vc04_services/interface/TESTING @@ -0,0 +1,82 @@ +This document contains some hints to test the function of the VCHIQ driver +without having additional hardware to the Raspberry Pi. + +* Requirements & limitations + +Testing the VCHIQ driver requires a Raspberry Pi with one of the following SoC: + - BCM2835 ( e.g. Raspberry Pi Zero W ) + - BCM2836 ( e.g. Raspberry Pi 2 ) + - BCM2837 ( e.g. Raspberry Pi 3 B+ ) + +The BCM2711 used in the Raspberry Pi 4 is currently not supported in the +mainline kernel. + +There are no specific requirements to the VideoCore firmware to get VCHIQ +working. + +The test scenarios described in this document based on the tool vchiq_test. +Its source code is available here: https://github.com/raspberrypi/userland + +* Configuration + +Here are the most common kernel configurations: + + 1. BCM2835 target SoC (ARM 32 bit) + + Just use bcm2835_defconfig which already has VCHIQ enabled. + + 2. BCM2836/7 target SoC (ARM 32 bit) + + Use the multi_v7_defconfig as a base and then enable all VCHIQ options. + + 3. BCM2837 target SoC (ARM 64 bit) + + Use the defconfig as a base and then enable all VCHIQ options. + +* Scenarios + + * Initial test + + Check the driver is probed and /dev/vchiq is created + + * Functional test + + Command: vchiq_test -f 10 + + Expected output: + Functional test - iters:10 + ======== iteration 1 ======== + Testing bulk transfer for alignment. + Testing bulk transfer at PAGE_SIZE. + ... + + * Ping test + + Command: vchiq_test -p 1 + + Expected output: + Ping test - service:echo, iters:1, version 3 + vchi ping (size 0) -> 57.000000us + vchi ping (size 0, 0 async, 0 oneway) -> 122.000000us + vchi bulk (size 0, 0 async, 0 oneway) -> 546.000000us + vchi bulk (size 0, 0 oneway) -> 230.000000us + vchi ping (size 0) -> 49.000000us + vchi ping (size 0, 0 async, 0 oneway) -> 70.000000us + vchi bulk (size 0, 0 async, 0 oneway) -> 296.000000us + vchi bulk (size 0, 0 oneway) -> 266.000000us + vchi ping (size 0, 1 async, 0 oneway) -> 65.000000us + vchi bulk (size 0, 0 oneway) -> 456.000000us + vchi ping (size 0, 2 async, 0 oneway) -> 74.000000us + vchi bulk (size 0, 0 oneway) -> 640.000000us + vchi ping (size 0, 10 async, 0 oneway) -> 125.000000us + vchi bulk (size 0, 0 oneway) -> 2309.000000us + vchi ping (size 0, 0 async, 1 oneway) -> 70.000000us + vchi ping (size 0, 0 async, 2 oneway) -> 76.000000us + vchi ping (size 0, 0 async, 10 oneway) -> 105.000000us + vchi ping (size 0, 10 async, 10 oneway) -> 165.000000us + vchi ping (size 0, 100 async, 0 oneway) -> nanus + vchi bulk (size 0, 0 oneway) -> nanus + vchi ping (size 0, 0 async, 100 oneway) -> nanus + vchi ping (size 0, 100 async, 100 oneway) -> infus + vchi ping (size 0, 200 async, 0 oneway) -> infus + ... diff --git a/drivers/staging/vc04_services/interface/TODO b/drivers/staging/vc04_services/interface/TODO new file mode 100644 index 000000000..97085a0b3 --- /dev/null +++ b/drivers/staging/vc04_services/interface/TODO @@ -0,0 +1,73 @@ +* Import drivers using VCHI. + +VCHI is just a tool to let drivers talk to the firmware. Here are +some of the ones we want: + + - vc_mem (https://github.com/raspberrypi/linux/blob/rpi-4.4.y/drivers/char/broadcom/vc_mem.c) + + This driver is what the vcdbg userspace program uses to set up its + requests to the firmware, which are transmitted across VCHIQ. vcdbg + is really useful for debugging firmware interactions. + + - VCSM (https://github.com/raspberrypi/linux/tree/rpi-4.4.y/drivers/char/broadcom/vc_sm) + + This driver is used for talking about regions of VC memory across + firmware protocols including VCHI. We'll want to extend this driver + to manage these buffers as dmabufs so that we can zero-copy import + camera images into vc4 for rendering/display. + +* Fix kernel module support + +Even the VPU firmware doesn't support a VCHI re-connect, the driver +should properly handle a module unload. This also includes that all +resources must be freed (kthreads, debugfs entries, ...) and global +variables avoided. + +* Cleanup logging mechanism + +The driver should probably be using the standard kernel logging mechanisms +such as dev_info, dev_dbg, and friends. + +* Documentation + +A short top-down description of this driver's architecture (function of +kthreads, userspace, limitations) could be very helpful for reviewers. + +* Review and comment memory barriers + +There is a heavy use of memory barriers in this driver, it would be very +beneficial to go over all of them and, if correct, comment on their merits. +Extra points to whomever confidently reviews the remote_event_*() family of +functions. + +* Get rid of custom function return values + +Most functions use a custom set of return values, we should force proper Linux +error numbers. Special care is needed for VCHIQ_RETRY. + +* Reformat core code with more sane indentations + +The code follows the 80 characters limitation yet tends to go 3 or 4 levels of +indentation deep making it very unpleasant to read. This is specially relevant +in the character driver ioctl code and in the core thread functions. + +* Get rid of all non essential global structures and create a proper per +device structure + +The first thing one generally sees in a probe function is a memory allocation +for all the device specific data. This structure is then passed all over the +driver. This is good practice since it makes the driver work regardless of the +number of devices probed. + +* Clean up Sparse warnings from __user annotations. See +vchiq_irq_queue_bulk_tx_rx(). Ensure that the address of "&waiter->bulk_waiter" +is never disclosed to userspace. + +* Fix behavior of message handling + +The polling behavior of vchiq_bulk_transmit(), vchiq_bulk_receive() and +vchiq_queue_kernel_message() looks broken. A possible signal should be +propagated back to user space to let the calling task handle it before +retrying. Hopefully these msleep(1) shouldn't be necessary anymore. + +https://lore.kernel.org/linux-staging/CAK8P3a3HGm1cPo4sW9fOY4E8AN8yAq3tevXxU5m8bmtmsU8WKw@mail.gmail.com/ diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c new file mode 100644 index 000000000..705c5e283 --- /dev/null +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c @@ -0,0 +1,1885 @@ +// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause +/* + * Copyright (c) 2014 Raspberry Pi (Trading) Ltd. All rights reserved. + * Copyright (c) 2010-2012 Broadcom. All rights reserved. + */ + +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/sched/signal.h> +#include <linux/types.h> +#include <linux/errno.h> +#include <linux/cdev.h> +#include <linux/fs.h> +#include <linux/device.h> +#include <linux/mm.h> +#include <linux/highmem.h> +#include <linux/pagemap.h> +#include <linux/bug.h> +#include <linux/completion.h> +#include <linux/list.h> +#include <linux/of.h> +#include <linux/platform_device.h> +#include <linux/compat.h> +#include <linux/dma-mapping.h> +#include <linux/rcupdate.h> +#include <linux/delay.h> +#include <linux/slab.h> +#include <linux/interrupt.h> +#include <linux/io.h> +#include <linux/uaccess.h> +#include <soc/bcm2835/raspberrypi-firmware.h> + +#include "vchiq_core.h" +#include "vchiq_ioctl.h" +#include "vchiq_arm.h" +#include "vchiq_debugfs.h" +#include "vchiq_connected.h" +#include "vchiq_pagelist.h" + +#define DEVICE_NAME "vchiq" + +#define TOTAL_SLOTS (VCHIQ_SLOT_ZERO_SLOTS + 2 * 32) + +#define MAX_FRAGMENTS (VCHIQ_NUM_CURRENT_BULKS * 2) + +#define VCHIQ_PLATFORM_FRAGMENTS_OFFSET_IDX 0 +#define VCHIQ_PLATFORM_FRAGMENTS_COUNT_IDX 1 + +#define BELL0 0x00 +#define BELL2 0x08 + +#define ARM_DS_ACTIVE BIT(2) + +/* Override the default prefix, which would be vchiq_arm (from the filename) */ +#undef MODULE_PARAM_PREFIX +#define MODULE_PARAM_PREFIX DEVICE_NAME "." + +#define KEEPALIVE_VER 1 +#define KEEPALIVE_VER_MIN KEEPALIVE_VER + +/* Run time control of log level, based on KERN_XXX level. */ +int vchiq_arm_log_level = VCHIQ_LOG_DEFAULT; +int vchiq_susp_log_level = VCHIQ_LOG_ERROR; + +DEFINE_SPINLOCK(msg_queue_spinlock); +struct vchiq_state g_state; + +static struct platform_device *bcm2835_camera; +static struct platform_device *bcm2835_audio; + +struct vchiq_drvdata { + const unsigned int cache_line_size; + struct rpi_firmware *fw; +}; + +static struct vchiq_drvdata bcm2835_drvdata = { + .cache_line_size = 32, +}; + +static struct vchiq_drvdata bcm2836_drvdata = { + .cache_line_size = 64, +}; + +struct vchiq_arm_state { + /* Keepalive-related data */ + struct task_struct *ka_thread; + struct completion ka_evt; + atomic_t ka_use_count; + atomic_t ka_use_ack_count; + atomic_t ka_release_count; + + rwlock_t susp_res_lock; + + struct vchiq_state *state; + + /* + * Global use count for videocore. + * This is equal to the sum of the use counts for all services. When + * this hits zero the videocore suspend procedure will be initiated. + */ + int videocore_use_count; + + /* + * Use count to track requests from videocore peer. + * This use count is not associated with a service, so needs to be + * tracked separately with the state. + */ + int peer_use_count; + + /* + * Flag to indicate that the first vchiq connect has made it through. + * This means that both sides should be fully ready, and we should + * be able to suspend after this point. + */ + int first_connect; +}; + +struct vchiq_2835_state { + int inited; + struct vchiq_arm_state arm_state; +}; + +struct vchiq_pagelist_info { + struct pagelist *pagelist; + size_t pagelist_buffer_size; + dma_addr_t dma_addr; + enum dma_data_direction dma_dir; + unsigned int num_pages; + unsigned int pages_need_release; + struct page **pages; + struct scatterlist *scatterlist; + unsigned int scatterlist_mapped; +}; + +static void __iomem *g_regs; +/* This value is the size of the L2 cache lines as understood by the + * VPU firmware, which determines the required alignment of the + * offsets/sizes in pagelists. + * + * Modern VPU firmware looks for a DT "cache-line-size" property in + * the VCHIQ node and will overwrite it with the actual L2 cache size, + * which the kernel must then respect. That property was rejected + * upstream, so we have to use the VPU firmware's compatibility value + * of 32. + */ +static unsigned int g_cache_line_size = 32; +static unsigned int g_fragments_size; +static char *g_fragments_base; +static char *g_free_fragments; +static struct semaphore g_free_fragments_sema; + +static DEFINE_SEMAPHORE(g_free_fragments_mutex); + +static enum vchiq_status +vchiq_blocking_bulk_transfer(struct vchiq_instance *instance, unsigned int handle, void *data, + unsigned int size, enum vchiq_bulk_dir dir); + +static irqreturn_t +vchiq_doorbell_irq(int irq, void *dev_id) +{ + struct vchiq_state *state = dev_id; + irqreturn_t ret = IRQ_NONE; + unsigned int status; + + /* Read (and clear) the doorbell */ + status = readl(g_regs + BELL0); + + if (status & ARM_DS_ACTIVE) { /* Was the doorbell rung? */ + remote_event_pollall(state); + ret = IRQ_HANDLED; + } + + return ret; +} + +static void +cleanup_pagelistinfo(struct vchiq_instance *instance, struct vchiq_pagelist_info *pagelistinfo) +{ + if (pagelistinfo->scatterlist_mapped) { + dma_unmap_sg(instance->state->dev, pagelistinfo->scatterlist, + pagelistinfo->num_pages, pagelistinfo->dma_dir); + } + + if (pagelistinfo->pages_need_release) + unpin_user_pages(pagelistinfo->pages, pagelistinfo->num_pages); + + dma_free_coherent(instance->state->dev, pagelistinfo->pagelist_buffer_size, + pagelistinfo->pagelist, pagelistinfo->dma_addr); +} + +static inline bool +is_adjacent_block(u32 *addrs, u32 addr, unsigned int k) +{ + u32 tmp; + + if (!k) + return false; + + tmp = (addrs[k - 1] & PAGE_MASK) + + (((addrs[k - 1] & ~PAGE_MASK) + 1) << PAGE_SHIFT); + + return tmp == (addr & PAGE_MASK); +} + +/* There is a potential problem with partial cache lines (pages?) + * at the ends of the block when reading. If the CPU accessed anything in + * the same line (page?) then it may have pulled old data into the cache, + * obscuring the new data underneath. We can solve this by transferring the + * partial cache lines separately, and allowing the ARM to copy into the + * cached area. + */ + +static struct vchiq_pagelist_info * +create_pagelist(struct vchiq_instance *instance, char *buf, char __user *ubuf, + size_t count, unsigned short type) +{ + struct pagelist *pagelist; + struct vchiq_pagelist_info *pagelistinfo; + struct page **pages; + u32 *addrs; + unsigned int num_pages, offset, i, k; + int actual_pages; + size_t pagelist_size; + struct scatterlist *scatterlist, *sg; + int dma_buffers; + dma_addr_t dma_addr; + + if (count >= INT_MAX - PAGE_SIZE) + return NULL; + + if (buf) + offset = (uintptr_t)buf & (PAGE_SIZE - 1); + else + offset = (uintptr_t)ubuf & (PAGE_SIZE - 1); + num_pages = DIV_ROUND_UP(count + offset, PAGE_SIZE); + + if ((size_t)num_pages > (SIZE_MAX - sizeof(struct pagelist) - + sizeof(struct vchiq_pagelist_info)) / + (sizeof(u32) + sizeof(pages[0]) + + sizeof(struct scatterlist))) + return NULL; + + pagelist_size = sizeof(struct pagelist) + + (num_pages * sizeof(u32)) + + (num_pages * sizeof(pages[0]) + + (num_pages * sizeof(struct scatterlist))) + + sizeof(struct vchiq_pagelist_info); + + /* Allocate enough storage to hold the page pointers and the page + * list + */ + pagelist = dma_alloc_coherent(instance->state->dev, pagelist_size, &dma_addr, + GFP_KERNEL); + + vchiq_log_trace(vchiq_arm_log_level, "%s - %pK", __func__, pagelist); + + if (!pagelist) + return NULL; + + addrs = pagelist->addrs; + pages = (struct page **)(addrs + num_pages); + scatterlist = (struct scatterlist *)(pages + num_pages); + pagelistinfo = (struct vchiq_pagelist_info *) + (scatterlist + num_pages); + + pagelist->length = count; + pagelist->type = type; + pagelist->offset = offset; + + /* Populate the fields of the pagelistinfo structure */ + pagelistinfo->pagelist = pagelist; + pagelistinfo->pagelist_buffer_size = pagelist_size; + pagelistinfo->dma_addr = dma_addr; + pagelistinfo->dma_dir = (type == PAGELIST_WRITE) ? + DMA_TO_DEVICE : DMA_FROM_DEVICE; + pagelistinfo->num_pages = num_pages; + pagelistinfo->pages_need_release = 0; + pagelistinfo->pages = pages; + pagelistinfo->scatterlist = scatterlist; + pagelistinfo->scatterlist_mapped = 0; + + if (buf) { + unsigned long length = count; + unsigned int off = offset; + + for (actual_pages = 0; actual_pages < num_pages; + actual_pages++) { + struct page *pg = + vmalloc_to_page((buf + + (actual_pages * PAGE_SIZE))); + size_t bytes = PAGE_SIZE - off; + + if (!pg) { + cleanup_pagelistinfo(instance, pagelistinfo); + return NULL; + } + + if (bytes > length) + bytes = length; + pages[actual_pages] = pg; + length -= bytes; + off = 0; + } + /* do not try and release vmalloc pages */ + } else { + actual_pages = pin_user_pages_fast((unsigned long)ubuf & PAGE_MASK, num_pages, + type == PAGELIST_READ, pages); + + if (actual_pages != num_pages) { + vchiq_log_info(vchiq_arm_log_level, + "%s - only %d/%d pages locked", + __func__, actual_pages, num_pages); + + /* This is probably due to the process being killed */ + if (actual_pages > 0) + unpin_user_pages(pages, actual_pages); + cleanup_pagelistinfo(instance, pagelistinfo); + return NULL; + } + /* release user pages */ + pagelistinfo->pages_need_release = 1; + } + + /* + * Initialize the scatterlist so that the magic cookie + * is filled if debugging is enabled + */ + sg_init_table(scatterlist, num_pages); + /* Now set the pages for each scatterlist */ + for (i = 0; i < num_pages; i++) { + unsigned int len = PAGE_SIZE - offset; + + if (len > count) + len = count; + sg_set_page(scatterlist + i, pages[i], len, offset); + offset = 0; + count -= len; + } + + dma_buffers = dma_map_sg(instance->state->dev, + scatterlist, + num_pages, + pagelistinfo->dma_dir); + + if (dma_buffers == 0) { + cleanup_pagelistinfo(instance, pagelistinfo); + return NULL; + } + + pagelistinfo->scatterlist_mapped = 1; + + /* Combine adjacent blocks for performance */ + k = 0; + for_each_sg(scatterlist, sg, dma_buffers, i) { + u32 len = sg_dma_len(sg); + u32 addr = sg_dma_address(sg); + + /* Note: addrs is the address + page_count - 1 + * The firmware expects blocks after the first to be page- + * aligned and a multiple of the page size + */ + WARN_ON(len == 0); + WARN_ON(i && (i != (dma_buffers - 1)) && (len & ~PAGE_MASK)); + WARN_ON(i && (addr & ~PAGE_MASK)); + if (is_adjacent_block(addrs, addr, k)) + addrs[k - 1] += ((len + PAGE_SIZE - 1) >> PAGE_SHIFT); + else + addrs[k++] = (addr & PAGE_MASK) | + (((len + PAGE_SIZE - 1) >> PAGE_SHIFT) - 1); + } + + /* Partial cache lines (fragments) require special measures */ + if ((type == PAGELIST_READ) && + ((pagelist->offset & (g_cache_line_size - 1)) || + ((pagelist->offset + pagelist->length) & + (g_cache_line_size - 1)))) { + char *fragments; + + if (down_interruptible(&g_free_fragments_sema)) { + cleanup_pagelistinfo(instance, pagelistinfo); + return NULL; + } + + WARN_ON(!g_free_fragments); + + down(&g_free_fragments_mutex); + fragments = g_free_fragments; + WARN_ON(!fragments); + g_free_fragments = *(char **)g_free_fragments; + up(&g_free_fragments_mutex); + pagelist->type = PAGELIST_READ_WITH_FRAGMENTS + + (fragments - g_fragments_base) / g_fragments_size; + } + + return pagelistinfo; +} + +static void +free_pagelist(struct vchiq_instance *instance, struct vchiq_pagelist_info *pagelistinfo, + int actual) +{ + struct pagelist *pagelist = pagelistinfo->pagelist; + struct page **pages = pagelistinfo->pages; + unsigned int num_pages = pagelistinfo->num_pages; + + vchiq_log_trace(vchiq_arm_log_level, "%s - %pK, %d", + __func__, pagelistinfo->pagelist, actual); + + /* + * NOTE: dma_unmap_sg must be called before the + * cpu can touch any of the data/pages. + */ + dma_unmap_sg(instance->state->dev, pagelistinfo->scatterlist, + pagelistinfo->num_pages, pagelistinfo->dma_dir); + pagelistinfo->scatterlist_mapped = 0; + + /* Deal with any partial cache lines (fragments) */ + if (pagelist->type >= PAGELIST_READ_WITH_FRAGMENTS && g_fragments_base) { + char *fragments = g_fragments_base + + (pagelist->type - PAGELIST_READ_WITH_FRAGMENTS) * + g_fragments_size; + int head_bytes, tail_bytes; + + head_bytes = (g_cache_line_size - pagelist->offset) & + (g_cache_line_size - 1); + tail_bytes = (pagelist->offset + actual) & + (g_cache_line_size - 1); + + if ((actual >= 0) && (head_bytes != 0)) { + if (head_bytes > actual) + head_bytes = actual; + + memcpy_to_page(pages[0], + pagelist->offset, + fragments, + head_bytes); + } + if ((actual >= 0) && (head_bytes < actual) && + (tail_bytes != 0)) + memcpy_to_page(pages[num_pages - 1], + (pagelist->offset + actual) & + (PAGE_SIZE - 1) & ~(g_cache_line_size - 1), + fragments + g_cache_line_size, + tail_bytes); + + down(&g_free_fragments_mutex); + *(char **)fragments = g_free_fragments; + g_free_fragments = fragments; + up(&g_free_fragments_mutex); + up(&g_free_fragments_sema); + } + + /* Need to mark all the pages dirty. */ + if (pagelist->type != PAGELIST_WRITE && + pagelistinfo->pages_need_release) { + unsigned int i; + + for (i = 0; i < num_pages; i++) + set_page_dirty(pages[i]); + } + + cleanup_pagelistinfo(instance, pagelistinfo); +} + +static int vchiq_platform_init(struct platform_device *pdev, struct vchiq_state *state) +{ + struct device *dev = &pdev->dev; + struct vchiq_drvdata *drvdata = platform_get_drvdata(pdev); + struct rpi_firmware *fw = drvdata->fw; + struct vchiq_slot_zero *vchiq_slot_zero; + void *slot_mem; + dma_addr_t slot_phys; + u32 channelbase; + int slot_mem_size, frag_mem_size; + int err, irq, i; + + /* + * VCHI messages between the CPU and firmware use + * 32-bit bus addresses. + */ + err = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32)); + + if (err < 0) + return err; + + g_cache_line_size = drvdata->cache_line_size; + g_fragments_size = 2 * g_cache_line_size; + + /* Allocate space for the channels in coherent memory */ + slot_mem_size = PAGE_ALIGN(TOTAL_SLOTS * VCHIQ_SLOT_SIZE); + frag_mem_size = PAGE_ALIGN(g_fragments_size * MAX_FRAGMENTS); + + slot_mem = dmam_alloc_coherent(dev, slot_mem_size + frag_mem_size, + &slot_phys, GFP_KERNEL); + if (!slot_mem) { + dev_err(dev, "could not allocate DMA memory\n"); + return -ENOMEM; + } + + WARN_ON(((unsigned long)slot_mem & (PAGE_SIZE - 1)) != 0); + + vchiq_slot_zero = vchiq_init_slots(slot_mem, slot_mem_size); + if (!vchiq_slot_zero) + return -EINVAL; + + vchiq_slot_zero->platform_data[VCHIQ_PLATFORM_FRAGMENTS_OFFSET_IDX] = + (int)slot_phys + slot_mem_size; + vchiq_slot_zero->platform_data[VCHIQ_PLATFORM_FRAGMENTS_COUNT_IDX] = + MAX_FRAGMENTS; + + g_fragments_base = (char *)slot_mem + slot_mem_size; + + g_free_fragments = g_fragments_base; + for (i = 0; i < (MAX_FRAGMENTS - 1); i++) { + *(char **)&g_fragments_base[i * g_fragments_size] = + &g_fragments_base[(i + 1) * g_fragments_size]; + } + *(char **)&g_fragments_base[i * g_fragments_size] = NULL; + sema_init(&g_free_fragments_sema, MAX_FRAGMENTS); + + err = vchiq_init_state(state, vchiq_slot_zero, dev); + if (err) + return err; + + g_regs = devm_platform_ioremap_resource(pdev, 0); + if (IS_ERR(g_regs)) + return PTR_ERR(g_regs); + + irq = platform_get_irq(pdev, 0); + if (irq <= 0) + return irq; + + err = devm_request_irq(dev, irq, vchiq_doorbell_irq, IRQF_IRQPOLL, + "VCHIQ doorbell", state); + if (err) { + dev_err(dev, "failed to register irq=%d\n", irq); + return err; + } + + /* Send the base address of the slots to VideoCore */ + channelbase = slot_phys; + err = rpi_firmware_property(fw, RPI_FIRMWARE_VCHIQ_INIT, + &channelbase, sizeof(channelbase)); + if (err || channelbase) { + dev_err(dev, "failed to set channelbase\n"); + return err ? : -ENXIO; + } + + vchiq_log_info(vchiq_arm_log_level, "vchiq_init - done (slots %pK, phys %pad)", + vchiq_slot_zero, &slot_phys); + + vchiq_call_connected_callbacks(); + + return 0; +} + +static void +vchiq_arm_init_state(struct vchiq_state *state, + struct vchiq_arm_state *arm_state) +{ + if (arm_state) { + rwlock_init(&arm_state->susp_res_lock); + + init_completion(&arm_state->ka_evt); + atomic_set(&arm_state->ka_use_count, 0); + atomic_set(&arm_state->ka_use_ack_count, 0); + atomic_set(&arm_state->ka_release_count, 0); + + arm_state->state = state; + arm_state->first_connect = 0; + } +} + +int +vchiq_platform_init_state(struct vchiq_state *state) +{ + struct vchiq_2835_state *platform_state; + + state->platform_state = kzalloc(sizeof(*platform_state), GFP_KERNEL); + if (!state->platform_state) + return -ENOMEM; + + platform_state = (struct vchiq_2835_state *)state->platform_state; + + platform_state->inited = 1; + vchiq_arm_init_state(state, &platform_state->arm_state); + + return 0; +} + +static struct vchiq_arm_state *vchiq_platform_get_arm_state(struct vchiq_state *state) +{ + struct vchiq_2835_state *platform_state; + + platform_state = (struct vchiq_2835_state *)state->platform_state; + + WARN_ON_ONCE(!platform_state->inited); + + return &platform_state->arm_state; +} + +void +remote_event_signal(struct remote_event *event) +{ + /* + * Ensure that all writes to shared data structures have completed + * before signalling the peer. + */ + wmb(); + + event->fired = 1; + + dsb(sy); /* data barrier operation */ + + if (event->armed) + writel(0, g_regs + BELL2); /* trigger vc interrupt */ +} + +int +vchiq_prepare_bulk_data(struct vchiq_instance *instance, struct vchiq_bulk *bulk, void *offset, + void __user *uoffset, int size, int dir) +{ + struct vchiq_pagelist_info *pagelistinfo; + + pagelistinfo = create_pagelist(instance, offset, uoffset, size, + (dir == VCHIQ_BULK_RECEIVE) + ? PAGELIST_READ + : PAGELIST_WRITE); + + if (!pagelistinfo) + return -ENOMEM; + + bulk->data = pagelistinfo->dma_addr; + + /* + * Store the pagelistinfo address in remote_data, + * which isn't used by the slave. + */ + bulk->remote_data = pagelistinfo; + + return 0; +} + +void +vchiq_complete_bulk(struct vchiq_instance *instance, struct vchiq_bulk *bulk) +{ + if (bulk && bulk->remote_data && bulk->actual) + free_pagelist(instance, (struct vchiq_pagelist_info *)bulk->remote_data, + bulk->actual); +} + +int vchiq_dump_platform_state(void *dump_context) +{ + char buf[80]; + int len; + + len = snprintf(buf, sizeof(buf), " Platform: 2835 (VC master)"); + return vchiq_dump(dump_context, buf, len + 1); +} + +#define VCHIQ_INIT_RETRIES 10 +int vchiq_initialise(struct vchiq_instance **instance_out) +{ + struct vchiq_state *state; + struct vchiq_instance *instance = NULL; + int i, ret; + + /* + * VideoCore may not be ready due to boot up timing. + * It may never be ready if kernel and firmware are mismatched,so don't + * block forever. + */ + for (i = 0; i < VCHIQ_INIT_RETRIES; i++) { + state = vchiq_get_state(); + if (state) + break; + usleep_range(500, 600); + } + if (i == VCHIQ_INIT_RETRIES) { + vchiq_log_error(vchiq_core_log_level, "%s: videocore not initialized\n", __func__); + ret = -ENOTCONN; + goto failed; + } else if (i > 0) { + vchiq_log_warning(vchiq_core_log_level, + "%s: videocore initialized after %d retries\n", __func__, i); + } + + instance = kzalloc(sizeof(*instance), GFP_KERNEL); + if (!instance) { + vchiq_log_error(vchiq_core_log_level, + "%s: error allocating vchiq instance\n", __func__); + ret = -ENOMEM; + goto failed; + } + + instance->connected = 0; + instance->state = state; + mutex_init(&instance->bulk_waiter_list_mutex); + INIT_LIST_HEAD(&instance->bulk_waiter_list); + + *instance_out = instance; + + ret = 0; + +failed: + vchiq_log_trace(vchiq_core_log_level, "%s(%p): returning %d", __func__, instance, ret); + + return ret; +} +EXPORT_SYMBOL(vchiq_initialise); + +void free_bulk_waiter(struct vchiq_instance *instance) +{ + struct bulk_waiter_node *waiter, *next; + + list_for_each_entry_safe(waiter, next, + &instance->bulk_waiter_list, list) { + list_del(&waiter->list); + vchiq_log_info(vchiq_arm_log_level, "bulk_waiter - cleaned up %pK for pid %d", + waiter, waiter->pid); + kfree(waiter); + } +} + +enum vchiq_status vchiq_shutdown(struct vchiq_instance *instance) +{ + enum vchiq_status status = VCHIQ_SUCCESS; + struct vchiq_state *state = instance->state; + + if (mutex_lock_killable(&state->mutex)) + return VCHIQ_RETRY; + + /* Remove all services */ + vchiq_shutdown_internal(state, instance); + + mutex_unlock(&state->mutex); + + vchiq_log_trace(vchiq_core_log_level, "%s(%p): returning %d", __func__, instance, status); + + free_bulk_waiter(instance); + kfree(instance); + + return status; +} +EXPORT_SYMBOL(vchiq_shutdown); + +static int vchiq_is_connected(struct vchiq_instance *instance) +{ + return instance->connected; +} + +enum vchiq_status vchiq_connect(struct vchiq_instance *instance) +{ + enum vchiq_status status; + struct vchiq_state *state = instance->state; + + if (mutex_lock_killable(&state->mutex)) { + vchiq_log_trace(vchiq_core_log_level, "%s: call to mutex_lock failed", __func__); + status = VCHIQ_RETRY; + goto failed; + } + status = vchiq_connect_internal(state, instance); + + if (status == VCHIQ_SUCCESS) + instance->connected = 1; + + mutex_unlock(&state->mutex); + +failed: + vchiq_log_trace(vchiq_core_log_level, "%s(%p): returning %d", __func__, instance, status); + + return status; +} +EXPORT_SYMBOL(vchiq_connect); + +static enum vchiq_status +vchiq_add_service(struct vchiq_instance *instance, + const struct vchiq_service_params_kernel *params, + unsigned int *phandle) +{ + enum vchiq_status status; + struct vchiq_state *state = instance->state; + struct vchiq_service *service = NULL; + int srvstate; + + *phandle = VCHIQ_SERVICE_HANDLE_INVALID; + + srvstate = vchiq_is_connected(instance) + ? VCHIQ_SRVSTATE_LISTENING + : VCHIQ_SRVSTATE_HIDDEN; + + service = vchiq_add_service_internal(state, params, srvstate, instance, NULL); + + if (service) { + *phandle = service->handle; + status = VCHIQ_SUCCESS; + } else { + status = VCHIQ_ERROR; + } + + vchiq_log_trace(vchiq_core_log_level, "%s(%p): returning %d", __func__, instance, status); + + return status; +} + +enum vchiq_status +vchiq_open_service(struct vchiq_instance *instance, + const struct vchiq_service_params_kernel *params, + unsigned int *phandle) +{ + enum vchiq_status status = VCHIQ_ERROR; + struct vchiq_state *state = instance->state; + struct vchiq_service *service = NULL; + + *phandle = VCHIQ_SERVICE_HANDLE_INVALID; + + if (!vchiq_is_connected(instance)) + goto failed; + + service = vchiq_add_service_internal(state, params, VCHIQ_SRVSTATE_OPENING, instance, NULL); + + if (service) { + *phandle = service->handle; + status = vchiq_open_service_internal(service, current->pid); + if (status != VCHIQ_SUCCESS) { + vchiq_remove_service(instance, service->handle); + *phandle = VCHIQ_SERVICE_HANDLE_INVALID; + } + } + +failed: + vchiq_log_trace(vchiq_core_log_level, "%s(%p): returning %d", __func__, instance, status); + + return status; +} +EXPORT_SYMBOL(vchiq_open_service); + +enum vchiq_status +vchiq_bulk_transmit(struct vchiq_instance *instance, unsigned int handle, const void *data, + unsigned int size, void *userdata, enum vchiq_bulk_mode mode) +{ + enum vchiq_status status; + + while (1) { + switch (mode) { + case VCHIQ_BULK_MODE_NOCALLBACK: + case VCHIQ_BULK_MODE_CALLBACK: + status = vchiq_bulk_transfer(instance, handle, + (void *)data, NULL, + size, userdata, mode, + VCHIQ_BULK_TRANSMIT); + break; + case VCHIQ_BULK_MODE_BLOCKING: + status = vchiq_blocking_bulk_transfer(instance, handle, (void *)data, size, + VCHIQ_BULK_TRANSMIT); + break; + default: + return VCHIQ_ERROR; + } + + /* + * vchiq_*_bulk_transfer() may return VCHIQ_RETRY, so we need + * to implement a retry mechanism since this function is + * supposed to block until queued + */ + if (status != VCHIQ_RETRY) + break; + + msleep(1); + } + + return status; +} +EXPORT_SYMBOL(vchiq_bulk_transmit); + +enum vchiq_status vchiq_bulk_receive(struct vchiq_instance *instance, unsigned int handle, + void *data, unsigned int size, void *userdata, + enum vchiq_bulk_mode mode) +{ + enum vchiq_status status; + + while (1) { + switch (mode) { + case VCHIQ_BULK_MODE_NOCALLBACK: + case VCHIQ_BULK_MODE_CALLBACK: + status = vchiq_bulk_transfer(instance, handle, data, NULL, + size, userdata, + mode, VCHIQ_BULK_RECEIVE); + break; + case VCHIQ_BULK_MODE_BLOCKING: + status = vchiq_blocking_bulk_transfer(instance, handle, (void *)data, size, + VCHIQ_BULK_RECEIVE); + break; + default: + return VCHIQ_ERROR; + } + + /* + * vchiq_*_bulk_transfer() may return VCHIQ_RETRY, so we need + * to implement a retry mechanism since this function is + * supposed to block until queued + */ + if (status != VCHIQ_RETRY) + break; + + msleep(1); + } + + return status; +} +EXPORT_SYMBOL(vchiq_bulk_receive); + +static enum vchiq_status +vchiq_blocking_bulk_transfer(struct vchiq_instance *instance, unsigned int handle, void *data, + unsigned int size, enum vchiq_bulk_dir dir) +{ + struct vchiq_service *service; + enum vchiq_status status; + struct bulk_waiter_node *waiter = NULL, *iter; + + service = find_service_by_handle(instance, handle); + if (!service) + return VCHIQ_ERROR; + + vchiq_service_put(service); + + mutex_lock(&instance->bulk_waiter_list_mutex); + list_for_each_entry(iter, &instance->bulk_waiter_list, list) { + if (iter->pid == current->pid) { + list_del(&iter->list); + waiter = iter; + break; + } + } + mutex_unlock(&instance->bulk_waiter_list_mutex); + + if (waiter) { + struct vchiq_bulk *bulk = waiter->bulk_waiter.bulk; + + if (bulk) { + /* This thread has an outstanding bulk transfer. */ + /* FIXME: why compare a dma address to a pointer? */ + if ((bulk->data != (dma_addr_t)(uintptr_t)data) || (bulk->size != size)) { + /* + * This is not a retry of the previous one. + * Cancel the signal when the transfer completes. + */ + spin_lock(&bulk_waiter_spinlock); + bulk->userdata = NULL; + spin_unlock(&bulk_waiter_spinlock); + } + } + } else { + waiter = kzalloc(sizeof(*waiter), GFP_KERNEL); + if (!waiter) { + vchiq_log_error(vchiq_core_log_level, "%s - out of memory", __func__); + return VCHIQ_ERROR; + } + } + + status = vchiq_bulk_transfer(instance, handle, data, NULL, size, + &waiter->bulk_waiter, + VCHIQ_BULK_MODE_BLOCKING, dir); + if ((status != VCHIQ_RETRY) || fatal_signal_pending(current) || !waiter->bulk_waiter.bulk) { + struct vchiq_bulk *bulk = waiter->bulk_waiter.bulk; + + if (bulk) { + /* Cancel the signal when the transfer completes. */ + spin_lock(&bulk_waiter_spinlock); + bulk->userdata = NULL; + spin_unlock(&bulk_waiter_spinlock); + } + kfree(waiter); + } else { + waiter->pid = current->pid; + mutex_lock(&instance->bulk_waiter_list_mutex); + list_add(&waiter->list, &instance->bulk_waiter_list); + mutex_unlock(&instance->bulk_waiter_list_mutex); + vchiq_log_info(vchiq_arm_log_level, "saved bulk_waiter %pK for pid %d", waiter, + current->pid); + } + + return status; +} + +static enum vchiq_status +add_completion(struct vchiq_instance *instance, enum vchiq_reason reason, + struct vchiq_header *header, struct user_service *user_service, + void *bulk_userdata) +{ + struct vchiq_completion_data_kernel *completion; + int insert; + + DEBUG_INITIALISE(g_state.local); + + insert = instance->completion_insert; + while ((insert - instance->completion_remove) >= MAX_COMPLETIONS) { + /* Out of space - wait for the client */ + DEBUG_TRACE(SERVICE_CALLBACK_LINE); + vchiq_log_trace(vchiq_arm_log_level, "%s - completion queue full", __func__); + DEBUG_COUNT(COMPLETION_QUEUE_FULL_COUNT); + if (wait_for_completion_interruptible(&instance->remove_event)) { + vchiq_log_info(vchiq_arm_log_level, "service_callback interrupted"); + return VCHIQ_RETRY; + } else if (instance->closing) { + vchiq_log_info(vchiq_arm_log_level, "service_callback closing"); + return VCHIQ_SUCCESS; + } + DEBUG_TRACE(SERVICE_CALLBACK_LINE); + } + + completion = &instance->completions[insert & (MAX_COMPLETIONS - 1)]; + + completion->header = header; + completion->reason = reason; + /* N.B. service_userdata is updated while processing AWAIT_COMPLETION */ + completion->service_userdata = user_service->service; + completion->bulk_userdata = bulk_userdata; + + if (reason == VCHIQ_SERVICE_CLOSED) { + /* + * Take an extra reference, to be held until + * this CLOSED notification is delivered. + */ + vchiq_service_get(user_service->service); + if (instance->use_close_delivered) + user_service->close_pending = 1; + } + + /* + * A write barrier is needed here to ensure that the entire completion + * record is written out before the insert point. + */ + wmb(); + + if (reason == VCHIQ_MESSAGE_AVAILABLE) + user_service->message_available_pos = insert; + + insert++; + instance->completion_insert = insert; + + complete(&instance->insert_event); + + return VCHIQ_SUCCESS; +} + +enum vchiq_status +service_callback(struct vchiq_instance *instance, enum vchiq_reason reason, + struct vchiq_header *header, unsigned int handle, void *bulk_userdata) +{ + /* + * How do we ensure the callback goes to the right client? + * The service_user data points to a user_service record + * containing the original callback and the user state structure, which + * contains a circular buffer for completion records. + */ + struct user_service *user_service; + struct vchiq_service *service; + bool skip_completion = false; + + DEBUG_INITIALISE(g_state.local); + + DEBUG_TRACE(SERVICE_CALLBACK_LINE); + + rcu_read_lock(); + service = handle_to_service(instance, handle); + if (WARN_ON(!service)) { + rcu_read_unlock(); + return VCHIQ_SUCCESS; + } + + user_service = (struct user_service *)service->base.userdata; + + if (!instance || instance->closing) { + rcu_read_unlock(); + return VCHIQ_SUCCESS; + } + + /* + * As hopping around different synchronization mechanism, + * taking an extra reference results in simpler implementation. + */ + vchiq_service_get(service); + rcu_read_unlock(); + + vchiq_log_trace(vchiq_arm_log_level, + "%s - service %lx(%d,%p), reason %d, header %lx, instance %lx, bulk_userdata %lx", + __func__, (unsigned long)user_service, service->localport, + user_service->userdata, reason, (unsigned long)header, + (unsigned long)instance, (unsigned long)bulk_userdata); + + if (header && user_service->is_vchi) { + spin_lock(&msg_queue_spinlock); + while (user_service->msg_insert == + (user_service->msg_remove + MSG_QUEUE_SIZE)) { + spin_unlock(&msg_queue_spinlock); + DEBUG_TRACE(SERVICE_CALLBACK_LINE); + DEBUG_COUNT(MSG_QUEUE_FULL_COUNT); + vchiq_log_trace(vchiq_arm_log_level, "%s - msg queue full", __func__); + /* + * If there is no MESSAGE_AVAILABLE in the completion + * queue, add one + */ + if ((user_service->message_available_pos - + instance->completion_remove) < 0) { + enum vchiq_status status; + + vchiq_log_info(vchiq_arm_log_level, + "Inserting extra MESSAGE_AVAILABLE"); + DEBUG_TRACE(SERVICE_CALLBACK_LINE); + status = add_completion(instance, reason, NULL, user_service, + bulk_userdata); + if (status != VCHIQ_SUCCESS) { + DEBUG_TRACE(SERVICE_CALLBACK_LINE); + vchiq_service_put(service); + return status; + } + } + + DEBUG_TRACE(SERVICE_CALLBACK_LINE); + if (wait_for_completion_interruptible(&user_service->remove_event)) { + vchiq_log_info(vchiq_arm_log_level, "%s interrupted", __func__); + DEBUG_TRACE(SERVICE_CALLBACK_LINE); + vchiq_service_put(service); + return VCHIQ_RETRY; + } else if (instance->closing) { + vchiq_log_info(vchiq_arm_log_level, "%s closing", __func__); + DEBUG_TRACE(SERVICE_CALLBACK_LINE); + vchiq_service_put(service); + return VCHIQ_ERROR; + } + DEBUG_TRACE(SERVICE_CALLBACK_LINE); + spin_lock(&msg_queue_spinlock); + } + + user_service->msg_queue[user_service->msg_insert & + (MSG_QUEUE_SIZE - 1)] = header; + user_service->msg_insert++; + + /* + * If there is a thread waiting in DEQUEUE_MESSAGE, or if + * there is a MESSAGE_AVAILABLE in the completion queue then + * bypass the completion queue. + */ + if (((user_service->message_available_pos - + instance->completion_remove) >= 0) || + user_service->dequeue_pending) { + user_service->dequeue_pending = 0; + skip_completion = true; + } + + spin_unlock(&msg_queue_spinlock); + complete(&user_service->insert_event); + + header = NULL; + } + DEBUG_TRACE(SERVICE_CALLBACK_LINE); + vchiq_service_put(service); + + if (skip_completion) + return VCHIQ_SUCCESS; + + return add_completion(instance, reason, header, user_service, + bulk_userdata); +} + +int vchiq_dump(void *dump_context, const char *str, int len) +{ + struct dump_context *context = (struct dump_context *)dump_context; + int copy_bytes; + + if (context->actual >= context->space) + return 0; + + if (context->offset > 0) { + int skip_bytes = min_t(int, len, context->offset); + + str += skip_bytes; + len -= skip_bytes; + context->offset -= skip_bytes; + if (context->offset > 0) + return 0; + } + copy_bytes = min_t(int, len, context->space - context->actual); + if (copy_bytes == 0) + return 0; + if (copy_to_user(context->buf + context->actual, str, + copy_bytes)) + return -EFAULT; + context->actual += copy_bytes; + len -= copy_bytes; + + /* + * If the terminating NUL is included in the length, then it + * marks the end of a line and should be replaced with a + * carriage return. + */ + if ((len == 0) && (str[copy_bytes - 1] == '\0')) { + char cr = '\n'; + + if (copy_to_user(context->buf + context->actual - 1, + &cr, 1)) + return -EFAULT; + } + return 0; +} + +int vchiq_dump_platform_instances(void *dump_context) +{ + struct vchiq_state *state = vchiq_get_state(); + char buf[80]; + int len; + int i; + + if (!state) + return -ENOTCONN; + + /* + * There is no list of instances, so instead scan all services, + * marking those that have been dumped. + */ + + rcu_read_lock(); + for (i = 0; i < state->unused_service; i++) { + struct vchiq_service *service; + struct vchiq_instance *instance; + + service = rcu_dereference(state->services[i]); + if (!service || service->base.callback != service_callback) + continue; + + instance = service->instance; + if (instance) + instance->mark = 0; + } + rcu_read_unlock(); + + for (i = 0; i < state->unused_service; i++) { + struct vchiq_service *service; + struct vchiq_instance *instance; + int err; + + rcu_read_lock(); + service = rcu_dereference(state->services[i]); + if (!service || service->base.callback != service_callback) { + rcu_read_unlock(); + continue; + } + + instance = service->instance; + if (!instance || instance->mark) { + rcu_read_unlock(); + continue; + } + rcu_read_unlock(); + + len = snprintf(buf, sizeof(buf), + "Instance %pK: pid %d,%s completions %d/%d", + instance, instance->pid, + instance->connected ? " connected, " : + "", + instance->completion_insert - + instance->completion_remove, + MAX_COMPLETIONS); + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + instance->mark = 1; + } + return 0; +} + +int vchiq_dump_platform_service_state(void *dump_context, + struct vchiq_service *service) +{ + struct user_service *user_service = + (struct user_service *)service->base.userdata; + char buf[80]; + int len; + + len = scnprintf(buf, sizeof(buf), " instance %pK", service->instance); + + if ((service->base.callback == service_callback) && user_service->is_vchi) { + len += scnprintf(buf + len, sizeof(buf) - len, ", %d/%d messages", + user_service->msg_insert - user_service->msg_remove, + MSG_QUEUE_SIZE); + + if (user_service->dequeue_pending) + len += scnprintf(buf + len, sizeof(buf) - len, + " (dequeue pending)"); + } + + return vchiq_dump(dump_context, buf, len + 1); +} + +struct vchiq_state * +vchiq_get_state(void) +{ + if (!g_state.remote) { + pr_err("%s: g_state.remote == NULL\n", __func__); + return NULL; + } + + if (g_state.remote->initialised != 1) { + pr_notice("%s: g_state.remote->initialised != 1 (%d)\n", + __func__, g_state.remote->initialised); + return NULL; + } + + return &g_state; +} + +/* + * Autosuspend related functionality + */ + +static enum vchiq_status +vchiq_keepalive_vchiq_callback(struct vchiq_instance *instance, + enum vchiq_reason reason, + struct vchiq_header *header, + unsigned int service_user, void *bulk_user) +{ + vchiq_log_error(vchiq_susp_log_level, "%s callback reason %d", __func__, reason); + return 0; +} + +static int +vchiq_keepalive_thread_func(void *v) +{ + struct vchiq_state *state = (struct vchiq_state *)v; + struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state); + + enum vchiq_status status; + struct vchiq_instance *instance; + unsigned int ka_handle; + int ret; + + struct vchiq_service_params_kernel params = { + .fourcc = VCHIQ_MAKE_FOURCC('K', 'E', 'E', 'P'), + .callback = vchiq_keepalive_vchiq_callback, + .version = KEEPALIVE_VER, + .version_min = KEEPALIVE_VER_MIN + }; + + ret = vchiq_initialise(&instance); + if (ret) { + vchiq_log_error(vchiq_susp_log_level, "%s vchiq_initialise failed %d", __func__, + ret); + goto exit; + } + + status = vchiq_connect(instance); + if (status != VCHIQ_SUCCESS) { + vchiq_log_error(vchiq_susp_log_level, "%s vchiq_connect failed %d", __func__, + status); + goto shutdown; + } + + status = vchiq_add_service(instance, ¶ms, &ka_handle); + if (status != VCHIQ_SUCCESS) { + vchiq_log_error(vchiq_susp_log_level, "%s vchiq_open_service failed %d", __func__, + status); + goto shutdown; + } + + while (1) { + long rc = 0, uc = 0; + + if (wait_for_completion_interruptible(&arm_state->ka_evt)) { + vchiq_log_error(vchiq_susp_log_level, "%s interrupted", __func__); + flush_signals(current); + continue; + } + + /* + * read and clear counters. Do release_count then use_count to + * prevent getting more releases than uses + */ + rc = atomic_xchg(&arm_state->ka_release_count, 0); + uc = atomic_xchg(&arm_state->ka_use_count, 0); + + /* + * Call use/release service the requisite number of times. + * Process use before release so use counts don't go negative + */ + while (uc--) { + atomic_inc(&arm_state->ka_use_ack_count); + status = vchiq_use_service(instance, ka_handle); + if (status != VCHIQ_SUCCESS) { + vchiq_log_error(vchiq_susp_log_level, + "%s vchiq_use_service error %d", __func__, status); + } + } + while (rc--) { + status = vchiq_release_service(instance, ka_handle); + if (status != VCHIQ_SUCCESS) { + vchiq_log_error(vchiq_susp_log_level, + "%s vchiq_release_service error %d", __func__, + status); + } + } + } + +shutdown: + vchiq_shutdown(instance); +exit: + return 0; +} + +int +vchiq_use_internal(struct vchiq_state *state, struct vchiq_service *service, + enum USE_TYPE_E use_type) +{ + struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state); + int ret = 0; + char entity[16]; + int *entity_uc; + int local_uc; + + if (!arm_state) { + ret = -EINVAL; + goto out; + } + + if (use_type == USE_TYPE_VCHIQ) { + sprintf(entity, "VCHIQ: "); + entity_uc = &arm_state->peer_use_count; + } else if (service) { + sprintf(entity, "%c%c%c%c:%03d", + VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), + service->client_id); + entity_uc = &service->service_use_count; + } else { + vchiq_log_error(vchiq_susp_log_level, "%s null service ptr", __func__); + ret = -EINVAL; + goto out; + } + + write_lock_bh(&arm_state->susp_res_lock); + local_uc = ++arm_state->videocore_use_count; + ++(*entity_uc); + + vchiq_log_trace(vchiq_susp_log_level, "%s %s count %d, state count %d", __func__, entity, + *entity_uc, local_uc); + + write_unlock_bh(&arm_state->susp_res_lock); + + if (!ret) { + enum vchiq_status status = VCHIQ_SUCCESS; + long ack_cnt = atomic_xchg(&arm_state->ka_use_ack_count, 0); + + while (ack_cnt && (status == VCHIQ_SUCCESS)) { + /* Send the use notify to videocore */ + status = vchiq_send_remote_use_active(state); + if (status == VCHIQ_SUCCESS) + ack_cnt--; + else + atomic_add(ack_cnt, &arm_state->ka_use_ack_count); + } + } + +out: + vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret); + return ret; +} + +int +vchiq_release_internal(struct vchiq_state *state, struct vchiq_service *service) +{ + struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state); + int ret = 0; + char entity[16]; + int *entity_uc; + + if (!arm_state) { + ret = -EINVAL; + goto out; + } + + if (service) { + sprintf(entity, "%c%c%c%c:%03d", + VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), + service->client_id); + entity_uc = &service->service_use_count; + } else { + sprintf(entity, "PEER: "); + entity_uc = &arm_state->peer_use_count; + } + + write_lock_bh(&arm_state->susp_res_lock); + if (!arm_state->videocore_use_count || !(*entity_uc)) { + /* Don't use BUG_ON - don't allow user thread to crash kernel */ + WARN_ON(!arm_state->videocore_use_count); + WARN_ON(!(*entity_uc)); + ret = -EINVAL; + goto unlock; + } + --arm_state->videocore_use_count; + --(*entity_uc); + + vchiq_log_trace(vchiq_susp_log_level, "%s %s count %d, state count %d", __func__, entity, + *entity_uc, arm_state->videocore_use_count); + +unlock: + write_unlock_bh(&arm_state->susp_res_lock); + +out: + vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret); + return ret; +} + +void +vchiq_on_remote_use(struct vchiq_state *state) +{ + struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state); + + atomic_inc(&arm_state->ka_use_count); + complete(&arm_state->ka_evt); +} + +void +vchiq_on_remote_release(struct vchiq_state *state) +{ + struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state); + + atomic_inc(&arm_state->ka_release_count); + complete(&arm_state->ka_evt); +} + +int +vchiq_use_service_internal(struct vchiq_service *service) +{ + return vchiq_use_internal(service->state, service, USE_TYPE_SERVICE); +} + +int +vchiq_release_service_internal(struct vchiq_service *service) +{ + return vchiq_release_internal(service->state, service); +} + +struct vchiq_debugfs_node * +vchiq_instance_get_debugfs_node(struct vchiq_instance *instance) +{ + return &instance->debugfs_node; +} + +int +vchiq_instance_get_use_count(struct vchiq_instance *instance) +{ + struct vchiq_service *service; + int use_count = 0, i; + + i = 0; + rcu_read_lock(); + while ((service = __next_service_by_instance(instance->state, + instance, &i))) + use_count += service->service_use_count; + rcu_read_unlock(); + return use_count; +} + +int +vchiq_instance_get_pid(struct vchiq_instance *instance) +{ + return instance->pid; +} + +int +vchiq_instance_get_trace(struct vchiq_instance *instance) +{ + return instance->trace; +} + +void +vchiq_instance_set_trace(struct vchiq_instance *instance, int trace) +{ + struct vchiq_service *service; + int i; + + i = 0; + rcu_read_lock(); + while ((service = __next_service_by_instance(instance->state, + instance, &i))) + service->trace = trace; + rcu_read_unlock(); + instance->trace = (trace != 0); +} + +enum vchiq_status +vchiq_use_service(struct vchiq_instance *instance, unsigned int handle) +{ + enum vchiq_status ret = VCHIQ_ERROR; + struct vchiq_service *service = find_service_by_handle(instance, handle); + + if (service) { + ret = vchiq_use_internal(service->state, service, USE_TYPE_SERVICE); + vchiq_service_put(service); + } + return ret; +} +EXPORT_SYMBOL(vchiq_use_service); + +enum vchiq_status +vchiq_release_service(struct vchiq_instance *instance, unsigned int handle) +{ + enum vchiq_status ret = VCHIQ_ERROR; + struct vchiq_service *service = find_service_by_handle(instance, handle); + + if (service) { + ret = vchiq_release_internal(service->state, service); + vchiq_service_put(service); + } + return ret; +} +EXPORT_SYMBOL(vchiq_release_service); + +struct service_data_struct { + int fourcc; + int clientid; + int use_count; +}; + +void +vchiq_dump_service_use_state(struct vchiq_state *state) +{ + struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state); + struct service_data_struct *service_data; + int i, found = 0; + /* + * If there's more than 64 services, only dump ones with + * non-zero counts + */ + int only_nonzero = 0; + static const char *nz = "<-- preventing suspend"; + + int peer_count; + int vc_use_count; + int active_services; + + if (!arm_state) + return; + + service_data = kmalloc_array(MAX_SERVICES, sizeof(*service_data), + GFP_KERNEL); + if (!service_data) + return; + + read_lock_bh(&arm_state->susp_res_lock); + peer_count = arm_state->peer_use_count; + vc_use_count = arm_state->videocore_use_count; + active_services = state->unused_service; + if (active_services > MAX_SERVICES) + only_nonzero = 1; + + rcu_read_lock(); + for (i = 0; i < active_services; i++) { + struct vchiq_service *service_ptr = + rcu_dereference(state->services[i]); + + if (!service_ptr) + continue; + + if (only_nonzero && !service_ptr->service_use_count) + continue; + + if (service_ptr->srvstate == VCHIQ_SRVSTATE_FREE) + continue; + + service_data[found].fourcc = service_ptr->base.fourcc; + service_data[found].clientid = service_ptr->client_id; + service_data[found].use_count = service_ptr->service_use_count; + found++; + if (found >= MAX_SERVICES) + break; + } + rcu_read_unlock(); + + read_unlock_bh(&arm_state->susp_res_lock); + + if (only_nonzero) + vchiq_log_warning(vchiq_susp_log_level, "Too many active services (%d). Only dumping up to first %d services with non-zero use-count", + active_services, found); + + for (i = 0; i < found; i++) { + vchiq_log_warning(vchiq_susp_log_level, "----- %c%c%c%c:%d service count %d %s", + VCHIQ_FOURCC_AS_4CHARS(service_data[i].fourcc), + service_data[i].clientid, service_data[i].use_count, + service_data[i].use_count ? nz : ""); + } + vchiq_log_warning(vchiq_susp_log_level, "----- VCHIQ use count %d", peer_count); + vchiq_log_warning(vchiq_susp_log_level, "--- Overall vchiq instance use count %d", + vc_use_count); + + kfree(service_data); +} + +enum vchiq_status +vchiq_check_service(struct vchiq_service *service) +{ + struct vchiq_arm_state *arm_state; + enum vchiq_status ret = VCHIQ_ERROR; + + if (!service || !service->state) + goto out; + + arm_state = vchiq_platform_get_arm_state(service->state); + + read_lock_bh(&arm_state->susp_res_lock); + if (service->service_use_count) + ret = VCHIQ_SUCCESS; + read_unlock_bh(&arm_state->susp_res_lock); + + if (ret == VCHIQ_ERROR) { + vchiq_log_error(vchiq_susp_log_level, + "%s ERROR - %c%c%c%c:%d service count %d, state count %d", __func__, + VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), service->client_id, + service->service_use_count, arm_state->videocore_use_count); + vchiq_dump_service_use_state(service->state); + } +out: + return ret; +} + +void vchiq_platform_conn_state_changed(struct vchiq_state *state, + enum vchiq_connstate oldstate, + enum vchiq_connstate newstate) +{ + struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state); + char threadname[16]; + + vchiq_log_info(vchiq_susp_log_level, "%d: %s->%s", state->id, + get_conn_state_name(oldstate), get_conn_state_name(newstate)); + if (state->conn_state != VCHIQ_CONNSTATE_CONNECTED) + return; + + write_lock_bh(&arm_state->susp_res_lock); + if (arm_state->first_connect) { + write_unlock_bh(&arm_state->susp_res_lock); + return; + } + + arm_state->first_connect = 1; + write_unlock_bh(&arm_state->susp_res_lock); + snprintf(threadname, sizeof(threadname), "vchiq-keep/%d", + state->id); + arm_state->ka_thread = kthread_create(&vchiq_keepalive_thread_func, + (void *)state, + threadname); + if (IS_ERR(arm_state->ka_thread)) { + vchiq_log_error(vchiq_susp_log_level, + "vchiq: FATAL: couldn't create thread %s", + threadname); + } else { + wake_up_process(arm_state->ka_thread); + } +} + +static const struct of_device_id vchiq_of_match[] = { + { .compatible = "brcm,bcm2835-vchiq", .data = &bcm2835_drvdata }, + { .compatible = "brcm,bcm2836-vchiq", .data = &bcm2836_drvdata }, + {}, +}; +MODULE_DEVICE_TABLE(of, vchiq_of_match); + +static struct platform_device * +vchiq_register_child(struct platform_device *pdev, const char *name) +{ + struct platform_device_info pdevinfo; + struct platform_device *child; + + memset(&pdevinfo, 0, sizeof(pdevinfo)); + + pdevinfo.parent = &pdev->dev; + pdevinfo.name = name; + pdevinfo.id = PLATFORM_DEVID_NONE; + pdevinfo.dma_mask = DMA_BIT_MASK(32); + + child = platform_device_register_full(&pdevinfo); + if (IS_ERR(child)) { + dev_warn(&pdev->dev, "%s not registered\n", name); + child = NULL; + } + + return child; +} + +static int vchiq_probe(struct platform_device *pdev) +{ + struct device_node *fw_node; + const struct of_device_id *of_id; + struct vchiq_drvdata *drvdata; + int err; + + of_id = of_match_node(vchiq_of_match, pdev->dev.of_node); + drvdata = (struct vchiq_drvdata *)of_id->data; + if (!drvdata) + return -EINVAL; + + fw_node = of_find_compatible_node(NULL, NULL, + "raspberrypi,bcm2835-firmware"); + if (!fw_node) { + dev_err(&pdev->dev, "Missing firmware node\n"); + return -ENOENT; + } + + drvdata->fw = devm_rpi_firmware_get(&pdev->dev, fw_node); + of_node_put(fw_node); + if (!drvdata->fw) + return -EPROBE_DEFER; + + platform_set_drvdata(pdev, drvdata); + + err = vchiq_platform_init(pdev, &g_state); + if (err) + goto failed_platform_init; + + vchiq_debugfs_init(); + + vchiq_log_info(vchiq_arm_log_level, + "vchiq: platform initialised - version %d (min %d)", + VCHIQ_VERSION, VCHIQ_VERSION_MIN); + + /* + * Simply exit on error since the function handles cleanup in + * cases of failure. + */ + err = vchiq_register_chrdev(&pdev->dev); + if (err) { + vchiq_log_warning(vchiq_arm_log_level, + "Failed to initialize vchiq cdev"); + goto error_exit; + } + + bcm2835_camera = vchiq_register_child(pdev, "bcm2835-camera"); + bcm2835_audio = vchiq_register_child(pdev, "bcm2835_audio"); + + return 0; + +failed_platform_init: + vchiq_log_warning(vchiq_arm_log_level, "could not initialize vchiq platform"); +error_exit: + return err; +} + +static int vchiq_remove(struct platform_device *pdev) +{ + platform_device_unregister(bcm2835_audio); + platform_device_unregister(bcm2835_camera); + vchiq_debugfs_deinit(); + vchiq_deregister_chrdev(); + + return 0; +} + +static struct platform_driver vchiq_driver = { + .driver = { + .name = "bcm2835_vchiq", + .of_match_table = vchiq_of_match, + }, + .probe = vchiq_probe, + .remove = vchiq_remove, +}; + +static int __init vchiq_driver_init(void) +{ + int ret; + + ret = platform_driver_register(&vchiq_driver); + if (ret) + pr_err("Failed to register vchiq driver\n"); + + return ret; +} +module_init(vchiq_driver_init); + +static void __exit vchiq_driver_exit(void) +{ + platform_driver_unregister(&vchiq_driver); +} +module_exit(vchiq_driver_exit); + +MODULE_LICENSE("Dual BSD/GPL"); +MODULE_DESCRIPTION("Videocore VCHIQ driver"); +MODULE_AUTHOR("Broadcom Corporation"); diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.h new file mode 100644 index 000000000..cd20eb18f --- /dev/null +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.h @@ -0,0 +1,147 @@ +/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */ +/* + * Copyright (c) 2014 Raspberry Pi (Trading) Ltd. All rights reserved. + * Copyright (c) 2010-2012 Broadcom. All rights reserved. + */ + +#ifndef VCHIQ_ARM_H +#define VCHIQ_ARM_H + +#include <linux/mutex.h> +#include <linux/platform_device.h> +#include <linux/semaphore.h> +#include <linux/atomic.h> +#include "vchiq_core.h" +#include "vchiq_debugfs.h" + +/* Some per-instance constants */ +#define MAX_COMPLETIONS 128 +#define MAX_SERVICES 64 +#define MAX_ELEMENTS 8 +#define MSG_QUEUE_SIZE 128 + +enum USE_TYPE_E { + USE_TYPE_SERVICE, + USE_TYPE_VCHIQ +}; + +struct user_service { + struct vchiq_service *service; + void __user *userdata; + struct vchiq_instance *instance; + char is_vchi; + char dequeue_pending; + char close_pending; + int message_available_pos; + int msg_insert; + int msg_remove; + struct completion insert_event; + struct completion remove_event; + struct completion close_event; + struct vchiq_header *msg_queue[MSG_QUEUE_SIZE]; +}; + +struct bulk_waiter_node { + struct bulk_waiter bulk_waiter; + int pid; + struct list_head list; +}; + +struct vchiq_instance { + struct vchiq_state *state; + struct vchiq_completion_data_kernel completions[MAX_COMPLETIONS]; + int completion_insert; + int completion_remove; + struct completion insert_event; + struct completion remove_event; + struct mutex completion_mutex; + + int connected; + int closing; + int pid; + int mark; + int use_close_delivered; + int trace; + + struct list_head bulk_waiter_list; + struct mutex bulk_waiter_list_mutex; + + struct vchiq_debugfs_node debugfs_node; +}; + +struct dump_context { + char __user *buf; + size_t actual; + size_t space; + loff_t offset; +}; + +extern int vchiq_arm_log_level; +extern int vchiq_susp_log_level; + +extern spinlock_t msg_queue_spinlock; +extern struct vchiq_state g_state; + +extern struct vchiq_state * +vchiq_get_state(void); + +enum vchiq_status +vchiq_use_service(struct vchiq_instance *instance, unsigned int handle); + +extern enum vchiq_status +vchiq_release_service(struct vchiq_instance *instance, unsigned int handle); + +extern enum vchiq_status +vchiq_check_service(struct vchiq_service *service); + +extern void +vchiq_dump_platform_use_state(struct vchiq_state *state); + +extern void +vchiq_dump_service_use_state(struct vchiq_state *state); + +extern int +vchiq_use_internal(struct vchiq_state *state, struct vchiq_service *service, + enum USE_TYPE_E use_type); +extern int +vchiq_release_internal(struct vchiq_state *state, + struct vchiq_service *service); + +extern struct vchiq_debugfs_node * +vchiq_instance_get_debugfs_node(struct vchiq_instance *instance); + +extern int +vchiq_instance_get_use_count(struct vchiq_instance *instance); + +extern int +vchiq_instance_get_pid(struct vchiq_instance *instance); + +extern int +vchiq_instance_get_trace(struct vchiq_instance *instance); + +extern void +vchiq_instance_set_trace(struct vchiq_instance *instance, int trace); + +#if IS_ENABLED(CONFIG_VCHIQ_CDEV) + +extern void +vchiq_deregister_chrdev(void); + +extern int +vchiq_register_chrdev(struct device *parent); + +#else + +static inline void vchiq_deregister_chrdev(void) { } +static inline int vchiq_register_chrdev(struct device *parent) { return 0; } + +#endif /* IS_ENABLED(CONFIG_VCHIQ_CDEV) */ + +extern enum vchiq_status +service_callback(struct vchiq_instance *vchiq_instance, enum vchiq_reason reason, + struct vchiq_header *header, unsigned int handle, void *bulk_userdata); + +extern void +free_bulk_waiter(struct vchiq_instance *instance); + +#endif /* VCHIQ_ARM_H */ diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_cfg.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_cfg.h new file mode 100644 index 000000000..a16d02999 --- /dev/null +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_cfg.h @@ -0,0 +1,41 @@ +/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */ +/* Copyright (c) 2010-2014 Broadcom. All rights reserved. */ + +#ifndef VCHIQ_CFG_H +#define VCHIQ_CFG_H + +#define VCHIQ_MAGIC VCHIQ_MAKE_FOURCC('V', 'C', 'H', 'I') +/* The version of VCHIQ - change with any non-trivial change */ +#define VCHIQ_VERSION 8 +/* + * The minimum compatible version - update to match VCHIQ_VERSION with any + * incompatible change + */ +#define VCHIQ_VERSION_MIN 3 + +/* The version that introduced the VCHIQ_IOC_LIB_VERSION ioctl */ +#define VCHIQ_VERSION_LIB_VERSION 7 + +/* The version that introduced the VCHIQ_IOC_CLOSE_DELIVERED ioctl */ +#define VCHIQ_VERSION_CLOSE_DELIVERED 7 + +/* The version that made it safe to use SYNCHRONOUS mode */ +#define VCHIQ_VERSION_SYNCHRONOUS_MODE 8 + +#define VCHIQ_MAX_STATES 1 +#define VCHIQ_MAX_SERVICES 4096 +#define VCHIQ_MAX_SLOTS 128 +#define VCHIQ_MAX_SLOTS_PER_SIDE 64 + +#define VCHIQ_NUM_CURRENT_BULKS 32 +#define VCHIQ_NUM_SERVICE_BULKS 4 + +#ifndef VCHIQ_ENABLE_DEBUG +#define VCHIQ_ENABLE_DEBUG 1 +#endif + +#ifndef VCHIQ_ENABLE_STATS +#define VCHIQ_ENABLE_STATS 1 +#endif + +#endif /* VCHIQ_CFG_H */ diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c new file mode 100644 index 000000000..bdb0ab617 --- /dev/null +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c @@ -0,0 +1,74 @@ +// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause +/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */ + +#include "vchiq_connected.h" +#include "vchiq_core.h" +#include <linux/module.h> +#include <linux/mutex.h> + +#define MAX_CALLBACKS 10 + +static int g_connected; +static int g_num_deferred_callbacks; +static void (*g_deferred_callback[MAX_CALLBACKS])(void); +static int g_once_init; +static DEFINE_MUTEX(g_connected_mutex); + +/* Function to initialize our lock */ +static void connected_init(void) +{ + if (!g_once_init) + g_once_init = 1; +} + +/* + * This function is used to defer initialization until the vchiq stack is + * initialized. If the stack is already initialized, then the callback will + * be made immediately, otherwise it will be deferred until + * vchiq_call_connected_callbacks is called. + */ +void vchiq_add_connected_callback(void (*callback)(void)) +{ + connected_init(); + + if (mutex_lock_killable(&g_connected_mutex)) + return; + + if (g_connected) { + /* We're already connected. Call the callback immediately. */ + callback(); + } else { + if (g_num_deferred_callbacks >= MAX_CALLBACKS) { + vchiq_log_error(vchiq_core_log_level, + "There already %d callback registered - please increase MAX_CALLBACKS", + g_num_deferred_callbacks); + } else { + g_deferred_callback[g_num_deferred_callbacks] = + callback; + g_num_deferred_callbacks++; + } + } + mutex_unlock(&g_connected_mutex); +} +EXPORT_SYMBOL(vchiq_add_connected_callback); + +/* + * This function is called by the vchiq stack once it has been connected to + * the videocore and clients can start to use the stack. + */ +void vchiq_call_connected_callbacks(void) +{ + int i; + + connected_init(); + + if (mutex_lock_killable(&g_connected_mutex)) + return; + + for (i = 0; i < g_num_deferred_callbacks; i++) + g_deferred_callback[i](); + + g_num_deferred_callbacks = 0; + g_connected = 1; + mutex_unlock(&g_connected_mutex); +} diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.h new file mode 100644 index 000000000..4caf5e300 --- /dev/null +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.h @@ -0,0 +1,10 @@ +/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */ +/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */ + +#ifndef VCHIQ_CONNECTED_H +#define VCHIQ_CONNECTED_H + +void vchiq_add_connected_callback(void (*callback)(void)); +void vchiq_call_connected_callbacks(void); + +#endif /* VCHIQ_CONNECTED_H */ diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c new file mode 100644 index 000000000..45ed30bfd --- /dev/null +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c @@ -0,0 +1,3700 @@ +// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause +/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */ + +#include <linux/types.h> +#include <linux/completion.h> +#include <linux/mutex.h> +#include <linux/bitops.h> +#include <linux/kthread.h> +#include <linux/wait.h> +#include <linux/delay.h> +#include <linux/slab.h> +#include <linux/kref.h> +#include <linux/rcupdate.h> +#include <linux/sched/signal.h> + +#include "vchiq_arm.h" +#include "vchiq_core.h" + +#define VCHIQ_SLOT_HANDLER_STACK 8192 + +#define VCHIQ_MSG_PADDING 0 /* - */ +#define VCHIQ_MSG_CONNECT 1 /* - */ +#define VCHIQ_MSG_OPEN 2 /* + (srcport, -), fourcc, client_id */ +#define VCHIQ_MSG_OPENACK 3 /* + (srcport, dstport) */ +#define VCHIQ_MSG_CLOSE 4 /* + (srcport, dstport) */ +#define VCHIQ_MSG_DATA 5 /* + (srcport, dstport) */ +#define VCHIQ_MSG_BULK_RX 6 /* + (srcport, dstport), data, size */ +#define VCHIQ_MSG_BULK_TX 7 /* + (srcport, dstport), data, size */ +#define VCHIQ_MSG_BULK_RX_DONE 8 /* + (srcport, dstport), actual */ +#define VCHIQ_MSG_BULK_TX_DONE 9 /* + (srcport, dstport), actual */ +#define VCHIQ_MSG_PAUSE 10 /* - */ +#define VCHIQ_MSG_RESUME 11 /* - */ +#define VCHIQ_MSG_REMOTE_USE 12 /* - */ +#define VCHIQ_MSG_REMOTE_RELEASE 13 /* - */ +#define VCHIQ_MSG_REMOTE_USE_ACTIVE 14 /* - */ + +#define TYPE_SHIFT 24 + +#define VCHIQ_PORT_MAX (VCHIQ_MAX_SERVICES - 1) +#define VCHIQ_PORT_FREE 0x1000 +#define VCHIQ_PORT_IS_VALID(port) ((port) < VCHIQ_PORT_FREE) +#define VCHIQ_MAKE_MSG(type, srcport, dstport) \ + (((type) << TYPE_SHIFT) | ((srcport) << 12) | ((dstport) << 0)) +#define VCHIQ_MSG_TYPE(msgid) ((unsigned int)(msgid) >> TYPE_SHIFT) +#define VCHIQ_MSG_SRCPORT(msgid) \ + (unsigned short)(((unsigned int)(msgid) >> 12) & 0xfff) +#define VCHIQ_MSG_DSTPORT(msgid) \ + ((unsigned short)(msgid) & 0xfff) + +#define MAKE_CONNECT (VCHIQ_MSG_CONNECT << TYPE_SHIFT) +#define MAKE_OPEN(srcport) \ + ((VCHIQ_MSG_OPEN << TYPE_SHIFT) | ((srcport) << 12)) +#define MAKE_OPENACK(srcport, dstport) \ + ((VCHIQ_MSG_OPENACK << TYPE_SHIFT) | ((srcport) << 12) | ((dstport) << 0)) +#define MAKE_CLOSE(srcport, dstport) \ + ((VCHIQ_MSG_CLOSE << TYPE_SHIFT) | ((srcport) << 12) | ((dstport) << 0)) +#define MAKE_DATA(srcport, dstport) \ + ((VCHIQ_MSG_DATA << TYPE_SHIFT) | ((srcport) << 12) | ((dstport) << 0)) +#define MAKE_PAUSE (VCHIQ_MSG_PAUSE << TYPE_SHIFT) +#define MAKE_RESUME (VCHIQ_MSG_RESUME << TYPE_SHIFT) +#define MAKE_REMOTE_USE (VCHIQ_MSG_REMOTE_USE << TYPE_SHIFT) +#define MAKE_REMOTE_USE_ACTIVE (VCHIQ_MSG_REMOTE_USE_ACTIVE << TYPE_SHIFT) + +/* Ensure the fields are wide enough */ +static_assert(VCHIQ_MSG_SRCPORT(VCHIQ_MAKE_MSG(0, 0, VCHIQ_PORT_MAX)) + == 0); +static_assert(VCHIQ_MSG_TYPE(VCHIQ_MAKE_MSG(0, VCHIQ_PORT_MAX, 0)) == 0); +static_assert((unsigned int)VCHIQ_PORT_MAX < + (unsigned int)VCHIQ_PORT_FREE); + +#define VCHIQ_MSGID_PADDING VCHIQ_MAKE_MSG(VCHIQ_MSG_PADDING, 0, 0) +#define VCHIQ_MSGID_CLAIMED 0x40000000 + +#define VCHIQ_FOURCC_INVALID 0x00000000 +#define VCHIQ_FOURCC_IS_LEGAL(fourcc) ((fourcc) != VCHIQ_FOURCC_INVALID) + +#define VCHIQ_BULK_ACTUAL_ABORTED -1 + +#if VCHIQ_ENABLE_STATS +#define VCHIQ_STATS_INC(state, stat) (state->stats. stat++) +#define VCHIQ_SERVICE_STATS_INC(service, stat) (service->stats. stat++) +#define VCHIQ_SERVICE_STATS_ADD(service, stat, addend) \ + (service->stats. stat += addend) +#else +#define VCHIQ_STATS_INC(state, stat) ((void)0) +#define VCHIQ_SERVICE_STATS_INC(service, stat) ((void)0) +#define VCHIQ_SERVICE_STATS_ADD(service, stat, addend) ((void)0) +#endif + +#define HANDLE_STATE_SHIFT 12 + +#define SLOT_INFO_FROM_INDEX(state, index) (state->slot_info + (index)) +#define SLOT_DATA_FROM_INDEX(state, index) (state->slot_data + (index)) +#define SLOT_INDEX_FROM_DATA(state, data) \ + (((unsigned int)((char *)data - (char *)state->slot_data)) / \ + VCHIQ_SLOT_SIZE) +#define SLOT_INDEX_FROM_INFO(state, info) \ + ((unsigned int)(info - state->slot_info)) +#define SLOT_QUEUE_INDEX_FROM_POS(pos) \ + ((int)((unsigned int)(pos) / VCHIQ_SLOT_SIZE)) +#define SLOT_QUEUE_INDEX_FROM_POS_MASKED(pos) \ + (SLOT_QUEUE_INDEX_FROM_POS(pos) & VCHIQ_SLOT_QUEUE_MASK) + +#define BULK_INDEX(x) ((x) & (VCHIQ_NUM_SERVICE_BULKS - 1)) + +#define SRVTRACE_LEVEL(srv) \ + (((srv) && (srv)->trace) ? VCHIQ_LOG_TRACE : vchiq_core_msg_log_level) +#define SRVTRACE_ENABLED(srv, lev) \ + (((srv) && (srv)->trace) || (vchiq_core_msg_log_level >= (lev))) + +#define NO_CLOSE_RECVD 0 +#define CLOSE_RECVD 1 + +#define NO_RETRY_POLL 0 +#define RETRY_POLL 1 + +struct vchiq_open_payload { + int fourcc; + int client_id; + short version; + short version_min; +}; + +struct vchiq_openack_payload { + short version; +}; + +enum { + QMFLAGS_IS_BLOCKING = BIT(0), + QMFLAGS_NO_MUTEX_LOCK = BIT(1), + QMFLAGS_NO_MUTEX_UNLOCK = BIT(2) +}; + +enum { + VCHIQ_POLL_TERMINATE, + VCHIQ_POLL_REMOVE, + VCHIQ_POLL_TXNOTIFY, + VCHIQ_POLL_RXNOTIFY, + VCHIQ_POLL_COUNT +}; + +/* we require this for consistency between endpoints */ +static_assert(sizeof(struct vchiq_header) == 8); +static_assert(VCHIQ_VERSION >= VCHIQ_VERSION_MIN); + +static inline void check_sizes(void) +{ + BUILD_BUG_ON_NOT_POWER_OF_2(VCHIQ_SLOT_SIZE); + BUILD_BUG_ON_NOT_POWER_OF_2(VCHIQ_MAX_SLOTS); + BUILD_BUG_ON_NOT_POWER_OF_2(VCHIQ_MAX_SLOTS_PER_SIDE); + BUILD_BUG_ON_NOT_POWER_OF_2(sizeof(struct vchiq_header)); + BUILD_BUG_ON_NOT_POWER_OF_2(VCHIQ_NUM_CURRENT_BULKS); + BUILD_BUG_ON_NOT_POWER_OF_2(VCHIQ_NUM_SERVICE_BULKS); + BUILD_BUG_ON_NOT_POWER_OF_2(VCHIQ_MAX_SERVICES); +} + +/* Run time control of log level, based on KERN_XXX level. */ +int vchiq_core_log_level = VCHIQ_LOG_DEFAULT; +int vchiq_core_msg_log_level = VCHIQ_LOG_DEFAULT; +int vchiq_sync_log_level = VCHIQ_LOG_DEFAULT; + +DEFINE_SPINLOCK(bulk_waiter_spinlock); +static DEFINE_SPINLOCK(quota_spinlock); + +static unsigned int handle_seq; + +static const char *const srvstate_names[] = { + "FREE", + "HIDDEN", + "LISTENING", + "OPENING", + "OPEN", + "OPENSYNC", + "CLOSESENT", + "CLOSERECVD", + "CLOSEWAIT", + "CLOSED" +}; + +static const char *const reason_names[] = { + "SERVICE_OPENED", + "SERVICE_CLOSED", + "MESSAGE_AVAILABLE", + "BULK_TRANSMIT_DONE", + "BULK_RECEIVE_DONE", + "BULK_TRANSMIT_ABORTED", + "BULK_RECEIVE_ABORTED" +}; + +static const char *const conn_state_names[] = { + "DISCONNECTED", + "CONNECTING", + "CONNECTED", + "PAUSING", + "PAUSE_SENT", + "PAUSED", + "RESUMING", + "PAUSE_TIMEOUT", + "RESUME_TIMEOUT" +}; + +static void +release_message_sync(struct vchiq_state *state, struct vchiq_header *header); + +static const char *msg_type_str(unsigned int msg_type) +{ + switch (msg_type) { + case VCHIQ_MSG_PADDING: return "PADDING"; + case VCHIQ_MSG_CONNECT: return "CONNECT"; + case VCHIQ_MSG_OPEN: return "OPEN"; + case VCHIQ_MSG_OPENACK: return "OPENACK"; + case VCHIQ_MSG_CLOSE: return "CLOSE"; + case VCHIQ_MSG_DATA: return "DATA"; + case VCHIQ_MSG_BULK_RX: return "BULK_RX"; + case VCHIQ_MSG_BULK_TX: return "BULK_TX"; + case VCHIQ_MSG_BULK_RX_DONE: return "BULK_RX_DONE"; + case VCHIQ_MSG_BULK_TX_DONE: return "BULK_TX_DONE"; + case VCHIQ_MSG_PAUSE: return "PAUSE"; + case VCHIQ_MSG_RESUME: return "RESUME"; + case VCHIQ_MSG_REMOTE_USE: return "REMOTE_USE"; + case VCHIQ_MSG_REMOTE_RELEASE: return "REMOTE_RELEASE"; + case VCHIQ_MSG_REMOTE_USE_ACTIVE: return "REMOTE_USE_ACTIVE"; + } + return "???"; +} + +static inline void +set_service_state(struct vchiq_service *service, int newstate) +{ + vchiq_log_info(vchiq_core_log_level, "%d: srv:%d %s->%s", + service->state->id, service->localport, + srvstate_names[service->srvstate], + srvstate_names[newstate]); + service->srvstate = newstate; +} + +struct vchiq_service *handle_to_service(struct vchiq_instance *instance, unsigned int handle) +{ + int idx = handle & (VCHIQ_MAX_SERVICES - 1); + + return rcu_dereference(instance->state->services[idx]); +} +struct vchiq_service * +find_service_by_handle(struct vchiq_instance *instance, unsigned int handle) +{ + struct vchiq_service *service; + + rcu_read_lock(); + service = handle_to_service(instance, handle); + if (service && service->srvstate != VCHIQ_SRVSTATE_FREE && + service->handle == handle && + kref_get_unless_zero(&service->ref_count)) { + service = rcu_pointer_handoff(service); + rcu_read_unlock(); + return service; + } + rcu_read_unlock(); + vchiq_log_info(vchiq_core_log_level, + "Invalid service handle 0x%x", handle); + return NULL; +} + +struct vchiq_service * +find_service_by_port(struct vchiq_state *state, unsigned int localport) +{ + if (localport <= VCHIQ_PORT_MAX) { + struct vchiq_service *service; + + rcu_read_lock(); + service = rcu_dereference(state->services[localport]); + if (service && service->srvstate != VCHIQ_SRVSTATE_FREE && + kref_get_unless_zero(&service->ref_count)) { + service = rcu_pointer_handoff(service); + rcu_read_unlock(); + return service; + } + rcu_read_unlock(); + } + vchiq_log_info(vchiq_core_log_level, + "Invalid port %u", localport); + return NULL; +} + +struct vchiq_service * +find_service_for_instance(struct vchiq_instance *instance, unsigned int handle) +{ + struct vchiq_service *service; + + rcu_read_lock(); + service = handle_to_service(instance, handle); + if (service && service->srvstate != VCHIQ_SRVSTATE_FREE && + service->handle == handle && + service->instance == instance && + kref_get_unless_zero(&service->ref_count)) { + service = rcu_pointer_handoff(service); + rcu_read_unlock(); + return service; + } + rcu_read_unlock(); + vchiq_log_info(vchiq_core_log_level, + "Invalid service handle 0x%x", handle); + return NULL; +} + +struct vchiq_service * +find_closed_service_for_instance(struct vchiq_instance *instance, unsigned int handle) +{ + struct vchiq_service *service; + + rcu_read_lock(); + service = handle_to_service(instance, handle); + if (service && + (service->srvstate == VCHIQ_SRVSTATE_FREE || + service->srvstate == VCHIQ_SRVSTATE_CLOSED) && + service->handle == handle && + service->instance == instance && + kref_get_unless_zero(&service->ref_count)) { + service = rcu_pointer_handoff(service); + rcu_read_unlock(); + return service; + } + rcu_read_unlock(); + vchiq_log_info(vchiq_core_log_level, + "Invalid service handle 0x%x", handle); + return service; +} + +struct vchiq_service * +__next_service_by_instance(struct vchiq_state *state, + struct vchiq_instance *instance, + int *pidx) +{ + struct vchiq_service *service = NULL; + int idx = *pidx; + + while (idx < state->unused_service) { + struct vchiq_service *srv; + + srv = rcu_dereference(state->services[idx]); + idx++; + if (srv && srv->srvstate != VCHIQ_SRVSTATE_FREE && + srv->instance == instance) { + service = srv; + break; + } + } + + *pidx = idx; + return service; +} + +struct vchiq_service * +next_service_by_instance(struct vchiq_state *state, + struct vchiq_instance *instance, + int *pidx) +{ + struct vchiq_service *service; + + rcu_read_lock(); + while (1) { + service = __next_service_by_instance(state, instance, pidx); + if (!service) + break; + if (kref_get_unless_zero(&service->ref_count)) { + service = rcu_pointer_handoff(service); + break; + } + } + rcu_read_unlock(); + return service; +} + +void +vchiq_service_get(struct vchiq_service *service) +{ + if (!service) { + WARN(1, "%s service is NULL\n", __func__); + return; + } + kref_get(&service->ref_count); +} + +static void service_release(struct kref *kref) +{ + struct vchiq_service *service = + container_of(kref, struct vchiq_service, ref_count); + struct vchiq_state *state = service->state; + + WARN_ON(service->srvstate != VCHIQ_SRVSTATE_FREE); + rcu_assign_pointer(state->services[service->localport], NULL); + if (service->userdata_term) + service->userdata_term(service->base.userdata); + kfree_rcu(service, rcu); +} + +void +vchiq_service_put(struct vchiq_service *service) +{ + if (!service) { + WARN(1, "%s: service is NULL\n", __func__); + return; + } + kref_put(&service->ref_count, service_release); +} + +int +vchiq_get_client_id(struct vchiq_instance *instance, unsigned int handle) +{ + struct vchiq_service *service; + int id; + + rcu_read_lock(); + service = handle_to_service(instance, handle); + id = service ? service->client_id : 0; + rcu_read_unlock(); + return id; +} + +void * +vchiq_get_service_userdata(struct vchiq_instance *instance, unsigned int handle) +{ + void *userdata; + struct vchiq_service *service; + + rcu_read_lock(); + service = handle_to_service(instance, handle); + userdata = service ? service->base.userdata : NULL; + rcu_read_unlock(); + return userdata; +} +EXPORT_SYMBOL(vchiq_get_service_userdata); + +static void +mark_service_closing_internal(struct vchiq_service *service, int sh_thread) +{ + struct vchiq_state *state = service->state; + struct vchiq_service_quota *quota; + + service->closing = 1; + + /* Synchronise with other threads. */ + mutex_lock(&state->recycle_mutex); + mutex_unlock(&state->recycle_mutex); + if (!sh_thread || (state->conn_state != VCHIQ_CONNSTATE_PAUSE_SENT)) { + /* + * If we're pausing then the slot_mutex is held until resume + * by the slot handler. Therefore don't try to acquire this + * mutex if we're the slot handler and in the pause sent state. + * We don't need to in this case anyway. + */ + mutex_lock(&state->slot_mutex); + mutex_unlock(&state->slot_mutex); + } + + /* Unblock any sending thread. */ + quota = &state->service_quotas[service->localport]; + complete("a->quota_event); +} + +static void +mark_service_closing(struct vchiq_service *service) +{ + mark_service_closing_internal(service, 0); +} + +static inline enum vchiq_status +make_service_callback(struct vchiq_service *service, enum vchiq_reason reason, + struct vchiq_header *header, void *bulk_userdata) +{ + enum vchiq_status status; + + vchiq_log_trace(vchiq_core_log_level, "%d: callback:%d (%s, %pK, %pK)", + service->state->id, service->localport, reason_names[reason], + header, bulk_userdata); + status = service->base.callback(service->instance, reason, header, service->handle, + bulk_userdata); + if (status == VCHIQ_ERROR) { + vchiq_log_warning(vchiq_core_log_level, + "%d: ignoring ERROR from callback to service %x", + service->state->id, service->handle); + status = VCHIQ_SUCCESS; + } + + if (reason != VCHIQ_MESSAGE_AVAILABLE) + vchiq_release_message(service->instance, service->handle, header); + + return status; +} + +inline void +vchiq_set_conn_state(struct vchiq_state *state, enum vchiq_connstate newstate) +{ + enum vchiq_connstate oldstate = state->conn_state; + + vchiq_log_info(vchiq_core_log_level, "%d: %s->%s", state->id, conn_state_names[oldstate], + conn_state_names[newstate]); + state->conn_state = newstate; + vchiq_platform_conn_state_changed(state, oldstate, newstate); +} + +static inline void +remote_event_create(wait_queue_head_t *wq, struct remote_event *event) +{ + event->armed = 0; + /* + * Don't clear the 'fired' flag because it may already have been set + * by the other side. + */ + init_waitqueue_head(wq); +} + +/* + * All the event waiting routines in VCHIQ used a custom semaphore + * implementation that filtered most signals. This achieved a behaviour similar + * to the "killable" family of functions. While cleaning up this code all the + * routines where switched to the "interruptible" family of functions, as the + * former was deemed unjustified and the use "killable" set all VCHIQ's + * threads in D state. + */ +static inline int +remote_event_wait(wait_queue_head_t *wq, struct remote_event *event) +{ + if (!event->fired) { + event->armed = 1; + dsb(sy); + if (wait_event_interruptible(*wq, event->fired)) { + event->armed = 0; + return 0; + } + event->armed = 0; + /* Ensure that the peer sees that we are not waiting (armed == 0). */ + wmb(); + } + + event->fired = 0; + return 1; +} + +static inline void +remote_event_signal_local(wait_queue_head_t *wq, struct remote_event *event) +{ + event->fired = 1; + event->armed = 0; + wake_up_all(wq); +} + +static inline void +remote_event_poll(wait_queue_head_t *wq, struct remote_event *event) +{ + if (event->fired && event->armed) + remote_event_signal_local(wq, event); +} + +void +remote_event_pollall(struct vchiq_state *state) +{ + remote_event_poll(&state->sync_trigger_event, &state->local->sync_trigger); + remote_event_poll(&state->sync_release_event, &state->local->sync_release); + remote_event_poll(&state->trigger_event, &state->local->trigger); + remote_event_poll(&state->recycle_event, &state->local->recycle); +} + +/* + * Round up message sizes so that any space at the end of a slot is always big + * enough for a header. This relies on header size being a power of two, which + * has been verified earlier by a static assertion. + */ + +static inline size_t +calc_stride(size_t size) +{ + /* Allow room for the header */ + size += sizeof(struct vchiq_header); + + /* Round up */ + return (size + sizeof(struct vchiq_header) - 1) & + ~(sizeof(struct vchiq_header) - 1); +} + +/* Called by the slot handler thread */ +static struct vchiq_service * +get_listening_service(struct vchiq_state *state, int fourcc) +{ + int i; + + WARN_ON(fourcc == VCHIQ_FOURCC_INVALID); + + rcu_read_lock(); + for (i = 0; i < state->unused_service; i++) { + struct vchiq_service *service; + + service = rcu_dereference(state->services[i]); + if (service && + service->public_fourcc == fourcc && + (service->srvstate == VCHIQ_SRVSTATE_LISTENING || + (service->srvstate == VCHIQ_SRVSTATE_OPEN && + service->remoteport == VCHIQ_PORT_FREE)) && + kref_get_unless_zero(&service->ref_count)) { + service = rcu_pointer_handoff(service); + rcu_read_unlock(); + return service; + } + } + rcu_read_unlock(); + return NULL; +} + +/* Called by the slot handler thread */ +static struct vchiq_service * +get_connected_service(struct vchiq_state *state, unsigned int port) +{ + int i; + + rcu_read_lock(); + for (i = 0; i < state->unused_service; i++) { + struct vchiq_service *service = + rcu_dereference(state->services[i]); + + if (service && service->srvstate == VCHIQ_SRVSTATE_OPEN && + service->remoteport == port && + kref_get_unless_zero(&service->ref_count)) { + service = rcu_pointer_handoff(service); + rcu_read_unlock(); + return service; + } + } + rcu_read_unlock(); + return NULL; +} + +inline void +request_poll(struct vchiq_state *state, struct vchiq_service *service, + int poll_type) +{ + u32 value; + int index; + + if (!service) + goto skip_service; + + do { + value = atomic_read(&service->poll_flags); + } while (atomic_cmpxchg(&service->poll_flags, value, + value | BIT(poll_type)) != value); + + index = BITSET_WORD(service->localport); + do { + value = atomic_read(&state->poll_services[index]); + } while (atomic_cmpxchg(&state->poll_services[index], + value, value | BIT(service->localport & 0x1f)) != value); + +skip_service: + state->poll_needed = 1; + /* Ensure the slot handler thread sees the poll_needed flag. */ + wmb(); + + /* ... and ensure the slot handler runs. */ + remote_event_signal_local(&state->trigger_event, &state->local->trigger); +} + +/* + * Called from queue_message, by the slot handler and application threads, + * with slot_mutex held + */ +static struct vchiq_header * +reserve_space(struct vchiq_state *state, size_t space, int is_blocking) +{ + struct vchiq_shared_state *local = state->local; + int tx_pos = state->local_tx_pos; + int slot_space = VCHIQ_SLOT_SIZE - (tx_pos & VCHIQ_SLOT_MASK); + + if (space > slot_space) { + struct vchiq_header *header; + /* Fill the remaining space with padding */ + WARN_ON(!state->tx_data); + header = (struct vchiq_header *) + (state->tx_data + (tx_pos & VCHIQ_SLOT_MASK)); + header->msgid = VCHIQ_MSGID_PADDING; + header->size = slot_space - sizeof(struct vchiq_header); + + tx_pos += slot_space; + } + + /* If necessary, get the next slot. */ + if ((tx_pos & VCHIQ_SLOT_MASK) == 0) { + int slot_index; + + /* If there is no free slot... */ + + if (!try_wait_for_completion(&state->slot_available_event)) { + /* ...wait for one. */ + + VCHIQ_STATS_INC(state, slot_stalls); + + /* But first, flush through the last slot. */ + state->local_tx_pos = tx_pos; + local->tx_pos = tx_pos; + remote_event_signal(&state->remote->trigger); + + if (!is_blocking || + (wait_for_completion_interruptible(&state->slot_available_event))) + return NULL; /* No space available */ + } + + if (tx_pos == (state->slot_queue_available * VCHIQ_SLOT_SIZE)) { + complete(&state->slot_available_event); + pr_warn("%s: invalid tx_pos: %d\n", __func__, tx_pos); + return NULL; + } + + slot_index = local->slot_queue[SLOT_QUEUE_INDEX_FROM_POS_MASKED(tx_pos)]; + state->tx_data = + (char *)SLOT_DATA_FROM_INDEX(state, slot_index); + } + + state->local_tx_pos = tx_pos + space; + + return (struct vchiq_header *)(state->tx_data + + (tx_pos & VCHIQ_SLOT_MASK)); +} + +static void +process_free_data_message(struct vchiq_state *state, u32 *service_found, + struct vchiq_header *header) +{ + int msgid = header->msgid; + int port = VCHIQ_MSG_SRCPORT(msgid); + struct vchiq_service_quota *quota = &state->service_quotas[port]; + int count; + + spin_lock("a_spinlock); + count = quota->message_use_count; + if (count > 0) + quota->message_use_count = count - 1; + spin_unlock("a_spinlock); + + if (count == quota->message_quota) { + /* + * Signal the service that it + * has dropped below its quota + */ + complete("a->quota_event); + } else if (count == 0) { + vchiq_log_error(vchiq_core_log_level, + "service %d message_use_count=%d (header %pK, msgid %x, header->msgid %x, header->size %x)", + port, quota->message_use_count, header, msgid, header->msgid, + header->size); + WARN(1, "invalid message use count\n"); + } + if (!BITSET_IS_SET(service_found, port)) { + /* Set the found bit for this service */ + BITSET_SET(service_found, port); + + spin_lock("a_spinlock); + count = quota->slot_use_count; + if (count > 0) + quota->slot_use_count = count - 1; + spin_unlock("a_spinlock); + + if (count > 0) { + /* + * Signal the service in case + * it has dropped below its quota + */ + complete("a->quota_event); + vchiq_log_trace(vchiq_core_log_level, "%d: pfq:%d %x@%pK - slot_use->%d", + state->id, port, header->size, header, count - 1); + } else { + vchiq_log_error(vchiq_core_log_level, + "service %d slot_use_count=%d (header %pK, msgid %x, header->msgid %x, header->size %x)", + port, count, header, msgid, header->msgid, header->size); + WARN(1, "bad slot use count\n"); + } + } +} + +/* Called by the recycle thread. */ +static void +process_free_queue(struct vchiq_state *state, u32 *service_found, + size_t length) +{ + struct vchiq_shared_state *local = state->local; + int slot_queue_available; + + /* + * Find slots which have been freed by the other side, and return them + * to the available queue. + */ + slot_queue_available = state->slot_queue_available; + + /* + * Use a memory barrier to ensure that any state that may have been + * modified by another thread is not masked by stale prefetched + * values. + */ + mb(); + + while (slot_queue_available != local->slot_queue_recycle) { + unsigned int pos; + int slot_index = local->slot_queue[slot_queue_available & + VCHIQ_SLOT_QUEUE_MASK]; + char *data = (char *)SLOT_DATA_FROM_INDEX(state, slot_index); + int data_found = 0; + + slot_queue_available++; + /* + * Beware of the address dependency - data is calculated + * using an index written by the other side. + */ + rmb(); + + vchiq_log_trace(vchiq_core_log_level, "%d: pfq %d=%pK %x %x", + state->id, slot_index, data, local->slot_queue_recycle, + slot_queue_available); + + /* Initialise the bitmask for services which have used this slot */ + memset(service_found, 0, length); + + pos = 0; + + while (pos < VCHIQ_SLOT_SIZE) { + struct vchiq_header *header = + (struct vchiq_header *)(data + pos); + int msgid = header->msgid; + + if (VCHIQ_MSG_TYPE(msgid) == VCHIQ_MSG_DATA) { + process_free_data_message(state, service_found, + header); + data_found = 1; + } + + pos += calc_stride(header->size); + if (pos > VCHIQ_SLOT_SIZE) { + vchiq_log_error(vchiq_core_log_level, + "pfq - pos %x: header %pK, msgid %x, header->msgid %x, header->size %x", + pos, header, msgid, header->msgid, header->size); + WARN(1, "invalid slot position\n"); + } + } + + if (data_found) { + int count; + + spin_lock("a_spinlock); + count = state->data_use_count; + if (count > 0) + state->data_use_count = count - 1; + spin_unlock("a_spinlock); + if (count == state->data_quota) + complete(&state->data_quota_event); + } + + /* + * Don't allow the slot to be reused until we are no + * longer interested in it. + */ + mb(); + + state->slot_queue_available = slot_queue_available; + complete(&state->slot_available_event); + } +} + +static ssize_t +memcpy_copy_callback(void *context, void *dest, size_t offset, size_t maxsize) +{ + memcpy(dest + offset, context + offset, maxsize); + return maxsize; +} + +static ssize_t +copy_message_data(ssize_t (*copy_callback)(void *context, void *dest, size_t offset, + size_t maxsize), + void *context, + void *dest, + size_t size) +{ + size_t pos = 0; + + while (pos < size) { + ssize_t callback_result; + size_t max_bytes = size - pos; + + callback_result = copy_callback(context, dest + pos, pos, + max_bytes); + + if (callback_result < 0) + return callback_result; + + if (!callback_result) + return -EIO; + + if (callback_result > max_bytes) + return -EIO; + + pos += callback_result; + } + + return size; +} + +/* Called by the slot handler and application threads */ +static enum vchiq_status +queue_message(struct vchiq_state *state, struct vchiq_service *service, + int msgid, + ssize_t (*copy_callback)(void *context, void *dest, + size_t offset, size_t maxsize), + void *context, size_t size, int flags) +{ + struct vchiq_shared_state *local; + struct vchiq_service_quota *quota = NULL; + struct vchiq_header *header; + int type = VCHIQ_MSG_TYPE(msgid); + + size_t stride; + + local = state->local; + + stride = calc_stride(size); + + WARN_ON(stride > VCHIQ_SLOT_SIZE); + + if (!(flags & QMFLAGS_NO_MUTEX_LOCK) && + mutex_lock_killable(&state->slot_mutex)) + return VCHIQ_RETRY; + + if (type == VCHIQ_MSG_DATA) { + int tx_end_index; + + if (!service) { + WARN(1, "%s: service is NULL\n", __func__); + mutex_unlock(&state->slot_mutex); + return VCHIQ_ERROR; + } + + WARN_ON(flags & (QMFLAGS_NO_MUTEX_LOCK | + QMFLAGS_NO_MUTEX_UNLOCK)); + + if (service->closing) { + /* The service has been closed */ + mutex_unlock(&state->slot_mutex); + return VCHIQ_ERROR; + } + + quota = &state->service_quotas[service->localport]; + + spin_lock("a_spinlock); + + /* + * Ensure this service doesn't use more than its quota of + * messages or slots + */ + tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos + stride - 1); + + /* + * Ensure data messages don't use more than their quota of + * slots + */ + while ((tx_end_index != state->previous_data_index) && + (state->data_use_count == state->data_quota)) { + VCHIQ_STATS_INC(state, data_stalls); + spin_unlock("a_spinlock); + mutex_unlock(&state->slot_mutex); + + if (wait_for_completion_interruptible(&state->data_quota_event)) + return VCHIQ_RETRY; + + mutex_lock(&state->slot_mutex); + spin_lock("a_spinlock); + tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos + stride - 1); + if ((tx_end_index == state->previous_data_index) || + (state->data_use_count < state->data_quota)) { + /* Pass the signal on to other waiters */ + complete(&state->data_quota_event); + break; + } + } + + while ((quota->message_use_count == quota->message_quota) || + ((tx_end_index != quota->previous_tx_index) && + (quota->slot_use_count == quota->slot_quota))) { + spin_unlock("a_spinlock); + vchiq_log_trace(vchiq_core_log_level, + "%d: qm:%d %s,%zx - quota stall (msg %d, slot %d)", + state->id, service->localport, msg_type_str(type), size, + quota->message_use_count, quota->slot_use_count); + VCHIQ_SERVICE_STATS_INC(service, quota_stalls); + mutex_unlock(&state->slot_mutex); + if (wait_for_completion_interruptible("a->quota_event)) + return VCHIQ_RETRY; + if (service->closing) + return VCHIQ_ERROR; + if (mutex_lock_killable(&state->slot_mutex)) + return VCHIQ_RETRY; + if (service->srvstate != VCHIQ_SRVSTATE_OPEN) { + /* The service has been closed */ + mutex_unlock(&state->slot_mutex); + return VCHIQ_ERROR; + } + spin_lock("a_spinlock); + tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos + stride - 1); + } + + spin_unlock("a_spinlock); + } + + header = reserve_space(state, stride, flags & QMFLAGS_IS_BLOCKING); + + if (!header) { + if (service) + VCHIQ_SERVICE_STATS_INC(service, slot_stalls); + /* + * In the event of a failure, return the mutex to the + * state it was in + */ + if (!(flags & QMFLAGS_NO_MUTEX_LOCK)) + mutex_unlock(&state->slot_mutex); + return VCHIQ_RETRY; + } + + if (type == VCHIQ_MSG_DATA) { + ssize_t callback_result; + int tx_end_index; + int slot_use_count; + + vchiq_log_info(vchiq_core_log_level, "%d: qm %s@%pK,%zx (%d->%d)", state->id, + msg_type_str(VCHIQ_MSG_TYPE(msgid)), header, size, + VCHIQ_MSG_SRCPORT(msgid), VCHIQ_MSG_DSTPORT(msgid)); + + WARN_ON(flags & (QMFLAGS_NO_MUTEX_LOCK | + QMFLAGS_NO_MUTEX_UNLOCK)); + + callback_result = + copy_message_data(copy_callback, context, + header->data, size); + + if (callback_result < 0) { + mutex_unlock(&state->slot_mutex); + VCHIQ_SERVICE_STATS_INC(service, error_count); + return VCHIQ_ERROR; + } + + if (SRVTRACE_ENABLED(service, + VCHIQ_LOG_INFO)) + vchiq_log_dump_mem("Sent", 0, + header->data, + min_t(size_t, 16, callback_result)); + + spin_lock("a_spinlock); + quota->message_use_count++; + + tx_end_index = + SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos - 1); + + /* + * If this transmission can't fit in the last slot used by any + * service, the data_use_count must be increased. + */ + if (tx_end_index != state->previous_data_index) { + state->previous_data_index = tx_end_index; + state->data_use_count++; + } + + /* + * If this isn't the same slot last used by this service, + * the service's slot_use_count must be increased. + */ + if (tx_end_index != quota->previous_tx_index) { + quota->previous_tx_index = tx_end_index; + slot_use_count = ++quota->slot_use_count; + } else { + slot_use_count = 0; + } + + spin_unlock("a_spinlock); + + if (slot_use_count) + vchiq_log_trace(vchiq_core_log_level, + "%d: qm:%d %s,%zx - slot_use->%d (hdr %p)", state->id, + service->localport, msg_type_str(VCHIQ_MSG_TYPE(msgid)), + size, slot_use_count, header); + + VCHIQ_SERVICE_STATS_INC(service, ctrl_tx_count); + VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size); + } else { + vchiq_log_info(vchiq_core_log_level, "%d: qm %s@%pK,%zx (%d->%d)", state->id, + msg_type_str(VCHIQ_MSG_TYPE(msgid)), header, size, + VCHIQ_MSG_SRCPORT(msgid), VCHIQ_MSG_DSTPORT(msgid)); + if (size != 0) { + /* + * It is assumed for now that this code path + * only happens from calls inside this file. + * + * External callers are through the vchiq_queue_message + * path which always sets the type to be VCHIQ_MSG_DATA + * + * At first glance this appears to be correct but + * more review is needed. + */ + copy_message_data(copy_callback, context, + header->data, size); + } + VCHIQ_STATS_INC(state, ctrl_tx_count); + } + + header->msgid = msgid; + header->size = size; + + { + int svc_fourcc; + + svc_fourcc = service + ? service->base.fourcc + : VCHIQ_MAKE_FOURCC('?', '?', '?', '?'); + + vchiq_log_info(SRVTRACE_LEVEL(service), + "Sent Msg %s(%u) to %c%c%c%c s:%u d:%d len:%zu", + msg_type_str(VCHIQ_MSG_TYPE(msgid)), VCHIQ_MSG_TYPE(msgid), + VCHIQ_FOURCC_AS_4CHARS(svc_fourcc), VCHIQ_MSG_SRCPORT(msgid), + VCHIQ_MSG_DSTPORT(msgid), size); + } + + /* Make sure the new header is visible to the peer. */ + wmb(); + + /* Make the new tx_pos visible to the peer. */ + local->tx_pos = state->local_tx_pos; + wmb(); + + if (service && (type == VCHIQ_MSG_CLOSE)) + set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT); + + if (!(flags & QMFLAGS_NO_MUTEX_UNLOCK)) + mutex_unlock(&state->slot_mutex); + + remote_event_signal(&state->remote->trigger); + + return VCHIQ_SUCCESS; +} + +/* Called by the slot handler and application threads */ +static enum vchiq_status +queue_message_sync(struct vchiq_state *state, struct vchiq_service *service, + int msgid, + ssize_t (*copy_callback)(void *context, void *dest, + size_t offset, size_t maxsize), + void *context, int size, int is_blocking) +{ + struct vchiq_shared_state *local; + struct vchiq_header *header; + ssize_t callback_result; + + local = state->local; + + if (VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_RESUME && + mutex_lock_killable(&state->sync_mutex)) + return VCHIQ_RETRY; + + remote_event_wait(&state->sync_release_event, &local->sync_release); + + /* Ensure that reads don't overtake the remote_event_wait. */ + rmb(); + + header = (struct vchiq_header *)SLOT_DATA_FROM_INDEX(state, + local->slot_sync); + + { + int oldmsgid = header->msgid; + + if (oldmsgid != VCHIQ_MSGID_PADDING) + vchiq_log_error(vchiq_core_log_level, "%d: qms - msgid %x, not PADDING", + state->id, oldmsgid); + } + + vchiq_log_info(vchiq_sync_log_level, + "%d: qms %s@%pK,%x (%d->%d)", state->id, + msg_type_str(VCHIQ_MSG_TYPE(msgid)), + header, size, VCHIQ_MSG_SRCPORT(msgid), + VCHIQ_MSG_DSTPORT(msgid)); + + callback_result = + copy_message_data(copy_callback, context, + header->data, size); + + if (callback_result < 0) { + mutex_unlock(&state->slot_mutex); + VCHIQ_SERVICE_STATS_INC(service, error_count); + return VCHIQ_ERROR; + } + + if (service) { + if (SRVTRACE_ENABLED(service, + VCHIQ_LOG_INFO)) + vchiq_log_dump_mem("Sent", 0, + header->data, + min_t(size_t, 16, callback_result)); + + VCHIQ_SERVICE_STATS_INC(service, ctrl_tx_count); + VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size); + } else { + VCHIQ_STATS_INC(state, ctrl_tx_count); + } + + header->size = size; + header->msgid = msgid; + + if (vchiq_sync_log_level >= VCHIQ_LOG_TRACE) { + int svc_fourcc; + + svc_fourcc = service + ? service->base.fourcc + : VCHIQ_MAKE_FOURCC('?', '?', '?', '?'); + + vchiq_log_trace(vchiq_sync_log_level, + "Sent Sync Msg %s(%u) to %c%c%c%c s:%u d:%d len:%d", + msg_type_str(VCHIQ_MSG_TYPE(msgid)), VCHIQ_MSG_TYPE(msgid), + VCHIQ_FOURCC_AS_4CHARS(svc_fourcc), VCHIQ_MSG_SRCPORT(msgid), + VCHIQ_MSG_DSTPORT(msgid), size); + } + + remote_event_signal(&state->remote->sync_trigger); + + if (VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_PAUSE) + mutex_unlock(&state->sync_mutex); + + return VCHIQ_SUCCESS; +} + +static inline void +claim_slot(struct vchiq_slot_info *slot) +{ + slot->use_count++; +} + +static void +release_slot(struct vchiq_state *state, struct vchiq_slot_info *slot_info, + struct vchiq_header *header, struct vchiq_service *service) +{ + mutex_lock(&state->recycle_mutex); + + if (header) { + int msgid = header->msgid; + + if (((msgid & VCHIQ_MSGID_CLAIMED) == 0) || (service && service->closing)) { + mutex_unlock(&state->recycle_mutex); + return; + } + + /* Rewrite the message header to prevent a double release */ + header->msgid = msgid & ~VCHIQ_MSGID_CLAIMED; + } + + slot_info->release_count++; + + if (slot_info->release_count == slot_info->use_count) { + int slot_queue_recycle; + /* Add to the freed queue */ + + /* + * A read barrier is necessary here to prevent speculative + * fetches of remote->slot_queue_recycle from overtaking the + * mutex. + */ + rmb(); + + slot_queue_recycle = state->remote->slot_queue_recycle; + state->remote->slot_queue[slot_queue_recycle & + VCHIQ_SLOT_QUEUE_MASK] = + SLOT_INDEX_FROM_INFO(state, slot_info); + state->remote->slot_queue_recycle = slot_queue_recycle + 1; + vchiq_log_info(vchiq_core_log_level, "%d: %s %d - recycle->%x", state->id, __func__, + SLOT_INDEX_FROM_INFO(state, slot_info), + state->remote->slot_queue_recycle); + + /* + * A write barrier is necessary, but remote_event_signal + * contains one. + */ + remote_event_signal(&state->remote->recycle); + } + + mutex_unlock(&state->recycle_mutex); +} + +static inline enum vchiq_reason +get_bulk_reason(struct vchiq_bulk *bulk) +{ + if (bulk->dir == VCHIQ_BULK_TRANSMIT) { + if (bulk->actual == VCHIQ_BULK_ACTUAL_ABORTED) + return VCHIQ_BULK_TRANSMIT_ABORTED; + + return VCHIQ_BULK_TRANSMIT_DONE; + } + + if (bulk->actual == VCHIQ_BULK_ACTUAL_ABORTED) + return VCHIQ_BULK_RECEIVE_ABORTED; + + return VCHIQ_BULK_RECEIVE_DONE; +} + +/* Called by the slot handler - don't hold the bulk mutex */ +static enum vchiq_status +notify_bulks(struct vchiq_service *service, struct vchiq_bulk_queue *queue, + int retry_poll) +{ + enum vchiq_status status = VCHIQ_SUCCESS; + + vchiq_log_trace(vchiq_core_log_level, "%d: nb:%d %cx - p=%x rn=%x r=%x", service->state->id, + service->localport, (queue == &service->bulk_tx) ? 't' : 'r', + queue->process, queue->remote_notify, queue->remove); + + queue->remote_notify = queue->process; + + while (queue->remove != queue->remote_notify) { + struct vchiq_bulk *bulk = + &queue->bulks[BULK_INDEX(queue->remove)]; + + /* + * Only generate callbacks for non-dummy bulk + * requests, and non-terminated services + */ + if (bulk->data && service->instance) { + if (bulk->actual != VCHIQ_BULK_ACTUAL_ABORTED) { + if (bulk->dir == VCHIQ_BULK_TRANSMIT) { + VCHIQ_SERVICE_STATS_INC(service, bulk_tx_count); + VCHIQ_SERVICE_STATS_ADD(service, bulk_tx_bytes, + bulk->actual); + } else { + VCHIQ_SERVICE_STATS_INC(service, bulk_rx_count); + VCHIQ_SERVICE_STATS_ADD(service, bulk_rx_bytes, + bulk->actual); + } + } else { + VCHIQ_SERVICE_STATS_INC(service, bulk_aborted_count); + } + if (bulk->mode == VCHIQ_BULK_MODE_BLOCKING) { + struct bulk_waiter *waiter; + + spin_lock(&bulk_waiter_spinlock); + waiter = bulk->userdata; + if (waiter) { + waiter->actual = bulk->actual; + complete(&waiter->event); + } + spin_unlock(&bulk_waiter_spinlock); + } else if (bulk->mode == VCHIQ_BULK_MODE_CALLBACK) { + enum vchiq_reason reason = + get_bulk_reason(bulk); + status = make_service_callback(service, reason, NULL, + bulk->userdata); + if (status == VCHIQ_RETRY) + break; + } + } + + queue->remove++; + complete(&service->bulk_remove_event); + } + if (!retry_poll) + status = VCHIQ_SUCCESS; + + if (status == VCHIQ_RETRY) + request_poll(service->state, service, (queue == &service->bulk_tx) ? + VCHIQ_POLL_TXNOTIFY : VCHIQ_POLL_RXNOTIFY); + + return status; +} + +static void +poll_services_of_group(struct vchiq_state *state, int group) +{ + u32 flags = atomic_xchg(&state->poll_services[group], 0); + int i; + + for (i = 0; flags; i++) { + struct vchiq_service *service; + u32 service_flags; + + if ((flags & BIT(i)) == 0) + continue; + + service = find_service_by_port(state, (group << 5) + i); + flags &= ~BIT(i); + + if (!service) + continue; + + service_flags = atomic_xchg(&service->poll_flags, 0); + if (service_flags & BIT(VCHIQ_POLL_REMOVE)) { + vchiq_log_info(vchiq_core_log_level, "%d: ps - remove %d<->%d", + state->id, service->localport, + service->remoteport); + + /* + * Make it look like a client, because + * it must be removed and not left in + * the LISTENING state. + */ + service->public_fourcc = VCHIQ_FOURCC_INVALID; + + if (vchiq_close_service_internal(service, NO_CLOSE_RECVD) != + VCHIQ_SUCCESS) + request_poll(state, service, VCHIQ_POLL_REMOVE); + } else if (service_flags & BIT(VCHIQ_POLL_TERMINATE)) { + vchiq_log_info(vchiq_core_log_level, "%d: ps - terminate %d<->%d", + state->id, service->localport, service->remoteport); + if (vchiq_close_service_internal(service, NO_CLOSE_RECVD) != VCHIQ_SUCCESS) + request_poll(state, service, VCHIQ_POLL_TERMINATE); + } + if (service_flags & BIT(VCHIQ_POLL_TXNOTIFY)) + notify_bulks(service, &service->bulk_tx, RETRY_POLL); + if (service_flags & BIT(VCHIQ_POLL_RXNOTIFY)) + notify_bulks(service, &service->bulk_rx, RETRY_POLL); + vchiq_service_put(service); + } +} + +/* Called by the slot handler thread */ +static void +poll_services(struct vchiq_state *state) +{ + int group; + + for (group = 0; group < BITSET_SIZE(state->unused_service); group++) + poll_services_of_group(state, group); +} + +/* Called with the bulk_mutex held */ +static void +abort_outstanding_bulks(struct vchiq_service *service, + struct vchiq_bulk_queue *queue) +{ + int is_tx = (queue == &service->bulk_tx); + + vchiq_log_trace(vchiq_core_log_level, "%d: aob:%d %cx - li=%x ri=%x p=%x", + service->state->id, service->localport, is_tx ? 't' : 'r', + queue->local_insert, queue->remote_insert, queue->process); + + WARN_ON((int)(queue->local_insert - queue->process) < 0); + WARN_ON((int)(queue->remote_insert - queue->process) < 0); + + while ((queue->process != queue->local_insert) || + (queue->process != queue->remote_insert)) { + struct vchiq_bulk *bulk = &queue->bulks[BULK_INDEX(queue->process)]; + + if (queue->process == queue->remote_insert) { + /* fabricate a matching dummy bulk */ + bulk->remote_data = NULL; + bulk->remote_size = 0; + queue->remote_insert++; + } + + if (queue->process != queue->local_insert) { + vchiq_complete_bulk(service->instance, bulk); + + vchiq_log_info(SRVTRACE_LEVEL(service), + "%s %c%c%c%c d:%d ABORTED - tx len:%d, rx len:%d", + is_tx ? "Send Bulk to" : "Recv Bulk from", + VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), + service->remoteport, bulk->size, bulk->remote_size); + } else { + /* fabricate a matching dummy bulk */ + bulk->data = 0; + bulk->size = 0; + bulk->actual = VCHIQ_BULK_ACTUAL_ABORTED; + bulk->dir = is_tx ? VCHIQ_BULK_TRANSMIT : + VCHIQ_BULK_RECEIVE; + queue->local_insert++; + } + + queue->process++; + } +} + +static int +parse_open(struct vchiq_state *state, struct vchiq_header *header) +{ + const struct vchiq_open_payload *payload; + struct vchiq_service *service = NULL; + int msgid, size; + unsigned int localport, remoteport, fourcc; + short version, version_min; + + msgid = header->msgid; + size = header->size; + localport = VCHIQ_MSG_DSTPORT(msgid); + remoteport = VCHIQ_MSG_SRCPORT(msgid); + if (size < sizeof(struct vchiq_open_payload)) + goto fail_open; + + payload = (struct vchiq_open_payload *)header->data; + fourcc = payload->fourcc; + vchiq_log_info(vchiq_core_log_level, "%d: prs OPEN@%pK (%d->'%c%c%c%c')", + state->id, header, localport, VCHIQ_FOURCC_AS_4CHARS(fourcc)); + + service = get_listening_service(state, fourcc); + if (!service) + goto fail_open; + + /* A matching service exists */ + version = payload->version; + version_min = payload->version_min; + + if ((service->version < version_min) || (version < service->version_min)) { + /* Version mismatch */ + vchiq_loud_error_header(); + vchiq_loud_error("%d: service %d (%c%c%c%c) version mismatch - local (%d, min %d) vs. remote (%d, min %d)", + state->id, service->localport, VCHIQ_FOURCC_AS_4CHARS(fourcc), + service->version, service->version_min, version, version_min); + vchiq_loud_error_footer(); + vchiq_service_put(service); + service = NULL; + goto fail_open; + } + service->peer_version = version; + + if (service->srvstate == VCHIQ_SRVSTATE_LISTENING) { + struct vchiq_openack_payload ack_payload = { + service->version + }; + int openack_id = MAKE_OPENACK(service->localport, remoteport); + + if (state->version_common < + VCHIQ_VERSION_SYNCHRONOUS_MODE) + service->sync = 0; + + /* Acknowledge the OPEN */ + if (service->sync) { + if (queue_message_sync(state, NULL, openack_id, memcpy_copy_callback, + &ack_payload, sizeof(ack_payload), 0) == VCHIQ_RETRY) + goto bail_not_ready; + + /* The service is now open */ + set_service_state(service, VCHIQ_SRVSTATE_OPENSYNC); + } else { + if (queue_message(state, NULL, openack_id, memcpy_copy_callback, + &ack_payload, sizeof(ack_payload), 0) == VCHIQ_RETRY) + goto bail_not_ready; + + /* The service is now open */ + set_service_state(service, VCHIQ_SRVSTATE_OPEN); + } + } + + /* Success - the message has been dealt with */ + vchiq_service_put(service); + return 1; + +fail_open: + /* No available service, or an invalid request - send a CLOSE */ + if (queue_message(state, NULL, MAKE_CLOSE(0, VCHIQ_MSG_SRCPORT(msgid)), + NULL, NULL, 0, 0) == VCHIQ_RETRY) + goto bail_not_ready; + + return 1; + +bail_not_ready: + if (service) + vchiq_service_put(service); + + return 0; +} + +/** + * parse_message() - parses a single message from the rx slot + * @state: vchiq state struct + * @header: message header + * + * Context: Process context + * + * Return: + * * >= 0 - size of the parsed message payload (without header) + * * -EINVAL - fatal error occurred, bail out is required + */ +static int +parse_message(struct vchiq_state *state, struct vchiq_header *header) +{ + struct vchiq_service *service = NULL; + unsigned int localport, remoteport; + int msgid, size, type, ret = -EINVAL; + + DEBUG_INITIALISE(state->local); + + DEBUG_VALUE(PARSE_HEADER, (int)(long)header); + msgid = header->msgid; + DEBUG_VALUE(PARSE_MSGID, msgid); + size = header->size; + type = VCHIQ_MSG_TYPE(msgid); + localport = VCHIQ_MSG_DSTPORT(msgid); + remoteport = VCHIQ_MSG_SRCPORT(msgid); + + if (type != VCHIQ_MSG_DATA) + VCHIQ_STATS_INC(state, ctrl_rx_count); + + switch (type) { + case VCHIQ_MSG_OPENACK: + case VCHIQ_MSG_CLOSE: + case VCHIQ_MSG_DATA: + case VCHIQ_MSG_BULK_RX: + case VCHIQ_MSG_BULK_TX: + case VCHIQ_MSG_BULK_RX_DONE: + case VCHIQ_MSG_BULK_TX_DONE: + service = find_service_by_port(state, localport); + if ((!service || + ((service->remoteport != remoteport) && + (service->remoteport != VCHIQ_PORT_FREE))) && + (localport == 0) && + (type == VCHIQ_MSG_CLOSE)) { + /* + * This could be a CLOSE from a client which + * hadn't yet received the OPENACK - look for + * the connected service + */ + if (service) + vchiq_service_put(service); + service = get_connected_service(state, remoteport); + if (service) + vchiq_log_warning(vchiq_core_log_level, + "%d: prs %s@%pK (%d->%d) - found connected service %d", + state->id, msg_type_str(type), header, + remoteport, localport, service->localport); + } + + if (!service) { + vchiq_log_error(vchiq_core_log_level, + "%d: prs %s@%pK (%d->%d) - invalid/closed service %d", + state->id, msg_type_str(type), header, remoteport, + localport, localport); + goto skip_message; + } + break; + default: + break; + } + + if (SRVTRACE_ENABLED(service, VCHIQ_LOG_INFO)) { + int svc_fourcc; + + svc_fourcc = service + ? service->base.fourcc + : VCHIQ_MAKE_FOURCC('?', '?', '?', '?'); + vchiq_log_info(SRVTRACE_LEVEL(service), + "Rcvd Msg %s(%u) from %c%c%c%c s:%d d:%d len:%d", + msg_type_str(type), type, VCHIQ_FOURCC_AS_4CHARS(svc_fourcc), + remoteport, localport, size); + if (size > 0) + vchiq_log_dump_mem("Rcvd", 0, header->data, min(16, size)); + } + + if (((unsigned long)header & VCHIQ_SLOT_MASK) + + calc_stride(size) > VCHIQ_SLOT_SIZE) { + vchiq_log_error(vchiq_core_log_level, + "header %pK (msgid %x) - size %x too big for slot", + header, (unsigned int)msgid, (unsigned int)size); + WARN(1, "oversized for slot\n"); + } + + switch (type) { + case VCHIQ_MSG_OPEN: + WARN_ON(VCHIQ_MSG_DSTPORT(msgid)); + if (!parse_open(state, header)) + goto bail_not_ready; + break; + case VCHIQ_MSG_OPENACK: + if (size >= sizeof(struct vchiq_openack_payload)) { + const struct vchiq_openack_payload *payload = + (struct vchiq_openack_payload *) + header->data; + service->peer_version = payload->version; + } + vchiq_log_info(vchiq_core_log_level, "%d: prs OPENACK@%pK,%x (%d->%d) v:%d", + state->id, header, size, remoteport, localport, + service->peer_version); + if (service->srvstate == VCHIQ_SRVSTATE_OPENING) { + service->remoteport = remoteport; + set_service_state(service, VCHIQ_SRVSTATE_OPEN); + complete(&service->remove_event); + } else { + vchiq_log_error(vchiq_core_log_level, "OPENACK received in state %s", + srvstate_names[service->srvstate]); + } + break; + case VCHIQ_MSG_CLOSE: + WARN_ON(size); /* There should be no data */ + + vchiq_log_info(vchiq_core_log_level, "%d: prs CLOSE@%pK (%d->%d)", + state->id, header, remoteport, localport); + + mark_service_closing_internal(service, 1); + + if (vchiq_close_service_internal(service, CLOSE_RECVD) == VCHIQ_RETRY) + goto bail_not_ready; + + vchiq_log_info(vchiq_core_log_level, "Close Service %c%c%c%c s:%u d:%d", + VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), + service->localport, service->remoteport); + break; + case VCHIQ_MSG_DATA: + vchiq_log_info(vchiq_core_log_level, "%d: prs DATA@%pK,%x (%d->%d)", + state->id, header, size, remoteport, localport); + + if ((service->remoteport == remoteport) && + (service->srvstate == VCHIQ_SRVSTATE_OPEN)) { + header->msgid = msgid | VCHIQ_MSGID_CLAIMED; + claim_slot(state->rx_info); + DEBUG_TRACE(PARSE_LINE); + if (make_service_callback(service, VCHIQ_MESSAGE_AVAILABLE, header, + NULL) == VCHIQ_RETRY) { + DEBUG_TRACE(PARSE_LINE); + goto bail_not_ready; + } + VCHIQ_SERVICE_STATS_INC(service, ctrl_rx_count); + VCHIQ_SERVICE_STATS_ADD(service, ctrl_rx_bytes, size); + } else { + VCHIQ_STATS_INC(state, error_count); + } + break; + case VCHIQ_MSG_CONNECT: + vchiq_log_info(vchiq_core_log_level, "%d: prs CONNECT@%pK", state->id, header); + state->version_common = ((struct vchiq_slot_zero *) + state->slot_data)->version; + complete(&state->connect); + break; + case VCHIQ_MSG_BULK_RX: + case VCHIQ_MSG_BULK_TX: + /* + * We should never receive a bulk request from the + * other side since we're not setup to perform as the + * master. + */ + WARN_ON(1); + break; + case VCHIQ_MSG_BULK_RX_DONE: + case VCHIQ_MSG_BULK_TX_DONE: + if ((service->remoteport == remoteport) && + (service->srvstate != VCHIQ_SRVSTATE_FREE)) { + struct vchiq_bulk_queue *queue; + struct vchiq_bulk *bulk; + + queue = (type == VCHIQ_MSG_BULK_RX_DONE) ? + &service->bulk_rx : &service->bulk_tx; + + DEBUG_TRACE(PARSE_LINE); + if (mutex_lock_killable(&service->bulk_mutex)) { + DEBUG_TRACE(PARSE_LINE); + goto bail_not_ready; + } + if ((int)(queue->remote_insert - + queue->local_insert) >= 0) { + vchiq_log_error(vchiq_core_log_level, + "%d: prs %s@%pK (%d->%d) unexpected (ri=%d,li=%d)", + state->id, msg_type_str(type), header, remoteport, + localport, queue->remote_insert, + queue->local_insert); + mutex_unlock(&service->bulk_mutex); + break; + } + if (queue->process != queue->remote_insert) { + pr_err("%s: p %x != ri %x\n", + __func__, + queue->process, + queue->remote_insert); + mutex_unlock(&service->bulk_mutex); + goto bail_not_ready; + } + + bulk = &queue->bulks[BULK_INDEX(queue->remote_insert)]; + bulk->actual = *(int *)header->data; + queue->remote_insert++; + + vchiq_log_info(vchiq_core_log_level, "%d: prs %s@%pK (%d->%d) %x@%pad", + state->id, msg_type_str(type), header, remoteport, localport, + bulk->actual, &bulk->data); + + vchiq_log_trace(vchiq_core_log_level, "%d: prs:%d %cx li=%x ri=%x p=%x", + state->id, localport, + (type == VCHIQ_MSG_BULK_RX_DONE) ? 'r' : 't', + queue->local_insert, queue->remote_insert, queue->process); + + DEBUG_TRACE(PARSE_LINE); + WARN_ON(queue->process == queue->local_insert); + vchiq_complete_bulk(service->instance, bulk); + queue->process++; + mutex_unlock(&service->bulk_mutex); + DEBUG_TRACE(PARSE_LINE); + notify_bulks(service, queue, RETRY_POLL); + DEBUG_TRACE(PARSE_LINE); + } + break; + case VCHIQ_MSG_PADDING: + vchiq_log_trace(vchiq_core_log_level, "%d: prs PADDING@%pK,%x", + state->id, header, size); + break; + case VCHIQ_MSG_PAUSE: + /* If initiated, signal the application thread */ + vchiq_log_trace(vchiq_core_log_level, "%d: prs PAUSE@%pK,%x", + state->id, header, size); + if (state->conn_state == VCHIQ_CONNSTATE_PAUSED) { + vchiq_log_error(vchiq_core_log_level, "%d: PAUSE received in state PAUSED", + state->id); + break; + } + if (state->conn_state != VCHIQ_CONNSTATE_PAUSE_SENT) { + /* Send a PAUSE in response */ + if (queue_message(state, NULL, MAKE_PAUSE, NULL, NULL, 0, + QMFLAGS_NO_MUTEX_UNLOCK) == VCHIQ_RETRY) + goto bail_not_ready; + } + /* At this point slot_mutex is held */ + vchiq_set_conn_state(state, VCHIQ_CONNSTATE_PAUSED); + break; + case VCHIQ_MSG_RESUME: + vchiq_log_trace(vchiq_core_log_level, "%d: prs RESUME@%pK,%x", + state->id, header, size); + /* Release the slot mutex */ + mutex_unlock(&state->slot_mutex); + vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED); + break; + + case VCHIQ_MSG_REMOTE_USE: + vchiq_on_remote_use(state); + break; + case VCHIQ_MSG_REMOTE_RELEASE: + vchiq_on_remote_release(state); + break; + case VCHIQ_MSG_REMOTE_USE_ACTIVE: + break; + + default: + vchiq_log_error(vchiq_core_log_level, "%d: prs invalid msgid %x@%pK,%x", + state->id, msgid, header, size); + WARN(1, "invalid message\n"); + break; + } + +skip_message: + ret = size; + +bail_not_ready: + if (service) + vchiq_service_put(service); + + return ret; +} + +/* Called by the slot handler thread */ +static void +parse_rx_slots(struct vchiq_state *state) +{ + struct vchiq_shared_state *remote = state->remote; + int tx_pos; + + DEBUG_INITIALISE(state->local); + + tx_pos = remote->tx_pos; + + while (state->rx_pos != tx_pos) { + struct vchiq_header *header; + int size; + + DEBUG_TRACE(PARSE_LINE); + if (!state->rx_data) { + int rx_index; + + WARN_ON(state->rx_pos & VCHIQ_SLOT_MASK); + rx_index = remote->slot_queue[ + SLOT_QUEUE_INDEX_FROM_POS_MASKED(state->rx_pos)]; + state->rx_data = (char *)SLOT_DATA_FROM_INDEX(state, + rx_index); + state->rx_info = SLOT_INFO_FROM_INDEX(state, rx_index); + + /* + * Initialise use_count to one, and increment + * release_count at the end of the slot to avoid + * releasing the slot prematurely. + */ + state->rx_info->use_count = 1; + state->rx_info->release_count = 0; + } + + header = (struct vchiq_header *)(state->rx_data + + (state->rx_pos & VCHIQ_SLOT_MASK)); + size = parse_message(state, header); + if (size < 0) + return; + + state->rx_pos += calc_stride(size); + + DEBUG_TRACE(PARSE_LINE); + /* + * Perform some housekeeping when the end of the slot is + * reached. + */ + if ((state->rx_pos & VCHIQ_SLOT_MASK) == 0) { + /* Remove the extra reference count. */ + release_slot(state, state->rx_info, NULL, NULL); + state->rx_data = NULL; + } + } +} + +/** + * handle_poll() - handle service polling and other rare conditions + * @state: vchiq state struct + * + * Context: Process context + * + * Return: + * * 0 - poll handled successful + * * -EAGAIN - retry later + */ +static int +handle_poll(struct vchiq_state *state) +{ + switch (state->conn_state) { + case VCHIQ_CONNSTATE_CONNECTED: + /* Poll the services as requested */ + poll_services(state); + break; + + case VCHIQ_CONNSTATE_PAUSING: + if (queue_message(state, NULL, MAKE_PAUSE, NULL, NULL, 0, + QMFLAGS_NO_MUTEX_UNLOCK) != VCHIQ_RETRY) { + vchiq_set_conn_state(state, VCHIQ_CONNSTATE_PAUSE_SENT); + } else { + /* Retry later */ + return -EAGAIN; + } + break; + + case VCHIQ_CONNSTATE_RESUMING: + if (queue_message(state, NULL, MAKE_RESUME, NULL, NULL, 0, + QMFLAGS_NO_MUTEX_LOCK) != VCHIQ_RETRY) { + vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED); + } else { + /* + * This should really be impossible, + * since the PAUSE should have flushed + * through outstanding messages. + */ + vchiq_log_error(vchiq_core_log_level, "Failed to send RESUME message"); + } + break; + default: + break; + } + + return 0; +} + +/* Called by the slot handler thread */ +static int +slot_handler_func(void *v) +{ + struct vchiq_state *state = v; + struct vchiq_shared_state *local = state->local; + + DEBUG_INITIALISE(local); + + while (1) { + DEBUG_COUNT(SLOT_HANDLER_COUNT); + DEBUG_TRACE(SLOT_HANDLER_LINE); + remote_event_wait(&state->trigger_event, &local->trigger); + + /* Ensure that reads don't overtake the remote_event_wait. */ + rmb(); + + DEBUG_TRACE(SLOT_HANDLER_LINE); + if (state->poll_needed) { + state->poll_needed = 0; + + /* + * Handle service polling and other rare conditions here + * out of the mainline code + */ + if (handle_poll(state) == -EAGAIN) + state->poll_needed = 1; + } + + DEBUG_TRACE(SLOT_HANDLER_LINE); + parse_rx_slots(state); + } + return 0; +} + +/* Called by the recycle thread */ +static int +recycle_func(void *v) +{ + struct vchiq_state *state = v; + struct vchiq_shared_state *local = state->local; + u32 *found; + size_t length; + + length = sizeof(*found) * BITSET_SIZE(VCHIQ_MAX_SERVICES); + + found = kmalloc_array(BITSET_SIZE(VCHIQ_MAX_SERVICES), sizeof(*found), + GFP_KERNEL); + if (!found) + return -ENOMEM; + + while (1) { + remote_event_wait(&state->recycle_event, &local->recycle); + + process_free_queue(state, found, length); + } + return 0; +} + +/* Called by the sync thread */ +static int +sync_func(void *v) +{ + struct vchiq_state *state = v; + struct vchiq_shared_state *local = state->local; + struct vchiq_header *header = + (struct vchiq_header *)SLOT_DATA_FROM_INDEX(state, + state->remote->slot_sync); + + while (1) { + struct vchiq_service *service; + int msgid, size; + int type; + unsigned int localport, remoteport; + + remote_event_wait(&state->sync_trigger_event, &local->sync_trigger); + + /* Ensure that reads don't overtake the remote_event_wait. */ + rmb(); + + msgid = header->msgid; + size = header->size; + type = VCHIQ_MSG_TYPE(msgid); + localport = VCHIQ_MSG_DSTPORT(msgid); + remoteport = VCHIQ_MSG_SRCPORT(msgid); + + service = find_service_by_port(state, localport); + + if (!service) { + vchiq_log_error(vchiq_sync_log_level, + "%d: sf %s@%pK (%d->%d) - invalid/closed service %d", + state->id, msg_type_str(type), header, + remoteport, localport, localport); + release_message_sync(state, header); + continue; + } + + if (vchiq_sync_log_level >= VCHIQ_LOG_TRACE) { + int svc_fourcc; + + svc_fourcc = service + ? service->base.fourcc + : VCHIQ_MAKE_FOURCC('?', '?', '?', '?'); + vchiq_log_trace(vchiq_sync_log_level, + "Rcvd Msg %s from %c%c%c%c s:%d d:%d len:%d", + msg_type_str(type), VCHIQ_FOURCC_AS_4CHARS(svc_fourcc), + remoteport, localport, size); + if (size > 0) + vchiq_log_dump_mem("Rcvd", 0, header->data, min(16, size)); + } + + switch (type) { + case VCHIQ_MSG_OPENACK: + if (size >= sizeof(struct vchiq_openack_payload)) { + const struct vchiq_openack_payload *payload = + (struct vchiq_openack_payload *) + header->data; + service->peer_version = payload->version; + } + vchiq_log_info(vchiq_sync_log_level, "%d: sf OPENACK@%pK,%x (%d->%d) v:%d", + state->id, header, size, remoteport, localport, + service->peer_version); + if (service->srvstate == VCHIQ_SRVSTATE_OPENING) { + service->remoteport = remoteport; + set_service_state(service, VCHIQ_SRVSTATE_OPENSYNC); + service->sync = 1; + complete(&service->remove_event); + } + release_message_sync(state, header); + break; + + case VCHIQ_MSG_DATA: + vchiq_log_trace(vchiq_sync_log_level, "%d: sf DATA@%pK,%x (%d->%d)", + state->id, header, size, remoteport, localport); + + if ((service->remoteport == remoteport) && + (service->srvstate == VCHIQ_SRVSTATE_OPENSYNC)) { + if (make_service_callback(service, VCHIQ_MESSAGE_AVAILABLE, header, + NULL) == VCHIQ_RETRY) + vchiq_log_error(vchiq_sync_log_level, + "synchronous callback to service %d returns VCHIQ_RETRY", + localport); + } + break; + + default: + vchiq_log_error(vchiq_sync_log_level, "%d: sf unexpected msgid %x@%pK,%x", + state->id, msgid, header, size); + release_message_sync(state, header); + break; + } + + vchiq_service_put(service); + } + + return 0; +} + +inline const char * +get_conn_state_name(enum vchiq_connstate conn_state) +{ + return conn_state_names[conn_state]; +} + +struct vchiq_slot_zero * +vchiq_init_slots(void *mem_base, int mem_size) +{ + int mem_align = + (int)((VCHIQ_SLOT_SIZE - (long)mem_base) & VCHIQ_SLOT_MASK); + struct vchiq_slot_zero *slot_zero = + (struct vchiq_slot_zero *)(mem_base + mem_align); + int num_slots = (mem_size - mem_align) / VCHIQ_SLOT_SIZE; + int first_data_slot = VCHIQ_SLOT_ZERO_SLOTS; + + check_sizes(); + + /* Ensure there is enough memory to run an absolutely minimum system */ + num_slots -= first_data_slot; + + if (num_slots < 4) { + vchiq_log_error(vchiq_core_log_level, "%s - insufficient memory %x bytes", + __func__, mem_size); + return NULL; + } + + memset(slot_zero, 0, sizeof(struct vchiq_slot_zero)); + + slot_zero->magic = VCHIQ_MAGIC; + slot_zero->version = VCHIQ_VERSION; + slot_zero->version_min = VCHIQ_VERSION_MIN; + slot_zero->slot_zero_size = sizeof(struct vchiq_slot_zero); + slot_zero->slot_size = VCHIQ_SLOT_SIZE; + slot_zero->max_slots = VCHIQ_MAX_SLOTS; + slot_zero->max_slots_per_side = VCHIQ_MAX_SLOTS_PER_SIDE; + + slot_zero->master.slot_sync = first_data_slot; + slot_zero->master.slot_first = first_data_slot + 1; + slot_zero->master.slot_last = first_data_slot + (num_slots / 2) - 1; + slot_zero->slave.slot_sync = first_data_slot + (num_slots / 2); + slot_zero->slave.slot_first = first_data_slot + (num_slots / 2) + 1; + slot_zero->slave.slot_last = first_data_slot + num_slots - 1; + + return slot_zero; +} + +int +vchiq_init_state(struct vchiq_state *state, struct vchiq_slot_zero *slot_zero, struct device *dev) +{ + struct vchiq_shared_state *local; + struct vchiq_shared_state *remote; + char threadname[16]; + int i, ret; + + local = &slot_zero->slave; + remote = &slot_zero->master; + + if (local->initialised) { + vchiq_loud_error_header(); + if (remote->initialised) + vchiq_loud_error("local state has already been initialised"); + else + vchiq_loud_error("master/slave mismatch two slaves"); + vchiq_loud_error_footer(); + return -EINVAL; + } + + memset(state, 0, sizeof(struct vchiq_state)); + + state->dev = dev; + + /* + * initialize shared state pointers + */ + + state->local = local; + state->remote = remote; + state->slot_data = (struct vchiq_slot *)slot_zero; + + /* + * initialize events and mutexes + */ + + init_completion(&state->connect); + mutex_init(&state->mutex); + mutex_init(&state->slot_mutex); + mutex_init(&state->recycle_mutex); + mutex_init(&state->sync_mutex); + mutex_init(&state->bulk_transfer_mutex); + + init_completion(&state->slot_available_event); + init_completion(&state->slot_remove_event); + init_completion(&state->data_quota_event); + + state->slot_queue_available = 0; + + for (i = 0; i < VCHIQ_MAX_SERVICES; i++) { + struct vchiq_service_quota *quota = &state->service_quotas[i]; + init_completion("a->quota_event); + } + + for (i = local->slot_first; i <= local->slot_last; i++) { + local->slot_queue[state->slot_queue_available] = i; + state->slot_queue_available++; + complete(&state->slot_available_event); + } + + state->default_slot_quota = state->slot_queue_available / 2; + state->default_message_quota = + min_t(unsigned short, state->default_slot_quota * 256, ~0); + + state->previous_data_index = -1; + state->data_use_count = 0; + state->data_quota = state->slot_queue_available - 1; + + remote_event_create(&state->trigger_event, &local->trigger); + local->tx_pos = 0; + remote_event_create(&state->recycle_event, &local->recycle); + local->slot_queue_recycle = state->slot_queue_available; + remote_event_create(&state->sync_trigger_event, &local->sync_trigger); + remote_event_create(&state->sync_release_event, &local->sync_release); + + /* At start-of-day, the slot is empty and available */ + ((struct vchiq_header *) + SLOT_DATA_FROM_INDEX(state, local->slot_sync))->msgid = + VCHIQ_MSGID_PADDING; + remote_event_signal_local(&state->sync_release_event, &local->sync_release); + + local->debug[DEBUG_ENTRIES] = DEBUG_MAX; + + ret = vchiq_platform_init_state(state); + if (ret) + return ret; + + /* + * bring up slot handler thread + */ + snprintf(threadname, sizeof(threadname), "vchiq-slot/%d", state->id); + state->slot_handler_thread = kthread_create(&slot_handler_func, (void *)state, threadname); + + if (IS_ERR(state->slot_handler_thread)) { + vchiq_loud_error_header(); + vchiq_loud_error("couldn't create thread %s", threadname); + vchiq_loud_error_footer(); + return PTR_ERR(state->slot_handler_thread); + } + set_user_nice(state->slot_handler_thread, -19); + + snprintf(threadname, sizeof(threadname), "vchiq-recy/%d", state->id); + state->recycle_thread = kthread_create(&recycle_func, (void *)state, threadname); + if (IS_ERR(state->recycle_thread)) { + vchiq_loud_error_header(); + vchiq_loud_error("couldn't create thread %s", threadname); + vchiq_loud_error_footer(); + ret = PTR_ERR(state->recycle_thread); + goto fail_free_handler_thread; + } + set_user_nice(state->recycle_thread, -19); + + snprintf(threadname, sizeof(threadname), "vchiq-sync/%d", state->id); + state->sync_thread = kthread_create(&sync_func, (void *)state, threadname); + if (IS_ERR(state->sync_thread)) { + vchiq_loud_error_header(); + vchiq_loud_error("couldn't create thread %s", threadname); + vchiq_loud_error_footer(); + ret = PTR_ERR(state->sync_thread); + goto fail_free_recycle_thread; + } + set_user_nice(state->sync_thread, -20); + + wake_up_process(state->slot_handler_thread); + wake_up_process(state->recycle_thread); + wake_up_process(state->sync_thread); + + /* Indicate readiness to the other side */ + local->initialised = 1; + + return 0; + +fail_free_recycle_thread: + kthread_stop(state->recycle_thread); +fail_free_handler_thread: + kthread_stop(state->slot_handler_thread); + + return ret; +} + +void vchiq_msg_queue_push(struct vchiq_instance *instance, unsigned int handle, + struct vchiq_header *header) +{ + struct vchiq_service *service = find_service_by_handle(instance, handle); + int pos; + + if (!service) + return; + + while (service->msg_queue_write == service->msg_queue_read + + VCHIQ_MAX_SLOTS) { + if (wait_for_completion_interruptible(&service->msg_queue_pop)) + flush_signals(current); + } + + pos = service->msg_queue_write & (VCHIQ_MAX_SLOTS - 1); + service->msg_queue_write++; + service->msg_queue[pos] = header; + + complete(&service->msg_queue_push); +} +EXPORT_SYMBOL(vchiq_msg_queue_push); + +struct vchiq_header *vchiq_msg_hold(struct vchiq_instance *instance, unsigned int handle) +{ + struct vchiq_service *service = find_service_by_handle(instance, handle); + struct vchiq_header *header; + int pos; + + if (!service) + return NULL; + + if (service->msg_queue_write == service->msg_queue_read) + return NULL; + + while (service->msg_queue_write == service->msg_queue_read) { + if (wait_for_completion_interruptible(&service->msg_queue_push)) + flush_signals(current); + } + + pos = service->msg_queue_read & (VCHIQ_MAX_SLOTS - 1); + service->msg_queue_read++; + header = service->msg_queue[pos]; + + complete(&service->msg_queue_pop); + + return header; +} +EXPORT_SYMBOL(vchiq_msg_hold); + +static int vchiq_validate_params(const struct vchiq_service_params_kernel *params) +{ + if (!params->callback || !params->fourcc) { + vchiq_loud_error("Can't add service, invalid params\n"); + return -EINVAL; + } + + return 0; +} + +/* Called from application thread when a client or server service is created. */ +struct vchiq_service * +vchiq_add_service_internal(struct vchiq_state *state, + const struct vchiq_service_params_kernel *params, + int srvstate, struct vchiq_instance *instance, + void (*userdata_term)(void *userdata)) +{ + struct vchiq_service *service; + struct vchiq_service __rcu **pservice = NULL; + struct vchiq_service_quota *quota; + int ret; + int i; + + ret = vchiq_validate_params(params); + if (ret) + return NULL; + + service = kzalloc(sizeof(*service), GFP_KERNEL); + if (!service) + return service; + + service->base.fourcc = params->fourcc; + service->base.callback = params->callback; + service->base.userdata = params->userdata; + service->handle = VCHIQ_SERVICE_HANDLE_INVALID; + kref_init(&service->ref_count); + service->srvstate = VCHIQ_SRVSTATE_FREE; + service->userdata_term = userdata_term; + service->localport = VCHIQ_PORT_FREE; + service->remoteport = VCHIQ_PORT_FREE; + + service->public_fourcc = (srvstate == VCHIQ_SRVSTATE_OPENING) ? + VCHIQ_FOURCC_INVALID : params->fourcc; + service->auto_close = 1; + atomic_set(&service->poll_flags, 0); + service->version = params->version; + service->version_min = params->version_min; + service->state = state; + service->instance = instance; + init_completion(&service->remove_event); + init_completion(&service->bulk_remove_event); + init_completion(&service->msg_queue_pop); + init_completion(&service->msg_queue_push); + mutex_init(&service->bulk_mutex); + + /* + * Although it is perfectly possible to use a spinlock + * to protect the creation of services, it is overkill as it + * disables interrupts while the array is searched. + * The only danger is of another thread trying to create a + * service - service deletion is safe. + * Therefore it is preferable to use state->mutex which, + * although slower to claim, doesn't block interrupts while + * it is held. + */ + + mutex_lock(&state->mutex); + + /* Prepare to use a previously unused service */ + if (state->unused_service < VCHIQ_MAX_SERVICES) + pservice = &state->services[state->unused_service]; + + if (srvstate == VCHIQ_SRVSTATE_OPENING) { + for (i = 0; i < state->unused_service; i++) { + if (!rcu_access_pointer(state->services[i])) { + pservice = &state->services[i]; + break; + } + } + } else { + rcu_read_lock(); + for (i = (state->unused_service - 1); i >= 0; i--) { + struct vchiq_service *srv; + + srv = rcu_dereference(state->services[i]); + if (!srv) { + pservice = &state->services[i]; + } else if ((srv->public_fourcc == params->fourcc) && + ((srv->instance != instance) || + (srv->base.callback != params->callback))) { + /* + * There is another server using this + * fourcc which doesn't match. + */ + pservice = NULL; + break; + } + } + rcu_read_unlock(); + } + + if (pservice) { + service->localport = (pservice - state->services); + if (!handle_seq) + handle_seq = VCHIQ_MAX_STATES * + VCHIQ_MAX_SERVICES; + service->handle = handle_seq | + (state->id * VCHIQ_MAX_SERVICES) | + service->localport; + handle_seq += VCHIQ_MAX_STATES * VCHIQ_MAX_SERVICES; + rcu_assign_pointer(*pservice, service); + if (pservice == &state->services[state->unused_service]) + state->unused_service++; + } + + mutex_unlock(&state->mutex); + + if (!pservice) { + kfree(service); + return NULL; + } + + quota = &state->service_quotas[service->localport]; + quota->slot_quota = state->default_slot_quota; + quota->message_quota = state->default_message_quota; + if (quota->slot_use_count == 0) + quota->previous_tx_index = + SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos) + - 1; + + /* Bring this service online */ + set_service_state(service, srvstate); + + vchiq_log_info(vchiq_core_msg_log_level, "%s Service %c%c%c%c SrcPort:%d", + (srvstate == VCHIQ_SRVSTATE_OPENING) ? "Open" : "Add", + VCHIQ_FOURCC_AS_4CHARS(params->fourcc), service->localport); + + /* Don't unlock the service - leave it with a ref_count of 1. */ + + return service; +} + +enum vchiq_status +vchiq_open_service_internal(struct vchiq_service *service, int client_id) +{ + struct vchiq_open_payload payload = { + service->base.fourcc, + client_id, + service->version, + service->version_min + }; + enum vchiq_status status = VCHIQ_SUCCESS; + + service->client_id = client_id; + vchiq_use_service_internal(service); + status = queue_message(service->state, + NULL, MAKE_OPEN(service->localport), + memcpy_copy_callback, + &payload, + sizeof(payload), + QMFLAGS_IS_BLOCKING); + + if (status != VCHIQ_SUCCESS) + return status; + + /* Wait for the ACK/NAK */ + if (wait_for_completion_interruptible(&service->remove_event)) { + status = VCHIQ_RETRY; + vchiq_release_service_internal(service); + } else if ((service->srvstate != VCHIQ_SRVSTATE_OPEN) && + (service->srvstate != VCHIQ_SRVSTATE_OPENSYNC)) { + if (service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT) + vchiq_log_error(vchiq_core_log_level, + "%d: osi - srvstate = %s (ref %u)", + service->state->id, + srvstate_names[service->srvstate], + kref_read(&service->ref_count)); + status = VCHIQ_ERROR; + VCHIQ_SERVICE_STATS_INC(service, error_count); + vchiq_release_service_internal(service); + } + + return status; +} + +static void +release_service_messages(struct vchiq_service *service) +{ + struct vchiq_state *state = service->state; + int slot_last = state->remote->slot_last; + int i; + + /* Release any claimed messages aimed at this service */ + + if (service->sync) { + struct vchiq_header *header = + (struct vchiq_header *)SLOT_DATA_FROM_INDEX(state, + state->remote->slot_sync); + if (VCHIQ_MSG_DSTPORT(header->msgid) == service->localport) + release_message_sync(state, header); + + return; + } + + for (i = state->remote->slot_first; i <= slot_last; i++) { + struct vchiq_slot_info *slot_info = + SLOT_INFO_FROM_INDEX(state, i); + unsigned int pos, end; + char *data; + + if (slot_info->release_count == slot_info->use_count) + continue; + + data = (char *)SLOT_DATA_FROM_INDEX(state, i); + end = VCHIQ_SLOT_SIZE; + if (data == state->rx_data) + /* + * This buffer is still being read from - stop + * at the current read position + */ + end = state->rx_pos & VCHIQ_SLOT_MASK; + + pos = 0; + + while (pos < end) { + struct vchiq_header *header = + (struct vchiq_header *)(data + pos); + int msgid = header->msgid; + int port = VCHIQ_MSG_DSTPORT(msgid); + + if ((port == service->localport) && (msgid & VCHIQ_MSGID_CLAIMED)) { + vchiq_log_info(vchiq_core_log_level, " fsi - hdr %pK", header); + release_slot(state, slot_info, header, NULL); + } + pos += calc_stride(header->size); + if (pos > VCHIQ_SLOT_SIZE) { + vchiq_log_error(vchiq_core_log_level, + "fsi - pos %x: header %pK, msgid %x, header->msgid %x, header->size %x", + pos, header, msgid, header->msgid, header->size); + WARN(1, "invalid slot position\n"); + } + } + } +} + +static int +do_abort_bulks(struct vchiq_service *service) +{ + enum vchiq_status status; + + /* Abort any outstanding bulk transfers */ + if (mutex_lock_killable(&service->bulk_mutex)) + return 0; + abort_outstanding_bulks(service, &service->bulk_tx); + abort_outstanding_bulks(service, &service->bulk_rx); + mutex_unlock(&service->bulk_mutex); + + status = notify_bulks(service, &service->bulk_tx, NO_RETRY_POLL); + if (status != VCHIQ_SUCCESS) + return 0; + + status = notify_bulks(service, &service->bulk_rx, NO_RETRY_POLL); + return (status == VCHIQ_SUCCESS); +} + +static enum vchiq_status +close_service_complete(struct vchiq_service *service, int failstate) +{ + enum vchiq_status status; + int is_server = (service->public_fourcc != VCHIQ_FOURCC_INVALID); + int newstate; + + switch (service->srvstate) { + case VCHIQ_SRVSTATE_OPEN: + case VCHIQ_SRVSTATE_CLOSESENT: + case VCHIQ_SRVSTATE_CLOSERECVD: + if (is_server) { + if (service->auto_close) { + service->client_id = 0; + service->remoteport = VCHIQ_PORT_FREE; + newstate = VCHIQ_SRVSTATE_LISTENING; + } else { + newstate = VCHIQ_SRVSTATE_CLOSEWAIT; + } + } else { + newstate = VCHIQ_SRVSTATE_CLOSED; + } + set_service_state(service, newstate); + break; + case VCHIQ_SRVSTATE_LISTENING: + break; + default: + vchiq_log_error(vchiq_core_log_level, "%s(%x) called in state %s", __func__, + service->handle, srvstate_names[service->srvstate]); + WARN(1, "%s in unexpected state\n", __func__); + return VCHIQ_ERROR; + } + + status = make_service_callback(service, VCHIQ_SERVICE_CLOSED, NULL, NULL); + + if (status != VCHIQ_RETRY) { + int uc = service->service_use_count; + int i; + /* Complete the close process */ + for (i = 0; i < uc; i++) + /* + * cater for cases where close is forced and the + * client may not close all it's handles + */ + vchiq_release_service_internal(service); + + service->client_id = 0; + service->remoteport = VCHIQ_PORT_FREE; + + if (service->srvstate == VCHIQ_SRVSTATE_CLOSED) { + vchiq_free_service_internal(service); + } else if (service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT) { + if (is_server) + service->closing = 0; + + complete(&service->remove_event); + } + } else { + set_service_state(service, failstate); + } + + return status; +} + +/* Called by the slot handler */ +enum vchiq_status +vchiq_close_service_internal(struct vchiq_service *service, int close_recvd) +{ + struct vchiq_state *state = service->state; + enum vchiq_status status = VCHIQ_SUCCESS; + int is_server = (service->public_fourcc != VCHIQ_FOURCC_INVALID); + int close_id = MAKE_CLOSE(service->localport, + VCHIQ_MSG_DSTPORT(service->remoteport)); + + vchiq_log_info(vchiq_core_log_level, "%d: csi:%d,%d (%s)", service->state->id, + service->localport, close_recvd, srvstate_names[service->srvstate]); + + switch (service->srvstate) { + case VCHIQ_SRVSTATE_CLOSED: + case VCHIQ_SRVSTATE_HIDDEN: + case VCHIQ_SRVSTATE_LISTENING: + case VCHIQ_SRVSTATE_CLOSEWAIT: + if (close_recvd) { + vchiq_log_error(vchiq_core_log_level, "%s(1) called in state %s", + __func__, srvstate_names[service->srvstate]); + } else if (is_server) { + if (service->srvstate == VCHIQ_SRVSTATE_LISTENING) { + status = VCHIQ_ERROR; + } else { + service->client_id = 0; + service->remoteport = VCHIQ_PORT_FREE; + if (service->srvstate == VCHIQ_SRVSTATE_CLOSEWAIT) + set_service_state(service, VCHIQ_SRVSTATE_LISTENING); + } + complete(&service->remove_event); + } else { + vchiq_free_service_internal(service); + } + break; + case VCHIQ_SRVSTATE_OPENING: + if (close_recvd) { + /* The open was rejected - tell the user */ + set_service_state(service, VCHIQ_SRVSTATE_CLOSEWAIT); + complete(&service->remove_event); + } else { + /* Shutdown mid-open - let the other side know */ + status = queue_message(state, service, close_id, NULL, NULL, 0, 0); + } + break; + + case VCHIQ_SRVSTATE_OPENSYNC: + mutex_lock(&state->sync_mutex); + fallthrough; + case VCHIQ_SRVSTATE_OPEN: + if (close_recvd) { + if (!do_abort_bulks(service)) + status = VCHIQ_RETRY; + } + + release_service_messages(service); + + if (status == VCHIQ_SUCCESS) + status = queue_message(state, service, close_id, NULL, + NULL, 0, QMFLAGS_NO_MUTEX_UNLOCK); + + if (status != VCHIQ_SUCCESS) { + if (service->srvstate == VCHIQ_SRVSTATE_OPENSYNC) + mutex_unlock(&state->sync_mutex); + break; + } + + if (!close_recvd) { + /* Change the state while the mutex is still held */ + set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT); + mutex_unlock(&state->slot_mutex); + if (service->sync) + mutex_unlock(&state->sync_mutex); + break; + } + + /* Change the state while the mutex is still held */ + set_service_state(service, VCHIQ_SRVSTATE_CLOSERECVD); + mutex_unlock(&state->slot_mutex); + if (service->sync) + mutex_unlock(&state->sync_mutex); + + status = close_service_complete(service, VCHIQ_SRVSTATE_CLOSERECVD); + break; + + case VCHIQ_SRVSTATE_CLOSESENT: + if (!close_recvd) + /* This happens when a process is killed mid-close */ + break; + + if (!do_abort_bulks(service)) { + status = VCHIQ_RETRY; + break; + } + + if (status == VCHIQ_SUCCESS) + status = close_service_complete(service, VCHIQ_SRVSTATE_CLOSERECVD); + break; + + case VCHIQ_SRVSTATE_CLOSERECVD: + if (!close_recvd && is_server) + /* Force into LISTENING mode */ + set_service_state(service, VCHIQ_SRVSTATE_LISTENING); + status = close_service_complete(service, VCHIQ_SRVSTATE_CLOSERECVD); + break; + + default: + vchiq_log_error(vchiq_core_log_level, "%s(%d) called in state %s", __func__, + close_recvd, srvstate_names[service->srvstate]); + break; + } + + return status; +} + +/* Called from the application process upon process death */ +void +vchiq_terminate_service_internal(struct vchiq_service *service) +{ + struct vchiq_state *state = service->state; + + vchiq_log_info(vchiq_core_log_level, "%d: tsi - (%d<->%d)", state->id, + service->localport, service->remoteport); + + mark_service_closing(service); + + /* Mark the service for removal by the slot handler */ + request_poll(state, service, VCHIQ_POLL_REMOVE); +} + +/* Called from the slot handler */ +void +vchiq_free_service_internal(struct vchiq_service *service) +{ + struct vchiq_state *state = service->state; + + vchiq_log_info(vchiq_core_log_level, "%d: fsi - (%d)", state->id, service->localport); + + switch (service->srvstate) { + case VCHIQ_SRVSTATE_OPENING: + case VCHIQ_SRVSTATE_CLOSED: + case VCHIQ_SRVSTATE_HIDDEN: + case VCHIQ_SRVSTATE_LISTENING: + case VCHIQ_SRVSTATE_CLOSEWAIT: + break; + default: + vchiq_log_error(vchiq_core_log_level, "%d: fsi - (%d) in state %s", state->id, + service->localport, srvstate_names[service->srvstate]); + return; + } + + set_service_state(service, VCHIQ_SRVSTATE_FREE); + + complete(&service->remove_event); + + /* Release the initial lock */ + vchiq_service_put(service); +} + +enum vchiq_status +vchiq_connect_internal(struct vchiq_state *state, struct vchiq_instance *instance) +{ + struct vchiq_service *service; + int i; + + /* Find all services registered to this client and enable them. */ + i = 0; + while ((service = next_service_by_instance(state, instance, &i)) != NULL) { + if (service->srvstate == VCHIQ_SRVSTATE_HIDDEN) + set_service_state(service, VCHIQ_SRVSTATE_LISTENING); + vchiq_service_put(service); + } + + if (state->conn_state == VCHIQ_CONNSTATE_DISCONNECTED) { + if (queue_message(state, NULL, MAKE_CONNECT, NULL, NULL, 0, + QMFLAGS_IS_BLOCKING) == VCHIQ_RETRY) + return VCHIQ_RETRY; + + vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTING); + } + + if (state->conn_state == VCHIQ_CONNSTATE_CONNECTING) { + if (wait_for_completion_interruptible(&state->connect)) + return VCHIQ_RETRY; + + vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED); + complete(&state->connect); + } + + return VCHIQ_SUCCESS; +} + +void +vchiq_shutdown_internal(struct vchiq_state *state, struct vchiq_instance *instance) +{ + struct vchiq_service *service; + int i; + + /* Find all services registered to this client and remove them. */ + i = 0; + while ((service = next_service_by_instance(state, instance, &i)) != NULL) { + (void)vchiq_remove_service(instance, service->handle); + vchiq_service_put(service); + } +} + +enum vchiq_status +vchiq_close_service(struct vchiq_instance *instance, unsigned int handle) +{ + /* Unregister the service */ + struct vchiq_service *service = find_service_by_handle(instance, handle); + enum vchiq_status status = VCHIQ_SUCCESS; + + if (!service) + return VCHIQ_ERROR; + + vchiq_log_info(vchiq_core_log_level, "%d: close_service:%d", + service->state->id, service->localport); + + if ((service->srvstate == VCHIQ_SRVSTATE_FREE) || + (service->srvstate == VCHIQ_SRVSTATE_LISTENING) || + (service->srvstate == VCHIQ_SRVSTATE_HIDDEN)) { + vchiq_service_put(service); + return VCHIQ_ERROR; + } + + mark_service_closing(service); + + if (current == service->state->slot_handler_thread) { + status = vchiq_close_service_internal(service, NO_CLOSE_RECVD); + WARN_ON(status == VCHIQ_RETRY); + } else { + /* Mark the service for termination by the slot handler */ + request_poll(service->state, service, VCHIQ_POLL_TERMINATE); + } + + while (1) { + if (wait_for_completion_interruptible(&service->remove_event)) { + status = VCHIQ_RETRY; + break; + } + + if ((service->srvstate == VCHIQ_SRVSTATE_FREE) || + (service->srvstate == VCHIQ_SRVSTATE_LISTENING) || + (service->srvstate == VCHIQ_SRVSTATE_OPEN)) + break; + + vchiq_log_warning(vchiq_core_log_level, + "%d: close_service:%d - waiting in state %s", + service->state->id, service->localport, + srvstate_names[service->srvstate]); + } + + if ((status == VCHIQ_SUCCESS) && + (service->srvstate != VCHIQ_SRVSTATE_FREE) && + (service->srvstate != VCHIQ_SRVSTATE_LISTENING)) + status = VCHIQ_ERROR; + + vchiq_service_put(service); + + return status; +} +EXPORT_SYMBOL(vchiq_close_service); + +enum vchiq_status +vchiq_remove_service(struct vchiq_instance *instance, unsigned int handle) +{ + /* Unregister the service */ + struct vchiq_service *service = find_service_by_handle(instance, handle); + enum vchiq_status status = VCHIQ_SUCCESS; + + if (!service) + return VCHIQ_ERROR; + + vchiq_log_info(vchiq_core_log_level, "%d: remove_service:%d", + service->state->id, service->localport); + + if (service->srvstate == VCHIQ_SRVSTATE_FREE) { + vchiq_service_put(service); + return VCHIQ_ERROR; + } + + mark_service_closing(service); + + if ((service->srvstate == VCHIQ_SRVSTATE_HIDDEN) || + (current == service->state->slot_handler_thread)) { + /* + * Make it look like a client, because it must be removed and + * not left in the LISTENING state. + */ + service->public_fourcc = VCHIQ_FOURCC_INVALID; + + status = vchiq_close_service_internal(service, NO_CLOSE_RECVD); + WARN_ON(status == VCHIQ_RETRY); + } else { + /* Mark the service for removal by the slot handler */ + request_poll(service->state, service, VCHIQ_POLL_REMOVE); + } + while (1) { + if (wait_for_completion_interruptible(&service->remove_event)) { + status = VCHIQ_RETRY; + break; + } + + if ((service->srvstate == VCHIQ_SRVSTATE_FREE) || + (service->srvstate == VCHIQ_SRVSTATE_OPEN)) + break; + + vchiq_log_warning(vchiq_core_log_level, + "%d: remove_service:%d - waiting in state %s", + service->state->id, service->localport, + srvstate_names[service->srvstate]); + } + + if ((status == VCHIQ_SUCCESS) && + (service->srvstate != VCHIQ_SRVSTATE_FREE)) + status = VCHIQ_ERROR; + + vchiq_service_put(service); + + return status; +} + +/* + * This function may be called by kernel threads or user threads. + * User threads may receive VCHIQ_RETRY to indicate that a signal has been + * received and the call should be retried after being returned to user + * context. + * When called in blocking mode, the userdata field points to a bulk_waiter + * structure. + */ +enum vchiq_status vchiq_bulk_transfer(struct vchiq_instance *instance, unsigned int handle, + void *offset, void __user *uoffset, int size, void *userdata, + enum vchiq_bulk_mode mode, enum vchiq_bulk_dir dir) +{ + struct vchiq_service *service = find_service_by_handle(instance, handle); + struct vchiq_bulk_queue *queue; + struct vchiq_bulk *bulk; + struct vchiq_state *state; + struct bulk_waiter *bulk_waiter = NULL; + const char dir_char = (dir == VCHIQ_BULK_TRANSMIT) ? 't' : 'r'; + const int dir_msgtype = (dir == VCHIQ_BULK_TRANSMIT) ? + VCHIQ_MSG_BULK_TX : VCHIQ_MSG_BULK_RX; + enum vchiq_status status = VCHIQ_ERROR; + int payload[2]; + + if (!service) + goto error_exit; + + if (service->srvstate != VCHIQ_SRVSTATE_OPEN) + goto error_exit; + + if (!offset && !uoffset) + goto error_exit; + + if (vchiq_check_service(service) != VCHIQ_SUCCESS) + goto error_exit; + + switch (mode) { + case VCHIQ_BULK_MODE_NOCALLBACK: + case VCHIQ_BULK_MODE_CALLBACK: + break; + case VCHIQ_BULK_MODE_BLOCKING: + bulk_waiter = userdata; + init_completion(&bulk_waiter->event); + bulk_waiter->actual = 0; + bulk_waiter->bulk = NULL; + break; + case VCHIQ_BULK_MODE_WAITING: + bulk_waiter = userdata; + bulk = bulk_waiter->bulk; + goto waiting; + default: + goto error_exit; + } + + state = service->state; + + queue = (dir == VCHIQ_BULK_TRANSMIT) ? + &service->bulk_tx : &service->bulk_rx; + + if (mutex_lock_killable(&service->bulk_mutex)) { + status = VCHIQ_RETRY; + goto error_exit; + } + + if (queue->local_insert == queue->remove + VCHIQ_NUM_SERVICE_BULKS) { + VCHIQ_SERVICE_STATS_INC(service, bulk_stalls); + do { + mutex_unlock(&service->bulk_mutex); + if (wait_for_completion_interruptible(&service->bulk_remove_event)) { + status = VCHIQ_RETRY; + goto error_exit; + } + if (mutex_lock_killable(&service->bulk_mutex)) { + status = VCHIQ_RETRY; + goto error_exit; + } + } while (queue->local_insert == queue->remove + + VCHIQ_NUM_SERVICE_BULKS); + } + + bulk = &queue->bulks[BULK_INDEX(queue->local_insert)]; + + bulk->mode = mode; + bulk->dir = dir; + bulk->userdata = userdata; + bulk->size = size; + bulk->actual = VCHIQ_BULK_ACTUAL_ABORTED; + + if (vchiq_prepare_bulk_data(instance, bulk, offset, uoffset, size, dir)) + goto unlock_error_exit; + + /* + * Ensure that the bulk data record is visible to the peer + * before proceeding. + */ + wmb(); + + vchiq_log_info(vchiq_core_log_level, "%d: bt (%d->%d) %cx %x@%pad %pK", + state->id, service->localport, service->remoteport, + dir_char, size, &bulk->data, userdata); + + /* + * The slot mutex must be held when the service is being closed, so + * claim it here to ensure that isn't happening + */ + if (mutex_lock_killable(&state->slot_mutex)) { + status = VCHIQ_RETRY; + goto cancel_bulk_error_exit; + } + + if (service->srvstate != VCHIQ_SRVSTATE_OPEN) + goto unlock_both_error_exit; + + payload[0] = lower_32_bits(bulk->data); + payload[1] = bulk->size; + status = queue_message(state, + NULL, + VCHIQ_MAKE_MSG(dir_msgtype, + service->localport, + service->remoteport), + memcpy_copy_callback, + &payload, + sizeof(payload), + QMFLAGS_IS_BLOCKING | + QMFLAGS_NO_MUTEX_LOCK | + QMFLAGS_NO_MUTEX_UNLOCK); + if (status != VCHIQ_SUCCESS) + goto unlock_both_error_exit; + + queue->local_insert++; + + mutex_unlock(&state->slot_mutex); + mutex_unlock(&service->bulk_mutex); + + vchiq_log_trace(vchiq_core_log_level, "%d: bt:%d %cx li=%x ri=%x p=%x", + state->id, service->localport, dir_char, queue->local_insert, + queue->remote_insert, queue->process); + +waiting: + vchiq_service_put(service); + + status = VCHIQ_SUCCESS; + + if (bulk_waiter) { + bulk_waiter->bulk = bulk; + if (wait_for_completion_interruptible(&bulk_waiter->event)) + status = VCHIQ_RETRY; + else if (bulk_waiter->actual == VCHIQ_BULK_ACTUAL_ABORTED) + status = VCHIQ_ERROR; + } + + return status; + +unlock_both_error_exit: + mutex_unlock(&state->slot_mutex); +cancel_bulk_error_exit: + vchiq_complete_bulk(service->instance, bulk); +unlock_error_exit: + mutex_unlock(&service->bulk_mutex); + +error_exit: + if (service) + vchiq_service_put(service); + return status; +} + +enum vchiq_status +vchiq_queue_message(struct vchiq_instance *instance, unsigned int handle, + ssize_t (*copy_callback)(void *context, void *dest, + size_t offset, size_t maxsize), + void *context, + size_t size) +{ + struct vchiq_service *service = find_service_by_handle(instance, handle); + enum vchiq_status status = VCHIQ_ERROR; + int data_id; + + if (!service) + goto error_exit; + + if (vchiq_check_service(service) != VCHIQ_SUCCESS) + goto error_exit; + + if (!size) { + VCHIQ_SERVICE_STATS_INC(service, error_count); + goto error_exit; + } + + if (size > VCHIQ_MAX_MSG_SIZE) { + VCHIQ_SERVICE_STATS_INC(service, error_count); + goto error_exit; + } + + data_id = MAKE_DATA(service->localport, service->remoteport); + + switch (service->srvstate) { + case VCHIQ_SRVSTATE_OPEN: + status = queue_message(service->state, service, data_id, + copy_callback, context, size, 1); + break; + case VCHIQ_SRVSTATE_OPENSYNC: + status = queue_message_sync(service->state, service, data_id, + copy_callback, context, size, 1); + break; + default: + status = VCHIQ_ERROR; + break; + } + +error_exit: + if (service) + vchiq_service_put(service); + + return status; +} + +int vchiq_queue_kernel_message(struct vchiq_instance *instance, unsigned int handle, void *data, + unsigned int size) +{ + enum vchiq_status status; + + while (1) { + status = vchiq_queue_message(instance, handle, memcpy_copy_callback, + data, size); + + /* + * vchiq_queue_message() may return VCHIQ_RETRY, so we need to + * implement a retry mechanism since this function is supposed + * to block until queued + */ + if (status != VCHIQ_RETRY) + break; + + msleep(1); + } + + return status; +} +EXPORT_SYMBOL(vchiq_queue_kernel_message); + +void +vchiq_release_message(struct vchiq_instance *instance, unsigned int handle, + struct vchiq_header *header) +{ + struct vchiq_service *service = find_service_by_handle(instance, handle); + struct vchiq_shared_state *remote; + struct vchiq_state *state; + int slot_index; + + if (!service) + return; + + state = service->state; + remote = state->remote; + + slot_index = SLOT_INDEX_FROM_DATA(state, (void *)header); + + if ((slot_index >= remote->slot_first) && + (slot_index <= remote->slot_last)) { + int msgid = header->msgid; + + if (msgid & VCHIQ_MSGID_CLAIMED) { + struct vchiq_slot_info *slot_info = + SLOT_INFO_FROM_INDEX(state, slot_index); + + release_slot(state, slot_info, header, service); + } + } else if (slot_index == remote->slot_sync) { + release_message_sync(state, header); + } + + vchiq_service_put(service); +} +EXPORT_SYMBOL(vchiq_release_message); + +static void +release_message_sync(struct vchiq_state *state, struct vchiq_header *header) +{ + header->msgid = VCHIQ_MSGID_PADDING; + remote_event_signal(&state->remote->sync_release); +} + +enum vchiq_status +vchiq_get_peer_version(struct vchiq_instance *instance, unsigned int handle, short *peer_version) +{ + enum vchiq_status status = VCHIQ_ERROR; + struct vchiq_service *service = find_service_by_handle(instance, handle); + + if (!service) + goto exit; + + if (vchiq_check_service(service) != VCHIQ_SUCCESS) + goto exit; + + if (!peer_version) + goto exit; + + *peer_version = service->peer_version; + status = VCHIQ_SUCCESS; + +exit: + if (service) + vchiq_service_put(service); + return status; +} +EXPORT_SYMBOL(vchiq_get_peer_version); + +void vchiq_get_config(struct vchiq_config *config) +{ + config->max_msg_size = VCHIQ_MAX_MSG_SIZE; + config->bulk_threshold = VCHIQ_MAX_MSG_SIZE; + config->max_outstanding_bulks = VCHIQ_NUM_SERVICE_BULKS; + config->max_services = VCHIQ_MAX_SERVICES; + config->version = VCHIQ_VERSION; + config->version_min = VCHIQ_VERSION_MIN; +} + +int +vchiq_set_service_option(struct vchiq_instance *instance, unsigned int handle, + enum vchiq_service_option option, int value) +{ + struct vchiq_service *service = find_service_by_handle(instance, handle); + struct vchiq_service_quota *quota; + int ret = -EINVAL; + + if (!service) + return -EINVAL; + + switch (option) { + case VCHIQ_SERVICE_OPTION_AUTOCLOSE: + service->auto_close = value; + ret = 0; + break; + + case VCHIQ_SERVICE_OPTION_SLOT_QUOTA: + quota = &service->state->service_quotas[service->localport]; + if (value == 0) + value = service->state->default_slot_quota; + if ((value >= quota->slot_use_count) && + (value < (unsigned short)~0)) { + quota->slot_quota = value; + if ((value >= quota->slot_use_count) && + (quota->message_quota >= quota->message_use_count)) + /* + * Signal the service that it may have + * dropped below its quota + */ + complete("a->quota_event); + ret = 0; + } + break; + + case VCHIQ_SERVICE_OPTION_MESSAGE_QUOTA: + quota = &service->state->service_quotas[service->localport]; + if (value == 0) + value = service->state->default_message_quota; + if ((value >= quota->message_use_count) && + (value < (unsigned short)~0)) { + quota->message_quota = value; + if ((value >= quota->message_use_count) && + (quota->slot_quota >= quota->slot_use_count)) + /* + * Signal the service that it may have + * dropped below its quota + */ + complete("a->quota_event); + ret = 0; + } + break; + + case VCHIQ_SERVICE_OPTION_SYNCHRONOUS: + if ((service->srvstate == VCHIQ_SRVSTATE_HIDDEN) || + (service->srvstate == VCHIQ_SRVSTATE_LISTENING)) { + service->sync = value; + ret = 0; + } + break; + + case VCHIQ_SERVICE_OPTION_TRACE: + service->trace = value; + ret = 0; + break; + + default: + break; + } + vchiq_service_put(service); + + return ret; +} + +static int +vchiq_dump_shared_state(void *dump_context, struct vchiq_state *state, + struct vchiq_shared_state *shared, const char *label) +{ + static const char *const debug_names[] = { + "<entries>", + "SLOT_HANDLER_COUNT", + "SLOT_HANDLER_LINE", + "PARSE_LINE", + "PARSE_HEADER", + "PARSE_MSGID", + "AWAIT_COMPLETION_LINE", + "DEQUEUE_MESSAGE_LINE", + "SERVICE_CALLBACK_LINE", + "MSG_QUEUE_FULL_COUNT", + "COMPLETION_QUEUE_FULL_COUNT" + }; + int i; + char buf[80]; + int len; + int err; + + len = scnprintf(buf, sizeof(buf), " %s: slots %d-%d tx_pos=%x recycle=%x", + label, shared->slot_first, shared->slot_last, + shared->tx_pos, shared->slot_queue_recycle); + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + + len = scnprintf(buf, sizeof(buf), " Slots claimed:"); + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + + for (i = shared->slot_first; i <= shared->slot_last; i++) { + struct vchiq_slot_info slot_info = + *SLOT_INFO_FROM_INDEX(state, i); + if (slot_info.use_count != slot_info.release_count) { + len = scnprintf(buf, sizeof(buf), " %d: %d/%d", i, slot_info.use_count, + slot_info.release_count); + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + } + } + + for (i = 1; i < shared->debug[DEBUG_ENTRIES]; i++) { + len = scnprintf(buf, sizeof(buf), " DEBUG: %s = %d(%x)", + debug_names[i], shared->debug[i], shared->debug[i]); + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + } + return 0; +} + +int vchiq_dump_state(void *dump_context, struct vchiq_state *state) +{ + char buf[80]; + int len; + int i; + int err; + + len = scnprintf(buf, sizeof(buf), "State %d: %s", state->id, + conn_state_names[state->conn_state]); + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + + len = scnprintf(buf, sizeof(buf), " tx_pos=%x(@%pK), rx_pos=%x(@%pK)", + state->local->tx_pos, + state->tx_data + (state->local_tx_pos & VCHIQ_SLOT_MASK), + state->rx_pos, + state->rx_data + (state->rx_pos & VCHIQ_SLOT_MASK)); + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + + len = scnprintf(buf, sizeof(buf), " Version: %d (min %d)", + VCHIQ_VERSION, VCHIQ_VERSION_MIN); + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + + if (VCHIQ_ENABLE_STATS) { + len = scnprintf(buf, sizeof(buf), + " Stats: ctrl_tx_count=%d, ctrl_rx_count=%d, error_count=%d", + state->stats.ctrl_tx_count, state->stats.ctrl_rx_count, + state->stats.error_count); + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + } + + len = scnprintf(buf, sizeof(buf), + " Slots: %d available (%d data), %d recyclable, %d stalls (%d data)", + ((state->slot_queue_available * VCHIQ_SLOT_SIZE) - + state->local_tx_pos) / VCHIQ_SLOT_SIZE, + state->data_quota - state->data_use_count, + state->local->slot_queue_recycle - state->slot_queue_available, + state->stats.slot_stalls, state->stats.data_stalls); + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + + err = vchiq_dump_platform_state(dump_context); + if (err) + return err; + + err = vchiq_dump_shared_state(dump_context, + state, + state->local, + "Local"); + if (err) + return err; + err = vchiq_dump_shared_state(dump_context, + state, + state->remote, + "Remote"); + if (err) + return err; + + err = vchiq_dump_platform_instances(dump_context); + if (err) + return err; + + for (i = 0; i < state->unused_service; i++) { + struct vchiq_service *service = find_service_by_port(state, i); + + if (service) { + err = vchiq_dump_service_state(dump_context, service); + vchiq_service_put(service); + if (err) + return err; + } + } + return 0; +} + +int vchiq_dump_service_state(void *dump_context, struct vchiq_service *service) +{ + char buf[80]; + int len; + int err; + unsigned int ref_count; + + /*Don't include the lock just taken*/ + ref_count = kref_read(&service->ref_count) - 1; + len = scnprintf(buf, sizeof(buf), "Service %u: %s (ref %u)", + service->localport, srvstate_names[service->srvstate], + ref_count); + + if (service->srvstate != VCHIQ_SRVSTATE_FREE) { + char remoteport[30]; + struct vchiq_service_quota *quota = + &service->state->service_quotas[service->localport]; + int fourcc = service->base.fourcc; + int tx_pending, rx_pending; + + if (service->remoteport != VCHIQ_PORT_FREE) { + int len2 = scnprintf(remoteport, sizeof(remoteport), + "%u", service->remoteport); + + if (service->public_fourcc != VCHIQ_FOURCC_INVALID) + scnprintf(remoteport + len2, sizeof(remoteport) - len2, + " (client %x)", service->client_id); + } else { + strscpy(remoteport, "n/a", sizeof(remoteport)); + } + + len += scnprintf(buf + len, sizeof(buf) - len, + " '%c%c%c%c' remote %s (msg use %d/%d, slot use %d/%d)", + VCHIQ_FOURCC_AS_4CHARS(fourcc), remoteport, + quota->message_use_count, quota->message_quota, + quota->slot_use_count, quota->slot_quota); + + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + + tx_pending = service->bulk_tx.local_insert - + service->bulk_tx.remote_insert; + + rx_pending = service->bulk_rx.local_insert - + service->bulk_rx.remote_insert; + + len = scnprintf(buf, sizeof(buf), + " Bulk: tx_pending=%d (size %d), rx_pending=%d (size %d)", + tx_pending, + tx_pending ? + service->bulk_tx.bulks[BULK_INDEX(service->bulk_tx.remove)].size : + 0, rx_pending, rx_pending ? + service->bulk_rx.bulks[BULK_INDEX(service->bulk_rx.remove)].size : + 0); + + if (VCHIQ_ENABLE_STATS) { + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + + len = scnprintf(buf, sizeof(buf), + " Ctrl: tx_count=%d, tx_bytes=%llu, rx_count=%d, rx_bytes=%llu", + service->stats.ctrl_tx_count, service->stats.ctrl_tx_bytes, + service->stats.ctrl_rx_count, service->stats.ctrl_rx_bytes); + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + + len = scnprintf(buf, sizeof(buf), + " Bulk: tx_count=%d, tx_bytes=%llu, rx_count=%d, rx_bytes=%llu", + service->stats.bulk_tx_count, service->stats.bulk_tx_bytes, + service->stats.bulk_rx_count, service->stats.bulk_rx_bytes); + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + + len = scnprintf(buf, sizeof(buf), + " %d quota stalls, %d slot stalls, %d bulk stalls, %d aborted, %d errors", + service->stats.quota_stalls, service->stats.slot_stalls, + service->stats.bulk_stalls, + service->stats.bulk_aborted_count, + service->stats.error_count); + } + } + + err = vchiq_dump(dump_context, buf, len + 1); + if (err) + return err; + + if (service->srvstate != VCHIQ_SRVSTATE_FREE) + err = vchiq_dump_platform_service_state(dump_context, service); + return err; +} + +void +vchiq_loud_error_header(void) +{ + vchiq_log_error(vchiq_core_log_level, + "============================================================================"); + vchiq_log_error(vchiq_core_log_level, + "============================================================================"); + vchiq_log_error(vchiq_core_log_level, "====="); +} + +void +vchiq_loud_error_footer(void) +{ + vchiq_log_error(vchiq_core_log_level, "====="); + vchiq_log_error(vchiq_core_log_level, + "============================================================================"); + vchiq_log_error(vchiq_core_log_level, + "============================================================================"); +} + +enum vchiq_status vchiq_send_remote_use(struct vchiq_state *state) +{ + if (state->conn_state == VCHIQ_CONNSTATE_DISCONNECTED) + return VCHIQ_RETRY; + + return queue_message(state, NULL, MAKE_REMOTE_USE, NULL, NULL, 0, 0); +} + +enum vchiq_status vchiq_send_remote_use_active(struct vchiq_state *state) +{ + if (state->conn_state == VCHIQ_CONNSTATE_DISCONNECTED) + return VCHIQ_RETRY; + + return queue_message(state, NULL, MAKE_REMOTE_USE_ACTIVE, + NULL, NULL, 0, 0); +} + +void vchiq_log_dump_mem(const char *label, u32 addr, const void *void_mem, size_t num_bytes) +{ + const u8 *mem = void_mem; + size_t offset; + char line_buf[100]; + char *s; + + while (num_bytes > 0) { + s = line_buf; + + for (offset = 0; offset < 16; offset++) { + if (offset < num_bytes) + s += scnprintf(s, 4, "%02x ", mem[offset]); + else + s += scnprintf(s, 4, " "); + } + + for (offset = 0; offset < 16; offset++) { + if (offset < num_bytes) { + u8 ch = mem[offset]; + + if ((ch < ' ') || (ch > '~')) + ch = '.'; + *s++ = (char)ch; + } + } + *s++ = '\0'; + + if (label && (*label != '\0')) + vchiq_log_trace(VCHIQ_LOG_TRACE, "%s: %08x: %s", label, addr, line_buf); + else + vchiq_log_trace(VCHIQ_LOG_TRACE, "%08x: %s", addr, line_buf); + + addr += 16; + mem += 16; + if (num_bytes > 16) + num_bytes -= 16; + else + num_bytes = 0; + } +} diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h new file mode 100644 index 000000000..8b4a38f5b --- /dev/null +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h @@ -0,0 +1,596 @@ +/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */ +/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */ + +#ifndef VCHIQ_CORE_H +#define VCHIQ_CORE_H + +#include <linux/mutex.h> +#include <linux/completion.h> +#include <linux/kthread.h> +#include <linux/kref.h> +#include <linux/rcupdate.h> +#include <linux/wait.h> +#include <linux/raspberrypi/vchiq.h> + +#include "vchiq_cfg.h" + +/* Do this so that we can test-build the code on non-rpi systems */ +#if IS_ENABLED(CONFIG_RASPBERRYPI_FIRMWARE) + +#else + +#ifndef dsb +#define dsb(a) +#endif + +#endif /* IS_ENABLED(CONFIG_RASPBERRYPI_FIRMWARE) */ + +#define VCHIQ_SERVICE_HANDLE_INVALID 0 + +#define VCHIQ_SLOT_SIZE 4096 +#define VCHIQ_MAX_MSG_SIZE (VCHIQ_SLOT_SIZE - sizeof(struct vchiq_header)) + +/* Run time control of log level, based on KERN_XXX level. */ +#define VCHIQ_LOG_DEFAULT 4 +#define VCHIQ_LOG_ERROR 3 +#define VCHIQ_LOG_WARNING 4 +#define VCHIQ_LOG_INFO 6 +#define VCHIQ_LOG_TRACE 7 + +#define VCHIQ_LOG_PREFIX KERN_INFO "vchiq: " + +#ifndef vchiq_log_error +#define vchiq_log_error(cat, fmt, ...) \ + do { if (cat >= VCHIQ_LOG_ERROR) \ + printk(VCHIQ_LOG_PREFIX fmt "\n", ##__VA_ARGS__); } while (0) +#endif +#ifndef vchiq_log_warning +#define vchiq_log_warning(cat, fmt, ...) \ + do { if (cat >= VCHIQ_LOG_WARNING) \ + printk(VCHIQ_LOG_PREFIX fmt "\n", ##__VA_ARGS__); } while (0) +#endif +#ifndef vchiq_log_info +#define vchiq_log_info(cat, fmt, ...) \ + do { if (cat >= VCHIQ_LOG_INFO) \ + printk(VCHIQ_LOG_PREFIX fmt "\n", ##__VA_ARGS__); } while (0) +#endif +#ifndef vchiq_log_trace +#define vchiq_log_trace(cat, fmt, ...) \ + do { if (cat >= VCHIQ_LOG_TRACE) \ + printk(VCHIQ_LOG_PREFIX fmt "\n", ##__VA_ARGS__); } while (0) +#endif + +#define vchiq_loud_error(...) \ + vchiq_log_error(vchiq_core_log_level, "===== " __VA_ARGS__) + +#define VCHIQ_SLOT_MASK (VCHIQ_SLOT_SIZE - 1) +#define VCHIQ_SLOT_QUEUE_MASK (VCHIQ_MAX_SLOTS_PER_SIDE - 1) +#define VCHIQ_SLOT_ZERO_SLOTS DIV_ROUND_UP(sizeof(struct vchiq_slot_zero), \ + VCHIQ_SLOT_SIZE) + +#define VCHIQ_FOURCC_AS_4CHARS(fourcc) \ + ((fourcc) >> 24) & 0xff, \ + ((fourcc) >> 16) & 0xff, \ + ((fourcc) >> 8) & 0xff, \ + (fourcc) & 0xff + +#define BITSET_SIZE(b) ((b + 31) >> 5) +#define BITSET_WORD(b) (b >> 5) +#define BITSET_BIT(b) (1 << (b & 31)) +#define BITSET_IS_SET(bs, b) (bs[BITSET_WORD(b)] & BITSET_BIT(b)) +#define BITSET_SET(bs, b) (bs[BITSET_WORD(b)] |= BITSET_BIT(b)) + +enum { + DEBUG_ENTRIES, +#if VCHIQ_ENABLE_DEBUG + DEBUG_SLOT_HANDLER_COUNT, + DEBUG_SLOT_HANDLER_LINE, + DEBUG_PARSE_LINE, + DEBUG_PARSE_HEADER, + DEBUG_PARSE_MSGID, + DEBUG_AWAIT_COMPLETION_LINE, + DEBUG_DEQUEUE_MESSAGE_LINE, + DEBUG_SERVICE_CALLBACK_LINE, + DEBUG_MSG_QUEUE_FULL_COUNT, + DEBUG_COMPLETION_QUEUE_FULL_COUNT, +#endif + DEBUG_MAX +}; + +#if VCHIQ_ENABLE_DEBUG + +#define DEBUG_INITIALISE(local) int *debug_ptr = (local)->debug +#define DEBUG_TRACE(d) \ + do { debug_ptr[DEBUG_ ## d] = __LINE__; dsb(sy); } while (0) +#define DEBUG_VALUE(d, v) \ + do { debug_ptr[DEBUG_ ## d] = (v); dsb(sy); } while (0) +#define DEBUG_COUNT(d) \ + do { debug_ptr[DEBUG_ ## d]++; dsb(sy); } while (0) + +#else /* VCHIQ_ENABLE_DEBUG */ + +#define DEBUG_INITIALISE(local) +#define DEBUG_TRACE(d) +#define DEBUG_VALUE(d, v) +#define DEBUG_COUNT(d) + +#endif /* VCHIQ_ENABLE_DEBUG */ + +enum vchiq_connstate { + VCHIQ_CONNSTATE_DISCONNECTED, + VCHIQ_CONNSTATE_CONNECTING, + VCHIQ_CONNSTATE_CONNECTED, + VCHIQ_CONNSTATE_PAUSING, + VCHIQ_CONNSTATE_PAUSE_SENT, + VCHIQ_CONNSTATE_PAUSED, + VCHIQ_CONNSTATE_RESUMING, + VCHIQ_CONNSTATE_PAUSE_TIMEOUT, + VCHIQ_CONNSTATE_RESUME_TIMEOUT +}; + +enum { + VCHIQ_SRVSTATE_FREE, + VCHIQ_SRVSTATE_HIDDEN, + VCHIQ_SRVSTATE_LISTENING, + VCHIQ_SRVSTATE_OPENING, + VCHIQ_SRVSTATE_OPEN, + VCHIQ_SRVSTATE_OPENSYNC, + VCHIQ_SRVSTATE_CLOSESENT, + VCHIQ_SRVSTATE_CLOSERECVD, + VCHIQ_SRVSTATE_CLOSEWAIT, + VCHIQ_SRVSTATE_CLOSED +}; + +enum vchiq_bulk_dir { + VCHIQ_BULK_TRANSMIT, + VCHIQ_BULK_RECEIVE +}; + +struct vchiq_bulk { + short mode; + short dir; + void *userdata; + dma_addr_t data; + int size; + void *remote_data; + int remote_size; + int actual; +}; + +struct vchiq_bulk_queue { + int local_insert; /* Where to insert the next local bulk */ + int remote_insert; /* Where to insert the next remote bulk (master) */ + int process; /* Bulk to transfer next */ + int remote_notify; /* Bulk to notify the remote client of next (mstr) */ + int remove; /* Bulk to notify the local client of, and remove, next */ + struct vchiq_bulk bulks[VCHIQ_NUM_SERVICE_BULKS]; +}; + +struct remote_event { + int armed; + int fired; + u32 __unused; +}; + +struct opaque_platform_state; + +struct vchiq_slot { + char data[VCHIQ_SLOT_SIZE]; +}; + +struct vchiq_slot_info { + /* Use two counters rather than one to avoid the need for a mutex. */ + short use_count; + short release_count; +}; + +struct vchiq_service { + struct vchiq_service_base base; + unsigned int handle; + struct kref ref_count; + struct rcu_head rcu; + int srvstate; + void (*userdata_term)(void *userdata); + unsigned int localport; + unsigned int remoteport; + int public_fourcc; + int client_id; + char auto_close; + char sync; + char closing; + char trace; + atomic_t poll_flags; + short version; + short version_min; + short peer_version; + + struct vchiq_state *state; + struct vchiq_instance *instance; + + int service_use_count; + + struct vchiq_bulk_queue bulk_tx; + struct vchiq_bulk_queue bulk_rx; + + struct completion remove_event; + struct completion bulk_remove_event; + struct mutex bulk_mutex; + + struct service_stats_struct { + int quota_stalls; + int slot_stalls; + int bulk_stalls; + int error_count; + int ctrl_tx_count; + int ctrl_rx_count; + int bulk_tx_count; + int bulk_rx_count; + int bulk_aborted_count; + u64 ctrl_tx_bytes; + u64 ctrl_rx_bytes; + u64 bulk_tx_bytes; + u64 bulk_rx_bytes; + } stats; + + int msg_queue_read; + int msg_queue_write; + struct completion msg_queue_pop; + struct completion msg_queue_push; + struct vchiq_header *msg_queue[VCHIQ_MAX_SLOTS]; +}; + +/* + * The quota information is outside struct vchiq_service so that it can + * be statically allocated, since for accounting reasons a service's slot + * usage is carried over between users of the same port number. + */ +struct vchiq_service_quota { + unsigned short slot_quota; + unsigned short slot_use_count; + unsigned short message_quota; + unsigned short message_use_count; + struct completion quota_event; + int previous_tx_index; +}; + +struct vchiq_shared_state { + /* A non-zero value here indicates that the content is valid. */ + int initialised; + + /* The first and last (inclusive) slots allocated to the owner. */ + int slot_first; + int slot_last; + + /* The slot allocated to synchronous messages from the owner. */ + int slot_sync; + + /* + * Signalling this event indicates that owner's slot handler thread + * should run. + */ + struct remote_event trigger; + + /* + * Indicates the byte position within the stream where the next message + * will be written. The least significant bits are an index into the + * slot. The next bits are the index of the slot in slot_queue. + */ + int tx_pos; + + /* This event should be signalled when a slot is recycled. */ + struct remote_event recycle; + + /* The slot_queue index where the next recycled slot will be written. */ + int slot_queue_recycle; + + /* This event should be signalled when a synchronous message is sent. */ + struct remote_event sync_trigger; + + /* + * This event should be signalled when a synchronous message has been + * released. + */ + struct remote_event sync_release; + + /* A circular buffer of slot indexes. */ + int slot_queue[VCHIQ_MAX_SLOTS_PER_SIDE]; + + /* Debugging state */ + int debug[DEBUG_MAX]; +}; + +struct vchiq_slot_zero { + int magic; + short version; + short version_min; + int slot_zero_size; + int slot_size; + int max_slots; + int max_slots_per_side; + int platform_data[2]; + struct vchiq_shared_state master; + struct vchiq_shared_state slave; + struct vchiq_slot_info slots[VCHIQ_MAX_SLOTS]; +}; + +struct vchiq_state { + struct device *dev; + int id; + int initialised; + enum vchiq_connstate conn_state; + short version_common; + + struct vchiq_shared_state *local; + struct vchiq_shared_state *remote; + struct vchiq_slot *slot_data; + + unsigned short default_slot_quota; + unsigned short default_message_quota; + + /* Event indicating connect message received */ + struct completion connect; + + /* Mutex protecting services */ + struct mutex mutex; + struct vchiq_instance **instance; + + /* Processes incoming messages */ + struct task_struct *slot_handler_thread; + + /* Processes recycled slots */ + struct task_struct *recycle_thread; + + /* Processes synchronous messages */ + struct task_struct *sync_thread; + + /* Local implementation of the trigger remote event */ + wait_queue_head_t trigger_event; + + /* Local implementation of the recycle remote event */ + wait_queue_head_t recycle_event; + + /* Local implementation of the sync trigger remote event */ + wait_queue_head_t sync_trigger_event; + + /* Local implementation of the sync release remote event */ + wait_queue_head_t sync_release_event; + + char *tx_data; + char *rx_data; + struct vchiq_slot_info *rx_info; + + struct mutex slot_mutex; + + struct mutex recycle_mutex; + + struct mutex sync_mutex; + + struct mutex bulk_transfer_mutex; + + /* + * Indicates the byte position within the stream from where the next + * message will be read. The least significant bits are an index into + * the slot.The next bits are the index of the slot in + * remote->slot_queue. + */ + int rx_pos; + + /* + * A cached copy of local->tx_pos. Only write to local->tx_pos, and read + * from remote->tx_pos. + */ + int local_tx_pos; + + /* The slot_queue index of the slot to become available next. */ + int slot_queue_available; + + /* A flag to indicate if any poll has been requested */ + int poll_needed; + + /* Ths index of the previous slot used for data messages. */ + int previous_data_index; + + /* The number of slots occupied by data messages. */ + unsigned short data_use_count; + + /* The maximum number of slots to be occupied by data messages. */ + unsigned short data_quota; + + /* An array of bit sets indicating which services must be polled. */ + atomic_t poll_services[BITSET_SIZE(VCHIQ_MAX_SERVICES)]; + + /* The number of the first unused service */ + int unused_service; + + /* Signalled when a free slot becomes available. */ + struct completion slot_available_event; + + struct completion slot_remove_event; + + /* Signalled when a free data slot becomes available. */ + struct completion data_quota_event; + + struct state_stats_struct { + int slot_stalls; + int data_stalls; + int ctrl_tx_count; + int ctrl_rx_count; + int error_count; + } stats; + + struct vchiq_service __rcu *services[VCHIQ_MAX_SERVICES]; + struct vchiq_service_quota service_quotas[VCHIQ_MAX_SERVICES]; + struct vchiq_slot_info slot_info[VCHIQ_MAX_SLOTS]; + + struct opaque_platform_state *platform_state; +}; + +struct bulk_waiter { + struct vchiq_bulk *bulk; + struct completion event; + int actual; +}; + +struct vchiq_config { + unsigned int max_msg_size; + unsigned int bulk_threshold; /* The message size above which it + * is better to use a bulk transfer + * (<= max_msg_size) + */ + unsigned int max_outstanding_bulks; + unsigned int max_services; + short version; /* The version of VCHIQ */ + short version_min; /* The minimum compatible version of VCHIQ */ +}; + +extern spinlock_t bulk_waiter_spinlock; + +extern int vchiq_core_log_level; +extern int vchiq_core_msg_log_level; +extern int vchiq_sync_log_level; + +extern const char * +get_conn_state_name(enum vchiq_connstate conn_state); + +extern struct vchiq_slot_zero * +vchiq_init_slots(void *mem_base, int mem_size); + +extern int +vchiq_init_state(struct vchiq_state *state, struct vchiq_slot_zero *slot_zero, struct device *dev); + +extern enum vchiq_status +vchiq_connect_internal(struct vchiq_state *state, struct vchiq_instance *instance); + +struct vchiq_service * +vchiq_add_service_internal(struct vchiq_state *state, + const struct vchiq_service_params_kernel *params, + int srvstate, struct vchiq_instance *instance, + void (*userdata_term)(void *userdata)); + +extern enum vchiq_status +vchiq_open_service_internal(struct vchiq_service *service, int client_id); + +extern enum vchiq_status +vchiq_close_service_internal(struct vchiq_service *service, int close_recvd); + +extern void +vchiq_terminate_service_internal(struct vchiq_service *service); + +extern void +vchiq_free_service_internal(struct vchiq_service *service); + +extern void +vchiq_shutdown_internal(struct vchiq_state *state, struct vchiq_instance *instance); + +extern void +remote_event_pollall(struct vchiq_state *state); + +extern enum vchiq_status +vchiq_bulk_transfer(struct vchiq_instance *instance, unsigned int handle, void *offset, + void __user *uoffset, int size, void *userdata, enum vchiq_bulk_mode mode, + enum vchiq_bulk_dir dir); + +extern int +vchiq_dump_state(void *dump_context, struct vchiq_state *state); + +extern int +vchiq_dump_service_state(void *dump_context, struct vchiq_service *service); + +extern void +vchiq_loud_error_header(void); + +extern void +vchiq_loud_error_footer(void); + +extern void +request_poll(struct vchiq_state *state, struct vchiq_service *service, + int poll_type); + +struct vchiq_service *handle_to_service(struct vchiq_instance *instance, unsigned int handle); + +extern struct vchiq_service * +find_service_by_handle(struct vchiq_instance *instance, unsigned int handle); + +extern struct vchiq_service * +find_service_by_port(struct vchiq_state *state, unsigned int localport); + +extern struct vchiq_service * +find_service_for_instance(struct vchiq_instance *instance, unsigned int handle); + +extern struct vchiq_service * +find_closed_service_for_instance(struct vchiq_instance *instance, unsigned int handle); + +extern struct vchiq_service * +__next_service_by_instance(struct vchiq_state *state, + struct vchiq_instance *instance, + int *pidx); + +extern struct vchiq_service * +next_service_by_instance(struct vchiq_state *state, + struct vchiq_instance *instance, + int *pidx); + +extern void +vchiq_service_get(struct vchiq_service *service); + +extern void +vchiq_service_put(struct vchiq_service *service); + +extern enum vchiq_status +vchiq_queue_message(struct vchiq_instance *instance, unsigned int handle, + ssize_t (*copy_callback)(void *context, void *dest, + size_t offset, size_t maxsize), + void *context, + size_t size); + +int vchiq_prepare_bulk_data(struct vchiq_instance *instance, struct vchiq_bulk *bulk, void *offset, + void __user *uoffset, int size, int dir); + +void vchiq_complete_bulk(struct vchiq_instance *instance, struct vchiq_bulk *bulk); + +void remote_event_signal(struct remote_event *event); + +int vchiq_dump(void *dump_context, const char *str, int len); + +int vchiq_dump_platform_state(void *dump_context); + +int vchiq_dump_platform_instances(void *dump_context); + +int vchiq_dump_platform_service_state(void *dump_context, struct vchiq_service *service); + +int vchiq_use_service_internal(struct vchiq_service *service); + +int vchiq_release_service_internal(struct vchiq_service *service); + +void vchiq_on_remote_use(struct vchiq_state *state); + +void vchiq_on_remote_release(struct vchiq_state *state); + +int vchiq_platform_init_state(struct vchiq_state *state); + +enum vchiq_status vchiq_check_service(struct vchiq_service *service); + +void vchiq_on_remote_use_active(struct vchiq_state *state); + +enum vchiq_status vchiq_send_remote_use(struct vchiq_state *state); + +enum vchiq_status vchiq_send_remote_use_active(struct vchiq_state *state); + +void vchiq_platform_conn_state_changed(struct vchiq_state *state, + enum vchiq_connstate oldstate, + enum vchiq_connstate newstate); + +void vchiq_set_conn_state(struct vchiq_state *state, enum vchiq_connstate newstate); + +void vchiq_log_dump_mem(const char *label, u32 addr, const void *void_mem, size_t num_bytes); + +enum vchiq_status vchiq_remove_service(struct vchiq_instance *instance, unsigned int service); + +int vchiq_get_client_id(struct vchiq_instance *instance, unsigned int service); + +void vchiq_get_config(struct vchiq_config *config); + +int vchiq_set_service_option(struct vchiq_instance *instance, unsigned int service, + enum vchiq_service_option option, int value); + +#endif diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.c new file mode 100644 index 000000000..dc667afd1 --- /dev/null +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.c @@ -0,0 +1,247 @@ +// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause +/* + * Copyright (c) 2014 Raspberry Pi (Trading) Ltd. All rights reserved. + * Copyright (c) 2010-2012 Broadcom. All rights reserved. + */ + +#include <linux/debugfs.h> +#include "vchiq_core.h" +#include "vchiq_arm.h" +#include "vchiq_debugfs.h" + +#ifdef CONFIG_DEBUG_FS + +#define DEBUGFS_WRITE_BUF_SIZE 256 + +#define VCHIQ_LOG_ERROR_STR "error" +#define VCHIQ_LOG_WARNING_STR "warning" +#define VCHIQ_LOG_INFO_STR "info" +#define VCHIQ_LOG_TRACE_STR "trace" + +/* Global 'vchiq' debugfs and clients entry used by all instances */ +static struct dentry *vchiq_dbg_dir; +static struct dentry *vchiq_dbg_clients; + +/* Log category debugfs entries */ +struct vchiq_debugfs_log_entry { + const char *name; + void *plevel; +}; + +static struct vchiq_debugfs_log_entry vchiq_debugfs_log_entries[] = { + { "core", &vchiq_core_log_level }, + { "msg", &vchiq_core_msg_log_level }, + { "sync", &vchiq_sync_log_level }, + { "susp", &vchiq_susp_log_level }, + { "arm", &vchiq_arm_log_level }, +}; + +static int debugfs_log_show(struct seq_file *f, void *offset) +{ + int *levp = f->private; + char *log_value = NULL; + + switch (*levp) { + case VCHIQ_LOG_ERROR: + log_value = VCHIQ_LOG_ERROR_STR; + break; + case VCHIQ_LOG_WARNING: + log_value = VCHIQ_LOG_WARNING_STR; + break; + case VCHIQ_LOG_INFO: + log_value = VCHIQ_LOG_INFO_STR; + break; + case VCHIQ_LOG_TRACE: + log_value = VCHIQ_LOG_TRACE_STR; + break; + default: + break; + } + + seq_printf(f, "%s\n", log_value ? log_value : "(null)"); + + return 0; +} + +static int debugfs_log_open(struct inode *inode, struct file *file) +{ + return single_open(file, debugfs_log_show, inode->i_private); +} + +static ssize_t debugfs_log_write(struct file *file, + const char __user *buffer, + size_t count, loff_t *ppos) +{ + struct seq_file *f = (struct seq_file *)file->private_data; + int *levp = f->private; + char kbuf[DEBUGFS_WRITE_BUF_SIZE + 1]; + + memset(kbuf, 0, DEBUGFS_WRITE_BUF_SIZE + 1); + if (count >= DEBUGFS_WRITE_BUF_SIZE) + count = DEBUGFS_WRITE_BUF_SIZE; + + if (copy_from_user(kbuf, buffer, count)) + return -EFAULT; + kbuf[count - 1] = 0; + + if (strncmp("error", kbuf, strlen("error")) == 0) + *levp = VCHIQ_LOG_ERROR; + else if (strncmp("warning", kbuf, strlen("warning")) == 0) + *levp = VCHIQ_LOG_WARNING; + else if (strncmp("info", kbuf, strlen("info")) == 0) + *levp = VCHIQ_LOG_INFO; + else if (strncmp("trace", kbuf, strlen("trace")) == 0) + *levp = VCHIQ_LOG_TRACE; + else + *levp = VCHIQ_LOG_DEFAULT; + + *ppos += count; + + return count; +} + +static const struct file_operations debugfs_log_fops = { + .owner = THIS_MODULE, + .open = debugfs_log_open, + .write = debugfs_log_write, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int debugfs_usecount_show(struct seq_file *f, void *offset) +{ + struct vchiq_instance *instance = f->private; + int use_count; + + use_count = vchiq_instance_get_use_count(instance); + seq_printf(f, "%d\n", use_count); + + return 0; +} +DEFINE_SHOW_ATTRIBUTE(debugfs_usecount); + +static int debugfs_trace_show(struct seq_file *f, void *offset) +{ + struct vchiq_instance *instance = f->private; + int trace; + + trace = vchiq_instance_get_trace(instance); + seq_printf(f, "%s\n", trace ? "Y" : "N"); + + return 0; +} + +static int debugfs_trace_open(struct inode *inode, struct file *file) +{ + return single_open(file, debugfs_trace_show, inode->i_private); +} + +static ssize_t debugfs_trace_write(struct file *file, + const char __user *buffer, + size_t count, loff_t *ppos) +{ + struct seq_file *f = (struct seq_file *)file->private_data; + struct vchiq_instance *instance = f->private; + char firstchar; + + if (copy_from_user(&firstchar, buffer, 1)) + return -EFAULT; + + switch (firstchar) { + case 'Y': + case 'y': + case '1': + vchiq_instance_set_trace(instance, 1); + break; + case 'N': + case 'n': + case '0': + vchiq_instance_set_trace(instance, 0); + break; + default: + break; + } + + *ppos += count; + + return count; +} + +static const struct file_operations debugfs_trace_fops = { + .owner = THIS_MODULE, + .open = debugfs_trace_open, + .write = debugfs_trace_write, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +/* add an instance (process) to the debugfs entries */ +void vchiq_debugfs_add_instance(struct vchiq_instance *instance) +{ + char pidstr[16]; + struct dentry *top; + + snprintf(pidstr, sizeof(pidstr), "%d", + vchiq_instance_get_pid(instance)); + + top = debugfs_create_dir(pidstr, vchiq_dbg_clients); + + debugfs_create_file("use_count", 0444, top, instance, + &debugfs_usecount_fops); + debugfs_create_file("trace", 0644, top, instance, &debugfs_trace_fops); + + vchiq_instance_get_debugfs_node(instance)->dentry = top; +} + +void vchiq_debugfs_remove_instance(struct vchiq_instance *instance) +{ + struct vchiq_debugfs_node *node = + vchiq_instance_get_debugfs_node(instance); + + debugfs_remove_recursive(node->dentry); +} + +void vchiq_debugfs_init(void) +{ + struct dentry *dir; + int i; + + vchiq_dbg_dir = debugfs_create_dir("vchiq", NULL); + vchiq_dbg_clients = debugfs_create_dir("clients", vchiq_dbg_dir); + + /* create an entry under <debugfs>/vchiq/log for each log category */ + dir = debugfs_create_dir("log", vchiq_dbg_dir); + + for (i = 0; i < ARRAY_SIZE(vchiq_debugfs_log_entries); i++) + debugfs_create_file(vchiq_debugfs_log_entries[i].name, 0644, + dir, vchiq_debugfs_log_entries[i].plevel, + &debugfs_log_fops); +} + +/* remove all the debugfs entries */ +void vchiq_debugfs_deinit(void) +{ + debugfs_remove_recursive(vchiq_dbg_dir); +} + +#else /* CONFIG_DEBUG_FS */ + +void vchiq_debugfs_init(void) +{ +} + +void vchiq_debugfs_deinit(void) +{ +} + +void vchiq_debugfs_add_instance(struct vchiq_instance *instance) +{ +} + +void vchiq_debugfs_remove_instance(struct vchiq_instance *instance) +{ +} + +#endif /* CONFIG_DEBUG_FS */ diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.h new file mode 100644 index 000000000..e9bf055a4 --- /dev/null +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.h @@ -0,0 +1,21 @@ +/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */ +/* Copyright (c) 2014 Raspberry Pi (Trading) Ltd. All rights reserved. */ + +#ifndef VCHIQ_DEBUGFS_H +#define VCHIQ_DEBUGFS_H + +#include "vchiq_core.h" + +struct vchiq_debugfs_node { + struct dentry *dentry; +}; + +void vchiq_debugfs_init(void); + +void vchiq_debugfs_deinit(void); + +void vchiq_debugfs_add_instance(struct vchiq_instance *instance); + +void vchiq_debugfs_remove_instance(struct vchiq_instance *instance); + +#endif /* VCHIQ_DEBUGFS_H */ diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_dev.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_dev.c new file mode 100644 index 000000000..7e2974944 --- /dev/null +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_dev.c @@ -0,0 +1,1370 @@ +// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause +/* + * Copyright (c) 2014 Raspberry Pi (Trading) Ltd. All rights reserved. + * Copyright (c) 2010-2012 Broadcom. All rights reserved. + */ + +#include <linux/cdev.h> +#include <linux/fs.h> +#include <linux/device.h> +#include <linux/slab.h> +#include <linux/compat.h> +#include <linux/miscdevice.h> + +#include "vchiq_core.h" +#include "vchiq_ioctl.h" +#include "vchiq_arm.h" +#include "vchiq_debugfs.h" + +static const char *const ioctl_names[] = { + "CONNECT", + "SHUTDOWN", + "CREATE_SERVICE", + "REMOVE_SERVICE", + "QUEUE_MESSAGE", + "QUEUE_BULK_TRANSMIT", + "QUEUE_BULK_RECEIVE", + "AWAIT_COMPLETION", + "DEQUEUE_MESSAGE", + "GET_CLIENT_ID", + "GET_CONFIG", + "CLOSE_SERVICE", + "USE_SERVICE", + "RELEASE_SERVICE", + "SET_SERVICE_OPTION", + "DUMP_PHYS_MEM", + "LIB_VERSION", + "CLOSE_DELIVERED" +}; + +static_assert(ARRAY_SIZE(ioctl_names) == (VCHIQ_IOC_MAX + 1)); + +static void +user_service_free(void *userdata) +{ + kfree(userdata); +} + +static void close_delivered(struct user_service *user_service) +{ + vchiq_log_info(vchiq_arm_log_level, + "%s(handle=%x)", + __func__, user_service->service->handle); + + if (user_service->close_pending) { + /* Allow the underlying service to be culled */ + vchiq_service_put(user_service->service); + + /* Wake the user-thread blocked in close_ or remove_service */ + complete(&user_service->close_event); + + user_service->close_pending = 0; + } +} + +struct vchiq_io_copy_callback_context { + struct vchiq_element *element; + size_t element_offset; + unsigned long elements_to_go; +}; + +static ssize_t vchiq_ioc_copy_element_data(void *context, void *dest, + size_t offset, size_t maxsize) +{ + struct vchiq_io_copy_callback_context *cc = context; + size_t total_bytes_copied = 0; + size_t bytes_this_round; + + while (total_bytes_copied < maxsize) { + if (!cc->elements_to_go) + return total_bytes_copied; + + if (!cc->element->size) { + cc->elements_to_go--; + cc->element++; + cc->element_offset = 0; + continue; + } + + bytes_this_round = min(cc->element->size - cc->element_offset, + maxsize - total_bytes_copied); + + if (copy_from_user(dest + total_bytes_copied, + cc->element->data + cc->element_offset, + bytes_this_round)) + return -EFAULT; + + cc->element_offset += bytes_this_round; + total_bytes_copied += bytes_this_round; + + if (cc->element_offset == cc->element->size) { + cc->elements_to_go--; + cc->element++; + cc->element_offset = 0; + } + } + + return maxsize; +} + +static int +vchiq_ioc_queue_message(struct vchiq_instance *instance, unsigned int handle, + struct vchiq_element *elements, unsigned long count) +{ + struct vchiq_io_copy_callback_context context; + enum vchiq_status status = VCHIQ_SUCCESS; + unsigned long i; + size_t total_size = 0; + + context.element = elements; + context.element_offset = 0; + context.elements_to_go = count; + + for (i = 0; i < count; i++) { + if (!elements[i].data && elements[i].size != 0) + return -EFAULT; + + total_size += elements[i].size; + } + + status = vchiq_queue_message(instance, handle, vchiq_ioc_copy_element_data, + &context, total_size); + + if (status == VCHIQ_ERROR) + return -EIO; + else if (status == VCHIQ_RETRY) + return -EINTR; + return 0; +} + +static int vchiq_ioc_create_service(struct vchiq_instance *instance, + struct vchiq_create_service *args) +{ + struct user_service *user_service = NULL; + struct vchiq_service *service; + enum vchiq_status status = VCHIQ_SUCCESS; + struct vchiq_service_params_kernel params; + int srvstate; + + if (args->is_open && !instance->connected) + return -ENOTCONN; + + user_service = kmalloc(sizeof(*user_service), GFP_KERNEL); + if (!user_service) + return -ENOMEM; + + if (args->is_open) { + srvstate = VCHIQ_SRVSTATE_OPENING; + } else { + srvstate = instance->connected ? + VCHIQ_SRVSTATE_LISTENING : VCHIQ_SRVSTATE_HIDDEN; + } + + params = (struct vchiq_service_params_kernel) { + .fourcc = args->params.fourcc, + .callback = service_callback, + .userdata = user_service, + .version = args->params.version, + .version_min = args->params.version_min, + }; + service = vchiq_add_service_internal(instance->state, ¶ms, + srvstate, instance, + user_service_free); + if (!service) { + kfree(user_service); + return -EEXIST; + } + + user_service->service = service; + user_service->userdata = args->params.userdata; + user_service->instance = instance; + user_service->is_vchi = (args->is_vchi != 0); + user_service->dequeue_pending = 0; + user_service->close_pending = 0; + user_service->message_available_pos = instance->completion_remove - 1; + user_service->msg_insert = 0; + user_service->msg_remove = 0; + init_completion(&user_service->insert_event); + init_completion(&user_service->remove_event); + init_completion(&user_service->close_event); + + if (args->is_open) { + status = vchiq_open_service_internal(service, instance->pid); + if (status != VCHIQ_SUCCESS) { + vchiq_remove_service(instance, service->handle); + return (status == VCHIQ_RETRY) ? + -EINTR : -EIO; + } + } + args->handle = service->handle; + + return 0; +} + +static int vchiq_ioc_dequeue_message(struct vchiq_instance *instance, + struct vchiq_dequeue_message *args) +{ + struct user_service *user_service; + struct vchiq_service *service; + struct vchiq_header *header; + int ret; + + DEBUG_INITIALISE(g_state.local); + DEBUG_TRACE(DEQUEUE_MESSAGE_LINE); + service = find_service_for_instance(instance, args->handle); + if (!service) + return -EINVAL; + + user_service = (struct user_service *)service->base.userdata; + if (user_service->is_vchi == 0) { + ret = -EINVAL; + goto out; + } + + spin_lock(&msg_queue_spinlock); + if (user_service->msg_remove == user_service->msg_insert) { + if (!args->blocking) { + spin_unlock(&msg_queue_spinlock); + DEBUG_TRACE(DEQUEUE_MESSAGE_LINE); + ret = -EWOULDBLOCK; + goto out; + } + user_service->dequeue_pending = 1; + ret = 0; + do { + spin_unlock(&msg_queue_spinlock); + DEBUG_TRACE(DEQUEUE_MESSAGE_LINE); + if (wait_for_completion_interruptible(&user_service->insert_event)) { + vchiq_log_info(vchiq_arm_log_level, + "DEQUEUE_MESSAGE interrupted"); + ret = -EINTR; + break; + } + spin_lock(&msg_queue_spinlock); + } while (user_service->msg_remove == user_service->msg_insert); + + if (ret) + goto out; + } + + if (WARN_ON_ONCE((int)(user_service->msg_insert - + user_service->msg_remove) < 0)) { + spin_unlock(&msg_queue_spinlock); + ret = -EINVAL; + goto out; + } + + header = user_service->msg_queue[user_service->msg_remove & + (MSG_QUEUE_SIZE - 1)]; + user_service->msg_remove++; + spin_unlock(&msg_queue_spinlock); + + complete(&user_service->remove_event); + if (!header) { + ret = -ENOTCONN; + } else if (header->size <= args->bufsize) { + /* Copy to user space if msgbuf is not NULL */ + if (!args->buf || (copy_to_user(args->buf, header->data, header->size) == 0)) { + ret = header->size; + vchiq_release_message(instance, service->handle, header); + } else { + ret = -EFAULT; + } + } else { + vchiq_log_error(vchiq_arm_log_level, + "header %pK: bufsize %x < size %x", + header, args->bufsize, header->size); + WARN(1, "invalid size\n"); + ret = -EMSGSIZE; + } + DEBUG_TRACE(DEQUEUE_MESSAGE_LINE); +out: + vchiq_service_put(service); + return ret; +} + +static int vchiq_irq_queue_bulk_tx_rx(struct vchiq_instance *instance, + struct vchiq_queue_bulk_transfer *args, + enum vchiq_bulk_dir dir, + enum vchiq_bulk_mode __user *mode) +{ + struct vchiq_service *service; + struct bulk_waiter_node *waiter = NULL, *iter; + void *userdata; + int status = 0; + int ret; + + service = find_service_for_instance(instance, args->handle); + if (!service) + return -EINVAL; + + if (args->mode == VCHIQ_BULK_MODE_BLOCKING) { + waiter = kzalloc(sizeof(*waiter), GFP_KERNEL); + if (!waiter) { + ret = -ENOMEM; + goto out; + } + + userdata = &waiter->bulk_waiter; + } else if (args->mode == VCHIQ_BULK_MODE_WAITING) { + mutex_lock(&instance->bulk_waiter_list_mutex); + list_for_each_entry(iter, &instance->bulk_waiter_list, + list) { + if (iter->pid == current->pid) { + list_del(&iter->list); + waiter = iter; + break; + } + } + mutex_unlock(&instance->bulk_waiter_list_mutex); + if (!waiter) { + vchiq_log_error(vchiq_arm_log_level, + "no bulk_waiter found for pid %d", current->pid); + ret = -ESRCH; + goto out; + } + vchiq_log_info(vchiq_arm_log_level, + "found bulk_waiter %pK for pid %d", waiter, current->pid); + userdata = &waiter->bulk_waiter; + } else { + userdata = args->userdata; + } + + status = vchiq_bulk_transfer(instance, args->handle, NULL, args->data, args->size, + userdata, args->mode, dir); + + if (!waiter) { + ret = 0; + goto out; + } + + if ((status != VCHIQ_RETRY) || fatal_signal_pending(current) || + !waiter->bulk_waiter.bulk) { + if (waiter->bulk_waiter.bulk) { + /* Cancel the signal when the transfer completes. */ + spin_lock(&bulk_waiter_spinlock); + waiter->bulk_waiter.bulk->userdata = NULL; + spin_unlock(&bulk_waiter_spinlock); + } + kfree(waiter); + ret = 0; + } else { + const enum vchiq_bulk_mode mode_waiting = + VCHIQ_BULK_MODE_WAITING; + waiter->pid = current->pid; + mutex_lock(&instance->bulk_waiter_list_mutex); + list_add(&waiter->list, &instance->bulk_waiter_list); + mutex_unlock(&instance->bulk_waiter_list_mutex); + vchiq_log_info(vchiq_arm_log_level, + "saved bulk_waiter %pK for pid %d", waiter, current->pid); + + ret = put_user(mode_waiting, mode); + } +out: + vchiq_service_put(service); + if (ret) + return ret; + else if (status == VCHIQ_ERROR) + return -EIO; + else if (status == VCHIQ_RETRY) + return -EINTR; + return 0; +} + +/* read a user pointer value from an array pointers in user space */ +static inline int vchiq_get_user_ptr(void __user **buf, void __user *ubuf, int index) +{ + int ret; + + if (in_compat_syscall()) { + compat_uptr_t ptr32; + compat_uptr_t __user *uptr = ubuf; + + ret = get_user(ptr32, uptr + index); + if (ret) + return ret; + + *buf = compat_ptr(ptr32); + } else { + uintptr_t ptr, __user *uptr = ubuf; + + ret = get_user(ptr, uptr + index); + + if (ret) + return ret; + + *buf = (void __user *)ptr; + } + + return 0; +} + +struct vchiq_completion_data32 { + enum vchiq_reason reason; + compat_uptr_t header; + compat_uptr_t service_userdata; + compat_uptr_t bulk_userdata; +}; + +static int vchiq_put_completion(struct vchiq_completion_data __user *buf, + struct vchiq_completion_data *completion, + int index) +{ + struct vchiq_completion_data32 __user *buf32 = (void __user *)buf; + + if (in_compat_syscall()) { + struct vchiq_completion_data32 tmp = { + .reason = completion->reason, + .header = ptr_to_compat(completion->header), + .service_userdata = ptr_to_compat(completion->service_userdata), + .bulk_userdata = ptr_to_compat(completion->bulk_userdata), + }; + if (copy_to_user(&buf32[index], &tmp, sizeof(tmp))) + return -EFAULT; + } else { + if (copy_to_user(&buf[index], completion, sizeof(*completion))) + return -EFAULT; + } + + return 0; +} + +static int vchiq_ioc_await_completion(struct vchiq_instance *instance, + struct vchiq_await_completion *args, + int __user *msgbufcountp) +{ + int msgbufcount; + int remove; + int ret; + + DEBUG_INITIALISE(g_state.local); + + DEBUG_TRACE(AWAIT_COMPLETION_LINE); + if (!instance->connected) + return -ENOTCONN; + + mutex_lock(&instance->completion_mutex); + + DEBUG_TRACE(AWAIT_COMPLETION_LINE); + while ((instance->completion_remove == instance->completion_insert) && !instance->closing) { + int rc; + + DEBUG_TRACE(AWAIT_COMPLETION_LINE); + mutex_unlock(&instance->completion_mutex); + rc = wait_for_completion_interruptible(&instance->insert_event); + mutex_lock(&instance->completion_mutex); + if (rc) { + DEBUG_TRACE(AWAIT_COMPLETION_LINE); + vchiq_log_info(vchiq_arm_log_level, + "AWAIT_COMPLETION interrupted"); + ret = -EINTR; + goto out; + } + } + DEBUG_TRACE(AWAIT_COMPLETION_LINE); + + msgbufcount = args->msgbufcount; + remove = instance->completion_remove; + + for (ret = 0; ret < args->count; ret++) { + struct vchiq_completion_data_kernel *completion; + struct vchiq_completion_data user_completion; + struct vchiq_service *service; + struct user_service *user_service; + struct vchiq_header *header; + + if (remove == instance->completion_insert) + break; + + completion = &instance->completions[remove & (MAX_COMPLETIONS - 1)]; + + /* + * A read memory barrier is needed to stop + * prefetch of a stale completion record + */ + rmb(); + + service = completion->service_userdata; + user_service = service->base.userdata; + + memset(&user_completion, 0, sizeof(user_completion)); + user_completion = (struct vchiq_completion_data) { + .reason = completion->reason, + .service_userdata = user_service->userdata, + }; + + header = completion->header; + if (header) { + void __user *msgbuf; + int msglen; + + msglen = header->size + sizeof(struct vchiq_header); + /* This must be a VCHIQ-style service */ + if (args->msgbufsize < msglen) { + vchiq_log_error(vchiq_arm_log_level, + "header %pK: msgbufsize %x < msglen %x", + header, args->msgbufsize, msglen); + WARN(1, "invalid message size\n"); + if (ret == 0) + ret = -EMSGSIZE; + break; + } + if (msgbufcount <= 0) + /* Stall here for lack of a buffer for the message. */ + break; + /* Get the pointer from user space */ + msgbufcount--; + if (vchiq_get_user_ptr(&msgbuf, args->msgbufs, + msgbufcount)) { + if (ret == 0) + ret = -EFAULT; + break; + } + + /* Copy the message to user space */ + if (copy_to_user(msgbuf, header, msglen)) { + if (ret == 0) + ret = -EFAULT; + break; + } + + /* Now it has been copied, the message can be released. */ + vchiq_release_message(instance, service->handle, header); + + /* The completion must point to the msgbuf. */ + user_completion.header = msgbuf; + } + + if ((completion->reason == VCHIQ_SERVICE_CLOSED) && + !instance->use_close_delivered) + vchiq_service_put(service); + + /* + * FIXME: address space mismatch, does bulk_userdata + * actually point to user or kernel memory? + */ + user_completion.bulk_userdata = completion->bulk_userdata; + + if (vchiq_put_completion(args->buf, &user_completion, ret)) { + if (ret == 0) + ret = -EFAULT; + break; + } + + /* + * Ensure that the above copy has completed + * before advancing the remove pointer. + */ + mb(); + remove++; + instance->completion_remove = remove; + } + + if (msgbufcount != args->msgbufcount) { + if (put_user(msgbufcount, msgbufcountp)) + ret = -EFAULT; + } +out: + if (ret) + complete(&instance->remove_event); + mutex_unlock(&instance->completion_mutex); + DEBUG_TRACE(AWAIT_COMPLETION_LINE); + + return ret; +} + +static long +vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg) +{ + struct vchiq_instance *instance = file->private_data; + enum vchiq_status status = VCHIQ_SUCCESS; + struct vchiq_service *service = NULL; + long ret = 0; + int i, rc; + + vchiq_log_trace(vchiq_arm_log_level, + "%s - instance %pK, cmd %s, arg %lx", __func__, instance, + ((_IOC_TYPE(cmd) == VCHIQ_IOC_MAGIC) && (_IOC_NR(cmd) <= VCHIQ_IOC_MAX)) ? + ioctl_names[_IOC_NR(cmd)] : "<invalid>", arg); + + switch (cmd) { + case VCHIQ_IOC_SHUTDOWN: + if (!instance->connected) + break; + + /* Remove all services */ + i = 0; + while ((service = next_service_by_instance(instance->state, + instance, &i))) { + status = vchiq_remove_service(instance, service->handle); + vchiq_service_put(service); + if (status != VCHIQ_SUCCESS) + break; + } + service = NULL; + + if (status == VCHIQ_SUCCESS) { + /* Wake the completion thread and ask it to exit */ + instance->closing = 1; + complete(&instance->insert_event); + } + + break; + + case VCHIQ_IOC_CONNECT: + if (instance->connected) { + ret = -EINVAL; + break; + } + rc = mutex_lock_killable(&instance->state->mutex); + if (rc) { + vchiq_log_error(vchiq_arm_log_level, + "vchiq: connect: could not lock mutex for state %d: %d", + instance->state->id, rc); + ret = -EINTR; + break; + } + status = vchiq_connect_internal(instance->state, instance); + mutex_unlock(&instance->state->mutex); + + if (status == VCHIQ_SUCCESS) + instance->connected = 1; + else + vchiq_log_error(vchiq_arm_log_level, + "vchiq: could not connect: %d", status); + break; + + case VCHIQ_IOC_CREATE_SERVICE: { + struct vchiq_create_service __user *argp; + struct vchiq_create_service args; + + argp = (void __user *)arg; + if (copy_from_user(&args, argp, sizeof(args))) { + ret = -EFAULT; + break; + } + + ret = vchiq_ioc_create_service(instance, &args); + if (ret < 0) + break; + + if (put_user(args.handle, &argp->handle)) { + vchiq_remove_service(instance, args.handle); + ret = -EFAULT; + } + } break; + + case VCHIQ_IOC_CLOSE_SERVICE: + case VCHIQ_IOC_REMOVE_SERVICE: { + unsigned int handle = (unsigned int)arg; + struct user_service *user_service; + + service = find_service_for_instance(instance, handle); + if (!service) { + ret = -EINVAL; + break; + } + + user_service = service->base.userdata; + + /* + * close_pending is false on first entry, and when the + * wait in vchiq_close_service has been interrupted. + */ + if (!user_service->close_pending) { + status = (cmd == VCHIQ_IOC_CLOSE_SERVICE) ? + vchiq_close_service(instance, service->handle) : + vchiq_remove_service(instance, service->handle); + if (status != VCHIQ_SUCCESS) + break; + } + + /* + * close_pending is true once the underlying service + * has been closed until the client library calls the + * CLOSE_DELIVERED ioctl, signalling close_event. + */ + if (user_service->close_pending && + wait_for_completion_interruptible(&user_service->close_event)) + status = VCHIQ_RETRY; + break; + } + + case VCHIQ_IOC_USE_SERVICE: + case VCHIQ_IOC_RELEASE_SERVICE: { + unsigned int handle = (unsigned int)arg; + + service = find_service_for_instance(instance, handle); + if (service) { + ret = (cmd == VCHIQ_IOC_USE_SERVICE) ? + vchiq_use_service_internal(service) : + vchiq_release_service_internal(service); + if (ret) { + vchiq_log_error(vchiq_susp_log_level, + "%s: cmd %s returned error %ld for service %c%c%c%c:%03d", + __func__, (cmd == VCHIQ_IOC_USE_SERVICE) ? + "VCHIQ_IOC_USE_SERVICE" : + "VCHIQ_IOC_RELEASE_SERVICE", + ret, + VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), + service->client_id); + } + } else { + ret = -EINVAL; + } + } break; + + case VCHIQ_IOC_QUEUE_MESSAGE: { + struct vchiq_queue_message args; + + if (copy_from_user(&args, (const void __user *)arg, + sizeof(args))) { + ret = -EFAULT; + break; + } + + service = find_service_for_instance(instance, args.handle); + + if (service && (args.count <= MAX_ELEMENTS)) { + /* Copy elements into kernel space */ + struct vchiq_element elements[MAX_ELEMENTS]; + + if (copy_from_user(elements, args.elements, + args.count * sizeof(struct vchiq_element)) == 0) + ret = vchiq_ioc_queue_message(instance, args.handle, elements, + args.count); + else + ret = -EFAULT; + } else { + ret = -EINVAL; + } + } break; + + case VCHIQ_IOC_QUEUE_BULK_TRANSMIT: + case VCHIQ_IOC_QUEUE_BULK_RECEIVE: { + struct vchiq_queue_bulk_transfer args; + struct vchiq_queue_bulk_transfer __user *argp; + + enum vchiq_bulk_dir dir = + (cmd == VCHIQ_IOC_QUEUE_BULK_TRANSMIT) ? + VCHIQ_BULK_TRANSMIT : VCHIQ_BULK_RECEIVE; + + argp = (void __user *)arg; + if (copy_from_user(&args, argp, sizeof(args))) { + ret = -EFAULT; + break; + } + + ret = vchiq_irq_queue_bulk_tx_rx(instance, &args, + dir, &argp->mode); + } break; + + case VCHIQ_IOC_AWAIT_COMPLETION: { + struct vchiq_await_completion args; + struct vchiq_await_completion __user *argp; + + argp = (void __user *)arg; + if (copy_from_user(&args, argp, sizeof(args))) { + ret = -EFAULT; + break; + } + + ret = vchiq_ioc_await_completion(instance, &args, + &argp->msgbufcount); + } break; + + case VCHIQ_IOC_DEQUEUE_MESSAGE: { + struct vchiq_dequeue_message args; + + if (copy_from_user(&args, (const void __user *)arg, + sizeof(args))) { + ret = -EFAULT; + break; + } + + ret = vchiq_ioc_dequeue_message(instance, &args); + } break; + + case VCHIQ_IOC_GET_CLIENT_ID: { + unsigned int handle = (unsigned int)arg; + + ret = vchiq_get_client_id(instance, handle); + } break; + + case VCHIQ_IOC_GET_CONFIG: { + struct vchiq_get_config args; + struct vchiq_config config; + + if (copy_from_user(&args, (const void __user *)arg, + sizeof(args))) { + ret = -EFAULT; + break; + } + if (args.config_size > sizeof(config)) { + ret = -EINVAL; + break; + } + + vchiq_get_config(&config); + if (copy_to_user(args.pconfig, &config, args.config_size)) { + ret = -EFAULT; + break; + } + } break; + + case VCHIQ_IOC_SET_SERVICE_OPTION: { + struct vchiq_set_service_option args; + + if (copy_from_user(&args, (const void __user *)arg, + sizeof(args))) { + ret = -EFAULT; + break; + } + + service = find_service_for_instance(instance, args.handle); + if (!service) { + ret = -EINVAL; + break; + } + + ret = vchiq_set_service_option(instance, args.handle, args.option, + args.value); + } break; + + case VCHIQ_IOC_LIB_VERSION: { + unsigned int lib_version = (unsigned int)arg; + + if (lib_version < VCHIQ_VERSION_MIN) + ret = -EINVAL; + else if (lib_version >= VCHIQ_VERSION_CLOSE_DELIVERED) + instance->use_close_delivered = 1; + } break; + + case VCHIQ_IOC_CLOSE_DELIVERED: { + unsigned int handle = (unsigned int)arg; + + service = find_closed_service_for_instance(instance, handle); + if (service) { + struct user_service *user_service = + (struct user_service *)service->base.userdata; + close_delivered(user_service); + } else { + ret = -EINVAL; + } + } break; + + default: + ret = -ENOTTY; + break; + } + + if (service) + vchiq_service_put(service); + + if (ret == 0) { + if (status == VCHIQ_ERROR) + ret = -EIO; + else if (status == VCHIQ_RETRY) + ret = -EINTR; + } + + if ((status == VCHIQ_SUCCESS) && (ret < 0) && (ret != -EINTR) && (ret != -EWOULDBLOCK)) + vchiq_log_info(vchiq_arm_log_level, + " ioctl instance %pK, cmd %s -> status %d, %ld", + instance, (_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ? + ioctl_names[_IOC_NR(cmd)] : "<invalid>", status, ret); + else + vchiq_log_trace(vchiq_arm_log_level, + " ioctl instance %pK, cmd %s -> status %d, %ld", + instance, (_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ? + ioctl_names[_IOC_NR(cmd)] : "<invalid>", status, ret); + + return ret; +} + +#if defined(CONFIG_COMPAT) + +struct vchiq_service_params32 { + int fourcc; + compat_uptr_t callback; + compat_uptr_t userdata; + short version; /* Increment for non-trivial changes */ + short version_min; /* Update for incompatible changes */ +}; + +struct vchiq_create_service32 { + struct vchiq_service_params32 params; + int is_open; + int is_vchi; + unsigned int handle; /* OUT */ +}; + +#define VCHIQ_IOC_CREATE_SERVICE32 \ + _IOWR(VCHIQ_IOC_MAGIC, 2, struct vchiq_create_service32) + +static long +vchiq_compat_ioctl_create_service(struct file *file, unsigned int cmd, + struct vchiq_create_service32 __user *ptrargs32) +{ + struct vchiq_create_service args; + struct vchiq_create_service32 args32; + struct vchiq_instance *instance = file->private_data; + long ret; + + if (copy_from_user(&args32, ptrargs32, sizeof(args32))) + return -EFAULT; + + args = (struct vchiq_create_service) { + .params = { + .fourcc = args32.params.fourcc, + .callback = compat_ptr(args32.params.callback), + .userdata = compat_ptr(args32.params.userdata), + .version = args32.params.version, + .version_min = args32.params.version_min, + }, + .is_open = args32.is_open, + .is_vchi = args32.is_vchi, + .handle = args32.handle, + }; + + ret = vchiq_ioc_create_service(instance, &args); + if (ret < 0) + return ret; + + if (put_user(args.handle, &ptrargs32->handle)) { + vchiq_remove_service(instance, args.handle); + return -EFAULT; + } + + return 0; +} + +struct vchiq_element32 { + compat_uptr_t data; + unsigned int size; +}; + +struct vchiq_queue_message32 { + unsigned int handle; + unsigned int count; + compat_uptr_t elements; +}; + +#define VCHIQ_IOC_QUEUE_MESSAGE32 \ + _IOW(VCHIQ_IOC_MAGIC, 4, struct vchiq_queue_message32) + +static long +vchiq_compat_ioctl_queue_message(struct file *file, + unsigned int cmd, + struct vchiq_queue_message32 __user *arg) +{ + struct vchiq_queue_message args; + struct vchiq_queue_message32 args32; + struct vchiq_service *service; + struct vchiq_instance *instance = file->private_data; + int ret; + + if (copy_from_user(&args32, arg, sizeof(args32))) + return -EFAULT; + + args = (struct vchiq_queue_message) { + .handle = args32.handle, + .count = args32.count, + .elements = compat_ptr(args32.elements), + }; + + if (args32.count > MAX_ELEMENTS) + return -EINVAL; + + service = find_service_for_instance(instance, args.handle); + if (!service) + return -EINVAL; + + if (args32.elements && args32.count) { + struct vchiq_element32 element32[MAX_ELEMENTS]; + struct vchiq_element elements[MAX_ELEMENTS]; + unsigned int count; + + if (copy_from_user(&element32, args.elements, + sizeof(element32))) { + vchiq_service_put(service); + return -EFAULT; + } + + for (count = 0; count < args32.count; count++) { + elements[count].data = + compat_ptr(element32[count].data); + elements[count].size = element32[count].size; + } + ret = vchiq_ioc_queue_message(instance, args.handle, elements, + args.count); + } else { + ret = -EINVAL; + } + vchiq_service_put(service); + + return ret; +} + +struct vchiq_queue_bulk_transfer32 { + unsigned int handle; + compat_uptr_t data; + unsigned int size; + compat_uptr_t userdata; + enum vchiq_bulk_mode mode; +}; + +#define VCHIQ_IOC_QUEUE_BULK_TRANSMIT32 \ + _IOWR(VCHIQ_IOC_MAGIC, 5, struct vchiq_queue_bulk_transfer32) +#define VCHIQ_IOC_QUEUE_BULK_RECEIVE32 \ + _IOWR(VCHIQ_IOC_MAGIC, 6, struct vchiq_queue_bulk_transfer32) + +static long +vchiq_compat_ioctl_queue_bulk(struct file *file, + unsigned int cmd, + struct vchiq_queue_bulk_transfer32 __user *argp) +{ + struct vchiq_queue_bulk_transfer32 args32; + struct vchiq_queue_bulk_transfer args; + enum vchiq_bulk_dir dir = (cmd == VCHIQ_IOC_QUEUE_BULK_TRANSMIT32) ? + VCHIQ_BULK_TRANSMIT : VCHIQ_BULK_RECEIVE; + + if (copy_from_user(&args32, argp, sizeof(args32))) + return -EFAULT; + + args = (struct vchiq_queue_bulk_transfer) { + .handle = args32.handle, + .data = compat_ptr(args32.data), + .size = args32.size, + .userdata = compat_ptr(args32.userdata), + .mode = args32.mode, + }; + + return vchiq_irq_queue_bulk_tx_rx(file->private_data, &args, + dir, &argp->mode); +} + +struct vchiq_await_completion32 { + unsigned int count; + compat_uptr_t buf; + unsigned int msgbufsize; + unsigned int msgbufcount; /* IN/OUT */ + compat_uptr_t msgbufs; +}; + +#define VCHIQ_IOC_AWAIT_COMPLETION32 \ + _IOWR(VCHIQ_IOC_MAGIC, 7, struct vchiq_await_completion32) + +static long +vchiq_compat_ioctl_await_completion(struct file *file, + unsigned int cmd, + struct vchiq_await_completion32 __user *argp) +{ + struct vchiq_await_completion args; + struct vchiq_await_completion32 args32; + + if (copy_from_user(&args32, argp, sizeof(args32))) + return -EFAULT; + + args = (struct vchiq_await_completion) { + .count = args32.count, + .buf = compat_ptr(args32.buf), + .msgbufsize = args32.msgbufsize, + .msgbufcount = args32.msgbufcount, + .msgbufs = compat_ptr(args32.msgbufs), + }; + + return vchiq_ioc_await_completion(file->private_data, &args, + &argp->msgbufcount); +} + +struct vchiq_dequeue_message32 { + unsigned int handle; + int blocking; + unsigned int bufsize; + compat_uptr_t buf; +}; + +#define VCHIQ_IOC_DEQUEUE_MESSAGE32 \ + _IOWR(VCHIQ_IOC_MAGIC, 8, struct vchiq_dequeue_message32) + +static long +vchiq_compat_ioctl_dequeue_message(struct file *file, + unsigned int cmd, + struct vchiq_dequeue_message32 __user *arg) +{ + struct vchiq_dequeue_message32 args32; + struct vchiq_dequeue_message args; + + if (copy_from_user(&args32, arg, sizeof(args32))) + return -EFAULT; + + args = (struct vchiq_dequeue_message) { + .handle = args32.handle, + .blocking = args32.blocking, + .bufsize = args32.bufsize, + .buf = compat_ptr(args32.buf), + }; + + return vchiq_ioc_dequeue_message(file->private_data, &args); +} + +struct vchiq_get_config32 { + unsigned int config_size; + compat_uptr_t pconfig; +}; + +#define VCHIQ_IOC_GET_CONFIG32 \ + _IOWR(VCHIQ_IOC_MAGIC, 10, struct vchiq_get_config32) + +static long +vchiq_compat_ioctl_get_config(struct file *file, + unsigned int cmd, + struct vchiq_get_config32 __user *arg) +{ + struct vchiq_get_config32 args32; + struct vchiq_config config; + void __user *ptr; + + if (copy_from_user(&args32, arg, sizeof(args32))) + return -EFAULT; + if (args32.config_size > sizeof(config)) + return -EINVAL; + + vchiq_get_config(&config); + ptr = compat_ptr(args32.pconfig); + if (copy_to_user(ptr, &config, args32.config_size)) + return -EFAULT; + + return 0; +} + +static long +vchiq_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg) +{ + void __user *argp = compat_ptr(arg); + + switch (cmd) { + case VCHIQ_IOC_CREATE_SERVICE32: + return vchiq_compat_ioctl_create_service(file, cmd, argp); + case VCHIQ_IOC_QUEUE_MESSAGE32: + return vchiq_compat_ioctl_queue_message(file, cmd, argp); + case VCHIQ_IOC_QUEUE_BULK_TRANSMIT32: + case VCHIQ_IOC_QUEUE_BULK_RECEIVE32: + return vchiq_compat_ioctl_queue_bulk(file, cmd, argp); + case VCHIQ_IOC_AWAIT_COMPLETION32: + return vchiq_compat_ioctl_await_completion(file, cmd, argp); + case VCHIQ_IOC_DEQUEUE_MESSAGE32: + return vchiq_compat_ioctl_dequeue_message(file, cmd, argp); + case VCHIQ_IOC_GET_CONFIG32: + return vchiq_compat_ioctl_get_config(file, cmd, argp); + default: + return vchiq_ioctl(file, cmd, (unsigned long)argp); + } +} + +#endif + +static int vchiq_open(struct inode *inode, struct file *file) +{ + struct vchiq_state *state = vchiq_get_state(); + struct vchiq_instance *instance; + + vchiq_log_info(vchiq_arm_log_level, "vchiq_open"); + + if (!state) { + vchiq_log_error(vchiq_arm_log_level, + "vchiq has no connection to VideoCore"); + return -ENOTCONN; + } + + instance = kzalloc(sizeof(*instance), GFP_KERNEL); + if (!instance) + return -ENOMEM; + + instance->state = state; + instance->pid = current->tgid; + + vchiq_debugfs_add_instance(instance); + + init_completion(&instance->insert_event); + init_completion(&instance->remove_event); + mutex_init(&instance->completion_mutex); + mutex_init(&instance->bulk_waiter_list_mutex); + INIT_LIST_HEAD(&instance->bulk_waiter_list); + + file->private_data = instance; + + return 0; +} + +static int vchiq_release(struct inode *inode, struct file *file) +{ + struct vchiq_instance *instance = file->private_data; + struct vchiq_state *state = vchiq_get_state(); + struct vchiq_service *service; + int ret = 0; + int i; + + vchiq_log_info(vchiq_arm_log_level, "%s: instance=%lx", __func__, + (unsigned long)instance); + + if (!state) { + ret = -EPERM; + goto out; + } + + /* Ensure videocore is awake to allow termination. */ + vchiq_use_internal(instance->state, NULL, USE_TYPE_VCHIQ); + + mutex_lock(&instance->completion_mutex); + + /* Wake the completion thread and ask it to exit */ + instance->closing = 1; + complete(&instance->insert_event); + + mutex_unlock(&instance->completion_mutex); + + /* Wake the slot handler if the completion queue is full. */ + complete(&instance->remove_event); + + /* Mark all services for termination... */ + i = 0; + while ((service = next_service_by_instance(state, instance, &i))) { + struct user_service *user_service = service->base.userdata; + + /* Wake the slot handler if the msg queue is full. */ + complete(&user_service->remove_event); + + vchiq_terminate_service_internal(service); + vchiq_service_put(service); + } + + /* ...and wait for them to die */ + i = 0; + while ((service = next_service_by_instance(state, instance, &i))) { + struct user_service *user_service = service->base.userdata; + + wait_for_completion(&service->remove_event); + + if (WARN_ON(service->srvstate != VCHIQ_SRVSTATE_FREE)) { + vchiq_service_put(service); + break; + } + + spin_lock(&msg_queue_spinlock); + + while (user_service->msg_remove != user_service->msg_insert) { + struct vchiq_header *header; + int m = user_service->msg_remove & (MSG_QUEUE_SIZE - 1); + + header = user_service->msg_queue[m]; + user_service->msg_remove++; + spin_unlock(&msg_queue_spinlock); + + if (header) + vchiq_release_message(instance, service->handle, header); + spin_lock(&msg_queue_spinlock); + } + + spin_unlock(&msg_queue_spinlock); + + vchiq_service_put(service); + } + + /* Release any closed services */ + while (instance->completion_remove != instance->completion_insert) { + struct vchiq_completion_data_kernel *completion; + struct vchiq_service *service; + + completion = &instance->completions[instance->completion_remove + & (MAX_COMPLETIONS - 1)]; + service = completion->service_userdata; + if (completion->reason == VCHIQ_SERVICE_CLOSED) { + struct user_service *user_service = + service->base.userdata; + + /* Wake any blocked user-thread */ + if (instance->use_close_delivered) + complete(&user_service->close_event); + vchiq_service_put(service); + } + instance->completion_remove++; + } + + /* Release the PEER service count. */ + vchiq_release_internal(instance->state, NULL); + + free_bulk_waiter(instance); + + vchiq_debugfs_remove_instance(instance); + + kfree(instance); + file->private_data = NULL; + +out: + return ret; +} + +static ssize_t +vchiq_read(struct file *file, char __user *buf, size_t count, loff_t *ppos) +{ + struct dump_context context; + int err; + + context.buf = buf; + context.actual = 0; + context.space = count; + context.offset = *ppos; + + err = vchiq_dump_state(&context, &g_state); + if (err) + return err; + + *ppos += context.actual; + + return context.actual; +} + +static const struct file_operations +vchiq_fops = { + .owner = THIS_MODULE, + .unlocked_ioctl = vchiq_ioctl, +#if defined(CONFIG_COMPAT) + .compat_ioctl = vchiq_compat_ioctl, +#endif + .open = vchiq_open, + .release = vchiq_release, + .read = vchiq_read +}; + +static struct miscdevice vchiq_miscdev = { + .fops = &vchiq_fops, + .minor = MISC_DYNAMIC_MINOR, + .name = "vchiq", + +}; + +/** + * vchiq_register_chrdev - Register the char driver for vchiq + * and create the necessary class and + * device files in userspace. + * @parent The parent of the char device. + * + * Returns 0 on success else returns the error code. + */ +int vchiq_register_chrdev(struct device *parent) +{ + vchiq_miscdev.parent = parent; + + return misc_register(&vchiq_miscdev); +} + +/** + * vchiq_deregister_chrdev - Deregister and cleanup the vchiq char + * driver and device files + */ +void vchiq_deregister_chrdev(void) +{ + misc_deregister(&vchiq_miscdev); +} diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_ioctl.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_ioctl.h new file mode 100644 index 000000000..86d77f2ee --- /dev/null +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_ioctl.h @@ -0,0 +1,112 @@ +/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */ +/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */ + +#ifndef VCHIQ_IOCTLS_H +#define VCHIQ_IOCTLS_H + +#include <linux/ioctl.h> +#include <linux/raspberrypi/vchiq.h> + +#define VCHIQ_IOC_MAGIC 0xc4 +#define VCHIQ_INVALID_HANDLE (~0) + +struct vchiq_service_params { + int fourcc; + enum vchiq_status __user (*callback)(enum vchiq_reason reason, + struct vchiq_header *header, + unsigned int handle, + void *bulk_userdata); + void __user *userdata; + short version; /* Increment for non-trivial changes */ + short version_min; /* Update for incompatible changes */ +}; + +struct vchiq_create_service { + struct vchiq_service_params params; + int is_open; + int is_vchi; + unsigned int handle; /* OUT */ +}; + +struct vchiq_queue_message { + unsigned int handle; + unsigned int count; + const struct vchiq_element __user *elements; +}; + +struct vchiq_queue_bulk_transfer { + unsigned int handle; + void __user *data; + unsigned int size; + void __user *userdata; + enum vchiq_bulk_mode mode; +}; + +struct vchiq_completion_data { + enum vchiq_reason reason; + struct vchiq_header __user *header; + void __user *service_userdata; + void __user *bulk_userdata; +}; + +struct vchiq_await_completion { + unsigned int count; + struct vchiq_completion_data __user *buf; + unsigned int msgbufsize; + unsigned int msgbufcount; /* IN/OUT */ + void * __user *msgbufs; +}; + +struct vchiq_dequeue_message { + unsigned int handle; + int blocking; + unsigned int bufsize; + void __user *buf; +}; + +struct vchiq_get_config { + unsigned int config_size; + struct vchiq_config __user *pconfig; +}; + +struct vchiq_set_service_option { + unsigned int handle; + enum vchiq_service_option option; + int value; +}; + +struct vchiq_dump_mem { + void __user *virt_addr; + size_t num_bytes; +}; + +#define VCHIQ_IOC_CONNECT _IO(VCHIQ_IOC_MAGIC, 0) +#define VCHIQ_IOC_SHUTDOWN _IO(VCHIQ_IOC_MAGIC, 1) +#define VCHIQ_IOC_CREATE_SERVICE \ + _IOWR(VCHIQ_IOC_MAGIC, 2, struct vchiq_create_service) +#define VCHIQ_IOC_REMOVE_SERVICE _IO(VCHIQ_IOC_MAGIC, 3) +#define VCHIQ_IOC_QUEUE_MESSAGE \ + _IOW(VCHIQ_IOC_MAGIC, 4, struct vchiq_queue_message) +#define VCHIQ_IOC_QUEUE_BULK_TRANSMIT \ + _IOWR(VCHIQ_IOC_MAGIC, 5, struct vchiq_queue_bulk_transfer) +#define VCHIQ_IOC_QUEUE_BULK_RECEIVE \ + _IOWR(VCHIQ_IOC_MAGIC, 6, struct vchiq_queue_bulk_transfer) +#define VCHIQ_IOC_AWAIT_COMPLETION \ + _IOWR(VCHIQ_IOC_MAGIC, 7, struct vchiq_await_completion) +#define VCHIQ_IOC_DEQUEUE_MESSAGE \ + _IOWR(VCHIQ_IOC_MAGIC, 8, struct vchiq_dequeue_message) +#define VCHIQ_IOC_GET_CLIENT_ID _IO(VCHIQ_IOC_MAGIC, 9) +#define VCHIQ_IOC_GET_CONFIG \ + _IOWR(VCHIQ_IOC_MAGIC, 10, struct vchiq_get_config) +#define VCHIQ_IOC_CLOSE_SERVICE _IO(VCHIQ_IOC_MAGIC, 11) +#define VCHIQ_IOC_USE_SERVICE _IO(VCHIQ_IOC_MAGIC, 12) +#define VCHIQ_IOC_RELEASE_SERVICE _IO(VCHIQ_IOC_MAGIC, 13) +#define VCHIQ_IOC_SET_SERVICE_OPTION \ + _IOW(VCHIQ_IOC_MAGIC, 14, struct vchiq_set_service_option) +#define VCHIQ_IOC_DUMP_PHYS_MEM \ + _IOW(VCHIQ_IOC_MAGIC, 15, struct vchiq_dump_mem) +#define VCHIQ_IOC_LIB_VERSION _IO(VCHIQ_IOC_MAGIC, 16) +#define VCHIQ_IOC_CLOSE_DELIVERED _IO(VCHIQ_IOC_MAGIC, 17) +#define VCHIQ_IOC_MAX 17 + +#endif diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_pagelist.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_pagelist.h new file mode 100644 index 000000000..ebd12bfab --- /dev/null +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_pagelist.h @@ -0,0 +1,21 @@ +/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */ +/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */ + +#ifndef VCHIQ_PAGELIST_H +#define VCHIQ_PAGELIST_H + +#define PAGELIST_WRITE 0 +#define PAGELIST_READ 1 +#define PAGELIST_READ_WITH_FRAGMENTS 2 + +struct pagelist { + u32 length; + u16 type; + u16 offset; + u32 addrs[1]; /* N.B. 12 LSBs hold the number + * of following pages at consecutive + * addresses. + */ +}; + +#endif /* VCHIQ_PAGELIST_H */ diff --git a/drivers/staging/vc04_services/vchiq-mmal/Kconfig b/drivers/staging/vc04_services/vchiq-mmal/Kconfig new file mode 100644 index 000000000..c99525a0b --- /dev/null +++ b/drivers/staging/vc04_services/vchiq-mmal/Kconfig @@ -0,0 +1,7 @@ +config BCM2835_VCHIQ_MMAL + tristate "BCM2835 MMAL VCHIQ service" + depends on BCM2835_VCHIQ + help + Enables the MMAL API over VCHIQ interface as used for the + majority of the multimedia services on VideoCore. + Defaults to Y when the Broadcomd BCM2835 camera host is selected. diff --git a/drivers/staging/vc04_services/vchiq-mmal/Makefile b/drivers/staging/vc04_services/vchiq-mmal/Makefile new file mode 100644 index 000000000..b2a830f48 --- /dev/null +++ b/drivers/staging/vc04_services/vchiq-mmal/Makefile @@ -0,0 +1,9 @@ +# SPDX-License-Identifier: GPL-2.0 +bcm2835-mmal-vchiq-objs := mmal-vchiq.o + +obj-$(CONFIG_BCM2835_VCHIQ_MMAL) += bcm2835-mmal-vchiq.o + +ccflags-y += \ + -I$(srctree)/$(src)/.. \ + -I$(srctree)/$(src)/../include \ + -D__VCCOREVER__=0x04000000 diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-common.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-common.h new file mode 100644 index 000000000..b33129403 --- /dev/null +++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-common.h @@ -0,0 +1,65 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * Broadcom BCM2835 V4L2 driver + * + * Copyright © 2013 Raspberry Pi (Trading) Ltd. + * + * Authors: Vincent Sanders @ Collabora + * Dave Stevenson @ Broadcom + * (now dave.stevenson@raspberrypi.org) + * Simon Mellor @ Broadcom + * Luke Diamand @ Broadcom + * + * MMAL structures + * + */ +#ifndef MMAL_COMMON_H +#define MMAL_COMMON_H + +#define MMAL_FOURCC(a, b, c, d) ((a) | (b << 8) | (c << 16) | (d << 24)) +#define MMAL_MAGIC MMAL_FOURCC('m', 'm', 'a', 'l') + +/** Special value signalling that time is not known */ +#define MMAL_TIME_UNKNOWN BIT_ULL(63) + +struct mmal_msg_context; + +/* mapping between v4l and mmal video modes */ +struct mmal_fmt { + u32 fourcc; /* v4l2 format id */ + int flags; /* v4l2 flags field */ + u32 mmal; + int depth; + u32 mmal_component; /* MMAL component index to be used to encode */ + u32 ybbp; /* depth of first Y plane for planar formats */ + bool remove_padding; /* Does the GPU have to remove padding, + * or can we do hide padding via bytesperline. + */ +}; + +/* buffer for one video frame */ +struct mmal_buffer { + /* v4l buffer data -- must be first */ + struct vb2_v4l2_buffer vb; + + /* list of buffers available */ + struct list_head list; + + void *buffer; /* buffer pointer */ + unsigned long buffer_size; /* size of allocated buffer */ + + struct mmal_msg_context *msg_context; + + unsigned long length; + u32 mmal_flags; + s64 dts; + s64 pts; +}; + +/* */ +struct mmal_colourfx { + s32 enable; + u32 u; + u32 v; +}; +#endif diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-encodings.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-encodings.h new file mode 100644 index 000000000..e15ae7b24 --- /dev/null +++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-encodings.h @@ -0,0 +1,124 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * Broadcom BCM2835 V4L2 driver + * + * Copyright © 2013 Raspberry Pi (Trading) Ltd. + * + * Authors: Vincent Sanders @ Collabora + * Dave Stevenson @ Broadcom + * (now dave.stevenson@raspberrypi.org) + * Simon Mellor @ Broadcom + * Luke Diamand @ Broadcom + */ +#ifndef MMAL_ENCODINGS_H +#define MMAL_ENCODINGS_H + +#define MMAL_ENCODING_H264 MMAL_FOURCC('H', '2', '6', '4') +#define MMAL_ENCODING_H263 MMAL_FOURCC('H', '2', '6', '3') +#define MMAL_ENCODING_MP4V MMAL_FOURCC('M', 'P', '4', 'V') +#define MMAL_ENCODING_MP2V MMAL_FOURCC('M', 'P', '2', 'V') +#define MMAL_ENCODING_MP1V MMAL_FOURCC('M', 'P', '1', 'V') +#define MMAL_ENCODING_WMV3 MMAL_FOURCC('W', 'M', 'V', '3') +#define MMAL_ENCODING_WMV2 MMAL_FOURCC('W', 'M', 'V', '2') +#define MMAL_ENCODING_WMV1 MMAL_FOURCC('W', 'M', 'V', '1') +#define MMAL_ENCODING_WVC1 MMAL_FOURCC('W', 'V', 'C', '1') +#define MMAL_ENCODING_VP8 MMAL_FOURCC('V', 'P', '8', ' ') +#define MMAL_ENCODING_VP7 MMAL_FOURCC('V', 'P', '7', ' ') +#define MMAL_ENCODING_VP6 MMAL_FOURCC('V', 'P', '6', ' ') +#define MMAL_ENCODING_THEORA MMAL_FOURCC('T', 'H', 'E', 'O') +#define MMAL_ENCODING_SPARK MMAL_FOURCC('S', 'P', 'R', 'K') +#define MMAL_ENCODING_MJPEG MMAL_FOURCC('M', 'J', 'P', 'G') + +#define MMAL_ENCODING_JPEG MMAL_FOURCC('J', 'P', 'E', 'G') +#define MMAL_ENCODING_GIF MMAL_FOURCC('G', 'I', 'F', ' ') +#define MMAL_ENCODING_PNG MMAL_FOURCC('P', 'N', 'G', ' ') +#define MMAL_ENCODING_PPM MMAL_FOURCC('P', 'P', 'M', ' ') +#define MMAL_ENCODING_TGA MMAL_FOURCC('T', 'G', 'A', ' ') +#define MMAL_ENCODING_BMP MMAL_FOURCC('B', 'M', 'P', ' ') + +#define MMAL_ENCODING_I420 MMAL_FOURCC('I', '4', '2', '0') +#define MMAL_ENCODING_I420_SLICE MMAL_FOURCC('S', '4', '2', '0') +#define MMAL_ENCODING_YV12 MMAL_FOURCC('Y', 'V', '1', '2') +#define MMAL_ENCODING_I422 MMAL_FOURCC('I', '4', '2', '2') +#define MMAL_ENCODING_I422_SLICE MMAL_FOURCC('S', '4', '2', '2') +#define MMAL_ENCODING_YUYV MMAL_FOURCC('Y', 'U', 'Y', 'V') +#define MMAL_ENCODING_YVYU MMAL_FOURCC('Y', 'V', 'Y', 'U') +#define MMAL_ENCODING_UYVY MMAL_FOURCC('U', 'Y', 'V', 'Y') +#define MMAL_ENCODING_VYUY MMAL_FOURCC('V', 'Y', 'U', 'Y') +#define MMAL_ENCODING_NV12 MMAL_FOURCC('N', 'V', '1', '2') +#define MMAL_ENCODING_NV21 MMAL_FOURCC('N', 'V', '2', '1') +#define MMAL_ENCODING_ARGB MMAL_FOURCC('A', 'R', 'G', 'B') +#define MMAL_ENCODING_RGBA MMAL_FOURCC('R', 'G', 'B', 'A') +#define MMAL_ENCODING_ABGR MMAL_FOURCC('A', 'B', 'G', 'R') +#define MMAL_ENCODING_BGRA MMAL_FOURCC('B', 'G', 'R', 'A') +#define MMAL_ENCODING_RGB16 MMAL_FOURCC('R', 'G', 'B', '2') +#define MMAL_ENCODING_RGB24 MMAL_FOURCC('R', 'G', 'B', '3') +#define MMAL_ENCODING_RGB32 MMAL_FOURCC('R', 'G', 'B', '4') +#define MMAL_ENCODING_BGR16 MMAL_FOURCC('B', 'G', 'R', '2') +#define MMAL_ENCODING_BGR24 MMAL_FOURCC('B', 'G', 'R', '3') +#define MMAL_ENCODING_BGR32 MMAL_FOURCC('B', 'G', 'R', '4') + +/** SAND Video (YUVUV128) format, native format understood by VideoCore. + * This format is *not* opaque - if requested you will receive full frames + * of YUV_UV video. + */ +#define MMAL_ENCODING_YUVUV128 MMAL_FOURCC('S', 'A', 'N', 'D') + +/** VideoCore opaque image format, image handles are returned to + * the host but not the actual image data. + */ +#define MMAL_ENCODING_OPAQUE MMAL_FOURCC('O', 'P', 'Q', 'V') + +/** An EGL image handle + */ +#define MMAL_ENCODING_EGL_IMAGE MMAL_FOURCC('E', 'G', 'L', 'I') + +/* }@ */ + +/** \name Pre-defined audio encodings */ +/* @{ */ +#define MMAL_ENCODING_PCM_UNSIGNED_BE MMAL_FOURCC('P', 'C', 'M', 'U') +#define MMAL_ENCODING_PCM_UNSIGNED_LE MMAL_FOURCC('p', 'c', 'm', 'u') +#define MMAL_ENCODING_PCM_SIGNED_BE MMAL_FOURCC('P', 'C', 'M', 'S') +#define MMAL_ENCODING_PCM_SIGNED_LE MMAL_FOURCC('p', 'c', 'm', 's') +#define MMAL_ENCODING_PCM_FLOAT_BE MMAL_FOURCC('P', 'C', 'M', 'F') +#define MMAL_ENCODING_PCM_FLOAT_LE MMAL_FOURCC('p', 'c', 'm', 'f') + +/* Pre-defined H264 encoding variants */ + +/** ISO 14496-10 Annex B byte stream format */ +#define MMAL_ENCODING_VARIANT_H264_DEFAULT 0 +/** ISO 14496-15 AVC stream format */ +#define MMAL_ENCODING_VARIANT_H264_AVC1 MMAL_FOURCC('A', 'V', 'C', '1') +/** Implicitly delineated NAL units without emulation prevention */ +#define MMAL_ENCODING_VARIANT_H264_RAW MMAL_FOURCC('R', 'A', 'W', ' ') + +/** \defgroup MmalColorSpace List of pre-defined video color spaces + * This defines a list of common color spaces. This list isn't exhaustive and + * is only provided as a convenience to avoid clients having to use FourCC + * codes directly. However components are allowed to define and use their own + * FourCC codes. + */ +/* @{ */ + +/** Unknown color space */ +#define MMAL_COLOR_SPACE_UNKNOWN 0 +/** ITU-R BT.601-5 [SDTV] */ +#define MMAL_COLOR_SPACE_ITUR_BT601 MMAL_FOURCC('Y', '6', '0', '1') +/** ITU-R BT.709-3 [HDTV] */ +#define MMAL_COLOR_SPACE_ITUR_BT709 MMAL_FOURCC('Y', '7', '0', '9') +/** JPEG JFIF */ +#define MMAL_COLOR_SPACE_JPEG_JFIF MMAL_FOURCC('Y', 'J', 'F', 'I') +/** Title 47 Code of Federal Regulations (2003) 73.682 (a) (20) */ +#define MMAL_COLOR_SPACE_FCC MMAL_FOURCC('Y', 'F', 'C', 'C') +/** Society of Motion Picture and Television Engineers 240M (1999) */ +#define MMAL_COLOR_SPACE_SMPTE240M MMAL_FOURCC('Y', '2', '4', '0') +/** ITU-R BT.470-2 System M */ +#define MMAL_COLOR_SPACE_BT470_2_M MMAL_FOURCC('Y', '_', '_', 'M') +/** ITU-R BT.470-2 System BG */ +#define MMAL_COLOR_SPACE_BT470_2_BG MMAL_FOURCC('Y', '_', 'B', 'G') +/** JPEG JFIF, but with 16..255 luma */ +#define MMAL_COLOR_SPACE_JFIF_Y16_255 MMAL_FOURCC('Y', 'Y', '1', '6') +/* @} MmalColorSpace List */ + +#endif /* MMAL_ENCODINGS_H */ diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-common.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-common.h new file mode 100644 index 000000000..492d4c5dc --- /dev/null +++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-common.h @@ -0,0 +1,45 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * Broadcom BCM2835 V4L2 driver + * + * Copyright © 2013 Raspberry Pi (Trading) Ltd. + * + * Authors: Vincent Sanders @ Collabora + * Dave Stevenson @ Broadcom + * (now dave.stevenson@raspberrypi.org) + * Simon Mellor @ Broadcom + * Luke Diamand @ Broadcom + */ + +#ifndef MMAL_MSG_COMMON_H +#define MMAL_MSG_COMMON_H + +#include <linux/types.h> + +enum mmal_msg_status { + MMAL_MSG_STATUS_SUCCESS = 0, /**< Success */ + MMAL_MSG_STATUS_ENOMEM, /**< Out of memory */ + MMAL_MSG_STATUS_ENOSPC, /**< Out of resources other than memory */ + MMAL_MSG_STATUS_EINVAL, /**< Argument is invalid */ + MMAL_MSG_STATUS_ENOSYS, /**< Function not implemented */ + MMAL_MSG_STATUS_ENOENT, /**< No such file or directory */ + MMAL_MSG_STATUS_ENXIO, /**< No such device or address */ + MMAL_MSG_STATUS_EIO, /**< I/O error */ + MMAL_MSG_STATUS_ESPIPE, /**< Illegal seek */ + MMAL_MSG_STATUS_ECORRUPT, /**< Data is corrupt \attention */ + MMAL_MSG_STATUS_ENOTREADY, /**< Component is not ready */ + MMAL_MSG_STATUS_ECONFIG, /**< Component is not configured */ + MMAL_MSG_STATUS_EISCONN, /**< Port is already connected */ + MMAL_MSG_STATUS_ENOTCONN, /**< Port is disconnected */ + MMAL_MSG_STATUS_EAGAIN, /**< Resource temporarily unavailable. */ + MMAL_MSG_STATUS_EFAULT, /**< Bad address */ +}; + +struct mmal_rect { + s32 x; /**< x coordinate (from left) */ + s32 y; /**< y coordinate (from top) */ + s32 width; /**< width */ + s32 height; /**< height */ +}; + +#endif /* MMAL_MSG_COMMON_H */ diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-format.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-format.h new file mode 100644 index 000000000..5569876d8 --- /dev/null +++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-format.h @@ -0,0 +1,108 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * Broadcom BCM2835 V4L2 driver + * + * Copyright © 2013 Raspberry Pi (Trading) Ltd. + * + * Authors: Vincent Sanders @ Collabora + * Dave Stevenson @ Broadcom + * (now dave.stevenson@raspberrypi.org) + * Simon Mellor @ Broadcom + * Luke Diamand @ Broadcom + */ + +#ifndef MMAL_MSG_FORMAT_H +#define MMAL_MSG_FORMAT_H + +#include <linux/math.h> + +#include "mmal-msg-common.h" + +/* MMAL_ES_FORMAT_T */ + +struct mmal_audio_format { + u32 channels; /* Number of audio channels */ + u32 sample_rate; /* Sample rate */ + + u32 bits_per_sample; /* Bits per sample */ + u32 block_align; /* Size of a block of data */ +}; + +struct mmal_video_format { + u32 width; /* Width of frame in pixels */ + u32 height; /* Height of frame in rows of pixels */ + struct mmal_rect crop; /* Visible region of the frame */ + struct s32_fract frame_rate; /* Frame rate */ + struct s32_fract par; /* Pixel aspect ratio */ + + /* + * FourCC specifying the color space of the video stream. See the + * MmalColorSpace "pre-defined color spaces" for some examples. + */ + u32 color_space; +}; + +struct mmal_subpicture_format { + u32 x_offset; + u32 y_offset; +}; + +union mmal_es_specific_format { + struct mmal_audio_format audio; + struct mmal_video_format video; + struct mmal_subpicture_format subpicture; +}; + +/* Definition of an elementary stream format (MMAL_ES_FORMAT_T) */ +struct mmal_es_format_local { + u32 type; /* enum mmal_es_type */ + + u32 encoding; /* FourCC specifying encoding of the elementary + * stream. + */ + u32 encoding_variant; /* FourCC specifying the specific + * encoding variant of the elementary + * stream. + */ + + union mmal_es_specific_format *es; /* Type specific + * information for the + * elementary stream + */ + + u32 bitrate; /* Bitrate in bits per second */ + u32 flags; /* Flags describing properties of the elementary + * stream. + */ + + u32 extradata_size; /* Size of the codec specific data */ + u8 *extradata; /* Codec specific data */ +}; + +/* Remote definition of an elementary stream format (MMAL_ES_FORMAT_T) */ +struct mmal_es_format { + u32 type; /* enum mmal_es_type */ + + u32 encoding; /* FourCC specifying encoding of the elementary + * stream. + */ + u32 encoding_variant; /* FourCC specifying the specific + * encoding variant of the elementary + * stream. + */ + + u32 es; /* Type specific + * information for the + * elementary stream + */ + + u32 bitrate; /* Bitrate in bits per second */ + u32 flags; /* Flags describing properties of the elementary + * stream. + */ + + u32 extradata_size; /* Size of the codec specific data */ + u32 extradata; /* Codec specific data */ +}; + +#endif /* MMAL_MSG_FORMAT_H */ diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-port.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-port.h new file mode 100644 index 000000000..6ee4c1ed7 --- /dev/null +++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-port.h @@ -0,0 +1,109 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * Broadcom BCM2835 V4L2 driver + * + * Copyright © 2013 Raspberry Pi (Trading) Ltd. + * + * Authors: Vincent Sanders @ Collabora + * Dave Stevenson @ Broadcom + * (now dave.stevenson@raspberrypi.org) + * Simon Mellor @ Broadcom + * Luke Diamand @ Broadcom + */ + +/* MMAL_PORT_TYPE_T */ +enum mmal_port_type { + MMAL_PORT_TYPE_UNKNOWN = 0, /* Unknown port type */ + MMAL_PORT_TYPE_CONTROL, /* Control port */ + MMAL_PORT_TYPE_INPUT, /* Input port */ + MMAL_PORT_TYPE_OUTPUT, /* Output port */ + MMAL_PORT_TYPE_CLOCK, /* Clock port */ +}; + +/* The port is pass-through and doesn't need buffer headers allocated */ +#define MMAL_PORT_CAPABILITY_PASSTHROUGH 0x01 +/* + *The port wants to allocate the buffer payloads. + * This signals a preference that payload allocation should be done + * on this port for efficiency reasons. + */ +#define MMAL_PORT_CAPABILITY_ALLOCATION 0x02 +/* + * The port supports format change events. + * This applies to input ports and is used to let the client know + * whether the port supports being reconfigured via a format + * change event (i.e. without having to disable the port). + */ +#define MMAL_PORT_CAPABILITY_SUPPORTS_EVENT_FORMAT_CHANGE 0x04 + +/* + * mmal port structure (MMAL_PORT_T) + * + * most elements are informational only, the pointer values for + * interogation messages are generally provided as additional + * structures within the message. When used to set values only the + * buffer_num, buffer_size and userdata parameters are writable. + */ +struct mmal_port { + u32 priv; /* Private member used by the framework */ + u32 name; /* Port name. Used for debugging purposes (RO) */ + + u32 type; /* Type of the port (RO) enum mmal_port_type */ + u16 index; /* Index of the port in its type list (RO) */ + u16 index_all; /* Index of the port in the list of all ports (RO) */ + + u32 is_enabled; /* Indicates whether the port is enabled or not (RO) */ + u32 format; /* Format of the elementary stream */ + + u32 buffer_num_min; /* Minimum number of buffers the port + * requires (RO). This is set by the + * component. + */ + + u32 buffer_size_min; /* Minimum size of buffers the port + * requires (RO). This is set by the + * component. + */ + + u32 buffer_alignment_min;/* Minimum alignment requirement for + * the buffers (RO). A value of + * zero means no special alignment + * requirements. This is set by the + * component. + */ + + u32 buffer_num_recommended; /* Number of buffers the port + * recommends for optimal + * performance (RO). A value of + * zero means no special + * recommendation. This is set + * by the component. + */ + + u32 buffer_size_recommended; /* Size of buffers the port + * recommends for optimal + * performance (RO). A value of + * zero means no special + * recommendation. This is set + * by the component. + */ + + u32 buffer_num; /* Actual number of buffers the port will use. + * This is set by the client. + */ + + u32 buffer_size; /* Actual maximum size of the buffers that + * will be sent to the port. This is set by + * the client. + */ + + u32 component; /* Component this port belongs to (Read Only) */ + + u32 userdata; /* Field reserved for use by the client */ + + u32 capabilities; /* Flags describing the capabilities of a + * port (RO). Bitwise combination of \ref + * portcapabilities "Port capabilities" + * values. + */ +}; diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg.h new file mode 100644 index 000000000..471413248 --- /dev/null +++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg.h @@ -0,0 +1,406 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * Broadcom BCM2835 V4L2 driver + * + * Copyright © 2013 Raspberry Pi (Trading) Ltd. + * + * Authors: Vincent Sanders @ Collabora + * Dave Stevenson @ Broadcom + * (now dave.stevenson@raspberrypi.org) + * Simon Mellor @ Broadcom + * Luke Diamand @ Broadcom + */ + +/* + * all the data structures which serialise the MMAL protocol. note + * these are directly mapped onto the recived message data. + * + * BEWARE: They seem to *assume* pointers are u32 and that there is no + * structure padding! + * + * NOTE: this implementation uses kernel types to ensure sizes. Rather + * than assigning values to enums to force their size the + * implementation uses fixed size types and not the enums (though the + * comments have the actual enum type + */ +#ifndef MMAL_MSG_H +#define MMAL_MSG_H + +#define VC_MMAL_VER 15 +#define VC_MMAL_MIN_VER 10 + +/* max total message size is 512 bytes */ +#define MMAL_MSG_MAX_SIZE 512 +/* with six 32bit header elements max payload is therefore 488 bytes */ +#define MMAL_MSG_MAX_PAYLOAD 488 + +#include "mmal-msg-common.h" +#include "mmal-msg-format.h" +#include "mmal-msg-port.h" +#include "mmal-vchiq.h" + +enum mmal_msg_type { + MMAL_MSG_TYPE_QUIT = 1, + MMAL_MSG_TYPE_SERVICE_CLOSED, + MMAL_MSG_TYPE_GET_VERSION, + MMAL_MSG_TYPE_COMPONENT_CREATE, + MMAL_MSG_TYPE_COMPONENT_DESTROY, /* 5 */ + MMAL_MSG_TYPE_COMPONENT_ENABLE, + MMAL_MSG_TYPE_COMPONENT_DISABLE, + MMAL_MSG_TYPE_PORT_INFO_GET, + MMAL_MSG_TYPE_PORT_INFO_SET, + MMAL_MSG_TYPE_PORT_ACTION, /* 10 */ + MMAL_MSG_TYPE_BUFFER_FROM_HOST, + MMAL_MSG_TYPE_BUFFER_TO_HOST, + MMAL_MSG_TYPE_GET_STATS, + MMAL_MSG_TYPE_PORT_PARAMETER_SET, + MMAL_MSG_TYPE_PORT_PARAMETER_GET, /* 15 */ + MMAL_MSG_TYPE_EVENT_TO_HOST, + MMAL_MSG_TYPE_GET_CORE_STATS_FOR_PORT, + MMAL_MSG_TYPE_OPAQUE_ALLOCATOR, + MMAL_MSG_TYPE_CONSUME_MEM, + MMAL_MSG_TYPE_LMK, /* 20 */ + MMAL_MSG_TYPE_OPAQUE_ALLOCATOR_DESC, + MMAL_MSG_TYPE_DRM_GET_LHS32, + MMAL_MSG_TYPE_DRM_GET_TIME, + MMAL_MSG_TYPE_BUFFER_FROM_HOST_ZEROLEN, + MMAL_MSG_TYPE_PORT_FLUSH, /* 25 */ + MMAL_MSG_TYPE_HOST_LOG, + MMAL_MSG_TYPE_MSG_LAST +}; + +/* port action request messages differ depending on the action type */ +enum mmal_msg_port_action_type { + MMAL_MSG_PORT_ACTION_TYPE_UNKNOWN = 0, /* Unknown action */ + MMAL_MSG_PORT_ACTION_TYPE_ENABLE, /* Enable a port */ + MMAL_MSG_PORT_ACTION_TYPE_DISABLE, /* Disable a port */ + MMAL_MSG_PORT_ACTION_TYPE_FLUSH, /* Flush a port */ + MMAL_MSG_PORT_ACTION_TYPE_CONNECT, /* Connect ports */ + MMAL_MSG_PORT_ACTION_TYPE_DISCONNECT, /* Disconnect ports */ + MMAL_MSG_PORT_ACTION_TYPE_SET_REQUIREMENTS, /* Set buffer requirements*/ +}; + +struct mmal_msg_header { + u32 magic; + u32 type; /* enum mmal_msg_type */ + + /* Opaque handle to the control service */ + u32 control_service; + + u32 context; /* a u32 per message context */ + u32 status; /* The status of the vchiq operation */ + u32 padding; +}; + +/* Send from VC to host to report version */ +struct mmal_msg_version { + u32 flags; + u32 major; + u32 minor; + u32 minimum; +}; + +/* request to VC to create component */ +struct mmal_msg_component_create { + u32 client_component; /* component context */ + char name[128]; + u32 pid; /* For debug */ +}; + +/* reply from VC to component creation request */ +struct mmal_msg_component_create_reply { + u32 status; /* enum mmal_msg_status - how does this differ to + * the one in the header? + */ + u32 component_handle; /* VideoCore handle for component */ + u32 input_num; /* Number of input ports */ + u32 output_num; /* Number of output ports */ + u32 clock_num; /* Number of clock ports */ +}; + +/* request to VC to destroy a component */ +struct mmal_msg_component_destroy { + u32 component_handle; +}; + +struct mmal_msg_component_destroy_reply { + u32 status; /* The component destruction status */ +}; + +/* request and reply to VC to enable a component */ +struct mmal_msg_component_enable { + u32 component_handle; +}; + +struct mmal_msg_component_enable_reply { + u32 status; /* The component enable status */ +}; + +/* request and reply to VC to disable a component */ +struct mmal_msg_component_disable { + u32 component_handle; +}; + +struct mmal_msg_component_disable_reply { + u32 status; /* The component disable status */ +}; + +/* request to VC to get port information */ +struct mmal_msg_port_info_get { + u32 component_handle; /* component handle port is associated with */ + u32 port_type; /* enum mmal_msg_port_type */ + u32 index; /* port index to query */ +}; + +/* reply from VC to get port info request */ +struct mmal_msg_port_info_get_reply { + u32 status; /* enum mmal_msg_status */ + u32 component_handle; /* component handle port is associated with */ + u32 port_type; /* enum mmal_msg_port_type */ + u32 port_index; /* port indexed in query */ + s32 found; /* unused */ + u32 port_handle; /* Handle to use for this port */ + struct mmal_port port; + struct mmal_es_format format; /* elementary stream format */ + union mmal_es_specific_format es; /* es type specific data */ + u8 extradata[MMAL_FORMAT_EXTRADATA_MAX_SIZE]; /* es extra data */ +}; + +/* request to VC to set port information */ +struct mmal_msg_port_info_set { + u32 component_handle; + u32 port_type; /* enum mmal_msg_port_type */ + u32 port_index; /* port indexed in query */ + struct mmal_port port; + struct mmal_es_format format; + union mmal_es_specific_format es; + u8 extradata[MMAL_FORMAT_EXTRADATA_MAX_SIZE]; +}; + +/* reply from VC to port info set request */ +struct mmal_msg_port_info_set_reply { + u32 status; + u32 component_handle; /* component handle port is associated with */ + u32 port_type; /* enum mmal_msg_port_type */ + u32 index; /* port indexed in query */ + s32 found; /* unused */ + u32 port_handle; /* Handle to use for this port */ + struct mmal_port port; + struct mmal_es_format format; + union mmal_es_specific_format es; + u8 extradata[MMAL_FORMAT_EXTRADATA_MAX_SIZE]; +}; + +/* port action requests that take a mmal_port as a parameter */ +struct mmal_msg_port_action_port { + u32 component_handle; + u32 port_handle; + u32 action; /* enum mmal_msg_port_action_type */ + struct mmal_port port; +}; + +/* port action requests that take handles as a parameter */ +struct mmal_msg_port_action_handle { + u32 component_handle; + u32 port_handle; + u32 action; /* enum mmal_msg_port_action_type */ + u32 connect_component_handle; + u32 connect_port_handle; +}; + +struct mmal_msg_port_action_reply { + u32 status; /* The port action operation status */ +}; + +/* MMAL buffer transfer */ + +/* Size of space reserved in a buffer message for short messages. */ +#define MMAL_VC_SHORT_DATA 128 + +/* Signals that the current payload is the end of the stream of data */ +#define MMAL_BUFFER_HEADER_FLAG_EOS BIT(0) +/* Signals that the start of the current payload starts a frame */ +#define MMAL_BUFFER_HEADER_FLAG_FRAME_START BIT(1) +/* Signals that the end of the current payload ends a frame */ +#define MMAL_BUFFER_HEADER_FLAG_FRAME_END BIT(2) +/* Signals that the current payload contains only complete frames (>1) */ +#define MMAL_BUFFER_HEADER_FLAG_FRAME \ + (MMAL_BUFFER_HEADER_FLAG_FRAME_START | \ + MMAL_BUFFER_HEADER_FLAG_FRAME_END) +/* Signals that the current payload is a keyframe (i.e. self decodable) */ +#define MMAL_BUFFER_HEADER_FLAG_KEYFRAME BIT(3) +/* + * Signals a discontinuity in the stream of data (e.g. after a seek). + * Can be used for instance by a decoder to reset its state + */ +#define MMAL_BUFFER_HEADER_FLAG_DISCONTINUITY BIT(4) +/* + * Signals a buffer containing some kind of config data for the component + * (e.g. codec config data) + */ +#define MMAL_BUFFER_HEADER_FLAG_CONFIG BIT(5) +/* Signals an encrypted payload */ +#define MMAL_BUFFER_HEADER_FLAG_ENCRYPTED BIT(6) +/* Signals a buffer containing side information */ +#define MMAL_BUFFER_HEADER_FLAG_CODECSIDEINFO BIT(7) +/* + * Signals a buffer which is the snapshot/postview image from a stills + * capture + */ +#define MMAL_BUFFER_HEADER_FLAGS_SNAPSHOT BIT(8) +/* Signals a buffer which contains data known to be corrupted */ +#define MMAL_BUFFER_HEADER_FLAG_CORRUPTED BIT(9) +/* Signals that a buffer failed to be transmitted */ +#define MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED BIT(10) + +struct mmal_driver_buffer { + u32 magic; + u32 component_handle; + u32 port_handle; + u32 client_context; +}; + +/* buffer header */ +struct mmal_buffer_header { + u32 next; /* next header */ + u32 priv; /* framework private data */ + u32 cmd; + u32 data; + u32 alloc_size; + u32 length; + u32 offset; + u32 flags; + s64 pts; + s64 dts; + u32 type; + u32 user_data; +}; + +struct mmal_buffer_header_type_specific { + union { + struct { + u32 planes; + u32 offset[4]; + u32 pitch[4]; + u32 flags; + } video; + } u; +}; + +struct mmal_msg_buffer_from_host { + /* + *The front 32 bytes of the buffer header are copied + * back to us in the reply to allow for context. This + * area is used to store two mmal_driver_buffer structures to + * allow for multiple concurrent service users. + */ + /* control data */ + struct mmal_driver_buffer drvbuf; + + /* referenced control data for passthrough buffer management */ + struct mmal_driver_buffer drvbuf_ref; + struct mmal_buffer_header buffer_header; /* buffer header itself */ + struct mmal_buffer_header_type_specific buffer_header_type_specific; + s32 is_zero_copy; + s32 has_reference; + + /* allows short data to be xfered in control message */ + u32 payload_in_message; + u8 short_data[MMAL_VC_SHORT_DATA]; +}; + +/* port parameter setting */ + +#define MMAL_WORKER_PORT_PARAMETER_SPACE 96 + +struct mmal_msg_port_parameter_set { + u32 component_handle; /* component */ + u32 port_handle; /* port */ + u32 id; /* Parameter ID */ + u32 size; /* Parameter size */ + u32 value[MMAL_WORKER_PORT_PARAMETER_SPACE]; +}; + +struct mmal_msg_port_parameter_set_reply { + u32 status; /* enum mmal_msg_status todo: how does this + * differ to the one in the header? + */ +}; + +/* port parameter getting */ + +struct mmal_msg_port_parameter_get { + u32 component_handle; /* component */ + u32 port_handle; /* port */ + u32 id; /* Parameter ID */ + u32 size; /* Parameter size */ +}; + +struct mmal_msg_port_parameter_get_reply { + u32 status; /* Status of mmal_port_parameter_get call */ + u32 id; /* Parameter ID */ + u32 size; /* Parameter size */ + u32 value[MMAL_WORKER_PORT_PARAMETER_SPACE]; +}; + +/* event messages */ +#define MMAL_WORKER_EVENT_SPACE 256 + +struct mmal_msg_event_to_host { + u32 client_component; /* component context */ + + u32 port_type; + u32 port_num; + + u32 cmd; + u32 length; + u8 data[MMAL_WORKER_EVENT_SPACE]; + u32 delayed_buffer; +}; + +/* all mmal messages are serialised through this structure */ +struct mmal_msg { + /* header */ + struct mmal_msg_header h; + /* payload */ + union { + struct mmal_msg_version version; + + struct mmal_msg_component_create component_create; + struct mmal_msg_component_create_reply component_create_reply; + + struct mmal_msg_component_destroy component_destroy; + struct mmal_msg_component_destroy_reply component_destroy_reply; + + struct mmal_msg_component_enable component_enable; + struct mmal_msg_component_enable_reply component_enable_reply; + + struct mmal_msg_component_disable component_disable; + struct mmal_msg_component_disable_reply component_disable_reply; + + struct mmal_msg_port_info_get port_info_get; + struct mmal_msg_port_info_get_reply port_info_get_reply; + + struct mmal_msg_port_info_set port_info_set; + struct mmal_msg_port_info_set_reply port_info_set_reply; + + struct mmal_msg_port_action_port port_action_port; + struct mmal_msg_port_action_handle port_action_handle; + struct mmal_msg_port_action_reply port_action_reply; + + struct mmal_msg_buffer_from_host buffer_from_host; + + struct mmal_msg_port_parameter_set port_parameter_set; + struct mmal_msg_port_parameter_set_reply + port_parameter_set_reply; + struct mmal_msg_port_parameter_get + port_parameter_get; + struct mmal_msg_port_parameter_get_reply + port_parameter_get_reply; + + struct mmal_msg_event_to_host event_to_host; + + u8 payload[MMAL_MSG_MAX_PAYLOAD]; + } u; +}; +#endif diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h new file mode 100644 index 000000000..a0cdd2810 --- /dev/null +++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h @@ -0,0 +1,752 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * Broadcom BCM2835 V4L2 driver + * + * Copyright © 2013 Raspberry Pi (Trading) Ltd. + * + * Authors: Vincent Sanders @ Collabora + * Dave Stevenson @ Broadcom + * (now dave.stevenson@raspberrypi.org) + * Simon Mellor @ Broadcom + * Luke Diamand @ Broadcom + */ + +/* common parameters */ + +/** @name Parameter groups + * Parameters are divided into groups, and then allocated sequentially within + * a group using an enum. + * @{ + */ + +#ifndef MMAL_PARAMETERS_H +#define MMAL_PARAMETERS_H + +#include <linux/math.h> + +/** Common parameter ID group, used with many types of component. */ +#define MMAL_PARAMETER_GROUP_COMMON (0 << 16) +/** Camera-specific parameter ID group. */ +#define MMAL_PARAMETER_GROUP_CAMERA (1 << 16) +/** Video-specific parameter ID group. */ +#define MMAL_PARAMETER_GROUP_VIDEO (2 << 16) +/** Audio-specific parameter ID group. */ +#define MMAL_PARAMETER_GROUP_AUDIO (3 << 16) +/** Clock-specific parameter ID group. */ +#define MMAL_PARAMETER_GROUP_CLOCK (4 << 16) +/** Miracast-specific parameter ID group. */ +#define MMAL_PARAMETER_GROUP_MIRACAST (5 << 16) + +/* Common parameters */ +enum mmal_parameter_common_type { + /**< Never a valid parameter ID */ + MMAL_PARAMETER_UNUSED = MMAL_PARAMETER_GROUP_COMMON, + + /**< MMAL_PARAMETER_ENCODING_T */ + MMAL_PARAMETER_SUPPORTED_ENCODINGS, + /**< MMAL_PARAMETER_URI_T */ + MMAL_PARAMETER_URI, + /** MMAL_PARAMETER_CHANGE_EVENT_REQUEST_T */ + MMAL_PARAMETER_CHANGE_EVENT_REQUEST, + /** MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_ZERO_COPY, + /**< MMAL_PARAMETER_BUFFER_REQUIREMENTS_T */ + MMAL_PARAMETER_BUFFER_REQUIREMENTS, + /**< MMAL_PARAMETER_STATISTICS_T */ + MMAL_PARAMETER_STATISTICS, + /**< MMAL_PARAMETER_CORE_STATISTICS_T */ + MMAL_PARAMETER_CORE_STATISTICS, + /**< MMAL_PARAMETER_MEM_USAGE_T */ + MMAL_PARAMETER_MEM_USAGE, + /**< MMAL_PARAMETER_UINT32_T */ + MMAL_PARAMETER_BUFFER_FLAG_FILTER, + /**< MMAL_PARAMETER_SEEK_T */ + MMAL_PARAMETER_SEEK, + /**< MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_POWERMON_ENABLE, + /**< MMAL_PARAMETER_LOGGING_T */ + MMAL_PARAMETER_LOGGING, + /**< MMAL_PARAMETER_UINT64_T */ + MMAL_PARAMETER_SYSTEM_TIME, + /**< MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_NO_IMAGE_PADDING, +}; + +/* camera parameters */ + +enum mmal_parameter_camera_type { + /* 0 */ + /** @ref MMAL_PARAMETER_THUMBNAIL_CONFIG_T */ + MMAL_PARAMETER_THUMBNAIL_CONFIGURATION = + MMAL_PARAMETER_GROUP_CAMERA, + /**< Unused? */ + MMAL_PARAMETER_CAPTURE_QUALITY, + /**< @ref MMAL_PARAMETER_INT32_T */ + MMAL_PARAMETER_ROTATION, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_EXIF_DISABLE, + /**< @ref MMAL_PARAMETER_EXIF_T */ + MMAL_PARAMETER_EXIF, + /**< @ref MMAL_PARAM_AWBMODE_T */ + MMAL_PARAMETER_AWB_MODE, + /**< @ref MMAL_PARAMETER_IMAGEFX_T */ + MMAL_PARAMETER_IMAGE_EFFECT, + /**< @ref MMAL_PARAMETER_COLOURFX_T */ + MMAL_PARAMETER_COLOUR_EFFECT, + /**< @ref MMAL_PARAMETER_FLICKERAVOID_T */ + MMAL_PARAMETER_FLICKER_AVOID, + /**< @ref MMAL_PARAMETER_FLASH_T */ + MMAL_PARAMETER_FLASH, + /**< @ref MMAL_PARAMETER_REDEYE_T */ + MMAL_PARAMETER_REDEYE, + /**< @ref MMAL_PARAMETER_FOCUS_T */ + MMAL_PARAMETER_FOCUS, + /**< Unused? */ + MMAL_PARAMETER_FOCAL_LENGTHS, + /**< @ref MMAL_PARAMETER_INT32_T */ + MMAL_PARAMETER_EXPOSURE_COMP, + /**< @ref MMAL_PARAMETER_SCALEFACTOR_T */ + MMAL_PARAMETER_ZOOM, + /**< @ref MMAL_PARAMETER_MIRROR_T */ + MMAL_PARAMETER_MIRROR, + + /* 0x10 */ + /**< @ref MMAL_PARAMETER_UINT32_T */ + MMAL_PARAMETER_CAMERA_NUM, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_CAPTURE, + /**< @ref MMAL_PARAMETER_EXPOSUREMODE_T */ + MMAL_PARAMETER_EXPOSURE_MODE, + /**< @ref MMAL_PARAMETER_EXPOSUREMETERINGMODE_T */ + MMAL_PARAMETER_EXP_METERING_MODE, + /**< @ref MMAL_PARAMETER_FOCUS_STATUS_T */ + MMAL_PARAMETER_FOCUS_STATUS, + /**< @ref MMAL_PARAMETER_CAMERA_CONFIG_T */ + MMAL_PARAMETER_CAMERA_CONFIG, + /**< @ref MMAL_PARAMETER_CAPTURE_STATUS_T */ + MMAL_PARAMETER_CAPTURE_STATUS, + /**< @ref MMAL_PARAMETER_FACE_TRACK_T */ + MMAL_PARAMETER_FACE_TRACK, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_DRAW_BOX_FACES_AND_FOCUS, + /**< @ref MMAL_PARAMETER_UINT32_T */ + MMAL_PARAMETER_JPEG_Q_FACTOR, + /**< @ref MMAL_PARAMETER_FRAME_RATE_T */ + MMAL_PARAMETER_FRAME_RATE, + /**< @ref MMAL_PARAMETER_CAMERA_STC_MODE_T */ + MMAL_PARAMETER_USE_STC, + /**< @ref MMAL_PARAMETER_CAMERA_INFO_T */ + MMAL_PARAMETER_CAMERA_INFO, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_VIDEO_STABILISATION, + /**< @ref MMAL_PARAMETER_FACE_TRACK_RESULTS_T */ + MMAL_PARAMETER_FACE_TRACK_RESULTS, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_ENABLE_RAW_CAPTURE, + + /* 0x20 */ + /**< @ref MMAL_PARAMETER_URI_T */ + MMAL_PARAMETER_DPF_FILE, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_ENABLE_DPF_FILE, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_DPF_FAIL_IS_FATAL, + /**< @ref MMAL_PARAMETER_CAPTUREMODE_T */ + MMAL_PARAMETER_CAPTURE_MODE, + /**< @ref MMAL_PARAMETER_FOCUS_REGIONS_T */ + MMAL_PARAMETER_FOCUS_REGIONS, + /**< @ref MMAL_PARAMETER_INPUT_CROP_T */ + MMAL_PARAMETER_INPUT_CROP, + /**< @ref MMAL_PARAMETER_SENSOR_INFORMATION_T */ + MMAL_PARAMETER_SENSOR_INFORMATION, + /**< @ref MMAL_PARAMETER_FLASH_SELECT_T */ + MMAL_PARAMETER_FLASH_SELECT, + /**< @ref MMAL_PARAMETER_FIELD_OF_VIEW_T */ + MMAL_PARAMETER_FIELD_OF_VIEW, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_HIGH_DYNAMIC_RANGE, + /**< @ref MMAL_PARAMETER_DRC_T */ + MMAL_PARAMETER_DYNAMIC_RANGE_COMPRESSION, + /**< @ref MMAL_PARAMETER_ALGORITHM_CONTROL_T */ + MMAL_PARAMETER_ALGORITHM_CONTROL, + /**< @ref MMAL_PARAMETER_RATIONAL_T */ + MMAL_PARAMETER_SHARPNESS, + /**< @ref MMAL_PARAMETER_RATIONAL_T */ + MMAL_PARAMETER_CONTRAST, + /**< @ref MMAL_PARAMETER_RATIONAL_T */ + MMAL_PARAMETER_BRIGHTNESS, + /**< @ref MMAL_PARAMETER_RATIONAL_T */ + MMAL_PARAMETER_SATURATION, + + /* 0x30 */ + /**< @ref MMAL_PARAMETER_UINT32_T */ + MMAL_PARAMETER_ISO, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_ANTISHAKE, + /** @ref MMAL_PARAMETER_IMAGEFX_PARAMETERS_T */ + MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS, + /** @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_CAMERA_BURST_CAPTURE, + /** @ref MMAL_PARAMETER_UINT32_T */ + MMAL_PARAMETER_CAMERA_MIN_ISO, + /** @ref MMAL_PARAMETER_CAMERA_USE_CASE_T */ + MMAL_PARAMETER_CAMERA_USE_CASE, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_CAPTURE_STATS_PASS, + /** @ref MMAL_PARAMETER_UINT32_T */ + MMAL_PARAMETER_CAMERA_CUSTOM_SENSOR_CONFIG, + /** @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_ENABLE_REGISTER_FILE, + /** @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_REGISTER_FAIL_IS_FATAL, + /** @ref MMAL_PARAMETER_CONFIGFILE_T */ + MMAL_PARAMETER_CONFIGFILE_REGISTERS, + /** @ref MMAL_PARAMETER_CONFIGFILE_CHUNK_T */ + MMAL_PARAMETER_CONFIGFILE_CHUNK_REGISTERS, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_JPEG_ATTACH_LOG, + /**< @ref MMAL_PARAMETER_ZEROSHUTTERLAG_T */ + MMAL_PARAMETER_ZERO_SHUTTER_LAG, + /**< @ref MMAL_PARAMETER_FPS_RANGE_T */ + MMAL_PARAMETER_FPS_RANGE, + /**< @ref MMAL_PARAMETER_INT32_T */ + MMAL_PARAMETER_CAPTURE_EXPOSURE_COMP, + + /* 0x40 */ + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_SW_SHARPEN_DISABLE, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_FLASH_REQUIRED, + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_SW_SATURATION_DISABLE, + /**< Takes a @ref MMAL_PARAMETER_UINT32_T */ + MMAL_PARAMETER_SHUTTER_SPEED, + /**< Takes a @ref MMAL_PARAMETER_AWB_GAINS_T */ + MMAL_PARAMETER_CUSTOM_AWB_GAINS, +}; + +enum mmal_parameter_camera_config_timestamp_mode { + MMAL_PARAM_TIMESTAMP_MODE_ZERO = 0, /* Always timestamp frames as 0 */ + MMAL_PARAM_TIMESTAMP_MODE_RAW_STC, /* Use the raw STC value + * for the frame timestamp + */ + MMAL_PARAM_TIMESTAMP_MODE_RESET_STC, /* Use the STC timestamp + * but subtract the + * timestamp of the first + * frame sent to give a + * zero based timestamp. + */ +}; + +struct mmal_parameter_fps_range { + /**< Low end of the permitted framerate range */ + struct s32_fract fps_low; + /**< High end of the permitted framerate range */ + struct s32_fract fps_high; +}; + +/* camera configuration parameter */ +struct mmal_parameter_camera_config { + /* Parameters for setting up the image pools */ + u32 max_stills_w; /* Max size of stills capture */ + u32 max_stills_h; + u32 stills_yuv422; /* Allow YUV422 stills capture */ + u32 one_shot_stills; /* Continuous or one shot stills captures. */ + + u32 max_preview_video_w; /* Max size of the preview or video + * capture frames + */ + u32 max_preview_video_h; + u32 num_preview_video_frames; + + /** Sets the height of the circular buffer for stills capture. */ + u32 stills_capture_circular_buffer_height; + + /** Allows preview/encode to resume as fast as possible after the stills + * input frame has been received, and then processes the still frame in + * the background whilst preview/encode has resumed. + * Actual mode is controlled by MMAL_PARAMETER_CAPTURE_MODE. + */ + u32 fast_preview_resume; + + /** Selects algorithm for timestamping frames if + * there is no clock component connected. + * enum mmal_parameter_camera_config_timestamp_mode + */ + s32 use_stc_timestamp; +}; + +enum mmal_parameter_exposuremode { + MMAL_PARAM_EXPOSUREMODE_OFF, + MMAL_PARAM_EXPOSUREMODE_AUTO, + MMAL_PARAM_EXPOSUREMODE_NIGHT, + MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW, + MMAL_PARAM_EXPOSUREMODE_BACKLIGHT, + MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT, + MMAL_PARAM_EXPOSUREMODE_SPORTS, + MMAL_PARAM_EXPOSUREMODE_SNOW, + MMAL_PARAM_EXPOSUREMODE_BEACH, + MMAL_PARAM_EXPOSUREMODE_VERYLONG, + MMAL_PARAM_EXPOSUREMODE_FIXEDFPS, + MMAL_PARAM_EXPOSUREMODE_ANTISHAKE, + MMAL_PARAM_EXPOSUREMODE_FIREWORKS, +}; + +enum mmal_parameter_exposuremeteringmode { + MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE, + MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT, + MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT, + MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX, +}; + +enum mmal_parameter_awbmode { + MMAL_PARAM_AWBMODE_OFF, + MMAL_PARAM_AWBMODE_AUTO, + MMAL_PARAM_AWBMODE_SUNLIGHT, + MMAL_PARAM_AWBMODE_CLOUDY, + MMAL_PARAM_AWBMODE_SHADE, + MMAL_PARAM_AWBMODE_TUNGSTEN, + MMAL_PARAM_AWBMODE_FLUORESCENT, + MMAL_PARAM_AWBMODE_INCANDESCENT, + MMAL_PARAM_AWBMODE_FLASH, + MMAL_PARAM_AWBMODE_HORIZON, +}; + +enum mmal_parameter_imagefx { + MMAL_PARAM_IMAGEFX_NONE, + MMAL_PARAM_IMAGEFX_NEGATIVE, + MMAL_PARAM_IMAGEFX_SOLARIZE, + MMAL_PARAM_IMAGEFX_POSTERIZE, + MMAL_PARAM_IMAGEFX_WHITEBOARD, + MMAL_PARAM_IMAGEFX_BLACKBOARD, + MMAL_PARAM_IMAGEFX_SKETCH, + MMAL_PARAM_IMAGEFX_DENOISE, + MMAL_PARAM_IMAGEFX_EMBOSS, + MMAL_PARAM_IMAGEFX_OILPAINT, + MMAL_PARAM_IMAGEFX_HATCH, + MMAL_PARAM_IMAGEFX_GPEN, + MMAL_PARAM_IMAGEFX_PASTEL, + MMAL_PARAM_IMAGEFX_WATERCOLOUR, + MMAL_PARAM_IMAGEFX_FILM, + MMAL_PARAM_IMAGEFX_BLUR, + MMAL_PARAM_IMAGEFX_SATURATION, + MMAL_PARAM_IMAGEFX_COLOURSWAP, + MMAL_PARAM_IMAGEFX_WASHEDOUT, + MMAL_PARAM_IMAGEFX_POSTERISE, + MMAL_PARAM_IMAGEFX_COLOURPOINT, + MMAL_PARAM_IMAGEFX_COLOURBALANCE, + MMAL_PARAM_IMAGEFX_CARTOON, +}; + +enum MMAL_PARAM_FLICKERAVOID { + MMAL_PARAM_FLICKERAVOID_OFF, + MMAL_PARAM_FLICKERAVOID_AUTO, + MMAL_PARAM_FLICKERAVOID_50HZ, + MMAL_PARAM_FLICKERAVOID_60HZ, + MMAL_PARAM_FLICKERAVOID_MAX = 0x7FFFFFFF +}; + +struct mmal_parameter_awbgains { + struct s32_fract r_gain; /**< Red gain */ + struct s32_fract b_gain; /**< Blue gain */ +}; + +/** Manner of video rate control */ +enum mmal_parameter_rate_control_mode { + MMAL_VIDEO_RATECONTROL_DEFAULT, + MMAL_VIDEO_RATECONTROL_VARIABLE, + MMAL_VIDEO_RATECONTROL_CONSTANT, + MMAL_VIDEO_RATECONTROL_VARIABLE_SKIP_FRAMES, + MMAL_VIDEO_RATECONTROL_CONSTANT_SKIP_FRAMES +}; + +enum mmal_video_profile { + MMAL_VIDEO_PROFILE_H263_BASELINE, + MMAL_VIDEO_PROFILE_H263_H320CODING, + MMAL_VIDEO_PROFILE_H263_BACKWARDCOMPATIBLE, + MMAL_VIDEO_PROFILE_H263_ISWV2, + MMAL_VIDEO_PROFILE_H263_ISWV3, + MMAL_VIDEO_PROFILE_H263_HIGHCOMPRESSION, + MMAL_VIDEO_PROFILE_H263_INTERNET, + MMAL_VIDEO_PROFILE_H263_INTERLACE, + MMAL_VIDEO_PROFILE_H263_HIGHLATENCY, + MMAL_VIDEO_PROFILE_MP4V_SIMPLE, + MMAL_VIDEO_PROFILE_MP4V_SIMPLESCALABLE, + MMAL_VIDEO_PROFILE_MP4V_CORE, + MMAL_VIDEO_PROFILE_MP4V_MAIN, + MMAL_VIDEO_PROFILE_MP4V_NBIT, + MMAL_VIDEO_PROFILE_MP4V_SCALABLETEXTURE, + MMAL_VIDEO_PROFILE_MP4V_SIMPLEFACE, + MMAL_VIDEO_PROFILE_MP4V_SIMPLEFBA, + MMAL_VIDEO_PROFILE_MP4V_BASICANIMATED, + MMAL_VIDEO_PROFILE_MP4V_HYBRID, + MMAL_VIDEO_PROFILE_MP4V_ADVANCEDREALTIME, + MMAL_VIDEO_PROFILE_MP4V_CORESCALABLE, + MMAL_VIDEO_PROFILE_MP4V_ADVANCEDCODING, + MMAL_VIDEO_PROFILE_MP4V_ADVANCEDCORE, + MMAL_VIDEO_PROFILE_MP4V_ADVANCEDSCALABLE, + MMAL_VIDEO_PROFILE_MP4V_ADVANCEDSIMPLE, + MMAL_VIDEO_PROFILE_H264_BASELINE, + MMAL_VIDEO_PROFILE_H264_MAIN, + MMAL_VIDEO_PROFILE_H264_EXTENDED, + MMAL_VIDEO_PROFILE_H264_HIGH, + MMAL_VIDEO_PROFILE_H264_HIGH10, + MMAL_VIDEO_PROFILE_H264_HIGH422, + MMAL_VIDEO_PROFILE_H264_HIGH444, + MMAL_VIDEO_PROFILE_H264_CONSTRAINED_BASELINE, + MMAL_VIDEO_PROFILE_DUMMY = 0x7FFFFFFF +}; + +enum mmal_video_level { + MMAL_VIDEO_LEVEL_H263_10, + MMAL_VIDEO_LEVEL_H263_20, + MMAL_VIDEO_LEVEL_H263_30, + MMAL_VIDEO_LEVEL_H263_40, + MMAL_VIDEO_LEVEL_H263_45, + MMAL_VIDEO_LEVEL_H263_50, + MMAL_VIDEO_LEVEL_H263_60, + MMAL_VIDEO_LEVEL_H263_70, + MMAL_VIDEO_LEVEL_MP4V_0, + MMAL_VIDEO_LEVEL_MP4V_0b, + MMAL_VIDEO_LEVEL_MP4V_1, + MMAL_VIDEO_LEVEL_MP4V_2, + MMAL_VIDEO_LEVEL_MP4V_3, + MMAL_VIDEO_LEVEL_MP4V_4, + MMAL_VIDEO_LEVEL_MP4V_4a, + MMAL_VIDEO_LEVEL_MP4V_5, + MMAL_VIDEO_LEVEL_MP4V_6, + MMAL_VIDEO_LEVEL_H264_1, + MMAL_VIDEO_LEVEL_H264_1b, + MMAL_VIDEO_LEVEL_H264_11, + MMAL_VIDEO_LEVEL_H264_12, + MMAL_VIDEO_LEVEL_H264_13, + MMAL_VIDEO_LEVEL_H264_2, + MMAL_VIDEO_LEVEL_H264_21, + MMAL_VIDEO_LEVEL_H264_22, + MMAL_VIDEO_LEVEL_H264_3, + MMAL_VIDEO_LEVEL_H264_31, + MMAL_VIDEO_LEVEL_H264_32, + MMAL_VIDEO_LEVEL_H264_4, + MMAL_VIDEO_LEVEL_H264_41, + MMAL_VIDEO_LEVEL_H264_42, + MMAL_VIDEO_LEVEL_H264_5, + MMAL_VIDEO_LEVEL_H264_51, + MMAL_VIDEO_LEVEL_DUMMY = 0x7FFFFFFF +}; + +struct mmal_parameter_video_profile { + enum mmal_video_profile profile; + enum mmal_video_level level; +}; + +/* video parameters */ + +enum mmal_parameter_video_type { + /** @ref MMAL_DISPLAYREGION_T */ + MMAL_PARAMETER_DISPLAYREGION = MMAL_PARAMETER_GROUP_VIDEO, + + /** @ref MMAL_PARAMETER_VIDEO_PROFILE_T */ + MMAL_PARAMETER_SUPPORTED_PROFILES, + + /** @ref MMAL_PARAMETER_VIDEO_PROFILE_T */ + MMAL_PARAMETER_PROFILE, + + /** @ref MMAL_PARAMETER_UINT32_T */ + MMAL_PARAMETER_INTRAPERIOD, + + /** @ref MMAL_PARAMETER_VIDEO_RATECONTROL_T */ + MMAL_PARAMETER_RATECONTROL, + + /** @ref MMAL_PARAMETER_VIDEO_NALUNITFORMAT_T */ + MMAL_PARAMETER_NALUNITFORMAT, + + /** @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_MINIMISE_FRAGMENTATION, + + /** @ref MMAL_PARAMETER_UINT32_T. + * Setting the value to zero resets to the default (one slice per + * frame). + */ + MMAL_PARAMETER_MB_ROWS_PER_SLICE, + + /** @ref MMAL_PARAMETER_VIDEO_LEVEL_EXTENSION_T */ + MMAL_PARAMETER_VIDEO_LEVEL_EXTENSION, + + /** @ref MMAL_PARAMETER_VIDEO_EEDE_ENABLE_T */ + MMAL_PARAMETER_VIDEO_EEDE_ENABLE, + + /** @ref MMAL_PARAMETER_VIDEO_EEDE_LOSSRATE_T */ + MMAL_PARAMETER_VIDEO_EEDE_LOSSRATE, + + /** @ref MMAL_PARAMETER_BOOLEAN_T. Request an I-frame. */ + MMAL_PARAMETER_VIDEO_REQUEST_I_FRAME, + /** @ref MMAL_PARAMETER_VIDEO_INTRA_REFRESH_T */ + MMAL_PARAMETER_VIDEO_INTRA_REFRESH, + + /** @ref MMAL_PARAMETER_BOOLEAN_T. */ + MMAL_PARAMETER_VIDEO_IMMUTABLE_INPUT, + + /** @ref MMAL_PARAMETER_UINT32_T. Run-time bit rate control */ + MMAL_PARAMETER_VIDEO_BIT_RATE, + + /** @ref MMAL_PARAMETER_FRAME_RATE_T */ + MMAL_PARAMETER_VIDEO_FRAME_RATE, + + /** @ref MMAL_PARAMETER_UINT32_T. */ + MMAL_PARAMETER_VIDEO_ENCODE_MIN_QUANT, + + /** @ref MMAL_PARAMETER_UINT32_T. */ + MMAL_PARAMETER_VIDEO_ENCODE_MAX_QUANT, + + /** @ref MMAL_PARAMETER_VIDEO_ENCODE_RC_MODEL_T. */ + MMAL_PARAMETER_VIDEO_ENCODE_RC_MODEL, + + MMAL_PARAMETER_EXTRA_BUFFERS, /**< @ref MMAL_PARAMETER_UINT32_T. */ + /** @ref MMAL_PARAMETER_UINT32_T. + * Changing this parameter from the default can reduce frame rate + * because image buffers need to be re-pitched. + */ + MMAL_PARAMETER_VIDEO_ALIGN_HORIZ, + + /** @ref MMAL_PARAMETER_UINT32_T. + * Changing this parameter from the default can reduce frame rate + * because image buffers need to be re-pitched. + */ + MMAL_PARAMETER_VIDEO_ALIGN_VERT, + + /** @ref MMAL_PARAMETER_BOOLEAN_T. */ + MMAL_PARAMETER_VIDEO_DROPPABLE_PFRAMES, + + /** @ref MMAL_PARAMETER_UINT32_T. */ + MMAL_PARAMETER_VIDEO_ENCODE_INITIAL_QUANT, + + /**< @ref MMAL_PARAMETER_UINT32_T. */ + MMAL_PARAMETER_VIDEO_ENCODE_QP_P, + + /**< @ref MMAL_PARAMETER_UINT32_T. */ + MMAL_PARAMETER_VIDEO_ENCODE_RC_SLICE_DQUANT, + + /** @ref MMAL_PARAMETER_UINT32_T */ + MMAL_PARAMETER_VIDEO_ENCODE_FRAME_LIMIT_BITS, + + /** @ref MMAL_PARAMETER_UINT32_T. */ + MMAL_PARAMETER_VIDEO_ENCODE_PEAK_RATE, + + /* H264 specific parameters */ + + /** @ref MMAL_PARAMETER_BOOLEAN_T. */ + MMAL_PARAMETER_VIDEO_ENCODE_H264_DISABLE_CABAC, + + /** @ref MMAL_PARAMETER_BOOLEAN_T. */ + MMAL_PARAMETER_VIDEO_ENCODE_H264_LOW_LATENCY, + + /** @ref MMAL_PARAMETER_BOOLEAN_T. */ + MMAL_PARAMETER_VIDEO_ENCODE_H264_AU_DELIMITERS, + + /** @ref MMAL_PARAMETER_UINT32_T. */ + MMAL_PARAMETER_VIDEO_ENCODE_H264_DEBLOCK_IDC, + + /** @ref MMAL_PARAMETER_VIDEO_ENCODER_H264_MB_INTRA_MODES_T. */ + MMAL_PARAMETER_VIDEO_ENCODE_H264_MB_INTRA_MODE, + + /** @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_VIDEO_ENCODE_HEADER_ON_OPEN, + + /** @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_VIDEO_ENCODE_PRECODE_FOR_QP, + + /** @ref MMAL_PARAMETER_VIDEO_DRM_INIT_INFO_T. */ + MMAL_PARAMETER_VIDEO_DRM_INIT_INFO, + + /** @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_VIDEO_TIMESTAMP_FIFO, + + /** @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_VIDEO_DECODE_ERROR_CONCEALMENT, + + /** @ref MMAL_PARAMETER_VIDEO_DRM_PROTECT_BUFFER_T. */ + MMAL_PARAMETER_VIDEO_DRM_PROTECT_BUFFER, + + /** @ref MMAL_PARAMETER_BYTES_T */ + MMAL_PARAMETER_VIDEO_DECODE_CONFIG_VD3, + + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_VIDEO_ENCODE_H264_VCL_HRD_PARAMETERS, + + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_VIDEO_ENCODE_H264_LOW_DELAY_HRD_FLAG, + + /**< @ref MMAL_PARAMETER_BOOLEAN_T */ + MMAL_PARAMETER_VIDEO_ENCODE_INLINE_HEADER +}; + +/** Valid mirror modes */ +enum mmal_parameter_mirror { + MMAL_PARAM_MIRROR_NONE, + MMAL_PARAM_MIRROR_VERTICAL, + MMAL_PARAM_MIRROR_HORIZONTAL, + MMAL_PARAM_MIRROR_BOTH, +}; + +enum mmal_parameter_displaytransform { + MMAL_DISPLAY_ROT0 = 0, + MMAL_DISPLAY_MIRROR_ROT0 = 1, + MMAL_DISPLAY_MIRROR_ROT180 = 2, + MMAL_DISPLAY_ROT180 = 3, + MMAL_DISPLAY_MIRROR_ROT90 = 4, + MMAL_DISPLAY_ROT270 = 5, + MMAL_DISPLAY_ROT90 = 6, + MMAL_DISPLAY_MIRROR_ROT270 = 7, +}; + +enum mmal_parameter_displaymode { + MMAL_DISPLAY_MODE_FILL = 0, + MMAL_DISPLAY_MODE_LETTERBOX = 1, +}; + +enum mmal_parameter_displayset { + MMAL_DISPLAY_SET_NONE = 0, + MMAL_DISPLAY_SET_NUM = 1, + MMAL_DISPLAY_SET_FULLSCREEN = 2, + MMAL_DISPLAY_SET_TRANSFORM = 4, + MMAL_DISPLAY_SET_DEST_RECT = 8, + MMAL_DISPLAY_SET_SRC_RECT = 0x10, + MMAL_DISPLAY_SET_MODE = 0x20, + MMAL_DISPLAY_SET_PIXEL = 0x40, + MMAL_DISPLAY_SET_NOASPECT = 0x80, + MMAL_DISPLAY_SET_LAYER = 0x100, + MMAL_DISPLAY_SET_COPYPROTECT = 0x200, + MMAL_DISPLAY_SET_ALPHA = 0x400, +}; + +/* rectangle, used lots so it gets its own struct */ +struct vchiq_mmal_rect { + s32 x; + s32 y; + s32 width; + s32 height; +}; + +struct mmal_parameter_displayregion { + /** Bitfield that indicates which fields are set and should be + * used. All other fields will maintain their current value. + * \ref MMAL_DISPLAYSET_T defines the bits that can be + * combined. + */ + u32 set; + + /** Describes the display output device, with 0 typically + * being a directly connected LCD display. The actual values + * will depend on the hardware. Code using hard-wired numbers + * (e.g. 2) is certain to fail. + */ + + u32 display_num; + /** Indicates that we are using the full device screen area, + * rather than a window of the display. If zero, then + * dest_rect is used to specify a region of the display to + * use. + */ + + s32 fullscreen; + /** Indicates any rotation or flipping used to map frames onto + * the natural display orientation. + */ + u32 transform; /* enum mmal_parameter_displaytransform */ + + /** Where to display the frame within the screen, if + * fullscreen is zero. + */ + struct vchiq_mmal_rect dest_rect; + + /** Indicates which area of the frame to display. If all + * values are zero, the whole frame will be used. + */ + struct vchiq_mmal_rect src_rect; + + /** If set to non-zero, indicates that any display scaling + * should disregard the aspect ratio of the frame region being + * displayed. + */ + s32 noaspect; + + /** Indicates how the image should be scaled to fit the + * display. \code MMAL_DISPLAY_MODE_FILL \endcode indicates + * that the image should fill the screen by potentially + * cropping the frames. Setting \code mode \endcode to \code + * MMAL_DISPLAY_MODE_LETTERBOX \endcode indicates that all the + * source region should be displayed and black bars added if + * necessary. + */ + u32 mode; /* enum mmal_parameter_displaymode */ + + /** If non-zero, defines the width of a source pixel relative + * to \code pixel_y \endcode. If zero, then pixels default to + * being square. + */ + u32 pixel_x; + + /** If non-zero, defines the height of a source pixel relative + * to \code pixel_x \endcode. If zero, then pixels default to + * being square. + */ + u32 pixel_y; + + /** Sets the relative depth of the images, with greater values + * being in front of smaller values. + */ + u32 layer; + + /** Set to non-zero to ensure copy protection is used on + * output. + */ + s32 copyprotect_required; + + /** Level of opacity of the layer, where zero is fully + * transparent and 255 is fully opaque. + */ + u32 alpha; +}; + +#define MMAL_MAX_IMAGEFX_PARAMETERS 5 + +struct mmal_parameter_imagefx_parameters { + enum mmal_parameter_imagefx effect; + u32 num_effect_params; + u32 effect_parameter[MMAL_MAX_IMAGEFX_PARAMETERS]; +}; + +#define MMAL_PARAMETER_CAMERA_INFO_MAX_CAMERAS 4 +#define MMAL_PARAMETER_CAMERA_INFO_MAX_FLASHES 2 +#define MMAL_PARAMETER_CAMERA_INFO_MAX_STR_LEN 16 + +struct mmal_parameter_camera_info_camera { + u32 port_id; + u32 max_width; + u32 max_height; + u32 lens_present; + u8 camera_name[MMAL_PARAMETER_CAMERA_INFO_MAX_STR_LEN]; +}; + +enum mmal_parameter_camera_info_flash_type { + /* Make values explicit to ensure they match values in config ini */ + MMAL_PARAMETER_CAMERA_INFO_FLASH_TYPE_XENON = 0, + MMAL_PARAMETER_CAMERA_INFO_FLASH_TYPE_LED = 1, + MMAL_PARAMETER_CAMERA_INFO_FLASH_TYPE_OTHER = 2, + MMAL_PARAMETER_CAMERA_INFO_FLASH_TYPE_MAX = 0x7FFFFFFF +}; + +struct mmal_parameter_camera_info_flash { + enum mmal_parameter_camera_info_flash_type flash_type; +}; + +struct mmal_parameter_camera_info { + u32 num_cameras; + u32 num_flashes; + struct mmal_parameter_camera_info_camera + cameras[MMAL_PARAMETER_CAMERA_INFO_MAX_CAMERAS]; + struct mmal_parameter_camera_info_flash + flashes[MMAL_PARAMETER_CAMERA_INFO_MAX_FLASHES]; +}; + +#endif diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.c b/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.c new file mode 100644 index 000000000..cb921c949 --- /dev/null +++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.c @@ -0,0 +1,1945 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Broadcom BCM2835 V4L2 driver + * + * Copyright © 2013 Raspberry Pi (Trading) Ltd. + * + * Authors: Vincent Sanders @ Collabora + * Dave Stevenson @ Broadcom + * (now dave.stevenson@raspberrypi.org) + * Simon Mellor @ Broadcom + * Luke Diamand @ Broadcom + * + * V4L2 driver MMAL vchiq interface code + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt + +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/mutex.h> +#include <linux/mm.h> +#include <linux/slab.h> +#include <linux/completion.h> +#include <linux/vmalloc.h> +#include <linux/raspberrypi/vchiq.h> +#include <media/videobuf2-vmalloc.h> + +#include "mmal-common.h" +#include "mmal-vchiq.h" +#include "mmal-msg.h" + +/* + * maximum number of components supported. + * This matches the maximum permitted by default on the VPU + */ +#define VCHIQ_MMAL_MAX_COMPONENTS 64 + +/* + * Timeout for synchronous msg responses in seconds. + * Helpful to increase this if stopping in the VPU debugger. + */ +#define SYNC_MSG_TIMEOUT 3 + +/*#define FULL_MSG_DUMP 1*/ + +#ifdef DEBUG +static const char *const msg_type_names[] = { + "UNKNOWN", + "QUIT", + "SERVICE_CLOSED", + "GET_VERSION", + "COMPONENT_CREATE", + "COMPONENT_DESTROY", + "COMPONENT_ENABLE", + "COMPONENT_DISABLE", + "PORT_INFO_GET", + "PORT_INFO_SET", + "PORT_ACTION", + "BUFFER_FROM_HOST", + "BUFFER_TO_HOST", + "GET_STATS", + "PORT_PARAMETER_SET", + "PORT_PARAMETER_GET", + "EVENT_TO_HOST", + "GET_CORE_STATS_FOR_PORT", + "OPAQUE_ALLOCATOR", + "CONSUME_MEM", + "LMK", + "OPAQUE_ALLOCATOR_DESC", + "DRM_GET_LHS32", + "DRM_GET_TIME", + "BUFFER_FROM_HOST_ZEROLEN", + "PORT_FLUSH", + "HOST_LOG", +}; +#endif + +static const char *const port_action_type_names[] = { + "UNKNOWN", + "ENABLE", + "DISABLE", + "FLUSH", + "CONNECT", + "DISCONNECT", + "SET_REQUIREMENTS", +}; + +#if defined(DEBUG) +#if defined(FULL_MSG_DUMP) +#define DBG_DUMP_MSG(MSG, MSG_LEN, TITLE) \ + do { \ + pr_debug(TITLE" type:%s(%d) length:%d\n", \ + msg_type_names[(MSG)->h.type], \ + (MSG)->h.type, (MSG_LEN)); \ + print_hex_dump(KERN_DEBUG, "<<h: ", DUMP_PREFIX_OFFSET, \ + 16, 4, (MSG), \ + sizeof(struct mmal_msg_header), 1); \ + print_hex_dump(KERN_DEBUG, "<<p: ", DUMP_PREFIX_OFFSET, \ + 16, 4, \ + ((u8 *)(MSG)) + sizeof(struct mmal_msg_header),\ + (MSG_LEN) - sizeof(struct mmal_msg_header), 1); \ + } while (0) +#else +#define DBG_DUMP_MSG(MSG, MSG_LEN, TITLE) \ + { \ + pr_debug(TITLE" type:%s(%d) length:%d\n", \ + msg_type_names[(MSG)->h.type], \ + (MSG)->h.type, (MSG_LEN)); \ + } +#endif +#else +#define DBG_DUMP_MSG(MSG, MSG_LEN, TITLE) +#endif + +struct vchiq_mmal_instance; + +/* normal message context */ +struct mmal_msg_context { + struct vchiq_mmal_instance *instance; + + /* Index in the context_map idr so that we can find the + * mmal_msg_context again when servicing the VCHI reply. + */ + int handle; + + union { + struct { + /* work struct for buffer_cb callback */ + struct work_struct work; + /* work struct for deferred callback */ + struct work_struct buffer_to_host_work; + /* mmal instance */ + struct vchiq_mmal_instance *instance; + /* mmal port */ + struct vchiq_mmal_port *port; + /* actual buffer used to store bulk reply */ + struct mmal_buffer *buffer; + /* amount of buffer used */ + unsigned long buffer_used; + /* MMAL buffer flags */ + u32 mmal_flags; + /* Presentation and Decode timestamps */ + s64 pts; + s64 dts; + + int status; /* context status */ + + } bulk; /* bulk data */ + + struct { + /* message handle to release */ + struct vchiq_header *msg_handle; + /* pointer to received message */ + struct mmal_msg *msg; + /* received message length */ + u32 msg_len; + /* completion upon reply */ + struct completion cmplt; + } sync; /* synchronous response */ + } u; + +}; + +struct vchiq_mmal_instance { + unsigned int service_handle; + + /* ensure serialised access to service */ + struct mutex vchiq_mutex; + + struct idr context_map; + /* protect accesses to context_map */ + struct mutex context_map_lock; + + struct vchiq_mmal_component component[VCHIQ_MMAL_MAX_COMPONENTS]; + + /* ordered workqueue to process all bulk operations */ + struct workqueue_struct *bulk_wq; + + /* handle for a vchiq instance */ + struct vchiq_instance *vchiq_instance; +}; + +static struct mmal_msg_context * +get_msg_context(struct vchiq_mmal_instance *instance) +{ + struct mmal_msg_context *msg_context; + int handle; + + /* todo: should this be allocated from a pool to avoid kzalloc */ + msg_context = kzalloc(sizeof(*msg_context), GFP_KERNEL); + + if (!msg_context) + return ERR_PTR(-ENOMEM); + + /* Create an ID that will be passed along with our message so + * that when we service the VCHI reply, we can look up what + * message is being replied to. + */ + mutex_lock(&instance->context_map_lock); + handle = idr_alloc(&instance->context_map, msg_context, + 0, 0, GFP_KERNEL); + mutex_unlock(&instance->context_map_lock); + + if (handle < 0) { + kfree(msg_context); + return ERR_PTR(handle); + } + + msg_context->instance = instance; + msg_context->handle = handle; + + return msg_context; +} + +static struct mmal_msg_context * +lookup_msg_context(struct vchiq_mmal_instance *instance, int handle) +{ + return idr_find(&instance->context_map, handle); +} + +static void +release_msg_context(struct mmal_msg_context *msg_context) +{ + struct vchiq_mmal_instance *instance = msg_context->instance; + + mutex_lock(&instance->context_map_lock); + idr_remove(&instance->context_map, msg_context->handle); + mutex_unlock(&instance->context_map_lock); + kfree(msg_context); +} + +/* deals with receipt of event to host message */ +static void event_to_host_cb(struct vchiq_mmal_instance *instance, + struct mmal_msg *msg, u32 msg_len) +{ + pr_debug("unhandled event\n"); + pr_debug("component:%u port type:%d num:%d cmd:0x%x length:%d\n", + msg->u.event_to_host.client_component, + msg->u.event_to_host.port_type, + msg->u.event_to_host.port_num, + msg->u.event_to_host.cmd, msg->u.event_to_host.length); +} + +/* workqueue scheduled callback + * + * we do this because it is important we do not call any other vchiq + * sync calls from witin the message delivery thread + */ +static void buffer_work_cb(struct work_struct *work) +{ + struct mmal_msg_context *msg_context = + container_of(work, struct mmal_msg_context, u.bulk.work); + struct mmal_buffer *buffer = msg_context->u.bulk.buffer; + + if (!buffer) { + pr_err("%s: ctx: %p, No mmal buffer to pass details\n", + __func__, msg_context); + return; + } + + buffer->length = msg_context->u.bulk.buffer_used; + buffer->mmal_flags = msg_context->u.bulk.mmal_flags; + buffer->dts = msg_context->u.bulk.dts; + buffer->pts = msg_context->u.bulk.pts; + + atomic_dec(&msg_context->u.bulk.port->buffers_with_vpu); + + msg_context->u.bulk.port->buffer_cb(msg_context->u.bulk.instance, + msg_context->u.bulk.port, + msg_context->u.bulk.status, + msg_context->u.bulk.buffer); +} + +/* workqueue scheduled callback to handle receiving buffers + * + * VCHI will allow up to 4 bulk receives to be scheduled before blocking. + * If we block in the service_callback context then we can't process the + * VCHI_CALLBACK_BULK_RECEIVED message that would otherwise allow the blocked + * vchiq_bulk_receive() call to complete. + */ +static void buffer_to_host_work_cb(struct work_struct *work) +{ + struct mmal_msg_context *msg_context = + container_of(work, struct mmal_msg_context, + u.bulk.buffer_to_host_work); + struct vchiq_mmal_instance *instance = msg_context->instance; + unsigned long len = msg_context->u.bulk.buffer_used; + int ret; + + if (!len) + /* Dummy receive to ensure the buffers remain in order */ + len = 8; + /* queue the bulk submission */ + vchiq_use_service(instance->vchiq_instance, instance->service_handle); + ret = vchiq_bulk_receive(instance->vchiq_instance, instance->service_handle, + msg_context->u.bulk.buffer->buffer, + /* Actual receive needs to be a multiple + * of 4 bytes + */ + (len + 3) & ~3, + msg_context, + VCHIQ_BULK_MODE_CALLBACK); + + vchiq_release_service(instance->vchiq_instance, instance->service_handle); + + if (ret != 0) + pr_err("%s: ctx: %p, vchiq_bulk_receive failed %d\n", + __func__, msg_context, ret); +} + +/* enqueue a bulk receive for a given message context */ +static int bulk_receive(struct vchiq_mmal_instance *instance, + struct mmal_msg *msg, + struct mmal_msg_context *msg_context) +{ + unsigned long rd_len; + + rd_len = msg->u.buffer_from_host.buffer_header.length; + + if (!msg_context->u.bulk.buffer) { + pr_err("bulk.buffer not configured - error in buffer_from_host\n"); + + /* todo: this is a serious error, we should never have + * committed a buffer_to_host operation to the mmal + * port without the buffer to back it up (underflow + * handling) and there is no obvious way to deal with + * this - how is the mmal servie going to react when + * we fail to do the xfer and reschedule a buffer when + * it arrives? perhaps a starved flag to indicate a + * waiting bulk receive? + */ + + return -EINVAL; + } + + /* ensure we do not overrun the available buffer */ + if (rd_len > msg_context->u.bulk.buffer->buffer_size) { + rd_len = msg_context->u.bulk.buffer->buffer_size; + pr_warn("short read as not enough receive buffer space\n"); + /* todo: is this the correct response, what happens to + * the rest of the message data? + */ + } + + /* store length */ + msg_context->u.bulk.buffer_used = rd_len; + msg_context->u.bulk.dts = msg->u.buffer_from_host.buffer_header.dts; + msg_context->u.bulk.pts = msg->u.buffer_from_host.buffer_header.pts; + + queue_work(msg_context->instance->bulk_wq, + &msg_context->u.bulk.buffer_to_host_work); + + return 0; +} + +/* data in message, memcpy from packet into output buffer */ +static int inline_receive(struct vchiq_mmal_instance *instance, + struct mmal_msg *msg, + struct mmal_msg_context *msg_context) +{ + memcpy(msg_context->u.bulk.buffer->buffer, + msg->u.buffer_from_host.short_data, + msg->u.buffer_from_host.payload_in_message); + + msg_context->u.bulk.buffer_used = + msg->u.buffer_from_host.payload_in_message; + + return 0; +} + +/* queue the buffer availability with MMAL_MSG_TYPE_BUFFER_FROM_HOST */ +static int +buffer_from_host(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, struct mmal_buffer *buf) +{ + struct mmal_msg_context *msg_context; + struct mmal_msg m; + int ret; + + if (!port->enabled) + return -EINVAL; + + pr_debug("instance:%u buffer:%p\n", instance->service_handle, buf); + + /* get context */ + if (!buf->msg_context) { + pr_err("%s: msg_context not allocated, buf %p\n", __func__, + buf); + return -EINVAL; + } + msg_context = buf->msg_context; + + /* store bulk message context for when data arrives */ + msg_context->u.bulk.instance = instance; + msg_context->u.bulk.port = port; + msg_context->u.bulk.buffer = buf; + msg_context->u.bulk.buffer_used = 0; + + /* initialise work structure ready to schedule callback */ + INIT_WORK(&msg_context->u.bulk.work, buffer_work_cb); + INIT_WORK(&msg_context->u.bulk.buffer_to_host_work, + buffer_to_host_work_cb); + + atomic_inc(&port->buffers_with_vpu); + + /* prep the buffer from host message */ + memset(&m, 0xbc, sizeof(m)); /* just to make debug clearer */ + + m.h.type = MMAL_MSG_TYPE_BUFFER_FROM_HOST; + m.h.magic = MMAL_MAGIC; + m.h.context = msg_context->handle; + m.h.status = 0; + + /* drvbuf is our private data passed back */ + m.u.buffer_from_host.drvbuf.magic = MMAL_MAGIC; + m.u.buffer_from_host.drvbuf.component_handle = port->component->handle; + m.u.buffer_from_host.drvbuf.port_handle = port->handle; + m.u.buffer_from_host.drvbuf.client_context = msg_context->handle; + + /* buffer header */ + m.u.buffer_from_host.buffer_header.cmd = 0; + m.u.buffer_from_host.buffer_header.data = + (u32)(unsigned long)buf->buffer; + m.u.buffer_from_host.buffer_header.alloc_size = buf->buffer_size; + m.u.buffer_from_host.buffer_header.length = 0; /* nothing used yet */ + m.u.buffer_from_host.buffer_header.offset = 0; /* no offset */ + m.u.buffer_from_host.buffer_header.flags = 0; /* no flags */ + m.u.buffer_from_host.buffer_header.pts = MMAL_TIME_UNKNOWN; + m.u.buffer_from_host.buffer_header.dts = MMAL_TIME_UNKNOWN; + + /* clear buffer type specific data */ + memset(&m.u.buffer_from_host.buffer_header_type_specific, 0, + sizeof(m.u.buffer_from_host.buffer_header_type_specific)); + + /* no payload in message */ + m.u.buffer_from_host.payload_in_message = 0; + + vchiq_use_service(instance->vchiq_instance, instance->service_handle); + + ret = vchiq_queue_kernel_message(instance->vchiq_instance, instance->service_handle, &m, + sizeof(struct mmal_msg_header) + + sizeof(m.u.buffer_from_host)); + if (ret) + atomic_dec(&port->buffers_with_vpu); + + vchiq_release_service(instance->vchiq_instance, instance->service_handle); + + return ret; +} + +/* deals with receipt of buffer to host message */ +static void buffer_to_host_cb(struct vchiq_mmal_instance *instance, + struct mmal_msg *msg, u32 msg_len) +{ + struct mmal_msg_context *msg_context; + u32 handle; + + pr_debug("%s: instance:%p msg:%p msg_len:%d\n", + __func__, instance, msg, msg_len); + + if (msg->u.buffer_from_host.drvbuf.magic == MMAL_MAGIC) { + handle = msg->u.buffer_from_host.drvbuf.client_context; + msg_context = lookup_msg_context(instance, handle); + + if (!msg_context) { + pr_err("drvbuf.client_context(%u) is invalid\n", + handle); + return; + } + } else { + pr_err("MMAL_MSG_TYPE_BUFFER_TO_HOST with bad magic\n"); + return; + } + + msg_context->u.bulk.mmal_flags = + msg->u.buffer_from_host.buffer_header.flags; + + if (msg->h.status != MMAL_MSG_STATUS_SUCCESS) { + /* message reception had an error */ + pr_warn("error %d in reply\n", msg->h.status); + + msg_context->u.bulk.status = msg->h.status; + + } else if (msg->u.buffer_from_host.buffer_header.length == 0) { + /* empty buffer */ + if (msg->u.buffer_from_host.buffer_header.flags & + MMAL_BUFFER_HEADER_FLAG_EOS) { + msg_context->u.bulk.status = + bulk_receive(instance, msg, msg_context); + if (msg_context->u.bulk.status == 0) + return; /* successful bulk submission, bulk + * completion will trigger callback + */ + } else { + /* do callback with empty buffer - not EOS though */ + msg_context->u.bulk.status = 0; + msg_context->u.bulk.buffer_used = 0; + } + } else if (msg->u.buffer_from_host.payload_in_message == 0) { + /* data is not in message, queue a bulk receive */ + msg_context->u.bulk.status = + bulk_receive(instance, msg, msg_context); + if (msg_context->u.bulk.status == 0) + return; /* successful bulk submission, bulk + * completion will trigger callback + */ + + /* failed to submit buffer, this will end badly */ + pr_err("error %d on bulk submission\n", + msg_context->u.bulk.status); + + } else if (msg->u.buffer_from_host.payload_in_message <= + MMAL_VC_SHORT_DATA) { + /* data payload within message */ + msg_context->u.bulk.status = inline_receive(instance, msg, + msg_context); + } else { + pr_err("message with invalid short payload\n"); + + /* signal error */ + msg_context->u.bulk.status = -EINVAL; + msg_context->u.bulk.buffer_used = + msg->u.buffer_from_host.payload_in_message; + } + + /* schedule the port callback */ + schedule_work(&msg_context->u.bulk.work); +} + +static void bulk_receive_cb(struct vchiq_mmal_instance *instance, + struct mmal_msg_context *msg_context) +{ + msg_context->u.bulk.status = 0; + + /* schedule the port callback */ + schedule_work(&msg_context->u.bulk.work); +} + +static void bulk_abort_cb(struct vchiq_mmal_instance *instance, + struct mmal_msg_context *msg_context) +{ + pr_err("%s: bulk ABORTED msg_context:%p\n", __func__, msg_context); + + msg_context->u.bulk.status = -EINTR; + + schedule_work(&msg_context->u.bulk.work); +} + +/* incoming event service callback */ +static enum vchiq_status service_callback(struct vchiq_instance *vchiq_instance, + enum vchiq_reason reason, + struct vchiq_header *header, + unsigned int handle, void *bulk_ctx) +{ + struct vchiq_mmal_instance *instance = vchiq_get_service_userdata(vchiq_instance, handle); + u32 msg_len; + struct mmal_msg *msg; + struct mmal_msg_context *msg_context; + + if (!instance) { + pr_err("Message callback passed NULL instance\n"); + return VCHIQ_SUCCESS; + } + + switch (reason) { + case VCHIQ_MESSAGE_AVAILABLE: + msg = (void *)header->data; + msg_len = header->size; + + DBG_DUMP_MSG(msg, msg_len, "<<< reply message"); + + /* handling is different for buffer messages */ + switch (msg->h.type) { + case MMAL_MSG_TYPE_BUFFER_FROM_HOST: + vchiq_release_message(vchiq_instance, handle, header); + break; + + case MMAL_MSG_TYPE_EVENT_TO_HOST: + event_to_host_cb(instance, msg, msg_len); + vchiq_release_message(vchiq_instance, handle, header); + + break; + + case MMAL_MSG_TYPE_BUFFER_TO_HOST: + buffer_to_host_cb(instance, msg, msg_len); + vchiq_release_message(vchiq_instance, handle, header); + break; + + default: + /* messages dependent on header context to complete */ + if (!msg->h.context) { + pr_err("received message context was null!\n"); + vchiq_release_message(vchiq_instance, handle, header); + break; + } + + msg_context = lookup_msg_context(instance, + msg->h.context); + if (!msg_context) { + pr_err("received invalid message context %u!\n", + msg->h.context); + vchiq_release_message(vchiq_instance, handle, header); + break; + } + + /* fill in context values */ + msg_context->u.sync.msg_handle = header; + msg_context->u.sync.msg = msg; + msg_context->u.sync.msg_len = msg_len; + + /* todo: should this check (completion_done() + * == 1) for no one waiting? or do we need a + * flag to tell us the completion has been + * interrupted so we can free the message and + * its context. This probably also solves the + * message arriving after interruption todo + * below + */ + + /* complete message so caller knows it happened */ + complete(&msg_context->u.sync.cmplt); + break; + } + + break; + + case VCHIQ_BULK_RECEIVE_DONE: + bulk_receive_cb(instance, bulk_ctx); + break; + + case VCHIQ_BULK_RECEIVE_ABORTED: + bulk_abort_cb(instance, bulk_ctx); + break; + + case VCHIQ_SERVICE_CLOSED: + /* TODO: consider if this requires action if received when + * driver is not explicitly closing the service + */ + break; + + default: + pr_err("Received unhandled message reason %d\n", reason); + break; + } + + return VCHIQ_SUCCESS; +} + +static int send_synchronous_mmal_msg(struct vchiq_mmal_instance *instance, + struct mmal_msg *msg, + unsigned int payload_len, + struct mmal_msg **msg_out, + struct vchiq_header **msg_handle) +{ + struct mmal_msg_context *msg_context; + int ret; + unsigned long timeout; + + /* payload size must not cause message to exceed max size */ + if (payload_len > + (MMAL_MSG_MAX_SIZE - sizeof(struct mmal_msg_header))) { + pr_err("payload length %d exceeds max:%d\n", payload_len, + (int)(MMAL_MSG_MAX_SIZE - + sizeof(struct mmal_msg_header))); + return -EINVAL; + } + + msg_context = get_msg_context(instance); + if (IS_ERR(msg_context)) + return PTR_ERR(msg_context); + + init_completion(&msg_context->u.sync.cmplt); + + msg->h.magic = MMAL_MAGIC; + msg->h.context = msg_context->handle; + msg->h.status = 0; + + DBG_DUMP_MSG(msg, (sizeof(struct mmal_msg_header) + payload_len), + ">>> sync message"); + + vchiq_use_service(instance->vchiq_instance, instance->service_handle); + + ret = vchiq_queue_kernel_message(instance->vchiq_instance, instance->service_handle, msg, + sizeof(struct mmal_msg_header) + + payload_len); + + vchiq_release_service(instance->vchiq_instance, instance->service_handle); + + if (ret) { + pr_err("error %d queuing message\n", ret); + release_msg_context(msg_context); + return ret; + } + + timeout = wait_for_completion_timeout(&msg_context->u.sync.cmplt, + SYNC_MSG_TIMEOUT * HZ); + if (timeout == 0) { + pr_err("timed out waiting for sync completion\n"); + ret = -ETIME; + /* todo: what happens if the message arrives after aborting */ + release_msg_context(msg_context); + return ret; + } + + *msg_out = msg_context->u.sync.msg; + *msg_handle = msg_context->u.sync.msg_handle; + release_msg_context(msg_context); + + return 0; +} + +static void dump_port_info(struct vchiq_mmal_port *port) +{ + pr_debug("port handle:0x%x enabled:%d\n", port->handle, port->enabled); + + pr_debug("buffer minimum num:%d size:%d align:%d\n", + port->minimum_buffer.num, + port->minimum_buffer.size, port->minimum_buffer.alignment); + + pr_debug("buffer recommended num:%d size:%d align:%d\n", + port->recommended_buffer.num, + port->recommended_buffer.size, + port->recommended_buffer.alignment); + + pr_debug("buffer current values num:%d size:%d align:%d\n", + port->current_buffer.num, + port->current_buffer.size, port->current_buffer.alignment); + + pr_debug("elementary stream: type:%d encoding:0x%x variant:0x%x\n", + port->format.type, + port->format.encoding, port->format.encoding_variant); + + pr_debug(" bitrate:%d flags:0x%x\n", + port->format.bitrate, port->format.flags); + + if (port->format.type == MMAL_ES_TYPE_VIDEO) { + pr_debug + ("es video format: width:%d height:%d colourspace:0x%x\n", + port->es.video.width, port->es.video.height, + port->es.video.color_space); + + pr_debug(" : crop xywh %d,%d,%d,%d\n", + port->es.video.crop.x, + port->es.video.crop.y, + port->es.video.crop.width, port->es.video.crop.height); + pr_debug(" : framerate %d/%d aspect %d/%d\n", + port->es.video.frame_rate.numerator, + port->es.video.frame_rate.denominator, + port->es.video.par.numerator, port->es.video.par.denominator); + } +} + +static void port_to_mmal_msg(struct vchiq_mmal_port *port, struct mmal_port *p) +{ + /* todo do readonly fields need setting at all? */ + p->type = port->type; + p->index = port->index; + p->index_all = 0; + p->is_enabled = port->enabled; + p->buffer_num_min = port->minimum_buffer.num; + p->buffer_size_min = port->minimum_buffer.size; + p->buffer_alignment_min = port->minimum_buffer.alignment; + p->buffer_num_recommended = port->recommended_buffer.num; + p->buffer_size_recommended = port->recommended_buffer.size; + + /* only three writable fields in a port */ + p->buffer_num = port->current_buffer.num; + p->buffer_size = port->current_buffer.size; + p->userdata = (u32)(unsigned long)port; +} + +static int port_info_set(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port) +{ + int ret; + struct mmal_msg m; + struct mmal_msg *rmsg; + struct vchiq_header *rmsg_handle; + + pr_debug("setting port info port %p\n", port); + if (!port) + return -1; + dump_port_info(port); + + m.h.type = MMAL_MSG_TYPE_PORT_INFO_SET; + + m.u.port_info_set.component_handle = port->component->handle; + m.u.port_info_set.port_type = port->type; + m.u.port_info_set.port_index = port->index; + + port_to_mmal_msg(port, &m.u.port_info_set.port); + + /* elementary stream format setup */ + m.u.port_info_set.format.type = port->format.type; + m.u.port_info_set.format.encoding = port->format.encoding; + m.u.port_info_set.format.encoding_variant = + port->format.encoding_variant; + m.u.port_info_set.format.bitrate = port->format.bitrate; + m.u.port_info_set.format.flags = port->format.flags; + + memcpy(&m.u.port_info_set.es, &port->es, + sizeof(union mmal_es_specific_format)); + + m.u.port_info_set.format.extradata_size = port->format.extradata_size; + memcpy(&m.u.port_info_set.extradata, port->format.extradata, + port->format.extradata_size); + + ret = send_synchronous_mmal_msg(instance, &m, + sizeof(m.u.port_info_set), + &rmsg, &rmsg_handle); + if (ret) + return ret; + + if (rmsg->h.type != MMAL_MSG_TYPE_PORT_INFO_SET) { + /* got an unexpected message type in reply */ + ret = -EINVAL; + goto release_msg; + } + + /* return operation status */ + ret = -rmsg->u.port_info_get_reply.status; + + pr_debug("%s:result:%d component:0x%x port:%d\n", __func__, ret, + port->component->handle, port->handle); + +release_msg: + vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle); + + return ret; +} + +/* use port info get message to retrieve port information */ +static int port_info_get(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port) +{ + int ret; + struct mmal_msg m; + struct mmal_msg *rmsg; + struct vchiq_header *rmsg_handle; + + /* port info time */ + m.h.type = MMAL_MSG_TYPE_PORT_INFO_GET; + m.u.port_info_get.component_handle = port->component->handle; + m.u.port_info_get.port_type = port->type; + m.u.port_info_get.index = port->index; + + ret = send_synchronous_mmal_msg(instance, &m, + sizeof(m.u.port_info_get), + &rmsg, &rmsg_handle); + if (ret) + return ret; + + if (rmsg->h.type != MMAL_MSG_TYPE_PORT_INFO_GET) { + /* got an unexpected message type in reply */ + ret = -EINVAL; + goto release_msg; + } + + /* return operation status */ + ret = -rmsg->u.port_info_get_reply.status; + if (ret != MMAL_MSG_STATUS_SUCCESS) + goto release_msg; + + if (rmsg->u.port_info_get_reply.port.is_enabled == 0) + port->enabled = 0; + else + port->enabled = 1; + + /* copy the values out of the message */ + port->handle = rmsg->u.port_info_get_reply.port_handle; + + /* port type and index cached to use on port info set because + * it does not use a port handle + */ + port->type = rmsg->u.port_info_get_reply.port_type; + port->index = rmsg->u.port_info_get_reply.port_index; + + port->minimum_buffer.num = + rmsg->u.port_info_get_reply.port.buffer_num_min; + port->minimum_buffer.size = + rmsg->u.port_info_get_reply.port.buffer_size_min; + port->minimum_buffer.alignment = + rmsg->u.port_info_get_reply.port.buffer_alignment_min; + + port->recommended_buffer.alignment = + rmsg->u.port_info_get_reply.port.buffer_alignment_min; + port->recommended_buffer.num = + rmsg->u.port_info_get_reply.port.buffer_num_recommended; + + port->current_buffer.num = rmsg->u.port_info_get_reply.port.buffer_num; + port->current_buffer.size = + rmsg->u.port_info_get_reply.port.buffer_size; + + /* stream format */ + port->format.type = rmsg->u.port_info_get_reply.format.type; + port->format.encoding = rmsg->u.port_info_get_reply.format.encoding; + port->format.encoding_variant = + rmsg->u.port_info_get_reply.format.encoding_variant; + port->format.bitrate = rmsg->u.port_info_get_reply.format.bitrate; + port->format.flags = rmsg->u.port_info_get_reply.format.flags; + + /* elementary stream format */ + memcpy(&port->es, + &rmsg->u.port_info_get_reply.es, + sizeof(union mmal_es_specific_format)); + port->format.es = &port->es; + + port->format.extradata_size = + rmsg->u.port_info_get_reply.format.extradata_size; + memcpy(port->format.extradata, + rmsg->u.port_info_get_reply.extradata, + port->format.extradata_size); + + pr_debug("received port info\n"); + dump_port_info(port); + +release_msg: + + pr_debug("%s:result:%d component:0x%x port:%d\n", + __func__, ret, port->component->handle, port->handle); + + vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle); + + return ret; +} + +/* create component on vc */ +static int create_component(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_component *component, + const char *name) +{ + int ret; + struct mmal_msg m; + struct mmal_msg *rmsg; + struct vchiq_header *rmsg_handle; + + /* build component create message */ + m.h.type = MMAL_MSG_TYPE_COMPONENT_CREATE; + m.u.component_create.client_component = component->client_component; + strncpy(m.u.component_create.name, name, + sizeof(m.u.component_create.name)); + + ret = send_synchronous_mmal_msg(instance, &m, + sizeof(m.u.component_create), + &rmsg, &rmsg_handle); + if (ret) + return ret; + + if (rmsg->h.type != m.h.type) { + /* got an unexpected message type in reply */ + ret = -EINVAL; + goto release_msg; + } + + ret = -rmsg->u.component_create_reply.status; + if (ret != MMAL_MSG_STATUS_SUCCESS) + goto release_msg; + + /* a valid component response received */ + component->handle = rmsg->u.component_create_reply.component_handle; + component->inputs = rmsg->u.component_create_reply.input_num; + component->outputs = rmsg->u.component_create_reply.output_num; + component->clocks = rmsg->u.component_create_reply.clock_num; + + pr_debug("Component handle:0x%x in:%d out:%d clock:%d\n", + component->handle, + component->inputs, component->outputs, component->clocks); + +release_msg: + vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle); + + return ret; +} + +/* destroys a component on vc */ +static int destroy_component(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_component *component) +{ + int ret; + struct mmal_msg m; + struct mmal_msg *rmsg; + struct vchiq_header *rmsg_handle; + + m.h.type = MMAL_MSG_TYPE_COMPONENT_DESTROY; + m.u.component_destroy.component_handle = component->handle; + + ret = send_synchronous_mmal_msg(instance, &m, + sizeof(m.u.component_destroy), + &rmsg, &rmsg_handle); + if (ret) + return ret; + + if (rmsg->h.type != m.h.type) { + /* got an unexpected message type in reply */ + ret = -EINVAL; + goto release_msg; + } + + ret = -rmsg->u.component_destroy_reply.status; + +release_msg: + + vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle); + + return ret; +} + +/* enable a component on vc */ +static int enable_component(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_component *component) +{ + int ret; + struct mmal_msg m; + struct mmal_msg *rmsg; + struct vchiq_header *rmsg_handle; + + m.h.type = MMAL_MSG_TYPE_COMPONENT_ENABLE; + m.u.component_enable.component_handle = component->handle; + + ret = send_synchronous_mmal_msg(instance, &m, + sizeof(m.u.component_enable), + &rmsg, &rmsg_handle); + if (ret) + return ret; + + if (rmsg->h.type != m.h.type) { + /* got an unexpected message type in reply */ + ret = -EINVAL; + goto release_msg; + } + + ret = -rmsg->u.component_enable_reply.status; + +release_msg: + vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle); + + return ret; +} + +/* disable a component on vc */ +static int disable_component(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_component *component) +{ + int ret; + struct mmal_msg m; + struct mmal_msg *rmsg; + struct vchiq_header *rmsg_handle; + + m.h.type = MMAL_MSG_TYPE_COMPONENT_DISABLE; + m.u.component_disable.component_handle = component->handle; + + ret = send_synchronous_mmal_msg(instance, &m, + sizeof(m.u.component_disable), + &rmsg, &rmsg_handle); + if (ret) + return ret; + + if (rmsg->h.type != m.h.type) { + /* got an unexpected message type in reply */ + ret = -EINVAL; + goto release_msg; + } + + ret = -rmsg->u.component_disable_reply.status; + +release_msg: + + vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle); + + return ret; +} + +/* get version of mmal implementation */ +static int get_version(struct vchiq_mmal_instance *instance, + u32 *major_out, u32 *minor_out) +{ + int ret; + struct mmal_msg m; + struct mmal_msg *rmsg; + struct vchiq_header *rmsg_handle; + + m.h.type = MMAL_MSG_TYPE_GET_VERSION; + + ret = send_synchronous_mmal_msg(instance, &m, + sizeof(m.u.version), + &rmsg, &rmsg_handle); + if (ret) + return ret; + + if (rmsg->h.type != m.h.type) { + /* got an unexpected message type in reply */ + ret = -EINVAL; + goto release_msg; + } + + *major_out = rmsg->u.version.major; + *minor_out = rmsg->u.version.minor; + +release_msg: + vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle); + + return ret; +} + +/* do a port action with a port as a parameter */ +static int port_action_port(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + enum mmal_msg_port_action_type action_type) +{ + int ret; + struct mmal_msg m; + struct mmal_msg *rmsg; + struct vchiq_header *rmsg_handle; + + m.h.type = MMAL_MSG_TYPE_PORT_ACTION; + m.u.port_action_port.component_handle = port->component->handle; + m.u.port_action_port.port_handle = port->handle; + m.u.port_action_port.action = action_type; + + port_to_mmal_msg(port, &m.u.port_action_port.port); + + ret = send_synchronous_mmal_msg(instance, &m, + sizeof(m.u.port_action_port), + &rmsg, &rmsg_handle); + if (ret) + return ret; + + if (rmsg->h.type != MMAL_MSG_TYPE_PORT_ACTION) { + /* got an unexpected message type in reply */ + ret = -EINVAL; + goto release_msg; + } + + ret = -rmsg->u.port_action_reply.status; + + pr_debug("%s:result:%d component:0x%x port:%d action:%s(%d)\n", + __func__, + ret, port->component->handle, port->handle, + port_action_type_names[action_type], action_type); + +release_msg: + vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle); + + return ret; +} + +/* do a port action with handles as parameters */ +static int port_action_handle(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + enum mmal_msg_port_action_type action_type, + u32 connect_component_handle, + u32 connect_port_handle) +{ + int ret; + struct mmal_msg m; + struct mmal_msg *rmsg; + struct vchiq_header *rmsg_handle; + + m.h.type = MMAL_MSG_TYPE_PORT_ACTION; + + m.u.port_action_handle.component_handle = port->component->handle; + m.u.port_action_handle.port_handle = port->handle; + m.u.port_action_handle.action = action_type; + + m.u.port_action_handle.connect_component_handle = + connect_component_handle; + m.u.port_action_handle.connect_port_handle = connect_port_handle; + + ret = send_synchronous_mmal_msg(instance, &m, + sizeof(m.u.port_action_handle), + &rmsg, &rmsg_handle); + if (ret) + return ret; + + if (rmsg->h.type != MMAL_MSG_TYPE_PORT_ACTION) { + /* got an unexpected message type in reply */ + ret = -EINVAL; + goto release_msg; + } + + ret = -rmsg->u.port_action_reply.status; + + pr_debug("%s:result:%d component:0x%x port:%d action:%s(%d) connect component:0x%x connect port:%d\n", + __func__, + ret, port->component->handle, port->handle, + port_action_type_names[action_type], + action_type, connect_component_handle, connect_port_handle); + +release_msg: + vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle); + + return ret; +} + +static int port_parameter_set(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + u32 parameter_id, void *value, u32 value_size) +{ + int ret; + struct mmal_msg m; + struct mmal_msg *rmsg; + struct vchiq_header *rmsg_handle; + + m.h.type = MMAL_MSG_TYPE_PORT_PARAMETER_SET; + + m.u.port_parameter_set.component_handle = port->component->handle; + m.u.port_parameter_set.port_handle = port->handle; + m.u.port_parameter_set.id = parameter_id; + m.u.port_parameter_set.size = (2 * sizeof(u32)) + value_size; + memcpy(&m.u.port_parameter_set.value, value, value_size); + + ret = send_synchronous_mmal_msg(instance, &m, + (4 * sizeof(u32)) + value_size, + &rmsg, &rmsg_handle); + if (ret) + return ret; + + if (rmsg->h.type != MMAL_MSG_TYPE_PORT_PARAMETER_SET) { + /* got an unexpected message type in reply */ + ret = -EINVAL; + goto release_msg; + } + + ret = -rmsg->u.port_parameter_set_reply.status; + + pr_debug("%s:result:%d component:0x%x port:%d parameter:%d\n", + __func__, + ret, port->component->handle, port->handle, parameter_id); + +release_msg: + vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle); + + return ret; +} + +static int port_parameter_get(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + u32 parameter_id, void *value, u32 *value_size) +{ + int ret; + struct mmal_msg m; + struct mmal_msg *rmsg; + struct vchiq_header *rmsg_handle; + + m.h.type = MMAL_MSG_TYPE_PORT_PARAMETER_GET; + + m.u.port_parameter_get.component_handle = port->component->handle; + m.u.port_parameter_get.port_handle = port->handle; + m.u.port_parameter_get.id = parameter_id; + m.u.port_parameter_get.size = (2 * sizeof(u32)) + *value_size; + + ret = send_synchronous_mmal_msg(instance, &m, + sizeof(struct + mmal_msg_port_parameter_get), + &rmsg, &rmsg_handle); + if (ret) + return ret; + + if (rmsg->h.type != MMAL_MSG_TYPE_PORT_PARAMETER_GET) { + /* got an unexpected message type in reply */ + pr_err("Incorrect reply type %d\n", rmsg->h.type); + ret = -EINVAL; + goto release_msg; + } + + ret = rmsg->u.port_parameter_get_reply.status; + + /* port_parameter_get_reply.size includes the header, + * whilst *value_size doesn't. + */ + rmsg->u.port_parameter_get_reply.size -= (2 * sizeof(u32)); + + if (ret || rmsg->u.port_parameter_get_reply.size > *value_size) { + /* Copy only as much as we have space for + * but report true size of parameter + */ + memcpy(value, &rmsg->u.port_parameter_get_reply.value, + *value_size); + } else { + memcpy(value, &rmsg->u.port_parameter_get_reply.value, + rmsg->u.port_parameter_get_reply.size); + } + /* Always report the size of the returned parameter to the caller */ + *value_size = rmsg->u.port_parameter_get_reply.size; + + pr_debug("%s:result:%d component:0x%x port:%d parameter:%d\n", __func__, + ret, port->component->handle, port->handle, parameter_id); + +release_msg: + vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle); + + return ret; +} + +/* disables a port and drains buffers from it */ +static int port_disable(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port) +{ + int ret; + struct list_head *q, *buf_head; + unsigned long flags = 0; + + if (!port->enabled) + return 0; + + port->enabled = 0; + + ret = port_action_port(instance, port, + MMAL_MSG_PORT_ACTION_TYPE_DISABLE); + if (ret == 0) { + /* + * Drain all queued buffers on port. This should only + * apply to buffers that have been queued before the port + * has been enabled. If the port has been enabled and buffers + * passed, then the buffers should have been removed from this + * list, and we should get the relevant callbacks via VCHIQ + * to release the buffers. + */ + spin_lock_irqsave(&port->slock, flags); + + list_for_each_safe(buf_head, q, &port->buffers) { + struct mmal_buffer *mmalbuf; + + mmalbuf = list_entry(buf_head, struct mmal_buffer, + list); + list_del(buf_head); + if (port->buffer_cb) { + mmalbuf->length = 0; + mmalbuf->mmal_flags = 0; + mmalbuf->dts = MMAL_TIME_UNKNOWN; + mmalbuf->pts = MMAL_TIME_UNKNOWN; + port->buffer_cb(instance, + port, 0, mmalbuf); + } + } + + spin_unlock_irqrestore(&port->slock, flags); + + ret = port_info_get(instance, port); + } + + return ret; +} + +/* enable a port */ +static int port_enable(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port) +{ + unsigned int hdr_count; + struct list_head *q, *buf_head; + int ret; + + if (port->enabled) + return 0; + + ret = port_action_port(instance, port, + MMAL_MSG_PORT_ACTION_TYPE_ENABLE); + if (ret) + goto done; + + port->enabled = 1; + + if (port->buffer_cb) { + /* send buffer headers to videocore */ + hdr_count = 1; + list_for_each_safe(buf_head, q, &port->buffers) { + struct mmal_buffer *mmalbuf; + + mmalbuf = list_entry(buf_head, struct mmal_buffer, + list); + ret = buffer_from_host(instance, port, mmalbuf); + if (ret) + goto done; + + list_del(buf_head); + hdr_count++; + if (hdr_count > port->current_buffer.num) + break; + } + } + + ret = port_info_get(instance, port); + +done: + return ret; +} + +/* ------------------------------------------------------------------ + * Exported API + *------------------------------------------------------------------ + */ + +int vchiq_mmal_port_set_format(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port) +{ + int ret; + + if (mutex_lock_interruptible(&instance->vchiq_mutex)) + return -EINTR; + + ret = port_info_set(instance, port); + if (ret) + goto release_unlock; + + /* read what has actually been set */ + ret = port_info_get(instance, port); + +release_unlock: + mutex_unlock(&instance->vchiq_mutex); + + return ret; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_port_set_format); + +int vchiq_mmal_port_parameter_set(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + u32 parameter, void *value, u32 value_size) +{ + int ret; + + if (mutex_lock_interruptible(&instance->vchiq_mutex)) + return -EINTR; + + ret = port_parameter_set(instance, port, parameter, value, value_size); + + mutex_unlock(&instance->vchiq_mutex); + + return ret; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_port_parameter_set); + +int vchiq_mmal_port_parameter_get(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + u32 parameter, void *value, u32 *value_size) +{ + int ret; + + if (mutex_lock_interruptible(&instance->vchiq_mutex)) + return -EINTR; + + ret = port_parameter_get(instance, port, parameter, value, value_size); + + mutex_unlock(&instance->vchiq_mutex); + + return ret; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_port_parameter_get); + +/* enable a port + * + * enables a port and queues buffers for satisfying callbacks if we + * provide a callback handler + */ +int vchiq_mmal_port_enable(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + vchiq_mmal_buffer_cb buffer_cb) +{ + int ret; + + if (mutex_lock_interruptible(&instance->vchiq_mutex)) + return -EINTR; + + /* already enabled - noop */ + if (port->enabled) { + ret = 0; + goto unlock; + } + + port->buffer_cb = buffer_cb; + + ret = port_enable(instance, port); + +unlock: + mutex_unlock(&instance->vchiq_mutex); + + return ret; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_port_enable); + +int vchiq_mmal_port_disable(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port) +{ + int ret; + + if (mutex_lock_interruptible(&instance->vchiq_mutex)) + return -EINTR; + + if (!port->enabled) { + mutex_unlock(&instance->vchiq_mutex); + return 0; + } + + ret = port_disable(instance, port); + + mutex_unlock(&instance->vchiq_mutex); + + return ret; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_port_disable); + +/* ports will be connected in a tunneled manner so data buffers + * are not handled by client. + */ +int vchiq_mmal_port_connect_tunnel(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *src, + struct vchiq_mmal_port *dst) +{ + int ret; + + if (mutex_lock_interruptible(&instance->vchiq_mutex)) + return -EINTR; + + /* disconnect ports if connected */ + if (src->connected) { + ret = port_disable(instance, src); + if (ret) { + pr_err("failed disabling src port(%d)\n", ret); + goto release_unlock; + } + + /* do not need to disable the destination port as they + * are connected and it is done automatically + */ + + ret = port_action_handle(instance, src, + MMAL_MSG_PORT_ACTION_TYPE_DISCONNECT, + src->connected->component->handle, + src->connected->handle); + if (ret < 0) { + pr_err("failed disconnecting src port\n"); + goto release_unlock; + } + src->connected->enabled = 0; + src->connected = NULL; + } + + if (!dst) { + /* do not make new connection */ + ret = 0; + pr_debug("not making new connection\n"); + goto release_unlock; + } + + /* copy src port format to dst */ + dst->format.encoding = src->format.encoding; + dst->es.video.width = src->es.video.width; + dst->es.video.height = src->es.video.height; + dst->es.video.crop.x = src->es.video.crop.x; + dst->es.video.crop.y = src->es.video.crop.y; + dst->es.video.crop.width = src->es.video.crop.width; + dst->es.video.crop.height = src->es.video.crop.height; + dst->es.video.frame_rate.numerator = src->es.video.frame_rate.numerator; + dst->es.video.frame_rate.denominator = src->es.video.frame_rate.denominator; + + /* set new format */ + ret = port_info_set(instance, dst); + if (ret) { + pr_debug("setting port info failed\n"); + goto release_unlock; + } + + /* read what has actually been set */ + ret = port_info_get(instance, dst); + if (ret) { + pr_debug("read back port info failed\n"); + goto release_unlock; + } + + /* connect two ports together */ + ret = port_action_handle(instance, src, + MMAL_MSG_PORT_ACTION_TYPE_CONNECT, + dst->component->handle, dst->handle); + if (ret < 0) { + pr_debug("connecting port %d:%d to %d:%d failed\n", + src->component->handle, src->handle, + dst->component->handle, dst->handle); + goto release_unlock; + } + src->connected = dst; + +release_unlock: + + mutex_unlock(&instance->vchiq_mutex); + + return ret; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_port_connect_tunnel); + +int vchiq_mmal_submit_buffer(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + struct mmal_buffer *buffer) +{ + unsigned long flags = 0; + int ret; + + ret = buffer_from_host(instance, port, buffer); + if (ret == -EINVAL) { + /* Port is disabled. Queue for when it is enabled. */ + spin_lock_irqsave(&port->slock, flags); + list_add_tail(&buffer->list, &port->buffers); + spin_unlock_irqrestore(&port->slock, flags); + } + + return 0; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_submit_buffer); + +int mmal_vchi_buffer_init(struct vchiq_mmal_instance *instance, + struct mmal_buffer *buf) +{ + struct mmal_msg_context *msg_context = get_msg_context(instance); + + if (IS_ERR(msg_context)) + return (PTR_ERR(msg_context)); + + buf->msg_context = msg_context; + return 0; +} +EXPORT_SYMBOL_GPL(mmal_vchi_buffer_init); + +int mmal_vchi_buffer_cleanup(struct mmal_buffer *buf) +{ + struct mmal_msg_context *msg_context = buf->msg_context; + + if (msg_context) + release_msg_context(msg_context); + buf->msg_context = NULL; + + return 0; +} +EXPORT_SYMBOL_GPL(mmal_vchi_buffer_cleanup); + +/* Initialise a mmal component and its ports + * + */ +int vchiq_mmal_component_init(struct vchiq_mmal_instance *instance, + const char *name, + struct vchiq_mmal_component **component_out) +{ + int ret; + int idx; /* port index */ + struct vchiq_mmal_component *component = NULL; + + if (mutex_lock_interruptible(&instance->vchiq_mutex)) + return -EINTR; + + for (idx = 0; idx < VCHIQ_MMAL_MAX_COMPONENTS; idx++) { + if (!instance->component[idx].in_use) { + component = &instance->component[idx]; + component->in_use = 1; + break; + } + } + + if (!component) { + ret = -EINVAL; /* todo is this correct error? */ + goto unlock; + } + + /* We need a handle to reference back to our component structure. + * Use the array index in instance->component rather than rolling + * another IDR. + */ + component->client_component = idx; + + ret = create_component(instance, component, name); + if (ret < 0) { + pr_err("%s: failed to create component %d (Not enough GPU mem?)\n", + __func__, ret); + goto unlock; + } + + /* ports info needs gathering */ + component->control.type = MMAL_PORT_TYPE_CONTROL; + component->control.index = 0; + component->control.component = component; + spin_lock_init(&component->control.slock); + INIT_LIST_HEAD(&component->control.buffers); + ret = port_info_get(instance, &component->control); + if (ret < 0) + goto release_component; + + for (idx = 0; idx < component->inputs; idx++) { + component->input[idx].type = MMAL_PORT_TYPE_INPUT; + component->input[idx].index = idx; + component->input[idx].component = component; + spin_lock_init(&component->input[idx].slock); + INIT_LIST_HEAD(&component->input[idx].buffers); + ret = port_info_get(instance, &component->input[idx]); + if (ret < 0) + goto release_component; + } + + for (idx = 0; idx < component->outputs; idx++) { + component->output[idx].type = MMAL_PORT_TYPE_OUTPUT; + component->output[idx].index = idx; + component->output[idx].component = component; + spin_lock_init(&component->output[idx].slock); + INIT_LIST_HEAD(&component->output[idx].buffers); + ret = port_info_get(instance, &component->output[idx]); + if (ret < 0) + goto release_component; + } + + for (idx = 0; idx < component->clocks; idx++) { + component->clock[idx].type = MMAL_PORT_TYPE_CLOCK; + component->clock[idx].index = idx; + component->clock[idx].component = component; + spin_lock_init(&component->clock[idx].slock); + INIT_LIST_HEAD(&component->clock[idx].buffers); + ret = port_info_get(instance, &component->clock[idx]); + if (ret < 0) + goto release_component; + } + + *component_out = component; + + mutex_unlock(&instance->vchiq_mutex); + + return 0; + +release_component: + destroy_component(instance, component); +unlock: + if (component) + component->in_use = 0; + mutex_unlock(&instance->vchiq_mutex); + + return ret; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_component_init); + +/* + * cause a mmal component to be destroyed + */ +int vchiq_mmal_component_finalise(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_component *component) +{ + int ret; + + if (mutex_lock_interruptible(&instance->vchiq_mutex)) + return -EINTR; + + if (component->enabled) + ret = disable_component(instance, component); + + ret = destroy_component(instance, component); + + component->in_use = 0; + + mutex_unlock(&instance->vchiq_mutex); + + return ret; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_component_finalise); + +/* + * cause a mmal component to be enabled + */ +int vchiq_mmal_component_enable(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_component *component) +{ + int ret; + + if (mutex_lock_interruptible(&instance->vchiq_mutex)) + return -EINTR; + + if (component->enabled) { + mutex_unlock(&instance->vchiq_mutex); + return 0; + } + + ret = enable_component(instance, component); + if (ret == 0) + component->enabled = true; + + mutex_unlock(&instance->vchiq_mutex); + + return ret; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_component_enable); + +/* + * cause a mmal component to be enabled + */ +int vchiq_mmal_component_disable(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_component *component) +{ + int ret; + + if (mutex_lock_interruptible(&instance->vchiq_mutex)) + return -EINTR; + + if (!component->enabled) { + mutex_unlock(&instance->vchiq_mutex); + return 0; + } + + ret = disable_component(instance, component); + if (ret == 0) + component->enabled = 0; + + mutex_unlock(&instance->vchiq_mutex); + + return ret; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_component_disable); + +int vchiq_mmal_version(struct vchiq_mmal_instance *instance, + u32 *major_out, u32 *minor_out) +{ + int ret; + + if (mutex_lock_interruptible(&instance->vchiq_mutex)) + return -EINTR; + + ret = get_version(instance, major_out, minor_out); + + mutex_unlock(&instance->vchiq_mutex); + + return ret; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_version); + +int vchiq_mmal_finalise(struct vchiq_mmal_instance *instance) +{ + int status = 0; + + if (!instance) + return -EINVAL; + + if (mutex_lock_interruptible(&instance->vchiq_mutex)) + return -EINTR; + + vchiq_use_service(instance->vchiq_instance, instance->service_handle); + + status = vchiq_close_service(instance->vchiq_instance, instance->service_handle); + if (status != 0) + pr_err("mmal-vchiq: VCHIQ close failed\n"); + + mutex_unlock(&instance->vchiq_mutex); + + vchiq_shutdown(instance->vchiq_instance); + destroy_workqueue(instance->bulk_wq); + + idr_destroy(&instance->context_map); + + kfree(instance); + + return status; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_finalise); + +int vchiq_mmal_init(struct vchiq_mmal_instance **out_instance) +{ + int status; + int err = -ENODEV; + struct vchiq_mmal_instance *instance; + struct vchiq_instance *vchiq_instance; + struct vchiq_service_params_kernel params = { + .version = VC_MMAL_VER, + .version_min = VC_MMAL_MIN_VER, + .fourcc = VCHIQ_MAKE_FOURCC('m', 'm', 'a', 'l'), + .callback = service_callback, + .userdata = NULL, + }; + + /* compile time checks to ensure structure size as they are + * directly (de)serialised from memory. + */ + + /* ensure the header structure has packed to the correct size */ + BUILD_BUG_ON(sizeof(struct mmal_msg_header) != 24); + + /* ensure message structure does not exceed maximum length */ + BUILD_BUG_ON(sizeof(struct mmal_msg) > MMAL_MSG_MAX_SIZE); + + /* mmal port struct is correct size */ + BUILD_BUG_ON(sizeof(struct mmal_port) != 64); + + /* create a vchi instance */ + status = vchiq_initialise(&vchiq_instance); + if (status) { + pr_err("Failed to initialise VCHI instance (status=%d)\n", + status); + return -EIO; + } + + status = vchiq_connect(vchiq_instance); + if (status) { + pr_err("Failed to connect VCHI instance (status=%d)\n", status); + err = -EIO; + goto err_shutdown_vchiq; + } + + instance = kzalloc(sizeof(*instance), GFP_KERNEL); + + if (!instance) { + err = -ENOMEM; + goto err_shutdown_vchiq; + } + + mutex_init(&instance->vchiq_mutex); + + instance->vchiq_instance = vchiq_instance; + + mutex_init(&instance->context_map_lock); + idr_init_base(&instance->context_map, 1); + + params.userdata = instance; + + instance->bulk_wq = alloc_ordered_workqueue("mmal-vchiq", + WQ_MEM_RECLAIM); + if (!instance->bulk_wq) + goto err_free; + + status = vchiq_open_service(vchiq_instance, ¶ms, + &instance->service_handle); + if (status) { + pr_err("Failed to open VCHI service connection (status=%d)\n", + status); + goto err_close_services; + } + + vchiq_release_service(instance->vchiq_instance, instance->service_handle); + + *out_instance = instance; + + return 0; + +err_close_services: + vchiq_close_service(instance->vchiq_instance, instance->service_handle); + destroy_workqueue(instance->bulk_wq); +err_free: + kfree(instance); +err_shutdown_vchiq: + vchiq_shutdown(vchiq_instance); + return err; +} +EXPORT_SYMBOL_GPL(vchiq_mmal_init); + +MODULE_DESCRIPTION("BCM2835 MMAL VCHIQ interface"); +MODULE_AUTHOR("Dave Stevenson, <dave.stevenson@raspberrypi.org>"); +MODULE_LICENSE("GPL"); diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.h new file mode 100644 index 000000000..6006e2923 --- /dev/null +++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.h @@ -0,0 +1,168 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * Broadcom BCM2835 V4L2 driver + * + * Copyright © 2013 Raspberry Pi (Trading) Ltd. + * + * Authors: Vincent Sanders @ Collabora + * Dave Stevenson @ Broadcom + * (now dave.stevenson@raspberrypi.org) + * Simon Mellor @ Broadcom + * Luke Diamand @ Broadcom + * + * MMAL interface to VCHIQ message passing + */ + +#ifndef MMAL_VCHIQ_H +#define MMAL_VCHIQ_H + +#include "mmal-common.h" +#include "mmal-msg-format.h" + +#define MAX_PORT_COUNT 4 + +/* Maximum size of the format extradata. */ +#define MMAL_FORMAT_EXTRADATA_MAX_SIZE 128 + +struct vchiq_mmal_instance; + +enum vchiq_mmal_es_type { + MMAL_ES_TYPE_UNKNOWN, /**< Unknown elementary stream type */ + MMAL_ES_TYPE_CONTROL, /**< Elementary stream of control commands */ + MMAL_ES_TYPE_AUDIO, /**< Audio elementary stream */ + MMAL_ES_TYPE_VIDEO, /**< Video elementary stream */ + MMAL_ES_TYPE_SUBPICTURE /**< Sub-picture elementary stream */ +}; + +struct vchiq_mmal_port_buffer { + unsigned int num; /* number of buffers */ + u32 size; /* size of buffers */ + u32 alignment; /* alignment of buffers */ +}; + +struct vchiq_mmal_port; + +typedef void (*vchiq_mmal_buffer_cb)( + struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + int status, struct mmal_buffer *buffer); + +struct vchiq_mmal_port { + u32 enabled:1; + u32 handle; + u32 type; /* port type, cached to use on port info set */ + u32 index; /* port index, cached to use on port info set */ + + /* component port belongs to, allows simple deref */ + struct vchiq_mmal_component *component; + + struct vchiq_mmal_port *connected; /* port connected to */ + + /* buffer info */ + struct vchiq_mmal_port_buffer minimum_buffer; + struct vchiq_mmal_port_buffer recommended_buffer; + struct vchiq_mmal_port_buffer current_buffer; + + /* stream format */ + struct mmal_es_format_local format; + /* elementary stream format */ + union mmal_es_specific_format es; + + /* data buffers to fill */ + struct list_head buffers; + /* lock to serialise adding and removing buffers from list */ + spinlock_t slock; + + /* Count of buffers the VPU has yet to return */ + atomic_t buffers_with_vpu; + /* callback on buffer completion */ + vchiq_mmal_buffer_cb buffer_cb; + /* callback context */ + void *cb_ctx; +}; + +struct vchiq_mmal_component { + u32 in_use:1; + u32 enabled:1; + u32 handle; /* VideoCore handle for component */ + u32 inputs; /* Number of input ports */ + u32 outputs; /* Number of output ports */ + u32 clocks; /* Number of clock ports */ + struct vchiq_mmal_port control; /* control port */ + struct vchiq_mmal_port input[MAX_PORT_COUNT]; /* input ports */ + struct vchiq_mmal_port output[MAX_PORT_COUNT]; /* output ports */ + struct vchiq_mmal_port clock[MAX_PORT_COUNT]; /* clock ports */ + u32 client_component; /* Used to ref back to client struct */ +}; + +int vchiq_mmal_init(struct vchiq_mmal_instance **out_instance); +int vchiq_mmal_finalise(struct vchiq_mmal_instance *instance); + +/* Initialise a mmal component and its ports + * + */ +int vchiq_mmal_component_init( + struct vchiq_mmal_instance *instance, + const char *name, + struct vchiq_mmal_component **component_out); + +int vchiq_mmal_component_finalise( + struct vchiq_mmal_instance *instance, + struct vchiq_mmal_component *component); + +int vchiq_mmal_component_enable( + struct vchiq_mmal_instance *instance, + struct vchiq_mmal_component *component); + +int vchiq_mmal_component_disable( + struct vchiq_mmal_instance *instance, + struct vchiq_mmal_component *component); + +/* enable a mmal port + * + * enables a port and if a buffer callback provided enque buffer + * headers as appropriate for the port. + */ +int vchiq_mmal_port_enable( + struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + vchiq_mmal_buffer_cb buffer_cb); + +/* disable a port + * + * disable a port will dequeue any pending buffers + */ +int vchiq_mmal_port_disable(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port); + +int vchiq_mmal_port_parameter_set(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + u32 parameter, + void *value, + u32 value_size); + +int vchiq_mmal_port_parameter_get(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + u32 parameter, + void *value, + u32 *value_size); + +int vchiq_mmal_port_set_format(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port); + +int vchiq_mmal_port_connect_tunnel(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *src, + struct vchiq_mmal_port *dst); + +int vchiq_mmal_version(struct vchiq_mmal_instance *instance, + u32 *major_out, + u32 *minor_out); + +int vchiq_mmal_submit_buffer(struct vchiq_mmal_instance *instance, + struct vchiq_mmal_port *port, + struct mmal_buffer *buf); + +int mmal_vchi_buffer_init(struct vchiq_mmal_instance *instance, + struct mmal_buffer *buf); +int mmal_vchi_buffer_cleanup(struct mmal_buffer *buf); +#endif /* MMAL_VCHIQ_H */ |