android sensor sevice 1.0启动流程简介

Android Service是一种可以在后台执行长时间运行操作而不提供用户界面的应用组件。native层一般通过getService()方法获取service，然后通过service来对底层硬件进行操作，如下：

frameworks\native\services\sensorservice\SensorDevice.cpp

SensorDevice::HalConnectionStatus SensorDevice::connectHidlServiceV1_0() {
    // SensorDevice will wait for HAL service to start if HAL is declared in device manifest.
    size_t retry = 10;
    HalConnectionStatus connectionStatus = HalConnectionStatus::UNKNOWN;

    while (retry-- > 0) {
        sp<V1_0::ISensors> sensors = V1_0::ISensors::getService(); //1
        if (sensors == nullptr) {
            // no sensor hidl service found
            connectionStatus = HalConnectionStatus::DOES_NOT_EXIST;
            break;
        }

        mSensors = new SensorServiceUtil::SensorsWrapperV1_0(sensors);
        mRestartWaiter->reset();
        // Poke ISensor service. If it has lingering connection from previous generation of
        // system server, it will kill itself. There is no intention to handle the poll result,
        // which will be done since the size is 0.
        if(mSensors->poll(0, [](auto, const auto &, const auto &) {}).isOk()) {
            // ok to continue
            connectionStatus = HalConnectionStatus::CONNECTED;
            break;
        }

        // hidl service is restarting, pointer is invalid.
        mSensors = nullptr;
        connectionStatus = HalConnectionStatus::FAILED_TO_CONNECT;
        ALOGI("%s unsuccessful, remaining retry %zu.", __FUNCTION__, retry);
        mRestartWaiter->wait();
    }

    return connectionStatus;
}

service 启动：

hardware\interfaces\sensors\1.0\default\service.cpp

#define LOG_TAG "android.hardware.sensors@1.0-service"

#include <android/hardware/sensors/1.0/ISensors.h>
#include <hidl/LegacySupport.h>

using android::hardware::sensors::V1_0::ISensors;
using android::hardware::defaultPassthroughServiceImplementation;

int main() {
    /* Sensors framework service needs at least two threads.
     * One thread blocks on a "poll"
     * The second thread is needed for all other HAL methods.
     */
    return defaultPassthroughServiceImplementation<ISensors>(2);
}

defaultPassthroughServiceImplementation创建hidl服务并调用HIDL_FETCH_ISensors函数

hardware\interfaces\sensors\1.0\default\Sensors.cpp

ISensors *HIDL_FETCH_ISensors(const char * /* hal */) {
    Sensors *sensors = new Sensors; //2
    if (sensors->initCheck() != OK) {
        delete sensors;
        sensors = nullptr;

        return nullptr;
    }

    return sensors;
}

new Sensors将会走到如下函数

hardware\interfaces\sensors\1.0\default\Sensors.cpp

Sensors::Sensors()
    : mInitCheck(NO_INIT),
      mSensorModule(nullptr),
      mSensorDevice(nullptr) {      //3
    status_t err = OK;
    if (UseMultiHal()) {
        mSensorModule = ::get_multi_hal_module_info();     //4
    } else {
        err = hw_get_module(
            SENSORS_HARDWARE_MODULE_ID,
            (hw_module_t const **)&mSensorModule);   //5
    }
    if (mSensorModule == NULL) {
        err = UNKNOWN_ERROR;
    }

    if (err != OK) {
        LOG(ERROR) << "Couldn't load "
                   << SENSORS_HARDWARE_MODULE_ID
                   << " module ("
                   << strerror(-err)
                   << ")";

        mInitCheck = err;
        return;
    }

    err = sensors_open_1(&mSensorModule->common, &mSensorDevice);   //6

    if (err != OK) {
        LOG(ERROR) << "Couldn't open device for module "
                   << SENSORS_HARDWARE_MODULE_ID
                   << " ("
                   << strerror(-err)
                   << ")";

        mInitCheck = err;
        return;
    }

    // Require all the old HAL APIs to be present except for injection, which
    // is considered optional.
    CHECK_GE(getHalDeviceVersion(), SENSORS_DEVICE_API_VERSION_1_3);

    if (getHalDeviceVersion() == SENSORS_DEVICE_API_VERSION_1_4) {
        if (mSensorDevice->inject_sensor_data == nullptr) {
            LOG(ERROR) << "HAL specifies version 1.4, but does not implement inject_sensor_data()";
        }
        if (mSensorModule->set_operation_mode == nullptr) {
            LOG(ERROR) << "HAL specifies version 1.4, but does not implement set_operation_mode()";
        }
    }

    mInitCheck = OK;
}

