From 45b7e7572db587057974a73316afa386f3b4312f Mon Sep 17 00:00:00 2001 From: =?utf8?q?Sjur=20Br=C3=A6ndeland?= Date: Fri, 9 Dec 2011 12:00:18 +0100 Subject: [PATCH] modem_shm: SHM Configuration data handling MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit Add configuration handling of modem shared memory. A gen-netlink endpoint for "modem-shm" is implemented handling channel configuration requests. The channel configuration is stored in the device and in shared memory. Signed-off-by: Sjur Brændeland --- drivers/modem_shm/shm_boot.c | 1208 ++++++++++++++++++++++++++++++++++ 1 file changed, 1208 insertions(+) create mode 100644 drivers/modem_shm/shm_boot.c diff --git a/drivers/modem_shm/shm_boot.c b/drivers/modem_shm/shm_boot.c new file mode 100644 index 000000000000..87ff0539878c --- /dev/null +++ b/drivers/modem_shm/shm_boot.c @@ -0,0 +1,1208 @@ +/* + * Copyright (C) ST-Ericsson AB 2012 + * Author: Sjur Brændeland / sjur.brandeland@stericsson.com + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":" fmt +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "shm_ipctoc.h" +#include +#include +#include + +MODULE_LICENSE("GPL"); +MODULE_AUTHOR("Sjur Brændland "); + +#define SHM_VERSION 0x1 +#define SHM_SUBVER 0x0 +#define TOC_SZ 512 +#define IMG_MAX_SZ 65536 +#define SHM_ALIGNMT sizeof(u32) +#define SHM_MIN_CHSZ 3 +#define SHM_PAYL_ALIGN max(32, L1_CACHE_BYTES) +#define STE_BOOTIMG_NAME "ste-bootimg.bin" + +#define GET_OFFSET(base, ptr) (((u8 *)(ptr)) - ((u8 *)(base))) +#define OFFS2PTR(base, offs) ((void *) ((u8 *)base + offs)) +#define LEOFFS2PTR(base, offs) ((void *) ((u8 *)base + le32_to_cpu(offs))) + +/* Structure use in debug mode for integrity checking */ +struct ipctoc_hash { + u16 img_hash; + u16 ch_hash; + u16 ch_size; +}; +struct shm_modem { + bool config_error; + bool commited; + bool registered; + bool addr_set; + bool fw_requested; + u32 modem_bootimg_size; + void *shm_start; + u32 channels; + struct shm_channel *channel[SHM_MAX_CHANNELS + 1]; + struct shm_ipctoc *ipctoc; + bool gennetl_reg; +}; + +static unsigned long shm_start; +module_param(shm_start, ulong, S_IRUGO | S_IWUSR); +MODULE_PARM_DESC(shm_start, "Address for memory shared by host/modem."); + +static unsigned long shm_c2c_bootaddr; +module_param(shm_c2c_bootaddr, ulong, S_IRUGO | S_IWUSR); +MODULE_PARM_DESC(shm_c2c_bootaddr, + "Address given to modem (through GENI register)"); + +static long shm_size; +module_param(shm_size, long, S_IRUGO | S_IWUSR); +MODULE_PARM_DESC(shm_size, "Size of SHM area"); + +#ifdef DEBUG + +/* In debug mode we pad around all payload area in order to detect overwrite */ +#define MAGIC_PAD_LEN 32 +#define MAGIC_PAD 0xbc + +/* Verify a magic-pad area */ +static inline bool padok(void *mag) +{ + u32 *p = mag, v = 0xbcbcbcbc; + int i; + + for (i = 0; i < 8; i++) + if (*p++ != v) + return false; + return true; +} + +/* Insert a magic-pad area */ +static inline void add_magic_pad(u32 *offset, void *base) +{ + if (*offset < shm_size) + memset(base + *offset, MAGIC_PAD, MAGIC_PAD_LEN); + *offset += MAGIC_PAD_LEN; +} + +/* Abuse the pad area to create a checksum of the ipc-toc and descriptors */ +static inline void store_checksum(struct shm_modem *modem, u32 size) +{ + struct ipctoc_hash *hash = (void *)modem->ipctoc; + --hash; + hash->img_hash = + crc_ccitt(0xffff, (u8 *) modem->shm_start, + modem->modem_bootimg_size); + hash->ch_hash = crc_ccitt(0xffff, (u8 *) modem->ipctoc, size); + hash->ch_size = size; +} + +/* Verify that shm config has not been accidently tampered. */ +static inline bool ok_checksum(struct shm_modem *modem, + struct shm_ipctoc *ipctoc) +{ + struct ipctoc_hash *hash = (void *) ipctoc; + u16 new_hash, new_imghash; + int i; + u8 *p; + + if (!modem->commited) + return false; + + for (i = 0; i < modem->channels; i++) { + struct shm_ipctoc_channel *ch; + + ch = LEOFFS2PTR(modem->shm_start, + ipctoc->channel_offsets[i].rx); + p = LEOFFS2PTR(modem->shm_start, ch->ipc); + if (!padok(p - MAGIC_PAD_LEN)) + return false; + p = LEOFFS2PTR(modem->shm_start, ch->offset); + if (!padok(p - MAGIC_PAD_LEN)) + return false; + ch = LEOFFS2PTR(modem->shm_start, + ipctoc->channel_offsets[i].tx); + p = LEOFFS2PTR(modem->shm_start, ch->ipc); + if (!padok(p - MAGIC_PAD_LEN)) + return false; + p = LEOFFS2PTR(modem->shm_start, ch->offset); + if (!padok(p - MAGIC_PAD_LEN)) + return false; + } + + --hash; + new_hash = crc_ccitt(0xffff, (u8 *) ipctoc, hash->ch_size); + new_imghash = + crc_ccitt(0xffff, (u8 *) modem->shm_start, + modem->modem_bootimg_size); + pr_debug("Hash result:size:%d chksm:%u/%u img:%u/%u\n", + hash->ch_size, hash->ch_hash, new_hash, + hash->img_hash, new_imghash); + return hash->ch_hash == new_hash && hash->img_hash == new_imghash; +} + +static inline void init_data(void *shm_start, u32 offset, + int ch, u32 size) +{ + memset((u8 *)shm_start + offset, ch + 1, size); +} +#else +#define MAGIC_PAD_LEN 0 +static inline void add_magic_pad(u32 *offset, void *base) +{ +} +static inline void store_checksum(struct shm_modem *modem, u32 size) +{ +} +static inline bool ok_checksum(struct shm_modem *modem, void *ipctoc) +{ + return true; +} +static inline void init_data(struct shm_modem *modem, u32 offs, int ch, + u32 size) +{ +} +#endif + +/* write_to_shm - Write SHM Channel descriptors to SHM. + * + * Based on the configuration data channel configuration + * is written to the shared memory area. + * This is the data layout: + * + * +------------+ <---- shm_start + * | TOC | + * +------------+ + * | Boot IMG | + * +------------+ <---- rw_start + * | RW Data | + * +------------+ + * | RW Buf idx | + * +------------+ <---- ipctoc + * | IPC TOC | + * +------------+ + * | RW Ch Decr | + * +------------+ <---- ro_start + * | RO Ch Decr | + * +------------+ + * | RO Buf idx | + * +------------+ + * | RO Data | + * +------------+ + */ + +static int write_to_shm(struct shm_modem *modem) +{ + int i, pri, bitno; + u32 offset, ro_start, rw_start, ipctoc_offs, ipcro_offs; + bool found; + struct shm_ipctoc_channel *ch; + struct toc_entry *toc_entry; + struct shm_bufidx *bix; + + /* + * Find where to put IPC-TOC by adding up + * the size of Payload buffers pluss buf-indices + */ + ipctoc_offs = ALIGN(modem->modem_bootimg_size, SHM_PAYL_ALIGN); + rw_start = ipctoc_offs; + for (i = 0; i < modem->channels; i++) { + int n = modem->channel[i]->tx.buffers; + ipctoc_offs += MAGIC_PAD_LEN; + ipctoc_offs += offsetof(struct shm_bufidx, size[n + 2]); + ipctoc_offs = ALIGN(ipctoc_offs, SHM_PAYL_ALIGN); + ipctoc_offs += MAGIC_PAD_LEN; + ipctoc_offs += modem->channel[i]->tx.ch_size; + ipctoc_offs = ALIGN(ipctoc_offs, SHM_PAYL_ALIGN); + } + add_magic_pad(&ipctoc_offs, modem->shm_start); + pr_debug("IPC toc @ %08x\n", ipctoc_offs); + + /* + * Allocate the IPC-TOC and, initiatlize it. + * The IPC toc will be located after the RW Data and + * buffer indices. + */ + offset = ipctoc_offs; + modem->ipctoc = OFFS2PTR(modem->shm_start, ipctoc_offs); + modem->ipctoc->magic[0] = SHM_IPCTOC_MAGIC1; + modem->ipctoc->magic[1] = SHM_IPCTOC_MAGIC2; + modem->ipctoc->version = SHM_VERSION; + modem->ipctoc->subver = SHM_SUBVER; + memset(modem->ipctoc->channel_offsets, 0, + sizeof(modem->ipctoc->channel_offsets)); + + /* Find start of first channel descriptor */ + offset += sizeof(struct shm_ipctoc); + + /* + * Allocate the location for the RW Channel descriptors. + * It will be located after the IPC-TOC. + */ + offset = ALIGN(offset, SHM_ALIGNMT); + for (i = 0; i < modem->channels; i++) { + pr_debug("Channel descriptor %d RW @ 0x%08x\n", i, offset); + modem->ipctoc->channel_offsets[i].tx = cpu_to_le32(offset); + offset += sizeof(struct shm_ipctoc_channel); + offset = ALIGN(offset, SHM_ALIGNMT); + if (offset > shm_size) + goto badsize; + } + ro_start = offset; + + /* + * Allocate the location for the RO Channel descriptors. + * It will be located after the RW Channels. + */ + for (i = 0; i < modem->channels; i++) { + pr_debug("Channel descriptor %d RO @ 0x%08x\n", i, offset); + modem->ipctoc->channel_offsets[i].rx = cpu_to_le32(offset); + offset += sizeof(struct shm_ipctoc_channel); + offset = ALIGN(offset, SHM_ALIGNMT); + if (offset > shm_size) + goto badsize; + } + + /* + * Allocate the location for the RO Buffer Indices. + * It will be located after the RO Channels. + */ + offset = ALIGN(offset, SHM_PAYL_ALIGN); + ipcro_offs = offset; + for (i = 0; i < modem->channels; i++) { + int n = modem->channel[i]->rx.buffers; + ch = LEOFFS2PTR(modem->shm_start, + modem->ipctoc->channel_offsets[i].rx); + add_magic_pad(&offset, modem->shm_start); + ch->ipc = cpu_to_le32(offset); + + bix = OFFS2PTR(modem->shm_start, offset); + bix->read_index = cpu_to_le32(0); + bix->write_index = cpu_to_le32(0); + bix->state = cpu_to_le32(SHM_CLOSED); + bix->size[0] = cpu_to_le32(0); + + pr_debug("IPC RO[%d] @: 0x%08x\n", i, offset); + offset += offsetof(struct shm_bufidx, size[n + 2]); + offset = ALIGN(offset, SHM_PAYL_ALIGN); + if (offset > shm_size) + goto badsize; + } + + /* + * Allocate RO Data Area. This will located after + * the RO Buffer Indices at the end of the Share Memory + * area. + */ + offset = ALIGN(offset, SHM_PAYL_ALIGN); + for (i = 0; i < modem->channels; i++) { + u8 align; + u32 size; + ch = LEOFFS2PTR(modem->shm_start, + modem->ipctoc->channel_offsets[i].rx); + add_magic_pad(&offset, modem->shm_start); + ch->offset = cpu_to_le32(offset); + + BUILD_BUG_ON(sizeof(ch->mode) != 1); + ch->mode = modem->channel[i]->mode & SHM_MODE_MASK; + ch->buffers = cpu_to_le32(modem->channel[i]->rx.buffers); + align = rounddown_pow_of_two(modem->channel[i]->rx.alignment); + ch->alignment = align; + ch->packets = modem->channel[i]->rx.packets; + ch->mtu = modem->channel[i]->rx.mtu; + size = modem->channel[i]->tx.ch_size; + if (modem->channel[i]->mode & SHM_PACKET_MODE) { + u32 buf_size; + buf_size = size / modem->channel[i]->tx.buffers; + buf_size = rounddown(buf_size, align); + size = buf_size * modem->channel[i]->tx.buffers; + } + pr_debug("Buffer area RO for Channel[%d] at: 0x%08x size:%d\n", + i, offset, size); + ch->size = cpu_to_le32(size); + + init_data(modem->shm_start, offset, i, + modem->channel[i]->rx.ch_size); + offset += modem->channel[i]->rx.ch_size; + offset = ALIGN(offset, SHM_PAYL_ALIGN); + if (offset > shm_size) + goto badsize; + } + + /* + * Allocate RW Data Area. This will located in the beginning + * just after the Modem Boot Image. + */ + offset = rw_start; + for (i = 0; i < modem->channels; i++) { + u8 align; + u32 size; + ch = LEOFFS2PTR(modem->shm_start, + modem->ipctoc->channel_offsets[i].tx); + add_magic_pad(&offset, modem->shm_start); + ch->offset = cpu_to_le32(offset); + init_data(modem->shm_start, offset, i, + modem->channel[i]->tx.ch_size); + ch->mode = modem->channel[i]->mode & + SHM_MODE_MASK; + ch->buffers = cpu_to_le32(modem->channel[i]->tx.buffers); + align = rounddown_pow_of_two(modem->channel[i]->rx.alignment); + ch->alignment = align; + ch->packets = modem->channel[i]->rx.packets; + ch->mtu = modem->channel[i]->rx.mtu; + size = modem->channel[i]->tx.ch_size; + if (modem->channel[i]->mode & SHM_PACKET_MODE) { + u32 buf_size; + buf_size = size / modem->channel[i]->tx.buffers; + buf_size = rounddown(buf_size, align); + size = buf_size * modem->channel[i]->tx.buffers; + } + ch->size = cpu_to_le32(size); + pr_debug("Buffer area RW for Channel[%d] at: 0x%08x size:%d\n", + i, offset, size); + offset += modem->channel[i]->tx.ch_size; + offset = ALIGN(offset, SHM_PAYL_ALIGN); + if (offset > ro_start) + goto badsize; + } + + /* + * Allocate RW IPC Area. This will located after RW data area, + * just before the IPC-TOC. + */ + offset = ALIGN(offset, SHM_PAYL_ALIGN); + for (i = 0; i < modem->channels; i++) { + int n = modem->channel[i]->tx.buffers; + ch = LEOFFS2PTR(modem->shm_start, + modem->ipctoc->channel_offsets[i].tx); + add_magic_pad(&offset, modem->shm_start); + ch->ipc = cpu_to_le32(offset); + bix = OFFS2PTR(modem->shm_start, offset); + bix->read_index = cpu_to_le32(0); + bix->write_index = cpu_to_le32(0); + bix->state = cpu_to_le32(SHM_CLOSED); + bix->size[0] = cpu_to_le32(0); + + pr_debug("IPC RW[%d] @: 0x%08x\n", i, offset); + offset += offsetof(struct shm_bufidx, size[n + 2]); + offset = ALIGN(offset, SHM_PAYL_ALIGN); + if (offset > shm_size) + goto badsize; + } + + /* Allocate genio bits for each channel according to priority*/ + bitno = 0; + for (pri = 0; pri < 8; pri++) { + for (i = 0; i < modem->channels; i++) { + if (modem->channel[i]->priority == pri) { + ch = LEOFFS2PTR(modem->shm_start, + modem->ipctoc->channel_offsets[i].tx); + ch->write_bit = cpu_to_le16(bitno * 4); + ch->read_bit = cpu_to_le16(bitno * 4 + 2); + ch = LEOFFS2PTR(modem->shm_start, + modem->ipctoc->channel_offsets[i].rx); + ch->write_bit = cpu_to_le16(bitno * 4 + 1); + ch->read_bit = cpu_to_le16(bitno * 4 + 3); + bitno++; + } + } + } + + /* + * The Master TOC points out the boot images for the modem, + * Use the first avilable entry in the toc to write the pointer, + * to the IPC-TOC defined above. + */ + found = false; + for (toc_entry = modem->shm_start, i = 0; i < 16; i++, toc_entry++) + if (toc_entry->start == cpu_to_le32(0xffffffff)) { + pr_debug("IPCTOC address written into Master TOC" + " @ 0x%08x\n", i * 32); + toc_entry->start = + cpu_to_le32(GET_OFFSET(modem->shm_start, + modem->ipctoc)); + toc_entry->size = cpu_to_le32(0); + toc_entry->flags = cpu_to_le32(0); + toc_entry->entry_point = cpu_to_le32(0); + toc_entry->load_addr = cpu_to_le32(0xffffffff); + memset(toc_entry->name, 0, sizeof(toc_entry->name)); + sprintf(toc_entry->name, "ipc-toc"); + found = true; + break; + } + if (!found) { + pr_debug("Cannot insert IPC-TOC in toc\n"); + goto bad_config; + } + + store_checksum(modem, ipcro_offs - ipctoc_offs); + + return 0; + +badsize: + pr_debug("IPCTOC not enough space offset (size:0x%lx offset:0x%x\n", + shm_size, offset); + return -ENOSPC; + +bad_config: + pr_debug("IPCTOC bad configuration data\n"); + return -EINVAL; +} + +static int shm_verify_config(struct shm_modem *modem, + struct shm_channel *xcfg) +{ + int j; + u32 size; + + if (modem->channels <= 0 || modem->channels > SHM_MAX_CHANNELS) { + pr_debug("Bad config: channel mode must be set\n"); + return -EINVAL; + } + + if ((xcfg->mode & SHM_MODE_MASK) != SHM_PACKET_MODE && + (xcfg->mode & SHM_MODE_MASK) != SHM_STREAM_MODE) { + pr_debug("Bad config: channel mode must be set\n"); + return -EINVAL; + } + if (xcfg->mode & SHM_PACKET_MODE && xcfg->rx.buffers < 2) { + pr_debug("Bad config:minimum 2 buffers " + "must be set for packet mode\n"); + return -EINVAL; + } + + if (xcfg->rx.ch_size < SHM_MIN_CHSZ) { + pr_debug("Bad config:" + "Channel size must be larger than %d\n", + SHM_MIN_CHSZ); + return -EINVAL; + } + + if (xcfg->mode & SHM_PACKET_MODE) { + if (xcfg->tx.buffers < 2) { + pr_debug("Bad config:" + "buffers must be minimum 2 packet mode\n"); + return -EINVAL; + } + if (xcfg->tx.packets < 1) { + pr_debug("Bad config:" + "packets must be set for packet mode\n"); + return -EINVAL; + } + } + + if (xcfg->tx.ch_size < SHM_MIN_CHSZ) { + pr_debug("Bad config:" + "Channel size must be larger than %d\n", + SHM_MIN_CHSZ); + return -EINVAL; + } + + if (xcfg->name[0] == '\0') { + pr_debug("Channel must be named\n"); + return -EINVAL; + } + + size = modem->modem_bootimg_size; + + for (j = 0; j < modem->channels; j++) { + struct shm_channel *xcfg2 = modem->channel[j]; + size = xcfg2->tx.ch_size + xcfg2->rx.ch_size; + if (xcfg != xcfg2 && strcmp(xcfg->name, xcfg2->name) == 0) { + pr_debug("Channels has same name:%s\n", + xcfg->name); + return -EINVAL; + } + } + + if (size > shm_size) { + pr_debug("Channel size too big\n"); + return -EINVAL; + } + + return 0; +} + +static int verify_config(struct shm_modem *modem) +{ + int i; + + if (modem->channels == 0) { + pr_debug("Bad config: minimum one channel must be defined\n"); + return -EINVAL; + } + for (i = 0; i < modem->channels; i++) { + int err = shm_verify_config(modem, modem->channel[i]); + if (err) + return err; + } + return 0; +} + +/* + * Create Configuration data for the shm devices. + */ +static void create_devs(struct shm_modem *modem) +{ + int i; + + for (i = 0; i < modem->channels; i++) { + struct shm_bufidx *buf_rx, *buf_tx; + struct shm_ipctoc_channel *ch_rx, *ch_tx; + struct shm_channel *xcfg = modem->channel[i]; + ch_rx = LEOFFS2PTR(modem->shm_start, + modem->ipctoc->channel_offsets[i].rx); + buf_rx = LEOFFS2PTR(modem->shm_start, ch_rx->ipc); + ch_tx = LEOFFS2PTR(modem->shm_start, + modem->ipctoc->channel_offsets[i].tx); + buf_tx = LEOFFS2PTR(modem->shm_start, ch_tx->ipc); + + /* + * Due to restricted read-only access + * we swap positions for read/write + * pointers. + */ + xcfg->tx.write = &buf_tx->write_index; + xcfg->tx.read = &buf_rx->read_index; + + xcfg->rx.write = &buf_rx->write_index; + xcfg->rx.read = &buf_tx->read_index; + + xcfg->rx.addr = LEOFFS2PTR(modem->shm_start, ch_rx->offset); + xcfg->tx.addr = LEOFFS2PTR(modem->shm_start, ch_tx->offset); + xcfg->rx.state = &buf_rx->state; + xcfg->tx.state = &buf_tx->state; + xcfg->tx.buf_size = buf_tx->size; + xcfg->rx.buf_size = buf_rx->size; + + xcfg->rx.xfer_bit = le16_to_cpu(ch_rx->write_bit); + xcfg->tx.xfer_bit = le16_to_cpu(ch_tx->write_bit); + xcfg->rx.xfer_done_bit = le16_to_cpu(ch_rx->read_bit); + xcfg->tx.xfer_done_bit = le16_to_cpu(ch_tx->read_bit); + + if (xcfg->mode & SHM_PAIR_MODE) { + struct shm_channel *pair; + pr_debug("Channel[%d] is in PAIR mode\n", i); + if (i < 1) { + pr_debug("No channel to pair with\n"); + continue; + } + /* Cross couple rx/tx on the pair */ + pair = modem->channel[i - 1]; + + /* Copy everything but the kobj which is at the end */ + memcpy(&xcfg->tx, &pair->rx, + offsetof(struct shm_udchannel, kobj)); + memcpy(&xcfg->rx, &pair->tx, + offsetof(struct shm_udchannel, kobj)); + } else if (xcfg->mode & SHM_LOOP_MODE) { + pr_debug("Channel[%d] is in LOOP mode\n", i); + /* + * Connect rx/tx in a pair. Copy everything, + * but the kobj which is at the end + */ + memcpy(&xcfg->tx, &xcfg->rx, + offsetof(struct shm_udchannel, kobj)); + } + + pr_devel("RX[%d] wi:%p ri:%p\n", i, xcfg->rx.read, + xcfg->rx.write); + pr_devel("TX[%d] wi:%p ri:%p\n", i, xcfg->tx.read, + xcfg->tx.write); + } +} + +struct shm_modem *modem; + +struct shm_modem *get_modem(struct sk_buff *skb, struct genl_info *info) +{ + return modem; +} + +static int do_commit(struct shm_modem *modem) +{ + int err; + + if (modem->config_error) { + pr_devel("config error detected\n"); + return -EINVAL; + } + + if (modem->commited) { + pr_devel("already commited\n"); + modem->config_error = true; + return -EINVAL; + } + err = verify_config(modem); + if (err) { + pr_devel("bad config\n"); + modem->config_error = true; + return err; + } + err = write_to_shm(modem); + if (err) { + pr_devel("writei to SHM failed\n"); + modem->config_error = true; + return err; + } + modem->commited = true; + create_devs(modem); + return 0; +} + +static int do_register(struct shm_modem *modem) +{ + int err; + + if (!modem->commited || modem->registered || modem->config_error) { + pr_devel("bad sequence of requests\n"); + modem->config_error = true; + return -EINVAL; + } + + err = verify_config(modem); + if (err) { + modem->config_error = true; + pr_devel("bad config\n"); + return err; + } + modem->registered = true; + + modem_shm_register_devices(modem->channel, modem->channels); + + return 0; +} + +static void shm_firmware(const struct firmware *fw, void *context) +{ + struct shm_modem *modem = context; + struct toc_entry *toc_entry; + int i; + + modem->fw_requested = false; + + pr_debug("recevied firmware\n"); + if (fw == NULL) { + pr_warn("No boot-img provided\n"); + goto out; + } + + if (modem->commited) { + pr_warn("Received firmware too late in modem boot\n"); + goto out; + } + + toc_entry = (void *)fw->data; + if (fw->size < TOC_SZ) { + pr_debug("Modem image too short\n"); + goto bad_img; + } + + for (i = 0; i < 16; i++, toc_entry++) + if (toc_entry->start == cpu_to_le32(0xffffffff)) + break; + + if (i == 0) { + pr_debug("Empty TOC in firmware image\n"); + goto bad_img; + } + + if (i >= 15) { + pr_debug("No free slot for IPC-TOC in received image\n"); + goto bad_img; + } + + modem->modem_bootimg_size = fw->size; + memcpy(modem->shm_start, fw->data, fw->size); +out: + release_firmware(fw); + return; +bad_img: + pr_warn("Bad modem firmware received\n"); + modem->config_error = true; + release_firmware(fw); +} + +static void do_reset(struct shm_modem *modem) +{ + int i; + + modem_shm_reset(); + modem->config_error = false; + modem->registered = false; + modem->commited = false; + modem->addr_set = false; + modem->modem_bootimg_size = TOC_SZ; + for (i = 0; i < modem->channels; i++) { + kfree(modem->channel[i]); + modem->channel[i] = NULL; + } + modem->channels = 0; + + /* Initiate the Master TOC to 0xff for the first 512 bytes */ + memset(modem->shm_start, 0xff, TOC_SZ); + + if (!modem->fw_requested) + modem_shm_request_firmware(modem, THIS_MODULE, + STE_BOOTIMG_NAME, shm_firmware); + modem->fw_requested = true; +} + +static int do_set_addr(struct shm_modem *modem) +{ + int err; + if (!modem->commited || modem->addr_set || modem->config_error) { + pr_devel("bad sequence of requests\n"); + modem->config_error = true; + return -EINVAL; + } + err = verify_config(modem); + if (err) { + modem->config_error = true; + pr_devel("bad config\n"); + return err; + } + modem->addr_set = true; + return genio_set_shm_addr(shm_c2c_bootaddr, genio_ipc_ready_cb); +} + +static int copy_name(const char *src, char *d, size_t count) +{ + const char *s, *end = src + count; + for (s = src; *s && s < end; s++, d++) + if (*s == '\0' || *s == '\n') + break; + else if (!isalnum(*s)) { + pr_debug("Illegal chr:'%c' in name:'%s'\n", *s, src); + return -EINVAL; + } else if (s - src >= SHM_NAMESZ - 1) { + pr_debug("Name '%s'too long\n", src); + return -EINVAL; + } else + *d = *s; + *d = '\0'; + + return count; +} + +/* SHM Generic NETLINK family */ +static struct genl_family shm_gnl_family = { + .id = GENL_ID_GENERATE, + .hdrsize = 0, + .name = "modem_shm", + .version = SHM_PROTO_VERSION, + .maxattr = SHM_A_MAX, +}; + +/* SHM Netlink attribute policy */ +static const struct nla_policy shm_genl_policy[SHM_A_MAX + 1] = { + [SHM_A_VERSION] = { .type = NLA_U8 }, + [SHM_A_SUB_VERSION] = { .type = NLA_U8 }, + [__SHM_A_FLAGS] = { .type = NLA_U32 }, + [SHM_A_NAME] = { .type = NLA_NUL_STRING, .len = SHM_NAMESZ}, + [SHM_A_RX_CHANNEL] = { .type = NLA_NESTED }, + [SHM_A_TX_CHANNEL] = { .type = NLA_NESTED }, + [SHM_A_PRIORITY] = { .type = NLA_U8 }, + [SHM_A_LATENCY] = { .type = NLA_U8 }, +}; + +/* Policy for uni-directional attributes for stream */ +static const struct nla_policy stream_policy[SHM_A_MAX + 1] = { + [SHM_A_CHANNEL_SIZE] = { .type = NLA_U32 }, +}; + +/* Policy for uni-directional attributes for packet */ +static const struct nla_policy packet_policy[SHM_A_MAX + 1] = { + [SHM_A_CHANNEL_SIZE] = { .type = NLA_U32 }, + [SHM_A_CHANNEL_BUFFERS] = { .type = NLA_U32 }, + [SHM_A_MTU] = { .type = NLA_U16 }, + [SHM_A_ALIGNMENT] = { .type = NLA_U8 }, + [SHM_A_PACKETS] = { .type = NLA_U8 }, +}; + +static int shm_add_udchannel(struct shm_udchannel *chn, int attr, + struct genl_info *info, struct nla_policy const *policy) +{ + struct nlattr *nla; + int nla_rem; + + if (!info->attrs[attr]) + return -EINVAL; + + if (nla_validate_nested(info->attrs[attr], + SHM_A_MAX, + policy) != 0) { + pr_info("Invalid RX channel attributes\n"); + return -EINVAL; + } + + nla_for_each_nested(nla, info->attrs[attr], nla_rem) { + + if (nla_type(nla) == SHM_A_CHANNEL_SIZE) + chn->ch_size = nla_get_u32(nla); + + if (nla_type(nla) == SHM_A_CHANNEL_BUFFERS) + chn->buffers = nla_get_u32(nla); + + if (nla_type(nla) == SHM_A_MTU) + chn->mtu = nla_get_u16(nla); + + if (nla_type(nla) == SHM_A_PACKETS) + chn->packets = nla_get_u8(nla); + + if (nla_type(nla) == SHM_A_ALIGNMENT) { + chn->alignment = nla_get_u8(nla); + chn->alignment = rounddown_pow_of_two(chn->alignment); + } + + } + return 0; +} + +static int shm_add_channel(struct shm_channel *cfg, struct genl_info *info, + int mode) +{ + int len, err; + struct nla_policy const *policy; + char name[SHM_NAMESZ]; + + policy = (mode == SHM_PACKET_MODE) ? packet_policy : stream_policy; + + if (info->attrs[SHM_A_VERSION]) { + u8 version; + u8 sub_version; + + version = nla_get_u8(info->attrs[SHM_A_VERSION]); + if (!info->attrs[SHM_A_SUB_VERSION]) + return -EINVAL; + sub_version = nla_get_u8(info->attrs[SHM_A_SUB_VERSION]); + if (version != 1 || sub_version != 0) { + pr_info("Bad version or sub_version\n"); + return -EINVAL; + } + } + + if (!info->attrs[SHM_A_NAME]) { + pr_debug("Name not specified\n"); + return -EINVAL; + } + + len = nla_strlcpy(name, info->attrs[SHM_A_NAME], + SHM_NAMESZ); + + if (len > SHM_NAMESZ) + return -EINVAL; + + err = copy_name(name, cfg->name, sizeof(name)); + if (err < 0) + return err; + + cfg->excl_group = 1; + if (info->attrs[SHM_A_EXCL_GROUP]) + cfg->excl_group = nla_get_u8(info->attrs[SHM_A_EXCL_GROUP]); + + err = shm_add_udchannel(&cfg->rx, SHM_A_RX_CHANNEL, info, policy); + if (err) + return err; + + err = shm_add_udchannel(&cfg->tx, SHM_A_TX_CHANNEL, info, policy); + if (err) + return err; + + if (cfg->tx.ch_size + cfg->rx.ch_size > shm_size) { + pr_debug("Channel size too big\n"); + return -EINVAL; + } + + if (info->attrs[SHM_A_PRIORITY]) { + cfg->priority = nla_get_u8(info->attrs[SHM_A_PRIORITY]); + /* silently fixup bad value */ + if (cfg->priority > 7) + cfg->priority = 0; + } + + if (info->attrs[SHM_A_LATENCY]) + cfg->latency = nla_get_u8(info->attrs[SHM_A_LATENCY]); + + if (info->attrs[__SHM_A_FLAGS]) + cfg->mode |= nla_get_u32(info->attrs[__SHM_A_FLAGS]); + + return 0; +} + +static int do_reply(struct genl_info *info, int result) +{ + struct sk_buff *msg; + int err; + void *reply; + + msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL); + if (msg == NULL) + return -ENOMEM; + + reply = genlmsg_put_reply(msg, info, &shm_gnl_family, 0, result); + if (reply == NULL) { + kfree_skb(msg); + return -EMSGSIZE; + } + + genlmsg_end(msg, reply); + err = genlmsg_reply(msg, info); + return err; +} + +static int shm_add_ch(struct sk_buff *skb, struct genl_info *info, int mode) +{ + int err; + struct shm_channel *cfg; + struct shm_modem *modem = get_modem(skb, info); + if (!modem) + return -EINVAL; + + if (modem->channels + 1 > SHM_MAX_CHANNELS) { + pr_debug("Too many channels added\n"); + return -EINVAL; + } + + + cfg = kzalloc(sizeof(*cfg), GFP_KERNEL); + if (cfg == NULL) + return -ENOMEM; + + cfg->mode = mode; + err = shm_add_channel(cfg, info, mode); + if (err) + goto error; + + modem->channel[modem->channels] = cfg; + ++modem->channels; + + err = shm_verify_config(modem, cfg); + if (err) + goto error_remove; + + err = do_reply(info, 0); + if (err) + goto error_remove; + return err; + +error_remove: + --modem->channels; +error: + kfree(cfg); + return err; +} + +static int shm_add_packet_ch(struct sk_buff *skb, struct genl_info *info) +{ + return shm_add_ch(skb, info, SHM_PACKET_MODE); +} + +static int shm_add_stream_ch(struct sk_buff *skb, struct genl_info *info) +{ + return shm_add_ch(skb, info, SHM_STREAM_MODE); +} + + +static int shm_c_commit(struct sk_buff *skb, struct genl_info *info) +{ + struct shm_modem *modem = get_modem(skb, info); + int err; + if (!modem) + return -EINVAL; + + err = do_commit(modem); + if (!err) + do_reply(info, 0); + return err; +} + +static int shm_c_register(struct sk_buff *skb, struct genl_info *info) +{ + int err; + struct shm_modem *modem = get_modem(skb, info); + if (!modem) + return -EINVAL; + + err = do_register(modem); + if (!err) + do_reply(info, 0); + return err; +} + +static int shm_c_set_addr(struct sk_buff *skb, struct genl_info *info) +{ + int err; + struct shm_modem *modem = get_modem(skb, info); + if (!modem) + return -EINVAL; + + err = do_set_addr(modem); + if (!err) + do_reply(info, 0); + return err; +} + +static int shm_c_reset(struct sk_buff *skb, struct genl_info *info) +{ + struct shm_modem *modem = get_modem(skb, info); + if (!modem) + return -EINVAL; + + do_reset(modem); + do_reply(info, 0); + return 0; +} + +static int shm_c_verify(struct sk_buff *skb, struct genl_info *info) +{ + int err; + struct shm_modem *modem = get_modem(skb, info); + if (!modem) + return -EINVAL; + + err = verify_config(modem); + if (!err) + do_reply(info, 0); + return err; +} + +static struct genl_ops shm_genl_ops[] = { + { + .cmd = SHM_C_ADD_STREAM_CHANNEL, + .flags = GENL_ADMIN_PERM, + .policy = shm_genl_policy, + .doit = shm_add_stream_ch, + .dumpit = NULL, + }, + { + .cmd = SHM_C_ADD_PACKET_CHANNEL, + .flags = GENL_ADMIN_PERM, + .policy = shm_genl_policy, + .doit = shm_add_packet_ch, + .dumpit = NULL, + }, + { + .cmd = SHM_C_COMMIT, + .flags = GENL_ADMIN_PERM, + .doit = shm_c_commit, + .dumpit = NULL, + }, + { + .cmd = SHM_C_REGISTER, + .flags = GENL_ADMIN_PERM, + .doit = shm_c_register, + .dumpit = NULL, + }, + { + .cmd = SHM_C_SET_ADDR, + .flags = GENL_ADMIN_PERM, + .doit = shm_c_set_addr, + .dumpit = NULL, + }, + { + .cmd = SHM_C_RESET, + .flags = GENL_ADMIN_PERM, + .doit = shm_c_reset, + .dumpit = NULL, + }, + { + .cmd = __SHM_C_VERIFY, + .flags = GENL_ADMIN_PERM, + .doit = shm_c_verify, + .dumpit = NULL, + }, + +}; + +/* Initialize boot handling and create sysfs entries*/ +int __init shm_boot_init(void) +{ + int err = -EINVAL; + bool shm_fake = false; + + modem = kzalloc(sizeof(struct shm_modem), GFP_KERNEL); + if (modem == NULL) + return -ENOMEM; + + /* Negative shm_size indicates module test without real SHM */ + if (shm_size < 0) { + shm_fake = true; + shm_size = abs(shm_size); + } + + if (shm_size < TOC_SZ) + goto bad_config; + + if (shm_fake) { + modem->shm_start = kzalloc(shm_size, GFP_KERNEL); + err = -ENOMEM; + if (!modem->shm_start) + goto error; + shm_start = (unsigned long) modem->shm_start; + memset(modem->shm_start, 0xaa, shm_size); + } else { + if (shm_start == 0) + goto bad_config; + modem->shm_start = ioremap(shm_start, shm_size); + } + + /* Initiate the Master TOC to 0xff for the first 512 bytes */ + if (shm_size > TOC_SZ) + memset(modem->shm_start, 0xff, TOC_SZ); + + modem->modem_bootimg_size = TOC_SZ; + + pr_debug("Boot image addr: %p size:%d\n", modem->shm_start, + modem->modem_bootimg_size); + + err = genl_register_family_with_ops(&shm_gnl_family, + shm_genl_ops, ARRAY_SIZE(shm_genl_ops)); + if (err) + goto error; + + modem->gennetl_reg = true; + modem->fw_requested = true; + err = modem_shm_request_firmware(modem, THIS_MODULE, + STE_BOOTIMG_NAME, shm_firmware); + if (err) + goto error; + + return err; +error: + pr_debug("initialization failed\n"); + + if (shm_fake) + kfree(modem->shm_start); + return err; +bad_config: + pr_err("Bad module configuration:" + " shm_base_address:%lu shm_size:%lu err:%d\n", + shm_start, shm_size, err); + /* Buildin module should not return error */ + return -EINVAL; +} + +void __exit shm_boot_exit(void) +{ + + if (modem->gennetl_reg) + genl_unregister_family(&shm_gnl_family); + modem->gennetl_reg = false; +} +module_init(shm_boot_init); +module_exit(shm_boot_exit); -- 2.39.5