1860 return ret; |
1861 return ret; |
1861 } |
1862 } |
1862 |
1863 |
1863 /*****************************************************************************/ |
1864 /*****************************************************************************/ |
1864 |
1865 |
|
1866 /** Create a VoE handler. |
|
1867 */ |
|
1868 int ec_cdev_ioctl_sc_create_voe_handler( |
|
1869 ec_master_t *master, /**< EtherCAT master. */ |
|
1870 unsigned long arg, /**< ioctl() argument. */ |
|
1871 ec_cdev_priv_t *priv /**< Private data structure of file handle. */ |
|
1872 ) |
|
1873 { |
|
1874 ec_ioctl_voe_t data; |
|
1875 ec_slave_config_t *sc; |
|
1876 ec_voe_handler_t *voe; |
|
1877 |
|
1878 if (unlikely(!priv->requested)) |
|
1879 return -EPERM; |
|
1880 |
|
1881 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
|
1882 return -EFAULT; |
|
1883 } |
|
1884 |
|
1885 data.voe_index = 0; |
|
1886 |
|
1887 if (down_interruptible(&master->master_sem)) |
|
1888 return -EINTR; |
|
1889 |
|
1890 sc = ec_master_get_config(master, data.config_index); |
|
1891 if (!sc) { |
|
1892 up(&master->master_sem); |
|
1893 return -ESRCH; |
|
1894 } |
|
1895 |
|
1896 list_for_each_entry(voe, &sc->voe_handlers, list) { |
|
1897 data.voe_index++; |
|
1898 } |
|
1899 |
|
1900 up(&master->master_sem); |
|
1901 |
|
1902 voe = ecrt_slave_config_create_voe_handler(sc, data.size); |
|
1903 if (!voe) |
|
1904 return -ENOMEM; |
|
1905 |
|
1906 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
|
1907 return -EFAULT; |
|
1908 |
|
1909 return 0; |
|
1910 } |
|
1911 |
|
1912 /*****************************************************************************/ |
|
1913 |
1865 /** Get the slave configuration's state. |
1914 /** Get the slave configuration's state. |
1866 */ |
1915 */ |
1867 int ec_cdev_ioctl_sc_state( |
1916 int ec_cdev_ioctl_sc_state( |
1868 ec_master_t *master, /**< EtherCAT master. */ |
1917 ec_master_t *master, /**< EtherCAT master. */ |
1869 unsigned long arg, /**< ioctl() argument. */ |
1918 unsigned long arg, /**< ioctl() argument. */ |
2019 ecrt_domain_state(domain, &state); |
2068 ecrt_domain_state(domain, &state); |
2020 |
2069 |
2021 up(&master->master_sem); |
2070 up(&master->master_sem); |
2022 |
2071 |
2023 if (copy_to_user((void __user *) data.state, &state, sizeof(state))) |
2072 if (copy_to_user((void __user *) data.state, &state, sizeof(state))) |
|
2073 return -EFAULT; |
|
2074 |
|
2075 return 0; |
|
2076 } |
|
2077 |
|
2078 /*****************************************************************************/ |
|
2079 |
|
2080 /** Sets the VoE send header. |
|
2081 */ |
|
2082 int ec_cdev_ioctl_voe_send_header( |
|
2083 ec_master_t *master, /**< EtherCAT master. */ |
|
2084 unsigned long arg, /**< ioctl() argument. */ |
|
2085 ec_cdev_priv_t *priv /**< Private data structure of file handle. */ |
|
2086 ) |
|
2087 { |
|
2088 ec_ioctl_voe_t data; |
|
2089 ec_slave_config_t *sc; |
|
2090 ec_voe_handler_t *voe; |
|
2091 uint32_t vendor_id; |
|
2092 uint16_t vendor_type; |
|
2093 |
|
2094 if (unlikely(!priv->requested)) |
|
2095 return -EPERM; |
|
2096 |
|
2097 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
|
2098 return -EFAULT; |
|
2099 |
|
2100 if (get_user(vendor_id, data.vendor_id)) |
|
2101 return -EFAULT; |
|
2102 |
|
2103 if (get_user(vendor_type, data.vendor_type)) |
|
2104 return -EFAULT; |
|
2105 |
|
2106 if (down_interruptible(&master->master_sem)) |
|
2107 return -EINTR; |
|
2108 |
|
2109 if (!(sc = ec_master_get_config(master, data.config_index))) { |
|
2110 up(&master->master_sem); |
|
2111 return -ESRCH; |
|
2112 } |
|
2113 |
|
2114 if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) { |
|
2115 up(&master->master_sem); |
|
2116 return -ESRCH; |
|
2117 } |
|
2118 |
|
2119 up(&master->master_sem); |
|
2120 |
|
2121 ecrt_voe_handler_send_header(voe, vendor_id, vendor_type); |
|
2122 return 0; |
|
2123 } |
|
2124 |
|
2125 /*****************************************************************************/ |
|
2126 |
|
2127 /** Gets the received VoE header. |
|
2128 */ |
|
2129 int ec_cdev_ioctl_voe_rec_header( |
|
2130 ec_master_t *master, /**< EtherCAT master. */ |
|
2131 unsigned long arg, /**< ioctl() argument. */ |
|
2132 ec_cdev_priv_t *priv /**< Private data structure of file handle. */ |
|
2133 ) |
|
2134 { |
|
2135 ec_ioctl_voe_t data; |
|
2136 ec_slave_config_t *sc; |
|
2137 ec_voe_handler_t *voe; |
|
2138 uint32_t vendor_id; |
|
2139 uint16_t vendor_type; |
|
2140 |
|
2141 if (unlikely(!priv->requested)) |
|
2142 return -EPERM; |
|
2143 |
|
2144 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
|
2145 return -EFAULT; |
|
2146 |
|
2147 if (down_interruptible(&master->master_sem)) |
|
2148 return -EINTR; |
|
2149 |
|
2150 if (!(sc = ec_master_get_config(master, data.config_index))) { |
|
2151 up(&master->master_sem); |
|
2152 return -ESRCH; |
|
2153 } |
|
2154 |
|
2155 if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) { |
|
2156 up(&master->master_sem); |
|
2157 return -ESRCH; |
|
2158 } |
|
2159 |
|
2160 ecrt_voe_handler_received_header(voe, &vendor_id, &vendor_type); |
|
2161 |
|
2162 up(&master->master_sem); |
|
2163 |
|
2164 if (likely(data.vendor_id)) |
|
2165 if (put_user(vendor_id, data.vendor_id)) |
|
2166 return -EFAULT; |
|
2167 |
|
2168 if (likely(data.vendor_type)) |
|
2169 if (put_user(vendor_type, data.vendor_type)) |
|
2170 return -EFAULT; |
|
2171 |
|
2172 return 0; |
|
2173 } |
|
2174 |
|
2175 /*****************************************************************************/ |
|
2176 |
|
2177 /** Starts a VoE read operation. |
|
2178 */ |
|
2179 int ec_cdev_ioctl_voe_read( |
|
2180 ec_master_t *master, /**< EtherCAT master. */ |
|
2181 unsigned long arg, /**< ioctl() argument. */ |
|
2182 ec_cdev_priv_t *priv /**< Private data structure of file handle. */ |
|
2183 ) |
|
2184 { |
|
2185 ec_ioctl_voe_t data; |
|
2186 ec_slave_config_t *sc; |
|
2187 ec_voe_handler_t *voe; |
|
2188 |
|
2189 if (unlikely(!priv->requested)) |
|
2190 return -EPERM; |
|
2191 |
|
2192 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
|
2193 return -EFAULT; |
|
2194 |
|
2195 if (down_interruptible(&master->master_sem)) |
|
2196 return -EINTR; |
|
2197 |
|
2198 if (!(sc = ec_master_get_config(master, data.config_index))) { |
|
2199 up(&master->master_sem); |
|
2200 return -ESRCH; |
|
2201 } |
|
2202 |
|
2203 if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) { |
|
2204 up(&master->master_sem); |
|
2205 return -ESRCH; |
|
2206 } |
|
2207 |
|
2208 up(&master->master_sem); |
|
2209 |
|
2210 ecrt_voe_handler_read(voe); |
|
2211 return 0; |
|
2212 } |
|
2213 |
|
2214 /*****************************************************************************/ |
|
2215 |
|
2216 /** Starts a VoE write operation. |
|
2217 */ |
|
2218 int ec_cdev_ioctl_voe_write( |
|
2219 ec_master_t *master, /**< EtherCAT master. */ |
|
2220 unsigned long arg, /**< ioctl() argument. */ |
|
2221 ec_cdev_priv_t *priv /**< Private data structure of file handle. */ |
|
2222 ) |
|
2223 { |
|
2224 ec_ioctl_voe_t data; |
|
2225 ec_slave_config_t *sc; |
|
2226 ec_voe_handler_t *voe; |
|
2227 |
|
2228 if (unlikely(!priv->requested)) |
|
2229 return -EPERM; |
|
2230 |
|
2231 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
|
2232 return -EFAULT; |
|
2233 |
|
2234 if (down_interruptible(&master->master_sem)) |
|
2235 return -EINTR; |
|
2236 |
|
2237 if (!(sc = ec_master_get_config(master, data.config_index))) { |
|
2238 up(&master->master_sem); |
|
2239 return -ESRCH; |
|
2240 } |
|
2241 |
|
2242 if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) { |
|
2243 up(&master->master_sem); |
|
2244 return -ESRCH; |
|
2245 } |
|
2246 |
|
2247 up(&master->master_sem); |
|
2248 |
|
2249 if (data.size) { |
|
2250 if (data.size > ec_voe_handler_mem_size(voe)) |
|
2251 return -EOVERFLOW; |
|
2252 |
|
2253 if (copy_from_user(ecrt_voe_handler_data(voe), |
|
2254 (void __user *) data.data, data.size)) |
|
2255 return -EFAULT; |
|
2256 } |
|
2257 |
|
2258 ecrt_voe_handler_write(voe, data.size); |
|
2259 return 0; |
|
2260 } |
|
2261 |
|
2262 /*****************************************************************************/ |
|
2263 |
|
2264 /** Executes the VoE state machine. |
|
2265 */ |
|
2266 int ec_cdev_ioctl_voe_exec( |
|
2267 ec_master_t *master, /**< EtherCAT master. */ |
|
2268 unsigned long arg, /**< ioctl() argument. */ |
|
2269 ec_cdev_priv_t *priv /**< Private data structure of file handle. */ |
|
2270 ) |
|
2271 { |
|
2272 ec_ioctl_voe_t data; |
|
2273 ec_slave_config_t *sc; |
|
2274 ec_voe_handler_t *voe; |
|
2275 |
|
2276 if (unlikely(!priv->requested)) |
|
2277 return -EPERM; |
|
2278 |
|
2279 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
|
2280 return -EFAULT; |
|
2281 |
|
2282 if (down_interruptible(&master->master_sem)) |
|
2283 return -EINTR; |
|
2284 |
|
2285 if (!(sc = ec_master_get_config(master, data.config_index))) { |
|
2286 up(&master->master_sem); |
|
2287 return -ESRCH; |
|
2288 } |
|
2289 |
|
2290 if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) { |
|
2291 up(&master->master_sem); |
|
2292 return -ESRCH; |
|
2293 } |
|
2294 |
|
2295 up(&master->master_sem); |
|
2296 |
|
2297 data.state = ecrt_voe_handler_execute(voe); |
|
2298 if (data.state == EC_REQUEST_SUCCESS && voe->dir == EC_DIR_INPUT) |
|
2299 data.size = ecrt_voe_handler_data_size(voe); |
|
2300 |
|
2301 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
|
2302 return -EFAULT; |
|
2303 |
|
2304 return 0; |
|
2305 } |
|
2306 |
|
2307 /*****************************************************************************/ |
|
2308 |
|
2309 /** Reads the received VoE data. |
|
2310 */ |
|
2311 int ec_cdev_ioctl_voe_data( |
|
2312 ec_master_t *master, /**< EtherCAT master. */ |
|
2313 unsigned long arg, /**< ioctl() argument. */ |
|
2314 ec_cdev_priv_t *priv /**< Private data structure of file handle. */ |
|
2315 ) |
|
2316 { |
|
2317 ec_ioctl_voe_t data; |
|
2318 ec_slave_config_t *sc; |
|
2319 ec_voe_handler_t *voe; |
|
2320 |
|
2321 if (unlikely(!priv->requested)) |
|
2322 return -EPERM; |
|
2323 |
|
2324 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
|
2325 return -EFAULT; |
|
2326 |
|
2327 if (down_interruptible(&master->master_sem)) |
|
2328 return -EINTR; |
|
2329 |
|
2330 if (!(sc = ec_master_get_config(master, data.config_index))) { |
|
2331 up(&master->master_sem); |
|
2332 return -ESRCH; |
|
2333 } |
|
2334 |
|
2335 if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) { |
|
2336 up(&master->master_sem); |
|
2337 return -ESRCH; |
|
2338 } |
|
2339 |
|
2340 up(&master->master_sem); |
|
2341 |
|
2342 if (copy_to_user((void __user *) data.data, ecrt_voe_handler_data(voe), |
|
2343 ecrt_voe_handler_data_size(voe))) |
2024 return -EFAULT; |
2344 return -EFAULT; |
2025 |
2345 |
2026 return 0; |
2346 return 0; |
2027 } |
2347 } |
2028 |
2348 |
2210 if (!(filp->f_mode & FMODE_WRITE)) |
2534 if (!(filp->f_mode & FMODE_WRITE)) |
2211 return -EPERM; |
2535 return -EPERM; |
2212 return ec_cdev_ioctl_domain_queue(master, arg, priv); |
2536 return ec_cdev_ioctl_domain_queue(master, arg, priv); |
2213 case EC_IOCTL_DOMAIN_STATE: |
2537 case EC_IOCTL_DOMAIN_STATE: |
2214 return ec_cdev_ioctl_domain_state(master, arg, priv); |
2538 return ec_cdev_ioctl_domain_state(master, arg, priv); |
|
2539 case EC_IOCTL_VOE_SEND_HEADER: |
|
2540 if (!(filp->f_mode & FMODE_WRITE)) |
|
2541 return -EPERM; |
|
2542 return ec_cdev_ioctl_voe_send_header(master, arg, priv); |
|
2543 case EC_IOCTL_VOE_REC_HEADER: |
|
2544 return ec_cdev_ioctl_voe_rec_header(master, arg, priv); |
|
2545 case EC_IOCTL_VOE_READ: |
|
2546 if (!(filp->f_mode & FMODE_WRITE)) |
|
2547 return -EPERM; |
|
2548 return ec_cdev_ioctl_voe_read(master, arg, priv); |
|
2549 case EC_IOCTL_VOE_WRITE: |
|
2550 if (!(filp->f_mode & FMODE_WRITE)) |
|
2551 return -EPERM; |
|
2552 return ec_cdev_ioctl_voe_write(master, arg, priv); |
|
2553 case EC_IOCTL_VOE_EXEC: |
|
2554 if (!(filp->f_mode & FMODE_WRITE)) |
|
2555 return -EPERM; |
|
2556 return ec_cdev_ioctl_voe_exec(master, arg, priv); |
|
2557 case EC_IOCTL_VOE_DATA: |
|
2558 return ec_cdev_ioctl_voe_data(master, arg, priv); |
2215 default: |
2559 default: |
2216 return -ENOTTY; |
2560 return -ENOTTY; |
2217 } |
2561 } |
2218 } |
2562 } |
2219 |
2563 |