hardware\libhardware\include\hardware\sensors.h

static inline int sensors_open_1(const struct hw_module_t* module,
        sensors_poll_device_1_t** device) {
    return module->methods->open(module,
            SENSORS_HARDWARE_POLL, TO_HW_DEVICE_T_OPEN(device)); //7
}

如上中4和5处目的是一样的，都是通过 SENSORS_HARDWARE_MODULE_ID得到mSensorModule，我这里以5处进行介绍，mSensorModule得到的实际就是下面的HAL_MODULE_INFO_SYM.

从上面6处和7处可以看出，调用的实际就是下面的open_sensors函数

vendor\sprd\modules\sensors\libsensorhub\sensors.cpp

struct sensors_module_t HAL_MODULE_INFO_SYM = {
    .common = {
        .tag = HARDWARE_MODULE_TAG,
        .version_major = 1,
        .version_minor = 0,
        .id =  SENSORS_HARDWARE_MODULE_ID,
        .name = "Sensor module",
        .author = "Sensor Platforms Inc.",
        .methods = &sensors_module_methods,
        .dso = 0,
        .reserved = {0}
    },
    .get_sensors_list = sensors__get_sensors_list,
    .set_operation_mode = sensors__set_operation_mode,
};

static struct hw_module_methods_t sensors_module_methods = {
        .open = open_sensors
};

/** Open a new instance of a sensor device using name */
static int open_sensors(const struct hw_module_t* module, const char* id,
                        struct hw_device_t** device) {  //8
    int status = -EINVAL;
    SH_LOG("id[%s]", id);
    /*
     * Write Opcode first
     * */
    SensorHubOpcodeExtrator();

    sensors_poll_context_t *dev = new sensors_poll_context_t();   //9

    memset(&dev->device, 0, sizeof(dev->device));

    dev->device.common.tag = HARDWARE_DEVICE_TAG;
    dev->device.common.version  = SENSORS_DEVICE_API_VERSION_1_3;
    dev->device.common.module   = const_cast<hw_module_t*>(module);
    dev->device.common.close    = poll__close;
    dev->device.activate        = poll__activate;
    dev->device.setDelay        = poll__setDelay;
    dev->device.poll            = poll__poll;
    dev->device.batch           = poll__batch;
    dev->device.flush           = poll__flush;

    *device = &dev->device.common; //10
    status = 0;

    return status;
}

从上面6、7、8、10可以看出open_sensors函数实际就是将hal层的接口函数都传给了mSensorDevice,供service调用。

接下来将以poll__activate为例继续向下进行分析,需要注意的是往将涉及更多平台相关的内容，各家平台设计各有不同，这里以展锐平台做个简单的流程分析

static int poll__activate(struct sensors_poll_device_t *dev,
        int handle, int enabled) {
    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
    return ctx->activate(handle, enabled); //11
}

vendor\sprd\modules\sensors\libsensorhub\sensors.cpp

sensors_poll_context_t::sensors_poll_context_t() {
    int result;
    int idx = 0;
    pshub_cfg_t cfg = shubGetDrvConfig();

    if (cfg) {
        SH_LOG("GetDrvConfig cfgSize = %d", cfg->cfgSize);

        numSensorDrivers = cfg->cfgSize;
        numFds  = numSensorDrivers + 1;
        wake = numSensorDrivers;  // numFds - 1;

        mSensors = (SensorBase **)malloc(sizeof(SensorBase *) * numSensorDrivers);
        mPollFds = (struct pollfd*)malloc(sizeof(struct pollfd) * numFds);

        if (mSensors && mPollFds) {
            for (idx = 0; idx < numSensorDrivers; idx++) {
                SH_LOG("GetDrvConfig ctlName = %s, drvName = %s",
                        cfg->cfg[idx].ctlName, cfg->cfg[idx].drvName);

                mSensors[idx] = new SensorHubInputSensor((cfg->cfg[idx])); //12

                mPollFds[idx].fd = mSensors[idx]->getFd();
                mPollFds[idx].events = POLLIN;
                mPollFds[idx].revents = 0;
            }

            int wakeFds[2];
            result = pipe(wakeFds);
            ALOGE_IF(result < 0, "error creating wake pipe (%s)", strerror(errno));
            fcntl(wakeFds[0], F_SETFL, O_NONBLOCK);  // @@ read should probably be blocking but since
            fcntl(wakeFds[1], F_SETFL, O_NONBLOCK);  // we are using 'poll' it doesn't matter
            mWritePipeFd = wakeFds[1];

            mPollFds[wake].fd = wakeFds[0];
            mPollFds[wake].events = POLLIN;
            mPollFds[wake].revents = 0;
        } else {
            SH_ERR("Malloc 'SensorBase' and 'struct pollfd' failed, return");
        }
    } else {
        SH_ERR("GetDrvConfig failed, return");
    }
}

int sensors_poll_context_t::activate(int handle, int enabled) {
    int index = handleToDriver(handle);
    if (index < 0) return index;

    int err =  mSensors[index]->setEnable(handle, enabled); //13
    ALOGE_IF(err != 0, "Sensor-activate failed (%d)", err);
    if (enabled && !err) {
        const char wakeMessage(WAKE_MESSAGE);
        int result = write(mWritePipeFd, &wakeMessage, 1);
        ALOGE_IF(result < 0, "error sending wake message (%s)", strerror(errno));
    }
    return err;
}

poll_activate 中的dev来自上面的10处，由此11函数既是 sensors_poll_context_t::activate(int handle, int enabled)函数，13处的mSensors来自12处，由此得到如下：

vendor\sprd\modules\sensors\libsensorhub\SensorHubInputSensor.cpp

int SensorHubInputSensor::setEnable(int32_t handle, int enabled) {
    int data[3];  // 1(handle)+1(enable)+1(pad)
    int err = 0;
    int flags = !!enabled;

    SH_LOG("handle[%d], enabled[%d]", handle, enabled);

    pSensorInfo_t ptr = getSensorInfo(handle);

    if (NULL == ptr)
        return -EINVAL;

    if (flags != ptr->enabled) {
        //  ptr->enabled = flags;
        data[0] = handle;
        data[1] = flags;
        data[2] = 0xEF;
        err = sendCmdtoDriver(efCmdsetEnable, data);
        if (err == 0) {  //  0 means success
            ptr->enabled = flags;
        }
    } else {
        SH_ERR("Sensor %s has been %s", ptr->pSensorList->name, flags ? "Open" : "Close");
    }

    return err;
}

vendor\sprd\modules\sensors\libsensorhub\ConfigFeature\SensorList_hub.cpp

static shub_drv_cfg_t sSensorDrvCfg[] = {
    {
        .ctlName    = "/sys/class/sprd_sensorhub/sensor_hub/",
        .ctlMode    = 1,
        .drvName    = "sprd-sensor",
        .drvMode    = 1,
        .sensorList = sSensorList,
        .listSize = ARRAY_SIZE(sSensorList),
    },
};

int SensorHubInputSensor::sendCmdtoDriver(int operate, int* para) {
    int rst = 0;

    switch (ctlMode) {
        case 0: /* IO Ctl */
            rst = sendCmdtoDriverIoctl(operate, para);
            break;
        case 1: /* Sysfs */
            rst = sendCmdtoDriverSysfs(operate, para);
            break;
        default:
            break;
    }

    rst = (rst < 0) ? rst : 0;
    return rst;
}

int SensorHubInputSensor::sendCmdtoDriverSysfs(int operate, int* para) {
    int rst = 0;
    int fd  = 0;
    char buf[128] = {0};
    int64_t data = 0;
    /*
     * Just for debug begin
     * */
    char fixed_sysfs_path[PATH_MAX];
    int fixed_sysfs_path_len;

    strcpy(fixed_sysfs_path, udevName);
    fixed_sysfs_path_len = strlen(fixed_sysfs_path);
    /*
     * Just for debug end
     * */
    switch (operate) {
        case efCmdsetEnable:
            strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "enable");
            sprintf(buf, "%d %d\n", para[0], para[1]);
            break;
        case efCmdsetDelay:
            strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "delay");
            sprintf(buf, "%d %d %d\n", para[0], para[1], para[2]);
            break;
        case efCmdbatch:
            strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "batch");
            memcpy(&data, &para[3], sizeof(int64_t));
            sprintf(buf, "%d %d %d %" SCNd64"\n", para[0], para[1], para[2], data);
            break;
        case efCmdflush:
            strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "flush");
            sprintf(buf, "%d\n", para[0]);
            break;
        case efCmdUpdateHubStatus:
            strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "sensorhub");
            sprintf(buf, "%d\n", para[0]);
            break;
        case efCmdSetVersion:
            strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "version");
            sprintf(buf, "%d\n", para[0]);
            break;
        default:
            break;
    }

    SH_SYSFS("write (%s) %s", fixed_sysfs_path, buf);
    fd = open(fixed_sysfs_path, O_RDWR);
    if (fd >= 0) {
        rst = write(fd, buf, sizeof(buf));
        if (rst < 0)
            SH_ERR("write (%s) to (%s) failed!", buf, fixed_sysfs_path);

        close(fd);
    } else {
        SH_ERR("%s: open (%s) failed!", __func__, fixed_sysfs_path);
        return -EINVAL;
    }

    return rst;
}

由上这里写的文件是/sys/class/sprd_sensorhub/sensor_hub/enable，下面看下底层这个节点创建

kernel4.14/drivers/iio/sprd_hub/shub_core.c

static int create_sysfs_interfaces(struct shub_data *mcu_data)
{
    int ret;

    mcu_data->sensor_class = class_create(THIS_MODULE, "sprd_sensorhub");
    if (IS_ERR(mcu_data->sensor_class))
        return PTR_ERR(mcu_data->sensor_class);

    mcu_data->sensor_dev =
        device_create(mcu_data->sensor_class, NULL, 0, "%s", "sensor_hub");
    if (IS_ERR(mcu_data->sensor_dev)) {
        ret = PTR_ERR(mcu_data->sensor_dev);
        goto err_device_create;
    }
    debugfs_create_symlink("sensor", NULL,
                   "/sys/class/sprd_sensorhub/sensor_hub");

    dev_set_drvdata(mcu_data->sensor_dev, mcu_data);

    ret = sysfs_create_groups(&mcu_data->sensor_dev->kobj,
                  sensorhub_groups);
    if (ret) {
        dev_err(mcu_data->sensor_dev, "failed to create sysfs device attributes\n");
        goto error;
    }

    ret = sysfs_create_link(&mcu_data->sensor_dev->kobj,
                &mcu_data->indio_dev->dev.kobj, "iio");
    if (ret < 0)
        goto error;

    return 0;

error:
    put_device(mcu_data->sensor_dev);
    device_unregister(mcu_data->sensor_dev);
err_device_create:
    class_destroy(mcu_data->sensor_class);
    return ret;
}

static struct attribute *sensorhub_attrs[] = {
    &dev_attr_debug_data.attr,
    &dev_attr_reader_enable.attr,
    &dev_attr_op_download.attr,
    &dev_attr_logctl.attr,
    &dev_attr_enable.attr,
    &dev_attr_batch.attr,
    &dev_attr_flush.attr,
    &dev_attr_mcu_mode.attr,
    &dev_attr_calibrator_cmd.attr,
    &dev_attr_calibrator_data.attr,
    &dev_attr_light_sensor_calibrator.attr,
    &dev_attr_version.attr,
    &dev_attr_als_mode.attr,
    &dev_attr_data_to_dynamic.attr,
    &dev_attr_raw_data_acc.attr,
    &dev_attr_raw_data_mag.attr,
    &dev_attr_raw_data_gyro.attr,
    &dev_attr_raw_data_als.attr,
    &dev_attr_raw_data_ps.attr,
    &dev_attr_sensorhub.attr,
    &dev_attr_hwsensor_id.attr,
    &dev_attr_sensor_info.attr,
    &dev_attr_mag_cali_flag.attr,
    &dev_attr_shub_debug.attr,
    &dev_attr_cm4_operate.attr,
    &dev_attr_cm4_spi_set.attr,
    &dev_attr_cm4_spi_sync.attr,
    NULL,
};
ATTRIBUTE_GROUPS(sensorhub);

static ssize_t enable_store(struct device *dev,
                struct device_attribute *attr,
                const char *buf, size_t count)
{
    struct shub_data *sensor = dev_get_drvdata(dev);
    int handle, enabled;
    enum shub_subtype_id subtype;

    dev_info(&sensor->sensor_pdev->dev, "buf=%s\n", buf);
    if (sensor->mcu_mode <= SHUB_OPDOWNLOAD) {
        dev_info(&sensor->sensor_pdev->dev,
             "[%s]mcu_mode == %d!\n", __func__, sensor->mcu_mode);
        return -EAGAIN;
    }

    if (sscanf(buf, "%d %d\n", &handle, &enabled) != 2)
        return -EINVAL;
    dev_info(&sensor->sensor_pdev->dev,
        "handle = %d, enabled = %d\n", handle, enabled);
    subtype = (enabled == 0) ? SHUB_SET_DISABLE_SUBTYPE :
        SHUB_SET_ENABLE_SUBTYPE;
    if (shub_send_command(sensor, handle, subtype, NULL, 0) < 0)
        dev_err(&sensor->sensor_pdev->dev, "Write SetEn/Disable fail\n");

    if (handle < MAX_SENSOR_HANDLE && handle > 0)
        sensor_status[handle] = enabled;

    return count;
}
static DEVICE_ATTR_WO(enable);

以上就是就完成了/sys/class/sprd_sensorhub/sensor_hub/enable的创建，当我们写该节点时，实际就是调用enable_store函数，enable_store实际就是调用shub_send_command将sensor的使能发送给sensorhub完成使能。

static int shub_send_command(struct shub_data *sensor, int sensor_ID,
                 enum shub_subtype_id opcode,
                 const char *data, int len)
{
    struct cmd_data cmddata;
    int encode_len;
    int nwrite = 0;
    int ret = 0;

    mutex_lock(&sensor->send_command_mutex);

    if (opcode == SHUB_DOWNLOAD_OPCODE_SUBTYPE) {
        sensor->sent_cmddata.type = sensor_ID;
        sensor->sent_cmddata.sub_type = opcode;
        sensor->sent_cmddata.status = RESPONSE_FAIL;
        sensor->sent_cmddata.condition = false;
    }

    cmddata.type = sensor_ID;
    cmddata.subtype = opcode;
    cmddata.length = len;
    /* no data command  set default data 0xFF */
    if ((len == 0) || (data == NULL)) {
        cmddata.buff[0] = 0xFF;
        cmddata.length = 1;
        len = 1;
    } else {
        memcpy(cmddata.buff, data, len);
    }
    encode_len =
        shub_encode_one_packet(&cmddata, sensor->writebuff,
                   SERIAL_WRITE_BUFFER_MAX);

    if (encode_len > SHUB_MAX_HEAD_LEN + SHUB_MAX_DATA_CRC) {
        nwrite =
            shub_send_data(sensor, sensor->writebuff, encode_len);
    }
    /* command timeout test */

    if (opcode == SHUB_DOWNLOAD_OPCODE_SUBTYPE) {
        ret =
        wait_event_timeout(sensor->rw_wait_queue,
                   (sensor-

static int shub_send_data(struct shub_data *sensor, u8 *buf, u32 len)
{
    int nwrite = 0;
    int sent_len = 0;
    int timeout = 100;
    int retry = 0;

    do {
        nwrite =
            sbuf_write(sensor->sipc_sensorhub_id, SMSG_CH_PIPE,
                    SIPC_PM_BUFID0,
                    (void *)(buf + sent_len),
                    len - sent_len,
                    msecs_to_jiffies(timeout));
        if (nwrite > 0)
            sent_len += nwrite;
        if (nwrite < len || nwrite < 0)
            dev_err(&sensor->sensor_pdev->dev,
                "nwrite=%d,len=%d,sent_len=%d,timeout=%dms\n",
                nwrite, len, sent_len, timeout);
        /* only handle boot exception */
        if (nwrite < 0) {
            if (nwrite == -ENODEV) {
                msleep(100);
                retry++;
                if (retry > 10)
                    break;
            } else {
                dev_err(&sensor->sensor_pdev->dev,
                    "nwrite=%d\n", nwrite);
                dev_err(&sensor->sensor_pdev->dev,
                    "task #: %s, pid = %d, tgid = %d\n",
                    current->comm, current->pid,
                    current->tgid);
                WARN_ONCE(1, "%s timeout: %dms\n",
                      __func__, timeout);
                /* BUG(); */
                break;
            }
        }
    } while (nwrite < 0 || sent_len  < len);
    return nwrite;
}

>sent_cmddata.condition),
                   msecs_to_jiffies(RESPONSE_WAIT_TIMEOUT_MS));
        dev_info(&sensor->sensor_pdev->dev,
             "send_wait end ret=%d\n", ret);
        /* ret= 0 :timeout */
        if (!ret) {
            sensor->sent_cmddata.status = RESPONSE_TIMEOUT;
            ret = RESPONSE_TIMEOUT;
        } else {
            /* get from callback */
            ret = sensor->sent_cmddata.status;
        }
    } else {
        ret = nwrite;
    }

    mutex_unlock(&sensor->send_command_mutex);

    return ret;
}

int sbuf_write(u8 dst, u8 channel, u32 bufid,
           void *buf, u32 len, int timeout)
{
    struct sbuf_mgr *sbuf;
    struct sbuf_ring *ring = NULL;
    struct sbuf_ring_header_op *hd_op;
    struct smsg mevt;
    void *txpos;
    int rval, left, tail, txsize;
    u8 ch_index;
    union sbuf_buf u_buf;
    bool no_data;
    unsigned long flags;

    u_buf.buf = buf;
    ch_index = sipc_channel2index(channel);
    if (ch_index == INVALID_CHANEL_INDEX) {
        pr_err("%s:channel %d invalid!\n", __func__, channel);
        return -EINVAL;
    }

    sbuf = sbufs[dst][ch_index];
    if (!sbuf)
        return -ENODEV;
    ring = &sbuf->rings[bufid];
    hd_op = &ring->header_op;
    if (sbuf->state != SBUF_STATE_READY) {
        pr_info("sbuf-%d-%d not ready to write!\n",
            dst, channel);
        return -ENODEV;
    }

    pr_debug("%s: dst=%d, channel=%d, bufid=%d, len=%d, timeout=%d\n",
         __func__,
         dst,
         channel,
         bufid,
         len,
         timeout);

    rval = 0;
    left = len;

    if (timeout) {
        mutex_lock(&ring->txlock);
    } else {
        if (!mutex_trylock(&ring->txlock)) {
            pr_debug("sbuf_read busy, dst=%d, channel=%d, bufid=%d\n",
                 dst, channel, bufid);
            return -EBUSY;
        }
    }

#if defined(SIPC_DEBUG_SBUF_RDWT_OWNER)
    sbuf_record_rdwt_owner(ring, 0);
#endif

    /* must request resource before read or write share memory */
    rval = sipc_smem_request_resource(ring->tx_pms, sbuf->dst, -1);
    if (rval < 0) {
        mutex_unlock(&ring->txlock);
        return rval;
    }

    pr_debug("%s: channel=%d, wrptr=%d, rdptr=%d\n",
         __func__,
         channel,
         *(hd_op->tx_wt_p),
         *(hd_op->tx_rd_p));
    no_data = ((int)(*(hd_op->tx_wt_p) - *(hd_op->tx_rd_p)) >=
                    hd_op->tx_size);

    /* update write mask */
    spin_lock_irqsave(&ring->poll_lock, flags);
    if (no_data)
        ring->poll_mask &= ~(POLLOUT | POLLWRNORM);
    else
        ring->poll_mask |= POLLOUT | POLLWRNORM;
    spin_unlock_irqrestore(&ring->poll_lock, flags);

    /* release resource */
    sipc_smem_release_resource(ring->tx_pms, sbuf->dst);

    if (no_data) {
        if (timeout == 0) {
            pr_info("%s: %d-%d ring %d txbuf is full!\n",
                __func__, dst, channel, bufid);
            rval = -EBUSY;
        } else if (timeout < 0) {
            /* wait forever */
            rval = wait_event_interruptible(
                ring->txwait,
                sbuf_has_data(ring, dst, true) ||
                sbuf->state == SBUF_STATE_IDLE);
            if (rval < 0)
                pr_debug("%s: wait interrupted!\n", __func__);

            if (sbuf->state == SBUF_STATE_IDLE) {
                pr_err("%s: sbuf state is idle!\n", __func__);
                rval = -EIO;
            }
        } else {
            /* wait timeout */
            rval = wait_event_interruptible_timeout(
                ring->txwait,
                sbuf_has_data(ring, dst, true) ||
                sbuf->state == SBUF_STATE_IDLE,
                timeout);
            if (rval < 0) {
                pr_debug("%s: wait interrupted!\n", __func__);
            } else if (rval == 0) {
                pr_info("%s: wait timeout!\n", __func__);
                rval = -ETIME;
            }

            if (sbuf->state == SBUF_STATE_IDLE) {
                pr_err("%s: sbuf state is idle!\n", __func__);
                rval = -EIO;
            }
        }
    }

    if (rval < 0) {
        mutex_unlock(&ring->txlock);
        return rval;
    }

    /* must request resource before read or write share memory */
    rval = sipc_smem_request_resource(ring->tx_pms, sbuf->dst, -1);
    if (rval < 0) {
        mutex_unlock(&ring->txlock);
        return rval;
    }

    while (left && (int)(*(hd_op->tx_wt_p) - *(hd_op->tx_rd_p)) <
           hd_op->tx_size && sbuf->state == SBUF_STATE_READY) {
        /* calc txpos & txsize */
        txpos = ring->txbuf_virt +
            *(hd_op->tx_wt_p) % hd_op->tx_size;
        txsize = hd_op->tx_size -
            (int)(*(hd_op->tx_wt_p) - *(hd_op->tx_rd_p));
        txsize = min(txsize, left);

        tail = txpos + txsize - (ring->txbuf_virt + hd_op->tx_size);
        if (tail > 0) {
            /* ring buffer is rounded */
            if ((uintptr_t)u_buf.buf > TASK_SIZE) {
                unalign_memcpy(txpos, u_buf.buf, txsize - tail);
                unalign_memcpy(ring->txbuf_virt,
                           u_buf.buf + txsize - tail, tail);
            } else {
                if (unalign_copy_from_user(
                    txpos,
                    u_buf.ubuf,
                    txsize - tail) ||
                   unalign_copy_from_user(
                    ring->txbuf_virt,
                    u_buf.ubuf + txsize - tail,
                    tail)) {
                    pr_err("%s:failed to copy from user!\n",
                           __func__);
                    rval = -EFAULT;
                    break;
                }
            }
        } else {
            if ((uintptr_t)u_buf.buf > TASK_SIZE) {
                unalign_memcpy(txpos, u_buf.buf, txsize);
            } else {
                /* handle the user space address */
                if (unalign_copy_from_user(
                        txpos,
                        u_buf.ubuf,
                        txsize)) {
                    pr_err("%s:failed to copy from user!\n",
                           __func__);
                    rval = -EFAULT;
                    break;
                }
            }
        }

        pr_debug("%s: channel=%d, txpos=%p, txsize=%d\n",
             __func__, channel, txpos, txsize);

        /* update tx wrptr */
        *(hd_op->tx_wt_p) = *(hd_op->tx_wt_p) + txsize;
        /*
         * force send be true or tx ringbuf is empty,
         * need to notify peer side
         */
        if (sbuf->force_send ||
            *(hd_op->tx_wt_p) - *(hd_op->tx_rd_p) == txsize) {
            smsg_set(&mevt, channel,
                 SMSG_TYPE_EVENT,
                 SMSG_EVENT_SBUF_WRPTR,
                 bufid);
            smsg_send(dst, &mevt, -1);
        }

        left -= txsize;
        u_buf.buf += txsize;
    }

    /* update write mask */
    spin_lock_irqsave(&ring->poll_lock, flags);
    if ((int)(*(hd_op->tx_wt_p) - *(hd_op->tx_rd_p)) >=
        hd_op->tx_size)
        ring->poll_mask &= ~(POLLOUT | POLLWRNORM);
    else
        ring->poll_mask |= POLLOUT | POLLWRNORM;
    spin_unlock_irqrestore(&ring->poll_lock, flags);

    /* release resource */
    sipc_smem_release_resource(ring->tx_pms, sbuf->dst);
    if (ring->need_wake_lock)
        sprd_pms_release_wakelock_later(ring->tx_pms, 20);

    mutex_unlock(&ring->txlock);

    pr_debug("%s: done, channel=%d, len=%d\n",
         __func__, channel, len - left);

    if (len == left)
        return rval;
    else
        return (len - left);
}
EXPORT_SYMBOL_GPL(sbuf_write);